Vehicle: guard ArduPilot specific commands

This still needs implementation in some way for PX4. Signed-off-by: Julian Oes <[email protected]>
mavlink · Apr 24, 2023 · 310d1c6 · 310d1c6
1 parent c61b981
commit 310d1c6
Showing 1 changed file with 17 additions and 10 deletions.
diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc
@@ -4375,11 +4375,14 @@ void Vehicle::toggleGimbalNeutral(bool force, bool set)
 // Not implemented in Ardupilot yet
 void Vehicle::toggleGimbalYawLock(bool force, bool set) 
 {
-    sendMavCommand(_defaultComponentId,
-        MAV_CMD_DO_AUX_FUNCTION,
-        true,    // show errors
-        163,     // yaw lock
-        set ? 2 : 0);  // Switch position
+
+    if (apmFirmware()) {
+        sendMavCommand(_defaultComponentId,
+            MAV_CMD_DO_AUX_FUNCTION,
+            true,    // show errors
+            163,     // yaw lock
+            set ? 2 : 0);  // Switch position
+    }
 
     // This is how it should be...  
     //
@@ -4401,11 +4404,15 @@ void Vehicle::toggleGimbalYawLock(bool force, bool set)
 
 void Vehicle::setGimbalRcTargeting() 
 {
-    sendMavCommand(_defaultComponentId,
-        MAV_CMD_DO_AUX_FUNCTION,
-        true,    // show errors
-        27,      // retract mount
-        0);      // low switch, sets it to default rc mode
+    if (apmFirmware()) {
+        sendMavCommand(_defaultComponentId,
+            MAV_CMD_DO_AUX_FUNCTION,
+            true,    // show errors
+            27,      // retract mount
+            0);      // low switch, sets it to default rc mode
+    } else {
+        // Do the standard thing for PX4.
+    }
 }
 
 void Vehicle::sendGimbalManagerPitchYawFlags(uint32_t flags)