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

Plane: move loiter heading init for auto into auto helper, and properly reset it in loiter mode enter

This commit is contained in:
Tom Pittenger 2019-06-05 13:24:15 -07:00 committed by Andrew Tridgell
parent fd4b7c823e
commit ec8f82422a
4 changed files with 10 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduPlane/commands_logic.cpp
View File

@ -1074,5 +1074,9 @@ bool Plane::verify_loiter_heading(bool init)
return true; return true;
} }
return plane.mode_loiter.headingLinedUp(init, next_WP_loc, next_nav_cmd.content.location); if (init) {
loiter.sum_cd = 0;
}
return plane.mode_loiter.headingLinedUp(next_WP_loc, next_nav_cmd.content.location);
} }

2
ArduPlane/mode.h
View File

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

7
ArduPlane/mode_loiter.cpp
View File

@ -7,6 +7,7 @@ bool ModeLoiter::_enter()
plane.auto_throttle_mode = true; plane.auto_throttle_mode = true;
plane.auto_navigation_mode = true; plane.auto_navigation_mode = true;
plane.do_loiter_at_location(); plane.do_loiter_at_location();
plane.loiter_angle_reset();
#if SOARING_ENABLED == ENABLED #if SOARING_ENABLED == ENABLED
if (plane.g2.soaring_controller.is_active() && plane.g2.soaring_controller.suppress_throttle()) { if (plane.g2.soaring_controller.is_active() && plane.g2.soaring_controller.suppress_throttle()) {
@ -25,7 +26,7 @@ void ModeLoiter::update()
plane.calc_throttle(); plane.calc_throttle();
} }
bool ModeLoiter::headingLinedUp(const bool init, const Location loiterCenterLoc, const Location targetLoc) bool ModeLoiter::headingLinedUp(const Location loiterCenterLoc, const Location targetLoc)
{ {
const uint16_t loiterRadius = abs(plane.aparm.loiter_radius); const uint16_t loiterRadius = abs(plane.aparm.loiter_radius);
if (loiterCenterLoc.get_distance(targetLoc) < loiterRadius + loiterRadius*0.05) { if (loiterCenterLoc.get_distance(targetLoc) < loiterRadius + loiterRadius*0.05) {
@ -44,10 +45,6 @@ bool ModeLoiter::headingLinedUp(const bool init, const Location loiterCenterLoc,
const int32_t heading_err_cd = wrap_180_cd(bearing_cd - heading_cd); const int32_t heading_err_cd = wrap_180_cd(bearing_cd - heading_cd);
if (init) {
plane.loiter.sum_cd = 0;
}
/* /*
Check to see if the the plane is heading toward the land Check to see if the the plane is heading toward the land
waypoint. We use 20 degrees (+/-10 deg) of margin so that waypoint. We use 20 degrees (+/-10 deg) of margin so that

4
ArduPlane/soaring.cpp
View File

@ -79,7 +79,7 @@ void Plane::update_soaring() {
switch (loiterStatus) { switch (loiterStatus) {
case SoaringController::LoiterStatus::ALT_TOO_HIGH: { case SoaringController::LoiterStatus::ALT_TOO_HIGH: {
const bool homeIsSet = AP::ahrs().home_is_set(); const bool homeIsSet = AP::ahrs().home_is_set();
const bool headingLinedupToHome = homeIsSet && plane.mode_loiter.headingLinedUp(false, next_WP_loc, AP::ahrs().get_home()); const bool headingLinedupToHome = homeIsSet && plane.mode_loiter.headingLinedUp(next_WP_loc, AP::ahrs().get_home());
if (!homeIsSet || headingLinedupToHome) { if (!homeIsSet || headingLinedupToHome) {
gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooHigh, mode_fbwb.name()); gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooHigh, mode_fbwb.name());
set_mode(mode_fbwb, ModeReason::SOARING_ALT_TOO_HIGH); set_mode(mode_fbwb, ModeReason::SOARING_ALT_TOO_HIGH);
@ -137,7 +137,7 @@ void Plane::update_soaring() {
//Get the lat/lon of next Nav waypoint after this one: //Get the lat/lon of next Nav waypoint after this one:
AP_Mission::Mission_Command next_nav_cmd; 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 nextWpisValid = mission.get_next_nav_cmd(mission.get_current_nav_index() + 1, next_nav_cmd);
const bool headingLinedupToWP = nextWpisValid && plane.mode_loiter.headingLinedUp(false, next_WP_loc, next_nav_cmd.content.location); const bool headingLinedupToWP = nextWpisValid && plane.mode_loiter.headingLinedUp(next_WP_loc, next_nav_cmd.content.location);
if (!nextWpisValid || headingLinedupToWP) { if (!nextWpisValid || headingLinedupToWP) {
gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooWeak, mode_auto.name()); gcs().send_text(MAV_SEVERITY_INFO, "%s%s", strTooWeak, mode_auto.name());
set_mode(mode_auto, ModeReason::SOARING_THERMAL_ESTIMATE_DETERIORATED); set_mode(mode_auto, ModeReason::SOARING_THERMAL_ESTIMATE_DETERIORATED);