aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions 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_AUX_FUNCTION:
case MAV_CMD_DO_SET_RESUME_REPEAT_DIST:
case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
return true;
default:
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;
}
// 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
if (!isnan(cmd.content.gimbal_manager_pitchyaw.pitch_angle_deg) && !isnan(cmd.content.gimbal_manager_pitchyaw.yaw_angle_deg)) {
mount->set_angle_targets(0, cmd.content.gimbal_manager_pitchyaw.pitch_angle_deg, cmd.content.gimbal_manager_pitchyaw.yaw_angle_deg);
// 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);
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;
}