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

AP_Soaring: add reason to exit Thermal loiter

Browse Source
This commit is contained in:
Tom Pittenger 2019-06-05 11:23:29 -07:00 committed by Andrew Tridgell
parent 73c5c2e1bb
commit 52ae093a97
2 changed files with 31 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
libraries/AP_Soaring/AP_Soaring.cpp
View File

@ -171,20 +171,31 @@ bool SoaringController::check_thermal_criteria()
&& _vario.alt > alt_min);
}
bool SoaringController::check_cruise_criteria()
{
float thermalability = (_ekf.X[0]*expf(-powf(_loiter_rad / _ekf.X[1], 2))) - EXPECTED_THERMALLING_SINK;
float alt = _vario.alt;
if (soar_active && (AP_HAL::micros64() - _thermal_start_time_us) > ((unsigned)min_thermal_s * 1e6) && thermalability < McCready(alt)) {
gcs().send_text(MAV_SEVERITY_INFO, "Thermal weak, recommend quitting: W %f R %f th %f alt %f Mc %f", (double)_ekf.X[0], (double)_ekf.X[1], (double)thermalability, (double)alt, (double)McCready(alt));
return true;
} else if (soar_active && (alt>alt_max || alt<alt_min)) {
gcs().send_text(MAV_SEVERITY_ALERT, "Out of allowable altitude range, beginning cruise. Alt = %f", (double)alt);
return true;
SoaringController::LoiterStatus SoaringController::check_cruise_criteria()
{
if (!soar_active) {
return SOARING_DISABLED;
}
return false;
const float alt = _vario.alt;
if (alt > alt_max) {
gcs().send_text(MAV_SEVERITY_ALERT, "Reached upper altitude, beginning cruise. Alt = %f2", (double)alt);
return ALT_TOO_HIGH;
} else if (alt < alt_min) {
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 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);
return THERMAL_WEAK;
}
}
return THERMAL_GOOD_TO_KEEP_LOITERING;
}
bool SoaringController::check_init_thermal_criteria()

10
libraries/AP_Soaring/AP_Soaring.h
View File

@ -69,12 +69,20 @@ protected:
public:
SoaringController(AP_AHRS &ahrs, AP_SpdHgtControl &spdHgt, const AP_Vehicle::FixedWing &parms);
enum LoiterStatus {
SOARING_DISABLED,
ALT_TOO_HIGH,
ALT_TOO_LOW,
THERMAL_WEAK,
THERMAL_GOOD_TO_KEEP_LOITERING,
};
// this supports the TECS_* user settable parameters
static const struct AP_Param::GroupInfo var_info[];
void get_target(Location & wp);
bool suppress_throttle();
bool check_thermal_criteria();
bool check_cruise_criteria();
LoiterStatus check_cruise_criteria();
bool check_init_thermal_criteria();
void init_thermalling();
void init_cruising();