From ca7a3b6cc288de1c4781730e6a773710da30fa85 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 26 Jul 2024 13:58:32 +1200 Subject: [PATCH] Gimbal: send commands to gimbal component We shouldn't just send the commands to the vehicle because the gimbal manager might be implemented on any component, not just the autopilot. --- src/Gimbal/GimbalController.cc | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/Gimbal/GimbalController.cc b/src/Gimbal/GimbalController.cc index c1c6412d02d..622b09e3995 100644 --- a/src/Gimbal/GimbalController.cc +++ b/src/Gimbal/GimbalController.cc @@ -520,7 +520,7 @@ void GimbalController::sendPitchBodyYaw(float pitch, float yaw, bool showError) | GIMBAL_MANAGER_FLAGS_YAW_IN_VEHICLE_FRAME; _vehicle->sendMavCommand( - _vehicle->compId(), + _activeGimbal->managerCompid()->rawValue().toUInt(), MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW, showError, pitch, @@ -553,7 +553,7 @@ void GimbalController::sendPitchAbsoluteYaw(float pitch, float yaw, bool showErr | GIMBAL_MANAGER_FLAGS_YAW_IN_EARTH_FRAME; _vehicle->sendMavCommand( - _vehicle->compId(), + _activeGimbal->managerCompid()->rawValue().toUInt(), MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW, showError, pitch, @@ -601,7 +601,7 @@ void GimbalController::sendPitchYawFlags(uint32_t flags) const bool yaw_in_vehicle_frame = _yawInVehicleFrame(flags); _vehicle->sendMavCommand( - _vehicle->compId(), + _activeGimbal->managerCompid()->rawValue().toUInt(), MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW, true, _activeGimbal->absolutePitch()->rawValue().toFloat(), @@ -620,7 +620,7 @@ void GimbalController::acquireGimbalControl() return; } _vehicle->sendMavCommand( - _vehicle->compId(), + _activeGimbal->managerCompid()->rawValue().toUInt(), MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE, true, _mavlink->getSystemId(), // Set us in primary control. @@ -639,7 +639,7 @@ void GimbalController::releaseGimbalControl() return; } _vehicle->sendMavCommand( - _vehicle->compId(), + _activeGimbal->managerCompid()->rawValue().toUInt(), MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE, true, -3.f, // Release primary control if we have control