aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Rover: Fixed a bug in the wp loiter functionality

Browse Source 
Had a bug where the rover would loiter for ever and not resume even
though its loiter time had expired.  This resolves that.
This commit is contained in:
Grant Morphett 2016-12-20 14:31:42 +11:00
parent 9569ed94a9
commit 378bd84b29
2 changed files with 8 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
APMrover2/Steering.cpp
View File

@ -83,10 +83,11 @@ bool Rover::in_stationary_loiter()
// Confirm we are in AUTO mode and need to loiter for a time period
if ((loiter_start_time > 0) && (control_mode == AUTO)) {
// Check if active loiter is enabled AND we are outside the waypoint loiter radius
// then NOT the result for the if logic
if (!(active_loiter && (wp_distance > g.waypoint_radius))) {
return true;
// then the vehicle still needs to move so return false
if (active_loiter && (wp_distance > g.waypoint_radius)) {
return false;
}
return true;
}
return false;

5
APMrover2/commands_logic.cpp
View File

@ -288,7 +288,10 @@ bool Rover::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
// have we gone past the waypoint?
// We should always go through the waypoint i.e. the above code
// first before we go past it but sometimes we don't.
if (location_passed_point(current_loc, prev_WP, next_WP)) {
// OR have we reached the waypoint previously be we aren't actively loitering
// This second check is required for when we roll past the waypoint radius
if (location_passed_point(current_loc, prev_WP, next_WP) ||
(!active_loiter && previously_reached_wp)) {
// As we have passed the waypoint navigation needs to be done from current location
prev_WP = current_loc;
// Check if this is the first time we have reached the waypoint even though we have gone past it