From feae762e645e517e86a7fd9d61bca2cfc6b72070 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Fri, 9 Jul 2021 18:14:07 +0930 Subject: [PATCH] Copter: additional yaw modes and fixes --- ArduCopter/GCS_Mavlink.cpp | 12 ++---- ArduCopter/autoyaw.cpp | 77 ++++++++++++++++++++++++++------------ ArduCopter/defines.h | 5 ++- ArduCopter/mode.h | 11 ++++-- ArduCopter/mode_auto.cpp | 2 +- ArduCopter/mode_guided.cpp | 14 ++++--- 6 files changed, 79 insertions(+), 42 deletions(-) diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 9bad2ad6cd..6840300145 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -704,11 +704,9 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_mount(const mavlink_command_long_t // if vehicle has a camera mount but it doesn't do pan control then yaw the entire vehicle instead if ((copter.camera_mount.get_mount_type() != copter.camera_mount.MountType::Mount_Type_None) && !copter.camera_mount.has_pan_control()) { - copter.flightmode->auto_yaw.set_fixed_yaw( + copter.flightmode->auto_yaw.set_yaw_angle_rate( (float)packet.param3 * 0.01f, - 0.0f, - 0, - false); + 0.0f); } break; #endif @@ -974,11 +972,9 @@ void GCS_MAVLINK_Copter::handle_mount_message(const mavlink_message_t &msg) // if vehicle has a camera mount but it doesn't do pan control then yaw the entire vehicle instead if ((copter.camera_mount.get_mount_type() != copter.camera_mount.MountType::Mount_Type_None) && !copter.camera_mount.has_pan_control()) { - copter.flightmode->auto_yaw.set_fixed_yaw( + copter.flightmode->auto_yaw.set_yaw_angle_rate( mavlink_msg_mount_control_get_input_c(&msg) * 0.01f, - 0.0f, - 0, - false); + 0.0f); break; } diff --git a/ArduCopter/autoyaw.cpp b/ArduCopter/autoyaw.cpp index 78bec2dc50..cfb358fe1f 100644 --- a/ArduCopter/autoyaw.cpp +++ b/ArduCopter/autoyaw.cpp @@ -85,7 +85,7 @@ void Mode::AutoYaw::set_mode(autopilot_yaw_mode yaw_mode) case AUTO_YAW_RATE: // initialise target yaw rate to zero - _rate_cds = 0.0f; + _yaw_rate_cds = 0.0f; break; case AUTO_YAW_CIRCLE: @@ -95,35 +95,52 @@ void Mode::AutoYaw::set_mode(autopilot_yaw_mode yaw_mode) } // set_fixed_yaw - sets the yaw look at heading for auto mode -void Mode::AutoYaw::set_fixed_yaw(float angle_deg, float turn_rate_dps, int8_t direction, bool relative_angle) +void Mode::AutoYaw::set_fixed_yaw(float angle_deg, float turn_rate_ds, int8_t direction, bool relative_angle) { - const int32_t curr_yaw_target = copter.attitude_control->get_att_target_euler_cd().z; + fixed_last_update = millis(); + + _yaw_angle_cd = copter.attitude_control->get_att_target_euler_cd().z; + _yaw_rate_cds = 0.0; // calculate final angle as relative to vehicle heading or absolute - if (!relative_angle) { - // absolute angle - _fixed_yaw = wrap_360_cd(angle_deg * 100); - } else { - // relative angle - if (direction < 0) { - angle_deg = -angle_deg; + if (relative_angle) { + if (direction == 0) { + _fixed_yaw_offset_cd = angle_deg; + } else { + _fixed_yaw_offset_cd = angle_deg * direction; + } + } else { + // absolute angle + _fixed_yaw_offset_cd = wrap_180_cd(angle_deg * 100.0 - _yaw_angle_cd); + if ( direction < 0 && is_positive(_fixed_yaw_offset_cd) ) { + _fixed_yaw_offset_cd -= 36000.0; + } else if ( direction > 0 && is_negative(_fixed_yaw_offset_cd) ) { + _fixed_yaw_offset_cd += 36000.0; } - _fixed_yaw = wrap_360_cd((angle_deg * 100) + curr_yaw_target); } // get turn speed - if (is_zero(turn_rate_dps)) { - // default to regular auto slew rate - _fixed_yaw_slewrate = AUTO_YAW_SLEW_RATE; + if (!is_positive(turn_rate_ds)) { + // default to default slew rate + _fixed_yaw_slewrate_cds = copter.attitude_control->get_slew_yaw_cds(); } else { - const int32_t turn_rate = (wrap_180_cd(_fixed_yaw - curr_yaw_target) / 100) / turn_rate_dps; - _fixed_yaw_slewrate = constrain_int32(turn_rate, 1, 360); // deg / sec + _fixed_yaw_slewrate_cds = MIN(copter.attitude_control->get_slew_yaw_cds(), turn_rate_ds * 100.0); } // set yaw mode set_mode(AUTO_YAW_FIXED); +} - // TO-DO: restore support for clockwise and counter clockwise rotation held in cmd.content.yaw.direction. 1 = clockwise, -1 = counterclockwise +// set_fixed_yaw - sets the yaw look at heading for auto mode +void Mode::AutoYaw::set_yaw_angle_rate(float yaw_angle_d, float yaw_rate_ds) +{ + fixed_last_update = millis(); + + _yaw_angle_cd = yaw_angle_d * 100.0; + _yaw_rate_cds = yaw_rate_ds * 100.0; + + // set yaw mode + set_mode(AUTO_YAW_ANGLE_RATE); } // set_roi - sets the yaw to look at roi for auto mode @@ -169,7 +186,7 @@ void Mode::AutoYaw::set_roi(const Location &roi_location) void Mode::AutoYaw::set_rate(float turn_rate_cds) { set_mode(AUTO_YAW_RATE); - _rate_cds = turn_rate_cds; + _yaw_rate_cds = turn_rate_cds; } // yaw - returns target heading depending upon auto_yaw.mode() @@ -181,10 +198,16 @@ float Mode::AutoYaw::yaw() // point towards a location held in roi return roi_yaw(); - case AUTO_YAW_FIXED: + case AUTO_YAW_FIXED: { // keep heading pointing in the direction held in fixed_yaw // with no pilot input allowed - return _fixed_yaw; + float dt = millis() - fixed_last_update; + fixed_last_update = millis(); + float yaw_angle_step = constrain_float(_fixed_yaw_offset_cd, - dt * _fixed_yaw_slewrate_cds, dt * _fixed_yaw_slewrate_cds); + _fixed_yaw_offset_cd -= yaw_angle_step; + _yaw_angle_cd += yaw_angle_step; + return _yaw_angle_cd; + } case AUTO_YAW_LOOK_AHEAD: // Commanded Yaw to automatically look ahead. @@ -203,11 +226,18 @@ float Mode::AutoYaw::yaw() // return the current attitude target return wrap_360_cd(copter.attitude_control->get_att_target_euler_cd().z); + case AUTO_YAW_ANGLE_RATE:{ + float dt = millis() - fixed_last_update; + fixed_last_update = millis(); + _yaw_angle_cd += _yaw_rate_cds * dt; + return _yaw_angle_cd; + } + case AUTO_YAW_LOOK_AT_NEXT_WP: default: // point towards next waypoint. // we don't use wp_bearing because we don't want the copter to turn too much during flight - return copter.wp_nav->get_yaw(); + return copter.pos_control->get_yaw_cd(); } } @@ -225,11 +255,12 @@ float Mode::AutoYaw::rate_cds() const case AUTO_YAW_CIRCLE: return 0.0f; + case AUTO_YAW_ANGLE_RATE: case AUTO_YAW_RATE: - return _rate_cds; + return _yaw_rate_cds; case AUTO_YAW_LOOK_AT_NEXT_WP: - return copter.wp_nav->get_yaw_rate_cds(); + return copter.pos_control->get_yaw_rate_cds(); } // return zero turn rate (this should never happen) diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index d7f6d43b13..a45df46cdc 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -18,8 +18,9 @@ enum autopilot_yaw_mode { AUTO_YAW_FIXED = 3, // point towards a particular angle (no pilot input accepted) AUTO_YAW_LOOK_AHEAD = 4, // point in the direction the copter is moving AUTO_YAW_RESETTOARMEDYAW = 5, // point towards heading at time motors were armed - AUTO_YAW_RATE = 6, // turn at a specified rate (held in auto_yaw_rate) - AUTO_YAW_CIRCLE = 7, // use AC_Circle's provided yaw (used during Loiter-Turns commands) + AUTO_YAW_ANGLE_RATE = 6, // turn at a specified rate from a starting angle + AUTO_YAW_RATE = 7, // turn at a specified rate (held in auto_yaw_rate) + AUTO_YAW_CIRCLE = 8, // use AC_Circle's provided yaw (used during Loiter-Turns commands) }; // Frame types diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index fbdaef9705..b9f88e5b7c 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -207,6 +207,8 @@ public: int8_t direction, bool relative_angle); + void set_yaw_angle_rate(float yaw_angle_d, float yaw_rate_ds); + private: float look_ahead_yaw(); @@ -219,16 +221,19 @@ public: Vector3f roi; // yaw used for YAW_FIXED yaw_mode - int32_t _fixed_yaw; + float _fixed_yaw_offset_cd; // Deg/s we should turn - int16_t _fixed_yaw_slewrate; + float _fixed_yaw_slewrate_cds; + + uint32_t fixed_last_update; // heading when in yaw_look_ahead_yaw float _look_ahead_yaw; // turn rate (in cds) when auto_yaw_mode is set to AUTO_YAW_RATE - float _rate_cds; + float _yaw_angle_cd; + float _yaw_rate_cds; }; static AutoYaw auto_yaw; diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 3185157a57..520fb5ce03 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -1501,7 +1501,7 @@ void ModeAuto::do_mount_control(const AP_Mission::Mission_Command& cmd) // if vehicle has a camera mount but it doesn't do pan control then yaw the entire vehicle instead if ((copter.camera_mount.get_mount_type() != copter.camera_mount.MountType::Mount_Type_None) && !copter.camera_mount.has_pan_control()) { - auto_yaw.set_fixed_yaw(cmd.content.mount_control.yaw,0.0f,0,false); + auto_yaw.set_yaw_angle_rate(cmd.content.mount_control.yaw,0.0f); } // pass the target angles to the camera mount copter.camera_mount.set_angle_targets(cmd.content.mount_control.roll, cmd.content.mount_control.pitch, cmd.content.mount_control.yaw); diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index a82de8e3b4..dd6fa1a6df 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -576,7 +576,7 @@ void ModeGuided::pos_control_run() attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), auto_yaw.rate_cds()); } else { // roll & pitch from position controller, yaw heading from GCS or auto_heading() - attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw()); + attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw(), auto_yaw.rate_cds()); } } @@ -639,7 +639,7 @@ void ModeGuided::accel_control_run() attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), auto_yaw.rate_cds()); } else { // roll & pitch from position controller, yaw heading from GCS or auto_heading() - attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw()); + attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw(), auto_yaw.rate_cds()); } } @@ -713,7 +713,7 @@ void ModeGuided::velaccel_control_run() attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), auto_yaw.rate_cds()); } else { // roll & pitch from position controller, yaw heading from GCS or auto_heading() - attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw()); + attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw(), auto_yaw.rate_cds()); } } @@ -794,7 +794,7 @@ void ModeGuided::posvelaccel_control_run() attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), auto_yaw.rate_cds()); } else { // roll & pitch from position controller, yaw heading from GCS or auto_heading() - attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw()); + attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw(), auto_yaw.rate_cds()); } } @@ -887,8 +887,12 @@ void ModeGuided::set_desired_velocity_with_accel_and_fence_limits(const Vector3f // helper function to set yaw state and targets void ModeGuided::set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle) { - if (use_yaw) { + if (use_yaw && relative_angle) { auto_yaw.set_fixed_yaw(yaw_cd * 0.01f, 0.0f, 0, relative_angle); + } else if (use_yaw && use_yaw_rate) { + auto_yaw.set_yaw_angle_rate(yaw_cd * 0.01f, yaw_rate_cds * 0.01f); + } else if (use_yaw && !use_yaw_rate) { + auto_yaw.set_yaw_angle_rate(yaw_cd * 0.01f, 0.0f); } else if (use_yaw_rate) { auto_yaw.set_rate(yaw_rate_cds); }