Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 08:13:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_PosControl: allow control of xy rate constraint behavior

Browse Source
This commit is contained in:
Jonathan Challinger 2015-01-12 14:46:50 -08:00 committed by Randy Mackay
parent cabf21194a
commit 3faca88423
2 changed files with 38 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -518,7 +518,7 @@ void AC_PosControl::init_xy_controller(bool reset_I)
} }
/// update_xy_controller - run the horizontal position controller - should be called at 100hz or higher /// update_xy_controller - run the horizontal position controller - should be called at 100hz or higher
void AC_PosControl::update_xy_controller(bool use_desired_velocity, float ekfNavVelGainScaler) void AC_PosControl::update_xy_controller(xy_mode mode, float ekfNavVelGainScaler)
{ {
// compute dt // compute dt
uint32_t now = hal.scheduler->millis(); uint32_t now = hal.scheduler->millis();
@ -537,7 +537,7 @@ void AC_PosControl::update_xy_controller(bool use_desired_velocity, float ekfNav
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(use_desired_velocity, dt, ekfNavVelGainScaler); pos_to_rate_xy(mode, dt, ekfNavVelGainScaler);
// run position controller's velocity to acceleration step // run position controller's velocity to acceleration step
rate_to_accel_xy(dt, ekfNavVelGainScaler); rate_to_accel_xy(dt, ekfNavVelGainScaler);
@ -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(true, dt, ekfNavVelGainScaler); pos_to_rate_xy(XY_MODE_SLOW_POS_AND_VEL, dt, ekfNavVelGainScaler);
// run velocity to acceleration step // run velocity to acceleration step
rate_to_accel_xy(dt, ekfNavVelGainScaler); rate_to_accel_xy(dt, ekfNavVelGainScaler);
@ -653,7 +653,7 @@ void AC_PosControl::desired_vel_to_pos(float nav_dt)
/// when use_desired_rate is set to true: /// when use_desired_rate is set to true:
/// desired velocity (_vel_desired) is combined into final target velocity and /// desired velocity (_vel_desired) is combined into final target velocity and
/// velocity due to position error is reduce to a maximum of 1m/s /// velocity due to position error is reduce to a maximum of 1m/s
void AC_PosControl::pos_to_rate_xy(bool use_desired_rate, float dt, float ekfNavVelGainScaler) void AC_PosControl::pos_to_rate_xy(xy_mode mode, float dt, float ekfNavVelGainScaler)
{ {
Vector3f curr_pos = _inav.get_position(); Vector3f curr_pos = _inav.get_position();
float linear_distance; // the distance we swap between linear and sqrt velocity response float linear_distance; // the distance we swap between linear and sqrt velocity response
@ -692,28 +692,34 @@ void AC_PosControl::pos_to_rate_xy(bool use_desired_rate, float dt, float ekfNav
_vel_target.x = _p_pos_xy.kP() * _pos_error.x; _vel_target.x = _p_pos_xy.kP() * _pos_error.x;
_vel_target.y = _p_pos_xy.kP() * _pos_error.y; _vel_target.y = _p_pos_xy.kP() * _pos_error.y;
} }
// decide velocity limit due to position error if (mode == XY_MODE_SLOW_POS_AND_VEL) {
float vel_max_from_pos_error; // this mode is for loiter - rate-limiting the position correction
if (use_desired_rate) { // allows the pilot to always override the position correction in
// if desired velocity (i.e. velocity feed forward) is being used we limit the maximum velocity correction due to position error to 2m/s // the event of a disturbance
vel_max_from_pos_error = POSCONTROL_VEL_XY_MAX_FROM_POS_ERR;
}else{ // scale velocity within limit
// if desired velocity is not used, we allow position error to increase speed up to maximum speed float vel_total = pythagorous2(_vel_target.x, _vel_target.y);
vel_max_from_pos_error = _speed_cms; 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;
// scale velocity to stays within limits }
float vel_total = pythagorous2(_vel_target.x, _vel_target.y);
if (vel_total > vel_max_from_pos_error) { //add feedforward
_vel_target.x = vel_max_from_pos_error * _vel_target.x/vel_total;
_vel_target.y = vel_max_from_pos_error * _vel_target.y/vel_total;
}
// add desired velocity (i.e. feed forward).
if (use_desired_rate) {
_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 {
if (mode == XY_MODE_POS_AND_VEL) {
// add feedforward
_vel_target.x += _vel_desired.x;
_vel_target.y += _vel_desired.y;
}
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;
_vel_target.y = _speed_cms * _vel_target.y/vel_total;
}
} }
} }
} }

10
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -51,6 +51,12 @@ public:
const AP_Motors& motors, AC_AttitudeControl& attitude_control, 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_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);
enum xy_mode {
XY_MODE_POS_ONLY = 0,
XY_MODE_SLOW_POS_AND_VEL,
XY_MODE_POS_AND_VEL
};
/// ///
/// initialisation functions /// initialisation functions
@ -203,7 +209,7 @@ public:
/// update_xy_controller - run the horizontal position controller - should be called at 100hz or higher /// update_xy_controller - run the horizontal position controller - should be called at 100hz or higher
/// when use_desired_velocity is true the desired velocity (i.e. feed forward) is incorporated at the pos_to_rate step /// when use_desired_velocity is true the desired velocity (i.e. feed forward) is incorporated at the pos_to_rate step
void update_xy_controller(bool use_desired_velocity, float ekfNavVelGainScaler); void update_xy_controller(xy_mode mode, float ekfNavVelGainScaler);
/// set_target_to_stopping_point_xy - sets horizontal target to reasonable stopping position in cm from home /// set_target_to_stopping_point_xy - sets horizontal target to reasonable stopping position in cm from home
void set_target_to_stopping_point_xy(); void set_target_to_stopping_point_xy();
@ -307,7 +313,7 @@ private:
/// when use_desired_rate is set to true: /// when use_desired_rate is set to true:
/// desired velocity (_vel_desired) is combined into final target velocity and /// desired velocity (_vel_desired) is combined into final target velocity and
/// velocity due to position error is reduce to a maximum of 1m/s /// velocity due to position error is reduce to a maximum of 1m/s
void pos_to_rate_xy(bool use_desired_rate, float dt, float ekfNavVelGainScaler); void pos_to_rate_xy(xy_mode mode, float dt, float ekfNavVelGainScaler);
/// rate_to_accel_xy - horizontal desired rate to desired acceleration /// rate_to_accel_xy - horizontal desired rate to desired acceleration
/// converts desired velocities in lat/lon directions to accelerations in lat/lon frame /// converts desired velocities in lat/lon directions to accelerations in lat/lon frame