From f17c5d8d55dc6c39276fde20062080b0e24ed75e Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 18 Dec 2015 11:24:21 +0100 Subject: [PATCH] fixing takeoff mission and swtiching to previous flight mode after land/takeoff --- src/modules/commander/commander.cpp | 20 ++++++++------------ src/modules/navigator/mission_block.cpp | 12 ++++++++++-- src/modules/navigator/takeoff.cpp | 3 ++- 3 files changed, 20 insertions(+), 15 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 91501082fc..e8520c62d2 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2352,20 +2352,16 @@ int commander_thread_main(int argc, char *argv[]) } } - /* reset main state after takeoff and land */ - if (status.main_state == vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF - && mission_result.finished) { + /* reset main state after takeoff or land has been completed */ + /* only switch back to at least altitude controlled modes */ + if (status.main_state_prev == vehicle_status_s::MAIN_STATE_POSCTL || + status.main_state_prev == vehicle_status_s::MAIN_STATE_ALTCTL) { - if (status.main_state_prev < vehicle_status_s::MAIN_STATE_MAX - && status.main_state_prev != vehicle_status_s::MAIN_STATE_AUTO_LAND) { - main_state_transition(&status, status.main_state_prev); - } + if ((status.main_state == vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF + && mission_result.finished) || + (status.main_state == vehicle_status_s::MAIN_STATE_AUTO_LAND + && status.condition_landed)) { - } else if (status.main_state == vehicle_status_s::MAIN_STATE_AUTO_LAND - && status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { - - if (status.main_state_prev < vehicle_status_s::MAIN_STATE_MAX - && status.main_state_prev != vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF) { main_state_transition(&status, status.main_state_prev); } } diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 7c55eccf57..1819337a4c 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -119,8 +119,16 @@ MissionBlock::is_mission_item_reached() if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _navigator->get_vstatus()->is_rotary_wing) { /* require only altitude for takeoff for multicopter */ + + /* _mission_item.acceptance_radius is not always set */ + float mission_acceptance_radius = _mission_item.acceptance_radius; + /* if set to zero use the default instead */ + if (mission_acceptance_radius < NAV_EPSILON_POSITION) { + mission_acceptance_radius = _navigator->get_acceptance_radius(); + } + if (_navigator->get_global_position()->alt > - altitude_amsl - _navigator->get_acceptance_radius()) { + altitude_amsl - mission_acceptance_radius) { _waypoint_position_reached = true; } } else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) { @@ -301,7 +309,7 @@ MissionBlock::set_loiter_item(struct mission_item_s *item, float min_clearance) void MissionBlock::set_takeoff_item(struct mission_item_s *item, float min_clearance, float min_pitch) { - item->nav_cmd = NAV_CMD_LOITER_UNLIMITED; + item->nav_cmd = NAV_CMD_TAKEOFF; /* use current position and use return altitude as clearance */ item->lat = _navigator->get_global_position()->lat; diff --git a/src/modules/navigator/takeoff.cpp b/src/modules/navigator/takeoff.cpp index 1cf8c95d3f..4b5dc49776 100644 --- a/src/modules/navigator/takeoff.cpp +++ b/src/modules/navigator/takeoff.cpp @@ -78,6 +78,7 @@ Takeoff::on_activation() _navigator->get_mission_result()->reached = false; _navigator->get_mission_result()->finished = false; _navigator->set_mission_result_updated(); + reset_mission_item_reached(); /* convert mission item to current setpoint */ struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); @@ -96,7 +97,7 @@ Takeoff::on_activation() void Takeoff::on_active() { - if (is_mission_item_reached()) { + if (is_mission_item_reached() && !_navigator->get_mission_result()->finished) { _navigator->get_mission_result()->finished = true; _navigator->set_mission_result_updated(); }