From 7451e3d0a5b4e613ddb77edd2794e68dc808f449 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Wed, 7 May 2014 21:03:08 +0200 Subject: [PATCH 1/4] OP-975 fix some help text. --- .../calibration/levelcalibrationmodel.cpp | 4 ++-- .../calibration/sixpointcalibrationmodel.cpp | 24 +++++++++---------- 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/calibration/levelcalibrationmodel.cpp b/ground/openpilotgcs/src/plugins/config/calibration/levelcalibrationmodel.cpp index 78fa86025..fd46259de 100644 --- a/ground/openpilotgcs/src/plugins/config/calibration/levelcalibrationmodel.cpp +++ b/ground/openpilotgcs/src/plugins/config/calibration/levelcalibrationmodel.cpp @@ -62,7 +62,7 @@ void LevelCalibrationModel::start() attitudeState->setMetadata(mdata); /* Show instructions and enable controls */ - displayInstructions(tr("Place horizontally and click save position..."), true); + displayInstructions(tr("Place horizontally and click Save Position button..."), true); displayVisualHelp(CALIBRATION_HELPER_PLANE_PREFIX + CALIBRATION_HELPER_IMAGE_NED); disableAllCalibrations(); savePositionEnabledChanged(true); @@ -130,7 +130,7 @@ void LevelCalibrationModel::getSample(UAVObject *obj) rot_data_pitch = OpenPilot::CalibrationUtils::listMean(rot_accum_pitch); rot_data_roll = OpenPilot::CalibrationUtils::listMean(rot_accum_roll); - displayInstructions(tr("Leave horizontally, rotate 180° along yaw axis and click save position..."), false); + displayInstructions(tr("Leave horizontally, rotate 180° along yaw axis and click Save Position button..."), false); displayVisualHelp(CALIBRATION_HELPER_PLANE_PREFIX + CALIBRATION_HELPER_IMAGE_SWD); disableAllCalibrations(); diff --git a/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp b/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp index 5f9961e43..d4e87579b 100644 --- a/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp +++ b/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp @@ -52,31 +52,31 @@ SixPointCalibrationModel::SixPointCalibrationModel(QObject *parent) : calibrationStepsMag.clear(); calibrationStepsMag << CalibrationStep(CALIBRATION_HELPER_IMAGE_NED, - tr("Place horizontally, nose pointing north and click save position...")) + tr("Place horizontally, nose pointing north and click Save Position button...")) << CalibrationStep(CALIBRATION_HELPER_IMAGE_DWN, - tr("Place with nose down, right side west and click save position...")) + tr("Place with nose down, right side west and click Save Position button...")) << CalibrationStep(CALIBRATION_HELPER_IMAGE_WDS, - tr("Place right side down, nose west and click save position...")) + tr("Place right side down, nose west and click Save Position button...")) << CalibrationStep(CALIBRATION_HELPER_IMAGE_ENU, - tr("Place upside down, nose east and click save position...")) + tr("Place upside down, nose east and click Save Position button...")) << CalibrationStep(CALIBRATION_HELPER_IMAGE_USE, - tr("Place with nose up, left side north and click save position...")) + tr("Place with nose up, left side north and click Save Position button...")) << CalibrationStep(CALIBRATION_HELPER_IMAGE_SUW, - tr("Place with left side down, nose south and click save position...")); + tr("Place with left side down, nose south and click Save Position button...")); calibrationStepsAccelOnly.clear(); calibrationStepsAccelOnly << CalibrationStep(CALIBRATION_HELPER_IMAGE_NED, - tr("Place horizontally and click save position...")) + tr("Place horizontally and click Save Position button...")) << CalibrationStep(CALIBRATION_HELPER_IMAGE_DWN, - tr("Place with nose down and click save position...")) + tr("Place with nose down and click Save Position button...")) << CalibrationStep(CALIBRATION_HELPER_IMAGE_WDS, - tr("Place right side down and click save position...")) + tr("Place right side down and click Save Position button...")) << CalibrationStep(CALIBRATION_HELPER_IMAGE_ENU, - tr("Place upside down and click save position...")) + tr("Place upside down and click Save Position button...")) << CalibrationStep(CALIBRATION_HELPER_IMAGE_USE, - tr("Place with nose up and click save position...")) + tr("Place with nose up and click Save Position button...")) << CalibrationStep(CALIBRATION_HELPER_IMAGE_SUW, - tr("Place with left side down and click save position...")); + tr("Place with left side down and click Save Position button...")); } /********** Six point calibration **************/ From bd7b1f8d5718fba1afd5b7eabd2ba7c88f08939b Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Wed, 7 May 2014 21:07:08 +0200 Subject: [PATCH 2/4] OP-975 some visual enhancemets to UI (moving save button for level, adding spacers, showing percentage to progressbar) --- .../src/plugins/config/revosensors.ui | 89 +++++++++++-------- 1 file changed, 51 insertions(+), 38 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/revosensors.ui b/ground/openpilotgcs/src/plugins/config/revosensors.ui index bc9d4f2b0..0bc6b9717 100644 --- a/ground/openpilotgcs/src/plugins/config/revosensors.ui +++ b/ground/openpilotgcs/src/plugins/config/revosensors.ui @@ -7,7 +7,7 @@ 0 0 836 - 596 + 605 @@ -110,34 +110,34 @@ <!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:'Ubuntu'; 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:16pt; font-weight:600; font-style:italic;">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;">Following steps #1, #2 and #3 are necessary. Step #4 is optional, it may helps 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:14pt; font-weight:600; font-style:italic;">#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. </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=" 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; font-style:italic;">Note 1:</span><span style=" font-family:'Ubuntu'; font-size:11pt;"> Your </span><span style=" font-family:'Ubuntu'; font-size:11pt; font-weight:600;">HomeLocation must be set first</span><span style=" font-family:'Ubuntu'; font-size:11pt;">, 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; font-weight:600; font-style:italic;">Note 2:</span><span style=" font-family:'Ubuntu'; font-size:11pt;"> There is no need to point exactly at South/North/West/East. They are just used to easily tells the user how to point the plane/craft. </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;">You can simply assume that North is in front of you, right is East etc. and perform the calibration this way.</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:14pt; font-weight:600; font-style:italic;">#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:14pt; font-weight:600; font-style:italic;">#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:14pt; font-weight:600; font-style:italic;">#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> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:16pt; font-weight:600; font-style:italic;">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-size:11pt;">Following steps #1, #2 and #3 are necessary. Step #4 is optional, it may helps 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-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-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-size:14pt; font-weight:600; font-style:italic;">#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-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-size:11pt;">Press &quot;Calibrate Mag&quot; or &quot;Calibrate Accel&quot; to begin calibration, and follow the instructions which will be displayed here. </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-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-size:11pt;">Magnetometer calibration need to be performed inside your plane/copter to account for metal/magnetic stuffs on board.</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-size:11pt; font-weight:600; font-style:italic;">Note 1:</span><span style=" font-size:11pt;"> Your </span><span style=" font-size:11pt; font-weight:600;">HomeLocation must be set first</span><span style=" font-size:11pt;">, 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-size:11pt; font-weight:600; font-style:italic;">Note 2:</span><span style=" font-size:11pt;"> There is no need to point exactly at South/North/West/East. They are just used to easily tells the user how to point the plane/craft. </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-size:11pt;">You can simply assume that North is in front of you, right is East etc. and perform the calibration this way.</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-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-size:14pt; font-weight:600; font-style:italic;">#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-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-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-size:14pt; font-weight:600; font-style:italic;">#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-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-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-size:14pt; font-weight:600; font-style:italic;">#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-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-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-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-size:11pt;"><br /></p></td></tr></table></body></html> @@ -175,6 +175,19 @@ p, li { white-space: pre-wrap; } + + + + Qt::Horizontal + + + + 40 + 20 + + + + @@ -207,6 +220,16 @@ p, li { white-space: pre-wrap; } + + + + 0 + + + true + + + @@ -217,16 +240,6 @@ p, li { white-space: pre-wrap; } - - - - 0 - - - false - - - @@ -252,7 +265,7 @@ p, li { white-space: pre-wrap; } 0 - false + true @@ -380,7 +393,7 @@ p, li { white-space: pre-wrap; } 0 - false + true From 96904bdf503220bb915aa3df026d8d9fbf4d7a03 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Wed, 7 May 2014 22:34:27 +0200 Subject: [PATCH 3/4] OP-975 Use GyroSensor UAVO when available for gyro calibration (on revo GyroState is biased by EKF gyrobias, so it reads almost always 0) --- .../calibration/gyrobiascalibrationmodel.cpp | 70 ++++++++++++++----- .../calibration/gyrobiascalibrationmodel.h | 4 ++ .../calibration/sixpointcalibrationmodel.cpp | 49 +++++++------ .../calibration/sixpointcalibrationmodel.h | 10 +-- 4 files changed, 87 insertions(+), 46 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/calibration/gyrobiascalibrationmodel.cpp b/ground/openpilotgcs/src/plugins/config/calibration/gyrobiascalibrationmodel.cpp index 684cd9da2..1d35f0eef 100644 --- a/ground/openpilotgcs/src/plugins/config/calibration/gyrobiascalibrationmodel.cpp +++ b/ground/openpilotgcs/src/plugins/config/calibration/gyrobiascalibrationmodel.cpp @@ -29,6 +29,7 @@ #include #include "extensionsystem/pluginmanager.h" #include +#include #include #include #include "calibration/gyrobiascalibrationmodel.h" @@ -67,6 +68,7 @@ void GyroBiasCalibrationModel::start() attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_FALSE; attitudeSettings->setData(attitudeSettingsData); attitudeSettings->updated(); + displayVisualHelp(CALIBRATION_HELPER_PLANE_PREFIX + CALIBRATION_HELPER_IMAGE_NED); displayInstructions(tr("Calibrating the gyroscopes. Keep the copter/plane steady..."), true); @@ -74,19 +76,33 @@ void GyroBiasCalibrationModel::start() gyro_accum_y.clear(); gyro_accum_z.clear(); - UAVObject::Metadata mdata; + gyro_state_accum_x.clear(); + gyro_state_accum_y.clear(); + gyro_state_accum_z.clear(); + + UAVObject::Metadata metadata; GyroState *gyroState = GyroState::GetInstance(getObjectManager()); Q_ASSERT(gyroState); initialGyroStateMdata = gyroState->getMetadata(); - mdata = initialGyroStateMdata; - UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC); - mdata.flightTelemetryUpdatePeriod = 100; - gyroState->setMetadata(mdata); + metadata = initialGyroStateMdata; + UAVObject::SetFlightTelemetryUpdateMode(metadata, UAVObject::UPDATEMODE_PERIODIC); + metadata.flightTelemetryUpdatePeriod = 100; + gyroState->setMetadata(metadata); + + UAVObject::Metadata gyroSensorMetadata; + GyroSensor *gyroSensor = GyroSensor::GetInstance(getObjectManager()); + Q_ASSERT(gyroSensor); + initialGyroSensorMdata = gyroSensor->getMetadata(); + gyroSensorMetadata = initialGyroSensorMdata; + UAVObject::SetFlightTelemetryUpdateMode(gyroSensorMetadata, UAVObject::UPDATEMODE_PERIODIC); + gyroSensorMetadata.flightTelemetryUpdatePeriod = 100; + gyroSensor->setMetadata(gyroSensorMetadata); // Now connect to the accels and mag updates, gather for 100 samples collectingData = true; connect(gyroState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *))); + connect(gyroSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *))); } /** @@ -105,9 +121,20 @@ void GyroBiasCalibrationModel::getSample(UAVObject *obj) Q_ASSERT(gyroState); GyroState::DataFields gyroStateData = gyroState->getData(); - gyro_accum_x.append(gyroStateData.x); - gyro_accum_y.append(gyroStateData.y); - gyro_accum_z.append(gyroStateData.z); + gyro_state_accum_x.append(gyroStateData.x); + gyro_state_accum_y.append(gyroStateData.y); + gyro_state_accum_z.append(gyroStateData.z); + break; + } + case GyroSensor::OBJID: + { + GyroSensor *gyroSensor = GyroSensor::GetInstance(getObjectManager()); + Q_ASSERT(gyroSensor); + GyroSensor::DataFields gyroSensorData = gyroSensor->getData(); + + gyro_accum_x.append(gyroSensorData.x); + gyro_accum_y.append(gyroSensorData.y); + gyro_accum_z.append(gyroSensorData.z); break; } default: @@ -115,17 +142,21 @@ void GyroBiasCalibrationModel::getSample(UAVObject *obj) } // Work out the progress based on whichever has less - double p1 = (double)gyro_accum_x.size() / (double)LEVEL_SAMPLES; + double p1 = (double)gyro_state_accum_x.size() / (double)LEVEL_SAMPLES; double p2 = (double)gyro_accum_y.size() / (double)LEVEL_SAMPLES; - progressChanged(((p1 < p2) ? p1 : p2) * 100); + progressChanged(((p1 > p2) ? p1 : p2) * 100); - if (gyro_accum_y.size() >= LEVEL_SAMPLES && + if ((gyro_accum_y.size() >= LEVEL_SAMPLES || (gyro_accum_y.size() == 0 && gyro_state_accum_y.size() >= LEVEL_SAMPLES)) && collectingData == true) { collectingData = false; GyroState *gyroState = GyroState::GetInstance(getObjectManager()); - disconnect(gyroState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *))); + Q_ASSERT(gyroState); + + GyroSensor *gyroSensor = GyroSensor::GetInstance(getObjectManager()); + Q_ASSERT(gyroSensor); + disconnect(gyroSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *))); enableAllCalibrations(); @@ -138,11 +169,17 @@ void GyroBiasCalibrationModel::getSample(UAVObject *obj) AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData(); revoCalibrationData.BiasCorrectedRaw = RevoCalibration::BIASCORRECTEDRAW_TRUE; - // Update biases based on collected data - accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_X] += OpenPilot::CalibrationUtils::listMean(gyro_accum_x); - accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Y] += OpenPilot::CalibrationUtils::listMean(gyro_accum_y); - accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Z] += OpenPilot::CalibrationUtils::listMean(gyro_accum_z); + // check whether the board does supports gyroSensor(updates were received) + if (gyro_accum_x.count() < LEVEL_SAMPLES / 10) { + accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_X] += OpenPilot::CalibrationUtils::listMean(gyro_state_accum_x); + accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Y] += OpenPilot::CalibrationUtils::listMean(gyro_state_accum_y); + accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Z] += OpenPilot::CalibrationUtils::listMean(gyro_state_accum_z); + } else { + accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_X] += OpenPilot::CalibrationUtils::listMean(gyro_accum_x); + accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Y] += OpenPilot::CalibrationUtils::listMean(gyro_accum_y); + accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Z] += OpenPilot::CalibrationUtils::listMean(gyro_accum_z); + } revoCalibration->setData(revoCalibrationData); revoCalibration->updated(); @@ -157,6 +194,7 @@ void GyroBiasCalibrationModel::getSample(UAVObject *obj) attitudeSettings->updated(); gyroState->setMetadata(initialGyroStateMdata); + gyroSensor->setMetadata(initialGyroSensorMdata); displayInstructions(tr("Gyroscope calibration computed succesfully."), false); displayVisualHelp(CALIBRATION_HELPER_IMAGE_EMPTY); diff --git a/ground/openpilotgcs/src/plugins/config/calibration/gyrobiascalibrationmodel.h b/ground/openpilotgcs/src/plugins/config/calibration/gyrobiascalibrationmodel.h index 9afe9657d..14fa6c0eb 100644 --- a/ground/openpilotgcs/src/plugins/config/calibration/gyrobiascalibrationmodel.h +++ b/ground/openpilotgcs/src/plugins/config/calibration/gyrobiascalibrationmodel.h @@ -61,7 +61,11 @@ private: QList gyro_accum_x; QList gyro_accum_y; QList gyro_accum_z; + QList gyro_state_accum_x; + QList gyro_state_accum_y; + QList gyro_state_accum_z; UAVObject::Metadata initialGyroStateMdata; + UAVObject::Metadata initialGyroSensorMdata; UAVObjectManager *getObjectManager(); }; } diff --git a/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp b/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp index d4e87579b..444a16d05 100644 --- a/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp +++ b/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp @@ -47,36 +47,35 @@ SixPointCalibrationModel::SixPointCalibrationModel(QObject *parent) : calibratingMag(false), calibratingAccel(false), collectingData(false) - { calibrationStepsMag.clear(); calibrationStepsMag - << CalibrationStep(CALIBRATION_HELPER_IMAGE_NED, - tr("Place horizontally, nose pointing north and click Save Position button...")) - << CalibrationStep(CALIBRATION_HELPER_IMAGE_DWN, - tr("Place with nose down, right side west and click Save Position button...")) - << CalibrationStep(CALIBRATION_HELPER_IMAGE_WDS, - tr("Place right side down, nose west and click Save Position button...")) - << CalibrationStep(CALIBRATION_HELPER_IMAGE_ENU, - tr("Place upside down, nose east and click Save Position button...")) - << CalibrationStep(CALIBRATION_HELPER_IMAGE_USE, - tr("Place with nose up, left side north and click Save Position button...")) - << CalibrationStep(CALIBRATION_HELPER_IMAGE_SUW, - tr("Place with left side down, nose south and click Save Position button...")); + << CalibrationStep(CALIBRATION_HELPER_IMAGE_NED, + tr("Place horizontally, nose pointing north and click Save Position button...")) + << CalibrationStep(CALIBRATION_HELPER_IMAGE_DWN, + tr("Place with nose down, right side west and click Save Position button...")) + << CalibrationStep(CALIBRATION_HELPER_IMAGE_WDS, + tr("Place right side down, nose west and click Save Position button...")) + << CalibrationStep(CALIBRATION_HELPER_IMAGE_ENU, + tr("Place upside down, nose east and click Save Position button...")) + << CalibrationStep(CALIBRATION_HELPER_IMAGE_USE, + tr("Place with nose up, left side north and click Save Position button...")) + << CalibrationStep(CALIBRATION_HELPER_IMAGE_SUW, + tr("Place with left side down, nose south and click Save Position button...")); calibrationStepsAccelOnly.clear(); calibrationStepsAccelOnly << CalibrationStep(CALIBRATION_HELPER_IMAGE_NED, - tr("Place horizontally and click Save Position button...")) - << CalibrationStep(CALIBRATION_HELPER_IMAGE_DWN, - tr("Place with nose down and click Save Position button...")) - << CalibrationStep(CALIBRATION_HELPER_IMAGE_WDS, - tr("Place right side down and click Save Position button...")) - << CalibrationStep(CALIBRATION_HELPER_IMAGE_ENU, - tr("Place upside down and click Save Position button...")) - << CalibrationStep(CALIBRATION_HELPER_IMAGE_USE, - tr("Place with nose up and click Save Position button...")) - << CalibrationStep(CALIBRATION_HELPER_IMAGE_SUW, - tr("Place with left side down and click Save Position button...")); + tr("Place horizontally and click Save Position button...")) + << CalibrationStep(CALIBRATION_HELPER_IMAGE_DWN, + tr("Place with nose down and click Save Position button...")) + << CalibrationStep(CALIBRATION_HELPER_IMAGE_WDS, + tr("Place right side down and click Save Position button...")) + << CalibrationStep(CALIBRATION_HELPER_IMAGE_ENU, + tr("Place upside down and click Save Position button...")) + << CalibrationStep(CALIBRATION_HELPER_IMAGE_USE, + tr("Place with nose up and click Save Position button...")) + << CalibrationStep(CALIBRATION_HELPER_IMAGE_SUW, + tr("Place with left side down and click Save Position button...")); } /********** Six point calibration **************/ @@ -103,7 +102,7 @@ void SixPointCalibrationModel::start(bool calibrateAccel, bool calibrateMag) // Store and reset board rotation before calibration starts storeAndClearBoardRotation(); - if(calibrateMag){ + if (calibrateMag) { currentSteps = &calibrationStepsMag; } else { currentSteps = &calibrationStepsAccelOnly; diff --git a/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.h b/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.h index cf453af33..128ff884a 100644 --- a/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.h +++ b/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.h @@ -41,15 +41,15 @@ namespace OpenPilot { class SixPointCalibrationModel : public QObject { Q_OBJECT - class CalibrationStep{ - public: - CalibrationStep(QString newVisualHelp, QString newInstructions){ - visualHelp = newVisualHelp; + class CalibrationStep { +public: + CalibrationStep(QString newVisualHelp, QString newInstructions) + { + visualHelp = newVisualHelp; instructions = newInstructions; } QString visualHelp; QString instructions; - }; typedef struct { From 48b9edc1e0701eb161ddc41eb849eace57bd739e Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Thu, 8 May 2014 21:21:50 +0200 Subject: [PATCH 4/4] uncrustify --- flight/modules/Sensors/sensors.c | 20 +++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/flight/modules/Sensors/sensors.c b/flight/modules/Sensors/sensors.c index b21b67fd4..c1c76ee8f 100644 --- a/flight/modules/Sensors/sensors.c +++ b/flight/modules/Sensors/sensors.c @@ -84,7 +84,9 @@ AccelGyroSettingsData agcal; // These values are initialized by settings but can be updated by the attitude algorithm static float mag_bias[3] = { 0, 0, 0 }; -static float mag_transform[3][3]={{ 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1}}; +static float mag_transform[3][3] = { + { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } +}; // temp coefficient to calculate gyro bias static volatile bool gyro_temp_calibrated = false; static volatile bool accel_temp_calibrated = false; @@ -442,9 +444,9 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv) { RevoCalibrationGet(&cal); AccelGyroSettingsGet(&agcal); - mag_bias[0] = cal.mag_bias.X; - mag_bias[1] = cal.mag_bias.Y; - mag_bias[2] = cal.mag_bias.Z; + mag_bias[0] = cal.mag_bias.X; + mag_bias[1] = cal.mag_bias.Y; + mag_bias[2] = cal.mag_bias.Z; accel_temp_calibrated = (agcal.temp_calibrated_extent.max - agcal.temp_calibrated_extent.min > .1f) && (fabsf(agcal.accel_temp_coeff.X) > 1e-9f || fabsf(agcal.accel_temp_coeff.Y) > 1e-9f || fabsf(agcal.accel_temp_coeff.Z) > 1e-9f); @@ -459,16 +461,16 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv) // Indicates not to expend cycles on rotation if (fabsf(attitudeSettings.BoardRotation.Roll) < 0.00001f - && fabsf(attitudeSettings.BoardRotation.Pitch) < 0.00001f && - fabsf(attitudeSettings.BoardRotation.Yaw) <0.00001f ) { + && fabsf(attitudeSettings.BoardRotation.Pitch) < 0.00001f && + fabsf(attitudeSettings.BoardRotation.Yaw) < 0.00001f) { rotate = 0; } else { rotate = 1; } float rotationQuat[4]; - const float rpy[3] = { attitudeSettings.BoardRotation.Roll, - attitudeSettings.BoardRotation.Pitch, - attitudeSettings.BoardRotation.Yaw }; + const float rpy[3] = { attitudeSettings.BoardRotation.Roll, + attitudeSettings.BoardRotation.Pitch, + attitudeSettings.BoardRotation.Yaw }; RPY2Quaternion(rpy, rotationQuat); Quaternion2R(rotationQuat, R);