diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 4fc5e43050..1aed7af786 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -599,7 +599,7 @@ void AC_PosControl::update_vel_controller_xyz(float ekfNavVelGainScaler) desired_vel_to_pos(dt); // run position controller's position error to desired velocity step - pos_to_rate_xy(XY_MODE_SLOW_POS_AND_VEL, dt, ekfNavVelGainScaler); + pos_to_rate_xy(XY_MODE_POS_LIMITED_AND_VEL_FF, dt, ekfNavVelGainScaler); // run velocity to acceleration step rate_to_accel_xy(dt, ekfNavVelGainScaler); @@ -692,29 +692,30 @@ void AC_PosControl::pos_to_rate_xy(xy_mode mode, float dt, float ekfNavVelGainSc _vel_target.x = _p_pos_xy.kP() * _pos_error.x; _vel_target.y = _p_pos_xy.kP() * _pos_error.y; } - - if (mode == XY_MODE_SLOW_POS_AND_VEL) { + + if (mode == XY_MODE_POS_LIMITED_AND_VEL_FF) { // this mode is for loiter - rate-limiting the position correction // allows the pilot to always override the position correction in // the event of a disturbance - + // scale velocity within limit float vel_total = pythagorous2(_vel_target.x, _vel_target.y); if (vel_total > POSCONTROL_VEL_XY_MAX_FROM_POS_ERR) { _vel_target.x = POSCONTROL_VEL_XY_MAX_FROM_POS_ERR * _vel_target.x/vel_total; _vel_target.y = POSCONTROL_VEL_XY_MAX_FROM_POS_ERR * _vel_target.y/vel_total; } - - //add feedforward + + // add velocity feed-forward _vel_target.x += _vel_desired.x; _vel_target.y += _vel_desired.y; } else { - if (mode == XY_MODE_POS_AND_VEL) { - // add feedforward + if (mode == XY_MODE_POS_AND_VEL_FF) { + // add velocity feed-forward _vel_target.x += _vel_desired.x; _vel_target.y += _vel_desired.y; } - + + // scale velocity within speed limit float vel_total = pythagorous2(_vel_target.x, _vel_target.y); if (vel_total > _speed_cms) { _vel_target.x = _speed_cms * _vel_target.x/vel_total; diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 36958e705a..02d0a9a41e 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -51,11 +51,12 @@ public: const AP_Motors& motors, AC_AttitudeControl& attitude_control, AC_P& p_alt_pos, AC_P& p_alt_rate, AC_PID& pid_alt_accel, AC_P& p_pos_xy, AC_PID& pid_rate_lat, AC_PID& pid_rate_lon); - + + // xy_mode - specifies behavior of xy position controller enum xy_mode { - XY_MODE_POS_ONLY = 0, - XY_MODE_SLOW_POS_AND_VEL, - XY_MODE_POS_AND_VEL + XY_MODE_POS_ONLY = 0, // position correction only (i.e. no velocity feed-forward) + XY_MODE_POS_LIMITED_AND_VEL_FF, // for loiter - rate-limiting the position correction, velocity feed-forward + XY_MODE_POS_AND_VEL_FF // for velocity controller - unlimied position correction, velocity feed-forward }; ///