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

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

Browse Source
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) { 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); 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(); _prev_update_time = AP_HAL::micros64();
_thermal_start_time_us = AP_HAL::micros64(); _thermal_start_time_us = AP_HAL::micros64();
_thermal_start_alt = _vario.alt;
} }
void SoaringController::init_cruising() 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 // store time thermal was entered for hysteresis
unsigned long _thermal_start_time_us; 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 // store time cruise was entered for hysteresis
unsigned long _cruise_start_time_us; unsigned long _cruise_start_time_us;
@ -72,7 +75,8 @@ public:
ALT_TOO_HIGH, ALT_TOO_HIGH,
ALT_TOO_LOW, ALT_TOO_LOW,
THERMAL_WEAK, THERMAL_WEAK,
THERMAL_GOOD_TO_KEEP_LOITERING, ALT_LOST,
THERMAL_GOOD_TO_KEEP_LOITERING
} LoiterStatus; } LoiterStatus;
// this supports the TECS_* user settable parameters // this supports the TECS_* user settable parameters