Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 23:33:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: refactor loiter heading exit logic

Browse Source
This commit is contained in:
Tom Pittenger 2019-06-05 11:25:40 -07:00 committed by Andrew Tridgell
parent f6396d7a6d
commit f397c168e8
3 changed files with 51 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
ArduPlane/commands_logic.cpp
View File

@ -1074,47 +1074,5 @@ bool Plane::verify_loiter_heading(bool init)
return true;
}
if (next_WP_loc.get_distance(next_nav_cmd.content.location) < abs(aparm.loiter_radius)) {
/* Whenever next waypoint is within the loiter radius,
maintaining loiter would prevent us from ever pointing toward the next waypoint.
Hence break out of loiter immediately
*/
return true;
}
// Bearing in degrees
int32_t bearing_cd = current_loc.get_bearing_to(next_nav_cmd.content.location);
// get current heading.
int32_t heading_cd = gps.ground_course_cd();
int32_t heading_err_cd = wrap_180_cd(bearing_cd - heading_cd);
if (init) {
loiter.sum_cd = 0;
}
/*
Check to see if the the plane is heading toward the land
waypoint. We use 20 degrees (+/-10 deg) of margin so that
we can handle 200 degrees/second of yaw.
After every full circle, extend acceptance criteria to ensure
aircraft will not loop forever in case high winds are forcing
it beyond 200 deg/sec when passing the desired exit course
*/
// Use integer division to get discrete steps
int32_t expanded_acceptance = 1000 * (loiter.sum_cd / 36000);
if (labs(heading_err_cd) <= 1000 + expanded_acceptance) {
// Want to head in a straight line from _here_ to the next waypoint instead of center of loiter wp
// 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location
if (next_WP_loc.loiter_xtrack) {
next_WP_loc = current_loc;
}
return true;
}
return false;
return plane.mode_loiter.headingLinedUp(init, next_WP_loc, next_nav_cmd.content.location);
}

3
ArduPlane/mode.h
View File

@ -3,6 +3,7 @@
#include <AP_Param/AP_Param.h>
#include <AP_Common/Location.h>
#include <stdint.h>
#include <AP_Common/Location.h>
class Mode
{
@ -172,6 +173,8 @@ public:
// methods that affect movement of the vehicle in this mode
void update() override;
bool headingLinedUp(const bool init, const Location loiterCenterLoc, const Location targetLoc);
protected:
bool _enter() override;

47
ArduPlane/mode_loiter.cpp
View File

@ -25,3 +25,50 @@ void ModeLoiter::update()
plane.calc_throttle();
}
bool ModeLoiter::headingLinedUp(const bool init, const Location loiterCenterLoc, const Location targetLoc)
{
const uint16_t loiterRadius = abs(plane.aparm.loiter_radius);
if (loiterCenterLoc.get_distance(targetLoc) < loiterRadius + loiterRadius*0.05) {
/* Whenever next waypoint is within the loiter radius plus 5%,
maintaining loiter would prevent us from ever pointing toward the next waypoint.
Hence break out of loiter immediately
*/
return true;
}
// Bearing in degrees
const int32_t bearing_cd = plane.current_loc.get_bearing_to(targetLoc);
// get current heading.
const int32_t heading_cd = plane.gps.ground_course_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
waypoint. We use 20 degrees (+/-10 deg) of margin so that
we can handle 200 degrees/second of yaw.
After every full circle, extend acceptance criteria to ensure
aircraft will not loop forever in case high winds are forcing
it beyond 200 deg/sec when passing the desired exit course
*/
// Use integer division to get discrete steps
const int32_t expanded_acceptance = 1000 * (plane.loiter.sum_cd / 36000);
if (labs(heading_err_cd) <= 1000 + expanded_acceptance) {
// Want to head in a straight line from _here_ to the next waypoint instead of center of loiter wp
// 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location
if (plane.next_WP_loc.loiter_xtrack) {
plane.next_WP_loc = plane.current_loc;
}
return true;
}
return false;
}