Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-28 02:33:58 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: auto yaw re-work

Browse Source
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 // default_mode - returns auto_yaw.mode() based on WP_YAW_BEHAVIOR parameter
// set rtl parameter to true if this is during an RTL // 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) { switch (copter.g.wp_yaw_behavior) {
case WP_YAW_BEHAVIOR_NONE: case WP_YAW_BEHAVIOR_NONE:
return AUTO_YAW_HOLD; return Mode::HOLD;
case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL: case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL:
if (rtl) { if (rtl) {
return AUTO_YAW_HOLD; return Mode::HOLD;
} else { } else {
return AUTO_YAW_LOOK_AT_NEXT_WP; return Mode::LOOK_AT_NEXT_WP;
} }
case WP_YAW_BEHAVIOR_LOOK_AHEAD: case WP_YAW_BEHAVIOR_LOOK_AHEAD:
return AUTO_YAW_LOOK_AHEAD; return Mode::LOOK_AHEAD;
case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP: case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP:
default: default:
return AUTO_YAW_LOOK_AT_NEXT_WP; return Mode::LOOK_AT_NEXT_WP;
} }
} }
// set_mode - sets the yaw mode for auto // 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 // return immediately if no change
if (_mode == yaw_mode) { if (_mode == yaw_mode) {
@ -61,40 +61,41 @@ void Mode::AutoYaw::set_mode(autopilot_yaw_mode yaw_mode)
// perform initialisation // perform initialisation
switch (_mode) { switch (_mode) {
case AUTO_YAW_HOLD: case Mode::HOLD:
break; 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 // wpnav will initialise heading when wpnav's set_destination method is called
break; break;
case AUTO_YAW_ROI: case Mode::ROI:
// look ahead until we know otherwise // look ahead until we know otherwise
break; break;
case AUTO_YAW_FIXED: case Mode::FIXED:
// keep heading pointing in the direction held in fixed_yaw // keep heading pointing in the direction held in fixed_yaw
// caller should set the fixed_yaw // caller should set the fixed_yaw
break; break;
case AUTO_YAW_LOOK_AHEAD: case Mode::LOOK_AHEAD:
// Commanded Yaw to automatically look ahead. // Commanded Yaw to automatically look ahead.
_look_ahead_yaw = copter.ahrs.yaw_sensor; _look_ahead_yaw = copter.ahrs.yaw_sensor;
break; break;
case AUTO_YAW_RESETTOARMEDYAW: case Mode::RESETTOARMEDYAW:
// initial_armed_bearing will be set during arming so no init required // initial_armed_bearing will be set during arming so no init required
break; break;
case AUTO_YAW_ANGLE_RATE: case Mode::ANGLE_RATE:
break; break;
case AUTO_YAW_RATE: case Mode::RATE:
// initialise target yaw rate to zero // initialise target yaw rate to zero
_yaw_rate_cds = 0.0f; _yaw_rate_cds = 0.0f;
break; break;
case AUTO_YAW_CIRCLE: case Mode::CIRCLE:
case Mode::PILOT_RATE:
// no initialisation required // no initialisation required
break; 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 yaw mode
set_mode(AUTO_YAW_FIXED); set_mode(Mode::FIXED);
} }
// set_fixed_yaw - sets the yaw look at heading for auto mode // 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; _yaw_rate_cds = yaw_rate_ds * 100.0;
// set yaw mode // 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 // 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 // check if mount type requires us to rotate the quad
if (!copter.camera_mount.has_pan_control()) { if (!copter.camera_mount.has_pan_control()) {
if (roi_location.get_vector_from_origin_NEU(roi)) { 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 // send the command to the camera mount
@ -178,7 +179,7 @@ void Mode::AutoYaw::set_roi(const Location &roi_location)
#else #else
// if we have no camera mount aim the quad at the location // if we have no camera mount aim the quad at the location
if (roi_location.get_vector_from_origin_NEU(roi)) { 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 #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 // set auto yaw rate in centi-degrees per second
void Mode::AutoYaw::set_rate(float turn_rate_cds) void Mode::AutoYaw::set_rate(float turn_rate_cds)
{ {
set_mode(AUTO_YAW_RATE); set_mode(Mode::RATE);
_yaw_rate_cds = turn_rate_cds; _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() // yaw - returns target heading depending upon auto_yaw.mode()
float Mode::AutoYaw::yaw() float Mode::AutoYaw::yaw()
{ {
switch (_mode) { switch (_mode) {
case AUTO_YAW_ROI: case Mode::ROI:
// point towards a location held in roi // point towards a location held in roi
return roi_yaw(); return roi_yaw();
case AUTO_YAW_FIXED: { case Mode::FIXED: {
// keep heading pointing in the direction held in fixed_yaw // keep heading pointing in the direction held in fixed_yaw
// with no pilot input allowed // with no pilot input allowed
const uint32_t now_ms = millis(); const uint32_t now_ms = millis();
@ -212,15 +230,15 @@ float Mode::AutoYaw::yaw()
return _yaw_angle_cd; return _yaw_angle_cd;
} }
case AUTO_YAW_LOOK_AHEAD: case Mode::LOOK_AHEAD:
// Commanded Yaw to automatically look ahead. // Commanded Yaw to automatically look ahead.
return look_ahead_yaw(); return look_ahead_yaw();
case AUTO_YAW_RESETTOARMEDYAW: case Mode::RESETTOARMEDYAW:
// changes yaw to be same as when quad was armed // changes yaw to be same as when quad was armed
return copter.initial_armed_bearing; return copter.initial_armed_bearing;
case AUTO_YAW_CIRCLE: case Mode::CIRCLE:
#if MODE_CIRCLE_ENABLED #if MODE_CIRCLE_ENABLED
if (copter.circle_nav->is_active()) { if (copter.circle_nav->is_active()) {
return copter.circle_nav->get_yaw(); return copter.circle_nav->get_yaw();
@ -229,7 +247,7 @@ float Mode::AutoYaw::yaw()
// return the current attitude target // return the current attitude target
return wrap_360_cd(copter.attitude_control->get_att_target_euler_cd().z); 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(); const uint32_t now_ms = millis();
float dt = (now_ms - _last_update_ms) * 0.001; float dt = (now_ms - _last_update_ms) * 0.001;
_last_update_ms = now_ms; _last_update_ms = now_ms;
@ -237,7 +255,7 @@ float Mode::AutoYaw::yaw()
return _yaw_angle_cd; return _yaw_angle_cd;
} }
case AUTO_YAW_LOOK_AT_NEXT_WP: case Mode::LOOK_AT_NEXT_WP:
default: default:
// point towards next waypoint. // point towards next waypoint.
// we don't use wp_bearing because we don't want the copter to turn too much during flight // 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) { switch (_mode) {
case AUTO_YAW_HOLD: case Mode::HOLD:
case AUTO_YAW_ROI: case Mode::ROI:
case AUTO_YAW_FIXED: case Mode::FIXED:
case AUTO_YAW_LOOK_AHEAD: case Mode::LOOK_AHEAD:
case AUTO_YAW_RESETTOARMEDYAW: case Mode::RESETTOARMEDYAW:
case AUTO_YAW_CIRCLE: case Mode::CIRCLE:
return 0.0f; return 0.0f;
case AUTO_YAW_ANGLE_RATE: case Mode::ANGLE_RATE:
case AUTO_YAW_RATE: case Mode::RATE:
return _yaw_rate_cds; 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(); 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 zero turn rate (this should never happen)
return 0.0f; 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 ENABLE ENABLED
#define DISABLE DISABLED #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 // Frame types
#define UNDEFINED_FRAME 0 #define UNDEFINED_FRAME 0
#define MULTICOPTER_FRAME 1 #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() void Mode::land_run_horizontal_control()
{ {
Vector2f vel_correction; Vector2f vel_correction;
float target_yaw_rate = 0;
// relax loiter target if we might be landed // relax loiter target if we might be landed
if (copter.ap.land_complete_maybe) { if (copter.ap.land_complete_maybe) {
@ -671,12 +670,6 @@ void Mode::land_run_horizontal_control()
copter.ap.land_repo_active = true; 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 // 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 // call attitude controller
if (auto_yaw.mode() == AUTO_YAW_HOLD) { attitude_control->input_thrust_vector_heading(thrust_vector, auto_yaw.get_heading());
// 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());
}
} }
// run normal or precision landing (if enabled) // 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 // The passed in location is expected to be NED and in m
void Mode::precland_retry_position(const Vector3f &retry_pos) void Mode::precland_retry_position(const Vector3f &retry_pos)
{ {
float target_yaw_rate = 0;
if (!copter.failsafe.radio) { 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){ 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); 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; 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}; 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_xy_controller();
pos_control->update_z_controller(); pos_control->update_z_controller();
const Vector3f thrust_vector{pos_control->get_thrust_vector()};
// call attitude controller // call attitude controller
if (auto_yaw.mode() == AUTO_YAW_HOLD) { attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
// 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());
}
} }
// Run precland statemachine. This function should be called from any mode that wants to do precision landing. // 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: public:
// yaw(): main product of AutoYaw; the heading: // Autopilot Yaw Mode enumeration
float yaw(); 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: // 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_to_default(bool rtl);
void set_mode(autopilot_yaw_mode new_mode); void set_mode(Mode new_mode);
autopilot_yaw_mode default_mode(bool rtl) const; 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); void set_rate(float new_rate_cds);
// set_roi(...): set a "look at" location: // 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); 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: 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 look_ahead_yaw();
float roi_yaw() const; float roi_yaw() const;
// auto flight mode's yaw mode // 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; Vector3f roi;
// yaw used for YAW_FIXED yaw_mode // 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 // turn rate (in cds) when auto_yaw_mode is set to AUTO_YAW_RATE
float _yaw_angle_cd; float _yaw_angle_cd;
float _yaw_rate_cds; float _yaw_rate_cds;
float _pilot_yaw_rate_cds;
}; };
static AutoYaw auto_yaw; static AutoYaw auto_yaw;
@ -763,7 +782,6 @@ protected:
private: private:
// Circle // 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 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 // 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 // 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) { if (auto_yaw.mode() == AutoYaw::Mode::ROI) {
auto_yaw.set_mode(AUTO_YAW_HOLD); auto_yaw.set_mode(AutoYaw::Mode::HOLD);
} }
// initialise waypoint and spline controller // initialise waypoint and spline controller
@ -277,7 +277,7 @@ bool ModeAuto::loiter_start()
wp_nav->set_wp_destination(stopping_point); wp_nav->set_wp_destination(stopping_point);
// hold yaw at current heading // hold yaw at current heading
auto_yaw.set_mode(AUTO_YAW_HOLD); auto_yaw.set_mode(AutoYaw::Mode::HOLD);
return true; 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); alt_target_cm = MAX(alt_target_cm, alt_target_min_cm);
// initialise yaw // initialise yaw
auto_yaw.set_mode(AUTO_YAW_HOLD); auto_yaw.set_mode(AutoYaw::Mode::HOLD);
// clear i term when we're taking off // clear i term when we're taking off
pos_control->init_z_controller(); pos_control->init_z_controller();
@ -373,7 +373,7 @@ void ModeAuto::wp_start(const Location& dest_loc)
// initialise yaw // 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 // 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); auto_yaw.set_mode_to_default(false);
} }
@ -403,7 +403,7 @@ void ModeAuto::land_start()
} }
// initialise yaw // initialise yaw
auto_yaw.set_mode(AUTO_YAW_HOLD); auto_yaw.set_mode(AutoYaw::Mode::HOLD);
#if LANDING_GEAR_ENABLED == ENABLED #if LANDING_GEAR_ENABLED == ENABLED
// optionally deploy landing gear // 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()); const float dist_to_center = get_horizontal_distance_cm(inertial_nav.get_position_xy_cm().topostype(), copter.circle_nav->get_center().xy());
// initialise yaw // 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 // 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) { if (dist_to_center > copter.circle_nav->get_radius() && dist_to_center > 500) {
auto_yaw.set_mode_to_default(false); auto_yaw.set_mode_to_default(false);
} else { } else {
// vehicle is within circle so hold yaw to avoid spinning as we move to edge of circle // 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 // initialise circle controller
copter.circle_nav->init(copter.circle_nav->get_center(), copter.circle_nav->center_is_terrain_alt()); copter.circle_nav->init(copter.circle_nav->get_center(), copter.circle_nav->center_is_terrain_alt());
if (auto_yaw.mode() != AUTO_YAW_ROI) { if (auto_yaw.mode() != AutoYaw::Mode::ROI) {
auto_yaw.set_mode(AUTO_YAW_CIRCLE); auto_yaw.set_mode(AutoYaw::Mode::CIRCLE);
} }
// set submode to circle // set submode to circle
@ -552,7 +552,7 @@ void ModeAuto::payload_place_start()
} }
// initialise yaw // initialise yaw
auto_yaw.set_mode(AUTO_YAW_HOLD); auto_yaw.set_mode(AutoYaw::Mode::HOLD);
// set submode // set submode
set_submode(SubMode::NAV_PAYLOAD_PLACE); set_submode(SubMode::NAV_PAYLOAD_PLACE);
@ -958,16 +958,6 @@ void ModeAuto::takeoff_run()
// called by auto_run at 100hz or more // called by auto_run at 100hz or more
void ModeAuto::wp_run() 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 not armed set throttle to zero and exit immediately
if (is_disarmed_or_landed()) { if (is_disarmed_or_landed()) {
make_safe_ground_handling(); make_safe_ground_handling();
@ -984,14 +974,8 @@ void ModeAuto::wp_run()
// run the vertical position controller and set output throttle // run the vertical position controller and set output throttle
pos_control->update_z_controller(); pos_control->update_z_controller();
// call attitude controller // call attitude controller with auto yaw
if (auto_yaw.mode() == AUTO_YAW_HOLD) { attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
// 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());
}
} }
// auto_land_run - lands in auto mode // auto_land_run - lands in auto mode
@ -1024,16 +1008,6 @@ void ModeAuto::rtl_run()
// called by auto_run at 100hz or more // called by auto_run at 100hz or more
void ModeAuto::circle_run() 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 // call circle controller
copter.failsafe_terrain_set_status(copter.circle_nav->update()); 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 // run the vertical position controller and set output throttle
pos_control->update_z_controller(); pos_control->update_z_controller();
if (auto_yaw.mode() == AUTO_YAW_HOLD) { // call attitude controller with auto yaw
// roll & pitch from waypoint controller, yaw rate from pilot attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
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());
}
} }
#if NAV_GUIDED == ENABLED || AP_SCRIPTING_ENABLED #if NAV_GUIDED == ENABLED || AP_SCRIPTING_ENABLED
@ -1070,12 +1039,6 @@ void ModeAuto::loiter_run()
return; 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 // set motors to full range
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); 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()); copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
pos_control->update_z_controller(); 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 // 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 // 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 // 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); auto_yaw.set_mode_to_default(false);
} }
@ -1577,7 +1542,7 @@ void ModeAuto::do_spline_wp(const AP_Mission::Mission_Command& cmd)
// initialise yaw // 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 // 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); auto_yaw.set_mode_to_default(false);
} }
@ -2107,10 +2072,10 @@ bool ModeAuto::verify_within_distance()
bool ModeAuto::verify_yaw() 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 // 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 // check if we have reached the target heading
return auto_yaw.fixed_yaw_slew_finished() && (fabsf(wrap_180_cd(ahrs.yaw_sensor-auto_yaw.yaw())) <= 200); return auto_yaw.reached_fixed_yaw_target();
} }
// verify_nav_wp - check if we have reached the next way point // 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 // circle_init - initialise circle controller flight mode
bool ModeCircle::init(bool ignore_checks) bool ModeCircle::init(bool ignore_checks)
{ {
pilot_yaw_override = false;
speed_changing = false; speed_changing = false;
// set speed and acceleration limits // 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_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); 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 // Check for any change in params and update in real time
copter.circle_nav->check_param_change(); copter.circle_nav->check_param_change();
@ -106,14 +99,10 @@ void ModeCircle::run()
copter.surface_tracking.update_surface_offset(); copter.surface_tracking.update_surface_offset();
copter.failsafe_terrain_set_status(copter.circle_nav->update(target_climb_rate)); 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(); 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 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; guided_mode = SubMode::TakeOff;
// initialise yaw // initialise yaw
auto_yaw.set_mode(AUTO_YAW_HOLD); auto_yaw.set_mode(AutoYaw::Mode::HOLD);
// clear i term when we're taking off // clear i term when we're taking off
pos_control->init_z_controller(); pos_control->init_z_controller();
@ -178,16 +178,6 @@ void ModeGuided::wp_control_start()
// run guided mode's waypoint navigation controller // run guided mode's waypoint navigation controller
void ModeGuided::wp_control_run() 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 not armed set throttle to zero and exit immediately
if (is_disarmed_or_landed()) { if (is_disarmed_or_landed()) {
// do not spool down tradheli when on the ground with motor interlock enabled // 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) // call z-axis position controller (wpnav should have already updated it's alt target)
pos_control->update_z_controller(); pos_control->update_z_controller();
// call attitude controller // call attitude controller with auto yaw
if (auto_yaw.mode() == AUTO_YAW_HOLD) { attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
// 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());
}
} }
// initialise position controller // initialise position controller
@ -332,7 +313,7 @@ void ModeGuided::angle_control_start()
guided_angle_state.use_yaw_rate = false; guided_angle_state.use_yaw_rate = false;
// pilot always controls yaw // 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 // set_destination - sets guided mode's target destination
@ -682,17 +663,6 @@ void ModeGuided::takeoff_run()
// called from guided_run // called from guided_run
void ModeGuided::pos_control_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 not armed set throttle to zero and exit immediately
if (is_disarmed_or_landed()) { if (is_disarmed_or_landed()) {
// do not spool down tradheli when on the ground with motor interlock enabled // 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 // stop rotating if no updates received within timeout_ms
if (millis() - update_time_ms > get_timeout_ms()) { if (millis() - update_time_ms > get_timeout_ms()) {
if ((auto_yaw.mode() == AUTO_YAW_RATE) || (auto_yaw.mode() == AUTO_YAW_ANGLE_RATE)) { if ((auto_yaw.mode() == AutoYaw::Mode::RATE) || (auto_yaw.mode() == AutoYaw::Mode::ANGLE_RATE)) {
auto_yaw.set_rate(0.0f); auto_yaw.set_mode(AutoYaw::Mode::HOLD);
} }
} }
@ -732,33 +702,14 @@ void ModeGuided::pos_control_run()
pos_control->update_xy_controller(); pos_control->update_xy_controller();
pos_control->update_z_controller(); pos_control->update_z_controller();
// call attitude controller // call attitude controller with auto yaw
if (auto_yaw.mode() == AUTO_YAW_HOLD) { attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
// 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());
}
} }
// velaccel_control_run - runs the guided velocity controller // velaccel_control_run - runs the guided velocity controller
// called from guided_run // called from guided_run
void ModeGuided::accel_control_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 not armed set throttle to zero and exit immediately
if (is_disarmed_or_landed()) { if (is_disarmed_or_landed()) {
// do not spool down tradheli when on the ground with motor interlock enabled // 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()) { if (tnow - update_time_ms > get_timeout_ms()) {
guided_vel_target_cms.zero(); guided_vel_target_cms.zero();
guided_accel_target_cmss.zero(); guided_accel_target_cmss.zero();
if ((auto_yaw.mode() == AUTO_YAW_RATE) || (auto_yaw.mode() == AUTO_YAW_ANGLE_RATE)) { if ((auto_yaw.mode() == AutoYaw::Mode::RATE) || (auto_yaw.mode() == AutoYaw::Mode::ANGLE_RATE)) {
auto_yaw.set_rate(0.0f); 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_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); 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_xy_controller();
pos_control->update_z_controller(); pos_control->update_z_controller();
// call attitude controller // call attitude controller with auto yaw
if (auto_yaw.mode() == AUTO_YAW_HOLD) { attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
// 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());
}
} }
// velaccel_control_run - runs the guided velocity and acceleration controller // velaccel_control_run - runs the guided velocity and acceleration controller
// called from guided_run // called from guided_run
void ModeGuided::velaccel_control_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 not armed set throttle to zero and exit immediately
if (is_disarmed_or_landed()) { if (is_disarmed_or_landed()) {
// do not spool down tradheli when on the ground with motor interlock enabled // 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()) { if (tnow - update_time_ms > get_timeout_ms()) {
guided_vel_target_cms.zero(); guided_vel_target_cms.zero();
guided_accel_target_cmss.zero(); guided_accel_target_cmss.zero();
if ((auto_yaw.mode() == AUTO_YAW_RATE) || (auto_yaw.mode() == AUTO_YAW_ANGLE_RATE)) { if ((auto_yaw.mode() == AutoYaw::Mode::RATE) || (auto_yaw.mode() == AutoYaw::Mode::ANGLE_RATE)) {
auto_yaw.set_rate(0.0f); auto_yaw.set_mode(AutoYaw::Mode::HOLD);
} }
} }
@ -871,17 +803,8 @@ void ModeGuided::velaccel_control_run()
pos_control->update_xy_controller(); pos_control->update_xy_controller();
pos_control->update_z_controller(); pos_control->update_z_controller();
// call attitude controller // call attitude controller with auto yaw
if (auto_yaw.mode() == AUTO_YAW_HOLD) { attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
// 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());
}
} }
// pause_control_run - runs the guided mode pause controller // pause_control_run - runs the guided mode pause controller
@ -911,24 +834,13 @@ void ModeGuided::pause_control_run()
pos_control->update_z_controller(); pos_control->update_z_controller();
// call attitude 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 // posvelaccel_control_run - runs the guided position, velocity and acceleration controller
// called from guided_run // called from guided_run
void ModeGuided::posvelaccel_control_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 not armed set throttle to zero and exit immediately
if (is_disarmed_or_landed()) { if (is_disarmed_or_landed()) {
// do not spool down tradheli when on the ground with motor interlock enabled // 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()) { if (tnow - update_time_ms > get_timeout_ms()) {
guided_vel_target_cms.zero(); guided_vel_target_cms.zero();
guided_accel_target_cmss.zero(); guided_accel_target_cmss.zero();
if ((auto_yaw.mode() == AUTO_YAW_RATE) || (auto_yaw.mode() == AUTO_YAW_ANGLE_RATE)) { if ((auto_yaw.mode() == AutoYaw::Mode::RATE) || (auto_yaw.mode() == AutoYaw::Mode::ANGLE_RATE)) {
auto_yaw.set_rate(0.0f); auto_yaw.set_mode(AutoYaw::Mode::HOLD);
} }
} }
@ -983,17 +895,8 @@ void ModeGuided::posvelaccel_control_run()
pos_control->update_xy_controller(); pos_control->update_xy_controller();
pos_control->update_z_controller(); pos_control->update_z_controller();
// call attitude controller // call attitude controller with auto yaw
if (auto_yaw.mode() == AUTO_YAW_HOLD) { attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
// 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());
}
} }
// angle_control_run - runs the guided angle controller // 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; copter.ap.prec_land_active = false;
// initialise yaw // initialise yaw
auto_yaw.set_mode(AUTO_YAW_HOLD); auto_yaw.set_mode(AutoYaw::Mode::HOLD);
#if LANDING_GEAR_ENABLED == ENABLED #if LANDING_GEAR_ENABLED == ENABLED
// optionally deploy landing gear // optionally deploy landing gear
@ -98,7 +98,6 @@ void ModeLand::gps_run()
void ModeLand::nogps_run() void ModeLand::nogps_run()
{ {
float target_roll = 0.0f, target_pitch = 0.0f; float target_roll = 0.0f, target_pitch = 0.0f;
float target_yaw_rate = 0;
// process pilot inputs // process pilot inputs
if (!copter.failsafe.radio) { 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_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 // disarm when the landing detector says we've landed
@ -144,7 +138,7 @@ void ModeLand::nogps_run()
} }
// call attitude controller // 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 // 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 // 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 // rtl_return_start - initialise return to home
@ -166,16 +166,6 @@ void ModeRTL::climb_return_run()
return; 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 // set motors to full range
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); 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 // run the vertical position controller and set output throttle
pos_control->update_z_controller(); pos_control->update_z_controller();
// call attitude controller // call attitude controller with auto yaw
if (auto_yaw.mode() == AUTO_YAW_HOLD) { attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
// 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());
}
// check if we've completed this stage of RTL // check if we've completed this stage of RTL
_state_complete = wp_nav->reached_wp_destination(); _state_complete = wp_nav->reached_wp_destination();
@ -207,10 +191,10 @@ void ModeRTL::loiterathome_start()
_loiter_start_time = millis(); _loiter_start_time = millis();
// yaw back to initial take-off heading yaw unless pilot has already overridden yaw // yaw back to initial take-off heading yaw unless pilot has already overridden yaw
if (auto_yaw.default_mode(true) != AUTO_YAW_HOLD) { if (auto_yaw.default_mode(true) != AutoYaw::Mode::HOLD) {
auto_yaw.set_mode(AUTO_YAW_RESETTOARMEDYAW); auto_yaw.set_mode(AutoYaw::Mode::RESETTOARMEDYAW);
} else { } else {
auto_yaw.set_mode(AUTO_YAW_HOLD); auto_yaw.set_mode(AutoYaw::Mode::HOLD);
} }
} }
@ -224,16 +208,6 @@ void ModeRTL::loiterathome_run()
return; 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 // set motors to full range
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); 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 // run the vertical position controller and set output throttle
pos_control->update_z_controller(); pos_control->update_z_controller();
// call attitude controller // call attitude controller with auto yaw
if (auto_yaw.mode() == AUTO_YAW_HOLD) { attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
// 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());
}
// check if we've completed this stage of RTL // check if we've completed this stage of RTL
if ((millis() - _loiter_start_time) >= (uint32_t)g.rtl_loiter_time.get()) { 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 // 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) { if (abs(wrap_180_cd(ahrs.yaw_sensor-copter.initial_armed_bearing)) <= 200) {
_state_complete = true; _state_complete = true;
@ -277,7 +245,7 @@ void ModeRTL::descent_start()
pos_control->init_z_controller_stopping_point(); pos_control->init_z_controller_stopping_point();
// initialise yaw // initialise yaw
auto_yaw.set_mode(AUTO_YAW_HOLD); auto_yaw.set_mode(AutoYaw::Mode::HOLD);
#if LANDING_GEAR_ENABLED == ENABLED #if LANDING_GEAR_ENABLED == ENABLED
// optionally deploy landing gear // optionally deploy landing gear
@ -295,7 +263,6 @@ void ModeRTL::descent_start()
void ModeRTL::descent_run() void ModeRTL::descent_run()
{ {
Vector2f vel_correction; Vector2f vel_correction;
float target_yaw_rate = 0.0f;
// if not armed set throttle to zero and exit immediately // if not armed set throttle to zero and exit immediately
if (is_disarmed_or_landed()) { if (is_disarmed_or_landed()) {
@ -328,11 +295,6 @@ void ModeRTL::descent_run()
copter.ap.land_repo_active = true; 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 // set motors to full range
@ -348,7 +310,7 @@ void ModeRTL::descent_run()
pos_control->update_z_controller(); pos_control->update_z_controller();
// roll & pitch from waypoint controller, yaw rate from pilot // 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 // check if we've reached within 20cm of final altitude
_state_complete = labs(rtl_path.descent_target.alt - copter.current_loc.alt) < 20; _state_complete = labs(rtl_path.descent_target.alt - copter.current_loc.alt) < 20;
@ -375,7 +337,7 @@ void ModeRTL::land_start()
} }
// initialise yaw // initialise yaw
auto_yaw.set_mode(AUTO_YAW_HOLD); auto_yaw.set_mode(AutoYaw::Mode::HOLD);
#if LANDING_GEAR_ENABLED == ENABLED #if LANDING_GEAR_ENABLED == ENABLED
// optionally deploy landing gear // 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); motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
wp_nav->update_wpnav(); wp_nav->update_wpnav();
pos_control->update_z_controller(); 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 // check if return path is computed and if yes, begin journey home
if (g2.smart_rtl.request_thorough_cleanup()) { if (g2.smart_rtl.request_thorough_cleanup()) {
@ -81,15 +81,6 @@ void ModeSmartRTL::wait_cleanup_run()
void ModeSmartRTL::path_follow_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 we are close to current target point, switch the next point to be our target.
if (wp_nav->reached_wp_destination()) { if (wp_nav->reached_wp_destination()) {
Vector3f dest_NED; Vector3f dest_NED;
@ -140,14 +131,8 @@ void ModeSmartRTL::path_follow_run()
wp_nav->update_wpnav(); wp_nav->update_wpnav();
pos_control->update_z_controller(); pos_control->update_z_controller();
// call attitude controller // call attitude controller with auto yaw
if (auto_yaw.mode() == AUTO_YAW_HOLD) { attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
// 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());
}
} }
void ModeSmartRTL::pre_land_position_run() 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); motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
wp_nav->update_wpnav(); wp_nav->update_wpnav();
pos_control->update_z_controller(); 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 // 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 // set motors to full range
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); 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 // aircraft stays in landed state until rotor speed run up has finished
if (motors->get_spool_state() != AP_Motors::SpoolState::THROTTLE_UNLIMITED) { if (motors->get_spool_state() != AP_Motors::SpoolState::THROTTLE_UNLIMITED) {
// motors have not completed spool up yet so relax navigation and position controllers // 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 // run the vertical position controller and set output throttle
pos_control->update_z_controller(); pos_control->update_z_controller();
// call attitude controller // call attitude controller with auto yaw
if (auto_yaw.mode() == AUTO_YAW_HOLD) { attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
// 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());
}
// handle takeoff completion // 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); 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);