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

Rover: Implemented MAV_CMD_NAV_LOITER_UNLIM

Browse Source
This commit is contained in:
Grant Morphett 2016-07-14 11:34:22 +10:00
parent 32d5e6162a
commit d1799884b2
2 changed files with 25 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
APMrover2/Rover.h
View File

@ -502,7 +502,9 @@ private:
bool verify_command(const AP_Mission::Mission_Command& cmd);
bool verify_command_callback(const AP_Mission::Mission_Command& cmd);
void do_nav_wp(const AP_Mission::Mission_Command& cmd);
void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd);
bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);
bool verify_loiter_unlim();
void do_wait_delay(const AP_Mission::Mission_Command& cmd);
void do_within_distance(const AP_Mission::Mission_Command& cmd);
void do_change_speed(const AP_Mission::Mission_Command& cmd);

23
APMrover2/commands_logic.cpp
View File

@ -31,6 +31,10 @@ bool Rover::start_command(const AP_Mission::Mission_Command& cmd)
do_RTL();
break;
case MAV_CMD_NAV_LOITER_UNLIM: // Loiter indefinitely
do_loiter_unlimited(cmd);
break;
// Conditional commands
case MAV_CMD_CONDITION_DELAY:
do_wait_delay(cmd);
@ -153,6 +157,9 @@ bool Rover::verify_command(const AP_Mission::Mission_Command& cmd)
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
return verify_RTL();
case MAV_CMD_NAV_LOITER_UNLIM:
return verify_loiter_unlim();
case MAV_CMD_CONDITION_DELAY:
return verify_wait_delay();
@ -194,6 +201,15 @@ void Rover::do_nav_wp(const AP_Mission::Mission_Command& cmd)
set_next_WP(cmd.content.location);
}
void Rover::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
{
Location cmdloc = cmd.content.location;
location_sanitize(current_loc, cmdloc);
set_next_WP(cmdloc);
loiter_time_max = 100; // an arbitrary large loiter time
distance_past_wp = 0;
}
/********************************************************************************/
// Verify Nav (Must) commands
/********************************************************************************/
@ -264,6 +280,13 @@ bool Rover::verify_RTL()
return false;
}
bool Rover::verify_loiter_unlim()
{
// Continually increase the loiter time so it never finishes
loiter_time += loiter_time_max;
return false;
}
/********************************************************************************/
// Condition (May) commands
/********************************************************************************/