From 9e7709fa0908970a051b3e0659de805b8718698c Mon Sep 17 00:00:00 2001 From: priseborough Date: Sun, 16 Nov 2014 12:42:56 +1100 Subject: [PATCH] Copter: Expand EKF speed limit public method to handle control limits --- ArduCopter/ArduCopter.pde | 2 ++ ArduCopter/flight_mode.pde | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 417d252407..06682964c4 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -321,6 +321,8 @@ static AP_OpticalFlow_PX4 optflow(ahrs); #endif // gnd speed limit required to observe optical flow sensor limits static float ekfGndSpdLimit; +// scale factor applied to velocity controller gain to prevent optical flow noise causing excessive angle demand noise +static float ekfNavVelGainScaler; //////////////////////////////////////////////////////////////////////////////// // GCS selection diff --git a/ArduCopter/flight_mode.pde b/ArduCopter/flight_mode.pde index 206c5132e8..879b7f9899 100644 --- a/ArduCopter/flight_mode.pde +++ b/ArduCopter/flight_mode.pde @@ -121,7 +121,7 @@ static bool set_mode(uint8_t mode) static void update_flight_mode() { // Update EKF speed limit - used to limit speed when we are using optical flow - ekfGndSpdLimit = ahrs.getSpeedLimit(); + ahrs.getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler); switch (control_mode) { case ACRO: #if FRAME_CONFIG == HELI_FRAME