move flight termination and geofence flags from setpoint triplet to mission result

7 files changed, 41 insertions, 40 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index ddfbd65a1..f05f970e5 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1328,15 +1328,6 @@ int commander_thread_main(int argc, char *argv[])
 
 		if (updated) {
 			orb_copy(ORB_ID(position_setpoint_triplet), pos_sp_triplet_sub, &pos_sp_triplet);
-
-			/* Check for geofence violation */
-			if (armed.armed && (pos_sp_triplet.geofence_violated || pos_sp_triplet.flight_termination)) {
-				//XXX: make this configurable to select different actions (e.g. navigation modes)
-				/* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */
-				armed.force_failsafe = true;
-				status_changed = true;
-				warnx("Flight termination because of navigator request or geofence");
-			} // no reset is done here on purpose, on geofence violation we want to stay in flighttermination
 		}
 
 		if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
@@ -1429,6 +1420,15 @@ int commander_thread_main(int argc, char *argv[])
 
 		if (updated) {
 			orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result);
+
+			/* Check for geofence violation */
+			if (armed.armed && (mission_result.geofence_violated || mission_result.flight_termination)) {
+				//XXX: make this configurable to select different actions (e.g. navigation modes)
+				/* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */
+				armed.force_failsafe = true;
+				status_changed = true;
+				warnx("Flight termination because of navigator request or geofence");
+			} // no reset is done here on purpose, on geofence violation we want to stay in flighttermination
 		}
 
 		/* RC input check */
diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp
index 4e3d25840..66f1c8c73 100644
--- a/src/modules/navigator/datalinkloss.cpp
+++ b/src/modules/navigator/datalinkloss.cpp
@@ -154,7 +154,8 @@ DataLinkLoss::set_dll_item()
 	}
 	case DLL_STATE_TERMINATE: {
 		/* Request flight termination from the commander */
-		pos_sp_triplet->flight_termination = true;
+		_navigator->get_mission_result()->flight_termination = true;
+		_navigator->publish_mission_result();
 		warnx("not switched to manual: request flight termination");
 		pos_sp_triplet->previous.valid = false;
 		pos_sp_triplet->current.valid = false;
@@ -180,23 +181,25 @@ DataLinkLoss::advance_dll()
 	switch (_dll_state) {
 	case DLL_STATE_NONE:
 		/* Check the number of data link losses. If above home fly home directly */
-		if (!_param_skipcommshold.get()) {
-			/* if number of data link losses limit is not reached fly to comms hold wp */
-			if (_navigator->get_vstatus()->data_link_lost_counter > _param_numberdatalinklosses.get()) {
-				warnx("%d data link losses, limit is %d, fly to airfield home",
-						_navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get());
-				mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to home");
-				_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
-			} else {
+		/* if number of data link losses limit is not reached fly to comms hold wp */
+		if (_navigator->get_vstatus()->data_link_lost_counter > _param_numberdatalinklosses.get()) {
+			warnx("%d data link losses, limit is %d, fly to airfield home",
+					_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();
+			_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
+		} else {
+			if (!_param_skipcommshold.get()) {
 				warnx("fly to comms hold, datalink loss counter: %d", _navigator->get_vstatus()->data_link_lost_counter);
 				mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to comms hold");
 				_dll_state = DLL_STATE_FLYTOCOMMSHOLDWP;
+			} else {
+				/* comms hold wp not active, fly to airfield home directly */
+				warnx("Skipping comms hold wp. Flying directly to airfield home");
+				mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home, comms hold skipped");
+				_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
 			}
-		} else {
-			/* comms hold wp not active, fly to airfield home directly */
-			warnx("Skipping comms hold wp. Flying directly to airfield home");
-			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home, comms hold skipped");
-			_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
 		}
 		break;
 	case DLL_STATE_FLYTOCOMMSHOLDWP:
diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp
index f0bbbcf1c..cd55f60b0 100644
--- a/src/modules/navigator/gpsfailure.cpp
+++ b/src/modules/navigator/gpsfailure.cpp
@@ -140,7 +140,8 @@ GpsFailure::set_gpsf_item()
 	switch (_gpsf_state) {
 	case GPSF_STATE_TERMINATE: {
 		/* Request flight termination from the commander */
-		pos_sp_triplet->flight_termination = true;
+		_navigator->get_mission_result()->flight_termination = true;
+		_navigator->publish_mission_result();
 		warnx("gps fail: request flight termination");
 	}
 	default:
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 81ceaaca2..c173ecd50 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -395,11 +395,9 @@ Navigator::task_main()
 		if (have_geofence_position_data) {
 			bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter);
 			if (!inside) {
-				/* inform other apps via the sp triplet */
-				_pos_sp_triplet.geofence_violated = true;
-				if (_pos_sp_triplet.geofence_violated != true) {
-					_pos_sp_triplet_updated = true;
-				}
+				/* inform other apps via the mission result */
+				_mission_result.geofence_violated = true;
+				publish_mission_result();
 
 				/* Issue a warning about the geofence violation once */
 				if (!_geofence_violation_warning_sent) {
@@ -407,11 +405,9 @@ Navigator::task_main()
 					_geofence_violation_warning_sent = true;
 				}
 			} else {
-				/* inform other apps via the sp triplet */
-				_pos_sp_triplet.geofence_violated = false;
-				if (_pos_sp_triplet.geofence_violated != false) {
-					_pos_sp_triplet_updated = true;
-				}
+				/* inform other apps via the mission result */
+				_mission_result.geofence_violated = false;
+				publish_mission_result();
 				/* Reset the _geofence_violation_warning_sent field */
 				_geofence_violation_warning_sent = false;
 			}
diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp
index 54f1813f5..651e31184 100644
--- a/src/modules/navigator/rcloss.cpp
+++ b/src/modules/navigator/rcloss.cpp
@@ -127,7 +127,8 @@ RCLoss::set_rcl_item()
 	}
 	case RCL_STATE_TERMINATE: {
 		/* Request flight termination from the commander */
-		pos_sp_triplet->flight_termination = true;
+		_navigator->get_mission_result()->flight_termination = true;
+		_navigator->publish_mission_result();
 		warnx("rc not recovered: request flight termination");
 		pos_sp_triplet->previous.valid = false;
 		pos_sp_triplet->current.valid = false;
diff --git a/src/modules/uORB/topics/mission_result.h b/src/modules/uORB/topics/mission_result.h
index 65ddfb4ad..c7d25d1f0 100644
--- a/src/modules/uORB/topics/mission_result.h
+++ b/src/modules/uORB/topics/mission_result.h
@@ -55,9 +55,11 @@ struct mission_result_s
 {
 	unsigned seq_reached;		/**< Sequence of the mission item which has been reached */
 	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*/
+	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*/
+	bool geofence_violated;		/**< true if the geofence is violated */
+	bool flight_termination;	/**< true if the navigator demands a flight termination from the commander app */
 };
 
 /**
diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h
index e2b1525a2..ec2131abd 100644
--- a/src/modules/uORB/topics/position_setpoint_triplet.h
+++ b/src/modules/uORB/topics/position_setpoint_triplet.h
@@ -97,8 +97,6 @@ struct position_setpoint_triplet_s
 	struct position_setpoint_s next;
 
 	unsigned nav_state;				/**< report the navigation state */
-	bool geofence_violated;				/**< true if the geofence is violated */
-	bool flight_termination;			/**< true if the navigator demands a flight termination from the commander app */
 };
 
 /**