From 8a1495f6c3330e62ab00826e76b8ffb8dfc103a2 Mon Sep 17 00:00:00 2001
From: Don Gagne <don@thegagnes.com>
Date: Thu, 9 Jun 2016 18:27:35 -0700
Subject: [PATCH] Support mag/gyro/accel cal response

---
 src/comm/MockLink.cc | 207 +++++++++++++++++++++++++------------------
 src/comm/MockLink.h  |   1 +
 2 files changed, 121 insertions(+), 87 deletions(-)

diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc
index f25d15bee..fca9ac65d 100644
--- a/src/comm/MockLink.cc
+++ b/src/comm/MockLink.cc
@@ -28,13 +28,13 @@ QGC_LOGGING_CATEGORY(MockLinkVerboseLog, "MockLinkVerboseLog")
 
 enum PX4_CUSTOM_MAIN_MODE {
     PX4_CUSTOM_MAIN_MODE_MANUAL = 1,
-    PX4_CUSTOM_MAIN_MODE_ALTCTL,
-    PX4_CUSTOM_MAIN_MODE_POSCTL,
-    PX4_CUSTOM_MAIN_MODE_AUTO,
-    PX4_CUSTOM_MAIN_MODE_ACRO,
-    PX4_CUSTOM_MAIN_MODE_OFFBOARD,
-    PX4_CUSTOM_MAIN_MODE_STABILIZED,
-    PX4_CUSTOM_MAIN_MODE_RATTITUDE
+            PX4_CUSTOM_MAIN_MODE_ALTCTL,
+            PX4_CUSTOM_MAIN_MODE_POSCTL,
+            PX4_CUSTOM_MAIN_MODE_AUTO,
+            PX4_CUSTOM_MAIN_MODE_ACRO,
+            PX4_CUSTOM_MAIN_MODE_OFFBOARD,
+            PX4_CUSTOM_MAIN_MODE_STABILIZED,
+            PX4_CUSTOM_MAIN_MODE_RATTITUDE
 };
 
 enum PX4_CUSTOM_SUB_MODE_AUTO {
@@ -226,31 +226,31 @@ void MockLink::_loadParams(void)
 
         QVariant paramValue;
         switch (paramType) {
-            case MAV_PARAM_TYPE_REAL32:
-                paramValue = QVariant(valStr.toFloat());
-                break;
-            case MAV_PARAM_TYPE_UINT32:
-                paramValue = QVariant(valStr.toUInt());
-                break;
-            case MAV_PARAM_TYPE_INT32:
-                paramValue = QVariant(valStr.toInt());
-                break;
-            case MAV_PARAM_TYPE_UINT16:
-                paramValue = QVariant((quint16)valStr.toUInt());
-                break;
-            case MAV_PARAM_TYPE_INT16:
-                paramValue = QVariant((qint16)valStr.toInt());
-                break;
-            case MAV_PARAM_TYPE_UINT8:
-                paramValue = QVariant((quint8)valStr.toUInt());
-                break;
-            case MAV_PARAM_TYPE_INT8:
-                paramValue = QVariant((qint8)valStr.toUInt());
-                break;
-            default:
-                qCritical() << "Unknown type" << paramType;
-                paramValue = QVariant(valStr.toInt());
-                break;
+        case MAV_PARAM_TYPE_REAL32:
+            paramValue = QVariant(valStr.toFloat());
+            break;
+        case MAV_PARAM_TYPE_UINT32:
+            paramValue = QVariant(valStr.toUInt());
+            break;
+        case MAV_PARAM_TYPE_INT32:
+            paramValue = QVariant(valStr.toInt());
+            break;
+        case MAV_PARAM_TYPE_UINT16:
+            paramValue = QVariant((quint16)valStr.toUInt());
+            break;
+        case MAV_PARAM_TYPE_INT16:
+            paramValue = QVariant((qint16)valStr.toInt());
+            break;
+        case MAV_PARAM_TYPE_UINT8:
+            paramValue = QVariant((quint8)valStr.toUInt());
+            break;
+        case MAV_PARAM_TYPE_INT8:
+            paramValue = QVariant((qint8)valStr.toUInt());
+            break;
+        default:
+            qCritical() << "Unknown type" << paramType;
+            paramValue = QVariant(valStr.toInt());
+            break;
         }
 
         qCDebug(MockLinkVerboseLog) << "Loading param" << paramName << paramValue;
@@ -359,40 +359,40 @@ void MockLink::_handleIncomingMavlinkBytes(const uint8_t* bytes, int cBytes)
         }
 
         switch (msg.msgid) {
-            case MAVLINK_MSG_ID_HEARTBEAT:
-                _handleHeartBeat(msg);
-                break;
+        case MAVLINK_MSG_ID_HEARTBEAT:
+            _handleHeartBeat(msg);
+            break;
 
-            case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
-                _handleParamRequestList(msg);
-                break;
+        case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
+            _handleParamRequestList(msg);
+            break;
 
-            case MAVLINK_MSG_ID_SET_MODE:
-                _handleSetMode(msg);
-                break;
+        case MAVLINK_MSG_ID_SET_MODE:
+            _handleSetMode(msg);
+            break;
 
-            case MAVLINK_MSG_ID_PARAM_SET:
-                _handleParamSet(msg);
-                break;
+        case MAVLINK_MSG_ID_PARAM_SET:
+            _handleParamSet(msg);
+            break;
 
-            case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
-                _handleParamRequestRead(msg);
-                break;
+        case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
+            _handleParamRequestRead(msg);
+            break;
 
-            case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL:
-                _handleFTP(msg);
-                break;
+        case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL:
+            _handleFTP(msg);
+            break;
 
-            case MAVLINK_MSG_ID_COMMAND_LONG:
-                _handleCommandLong(msg);
-                break;
+        case MAVLINK_MSG_ID_COMMAND_LONG:
+            _handleCommandLong(msg);
+            break;
 
-            case MAVLINK_MSG_ID_MANUAL_CONTROL:
-                _handleManualControl(msg);
-                break;
+        case MAVLINK_MSG_ID_MANUAL_CONTROL:
+            _handleManualControl(msg);
+            break;
 
-            default:
-                break;
+        default:
+            break;
         }
     }
 }
