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

Rover: Fixed LOITER_UNLIM to be an active loiter

Browse Source 
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_unlimited(const AP_Mission::Mission_Command& cmd);
void do_loiter_time(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_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); bool verify_loiter_time(const AP_Mission::Mission_Command& cmd);
void do_wait_delay(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); 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(); return verify_RTL();
case MAV_CMD_NAV_LOITER_UNLIM: case MAV_CMD_NAV_LOITER_UNLIM:
return verify_loiter_unlim(); return verify_loiter_unlimited(cmd);
case MAV_CMD_NAV_LOITER_TIME: case MAV_CMD_NAV_LOITER_TIME:
return verify_loiter_time(cmd); 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 // this is the distance we travel past the waypoint - not there yet so 0 initially
distance_past_wp = 0; 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) void Rover::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
{ {
previously_reached_wp = false; active_loiter = true;
Location cmdloc = cmd.content.location; do_nav_wp(cmd);
location_sanitize(current_loc, cmdloc);
set_next_WP(cmdloc);
loiter_duration = 100; // an arbitrary large loiter time 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 // 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 // We should always go through the waypoint i.e. the above code
// first before we go past it but sometimes we don't. // first before we go past it but sometimes we don't.
if (location_passed_point(current_loc, prev_WP, next_WP)) { 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 // Check if this is the first time we have reached the waypoint even though we have gone past it
if (!previously_reached_wp) { if (!previously_reached_wp) {
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Reached waypoint #%i. Loiter for %i seconds", gcs_send_text_fmt(MAV_SEVERITY_INFO, "Reached waypoint #%i. Loiter for %i seconds",
@ -343,10 +344,11 @@ bool Rover::verify_RTL()
return false; 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 // Continually set loiter start time to now so it never finishes
loiter_start_time += loiter_duration; loiter_start_time += millis();
verify_nav_wp(cmd);
return false; return false;
} }