Replicate APM Mission quirks

src/comm/MockLink.cc

@@ -84,13 +84,14 @@ MockLink::MockLink(MockConfiguration* config)
     , _mavlinkStarted(false)
     , _mavBaseMode(MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_CUSTOM_MODE_ENABLED)
     , _mavState(MAV_STATE_STANDBY)
-    , _autopilotType(MAV_AUTOPILOT_PX4)
+    , _firmwareType(MAV_AUTOPILOT_PX4)
     , _fileServer(NULL)
     , _sendStatusText(false)
+    , _apmSendHomePositionOnEmptyList(false)
 {
     _config = config;
     if (_config) {
-        _autopilotType = config->firmwareType();
+        _firmwareType = config->firmwareType();
         _sendStatusText = config->sendStatusText();
     }
 
@@ -250,7 +251,7 @@ void MockLink::_sendHeartBeat(void)
                                _vehicleComponentId,
                                &msg,
                                MAV_TYPE_QUADROTOR,  // MAV_TYPE
-                               _autopilotType,      // MAV_AUTOPILOT
+                               _firmwareType,      // MAV_AUTOPILOT
                                _mavBaseMode,        // MAV_MODE
                                _mavCustomMode,      // custom mode
                                _mavState);          // MAV_STATE
@@ -436,7 +437,7 @@ float MockLink::_floatUnionForParam(int componentId, const QString& paramName)
 
     switch (paramType) {
         case MAV_PARAM_TYPE_INT8:
-            if (_autopilotType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
+            if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
                 valueUnion.param_float = (unsigned char)paramVar.toChar().toLatin1();
             } else {
                 valueUnion.param_int8 = (unsigned char)paramVar.toChar().toLatin1();
@@ -444,7 +445,7 @@ float MockLink::_floatUnionForParam(int componentId, const QString& paramName)
             break;
 
         case MAV_PARAM_TYPE_INT32:
-            if (_autopilotType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
+            if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
                 valueUnion.param_float = paramVar.toInt();
             } else {
                 valueUnion.param_int32 = paramVar.toInt();
@@ -452,7 +453,7 @@ float MockLink::_floatUnionForParam(int componentId, const QString& paramName)
             break;
 
         case MAV_PARAM_TYPE_UINT32:
-            if (_autopilotType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
+            if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
                 valueUnion.param_float = paramVar.toUInt();
             } else {
                 valueUnion.param_uint32 = paramVar.toUInt();

src/comm/MockLink.h

@@ -74,9 +74,16 @@ public:
 
     // MockLink methods
     int vehicleId(void) { return _vehicleSystemId; }
-    MAV_AUTOPILOT getAutopilotType(void) { return _autopilotType; }
-    void setAutopilotType(MAV_AUTOPILOT autopilot) { _autopilotType = autopilot; }
+    MAV_AUTOPILOT getFirmwareType(void) { return _firmwareType; }
+    void setFirmwareType(MAV_AUTOPILOT autopilot) { _firmwareType = autopilot; }
     void setSendStatusText(bool sendStatusText) { _sendStatusText = sendStatusText; }
+
+    /// APM stack has strange handling of the first item of the mission list. If it has no
+    /// onboard mission items, sometimes it sends back a home position in position 0 and
+    /// sometimes it doesn't. Don't ask. This option allows you to configure that behavior
+    /// for unit testing.
+    void setAPMMissionResponseMode(bool sendHomePositionOnEmptyList) { _apmSendHomePositionOnEmptyList = sendHomePositionOnEmptyList; }
+
     void emitRemoteControlChannelRawChanged(int channel, uint16_t raw);
     
     /// Sends the specified mavlink message to QGC
@@ -177,11 +184,12 @@ private:
     uint8_t     _mavState;
 
     MockConfiguration* _config;
-    MAV_AUTOPILOT _autopilotType;
+    MAV_AUTOPILOT _firmwareType;
     
     MockLinkFileServer* _fileServer;
 
     bool _sendStatusText;
+    bool _apmSendHomePositionOnEmptyList;
 
     static float _vehicleLatitude;
     static float _vehicleLongitude;

src/comm/MockLinkMissionItemHandler.cc

@@ -32,6 +32,7 @@ MockLinkMissionItemHandler::MockLinkMissionItemHandler(MockLink* mockLink)
     : _mockLink(mockLink)
     , _missionItemResponseTimer(NULL)
     , _failureMode(FailNone)
+    , _sendHomePositionOnEmptyList(false)
 {
     Q_ASSERT(mockLink);
 }
@@ -99,6 +100,11 @@ void MockLinkMissionItemHandler::_handleMissionRequestList(const mavlink_message
         mavlink_msg_mission_request_list_decode(&msg, &request);
         
         Q_ASSERT(request.target_system == _mockLink->vehicleId());
+
+        int itemCount = _missionItems.count();
+        if (itemCount == 0 && _sendHomePositionOnEmptyList) {
+            itemCount = 1;
+        }
         
         mavlink_message_t   responseMsg;
         
@@ -107,7 +113,7 @@ void MockLinkMissionItemHandler::_handleMissionRequestList(const mavlink_message
                                        &responseMsg,            // Outgoing message
                                        msg.sysid,               // Target is original sender
                                        msg.compid,              // Target is original sender
-                                       _missionItems.count());  // Number of mission items
+                                       itemCount);              // Number of mission items
         _mockLink->respondWithMavlinkMessage(responseMsg);
     } else {
         qCDebug(MockLinkMissionItemHandlerLog) << "_handleMissionRequestList not responding due to failure mode";
@@ -148,7 +154,16 @@ void MockLinkMissionItemHandler::_handleMissionRequest(const mavlink_message_t&
         } else {
             mavlink_message_t   responseMsg;
             
-            mavlink_mission_item_t item = _missionItems[request.seq];
+            mavlink_mission_item_t item;
+            if (_missionItems.count() == 0 && _sendHomePositionOnEmptyList) {
+                item.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
+                item.command = MAV_CMD_NAV_WAYPOINT;
+                item.current = false;
+                item.autocontinue = true;
+                item.param1 = item.param2 = item.param3 = item.param4 = item.x = item.y = item.z = 0;
+            } else {
+                item = _missionItems[request.seq];
+            }
             
             mavlink_msg_mission_item_pack(_mockLink->vehicleId(),
                                           MAV_COMP_ID_MISSIONPLANNER,

src/comm/MockLinkMissionItemHandler.h

@@ -88,6 +88,8 @@ public:
     /// Reset the state of the MissionItemHandler to no items, no transactions in progress.
     void reset(void) { _missionItems.clear(); }
 
+    void setSendHomePositionOnEmptyList(bool sendHomePositionOnEmptyList) { _sendHomePositionOnEmptyList = sendHomePositionOnEmptyList; }
+
 private slots:
     void _missionItemResponseTimeout(void);
 
@@ -112,6 +114,7 @@ private:
     QTimer* _missionItemResponseTimer;
     FailureMode_t   _failureMode;
     bool            _failureFirstTimeOnly;
+    bool            _sendHomePositionOnEmptyList;
 };
 
 #endif