Skip to content

Commit

Permalink
Plane: Refactor soaring.cpp.
Browse files Browse the repository at this point in the history
  • Loading branch information
@samuelctabor
samuelctabor committed Aug 3, 2020
1 parent 9d01fc6 commit 84d5621
Show file tree
Hide file tree
Showing 2 changed files with 84 additions and 118 deletions.
4 changes: 3 additions & 1 deletion ArduPlane/Plane.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -783,7 +783,7 @@ class Plane : public AP_Vehicle {
float rudder_dt;

// soaring mode-change timer
uint32_t soaring_mode_timer;
uint32_t soaring_mode_timer_ms;

// terrain disable for non AUTO modes, set with an RC Option switch
bool non_auto_terrain_disable;
Expand Down Expand Up @@ -1072,6 +1072,8 @@ class Plane : public AP_Vehicle {
// soaring.cpp
#if SOARING_ENABLED == ENABLED
void update_soaring();
bool soaring_exit_heading_aligned() const;
void soaring_restore_mode(const char *reason, ModeReason modereason, Mode &exit_mode);
#endif

// reverse_thrust.cpp
Expand Down
198 changes: 81 additions & 117 deletions ArduPlane/soaring.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -66,156 +66,120 @@ void Plane::update_soaring() {
// Update thermal estimate and check for switch back to AUTO
g2.soaring_controller.update_thermalling(); // Update estimate

// Thermalling is done in a home-relative coordinate system, so we need home to be set.
Vector3f position;
if (!ahrs.get_relative_position_NED_home(position)) {
return;
}

// Check distance to home against MAX_RADIUS.
if (g2.soaring_controller.max_radius >= 0 &&
powf(position.x,2)+powf(position.y,2) > powf(g2.soaring_controller.max_radius,2) &&
previous_mode->mode_number()!=Mode::Number::AUTO) {
// Some other loiter status, and outside of maximum soaring radius, and previous mode wasn't AUTO
gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Outside SOAR_MAX_RADIUS, RTL");
set_mode(mode_rtl, ModeReason::SOARING_DRIFT_EXCEEDED);
break;
}

// If previous mode was AUTO and there was a previous NAV command, we can use previous and next wps for drift calculation
// with respect to the desired direction of travel. If these vectors are zero, drift will be calculated from thermal start
// position only, without taking account of the desired direction of travel.
Vector2f prev_wp, next_wp;

// If previous mode was AUTO and there was a previous NAV command, we can use previous and next wps for drift calculation.
if (previous_mode->mode_number() == Mode::Number::AUTO) {
if (previous_mode == &mode_auto) {
AP_Mission::Mission_Command current_nav_cmd = mission.get_current_nav_cmd();
AP_Mission::Mission_Command prev_nav_cmd;

if (!(mission.get_next_nav_cmd(mission.get_prev_nav_cmd_with_wp_index(), prev_nav_cmd) &&
prev_nav_cmd.content.location.get_vector_xy_from_origin_NE(prev_wp) &&
prev_nav_cmd.content.location.get_vector_xy_from_origin_NE(prev_wp) &&
current_nav_cmd.content.location.get_vector_xy_from_origin_NE(next_wp))) {
prev_wp.x = 0.0;
prev_wp.y = 0.0;
next_wp.x = 0.0;
next_wp.y = 0.0;
prev_wp.zero();
next_wp.zero();
}
}

// Get the status of the soaring controller cruise checks.
const SoaringController::LoiterStatus loiterStatus = g2.soaring_controller.check_cruise_criteria(prev_wp/100, next_wp/100);

if (loiterStatus == SoaringController::LoiterStatus::GOOD_TO_KEEP_LOITERING) {
// Reset loiter angle, so that the loiter exit heading criteria
// only starts expanding when we're ready to exit.
plane.loiter.sum_cd = 0;
plane.soaring_mode_timer = AP_HAL::millis();
plane.soaring_mode_timer_ms = AP_HAL::millis();

//update the wp location
g2.soaring_controller.get_target(next_WP_loc);

break;
}

// Some other loiter status, we need to think about exiting loiter.
uint32_t timer = AP_HAL::millis() - plane.soaring_mode_timer;
const uint32_t time_in_loiter_ms = AP_HAL::millis() - plane.soaring_mode_timer_ms;

// Check distance to home.
Vector3f position;
if (!ahrs.get_relative_position_NED_home(position)) {
return;
} else if (g2.soaring_controller.max_radius >= 0 &&
powf(position.x,2)+powf(position.y,2) > powf(g2.soaring_controller.max_radius,2) &&
previous_mode->mode_number()!=Mode::Number::AUTO) {
// Some other loiter status, and outside of maximum soaring radius, and previous mode wasn't AUTO
gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Outside SOAR_MAX_RADIUS, RTL");
set_mode(mode_rtl, ModeReason::SOARING_DRIFT_EXCEEDED);
if (!soaring_exit_heading_aligned() && loiterStatus!=SoaringController::LoiterStatus::ALT_TOO_LOW && time_in_loiter_ms < 20000) {
// Heading not lined up, and not timed out or in a condition requiring immediate exit.
break;
}

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()){
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 && loiterStatus!=SoaringController::LoiterStatus::ALT_TOO_LOW && timer<20e3) {
break;
}
switch (loiterStatus) {
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;
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;
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;
}
// Heading lined up and loiter status not good to continue. Need to switch mode.

case Mode::Number::CRUISE: {
const bool headingLinedupToCruise = plane.mode_loiter.isHeadingLinedUp_cd(cruise_state.locked_heading_cd);
if (cruise_state.locked_heading && !headingLinedupToCruise && loiterStatus!=SoaringController::LoiterStatus::ALT_TOO_LOW && timer<20e3) {
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;
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);
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();
// Determine appropriate mode.
Mode* exit_mode = previous_mode;

if (loiterStatus == SoaringController::LoiterStatus::ALT_TOO_LOW &&
((previous_mode == &mode_cruise) || (previous_mode == &mode_fbwb))) {
exit_mode = &mode_rtl;
}

// Print message and set mode.
switch (loiterStatus) {
case SoaringController::LoiterStatus::ALT_TOO_HIGH:
soaring_restore_mode("Too high", ModeReason::SOARING_ALT_TOO_HIGH, *exit_mode);
break;
} // case Cruise

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 = plane.mode_loiter.isHeadingLinedUp(next_WP_loc, current_nav_cmd.content.location);
if (!headingLinedupToWP && loiterStatus!=SoaringController::LoiterStatus::ALT_TOO_LOW && timer<20e3) {
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;
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;
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;
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
case SoaringController::LoiterStatus::ALT_TOO_LOW:
soaring_restore_mode("Too low", ModeReason::SOARING_ALT_TOO_LOW, *exit_mode);
break;
default:
case SoaringController::LoiterStatus::THERMAL_WEAK:
soaring_restore_mode("Thermal ended", ModeReason::SOARING_THERMAL_ESTIMATE_DETERIORATED, *exit_mode);
break;
} // case AUTO
default: // all other modes
case SoaringController::LoiterStatus::DRIFT_EXCEEDED:
soaring_restore_mode("Drifted too far", ModeReason::SOARING_DRIFT_EXCEEDED, *exit_mode);
break;
} // switch previous_mode
} // switch loiterStatus

break;
} // case loiter

} // case loiter
} // switch control_mode
}


if (control_mode == &mode_loiter) {
// still in thermal - need to update the wp location
g2.soaring_controller.get_target(next_WP_loc);

bool Plane::soaring_exit_heading_aligned() const
{
// Return true if the current heading is aligned with the next objective.
// If home is not set, or heading not locked, return true to avoid delaying mode change.
switch (previous_mode->mode_number()) {
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();;
return plane.mode_loiter.isHeadingLinedUp(next_WP_loc, current_nav_cmd.content.location);
}
case Mode::Number::FLY_BY_WIRE_B:
return (!AP::ahrs().home_is_set() || plane.mode_loiter.isHeadingLinedUp(next_WP_loc, AP::ahrs().get_home()));
case Mode::Number::CRUISE:
return (!cruise_state.locked_heading || plane.mode_loiter.isHeadingLinedUp_cd(cruise_state.locked_heading_cd));
default:
break;
}
return true;
}

void Plane::soaring_restore_mode(const char *reason, ModeReason modereason, Mode &exit_mode)
{
gcs().send_text(MAV_SEVERITY_INFO, "Soaring: %s, restoring %s", reason, exit_mode.name());
set_mode(exit_mode, modereason);
}

#endif // SOARING_ENABLED

0 comments on commit 84d5621

Please sign in to comment.