Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_WPNav: new loiter including accel ff and braking

Browse Source 
Includes:
sanity check loiter accel max vs lean angle max
increase loiter aggressiveness
remove loiter calls to pos_con.freeze_ff_xy
use loiter max velocity correction
remove xy mode from loiter
move predictor to angle
add maximum pilot commanded angle to loiter
loiter use alt hold angle limit
set_pilot_desired_acceleration accept dt

Also includes:
remove loiter jerk and setting pos-con jerk
breaking gain is based on VelxyP
Jerk limit Loiter breaking
change breaking parameters use jerk limit
add maximum distance correction in Loiter to 2m
update Loiter parameter defaults
This commit is contained in:
Leonard Hall 2017-06-23 21:45:39 +09:30 committed by Randy Mackay
parent c7c3dd561a
commit 1c0678226c
2 changed files with 163 additions and 98 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

225
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -69,32 +69,32 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] = {
// @User: Standard
AP_GROUPINFO("ACCEL_Z", 6, AC_WPNav, _wp_accel_z_cms, WPNAV_WP_ACCEL_Z_DEFAULT),
// @Param: LOIT_JERK
// @DisplayName: Loiter maximum jerk
// @Description: Loiter maximum jerk in cm/s/s/s
// @Param: BRK_JERK
// @DisplayName: Loiter braking jerk
// @Description: Loiter braking jerk in cm/s/s/s. Higher values will remove braking faster if the pilot moves the sticks during a braking manuver.
// @Units: cm/s/s/s
// @Range: 500 5000
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("LOIT_JERK", 7, AC_WPNav, _loiter_jerk_max_cmsss, WPNAV_LOITER_JERK_MAX_DEFAULT),
AP_GROUPINFO("BRK_JERK", 7, AC_WPNav, _loiter_brake_jerk_max_cmsss, WPNAV_LOITER_BRAKE_JERK),
// @Param: LOIT_MAXA
// @DisplayName: Loiter maximum acceleration
// @Description: Loiter maximum acceleration in cm/s/s. Higher values cause the copter to accelerate and stop more quickly.
// @DisplayName: Loiter maximum correction acceleration
// @Description: Loiter maximum correction acceleration in cm/s/s. Higher values cause the copter to correct possition errors more aggressivly.
// @Units: cm/s/s
// @Range: 100 981
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("LOIT_MAXA", 8, AC_WPNav, _loiter_accel_cmss, WPNAV_LOITER_ACCEL),
AP_GROUPINFO("LOIT_MAXA", 8, AC_WPNav, _loiter_accel_cmss, WPNAV_LOITER_ACCEL_MAX),
// @Param: LOIT_MINA
// @DisplayName: Loiter minimum acceleration
// @Description: Loiter minimum acceleration in cm/s/s. Higher values stop the copter more quickly when the stick is centered, but cause a larger jerk when the copter stops.
// @Param: BRK_ACCEL
// @DisplayName: Loiter braking acceleration
// @Description: Loiter braking acceleration in cm/s/s. Higher values stop the copter more quickly when the stick is centered.
// @Units: cm/s/s
// @Range: 25 250
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("LOIT_MINA", 9, AC_WPNav, _loiter_accel_min_cmss, WPNAV_LOITER_ACCEL_MIN),
AP_GROUPINFO("BRK_ACCEL", 9, AC_WPNav, _loiter_brake_accel_cmss, WPNAV_LOITER_BRAKE_ACCEL),
// @Param: RFND_USE
// @DisplayName: Waypoint missions use rangefinder for terrain following
@ -102,7 +102,25 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] = {
// @Values: 0:Disable,1:Enable
// @User: Advanced
AP_GROUPINFO("RFND_USE", 10, AC_WPNav, _rangefinder_use, 1),
// @Param: BRK_DELAY
// @DisplayName: Loiter brake start delay (in seconds)
// @Description: Loiter brake start delay (in seconds)
// @Units: s
// @Range: 0 2
// @Increment: 0.1
// @User: Advanced
AP_GROUPINFO("BRK_DELAY", 11, AC_WPNav, _loiter_brake_delay, WPNAV_LOITER_BRAKE_START_DELAY),
// @Param: LOIT_ANGM
// @DisplayName: Loiter Angle Max
// @Description: Loiter maximum lean angle. Set to zero for 2/3 of PSC_ANGLE_MAX or ANGLE_MAX
// @Units: deg
// @Range: 0 45
// @Increment: 1
// @User: Advanced
AP_GROUPINFO("LOIT_ANGM", 12, AC_WPNav, _loiter_angle_max, 0.0f),
AP_GROUPEND
};
@ -115,8 +133,6 @@ AC_WPNav::AC_WPNav(const AP_InertialNav& inav, const AP_AHRS_View& ahrs, AC_PosC
_ahrs(ahrs),
_pos_control(pos_control),
_attitude_control(attitude_control),
_pilot_accel_fwd_cms(0),
_pilot_accel_rgt_cms(0),
_wp_last_update(0),
_wp_step(0),
_track_length(0.0f),
@ -144,6 +160,7 @@ AC_WPNav::AC_WPNav(const AP_InertialNav& inav, const AP_AHRS_View& ahrs, AC_PosC
// sanity check some parameters
_loiter_speed_cms = MAX(_loiter_speed_cms, WPNAV_LOITER_SPEED_MIN);
_loiter_accel_cmss = MIN(_loiter_accel_cmss, GRAVITY_MSS * 100.0f * tanf(ToRad(_attitude_control.lean_angle_max() * 0.01f)));
_wp_accel_cms = MIN(_wp_accel_cms, GRAVITY_MSS * 100.0f * tanf(ToRad(_attitude_control.lean_angle_max() * 0.01f)));
_wp_radius_cm = MAX(_wp_radius_cm, WPNAV_WP_RADIUS_MIN);
}
@ -155,27 +172,26 @@ AC_WPNav::AC_WPNav(const AP_InertialNav& inav, const AP_AHRS_View& ahrs, AC_PosC
/// init_loiter_target in cm from home
void AC_WPNav::init_loiter_target(const Vector3f& position, bool reset_I)
{
// initialise position controller
_pos_control.init_xy_controller(reset_I);
// initialise pos controller speed, acceleration and jerk
_pos_control.set_speed_xy(_loiter_speed_cms);
// initialise pos controller speed, acceleration
_pos_control.set_speed_xy(WPNAV_LOITER_VEL_CORRECTION_MAX);
_pos_control.set_accel_xy(_loiter_accel_cmss);
_pos_control.set_jerk_xy(_loiter_jerk_max_cmsss);
// initialise desired acceleration and angles to zero to remain on station
_loiter_predicted_accel.x = 0.0f;
_loiter_predicted_accel.y = 0.0f;
_loiter_desired_accel = _loiter_predicted_accel;
_loiter_predicted_euler_angle.x = 0.0f;
_loiter_predicted_euler_angle.y = 0.0f;
// set target position
_pos_control.set_xy_target(position.x, position.y);
// initialise feed forward velocity to zero
_pos_control.set_desired_velocity_xy(0,0);
// set vehicle velocity and acceleration to zero
_pos_control.set_desired_velocity_xy(0.0f,0.0f);
_pos_control.set_desired_accel_xy(0.0f,0.0f);
// initialise desired accel and add fake wind
_loiter_desired_accel.x = 0;
_loiter_desired_accel.y = 0;
// initialise pilot input
_pilot_accel_fwd_cms = 0;
_pilot_accel_rgt_cms = 0;
// initialise position controller
_pos_control.init_xy_controller();
}
/// init_loiter_target - initialize's loiter position and feed-forward velocity from current pos and velocity
@ -184,30 +200,32 @@ void AC_WPNav::init_loiter_target()
const Vector3f& curr_pos = _inav.get_position();
const Vector3f& curr_vel = _inav.get_velocity();
// initialise position controller
_pos_control.init_xy_controller();
// sanity check loiter speed
_loiter_speed_cms = MAX(_loiter_speed_cms, WPNAV_LOITER_SPEED_MIN);
// initialise pos controller speed and acceleration
_pos_control.set_speed_xy(_loiter_speed_cms);
_pos_control.set_speed_xy(WPNAV_LOITER_VEL_CORRECTION_MAX);
_pos_control.set_accel_xy(_loiter_accel_cmss);
_pos_control.set_jerk_xy(_loiter_jerk_max_cmsss);
_pos_control.set_leash_length_xy(WPNAV_LOITER_POS_CORRECTION_MAX);
// initialise desired acceleration based on the current velocity and the artificial drag
float pilot_acceleration_max = GRAVITY_MSS*100.0f * tanf(radians(get_loiter_angle_max_cd()*0.01f));
_loiter_predicted_accel.x = pilot_acceleration_max*curr_vel.x/_loiter_speed_cms;
_loiter_predicted_accel.y = pilot_acceleration_max*curr_vel.y/_loiter_speed_cms;
_loiter_desired_accel = _loiter_predicted_accel;
// this should be the current roll and pitch angle.
_loiter_predicted_euler_angle.x = radians(_attitude_control.get_att_target_euler_cd().x*0.01f);
_loiter_predicted_euler_angle.y = radians(_attitude_control.get_att_target_euler_cd().y*0.01f);
// set target position
_pos_control.set_xy_target(curr_pos.x, curr_pos.y);
// move current vehicle velocity into feed forward velocity
// set vehicle velocity and acceleration to current state
_pos_control.set_desired_velocity_xy(curr_vel.x, curr_vel.y);
_pos_control.set_desired_accel_xy(_loiter_desired_accel.x,_loiter_desired_accel.y);
// initialise desired accel and add fake wind
_loiter_desired_accel.x = (_loiter_accel_cmss)*curr_vel.x/_loiter_speed_cms;
_loiter_desired_accel.y = (_loiter_accel_cmss)*curr_vel.y/_loiter_speed_cms;
// initialise pilot input
_pilot_accel_fwd_cms = 0;
_pilot_accel_rgt_cms = 0;
// initialise position controller
_pos_control.init_xy_controller();
}
/// loiter_soften_for_landing - reduce response for landing
@ -217,21 +235,55 @@ void AC_WPNav::loiter_soften_for_landing()
// set target position to current position
_pos_control.set_xy_target(curr_pos.x, curr_pos.y);
_pos_control.freeze_ff_xy();
}
/// set_pilot_desired_acceleration - sets pilot desired acceleration from roll and pitch stick input
void AC_WPNav::set_pilot_desired_acceleration(float control_roll, float control_pitch)
/// set pilot desired acceleration in centi-degrees
// dt should be the time (in seconds) since the last call to this function
void AC_WPNav::set_pilot_desired_acceleration(float euler_roll_angle_cd, float euler_pitch_angle_cd, float dt)
{
// convert pilot input to desired acceleration in cm/s/s
_pilot_accel_fwd_cms = -control_pitch * _loiter_accel_cmss / 4500.0f;
_pilot_accel_rgt_cms = control_roll * _loiter_accel_cmss / 4500.0f;
// Convert from centidegrees on public interface to radians
const float euler_roll_angle = radians(euler_roll_angle_cd*0.01f);
const float euler_pitch_angle = radians(euler_pitch_angle_cd*0.01f);
// difference between where we think we should be and where we want to be
Vector2f angle_error(wrap_PI(euler_roll_angle-_loiter_predicted_euler_angle.x), wrap_PI(euler_pitch_angle-_loiter_predicted_euler_angle.y));
// calculate the angular velocity that we would expect given our desired and predicted attitude
_attitude_control.input_shaping_rate_predictor(angle_error, _loiter_predicted_euler_rate, dt);
// update our predicted attitude based on our predicted angular velocity
_loiter_predicted_euler_angle += _loiter_predicted_euler_rate * dt;
// convert our desired attitude to an acceleration vector assuming we are hovering
const float pilot_cos_pitch_target = cosf(euler_pitch_angle);
const float pilot_accel_rgt_cms = GRAVITY_MSS*100.0f * tanf(euler_roll_angle)/pilot_cos_pitch_target;
const float pilot_accel_fwd_cms = -GRAVITY_MSS*100.0f * tanf(euler_pitch_angle);
// convert our predicted attitude to an acceleration vector assuming we are hovering
const float pilot_predicted_cos_pitch_target = cosf(_loiter_predicted_euler_angle.y);
const float pilot_predicted_accel_rgt_cms = GRAVITY_MSS*100.0f * tanf(_loiter_predicted_euler_angle.x)/pilot_predicted_cos_pitch_target;
const float pilot_predicted_accel_fwd_cms = -GRAVITY_MSS*100.0f * tanf(_loiter_predicted_euler_angle.y);
// rotate acceleration vectors input to lat/lon frame
_loiter_desired_accel.x = (pilot_accel_fwd_cms*_ahrs.cos_yaw() - pilot_accel_rgt_cms*_ahrs.sin_yaw());
_loiter_desired_accel.y = (pilot_accel_fwd_cms*_ahrs.sin_yaw() + pilot_accel_rgt_cms*_ahrs.cos_yaw());
_loiter_predicted_accel.x = (pilot_predicted_accel_fwd_cms*_ahrs.cos_yaw() - pilot_predicted_accel_rgt_cms*_ahrs.sin_yaw());
_loiter_predicted_accel.y = (pilot_predicted_accel_fwd_cms*_ahrs.sin_yaw() + pilot_predicted_accel_rgt_cms*_ahrs.cos_yaw());
}
/// get_loiter_stopping_point_xy - returns vector to stopping point based on a horizontal position and velocity
void AC_WPNav::get_loiter_stopping_point_xy(Vector3f& stopping_point) const
{
_pos_control.get_stopping_point_xy(stopping_point);
_pos_control.get_stopping_point_xy(stopping_point);
}
/// get_loiter_angle_max - returns the maximum lean angle in loiter mode
float AC_WPNav::get_loiter_angle_max_cd() const
{
if (is_zero(_loiter_angle_max)){
return MIN(_attitude_control.lean_angle_max()*2.0f/3.0f, _pos_control.get_lean_angle_max_cd()*2.0f/3.0f);
}
return MIN(_loiter_angle_max*100.0f, _pos_control.get_lean_angle_max_cd());
}
/// calc_loiter_desired_velocity - updates desired velocity (i.e. feed forward) with pilot requested acceleration and fake wind resistance
@ -243,6 +295,8 @@ void AC_WPNav::calc_loiter_desired_velocity(float nav_dt, float ekfGndSpdLimit)
float gnd_speed_limit_cms = MIN(_loiter_speed_cms,ekfGndSpdLimit*100.0f);
gnd_speed_limit_cms = MAX(gnd_speed_limit_cms, WPNAV_LOITER_SPEED_MIN);
float pilot_acceleration_max = GRAVITY_MSS*100.0f * tanf(radians(get_loiter_angle_max_cd()*0.01f));
// range check nav_dt
if( nav_dt < 0 ) {
return;
@ -250,63 +304,66 @@ void AC_WPNav::calc_loiter_desired_velocity(float nav_dt, float ekfGndSpdLimit)
_pos_control.set_speed_xy(gnd_speed_limit_cms);
_pos_control.set_accel_xy(_loiter_accel_cmss);
_pos_control.set_jerk_xy(_loiter_jerk_max_cmsss);
_pos_control.set_leash_length_xy(WPNAV_LOITER_POS_CORRECTION_MAX);
// rotate pilot input to lat/lon frame
Vector2f desired_accel;
desired_accel.x = (_pilot_accel_fwd_cms*_ahrs.cos_yaw() - _pilot_accel_rgt_cms*_ahrs.sin_yaw());
desired_accel.y = (_pilot_accel_fwd_cms*_ahrs.sin_yaw() + _pilot_accel_rgt_cms*_ahrs.cos_yaw());
// calculate the difference
Vector2f des_accel_diff = (desired_accel - _loiter_desired_accel);
// constrain and scale the desired acceleration
float des_accel_change_total = norm(des_accel_diff.x, des_accel_diff.y);
float accel_change_max = _loiter_jerk_max_cmsss * nav_dt;
if (_loiter_jerk_max_cmsss > 0.0f && des_accel_change_total > accel_change_max && des_accel_change_total > 0.0f) {
des_accel_diff.x = accel_change_max * des_accel_diff.x/des_accel_change_total;
des_accel_diff.y = accel_change_max * des_accel_diff.y/des_accel_change_total;
}
// adjust the desired acceleration
_loiter_desired_accel += des_accel_diff;
// get pos_control's feed forward velocity
// get loiters desired velocity from the position controller where it is being stored.
const Vector3f &desired_vel_3d = _pos_control.get_desired_velocity();
Vector2f desired_vel(desired_vel_3d.x,desired_vel_3d.y);
// add pilot commanded acceleration
desired_vel.x += _loiter_desired_accel.x * nav_dt;
desired_vel.y += _loiter_desired_accel.y * nav_dt;
// update the desired velocity using our predicted acceleration
desired_vel.x += _loiter_predicted_accel.x * nav_dt;
desired_vel.y += _loiter_predicted_accel.y * nav_dt;
Vector2f loiter_accel_brake;
float desired_speed = desired_vel.length();
if (!is_zero(desired_speed)) {
Vector2f desired_vel_norm = desired_vel/desired_speed;
float drag_speed_delta = -_loiter_accel_cmss*nav_dt*desired_speed/gnd_speed_limit_cms;
if (_pilot_accel_fwd_cms == 0 && _pilot_accel_rgt_cms == 0) {
drag_speed_delta = MIN(drag_speed_delta,MAX(-_loiter_accel_min_cmss*nav_dt, -2.0f*desired_speed*nav_dt));
// TODO: consider using a velocity squared relationship like
// pilot_acceleration_max*(desired_speed/gnd_speed_limit_cms)^2;
// the drag characteristic of a multirotor should be examined to generate a curve
// we could add a expo function here to fine tune it
// calculate a drag acceleration based on the desired speed.
float drag_decel = pilot_acceleration_max*desired_speed/gnd_speed_limit_cms;
// calculate a braking acceleration if sticks are at zero
float loiter_brake_accel = 0.0f;
if (_loiter_desired_accel.is_zero()) {
if((AP_HAL::millis()-_brake_timer) > _loiter_brake_delay * 1000.0f){
float brake_gain = _pos_control.get_vel_xy_pid().kP()/2.0f;
loiter_brake_accel = constrain_float(AC_AttitudeControl::sqrt_controller(desired_speed, brake_gain, _loiter_brake_jerk_max_cmsss, nav_dt), 0.0f, _loiter_brake_accel_cmss);
}
} else {
loiter_brake_accel = 0.0f;
_brake_timer = AP_HAL::millis();
}
_loiter_brake_accel += constrain_float(loiter_brake_accel-_loiter_brake_accel, -_loiter_brake_jerk_max_cmsss*nav_dt, _loiter_brake_jerk_max_cmsss*nav_dt);
loiter_accel_brake = desired_vel_norm*_loiter_brake_accel;
desired_speed = MAX(desired_speed+drag_speed_delta,0.0f);
// update the desired velocity using the drag and braking accelerations
desired_speed = MAX(desired_speed-(drag_decel+_loiter_brake_accel)*nav_dt,0.0f);
desired_vel = desired_vel_norm*desired_speed;
}
// add braking to the desired acceleration
_loiter_desired_accel -= loiter_accel_brake;
// Apply EKF limit to desired velocity - this limit is calculated by the EKF and adjusted as required to ensure certain sensor limits are respected (eg optical flow sensing)
float horizSpdDem = sqrtf(sq(desired_vel.x) + sq(desired_vel.y));
float horizSpdDem = desired_vel.length();
if (horizSpdDem > gnd_speed_limit_cms) {
desired_vel.x = desired_vel.x * gnd_speed_limit_cms / horizSpdDem;
desired_vel.y = desired_vel.y * gnd_speed_limit_cms / horizSpdDem;
}
// Limit the velocity to prevent fence violations
// TODO: We need to also limit the _loiter_desired_accel
if (_avoid != nullptr) {
_avoid->adjust_velocity(_pos_control.get_pos_xy_p().kP(), _loiter_accel_cmss, desired_vel, nav_dt);
}
// send adjusted feed forward velocity back to position controller
// send adjusted feed forward acceleration and velocity back to the Position Controller
_pos_control.set_desired_accel_xy(_loiter_desired_accel.x,_loiter_desired_accel.y);
_pos_control.set_desired_velocity_xy(desired_vel.x,desired_vel.y);
}
@ -333,10 +390,9 @@ void AC_WPNav::update_loiter(float ekfGndSpdLimit, float ekfNavVelGainScaler)
// initialise pos controller speed and acceleration
_pos_control.set_speed_xy(_loiter_speed_cms);
_pos_control.set_accel_xy(_loiter_accel_cmss);
_pos_control.set_jerk_xy(_loiter_jerk_max_cmsss);
calc_loiter_desired_velocity(dt,ekfGndSpdLimit);
_pos_control.update_xy_controller(AC_PosControl::XY_MODE_POS_LIMITED_AND_VEL_FF, ekfNavVelGainScaler, true);
_pos_control.update_xy_controller(ekfNavVelGainScaler);
}
}
@ -353,7 +409,6 @@ void AC_WPNav::init_brake_target(float accel_cmss)
// initialise pos controller speed and acceleration
_pos_control.set_speed_xy(curr_vel.length());
_pos_control.set_accel_xy(accel_cmss);
_pos_control.set_jerk_xy(_loiter_jerk_max_cmsss);
_pos_control.calc_leash_length_xy();
_pos_control.get_stopping_point_xy(stopping_point);
@ -402,7 +457,6 @@ void AC_WPNav::wp_and_spline_init()
// initialise position controller speed and acceleration
_pos_control.set_speed_xy(_wp_speed_cms);
_pos_control.set_accel_xy(_wp_accel_cms);
_pos_control.set_jerk_xy_to_default();
_pos_control.set_speed_z(-_wp_speed_down_cms, _wp_speed_up_cms);
_pos_control.set_accel_z(_wp_accel_z_cms);
_pos_control.calc_leash_length_xy();
@ -749,7 +803,6 @@ bool AC_WPNav::update_wpnav()
// allow the accel and speed values to be set without changing
// out of auto mode. This makes it easier to tune auto flight
_pos_control.set_accel_xy(_wp_accel_cms);
_pos_control.set_jerk_xy_to_default();
_pos_control.set_accel_z(_wp_accel_z_cms);
// sanity check dt

