From 589102b7d702ea3362204cc7d507e14873744557 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 2 Jun 2022 21:23:47 +0900 Subject: [PATCH] AP_Mission: add DO_GIMBAL_MANAGER_PITCHYAW support --- libraries/AP_Mission/AP_Mission.cpp | 29 +++++++++++++-- libraries/AP_Mission/AP_Mission.h | 15 +++++++- libraries/AP_Mission/AP_Mission_Commands.cpp | 38 ++++++++++++++++++++ 3 files changed, 78 insertions(+), 4 deletions(-) diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index f655afcd3a..f8c2ea6820 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -363,6 +363,8 @@ bool AP_Mission::start_command(const Mission_Command& cmd) return start_command_do_sprayer(cmd); case MAV_CMD_DO_SET_RESUME_REPEAT_DIST: return command_do_set_repeat_dist(cmd); + case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW: + return start_command_do_gimbal_manager_pitchyaw(cmd); default: return _cmd_start_fn(cmd); } @@ -1201,7 +1203,16 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_ case MAV_CMD_DO_PAUSE_CONTINUE: cmd.p1 = packet.param1; break; - + + case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW: + cmd.content.gimbal_manager_pitchyaw.pitch_angle_deg = packet.param1; + cmd.content.gimbal_manager_pitchyaw.yaw_angle_deg = packet.param2; + cmd.content.gimbal_manager_pitchyaw.pitch_rate_degs = packet.param3; + cmd.content.gimbal_manager_pitchyaw.yaw_rate_degs = packet.param4; + cmd.content.gimbal_manager_pitchyaw.flags = packet.x; + cmd.content.gimbal_manager_pitchyaw.gimbal_id = packet.z; + break; + default: // unrecognised command return MAV_MISSION_UNSUPPORTED; @@ -1289,6 +1300,7 @@ MAV_MISSION_RESULT AP_Mission::convert_MISSION_ITEM_to_MISSION_ITEM_INT(const ma case MAV_CMD_DO_DIGICAM_CONTROL: case MAV_CMD_DO_DIGICAM_CONFIGURE: case MAV_CMD_NAV_ATTITUDE_TIME: + case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW: mav_cmd.x = packet.x; mav_cmd.y = packet.y; break; @@ -1331,6 +1343,7 @@ MAV_MISSION_RESULT AP_Mission::convert_MISSION_ITEM_INT_to_MISSION_ITEM(const ma case MAV_CMD_DO_DIGICAM_CONTROL: case MAV_CMD_DO_DIGICAM_CONFIGURE: case MAV_CMD_NAV_ATTITUDE_TIME: + case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW: item.x = item_int.x; item.y = item_int.y; break; @@ -1677,7 +1690,16 @@ bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& c case MAV_CMD_DO_PAUSE_CONTINUE: packet.param1 = cmd.p1; break; - + + case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW: + packet.param1 = cmd.content.gimbal_manager_pitchyaw.pitch_angle_deg; + packet.param2 = cmd.content.gimbal_manager_pitchyaw.yaw_angle_deg; + packet.param3 = cmd.content.gimbal_manager_pitchyaw.pitch_rate_degs; + packet.param4 = cmd.content.gimbal_manager_pitchyaw.yaw_rate_degs; + packet.x = cmd.content.gimbal_manager_pitchyaw.flags; + packet.z = cmd.content.gimbal_manager_pitchyaw.gimbal_id; + break; + default: // unrecognised command return false; @@ -2402,7 +2424,8 @@ const char *AP_Mission::Mission_Command::type() const return "NavAttitudeTime"; case MAV_CMD_DO_PAUSE_CONTINUE: return "PauseContinue"; - + case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW: + return "GimbalPitchYaw"; default: #if CONFIG_HAL_BOARD == HAL_BOARD_SITL AP_HAL::panic("Mission command with ID %u has no string", id); diff --git a/libraries/AP_Mission/AP_Mission.h b/libraries/AP_Mission/AP_Mission.h index 97876c951b..3516856a82 100644 --- a/libraries/AP_Mission/AP_Mission.h +++ b/libraries/AP_Mission/AP_Mission.h @@ -239,6 +239,16 @@ public: float climb_rate; }; + // MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW support + struct PACKED gimbal_manager_pitchyaw_Command { + int8_t pitch_angle_deg; + int16_t yaw_angle_deg; + int8_t pitch_rate_degs; + int8_t yaw_rate_degs; + uint8_t flags; + uint8_t gimbal_id; + }; + union Content { // jump structure Jump_Command jump; @@ -315,6 +325,9 @@ public: // nav attitude time nav_attitude_time_Command nav_attitude_time; + // MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW + gimbal_manager_pitchyaw_Command gimbal_manager_pitchyaw; + // location Location location{}; // Waypoint location }; @@ -760,7 +773,7 @@ private: bool start_command_do_sprayer(const AP_Mission::Mission_Command& cmd); bool start_command_do_scripting(const AP_Mission::Mission_Command& cmd); - + bool start_command_do_gimbal_manager_pitchyaw(const AP_Mission::Mission_Command& cmd); }; namespace AP diff --git a/libraries/AP_Mission/AP_Mission_Commands.cpp b/libraries/AP_Mission/AP_Mission_Commands.cpp index 0f5867a87e..7974710597 100644 --- a/libraries/AP_Mission/AP_Mission_Commands.cpp +++ b/libraries/AP_Mission/AP_Mission_Commands.cpp @@ -8,6 +8,7 @@ #include #include #include +#include bool AP_Mission::start_command_do_aux_function(const AP_Mission::Mission_Command& cmd) { @@ -204,3 +205,40 @@ bool AP_Mission::start_command_do_scripting(const AP_Mission::Mission_Command& c return false; #endif // AP_SCRIPTING_ENABLED } + +bool AP_Mission::start_command_do_gimbal_manager_pitchyaw(const AP_Mission::Mission_Command& cmd) +{ +#if HAL_MOUNT_ENABLED + AP_Mount *mount = AP::mount(); + if (mount == nullptr) { + return false; + } + // 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); + 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); + 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); + return true; + } + + // if we got this far then message is not handled + return false; +#else + return false; +#endif // HAL_MOUNT_ENABLED +}