aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Plane: Soaring - better reporting of exit due to drift.

Browse Source
This commit is contained in:
Samuel Tabor 2019-06-09 11:01:16 +01:00 committed by Andrew Tridgell
parent e1b2360781
commit 013628bd1d
4 changed files with 19 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
ArduPlane/soaring.cpp
View File

@ -74,6 +74,7 @@ void Plane::update_soaring() {
const char* strTooHigh = "Soaring: Too high, restoring ";
const char* strTooLow = "Soaring: Too low, restoring ";
const char* strTooWeak = "Soaring: Thermal ended, restoring ";
const char* strTooFar = "Soaring: Drifted too far, restoring ";
// Exit as soon as thermal state estimate deteriorates and we're lined up to next target
switch (previous_mode->mode_number()){
@ -97,6 +98,10 @@ void Plane::update_soaring() {
gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooWeak, mode_fbwb.name());
set_mode(mode_fbwb, ModeReason::SOARING_THERMAL_ESTIMATE_DETERIORATED);
break;
case SoaringController::LoiterStatus::DRIFT_EXCEEDED:
gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooFar, mode_fbwb.name());
set_mode(mode_fbwb, ModeReason::SOARING_DRIFT_EXCEEDED);
break;
} // switch louterStatus
break;
}
@ -122,6 +127,10 @@ void Plane::update_soaring() {
gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooWeak, mode_cruise.name());
set_mode(mode_cruise, ModeReason::SOARING_THERMAL_ESTIMATE_DETERIORATED);
break;
case SoaringController::LoiterStatus::DRIFT_EXCEEDED:
gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooFar, mode_cruise.name());
set_mode(mode_cruise, ModeReason::SOARING_DRIFT_EXCEEDED);
break;
} // switch loiterStatus
cruise_state = cruise;
set_target_altitude_current();
@ -131,7 +140,7 @@ void Plane::update_soaring() {
case Mode::Number::AUTO: {
//Get the lat/lon of next Nav waypoint after this one:
AP_Mission::Mission_Command current_nav_cmd = mission.get_current_nav_cmd();;
const bool headingLinedupToWP = isHeadingLinedUp(next_WP_loc, current_nav_cmd.content.location);
const bool headingLinedupToWP = plane.mode_loiter.isHeadingLinedUp(next_WP_loc, current_nav_cmd.content.location);
if (!headingLinedupToWP) {
break;
}
@ -149,6 +158,10 @@ void Plane::update_soaring() {
gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooWeak, mode_auto.name());
set_mode(mode_auto, ModeReason::SOARING_THERMAL_ESTIMATE_DETERIORATED);
break;
case SoaringController::LoiterStatus::DRIFT_EXCEEDED:
gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooFar, mode_auto.name());
set_mode(mode_auto, ModeReason::SOARING_DRIFT_EXCEEDED);
break;
} // switch loiterStatus
break;
} // case AUTO

6
libraries/AP_Soaring/AP_Soaring.cpp
View File

@ -121,7 +121,7 @@ const AP_Param::GroupInfo SoaringController::var_info[] = {
// @User: Advanced
AP_GROUPINFO("ENABLE_CH", 15, SoaringController, soar_active_ch, 0),
// @Param: MAX_DEV
// @Param: MAX_DRIFT
// @DisplayName: (Optional) Maximum drift distance to allow when thermalling.
// @Description: The previous mode will be restored if the horizontal distance to the thermalling start location exceeds this value. 0 to disable.
// @Range: 0 1000
@ -216,8 +216,8 @@ SoaringController::LoiterStatus SoaringController::check_cruise_criteria()
if (result != _cruise_criteria_msg_last) {
gcs().send_text(MAV_SEVERITY_INFO, "Not climbing");
}
} else if (max_drift && (powf(position.x - _thermal_start_pos.x, 2) + powf(position.y - _thermal_start_pos.y, 2)) > powf(max_drift,2)) {
result = DEVIATION_EXCEEDED;
} else if (!(max_drift<0) && (powf(position.x - _thermal_start_pos.x, 2) + powf(position.y - _thermal_start_pos.y, 2)) > powf(max_drift,2)) {
result = DRIFT_EXCEEDED;
if (result != _cruise_criteria_msg_last) {
gcs().send_text(MAV_SEVERITY_INFO, "Drifted too far");
}

2
libraries/AP_Soaring/AP_Soaring.h
View File

@ -78,7 +78,7 @@ public:
ALT_TOO_LOW,
THERMAL_WEAK,
ALT_LOST,
DEVIATION_EXCEEDED,
DRIFT_EXCEEDED,
THERMAL_GOOD_TO_KEEP_LOITERING
} LoiterStatus;

1
libraries/AP_Vehicle/ModeReason.h
View File

@ -55,4 +55,5 @@ enum class ModeReason : uint8_t {
AUTOROTATION_BAILOUT,
SOARING_ALT_TOO_HIGH,
SOARING_ALT_TOO_LOW,
SOARING_DRIFT_EXCEEDED,
};