diff --git a/libraries/AP_Soaring/AP_Soaring.cpp b/libraries/AP_Soaring/AP_Soaring.cpp index aa8e0a5854..77d7735cf3 100644 --- a/libraries/AP_Soaring/AP_Soaring.cpp +++ b/libraries/AP_Soaring/AP_Soaring.cpp @@ -253,7 +253,7 @@ void SoaringController::init_thermalling() } // New state vector filter will be reset. Thermal location is placed in front of a/c - const float init_xr[4] = {INITIAL_THERMAL_STRENGTH, + const float init_xr[4] = {_vario.get_trigger_value(), INITIAL_THERMAL_RADIUS, position.x + thermal_distance_ahead * cosf(_ahrs.yaw), position.y + thermal_distance_ahead * sinf(_ahrs.yaw)}; diff --git a/libraries/AP_Soaring/AP_Soaring.h b/libraries/AP_Soaring/AP_Soaring.h index 9751c876c3..9a772f6fd6 100644 --- a/libraries/AP_Soaring/AP_Soaring.h +++ b/libraries/AP_Soaring/AP_Soaring.h @@ -22,7 +22,6 @@ #if HAL_SOARING_ENABLED -#define INITIAL_THERMAL_STRENGTH 2.0 #define INITIAL_THERMAL_RADIUS 80.0 #define INITIAL_STRENGTH_COVARIANCE 0.0049 #define INITIAL_RADIUS_COVARIANCE 400.0