Merged in f5soh/librepilot/LP-590_Gyro_calibration_boardleveltrim (pull request #505)

LP-590 Gyro/Accel calibration boardleveltrim

Approved-by: Julian Lilov <jdl@abv.bg>
Approved-by: Lalanne Laurent <f5soh@free.fr>
Approved-by: Philippe Renon <philippe_renon@yahoo.fr>

Author: f5soh

ground/gcs/src/plugins/config/calibration/gyrobiascalibrationmodel.cpp

@@ -69,9 +69,13 @@ void GyroBiasCalibrationModel::start()
     // Disable gyro bias correction while calibrating
     attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_FALSE;
     // Zero board rotation
-    attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_YAW]   = 0;
-    attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_ROLL]  = 0;
-    attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_PITCH] = 0;
+    attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_YAW]     = 0;
+    attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_ROLL]    = 0;
+    attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_PITCH]   = 0;
+    // Zero board level trim
+    attitudeSettingsData.BoardLevelTrim[AttitudeSettings::BOARDLEVELTRIM_ROLL]  = 0;
+    attitudeSettingsData.BoardLevelTrim[AttitudeSettings::BOARDLEVELTRIM_PITCH] = 0;
+
     attitudeSettings->setData(attitudeSettingsData);
 
     UAVObject::Metadata gyroStateMetadata = gyroState->getMetadata();

ground/gcs/src/plugins/config/configrevowidget.cpp

@@ -238,19 +238,25 @@ void ConfigRevoWidget::storeAndClearBoardRotation()
     if (!isBoardRotationStored) {
         UAVObjectUpdaterHelper updateHelper;
 
-        // Store current board rotation
+        // Store current board rotation and board level trim
         isBoardRotationStored = true;
         AttitudeSettings *attitudeSettings = AttitudeSettings::GetInstance(getObjectManager());
         Q_ASSERT(attitudeSettings);
         AttitudeSettings::DataFields data  = attitudeSettings->getData();
-        storedBoardRotation[AttitudeSettings::BOARDROTATION_YAW]   = data.BoardRotation[AttitudeSettings::BOARDROTATION_YAW];
-        storedBoardRotation[AttitudeSettings::BOARDROTATION_ROLL]  = data.BoardRotation[AttitudeSettings::BOARDROTATION_ROLL];
-        storedBoardRotation[AttitudeSettings::BOARDROTATION_PITCH] = data.BoardRotation[AttitudeSettings::BOARDROTATION_PITCH];
+        storedBoardRotation[AttitudeSettings::BOARDROTATION_YAW]     = data.BoardRotation[AttitudeSettings::BOARDROTATION_YAW];
+        storedBoardRotation[AttitudeSettings::BOARDROTATION_ROLL]    = data.BoardRotation[AttitudeSettings::BOARDROTATION_ROLL];
+        storedBoardRotation[AttitudeSettings::BOARDROTATION_PITCH]   = data.BoardRotation[AttitudeSettings::BOARDROTATION_PITCH];
+        storedBoardLevelTrim[AttitudeSettings::BOARDLEVELTRIM_ROLL]  = data.BoardLevelTrim[AttitudeSettings::BOARDLEVELTRIM_ROLL];
+        storedBoardLevelTrim[AttitudeSettings::BOARDLEVELTRIM_PITCH] = data.BoardLevelTrim[AttitudeSettings::BOARDLEVELTRIM_PITCH];
 
-        // Set board rotation to no rotation
-        data.BoardRotation[AttitudeSettings::BOARDROTATION_YAW]    = 0;
-        data.BoardRotation[AttitudeSettings::BOARDROTATION_ROLL]   = 0;
-        data.BoardRotation[AttitudeSettings::BOARDROTATION_PITCH]  = 0;
+        // Set board rotation to zero
+        data.BoardRotation[AttitudeSettings::BOARDROTATION_YAW]     = 0;
+        data.BoardRotation[AttitudeSettings::BOARDROTATION_ROLL]    = 0;
+        data.BoardRotation[AttitudeSettings::BOARDROTATION_PITCH]   = 0;
+
+        // Set board level trim to zero
+        data.BoardLevelTrim[AttitudeSettings::BOARDLEVELTRIM_ROLL]  = 0;
+        data.BoardLevelTrim[AttitudeSettings::BOARDLEVELTRIM_PITCH] = 0;
 
         attitudeSettings->setData(data, false);
         updateHelper.doObjectAndWait(attitudeSettings);
@@ -281,13 +287,15 @@ void ConfigRevoWidget::recallBoardRotation()
         // Recall current board rotation
         isBoardRotationStored = false;
 
-        // Restore the flight controller board rotation
+        // Restore the flight controller board rotation and board level trim
         AttitudeSettings *attitudeSettings = AttitudeSettings::GetInstance(getObjectManager());
         Q_ASSERT(attitudeSettings);
         AttitudeSettings::DataFields data  = attitudeSettings->getData();
-        data.BoardRotation[AttitudeSettings::BOARDROTATION_YAW]   = storedBoardRotation[AttitudeSettings::BOARDROTATION_YAW];
-        data.BoardRotation[AttitudeSettings::BOARDROTATION_ROLL]  = storedBoardRotation[AttitudeSettings::BOARDROTATION_ROLL];
-        data.BoardRotation[AttitudeSettings::BOARDROTATION_PITCH] = storedBoardRotation[AttitudeSettings::BOARDROTATION_PITCH];
+        data.BoardRotation[AttitudeSettings::BOARDROTATION_YAW]     = storedBoardRotation[AttitudeSettings::BOARDROTATION_YAW];
+        data.BoardRotation[AttitudeSettings::BOARDROTATION_ROLL]    = storedBoardRotation[AttitudeSettings::BOARDROTATION_ROLL];
+        data.BoardRotation[AttitudeSettings::BOARDROTATION_PITCH]   = storedBoardRotation[AttitudeSettings::BOARDROTATION_PITCH];
+        data.BoardLevelTrim[AttitudeSettings::BOARDLEVELTRIM_ROLL]  = storedBoardLevelTrim[AttitudeSettings::BOARDLEVELTRIM_ROLL];
+        data.BoardLevelTrim[AttitudeSettings::BOARDLEVELTRIM_PITCH] = storedBoardLevelTrim[AttitudeSettings::BOARDLEVELTRIM_PITCH];
 
         attitudeSettings->setData(data, false);
         updateHelper.doObjectAndWait(attitudeSettings);

ground/gcs/src/plugins/config/configrevowidget.h

@@ -62,7 +62,8 @@ class ConfigRevoWidget : public ConfigTaskWidget {
     Ui_RevoSensorsWidget *m_ui;
 
     // Board rotation store/recall for FC and for aux mag
-    qint16 storedBoardRotation[3];
+    float storedBoardRotation[3];
+    float storedBoardLevelTrim[2];
     qint16 auxMagStoredBoardRotation[3];
     bool isBoardRotationStored;