Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: auto yaw re-work

This commit is contained in:
Iampete1 2022-09-24 15:12:33 +01:00 committed by Randy Mackay
parent aed694316d
commit 55e72a9848
11 changed files with 197 additions and 378 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

134
ArduCopter/autoyaw.cpp
View File

@ -26,31 +26,31 @@ void Mode::AutoYaw::set_mode_to_default(bool rtl)
// default_mode - returns auto_yaw.mode() based on WP_YAW_BEHAVIOR parameter
// set rtl parameter to true if this is during an RTL
autopilot_yaw_mode Mode::AutoYaw::default_mode(bool rtl) const
Mode::AutoYaw::Mode Mode::AutoYaw::default_mode(bool rtl) const
{
switch (copter.g.wp_yaw_behavior) {
case WP_YAW_BEHAVIOR_NONE:
return AUTO_YAW_HOLD;
return Mode::HOLD;
case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL:
if (rtl) {
return AUTO_YAW_HOLD;
return Mode::HOLD;
} else {
return AUTO_YAW_LOOK_AT_NEXT_WP;
return Mode::LOOK_AT_NEXT_WP;
}
case WP_YAW_BEHAVIOR_LOOK_AHEAD:
return AUTO_YAW_LOOK_AHEAD;
return Mode::LOOK_AHEAD;
case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP:
default:
return AUTO_YAW_LOOK_AT_NEXT_WP;
return Mode::LOOK_AT_NEXT_WP;
}
}
// set_mode - sets the yaw mode for auto
void Mode::AutoYaw::set_mode(autopilot_yaw_mode yaw_mode)
void Mode::AutoYaw::set_mode(Mode yaw_mode)
{
// return immediately if no change
if (_mode == yaw_mode) {
@ -61,40 +61,41 @@ void Mode::AutoYaw::set_mode(autopilot_yaw_mode yaw_mode)
// perform initialisation
switch (_mode) {
case AUTO_YAW_HOLD:
case Mode::HOLD:
break;
case AUTO_YAW_LOOK_AT_NEXT_WP:
case Mode::LOOK_AT_NEXT_WP:
// wpnav will initialise heading when wpnav's set_destination method is called
break;
case AUTO_YAW_ROI:
case Mode::ROI:
// look ahead until we know otherwise
break;
case AUTO_YAW_FIXED:
case Mode::FIXED:
// keep heading pointing in the direction held in fixed_yaw
// caller should set the fixed_yaw
break;
case AUTO_YAW_LOOK_AHEAD:
case Mode::LOOK_AHEAD:
// Commanded Yaw to automatically look ahead.
_look_ahead_yaw = copter.ahrs.yaw_sensor;
break;
case AUTO_YAW_RESETTOARMEDYAW:
case Mode::RESETTOARMEDYAW:
// initial_armed_bearing will be set during arming so no init required
break;
case AUTO_YAW_ANGLE_RATE:
case Mode::ANGLE_RATE:
break;
case AUTO_YAW_RATE:
case Mode::RATE:
// initialise target yaw rate to zero
_yaw_rate_cds = 0.0f;
break;
case AUTO_YAW_CIRCLE:
case Mode::CIRCLE:
case Mode::PILOT_RATE:
// no initialisation required
break;
}
@ -130,7 +131,7 @@ void Mode::AutoYaw::set_fixed_yaw(float angle_deg, float turn_rate_ds, int8_t di
}
// set yaw mode
set_mode(AUTO_YAW_FIXED);
set_mode(Mode::FIXED);
}
// set_fixed_yaw - sets the yaw look at heading for auto mode
@ -142,7 +143,7 @@ void Mode::AutoYaw::set_yaw_angle_rate(float yaw_angle_d, float yaw_rate_ds)
_yaw_rate_cds = yaw_rate_ds * 100.0;
// set yaw mode
set_mode(AUTO_YAW_ANGLE_RATE);
set_mode(Mode::ANGLE_RATE);
}
// set_roi - sets the yaw to look at roi for auto mode
@ -163,7 +164,7 @@ void Mode::AutoYaw::set_roi(const Location &roi_location)
// check if mount type requires us to rotate the quad
if (!copter.camera_mount.has_pan_control()) {
if (roi_location.get_vector_from_origin_NEU(roi)) {
auto_yaw.set_mode(AUTO_YAW_ROI);
auto_yaw.set_mode(Mode::ROI);
}
}
// send the command to the camera mount
@ -178,7 +179,7 @@ void Mode::AutoYaw::set_roi(const Location &roi_location)
#else
// if we have no camera mount aim the quad at the location
if (roi_location.get_vector_from_origin_NEU(roi)) {
auto_yaw.set_mode(AUTO_YAW_ROI);
auto_yaw.set_mode(Mode::ROI);
}
#endif // HAL_MOUNT_ENABLED
}
@ -187,20 +188,37 @@ void Mode::AutoYaw::set_roi(const Location &roi_location)
// set auto yaw rate in centi-degrees per second
void Mode::AutoYaw::set_rate(float turn_rate_cds)
{
set_mode(AUTO_YAW_RATE);
set_mode(Mode::RATE);
_yaw_rate_cds = turn_rate_cds;
}
// return true if fixed yaw target has been reached
bool Mode::AutoYaw::reached_fixed_yaw_target()
{
if (mode() != Mode::FIXED) {
// should not happen, not in the right mode
return true;
}
if (!is_zero(_fixed_yaw_offset_cd)) {
// still slewing yaw target
return false;
}
// Within 2 deg of target
return (fabsf(wrap_180_cd(copter.ahrs.yaw_sensor-_yaw_angle_cd)) <= 200);
}
// yaw - returns target heading depending upon auto_yaw.mode()
float Mode::AutoYaw::yaw()
{
switch (_mode) {
case AUTO_YAW_ROI:
case Mode::ROI:
// point towards a location held in roi
return roi_yaw();
case AUTO_YAW_FIXED: {
case Mode::FIXED: {
// keep heading pointing in the direction held in fixed_yaw
// with no pilot input allowed
const uint32_t now_ms = millis();
@ -212,15 +230,15 @@ float Mode::AutoYaw::yaw()
return _yaw_angle_cd;
}
case AUTO_YAW_LOOK_AHEAD:
case Mode::LOOK_AHEAD:
// Commanded Yaw to automatically look ahead.
return look_ahead_yaw();
case AUTO_YAW_RESETTOARMEDYAW:
case Mode::RESETTOARMEDYAW:
// changes yaw to be same as when quad was armed
return copter.initial_armed_bearing;
case AUTO_YAW_CIRCLE:
case Mode::CIRCLE:
#if MODE_CIRCLE_ENABLED
if (copter.circle_nav->is_active()) {
return copter.circle_nav->get_yaw();
@ -229,7 +247,7 @@ 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:{
case Mode::ANGLE_RATE:{
const uint32_t now_ms = millis();
float dt = (now_ms - _last_update_ms) * 0.001;
_last_update_ms = now_ms;
@ -237,7 +255,7 @@ float Mode::AutoYaw::yaw()
return _yaw_angle_cd;
}
case AUTO_YAW_LOOK_AT_NEXT_WP:
case Mode::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
@ -251,22 +269,64 @@ float Mode::AutoYaw::rate_cds() const
{
switch (_mode) {
case AUTO_YAW_HOLD:
case AUTO_YAW_ROI:
case AUTO_YAW_FIXED:
case AUTO_YAW_LOOK_AHEAD:
case AUTO_YAW_RESETTOARMEDYAW:
case AUTO_YAW_CIRCLE:
case Mode::HOLD:
case Mode::ROI:
case Mode::FIXED:
case Mode::LOOK_AHEAD:
case Mode::RESETTOARMEDYAW:
case Mode::CIRCLE:
return 0.0f;
case AUTO_YAW_ANGLE_RATE:
case AUTO_YAW_RATE:
case Mode::ANGLE_RATE:
case Mode::RATE:
return _yaw_rate_cds;
case AUTO_YAW_LOOK_AT_NEXT_WP:
case Mode::LOOK_AT_NEXT_WP:
return copter.pos_control->get_yaw_rate_cds();
case Mode::PILOT_RATE:
return _pilot_yaw_rate_cds;
}
// return zero turn rate (this should never happen)
return 0.0f;
}
AC_AttitudeControl::HeadingCommand Mode::AutoYaw::get_heading()
{
// process pilot's yaw input
_pilot_yaw_rate_cds = 0.0;
if (!copter.failsafe.radio && copter.flightmode->use_pilot_yaw()) {
// get pilot's desired yaw rate
_pilot_yaw_rate_cds = copter.flightmode->get_pilot_desired_yaw_rate(copter.channel_yaw->norm_input_dz());
if (!is_zero(_pilot_yaw_rate_cds)) {
auto_yaw.set_mode(AutoYaw::Mode::PILOT_RATE);
}
} else if (auto_yaw.mode() == AutoYaw::Mode::PILOT_RATE) {
// RC failsafe, or disabled make sure not in pilot control
auto_yaw.set_mode(AutoYaw::Mode::HOLD);
}
AC_AttitudeControl::HeadingCommand heading;
heading.yaw_angle_cd = yaw();
heading.yaw_rate_cds = auto_yaw.rate_cds();
switch (auto_yaw.mode()) {
case Mode::HOLD:
case Mode::RATE:
case Mode::PILOT_RATE:
heading.heading_mode = AC_AttitudeControl::HeadingMode::Rate_Only;
break;
case Mode::LOOK_AT_NEXT_WP:
case Mode::ROI:
case Mode::FIXED:
case Mode::LOOK_AHEAD:
case Mode::RESETTOARMEDYAW:
case Mode::ANGLE_RATE:
case Mode::CIRCLE:
heading.heading_mode = AC_AttitudeControl::HeadingMode::Angle_And_Rate;
break;
}
return heading;
}

13
ArduCopter/defines.h
View File

@ -10,19 +10,6 @@
#define ENABLE ENABLED
#define DISABLE DISABLED
// Autopilot Yaw Mode enumeration
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 (no 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_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
#define UNDEFINED_FRAME 0
#define MULTICOPTER_FRAME 1

33
ArduCopter/mode.cpp
View File

@ -636,7 +636,6 @@ void Mode::land_run_vertical_control(bool pause_descent)
void Mode::land_run_horizontal_control()
{
Vector2f vel_correction;
float target_yaw_rate = 0;
// relax loiter target if we might be landed
if (copter.ap.land_complete_maybe) {
@ -671,12 +670,6 @@ void Mode::land_run_horizontal_control()
copter.ap.land_repo_active = true;
}
}
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
if (!is_zero(target_yaw_rate)) {
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
}
// this variable will be updated if prec land target is in sight and pilot isn't trying to reposition the vehicle
@ -730,13 +723,8 @@ void Mode::land_run_horizontal_control()
}
// call attitude controller
if (auto_yaw.mode() == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control->input_thrust_vector_rate_heading(thrust_vector, target_yaw_rate);
} else {
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control->input_thrust_vector_heading(thrust_vector, auto_yaw.yaw());
}
attitude_control->input_thrust_vector_heading(thrust_vector, auto_yaw.get_heading());
}
// run normal or precision landing (if enabled)
@ -763,7 +751,6 @@ void Mode::land_run_normal_or_precland(bool pause_descent)
// The passed in location is expected to be NED and in m
void Mode::precland_retry_position(const Vector3f &retry_pos)
{
float target_yaw_rate = 0;
if (!copter.failsafe.radio) {
if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
AP::logger().Write_Event(LogEvent::LAND_CANCELLED_BY_PILOT);
@ -790,11 +777,6 @@ void Mode::precland_retry_position(const Vector3f &retry_pos)
copter.ap.land_repo_active = true;
}
}
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
if (!is_zero(target_yaw_rate)) {
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
}
Vector3p retry_pos_NEU{retry_pos.x, retry_pos.y, retry_pos.z * -1.0f};
@ -806,16 +788,9 @@ void Mode::precland_retry_position(const Vector3f &retry_pos)
pos_control->update_xy_controller();
pos_control->update_z_controller();
const Vector3f thrust_vector{pos_control->get_thrust_vector()};
// call attitude controller
if (auto_yaw.mode() == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control->input_thrust_vector_rate_heading(thrust_vector, target_yaw_rate);
} else {
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control->input_thrust_vector_heading(thrust_vector, auto_yaw.yaw());
}
attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
}
// Run precland statemachine. This function should be called from any mode that wants to do precision landing.

40
ArduCopter/mode.h
View File

@ -232,17 +232,27 @@ public:
public:
// yaw(): main product of AutoYaw; the heading:
float yaw();
// Autopilot Yaw Mode enumeration
enum class Mode {
HOLD = 0, // hold zero yaw rate
LOOK_AT_NEXT_WP = 1, // point towards next waypoint (no pilot input accepted)
ROI = 2, // point towards a location held in roi (no pilot input accepted)
FIXED = 3, // point towards a particular angle (no pilot input accepted)
LOOK_AHEAD = 4, // point in the direction the copter is moving
RESETTOARMEDYAW = 5, // point towards heading at time motors were armed
ANGLE_RATE = 6, // turn at a specified rate from a starting angle
RATE = 7, // turn at a specified rate (held in auto_yaw_rate)
CIRCLE = 8, // use AC_Circle's provided yaw (used during Loiter-Turns commands)
PILOT_RATE = 9, // target rate from pilot stick
};
// mode(): current method of determining desired yaw:
autopilot_yaw_mode mode() const { return (autopilot_yaw_mode)_mode; }
Mode mode() const { return _mode; }
void set_mode_to_default(bool rtl);
void set_mode(autopilot_yaw_mode new_mode);
autopilot_yaw_mode default_mode(bool rtl) const;
void set_mode(Mode new_mode);
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:
@ -255,17 +265,25 @@ public:
void set_yaw_angle_rate(float yaw_angle_d, float yaw_rate_ds);
bool fixed_yaw_slew_finished() { return is_zero(_fixed_yaw_offset_cd); }
bool reached_fixed_yaw_target();
AC_AttitudeControl::HeadingCommand get_heading();
private:
// yaw(): main product of AutoYaw; the heading:
float yaw();
// rate_cds(): desired yaw rate in centidegrees/second:
float rate_cds() const;
float look_ahead_yaw();
float roi_yaw() const;
// auto flight mode's yaw mode
uint8_t _mode = AUTO_YAW_LOOK_AT_NEXT_WP;
Mode _mode = Mode::LOOK_AT_NEXT_WP;
// Yaw will point at this location if mode is set to AUTO_YAW_ROI
// Yaw will point at this location if mode is set to Mode::ROI
Vector3f roi;
// yaw used for YAW_FIXED yaw_mode
@ -283,6 +301,7 @@ public:
// turn rate (in cds) when auto_yaw_mode is set to AUTO_YAW_RATE
float _yaw_angle_cd;
float _yaw_rate_cds;
float _pilot_yaw_rate_cds;
};
static AutoYaw auto_yaw;
@ -763,7 +782,6 @@ protected:
private:
// Circle
bool pilot_yaw_override = false; // true if pilot is overriding yaw
bool speed_changing = false; // true when the roll stick is being held to facilitate stopping at 0 rate
};

81
ArduCopter/mode_auto.cpp
View File

@ -34,8 +34,8 @@ bool 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 (auto_yaw.mode() == AUTO_YAW_ROI) {
auto_yaw.set_mode(AUTO_YAW_HOLD);
if (auto_yaw.mode() == AutoYaw::Mode::ROI) {
auto_yaw.set_mode(AutoYaw::Mode::HOLD);
}
// initialise waypoint and spline controller
@ -277,7 +277,7 @@ bool ModeAuto::loiter_start()
wp_nav->set_wp_destination(stopping_point);
// hold yaw at current heading
auto_yaw.set_mode(AUTO_YAW_HOLD);
auto_yaw.set_mode(AutoYaw::Mode::HOLD);
return true;
}
@ -337,7 +337,7 @@ void ModeAuto::takeoff_start(const Location& dest_loc)
alt_target_cm = MAX(alt_target_cm, alt_target_min_cm);
// initialise yaw
auto_yaw.set_mode(AUTO_YAW_HOLD);
auto_yaw.set_mode(AutoYaw::Mode::HOLD);
// clear i term when we're taking off
pos_control->init_z_controller();
@ -373,7 +373,7 @@ void ModeAuto::wp_start(const Location& 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 (auto_yaw.mode() != AUTO_YAW_ROI) {
if (auto_yaw.mode() != AutoYaw::Mode::ROI) {
auto_yaw.set_mode_to_default(false);
}
@ -403,7 +403,7 @@ void ModeAuto::land_start()
}
// initialise yaw
auto_yaw.set_mode(AUTO_YAW_HOLD);
auto_yaw.set_mode(AutoYaw::Mode::HOLD);
#if LANDING_GEAR_ENABLED == ENABLED
// optionally deploy landing gear
@ -460,12 +460,12 @@ void ModeAuto::circle_movetoedge_start(const Location &circle_center, float radi
const float dist_to_center = get_horizontal_distance_cm(inertial_nav.get_position_xy_cm().topostype(), copter.circle_nav->get_center().xy());
// 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 (auto_yaw.mode() != AUTO_YAW_ROI) {
if (auto_yaw.mode() != AutoYaw::Mode::ROI) {
if (dist_to_center > copter.circle_nav->get_radius() && dist_to_center > 500) {
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
auto_yaw.set_mode(AUTO_YAW_HOLD);
auto_yaw.set_mode(AutoYaw::Mode::HOLD);
}
}
@ -483,8 +483,8 @@ void ModeAuto::circle_start()
// initialise circle controller
copter.circle_nav->init(copter.circle_nav->get_center(), copter.circle_nav->center_is_terrain_alt());
if (auto_yaw.mode() != AUTO_YAW_ROI) {
auto_yaw.set_mode(AUTO_YAW_CIRCLE);
if (auto_yaw.mode() != AutoYaw::Mode::ROI) {
auto_yaw.set_mode(AutoYaw::Mode::CIRCLE);
}
// set submode to circle
@ -552,7 +552,7 @@ void ModeAuto::payload_place_start()
}
// initialise yaw
auto_yaw.set_mode(AUTO_YAW_HOLD);
auto_yaw.set_mode(AutoYaw::Mode::HOLD);
// set submode
set_submode(SubMode::NAV_PAYLOAD_PLACE);
@ -958,16 +958,6 @@ void ModeAuto::takeoff_run()
// called by auto_run at 100hz or more
void ModeAuto::wp_run()
{
// process pilot's yaw input
float target_yaw_rate = 0;
if (!copter.failsafe.radio && use_pilot_yaw()) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
if (!is_zero(target_yaw_rate)) {
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
}
// if not armed set throttle to zero and exit immediately
if (is_disarmed_or_landed()) {
make_safe_ground_handling();
@ -984,14 +974,8 @@ void ModeAuto::wp_run()
// run the vertical position controller and set output throttle
pos_control->update_z_controller();
// call attitude controller
if (auto_yaw.mode() == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control->input_thrust_vector_rate_heading(wp_nav->get_thrust_vector(), target_yaw_rate);
} else {
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control->input_thrust_vector_heading(wp_nav->get_thrust_vector(), auto_yaw.yaw(), auto_yaw.rate_cds());
}
// call attitude controller with auto yaw
attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
}
// auto_land_run - lands in auto mode
@ -1024,16 +1008,6 @@ void ModeAuto::rtl_run()
// called by auto_run at 100hz or more
void ModeAuto::circle_run()
{
// process pilot's yaw input
float target_yaw_rate = 0;
if (!copter.failsafe.radio && use_pilot_yaw()) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
if (!is_zero(target_yaw_rate)) {
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
}
// call circle controller
copter.failsafe_terrain_set_status(copter.circle_nav->update());
@ -1041,13 +1015,8 @@ void ModeAuto::circle_run()
// run the vertical position controller and set output throttle
pos_control->update_z_controller();
if (auto_yaw.mode() == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control->input_thrust_vector_rate_heading(copter.circle_nav->get_thrust_vector(), target_yaw_rate);
} else {
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control->input_thrust_vector_heading(copter.circle_nav->get_thrust_vector(), auto_yaw.yaw());
}
// call attitude controller with auto yaw
attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
}
#if NAV_GUIDED == ENABLED || AP_SCRIPTING_ENABLED
@ -1070,12 +1039,6 @@ void ModeAuto::loiter_run()
return;
}
// accept pilot input of yaw
float target_yaw_rate = 0;
if (!copter.failsafe.radio && use_pilot_yaw()) {
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
@ -1083,7 +1046,9 @@ void ModeAuto::loiter_run()
copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
pos_control->update_z_controller();
attitude_control->input_thrust_vector_rate_heading(wp_nav->get_thrust_vector(), target_yaw_rate);
// call attitude controller with auto yaw
attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
}
// auto_loiter_run - loiter to altitude in AUTO flight mode
@ -1355,7 +1320,7 @@ void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd)
// 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 (auto_yaw.mode() != AUTO_YAW_ROI) {
if (auto_yaw.mode() != AutoYaw::Mode::ROI) {
auto_yaw.set_mode_to_default(false);
}
@ -1577,7 +1542,7 @@ void ModeAuto::do_spline_wp(const AP_Mission::Mission_Command& cmd)
// 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 (auto_yaw.mode() != AUTO_YAW_ROI) {
if (auto_yaw.mode() != AutoYaw::Mode::ROI) {
auto_yaw.set_mode_to_default(false);
}
@ -2107,10 +2072,10 @@ bool ModeAuto::verify_within_distance()
bool ModeAuto::verify_yaw()
{
// make sure still in fixed yaw mode, the waypoint controller often retakes control of yaw as it executes a new waypoint command
auto_yaw.set_mode(AUTO_YAW_FIXED);
auto_yaw.set_mode(AutoYaw::Mode::FIXED);
// check if we are within 2 degrees of the target heading
return auto_yaw.fixed_yaw_slew_finished() && (fabsf(wrap_180_cd(ahrs.yaw_sensor-auto_yaw.yaw())) <= 200);
// check if we have reached the target heading
return auto_yaw.reached_fixed_yaw_target();
}
// verify_nav_wp - check if we have reached the next way point

17
ArduCopter/mode_circle.cpp
View File

@ -9,7 +9,6 @@
// circle_init - initialise circle controller flight mode
bool ModeCircle::init(bool ignore_checks)
{
pilot_yaw_override = false;
speed_changing = false;
// set speed and acceleration limits
@ -32,12 +31,6 @@ void ModeCircle::run()
pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
// get pilot's desired yaw rate (or zero if in radio failsafe)
float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
if (!is_zero(target_yaw_rate)) {
pilot_yaw_override = true;
}
// Check for any change in params and update in real time
copter.circle_nav->check_param_change();
@ -106,14 +99,10 @@ void ModeCircle::run()
copter.surface_tracking.update_surface_offset();
copter.failsafe_terrain_set_status(copter.circle_nav->update(target_climb_rate));
// call attitude controller
if (pilot_yaw_override) {
attitude_control->input_thrust_vector_rate_heading(copter.circle_nav->get_thrust_vector(), target_yaw_rate);
} else {
attitude_control->input_thrust_vector_heading(copter.circle_nav->get_thrust_vector(), copter.circle_nav->get_yaw());
}
pos_control->update_z_controller();
// call attitude controller with auto yaw
attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
}
uint32_t ModeCircle::wp_distance() const

139
ArduCopter/mode_guided.cpp
View File

@ -140,7 +140,7 @@ bool ModeGuided::do_user_takeoff_start(float takeoff_alt_cm)
guided_mode = SubMode::TakeOff;
// initialise yaw
auto_yaw.set_mode(AUTO_YAW_HOLD);
auto_yaw.set_mode(AutoYaw::Mode::HOLD);
// clear i term when we're taking off
pos_control->init_z_controller();
@ -178,16 +178,6 @@ void ModeGuided::wp_control_start()
// run guided mode's waypoint navigation controller
void ModeGuided::wp_control_run()
{
// process pilot's yaw input
float target_yaw_rate = 0;
if (!copter.failsafe.radio && use_pilot_yaw()) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
if (!is_zero(target_yaw_rate)) {
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
}
// if not armed set throttle to zero and exit immediately
if (is_disarmed_or_landed()) {
// do not spool down tradheli when on the ground with motor interlock enabled
@ -204,17 +194,8 @@ void ModeGuided::wp_control_run()
// call z-axis position controller (wpnav should have already updated it's alt target)
pos_control->update_z_controller();
// call attitude controller
if (auto_yaw.mode() == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control->input_thrust_vector_rate_heading(wp_nav->get_thrust_vector(), target_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_thrust_vector_rate_heading(wp_nav->get_thrust_vector(), auto_yaw.rate_cds());
} else {
// roll, pitch from waypoint controller, yaw heading from GCS or auto_heading()
attitude_control->input_thrust_vector_heading(wp_nav->get_thrust_vector(), auto_yaw.yaw());
}
// call attitude controller with auto yaw
attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
}
// initialise position controller
@ -332,7 +313,7 @@ void ModeGuided::angle_control_start()
guided_angle_state.use_yaw_rate = false;
// pilot always controls yaw
auto_yaw.set_mode(AUTO_YAW_HOLD);
auto_yaw.set_mode(AutoYaw::Mode::HOLD);
}
// set_destination - sets guided mode's target destination
@ -682,17 +663,6 @@ void ModeGuided::takeoff_run()
// called from guided_run
void ModeGuided::pos_control_run()
{
// process pilot's yaw input
float target_yaw_rate = 0;
if (!copter.failsafe.radio && use_pilot_yaw()) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
if (!is_zero(target_yaw_rate)) {
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
}
// if not armed set throttle to zero and exit immediately
if (is_disarmed_or_landed()) {
// do not spool down tradheli when on the ground with motor interlock enabled
@ -717,8 +687,8 @@ void ModeGuided::pos_control_run()
// stop rotating if no updates received within timeout_ms
if (millis() - update_time_ms > get_timeout_ms()) {
if ((auto_yaw.mode() == AUTO_YAW_RATE) || (auto_yaw.mode() == AUTO_YAW_ANGLE_RATE)) {
auto_yaw.set_rate(0.0f);
if ((auto_yaw.mode() == AutoYaw::Mode::RATE) || (auto_yaw.mode() == AutoYaw::Mode::ANGLE_RATE)) {
auto_yaw.set_mode(AutoYaw::Mode::HOLD);
}
}
@ -732,33 +702,14 @@ void ModeGuided::pos_control_run()
pos_control->update_xy_controller();
pos_control->update_z_controller();
// call attitude controller
if (auto_yaw.mode() == AUTO_YAW_HOLD) {
// roll & pitch from position controller, yaw rate from pilot
attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), target_yaw_rate);
} else if (auto_yaw.mode() == AUTO_YAW_RATE) {
// roll & pitch from position controller, yaw rate from mavlink command or mission item
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(), auto_yaw.rate_cds());
}
// call attitude controller with auto yaw
attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
}
// velaccel_control_run - runs the guided velocity controller
// called from guided_run
void ModeGuided::accel_control_run()
{
// process pilot's yaw input
float target_yaw_rate = 0;
if (!copter.failsafe.radio && use_pilot_yaw()) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
if (!is_zero(target_yaw_rate)) {
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
}
// if not armed set throttle to zero and exit immediately
if (is_disarmed_or_landed()) {
// do not spool down tradheli when on the ground with motor interlock enabled
@ -774,8 +725,8 @@ void ModeGuided::accel_control_run()
if (tnow - update_time_ms > get_timeout_ms()) {
guided_vel_target_cms.zero();
guided_accel_target_cmss.zero();
if ((auto_yaw.mode() == AUTO_YAW_RATE) || (auto_yaw.mode() == AUTO_YAW_ANGLE_RATE)) {
auto_yaw.set_rate(0.0f);
if ((auto_yaw.mode() == AutoYaw::Mode::RATE) || (auto_yaw.mode() == AutoYaw::Mode::ANGLE_RATE)) {
auto_yaw.set_mode(AutoYaw::Mode::HOLD);
}
pos_control->input_vel_accel_xy(guided_vel_target_cms.xy(), guided_accel_target_cmss.xy(), false);
pos_control->input_vel_accel_z(guided_vel_target_cms.z, guided_accel_target_cmss.z, false, false);
@ -796,33 +747,14 @@ void ModeGuided::accel_control_run()
pos_control->update_xy_controller();
pos_control->update_z_controller();
// call attitude controller
if (auto_yaw.mode() == AUTO_YAW_HOLD) {
// roll & pitch from position controller, yaw rate from pilot
attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), target_yaw_rate);
} else if (auto_yaw.mode() == AUTO_YAW_RATE) {
// roll & pitch from position controller, yaw rate from mavlink command or mission item
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(), auto_yaw.rate_cds());
}
// call attitude controller with auto yaw
attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
}
// velaccel_control_run - runs the guided velocity and acceleration controller
// called from guided_run
void ModeGuided::velaccel_control_run()
{
// process pilot's yaw input
float target_yaw_rate = 0;
if (!copter.failsafe.radio && use_pilot_yaw()) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
if (!is_zero(target_yaw_rate)) {
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
}
// if not armed set throttle to zero and exit immediately
if (is_disarmed_or_landed()) {
// do not spool down tradheli when on the ground with motor interlock enabled
@ -838,8 +770,8 @@ void ModeGuided::velaccel_control_run()
if (tnow - update_time_ms > get_timeout_ms()) {
guided_vel_target_cms.zero();
guided_accel_target_cmss.zero();
if ((auto_yaw.mode() == AUTO_YAW_RATE) || (auto_yaw.mode() == AUTO_YAW_ANGLE_RATE)) {
auto_yaw.set_rate(0.0f);
if ((auto_yaw.mode() == AutoYaw::Mode::RATE) || (auto_yaw.mode() == AutoYaw::Mode::ANGLE_RATE)) {
auto_yaw.set_mode(AutoYaw::Mode::HOLD);
}
}
@ -871,17 +803,8 @@ void ModeGuided::velaccel_control_run()
pos_control->update_xy_controller();
pos_control->update_z_controller();
// call attitude controller
if (auto_yaw.mode() == AUTO_YAW_HOLD) {
// roll & pitch from position controller, yaw rate from pilot
attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), target_yaw_rate);
} else if (auto_yaw.mode() == AUTO_YAW_RATE) {
// roll & pitch from position controller, yaw rate from mavlink command or mission item
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(), auto_yaw.rate_cds());
}
// call attitude controller with auto yaw
attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
}
// pause_control_run - runs the guided mode pause controller
@ -911,24 +834,13 @@ void ModeGuided::pause_control_run()
pos_control->update_z_controller();
// call attitude controller
attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), 0.0);
attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
}
// posvelaccel_control_run - runs the guided position, velocity and acceleration controller
// called from guided_run
void ModeGuided::posvelaccel_control_run()
{
// process pilot's yaw input
float target_yaw_rate = 0;
if (!copter.failsafe.radio && use_pilot_yaw()) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
if (!is_zero(target_yaw_rate)) {
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
}
// if not armed set throttle to zero and exit immediately
if (is_disarmed_or_landed()) {
// do not spool down tradheli when on the ground with motor interlock enabled
@ -944,8 +856,8 @@ void ModeGuided::posvelaccel_control_run()
if (tnow - update_time_ms > get_timeout_ms()) {
guided_vel_target_cms.zero();
guided_accel_target_cmss.zero();
if ((auto_yaw.mode() == AUTO_YAW_RATE) || (auto_yaw.mode() == AUTO_YAW_ANGLE_RATE)) {
auto_yaw.set_rate(0.0f);
if ((auto_yaw.mode() == AutoYaw::Mode::RATE) || (auto_yaw.mode() == AutoYaw::Mode::ANGLE_RATE)) {
auto_yaw.set_mode(AutoYaw::Mode::HOLD);
}
}
@ -983,17 +895,8 @@ void ModeGuided::posvelaccel_control_run()
pos_control->update_xy_controller();
pos_control->update_z_controller();
// call attitude controller
if (auto_yaw.mode() == AUTO_YAW_HOLD) {
// roll & pitch from position controller, yaw rate from pilot
attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), target_yaw_rate);
} else if (auto_yaw.mode() == AUTO_YAW_RATE) {
// roll & pitch from position controller, yaw rate from mavlink command or mission item
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(), auto_yaw.rate_cds());
}
// call attitude controller with auto yaw
attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
}
// angle_control_run - runs the guided angle controller

