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

AC_AttitudeControl: use EKF singleton to get ekfNavVelGainScaler

Browse Source
This commit is contained in:
Peter Barker 2018-09-05 19:42:32 +10:00 committed by Andrew Tridgell
parent 9b82bcc901
commit dabe8a13f7
2 changed files with 14 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -791,7 +791,7 @@ void AC_PosControl::init_xy_controller()
}
/// update_xy_controller - run the horizontal position controller - should be called at 100hz or higher
void AC_PosControl::update_xy_controller(float ekfNavVelGainScaler)
void AC_PosControl::update_xy_controller()
{
// compute dt
uint32_t now = AP_HAL::millis();
@ -812,7 +812,7 @@ void AC_PosControl::update_xy_controller(float ekfNavVelGainScaler)
desired_vel_to_pos(dt);
// run horizontal position controller
run_xy_controller(dt, ekfNavVelGainScaler);
run_xy_controller(dt);
// update xy update time
_last_update_xy_ms = now;
@ -888,7 +888,7 @@ void AC_PosControl::init_vel_controller_xyz()
/// velocity targets should we set using set_desired_velocity_xy() method
/// callers should use get_roll() and get_pitch() methods and sent to the attitude controller
/// throttle targets will be sent directly to the motors
void AC_PosControl::update_vel_controller_xy(float ekfNavVelGainScaler)
void AC_PosControl::update_vel_controller_xy()
{
// capture time since last iteration
uint32_t now = AP_HAL::millis();
@ -910,7 +910,7 @@ void AC_PosControl::update_vel_controller_xy(float ekfNavVelGainScaler)
desired_vel_to_pos(dt);
// run position controller
run_xy_controller(dt, ekfNavVelGainScaler);
run_xy_controller(dt);
// update xy update time
_last_update_xy_ms = now;
@ -921,9 +921,9 @@ void AC_PosControl::update_vel_controller_xy(float ekfNavVelGainScaler)
/// velocity targets should we set using set_desired_velocity_xyz() method
/// callers should use get_roll() and get_pitch() methods and sent to the attitude controller
/// throttle targets will be sent directly to the motors
void AC_PosControl::update_vel_controller_xyz(float ekfNavVelGainScaler)
void AC_PosControl::update_vel_controller_xyz()
{
update_vel_controller_xy(ekfNavVelGainScaler);
update_vel_controller_xy();
// update altitude target
set_alt_target_from_climb_rate_ff(_vel_desired.z, _dt, false);
@ -991,8 +991,11 @@ void AC_PosControl::desired_vel_to_pos(float nav_dt)
/// 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)
void AC_PosControl::run_xy_controller(float dt)
{
float ekfGndSpdLimit, ekfNavVelGainScaler;
AP::ahrs_navekf().getEkfControlLimits(ekfGndSpdLimit, 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

8
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -230,7 +230,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);
void update_xy_controller();
/// set_target_to_stopping_point_xy - sets horizontal target to reasonable stopping position in cm from home
void set_target_to_stopping_point_xy();
@ -257,13 +257,13 @@ public:
/// velocity targets should we set using set_desired_velocity_xy() method
/// callers should use get_roll() and get_pitch() methods and sent to the attitude controller
/// throttle targets will be sent directly to the motors
void update_vel_controller_xy(float ekfNavVelGainScaler);
void update_vel_controller_xy();
/// update_velocity_controller_xyz - run the velocity controller - should be called at 100hz or higher
/// velocity targets should we set using set_desired_velocity_xyz() method
/// callers should use get_roll() and get_pitch() methods and sent to the attitude controller
/// throttle targets will be sent directly to the motors
void update_vel_controller_xyz(float ekfNavVelGainScaler);
void update_vel_controller_xyz();
/// get desired roll, pitch which should be fed into stabilize controllers
float get_roll() const { return _roll_target; }
@ -347,7 +347,7 @@ protected:
/// 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);
void run_xy_controller(float dt);
/// 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;