AC_WPNav: allow position control to get ekf navigation scalars directly

libraries/AC_WPNav/AC_Circle.cpp

@@ -159,7 +159,7 @@ void AC_Circle::update()
     }
 
     // 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

libraries/AC_WPNav/AC_Loiter.cpp

@@ -203,7 +203,7 @@ float AC_Loiter::get_angle_max_cd() const
 }
 
 /// run the loiter controller
-void AC_Loiter::update(float ekfGndSpdLimit, float ekfNavVelGainScaler)
+void AC_Loiter::update()
 {
     // calculate dt
     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_accel_xy(_accel_cmss);
 
-    calc_desired_velocity(dt,ekfGndSpdLimit);
-    _pos_control.update_xy_controller(ekfNavVelGainScaler);
+    calc_desired_velocity(dt);
+    _pos_control.update_xy_controller();
 }
 
 // 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
 ///		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
     // parameter and the value set by the EKF to observe optical flow limits
     float gnd_speed_limit_cms = MIN(_speed_cms, ekfGndSpdLimit*100.0f);

libraries/AC_WPNav/AC_Loiter.h

@@ -51,7 +51,7 @@ public:
     float get_angle_max_cd() const;
 
     /// run the loiter controller
-    void update(float ekfGndSpdLimit, float ekfNavVelGainScaler);
+    void update();
 
     /// get desired roll, pitch which should be fed into stabilize controllers
     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
     ///		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
     const AP_InertialNav&   _inav;

libraries/AC_WPNav/AC_WPNav.cpp

@@ -118,11 +118,11 @@ void AC_WPNav::init_brake_target(float accel_cmss)
 }
 
 // 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
     _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.update_xy_controller(1.0f);
+    _pos_control.update_xy_controller();
     check_wp_leash_length();
 
     _wp_last_update = AP_HAL::millis();
@@ -814,7 +814,7 @@ bool AC_WPNav::update_spline()
     }
 
     // run horizontal position controller
-    _pos_control.update_xy_controller(1.0f);
+    _pos_control.update_xy_controller();
 
     _wp_last_update = AP_HAL::millis();
 

libraries/AC_WPNav/AC_WPNav.h

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