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

AC_WPNav: allow position control to get ekf navigation scalars directly

Browse Source
This commit is contained in:
Peter Barker 2018-09-05 19:44:08 +10:00 committed by Andrew Tridgell
parent dabe8a13f7
commit 3faf7824c0
5 changed files with 15 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AC_WPNav/AC_Circle.cpp
View File

@ -159,7 +159,7 @@ void AC_Circle::update()
} }
// update position controller // update position controller
_pos_control.update_xy_controller(1.0f); _pos_control.update_xy_controller();
} }
// get_closest_point_on_circle - returns closest point on the circle // get_closest_point_on_circle - returns closest point on the circle

11
libraries/AC_WPNav/AC_Loiter.cpp
View File

@ -203,7 +203,7 @@ float AC_Loiter::get_angle_max_cd() const
} }
/// run the loiter controller /// run the loiter controller
void AC_Loiter::update(float ekfGndSpdLimit, float ekfNavVelGainScaler) void AC_Loiter::update()
{ {
// calculate dt // calculate dt
float dt = _pos_control.time_since_last_xy_update(); float dt = _pos_control.time_since_last_xy_update();
@ -215,8 +215,8 @@ void AC_Loiter::update(float ekfGndSpdLimit, float ekfNavVelGainScaler)
_pos_control.set_max_speed_xy(_speed_cms); _pos_control.set_max_speed_xy(_speed_cms);
_pos_control.set_max_accel_xy(_accel_cmss); _pos_control.set_max_accel_xy(_accel_cmss);
calc_desired_velocity(dt,ekfGndSpdLimit); calc_desired_velocity(dt);
_pos_control.update_xy_controller(ekfNavVelGainScaler); _pos_control.update_xy_controller();
} }
// sanity check parameters // sanity check parameters
@ -228,8 +228,11 @@ void AC_Loiter::sanity_check_params()
/// calc_desired_velocity - updates desired velocity (i.e. feed forward) with pilot requested acceleration and fake wind resistance /// calc_desired_velocity - updates desired velocity (i.e. feed forward) with pilot requested acceleration and fake wind resistance
/// updated velocity sent directly to position controller /// updated velocity sent directly to position controller
void AC_Loiter::calc_desired_velocity(float nav_dt, float ekfGndSpdLimit) void AC_Loiter::calc_desired_velocity(float nav_dt)
{ {
float ekfGndSpdLimit, ekfNavVelGainScaler;
AP::ahrs_navekf().getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);
// calculate a loiter speed limit which is the minimum of the value set by the LOITER_SPEED // calculate a loiter speed limit which is the minimum of the value set by the LOITER_SPEED
// parameter and the value set by the EKF to observe optical flow limits // parameter and the value set by the EKF to observe optical flow limits
float gnd_speed_limit_cms = MIN(_speed_cms, ekfGndSpdLimit*100.0f); float gnd_speed_limit_cms = MIN(_speed_cms, ekfGndSpdLimit*100.0f);

4
libraries/AC_WPNav/AC_Loiter.h
View File

@ -51,7 +51,7 @@ public:
float get_angle_max_cd() const; float get_angle_max_cd() const;
/// run the loiter controller /// run the loiter controller
void update(float ekfGndSpdLimit, float ekfNavVelGainScaler); void update();
/// get desired roll, pitch which should be fed into stabilize controllers /// get desired roll, pitch which should be fed into stabilize controllers
int32_t get_roll() const { return _pos_control.get_roll(); } int32_t get_roll() const { return _pos_control.get_roll(); }
@ -66,7 +66,7 @@ protected:
/// updates desired velocity (i.e. feed forward) with pilot requested acceleration and fake wind resistance /// updates desired velocity (i.e. feed forward) with pilot requested acceleration and fake wind resistance
/// updated velocity sent directly to position controller /// updated velocity sent directly to position controller
void calc_desired_velocity(float nav_dt, float ekfGndSpdLimit); void calc_desired_velocity(float nav_dt);
// references and pointers to external libraries // references and pointers to external libraries
const AP_InertialNav& _inav; const AP_InertialNav& _inav;

8
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -118,11 +118,11 @@ void AC_WPNav::init_brake_target(float accel_cmss)
} }
// update_brake - run the stop controller - gets called at 400hz // update_brake - run the stop controller - gets called at 400hz
void AC_WPNav::update_brake(float ekfGndSpdLimit, float ekfNavVelGainScaler) void AC_WPNav::update_brake()
{ {
// send adjusted feed forward velocity back to position controller // send adjusted feed forward velocity back to position controller
_pos_control.set_desired_velocity_xy(0.0f, 0.0f); _pos_control.set_desired_velocity_xy(0.0f, 0.0f);
_pos_control.update_xy_controller(ekfNavVelGainScaler); _pos_control.update_xy_controller();
} }
/// ///
@ -522,7 +522,7 @@ bool AC_WPNav::update_wpnav()
_pos_control.freeze_ff_z(); _pos_control.freeze_ff_z();
} }
_pos_control.update_xy_controller(1.0f); _pos_control.update_xy_controller();
check_wp_leash_length(); check_wp_leash_length();
_wp_last_update = AP_HAL::millis(); _wp_last_update = AP_HAL::millis();
@ -814,7 +814,7 @@ bool AC_WPNav::update_spline()
} }
// run horizontal position controller // run horizontal position controller
_pos_control.update_xy_controller(1.0f); _pos_control.update_xy_controller();
_wp_last_update = AP_HAL::millis(); _wp_last_update = AP_HAL::millis();

2
libraries/AC_WPNav/AC_WPNav.h
View File

@ -64,7 +64,7 @@ public:
void init_brake_target(float accel_cmss); void init_brake_target(float accel_cmss);
/// ///
/// update_brake - run the brake controller - should be called at 400hz /// update_brake - run the brake controller - should be called at 400hz
void update_brake(float ekfGndSpdLimit, float ekfNavVelGainScaler); void update_brake();
/// ///
/// waypoint controller /// waypoint controller