Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Mount: allow gimbals other than primary to be retracted/neutralised

Browse Source
This commit is contained in:
Peter Barker 2023-03-02 14:08:27 +11:00 committed by Peter Barker
parent c33f6fc1a4
commit 8c47a3d268
1 changed files with 12 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AP_Mount/AP_Mount.cpp
View File

@ -279,22 +279,6 @@ MAV_RESULT AP_Mount::handle_command_do_mount_control(const mavlink_command_long_
MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_pitchyaw(const mavlink_command_long_t &packet)
{
if (!check_primary()) {
return MAV_RESULT_FAILED;
}
// check flags for change to RETRACT
uint32_t flags = (uint32_t)packet.param5;
if ((flags & GIMBAL_MANAGER_FLAGS_RETRACT) > 0) {
_backends[_primary]->set_mode(MAV_MOUNT_MODE_RETRACT);
return MAV_RESULT_ACCEPTED;
}
// check flags for change to NEUTRAL
if ((flags & GIMBAL_MANAGER_FLAGS_NEUTRAL) > 0) {
_backends[_primary]->set_mode(MAV_MOUNT_MODE_NEUTRAL);
return MAV_RESULT_ACCEPTED;
}
// check gimbal device id. 0 is primary, 1 is 1st gimbal, 2 is 2nd gimbal, etc
if ((packet.param7 < 0) || (packet.param7 > AP_MOUNT_MAX_INSTANCES)) {
return MAV_RESULT_FAILED;
@ -304,6 +288,18 @@ MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_pitchyaw(const mavlink_com
return MAV_RESULT_FAILED;
}
// check flags for change to RETRACT
uint32_t flags = (uint32_t)packet.param5;
if ((flags & GIMBAL_MANAGER_FLAGS_RETRACT) > 0) {
_backends[gimbal_instance]->set_mode(MAV_MOUNT_MODE_RETRACT);
return MAV_RESULT_ACCEPTED;
}
// check flags for change to NEUTRAL
if ((flags & GIMBAL_MANAGER_FLAGS_NEUTRAL) > 0) {
_backends[gimbal_instance]->set_mode(MAV_MOUNT_MODE_NEUTRAL);
return MAV_RESULT_ACCEPTED;
}
// param1 : pitch_angle (in degrees)
// param2 : yaw angle (in degrees)
const float pitch_angle_deg = packet.param1;