diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 43e3ac34a0..fa10312b4e 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -478,6 +478,9 @@ private: // heading when in yaw_look_ahead_bearing float yaw_look_ahead_bearing; + // turn rate (in cds) when auto_yaw_mode is set to AUTO_YAW_RATE + float auto_yaw_rate_cds; + // Delay Mission Scripting Command int32_t condition_value; // used in condition commands (eg delay, change alt, etc.) uint32_t condition_start; @@ -812,7 +815,9 @@ private: void set_auto_yaw_mode(uint8_t yaw_mode); void set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, uint8_t relative_angle); void set_auto_yaw_roi(const Location &roi_location); + void set_auto_yaw_rate(float turn_rate_cds); float get_auto_heading(void); + float get_auto_yaw_rate_cds(); bool autotune_init(bool ignore_checks); void autotune_stop(); bool autotune_start(bool ignore_checks); diff --git a/ArduCopter/control_auto.cpp b/ArduCopter/control_auto.cpp index 73ce4e7825..dfcba2b7c3 100644 --- a/ArduCopter/control_auto.cpp +++ b/ArduCopter/control_auto.cpp @@ -644,6 +644,11 @@ void Copter::set_auto_yaw_mode(uint8_t yaw_mode) case AUTO_YAW_RESETTOARMEDYAW: // initial_armed_bearing will be set during arming so no init required break; + + case AUTO_YAW_RATE: + // initialise target yaw rate to zero + auto_yaw_rate_cds = 0.0f; + break; } } @@ -717,6 +722,13 @@ void Copter::set_auto_yaw_roi(const Location &roi_location) } } +// set auto yaw rate in centi-degrees per second +void Copter::set_auto_yaw_rate(float turn_rate_cds) +{ + set_auto_yaw_mode(AUTO_YAW_RATE); + auto_yaw_rate_cds = turn_rate_cds; +} + // get_auto_heading - returns target heading depending upon auto_yaw_mode // 100hz update rate float Copter::get_auto_heading(void) @@ -747,6 +759,17 @@ float Copter::get_auto_heading(void) } } +// returns yaw rate held in auto_yaw_rate and normally set by SET_POSITION_TARGET mavlink messages (positive it clockwise, negative is counter clockwise) +float Copter::get_auto_yaw_rate_cds(void) +{ + if (auto_yaw_mode == AUTO_YAW_RATE) { + return auto_yaw_rate_cds; + } + + // return zero turn rate (this should never happen) + return 0.0f; +} + // auto_payload_place_start - initialises controller to implement a placing void Copter::auto_payload_place_start() { diff --git a/ArduCopter/control_guided.cpp b/ArduCopter/control_guided.cpp index 7767049570..45e255893e 100644 --- a/ArduCopter/control_guided.cpp +++ b/ArduCopter/control_guided.cpp @@ -23,9 +23,8 @@ struct { float yaw_cd; float yaw_rate_cds; float climb_rate_cms; - bool use_yaw; bool use_yaw_rate; -} static guided_angle_state = {0, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, false, false}; +} static guided_angle_state; struct Guided_Limit { uint32_t timeout_ms; // timeout (in seconds) from the time that guided is invoked @@ -439,14 +438,12 @@ void Copter::guided_pos_control_run() if (auto_yaw_mode == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain()); + } else if (auto_yaw_mode == AUTO_YAW_RATE) { + // roll & pitch from waypoint controller, yaw rate from mavlink command or mission item + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_yaw_rate_cds(), get_smoothing_gain()); } else { - if (guided_angle_state.use_yaw_rate) { - // roll & pitch from waypoint controller, yaw rate from GCS - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), guided_angle_state.yaw_rate_cds, get_smoothing_gain()); - } else { - // roll, pitch from waypoint controller, yaw heading from GCS or auto_heading() - attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), guided_angle_state.use_yaw ? guided_angle_state.yaw_cd: get_auto_heading(), true, get_smoothing_gain()); - } + // roll, pitch from waypoint controller, yaw heading from GCS or auto_heading() + attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(), true, get_smoothing_gain()); } } @@ -498,14 +495,12 @@ void Copter::guided_vel_control_run() if (auto_yaw_mode == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), target_yaw_rate, get_smoothing_gain()); + } else if (auto_yaw_mode == AUTO_YAW_RATE) { + // roll & pitch from velocity controller, yaw rate from mavlink command or mission item + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), get_auto_yaw_rate_cds(), get_smoothing_gain()); } else { - if (guided_angle_state.use_yaw_rate) { - // roll & pitch from waypoint controller, yaw rate from GCS - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), guided_angle_state.yaw_rate_cds, get_smoothing_gain()); - } else { - // roll, pitch from waypoint controller, yaw heading from GCS or auto_heading() - attitude_control->input_euler_angle_roll_pitch_yaw(pos_control->get_roll(), pos_control->get_pitch(), guided_angle_state.use_yaw ? guided_angle_state.yaw_cd: get_auto_heading(), true, get_smoothing_gain()); - } + // roll, pitch from waypoint controller, yaw heading from GCS or auto_heading() + attitude_control->input_euler_angle_roll_pitch_yaw(pos_control->get_roll(), pos_control->get_pitch(), get_auto_heading(), true, get_smoothing_gain()); } } @@ -577,14 +572,12 @@ void Copter::guided_posvel_control_run() if (auto_yaw_mode == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), target_yaw_rate, get_smoothing_gain()); + } else if (auto_yaw_mode == AUTO_YAW_RATE) { + // roll & pitch from position-velocity controller, yaw rate from mavlink command or mission item + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), get_auto_yaw_rate_cds(), get_smoothing_gain()); } else { - if (guided_angle_state.use_yaw_rate) { - // roll & pitch from waypoint controller, yaw rate from GCS - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), guided_angle_state.yaw_rate_cds, get_smoothing_gain()); - } else { - // roll, pitch from waypoint controller, yaw heading from GCS or auto_heading() - attitude_control->input_euler_angle_roll_pitch_yaw(pos_control->get_roll(), pos_control->get_pitch(), guided_angle_state.use_yaw ? guided_angle_state.yaw_cd: get_auto_heading(), true, get_smoothing_gain()); - } + // roll, pitch from waypoint controller, yaw heading from GCS or auto_heading() + attitude_control->input_euler_angle_roll_pitch_yaw(pos_control->get_roll(), pos_control->get_pitch(), get_auto_heading(), true, get_smoothing_gain()); } } @@ -690,13 +683,14 @@ void Copter::guided_set_desired_velocity_with_accel_and_fence_limits(const Vecto pos_control->set_desired_velocity(curr_vel_des); } -// helper function to set guided yaw state +// helper function to set yaw state and targets void Copter::guided_set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds) { - guided_angle_state.use_yaw = use_yaw; - guided_angle_state.yaw_cd = yaw_cd; - guided_angle_state.use_yaw_rate = use_yaw_rate; - guided_angle_state.yaw_rate_cds = yaw_rate_cds; + if (use_yaw) { + set_auto_yaw_look_at_heading(yaw_cd, 0.0f, 0, 0); + } else if (use_yaw_rate) { + set_auto_yaw_rate(yaw_rate_cds); + } } // Guided Limit code diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 075c38e9c2..e9fb6daffb 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -18,6 +18,7 @@ enum autopilot_yaw_mode { AUTO_YAW_LOOK_AT_HEADING = 3, // point towards a particular angle (not 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) }; // Ch6... Ch12 aux switch control