diff --git a/ground/openpilotgcs/src/plugins/config/configgadget.qrc b/ground/openpilotgcs/src/plugins/config/configgadget.qrc index bcc33a6ac..ee2c42c41 100644 --- a/ground/openpilotgcs/src/plugins/config/configgadget.qrc +++ b/ground/openpilotgcs/src/plugins/config/configgadget.qrc @@ -36,7 +36,7 @@ images/calibration/plane-left.png images/calibration/plane-right.png images/calibration/plane-up.png - images/calibration/plane-down-rotated.png + images/calibration/plane-horizontal-rotated.png images/calibration/snow.png diff --git a/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp b/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp index a0a24570d..973b54a25 100644 --- a/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp @@ -40,6 +40,7 @@ #include #include #include +#include #include #include #include @@ -114,10 +115,11 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) : // Connect the signals // gyro zero calibration - connect(m_ui->gyroBiasStart, SIGNAL(clicked()), this, SLOT(levelCalibrationStart())); + connect(m_ui->gyroBiasStart, SIGNAL(clicked()), this, SLOT(gyroCalibrationStart())); // level calibration connect(m_ui->boardLevelStart, SIGNAL(clicked()), this, SLOT(levelCalibrationStart())); + connect(m_ui->boardLevelSavePos, SIGNAL(clicked()), this, SLOT(levelCalibrationSavePosition())); // six point calibrations connect(m_ui->sixPointsStartAccel, SIGNAL(clicked()), this, SLOT(sixPointCalibrationAccelStart())); @@ -151,29 +153,25 @@ void ConfigRevoWidget::showEvent(QShowEvent *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->calibrationVisualHelp->fitInView(m_ui->calibrationVisualHelp->scene()->sceneRect(), Qt::IgnoreAspectRatio); + m_ui->calibrationVisualHelp->fitInView(m_ui->calibrationVisualHelp->scene()->sceneRect(), Qt::KeepAspectRatioByExpanding); m_thermalCalibrationModel->init(); } void ConfigRevoWidget::resizeEvent(QResizeEvent *event) { Q_UNUSED(event); - m_ui->calibrationVisualHelp->fitInView(m_ui->calibrationVisualHelp->scene()->sceneRect(), Qt::IgnoreAspectRatio); + m_ui->calibrationVisualHelp->fitInView(m_ui->calibrationVisualHelp->scene()->sceneRect(), Qt::KeepAspectRatioByExpanding); } - -/******* Level calibration *******/ -/** - * Starts an accelerometer bias calibration. - */ -void ConfigRevoWidget::levelCalibrationStart() +/******* gyro bias zero ******/ +void ConfigRevoWidget::gyroCalibrationStart() { // Store and reset board rotation before calibration starts isBoardRotationStored = false; storeAndClearBoardRotation(); disableAllCalibrations(); - m_ui->boardLevelProgress->setValue(0); + m_ui->gyroBiasProgress->setValue(0); RevoCalibration *revoCalibration = RevoCalibration::GetInstance(getObjectManager()); Q_ASSERT(revoCalibration); @@ -190,24 +188,12 @@ void ConfigRevoWidget::levelCalibrationStart() attitudeSettings->setData(attitudeSettingsData); attitudeSettings->updated(); - accel_accum_x.clear(); - accel_accum_y.clear(); - accel_accum_z.clear(); gyro_accum_x.clear(); gyro_accum_y.clear(); gyro_accum_z.clear(); UAVObject::Metadata mdata; - /* Need to get as many accel updates as possible */ - AccelState *accelState = AccelState::GetInstance(getObjectManager()); - Q_ASSERT(accelState); - initialAccelStateMdata = accelState->getMetadata(); - mdata = initialAccelStateMdata; - UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC); - mdata.flightTelemetryUpdatePeriod = 100; - accelState->setMetadata(mdata); - GyroState *gyroState = GyroState::GetInstance(getObjectManager()); Q_ASSERT(gyroState); initialGyroStateMdata = gyroState->getMetadata(); @@ -218,31 +204,19 @@ void ConfigRevoWidget::levelCalibrationStart() // Now connect to the accels and mag updates, gather for 100 samples collectingData = true; - connect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(levelCalibrationGetSample(UAVObject *))); - connect(gyroState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(levelCalibrationGetSample(UAVObject *))); + connect(gyroState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(gyroBiasCalibrationGetSample(UAVObject *))); } /** - Updates the accel bias raw values + Updates the gyro bias raw values */ -void ConfigRevoWidget::levelCalibrationGetSample(UAVObject *obj) +void ConfigRevoWidget::gyroBiasCalibrationGetSample(UAVObject *obj) { QMutexLocker lock(&sensorsUpdateLock); Q_UNUSED(lock); switch (obj->getObjID()) { - case AccelState::OBJID: - { - AccelState *accelState = AccelState::GetInstance(getObjectManager()); - Q_ASSERT(accelState); - AccelState::DataFields accelStateData = accelState->getData(); - - accel_accum_x.append(accelStateData.x); - accel_accum_y.append(accelStateData.y); - accel_accum_z.append(accelStateData.z); - break; - } case GyroState::OBJID: { GyroState *gyroState = GyroState::GetInstance(getObjectManager()); @@ -259,19 +233,16 @@ void ConfigRevoWidget::levelCalibrationGetSample(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->boardLevelProgress->setValue(((p1 < p2) ? p1 : p2) * 100); + double p1 = (double)gyro_accum_x.size() / (double)NOISE_SAMPLES; + double p2 = (double)gyro_accum_y.size() / (double)NOISE_SAMPLES; + m_ui->gyroBiasProgress->setValue(((p1 < p2) ? p1 : p2) * 50); - if (accel_accum_x.size() >= NOISE_SAMPLES && - gyro_accum_y.size() >= NOISE_SAMPLES && + if (gyro_accum_y.size() >= 2 * NOISE_SAMPLES && collectingData == true) { collectingData = false; - AccelState *accelState = AccelState::GetInstance(getObjectManager()); GyroState *gyroState = GyroState::GetInstance(getObjectManager()); - disconnect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(levelCalibrationGetSample(UAVObject *))); disconnect(gyroState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(levelCalibrationGetSample(UAVObject *))); enableAllCalibrations(); @@ -281,16 +252,12 @@ void ConfigRevoWidget::levelCalibrationGetSample(UAVObject *obj) AccelGyroSettings *accelGyroSettings = AccelGyroSettings::GetInstance(getObjectManager()); Q_ASSERT(accelGyroSettings); - RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData(); AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData(); revoCalibrationData.BiasCorrectedRaw = RevoCalibration::BIASCORRECTEDRAW_TRUE; - // Update the biases based on collected data - accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_X] += listMean(accel_accum_x); - accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Y] += listMean(accel_accum_y); - accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Z] += (listMean(accel_accum_z) + GRAVITY); + // Update biases based on collected data accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_X] += listMean(gyro_accum_x); accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Y] += listMean(gyro_accum_y); accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Z] += listMean(gyro_accum_z); @@ -307,7 +274,6 @@ void ConfigRevoWidget::levelCalibrationGetSample(UAVObject *obj) attitudeSettings->setData(attitudeSettingsData); attitudeSettings->updated(); - accelState->setMetadata(initialAccelStateMdata); gyroState->setMetadata(initialGyroStateMdata); // Recall saved board rotation @@ -315,6 +281,138 @@ void ConfigRevoWidget::levelCalibrationGetSample(UAVObject *obj) } } + + +/******* Level calibration *******/ +/** + * Starts an accelerometer bias calibration. + */ +void ConfigRevoWidget::levelCalibrationStart() +{ + + + // Store and reset board rotation before calibration starts + + disableAllCalibrations(); + m_ui->boardLevelProgress->setValue(0); + + rot_data_pitch = 0; + rot_data_roll = 0; + UAVObject::Metadata mdata; + + AttitudeState *attitudeState = AttitudeState::GetInstance(getObjectManager()); + Q_ASSERT(attitudeState); + initialAttitudeStateMdata = attitudeState->getMetadata(); + mdata = initialAttitudeStateMdata; + UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC); + mdata.flightTelemetryUpdatePeriod = 100; + attitudeState->setMetadata(mdata); + + /* Show instructions and enable controls */ + displayInstructions("Place horizontally and click save position...", true); + displayVisualHelp("plane-horizontal"); + disableAllCalibrations(); + m_ui->boardLevelSavePos->setEnabled(true); + position = 0; + +} + +void ConfigRevoWidget::levelCalibrationSavePosition(){ + QMutexLocker lock(&sensorsUpdateLock); + Q_UNUSED(lock); + + m_ui->boardLevelSavePos->setEnabled(false); + + rot_accum_pitch.clear(); + rot_accum_roll.clear(); + + collectingData = true; + + AttitudeState *attitudeState = AttitudeState::GetInstance(getObjectManager()); + Q_ASSERT(attitudeState); + connect(attitudeState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(levelCalibrationGetSample(UAVObject *))); + + displayInstructions("Hold..."); +} + +/** + Updates the accel bias raw values + */ +void ConfigRevoWidget::levelCalibrationGetSample(UAVObject *obj) +{ + QMutexLocker lock(&sensorsUpdateLock); + Q_UNUSED(lock); + + switch (obj->getObjID()) { + case AttitudeState::OBJID: + { + AttitudeState *attitudeState = AttitudeState::GetInstance(getObjectManager()); + Q_ASSERT(attitudeState); + AttitudeState::DataFields attitudeStateData = attitudeState->getData(); + rot_accum_roll.append(attitudeStateData.Roll); + rot_accum_pitch.append(attitudeStateData.Pitch); + break; + } + default: + Q_ASSERT(0); + } + + // Work out the progress based on whichever has less + double p1 = (double)rot_accum_roll.size() / (double)NOISE_SAMPLES; + m_ui->boardLevelProgress->setValue(p1 * 100); + + if (rot_accum_roll.size() >= NOISE_SAMPLES && + collectingData == true) { + collectingData = false; + + AttitudeState *attitudeState = AttitudeState::GetInstance(getObjectManager()); + + disconnect(attitudeState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(levelCalibrationGetSample(UAVObject *))); + + position++; + switch(position){ + case 1: + rot_data_pitch = listMean(rot_accum_pitch); + rot_data_roll = listMean(rot_accum_roll); + + displayInstructions("Leave horizontally, rotate 180° along yaw axis and click save position...", true); + displayVisualHelp("plane-horizontal-rotated"); + + disableAllCalibrations(); + + m_ui->boardLevelSavePos->setEnabled(true); + break; + case 2: + rot_data_pitch += listMean(rot_accum_pitch); + rot_data_pitch /= 2; + rot_data_roll += listMean(rot_accum_roll); + rot_data_roll /= 2; + attitudeState->setMetadata(initialAttitudeStateMdata); + levelCalibrationCompute(); + enableAllCalibrations(); + break; + } + + } +} +void ConfigRevoWidget::levelCalibrationCompute(){ + enableAllCalibrations(); + + AttitudeSettings *attitudeSettings = AttitudeSettings::GetInstance(getObjectManager()); + Q_ASSERT(attitudeSettings); + AttitudeSettings::DataFields attitudeSettingsData = attitudeSettings->getData(); + + // Update the biases based on collected data + // "rotate" the board in the opposite direction as the calculated offset + attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_PITCH] -= rot_data_pitch; + attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_ROLL] -= rot_data_roll; + + attitudeSettings->setData(attitudeSettingsData); + attitudeSettings->updated(); + + +} + /********** Six point calibration **************/ void ConfigRevoWidget::sixPointCalibrationMagStart(){ @@ -423,8 +521,7 @@ void ConfigRevoWidget::sixPointCalibrationStart(bool calibrateAccel, bool calibr mag->setMetadata(mdata); /* Show instructions and enable controls */ - m_ui->sixPointCalibInstructions->clear(); - m_ui->sixPointCalibInstructions->append("Place horizontally and click save position..."); + displayInstructions("Place horizontally and click save position...", true); displayVisualHelp("plane-horizontal"); disableAllCalibrations(); m_ui->sixPointsSave->setEnabled(true); @@ -459,7 +556,7 @@ void ConfigRevoWidget::sixPointCalibrationSavePositionData() connect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(sixPointCalibrationGetSample(UAVObject *))); connect(mag, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(sixPointCalibrationGetSample(UAVObject *))); - m_ui->sixPointCalibInstructions->append("Hold..."); + displayInstructions("Hold..."); } /** @@ -517,23 +614,23 @@ void ConfigRevoWidget::sixPointCalibrationGetSample(UAVObject *obj) position = (position + 1) % 6; if (position == 1) { - m_ui->sixPointCalibInstructions->append("Place with left side down and click save position..."); + displayInstructions("Place with left side down and click save position..."); displayVisualHelp("plane-left"); } if (position == 2) { - m_ui->sixPointCalibInstructions->append("Place upside down and click save position..."); + displayInstructions("Place upside down and click save position..."); displayVisualHelp("plane-flip"); } if (position == 3) { - m_ui->sixPointCalibInstructions->append("Place with right side down and click save position..."); + displayInstructions("Place with right side down and click save position..."); displayVisualHelp("plane-right"); } if (position == 4) { - m_ui->sixPointCalibInstructions->append("Place with nose up and click save position..."); + displayInstructions("Place with nose up and click save position..."); displayVisualHelp("plane-up"); } if (position == 5) { - m_ui->sixPointCalibInstructions->append("Place with nose down and click save position..."); + displayInstructions("Place with nose down and click save position..."); displayVisualHelp("plane-down"); } if (position == 0) { @@ -695,9 +792,17 @@ void ConfigRevoWidget::displayVisualHelp(QString elementID) QPixmap pixmap = QPixmap(":/configgadget/images/calibration/" + elementID + ".png"); m_ui->calibrationVisualHelp->scene()->addPixmap(pixmap); m_ui->calibrationVisualHelp->setSceneRect(pixmap.rect()); - m_ui->calibrationVisualHelp->fitInView(m_ui->calibrationVisualHelp->scene()->sceneRect(), Qt::IgnoreAspectRatio); + m_ui->calibrationVisualHelp->fitInView(m_ui->calibrationVisualHelp->scene()->sceneRect(), Qt::KeepAspectRatioByExpanding); } +void ConfigRevoWidget::displayInstructions(QString instructions, bool replace){ + if(replace || instructions.isNull()){ + m_ui->calibrationInstructions->clear(); + } + if(!instructions.isNull()){ + m_ui->calibrationInstructions->append(instructions); + } +} /********** UI Functions *************/ diff --git a/ground/openpilotgcs/src/plugins/config/configrevowidget.h b/ground/openpilotgcs/src/plugins/config/configrevowidget.h index 6b52344f5..539a980f6 100644 --- a/ground/openpilotgcs/src/plugins/config/configrevowidget.h +++ b/ground/openpilotgcs/src/plugins/config/configrevowidget.h @@ -83,12 +83,21 @@ private: QList mag_accum_y; QList mag_accum_z; + QList rot_accum_roll; + QList rot_accum_pitch; + + + 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]; + double rot_data_roll; + double rot_data_pitch; + bool calibratingMag; bool calibratingAccel; + UAVObject::Metadata initialAttitudeStateMdata; UAVObject::Metadata initialAccelStateMdata; UAVObject::Metadata initialGyroStateMdata; UAVObject::Metadata initialMagStateMdata; @@ -96,13 +105,14 @@ private: int position; - static const int NOISE_SAMPLES = 100; + static const int NOISE_SAMPLES = 50; // Board rotation store/recall qint16 storedBoardRotation[3]; bool isBoardRotationStored; void storeAndClearBoardRotation(); void recallBoardRotation(); + void displayInstructions(QString instructions = QString(), bool replace = false); private slots: @@ -118,9 +128,14 @@ private slots: // Slots for calibrating the accel and gyro void levelCalibrationStart(); - + void levelCalibrationSavePosition(); + void levelCalibrationCompute(); void levelCalibrationGetSample(UAVObject *); + // Slots for gyro bias zero + void gyroCalibrationStart(); + void gyroBiasCalibrationGetSample(UAVObject *obj); + // Slot for clearing home location void clearHomeLocation(); diff --git a/ground/openpilotgcs/src/plugins/config/images/calibration/plane-down-rotated.png b/ground/openpilotgcs/src/plugins/config/images/calibration/plane-horizontal-rotated.png similarity index 100% rename from ground/openpilotgcs/src/plugins/config/images/calibration/plane-down-rotated.png rename to ground/openpilotgcs/src/plugins/config/images/calibration/plane-horizontal-rotated.png diff --git a/ground/openpilotgcs/src/plugins/config/revosensors.ui b/ground/openpilotgcs/src/plugins/config/revosensors.ui index 026edf5a1..bbbd3d59f 100644 --- a/ground/openpilotgcs/src/plugins/config/revosensors.ui +++ b/ground/openpilotgcs/src/plugins/config/revosensors.ui @@ -7,7 +7,7 @@ 0 0 836 - 527 + 596 @@ -62,35 +62,55 @@ 6 - - - - - - - - + + + + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + + - - - Calibration instructions - - - Qt::ScrollBarAlwaysOff - - - <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> + + + + + Calibration status + + + Qt::ScrollBarAlwaysOff + + + QAbstractScrollArea::AdjustToContents + + + + + + + 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;"> +</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 #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 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:'MS Shell Dlg 2'; 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> @@ -108,8 +128,10 @@ p, li { white-space: pre-wrap; } <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> - - + + + + @@ -227,7 +249,7 @@ p, li { white-space: pre-wrap; } - + #4*: Thermal calibration