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

Rover: Fixed LOITER_UNLIM to be an active loiter 

This changes brings the LOITER commands in line so both LOITER_UNLIM
and LOITER_TIME are actively loitering.
This commit is contained in:
Grant Morphett 2016-11-12 13:55:37 +11:00
parent 4b04c6d46b
commit 5b411aef65
2 changed files with 13 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
APMrover2/Rover.h
View File

@ -526,7 +526,7 @@ private:
void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd);
void do_loiter_time(const AP_Mission::Mission_Command& cmd);
bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);
bool verify_loiter_unlim();
bool verify_loiter_unlimited(const AP_Mission::Mission_Command& cmd);
bool verify_loiter_time(const AP_Mission::Mission_Command& cmd);
void do_wait_delay(const AP_Mission::Mission_Command& cmd);
void do_within_distance(const AP_Mission::Mission_Command& cmd);

22
APMrover2/commands_logic.cpp
View File

@ -169,7 +169,7 @@ bool Rover::verify_command(const AP_Mission::Mission_Command& cmd)
return verify_RTL();
case MAV_CMD_NAV_LOITER_UNLIM:
return verify_loiter_unlim();
return verify_loiter_unlimited(cmd);
case MAV_CMD_NAV_LOITER_TIME:
return verify_loiter_time(cmd);
@ -229,17 +229,16 @@ void Rover::do_nav_wp(const AP_Mission::Mission_Command& cmd)
// this is the distance we travel past the waypoint - not there yet so 0 initially
distance_past_wp = 0;
set_next_WP(cmd.content.location);
Location cmdloc = cmd.content.location;
location_sanitize(current_loc, cmdloc);
set_next_WP(cmdloc);
}
void Rover::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
{
previously_reached_wp = false;
Location cmdloc = cmd.content.location;
location_sanitize(current_loc, cmdloc);
set_next_WP(cmdloc);
active_loiter = true;
do_nav_wp(cmd);
loiter_duration = 100; // an arbitrary large loiter time
distance_past_wp = 0;
}
// do_loiter_time - initiate loitering at a point for a given time period
@ -290,6 +289,8 @@ bool Rover::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
// 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)) {
// 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
if (!previously_reached_wp) {
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Reached waypoint #%i. Loiter for %i seconds",
@ -343,10 +344,11 @@ bool Rover::verify_RTL()
return false;
}
bool Rover::verify_loiter_unlim()
bool Rover::verify_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
{
// Continually increase the loiter time so it never finishes
loiter_start_time += loiter_duration;
// Continually set loiter start time to now so it never finishes
loiter_start_time += millis();
verify_nav_wp(cmd);
return false;
}