From 310d1c6a6ca78f6a0bfd952ab77961c713982c9a Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 24 Apr 2023 14:58:18 +1200 Subject: [PATCH] Vehicle: guard ArduPilot specific commands This still needs implementation in some way for PX4. Signed-off-by: Julian Oes --- src/Vehicle/Vehicle.cc | 27 +++++++++++++++++---------- 1 file changed, 17 insertions(+), 10 deletions(-) diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 271d53f8ac0..2a2db49b3fc 100644 --- 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)