From de6b29c6a1c998a5215885a6d7a33c9cd9238b67 Mon Sep 17 00:00:00 2001
From: Laurent Lalanne <f5soh@free.fr>
Date: Wed, 4 Apr 2018 03:14:45 +0200
Subject: [PATCH] LP-590 Store and clear BoardLevelTrim while calibrating
 accelerometers

---
 .../src/plugins/config/configrevowidget.cpp   | 32 ++++++++++++-------
 .../gcs/src/plugins/config/configrevowidget.h |  1 +
 2 files changed, 21 insertions(+), 12 deletions(-)

diff --git a/ground/gcs/src/plugins/config/configrevowidget.cpp b/ground/gcs/src/plugins/config/configrevowidget.cpp
index 2fb72481e..0a7d3ffe6 100644
--- a/ground/gcs/src/plugins/config/configrevowidget.cpp
+++ b/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);
diff --git a/ground/gcs/src/plugins/config/configrevowidget.h b/ground/gcs/src/plugins/config/configrevowidget.h
index 896cabd60..c7d1a14e0 100644
--- a/ground/gcs/src/plugins/config/configrevowidget.h
+++ b/ground/gcs/src/plugins/config/configrevowidget.h
@@ -63,6 +63,7 @@ private:
 
     // Board rotation store/recall for FC and for aux mag
     qint16 storedBoardRotation[3];
+    float storedBoardLevelTrim[2];
     qint16 auxMagStoredBoardRotation[3];
     bool isBoardRotationStored;