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: Fix incorrect trig function and log the expected sink.

Browse Source
This commit is contained in:
Samuel Tabor 2019-06-23 15:13:08 +01:00 committed by Andrew Tridgell
parent 17f1fa9600
commit 7555f5abf8
1 changed files with 6 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_Soaring/Variometer.cpp
View File

@ -59,7 +59,10 @@ void Variometer::update(const float polar_K, const float polar_Cd0, const float
_prev_update_time = AP_HAL::micros64();
AP::logger().Write("VAR", "TimeUS,aspd_raw,aspd_filt,alt,roll,raw,filt,cl,fc", "Qffffffff",
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);
AP::logger().Write("VAR", "TimeUS,aspd_raw,aspd_filt,alt,roll,raw,filt,cl,fc,exs", "Qfffffffff",
AP_HAL::micros64(),
(double)0.0,
(double)_aspd_filt_constrained,
@ -68,10 +71,8 @@ void Variometer::update(const float polar_K, const float polar_Cd0, const float
(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 = calculate_aircraft_sinkrate(expected_roll, polar_K, polar_Cd0, polar_B);
(double)smoothed_climb_rate,
(double)_expected_thermalling_sink);
}