From d7abd296ef82cc3dad576eece76c56e726e6e8b0 Mon Sep 17 00:00:00 2001
From: Samuel Tabor <samuelctabor@gmail.com>
Date: Fri, 29 Mar 2019 14:10:11 +0000
Subject: [PATCH] AP_Soaring: Avoid calculations in lat/lng.

---
 libraries/AP_Soaring/AP_Soaring.cpp | 28 ++++++++++++++++++++--------
 libraries/AP_Soaring/AP_Soaring.h   |  4 ++--
 2 files changed, 22 insertions(+), 10 deletions(-)

diff --git a/libraries/AP_Soaring/AP_Soaring.cpp b/libraries/AP_Soaring/AP_Soaring.cpp
index 21fe3674c2..7f8a40529d 100644
--- a/libraries/AP_Soaring/AP_Soaring.cpp
+++ b/libraries/AP_Soaring/AP_Soaring.cpp
@@ -136,7 +136,7 @@ SoaringController::SoaringController(AP_AHRS &ahrs, AP_SpdHgtControl &spdHgt, co
 
 void SoaringController::get_target(Location &wp)
 {
-    wp = _prev_update_location;
+    _ahrs.get_position(wp);
     wp.offset(_ekf.X[2], _ekf.X[3]);
 }
 
@@ -219,6 +219,12 @@ void SoaringController::init_thermalling()
     const float init_p[4] = {INITIAL_STRENGTH_COVARIANCE, INITIAL_RADIUS_COVARIANCE, INITIAL_POSITION_COVARIANCE, INITIAL_POSITION_COVARIANCE};
     const MatrixN<float,4> p{init_p};
 
+    Vector3f position;
+    
+    if (!_ahrs.get_relative_position_NED_home(position)) {
+        return;
+    }
+
     // New state vector filter will be reset. Thermal location is placed in front of a/c
     const float init_xr[4] = {INITIAL_THERMAL_STRENGTH,
                               INITIAL_THERMAL_RADIUS,
@@ -229,7 +235,7 @@ void SoaringController::init_thermalling()
     // Also reset covariance matrix p so filter is not affected by previous data
     _ekf.reset(xr, p, q, r);
 
-    _ahrs.get_position(_prev_update_location);
+    _prev_update_location = position;
     _prev_update_time = AP_HAL::micros64();
     _thermal_start_time_us = AP_HAL::micros64();
 }
@@ -243,9 +249,9 @@ void SoaringController::init_cruising()
     }
 }
 
-void SoaringController::get_wind_corrected_drift(const Location *current_loc, const Vector3f *wind, float *wind_drift_x, float *wind_drift_y, float *dx, float *dy)
+void SoaringController::get_wind_corrected_drift(Vector3f current_loc, const Vector3f *wind, float *wind_drift_x, float *wind_drift_y, float *dx, float *dy)
 {
-    const Vector2f diff = _prev_update_location.get_distance_NE(*current_loc); // get distances from previous update
+    Vector3f diff = current_loc - _prev_update_location; // get distances from previous update
     *dx = diff.x;
     *dy = diff.y;
 
@@ -263,15 +269,21 @@ void SoaringController::get_altitude_wrt_home(float *alt)
 }
 void SoaringController::update_thermalling()
 {
-    struct Location current_loc;
-    _ahrs.get_position(current_loc);
+    Vector3f current_position;
+
+    if (!_ahrs.get_relative_position_NED_home(current_position)) {
+        return;
+    }
 
     float dx = 0;
     float dy = 0;
     float dx_w = 0;
     float dy_w = 0;
     Vector3f wind = _ahrs.wind_estimate();
-    get_wind_corrected_drift(&current_loc, &wind, &dx_w, &dy_w, &dx, &dy);
+    get_wind_corrected_drift(current_position, &wind, &dx_w, &dy_w, &dx, &dy);
+
+    struct Location current_loc;
+    _ahrs.get_position(current_loc);
 
     // write log - save the data.
     AP::logger().Write("SOAR", "TimeUS,nettorate,dx,dy,x0,x1,x2,x3,lat,lng,alt,dx_w,dy_w", "QfffffffLLfff", 
@@ -292,7 +304,7 @@ void SoaringController::update_thermalling()
     //log_data();
     _ekf.update(_vario.reading,dx, dy);       // update the filter
 
-    _prev_update_location = current_loc;      // save for next time
+    _prev_update_location = current_position;      // save for next time
     _prev_update_time = AP_HAL::micros64();
 }
 
diff --git a/libraries/AP_Soaring/AP_Soaring.h b/libraries/AP_Soaring/AP_Soaring.h
index 54758cf227..3b58109db7 100644
--- a/libraries/AP_Soaring/AP_Soaring.h
+++ b/libraries/AP_Soaring/AP_Soaring.h
@@ -32,7 +32,7 @@ class SoaringController {
     const AP_Vehicle::FixedWing &_aparm;
 
     // store aircraft location at last update
-    struct Location _prev_update_location;
+    Vector3f _prev_update_location;
 
     // store time thermal was entered for hysteresis
     unsigned long _thermal_start_time_us;
@@ -46,7 +46,7 @@ class SoaringController {
     bool _throttle_suppressed;
 
     float McCready(float alt);
-    void get_wind_corrected_drift(const Location *current_loc, const Vector3f *wind, float *wind_drift_x, float *wind_drift_y, float *dx, float *dy);
+    void get_wind_corrected_drift(Vector3f current_loc, const Vector3f *wind, float *wind_drift_x, float *wind_drift_y, float *dx, float *dy);
     void get_altitude_wrt_home(float *alt);
 
 protected: