aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Mission: do-gimbal-manager-pitchyaw command supports multiple gimbals

Browse Source
This commit is contained in:
Randy Mackay 2022-09-02 14:33:21 +09:00
parent f932add7ea
commit 152ef7f78f
1 changed files with 11 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/AP_Mission/AP_Mission_Commands.cpp
View File

@ -213,30 +213,35 @@ bool AP_Mission::start_command_do_gimbal_manager_pitchyaw(const AP_Mission::Miss
if (mount == nullptr) {
return false;
}
// check gimbal device id. 0 is primary, 1 is 1st gimbal, 2 is 2nd gimbal, etc
uint8_t gimbal_instance = mount->get_primary();
if (cmd.content.gimbal_manager_pitchyaw.gimbal_id > 0) {
gimbal_instance = cmd.content.gimbal_manager_pitchyaw.gimbal_id - 1;
}
// check flags for change to RETRACT
if ((cmd.content.gimbal_manager_pitchyaw.flags & GIMBAL_MANAGER_FLAGS_RETRACT) > 0) {
mount->set_mode(MAV_MOUNT_MODE_RETRACT);
mount->set_mode(gimbal_instance, MAV_MOUNT_MODE_RETRACT);
return true;
}
// check flags for change to NEUTRAL
if ((cmd.content.gimbal_manager_pitchyaw.flags & GIMBAL_MANAGER_FLAGS_NEUTRAL) > 0) {
mount->set_mode(MAV_MOUNT_MODE_NEUTRAL);
mount->set_mode(gimbal_instance, MAV_MOUNT_MODE_NEUTRAL);
return true;
}
// To-Do: handle gimbal device id
// handle angle target
const bool pitch_angle_valid = !isnan(cmd.content.gimbal_manager_pitchyaw.pitch_angle_deg) && (fabsf(cmd.content.gimbal_manager_pitchyaw.pitch_angle_deg) <= 90);
const bool yaw_angle_valid = !isnan(cmd.content.gimbal_manager_pitchyaw.yaw_angle_deg) && (fabsf(cmd.content.gimbal_manager_pitchyaw.yaw_angle_deg) <= 360);
if (pitch_angle_valid && yaw_angle_valid) {
mount->set_angle_target(0, cmd.content.gimbal_manager_pitchyaw.pitch_angle_deg, cmd.content.gimbal_manager_pitchyaw.yaw_angle_deg, cmd.content.gimbal_manager_pitchyaw.flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK);
mount->set_angle_target(gimbal_instance, 0, cmd.content.gimbal_manager_pitchyaw.pitch_angle_deg, cmd.content.gimbal_manager_pitchyaw.yaw_angle_deg, cmd.content.gimbal_manager_pitchyaw.flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK);
return true;
}
// handle rate target
if (!isnan(cmd.content.gimbal_manager_pitchyaw.pitch_rate_degs) && !isnan(cmd.content.gimbal_manager_pitchyaw.yaw_rate_degs)) {
mount->set_rate_target(0, cmd.content.gimbal_manager_pitchyaw.pitch_rate_degs, cmd.content.gimbal_manager_pitchyaw.yaw_rate_degs, cmd.content.gimbal_manager_pitchyaw.flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK);
mount->set_rate_target(gimbal_instance, 0, cmd.content.gimbal_manager_pitchyaw.pitch_rate_degs, cmd.content.gimbal_manager_pitchyaw.yaw_rate_degs, cmd.content.gimbal_manager_pitchyaw.flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK);
return true;
}