Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Soaring: Store reference to glide polar params in variometer

This commit is contained in:
Samuel Tabor 2020-04-20 12:11:22 +01:00 committed by Andrew Tridgell
parent ff77a17fdd
commit 3ab9dda6f5
4 changed files with 31 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_Soaring/AP_Soaring.cpp
View File

@ -74,14 +74,14 @@ const AP_Param::GroupInfo SoaringController::var_info[] = {
// @Description: Zero lift drag coefficient
// @Range: 0.005 0.5
// @User: Advanced
AP_GROUPINFO("POLAR_CD0", 9, SoaringController, polar_CD0, 0.027),
AP_GROUPINFO("POLAR_CD0", 9, SoaringController, _polarParams.CD0, 0.027),
// @Param: POLAR_B
// @DisplayName: Induced drag coeffient
// @Description: Induced drag coeffient
// @Range: 0.005 0.05
// @User: Advanced
AP_GROUPINFO("POLAR_B", 10, SoaringController, polar_B, 0.031),
AP_GROUPINFO("POLAR_B", 10, SoaringController, _polarParams.B, 0.031),
// @Param: POLAR_K
// @DisplayName: Cl factor
@ -89,7 +89,7 @@ const AP_Param::GroupInfo SoaringController::var_info[] = {
// @Units: m.m/s/s
// @Range: 20 400
// @User: Advanced
AP_GROUPINFO("POLAR_K", 11, SoaringController, polar_K, 25.6),
AP_GROUPINFO("POLAR_K", 11, SoaringController, _polarParams.K, 25.6),
// @Param: ALT_MAX
// @DisplayName: Maximum soaring altitude, relative to the home location
@ -144,7 +144,7 @@ const AP_Param::GroupInfo SoaringController::var_info[] = {
SoaringController::SoaringController(AP_TECS &tecs, const AP_Vehicle::FixedWing &parms) :
_tecs(tecs),
_vario(parms),
_vario(parms,_polarParams),
_aparm(parms),
_throttle_suppressed(true)
{
@ -352,7 +352,7 @@ void SoaringController::update_cruising()
void SoaringController::update_vario()
{
_vario.update(thermal_bank, polar_K, polar_CD0, polar_B);
_vario.update(thermal_bank);
}

5
libraries/AP_Soaring/AP_Soaring.h
View File

@ -58,6 +58,8 @@ class SoaringController {
LowPassFilter<float> _position_x_filter{1/60.0};
LowPassFilter<float> _position_y_filter{1/60.0};
Variometer::PolarParams _polarParams;
protected:
AP_Int8 soar_active;
AP_Float thermal_vspeed;
@ -67,9 +69,6 @@ protected:
AP_Float thermal_distance_ahead;
AP_Int16 min_thermal_s;
AP_Int16 min_cruise_s;
AP_Float polar_CD0;
AP_Float polar_B;
AP_Float polar_K;
AP_Float alt_max;
AP_Float alt_min;
AP_Float alt_cutoff;

24
libraries/AP_Soaring/Variometer.cpp
View File

@ -6,12 +6,13 @@ Manages the estimation of aircraft total energy, drag and vertical air velocity.
#include <AP_Logger/AP_Logger.h>
Variometer::Variometer(const AP_Vehicle::FixedWing &parms) :
_aparm(parms)
Variometer::Variometer(const AP_Vehicle::FixedWing &parms, PolarParams &polarParams) :
_aparm(parms),
_polarParams(polarParams)
{
}
void Variometer::update(const float thermal_bank, const float polar_K, const float polar_Cd0, const float polar_B)
void Variometer::update(const float thermal_bank)
{
const AP_AHRS &_ahrs = AP::ahrs();
@ -26,7 +27,7 @@ void Variometer::update(const float thermal_bank, const float polar_K, const flo
float aspd_filt = _sp_filter.apply(aspd);
// Constrained airspeed.
const float minV = sqrtf(polar_K/1.5);
const float minV = sqrtf(_polarParams.K/1.5);
_aspd_filt_constrained = aspd_filt>minV ? aspd_filt : minV;
tau = calculate_circling_time_constant(radians(thermal_bank));
@ -62,7 +63,7 @@ void Variometer::update(const float thermal_bank, const float polar_K, const flo
// Compute still-air sinkrate
float roll = _ahrs.roll;
float sinkrate = calculate_aircraft_sinkrate(roll, polar_K, polar_Cd0, polar_B);
float sinkrate = calculate_aircraft_sinkrate(roll);
reading = raw_climb_rate + dsp_cor*_aspd_filt_constrained/GRAVITY_MSS + sinkrate;
@ -73,7 +74,7 @@ void Variometer::update(const float thermal_bank, const float polar_K, const flo
_prev_update_time = AP_HAL::micros64();
_expected_thermalling_sink = calculate_aircraft_sinkrate(radians(thermal_bank), polar_K, polar_Cd0, polar_B);
_expected_thermalling_sink = calculate_aircraft_sinkrate(radians(thermal_bank));
// @LoggerMessage: VAR
// @Vehicles: Plane
@ -106,19 +107,16 @@ void Variometer::update(const float thermal_bank, const float polar_K, const flo
}
float Variometer::calculate_aircraft_sinkrate(float phi,
const float polar_K,
const float polar_CD0,
const float polar_B) const
float Variometer::calculate_aircraft_sinkrate(float phi) const
{
// Remove aircraft sink rate
float CL0; // CL0 = 2*W/(rho*S*V^2)
float C1; // C1 = CD0/CL0
float C2; // C2 = CDi0/CL0 = B*CL0
CL0 = polar_K / (_aspd_filt_constrained * _aspd_filt_constrained);
CL0 = _polarParams.K / (_aspd_filt_constrained * _aspd_filt_constrained);
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
C1 = _polarParams.CD0 / CL0; // constant describing expected angle to overcome zero-lift drag
C2 = _polarParams.B * CL0; // constant describing expected angle to overcome lift induced drag at zero bank
float cosphi = (1 - phi * phi / 2); // first two terms of mclaurin series for cos(phi)

16
libraries/AP_Soaring/Variometer.h
View File

@ -44,13 +44,20 @@ class Variometer {
LowPassFilter<float> _vdotbias_filter{1/60.0};
public:
Variometer(const AP_Vehicle::FixedWing &parms);
struct PolarParams {
AP_Float K;
AP_Float CD0;
AP_Float B;
};
Variometer(const AP_Vehicle::FixedWing &parms, PolarParams &polarParams);
float alt;
float reading;
float tau;
void update(const float thermal_bank, const float polar_K, const float polar_CD0, const float polar_B);
float calculate_aircraft_sinkrate(float phi, const float polar_K, const float polar_CD0, const float polar_B) const;
void update(const float thermal_bank);
float calculate_aircraft_sinkrate(float phi) const;
void reset_climb_filter(float value) { _climb_filter.reset(value);}
@ -67,5 +74,8 @@ public:
float get_exp_thermalling_sink(void) const {return _expected_thermalling_sink;};
float calculate_circling_time_constant(const float thermal_bank);
private:
PolarParams &_polarParams;
};