Merge pull request #7365 from DonLakeFlyer/RoverFollow

ArduPilot: Add Follow mode to Rover

src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.cc

@@ -22,6 +22,7 @@ APMRoverMode::APMRoverMode(uint32_t mode, bool settable)
     enumToString.insert(STEERING,       "Steering");
     enumToString.insert(HOLD,           "Hold");
     enumToString.insert(LOITER,         "Loiter");
+    enumToString.insert(FOLLOW,         "Follow");
     enumToString.insert(SIMPLE,         "Simple");
     enumToString.insert(AUTO,           "Auto");
     enumToString.insert(RTL,            "RTL");
@@ -40,6 +41,7 @@ ArduRoverFirmwarePlugin::ArduRoverFirmwarePlugin(void)
     supportedFlightModes << APMRoverMode(APMRoverMode::STEERING     ,true);
     supportedFlightModes << APMRoverMode(APMRoverMode::HOLD         ,true);
     supportedFlightModes << APMRoverMode(APMRoverMode::LOITER       ,true);
+    supportedFlightModes << APMRoverMode(APMRoverMode::FOLLOW       ,true);
     supportedFlightModes << APMRoverMode(APMRoverMode::SIMPLE       ,true);
     supportedFlightModes << APMRoverMode(APMRoverMode::AUTO         ,true);
     supportedFlightModes << APMRoverMode(APMRoverMode::RTL          ,true);

src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h

@@ -25,6 +25,7 @@ public:
         STEERING        = 3,
         HOLD            = 4,
         LOITER          = 5,
+        FOLLOW          = 6,
         SIMPLE          = 7,
         AUTO            = 10,
         RTL             = 11,

src/QmlControls/QGroundControlQmlGlobal.cc

@@ -147,6 +147,15 @@ void QGroundControlQmlGlobal::startAPMArduSubMockLink(bool sendStatusText)
 #endif
 }
 
+void QGroundControlQmlGlobal::startAPMArduRoverMockLink(bool sendStatusText)
+{
+#ifdef QT_DEBUG
+    MockLink::startAPMArduRoverMockLink(sendStatusText);
+#else
+    Q_UNUSED(sendStatusText);
+#endif
+}
+
 void QGroundControlQmlGlobal::stopOneMockLink(void)
 {
 #ifdef QT_DEBUG

src/QmlControls/QGroundControlQmlGlobal.h

@@ -121,6 +121,7 @@ public:
     Q_INVOKABLE void    startAPMArduCopterMockLink  (bool sendStatusText);
     Q_INVOKABLE void    startAPMArduPlaneMockLink   (bool sendStatusText);
     Q_INVOKABLE void    startAPMArduSubMockLink     (bool sendStatusText);
+    Q_INVOKABLE void    startAPMArduRoverMockLink   (bool sendStatusText);
     Q_INVOKABLE void    stopOneMockLink             (void);
 
     /// Converts from meters to the user specified distance unit

src/comm/MockLink.cc

@@ -1159,64 +1159,46 @@ MockLink*  MockLink::_startMockLink(MockConfiguration* mockConfig)
     return qobject_cast<MockLink*>(linkMgr->createConnectedLink(config));
 }
 
-MockLink*  MockLink::startPX4MockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode)
+MockLink* MockLink::_startMockLinkWorker(QString configName, MAV_AUTOPILOT firmwareType, MAV_TYPE vehicleType, bool sendStatusText, MockConfiguration::FailureMode_t failureMode)
 {
-    MockConfiguration* mockConfig = new MockConfiguration("PX4 MockLink");
+    MockConfiguration* mockConfig = new MockConfiguration(configName);
 
-    mockConfig->setFirmwareType(MAV_AUTOPILOT_PX4);
-    mockConfig->setVehicleType(MAV_TYPE_QUADROTOR);
+    mockConfig->setFirmwareType(firmwareType);
+    mockConfig->setVehicleType(vehicleType);
     mockConfig->setSendStatusText(sendStatusText);
     mockConfig->setFailureMode(failureMode);
 
     return _startMockLink(mockConfig);
 }
 
-MockLink*  MockLink::startGenericMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode)
+MockLink*  MockLink::startPX4MockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode)
 {
-    MockConfiguration* mockConfig = new MockConfiguration("Generic MockLink");
-
-    mockConfig->setFirmwareType(MAV_AUTOPILOT_GENERIC);
-    mockConfig->setVehicleType(MAV_TYPE_QUADROTOR);
-    mockConfig->setSendStatusText(sendStatusText);
-    mockConfig->setFailureMode(failureMode);
+    return _startMockLinkWorker("PX4 MultiRotor MockLink", MAV_AUTOPILOT_PX4, MAV_TYPE_QUADROTOR, sendStatusText, failureMode);
+}
 