@@ -489,7 +489,7 @@ float MockLink::_floatUnionForParam(int componentId, const QString& paramName)
 
     switch (paramType) {
     case MAV_PARAM_TYPE_REAL32:
-            valueUnion.param_float = paramVar.toFloat();
+        valueUnion.param_float = paramVar.toFloat();
         break;
 
     case MAV_PARAM_TYPE_UINT32:
@@ -747,24 +747,24 @@ void MockLink::emitRemoteControlChannelRawChanged(int channel, uint16_t raw)
                                  0,                     // time since boot, ignored
                                  18,                    // channel count
                                  chanRaw[0],            // channel raw value
-                                 chanRaw[1],            // channel raw value
-                                 chanRaw[2],            // channel raw value
-                                 chanRaw[3],            // channel raw value
-                                 chanRaw[4],            // channel raw value
-                                 chanRaw[5],            // channel raw value
-                                 chanRaw[6],            // channel raw value
-                                 chanRaw[7],            // channel raw value
-                                 chanRaw[8],            // channel raw value
-                                 chanRaw[9],            // channel raw value
-                                 chanRaw[10],           // channel raw value
-                                 chanRaw[11],           // channel raw value
-                                 chanRaw[12],           // channel raw value
-                                 chanRaw[13],           // channel raw value
-                                 chanRaw[14],           // channel raw value
-                                 chanRaw[15],           // channel raw value
-                                 chanRaw[16],           // channel raw value
-                                 chanRaw[17],           // channel raw value
-                                 0);                    // rss
+            chanRaw[1],            // channel raw value
+            chanRaw[2],            // channel raw value
+            chanRaw[3],            // channel raw value
+            chanRaw[4],            // channel raw value
+            chanRaw[5],            // channel raw value
+            chanRaw[6],            // channel raw value
+            chanRaw[7],            // channel raw value
+            chanRaw[8],            // channel raw value
+            chanRaw[9],            // channel raw value
+            chanRaw[10],           // channel raw value
+            chanRaw[11],           // channel raw value
+            chanRaw[12],           // channel raw value
+            chanRaw[13],           // channel raw value
+            chanRaw[14],           // channel raw value
+            chanRaw[15],           // channel raw value
+            chanRaw[16],           // channel raw value
+            chanRaw[17],           // channel raw value
+            0);                    // rss
     respondWithMavlinkMessage(responseMsg);
 }
 
