TestrigGUI.cpp

00001 /*
00002  * testrigGUI.cc
00003  *
00004  *  Created on: Feb 18, 2010
00005  *      Author: uscr
00006  */
00007 
00008 #include <opencv/cv.h>
00009 #include <opencv/highgui.h>
00010 
00011 #include <pthread.h>
00012 #include <cmath>
00013 #include <cstdio>
00014 
00015 #include <QtCore/QString>
00016 #include <QtGui/QPen>
00017 
00018 #include "TestrigGUI.h"
00019 
00020 extern struct CollectedData myData;
00021 extern struct KalmanFilterData pitchFilter, rollFilter;
00022 extern pthread_mutex_t camMutex, imuMutex;
00023 extern IplImage * frame;
00024 
00025 TestrigGUI::TestrigGUI() {
00026         ui.setupUi(this);
00027         connect (this, SIGNAL(readyToUpdateGUI()), this, SLOT(updateGUIDisplay()));
00028         connect (ui.controlVelocity, SIGNAL(valueChanged(int)), this, SLOT(updateControls()));
00029         connect (ui.controlYaw, SIGNAL(valueChanged(int)), this, SLOT(updateControls()));
00030         connect (ui.reset_pitch, SIGNAL(clicked()), this, SLOT(updatePitchFilter()));
00031         connect (ui.reset_roll, SIGNAL(clicked()), this, SLOT(updateRollFilter()));
00032 
00033         //curIplImage = cvCreateImage(cvGetSize(frame), IPL_DEPTH_8U, 3);
00034         frameCounter = -1;
00035         ui.roll_cmp->setTitle(QString("Roll Filtered Angle"));
00036         ui.pitch_cmp->setTitle(QString("Pitch Filtered Angle"));
00037         ui.roll_raw->setTitle(QwtText("Roll Raw Data (<font color=#006699>Gyro</font>, <font color=#999966>Accel</font>)"));
00038         ui.pitch_raw->setTitle(QwtText("Pitch Raw Data (<font color=#006699>Gyro</font>, <font color=#999966>Accel</font>)"));
00039 
00040         for (int i = 0; i < NUM_POINTS; i++) {
00041                 myGuiPlotData.camPitchAngle[i] = 0;
00042                 myGuiPlotData.camRollAngle[i] = 0;
00043                 myGuiPlotData.imuPitchAccelVal[i] = 0;
00044                 myGuiPlotData.imuPitchAngle[i] = 0;
00045                 myGuiPlotData.imuPitchGyroVal[i] = 0;
00046                 myGuiPlotData.imuRollAccelVal[i] = 0;
00047                 myGuiPlotData.imuRollAngle[i] = 0;
00048                 myGuiPlotData.imuRollGyroVal[i] = 0;
00049                 myGuiPlotData.imuDeltaT[i] = 0;
00050         }
00051         myGuiPlotData.imuLastT = 0;
00052 
00053         rollGyroCurve = new QwtPlotCurve();
00054         rollAccelCurve = new QwtPlotCurve();
00055         rollFilterCurve = new QwtPlotCurve();
00056         pitchGyroCurve = new QwtPlotCurve();
00057         pitchAccelCurve = new QwtPlotCurve();
00058         pitchFilterCurve = new QwtPlotCurve();
00059 
00060         rollGyroCurve->setRawData(incrementalTime, myGuiPlotData.imuRollGyroVal, NUM_POINTS);
00061         rollGyroCurve->setPen(QPen(QColor(0, 102, 153)));
00062         rollAccelCurve->setRawData(incrementalTime, myGuiPlotData.imuRollAccelVal, NUM_POINTS);
00063         rollAccelCurve->setPen(QPen(QColor(153, 153, 102)));
00064         rollFilterCurve->setRawData(incrementalTime, myGuiPlotData.imuRollAngle, NUM_POINTS);
00065         pitchGyroCurve->setRawData(incrementalTime, myGuiPlotData.imuPitchGyroVal, NUM_POINTS);
00066         pitchGyroCurve->setPen(QPen(QColor(0, 102, 153)));
00067         pitchAccelCurve->setRawData(incrementalTime, myGuiPlotData.imuPitchAccelVal, NUM_POINTS);
00068         pitchAccelCurve->setPen(QPen(QColor(153, 153, 102)));
00069         pitchFilterCurve->setRawData(incrementalTime, myGuiPlotData.imuPitchAngle, NUM_POINTS);
00070 
00071         ui.roll_raw->setAutoReplot(false);
00072         ui.roll_cmp->setAutoReplot(false);
00073         ui.pitch_raw->setAutoReplot(false);
00074         ui.pitch_cmp->setAutoReplot(false);
00075 
00076         rollGyroCurve->attach(ui.roll_raw);
00077         rollAccelCurve->attach(ui.roll_raw);
00078         rollFilterCurve->attach(ui.roll_cmp);
00079         pitchGyroCurve->attach(ui.pitch_raw);
00080         pitchAccelCurve->attach(ui.pitch_raw);
00081         pitchFilterCurve->attach(ui.pitch_cmp);
00082         //ui.roll_raw->setAxisScale(0, 200, 800, 100);
00083         ui.roll_raw->setAxisScale(2, 0, 10500, 2000);
00084         ui.roll_cmp->setAxisScale(2, 0, 10500, 2000);
00085         ui.pitch_raw->setAxisScale(2, 0, 10500, 2000);
00086         ui.pitch_cmp->setAxisScale(2, 0, 10500, 2000);
00087 
00088 
00089         fft_in = ( fftw_complex* ) fftw_malloc( sizeof( fftw_complex ) * NUM_POINTS );
00090         fft_out = ( fftw_complex* ) fftw_malloc( sizeof( fftw_complex ) * NUM_POINTS );
00091         plan_forward  = fftw_plan_dft_1d( NUM_POINTS, fft_in, fft_out, FFTW_FORWARD, FFTW_ESTIMATE );
00092 
00093         fftCurve = new QwtPlotCurve();
00094         fftCurve->setRawData(incrementalTime, myGuiPlotData.imuPitchAngle, NUM_POINTS);
00095         fftCurve->setStyle(QwtPlotCurve::Dots);
00096         ui.fft_plot->setAutoReplot(false);
00097         fftCurve->attach(ui.fft_plot);
00098         ui.fft_plot->setAxisScale(0, 0, 10000000, 1000000);
00099         ui.fft_plot->setAxisScale(2, 500, 1000, 100);
00100 
00101         ui.fft_select->insertItem(0, QString("Pitch Filtered Data"));
00102         ui.fft_select->insertItem(1, QString("Pitch Raw Gyro Data"));
00103         ui.fft_select->insertItem(2, QString("Pitch Raw Accel Data"));
00104         ui.fft_select->insertItem(3, QString("Roll Filtered Data"));
00105         ui.fft_select->insertItem(4, QString("Roll Raw Gyro Data"));
00106         ui.fft_select->insertItem(5, QString("Roll Raw Accel Data"));
00107 
00108 }
00109 
00110 TestrigGUI::~TestrigGUI() {
00111         rollGyroCurve->detach();
00112 
00113     fftw_destroy_plan( plan_forward );
00114     fftw_free( fft_in );
00115     fftw_free( fft_out );
00116 
00117         //cvReleaseImage (&curIplImage);
00118 }
00119 
00120 void TestrigGUI::emitReadyToUpdateGUI() {
00121         emit readyToUpdateGUI();
00122 }
00123 
00124 double TestrigGUI::getMean(double * data) {
00125         double mean = 0;
00126 
00127         for (int i = 0; i < NUM_POINTS; i++) {
00128                 mean += data[i];
00129         }
00130 
00131         return mean / (double) NUM_POINTS;
00132 }
00133 
00134 double TestrigGUI::getStandardDeviation(double * data) {
00135 
00136         int sdWidth = 30;
00137         double mean = 0, var = 0;
00138         int startVal = myGuiData.oldestPoint - sdWidth;
00139         if (startVal < 0)
00140                 startVal += NUM_POINTS;
00141 
00142         for (int i = startVal; i != myGuiData.oldestPoint; i++) {
00143                 mean += data[i];
00144                 if (i + 1 == NUM_POINTS)
00145                         i = -1;
00146         }
00147 
00148         mean /= (double) sdWidth;
00149 
00150         for (int i = startVal; i != myGuiData.oldestPoint; i++) {
00151                 var += (data[i] - mean)*(data[i] - mean);
00152                 if (i + 1 == NUM_POINTS)
00153                         i = -1;
00154         }
00155 
00156         var /= (double) sdWidth;
00157 
00158         return var;
00159 }
00160 
00161 
00162 void TestrigGUI::updateGUIDisplay() {
00163 
00164 
00165         //Calculate Statistics:
00166         pthread_mutex_lock(&camMutex);
00167         pthread_mutex_lock(&imuMutex);
00168         memcpy(&myGuiData, &myData, sizeof(struct CollectedData));
00169         memcpy(&guiRollFilter, &rollFilter, sizeof(struct KalmanFilterData));
00170         memcpy(&guiPitchFilter, &pitchFilter, sizeof(struct KalmanFilterData));
00171         pthread_mutex_unlock(&imuMutex);
00172         pthread_mutex_unlock(&camMutex);
00173 
00174         //Throw in the stat stuff at the bottom of the page
00175         ui.pitch_accel_sd->setText(QString::number(getStandardDeviation(myGuiData.imuPitchAccelVal), 'g', 3));
00176         ui.pitch_gyro_sd->setText(QString::number(getStandardDeviation(myGuiData.imuPitchGyroVal), 'g', 3));
00177         ui.roll_accel_sd->setText(QString::number(getStandardDeviation(myGuiData.imuRollAccelVal), 'g', 3));
00178         ui.roll_gyro_sd->setText(QString::number(getStandardDeviation(myGuiData.imuRollGyroVal), 'g', 3));
00179         ui.imu_up_per_sec->setText(QString::number( (1000.0 / getMean(myGuiData.imuDeltaT)) , 'g', 3));
00180         //camupdates
00181 
00182         //update kalman stuff if the button is unchecked
00183         if (!ui.pauseKalmanUpdates->isChecked()) {
00184 
00185                 ui.roll_accel_var->setValue(guiRollFilter.Sz*1000);
00186                 ui.roll_gyro_var->setValue(guiRollFilter.Sw_00);
00187                 ui.roll_bias_var->setValue(guiRollFilter.Sw_11);
00188 
00189                 ui.pitch_accel_var->setValue(guiPitchFilter.Sz*1000);
00190                 ui.pitch_gyro_var->setValue(guiPitchFilter.Sw_00);
00191                 ui.pitch_bias_var->setValue(guiPitchFilter.Sw_11);
00192         }
00193 
00194         ui.roll_kalman_0->setText(QString::number(guiRollFilter.K_0));
00195         ui.roll_kalman_1->setText(QString::number(guiRollFilter.K_1));
00196         ui.roll_p_00->setText(QString::number(guiRollFilter.P_00));
00197         ui.roll_p_01->setText(QString::number(guiRollFilter.P_01));
00198         ui.roll_p_10->setText(QString::number(guiRollFilter.P_10));
00199         ui.roll_p_11->setText(QString::number(guiRollFilter.P_11));
00200 
00201         ui.pitch_kalman_0->setText(QString::number(guiPitchFilter.K_0));
00202         ui.pitch_kalman_1->setText(QString::number(guiPitchFilter.K_1));
00203         ui.pitch_p_00->setText(QString::number(guiPitchFilter.P_00));
00204         ui.pitch_p_01->setText(QString::number(guiPitchFilter.P_01));
00205         ui.pitch_p_10->setText(QString::number(guiPitchFilter.P_10));
00206         ui.pitch_p_11->setText(QString::number(guiPitchFilter.P_11));
00207 
00208         int startVal = myGuiData.oldestPoint + 1;
00209         if (startVal < 0)
00210                 startVal += NUM_POINTS;
00211 
00212         switch (ui.tabWidget->currentIndex()) {
00213         //=========================================================================
00214         case 0: //graph stuffs
00215 
00216                 //rotate all of the relevant data arrays to the correct value so that we don't
00217                 // get the line from the last value to the first:
00218                 //also get the incremental time (AKA ranges from 0ms-last ms)
00219                 incrementalTime[0] = 0;
00220                 myGuiPlotData.imuPitchAccelVal[0] = myGuiData.imuPitchAccelVal[myGuiData.oldestPoint];
00221                 myGuiPlotData.imuPitchGyroVal[0] = myGuiData.imuPitchGyroVal[myGuiData.oldestPoint];
00222                 myGuiPlotData.imuPitchAngle[0] = myGuiData.imuPitchAngle[myGuiData.oldestPoint];
00223 
00224                 myGuiPlotData.imuRollAccelVal[0] = myGuiData.imuRollAccelVal[myGuiData.oldestPoint];
00225                 myGuiPlotData.imuRollGyroVal[0] = myGuiData.imuRollGyroVal[myGuiData.oldestPoint];
00226                 myGuiPlotData.imuRollAngle[0] = myGuiData.imuRollAngle[myGuiData.oldestPoint];
00227 
00228                 for (int j = 1, i = startVal; j < NUM_POINTS; i++, j++) {
00229 
00230                         myGuiPlotData.imuPitchAccelVal[j] = myGuiData.imuPitchAccelVal[i];
00231                         myGuiPlotData.imuPitchGyroVal[j] = myGuiData.imuPitchGyroVal[i];
00232                         myGuiPlotData.imuPitchAngle[j] = myGuiData.imuPitchAngle[i];
00233                         myGuiPlotData.imuRollAccelVal[j] = myGuiData.imuRollAccelVal[i];
00234                         myGuiPlotData.imuRollGyroVal[j] = myGuiData.imuRollGyroVal[i];
00235                         myGuiPlotData.imuRollAngle[j] = myGuiData.imuRollAngle[i];
00236 
00237                         incrementalTime[j] =  myGuiData.imuDeltaT[i] + incrementalTime[j - 1];
00238 
00239                         if (i + 1 == NUM_POINTS)
00240                                 i = -1;
00241                 }
00242 
00243                 if (ui.constPitchAxis->isChecked()) {
00244                         ui.pitch_cmp->setAxisScale(0, -30.0, 30.0, 5.0);
00245                 } else {
00246                         ui.pitch_cmp->setAxisAutoScale(0);
00247                 }
00248                 if (ui.constRollAxis->isChecked()) {
00249                         ui.roll_cmp->setAxisScale(0, -30.0, 30.0, 5.0);
00250                 } else {
00251                         ui.roll_cmp->setAxisAutoScale(0);
00252                 }
00253 
00254                 ui.roll_raw->replot();
00255                 ui.pitch_raw->replot();
00256                 ui.roll_cmp->replot();
00257                 ui.pitch_cmp->replot();
00258                 break;
00259         //=========================================================================
00260         case 1: //fft stuffs
00261 
00262                 double * fftDataArray;
00263 
00264                 switch (ui.fft_select->currentIndex()) {
00265                 case 0: //pitch filtered data
00266                         fftDataArray = myGuiData.imuPitchAngle;
00267                         break;
00268                 case 1: //pitch gyro data
00269                         fftDataArray = myGuiData.imuPitchGyroVal;
00270                         break;
00271                 case 2: //pitch accel data
00272                         fftDataArray = myGuiData.imuPitchAccelVal;
00273                         break;
00274                 case 3: //roll filtered data
00275                         fftDataArray = myGuiData.imuRollAngle;
00276                         break;
00277                 case 4: //roll gyro data
00278                         fftDataArray = myGuiData.imuRollGyroVal;
00279                         break;
00280                 case 5: //roll accel data
00281                         fftDataArray = myGuiData.imuRollAccelVal;
00282                         break;
00283                 default:
00284                         fftDataArray = myGuiData.imuPitchAngle;
00285                         break;
00286                 }
00287 
00288                 for (int j = 1, i = startVal; j < NUM_POINTS; i++, j++) {
00289                         fft_in[j][0] = fftDataArray[i];
00290                         fft_in[j][1] = 0;
00291 
00292                         if (i + 1 == NUM_POINTS)
00293                                 i = -1;
00294                 }
00295 
00296                 fftw_execute( plan_forward );
00297 
00298                 for (int i = 0; i < NUM_POINTS; i++) {
00299                         incrementalTime[i] = i;
00300                         myGuiPlotData.imuPitchAngle[i] = fft_out[i][0]*fft_out[i][0] + fft_out[i][1]*fft_out[i][1];
00301                 }
00302 
00303                 ui.fft_plot->replot();
00304                 break;
00305         //=========================================================================
00306         case 2: //control stuffs
00307 
00308 
00309                 break;
00310         //=========================================================================
00311         case 3: //cam stuffs
00312                 if (frameCounter != myData.frameCounter) {
00313 
00314                         pthread_mutex_lock(&camMutex);
00315                         frameCounter = myData.frameCounter;
00316                         //cvCvtColor(myData.currentCamImage, curIplImage, CV_BGR2RGB);
00317                         pthread_mutex_unlock(&camMutex);
00318 
00319                         //curQImage = QImage ((unsigned char *) curIplImage->imageData, curIplImage->width, curIplImage->height,
00320                         //              curIplImage->widthStep, QImage::Format_RGB888);
00321                         //ui.cameraLabel->setPixmap(QPixmap::fromImage(curQImage));
00322                 }
00323                 break;
00324         default:
00325                 break;
00326         }
00327 }
00328 
00329 
00330 
00331 
00332 void TestrigGUI::updateControls() {
00333 
00334 }
00335 
00336 void TestrigGUI::updatePitchFilter() {
00337         //if paused only do the following
00338         if (ui.pauseKalmanUpdates->isChecked()) {
00339 
00340                 pthread_mutex_lock(&imuMutex);
00341                 initializeKalmanFilter(&pitchFilter, ui.pitch_accel_var->value()/1000.0,
00342                                 ui.pitch_gyro_var->value(), ui.pitch_bias_var->value());
00343                 pthread_mutex_unlock(&imuMutex);
00344         }
00345 }
00346 
00347 void TestrigGUI::updateRollFilter() {
00348         //if paused only do the following
00349         if (ui.pauseKalmanUpdates->isChecked()) {
00350 
00351                 pthread_mutex_lock(&imuMutex);
00352                 initializeKalmanFilter(&rollFilter, ui.roll_accel_var->value()/1000.0,
00353                                 ui.roll_gyro_var->value(), ui.roll_bias_var->value());
00354                 pthread_mutex_unlock(&imuMutex);
00355         }
00356 }