From 3e87ec5153f0035ec151cc752b869a28354d68e2 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 4 Aug 2016 16:46:03 +0200 Subject: [PATCH] navigator: mission check refactor The mission feasability checker was called with the same arguments twice which made it hard to understand when a mission is marked valid. The mission check should run in these two cases: - When initializing (if home comes up) if there is already a mission saved. - When the mission gets updated. --- src/modules/navigator/mission.cpp | 59 ++++++++++++++----------------- src/modules/navigator/mission.h | 3 +- 2 files changed, 28 insertions(+), 34 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index a9e0b96c05..40f2837463 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -81,7 +81,6 @@ Mission::Mission(Navigator *navigator, const char *name) : _need_takeoff(true), _mission_type(MISSION_TYPE_NONE), _inited(false), - _home_inited(false), _need_mission_reset(false), _missionFeasibilityChecker(), _min_current_sp_distance_xy(FLT_MAX), @@ -100,6 +99,11 @@ Mission::~Mission() void Mission::on_inactive() { + /* Without home a mission can't be valid yet anyway, let's wait. */ + if (!_navigator->home_position_valid()) { + return; + } + if (_inited) { /* check if missions have changed so that feedback to ground station is given */ bool onboard_updated = false; @@ -138,11 +142,12 @@ Mission::on_inactive() _current_offboard_mission_index = mission_state.current_seq; } + /* On init let's check the mission, maybe there is already one available. */ + check_mission_valid(); + _inited = true; } - check_mission_valid(); - /* require takeoff after non-loiter or landing */ if (!_navigator->get_can_loiter_at_sp() || _navigator->get_land_detected()->landed) { _need_takeoff = true; @@ -274,24 +279,14 @@ Mission::update_offboard_mission() /* otherwise, just leave it */ } - /* Check mission feasibility, for now do not handle the return value, - * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ - dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); + check_mission_valid(); - failed = !_missionFeasibilityChecker.checkMissionFeasible(_navigator->get_mavlink_log_pub(), (_navigator->get_vstatus()->is_rotary_wing || _navigator->get_vstatus()->is_vtol), - dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), - _navigator->get_home_position()->alt, _navigator->home_position_valid(), - _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, - _param_dist_1wp.get(), _navigator->get_mission_result()->warning, _navigator->get_default_acceptance_radius(), - _navigator->get_land_detected()->landed); + failed = !_navigator->get_mission_result()->valid; - _navigator->get_mission_result()->valid = !failed; if (!failed) { /* reset mission failure if we have an updated valid mission */ _navigator->get_mission_result()->mission_failure = false; } - _navigator->increment_mission_instance_count(); - _navigator->set_mission_result_updated(); } else { PX4_WARN("offboard mission update failed, handle: %d", _navigator->get_offboard_mission_sub()); @@ -1131,28 +1126,28 @@ Mission::set_mission_finished() _navigator->set_mission_result_updated(); } -bool +void Mission::check_mission_valid() { - /* check if the home position became valid in the meantime */ - if (!_home_inited && _navigator->home_position_valid()) { - dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); + dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); - _navigator->get_mission_result()->valid = _missionFeasibilityChecker.checkMissionFeasible(_navigator->get_mavlink_log_pub(), (_navigator->get_vstatus()->is_rotary_wing || _navigator->get_vstatus()->is_vtol), - dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), - _navigator->get_home_position()->alt, _navigator->home_position_valid(), - _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, - _param_dist_1wp.get(), _navigator->get_mission_result()->warning, _navigator->get_default_acceptance_radius(), - _navigator->get_land_detected()->landed); + _navigator->get_mission_result()->valid = + _missionFeasibilityChecker.checkMissionFeasible( + _navigator->get_mavlink_log_pub(), + (_navigator->get_vstatus()->is_rotary_wing || _navigator->get_vstatus()->is_vtol), + dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), + _navigator->get_home_position()->alt, + _navigator->home_position_valid(), + _navigator->get_global_position()->lat, + _navigator->get_global_position()->lon, + _param_dist_1wp.get(), + _navigator->get_mission_result()->warning, + _navigator->get_default_acceptance_radius(), + _navigator->get_land_detected()->landed); - _navigator->increment_mission_instance_count(); - _navigator->set_mission_result_updated(); - - _home_inited = true; - } - - return _navigator->get_mission_result()->valid; + _navigator->increment_mission_instance_count(); + _navigator->set_mission_result_updated(); } void diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 2bcfa85508..cd43327550 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -211,7 +211,7 @@ private: /** * Check wether a mission is ready to go */ - bool check_mission_valid(); + void check_mission_valid(); /** * Reset offboard mission @@ -244,7 +244,6 @@ private: } _mission_type; bool _inited; - bool _home_inited; bool _need_mission_reset; MissionFeasibilityChecker _missionFeasibilityChecker; /**< class that checks if a mission is feasible */