@@ -791,6 +791,9 @@ void MockLink::_handleCommandLong(const mavlink_message_t& msg)
         commandResult = MAV_RESULT_ACCEPTED;
         break;
     case MAV_CMD_PREFLIGHT_CALIBRATION:
+        _handlePreFlightCalibration(request);
+        commandResult = MAV_RESULT_ACCEPTED;
+        break;
     case MAV_CMD_PREFLIGHT_STORAGE:
         commandResult = MAV_RESULT_ACCEPTED;
         break;
@@ -889,16 +892,16 @@ void MockLink::_sendStatusTextMessages(void)
     };
 
     static const struct StatusMessage rgMessages[] = {
-        { MAV_SEVERITY_INFO,        "#Testing audio output" },
-        { MAV_SEVERITY_EMERGENCY,   "Status text emergency" },
-        { MAV_SEVERITY_ALERT,       "Status text alert" },
-        { MAV_SEVERITY_CRITICAL,    "Status text critical" },
-        { MAV_SEVERITY_ERROR,       "Status text error" },
-        { MAV_SEVERITY_WARNING,     "Status text warning" },
-        { MAV_SEVERITY_NOTICE,      "Status text notice" },
-        { MAV_SEVERITY_INFO,        "Status text info" },
-        { MAV_SEVERITY_DEBUG,       "Status text debug" },
-    };
+    { MAV_SEVERITY_INFO,        "#Testing audio output" },
+    { MAV_SEVERITY_EMERGENCY,   "Status text emergency" },
+    { MAV_SEVERITY_ALERT,       "Status text alert" },
+    { MAV_SEVERITY_CRITICAL,    "Status text critical" },
+    { MAV_SEVERITY_ERROR,       "Status text error" },
+    { MAV_SEVERITY_WARNING,     "Status text warning" },
+    { MAV_SEVERITY_NOTICE,      "Status text notice" },
+    { MAV_SEVERITY_INFO,        "Status text info" },
+    { MAV_SEVERITY_DEBUG,       "Status text debug" },
+};
 
     for (size_t i=0; i<sizeof(rgMessages)/sizeof(rgMessages[0]); i++) {
         mavlink_message_t msg;
@@ -1053,3 +1056,33 @@ void MockLink::_sendRCChannels(void)
     respondWithMavlinkMessage(msg);
 
 }
+
+void MockLink::_handlePreFlightCalibration(const mavlink_command_long_t& request)
+{
+    const char* pCalMessage;
+    static const char* gyroCalResponse = "[cal] calibration started: 2 gyro";
+    static const char* magCalResponse = "[cal] calibration started: 2 mag";
+    static const char* accelCalResponse = "[cal] calibration started: 2 accel";
+
+    if (request.param1 == 1) {
+        // Gyro cal
+        pCalMessage = gyroCalResponse;
+    } else if (request.param2 == 1) {
+        // Mag cal
+        pCalMessage = magCalResponse;
+    } else if (request.param5 == 1) {
+        // Accel cal
+        pCalMessage = accelCalResponse;
+    } else {
+        return;
+    }
+
+    mavlink_message_t msg;
+    mavlink_msg_statustext_pack(_vehicleSystemId,
+                                _vehicleComponentId,
+                                &msg,
+                                MAV_SEVERITY_INFO,
+                                pCalMessage);
+    respondWithMavlinkMessage(msg);
+}
+
diff --git a/src/comm/MockLink.h b/src/comm/MockLink.h
index ef6ab0a7e..3c2e813bd 100644
--- a/src/comm/MockLink.h
+++ b/src/comm/MockLink.h
@@ -166,6 +166,7 @@ private:
     void _handleFTP(const mavlink_message_t& msg);
     void _handleCommandLong(const mavlink_message_t& msg);
     void _handleManualControl(const mavlink_message_t& msg);
+    void _handlePreFlightCalibration(const mavlink_command_long_t& request);
     float _floatUnionForParam(int componentId, const QString& paramName);
     void _setParamFloatUnionIntoMap(int componentId, const QString& paramName, float paramFloat);
     void _sendHomePosition(void);
-- 
2.22.0