diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp index 87eae0e4cf..c2feec63f8 100644 --- a/ArduCopter/Attitude.cpp +++ b/ArduCopter/Attitude.cpp @@ -33,30 +33,27 @@ float Copter::get_pilot_desired_yaw_rate(int16_t stick_angle) * yaw controllers *************************************************************/ -// get_roi_yaw - returns heading towards location held in roi_WP -// should be called at 100hz -float Copter::get_roi_yaw() +// roi_yaw - returns heading towards location held in roi_WP +float Copter::Mode::AutoYaw::roi_yaw() { - static uint8_t roi_yaw_counter = 0; // used to reduce update rate to 100hz - roi_yaw_counter++; if (roi_yaw_counter >= 4) { roi_yaw_counter = 0; - yaw_look_at_WP_bearing = get_bearing_cd(inertial_nav.get_position(), roi_WP); + _roi_yaw = get_bearing_cd(copter.inertial_nav.get_position(), roi_WP); } - return yaw_look_at_WP_bearing; + return _roi_yaw; } -float Copter::get_look_ahead_yaw() +float Copter::Mode::AutoYaw::look_ahead_yaw() { - const Vector3f& vel = inertial_nav.get_velocity(); + const Vector3f& vel = copter.inertial_nav.get_velocity(); float speed = norm(vel.x,vel.y); // Commanded Yaw to automatically look ahead. - if (position_ok() && (speed > YAW_LOOK_AHEAD_MIN_SPEED)) { - yaw_look_ahead_bearing = degrees(atan2f(vel.y,vel.x))*100.0f; + if (copter.position_ok() && (speed > YAW_LOOK_AHEAD_MIN_SPEED)) { + _look_ahead_yaw = degrees(atan2f(vel.y,vel.x))*100.0f; } - return yaw_look_ahead_bearing; + return _look_ahead_yaw; } /************************************************************* diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 54f3af9163..fb81691952 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -32,7 +32,6 @@ Copter::Copter(void) super_simple_cos_yaw(1.0), land_accel_ef_filter(LAND_DETECTOR_ACCEL_LPF_CUTOFF), rc_throttle_control_in_filter(1.0f), - auto_yaw_mode(AUTO_YAW_LOOK_AT_NEXT_WP), inertial_nav(ahrs), param_loader(var_info), flightmode(&mode_stabilize) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 2e8a94396d..0563a7fe05 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -462,28 +462,6 @@ private: // Current location of the vehicle (altitude is relative to home) Location_Class current_loc; - // Navigation Yaw control - // auto flight mode's yaw mode - uint8_t auto_yaw_mode; - - // Yaw will point at this location if auto_yaw_mode is set to AUTO_YAW_ROI - Vector3f roi_WP; - - // bearing from current location to the yaw_look_at_WP - float yaw_look_at_WP_bearing; - - // yaw used for YAW_LOOK_AT_HEADING yaw_mode - int32_t yaw_look_at_heading; - - // Deg/s we should turn - int16_t yaw_look_at_heading_slew; - - // 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; - // IMU variables // Integration time (in seconds) for the gyros (DCM algorithm) // Updated with the fast loop @@ -693,8 +671,6 @@ private: // Attitude.cpp float get_pilot_desired_yaw_rate(int16_t stick_angle); - float get_roi_yaw(); - float get_look_ahead_yaw(); void update_throttle_hover(); void set_throttle_takeoff(); float get_pilot_desired_throttle(int16_t throttle_control, float thr_mid = 0.0f); @@ -844,15 +820,6 @@ private: void update_flight_mode(); void notify_flight_mode(); - // mode_auto.cpp - uint8_t get_default_auto_yaw_mode(bool rtl); - 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, bool 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(); - // mode_land.cpp void set_mode_land_with_pause(mode_reason_t reason); bool landing_with_GPS(); diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 1b1dd4eecb..86847ec7ac 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -924,7 +924,7 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) roi_loc.lat = packet.x; roi_loc.lng = packet.y; roi_loc.alt = (int32_t)(packet.z * 100.0f); - copter.set_auto_yaw_roi(roi_loc); + copter.flightmode->auto_yaw.set_roi(roi_loc); result = MAV_RESULT_ACCEPTED; break; } @@ -1002,7 +1002,11 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) if ((packet.param1 >= 0.0f) && (packet.param1 <= 360.0f) && (is_zero(packet.param4) || is_equal(packet.param4,1.0f))) { - copter.set_auto_yaw_look_at_heading(packet.param1, packet.param2, (int8_t)packet.param3, is_positive(packet.param4)); + copter.flightmode->auto_yaw.set_fixed_yaw( + packet.param1, + packet.param2, + (int8_t)packet.param3, + is_positive(packet.param4)); result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; @@ -1066,14 +1070,17 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) roi_loc.lat = (int32_t)(packet.param5 * 1.0e7f); roi_loc.lng = (int32_t)(packet.param6 * 1.0e7f); roi_loc.alt = (int32_t)(packet.param7 * 100.0f); - copter.set_auto_yaw_roi(roi_loc); + copter.flightmode->auto_yaw.set_roi(roi_loc); result = MAV_RESULT_ACCEPTED; break; case MAV_CMD_DO_MOUNT_CONTROL: #if MOUNT == ENABLED if(!copter.camera_mount.has_pan_control()) { - copter.set_auto_yaw_look_at_heading((float)packet.param3 / 100.0f,0.0f,0,0); + copter.flightmode->auto_yaw.set_fixed_yaw( + (float)packet.param3 / 100.0f, + 0.0f, + 0,0); } copter.camera_mount.control(packet.param1, packet.param2, packet.param3, (MAV_MOUNT_MODE) packet.param7); result = MAV_RESULT_ACCEPTED; @@ -1653,7 +1660,11 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) //deprecated. Use MAV_CMD_DO_MOUNT_CONTROL case MAVLINK_MSG_ID_MOUNT_CONTROL: if(!copter.camera_mount.has_pan_control()) { - copter.set_auto_yaw_look_at_heading(mavlink_msg_mount_control_get_input_c(msg)/100.0f, 0.0f, 0, 0); + copter.flightmode->auto_yaw.set_fixed_yaw( + mavlink_msg_mount_control_get_input_c(msg)/100.0f, + 0.0f, + 0, + 0); } copter.camera_mount.control_msg(msg); break; diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 388c2ef600..34933483f0 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -15,7 +15,7 @@ enum autopilot_yaw_mode { AUTO_YAW_HOLD = 0, // pilot controls the heading AUTO_YAW_LOOK_AT_NEXT_WP = 1, // point towards next waypoint (no pilot input accepted) AUTO_YAW_ROI = 2, // point towards a location held in roi_WP (no pilot input accepted) - AUTO_YAW_LOOK_AT_HEADING = 3, // point towards a particular angle (not pilot input accepted) + 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) diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index f87d8aada2..7952a7eec1 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -1,5 +1,7 @@ #include "Copter.h" +Copter::Mode::AutoYaw Copter::Mode::auto_yaw; + /* * High level calls to set and update flight modes logic for individual * flight modes is in control_acro.cpp, control_stabilize.cpp, etc @@ -26,11 +28,10 @@ Copter::Mode::Mode(void) : ap(copter.ap), takeoff_state(copter.takeoff_state), ekfGndSpdLimit(copter.ekfGndSpdLimit), - ekfNavVelGainScaler(copter.ekfNavVelGainScaler), #if FRAME_CONFIG == HELI_FRAME heli_flags(copter.heli_flags), #endif - auto_yaw_mode(copter.auto_yaw_mode) + ekfNavVelGainScaler(copter.ekfNavVelGainScaler) { }; // return the static controller object corresponding to supplied mode @@ -596,21 +597,6 @@ void Copter::Mode::set_throttle_takeoff() return copter.set_throttle_takeoff(); } -void Copter::Mode::set_auto_yaw_mode(uint8_t yaw_mode) -{ - return copter.set_auto_yaw_mode(yaw_mode); -} - -void Copter::Mode::set_auto_yaw_rate(float turn_rate_cds) -{ - return copter.set_auto_yaw_rate(turn_rate_cds); -} - -void Copter::Mode::set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, bool relative_angle) -{ - return copter.set_auto_yaw_look_at_heading(angle_deg, turn_rate_dps, direction, relative_angle); -} - void Copter::Mode::takeoff_timer_start(float alt_cm) { return copter.takeoff_timer_start(alt_cm); @@ -626,16 +612,6 @@ void Copter::Mode::takeoff_get_climb_rates(float& pilot_climb_rate, float& takeo return copter.takeoff_get_climb_rates(pilot_climb_rate, takeoff_climb_rate); } -float Copter::Mode::get_auto_heading() -{ - return copter.get_auto_heading(); -} - -float Copter::Mode::get_auto_yaw_rate_cds() -{ - return copter.get_auto_yaw_rate_cds(); -} - float Copter::Mode::get_avoidance_adjusted_climbrate(float target_rate) { return copter.get_avoidance_adjusted_climbrate(target_rate); @@ -645,3 +621,8 @@ uint16_t Copter::Mode::get_pilot_speed_dn() { return copter.get_pilot_speed_dn(); } + +void Copter::Mode::AutoYaw::set_mode_to_default(bool rtl) +{ + set_mode(default_mode(rtl)); +} diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index f3f8b39b35..bc51601871 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -10,7 +10,67 @@ class Mode { // constructor Mode(void); - + +public: + + // Navigation Yaw control + class AutoYaw { + + public: + + // yaw(): main product of AutoYaw; the heading: + float yaw(); + + // mode(): current method of determining desired yaw: + autopilot_yaw_mode mode() const { return (autopilot_yaw_mode)_mode; } + void set_mode_to_default(bool rtl); + void set_mode(autopilot_yaw_mode new_mode); + autopilot_yaw_mode default_mode(bool rtl) const; + + // rate_cds(): desired yaw rate in centidegrees/second: + float rate_cds() const; + void set_rate(float new_rate_cds); + + // set_roi(...): set a "look at" location: + void set_roi(const Location &roi_location); + + void set_fixed_yaw(float angle_deg, + float turn_rate_dps, + int8_t direction, + bool relative_angle); + + private: + + float look_ahead_yaw(); + float roi_yaw(); + + // auto flight mode's yaw mode + uint8_t _mode = AUTO_YAW_LOOK_AT_NEXT_WP; + + // Yaw will point at this location if mode is set to AUTO_YAW_ROI + Vector3f roi_WP; + + // bearing from current location to the ROI + float _roi_yaw; + + // yaw used for YAW_FIXED yaw_mode + int32_t _fixed_yaw; + + // Deg/s we should turn + int16_t _fixed_yaw_slewrate; + + // 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; + + // used to reduce update rate to 100hz: + uint8_t roi_yaw_counter; + + }; + static AutoYaw auto_yaw; + protected: virtual bool init(bool ignore_checks) = 0; @@ -74,10 +134,6 @@ protected: // scale factor applied to velocity controller gain to prevent optical flow noise causing excessive angle demand noise float &ekfNavVelGainScaler; - // Navigation Yaw control - // auto flight mode's yaw mode - uint8_t &auto_yaw_mode; - #if FRAME_CONFIG == HELI_FRAME heli_flags_t &heli_flags; #endif @@ -96,14 +152,9 @@ protected: GCS_Copter &gcs(); void Log_Write_Event(uint8_t id); void set_throttle_takeoff(void); - void set_auto_yaw_mode(uint8_t yaw_mode); - void set_auto_yaw_rate(float turn_rate_cds); - void set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, bool relative_angle); void takeoff_timer_start(float alt_cm); void takeoff_stop(void); void takeoff_get_climb_rates(float& pilot_climb_rate, float& takeoff_climb_rate); - float get_auto_heading(void); - float get_auto_yaw_rate_cds(void); float get_avoidance_adjusted_climbrate(float target_rate); uint16_t get_pilot_speed_dn(void); diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 667bffa602..50ea6f6f7a 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -33,8 +33,8 @@ bool Copter::ModeAuto::init(bool ignore_checks) // stop ROI from carrying over from previous runs of the mission // To-Do: reset the yaw as part of auto_wp_start when the previous command was not a wp command to remove the need for this special ROI check - if (copter.auto_yaw_mode == AUTO_YAW_ROI) { - set_auto_yaw_mode(AUTO_YAW_HOLD); + if (auto_yaw.mode() == AUTO_YAW_ROI) { + auto_yaw.set_mode(AUTO_YAW_HOLD); } // initialise waypoint and spline controller @@ -118,7 +118,7 @@ bool Copter::ModeAuto::loiter_start() wp_nav->set_wp_destination(stopping_point); // hold yaw at current heading - set_auto_yaw_mode(AUTO_YAW_HOLD); + auto_yaw.set_mode(AUTO_YAW_HOLD); return true; } @@ -170,7 +170,7 @@ void Copter::ModeAuto::takeoff_start(const Location& dest_loc) } // initialise yaw - set_auto_yaw_mode(AUTO_YAW_HOLD); + auto_yaw.set_mode(AUTO_YAW_HOLD); // clear i term when we're taking off set_throttle_takeoff(); @@ -189,8 +189,8 @@ void Copter::ModeAuto::wp_start(const Vector3f& destination) // initialise yaw // To-Do: reset the yaw only when the previous navigation command is not a WP. this would allow removing the special check for ROI - if (copter.auto_yaw_mode != AUTO_YAW_ROI) { - set_auto_yaw_mode(copter.get_default_auto_yaw_mode(false)); + if (auto_yaw.mode() != AUTO_YAW_ROI) { + auto_yaw.set_mode_to_default(false); } } @@ -208,8 +208,8 @@ void Copter::ModeAuto::wp_start(const Location_Class& dest_loc) // initialise yaw // To-Do: reset the yaw only when the previous navigation command is not a WP. this would allow removing the special check for ROI - if (copter.auto_yaw_mode != AUTO_YAW_ROI) { - set_auto_yaw_mode(copter.get_default_auto_yaw_mode(false)); + if (auto_yaw.mode() != AUTO_YAW_ROI) { + auto_yaw.set_mode_to_default(false); } } @@ -239,7 +239,7 @@ void Copter::ModeAuto::land_start(const Vector3f& destination) } // initialise yaw - set_auto_yaw_mode(AUTO_YAW_HOLD); + auto_yaw.set_mode(AUTO_YAW_HOLD); } // auto_circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location @@ -286,10 +286,10 @@ void Copter::ModeAuto::circle_movetoedge_start(const Location_Class &circle_cent const Vector3f &curr_pos = inertial_nav.get_position(); float dist_to_center = norm(circle_center_neu.x - curr_pos.x, circle_center_neu.y - curr_pos.y); if (dist_to_center > copter.circle_nav->get_radius() && dist_to_center > 500) { - set_auto_yaw_mode(copter.get_default_auto_yaw_mode(false)); + auto_yaw.set_mode_to_default(false); } else { // vehicle is within circle so hold yaw to avoid spinning as we move to edge of circle - set_auto_yaw_mode(AUTO_YAW_HOLD); + auto_yaw.set_mode(AUTO_YAW_HOLD); } } else { circle_start(); @@ -323,8 +323,8 @@ void Copter::ModeAuto::spline_start(const Location_Class& destination, bool stop // initialise yaw // To-Do: reset the yaw only when the previous navigation command is not a WP. this would allow removing the special check for ROI - if (copter.auto_yaw_mode != AUTO_YAW_ROI) { - set_auto_yaw_mode(copter.get_default_auto_yaw_mode(false)); + if (auto_yaw.mode() != AUTO_YAW_ROI) { + auto_yaw.set_mode_to_default(false); } } @@ -778,7 +778,7 @@ void Copter::ModeAuto::wp_run() // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); if (!is_zero(target_yaw_rate)) { - set_auto_yaw_mode(AUTO_YAW_HOLD); + auto_yaw.set_mode(AUTO_YAW_HOLD); } } @@ -792,12 +792,12 @@ void Copter::ModeAuto::wp_run() pos_control->update_z_controller(); // call attitude controller - if (copter.auto_yaw_mode == AUTO_YAW_HOLD) { + 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); } else { // roll, pitch from waypoint controller, yaw heading from auto_heading() - attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(),true); + attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.yaw(),true); } } @@ -821,7 +821,7 @@ void Copter::ModeAuto::spline_run() // get pilot's desired yaw rat target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); if (!is_zero(target_yaw_rate)) { - set_auto_yaw_mode(AUTO_YAW_HOLD); + auto_yaw.set_mode(AUTO_YAW_HOLD); } } @@ -835,12 +835,12 @@ void Copter::ModeAuto::spline_run() pos_control->update_z_controller(); // call attitude controller - if (copter.auto_yaw_mode == AUTO_YAW_HOLD) { + 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); } else { // roll, pitch from waypoint controller, yaw heading from auto_heading() - attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(), true); + attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.yaw(), true); } } @@ -937,7 +937,7 @@ void Copter::ModeAuto::payload_place_start(const Vector3f& destination) } // initialise yaw - set_auto_yaw_mode(AUTO_YAW_HOLD); + auto_yaw.set_mode(AUTO_YAW_HOLD); } // auto_payload_place_run - places an object in auto mode @@ -1320,7 +1320,7 @@ void Copter::ModeAuto::do_within_distance(const AP_Mission::Mission_Command& cmd void Copter::ModeAuto::do_yaw(const AP_Mission::Mission_Command& cmd) { - set_auto_yaw_look_at_heading( + auto_yaw.set_fixed_yaw( cmd.content.yaw.angle_deg, cmd.content.yaw.turn_rate_dps, cmd.content.yaw.direction, @@ -1355,7 +1355,7 @@ void Copter::ModeAuto::do_set_home(const AP_Mission::Mission_Command& cmd) // TO-DO: add support for other features of MAV_CMD_DO_SET_ROI including pointing at a given waypoint void Copter::ModeAuto::do_roi(const AP_Mission::Mission_Command& cmd) { - copter.set_auto_yaw_roi(cmd.content.location); + auto_yaw.set_roi(cmd.content.location); } // point the camera to a specified angle @@ -1363,7 +1363,7 @@ void Copter::ModeAuto::do_mount_control(const AP_Mission::Mission_Command& cmd) { #if MOUNT == ENABLED if(!copter.camera_mount.has_pan_control()) { - copter.set_auto_yaw_look_at_heading(cmd.content.mount_control.yaw,0.0f,0,0); + auto_yaw.set_fixed_yaw(cmd.content.mount_control.yaw,0.0f,0,0); } copter.camera_mount.set_angle_targets(cmd.content.mount_control.roll, cmd.content.mount_control.pitch, cmd.content.mount_control.yaw); #endif @@ -1758,16 +1758,12 @@ bool Copter::ModeAuto::verify_within_distance() bool Copter::ModeAuto::verify_yaw() { // set yaw mode if it has been changed (the waypoint controller often retakes control of yaw as it executes a new waypoint command) - if (auto_yaw_mode != AUTO_YAW_LOOK_AT_HEADING) { - set_auto_yaw_mode(AUTO_YAW_LOOK_AT_HEADING); + if (auto_yaw.mode() != AUTO_YAW_FIXED) { + auto_yaw.set_mode(AUTO_YAW_FIXED); } // check if we are within 2 degrees of the target heading - if (labs(wrap_180_cd(ahrs.yaw_sensor-copter.yaw_look_at_heading)) <= 200) { - return true; - }else{ - return false; - } + return (labs(wrap_180_cd(ahrs.yaw_sensor-auto_yaw.yaw())) <= 200); } // verify_nav_wp - check if we have reached the next way point @@ -1877,11 +1873,11 @@ bool Copter::ModeAuto::verify_nav_delay(const AP_Mission::Mission_Command& cmd) #endif -// get_default_auto_yaw_mode - returns auto_yaw_mode based on WP_YAW_BEHAVIOR parameter +// default_mode - returns auto_yaw.mode() based on WP_YAW_BEHAVIOR parameter // set rtl parameter to true if this is during an RTL -uint8_t Copter::get_default_auto_yaw_mode(bool rtl) +autopilot_yaw_mode Copter::Mode::AutoYaw::default_mode(bool rtl) const { - switch (g.wp_yaw_behavior) { + switch (copter.g.wp_yaw_behavior) { case WP_YAW_BEHAVIOR_NONE: return AUTO_YAW_HOLD; @@ -1902,35 +1898,35 @@ uint8_t Copter::get_default_auto_yaw_mode(bool rtl) } } -// set_auto_yaw_mode - sets the yaw mode for auto -void Copter::set_auto_yaw_mode(uint8_t yaw_mode) +// set_mode - sets the yaw mode for auto +void Copter::Mode::AutoYaw::set_mode(autopilot_yaw_mode yaw_mode) { // return immediately if no change - if (auto_yaw_mode == yaw_mode) { + if (_mode == yaw_mode) { return; } - auto_yaw_mode = yaw_mode; + _mode = yaw_mode; // perform initialisation - switch (auto_yaw_mode) { + switch (_mode) { case AUTO_YAW_LOOK_AT_NEXT_WP: // wpnav will initialise heading when wpnav's set_destination method is called break; case AUTO_YAW_ROI: - // point towards a location held in yaw_look_at_WP - yaw_look_at_WP_bearing = ahrs.yaw_sensor; + // look ahead until we know otherwise + _roi_yaw = copter.ahrs.yaw_sensor; break; - case AUTO_YAW_LOOK_AT_HEADING: - // keep heading pointing in the direction held in yaw_look_at_heading - // caller should set the yaw_look_at_heading + case AUTO_YAW_FIXED: + // keep heading pointing in the direction held in fixed_yaw + // caller should set the fixed_yaw break; case AUTO_YAW_LOOK_AHEAD: // Commanded Yaw to automatically look ahead. - yaw_look_ahead_bearing = ahrs.yaw_sensor; + _look_ahead_yaw = copter.ahrs.yaw_sensor; break; case AUTO_YAW_RESETTOARMEDYAW: @@ -1939,66 +1935,65 @@ void Copter::set_auto_yaw_mode(uint8_t yaw_mode) case AUTO_YAW_RATE: // initialise target yaw rate to zero - auto_yaw_rate_cds = 0.0f; + _rate_cds = 0.0f; break; } } -// set_auto_yaw_look_at_heading - sets the yaw look at heading for auto mode -void Copter::set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, bool relative_angle) +// set_fixed_yaw - sets the yaw look at heading for auto mode +void Copter::Mode::AutoYaw::set_fixed_yaw(float angle_deg, float turn_rate_dps, int8_t direction, bool relative_angle) { - // get current yaw target - int32_t curr_yaw_target = attitude_control->get_att_target_euler_cd().z; + const int32_t curr_yaw_target = copter.attitude_control->get_att_target_euler_cd().z; // calculate final angle as relative to vehicle heading or absolute if (!relative_angle) { // absolute angle - yaw_look_at_heading = wrap_360_cd(angle_deg * 100); + _fixed_yaw = wrap_360_cd(angle_deg * 100); } else { // relative angle if (direction < 0) { angle_deg = -angle_deg; } - yaw_look_at_heading = wrap_360_cd((angle_deg * 100) + curr_yaw_target); + _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 - yaw_look_at_heading_slew = AUTO_YAW_SLEW_RATE; + _fixed_yaw_slewrate = AUTO_YAW_SLEW_RATE; } else { - int32_t turn_rate = (wrap_180_cd(yaw_look_at_heading - curr_yaw_target) / 100) / turn_rate_dps; - yaw_look_at_heading_slew = constrain_int32(turn_rate, 1, 360); // deg / sec + 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 } // set yaw mode - set_auto_yaw_mode(AUTO_YAW_LOOK_AT_HEADING); + 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_auto_yaw_roi - sets the yaw to look at roi for auto mode -void Copter::set_auto_yaw_roi(const Location &roi_location) +// set_roi - sets the yaw to look at roi for auto mode +void Copter::Mode::AutoYaw::set_roi(const Location &roi_location) { // if location is zero lat, lon and altitude turn off ROI if (roi_location.alt == 0 && roi_location.lat == 0 && roi_location.lng == 0) { // set auto yaw mode back to default assuming the active command is a waypoint command. A more sophisticated method is required to ensure we return to the proper yaw control for the active command - set_auto_yaw_mode(get_default_auto_yaw_mode(false)); + auto_yaw.set_mode_to_default(false); #if MOUNT == ENABLED // switch off the camera tracking if enabled - if (camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) { - camera_mount.set_mode_to_default(); + if (copter.camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) { + copter.camera_mount.set_mode_to_default(); } #endif // MOUNT == ENABLED } else { #if MOUNT == ENABLED // check if mount type requires us to rotate the quad - if(!camera_mount.has_pan_control()) { - roi_WP = pv_location_to_vector(roi_location); - set_auto_yaw_mode(AUTO_YAW_ROI); + if(!copter.camera_mount.has_pan_control()) { + roi_WP = copter.pv_location_to_vector(roi_location); + auto_yaw.set_mode(AUTO_YAW_ROI); } // send the command to the camera mount - camera_mount.set_roi_target(roi_location); + copter.camera_mount.set_roi_target(roi_location); // TO-DO: expand handling of the do_nav_roi to support all modes of the MAVLink. Currently we only handle mode 4 (see below) // 0: do nothing @@ -2009,53 +2004,54 @@ void Copter::set_auto_yaw_roi(const Location &roi_location) #else // if we have no camera mount aim the quad at the location roi_WP = pv_location_to_vector(roi_location); - set_auto_yaw_mode(AUTO_YAW_ROI); + auto_yaw.set_mode(AUTO_YAW_ROI); #endif // MOUNT == ENABLED } } // set auto yaw rate in centi-degrees per second -void Copter::set_auto_yaw_rate(float turn_rate_cds) +void Copter::Mode::AutoYaw::set_rate(float turn_rate_cds) { - set_auto_yaw_mode(AUTO_YAW_RATE); - auto_yaw_rate_cds = turn_rate_cds; + set_mode(AUTO_YAW_RATE); + _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) +// yaw - returns target heading depending upon auto_yaw.mode() +float Copter::Mode::AutoYaw::yaw() { - switch(auto_yaw_mode) { + switch(_mode) { case AUTO_YAW_ROI: // point towards a location held in roi_WP - return get_roi_yaw(); + return roi_yaw(); - case AUTO_YAW_LOOK_AT_HEADING: - // keep heading pointing in the direction held in yaw_look_at_heading with no pilot input allowed - return yaw_look_at_heading; + case AUTO_YAW_FIXED: + // keep heading pointing in the direction held in fixed_yaw + // with no pilot input allowed + return _fixed_yaw; case AUTO_YAW_LOOK_AHEAD: // Commanded Yaw to automatically look ahead. - return get_look_ahead_yaw(); + return look_ahead_yaw(); case AUTO_YAW_RESETTOARMEDYAW: // changes yaw to be same as when quad was armed - return initial_armed_bearing; + return copter.initial_armed_bearing; 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 wp_nav->get_yaw(); + return copter.wp_nav->get_yaw(); } } -// 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) +// returns yaw rate normally set by SET_POSITION_TARGET mavlink +// messages (positive is clockwise, negative is counter clockwise) +float Copter::Mode::AutoYaw::rate_cds() const { - if (auto_yaw_mode == AUTO_YAW_RATE) { - return auto_yaw_rate_cds; + if (_mode == AUTO_YAW_RATE) { + return _rate_cds; } // return zero turn rate (this should never happen) diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 6761b67808..3bac766770 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -40,7 +40,7 @@ bool Copter::ModeGuided::init(bool ignore_checks) { if (copter.position_ok() || ignore_checks) { // initialise yaw - set_auto_yaw_mode(copter.get_default_auto_yaw_mode(false)); + auto_yaw.set_mode_to_default(false); // start in position control mode pos_control_start(); return true; @@ -67,7 +67,7 @@ bool Copter::ModeGuided::takeoff_start(float final_alt_above_home) } // initialise yaw - set_auto_yaw_mode(AUTO_YAW_HOLD); + auto_yaw.set_mode(AUTO_YAW_HOLD); // clear i term when we're taking off set_throttle_takeoff(); @@ -95,7 +95,7 @@ void Copter::ModeGuided::pos_control_start() wp_nav->set_wp_destination(stopping_point, false); // initialise yaw - set_auto_yaw_mode(copter.get_default_auto_yaw_mode(false)); + auto_yaw.set_mode_to_default(false); } // initialise guided mode's velocity controller @@ -140,7 +140,7 @@ void Copter::ModeGuided::posvel_control_start() pos_control->set_accel_z(wp_nav->get_accel_z()); // pilot always controls yaw - set_auto_yaw_mode(AUTO_YAW_HOLD); + auto_yaw.set_mode(AUTO_YAW_HOLD); } // initialise guided mode's angle controller @@ -169,7 +169,7 @@ void Copter::ModeGuided::angle_control_start() guided_angle_state.use_yaw_rate = false; // pilot always controls yaw - set_auto_yaw_mode(AUTO_YAW_HOLD); + auto_yaw.set_mode(AUTO_YAW_HOLD); } // guided_set_destination - sets guided mode's target destination @@ -417,7 +417,7 @@ void Copter::ModeGuided::pos_control_run() // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); if (!is_zero(target_yaw_rate)) { - set_auto_yaw_mode(AUTO_YAW_HOLD); + auto_yaw.set_mode(AUTO_YAW_HOLD); } } @@ -431,15 +431,15 @@ void Copter::ModeGuided::pos_control_run() pos_control->update_z_controller(); // call attitude controller - if (copter.auto_yaw_mode == AUTO_YAW_HOLD) { + 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); - } else if (auto_yaw_mode == AUTO_YAW_RATE) { + } 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()); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.rate_cds()); } 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(), get_auto_heading(), true); + attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.yaw(), true); } } @@ -461,7 +461,7 @@ void Copter::ModeGuided::vel_control_run() // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); if (!is_zero(target_yaw_rate)) { - set_auto_yaw_mode(AUTO_YAW_HOLD); + auto_yaw.set_mode(AUTO_YAW_HOLD); } } @@ -474,8 +474,8 @@ void Copter::ModeGuided::vel_control_run() if (!pos_control->get_desired_velocity().is_zero()) { set_desired_velocity_with_accel_and_fence_limits(Vector3f(0.0f, 0.0f, 0.0f)); } - if (auto_yaw_mode == AUTO_YAW_RATE) { - set_auto_yaw_rate(0.0f); + if (auto_yaw.mode() == AUTO_YAW_RATE) { + auto_yaw.set_rate(0.0f); } } else { set_desired_velocity_with_accel_and_fence_limits(guided_vel_target_cms); @@ -485,15 +485,15 @@ void Copter::ModeGuided::vel_control_run() pos_control->update_vel_controller_xyz(ekfNavVelGainScaler); // call attitude controller - if (copter.auto_yaw_mode == AUTO_YAW_HOLD) { + 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); - } else if (auto_yaw_mode == AUTO_YAW_RATE) { + } 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()); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), auto_yaw.rate_cds()); } 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(), get_auto_heading(), true); + attitude_control->input_euler_angle_roll_pitch_yaw(pos_control->get_roll(), pos_control->get_pitch(), auto_yaw.yaw(), true); } } @@ -517,7 +517,7 @@ void Copter::ModeGuided::posvel_control_run() // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); if (!is_zero(target_yaw_rate)) { - set_auto_yaw_mode(AUTO_YAW_HOLD); + auto_yaw.set_mode(AUTO_YAW_HOLD); } } @@ -528,8 +528,8 @@ void Copter::ModeGuided::posvel_control_run() uint32_t tnow = millis(); if (tnow - posvel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS) { guided_vel_target_cms.zero(); - if (auto_yaw_mode == AUTO_YAW_RATE) { - set_auto_yaw_rate(0.0f); + if (auto_yaw.mode() == AUTO_YAW_RATE) { + auto_yaw.set_rate(0.0f); } } @@ -553,15 +553,15 @@ void Copter::ModeGuided::posvel_control_run() pos_control->update_z_controller(); // call attitude controller - if (copter.auto_yaw_mode == AUTO_YAW_HOLD) { + 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); - } else if (auto_yaw_mode == AUTO_YAW_RATE) { + } 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()); + attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), auto_yaw.rate_cds()); } 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(), get_auto_heading(), true); + attitude_control->input_euler_angle_roll_pitch_yaw(pos_control->get_roll(), pos_control->get_pitch(), auto_yaw.yaw(), true); } } @@ -662,9 +662,9 @@ void Copter::ModeGuided::set_desired_velocity_with_accel_and_fence_limits(const void Copter::ModeGuided::set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle) { if (use_yaw) { - set_auto_yaw_look_at_heading(yaw_cd / 100.0f, 0.0f, 0, relative_angle); + auto_yaw.set_fixed_yaw(yaw_cd / 100.0f, 0.0f, 0, relative_angle); } else if (use_yaw_rate) { - set_auto_yaw_rate(yaw_rate_cds); + auto_yaw.set_rate(yaw_rate_cds); } } diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index cd85d694f5..b0cc07f1c7 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -108,7 +108,7 @@ void Copter::ModeRTL::climb_start() wp_nav->set_fast_waypoint(true); // hold current yaw during initial climb - set_auto_yaw_mode(AUTO_YAW_HOLD); + auto_yaw.set_mode(AUTO_YAW_HOLD); } // rtl_return_start - initialise return to home @@ -123,7 +123,7 @@ void Copter::ModeRTL::return_start() } // initialise yaw to point home (maybe) - set_auto_yaw_mode(copter.get_default_auto_yaw_mode(true)); + auto_yaw.set_mode_to_default(true); } // rtl_climb_return_run - implements the initial climb, return home and descent portions of RTL which all rely on the wp controller @@ -143,7 +143,7 @@ void Copter::ModeRTL::climb_return_run() // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); if (!is_zero(target_yaw_rate)) { - set_auto_yaw_mode(AUTO_YAW_HOLD); + auto_yaw.set_mode(AUTO_YAW_HOLD); } } @@ -157,12 +157,12 @@ void Copter::ModeRTL::climb_return_run() pos_control->update_z_controller(); // call attitude controller - if (copter.auto_yaw_mode == AUTO_YAW_HOLD) { + 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); }else{ // roll, pitch from waypoint controller, yaw heading from auto_heading() - attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(),true); + attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.yaw(),true); } // check if we've completed this stage of RTL @@ -177,10 +177,10 @@ void Copter::ModeRTL::loiterathome_start() _loiter_start_time = millis(); // yaw back to initial take-off heading yaw unless pilot has already overridden yaw - if(copter.get_default_auto_yaw_mode(true) != AUTO_YAW_HOLD) { - set_auto_yaw_mode(AUTO_YAW_RESETTOARMEDYAW); + if(auto_yaw.default_mode(true) != AUTO_YAW_HOLD) { + auto_yaw.set_mode(AUTO_YAW_RESETTOARMEDYAW); } else { - set_auto_yaw_mode(AUTO_YAW_HOLD); + auto_yaw.set_mode(AUTO_YAW_HOLD); } } @@ -201,7 +201,7 @@ void Copter::ModeRTL::loiterathome_run() // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); if (!is_zero(target_yaw_rate)) { - set_auto_yaw_mode(AUTO_YAW_HOLD); + auto_yaw.set_mode(AUTO_YAW_HOLD); } } @@ -215,17 +215,17 @@ void Copter::ModeRTL::loiterathome_run() pos_control->update_z_controller(); // call attitude controller - if (copter.auto_yaw_mode == AUTO_YAW_HOLD) { + 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); }else{ // roll, pitch from waypoint controller, yaw heading from auto_heading() - attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(),true); + attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.yaw(),true); } // check if we've completed this stage of RTL if ((millis() - _loiter_start_time) >= (uint32_t)g.rtl_loiter_time.get()) { - if (copter.auto_yaw_mode == AUTO_YAW_RESETTOARMEDYAW) { + if (auto_yaw.mode() == AUTO_YAW_RESETTOARMEDYAW) { // check if heading is within 2 degrees of heading when vehicle was armed if (labs(wrap_180_cd(ahrs.yaw_sensor-copter.initial_armed_bearing)) <= 200) { _state_complete = true; @@ -250,7 +250,7 @@ void Copter::ModeRTL::descent_start() pos_control->set_target_to_stopping_point_z(); // initialise yaw - set_auto_yaw_mode(AUTO_YAW_HOLD); + auto_yaw.set_mode(AUTO_YAW_HOLD); } // rtl_descent_run - implements the final descent to the RTL_ALT @@ -332,7 +332,7 @@ void Copter::ModeRTL::land_start() } // initialise yaw - set_auto_yaw_mode(AUTO_YAW_HOLD); + auto_yaw.set_mode(AUTO_YAW_HOLD); } bool Copter::ModeRTL::landing_gear_should_be_deployed() const diff --git a/ArduCopter/mode_smart_rtl.cpp b/ArduCopter/mode_smart_rtl.cpp index fa234c16fe..15e45e48b8 100644 --- a/ArduCopter/mode_smart_rtl.cpp +++ b/ArduCopter/mode_smart_rtl.cpp @@ -22,7 +22,7 @@ bool Copter::ModeSmartRTL::init(bool ignore_checks) wp_nav->set_wp_destination(stopping_point); // initialise yaw to obey user parameter - copter.set_auto_yaw_mode(copter.get_default_auto_yaw_mode(true)); + auto_yaw.set_mode_to_default(true); // wait for cleanup of return path smart_rtl_state = SmartRTL_WaitForPathCleanup; @@ -65,7 +65,7 @@ void Copter::ModeSmartRTL::wait_cleanup_run() motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); wp_nav->update_wpnav(); pos_control->update_z_controller(); - attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(),true); + attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.yaw(),true); // check if return path is computed and if yes, begin journey home if (g2.smart_rtl.request_thorough_cleanup()) { @@ -101,12 +101,12 @@ void Copter::ModeSmartRTL::path_follow_run() pos_control->update_z_controller(); // call attitude controller - if (auto_yaw_mode == AUTO_YAW_HOLD) { + 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(), 0); } else { // roll, pitch from waypoint controller, yaw heading from auto_heading() - attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(),true); + attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.yaw(), true); } } @@ -129,7 +129,7 @@ void Copter::ModeSmartRTL::pre_land_position_run() motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); wp_nav->update_wpnav(); pos_control->update_z_controller(); - attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(), true); + attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.yaw(), true); } // save current position for use by the smart_rtl flight mode