zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Plane: Refactor soaring.cpp.

c415-sdk
Samuel Tabor 5 years ago committed by Peter Barker
parent
d87986ecdd
commit
b2d63c5049
2 changed files with 84 additions and 118 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 4
      ArduPlane/Plane.h
  2. 198
      ArduPlane/soaring.cpp

4
ArduPlane/Plane.h
Unescape View File

@ -783,7 +783,7 @@ private: @@ -783,7 +783,7 @@ private:
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;
@ -1072,6 +1072,8 @@ private: @@ -1072,6 +1072,8 @@ private:
// 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

198
ArduPlane/soaring.cpp
Unescape View File

@ -66,156 +66,120 @@ void Plane::update_soaring() { @@ -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

Write Preview
Loading…
Cancel
Save