diff --git a/ground/openpilotgcs/src/plugins/config/calibration/calibrationutils.cpp b/ground/openpilotgcs/src/plugins/config/calibration/calibrationutils.cpp index 2a47c6a82..35aa95a04 100644 --- a/ground/openpilotgcs/src/plugins/config/calibration/calibrationutils.cpp +++ b/ground/openpilotgcs/src/plugins/config/calibration/calibrationutils.cpp @@ -328,4 +328,101 @@ void CalibrationUtils::EllipsoidFit(Eigen::VectorXf *samplesX, Eigen::VectorXf * (*radii) = (evals.segment(0, 3)).cwiseInverse().cwiseSqrt(); } + +int CalibrationUtils::SixPointInConstFieldCal(double ConstMag, double x[6], double y[6], double z[6], double S[3], double b[3]) +{ + int i; + double A[5][5]; + double f[5], c[5]; + double xp, yp, zp, Sx; + + // Fill in matrix A - + // write six difference-in-magnitude equations of the form + // Sx^2(x2^2-x1^2) + 2*Sx*bx*(x2-x1) + Sy^2(y2^2-y1^2) + 2*Sy*by*(y2-y1) + Sz^2(z2^2-z1^2) + 2*Sz*bz*(z2-z1) = 0 + // or in other words + // 2*Sx*bx*(x2-x1)/Sx^2 + Sy^2(y2^2-y1^2)/Sx^2 + 2*Sy*by*(y2-y1)/Sx^2 + Sz^2(z2^2-z1^2)/Sx^2 + 2*Sz*bz*(z2-z1)/Sx^2 = (x1^2-x2^2) + for (i = 0; i < 5; i++) { + A[i][0] = 2.0 * (x[i + 1] - x[i]); + A[i][1] = y[i + 1] * y[i + 1] - y[i] * y[i]; + A[i][2] = 2.0 * (y[i + 1] - y[i]); + A[i][3] = z[i + 1] * z[i + 1] - z[i] * z[i]; + A[i][4] = 2.0 * (z[i + 1] - z[i]); + f[i] = x[i] * x[i] - x[i + 1] * x[i + 1]; + } + + // solve for c0=bx/Sx, c1=Sy^2/Sx^2; c2=Sy*by/Sx^2, c3=Sz^2/Sx^2, c4=Sz*bz/Sx^2 + if (!LinearEquationsSolve(5, (double *)A, f, c)) { + return 0; + } + + // use one magnitude equation and c's to find Sx - doesn't matter which - all give the same answer + xp = x[0]; yp = y[0]; zp = z[0]; + Sx = sqrt(ConstMag * ConstMag / (xp * xp + 2 * c[0] * xp + c[0] * c[0] + c[1] * yp * yp + 2 * c[2] * yp + c[2] * c[2] / c[1] + c[3] * zp * zp + 2 * c[4] * zp + c[4] * c[4] / c[3])); + + S[0] = Sx; + b[0] = Sx * c[0]; + S[1] = sqrt(c[1] * Sx * Sx); + b[1] = c[2] * Sx * Sx / S[1]; + S[2] = sqrt(c[3] * Sx * Sx); + b[2] = c[4] * Sx * Sx / S[2]; + + return 1; } + + +int CalibrationUtils::LinearEquationsSolve(int nDim, double *pfMatr, double *pfVect, double *pfSolution) +{ + double fMaxElem; + double fAcc; + + int i, j, k, m; + + for (k = 0; k < (nDim - 1); k++) { // base row of matrix + // search of line with max element + fMaxElem = fabs(pfMatr[k * nDim + k]); + m = k; + for (i = k + 1; i < nDim; i++) { + if (fMaxElem < fabs(pfMatr[i * nDim + k])) { + fMaxElem = pfMatr[i * nDim + k]; + m = i; + } + } + + // permutation of base line (index k) and max element line(index m) + if (m != k) { + for (i = k; i < nDim; i++) { + fAcc = pfMatr[k * nDim + i]; + pfMatr[k * nDim + i] = pfMatr[m * nDim + i]; + pfMatr[m * nDim + i] = fAcc; + } + fAcc = pfVect[k]; + pfVect[k] = pfVect[m]; + pfVect[m] = fAcc; + } + + if (pfMatr[k * nDim + k] == 0.) { + return 0; // needs improvement !!! + } + // triangulation of matrix with coefficients + for (j = (k + 1); j < nDim; j++) { // current row of matrix + fAcc = -pfMatr[j * nDim + k] / pfMatr[k * nDim + k]; + for (i = k; i < nDim; i++) { + pfMatr[j * nDim + i] = pfMatr[j * nDim + i] + fAcc * pfMatr[k * nDim + i]; + } + pfVect[j] = pfVect[j] + fAcc * pfVect[k]; // free member recalculation + } + } + + for (k = (nDim - 1); k >= 0; k--) { + pfSolution[k] = pfVect[k]; + for (i = (k + 1); i < nDim; i++) { + pfSolution[k] -= (pfMatr[k * nDim + i] * pfSolution[i]); + } + pfSolution[k] = pfSolution[k] / pfMatr[k * nDim + k]; + } + + return 1; +} +} + + diff --git a/ground/openpilotgcs/src/plugins/config/calibration/calibrationutils.h b/ground/openpilotgcs/src/plugins/config/calibration/calibrationutils.h index 371e41871..83758b2a8 100644 --- a/ground/openpilotgcs/src/plugins/config/calibration/calibrationutils.h +++ b/ground/openpilotgcs/src/plugins/config/calibration/calibrationutils.h @@ -47,11 +47,15 @@ public: static void ComputePoly(Eigen::VectorXf *samplesX, Eigen::VectorXf *polynomial, Eigen::VectorXf *polyY); static float ComputeSigma(Eigen::VectorXf *samplesY); + static int SixPointInConstFieldCal(double ConstMag, double x[6], double y[6], double z[6], double S[3], double b[3]); + private: static void EllipsoidFit(Eigen::VectorXf *samplesX, Eigen::VectorXf *samplesY, Eigen::VectorXf *samplesZ, Eigen::Vector3f *center, Eigen::VectorXf *radii, Eigen::MatrixXf *evecs); + + static int LinearEquationsSolve(int nDim, double *pfMatr, double *pfVect, double *pfSolution); }; } #endif // CALIBRATIONUTILS_H diff --git a/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp b/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp index d0c36a934..fd10b9c27 100644 --- a/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp @@ -75,17 +75,16 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) : ConfigTaskWidget(parent), m_ui(new Ui_RevoSensorsWidget()), collectingData(false), + calibratingMag(false), + calibratingAccel(false), position(-1), isBoardRotationStored(false) { m_ui->setupUi(this); // Initialization of the Paper plane widget - m_ui->sixPointsHelp->setScene(new QGraphicsScene(this)); - displayPlane(m_ui->sixPointsHelp, "snow"); - - m_ui->levelingHelp->setScene(new QGraphicsScene(this)); - displayPlane(m_ui->levelingHelp, "snow"); + m_ui->calibrationVisualHelp->setScene(new QGraphicsScene(this)); + displayVisualHelp("snow"); // Must set up the UI (above) before setting up the UAVO mappings or refreshWidgetValues // will be dealing with some null pointers addUAVObject("RevoCalibration"); @@ -114,10 +113,16 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) : // note: init for m_thermalCalibrationModel is done in showEvent to prevent cases wiht "Start" button not enabled due to some itming issue. // Connect the signals - connect(m_ui->accelBiasStart, SIGNAL(clicked()), this, SLOT(doStartAccelGyroBiasCalibration())); - connect(m_ui->sixPointsStartAccel, SIGNAL(clicked()), this, SLOT(doStartSixPointCalibrationAccel())); - connect(m_ui->sixPointsStartMag, SIGNAL(clicked()), this, SLOT(doStartSixPointCalibrationMag())); - connect(m_ui->sixPointsSave, SIGNAL(clicked()), this, SLOT(savePositionData())); + // gyro zero calibration + connect(m_ui->gyroBiasStart, SIGNAL(clicked()), this, SLOT(levelCalibrationStart())); + + // level calibration + connect(m_ui->boardLevelStart, SIGNAL(clicked()), this, SLOT(levelCalibrationStart())); + + // six point calibrations + connect(m_ui->sixPointsStartAccel, SIGNAL(clicked()), this, SLOT(sixPointCalibrationAccelStart())); + connect(m_ui->sixPointsStartMag, SIGNAL(clicked()), this, SLOT(sixPointCalibrationMagStart())); + connect(m_ui->sixPointsSave, SIGNAL(clicked()), this, SLOT(sixPointCalibrationSavePositionData())); connect(m_ui->hlClearButton, SIGNAL(clicked()), this, SLOT(clearHomeLocation())); @@ -131,6 +136,7 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) : populateWidgets(); refreshWidgetsValues(); m_ui->tabWidget->setCurrentIndex(0); + enableAllCalibrations(); } ConfigRevoWidget::~ConfigRevoWidget() @@ -141,33 +147,33 @@ ConfigRevoWidget::~ConfigRevoWidget() void ConfigRevoWidget::showEvent(QShowEvent *event) { - Q_UNUSED(event) + Q_UNUSED(event); // Thit fitInView method should only be called now, once the // widget is shown, otherwise it cannot compute its values and // the result is usually a sensorsBargraph that is way too small. - m_ui->sixPointsHelp->fitInView(m_ui->sixPointsHelp->scene()->sceneRect(), Qt::IgnoreAspectRatio); - m_ui->levelingHelp->fitInView(m_ui->levelingHelp->scene()->sceneRect(), Qt::IgnoreAspectRatio); + m_ui->calibrationVisualHelp->fitInView(m_ui->calibrationVisualHelp->scene()->sceneRect(), Qt::IgnoreAspectRatio); m_thermalCalibrationModel->init(); } void ConfigRevoWidget::resizeEvent(QResizeEvent *event) { - Q_UNUSED(event) - m_ui->sixPointsHelp->fitInView(m_ui->sixPointsHelp->scene()->sceneRect(), Qt::IgnoreAspectRatio); - m_ui->levelingHelp->fitInView(m_ui->levelingHelp->scene()->sceneRect(), Qt::IgnoreAspectRatio); + Q_UNUSED(event); + m_ui->calibrationVisualHelp->fitInView(m_ui->calibrationVisualHelp->scene()->sceneRect(), Qt::IgnoreAspectRatio); } + +/******* Level calibration *******/ /** * Starts an accelerometer bias calibration. */ -void ConfigRevoWidget::doStartAccelGyroBiasCalibration() +void ConfigRevoWidget::levelCalibrationStart() { // Store and reset board rotation before calibration starts isBoardRotationStored = false; storeAndClearBoardRotation(); - m_ui->accelBiasStart->setEnabled(false); - m_ui->accelBiasProgress->setValue(0); + disableAllCalibrations(); + m_ui->boardLevelProgress->setValue(0); RevoCalibration *revoCalibration = RevoCalibration::GetInstance(getObjectManager()); Q_ASSERT(revoCalibration); @@ -212,14 +218,14 @@ void ConfigRevoWidget::doStartAccelGyroBiasCalibration() // Now connect to the accels and mag updates, gather for 100 samples collectingData = true; - connect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetAccelGyroBiasData(UAVObject *))); - connect(gyroState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetAccelGyroBiasData(UAVObject *))); + connect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(levelCalibrationGetSample(UAVObject *))); + connect(gyroState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(levelCalibrationGetSample(UAVObject *))); } /** Updates the accel bias raw values */ -void ConfigRevoWidget::doGetAccelGyroBiasData(UAVObject *obj) +void ConfigRevoWidget::levelCalibrationGetSample(UAVObject *obj) { QMutexLocker lock(&sensorsUpdateLock); @@ -255,7 +261,7 @@ void ConfigRevoWidget::doGetAccelGyroBiasData(UAVObject *obj) // Work out the progress based on whichever has less double p1 = (double)accel_accum_x.size() / (double)NOISE_SAMPLES; double p2 = (double)accel_accum_y.size() / (double)NOISE_SAMPLES; - m_ui->accelBiasProgress->setValue(((p1 < p2) ? p1 : p2) * 100); + m_ui->boardLevelProgress->setValue(((p1 < p2) ? p1 : p2) * 100); if (accel_accum_x.size() >= NOISE_SAMPLES && gyro_accum_y.size() >= NOISE_SAMPLES && @@ -265,10 +271,10 @@ void ConfigRevoWidget::doGetAccelGyroBiasData(UAVObject *obj) AccelState *accelState = AccelState::GetInstance(getObjectManager()); GyroState *gyroState = GyroState::GetInstance(getObjectManager()); - disconnect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetAccelGyroBiasData(UAVObject *))); - disconnect(gyroState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetAccelGyroBiasData(UAVObject *))); + disconnect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(levelCalibrationGetSample(UAVObject *))); + disconnect(gyroState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(levelCalibrationGetSample(UAVObject *))); - m_ui->accelBiasStart->setEnabled(true); + enableAllCalibrations(); RevoCalibration *revoCalibration = RevoCalibration::GetInstance(getObjectManager()); Q_ASSERT(revoCalibration); @@ -309,109 +315,13 @@ void ConfigRevoWidget::doGetAccelGyroBiasData(UAVObject *obj) } } +/********** Six point calibration **************/ -int LinearEquationsSolving(int nDim, double *pfMatr, double *pfVect, double *pfSolution) -{ - double fMaxElem; - double fAcc; - - int i, j, k, m; - - for (k = 0; k < (nDim - 1); k++) { // base row of matrix - // search of line with max element - fMaxElem = fabs(pfMatr[k * nDim + k]); - m = k; - for (i = k + 1; i < nDim; i++) { - if (fMaxElem < fabs(pfMatr[i * nDim + k])) { - fMaxElem = pfMatr[i * nDim + k]; - m = i; - } - } - - // permutation of base line (index k) and max element line(index m) - if (m != k) { - for (i = k; i < nDim; i++) { - fAcc = pfMatr[k * nDim + i]; - pfMatr[k * nDim + i] = pfMatr[m * nDim + i]; - pfMatr[m * nDim + i] = fAcc; - } - fAcc = pfVect[k]; - pfVect[k] = pfVect[m]; - pfVect[m] = fAcc; - } - - if (pfMatr[k * nDim + k] == 0.) { - return 0; // needs improvement !!! - } - // triangulation of matrix with coefficients - for (j = (k + 1); j < nDim; j++) { // current row of matrix - fAcc = -pfMatr[j * nDim + k] / pfMatr[k * nDim + k]; - for (i = k; i < nDim; i++) { - pfMatr[j * nDim + i] = pfMatr[j * nDim + i] + fAcc * pfMatr[k * nDim + i]; - } - pfVect[j] = pfVect[j] + fAcc * pfVect[k]; // free member recalculation - } - } - - for (k = (nDim - 1); k >= 0; k--) { - pfSolution[k] = pfVect[k]; - for (i = (k + 1); i < nDim; i++) { - pfSolution[k] -= (pfMatr[k * nDim + i] * pfSolution[i]); - } - pfSolution[k] = pfSolution[k] / pfMatr[k * nDim + k]; - } - - return 1; +void ConfigRevoWidget::sixPointCalibrationMagStart(){ + sixPointCalibrationStart(false, true); } - - -int SixPointInConstFieldCal(double ConstMag, double x[6], double y[6], double z[6], double S[3], double b[3]) -{ - int i; - double A[5][5]; - double f[5], c[5]; - double xp, yp, zp, Sx; - - // Fill in matrix A - - // write six difference-in-magnitude equations of the form - // Sx^2(x2^2-x1^2) + 2*Sx*bx*(x2-x1) + Sy^2(y2^2-y1^2) + 2*Sy*by*(y2-y1) + Sz^2(z2^2-z1^2) + 2*Sz*bz*(z2-z1) = 0 - // or in other words - // 2*Sx*bx*(x2-x1)/Sx^2 + Sy^2(y2^2-y1^2)/Sx^2 + 2*Sy*by*(y2-y1)/Sx^2 + Sz^2(z2^2-z1^2)/Sx^2 + 2*Sz*bz*(z2-z1)/Sx^2 = (x1^2-x2^2) - for (i = 0; i < 5; i++) { - A[i][0] = 2.0 * (x[i + 1] - x[i]); - A[i][1] = y[i + 1] * y[i + 1] - y[i] * y[i]; - A[i][2] = 2.0 * (y[i + 1] - y[i]); - A[i][3] = z[i + 1] * z[i + 1] - z[i] * z[i]; - A[i][4] = 2.0 * (z[i + 1] - z[i]); - f[i] = x[i] * x[i] - x[i + 1] * x[i + 1]; - } - - // solve for c0=bx/Sx, c1=Sy^2/Sx^2; c2=Sy*by/Sx^2, c3=Sz^2/Sx^2, c4=Sz*bz/Sx^2 - if (!LinearEquationsSolving(5, (double *)A, f, c)) { - return 0; - } - - // use one magnitude equation and c's to find Sx - doesn't matter which - all give the same answer - xp = x[0]; yp = y[0]; zp = z[0]; - Sx = sqrt(ConstMag * ConstMag / (xp * xp + 2 * c[0] * xp + c[0] * c[0] + c[1] * yp * yp + 2 * c[2] * yp + c[2] * c[2] / c[1] + c[3] * zp * zp + 2 * c[4] * zp + c[4] * c[4] / c[3])); - - S[0] = Sx; - b[0] = Sx * c[0]; - S[1] = sqrt(c[1] * Sx * Sx); - b[1] = c[2] * Sx * Sx / S[1]; - S[2] = sqrt(c[3] * Sx * Sx); - b[2] = c[4] * Sx * Sx / S[2]; - - return 1; -} - -/********** Functions for six point calibration **************/ - -void ConfigRevoWidget::doStartSixPointCalibrationMag(){ - doStartSixPointCalibration(false, true); -} -void ConfigRevoWidget::doStartSixPointCalibrationAccel(){ - doStartSixPointCalibration(true, false); +void ConfigRevoWidget::sixPointCalibrationAccelStart(){ + sixPointCalibrationStart(true, false); } /** @@ -420,7 +330,7 @@ void ConfigRevoWidget::doStartSixPointCalibrationAccel(){ * accel) to compute the scale and bias of this sensor based on the current * home location magnetic strength. */ -void ConfigRevoWidget::doStartSixPointCalibration(bool calibrateAccel, bool calibrateMag) +void ConfigRevoWidget::sixPointCalibrationStart(bool calibrateAccel, bool calibrateMag) { calibratingAccel = calibrateAccel; calibratingMag = calibrateMag; @@ -515,9 +425,8 @@ void ConfigRevoWidget::doStartSixPointCalibration(bool calibrateAccel, bool cali /* Show instructions and enable controls */ m_ui->sixPointCalibInstructions->clear(); m_ui->sixPointCalibInstructions->append("Place horizontally and click save position..."); - displayPlane(m_ui->sixPointsHelp, "plane-horizontal"); - m_ui->sixPointsStartAccel->setEnabled(false); - m_ui->sixPointsStartMag->setEnabled(false); + displayVisualHelp("plane-horizontal"); + disableAllCalibrations(); m_ui->sixPointsSave->setEnabled(true); position = 0; } @@ -527,7 +436,7 @@ void ConfigRevoWidget::doStartSixPointCalibration(bool calibrateAccel, bool cali * This is called when they click "save position" and starts * averaging data for this position. */ -void ConfigRevoWidget::savePositionData() +void ConfigRevoWidget::sixPointCalibrationSavePositionData() { QMutexLocker lock(&sensorsUpdateLock); @@ -547,8 +456,8 @@ void ConfigRevoWidget::savePositionData() MagState *mag = MagState::GetInstance(getObjectManager()); Q_ASSERT(mag); - connect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetSixPointCalibrationMeasurement(UAVObject *))); - connect(mag, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetSixPointCalibrationMeasurement(UAVObject *))); + connect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(sixPointCalibrationGetSample(UAVObject *))); + connect(mag, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(sixPointCalibrationGetSample(UAVObject *))); m_ui->sixPointCalibInstructions->append("Hold..."); } @@ -558,7 +467,7 @@ void ConfigRevoWidget::savePositionData() * store it for averaging. When sufficient points are collected advance * to the next position (give message to user) or compute the scale and bias */ -void ConfigRevoWidget::doGetSixPointCalibrationMeasurement(UAVObject *obj) +void ConfigRevoWidget::sixPointCalibrationGetSample(UAVObject *obj) { QMutexLocker lock(&sensorsUpdateLock); @@ -593,7 +502,7 @@ void ConfigRevoWidget::doGetSixPointCalibrationMeasurement(UAVObject *obj) // Store the mean for this position for the accel AccelState *accelState = AccelState::GetInstance(getObjectManager()); Q_ASSERT(accelState); - disconnect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetSixPointCalibrationMeasurement(UAVObject *))); + disconnect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(sixPointCalibrationGetSample(UAVObject *))); accel_data_x[position] = listMean(accel_accum_x); accel_data_y[position] = listMean(accel_accum_y); accel_data_z[position] = listMean(accel_accum_z); @@ -601,7 +510,7 @@ void ConfigRevoWidget::doGetSixPointCalibrationMeasurement(UAVObject *obj) // Store the mean for this position for the mag MagState *mag = MagState::GetInstance(getObjectManager()); Q_ASSERT(mag); - disconnect(mag, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetSixPointCalibrationMeasurement(UAVObject *))); + disconnect(mag, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(sixPointCalibrationGetSample(UAVObject *))); mag_data_x[position] = listMean(mag_accum_x); mag_data_y[position] = listMean(mag_accum_y); mag_data_z[position] = listMean(mag_accum_z); @@ -609,30 +518,30 @@ void ConfigRevoWidget::doGetSixPointCalibrationMeasurement(UAVObject *obj) position = (position + 1) % 6; if (position == 1) { m_ui->sixPointCalibInstructions->append("Place with left side down and click save position..."); - displayPlane(m_ui->sixPointsHelp, "plane-left"); + displayVisualHelp("plane-left"); } if (position == 2) { m_ui->sixPointCalibInstructions->append("Place upside down and click save position..."); - displayPlane(m_ui->sixPointsHelp, "plane-flip"); + displayVisualHelp("plane-flip"); } if (position == 3) { m_ui->sixPointCalibInstructions->append("Place with right side down and click save position..."); - displayPlane(m_ui->sixPointsHelp, "plane-right"); + displayVisualHelp("plane-right"); } if (position == 4) { m_ui->sixPointCalibInstructions->append("Place with nose up and click save position..."); - displayPlane(m_ui->sixPointsHelp, "plane-up"); + displayVisualHelp("plane-up"); } if (position == 5) { m_ui->sixPointCalibInstructions->append("Place with nose down and click save position..."); - displayPlane(m_ui->sixPointsHelp, "plane-down"); + displayVisualHelp("plane-down"); } if (position == 0) { - computeScaleBias(calibratingMag, calibratingAccel); - m_ui->sixPointsStartAccel->setEnabled(true); - m_ui->sixPointsStartMag->setEnabled(true); + sixPointCalibrationCompute(calibratingMag, calibratingAccel); m_ui->sixPointsSave->setEnabled(false); + enableAllCalibrations(); + /* Cleanup original settings */ accelState->setMetadata(initialAccelStateMdata); @@ -648,7 +557,7 @@ void ConfigRevoWidget::doGetSixPointCalibrationMeasurement(UAVObject *obj) * Computes the scale and bias for the magnetomer and (compile option) * for the accel once all the data has been collected in 6 positions. */ -void ConfigRevoWidget::computeScaleBias(bool mag, bool accel) +void ConfigRevoWidget::sixPointCalibrationCompute(bool mag, bool accel) { double S[3], b[3]; double Be_length; @@ -664,7 +573,7 @@ void ConfigRevoWidget::computeScaleBias(bool mag, bool accel) // Calibration accel if(accel) { - SixPointInConstFieldCal(homeLocationData.g_e, accel_data_x, accel_data_y, accel_data_z, S, b); + OpenPilot::CalibrationUtils::SixPointInConstFieldCal(homeLocationData.g_e, accel_data_x, accel_data_y, accel_data_z, S, b); accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_X] = fabs(S[0]); accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Y] = fabs(S[1]); accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Z] = fabs(S[2]); @@ -677,7 +586,7 @@ void ConfigRevoWidget::computeScaleBias(bool mag, bool accel) // Calibration mag if(mag){ Be_length = sqrt(pow(homeLocationData.Be[0], 2) + pow(homeLocationData.Be[1], 2) + pow(homeLocationData.Be[2], 2)); - SixPointInConstFieldCal(Be_length, mag_data_x, mag_data_y, mag_data_z, S, b); + OpenPilot::CalibrationUtils::SixPointInConstFieldCal(Be_length, mag_data_x, mag_data_y, mag_data_z, S, b); revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C0] = fabs(S[0]); revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C1] = fabs(S[1]); revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C2] = fabs(S[2]); @@ -779,15 +688,15 @@ void ConfigRevoWidget::recallBoardRotation() } /** - Rotate the paper plane + Show the selected visual aid */ -void ConfigRevoWidget::displayPlane(QGraphicsView *view, QString elementID) +void ConfigRevoWidget::displayVisualHelp(QString elementID) { - view->scene()->clear(); + m_ui->calibrationVisualHelp->scene()->clear(); QPixmap pixmap = QPixmap(":/configgadget/images/calibration/" + elementID + ".png"); - view->scene()->addPixmap(pixmap); - view->setSceneRect(pixmap.rect()); - view->fitInView(view->scene()->sceneRect(), Qt::IgnoreAspectRatio); + m_ui->calibrationVisualHelp->scene()->addPixmap(pixmap); + m_ui->calibrationVisualHelp->setSceneRect(pixmap.rect()); + m_ui->calibrationVisualHelp->fitInView(m_ui->calibrationVisualHelp->scene()->sceneRect(), Qt::IgnoreAspectRatio); } @@ -801,9 +710,7 @@ void ConfigRevoWidget::refreshWidgetsValues(UAVObject *object) { ConfigTaskWidget::refreshWidgetsValues(object); - m_ui->sixPointsStartAccel->setEnabled(true); - m_ui->sixPointsStartMag->setEnabled(true); - m_ui->accelBiasStart->setEnabled(true); + m_ui->calibInstructions->setText(QString("Press \"Start\" above to calibrate.")); m_ui->isSetCheckBox->setEnabled(false); @@ -832,3 +739,21 @@ void ConfigRevoWidget::clearHomeLocation() homeLocationData.Set = HomeLocation::SET_FALSE; homeLocation->setData(homeLocationData); } + +void ConfigRevoWidget::disableAllCalibrations() +{ + m_ui->sixPointsStartAccel->setEnabled(false); + m_ui->sixPointsStartMag->setEnabled(false); + m_ui->boardLevelStart->setEnabled(false); + m_ui->gyroBiasStart->setEnabled(false); + m_ui->ThermalBiasStart->setEnabled(false); +} + +void ConfigRevoWidget::enableAllCalibrations() +{ + m_ui->sixPointsStartAccel->setEnabled(true); + m_ui->sixPointsStartMag->setEnabled(true); + m_ui->boardLevelStart->setEnabled(true); + m_ui->gyroBiasStart->setEnabled(true); + m_ui->ThermalBiasStart->setEnabled(true); +} diff --git a/ground/openpilotgcs/src/plugins/config/configrevowidget.h b/ground/openpilotgcs/src/plugins/config/configrevowidget.h index 53bf3ce9f..6b52344f5 100644 --- a/ground/openpilotgcs/src/plugins/config/configrevowidget.h +++ b/ground/openpilotgcs/src/plugins/config/configrevowidget.h @@ -29,7 +29,6 @@ #include "ui_revosensors.h" #include "configtaskwidget.h" - #include "../uavobjectwidgetutils/configtaskwidget.h" #include "extensionsystem/pluginmanager.h" #include "uavobjectmanager.h" @@ -41,6 +40,8 @@ #include #include #include "calibration/thermal/thermalcalibrationmodel.h" +#include "calibration/calibrationutils.h" + class Ui_Widget; @@ -55,10 +56,10 @@ public: ~ConfigRevoWidget(); private: - void displayPlane(QGraphicsView *view, QString elementID); + void displayVisualHelp(QString elementID); // ! Computes the scale and bias of the mag based on collected data - void computeScaleBias(bool mag, bool accel); + void sixPointCalibrationCompute(bool mag, bool accel); SavedSettings savedSettings; @@ -85,13 +86,12 @@ private: double accel_data_x[6], accel_data_y[6], accel_data_z[6]; double mag_data_x[6], mag_data_y[6], mag_data_z[6]; - bool calibratingMag = false; - bool calibratingAccel = false; + bool calibratingMag; + bool calibratingAccel; UAVObject::Metadata initialAccelStateMdata; UAVObject::Metadata initialGyroStateMdata; UAVObject::Metadata initialMagStateMdata; - UAVObject::Metadata initialBaroSensorMdata; float initialMagCorrectionRate; int position; @@ -110,21 +110,23 @@ private slots: virtual void refreshWidgetsValues(UAVObject *object = NULL); // Slots for calibrating the mags - void doStartSixPointCalibrationMag(); - void doStartSixPointCalibrationAccel(); - void doStartSixPointCalibration(bool calibrateAccel, bool calibrateMag); - void doGetSixPointCalibrationMeasurement(UAVObject *obj); - void savePositionData(); + void sixPointCalibrationMagStart(); + void sixPointCalibrationAccelStart(); + void sixPointCalibrationStart(bool calibrateAccel, bool calibrateMag); + void sixPointCalibrationGetSample(UAVObject *obj); + void sixPointCalibrationSavePositionData(); // Slots for calibrating the accel and gyro - void doStartAccelGyroBiasCalibration(); + void levelCalibrationStart(); - - void doGetAccelGyroBiasData(UAVObject *); + void levelCalibrationGetSample(UAVObject *); // Slot for clearing home location void clearHomeLocation(); + void disableAllCalibrations(); + void enableAllCalibrations(); + protected: void showEvent(QShowEvent *event); void resizeEvent(QResizeEvent *event); diff --git a/ground/openpilotgcs/src/plugins/config/revosensors.ui b/ground/openpilotgcs/src/plugins/config/revosensors.ui index 037a834a9..026edf5a1 100644 --- a/ground/openpilotgcs/src/plugins/config/revosensors.ui +++ b/ground/openpilotgcs/src/plugins/config/revosensors.ui @@ -62,69 +62,179 @@ 6 - + - - #2: Accelerometer/Magnetometer calibration - - - - - - - - - false - - - Launch accelerometer range and bias calibration. - - - Calibrate Accel - - - - - - - false - - - Launch magnetometer range and bias calibration. - - - Calibrate Mag - - - - - - - false - - - Save settings (only enabled when calibration is running) - - - Save Position - - - - + - + + + + Calibration instructions + + + Qt::ScrollBarAlwaysOff + + + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'MS Shell Dlg 2'; font-size:8.25pt; font-weight:400; font-style:normal;"> +<table border="0" style="-qt-table-type: root; margin-top:4px; margin-bottom:4px; margin-left:4px; margin-right:4px;"> +<tr> +<td style="border: none;"> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt; font-weight:600;">Help</span></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">Step #1, #2 and #3 are necessary. Step #4 will help you achieve the best possible results.</span></p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'Ubuntu'; font-size:11pt;"><br /></p> +<p align="center" style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:8pt;"><br /></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">#1: Multi-Point calibration:</span></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">This calibration will compute the scale for Magnetometer or Accelerometer sensors. </span></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">Press &quot;Calibrate Mag&quot; or &quot;Calibrate Accel&quot; to begin calibration, and follow the instructions which will be displayed here. Note that your HomeLocation must be set first, including the local magnetic field vector (Be) and acceleration due to gravity (g_e).</span></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">For optimal calibration perform the Accel calibration with the board not mounted to the craft. in this way you can accurately level the board on your desk/table during the process. </span></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">Magnetometer calibration need to be performed inside your plane/copter to account for metal/magnetic stuffs on board.</span></p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'Ubuntu'; font-size:11pt;"><br /></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">#2: Board level calibration:</span></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">This step will ensure that board leveling is accurate. Place your airframe as horizontally as possible (use a spirit level if necessary), then press Start below and do not move your airframe at all until the end of the calibration.</span></p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'Ubuntu'; font-size:11pt;"><br /></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">#3: Gyro Bias calculation:</span></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">This step allows to calibrate the gyro measured value when the board is steady. To perform the calibration leave the board/aircraft completely steady and click start. </span></p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'Ubuntu'; font-size:11pt;"><br /></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">#4: Thermal Calibration:</span></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">The calibration will compute sensors bias variations at different temperatures while the board warms up.</span></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">This allow a certain amount of correction of those bias variations against temperature changes. It does improve both altitude hold and yaw performances.</span></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">To perform this calibration leave the board to cool down at room temperature in the coldest places available. after 15-20 minutes attach the usb connector to the board and Click the Calibrate button leaving the board steady. Wait until completed</span></p> +<p align="center" style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'Ubuntu'; font-size:11pt;"><br /></p></td></tr></table></body></html> + + + + + + + #1: Accelerometer/Magnetometer calibration + + + + + + false + + + Launch accelerometer range and bias calibration. + + + Calibrate Accel + + + + + + + false + + + Launch magnetometer range and bias calibration. + + + Calibrate Mag + + + + + + + false + + + Save settings (only enabled when calibration is running) + + + Save Position + + + + + + + + + + #2: Board level calibration + + + + + + false + + + Start + + + + + + + false + + + Save Position + + + + + + + 0 + + + false + + + + + + + + + + #3: Gyro bias calibration + + + + + + false + + + Start + + + + + + + 0 + + + false + + + + + + + - #1: Thermal calibration + #4*: Thermal calibration - + - + @@ -267,86 +377,6 @@ - - - - Six Point Calibration instructions - - - Qt::ScrollBarAlwaysOff - - - <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> -<html><head><meta name="qrichtext" content="1" /><style type="text/css"> -p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'Sans Serif'; font-size:9pt; font-weight:400; font-style:normal;"> -<table border="0" style="-qt-table-type: root; margin-top:4px; margin-bottom:4px; margin-left:4px; margin-right:4px;"> -<tr> -<td style="border: none;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt; font-weight:600;">Help</span></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">Step #2 and #3 are really necessary. Step #1 will help you achieve the best possible results.</span></p> -<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'Ubuntu'; font-size:11pt;"><br /></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">#1: Thermal Calibration:</span></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">The calibration will compute the variation of all sensors bias at different temperatures while the board warm up.</span></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">This allow a certain amount of correction of those bias variations against temperature changes. It does improve both altitude hold and yaw performances.</span></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">To perform this calibration leave the board to cool down at room temperature in the coldest places available. after 15-20 minutes attach the usb connector to the board and Click the Calibrate button leaving the board steady. Wait until completed</span></p> -<p align="center" style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'Ubuntu'; font-size:11pt;"><br /></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">#2: Multi-Point calibration:</span></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">This calibration will compute the scale for Magnetometer or Accelerometer sensors on the INS. Press &quot;Calibrate Mag&quot; or &quot;Calibrate Accel&quot; to begin calibration, and follow the instructions which will be displayed here. Note that your HomeLocation must be set first, including the local magnetic field vector (Be) and acceleration due to gravity (g_e).</span></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">For optimal calibration perform the Accel calibration with the board not mounted to the craft. in this way you can accurately level the board on your desk/table during the process. Magnetometer calibration need to be performed inside your plane/copter to account for metal/magnetic stuffs on board.</span></p> -<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'Ubuntu'; font-size:11pt;"><br /></p> -<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><br /></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">#3: Board level calibration:</span></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">This step will ensure that board leveling is accurate. Place your airframe as horizontally as possible (use a spirit level if necessary), then press Start below and do not move your airframe at all until the end of the calibration.</span></p></td></tr></table></body></html> - - - - - - - #3: Board level calibration - - - - - - - - - - - false - - - Start - - - - - - - false - - - Save Position - - - - - - - 0 - - - false - - - - - - - - @@ -395,10 +425,10 @@ p, li { white-space: pre-wrap; } - -90 + -90.000000000000000 - 90 + 90.000000000000000 @@ -477,10 +507,10 @@ margin:1px; - -180 + -180.000000000000000 - 180 + 180.000000000000000 @@ -510,10 +540,10 @@ margin:1px; - -180 + -180.000000000000000 - 180 + 180.000000000000000