Support mag/gyro/accel cal response

src/comm/MockLink.cc

@@ -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;
@@ -898,7 +901,7 @@ void MockLink::_sendStatusTextMessages(void)
     { 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);
+}
+

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);