Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: restart landing by jumping to DO_LAND_START or decrement mission 

only takes effect on mode change
This commit is contained in:
Tom Pittenger 2015-05-09 09:36:40 -07:00 committed by Andrew Tridgell
parent 41508457e1
commit 81f8358705
3 changed files with 35 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduPlane/Plane.h
View File

@ -938,6 +938,7 @@ private:
bool verify_command_callback(const AP_Mission::Mission_Command& cmd);
void print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode);
void run_cli(AP_HAL::UARTDriver *port);
void restart_landing_sequence();
void log_init();
uint32_t millis() const;
uint32_t micros() const;

29
ArduPlane/landing.cpp
View File

@ -211,6 +211,35 @@ void Plane::setup_landing_glide_slope(void)
constrain_target_altitude_location(loc, prev_WP_loc);
}
/*
Restart a landing by first checking for a DO_LAND_START and
jump there. Otherwise decrement waypoint so we would re-start
from the top with same glide slope. Return true if successful.
*/
void Plane::restart_landing_sequence()
{
if (mission.get_current_nav_cmd().id != MAV_CMD_NAV_LAND) {
return;
}
uint16_t do_land_start_index = mission.get_landing_sequence_start();
uint16_t prev_cmd_with_wp_index = mission.get_prev_nav_cmd_with_wp_index();
if (do_land_start_index != 0 &&
mission.set_current_cmd(do_land_start_index))
{
// look for a DO_LAND_START and use that index
gcs_send_text_fmt(PSTR("Restarted landing via DO_LAND_START: %d"),do_land_start_index);
} else if (prev_cmd_with_wp_index != AP_MISSION_CMD_INDEX_NONE &&
mission.set_current_cmd(prev_cmd_with_wp_index))
{
// if a suitable navigation waypoint was just executed, one that contains lat/lng/alt, then
// repeat that cmd to restart the landing from the top of approach to repeat intended glide slope
gcs_send_text_fmt(PSTR("Restarted landing sequence at waypoint %d"), prev_cmd_with_wp_index);
}
}
/*
find the nearest landing sequence starting point (DO_LAND_START) and
switch to that mission item. Returns false if no DO_LAND_START

5
ArduPlane/system.cpp
View File

@ -490,6 +490,11 @@ void Plane::exit_mode(enum FlightMode mode)
if (mode == AUTO) {
if (mission.state() == AP_Mission::MISSION_RUNNING) {
mission.stop();
if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND)
{
restart_landing_sequence();
}
}
auto_state.started_flying_in_auto_ms = 0;
}