AC_WPNav: new loiter including accel ff and braking

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

libraries/AC_WPNav/AC_WPNav.cpp

@@ -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
+    // @Param: BRK_JERK
-    // @DisplayName: Loiter maximum jerk
+    // @DisplayName: Loiter braking jerk
-    // @Description: Loiter maximum jerk in cm/s/s/s
+    // @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
+    // @DisplayName: Loiter maximum correction acceleration
-    // @Description: Loiter maximum acceleration in cm/s/s.  Higher values cause the copter to accelerate and stop more quickly.
+    // @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
+    // @Param: BRK_ACCEL
-    // @DisplayName: Loiter minimum acceleration
+    // @DisplayName: Loiter braking 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.
+    // @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
+    // initialise pos controller speed, acceleration
-    _pos_control.init_xy_controller(reset_I);
+    _pos_control.set_speed_xy(WPNAV_LOITER_VEL_CORRECTION_MAX);
-
-    // initialise pos controller speed, acceleration and jerk
-    _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);
+
+    // 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
+    // set vehicle velocity and acceleration to zero
-    _pos_control.set_desired_velocity_xy(0,0);
+    _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
+    // initialise position controller
-    _loiter_desired_accel.x = 0;
+    _pos_control.init_xy_controller();
-    _loiter_desired_accel.y = 0;
-
-    // initialise pilot input
-    _pilot_accel_fwd_cms = 0;
-    _pilot_accel_rgt_cms = 0;
 }
 
 /// 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
+    // initialise position controller
-    _loiter_desired_accel.x = (_loiter_accel_cmss)*curr_vel.x/_loiter_speed_cms;
+    _pos_control.init_xy_controller();
-    _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;
 }
 
 /// 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
+/// set pilot desired acceleration in centi-degrees
-void AC_WPNav::set_pilot_desired_acceleration(float control_roll, float control_pitch)
+//   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
+    // Convert from centidegrees on public interface to radians
-    _pilot_accel_fwd_cms = -control_pitch * _loiter_accel_cmss / 4500.0f;
+    const float euler_roll_angle = radians(euler_roll_angle_cd*0.01f);
-    _pilot_accel_rgt_cms = control_roll * _loiter_accel_cmss / 4500.0f;
+    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
+    // get loiters desired velocity from the position controller where it is being stored.
-    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
     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
+    // update the desired velocity using our predicted acceleration
-    desired_vel.x += _loiter_desired_accel.x * nav_dt;
+    desired_vel.x += _loiter_predicted_accel.x * nav_dt;
-    desired_vel.y += _loiter_desired_accel.y * 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) {
+        // TODO: consider using a velocity squared relationship like
-            drag_speed_delta = MIN(drag_speed_delta,MAX(-_loiter_accel_min_cmss*nav_dt, -2.0f*desired_speed*nav_dt));
+        // 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

libraries/AC_WPNav/AC_WPNav.h

@@ -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_MAX          500.0f      // default acceleration in loiter mode
-#define WPNAV_LOITER_ACCEL_MIN           25.0f      // minimum acceleration in loiter mode
+#define WPNAV_LOITER_BRAKE_ACCEL        250.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_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
+    /// set pilot desired acceleration in centi-degrees
-    void set_pilot_desired_acceleration(float control_roll, float control_pitch);
+    //   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