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

Copter: create an AutoYaw helper object to hold auto-yaw state

This commit is contained in:
Peter Barker 2017-12-26 14:55:42 +11:00 committed by Randy Mackay
parent 63e42e194f
commit ba8b3e2415
11 changed files with 219 additions and 217 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
ArduCopter/Attitude.cpp
View File

@ -33,30 +33,27 @@ float Copter::get_pilot_desired_yaw_rate(int16_t stick_angle)
* yaw controllers
*************************************************************/
// get_roi_yaw - returns heading towards location held in roi_WP
// should be called at 100hz
float Copter::get_roi_yaw()
// roi_yaw - returns heading towards location held in roi_WP
float Copter::Mode::AutoYaw::roi_yaw()
{
static uint8_t roi_yaw_counter = 0; // used to reduce update rate to 100hz
roi_yaw_counter++;
if (roi_yaw_counter >= 4) {
roi_yaw_counter = 0;
yaw_look_at_WP_bearing = get_bearing_cd(inertial_nav.get_position(), roi_WP);
_roi_yaw = get_bearing_cd(copter.inertial_nav.get_position(), roi_WP);
}
return yaw_look_at_WP_bearing;
return _roi_yaw;
}
float Copter::get_look_ahead_yaw()
float Copter::Mode::AutoYaw::look_ahead_yaw()
{
const Vector3f& vel = inertial_nav.get_velocity();
const Vector3f& vel = copter.inertial_nav.get_velocity();
float speed = norm(vel.x,vel.y);
// Commanded Yaw to automatically look ahead.
if (position_ok() && (speed > YAW_LOOK_AHEAD_MIN_SPEED)) {
yaw_look_ahead_bearing = degrees(atan2f(vel.y,vel.x))*100.0f;
if (copter.position_ok() && (speed > YAW_LOOK_AHEAD_MIN_SPEED)) {
_look_ahead_yaw = degrees(atan2f(vel.y,vel.x))*100.0f;
}
return yaw_look_ahead_bearing;
return _look_ahead_yaw;
}
/*************************************************************

1
ArduCopter/Copter.cpp
View File

@ -32,7 +32,6 @@ Copter::Copter(void)
super_simple_cos_yaw(1.0),
land_accel_ef_filter(LAND_DETECTOR_ACCEL_LPF_CUTOFF),
rc_throttle_control_in_filter(1.0f),
auto_yaw_mode(AUTO_YAW_LOOK_AT_NEXT_WP),
inertial_nav(ahrs),
param_loader(var_info),
flightmode(&mode_stabilize)

33
ArduCopter/Copter.h
View File

@ -462,28 +462,6 @@ private:
// Current location of the vehicle (altitude is relative to home)
Location_Class current_loc;
// Navigation Yaw control
// auto flight mode's yaw mode
uint8_t auto_yaw_mode;
// Yaw will point at this location if auto_yaw_mode is set to AUTO_YAW_ROI
Vector3f roi_WP;
// bearing from current location to the yaw_look_at_WP
float yaw_look_at_WP_bearing;
// yaw used for YAW_LOOK_AT_HEADING yaw_mode
int32_t yaw_look_at_heading;
// Deg/s we should turn
int16_t yaw_look_at_heading_slew;
// heading when in yaw_look_ahead_bearing
float yaw_look_ahead_bearing;
// turn rate (in cds) when auto_yaw_mode is set to AUTO_YAW_RATE
float auto_yaw_rate_cds;
// IMU variables
// Integration time (in seconds) for the gyros (DCM algorithm)
// Updated with the fast loop
@ -693,8 +671,6 @@ private:
// Attitude.cpp
float get_pilot_desired_yaw_rate(int16_t stick_angle);
float get_roi_yaw();
float get_look_ahead_yaw();
void update_throttle_hover();
void set_throttle_takeoff();
float get_pilot_desired_throttle(int16_t throttle_control, float thr_mid = 0.0f);
@ -844,15 +820,6 @@ private:
void update_flight_mode();
void notify_flight_mode();
// mode_auto.cpp
uint8_t get_default_auto_yaw_mode(bool rtl);
void set_auto_yaw_mode(uint8_t yaw_mode);
void set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, bool relative_angle);
void set_auto_yaw_roi(const Location &roi_location);
void set_auto_yaw_rate(float turn_rate_cds);
float get_auto_heading(void);
float get_auto_yaw_rate_cds();
// mode_land.cpp
void set_mode_land_with_pause(mode_reason_t reason);
bool landing_with_GPS();

21
ArduCopter/GCS_Mavlink.cpp
View File

@ -924,7 +924,7 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
roi_loc.lat = packet.x;
roi_loc.lng = packet.y;
roi_loc.alt = (int32_t)(packet.z * 100.0f);
copter.set_auto_yaw_roi(roi_loc);
copter.flightmode->auto_yaw.set_roi(roi_loc);
result = MAV_RESULT_ACCEPTED;
break;
}
@ -1002,7 +1002,11 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
if ((packet.param1 >= 0.0f) &&
(packet.param1 <= 360.0f) &&
(is_zero(packet.param4) || is_equal(packet.param4,1.0f))) {
copter.set_auto_yaw_look_at_heading(packet.param1, packet.param2, (int8_t)packet.param3, is_positive(packet.param4));
copter.flightmode->auto_yaw.set_fixed_yaw(
packet.param1,
packet.param2,
(int8_t)packet.param3,
is_positive(packet.param4));
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;
@ -1066,14 +1070,17 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
roi_loc.lat = (int32_t)(packet.param5 * 1.0e7f);
roi_loc.lng = (int32_t)(packet.param6 * 1.0e7f);
roi_loc.alt = (int32_t)(packet.param7 * 100.0f);
copter.set_auto_yaw_roi(roi_loc);
copter.flightmode->auto_yaw.set_roi(roi_loc);
result = MAV_RESULT_ACCEPTED;
break;
case MAV_CMD_DO_MOUNT_CONTROL:
#if MOUNT == ENABLED
if(!copter.camera_mount.has_pan_control()) {
copter.set_auto_yaw_look_at_heading((float)packet.param3 / 100.0f,0.0f,0,0);
copter.flightmode->auto_yaw.set_fixed_yaw(
(float)packet.param3 / 100.0f,
0.0f,
0,0);
}
copter.camera_mount.control(packet.param1, packet.param2, packet.param3, (MAV_MOUNT_MODE) packet.param7);
result = MAV_RESULT_ACCEPTED;
@ -1653,7 +1660,11 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
//deprecated. Use MAV_CMD_DO_MOUNT_CONTROL
case MAVLINK_MSG_ID_MOUNT_CONTROL:
if(!copter.camera_mount.has_pan_control()) {
copter.set_auto_yaw_look_at_heading(mavlink_msg_mount_control_get_input_c(msg)/100.0f, 0.0f, 0, 0);
copter.flightmode->auto_yaw.set_fixed_yaw(
mavlink_msg_mount_control_get_input_c(msg)/100.0f,
0.0f,
0,
0);
}
copter.camera_mount.control_msg(msg);
break;

2
ArduCopter/defines.h
View File

@ -15,7 +15,7 @@ enum autopilot_yaw_mode {
AUTO_YAW_HOLD = 0, // pilot controls the heading
AUTO_YAW_LOOK_AT_NEXT_WP = 1, // point towards next waypoint (no pilot input accepted)
AUTO_YAW_ROI = 2, // point towards a location held in roi_WP (no pilot input accepted)
AUTO_YAW_LOOK_AT_HEADING = 3, // point towards a particular angle (not pilot input accepted)
AUTO_YAW_FIXED = 3, // point towards a particular angle (no pilot input accepted)
AUTO_YAW_LOOK_AHEAD = 4, // point in the direction the copter is moving
AUTO_YAW_RESETTOARMEDYAW = 5, // point towards heading at time motors were armed
AUTO_YAW_RATE = 6, // turn at a specified rate (held in auto_yaw_rate)

35
ArduCopter/mode.cpp
View File

@ -1,5 +1,7 @@
#include "Copter.h"
Copter::Mode::AutoYaw Copter::Mode::auto_yaw;
/*
* High level calls to set and update flight modes logic for individual
* flight modes is in control_acro.cpp, control_stabilize.cpp, etc
@ -26,11 +28,10 @@ Copter::Mode::Mode(void) :
ap(copter.ap),
takeoff_state(copter.takeoff_state),
ekfGndSpdLimit(copter.ekfGndSpdLimit),
ekfNavVelGainScaler(copter.ekfNavVelGainScaler),
#if FRAME_CONFIG == HELI_FRAME
heli_flags(copter.heli_flags),
#endif
auto_yaw_mode(copter.auto_yaw_mode)
ekfNavVelGainScaler(copter.ekfNavVelGainScaler)
{ };
// return the static controller object corresponding to supplied mode
@ -596,21 +597,6 @@ void Copter::Mode::set_throttle_takeoff()
return copter.set_throttle_takeoff();
}
void Copter::Mode::set_auto_yaw_mode(uint8_t yaw_mode)
{
return copter.set_auto_yaw_mode(yaw_mode);
}
void Copter::Mode::set_auto_yaw_rate(float turn_rate_cds)
{
return copter.set_auto_yaw_rate(turn_rate_cds);
}
void Copter::Mode::set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, bool relative_angle)
{
return copter.set_auto_yaw_look_at_heading(angle_deg, turn_rate_dps, direction, relative_angle);
}
void Copter::Mode::takeoff_timer_start(float alt_cm)
{
return copter.takeoff_timer_start(alt_cm);
@ -626,16 +612,6 @@ void Copter::Mode::takeoff_get_climb_rates(float& pilot_climb_rate, float& takeo
return copter.takeoff_get_climb_rates(pilot_climb_rate, takeoff_climb_rate);
}
float Copter::Mode::get_auto_heading()
{
return copter.get_auto_heading();
}
float Copter::Mode::get_auto_yaw_rate_cds()
{
return copter.get_auto_yaw_rate_cds();
}
float Copter::Mode::get_avoidance_adjusted_climbrate(float target_rate)
{
return copter.get_avoidance_adjusted_climbrate(target_rate);
@ -645,3 +621,8 @@ uint16_t Copter::Mode::get_pilot_speed_dn()
{
return copter.get_pilot_speed_dn();
}
void Copter::Mode::AutoYaw::set_mode_to_default(bool rtl)
{
set_mode(default_mode(rtl));
}

71
ArduCopter/mode.h
View File

@ -10,7 +10,67 @@ class Mode {
// constructor
Mode(void);
public:
// Navigation Yaw control
class AutoYaw {
public:
// yaw(): main product of AutoYaw; the heading:
float yaw();
// mode(): current method of determining desired yaw:
autopilot_yaw_mode mode() const { return (autopilot_yaw_mode)_mode; }
void set_mode_to_default(bool rtl);
void set_mode(autopilot_yaw_mode new_mode);
autopilot_yaw_mode default_mode(bool rtl) const;
// rate_cds(): desired yaw rate in centidegrees/second:
float rate_cds() const;
void set_rate(float new_rate_cds);
// set_roi(...): set a "look at" location:
void set_roi(const Location &roi_location);
void set_fixed_yaw(float angle_deg,
float turn_rate_dps,
int8_t direction,
bool relative_angle);
private:
float look_ahead_yaw();
float roi_yaw();
// auto flight mode's yaw mode
uint8_t _mode = AUTO_YAW_LOOK_AT_NEXT_WP;
// Yaw will point at this location if mode is set to AUTO_YAW_ROI
Vector3f roi_WP;
// bearing from current location to the ROI
float _roi_yaw;
// yaw used for YAW_FIXED yaw_mode
int32_t _fixed_yaw;
// Deg/s we should turn
int16_t _fixed_yaw_slewrate;
// heading when in yaw_look_ahead_yaw
float _look_ahead_yaw;
// turn rate (in cds) when auto_yaw_mode is set to AUTO_YAW_RATE
float _rate_cds;
// used to reduce update rate to 100hz:
uint8_t roi_yaw_counter;
};
static AutoYaw auto_yaw;
protected:
virtual bool init(bool ignore_checks) = 0;
@ -74,10 +134,6 @@ protected:
// scale factor applied to velocity controller gain to prevent optical flow noise causing excessive angle demand noise
float &ekfNavVelGainScaler;
// Navigation Yaw control
// auto flight mode's yaw mode
uint8_t &auto_yaw_mode;
#if FRAME_CONFIG == HELI_FRAME
heli_flags_t &heli_flags;
#endif
@ -96,14 +152,9 @@ protected:
GCS_Copter &gcs();
void Log_Write_Event(uint8_t id);
void set_throttle_takeoff(void);
void set_auto_yaw_mode(uint8_t yaw_mode);
void set_auto_yaw_rate(float turn_rate_cds);
void set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, bool relative_angle);
void takeoff_timer_start(float alt_cm);
void takeoff_stop(void);
void takeoff_get_climb_rates(float& pilot_climb_rate, float& takeoff_climb_rate);
float get_auto_heading(void);
float get_auto_yaw_rate_cds(void);
float get_avoidance_adjusted_climbrate(float target_rate);
uint16_t get_pilot_speed_dn(void);

162
ArduCopter/mode_auto.cpp
View File

@ -33,8 +33,8 @@ bool Copter::ModeAuto::init(bool ignore_checks)
// stop ROI from carrying over from previous runs of the mission
// To-Do: reset the yaw as part of auto_wp_start when the previous command was not a wp command to remove the need for this special ROI check
if (copter.auto_yaw_mode == AUTO_YAW_ROI) {
set_auto_yaw_mode(AUTO_YAW_HOLD);
if (auto_yaw.mode() == AUTO_YAW_ROI) {
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
// initialise waypoint and spline controller
@ -118,7 +118,7 @@ bool Copter::ModeAuto::loiter_start()
wp_nav->set_wp_destination(stopping_point);
// hold yaw at current heading
set_auto_yaw_mode(AUTO_YAW_HOLD);
auto_yaw.set_mode(AUTO_YAW_HOLD);
return true;
}
@ -170,7 +170,7 @@ void Copter::ModeAuto::takeoff_start(const Location& dest_loc)
}
// initialise yaw
set_auto_yaw_mode(AUTO_YAW_HOLD);
auto_yaw.set_mode(AUTO_YAW_HOLD);
// clear i term when we're taking off
set_throttle_takeoff();
@ -189,8 +189,8 @@ void Copter::ModeAuto::wp_start(const Vector3f& destination)
// initialise yaw
// To-Do: reset the yaw only when the previous navigation command is not a WP. this would allow removing the special check for ROI
if (copter.auto_yaw_mode != AUTO_YAW_ROI) {
set_auto_yaw_mode(copter.get_default_auto_yaw_mode(false));
if (auto_yaw.mode() != AUTO_YAW_ROI) {
auto_yaw.set_mode_to_default(false);
}
}
@ -208,8 +208,8 @@ void Copter::ModeAuto::wp_start(const Location_Class& dest_loc)
// initialise yaw
// To-Do: reset the yaw only when the previous navigation command is not a WP. this would allow removing the special check for ROI
if (copter.auto_yaw_mode != AUTO_YAW_ROI) {
set_auto_yaw_mode(copter.get_default_auto_yaw_mode(false));
if (auto_yaw.mode() != AUTO_YAW_ROI) {
auto_yaw.set_mode_to_default(false);
}
}
@ -239,7 +239,7 @@ void Copter::ModeAuto::land_start(const Vector3f& destination)
}
// initialise yaw
set_auto_yaw_mode(AUTO_YAW_HOLD);
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
// auto_circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location
@ -286,10 +286,10 @@ void Copter::ModeAuto::circle_movetoedge_start(const Location_Class &circle_cent
const Vector3f &curr_pos = inertial_nav.get_position();
float dist_to_center = norm(circle_center_neu.x - curr_pos.x, circle_center_neu.y - curr_pos.y);
if (dist_to_center > copter.circle_nav->get_radius() && dist_to_center > 500) {
set_auto_yaw_mode(copter.get_default_auto_yaw_mode(false));
auto_yaw.set_mode_to_default(false);
} else {
// vehicle is within circle so hold yaw to avoid spinning as we move to edge of circle
set_auto_yaw_mode(AUTO_YAW_HOLD);
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
} else {
circle_start();
@ -323,8 +323,8 @@ void Copter::ModeAuto::spline_start(const Location_Class& destination, bool stop
// initialise yaw
// To-Do: reset the yaw only when the previous navigation command is not a WP. this would allow removing the special check for ROI
if (copter.auto_yaw_mode != AUTO_YAW_ROI) {
set_auto_yaw_mode(copter.get_default_auto_yaw_mode(false));
if (auto_yaw.mode() != AUTO_YAW_ROI) {
auto_yaw.set_mode_to_default(false);
}
}
@ -778,7 +778,7 @@ void Copter::ModeAuto::wp_run()
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (!is_zero(target_yaw_rate)) {
set_auto_yaw_mode(AUTO_YAW_HOLD);
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
}
@ -792,12 +792,12 @@ void Copter::ModeAuto::wp_run()
pos_control->update_z_controller();
// call attitude controller
if (copter.auto_yaw_mode == AUTO_YAW_HOLD) {
if (auto_yaw.mode() == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate);
} else {
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(),true);
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.yaw(),true);
}
}
@ -821,7 +821,7 @@ void Copter::ModeAuto::spline_run()
// get pilot's desired yaw rat
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (!is_zero(target_yaw_rate)) {
set_auto_yaw_mode(AUTO_YAW_HOLD);
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
}
@ -835,12 +835,12 @@ void Copter::ModeAuto::spline_run()
pos_control->update_z_controller();
// call attitude controller
if (copter.auto_yaw_mode == AUTO_YAW_HOLD) {
if (auto_yaw.mode() == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate);
} else {
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(), true);
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.yaw(), true);
}
}
@ -937,7 +937,7 @@ void Copter::ModeAuto::payload_place_start(const Vector3f& destination)
}
// initialise yaw
set_auto_yaw_mode(AUTO_YAW_HOLD);
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
// auto_payload_place_run - places an object in auto mode
@ -1320,7 +1320,7 @@ void Copter::ModeAuto::do_within_distance(const AP_Mission::Mission_Command& cmd
void Copter::ModeAuto::do_yaw(const AP_Mission::Mission_Command& cmd)
{
set_auto_yaw_look_at_heading(
auto_yaw.set_fixed_yaw(
cmd.content.yaw.angle_deg,
cmd.content.yaw.turn_rate_dps,
cmd.content.yaw.direction,
@ -1355,7 +1355,7 @@ void Copter::ModeAuto::do_set_home(const AP_Mission::Mission_Command& cmd)
// TO-DO: add support for other features of MAV_CMD_DO_SET_ROI including pointing at a given waypoint
void Copter::ModeAuto::do_roi(const AP_Mission::Mission_Command& cmd)
{
copter.set_auto_yaw_roi(cmd.content.location);
auto_yaw.set_roi(cmd.content.location);
}
// point the camera to a specified angle
@ -1363,7 +1363,7 @@ void Copter::ModeAuto::do_mount_control(const AP_Mission::Mission_Command& cmd)
{
#if MOUNT == ENABLED
if(!copter.camera_mount.has_pan_control()) {
copter.set_auto_yaw_look_at_heading(cmd.content.mount_control.yaw,0.0f,0,0);
auto_yaw.set_fixed_yaw(cmd.content.mount_control.yaw,0.0f,0,0);
}
copter.camera_mount.set_angle_targets(cmd.content.mount_control.roll, cmd.content.mount_control.pitch, cmd.content.mount_control.yaw);
#endif
@ -1758,16 +1758,12 @@ bool Copter::ModeAuto::verify_within_distance()
bool Copter::ModeAuto::verify_yaw()
{
// set yaw mode if it has been changed (the waypoint controller often retakes control of yaw as it executes a new waypoint command)
if (auto_yaw_mode != AUTO_YAW_LOOK_AT_HEADING) {
set_auto_yaw_mode(AUTO_YAW_LOOK_AT_HEADING);
if (auto_yaw.mode() != AUTO_YAW_FIXED) {
auto_yaw.set_mode(AUTO_YAW_FIXED);
}
// check if we are within 2 degrees of the target heading
if (labs(wrap_180_cd(ahrs.yaw_sensor-copter.yaw_look_at_heading)) <= 200) {
return true;
}else{
return false;
}
return (labs(wrap_180_cd(ahrs.yaw_sensor-auto_yaw.yaw())) <= 200);
}
// verify_nav_wp - check if we have reached the next way point
@ -1877,11 +1873,11 @@ bool Copter::ModeAuto::verify_nav_delay(const AP_Mission::Mission_Command& cmd)
#endif
// get_default_auto_yaw_mode - returns auto_yaw_mode based on WP_YAW_BEHAVIOR parameter
// default_mode - returns auto_yaw.mode() based on WP_YAW_BEHAVIOR parameter
// set rtl parameter to true if this is during an RTL
uint8_t Copter::get_default_auto_yaw_mode(bool rtl)
autopilot_yaw_mode Copter::Mode::AutoYaw::default_mode(bool rtl) const
{
switch (g.wp_yaw_behavior) {
switch (copter.g.wp_yaw_behavior) {
case WP_YAW_BEHAVIOR_NONE:
return AUTO_YAW_HOLD;
@ -1902,35 +1898,35 @@ uint8_t Copter::get_default_auto_yaw_mode(bool rtl)
}
}
// set_auto_yaw_mode - sets the yaw mode for auto
void Copter::set_auto_yaw_mode(uint8_t yaw_mode)
// set_mode - sets the yaw mode for auto
void Copter::Mode::AutoYaw::set_mode(autopilot_yaw_mode yaw_mode)
{
// return immediately if no change
if (auto_yaw_mode == yaw_mode) {
if (_mode == yaw_mode) {
return;
}
auto_yaw_mode = yaw_mode;
_mode = yaw_mode;
// perform initialisation
switch (auto_yaw_mode) {
switch (_mode) {
case AUTO_YAW_LOOK_AT_NEXT_WP:
// wpnav will initialise heading when wpnav's set_destination method is called
break;
case AUTO_YAW_ROI:
// point towards a location held in yaw_look_at_WP
yaw_look_at_WP_bearing = ahrs.yaw_sensor;
// look ahead until we know otherwise
_roi_yaw = copter.ahrs.yaw_sensor;
break;
case AUTO_YAW_LOOK_AT_HEADING:
// keep heading pointing in the direction held in yaw_look_at_heading
// caller should set the yaw_look_at_heading
case AUTO_YAW_FIXED:
// keep heading pointing in the direction held in fixed_yaw
// caller should set the fixed_yaw
break;
case AUTO_YAW_LOOK_AHEAD:
// Commanded Yaw to automatically look ahead.
yaw_look_ahead_bearing = ahrs.yaw_sensor;
_look_ahead_yaw = copter.ahrs.yaw_sensor;
break;
case AUTO_YAW_RESETTOARMEDYAW:
@ -1939,66 +1935,65 @@ void Copter::set_auto_yaw_mode(uint8_t yaw_mode)
case AUTO_YAW_RATE:
// initialise target yaw rate to zero
auto_yaw_rate_cds = 0.0f;
_rate_cds = 0.0f;
break;
}
}
// set_auto_yaw_look_at_heading - sets the yaw look at heading for auto mode
void Copter::set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, bool relative_angle)
// set_fixed_yaw - sets the yaw look at heading for auto mode
void Copter::Mode::AutoYaw::set_fixed_yaw(float angle_deg, float turn_rate_dps, int8_t direction, bool relative_angle)
{
// get current yaw target
int32_t curr_yaw_target = attitude_control->get_att_target_euler_cd().z;
const int32_t curr_yaw_target = copter.attitude_control->get_att_target_euler_cd().z;
// calculate final angle as relative to vehicle heading or absolute
if (!relative_angle) {
// absolute angle
yaw_look_at_heading = wrap_360_cd(angle_deg * 100);
_fixed_yaw = wrap_360_cd(angle_deg * 100);
} else {
// relative angle
if (direction < 0) {
angle_deg = -angle_deg;
}
yaw_look_at_heading = wrap_360_cd((angle_deg * 100) + curr_yaw_target);
_fixed_yaw = wrap_360_cd((angle_deg * 100) + curr_yaw_target);
}
// get turn speed
if (is_zero(turn_rate_dps)) {
// default to regular auto slew rate
yaw_look_at_heading_slew = AUTO_YAW_SLEW_RATE;
_fixed_yaw_slewrate = AUTO_YAW_SLEW_RATE;
} else {
int32_t turn_rate = (wrap_180_cd(yaw_look_at_heading - curr_yaw_target) / 100) / turn_rate_dps;
yaw_look_at_heading_slew = constrain_int32(turn_rate, 1, 360); // deg / sec
const int32_t turn_rate = (wrap_180_cd(_fixed_yaw - curr_yaw_target) / 100) / turn_rate_dps;
_fixed_yaw_slewrate = constrain_int32(turn_rate, 1, 360); // deg / sec
}
// set yaw mode
set_auto_yaw_mode(AUTO_YAW_LOOK_AT_HEADING);
set_mode(AUTO_YAW_FIXED);
// TO-DO: restore support for clockwise and counter clockwise rotation held in cmd.content.yaw.direction. 1 = clockwise, -1 = counterclockwise
}
// set_auto_yaw_roi - sets the yaw to look at roi for auto mode
void Copter::set_auto_yaw_roi(const Location &roi_location)
// set_roi - sets the yaw to look at roi for auto mode
void Copter::Mode::AutoYaw::set_roi(const Location &roi_location)
{
// if location is zero lat, lon and altitude turn off ROI
if (roi_location.alt == 0 && roi_location.lat == 0 && roi_location.lng == 0) {
// set auto yaw mode back to default assuming the active command is a waypoint command. A more sophisticated method is required to ensure we return to the proper yaw control for the active command
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
auto_yaw.set_mode_to_default(false);
#if MOUNT == ENABLED
// switch off the camera tracking if enabled
if (camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) {
camera_mount.set_mode_to_default();
if (copter.camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) {
copter.camera_mount.set_mode_to_default();
}
#endif // MOUNT == ENABLED
} else {
#if MOUNT == ENABLED
// check if mount type requires us to rotate the quad
if(!camera_mount.has_pan_control()) {
roi_WP = pv_location_to_vector(roi_location);
set_auto_yaw_mode(AUTO_YAW_ROI);
if(!copter.camera_mount.has_pan_control()) {
roi_WP = copter.pv_location_to_vector(roi_location);
auto_yaw.set_mode(AUTO_YAW_ROI);
}
// send the command to the camera mount
camera_mount.set_roi_target(roi_location);
copter.camera_mount.set_roi_target(roi_location);
// TO-DO: expand handling of the do_nav_roi to support all modes of the MAVLink. Currently we only handle mode 4 (see below)
// 0: do nothing
@ -2009,53 +2004,54 @@ void Copter::set_auto_yaw_roi(const Location &roi_location)
#else
// if we have no camera mount aim the quad at the location
roi_WP = pv_location_to_vector(roi_location);
set_auto_yaw_mode(AUTO_YAW_ROI);
auto_yaw.set_mode(AUTO_YAW_ROI);
#endif // MOUNT == ENABLED
}
}
// set auto yaw rate in centi-degrees per second
void Copter::set_auto_yaw_rate(float turn_rate_cds)
void Copter::Mode::AutoYaw::set_rate(float turn_rate_cds)
{
set_auto_yaw_mode(AUTO_YAW_RATE);
auto_yaw_rate_cds = turn_rate_cds;
set_mode(AUTO_YAW_RATE);
_rate_cds = turn_rate_cds;
}
// get_auto_heading - returns target heading depending upon auto_yaw_mode
// 100hz update rate
float Copter::get_auto_heading(void)
// yaw - returns target heading depending upon auto_yaw.mode()
float Copter::Mode::AutoYaw::yaw()
{
switch(auto_yaw_mode) {
switch(_mode) {
case AUTO_YAW_ROI:
// point towards a location held in roi_WP
return get_roi_yaw();
return roi_yaw();
case AUTO_YAW_LOOK_AT_HEADING:
// keep heading pointing in the direction held in yaw_look_at_heading with no pilot input allowed
return yaw_look_at_heading;
case AUTO_YAW_FIXED:
// keep heading pointing in the direction held in fixed_yaw
// with no pilot input allowed
return _fixed_yaw;
case AUTO_YAW_LOOK_AHEAD:
// Commanded Yaw to automatically look ahead.
return get_look_ahead_yaw();
return look_ahead_yaw();
case AUTO_YAW_RESETTOARMEDYAW:
// changes yaw to be same as when quad was armed
return initial_armed_bearing;
return copter.initial_armed_bearing;
case AUTO_YAW_LOOK_AT_NEXT_WP:
default:
// point towards next waypoint.
// we don't use wp_bearing because we don't want the copter to turn too much during flight
return wp_nav->get_yaw();
return copter.wp_nav->get_yaw();
}
}
// returns yaw rate held in auto_yaw_rate and normally set by SET_POSITION_TARGET mavlink messages (positive it clockwise, negative is counter clockwise)
float Copter::get_auto_yaw_rate_cds(void)
// returns yaw rate normally set by SET_POSITION_TARGET mavlink
// messages (positive is clockwise, negative is counter clockwise)
float Copter::Mode::AutoYaw::rate_cds() const
{
if (auto_yaw_mode == AUTO_YAW_RATE) {
return auto_yaw_rate_cds;
if (_mode == AUTO_YAW_RATE) {
return _rate_cds;
}
// return zero turn rate (this should never happen)

52
ArduCopter/mode_guided.cpp
View File

@ -40,7 +40,7 @@ bool Copter::ModeGuided::init(bool ignore_checks)
{
if (copter.position_ok() || ignore_checks) {
// initialise yaw
set_auto_yaw_mode(copter.get_default_auto_yaw_mode(false));
auto_yaw.set_mode_to_default(false);
// start in position control mode
pos_control_start();
return true;
@ -67,7 +67,7 @@ bool Copter::ModeGuided::takeoff_start(float final_alt_above_home)
}
// initialise yaw
set_auto_yaw_mode(AUTO_YAW_HOLD);
auto_yaw.set_mode(AUTO_YAW_HOLD);
// clear i term when we're taking off
set_throttle_takeoff();
@ -95,7 +95,7 @@ void Copter::ModeGuided::pos_control_start()
wp_nav->set_wp_destination(stopping_point, false);
// initialise yaw
set_auto_yaw_mode(copter.get_default_auto_yaw_mode(false));
auto_yaw.set_mode_to_default(false);
}
// initialise guided mode's velocity controller
@ -140,7 +140,7 @@ void Copter::ModeGuided::posvel_control_start()
pos_control->set_accel_z(wp_nav->get_accel_z());
// pilot always controls yaw
set_auto_yaw_mode(AUTO_YAW_HOLD);
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
// initialise guided mode's angle controller
@ -169,7 +169,7 @@ void Copter::ModeGuided::angle_control_start()
guided_angle_state.use_yaw_rate = false;
// pilot always controls yaw
set_auto_yaw_mode(AUTO_YAW_HOLD);
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
// guided_set_destination - sets guided mode's target destination
@ -417,7 +417,7 @@ void Copter::ModeGuided::pos_control_run()
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (!is_zero(target_yaw_rate)) {
set_auto_yaw_mode(AUTO_YAW_HOLD);
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
}
@ -431,15 +431,15 @@ void Copter::ModeGuided::pos_control_run()
pos_control->update_z_controller();
// call attitude controller
if (copter.auto_yaw_mode == AUTO_YAW_HOLD) {
if (auto_yaw.mode() == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate);
} else if (auto_yaw_mode == AUTO_YAW_RATE) {
} else if (auto_yaw.mode() == AUTO_YAW_RATE) {
// roll & pitch from waypoint controller, yaw rate from mavlink command or mission item
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_yaw_rate_cds());
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.rate_cds());
} else {
// roll, pitch from waypoint controller, yaw heading from GCS or auto_heading()
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(), true);
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.yaw(), true);
}
}
@ -461,7 +461,7 @@ void Copter::ModeGuided::vel_control_run()
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (!is_zero(target_yaw_rate)) {
set_auto_yaw_mode(AUTO_YAW_HOLD);
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
}
@ -474,8 +474,8 @@ void Copter::ModeGuided::vel_control_run()
if (!pos_control->get_desired_velocity().is_zero()) {
set_desired_velocity_with_accel_and_fence_limits(Vector3f(0.0f, 0.0f, 0.0f));
}
if (auto_yaw_mode == AUTO_YAW_RATE) {
set_auto_yaw_rate(0.0f);
if (auto_yaw.mode() == AUTO_YAW_RATE) {
auto_yaw.set_rate(0.0f);
}
} else {
set_desired_velocity_with_accel_and_fence_limits(guided_vel_target_cms);
@ -485,15 +485,15 @@ void Copter::ModeGuided::vel_control_run()
pos_control->update_vel_controller_xyz(ekfNavVelGainScaler);
// call attitude controller
if (copter.auto_yaw_mode == AUTO_YAW_HOLD) {
if (auto_yaw.mode() == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), target_yaw_rate);
} else if (auto_yaw_mode == AUTO_YAW_RATE) {
} else if (auto_yaw.mode() == AUTO_YAW_RATE) {
// roll & pitch from velocity controller, yaw rate from mavlink command or mission item
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), get_auto_yaw_rate_cds());
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), auto_yaw.rate_cds());
} else {
// roll, pitch from waypoint controller, yaw heading from GCS or auto_heading()
attitude_control->input_euler_angle_roll_pitch_yaw(pos_control->get_roll(), pos_control->get_pitch(), get_auto_heading(), true);
attitude_control->input_euler_angle_roll_pitch_yaw(pos_control->get_roll(), pos_control->get_pitch(), auto_yaw.yaw(), true);
}
}
@ -517,7 +517,7 @@ void Copter::ModeGuided::posvel_control_run()
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (!is_zero(target_yaw_rate)) {
set_auto_yaw_mode(AUTO_YAW_HOLD);
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
}
@ -528,8 +528,8 @@ void Copter::ModeGuided::posvel_control_run()
uint32_t tnow = millis();
if (tnow - posvel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS) {
guided_vel_target_cms.zero();
if (auto_yaw_mode == AUTO_YAW_RATE) {
set_auto_yaw_rate(0.0f);
if (auto_yaw.mode() == AUTO_YAW_RATE) {
auto_yaw.set_rate(0.0f);
}
}
@ -553,15 +553,15 @@ void Copter::ModeGuided::posvel_control_run()
pos_control->update_z_controller();
// call attitude controller
if (copter.auto_yaw_mode == AUTO_YAW_HOLD) {
if (auto_yaw.mode() == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), target_yaw_rate);
} else if (auto_yaw_mode == AUTO_YAW_RATE) {
} else if (auto_yaw.mode() == AUTO_YAW_RATE) {
// roll & pitch from position-velocity controller, yaw rate from mavlink command or mission item
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), get_auto_yaw_rate_cds());
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), auto_yaw.rate_cds());
} else {
// roll, pitch from waypoint controller, yaw heading from GCS or auto_heading()
attitude_control->input_euler_angle_roll_pitch_yaw(pos_control->get_roll(), pos_control->get_pitch(), get_auto_heading(), true);
attitude_control->input_euler_angle_roll_pitch_yaw(pos_control->get_roll(), pos_control->get_pitch(), auto_yaw.yaw(), true);
}
}
@ -662,9 +662,9 @@ void Copter::ModeGuided::set_desired_velocity_with_accel_and_fence_limits(const
void Copter::ModeGuided::set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle)
{
if (use_yaw) {
set_auto_yaw_look_at_heading(yaw_cd / 100.0f, 0.0f, 0, relative_angle);
auto_yaw.set_fixed_yaw(yaw_cd / 100.0f, 0.0f, 0, relative_angle);
} else if (use_yaw_rate) {
set_auto_yaw_rate(yaw_rate_cds);
auto_yaw.set_rate(yaw_rate_cds);
}
}

28
ArduCopter/mode_rtl.cpp
View File

@ -108,7 +108,7 @@ void Copter::ModeRTL::climb_start()
wp_nav->set_fast_waypoint(true);
// hold current yaw during initial climb
set_auto_yaw_mode(AUTO_YAW_HOLD);
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
// rtl_return_start - initialise return to home
@ -123,7 +123,7 @@ void Copter::ModeRTL::return_start()
}
// initialise yaw to point home (maybe)
set_auto_yaw_mode(copter.get_default_auto_yaw_mode(true));
auto_yaw.set_mode_to_default(true);
}
// rtl_climb_return_run - implements the initial climb, return home and descent portions of RTL which all rely on the wp controller
@ -143,7 +143,7 @@ void Copter::ModeRTL::climb_return_run()
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (!is_zero(target_yaw_rate)) {
set_auto_yaw_mode(AUTO_YAW_HOLD);
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
}
@ -157,12 +157,12 @@ void Copter::ModeRTL::climb_return_run()
pos_control->update_z_controller();
// call attitude controller
if (copter.auto_yaw_mode == AUTO_YAW_HOLD) {
if (auto_yaw.mode() == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate);
}else{
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(),true);
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.yaw(),true);
}
// check if we've completed this stage of RTL
@ -177,10 +177,10 @@ void Copter::ModeRTL::loiterathome_start()
_loiter_start_time = millis();
// yaw back to initial take-off heading yaw unless pilot has already overridden yaw
if(copter.get_default_auto_yaw_mode(true) != AUTO_YAW_HOLD) {
set_auto_yaw_mode(AUTO_YAW_RESETTOARMEDYAW);
if(auto_yaw.default_mode(true) != AUTO_YAW_HOLD) {
auto_yaw.set_mode(AUTO_YAW_RESETTOARMEDYAW);
} else {
set_auto_yaw_mode(AUTO_YAW_HOLD);
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
}
@ -201,7 +201,7 @@ void Copter::ModeRTL::loiterathome_run()
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (!is_zero(target_yaw_rate)) {
set_auto_yaw_mode(AUTO_YAW_HOLD);
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
}
@ -215,17 +215,17 @@ void Copter::ModeRTL::loiterathome_run()
pos_control->update_z_controller();
// call attitude controller
if (copter.auto_yaw_mode == AUTO_YAW_HOLD) {
if (auto_yaw.mode() == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate);
}else{
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(),true);
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.yaw(),true);
}
// check if we've completed this stage of RTL
if ((millis() - _loiter_start_time) >= (uint32_t)g.rtl_loiter_time.get()) {
if (copter.auto_yaw_mode == AUTO_YAW_RESETTOARMEDYAW) {
if (auto_yaw.mode() == AUTO_YAW_RESETTOARMEDYAW) {
// check if heading is within 2 degrees of heading when vehicle was armed
if (labs(wrap_180_cd(ahrs.yaw_sensor-copter.initial_armed_bearing)) <= 200) {
_state_complete = true;
@ -250,7 +250,7 @@ void Copter::ModeRTL::descent_start()
pos_control->set_target_to_stopping_point_z();
// initialise yaw
set_auto_yaw_mode(AUTO_YAW_HOLD);
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
// rtl_descent_run - implements the final descent to the RTL_ALT
@ -332,7 +332,7 @@ void Copter::ModeRTL::land_start()
}
// initialise yaw
set_auto_yaw_mode(AUTO_YAW_HOLD);
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
bool Copter::ModeRTL::landing_gear_should_be_deployed() const

10
ArduCopter/mode_smart_rtl.cpp
View File

@ -22,7 +22,7 @@ bool Copter::ModeSmartRTL::init(bool ignore_checks)
wp_nav->set_wp_destination(stopping_point);
// initialise yaw to obey user parameter
copter.set_auto_yaw_mode(copter.get_default_auto_yaw_mode(true));
auto_yaw.set_mode_to_default(true);
// wait for cleanup of return path
smart_rtl_state = SmartRTL_WaitForPathCleanup;
@ -65,7 +65,7 @@ void Copter::ModeSmartRTL::wait_cleanup_run()
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
wp_nav->update_wpnav();
pos_control->update_z_controller();
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(),true);
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.yaw(),true);
// check if return path is computed and if yes, begin journey home
if (g2.smart_rtl.request_thorough_cleanup()) {
@ -101,12 +101,12 @@ void Copter::ModeSmartRTL::path_follow_run()
pos_control->update_z_controller();
// call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) {
if (auto_yaw.mode() == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), 0);
} else {
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(),true);
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.yaw(), true);
}
}
@ -129,7 +129,7 @@ void Copter::ModeSmartRTL::pre_land_position_run()
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
wp_nav->update_wpnav();
pos_control->update_z_controller();
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(), true);
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.yaw(), true);
}
// save current position for use by the smart_rtl flight mode