From f0ff914b626fbfb497143f80b19376efb524b9e1 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Fri, 19 Dec 2014 23:36:32 +0000 Subject: navigator: don't publish mission result immediately but only after every iteration of the navigator --- src/modules/navigator/datalinkloss.cpp | 8 ++++---- src/modules/navigator/gpsfailure.cpp | 2 +- src/modules/navigator/mission.cpp | 6 +++--- src/modules/navigator/navigator.h | 13 ++++++++----- src/modules/navigator/navigator_main.cpp | 6 ++++++ src/modules/navigator/navigator_mode.cpp | 2 +- src/modules/navigator/rcloss.cpp | 6 +++--- 7 files changed, 26 insertions(+), 17 deletions(-) (limited to 'src') diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index e789fd10d..87a6e023a 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -155,7 +155,7 @@ DataLinkLoss::set_dll_item() case DLL_STATE_TERMINATE: { /* Request flight termination from the commander */ _navigator->get_mission_result()->flight_termination = true; - _navigator->publish_mission_result(); + _navigator->set_mission_result_updated(); reset_mission_item_reached(); warnx("not switched to manual: request flight termination"); pos_sp_triplet->previous.valid = false; @@ -188,7 +188,7 @@ DataLinkLoss::advance_dll() _navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get()); mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to airfield home"); _navigator->get_mission_result()->stay_in_failsafe = true; - _navigator->publish_mission_result(); + _navigator->set_mission_result_updated(); reset_mission_item_reached(); _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; } else { @@ -209,7 +209,7 @@ DataLinkLoss::advance_dll() mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home"); _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; _navigator->get_mission_result()->stay_in_failsafe = true; - _navigator->publish_mission_result(); + _navigator->set_mission_result_updated(); reset_mission_item_reached(); break; case DLL_STATE_FLYTOAIRFIELDHOMEWP: @@ -217,7 +217,7 @@ DataLinkLoss::advance_dll() warnx("time is up, state should have been changed manually by now"); mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no manual control, terminating"); _navigator->get_mission_result()->stay_in_failsafe = true; - _navigator->publish_mission_result(); + _navigator->set_mission_result_updated(); reset_mission_item_reached(); break; case DLL_STATE_TERMINATE: diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp index cd55f60b0..e370796c0 100644 --- a/src/modules/navigator/gpsfailure.cpp +++ b/src/modules/navigator/gpsfailure.cpp @@ -141,7 +141,7 @@ GpsFailure::set_gpsf_item() case GPSF_STATE_TERMINATE: { /* Request flight termination from the commander */ _navigator->get_mission_result()->flight_termination = true; - _navigator->publish_mission_result(); + _navigator->set_mission_result_updated(); warnx("gps fail: request flight termination"); } default: diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 371480603..8de8faf9d 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -711,7 +711,7 @@ Mission::set_mission_item_reached() { _navigator->get_mission_result()->reached = true; _navigator->get_mission_result()->seq_reached = _current_offboard_mission_index; - _navigator->publish_mission_result(); + _navigator->set_mission_result_updated(); reset_mission_item_reached(); } @@ -721,7 +721,7 @@ Mission::set_current_offboard_mission_item() _navigator->get_mission_result()->reached = false; _navigator->get_mission_result()->finished = false; _navigator->get_mission_result()->seq_current = _current_offboard_mission_index; - _navigator->publish_mission_result(); + _navigator->set_mission_result_updated(); save_offboard_mission_state(); } @@ -730,5 +730,5 @@ void Mission::set_mission_finished() { _navigator->get_mission_result()->finished = true; - _navigator->publish_mission_result(); + _navigator->set_mission_result_updated(); } diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index a701e3f44..d9d911d9c 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -107,11 +107,6 @@ 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(); - /** * Publish the geofence result */ @@ -128,6 +123,7 @@ public: */ void set_can_loiter_at_sp(bool can_loiter) { _can_loiter_at_sp = can_loiter; } void set_position_setpoint_triplet_updated() { _pos_sp_triplet_updated = true; } + void set_mission_result_updated() { _mission_result_updated = true; } /** * Getters @@ -215,6 +211,7 @@ private: bool _can_loiter_at_sp; /**< flags if current position SP can be used to loiter */ bool _pos_sp_triplet_updated; /**< flags if position SP triplet needs to be published */ bool _pos_sp_triplet_published_invalid_once; /**< flags if position SP triplet has been published once to UORB */ + bool _mission_result_updated; /**< flags if mission result has seen an update */ control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */ control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */ @@ -280,6 +277,12 @@ private: */ void publish_position_setpoint_triplet(); + + /** + * Publish the mission result so commander and mavlink know what is going on + */ + void publish_mission_result(); + /* this class has ptr data members, so it should not be copied, * consequently the copy constructors are private. */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 0ab85d56e..6a255878b 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -139,6 +139,7 @@ Navigator::Navigator() : _can_loiter_at_sp(false), _pos_sp_triplet_updated(false), _pos_sp_triplet_published_invalid_once(false), + _mission_result_updated(false), _param_loiter_radius(this, "LOITER_RAD"), _param_acceptance_radius(this, "ACC_RAD"), _param_datalinkloss_obc(this, "DLL_OBC"), @@ -491,6 +492,11 @@ Navigator::task_main() _pos_sp_triplet_updated = false; } + if (_mission_result_updated) { + publish_mission_result(); + _mission_result_updated = false; + } + perf_end(_loop_perf); } warnx("exiting."); diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp index 3807c5ea8..2f322031c 100644 --- a/src/modules/navigator/navigator_mode.cpp +++ b/src/modules/navigator/navigator_mode.cpp @@ -65,7 +65,7 @@ NavigatorMode::run(bool active) { _first_run = false; /* Reset stay in failsafe flag */ _navigator->get_mission_result()->stay_in_failsafe = false; - _navigator->publish_mission_result(); + _navigator->set_mission_result_updated(); on_activation(); } else { diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp index 42392e739..a7cde6325 100644 --- a/src/modules/navigator/rcloss.cpp +++ b/src/modules/navigator/rcloss.cpp @@ -128,7 +128,7 @@ RCLoss::set_rcl_item() case RCL_STATE_TERMINATE: { /* Request flight termination from the commander */ _navigator->get_mission_result()->flight_termination = true; - _navigator->publish_mission_result(); + _navigator->set_mission_result_updated(); warnx("rc not recovered: request flight termination"); pos_sp_triplet->previous.valid = false; pos_sp_triplet->current.valid = false; @@ -162,7 +162,7 @@ RCLoss::advance_rcl() mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc loss, terminating"); _rcl_state = RCL_STATE_TERMINATE; _navigator->get_mission_result()->stay_in_failsafe = true; - _navigator->publish_mission_result(); + _navigator->set_mission_result_updated(); reset_mission_item_reached(); } break; @@ -171,7 +171,7 @@ RCLoss::advance_rcl() warnx("time is up, no RC regain, terminating"); mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RC not regained, terminating"); _navigator->get_mission_result()->stay_in_failsafe = true; - _navigator->publish_mission_result(); + _navigator->set_mission_result_updated(); reset_mission_item_reached(); break; case RCL_STATE_TERMINATE: -- cgit v1.2.3