Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_PosControl: combine xy position control into single method

Browse Source 
Also always limit lean angle to maintain altitude
This commit is contained in:
Leonard Hall 2017-06-27 22:07:14 +09:00 committed by Randy Mackay
parent 74bb7616a7
commit 993e638752
2 changed files with 20 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -800,7 +800,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
void AC_PosControl::update_xy_controller(float ekfNavVelGainScaler, bool use_althold_lean_angle)
void AC_PosControl::update_xy_controller(float ekfNavVelGainScaler)
{
// compute dt
uint32_t now = AP_HAL::millis();
@ -821,14 +821,8 @@ void AC_PosControl::update_xy_controller(float ekfNavVelGainScaler, bool use_alt
// translate any adjustments from pilot to loiter target
desired_vel_to_pos(dt);
// run position controller's position error to desired velocity step
pos_to_rate_xy(dt, ekfNavVelGainScaler);
// run position controller's velocity to acceleration step
rate_to_accel_xy(dt, ekfNavVelGainScaler);
// run position controller's acceleration to lean angle step
accel_to_lean_angles(dt, ekfNavVelGainScaler, use_althold_lean_angle);
// run horizontal position controller
run_xy_controller(dt, ekfNavVelGainScaler);
}
float AC_PosControl::time_since_last_xy_update() const
@ -893,14 +887,8 @@ void AC_PosControl::update_vel_controller_xy(float ekfNavVelGainScaler)
// TODO: this will need to be removed and added to the calling function.
desired_vel_to_pos(dt);
// run position controller's position error to desired velocity step
pos_to_rate_xy(dt, ekfNavVelGainScaler);
// run velocity to acceleration step
rate_to_accel_xy(dt, ekfNavVelGainScaler);
// run acceleration to lean angle step
accel_to_lean_angles(dt, ekfNavVelGainScaler, false);
// run position controller
run_xy_controller(dt, ekfNavVelGainScaler);
// update xy update time
_last_update_xy_ms = now;
@ -976,12 +964,12 @@ void AC_PosControl::desired_vel_to_pos(float nav_dt)
}
}
/// pos_to_rate_xy - horizontal position error to velocity controller
/// run horizontal position controller correcting position and velocity
/// converts position (_pos_target) to target velocity (_vel_target)
/// when use_desired_rate is set to true:
/// desired velocity (_vel_desired) is combined into final target velocity and
/// velocity due to position error is reduce to a maximum of 1m/s
void AC_PosControl::pos_to_rate_xy(float dt, float ekfNavVelGainScaler)
/// desired velocity (_vel_desired) is combined into final target velocity
/// converts desired velocities in lat/lon directions to accelerations in lat/lon frame
/// converts desired accelerations provided in lat/lon frame to roll/pitch angles
void AC_PosControl::run_xy_controller(float dt, float ekfNavVelGainScaler)
{
Vector3f curr_pos = _inav.get_position();
float kP = ekfNavVelGainScaler * _p_pos_xy.kP(); // scale gains to compensate for noisy optical flow measurement in the EKF
@ -1011,12 +999,9 @@ void AC_PosControl::pos_to_rate_xy(float dt, float ekfNavVelGainScaler)
// add velocity feed-forward
_vel_target.x += _vel_desired.x;
_vel_target.y += _vel_desired.y;
}
/// rate_to_accel_xy - horizontal desired rate to desired acceleration
/// converts desired velocities in lat/lon directions to accelerations in lat/lon frame
void AC_PosControl::rate_to_accel_xy(float dt, float ekfNavVelGainScaler)
{
// this section converts desired velocities in lat/lon directions to accelerations in lat/lon frame
Vector2f accel_target, vel_xy_p, vel_xy_i, vel_xy_d;
// check if vehicle velocity is being overridden
@ -1071,12 +1056,9 @@ void AC_PosControl::rate_to_accel_xy(float dt, float ekfNavVelGainScaler)
// Add feed forward into the target acceleration output
_accel_target.x += _accel_desired.x;
_accel_target.y += _accel_desired.y;
}
/// accel_to_lean_angles - horizontal desired acceleration to lean angles
/// converts desired accelerations provided in lat/lon frame to roll/pitch angles
void AC_PosControl::accel_to_lean_angles(float dt, float ekfNavVelGainScaler, bool use_althold_lean_angle)
{
// This section converts desired accelerations provided in lat/lon frame to roll/pitch angles
float accel_right, accel_forward;
// limit acceleration using maximum lean angles

20
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -228,7 +228,7 @@ public:
/// 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
void update_xy_controller(float ekfNavVelGainScaler, bool use_althold_lean_angle);
void update_xy_controller(float ekfNavVelGainScaler);
/// set_target_to_stopping_point_xy - sets horizontal target to reasonable stopping position in cm from home
void set_target_to_stopping_point_xy();
@ -340,20 +340,12 @@ protected:
/// desired_vel_to_pos - move position target using desired velocities
void desired_vel_to_pos(float nav_dt);
/// pos_to_rate_xy - horizontal position error to velocity controller
/// run horizontal position controller correcting position and velocity
/// converts position (_pos_target) to target velocity (_vel_target)
/// when use_desired_rate is set to true:
/// desired velocity (_vel_desired) is combined into final target velocity and
/// velocity due to position error is reduce to a maximum of 1m/s
void pos_to_rate_xy(float dt, float ekfNavVelGainScaler);
/// rate_to_accel_xy - horizontal desired rate to desired acceleration
/// converts desired velocities in lat/lon directions to accelerations in lat/lon frame
void rate_to_accel_xy(float dt, float ekfNavVelGainScaler);
/// accel_to_lean_angles - horizontal desired acceleration to lean angles
/// converts desired accelerations provided in lat/lon frame to roll/pitch angles
void accel_to_lean_angles(float dt_xy, float ekfNavVelGainScaler, bool use_althold_lean_angle);
/// desired velocity (_vel_desired) is combined into final target velocity
/// converts desired velocities in lat/lon directions to accelerations in lat/lon frame
/// converts desired accelerations provided in lat/lon frame to roll/pitch angles
void run_xy_controller(float dt, float ekfNavVelGainScaler);
/// calc_leash_length - calculates the horizontal leash length given a maximum speed, acceleration and position kP gain
float calc_leash_length(float speed_cms, float accel_cms, float kP) const;