Merge pull request #1223 from DonLakeFlyer/CalFix

Support two param sets for calibration

src/AutoPilotPlugins/PX4/SensorsComponent.cc

@@ -31,12 +31,17 @@
 // These two list must be kept in sync
 
 /// @brief Parameters which signal a change in setupComplete state
-static const char* triggerParams[] = {  "SENS_MAG_XOFF", "SENS_GYRO_XOFF", "SENS_ACC_XOFF", "SENS_DPRES_OFF", NULL };
+static const char* triggerParamsV1[] = {  "SENS_MAG_XOFF", "SENS_GYRO_XOFF", "SENS_ACC_XOFF", "SENS_DPRES_OFF", NULL };
+static const char* triggerParamsV2[] = {  "CAL_MAG0_ID", "CAL_GYRO0_ID", "CAL_ACC0_ID", "SENS_DPRES_OFF", NULL };
 
 SensorsComponent::SensorsComponent(UASInterface* uas, AutoPilotPlugin* autopilot, QObject* parent) :
     PX4Component(uas, autopilot, parent),
     _name(tr("Sensors"))
 {
+    // Determine what set of parameters are available. This is a temporary hack for now. Will need real parameter
+    // mapping in the future.
+    QVariant value;
+    _paramsV1 = _paramMgr->getParameterValue(_paramMgr->getDefaultComponentId(), "SENS_MAG_XOFF", value);
 }
 
 QString SensorsComponent::name(void) const
@@ -97,14 +102,18 @@ QString SensorsComponent::setupStateDescription(void) const
 
 const char** SensorsComponent::setupCompleteChangedTriggerList(void) const
 {
-    return triggerParams;
+    return _paramsV1 ? triggerParamsV1 : triggerParamsV2;
 }
 
 QStringList SensorsComponent::paramFilterList(void) const
 {
     QStringList list;
     
-    list << "SENS_*";
+    if (_paramsV1) {
+        list << "SENS_*";
+    } else {
+        list << "CAL_*";
+    }
     
     return list;
 }

src/AutoPilotPlugins/PX4/SensorsComponent.h

@@ -55,6 +55,7 @@ public:
 private:
     const QString   _name;
     QVariantList    _summaryItems;
+    bool            _paramsV1;
 };
 
 #endif

src/AutoPilotPlugins/PX4/SensorsComponentSummary.qml

@@ -15,7 +15,8 @@ Column {
         Text {
             horizontalAlignment: Text.AlignRight;
             width: parent.width - compass.contentWidth;
-            text: autopilot.parameters["SENS_MAG_XOFF"].value == 0.0 ? "Setup required" : "Ready"
+            property bool setupRequiredValue: autopilot.parameters["SENS_MAG_XOFF"] ? autopilot.parameters["SENS_MAG_XOFF"].value : autopilot.parameters["CAL_MAG0_ID"].value
+            text: setupRequiredValue  == 0 ? "Setup required" : "Ready"
         }
     }
 
@@ -26,7 +27,8 @@ Column {
         Text {
             horizontalAlignment: Text.AlignRight;
             width: parent.width - gyro.contentWidth;
-            text: autopilot.parameters["SENS_GYRO_XOFF"].value == 0.0 ? "Setup required" : "Ready"
+            property bool setupRequiredValue: autopilot.parameters["SENS_GYRO_XOFF"] ? autopilot.parameters["SENS_GYRO_XOFF"].value : autopilot.parameters["CAL_GYRO0_ID"].value
+            text: setupRequiredValue  == 0 ? "Setup required" : "Ready"
         }
     }
 
@@ -37,7 +39,8 @@ Column {
         Text {
             horizontalAlignment: Text.AlignRight;
             width: parent.width - accel.contentWidth;
-            text: autopilot.parameters["SENS_ACC_XOFF"].value == 0.0 ? "Setup required" : "Ready"
+            property bool setupRequiredValue: autopilot.parameters["SENS_ACC_XOFF"] ? autopilot.parameters["SENS_ACC_XOFF"].value : autopilot.parameters["CAL_ACC0_ID"].value
+            text: setupRequiredValue  == 0 ? "Setup required" : "Ready"
         }
     }
 

src/ui/px4_configuration/QGCPX4SensorCalibration.cc

