diff --git a/libraries/AP_Soaring/AP_Soaring.cpp b/libraries/AP_Soaring/AP_Soaring.cpp index afb5ccc30d..943e666497 100644 --- a/libraries/AP_Soaring/AP_Soaring.cpp +++ b/libraries/AP_Soaring/AP_Soaring.cpp @@ -213,12 +213,11 @@ SoaringController::LoiterStatus SoaringController::check_cruise_criteria() gcs().send_text(MAV_SEVERITY_ALERT, "Reached lower alt = %dm", (int16_t)alt); } } else if ((AP_HAL::micros64() - _thermal_start_time_us) > ((unsigned)min_thermal_s * 1e6)) { - const float thermalability = (_ekf.X[0]*expf(-powf(_aparm.loiter_radius / _ekf.X[1], 2))) - EXPECTED_THERMALLING_SINK; const float mcCreadyAlt = McCready(alt); - if (thermalability < mcCreadyAlt) { + if (_thermalability < mcCreadyAlt) { result = THERMAL_WEAK; if (result != _cruise_criteria_msg_last) { - gcs().send_text(MAV_SEVERITY_INFO, "Thermal weak: W %f.3 R %f.3 th %f.1 alt %dm Mc %dm", (double)_ekf.X[0], (double)_ekf.X[1], (double)thermalability, (int32_t)alt, (int32_t)mcCreadyAlt); + gcs().send_text(MAV_SEVERITY_INFO, "Thermal weak: W %f.3 R %f.3 th %f.1 alt %dm Mc %dm", (double)_ekf.X[0], (double)_ekf.X[1], (double)_thermalability, (int32_t)alt, (int32_t)mcCreadyAlt); } } else if (alt < (-_thermal_start_pos.z) || _vario.smoothed_climb_rate < 0.0) { result = ALT_LOST; @@ -303,8 +302,16 @@ void SoaringController::update_thermalling() Vector3f wind_drift = _ahrs.wind_estimate()*deltaT; + // update the filter + _ekf.update(_vario.reading, current_position.x, current_position.y, wind_drift.x, wind_drift.y); + + + _thermalability = (_ekf.X[0]*expf(-powf(_aparm.loiter_radius / _ekf.X[1], 2))) - _vario.get_exp_thermalling_sink(); + + _prev_update_time = AP_HAL::micros64(); + // write log - save the data. - AP::logger().Write("SOAR", "TimeUS,nettorate,x0,x1,x2,x3,north,east,alt,dx_w,dy_w", "Qffffffffff", + AP::logger().Write("SOAR", "TimeUS,nettorate,x0,x1,x2,x3,north,east,alt,dx_w,dy_w,th", "Qfffffffffff", AP_HAL::micros64(), (double)_vario.reading, (double)_ekf.X[0], @@ -315,12 +322,8 @@ void SoaringController::update_thermalling() current_position.y, (double)_vario.alt, (double)wind_drift.x, - (double)wind_drift.y); - - // update the filter - _ekf.update(_vario.reading, current_position.x, current_position.y, wind_drift.x, wind_drift.y); - - _prev_update_time = AP_HAL::micros64(); + (double)wind_drift.y, + (double)_thermalability); } void SoaringController::update_cruising() diff --git a/libraries/AP_Soaring/AP_Soaring.h b/libraries/AP_Soaring/AP_Soaring.h index f90009fd24..69313cb89e 100644 --- a/libraries/AP_Soaring/AP_Soaring.h +++ b/libraries/AP_Soaring/AP_Soaring.h @@ -16,7 +16,6 @@ #include "Variometer.h" #include -#define EXPECTED_THERMALLING_SINK 0.7 #define INITIAL_THERMAL_STRENGTH 2.0 #define INITIAL_THERMAL_RADIUS 30.0 //150.0 #define INITIAL_STRENGTH_COVARIANCE 0.0049 @@ -50,6 +49,9 @@ class SoaringController { float McCready(float alt); + float _thermalability; + float _expected_sink; + protected: AP_Int8 soar_active; AP_Int8 soar_active_ch; diff --git a/libraries/AP_Soaring/Variometer.cpp b/libraries/AP_Soaring/Variometer.cpp index fa49069a1e..f5fbc3a80f 100644 --- a/libraries/AP_Soaring/Variometer.cpp +++ b/libraries/AP_Soaring/Variometer.cpp @@ -42,10 +42,15 @@ void Variometer::update(const float polar_K, const float polar_B, const float po float roll = _ahrs.roll; - // Compute still-air sinkrate - float sinkrate = correct_netto_rate(0.0f, roll, _aspd_filt, polar_K, polar_Cd0, polar_B); + // Constrained airspeed. + const float minV = sqrt(polar_K/1.5); + float aspd_filt_constrained = _aspd_filt>minV ? _aspd_filt : minV; - reading = raw_climb_rate + dsp*_aspd_filt/GRAVITY_MSS + sinkrate; + + // Compute still-air sinkrate + float sinkrate = correct_netto_rate(0.0f, roll, aspd_filt_constrained, polar_K, polar_Cd0, polar_B); + + reading = raw_climb_rate + dsp*aspd_filt_constrained/GRAVITY_MSS + sinkrate; filtered_reading = TE_FILT * reading + (1 - TE_FILT) * filtered_reading; // Apply low pass timeconst filter for noise @@ -56,13 +61,16 @@ void Variometer::update(const float polar_K, const float polar_B, const float po AP::logger().Write("VAR", "TimeUS,aspd_raw,aspd_filt,alt,roll,raw,filt,cl,fc", "Qffffffff", AP_HAL::micros64(), (double)0.0, - (double)_aspd_filt, + (double)aspd_filt_constrained, (double)alt, (double)roll, (double)reading, (double)filtered_reading, (double)raw_climb_rate, (double)smoothed_climb_rate); + + float expected_roll = asinf(constrain_float(powf(aspd_filt_constrained,2)/(GRAVITY_MSS*_aparm.loiter_radius),-1.0, 1.0)); + _expected_thermalling_sink = correct_netto_rate(0.0, expected_roll, aspd_filt_constrained, polar_K, polar_Cd0, polar_B); } @@ -80,6 +88,7 @@ float Variometer::correct_netto_rate(float climb_rate, float netto_rate; float cosphi; CL0 = polar_K / (aspd * aspd); + C1 = polar_CD0 / CL0; // constant describing expected angle to overcome zero-lift drag C2 = polar_B * CL0; // constant describing expected angle to overcome lift induced drag at zero bank diff --git a/libraries/AP_Soaring/Variometer.h b/libraries/AP_Soaring/Variometer.h index e3a710d123..b8dfadafc6 100644 --- a/libraries/AP_Soaring/Variometer.h +++ b/libraries/AP_Soaring/Variometer.h @@ -28,6 +28,7 @@ class Variometer { float _last_aspd; float _last_roll; float _last_total_E; + float _expected_thermalling_sink; // declares a 5point average filter using floats AverageFilterFloat_Size5 _vdot_filter; @@ -50,5 +51,9 @@ public: float correct_netto_rate(float climb_rate, float phi, float aspd, const float polar_K, const float polar_CD0, const float polar_B); void reset_filter(float value) { _climb_filter.reset(value);} + + float get_airspeed(void) {return _aspd_filt;}; + + float get_exp_thermalling_sink(void) {return _expected_thermalling_sink;}; };