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

Plane: Change mode to RTL on end of mission rather then staying in auto

Browse Source 
Deleted commands_process as it had 2 lines of useful code left, and was cleaner to move the remaining two lines into the caller case
This commit is contained in:
Michael du Breuil 2016-04-23 23:08:03 -07:00 committed by Andrew Tridgell
parent a9940a8ca5
commit 731c68f273
4 changed files with 13 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
ArduPlane/ArduPlane.cpp
View File

@ -487,13 +487,15 @@ void Plane::handle_auto_mode(void)
{ {
uint16_t nav_cmd_id; uint16_t nav_cmd_id;
// we should be either running a mission or RTLing home if (mission.state() != AP_Mission::MISSION_RUNNING) {
if (mission.state() == AP_Mission::MISSION_RUNNING) { // this should never be reached
nav_cmd_id = mission.get_current_nav_cmd().id; set_mode(RTL);
}else{ GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Aircraft in auto without a running mission");
nav_cmd_id = auto_rtl_command.id; return;
} }
nav_cmd_id = mission.get_current_nav_cmd().id;
if (quadplane.in_vtol_auto()) { if (quadplane.in_vtol_auto()) {
quadplane.control_auto(next_WP_loc); quadplane.control_auto(next_WP_loc);
} else if (nav_cmd_id == MAV_CMD_NAV_TAKEOFF || } else if (nav_cmd_id == MAV_CMD_NAV_TAKEOFF ||
@ -739,7 +741,9 @@ void Plane::update_navigation()
switch(control_mode) { switch(control_mode) {
case AUTO: case AUTO:
update_commands(); if (home_is_set != HOME_UNSET) {
mission.update();
}
break; break;
case RTL: case RTL:

4
ArduPlane/Plane.h
View File

@ -661,9 +661,6 @@ private:
// The location of the active waypoint in Guided mode. // The location of the active waypoint in Guided mode.
struct Location guided_WP_loc {}; struct Location guided_WP_loc {};
// special purpose command used only after mission completed to return vehicle to home or rally point
struct AP_Mission::Mission_Command auto_rtl_command;
// Altitude control // Altitude control
struct { struct {
// target altitude above sea level in cm. Used for barometric // target altitude above sea level in cm. Used for barometric
@ -854,7 +851,6 @@ private:
bool verify_loiter_heading(bool init); bool verify_loiter_heading(bool init);
void log_picture(); void log_picture();
void exit_mission_callback(); void exit_mission_callback();
void update_commands(void);
void mavlink_delay(uint32_t ms); void mavlink_delay(uint32_t ms);
void read_control_switch(); void read_control_switch();
uint8_t readSwitch(void); uint8_t readSwitch(void);

16
ArduPlane/commands_logic.cpp
View File

@ -607,10 +607,7 @@ bool Plane::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
bool Plane::verify_loiter_unlim() bool Plane::verify_loiter_unlim()
{ {
if (control_mode == AUTO && mission.state() != AP_Mission::MISSION_RUNNING) { if (mission.get_current_nav_cmd().p1 <= 1 && abs(g.rtl_radius) > 1) {
// end of mission RTL
update_loiter(g.rtl_radius? g.rtl_radius : g.loiter_radius);
} else if (mission.get_current_nav_cmd().p1 <= 1 && abs(g.rtl_radius) > 1) {
// if mission radius is 0,1, and rtl_radius is valid, use rtl_radius. // if mission radius is 0,1, and rtl_radius is valid, use rtl_radius.
loiter.direction = (g.rtl_radius < 0) ? -1 : 1; loiter.direction = (g.rtl_radius < 0) ? -1 : 1;
update_loiter(abs(g.rtl_radius)); update_loiter(abs(g.rtl_radius));
@ -1008,15 +1005,8 @@ bool Plane::verify_command_callback(const AP_Mission::Mission_Command& cmd)
void Plane::exit_mission_callback() void Plane::exit_mission_callback()
{ {
if (control_mode == AUTO) { if (control_mode == AUTO) {
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Returning to HOME"); set_mode(RTL);
memset(&auto_rtl_command, 0, sizeof(auto_rtl_command)); gcs_send_text_fmt(MAV_SEVERITY_INFO, "Mission complete, changing mode to RTL");
auto_rtl_command.content.location =
rally.calc_best_rally_or_home_location(current_loc, get_RTL_altitude());
auto_rtl_command.id = MAV_CMD_NAV_LOITER_UNLIM;
setup_terrain_target_alt(auto_rtl_command.content.location);
setup_glide_slope();
setup_turn_angle();
start_command(auto_rtl_command);
} }
} }

20
ArduPlane/commands_process.cpp
View File

@ -1,20 +0,0 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Plane.h"
// called by update navigation at 10Hz
// --------------------
void Plane::update_commands(void)
{
if(control_mode == AUTO) {
if (home_is_set != HOME_UNSET) {
if(mission.state() == AP_Mission::MISSION_RUNNING) {
mission.update();
} else {
// auto_rtl_command should have been set to MAV_CMD_NAV_LOITER_UNLIM by exit_mission
verify_command(auto_rtl_command);
}
}
}
}