From 47b5cf98ea87228dcd194926c512c02a635b681d Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Thu, 6 Sep 2018 08:37:00 +1000
Subject: [PATCH] Sub: make libraries get EKF control limits themselves

---
 ArduSub/Sub.h               | 6 ------
 ArduSub/control_auto.cpp    | 2 +-
 ArduSub/control_guided.cpp  | 4 ++--
 ArduSub/control_poshold.cpp | 2 +-
 ArduSub/flight_mode.cpp     | 3 ---
 5 files changed, 4 insertions(+), 13 deletions(-)

diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h
index 71ff0bfebe..cae9521e9b 100644
--- a/ArduSub/Sub.h
+++ b/ArduSub/Sub.h
@@ -207,12 +207,6 @@ private:
     OpticalFlow optflow{ahrs};
 #endif
 
-    // gnd speed limit required to observe optical flow sensor limits
-    float ekfGndSpdLimit;
-
-    // scale factor applied to velocity controller gain to prevent optical flow noise causing excessive angle demand noise
-    float ekfNavVelGainScaler;
-
     // system time in milliseconds of last recorded yaw reset from ekf
     uint32_t ekfYawReset_ms = 0;
 
diff --git a/ArduSub/control_auto.cpp b/ArduSub/control_auto.cpp
index abeffe06f4..f8f87565e4 100644
--- a/ArduSub/control_auto.cpp
+++ b/ArduSub/control_auto.cpp
@@ -734,7 +734,7 @@ void Sub::auto_terrain_recover_run()
     }
 
     // run loiter controller
-    loiter_nav.update(ekfGndSpdLimit, ekfNavVelGainScaler);
+    loiter_nav.update();
 
     ///////////////////////
     // update xy targets //
diff --git a/ArduSub/control_guided.cpp b/ArduSub/control_guided.cpp
index cc5fef0f95..6cef5e2c73 100644
--- a/ArduSub/control_guided.cpp
+++ b/ArduSub/control_guided.cpp
@@ -369,7 +369,7 @@ void Sub::guided_vel_control_run()
     }
 
     // call velocity controller which includes z axis controller
-    pos_control.update_vel_controller_xyz(ekfNavVelGainScaler);
+    pos_control.update_vel_controller_xyz();
 
     float lateral_out, forward_out;
     translate_pos_control_rp(lateral_out, forward_out);
@@ -440,7 +440,7 @@ void Sub::guided_posvel_control_run()
     pos_control.set_desired_velocity_xy(posvel_vel_target_cms.x, posvel_vel_target_cms.y);
 
     // run position controller
-    pos_control.update_xy_controller(ekfNavVelGainScaler);
+    pos_control.update_xy_controller();
 
     float lateral_out, forward_out;
     translate_pos_control_rp(lateral_out, forward_out);
diff --git a/ArduSub/control_poshold.cpp b/ArduSub/control_poshold.cpp
index 738dc2a4c5..09222556d5 100644
--- a/ArduSub/control_poshold.cpp
+++ b/ArduSub/control_poshold.cpp
@@ -52,7 +52,7 @@ void Sub::poshold_run()
     motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
 
     // run loiter controller
-    loiter_nav.update(ekfGndSpdLimit, ekfNavVelGainScaler);
+    loiter_nav.update();
 
     ///////////////////////
     // update xy outputs //
diff --git a/ArduSub/flight_mode.cpp b/ArduSub/flight_mode.cpp
index 89db87974b..f7e2ef8f62 100644
--- a/ArduSub/flight_mode.cpp
+++ b/ArduSub/flight_mode.cpp
@@ -98,9 +98,6 @@ bool Sub::set_mode(control_mode_t mode, mode_reason_t reason)
 // called at 100hz or more
 void Sub::update_flight_mode()
 {
-    // Update EKF speed limit - used to limit speed when we are using optical flow
-    ahrs.getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);
-
     switch (control_mode) {
     case ACRO:
         acro_run();