Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 16:48:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Mount: mav-cmd-do-gimbal-manager-pitchyaw supports multiple gimbals

Browse Source
This commit is contained in:
Randy Mackay 2022-09-02 14:32:55 +09:00
parent 73d817b985
commit f932add7ea
1 changed files with 10 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_Mount/AP_Mount.cpp
View File

@ -278,14 +278,21 @@ MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_pitchyaw(const mavlink_com
return MAV_RESULT_ACCEPTED;
}
// To-Do: handle gimbal device id
// 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;
}
const uint8_t gimbal_instance = (packet.param7 < 1) ? get_primary() : (uint8_t)packet.param7 - 1;
if (!check_instance(gimbal_instance)) {
return MAV_RESULT_FAILED;
}
// param1 : pitch_angle (in degrees)
// param2 : yaw angle (in degrees)
const float pitch_angle_deg = packet.param1;
const float yaw_angle_deg = packet.param2;
if (!isnan(pitch_angle_deg) && !isnan(yaw_angle_deg)) {
set_angle_target(0, pitch_angle_deg, yaw_angle_deg, flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK);
set_angle_target(gimbal_instance, 0, pitch_angle_deg, yaw_angle_deg, flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK);
return MAV_RESULT_ACCEPTED;
}
@ -294,7 +301,7 @@ MAV_RESULT AP_Mount::handle_command_do_gimbal_manager_pitchyaw(const mavlink_com
const float pitch_rate_degs = packet.param3;
const float yaw_rate_degs = packet.param4;
if (!isnan(pitch_rate_degs) && !isnan(yaw_rate_degs)) {
set_rate_target(0, pitch_rate_degs, yaw_rate_degs, flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK);
set_rate_target(gimbal_instance, 0, pitch_rate_degs, yaw_rate_degs, flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK);
return MAV_RESULT_ACCEPTED;
}