From 5fa0c593104621b9658aa05c1a22aeea2780ca2c Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
Date: Sun, 16 Nov 2014 11:50:35 +1100
Subject: [PATCH] AC_WPNav: Add EKF ground speed limit to loiter speed control

This is required to prevent the speed controller saturating the optical flow sensor during low altitude flying.
---
 libraries/AC_WPNav/AC_WPNav.cpp | 16 +++++++++++++---
 libraries/AC_WPNav/AC_WPNav.h   |  4 ++--
 2 files changed, 15 insertions(+), 5 deletions(-)

diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp
index f76079da1a..1a7419b6d5 100644
--- a/libraries/AC_WPNav/AC_WPNav.cpp
+++ b/libraries/AC_WPNav/AC_WPNav.cpp
@@ -191,6 +191,7 @@ void AC_WPNav::loiter_soften_for_landing()
 void AC_WPNav::set_loiter_velocity(float velocity_cms)
 {
     // range check velocity and update position controller
+    //float maxSpd_cms = _ahrs.getSpeedlimit();
     if (velocity_cms >= WPNAV_LOITER_SPEED_MIN) {
         _loiter_speed_cms = velocity_cms;
 
@@ -219,7 +220,7 @@ void AC_WPNav::get_loiter_stopping_point_xy(Vector3f& stopping_point) const
 
 /// calc_loiter_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_WPNav::calc_loiter_desired_velocity(float nav_dt)
+void AC_WPNav::calc_loiter_desired_velocity(float nav_dt, float ekfGndSpdLimit)
 {
     // range check nav_dt
     if( nav_dt < 0 ) {
@@ -276,6 +277,15 @@ void AC_WPNav::calc_loiter_desired_velocity(float nav_dt)
         }
     }
 
+    // limit EKF speed limit and convert to cm/s
+    ekfGndSpdLimit = 100.0f * max(ekfGndSpdLimit,0.1f);
+    // Apply EKF limit to desired velocity -  this limit is calculated by the EKF and adjusted as required to ensure certain sensor limits are respected (eg optical flow sensing)
+    float horizSpdDem = sqrtf(sq(desired_vel.x) + sq(desired_vel.y));
+    if (horizSpdDem > ekfGndSpdLimit) {
+        desired_vel.x = desired_vel.x * ekfGndSpdLimit / horizSpdDem;
+        desired_vel.y = desired_vel.y * ekfGndSpdLimit / horizSpdDem;
+    }
+
     // send adjusted feed forward velocity back to position controller
     _pos_control.set_desired_velocity_xy(desired_vel.x,desired_vel.y);
 }
@@ -287,7 +297,7 @@ int32_t AC_WPNav::get_loiter_bearing_to_target() const
 }
 
 /// update_loiter - run the loiter controller - should be called at 100hz
-void AC_WPNav::update_loiter()
+void AC_WPNav::update_loiter(float ekfGndSpdLimit)
 {
     // calculate dt
     uint32_t now = hal.scheduler->millis();
@@ -302,7 +312,7 @@ void AC_WPNav::update_loiter()
         // capture time since last iteration
         _loiter_last_update = now;
         // translate any adjustments from pilot to loiter target
-        calc_loiter_desired_velocity(dt);
+        calc_loiter_desired_velocity(dt,ekfGndSpdLimit);
         // trigger position controller on next update
         _pos_control.trigger_xy();
     }else{
diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h
index af1c27f6dc..77474c7092 100644
--- a/libraries/AC_WPNav/AC_WPNav.h
+++ b/libraries/AC_WPNav/AC_WPNav.h
@@ -95,7 +95,7 @@ public:
     int32_t get_loiter_bearing_to_target() const;
 
     /// update_loiter - run the loiter controller - should be called at 10hz
-    void update_loiter();
+    void update_loiter(float ekfGndSpdLimit);
 
     ///
     /// waypoint controller
@@ -245,7 +245,7 @@ protected:
 
     /// calc_loiter_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 calc_loiter_desired_velocity(float nav_dt);
+    void calc_loiter_desired_velocity(float nav_dt, float ekfGndSpdLimit);
 
     /// get_bearing_cd - return bearing in centi-degrees between two positions
     float get_bearing_cd(const Vector3f &origin, const Vector3f &destination) const;