aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AC_PosControl: rename xy_mode enum values

Browse Source 
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

11
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); desired_vel_to_pos(dt);
// run position controller's position error to desired velocity step // 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 // run velocity to acceleration step
rate_to_accel_xy(dt, ekfNavVelGainScaler); rate_to_accel_xy(dt, ekfNavVelGainScaler);
@ -693,7 +693,7 @@ void AC_PosControl::pos_to_rate_xy(xy_mode mode, float dt, float ekfNavVelGainSc
_vel_target.y = _p_pos_xy.kP() * _pos_error.y; _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 // this mode is for loiter - rate-limiting the position correction
// allows the pilot to always override the position correction in // allows the pilot to always override the position correction in
// the event of a disturbance // the event of a disturbance
@ -705,16 +705,17 @@ void AC_PosControl::pos_to_rate_xy(xy_mode mode, float dt, float ekfNavVelGainSc
_vel_target.y = POSCONTROL_VEL_XY_MAX_FROM_POS_ERR * _vel_target.y/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.x += _vel_desired.x;
_vel_target.y += _vel_desired.y; _vel_target.y += _vel_desired.y;
} else { } else {
if (mode == XY_MODE_POS_AND_VEL) { if (mode == XY_MODE_POS_AND_VEL_FF) {
// add feedforward // add velocity feed-forward
_vel_target.x += _vel_desired.x; _vel_target.x += _vel_desired.x;
_vel_target.y += _vel_desired.y; _vel_target.y += _vel_desired.y;
} }
// scale velocity within speed limit
float vel_total = pythagorous2(_vel_target.x, _vel_target.y); float vel_total = pythagorous2(_vel_target.x, _vel_target.y);
if (vel_total > _speed_cms) { if (vel_total > _speed_cms) {
_vel_target.x = _speed_cms * _vel_target.x/vel_total; _vel_target.x = _speed_cms * _vel_target.x/vel_total;

7
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -52,10 +52,11 @@ public:
AC_P& p_alt_pos, AC_P& p_alt_rate, AC_PID& pid_alt_accel, 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); 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 { enum xy_mode {
XY_MODE_POS_ONLY = 0, XY_MODE_POS_ONLY = 0, // position correction only (i.e. no velocity feed-forward)
XY_MODE_SLOW_POS_AND_VEL, XY_MODE_POS_LIMITED_AND_VEL_FF, // for loiter - rate-limiting the position correction, velocity feed-forward
XY_MODE_POS_AND_VEL XY_MODE_POS_AND_VEL_FF // for velocity controller - unlimied position correction, velocity feed-forward
}; };
/// ///