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

AP_Soaring: Add a check of whether altitude has been lost overall when thermalling.

This commit is contained in:
Samuel Tabor 2019-06-08 01:19:42 +01:00 committed by Andrew Tridgell
parent 5082c9be8b
commit 30249e8006
2 changed files with 11 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_Soaring/AP_Soaring.cpp
View File

@ -199,6 +199,11 @@ SoaringController::LoiterStatus SoaringController::check_cruise_criteria()
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);
}
} else if (alt < _thermal_start_alt) {
result = ALT_LOST;
if (result != _cruise_criteria_msg_last) {
gcs().send_text(MAV_SEVERITY_INFO, "Altitude lost from entry = %dm", (int32_t)_thermal_start_alt);
}
}
}
@ -246,6 +251,7 @@ void SoaringController::init_thermalling()
_prev_update_time = AP_HAL::micros64();
_thermal_start_time_us = AP_HAL::micros64();
_thermal_start_alt = _vario.alt;
}
void SoaringController::init_cruising()

6
libraries/AP_Soaring/AP_Soaring.h
View File

@ -37,6 +37,9 @@ class SoaringController {
// store time thermal was entered for hysteresis
unsigned long _thermal_start_time_us;
// store altitude thermal was entered as a backup check
float _thermal_start_alt;
// store time cruise was entered for hysteresis
unsigned long _cruise_start_time_us;
@ -72,7 +75,8 @@ public:
ALT_TOO_HIGH,
ALT_TOO_LOW,
THERMAL_WEAK,
THERMAL_GOOD_TO_KEEP_LOITERING,
ALT_LOST,
THERMAL_GOOD_TO_KEEP_LOITERING
} LoiterStatus;
// this supports the TECS_* user settable parameters