-    return _startMockLink(mockConfig);
+MockLink*  MockLink::startGenericMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode)
+{
+    return _startMockLinkWorker("Generic MockLink", MAV_AUTOPILOT_GENERIC, MAV_TYPE_QUADROTOR, sendStatusText, failureMode);
 }
 
 MockLink*  MockLink::startAPMArduCopterMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode)
 {
-    MockConfiguration* mockConfig = new MockConfiguration("APM ArduCopter MockLink");
-
-    mockConfig->setFirmwareType(MAV_AUTOPILOT_ARDUPILOTMEGA);
-    mockConfig->setVehicleType(MAV_TYPE_QUADROTOR);
-    mockConfig->setSendStatusText(sendStatusText);
-    mockConfig->setFailureMode(failureMode);
-
-    return _startMockLink(mockConfig);
+    return _startMockLinkWorker("ArduCopter MockLink",MAV_AUTOPILOT_ARDUPILOTMEGA, MAV_TYPE_QUADROTOR, sendStatusText, failureMode);
 }
 
 MockLink*  MockLink::startAPMArduPlaneMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode)
 {
-    MockConfiguration* mockConfig = new MockConfiguration("APM ArduPlane MockLink");
-
-    mockConfig->setFirmwareType(MAV_AUTOPILOT_ARDUPILOTMEGA);
-    mockConfig->setVehicleType(MAV_TYPE_FIXED_WING);
-    mockConfig->setSendStatusText(sendStatusText);
-    mockConfig->setFailureMode(failureMode);
-
-    return _startMockLink(mockConfig);
+    return _startMockLinkWorker("ArduPlane MockLink", MAV_AUTOPILOT_ARDUPILOTMEGA, MAV_TYPE_FIXED_WING, sendStatusText, failureMode);
 }
 
 MockLink*  MockLink::startAPMArduSubMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode)
 {
-    MockConfiguration* mockConfig = new MockConfiguration("APM ArduSub MockLink");
-
-    mockConfig->setFirmwareType(MAV_AUTOPILOT_ARDUPILOTMEGA);
-    mockConfig->setVehicleType(MAV_TYPE_SUBMARINE);
-    mockConfig->setSendStatusText(sendStatusText);
-    mockConfig->setFailureMode(failureMode);
+    return _startMockLinkWorker("ArduSub MockLink", MAV_AUTOPILOT_ARDUPILOTMEGA, MAV_TYPE_SUBMARINE, sendStatusText, failureMode);
+}
 
-    return _startMockLink(mockConfig);
+MockLink*  MockLink::startAPMArduRoverMockLink(bool sendStatusText, MockConfiguration::FailureMode_t failureMode)
+{
+    return _startMockLinkWorker("ArduRover MockLink", MAV_AUTOPILOT_ARDUPILOTMEGA, MAV_TYPE_GROUND_ROVER, sendStatusText, failureMode);
 }
 
 void MockLink::_sendRCChannels(void)

src/comm/MockLink.h

@@ -156,6 +156,7 @@ public:
     static MockLink* startAPMArduCopterMockLink  (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone);
     static MockLink* startAPMArduPlaneMockLink   (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone);
     static MockLink* startAPMArduSubMockLink     (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone);
+    static MockLink* startAPMArduRoverMockLink   (bool sendStatusText, MockConfiguration::FailureMode_t failureMode = MockConfiguration::FailNone);
 
 private slots:
     virtual void _writeBytes(const QByteArray bytes);
@@ -203,6 +204,7 @@ private:
     void _sendADSBVehicles(void);
     void _moveADSBVehicle(void);
 
+    static MockLink* _startMockLinkWorker(QString configName, MAV_AUTOPILOT firmwareType, MAV_TYPE vehicleType, bool sendStatusText, MockConfiguration::FailureMode_t failureMode);
     static MockLink* _startMockLink(MockConfiguration* mockConfig);
 
     MockLinkMissionItemHandler  _missionItemHandler;

src/ui/preferences/MockLink.qml

@@ -52,6 +52,10 @@ Rectangle {
                 text:       qsTr("APM ArduSub Vehicle")
                 onClicked:  QGroundControl.startAPMArduSubMockLink(sendStatusText.checked)
             }
+            QGCButton {
+                text:       qsTr("APM ArduRover Vehicle")
+                onClicked:  QGroundControl.startAPMArduRoverMockLink(sendStatusText.checked)
+            }
             QGCButton {
                 text:       qsTr("Generic Vehicle")
                 onClicked:  QGroundControl.startGenericMockLink(sendStatusText.checked)