@@ -142,7 +142,7 @@ void QGCPX4SensorCalibration::parameterChanged(int uas, int component, QString p
     }
 
     // Check mag calibration naively
-    if (parameterName.contains("SENS_MAG_XOFF")) {
+    if (parameterName.contains("SENS_MAG_XOFF") || parameterName.contains("CAL_MAG0_ID")) {
         float offset = value.toFloat();
         if (offset < 0.000001f && offset > -0.000001f) {
             // Must be zero, not good
@@ -153,7 +153,7 @@ void QGCPX4SensorCalibration::parameterChanged(int uas, int component, QString p
     }
 
     // Check gyro calibration naively
-    if (parameterName.contains("SENS_GYRO_XOFF")) {
+    if (parameterName.contains("SENS_GYRO_XOFF") || parameterName.contains("CAL_GYRO0_ID")) {
         float offset = value.toFloat();
         if (offset < 0.000001f && offset > -0.000001f) {
             // Must be zero, not good
@@ -164,7 +164,7 @@ void QGCPX4SensorCalibration::parameterChanged(int uas, int component, QString p
     }
 
     // Check accel calibration naively
-    if (parameterName.contains("SENS_ACC_XOFF")) {
+    if (parameterName.contains("SENS_ACC_XOFF") || parameterName.contains("CAL_ACC0_ID")) {
         float offset = value.toFloat();
         if (offset < 0.000001f && offset > -0.000001f) {
             // Must be zero, not good
@@ -351,21 +351,9 @@ void QGCPX4SensorCalibration::setActiveUAS(UASInterface* uas)
 
     updateSystemSpecs(uas->getUASID());
     
-    // If the parameters are ready, we aren't going to get paramterChanged signals. So fake them in order to make the UI work.
+    // If the parameters are ready, we aren't going to get paramterChanged signals. So re-request them in order to make the UI work.
     if (uas->getParamManager()->parametersReady()) {
-        QVariant value;
-        static const char* rgParams[] = { "SENS_BOARD_ROT", "SENS_EXT_MAG_ROT", "SENS_MAG_XOFF", "SENS_GYRO_XOFF", "SENS_ACC_XOFF", "SENS_DPRES_OFF" };
-        
-        QGCUASParamManagerInterface* paramMgr = uas->getParamManager();
-        
-        for (size_t i=0; i<sizeof(rgParams)/sizeof(rgParams[0]); i++) {
-            QVariant value;
-            
-            QList<int> compIds = paramMgr->getComponentForParam(rgParams[i]);
-            Q_ASSERT(compIds.count() == 1);
-            paramMgr->getParameterValue(compIds[0], rgParams[i], value);
-            parameterChanged(uas->getUASID(), compIds[0], rgParams[i], value);
-        }
+        _requestAllSensorParameters();
     }
 }
 
@@ -429,38 +417,7 @@ void QGCPX4SensorCalibration::handleTextMessage(int uasid, int compId, int sever
         }
 
         if (activeUAS) {
-            if (text.startsWith("accel ")) {
-                activeUAS->requestParameter(0, "SENS_ACC_XOFF");
-                activeUAS->requestParameter(0, "SENS_ACC_YOFF");
-                activeUAS->requestParameter(0, "SENS_ACC_ZOFF");
-                activeUAS->requestParameter(0, "SENS_ACC_XSCALE");
-                activeUAS->requestParameter(0, "SENS_ACC_YSCALE");
-                activeUAS->requestParameter(0, "SENS_ACC_ZSCALE");
-                activeUAS->requestParameter(0, "SENS_BOARD_ROT");
-            }
-            if (text.startsWith("gyro ")) {
-                activeUAS->requestParameter(0, "SENS_GYRO_XOFF");
-                activeUAS->requestParameter(0, "SENS_GYRO_YOFF");
-                activeUAS->requestParameter(0, "SENS_GYRO_ZOFF");
-                activeUAS->requestParameter(0, "SENS_GYRO_XSCALE");
-                activeUAS->requestParameter(0, "SENS_GYRO_YSCALE");
-                activeUAS->requestParameter(0, "SENS_GYRO_ZSCALE");
-                activeUAS->requestParameter(0, "SENS_BOARD_ROT");
-            }
-            if (text.startsWith("mag ")) {
-                activeUAS->requestParameter(0, "SENS_MAG_XOFF");
-                activeUAS->requestParameter(0, "SENS_MAG_YOFF");
-                activeUAS->requestParameter(0, "SENS_MAG_ZOFF");
-                activeUAS->requestParameter(0, "SENS_MAG_XSCALE");
-                activeUAS->requestParameter(0, "SENS_MAG_YSCALE");
-                activeUAS->requestParameter(0, "SENS_MAG_ZSCALE");
-                activeUAS->requestParameter(0, "SENS_EXT_MAG_ROT");
-            }
-
-            if (text.startsWith("dpress ")) {
-                activeUAS->requestParameter(0, "SENS_DPRES_OFF");
-                activeUAS->requestParameter(0, "SENS_DPRES_ANA");
-            }
+            _requestAllSensorParameters();
         }
     }
 
@@ -517,3 +474,34 @@ void QGCPX4SensorCalibration::contextMenuEvent(QContextMenuEvent* event)
     menu.addAction(clearAction);
     menu.exec(event->globalPos());
 }
+
+void QGCPX4SensorCalibration::_requestAllSensorParameters(void)
+{
+    static const char* rgSensorsCalParamsV1[] = {
+        "SENS_ACC_XOFF", "SENS_ACC_YOFF", "SENS_ACC_ZOFF", "SENS_ACC_XSCALE", "SENS_ACC_YSCALE", "SENS_ACC_ZSCALE",
+        "SENS_GYRO_XOFF", "SENS_GYRO_YOFF", "SENS_GYRO_ZOFF", "SENS_GYRO_XSCALE", "SENS_GYRO_YSCALE", "SENS_GYRO_ZSCALE",
+        "SENS_MAG_XOFF", "SENS_MAG_YOFF", "SENS_MAG_ZOFF", "SENS_MAG_XSCALE", "SENS_MAG_YSCALE", "SENS_MAG_ZSCALE", "SENS_EXT_MAG_ROT",
+        "SENS_DPRES_OFF", "SENS_DPRES_ANA",
+        "SENS_BOARD_ROT",
+        NULL };
+    
+    static const char* rgSensorsCalParamsV2[] = {
+        "CAL_ACC0_ID", "CAL_ACC0_XOFF", "CAL_ACC0_YOFF", "CAL_ACC0_ZOFF", "CAL_ACC0_XSCALE", "CAL_ACC0_YSCALE", "CAL_ACC0_ZSCALE",
+        "CAL_GYRO0_ID", "CAL_GYRO0_XOFF", "CAL_GYRO0_YOFF", "CAL_GYRO0_ZOFF", "CAL_GYRO0_XSCALE", "CAL_GYRO0_YSCALE", "CAL_GYRO0_ZSCALE",
+        "CAL_MAG0_ID", "CAL_MAG0_XOFF", "CAL_MAG0_YOFF", "CAL_MAG0_ZOFF", "CAL_MAG0_XSCALE", "CAL_MAG0_YSCALE", "CAL_MAG0_ZSCALE", "SENS_EXT_MAG_ROT",
+        "SENS_DPRES_OFF", "SENS_DPRES_ANA",
+        "SENS_BOARD_ROT",
+        NULL };
+
+    Q_ASSERT(activeUAS);
+    QGCUASParamManagerInterface* paramMgr = activeUAS->getParamManager();
+    
+    // Temp hack for parameter mapping
+    bool paramsV1 = paramMgr->getComponentForParam("SENS_MAG_XOFF").count();
+    static const char** prgParamList = paramsV1 ? rgSensorsCalParamsV1 : rgSensorsCalParamsV2;
+
+    for (size_t i=0; prgParamList[i] != NULL; i++) {
+        qDebug() << "Requesting" << prgParamList[i];
+        activeUAS->requestParameter(0, prgParamList[i]);
+    }
+}
\ No newline at end of file

src/ui/px4_configuration/QGCPX4SensorCalibration.h

@@ -83,6 +83,8 @@ protected:
     void updateIcons();
     
 private:
+    void _requestAllSensorParameters(void);
+    
     Ui::QGCPX4SensorCalibration *ui;
 
 };