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

AP_Soaring: Fix issue with loiter radius being saved as zero due to initialisation order. This caused incorrect calculation of thermal climb potential and cases of staying in thermals that should have been abandoned.

Browse Source
This commit is contained in:
Samuel Tabor 2019-05-16 11:39:56 +01:00 committed by Andrew Tridgell
parent c135b00b7b
commit 0cccc8dc29
2 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_Soaring/AP_Soaring.cpp
View File

@ -128,7 +128,7 @@ SoaringController::SoaringController(AP_AHRS &ahrs, AP_SpdHgtControl &spdHgt, co
_ahrs(ahrs),
_spdHgt(spdHgt),
_vario(ahrs,parms),
_loiter_rad(parms.loiter_radius),
_aparm(parms),
_throttle_suppressed(true)
{
AP_Param::setup_object_defaults(this, var_info);
@ -187,7 +187,7 @@ SoaringController::LoiterStatus SoaringController::check_cruise_criteria()
gcs().send_text(MAV_SEVERITY_ALERT, "Reached lower altitude, beginning cruise. Alt = %f2", (double)alt);
return ALT_TOO_LOW;
} else if ((AP_HAL::micros64() - _thermal_start_time_us) > ((unsigned)min_thermal_s * 1e6)) {
const float thermalability = (_ekf.X[0]*expf(-powf(_loiter_rad / _ekf.X[1], 2))) - EXPECTED_THERMALLING_SINK;
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) {
gcs().send_text(MAV_SEVERITY_INFO, "Thermal weak, recommend quitting: W %f2 R %f2 th %f2 alt %f2 Mc %f2", (double)_ekf.X[0], (double)_ekf.X[1], (double)thermalability, (double)alt, (double)mcCreadyAlt);

2
libraries/AP_Soaring/AP_Soaring.h
View File

@ -29,6 +29,7 @@ class SoaringController {
AP_AHRS &_ahrs;
AP_SpdHgtControl &_spdHgt;
Variometer _vario;
const AP_Vehicle::FixedWing &_aparm;
// store aircraft location at last update
struct Location _prev_update_location;
@ -42,7 +43,6 @@ class SoaringController {
// store time of last update
unsigned long _prev_update_time;
float _loiter_rad;
bool _throttle_suppressed;
float McCready(float alt);