From 719aa4bc530f84f0ad4f4f3f93addba04ba00141 Mon Sep 17 00:00:00 2001 From: Samuel Tabor Date: Sun, 28 Feb 2021 12:54:34 +0000 Subject: [PATCH] AP_Soaring: Add speed-to-fly calculation used if SOAR_CRSE_ARSPD<0. --- libraries/AP_Soaring/AP_Soaring.cpp | 33 +++++++++++++++-- libraries/AP_Soaring/AP_Soaring.h | 3 ++ libraries/AP_Soaring/SpeedToFly.cpp | 57 +++++++++++++++++++++++++++++ libraries/AP_Soaring/SpeedToFly.h | 22 +++++++++++ libraries/AP_Soaring/Variometer.cpp | 7 +++- libraries/AP_Soaring/Variometer.h | 5 +++ 6 files changed, 122 insertions(+), 5 deletions(-) create mode 100644 libraries/AP_Soaring/SpeedToFly.cpp create mode 100644 libraries/AP_Soaring/SpeedToFly.h diff --git a/libraries/AP_Soaring/AP_Soaring.cpp b/libraries/AP_Soaring/AP_Soaring.cpp index 5a967728cc..2043861e59 100644 --- a/libraries/AP_Soaring/AP_Soaring.cpp +++ b/libraries/AP_Soaring/AP_Soaring.cpp @@ -150,7 +150,7 @@ const AP_Param::GroupInfo SoaringController::var_info[] = { // @Param: CRSE_ARSPD // @DisplayName: Specific setting for airspeed when cruising. - // @Description: If non-zero this airspeed will be used when cruising. + // @Description: If non-zero this airspeed will be used when cruising. If set to -1, airspeed will be selected based on speed-to-fly theory. // @Range: 5 50 // @User: Advanced AP_GROUPINFO("CRSE_ARSPD", 21, SoaringController, soar_cruise_airspeed, 0), @@ -168,6 +168,7 @@ const AP_Param::GroupInfo SoaringController::var_info[] = { SoaringController::SoaringController(AP_TECS &tecs, const AP_Vehicle::FixedWing &parms) : _tecs(tecs), _vario(parms,_polarParams), + _speedToFly(_polarParams), _aparm(parms), _throttle_suppressed(true) { @@ -369,8 +370,31 @@ void SoaringController::update_thermalling() void SoaringController::update_cruising() { - // Reserved for future tasks that need to run continuously while in FBWB or AUTO mode, - // for example, calculation of optimal airspeed and flap angle. + // Calculate the optimal airspeed for the current conditions of wind along current direction, + // expected lift in next thermal and filtered sink rate. + + Vector3f wind = AP::ahrs().wind_estimate(); + Vector3f wind_bf = AP::ahrs().earth_to_body(wind); + + const float wx = wind_bf.x; + + const float wz = _vario.get_stf_value(); + + // Constraints on the airspeed calculation. + const float CLmin = _polarParams.K/(_aparm.airspeed_max*_aparm.airspeed_max); + const float CLmax = _polarParams.K/(_aparm.airspeed_min*_aparm.airspeed_min); + + // Update the calculation. + _speedToFly.update(wx, wz, thermal_vspeed, CLmin, CLmax); + + AP::logger().WriteStreaming("SORC", "TimeUS,wx,wz,wexp,CLmin,CLmax,Vopt", "Qffffff", + AP_HAL::micros64(), + (double)wx, + (double)wz, + (double)thermal_vspeed, + (double)CLmin, + (double)CLmax, + (double)_speedToFly.speed_to_fly()); } void SoaringController::update_vario() @@ -499,6 +523,9 @@ float SoaringController::get_thermalling_target_airspeed() float SoaringController::get_cruising_target_airspeed() { + if (soar_cruise_airspeed<0) { + return _speedToFly.speed_to_fly(); + } return soar_cruise_airspeed; } diff --git a/libraries/AP_Soaring/AP_Soaring.h b/libraries/AP_Soaring/AP_Soaring.h index 04f2796430..71d1e73bff 100644 --- a/libraries/AP_Soaring/AP_Soaring.h +++ b/libraries/AP_Soaring/AP_Soaring.h @@ -15,6 +15,7 @@ #include "ExtendedKalmanFilter.h" #include "Variometer.h" #include +#include "SpeedToFly.h" #ifndef HAL_SOARING_ENABLED #define HAL_SOARING_ENABLED !HAL_MINIMIZE_FEATURES @@ -32,6 +33,8 @@ class SoaringController { ExtendedKalmanFilter _ekf{}; AP_TECS &_tecs; Variometer _vario; + SpeedToFly _speedToFly; + const AP_Vehicle::FixedWing &_aparm; // store aircraft location at last update diff --git a/libraries/AP_Soaring/SpeedToFly.cpp b/libraries/AP_Soaring/SpeedToFly.cpp new file mode 100644 index 0000000000..e8073fc5d3 --- /dev/null +++ b/libraries/AP_Soaring/SpeedToFly.cpp @@ -0,0 +1,57 @@ +/* SpeedToFly class by Samuel Tabor, 2021. + +Calculates the optimal speed to fly given drag polar, expected climb rate in next thermal and +horizontal and vertical air movement between thermals. +*/ +#include "SpeedToFly.h" + +void SpeedToFly::update(float Wx, float Wz, float Wexp, float CLmin, float CLmax) +{ + // The solution to the speed-to-fly problem does not have a closed form solution. A Newton + // method with some additional checks will converge to an acceptable level within 3-4 iterations. + // However, to keep the computation constant per function call we just do a single iteration using + // the previous approximation as a starting point. + // This gives good accuracy as the inputs don't change rapidly. It would also be possible to store + // the inputs and converge the solution over 3-4 function calls, but this real-time iteration + // approach gives better accuracy in tests as well as simpler code. + + Wz *= -1.0f; // Sink defined positive. + + float sqrtfk = sqrtf(_polarParams.K); + float minSink = (sqrtfk/sqrtf(CLmax)*(_polarParams.CD0 + _polarParams.B*CLmax*CLmax))/CLmax; + + if (!is_positive(minSink+Wz+Wexp)) { + // Special case. If lift is greater than min sink speed, fly at min sink + // speed. + _CL_estimate = CLmax; + return; + } + + float CD0 = _polarParams.CD0; + float B = _polarParams.B; + float Wxp = Wx/sqrtfk; + float WZ = (Wz + Wexp)/sqrtfk; + + // Guess starting location. + float CL = _CL_estimate>0 ? _CL_estimate : 0.5f*(CLmax+CLmin); + + float t0 = powf(CL,1.5f); + float t1 = CD0 + B*CL*CL + t0*WZ; + float t2 = 1.5f*sqrtf(CL)*WZ + 2.0f*B*CL; + + float Jd = (1.5f*sqrtf(CL)*Wxp + 1.0f)/t1 - (t2*(CL + t0*Wxp))/(t1*t1); + + float Jdd = 2.0f*t2*t2*(CL + t0*Wxp)/powf(t1,3) - (2.0f*t2*(1.5f*sqrtf(CL)*Wxp + 1.0f))/(t1*t1) - ((2.0f*B + 0.75f*WZ/sqrtf(CL))*(CL + t0*Wxp))/(t1*t1) + 0.75f*Wxp/(sqrtf(CL)*t1); + + // Check we're heading to a maximum, not a minimum!! + if (is_positive(Jdd)) { + // Alternate mode, go uphill. + CL = CL + 0.1 * (Jd>0.0f ? 1.0f : -1.0f); + } else { + // Newton should work. + CL = CL - Jd/Jdd; + } + + _CL_estimate = CL; + _CL_estimate = constrain_float(_CL_estimate, CLmin, CLmax); +} diff --git a/libraries/AP_Soaring/SpeedToFly.h b/libraries/AP_Soaring/SpeedToFly.h new file mode 100644 index 0000000000..526653a043 --- /dev/null +++ b/libraries/AP_Soaring/SpeedToFly.h @@ -0,0 +1,22 @@ +/* SpeedToFly class by Samuel Tabor, 2021. + +Calculates the optimal speed to fly given drag polar, expected climb rate in next thermal and +horizontal and vertical air movement between thermals. +*/ +#pragma once + +#include "Variometer.h" + +class SpeedToFly { + + float _CL_estimate = -1.0f; + + Variometer::PolarParams &_polarParams; + +public: + SpeedToFly(Variometer::PolarParams &polarParams) :_polarParams(polarParams) {} + + void update(float Wx, float Wz, float Wexp, float CLmin, float CLmax); + + float speed_to_fly(void) {return _CL_estimate>0 ? sqrtf(_polarParams.K/_CL_estimate) : -1.0f;}; +}; diff --git a/libraries/AP_Soaring/Variometer.cpp b/libraries/AP_Soaring/Variometer.cpp index 63847aec88..28c19fc01f 100644 --- a/libraries/AP_Soaring/Variometer.cpp +++ b/libraries/AP_Soaring/Variometer.cpp @@ -67,10 +67,13 @@ void Variometer::update(const float thermal_bank) reading = raw_climb_rate + dsp_cor*_aspd_filt_constrained/GRAVITY_MSS + sinkrate; + // Update filters. - float filtered_reading = _trigger_filter.apply(reading, dt); // Apply low pass timeconst filter for noise + float filtered_reading = _trigger_filter.apply(reading, dt); - _audio_filter.apply(reading, dt); // Apply low pass timeconst filter for noise + _audio_filter.apply(reading, dt); + + _stf_filter.apply(reading, dt); _prev_update_time = AP_HAL::micros64(); diff --git a/libraries/AP_Soaring/Variometer.h b/libraries/AP_Soaring/Variometer.h index 20f08313d3..3f0ae2653b 100644 --- a/libraries/AP_Soaring/Variometer.h +++ b/libraries/AP_Soaring/Variometer.h @@ -43,6 +43,9 @@ class Variometer { // Longitudinal acceleration bias filter. LowPassFilter _vdotbias_filter{1/60.0}; + // Speed to fly vario filter. + LowPassFilter _stf_filter{1/20.0}; + public: struct PolarParams { AP_Float K; @@ -71,6 +74,8 @@ public: float get_trigger_value(void) const {return _trigger_filter.get();}; + float get_stf_value(void) const {return _stf_filter.get();}; + float get_exp_thermalling_sink(void) const {return _expected_thermalling_sink;}; float calculate_circling_time_constant(const float thermal_bank);