36
libraries/AC_WPNav/AC_WPNav.h
View File

@ -14,11 +14,14 @@
#define WPNAV_ACCELERATION 100.0f // defines the default velocity vs distant curve. maximum acceleration in cm/s/s that position controller asks for from acceleration controller
#define WPNAV_ACCELERATION_MIN 50.0f // minimum acceleration in cm/s/s - used for sanity checking _wp_accel parameter
#define WPNAV_LOITER_SPEED 500.0f // default loiter speed in cm/s
#define WPNAV_LOITER_SPEED 1250.0f // default loiter speed in cm/s
#define WPNAV_LOITER_SPEED_MIN 20.0f // minimum loiter speed in cm/s
#define WPNAV_LOITER_ACCEL 250.0f // default acceleration in loiter mode
#define WPNAV_LOITER_ACCEL_MIN 25.0f // minimum acceleration in loiter mode
#define WPNAV_LOITER_JERK_MAX_DEFAULT 1000.0f // maximum jerk in cm/s/s/s in loiter mode
#define WPNAV_LOITER_ACCEL_MAX 500.0f // default acceleration in loiter mode
#define WPNAV_LOITER_BRAKE_ACCEL 250.0f // minimum acceleration in loiter mode
#define WPNAV_LOITER_BRAKE_JERK 500.0f // maximum jerk in cm/s/s/s in loiter mode
#define WPNAV_LOITER_BRAKE_START_DELAY 1.0f // delay (in seconds) before loiter braking begins after sticks are released
#define WPNAV_LOITER_VEL_CORRECTION_MAX 200.0f // max speed used to correct position errors in loiter
#define WPNAV_LOITER_POS_CORRECTION_MAX 200.0f // max position error in loiter
#define WPNAV_WP_SPEED 500.0f // default horizontal speed between waypoints in cm/s
#define WPNAV_WP_SPEED_MIN 20.0f // minimum horizontal speed between waypoints in cm/s
@ -82,13 +85,14 @@ public:
/// calculate_loiter_leash_length - calculates the maximum distance in cm that the target position may be from the current location
void calculate_loiter_leash_length();
/// set_pilot_desired_acceleration - sets pilot desired acceleration from roll and pitch stick input
void set_pilot_desired_acceleration(float control_roll, float control_pitch);
/// set pilot desired acceleration in centi-degrees
// dt should be the time (in seconds) since the last call to this function
void set_pilot_desired_acceleration(float euler_roll_angle_cd, float euler_pitch_angle_cd, float dt);
/// get_pilot_desired_acceleration - gets pilot desired
/// acceleration, body frame, [forward,right]
Vector2f get_pilot_desired_acceleration() const { return Vector2f(_pilot_accel_fwd_cms, _pilot_accel_rgt_cms); }
Vector2f get_pilot_desired_acceleration() const { return Vector2f(_loiter_desired_accel.x, _loiter_desired_accel.y); }
/// clear_pilot_desired_acceleration - clear pilot desired acceleration
void clear_pilot_desired_acceleration() { _pilot_accel_fwd_cms = 0.0f; _pilot_accel_rgt_cms = 0.0f; }
void clear_pilot_desired_acceleration() { _loiter_desired_accel.x = 0.0f; _loiter_desired_accel.y = 0.0f; }
/// get_stopping_point - returns vector to stopping point based on a horizontal position and velocity
void get_loiter_stopping_point_xy(Vector3f& stopping_point) const;
@ -102,6 +106,9 @@ public:
/// get_loiter_target - returns loiter target position
const Vector3f& get_loiter_target() const { return _pos_control.get_pos_target(); }
/// get_loiter_angle_max - returns the maximum lean angle in loiter mode
float get_loiter_angle_max_cd() const;
/// update_loiter - run the loiter controller - should be called at 10hz
void update_loiter(float ekfGndSpdLimit, float ekfNavVelGainScaler);
@ -323,10 +330,12 @@ protected:
AC_Avoid *_avoid = nullptr;
// parameters
AP_Float _loiter_angle_max; // maximum pilot commanded angle in degrees. Set to zero for 2/3 Angle Max
AP_Float _loiter_speed_cms; // maximum horizontal speed in cm/s while in loiter
AP_Float _loiter_jerk_max_cmsss; // maximum jerk in cm/s/s/s while in loiter
AP_Float _loiter_accel_cmss; // loiter's max acceleration in cm/s/s
AP_Float _loiter_accel_min_cmss; // loiter's min acceleration in cm/s/s
AP_Float _loiter_brake_accel_cmss; // loiter's acceleration during braking in cm/s/s
AP_Float _loiter_brake_jerk_max_cmsss;
AP_Float _loiter_brake_delay; // delay (in seconds) before loiter braking begins after sticks are released
AP_Float _wp_speed_cms; // maximum horizontal speed in cm/s during missions
AP_Float _wp_speed_up_cms; // climb speed target in cm/s
AP_Float _wp_speed_down_cms; // descent speed target in cm/s
@ -335,9 +344,12 @@ protected:
AP_Float _wp_accel_z_cms; // vertical acceleration in cm/s/s during missions
// loiter controller internal variables
int16_t _pilot_accel_fwd_cms; // pilot's desired acceleration forward (body-frame)
int16_t _pilot_accel_rgt_cms; // pilot's desired acceleration right (body-frame)
Vector2f _loiter_desired_accel; // slewed pilot's desired acceleration in lat/lon frame
Vector2f _loiter_predicted_accel;//
Vector2f _loiter_predicted_euler_angle;//
Vector2f _loiter_predicted_euler_rate; //
float _brake_timer; //
float _loiter_brake_accel; //
// waypoint controller internal variables
uint32_t _wp_last_update; // time of last update_wpnav call