From c22d8b379f787d7e9358fee20c5095c1edc0574d Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 20 Jun 2022 20:51:35 +0900 Subject: [PATCH] AP_Mission: do_gimbal_manager_pitchyaw supports bf/ef angles and rates also minor bug fix so verify DO_GIMBAL_MANAGER_PITCHYAW returns true --- libraries/AP_Mission/AP_Mission.cpp | 1 + libraries/AP_Mission/AP_Mission_Commands.cpp | 18 +++++++++++------- 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index f8c2ea6820..9b78f224eb 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -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); diff --git a/libraries/AP_Mission/AP_Mission_Commands.cpp b/libraries/AP_Mission/AP_Mission_Commands.cpp index 7974710597..b989b0bf49 100644 --- a/libraries/AP_Mission/AP_Mission_Commands.cpp +++ b/libraries/AP_Mission/AP_Mission_Commands.cpp @@ -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; }