10
ArduCopter/mode_land.cpp
View File

@ -34,7 +34,7 @@ bool ModeLand::init(bool ignore_checks)
copter.ap.prec_land_active = false;
// initialise yaw
auto_yaw.set_mode(AUTO_YAW_HOLD);
auto_yaw.set_mode(AutoYaw::Mode::HOLD);
#if LANDING_GEAR_ENABLED == ENABLED
// optionally deploy landing gear
@ -98,7 +98,6 @@ void ModeLand::gps_run()
void ModeLand::nogps_run()
{
float target_roll = 0.0f, target_pitch = 0.0f;
float target_yaw_rate = 0;
// process pilot inputs
if (!copter.failsafe.radio) {
@ -116,11 +115,6 @@ void ModeLand::nogps_run()
get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, attitude_control->get_althold_lean_angle_max_cd());
}
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
if (!is_zero(target_yaw_rate)) {
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
}
// disarm when the landing detector says we've landed
@ -144,7 +138,7 @@ void ModeLand::nogps_run()
}
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, auto_yaw.get_heading().yaw_rate_cds);
}
// do_not_use_GPS - forces land-mode to not use the GPS but instead rely on pilot input for roll and pitch

62
ArduCopter/mode_rtl.cpp
View File

@ -138,7 +138,7 @@ void ModeRTL::climb_start()
}
// hold current yaw during initial climb
auto_yaw.set_mode(AUTO_YAW_HOLD);
auto_yaw.set_mode(AutoYaw::Mode::HOLD);
}
// rtl_return_start - initialise return to home
@ -166,16 +166,6 @@ void ModeRTL::climb_return_run()
return;
}
// process pilot's yaw input
float target_yaw_rate = 0;
if (!copter.failsafe.radio && use_pilot_yaw()) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
if (!is_zero(target_yaw_rate)) {
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
@ -186,14 +176,8 @@ void ModeRTL::climb_return_run()
// run the vertical position controller and set output throttle
pos_control->update_z_controller();
// call attitude controller
if (auto_yaw.mode() == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control->input_thrust_vector_rate_heading(wp_nav->get_thrust_vector(), target_yaw_rate);
} else {
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control->input_thrust_vector_heading(wp_nav->get_thrust_vector(), auto_yaw.yaw());
}
// call attitude controller with auto yaw
attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
// check if we've completed this stage of RTL
_state_complete = wp_nav->reached_wp_destination();
@ -207,10 +191,10 @@ void ModeRTL::loiterathome_start()
_loiter_start_time = millis();
// yaw back to initial take-off heading yaw unless pilot has already overridden yaw
if (auto_yaw.default_mode(true) != AUTO_YAW_HOLD) {
auto_yaw.set_mode(AUTO_YAW_RESETTOARMEDYAW);
if (auto_yaw.default_mode(true) != AutoYaw::Mode::HOLD) {
auto_yaw.set_mode(AutoYaw::Mode::RESETTOARMEDYAW);
} else {
auto_yaw.set_mode(AUTO_YAW_HOLD);
auto_yaw.set_mode(AutoYaw::Mode::HOLD);
}
}
@ -224,16 +208,6 @@ void ModeRTL::loiterathome_run()
return;
}
// process pilot's yaw input
float target_yaw_rate = 0;
if (!copter.failsafe.radio && use_pilot_yaw()) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
if (!is_zero(target_yaw_rate)) {
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
@ -244,18 +218,12 @@ void ModeRTL::loiterathome_run()
// run the vertical position controller and set output throttle
pos_control->update_z_controller();
// call attitude controller
if (auto_yaw.mode() == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control->input_thrust_vector_rate_heading(wp_nav->get_thrust_vector(), target_yaw_rate);
} else {
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control->input_thrust_vector_heading(wp_nav->get_thrust_vector(), auto_yaw.yaw());
}
// call attitude controller with auto yaw
attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
// check if we've completed this stage of RTL
if ((millis() - _loiter_start_time) >= (uint32_t)g.rtl_loiter_time.get()) {
if (auto_yaw.mode() == AUTO_YAW_RESETTOARMEDYAW) {
if (auto_yaw.mode() == AutoYaw::Mode::RESETTOARMEDYAW) {
// check if heading is within 2 degrees of heading when vehicle was armed
if (abs(wrap_180_cd(ahrs.yaw_sensor-copter.initial_armed_bearing)) <= 200) {
_state_complete = true;
@ -277,7 +245,7 @@ void ModeRTL::descent_start()
pos_control->init_z_controller_stopping_point();
// initialise yaw
auto_yaw.set_mode(AUTO_YAW_HOLD);
auto_yaw.set_mode(AutoYaw::Mode::HOLD);
#if LANDING_GEAR_ENABLED == ENABLED
// optionally deploy landing gear
@ -295,7 +263,6 @@ void ModeRTL::descent_start()
void ModeRTL::descent_run()
{
Vector2f vel_correction;
float target_yaw_rate = 0.0f;
// if not armed set throttle to zero and exit immediately
if (is_disarmed_or_landed()) {
@ -328,11 +295,6 @@ void ModeRTL::descent_run()
copter.ap.land_repo_active = true;
}
}
if (g.land_repositioning || use_pilot_yaw()) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
}
}
// set motors to full range
@ -348,7 +310,7 @@ void ModeRTL::descent_run()
pos_control->update_z_controller();
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), target_yaw_rate);
attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
// check if we've reached within 20cm of final altitude
_state_complete = labs(rtl_path.descent_target.alt - copter.current_loc.alt) < 20;
@ -375,7 +337,7 @@ void ModeRTL::land_start()
}
// initialise yaw
auto_yaw.set_mode(AUTO_YAW_HOLD);
auto_yaw.set_mode(AutoYaw::Mode::HOLD);
#if LANDING_GEAR_ENABLED == ENABLED
// optionally deploy landing gear

