AP_Soaring: Add a 60s first order filter on climb rate. If this becomes negative exit thermalling.

This commit is contained in:
Samuel Tabor 2019-06-08 13:51:00 +01:00 committed by Andrew Tridgell
parent a56b1dadb9
commit 936d4232c9
3 changed files with 18 additions and 7 deletions

View File

@ -199,10 +199,10 @@ SoaringController::LoiterStatus SoaringController::check_cruise_criteria()
if (result != _cruise_criteria_msg_last) { 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_alt) { } else if (alt < _thermal_start_alt || _vario.smoothed_climb_rate < 0.0) {
result = ALT_LOST; result = ALT_LOST;
if (result != _cruise_criteria_msg_last) { if (result != _cruise_criteria_msg_last) {
gcs().send_text(MAV_SEVERITY_INFO, "Altitude lost from entry = %dm", (int32_t)_thermal_start_alt); gcs().send_text(MAV_SEVERITY_INFO, "Not climbing");
} }
} }
} }
@ -252,6 +252,8 @@ void SoaringController::init_thermalling()
_prev_update_time = AP_HAL::micros64(); _prev_update_time = AP_HAL::micros64();
_thermal_start_time_us = AP_HAL::micros64(); _thermal_start_time_us = AP_HAL::micros64();
_thermal_start_alt = _vario.alt; _thermal_start_alt = _vario.alt;
_vario.reset_filter(0.0);
} }
void SoaringController::init_cruising() void SoaringController::init_cruising()

View File

@ -10,6 +10,7 @@ Variometer::Variometer(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms) :
_ahrs(ahrs), _ahrs(ahrs),
_aparm(parms) _aparm(parms)
{ {
_climb_filter = LowPassFilter<float>(1.0/60.0);
} }
void Variometer::update(const float polar_K, const float polar_B, const float polar_Cd0) void Variometer::update(const float polar_K, const float polar_B, const float polar_Cd0)
@ -26,12 +27,12 @@ void Variometer::update(const float polar_K, const float polar_B, const float po
// take 5 point moving average // take 5 point moving average
float dsp = _vdot_filter.apply(temp); float dsp = _vdot_filter.apply(temp);
float dh = 0;
Vector3f velned; Vector3f velned;
if (_ahrs.get_velocity_NED(velned)) { if (_ahrs.get_velocity_NED(velned)) {
// if possible use the EKF vertical velocity // if possible use the EKF vertical velocity
dh = -velned.z; raw_climb_rate = -velned.z;
} }
smoothed_climb_rate = _climb_filter.apply(raw_climb_rate, (AP_HAL::micros64() - _prev_update_time)/1e6);
float aspd = 0; float aspd = 0;
if (!_ahrs.airspeed_estimate(aspd)) { if (!_ahrs.airspeed_estimate(aspd)) {
@ -44,7 +45,7 @@ void Variometer::update(const float polar_K, const float polar_B, const float po
// Compute still-air sinkrate // Compute still-air sinkrate
float sinkrate = correct_netto_rate(0.0f, roll, _aspd_filt, polar_K, polar_Cd0, polar_B); float sinkrate = correct_netto_rate(0.0f, roll, _aspd_filt, polar_K, polar_Cd0, polar_B);
reading = dh + dsp*_aspd_filt/GRAVITY_MSS + sinkrate; reading = raw_climb_rate + dsp*_aspd_filt/GRAVITY_MSS + sinkrate;
filtered_reading = TE_FILT * reading + (1 - TE_FILT) * filtered_reading; // Apply low pass timeconst filter for noise filtered_reading = TE_FILT * reading + (1 - TE_FILT) * filtered_reading; // Apply low pass timeconst filter for noise
@ -52,14 +53,16 @@ void Variometer::update(const float polar_K, const float polar_B, const float po
_prev_update_time = AP_HAL::micros64(); _prev_update_time = AP_HAL::micros64();
AP::logger().Write("VAR", "TimeUS,aspd_raw,aspd_filt,alt,roll,raw,filt", "Qffffff", AP::logger().Write("VAR", "TimeUS,aspd_raw,aspd_filt,alt,roll,raw,filt,cl,fc", "Qffffffff",
AP_HAL::micros64(), AP_HAL::micros64(),
(double)0.0, (double)0.0,
(double)_aspd_filt, (double)_aspd_filt,
(double)alt, (double)alt,
(double)roll, (double)roll,
(double)reading, (double)reading,
(double)filtered_reading); (double)filtered_reading,
(double)raw_climb_rate,
(double)smoothed_climb_rate);
} }

View File

@ -34,15 +34,21 @@ class Variometer {
AverageFilterFloat_Size5 _sp_filter; AverageFilterFloat_Size5 _sp_filter;
// low pass filter @ 30s time constant
LowPassFilter<float> _climb_filter;
public: public:
Variometer(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms); Variometer(AP_AHRS &ahrs, const AP_Vehicle::FixedWing &parms);
float alt; float alt;
float reading; float reading;
float filtered_reading; float filtered_reading;
float displayed_reading; float displayed_reading;
float raw_climb_rate;
float smoothed_climb_rate;
void update(const float polar_K, const float polar_CD0, const float polar_B); void update(const float polar_K, const float polar_CD0, const float polar_B);
float correct_netto_rate(float climb_rate, float phi, float aspd, const float polar_K, const float polar_CD0, const float polar_B); 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);}
}; };