Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Soaring: Calculate expected thermalling sink live and avoid divide by zero by limiting airspeed to that corresponding to CL max (generously assumed 1.5 for glider).

Browse Source
This commit is contained in:
Samuel Tabor 2019-06-22 17:02:35 +01:00 committed by Andrew Tridgell
parent 5963164a22
commit fab74f8927
4 changed files with 34 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
libraries/AP_Soaring/AP_Soaring.cpp
View File

@ -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()

4
libraries/AP_Soaring/AP_Soaring.h
View File

@ -16,7 +16,6 @@
#include "Variometer.h"
#include <AP_SpdHgtControl/AP_SpdHgtControl.h>
#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;

17
libraries/AP_Soaring/Variometer.cpp
View File

@ -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

5
libraries/AP_Soaring/Variometer.h
View File

@ -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;};
};