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

AP_Mission: do_gimbal_manager_pitchyaw supports bf/ef angles and rates

Browse Source 
also minor bug fix so verify DO_GIMBAL_MANAGER_PITCHYAW returns true
This commit is contained in:
Randy Mackay 2022-06-20 20:51:35 +09:00
parent 33a4efa936
commit c22d8b379f
2 changed files with 12 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/AP_Mission/AP_Mission.cpp
View File

@ -326,6 +326,7 @@ bool AP_Mission::verify_command(const Mission_Command& cmd)
case MAV_CMD_DO_SPRAYER: case MAV_CMD_DO_SPRAYER:
case MAV_CMD_DO_AUX_FUNCTION: case MAV_CMD_DO_AUX_FUNCTION:
case MAV_CMD_DO_SET_RESUME_REPEAT_DIST: case MAV_CMD_DO_SET_RESUME_REPEAT_DIST:
case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
return true; return true;
default: default:
return _cmd_verify_fn(cmd); return _cmd_verify_fn(cmd);

18
libraries/AP_Mission/AP_Mission_Commands.cpp
View File

@ -224,15 +224,19 @@ bool AP_Mission::start_command_do_gimbal_manager_pitchyaw(const AP_Mission::Miss
return true; return true;
} }
// To-Do: handle earth-frame vs body-frame angles
//bool earth_frame = cmd.content.gimbal_manager_pitchyaw.flags.GIMBAL_MANAGER_FLAGS_ROLL_LOCK |
// cmd.content.gimbal_manager_pitchyaw.flags.GIMBAL_MANAGER_FLAGS_PITCH_LOCK |
// cmd.content.gimbal_manager_pitchyaw.flags.GIMBAL_MANAGER_FLAGS_YAW_LOCK;
// To-Do: handle pitch and yaw rates
// To-Do: handle gimbal device id // To-Do: handle gimbal device id
if (!isnan(cmd.content.gimbal_manager_pitchyaw.pitch_angle_deg) && !isnan(cmd.content.gimbal_manager_pitchyaw.yaw_angle_deg)) { // handle angle target
mount->set_angle_targets(0, cmd.content.gimbal_manager_pitchyaw.pitch_angle_deg, cmd.content.gimbal_manager_pitchyaw.yaw_angle_deg); 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);
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);
return true; return true;
} }