Skip to content

Commit

Permalink
Plane: unified soaring exit behavior: auto heads to next wp, cruise c…
Browse files Browse the repository at this point in the history 
…ontinues cruise heading, FBW to home
  • Loading branch information
@magicrub @tridge
magicrub authored and tridge committed Apr 7, 2020
1 parent f24095e commit 394fbb1
Show file tree
Hide file tree
Showing 4 changed files with 45 additions and 29 deletions.
2 changes: 1 addition & 1 deletion ArduPlane/commands_logic.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 changes: 3 additions & 1 deletion ArduPlane/mode.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -173,7 +173,9 @@ class ModeLoiter : public Mode
// 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:

Expand Down
14 changes: 12 additions & 2 deletions ArduPlane/mode_loiter.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand All @@ -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();

Expand Down
54 changes: 29 additions & 25 deletions ArduPlane/soaring.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -73,45 +73,49 @@ 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) {
case SoaringController::LoiterStatus::ALT_TOO_HIGH:
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);
Expand All @@ -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
Expand Down

0 comments on commit 394fbb1

Please sign in to comment.