From 2b40182b344c1fa6d1dabe04e933f50d6ea7247c Mon Sep 17 00:00:00 2001 From: Samuel Tabor Date: Sun, 23 Jun 2019 07:32:53 +0100 Subject: [PATCH] AP_Soaring: Reduce drift feed-forward by ratio of climb rate to thermal core strength. This is consistent with assumption that each packet of air, rising at core strength, is convected with the ambient wind speed. --- libraries/AP_Soaring/AP_Soaring.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Soaring/AP_Soaring.cpp b/libraries/AP_Soaring/AP_Soaring.cpp index ee355e6dcc..2c0d9a202e 100644 --- a/libraries/AP_Soaring/AP_Soaring.cpp +++ b/libraries/AP_Soaring/AP_Soaring.cpp @@ -295,7 +295,7 @@ void SoaringController::update_thermalling() return; } - Vector3f wind_drift = _ahrs.wind_estimate()*deltaT; + Vector3f wind_drift = _ahrs.wind_estimate()*deltaT*_vario.smoothed_climb_rate/_ekf.X[0]; // update the filter _ekf.update(_vario.reading, current_position.x, current_position.y, wind_drift.x, wind_drift.y);