From 24f380137ecb91fb9647e22e1d29c13da5fc0357 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 23 Jul 2014 22:58:19 +0200 Subject: [PATCH] add method to block fallback to mission failsafe navigation modes can use a flag in mission_result to tell the commander to not switch back to mission --- src/modules/commander/commander.cpp | 5 +-- .../commander/state_machine_helper.cpp | 3 +- src/modules/commander/state_machine_helper.h | 2 +- src/modules/navigator/datalinkloss.cpp | 2 ++ src/modules/navigator/mission.cpp | 32 ++++--------------- src/modules/navigator/mission.h | 8 ----- src/modules/navigator/navigator.h | 13 +++++++- src/modules/navigator/navigator_main.cpp | 22 +++++++++++++ src/modules/navigator/navigator_mode.cpp | 3 ++ src/modules/uORB/topics/mission_result.h | 1 + 10 files changed, 53 insertions(+), 38 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5c89e01238..cb09a68e31 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1461,7 +1461,7 @@ int commander_thread_main(int argc, char *argv[]) for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { if (telemetry_last_heartbeat[i] != 0 && hrt_elapsed_time(&telemetry_last_heartbeat[i]) < datalink_loss_timeout) { /* handle the case where data link was regained */ - if (telemetry_lost[i]) { + if (telemetry_lost[i]) {//XXX also add hysteresis here mavlink_log_critical(mavlink_fd, "data link %i regained", i); telemetry_lost[i] = false; } @@ -1545,7 +1545,8 @@ int commander_thread_main(int argc, char *argv[]) /* now set navigation state according to failsafe and main state */ bool nav_state_changed = set_nav_state(&status, (bool)datalink_loss_enabled, - mission_result.finished); + mission_result.finished, + mission_result.stay_in_failsafe); // TODO handle mode changes by commands if (main_state_changed) { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 7b26e3e8cb..4e1cfb9873 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -435,7 +435,8 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, /** * Check failsafe and main status and set navigation status for navigator accordingly */ -bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished) +bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, + const bool stay_in_failsafe) { navigation_state_t nav_state_old = status->nav_state; diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index bb1b87e712..4285d29777 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -63,7 +63,7 @@ transition_result_t main_state_transition(struct vehicle_status_s *current_state transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd); -bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished); +bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, const bool stay_in_failsafe); int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd); diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index a98e21139e..52f263aff0 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -200,6 +200,8 @@ DataLinkLoss::advance_dll() case DLL_STATE_FLYTOCOMMSHOLDWP: //XXX check here if time is over are over _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; + _navigator->get_mission_result()->stay_in_failsafe = true; + _navigator->publish_mission_result(); break; default: break; diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index ba766cd10f..7ce0e2f891 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -68,8 +68,6 @@ Mission::Mission(Navigator *navigator, const char *name) : _current_offboard_mission_index(-1), _need_takeoff(true), _takeoff(false), - _mission_result_pub(-1), - _mission_result({0}), _mission_type(MISSION_TYPE_NONE), _inited(false), _dist_1wp_ok(false) @@ -577,18 +575,18 @@ void Mission::report_mission_item_reached() { if (_mission_type == MISSION_TYPE_OFFBOARD) { - _mission_result.reached = true; - _mission_result.seq_reached = _current_offboard_mission_index; + _navigator->get_mission_result()->reached = true; + _navigator->get_mission_result()->seq_reached = _current_offboard_mission_index; } - publish_mission_result(); + _navigator->publish_mission_result(); } void Mission::report_current_offboard_mission_item() { warnx("current offboard mission index: %d", _current_offboard_mission_index); - _mission_result.seq_current = _current_offboard_mission_index; - publish_mission_result(); + _navigator->get_mission_result()->seq_current = _current_offboard_mission_index; + _navigator->publish_mission_result(); save_offboard_mission_state(); } @@ -596,23 +594,7 @@ Mission::report_current_offboard_mission_item() void Mission::report_mission_finished() { - _mission_result.finished = true; - publish_mission_result(); + _navigator->get_mission_result()->finished = true; + _navigator->publish_mission_result(); } -void -Mission::publish_mission_result() -{ - /* lazily publish the mission result only once available */ - if (_mission_result_pub > 0) { - /* publish mission result */ - orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result); - - } else { - /* advertise and publish */ - _mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result); - } - /* reset reached bool */ - _mission_result.reached = false; - _mission_result.finished = false; -} diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 4da6a11553..1b8c8c8740 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -128,11 +128,6 @@ private: */ void report_mission_finished(); - /** - * Publish the mission result so commander and mavlink know what is going on - */ - void publish_mission_result(); - control::BlockParamInt _param_onboard_enabled; control::BlockParamFloat _param_takeoff_alt; control::BlockParamFloat _param_dist_1wp; @@ -145,9 +140,6 @@ private: bool _need_takeoff; bool _takeoff; - orb_advert_t _mission_result_pub; - struct mission_result_s _mission_result; - enum { MISSION_TYPE_NONE, MISSION_TYPE_ONBOARD, diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index d0b2ed841f..363877bb83 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -52,6 +52,7 @@ #include #include #include +#include #include "navigator_mode.h" #include "mission.h" @@ -102,6 +103,11 @@ public: */ void load_fence_from_file(const char *filename); + /** + * Publish the mission result so commander and mavlink know what is going on + */ + void publish_mission_result(); + /** * Setters */ @@ -115,7 +121,9 @@ public: struct vehicle_control_mode_s* get_control_mode() { return &_control_mode; } struct vehicle_global_position_s* get_global_position() { return &_global_pos; } struct home_position_s* get_home_position() { return &_home_pos; } - struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; } + struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; } + struct mission_result_s* get_mission_result() { return &_mission_result; } + int get_onboard_mission_sub() { return _onboard_mission_sub; } int get_offboard_mission_sub() { return _offboard_mission_sub; } int get_offboard_control_sp_sub() { return _offboard_control_sp_sub; } @@ -143,6 +151,7 @@ private: int _param_update_sub; /**< param update subscription */ orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */ + orb_advert_t _mission_result_pub; vehicle_status_s _vstatus; /**< vehicle status */ vehicle_control_mode_s _control_mode; /**< vehicle control mode */ @@ -152,6 +161,8 @@ private: navigation_capabilities_s _nav_caps; /**< navigation capabilities */ position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */ + mission_result_s _mission_result; + bool _mission_item_valid; /**< flags if the current mission item is valid */ perf_counter_t _loop_perf; /**< loop performance counter */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 1ce6770c98..77124e8f65 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -108,6 +108,7 @@ Navigator::Navigator() : _offboard_mission_sub(-1), _param_update_sub(-1), _pos_sp_triplet_pub(-1), + _mission_result_pub(-1), _vstatus{}, _control_mode{}, _global_pos{}, @@ -115,6 +116,7 @@ Navigator::Navigator() : _mission_item{}, _nav_caps{}, _pos_sp_triplet{}, + _mission_result{}, _mission_item_valid(false), _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), _geofence{}, @@ -370,6 +372,9 @@ Navigator::task_main() _can_loiter_at_sp = false; break; case NAVIGATION_STATE_AUTO_MISSION: + /* Some failsafe modes prohibit the fallback to mission + * usually this is done after some time to make sure + * that the full failsafe operation is performed */ _navigation_mode = &_mission; break; case NAVIGATION_STATE_AUTO_LOITER: @@ -553,3 +558,20 @@ int navigator_main(int argc, char *argv[]) return 0; } + +void +Navigator::publish_mission_result() +{ + /* lazily publish the mission result only once available */ + if (_mission_result_pub > 0) { + /* publish mission result */ + orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result); + + } else { + /* advertise and publish */ + _mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result); + } + /* reset reached bool */ + _mission_result.reached = false; + _mission_result.finished = false; +} diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp index f432156652..3c6754c556 100644 --- a/src/modules/navigator/navigator_mode.cpp +++ b/src/modules/navigator/navigator_mode.cpp @@ -63,6 +63,9 @@ NavigatorMode::run(bool active) { if (_first_run) { /* first run */ _first_run = false; + /* Reset stay in failsafe flag */ + _navigator->get_mission_result()->stay_in_failsafe = true; + _navigator->publish_mission_result(); on_activation(); } else { diff --git a/src/modules/uORB/topics/mission_result.h b/src/modules/uORB/topics/mission_result.h index beb797e624..65ddfb4ad4 100644 --- a/src/modules/uORB/topics/mission_result.h +++ b/src/modules/uORB/topics/mission_result.h @@ -57,6 +57,7 @@ struct mission_result_s unsigned seq_current; /**< Sequence of the current mission item */ bool reached; /**< true if mission has been reached */ bool finished; /**< true if mission has been completed */ + bool stay_in_failsafe; /**< true if the commander should not switch out of the failsafe mode*/ }; /**