mirror of https://github.com/ArduPilot/ardupilot
Plane: RTL home when AUTO run with no mission
This commit is contained in:
parent
6b789cd711
commit
7f21e3885e
|
@ -1116,7 +1116,16 @@ static void update_GPS_10Hz(void)
|
|||
*/
|
||||
static void handle_auto_mode(void)
|
||||
{
|
||||
switch(mission.get_current_nav_cmd().id) {
|
||||
uint8_t nav_cmd_id;
|
||||
|
||||
// we should be either running a mission or RTLing home
|
||||
if (mission.state() == AP_Mission::MISSION_RUNNING) {
|
||||
mission.get_current_nav_cmd().id;
|
||||
}else{
|
||||
nav_cmd_id = auto_rtl_command.id;
|
||||
}
|
||||
|
||||
switch(nav_cmd_id) {
|
||||
case MAV_CMD_NAV_TAKEOFF:
|
||||
if (steer_state.hold_course_cd == -1) {
|
||||
// we don't yet have a heading to hold - just level
|
||||
|
|
|
@ -8,11 +8,11 @@ static bool verify_command(const AP_Mission::Mission_Command& cmd);
|
|||
static void update_commands(void)
|
||||
{
|
||||
if(control_mode == AUTO) {
|
||||
if(home_is_set == true && mission.num_commands() > 1) {
|
||||
if (home_is_set) {
|
||||
if(mission.state() == AP_Mission::MISSION_RUNNING) {
|
||||
mission.update();
|
||||
} else {
|
||||
// next_nav_command should have been set to MAV_CMD_NAV_LOITER_UNLIM by exit_mission
|
||||
// auto_rtl_command should have been set to MAV_CMD_NAV_LOITER_UNLIM by exit_mission
|
||||
verify_command(auto_rtl_command);
|
||||
}
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue