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

AC_PosControl: rename xy_mode enum values 

Also added a few comments and fixed formatting
This commit is contained in:
Randy Mackay 2015-02-06 16:59:00 +09:00
parent 626521c366
commit 186337f18e
2 changed files with 15 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -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;

9
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -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
};
///