23
ArduCopter/mode_smart_rtl.cpp
View File

@ -70,7 +70,7 @@ void ModeSmartRTL::wait_cleanup_run()
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
wp_nav->update_wpnav();
pos_control->update_z_controller();
attitude_control->input_thrust_vector_heading(wp_nav->get_thrust_vector(), auto_yaw.yaw());
attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
// check if return path is computed and if yes, begin journey home
if (g2.smart_rtl.request_thorough_cleanup()) {
@ -81,15 +81,6 @@ void ModeSmartRTL::wait_cleanup_run()
void ModeSmartRTL::path_follow_run()
{
float target_yaw_rate = 0.0f;
if (!copter.failsafe.radio && use_pilot_yaw()) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
if (!is_zero(target_yaw_rate)) {
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
}
// if we are close to current target point, switch the next point to be our target.
if (wp_nav->reached_wp_destination()) {
Vector3f dest_NED;
@ -140,14 +131,8 @@ void ModeSmartRTL::path_follow_run()
wp_nav->update_wpnav();
pos_control->update_z_controller();
// call attitude controller
if (auto_yaw.mode() == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control->input_thrust_vector_rate_heading(wp_nav->get_thrust_vector(), target_yaw_rate);
} else {
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control->input_thrust_vector_heading(wp_nav->get_thrust_vector(), auto_yaw.yaw());
}
// call attitude controller with auto yaw
attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
}
void ModeSmartRTL::pre_land_position_run()
@ -169,7 +154,7 @@ void ModeSmartRTL::pre_land_position_run()
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
wp_nav->update_wpnav();
pos_control->update_z_controller();
attitude_control->input_thrust_vector_heading(wp_nav->get_thrust_vector(), auto_yaw.yaw());
attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
}
// save current position for use by the smart_rtl flight mode

23
ArduCopter/takeoff.cpp
View File

@ -139,16 +139,6 @@ void Mode::auto_takeoff_run()
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// process pilot's yaw input
float target_yaw_rate = 0;
if (!copter.failsafe.radio && copter.flightmode->use_pilot_yaw()) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
if (!is_zero(target_yaw_rate)) {
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
}
// aircraft stays in landed state until rotor speed run up has finished
if (motors->get_spool_state() != AP_Motors::SpoolState::THROTTLE_UNLIMITED) {
// motors have not completed spool up yet so relax navigation and position controllers
@ -208,17 +198,8 @@ void Mode::auto_takeoff_run()
// run the vertical position controller and set output throttle
pos_control->update_z_controller();
// call attitude controller
if (auto_yaw.mode() == AUTO_YAW_HOLD) {
// roll & pitch from position controller, yaw rate from pilot
attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), target_yaw_rate);
} else if (auto_yaw.mode() == AUTO_YAW_RATE) {
// roll & pitch from position controller, yaw rate from mavlink command or mission item
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(), auto_yaw.rate_cds());
}
// call attitude controller with auto yaw
attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
// handle takeoff completion
bool reached_altitude = (copter.pos_control->get_pos_target_z_cm() - auto_takeoff_start_alt_cm) >= ((auto_takeoff_complete_alt_cm + terr_offset - auto_takeoff_start_alt_cm) * 0.90);