Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-03 04:03:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

Sub: Implement MAV_CMD_NAV_RETURN_TO_LAUNCH

Browse Source
This commit is contained in:
Jacob Walser 2017-03-11 11:06:03 -05:00
parent e23bb6702d
commit 0966dcb665
2 changed files with 18 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduSub/Sub.h
View File

@ -766,6 +766,7 @@ private:
bool do_guided(const AP_Mission::Mission_Command& cmd); bool do_guided(const AP_Mission::Mission_Command& cmd);
void do_nav_wp(const AP_Mission::Mission_Command& cmd); void do_nav_wp(const AP_Mission::Mission_Command& cmd);
void do_surface(const AP_Mission::Mission_Command& cmd); void do_surface(const AP_Mission::Mission_Command& cmd);
void do_RTL(void);
void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd); void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd);
void do_circle(const AP_Mission::Mission_Command& cmd); void do_circle(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);
@ -791,6 +792,7 @@ private:
#endif #endif
bool verify_nav_wp(const AP_Mission::Mission_Command& cmd); bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);
bool verify_surface(const AP_Mission::Mission_Command& cmd); bool verify_surface(const AP_Mission::Mission_Command& cmd);
bool verify_RTL(void);
bool verify_circle(const AP_Mission::Mission_Command& cmd); bool verify_circle(const AP_Mission::Mission_Command& cmd);
bool verify_spline_wp(const AP_Mission::Mission_Command& cmd); bool verify_spline_wp(const AP_Mission::Mission_Command& cmd);
#if NAV_GUIDED == ENABLED #if NAV_GUIDED == ENABLED

16
ArduSub/commands_logic.cpp
View File

@ -23,6 +23,10 @@ bool Sub::start_command(const AP_Mission::Mission_Command& cmd)
do_surface(cmd); do_surface(cmd);
break; break;
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
do_RTL();
break;
case MAV_CMD_NAV_LOITER_UNLIM: // 17 Loiter indefinitely case MAV_CMD_NAV_LOITER_UNLIM: // 17 Loiter indefinitely
do_loiter_unlimited(cmd); do_loiter_unlimited(cmd);
break; break;
@ -179,6 +183,9 @@ bool Sub::verify_command(const AP_Mission::Mission_Command& cmd)
case MAV_CMD_NAV_LAND: case MAV_CMD_NAV_LAND:
return verify_surface(cmd); return verify_surface(cmd);
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
return verify_RTL();
case MAV_CMD_NAV_LOITER_UNLIM: case MAV_CMD_NAV_LOITER_UNLIM:
return verify_loiter_unlimited(); return verify_loiter_unlimited();
@ -323,6 +330,11 @@ void Sub::do_surface(const AP_Mission::Mission_Command& cmd)
auto_wp_start(target_location); auto_wp_start(target_location);
} }
void Sub::do_RTL()
{
auto_wp_start(ahrs.get_home());
}
// do_loiter_unlimited - start loitering with no end conditions // do_loiter_unlimited - start loitering with no end conditions
// note: caller should set yaw_mode // note: caller should set yaw_mode
void Sub::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd) void Sub::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
@ -606,6 +618,10 @@ bool Sub::verify_surface(const AP_Mission::Mission_Command& cmd)
return retval; return retval;
} }
bool Sub::verify_RTL() {
return wp_nav.reached_wp_destination();
}
bool Sub::verify_loiter_unlimited() bool Sub::verify_loiter_unlimited()
{ {
return false; return false;