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

Plane: Soaring, don't wait for heading if too low, and add timeout otherwise.

This commit is contained in:
Samuel Tabor 2019-06-10 18:07:24 +01:00 committed by Andrew Tridgell
parent 684ee11fc3
commit 298088268a
2 changed files with 10 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduPlane/Plane.h
View File

@ -757,6 +757,9 @@ private:
// rudder mixing gain for differential thrust (0 - 1)
float rudder_dt;
// soaring mode-change timer
uint32_t soaring_mode_timer;
void adjust_nav_pitch_throttle(void);
void update_load_factor(void);
void send_fence_status(mavlink_channel_t chan);

10
ArduPlane/soaring.cpp
View File

@ -70,9 +70,13 @@ void Plane::update_soaring() {
// 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();
break;
}
// Some other loiter status, we need to think about exiting loiter.
uint32_t timer = AP_HAL::millis() - plane.soaring_mode_timer;
const char* strTooHigh = "Soaring: Too high, restoring ";
const char* strTooLow = "Soaring: Too low, restoring ";
const char* strTooWeak = "Soaring: Thermal ended, restoring ";
@ -83,7 +87,7 @@ void Plane::update_soaring() {
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) {
if (homeIsSet && !headingLinedupToHome && loiterStatus!=SoaringController::LoiterStatus::ALT_TOO_LOW && timer<20e3) {
break;
}
switch (loiterStatus) {
@ -110,7 +114,7 @@ void Plane::update_soaring() {
case Mode::Number::CRUISE: {
const bool headingLinedupToCruise = plane.mode_loiter.isHeadingLinedUp(cruise_state.locked_heading_cd);
if (cruise_state.locked_heading && !headingLinedupToCruise) {
if (cruise_state.locked_heading && !headingLinedupToCruise && loiterStatus!=SoaringController::LoiterStatus::ALT_TOO_LOW && timer<20e3) {
break;
}
// return to cruise with old ground course
@ -143,7 +147,7 @@ void Plane::update_soaring() {
//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) {
if (!headingLinedupToWP && loiterStatus!=SoaringController::LoiterStatus::ALT_TOO_LOW && timer<20e3) {
break;
}
switch (loiterStatus) {