Copter: create an AutoYaw helper object to hold auto-yaw state
This commit is contained in:
Peter Barker
Randy Mackay
parent
63e42e194f
commit
ba8b3e2415
@ -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;
|
||||
}
|
||||
|
||||
/*************************************************************
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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();
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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));
|
||||
}
|
||||
|
||||
@ -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);
|
||||
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user