From fd8c180f57132a7eda9a8afabf9edde38ad3f2c2 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Tue, 18 Jan 2022 13:01:02 -0500 Subject: [PATCH] Copter: Support MAVLINK_MSG_ID_SET_ATTITUDE_TARGET --- ArduCopter/Copter.cpp | 2 +- ArduCopter/GCS_Mavlink.cpp | 35 ++++++++++++++++------ ArduCopter/mode.h | 10 ++++++- ArduCopter/mode_guided.cpp | 61 +++++++++++++++----------------------- 4 files changed, 60 insertions(+), 48 deletions(-) diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 91d5f369ad..3210b11714 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -401,7 +401,7 @@ bool Copter::set_target_angle_and_climbrate(float roll_deg, float pitch_deg, flo Quaternion q; q.from_euler(radians(roll_deg),radians(pitch_deg),radians(yaw_deg)); - mode_guided.set_angle(q, climb_rate_ms*100, use_yaw_rate, radians(yaw_rate_degs), false); + mode_guided.set_angle(q, Vector3f{}, climb_rate_ms*100, false); return true; } diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 672c92141f..3a1deac9fc 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -1093,11 +1093,24 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg) break; } - // ensure type_mask specifies to use attitude and thrust - if ((packet.type_mask & ((1<<7)|(1<<6))) != 0) { + const bool roll_rate_ignore = packet.type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_ROLL_RATE_IGNORE; + const bool pitch_rate_ignore = packet.type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_PITCH_RATE_IGNORE; + const bool yaw_rate_ignore = packet.type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_YAW_RATE_IGNORE; + const bool throttle_ignore = packet.type_mask & ATTITUDE_TARGET_TYPEMASK_THROTTLE_IGNORE; + const bool attitude_ignore = packet.type_mask & ATTITUDE_TARGET_TYPEMASK_ATTITUDE_IGNORE; + + // ensure thrust field is not ignored + if (throttle_ignore) { break; } + Quaternion attitude_quat; + if (attitude_ignore) { + attitude_quat.zero(); + } else { + attitude_quat = Quaternion(packet.q[0],packet.q[1],packet.q[2],packet.q[3]); + } + // check if the message's thrust field should be interpreted as a climb rate or as thrust const bool use_thrust = copter.mode_guided.set_attitude_target_provides_thrust(); @@ -1119,15 +1132,19 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg) } } - // if the body_yaw_rate field is ignored, use the commanded yaw position - // otherwise use the commanded yaw rate - bool use_yaw_rate = false; - if ((packet.type_mask & (1<<2)) == 0) { - use_yaw_rate = true; + Vector3f ang_vel; + if (!roll_rate_ignore) { + ang_vel.x = packet.body_roll_rate; + } + if (!pitch_rate_ignore) { + ang_vel.y = packet.body_pitch_rate; + } + if (!yaw_rate_ignore) { + ang_vel.z = packet.body_yaw_rate; } - copter.mode_guided.set_angle(Quaternion(packet.q[0],packet.q[1],packet.q[2],packet.q[3]), - climb_rate_or_thrust, use_yaw_rate, packet.body_yaw_rate, use_thrust); + copter.mode_guided.set_angle(attitude_quat, ang_vel, + climb_rate_or_thrust, use_thrust); break; } diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 27d5eb6677..79460fc846 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -872,7 +872,15 @@ public: bool requires_terrain_failsafe() const override { return true; } - void set_angle(const Quaternion &q, float climb_rate_cms_or_thrust, bool use_yaw_rate, float yaw_rate_rads, bool use_thrust); + // Sets guided's angular target submode: Using a rotation quaternion, angular velocity, and climbrate or thrust (depends on user option) + // attitude_quat: IF zero: ang_vel (angular velocity) must be provided even if all zeroes + // IF non-zero: attitude_control is performed using both the attitude quaternion and angular velocity + // ang_vel: angular velocity (rad/s) + // climb_rate_cms_or_thrust: represents either the climb_rate (cm/s) or thrust scaled from [0, 1], unitless + // use_thrust: IF true: climb_rate_cms_or_thrust represents thrust + // IF false: climb_rate_cms_or_thrust represents climb_rate (cm/s) + void set_angle(const Quaternion &attitude_quat, const Vector3f &ang_vel, float climb_rate_cms_or_thrust, bool use_thrust); + bool set_destination(const Vector3f& destination, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false, bool terrain_alt = false); bool set_destination(const Location& dest_loc, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false); bool get_wp(Location &loc) const override; diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 82175fbfd2..f47a674bb6 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -14,9 +14,8 @@ static uint32_t update_time_ms; // system time of last target update struct { uint32_t update_time_ms; - float roll_cd; - float pitch_cd; - float yaw_cd; + Quaternion attitude_quat; + Vector3f ang_vel; float yaw_rate_cds; float climb_rate_cms; // climb rate in cms. Used if use_thrust is false float thrust; // thrust from -1 to 1. Used if use_thrust is true @@ -286,9 +285,8 @@ void ModeGuided::angle_control_start() // initialise targets guided_angle_state.update_time_ms = millis(); - guided_angle_state.roll_cd = ahrs.roll_sensor; - guided_angle_state.pitch_cd = ahrs.pitch_sensor; - guided_angle_state.yaw_cd = ahrs.yaw_sensor; + guided_angle_state.attitude_quat.initialise(); + guided_angle_state.ang_vel.zero(); guided_angle_state.climb_rate_cms = 0.0f; guided_angle_state.yaw_rate_cds = 0.0f; guided_angle_state.use_yaw_rate = false; @@ -590,21 +588,22 @@ bool ModeGuided::use_wpnav_for_position_control() const return ((copter.g2.guided_options.get() & uint32_t(Options::WPNavUsedForPosControl)) != 0); } -// set guided mode angle target and climbrate -void ModeGuided::set_angle(const Quaternion &q, float climb_rate_cms_or_thrust, bool use_yaw_rate, float yaw_rate_rads, bool use_thrust) +// Sets guided's angular target submode: Using a rotation quaternion, angular velocity, and climbrate or thrust (depends on user option) +// attitude_quat: IF zero: ang_vel (angular velocity) must be provided even if all zeroes +// IF non-zero: attitude_control is performed using both the attitude quaternion and angular velocity +// ang_vel: angular velocity (rad/s) +// climb_rate_cms_or_thrust: represents either the climb_rate (cm/s) or thrust scaled from [0, 1], unitless +// use_thrust: IF true: climb_rate_cms_or_thrust represents thrust +// IF false: climb_rate_cms_or_thrust represents climb_rate (cm/s) +void ModeGuided::set_angle(const Quaternion &attitude_quat, const Vector3f &ang_vel, float climb_rate_cms_or_thrust, bool use_thrust) { // check we are in velocity control mode if (guided_mode != SubMode::Angle) { angle_control_start(); } - // convert quaternion to euler angles - q.to_euler(guided_angle_state.roll_cd, guided_angle_state.pitch_cd, guided_angle_state.yaw_cd); - guided_angle_state.roll_cd = ToDeg(guided_angle_state.roll_cd) * 100.0f; - guided_angle_state.pitch_cd = ToDeg(guided_angle_state.pitch_cd) * 100.0f; - guided_angle_state.yaw_cd = wrap_180_cd(ToDeg(guided_angle_state.yaw_cd) * 100.0f); - guided_angle_state.yaw_rate_cds = ToDeg(yaw_rate_rads) * 100.0f; - guided_angle_state.use_yaw_rate = use_yaw_rate; + guided_angle_state.attitude_quat = attitude_quat; + guided_angle_state.ang_vel = ang_vel; guided_angle_state.use_thrust = use_thrust; if (use_thrust) { @@ -617,9 +616,13 @@ void ModeGuided::set_angle(const Quaternion &q, float climb_rate_cms_or_thrust, guided_angle_state.update_time_ms = millis(); + // convert quaternion to euler angles + float roll_rad, pitch_rad, yaw_rad; + attitude_quat.to_euler(roll_rad, pitch_rad, yaw_rad); + // log target copter.Log_Write_GuidedTarget(guided_mode, - Vector3f(guided_angle_state.roll_cd, guided_angle_state.pitch_cd, guided_angle_state.yaw_cd), + Vector3f(ToDeg(roll_rad) * 100.0f, ToDeg(pitch_rad) * 100.0f, ToDeg(pitch_rad) * 100.0f), false, Vector3f(0.0f, 0.0f, climb_rate_cms_or_thrust), Vector3f()); } @@ -930,21 +933,6 @@ void ModeGuided::posvelaccel_control_run() // called from guided_run void ModeGuided::angle_control_run() { - // constrain desired lean angles - float roll_in = guided_angle_state.roll_cd; - float pitch_in = guided_angle_state.pitch_cd; - float total_in = norm(roll_in, pitch_in); - float angle_max = MIN(attitude_control->get_althold_lean_angle_max_cd(), copter.aparm.angle_max); - if (total_in > angle_max) { - float ratio = angle_max / total_in; - roll_in *= ratio; - pitch_in *= ratio; - } - - // wrap yaw request - float yaw_in = wrap_180_cd(guided_angle_state.yaw_cd); - float yaw_rate_in = guided_angle_state.yaw_rate_cds; - float climb_rate_cms = 0.0f; if (!guided_angle_state.use_thrust) { // constrain climb rate @@ -957,10 +945,9 @@ void ModeGuided::angle_control_run() // check for timeout - set lean angles and climb rate to zero if no updates received for 3 seconds uint32_t tnow = millis(); if (tnow - guided_angle_state.update_time_ms > get_timeout_ms()) { - roll_in = 0.0f; - pitch_in = 0.0f; + guided_angle_state.attitude_quat.initialise(); + guided_angle_state.ang_vel.zero(); climb_rate_cms = 0.0f; - yaw_rate_in = 0.0f; if (guided_angle_state.use_thrust) { // initialise vertical velocity controller pos_control->init_z_controller(); @@ -997,10 +984,10 @@ void ModeGuided::angle_control_run() motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // call attitude controller - if (guided_angle_state.use_yaw_rate) { - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(roll_in, pitch_in, yaw_rate_in); + if (guided_angle_state.attitude_quat.is_zero()) { + attitude_control->input_rate_bf_roll_pitch_yaw(ToDeg(guided_angle_state.ang_vel.x) * 100.0f, ToDeg(guided_angle_state.ang_vel.y) * 100.0f, ToDeg(guided_angle_state.ang_vel.z) * 100.0f); } else { - attitude_control->input_euler_angle_roll_pitch_yaw(roll_in, pitch_in, yaw_in, true); + attitude_control->input_quaternion(guided_angle_state.attitude_quat, guided_angle_state.ang_vel); } // call position controller