From dccb2cbc47c165e53689631b8640c3ad623be2df Mon Sep 17 00:00:00 2001
From: Don Gagne <don@thegagnes.com>
Date: Wed, 24 Jun 2015 13:30:08 -0700
Subject: [PATCH] Change mag cal to 6 orientations

---
 src/AutoPilotPlugins/PX4/SensorsComponent.qml |  6 +++---
 .../PX4/SensorsComponentController.cc         | 21 +++++++++++++++++--
 .../PX4/SensorsComponentController.h          |  8 +++++++
 3 files changed, 30 insertions(+), 5 deletions(-)

diff --git a/src/AutoPilotPlugins/PX4/SensorsComponent.qml b/src/AutoPilotPlugins/PX4/SensorsComponent.qml
index 692bc4430..0fd3a3889 100644
--- a/src/AutoPilotPlugins/PX4/SensorsComponent.qml
+++ b/src/AutoPilotPlugins/PX4/SensorsComponent.qml
@@ -436,7 +436,7 @@ QGCView {
                             visible:            controller.orientationCalUpsideDownSideVisible
                             calValid:           controller.orientationCalUpsideDownSideDone
                             calInProgress:      controller.orientationCalUpsideDownSideInProgress
-                            calInProgressText:  "Hold Still"
+                            calInProgressText:  controller.orientationCalUpsideDownSideRotate ? "Rotate" : "Hold Still"
                             imageSource:        "qrc:///qmlimages/VehicleUpsideDown.png"
                         }
                         VehicleRotationCal {
@@ -450,7 +450,7 @@ QGCView {
                             visible:            controller.orientationCalTailDownSideVisible
                             calValid:           controller.orientationCalTailDownSideDone
                             calInProgress:      controller.orientationCalTailDownSideInProgress
-                            calInProgressText:  "Hold Still"
+                            calInProgressText:  controller.orientationCalTailDownSideRotate ? "Rotate" : "Hold Still"
                             imageSource:        "qrc:///qmlimages/VehicleTailDown.png"
                         }
                         VehicleRotationCal {
@@ -464,7 +464,7 @@ QGCView {
                             visible:            controller.orientationCalRightSideVisible
                             calValid:           controller.orientationCalRightSideDone
                             calInProgress:      controller.orientationCalRightSideInProgress
-                            calInProgressText:  "Hold Still"
+                            calInProgressText:  controller.orientationCalRightSideRotate ? "Rotate" : "Hold Still"
                             imageSource:        "qrc:///qmlimages/VehicleRight.png"
                         }
                     }
diff --git a/src/AutoPilotPlugins/PX4/SensorsComponentController.cc b/src/AutoPilotPlugins/PX4/SensorsComponentController.cc
index edca633d7..a09802a41 100644
--- a/src/AutoPilotPlugins/PX4/SensorsComponentController.cc
+++ b/src/AutoPilotPlugins/PX4/SensorsComponentController.cc
@@ -64,8 +64,11 @@ SensorsComponentController::SensorsComponentController(void) :
     _orientationCalNoseDownSideInProgress(false),
     _orientationCalTailDownSideInProgress(false),
     _orientationCalDownSideRotate(false),
+    _orientationCalUpsideDownSideRotate(false),
     _orientationCalLeftSideRotate(false),
+    _orientationCalRightSideRotate(false),
     _orientationCalNoseDownSideRotate(false),
+    _orientationCalTailDownSideRotate(false),
     _unknownFirmwareVersion(false),
     _waitingForCancel(false)
 {
@@ -242,9 +245,11 @@ void SensorsComponentController::_handleUASTextMessage(int uasId, int compId, in
         
         // Split version number and cal type
         QStringList parts = text.split(" ");
-        if (parts.count() != 2 && parts[0].toInt() != 1) {
+        if (parts.count() != 2 && parts[0].toInt() != _supportedFirmwareCalVersion) {
             _unknownFirmwareVersion = true;
-            qDebug() << "Unknown cal firmware version, using log";
+            QString msg = "Unsupported calibration firmware version, using log";
+            _appendStatusLog(msg);
+            qDebug() << msg;
             return;
         }
         
@@ -287,7 +292,10 @@ void SensorsComponentController::_handleUASTextMessage(int uasId, int compId, in
             } else if (text == "mag") {
                 _magCalInProgress = true;
                 _orientationCalDownSideVisible = true;
+                _orientationCalUpsideDownSideVisible = true;
                 _orientationCalLeftSideVisible = true;
+                _orientationCalRightSideVisible = true;
+                _orientationCalTailDownSideVisible = true;
                 _orientationCalNoseDownSideVisible = true;
             } else if (text == "gyro") {
                 _gyroCalInProgress = true;
@@ -314,6 +322,9 @@ void SensorsComponentController::_handleUASTextMessage(int uasId, int compId, in
             }
         } else if (side == "up") {
             _orientationCalUpsideDownSideInProgress = true;
+            if (_magCalInProgress) {
+                _orientationCalUpsideDownSideRotate = true;
+            }
         } else if (side == "left") {
             _orientationCalLeftSideInProgress = true;
             if (_magCalInProgress) {
@@ -321,6 +332,9 @@ void SensorsComponentController::_handleUASTextMessage(int uasId, int compId, in
             }
         } else if (side == "right") {
             _orientationCalRightSideInProgress = true;
+            if (_magCalInProgress) {
+                _orientationCalRightSideRotate = true;
+            }
         } else if (side == "front") {
             _orientationCalNoseDownSideInProgress = true;
             if (_magCalInProgress) {
@@ -328,6 +342,9 @@ void SensorsComponentController::_handleUASTextMessage(int uasId, int compId, in
             }
         } else if (side == "back") {
             _orientationCalTailDownSideInProgress = true;
+            if (_magCalInProgress) {
+                _orientationCalTailDownSideRotate = true;
+            }
         }
         
         if (_magCalInProgress) {
diff --git a/src/AutoPilotPlugins/PX4/SensorsComponentController.h b/src/AutoPilotPlugins/PX4/SensorsComponentController.h
index fb5648836..be4021eef 100644
--- a/src/AutoPilotPlugins/PX4/SensorsComponentController.h
+++ b/src/AutoPilotPlugins/PX4/SensorsComponentController.h
@@ -78,8 +78,11 @@ public:
     Q_PROPERTY(bool orientationCalTailDownSideInProgress MEMBER _orientationCalTailDownSideInProgress NOTIFY orientationCalSidesInProgressChanged)
     
     Q_PROPERTY(bool orientationCalDownSideRotate MEMBER _orientationCalDownSideRotate NOTIFY orientationCalSidesRotateChanged)
+    Q_PROPERTY(bool orientationCalUpsideDownSideRotate MEMBER _orientationCalUpsideDownSideRotate NOTIFY orientationCalSidesRotateChanged)
     Q_PROPERTY(bool orientationCalLeftSideRotate MEMBER _orientationCalLeftSideRotate NOTIFY orientationCalSidesRotateChanged)
+    Q_PROPERTY(bool orientationCalRightSideRotate MEMBER _orientationCalRightSideRotate NOTIFY orientationCalSidesRotateChanged)
     Q_PROPERTY(bool orientationCalNoseDownSideRotate MEMBER _orientationCalNoseDownSideRotate NOTIFY orientationCalSidesRotateChanged)
+    Q_PROPERTY(bool orientationCalTailDownSideRotate MEMBER _orientationCalTailDownSideRotate NOTIFY orientationCalSidesRotateChanged)
     
     Q_PROPERTY(bool waitingForCancel MEMBER _waitingForCancel NOTIFY waitingForCancelChanged)
     
@@ -161,11 +164,16 @@ private:
     bool _orientationCalTailDownSideInProgress;
     
     bool _orientationCalDownSideRotate;
+    bool _orientationCalUpsideDownSideRotate;
     bool _orientationCalLeftSideRotate;
+    bool _orientationCalRightSideRotate;
     bool _orientationCalNoseDownSideRotate;
+    bool _orientationCalTailDownSideRotate;
     
     bool _unknownFirmwareVersion;
     bool _waitingForCancel;
+    
+    static const int _supportedFirmwareCalVersion = 2;
 };
 
 #endif
-- 
2.22.0