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

Plane: unified soaring exit behavior: auto heads to next wp, cruise continues cruise heading, FBW to home

This commit is contained in:
Tom Pittenger 2019-06-07 08:08:28 -07:00 committed by Andrew Tridgell
parent f24095e9ed
commit 394fbb1a50
4 changed files with 45 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduPlane/commands_logic.cpp
View File

@ -1078,5 +1078,5 @@ bool Plane::verify_loiter_heading(bool init)
loiter.sum_cd = 0;
}
return plane.mode_loiter.headingLinedUp(next_WP_loc, next_nav_cmd.content.location);
return plane.mode_loiter.isHeadingLinedUp(next_WP_loc, next_nav_cmd.content.location);
}

4
ArduPlane/mode.h
View File

@ -173,7 +173,9 @@ public:
// methods that affect movement of the vehicle in this mode
void update() override;
bool headingLinedUp(const Location loiterCenterLoc, const Location targetLoc);
bool isHeadingLinedUp(const Location loiterCenterLoc, const Location targetLoc);
bool isHeadingLinedUp(const float bearing);
bool isHeadingLinedUp(const int32_t bearing_cd);
protected:

14
ArduPlane/mode_loiter.cpp
View File

@ -26,7 +26,7 @@ void ModeLoiter::update()
plane.calc_throttle();
}
bool ModeLoiter::headingLinedUp(const Location loiterCenterLoc, const Location targetLoc)
bool ModeLoiter::isHeadingLinedUp(const Location loiterCenterLoc, const Location targetLoc)
{
const uint16_t loiterRadius = abs(plane.aparm.loiter_radius);
if (loiterCenterLoc.get_distance(targetLoc) < loiterRadius + loiterRadius*0.05) {
@ -37,9 +37,19 @@ bool ModeLoiter::headingLinedUp(const Location loiterCenterLoc, const Location t
return true;
}
// Bearing in degrees
// Bearing in centi-degrees
const int32_t bearing_cd = plane.current_loc.get_bearing_to(targetLoc);
return isHeadingLinedUp(bearing_cd);
}
bool ModeLoiter::isHeadingLinedUp(const float bearing)
{
const int32_t bearing_cd = bearing*100;
return isHeadingLinedUp(bearing_cd);
}
bool ModeLoiter::isHeadingLinedUp(const int32_t bearing_cd)
{
// get current heading.
const int32_t heading_cd = plane.gps.ground_course_cd();

54
ArduPlane/soaring.cpp
View File

@ -73,33 +73,37 @@ void Plane::update_soaring() {
const char* strTooLow = "Soaring: Too low, restoring ";
const char* strTooWeak = "Soaring: Thermal ended, restoring ";
// Exit as soon as thermal state estimate deteriorates
switch (previous_mode->mode_number()) {
// Exit as soon as thermal state estimate deteriorates and we're lined up to next target
switch (previous_mode->mode_number()){
case Mode::Number::FLY_BY_WIRE_B: {
const bool homeIsSet = AP::ahrs().home_is_set();
const bool headingLinedupToHome = homeIsSet && plane.mode_loiter.isHeadingLinedUp(next_WP_loc, AP::ahrs().get_home());
if (homeIsSet && !headingLinedupToHome) {
break;
}
switch (loiterStatus) {
case SoaringController::LoiterStatus::ALT_TOO_HIGH: {
const bool homeIsSet = AP::ahrs().home_is_set();
const bool headingLinedupToHome = homeIsSet && plane.mode_loiter.headingLinedUp(next_WP_loc, AP::ahrs().get_home());
if (!homeIsSet || headingLinedupToHome) {
gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooHigh, mode_fbwb.name());
set_mode(mode_fbwb, ModeReason::SOARING_ALT_TOO_HIGH);
}
}
case SoaringController::LoiterStatus::ALT_TOO_HIGH:
gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooHigh, mode_fbwb.name());
set_mode(mode_fbwb, ModeReason::SOARING_ALT_TOO_HIGH);
break;
default:
case SoaringController::LoiterStatus::ALT_TOO_LOW:
gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooLow, mode_rtl.name());
set_mode(mode_rtl, ModeReason::SOARING_ALT_TOO_LOW);
break;
default:
case SoaringController::LoiterStatus::THERMAL_WEAK:
gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooWeak, mode_fbwb.name());
set_mode(mode_fbwb, ModeReason::SOARING_THERMAL_ESTIMATE_DETERIORATED);
break;
} // switch louterStatus
} // case FBWB
break;
}
case Mode::Number::CRUISE: {
const bool headingLinedupToCruise = plane.mode_loiter.isHeadingLinedUp(cruise_state.locked_heading_cd);
if (cruise_state.locked_heading && !headingLinedupToCruise) {
break;
}
// return to cruise with old ground course
const CruiseState cruise = cruise_state;
switch (loiterStatus) {
@ -107,11 +111,11 @@ void Plane::update_soaring() {
gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooHigh, mode_cruise.name());
set_mode(mode_cruise, ModeReason::SOARING_ALT_TOO_HIGH);
break;
default:
case SoaringController::LoiterStatus::ALT_TOO_LOW:
gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooLow, mode_rtl.name());
set_mode(mode_rtl, ModeReason::SOARING_ALT_TOO_LOW);
break;
default:
case SoaringController::LoiterStatus::THERMAL_WEAK:
gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooWeak, mode_cruise.name());
set_mode(mode_cruise, ModeReason::SOARING_THERMAL_ESTIMATE_DETERIORATED);
@ -122,31 +126,31 @@ void Plane::update_soaring() {
break;
} // case Cruise
case Mode::Number::AUTO:
case Mode::Number::AUTO: {
//Get the lat/lon of next Nav waypoint after this one:
AP_Mission::Mission_Command next_nav_cmd;
const bool nextWpisValid = mission.get_next_nav_cmd(mission.get_current_nav_index() + 1, next_nav_cmd);
const bool headingLinedupToWP = nextWpisValid && plane.mode_loiter.isHeadingLinedUp(next_WP_loc, next_nav_cmd.content.location);
if (nextWpisValid && !headingLinedupToWP) {
break;
}
switch (loiterStatus) {
case SoaringController::LoiterStatus::ALT_TOO_HIGH:
gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooHigh, mode_auto.name());
set_mode(mode_auto, ModeReason::SOARING_ALT_TOO_HIGH);
break;
default:
case SoaringController::LoiterStatus::ALT_TOO_LOW:
gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooLow, mode_auto.name());
set_mode(mode_auto, ModeReason::SOARING_ALT_TOO_LOW);
break;
case SoaringController::LoiterStatus::THERMAL_WEAK: {
//Get the lat/lon of next Nav waypoint after this one:
AP_Mission::Mission_Command next_nav_cmd;
const bool nextWpisValid = mission.get_next_nav_cmd(mission.get_current_nav_index() + 1, next_nav_cmd);
const bool headingLinedupToWP = nextWpisValid && plane.mode_loiter.headingLinedUp(next_WP_loc, next_nav_cmd.content.location);
if (!nextWpisValid || headingLinedupToWP) {
default:
case SoaringController::LoiterStatus::THERMAL_WEAK:
gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooWeak, mode_auto.name());
set_mode(mode_auto, ModeReason::SOARING_THERMAL_ESTIMATE_DETERIORATED);
}
}
break;
} // switch loiterStatus
} // switch loiterStatus
break;
} // case AUTO
default: // all other modes
break;
} // switch previous_mode