aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Soaring: Fix incorrect use of aparm.loiter_radius.

Browse Source
This commit is contained in:
Samuel Tabor 2021-02-28 13:33:29 +00:00 committed by Peter Barker
parent 936aa63135
commit d578b4d3db
3 changed files with 4 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_Soaring/AP_Soaring.cpp
View File

@ -371,7 +371,7 @@ void SoaringController::update_cruising()
void SoaringController::update_vario()
{
_vario.update(polar_K, polar_CD0, polar_B);
_vario.update(thermal_bank, polar_K, polar_CD0, polar_B);
}

5
libraries/AP_Soaring/Variometer.cpp
View File

@ -14,7 +14,7 @@ Variometer::Variometer(const AP_Vehicle::FixedWing &parms) :
_vdot_filter2 = LowPassFilter<float>(1.0f/60.0f);
}
void Variometer::update(const float polar_K, const float polar_Cd0, const float polar_B)
void Variometer::update(const float thermal_bank, const float polar_K, const float polar_Cd0, const float polar_B)
{
const AP_AHRS &_ahrs = AP::ahrs();
@ -73,8 +73,7 @@ void Variometer::update(const float polar_K, const float polar_Cd0, const float
_prev_update_time = AP_HAL::micros64();
float expected_roll = atanf(powf(_aspd_filt_constrained,2)/(GRAVITY_MSS*_aparm.loiter_radius));
_expected_thermalling_sink = calculate_aircraft_sinkrate(expected_roll, polar_K, polar_Cd0, polar_B);
_expected_thermalling_sink = calculate_aircraft_sinkrate(radians(thermal_bank), polar_K, polar_Cd0, polar_B);
// @LoggerMessage: VAR
// @Vehicles: Plane

2
libraries/AP_Soaring/Variometer.h
View File

@ -46,7 +46,7 @@ public:
float smoothed_climb_rate;
float tau;
void update(const float polar_K, const float polar_CD0, const float polar_B);
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 reset_filter(float value) { _climb_filter.reset(value);}