commander: internalize system status bools

Most condition bools in the commander are not used anywhere but in the
commander. It therefore makes sense to move them to a different internal
struct and remove them from the vehicle_status message.

Also, the land_detected should be used by all the modules instead of
getting it through the commander and system_status.

msg/vehicle_status.msg

@@ -74,21 +74,7 @@ bool is_vtol				# True if the system is VTOL capable
 bool vtol_fw_permanent_stab		# True if vtol should stabilize attitude for fw in manual mode
 bool in_transition_mode			# True if VTOL is doing a transition
 
-bool condition_system_in_air_restore		# true if we can restore in mid air
+float32 avionics_power_rail_voltage	# voltage of the avionics power rail
-bool condition_system_sensors_initialized
-bool condition_system_prearm_error_reported	# true if errors have already been reported
-bool condition_system_hotplug_timeout		# true if the hotplug sensor search is over
-bool condition_system_returned_to_home
-bool condition_auto_mission_available
-bool condition_global_position_valid		# set to true by the commander app if the quality of the position estimate is good enough to use it for navigation
-bool condition_home_position_valid		# indicates a valid home position (a valid home position is not always a valid launch)
-bool condition_local_position_valid
-bool condition_local_altitude_valid
-bool condition_airspeed_valid			# set to true by the commander app if there is a valid airspeed measurement available
-bool condition_landed				# true if vehicle is landed, always true if disarmed
-bool condition_power_input_valid		# set if input power is valid
-float32 avionics_power_rail_voltage		# voltage of the avionics power rail
-bool usb_connected				# status of the USB power supply
 
 bool rc_signal_found_once
 bool rc_signal_lost				# true if RC reception lost

src/modules/commander/state_machine_helper.cpp

@@ -109,17 +109,13 @@ static const char * const state_names[vehicle_status_s::ARMING_STATE_MAX] = {
 static hrt_abstime last_preflight_check = 0;	///< initialize so it gets checked immediately
 static int last_prearm_ret = 1;			///< initialize to fail
 
-transition_result_t
+transition_result_t arming_state_transition(struct vehicle_status_s *status,
-arming_state_transition(struct vehicle_status_s *status,		///< current vehicle status
+					    const struct safety_s *safety,
-			const struct safety_s   *safety,		///< current safety settings
+					    arming_state_t new_arming_state,
-			arming_state_t          new_arming_state,	///< arming state requested
+					    struct actuator_armed_s *armed,
-			struct actuator_armed_s *armed,			///< current armed status
+					    bool fRunPreArmChecks,
-			bool			fRunPreArmChecks,	///< true: run the pre-arm checks, false: no pre-arm checks, for unit testing
+					    orb_advert_t            *mavlink_log_pub,	///< uORB handle for mavlink log
-			orb_advert_t            *mavlink_log_pub)	///< uORB handle for mavlink log
+					    status_flags_s *status_flags)
-			bool circuit_breaker_engaged_airspd_check,
-			bool circuit_breaker_engaged_gpsfailure_check,
-			bool circuit_breaker_engaged_power_check,
-			bool cb_usb)
 {
 	// Double check that our static arrays are still valid
 	ASSERT(vehicle_status_s::ARMING_STATE_INIT == 0);
@@ -145,22 +141,18 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 				&& status->hil_state == vehicle_status_s::HIL_STATE_OFF) {
 
 			prearm_ret = preflight_check(status, mavlink_log_pub, true /* pre-arm */, false /* force_report */,
-						     circuit_breaker_engaged_airspd_check,
+						     status_flags);
-						     circuit_breaker_engaged_gpsfailure_check,
-						     cb_usb);
 		}
 		/* re-run the pre-flight check as long as sensors are failing */
-		if (!status->condition_system_sensors_initialized
+		if (!status_flags->condition_system_sensors_initialized
 				&& (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED
 				|| new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY)
 				&& status->hil_state == vehicle_status_s::HIL_STATE_OFF) {
 
 			if (last_preflight_check == 0 || hrt_absolute_time() - last_preflight_check > 1000 * 1000) {
 				prearm_ret = preflight_check(status, mavlink_log_pub, false /* pre-flight */,
-							     circuit_breaker_engaged_airspd_check,
+							     status_flags);
-						     	     circuit_breaker_engaged_gpsfailure_check,
+				status_flags->condition_system_sensors_initialized = !prearm_ret;
-							     cb_usb);
-				status->condition_system_sensors_initialized = !prearm_ret;
 				last_preflight_check = hrt_absolute_time();
 				last_prearm_ret = prearm_ret;
 			} else {
@@ -179,7 +171,7 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 		if (status->hil_state == vehicle_status_s::HIL_STATE_ON) {
 			armed->lockdown = true;
 			prearm_ret = OK;
-			status->condition_system_sensors_initialized = true;
+			status_flags->condition_system_sensors_initialized = true;
 
 			/* recover from a prearm fail */
 			if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {
@@ -218,9 +210,9 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 
 					// Perform power checks only if circuit breaker is not
 					// engaged for these checks
-					if (!circuit_breaker_engaged_power_check) {
+					if (!status_flags->circuit_breaker_engaged_power_check) {
 						// Fail transition if power is not good
-						if (!status->condition_power_input_valid) {
+						if (!status_flags->condition_power_input_valid) {
 
 							mavlink_and_console_log_critical(mavlink_log_pub, "NOT ARMING: Connect power module.");
 							feedback_provided = true;
@@ -229,7 +221,7 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 
 						// Fail transition if power levels on the avionics rail
 						// are measured but are insufficient
-						if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f)) {
+						if (status_flags->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f)) {
 							// Check avionics rail voltages
 							if (status->avionics_power_rail_voltage < AVIONICS_ERROR_VOLTAGE) {
 								mavlink_and_console_log_critical(mavlink_log_pub, "NOT ARMING: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage);
@@ -262,16 +254,26 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 
 			if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
 
+<<<<<<< 4ed69db0abb4b3b2f191ba77949e9c6817eaf169
 				if (status->condition_system_sensors_initialized) {
 					mavlink_and_console_log_critical(mavlink_log_pub, "Preflight check resolved, reboot before arming");
+=======
+				if (status_flags->condition_system_sensors_initialized) {
+					// mavlink_and_console_log_critical(mavlink_log_pub, "Preflight check resolved, reboot before arming");
+>>>>>>> commander: internalize system status bools
 				} else {
 					mavlink_and_console_log_critical(mavlink_log_pub, "Preflight check failed, refusing to arm");
 				}
 				feedback_provided = true;
 
 			} else if ((new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) &&
+<<<<<<< 4ed69db0abb4b3b2f191ba77949e9c6817eaf169
 					status->condition_system_sensors_initialized) {
 				mavlink_and_console_log_critical(mavlink_log_pub, "Preflight check resolved, reboot to complete");
+=======
+					status_flags->condition_system_sensors_initialized) {
+				// mavlink_and_console_log_critical(mavlink_log_pub, "Preflight check resolved, reboot to complete");
+>>>>>>> commander: internalize system status bools
 				feedback_provided = true;
 			} else {
 				// Silent ignore
@@ -282,12 +284,17 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 		} else if ((status->hil_state != vehicle_status_s::HIL_STATE_ON) &&
 			(new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) &&
 			(status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR) &&
-			(!status->condition_system_sensors_initialized)) {
+			(!status_flags->condition_system_sensors_initialized)) {
-			if ((!status->condition_system_prearm_error_reported &&
+			if ((!status_flags->condition_system_prearm_error_reported &&
-			      status->condition_system_hotplug_timeout) ||
+			      status_flags->condition_system_hotplug_timeout) ||
 			     (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)) {
+<<<<<<< 4ed69db0abb4b3b2f191ba77949e9c6817eaf169
 				mavlink_and_console_log_critical(mavlink_log_pub, "Not ready to fly: Sensors not set up correctly");
 				status->condition_system_prearm_error_reported = true;
+=======
+				mavlink_and_console_log_critical(mavlink_log_pub, "Not ready to fly: Sensors not initialized");
+				status_flags->condition_system_prearm_error_reported = true;
+>>>>>>> commander: internalize system status bools
 			}
 			feedback_provided = true;
 			valid_transition = false;
@@ -305,7 +312,7 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 		if (status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR &&
 			status->arming_state != vehicle_status_s::ARMING_STATE_INIT &&
 			valid_transition) {
-			status->condition_system_prearm_error_reported = false;
+			status_flags->condition_system_prearm_error_reported = false;
 		}
 
 		/* end of atomic state update */
@@ -339,7 +346,8 @@ bool is_safe(const struct vehicle_status_s *status, const struct safety_s *safet
 }
 
 transition_result_t
-main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state, uint8_t &main_state_prev)
+main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state, uint8_t &main_state_prev,
+		      status_flags_s *status_flags)
 {
 	transition_result_t ret = TRANSITION_DENIED;
 
@@ -356,23 +364,23 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
 		/* need at minimum altitude estimate */
 		/* TODO: add this for fixedwing as well */
 		if (!status->is_rotary_wing ||
-		    (status->condition_local_altitude_valid ||
+		    (status_flags->condition_local_altitude_valid ||
-		     status->condition_global_position_valid)) {
+		     status_flags->condition_global_position_valid)) {
 			ret = TRANSITION_CHANGED;
 		}
 		break;
 
 	case vehicle_status_s::MAIN_STATE_POSCTL:
 		/* need at minimum local position estimate */
-		if (status->condition_local_position_valid ||
+		if (status_flags->condition_local_position_valid ||
-		    status->condition_global_position_valid) {
+		    status_flags->condition_global_position_valid) {
 			ret = TRANSITION_CHANGED;
 		}
 		break;
 
 	case vehicle_status_s::MAIN_STATE_AUTO_LOITER:
 		/* need global position estimate */
-		if (status->condition_global_position_valid) {
+		if (status_flags->condition_global_position_valid) {
 			ret = TRANSITION_CHANGED;
 		}
 		break;
@@ -383,7 +391,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
 	case vehicle_status_s::MAIN_STATE_AUTO_TAKEOFF:
 	case vehicle_status_s::MAIN_STATE_AUTO_LAND:
 		/* need global position and home position */
-		if (status->condition_global_position_valid && status->condition_home_position_valid) {
+		if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
 			ret = TRANSITION_CHANGED;
 		}
 		break;
@@ -589,7 +597,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
  * 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,
-		   const bool stay_in_failsafe)
+		   const bool stay_in_failsafe, status_flags_s *status_flags, bool landed)
 {
 	navigation_state_t nav_state_old = status->nav_state;
 
@@ -605,14 +613,14 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 	case vehicle_status_s::MAIN_STATE_ALTCTL:
 	case vehicle_status_s::MAIN_STATE_POSCTL:
 		/* require RC for all manual modes */
-		if ((status->rc_signal_lost || status->rc_signal_lost_cmd) && armed && !status->condition_landed) {
+		if ((status->rc_signal_lost || status->rc_signal_lost_cmd) && armed && !landed) {
 			status->failsafe = true;
 
-			if (status->condition_global_position_valid && status->condition_home_position_valid) {
+			if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
-			} else if (status->condition_local_position_valid) {
+			} else if (status_flags->condition_local_position_valid) {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
-			} else if (status->condition_local_altitude_valid) {
+			} else if (status_flags->condition_local_altitude_valid) {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
 			} else {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
@@ -686,11 +694,11 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 		} else if (data_link_loss_enabled && status->data_link_lost) {
 			status->failsafe = true;
 
-			if (status->condition_global_position_valid && status->condition_home_position_valid) {
+			if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
-			} else if (status->condition_local_position_valid) {
+			} else if (status_flags->condition_local_position_valid) {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
-			} else if (status->condition_local_altitude_valid) {
+			} else if (status_flags->condition_local_altitude_valid) {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
 			} else {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
@@ -703,11 +711,11 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 						       (status->rc_signal_lost && mission_finished))) {
 			status->failsafe = true;
 
-			if (status->condition_global_position_valid && status->condition_home_position_valid) {
+			if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
-			} else if (status->condition_local_position_valid) {
+			} else if (status_flags->condition_local_position_valid) {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
-			} else if (status->condition_local_altitude_valid) {
+			} else if (status_flags->condition_local_altitude_valid) {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
 			} else {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
@@ -727,11 +735,11 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 		} else if (status->data_link_lost && data_link_loss_enabled) {
 			status->failsafe = true;
 
-			if (status->condition_global_position_valid && status->condition_home_position_valid) {
+			if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
-			} else if (status->condition_local_position_valid) {
+			} else if (status_flags->condition_local_position_valid) {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
-			} else if (status->condition_local_altitude_valid) {
+			} else if (status_flags->condition_local_altitude_valid) {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
 			} else {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
@@ -741,11 +749,11 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 		} else if (status->rc_signal_lost && !data_link_loss_enabled) {
 			status->failsafe = true;
 
-			if (status->condition_global_position_valid && status->condition_home_position_valid) {
+			if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
-			} else if (status->condition_local_position_valid) {
+			} else if (status_flags->condition_local_position_valid) {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
-			} else if (status->condition_local_altitude_valid) {
+			} else if (status_flags->condition_local_altitude_valid) {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
 			} else {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
@@ -767,13 +775,13 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 
 		if (status->engine_failure) {
 			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
-		} else if ((!status->condition_global_position_valid ||
+		} else if ((!status_flags->condition_global_position_valid ||
-					!status->condition_home_position_valid)) {
+					!status_flags->condition_home_position_valid)) {
 			status->failsafe = true;
 
-			if (status->condition_local_position_valid) {
+			if (status_flags->condition_local_position_valid) {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
-			} else if (status->condition_local_altitude_valid) {
+			} else if (status_flags->condition_local_altitude_valid) {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
 			} else {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
@@ -808,13 +816,13 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 
 		if (status->engine_failure) {
 			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
-		} else if ((!status->condition_global_position_valid ||
+		} else if ((!status_flags->condition_global_position_valid ||
-					!status->condition_home_position_valid)) {
+					!status_flags->condition_home_position_valid)) {
 			status->failsafe = true;
 
-			if (status->condition_local_position_valid) {
+			if (status_flags->condition_local_position_valid) {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
-			} else if (status->condition_local_altitude_valid) {
+			} else if (status_flags->condition_local_altitude_valid) {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
 			} else {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
@@ -829,11 +837,11 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 
 		if (status->engine_failure) {
 			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
-		} else if ((!status->condition_global_position_valid ||
+		} else if ((!status_flags->condition_global_position_valid ||
-					!status->condition_home_position_valid)) {
+					!status_flags->condition_home_position_valid)) {
 			status->failsafe = true;
 
-			if (status->condition_local_altitude_valid) {
+			if (status_flags->condition_local_altitude_valid) {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
 			} else {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
@@ -852,9 +860,9 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 		} else if (status->offboard_control_signal_lost && status->rc_signal_lost) {
 			status->failsafe = true;
 
-			if (status->condition_local_position_valid) {
+			if (status_flags->condition_local_position_valid) {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
-			} else if (status->condition_local_altitude_valid) {
+			} else if (status_flags->condition_local_altitude_valid) {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
 			} else {
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
@@ -869,28 +877,25 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 	return status->nav_state != nav_state_old;
 }
 
-int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, bool force_report,
+int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, bool force_report, status_flags_s *status_flags)
-		    bool circuit_breaker_engaged_airspd_check,
-		    bool circuit_breaker_engaged_gpsfailure_check,
-		    bool cb_usb)
 {
 	/*
 	 */
-	bool reportFailures = force_report || (!status->condition_system_prearm_error_reported &&
+	bool reportFailures = force_report || (!status_flags->condition_system_prearm_error_reported &&
-		status->condition_system_hotplug_timeout);
+		status_flags->condition_system_hotplug_timeout);
 
 	bool checkAirspeed = false;
 	/* Perform airspeed check only if circuit breaker is not
 	 * engaged and it's not a rotary wing */
-	if (!circuit_breaker_engaged_airspd_check && (!status->is_rotary_wing || status->is_vtol)) {
+	if (!status_flags->circuit_breaker_engaged_airspd_check && (!status->is_rotary_wing || status->is_vtol)) {
 		checkAirspeed = true;
 	}
 
 	bool preflight_ok = Commander::preflightCheck(mavlink_log_pub, true, true, true, true,
 				checkAirspeed, (status->rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT),
-				!circuit_breaker_engaged_gpsfailure_check, true, reportFailures);
+				!status_flags->circuit_breaker_engaged_gpsfailure_check, true, reportFailures);
 
-	if (!cb_usb && status->usb_connected && prearm) {
+	if (!status_flags->cb_usb && status_flags->usb_connected && prearm) {
 		preflight_ok = false;
 		if (reportFailures) {
 			mavlink_and_console_log_critical(mavlink_log_pub, "ARMING DENIED: Flying with USB is not safe");
@@ -906,7 +911,7 @@ int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_p
 
 	/* report once, then set the flag */
 	if (reportFailures && !preflight_ok) {
-		status->condition_system_prearm_error_reported = true;
+		status_flags->condition_system_prearm_error_reported = true;
 	}
 
 	return !preflight_ok;

src/modules/commander/state_machine_helper.h

@@ -54,6 +54,29 @@ typedef enum {
 
 } transition_result_t;
 
+
+// This is a struct used by the commander internally.
+struct status_flags_s {
+	bool condition_calibration_enabled;
+	bool condition_system_sensors_initialized;
+	bool condition_system_prearm_error_reported;	// true if errors have already been reported
+	bool condition_system_hotplug_timeout;		// true if the hotplug sensor search is over
+	bool condition_system_returned_to_home;
+	bool condition_auto_mission_available;
+	bool condition_global_position_valid;		// set to true by the commander app if the quality of the position estimate is good enough to use it for navigation
+	bool condition_home_position_valid;		// indicates a valid home position (a valid home position is not always a valid launch)
+	bool condition_local_position_valid;
+	bool condition_local_altitude_valid;
+	bool condition_airspeed_valid;			// set to true by the commander app if there is a valid airspeed measurement available
+	bool condition_power_input_valid;		// set if input power is valid
+	bool usb_connected;				// status of the USB power supply
+	bool circuit_breaker_engaged_power_check;
+	bool circuit_breaker_engaged_airspd_check;
+	bool circuit_breaker_engaged_enginefailure_check;
+	bool circuit_breaker_engaged_gpsfailure_check;
+	bool cb_usb;
+};
+
 bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);
 
 transition_result_t arming_state_transition(struct vehicle_status_s *current_state,
@@ -62,20 +85,18 @@ transition_result_t arming_state_transition(struct vehicle_status_s *current_sta
 					    struct actuator_armed_s *armed,
 					    bool fRunPreArmChecks,
 					    orb_advert_t *mavlink_log_pub,
-					    bool circuit_breaker_engaged_airspd_check,
+					    status_flags_s *status_flags);
-					    bool circuit_breaker_engaged_gpsfailure_check,
-					    bool circuit_breaker_engaged_power_check,
-					    bool cb_usb);
 
-transition_result_t main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state, uint8_t &main_state_prev);
+transition_result_t
+main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state, uint8_t &main_state_prev,
+		      status_flags_s *status_flags);
 
 transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t status_pub, struct vehicle_status_s *current_state, orb_advert_t *mavlink_log_pub);
 
-bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, const bool stay_in_failsafe);
+bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished,
+		   const bool stay_in_failsafe, status_flags_s *status_flags, bool landed);
 
 int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm,
-		    bool circuit_breaker_engaged_airspd_check,
+		    status_flags_s *status_flags);
-		    bool circuit_breaker_engaged_gpsfailure_check,
-		    bool cb_usb);
 
 #endif /* STATE_MACHINE_HELPER_H_ */

src/modules/ekf2/ekf2_main.cpp

@@ -75,11 +75,11 @@
 #include <uORB/topics/wind_estimate.h>
 #include <uORB/topics/estimator_status.h>
 #include <uORB/topics/ekf2_innovations.h>
-#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/actuator_armed.h>
-#include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/ekf2_replay.h>
 #include <uORB/topics/optical_flow.h>
 #include <uORB/topics/distance_sensor.h>
+#include <uORB/topics/vehicle_land_detected.h>
 
 #include <ecl/EKF/ekf.h>
 
@@ -139,6 +139,8 @@ private:
 	int 	_vehicle_status_sub = -1;
 	int 	_optical_flow_sub = -1;
 	int 	_range_finder_sub = -1;
+	int		_actuator_armed_sub = -1;
+	int		_vehicle_land_detected_sub = -1;
 
 	bool            _prev_motors_armed = false; // motors armed status from the previous frame
 
@@ -319,6 +321,8 @@ void Ekf2::task_main()
 	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
 	_optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
 	_range_finder_sub = orb_subscribe(ORB_ID(distance_sensor));
+	_actuator_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+	_vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
 
 	px4_pollfd_struct_t fds[2] = {};
 	fds[0].fd = _sensors_sub;
@@ -338,6 +342,7 @@ void Ekf2::task_main()
 	vehicle_control_mode_s vehicle_control_mode = {};
 	optical_flow_s optical_flow = {};
 	distance_sensor_s range_finder = {};
+	actuator_armed_s actuator_armed = {};
 
 	while (!_task_should_exit) {
 		int ret = px4_poll(fds, sizeof(fds) / sizeof(fds[0]), 1000);
@@ -371,6 +376,8 @@ void Ekf2::task_main()
 		bool vehicle_status_updated = false;
 		bool optical_flow_updated = false;
 		bool range_finder_updated = false;
+		bool actuator_armed_updated = false;
+		bool vehicle_land_detected_updated = false;
 
 		orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors);
 		// update all other topics if they have new data
@@ -465,14 +472,19 @@ void Ekf2::task_main()
 			_ekf->setRangeData(range_finder.timestamp, &range_finder.current_distance);
 		}
 
-		// read vehicle status if available for 'landed' information
+		orb_check(_actuator_armed_sub, &actuator_armed_updated);
-		orb_check(_vehicle_status_sub, &vehicle_status_updated);
 
-		if (vehicle_status_updated) {
+		if (actuator_armed_updated) {
-			struct vehicle_status_s status = {};
+			orb_copy(ORB_ID(actuator_armed), _actuator_armed_sub, &actuator_armed);
-			orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &status);
+			_ekf->set_arm_status(&actuator_armed.armed);
-			_ekf->set_in_air_status(!status.condition_landed);
+		}
-			_ekf->set_arm_status(status.arming_state & vehicle_status_s::ARMING_STATE_ARMED);
+
+		orb_check(_vehicle_land_detected_sub, &vehicle_land_detected_updated);
+
+		if (vehicle_land_detected_updated) {
+			struct vehicle_land_detected_s vehicle_land_detected = {};
+			orb_copy(ORB_ID(vehicle_land_detected), _vehicle_land_detected_sub, &vehicle_land_detected);
+			_ekf->set_in_air_status(!vehicle_land_detected.landed);
 		}
 
 		// run the EKF update and output
@@ -703,7 +715,7 @@ void Ekf2::task_main()
 		}
 
 		// save the declination to the EKF2_MAG_DECL parameter when a dis-arm event is detected
-		if ((_params->mag_declination_source & (1 << 1)) && _prev_motors_armed && !vehicle_control_mode.flag_armed) {
+		if ((_params->mag_declination_source & (1 << 1)) && _prev_motors_armed && !actuator_armed.armed) {
 			float decl_deg;
 			_ekf->copy_mag_decl_deg(&decl_deg);
 			_mag_declination_deg->set(decl_deg);

src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h

@@ -50,9 +50,9 @@
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/control_state.h>
+#include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/estimator_status.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/home_position.h>
@@ -147,7 +147,8 @@ private:
     int     _airspeed_sub;          /**< airspeed subscription */
     int     _baro_sub;          /**< barometer subscription */
     int     _gps_sub;           /**< GPS subscription */
-    int     _vstatus_sub;           /**< vehicle status subscription */
+    int     _vehicle_status_sub;
+    int     _vehicle_land_detected_sub;
     int     _params_sub;            /**< notification of parameter updates */
     int     _manual_control_sub;        /**< notification of manual control updates */
     int     _mission_sub;
@@ -169,13 +170,13 @@ private:
     struct mag_report                   _mag;
     struct airspeed_s                   _airspeed;      /**< airspeed */
     struct baro_report                  _baro;          /**< baro readings */
-    struct vehicle_status_s             _vstatus;       /**< vehicle status */
+    struct vehicle_status_s		_vehicle_status;
+    struct vehicle_land_detected_s      _vehicle_land_detected;
     struct vehicle_global_position_s    _global_pos;        /**< global vehicle position */
     struct vehicle_local_position_s     _local_pos;     /**< local vehicle position */
     struct vehicle_gps_position_s       _gps;           /**< GPS position */
     struct wind_estimate_s              _wind;          /**< wind estimate */
     struct distance_sensor_s            _distance;      /**< distance estimate */
-    struct vehicle_land_detected_s      _landDetector;
     struct actuator_armed_s             _armed;
 
     hrt_abstime _last_accel;
@@ -299,10 +300,15 @@ private:
     void        control_update();
 
     /**
-     * Check for changes in vehicle status.
+     * Check for changes in land detected.
      */
     void        vehicle_status_poll();
 
+    /**
+     * Check for changes in land detected.
+     */
+    void        vehicle_land_detected_poll();
+
     /**
      * Shim for calling task_main from task_create.
      */

src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp

@@ -132,12 +132,12 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 	_airspeed_sub(-1),
 	_baro_sub(-1),
 	_gps_sub(-1),
-	_vstatus_sub(-1),
+	_vehicle_status_sub(-1),
+	_vehicle_land_detected_sub(-1),
 	_params_sub(-1),
 	_manual_control_sub(-1),
 	_mission_sub(-1),
 	_home_sub(-1),
-	_landDetectorSub(-1),
 	_armedSub(-1),
 
 	/* publications */
@@ -155,13 +155,13 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 	_mag{},
 	_airspeed{},
 	_baro{},
-	_vstatus{},
+	_vehicle_status{},
+	_vehicle_land_detected{},
 	_global_pos{},
 	_local_pos{},
 	_gps{},
 	_wind{},
 	_distance{},
-	_landDetector{},
 	_armed{},
 
 	_last_accel(0),
@@ -346,22 +346,31 @@ int AttitudePositionEstimatorEKF::parameters_update()
 
 void AttitudePositionEstimatorEKF::vehicle_status_poll()
 {
-	bool vstatus_updated;
+	bool updated;
 
-	/* Check HIL state if vehicle status has changed */
+	orb_check(_vehicle_status_sub, &updated);
-	orb_check(_vstatus_sub, &vstatus_updated);
 
-	bool landed = _vstatus.condition_landed;
+	if (updated) {
 
-	if (vstatus_updated) {
+		orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
-
-		orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus);
 
 		// Tell EKF that the vehicle is a fixed wing or multi-rotor
-		_ekf->setIsFixedWing(!_vstatus.is_rotary_wing);
+		_ekf->setIsFixedWing(!_vehicle_status.is_rotary_wing);
+	}
+}
+
+void AttitudePositionEstimatorEKF::vehicle_land_detected_poll()
+{
+	bool updated;
+
+	orb_check(_vehicle_land_detected_sub, &updated);
+
+	if (updated) {
+
+		orb_copy(ORB_ID(vehicle_land_detected), _vehicle_land_detected_sub, &_vehicle_land_detected);
 
 		// Save params on landed
-		if (!landed && _vstatus.condition_landed) {
+		if (!_vehicle_land_detected.landed) {
 			_mag_offset_x.set(_ekf->magBias.x);
 			_mag_offset_x.commit();
 			_mag_offset_y.set(_ekf->magBias.y);
@@ -526,10 +535,10 @@ void AttitudePositionEstimatorEKF::task_main()
 	_baro_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 0);
 	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
 	_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
-	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
+	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
+	_vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_home_sub = orb_subscribe(ORB_ID(home_position));
-	_landDetectorSub = orb_subscribe(ORB_ID(vehicle_land_detected));
 	_armedSub = orb_subscribe(ORB_ID(actuator_armed));
 
 	_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
@@ -586,13 +595,14 @@ void AttitudePositionEstimatorEKF::task_main()
 		if (fds[1].revents & POLLIN) {
 
 			/* check vehicle status for changes to publication state */
-			bool prev_hil = (_vstatus.hil_state == vehicle_status_s::HIL_STATE_ON);
+			bool prev_hil = (_vehicle_status.hil_state == vehicle_status_s::HIL_STATE_ON);
 			vehicle_status_poll();
+			vehicle_land_detected_poll();
 
 			perf_count(_perf_gyro);
 
 			/* Reset baro reference if switching to HIL, reset sensor states */
-			if (!prev_hil && (_vstatus.hil_state == vehicle_status_s::HIL_STATE_ON)) {
+			if (!prev_hil && (_vehicle_status.hil_state == vehicle_status_s::HIL_STATE_ON)) {
 				/* system is in HIL now, wait for measurements to come in one last round */
 				usleep(60000);
 
@@ -690,7 +700,7 @@ void AttitudePositionEstimatorEKF::task_main()
 					}
 
 					// Check if on ground - status is used by covariance prediction
-					_ekf->setOnGround(_landDetector.landed);
+					_ekf->setOnGround(_vehicle_land_detected.landed);
 
 					// We're apparently initialized in this case now
 					// check (and reset the filter as needed)
@@ -1305,7 +1315,7 @@ void AttitudePositionEstimatorEKF::print_status()
 
 	PX4_INFO("states: %s %s %s %s %s %s %s %s %s %s",
 		 (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT",
-		 (_landDetector.landed) ? "ON_GROUND" : "AIRBORNE",
+		 (_vehicle_land_detected.landed) ? "ON_GROUND" : "AIRBORNE",
 		 (_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL",
 		 (_ekf->fusePosData) ? "FUSE_POS" : "INH_POS",
 		 (_ekf->fuseHgtData) ? "FUSE_HGT" : "INH_HGT",
@@ -1520,14 +1530,6 @@ void AttitudePositionEstimatorEKF::pollData()
 
 	//PX4_INFO("dang: %8.4f %8.4f dvel: %8.4f %8.4f", _ekf->dAngIMU.x, _ekf->dAngIMU.z, _ekf->dVelIMU.x, _ekf->dVelIMU.z);
 
-	//Update Land Detector
-	bool newLandData;
-	orb_check(_landDetectorSub, &newLandData);
-
-	if (newLandData) {
-		orb_copy(ORB_ID(vehicle_land_detected), _landDetectorSub, &_landDetector);
-	}
-
 	//Update AirSpeed
 	orb_check(_airspeed_sub, &_newAdsData);
 

src/modules/fw_att_control/fw_att_control_main.cpp

@@ -72,6 +72,7 @@
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_land_detected.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <systemlib/pid/pid.h>
@@ -135,6 +136,7 @@ private:
 	int 		_manual_sub;			/**< notification of manual control updates */
 	int		_global_pos_sub;		/**< global position subscription */
 	int		_vehicle_status_sub;		/**< vehicle status subscription */
+	int		_vehicle_land_detected_sub;	/**< vehicle land detected subscription */
 
 	orb_advert_t	_rate_sp_pub;			/**< rate setpoint publication */
 	orb_advert_t	_attitude_sp_pub;		/**< attitude setpoint point */
@@ -155,6 +157,7 @@ private:
 	struct actuator_controls_s			_actuators_airframe;	/**< actuator control inputs */
 	struct vehicle_global_position_s		_global_pos;		/**< global position */
 	struct vehicle_status_s				_vehicle_status;	/**< vehicle status */
+	struct vehicle_land_detected_s			_vehicle_land_detected;	/**< vehicle land detected */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 	perf_counter_t	_nonfinite_input_perf;		/**< performance counter for non finite input */
@@ -317,6 +320,11 @@ private:
 	 */
 	void		vehicle_status_poll();
 
+	/**
+	 * Check for vehicle land detected updates.
+	 */
+	void		vehicle_land_detected_poll();
+
 	/**
 	 * Shim for calling task_main from task_create.
 	 */
@@ -355,6 +363,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_manual_sub(-1),
 	_global_pos_sub(-1),
 	_vehicle_status_sub(-1),
+	_vehicle_land_detected_sub(-1),
 
 	/* publications */
 	_rate_sp_pub(nullptr),
@@ -387,6 +396,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_actuators_airframe = {};
 	_global_pos = {};
 	_vehicle_status = {};
+	_vehicle_land_detected = {};
 
 
 	_parameter_handles.p_tc = param_find("FW_P_TC");
@@ -652,6 +662,18 @@ FixedwingAttitudeControl::vehicle_status_poll()
 	}
 }
 
+void
+FixedwingAttitudeControl::vehicle_land_detected_poll()
+{
+	/* check if there is new status information */
+	bool vehicle_land_detected_updated;
+	orb_check(_vehicle_land_detected_sub, &vehicle_land_detected_updated);
+
+	if (vehicle_land_detected_updated) {
+		orb_copy(ORB_ID(vehicle_land_detected), _vehicle_land_detected_sub, &_vehicle_land_detected);
+	}
+}
+
 void
 FixedwingAttitudeControl::task_main_trampoline(int argc, char *argv[])
 {
@@ -672,6 +694,7 @@ FixedwingAttitudeControl::task_main()
 	_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
 	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
+	_vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
 
 	parameters_update();
 
@@ -681,6 +704,7 @@ FixedwingAttitudeControl::task_main()
 	vehicle_control_mode_poll();
 	vehicle_manual_poll();
 	vehicle_status_poll();
+	vehicle_land_detected_poll();
 
 	/* wakeup source */
 	px4_pollfd_struct_t fds[2];
@@ -806,6 +830,8 @@ FixedwingAttitudeControl::task_main()
 
 			vehicle_status_poll();
 
+			vehicle_land_detected_poll();
+
 			// the position controller will not emit attitude setpoints in some modes
 			// we need to make sure that this flag is reset
 			_att_sp.fw_control_yaw = _att_sp.fw_control_yaw && _vcontrol_mode.flag_control_auto_enabled;
@@ -1056,7 +1082,7 @@ FixedwingAttitudeControl::task_main()
 				}
 
 				/* If the aircraft is on ground reset the integrators */
-				if (_vehicle_status.condition_landed || _vehicle_status.is_rotary_wing) {
+				if (_vehicle_land_detected.landed || _vehicle_status.is_rotary_wing) {
 					_roll_ctrl.reset_integrator();
 					_pitch_ctrl.reset_integrator();
 					_yaw_ctrl.reset_integrator();

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -82,6 +82,7 @@
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/tecs_status.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
@@ -161,6 +162,7 @@ private:
 	int		_ctrl_state_sub;			/**< control state subscription */
 	int		_control_mode_sub;		/**< control mode subscription */
 	int		_vehicle_status_sub;		/**< vehicle status subscription */
+	int		_vehicle_land_detected_sub;	/**< vehicle land detected subscription */
 	int 		_params_sub;			/**< notification of parameter updates */
 	int 		_manual_control_sub;		/**< notification of manual control updates */
 	int		_sensor_combined_sub;		/**< for body frame accelerations */
@@ -177,6 +179,7 @@ private:
 	struct manual_control_setpoint_s		_manual;			/**< r/c channel data */
 	struct vehicle_control_mode_s			_control_mode;			/**< control mode */
 	struct vehicle_status_s				_vehicle_status;		/**< vehicle status */
+	struct vehicle_land_detected_s			_vehicle_land_detected;		/**< vehicle land detected */
 	struct vehicle_global_position_s		_global_pos;			/**< global vehicle position */
 	struct position_setpoint_triplet_s		_pos_sp_triplet;		/**< triplet of mission items */
 	struct sensor_combined_s			_sensor_combined;		/**< for body frame accelerations */
@@ -377,6 +380,11 @@ private:
 	 */
 	void		vehicle_status_poll();
 
+	/**
+	 * Check for changes in vehicle land detected.
+	 */
+	void		vehicle_land_detected_poll();
+
 	/**
 	 * Check for manual setpoint updates.
 	 */
@@ -516,6 +524,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_ctrl_state_sub(-1),
 	_control_mode_sub(-1),
 	_vehicle_status_sub(-1),
+	_vehicle_land_detected_sub(-1),
 	_params_sub(-1),
 	_manual_control_sub(-1),
 	_sensor_combined_sub(-1),
@@ -535,6 +544,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_manual(),
 	_control_mode(),
 	_vehicle_status(),
+	_vehicle_land_detected(),
 	_global_pos(),
 	_pos_sp_triplet(),
 	_sensor_combined(),
@@ -812,6 +822,18 @@ FixedwingPositionControl::vehicle_status_poll()
 	}
 }
 
+void
+FixedwingPositionControl::vehicle_land_detected_poll()
+{
+	bool updated;
+
+	orb_check(_vehicle_land_detected_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(vehicle_land_detected), _vehicle_land_detected_sub, &_vehicle_land_detected);
+	}
+}
+
 bool
 FixedwingPositionControl::vehicle_manual_control_setpoint_poll()
 {
@@ -1204,7 +1226,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 	math::Vector<3> accel_earth = _R_nb * accel_body;
 
 	/* tell TECS to update its state, but let it know when it cannot actually control the plane */
-	bool in_air_alt_control = (!_vehicle_status.condition_landed &&
+	bool in_air_alt_control = (!_vehicle_land_detected.landed &&
 				   (_control_mode.flag_control_auto_enabled ||
 				    _control_mode.flag_control_velocity_enabled ||
 				    _control_mode.flag_control_altitude_enabled));
@@ -1225,14 +1247,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 	float throttle_max = 1.0f;
 
 	/* save time when airplane is in air */
-	if (!_was_in_air && !_vehicle_status.condition_landed) {
+	if (!_was_in_air && !_vehicle_land_detected.landed) {
 		_was_in_air = true;
 		_time_went_in_air = hrt_absolute_time();
 		_takeoff_ground_alt = _global_pos.alt;
 	}
 
 	/* reset flag when airplane landed */
-	if (_vehicle_status.condition_landed) {
+	if (_vehicle_land_detected.landed) {
 		_was_in_air = false;
 	}
 
@@ -1993,7 +2015,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 	} else {
 		/* Copy thrust and pitch values from tecs */
-		if (_vehicle_status.condition_landed) {
+		if (_vehicle_land_detected.landed) {
 			// when we are landed state we want the motor to spin at idle speed
 			_att_sp.thrust = math::min(_parameters.throttle_idle, throttle_max);
 
@@ -2062,6 +2084,7 @@ FixedwingPositionControl::task_main()
 	_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
 	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
+	_vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 
@@ -2069,6 +2092,8 @@ FixedwingPositionControl::task_main()
 	orb_set_interval(_control_mode_sub, 200);
 	/* rate limit vehicle status updates to 5Hz */
 	orb_set_interval(_vehicle_status_sub, 200);
+	/* rate limit vehicle land detected updates to 5Hz */
+	orb_set_interval(_vehicle_land_detected_sub, 200);
 	/* rate limit position updates to 50 Hz */
 	orb_set_interval(_global_pos_sub, 20);
 
@@ -2112,6 +2137,9 @@ FixedwingPositionControl::task_main()
 		/* check vehicle status for changes to publication state */
 		vehicle_status_poll();
 
+		/* check vehicle land detected for changes to publication state */
+		vehicle_land_detected_poll();
+
 		/* only update parameters if they changed */
 		if (fds[0].revents & POLLIN) {
 			/* read from param to clear updated flag */
@@ -2222,7 +2250,7 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
 	_last_tecs_update = hrt_absolute_time();
 
 	// do not run TECS if we are not in air
-	run_tecs &= !_vehicle_status.condition_landed;
+	run_tecs &= !_vehicle_land_detected.landed;
 
 	// do not run TECS if vehicle is a VTOL and we are in rotary wing mode or in transition
 	// (it should also not run during VTOL blending because airspeed is too low still)

src/modules/mavlink/mavlink_messages.cpp

@@ -574,12 +574,13 @@ protected:
 		struct vehicle_status_s status;
 		struct battery_status_s battery_status;
 
-		bool updated = _status_sub->update(&status);
+		const bool updated_status = _status_sub->update(&status);
-		updated = (updated || _battery_status_sub->update(&battery_status));
+		const bool updated_battery = _battery_status_sub->update(&battery_status);
 
-		if (updated) {
+		if (updated_status || updated_battery) {
 			mavlink_sys_status_t msg;
 
+
 			msg.onboard_control_sensors_present = status.onboard_control_sensors_present;
 			msg.onboard_control_sensors_enabled = status.onboard_control_sensors_enabled;
 			msg.onboard_control_sensors_health = status.onboard_control_sensors_health;
@@ -595,6 +596,8 @@ protected:
 			msg.errors_count3 = 0;
 			msg.errors_count4 = 0;
 
+			PX4_INFO("send sys status: voltage: %d", msg.voltage_battery);
+
 			_mavlink->send_message(MAVLINK_MSG_ID_SYS_STATUS, &msg);
 
 			/* battery status message with higher resolution */

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -77,6 +77,7 @@
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_global_velocity_setpoint.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
+#include <uORB/topics/vehicle_land_detected.h>
 
 #include <systemlib/systemlib.h>
 #include <systemlib/mavlink_log.h>
@@ -126,6 +127,7 @@ private:
 	orb_advert_t	_mavlink_log_pub;		/**< mavlink log advert */
 
 	int		_vehicle_status_sub;		/**< vehicle status subscription */
+	int		_vehicle_land_detected_sub;	/**< vehicle land detected subscription */
 	int		_ctrl_state_sub;		/**< control state subscription */
 	int		_att_sp_sub;			/**< vehicle attitude setpoint */
 	int		_control_mode_sub;		/**< vehicle control mode subscription */
@@ -144,7 +146,8 @@ private:
 	orb_id_t _attitude_setpoint_id;
 
 	struct vehicle_status_s 			_vehicle_status; 	/**< vehicle status */
-	struct control_state_s				_ctrl_state;			/**< vehicle attitude */
+	struct vehicle_land_detected_s 			_vehicle_land_detected;	/**< vehicle land detected */
+	struct control_state_s				_ctrl_state;		/**< vehicle attitude */
 	struct vehicle_attitude_setpoint_s		_att_sp;		/**< vehicle attitude setpoint */
 	struct manual_control_setpoint_s		_manual;		/**< r/c channel data */
 	struct vehicle_control_mode_s			_control_mode;		/**< vehicle control mode */
@@ -607,6 +610,12 @@ MulticopterPositionControl::poll_subscriptions()
 		}
 	}
 
+	orb_check(_vehicle_land_detected_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(vehicle_land_detected), _vehicle_land_detected_sub, &_vehicle_land_detected);
+	}
+
 	orb_check(_ctrl_state_sub, &updated);
 
 	if (updated) {
@@ -1153,6 +1162,7 @@ MulticopterPositionControl::task_main()
 	 * do subscriptions
 	 */
 	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
+	_vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
 	_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
 	_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
 	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
@@ -1341,7 +1351,7 @@ MulticopterPositionControl::task_main()
 				}
 
 			} else if (_control_mode.flag_control_manual_enabled
-					&& _vehicle_status.condition_landed) {
+					&& _vehicle_land_detected.landed) {
 				/* don't run controller when landed */
 				_reset_pos_sp = true;
 				_reset_alt_sp = true;
@@ -1462,7 +1472,7 @@ MulticopterPositionControl::task_main()
 
 					// check if we are not already in air.
 					// if yes then we don't need a jumped takeoff anymore
-					if (!_takeoff_jumped && !_vehicle_status.condition_landed && fabsf(_takeoff_thrust_sp) < FLT_EPSILON) {
+					if (!_takeoff_jumped && !_vehicle_land_detected.landed && fabsf(_takeoff_thrust_sp) < FLT_EPSILON) {
 						_takeoff_jumped = true;
 					}
 
@@ -1906,7 +1916,7 @@ MulticopterPositionControl::task_main()
 			}
 
 			/* do not move yaw while sitting on the ground */
-			else if (!_vehicle_status.condition_landed &&
+			else if (!_vehicle_land_detected.landed &&
 					!(!_control_mode.flag_control_altitude_enabled && _manual.z < 0.1f)) {
 
 				/* we want to know the real constraint, and global overrides manual */
@@ -1939,7 +1949,7 @@ MulticopterPositionControl::task_main()
 				_att_sp.thrust = math::min(thr_val, _manual_thr_max.get());
 
 				/* enforce minimum throttle if not landed */
-				if (!_vehicle_status.condition_landed) {
+				if (!_vehicle_land_detected.landed) {
 					_att_sp.thrust = math::max(_att_sp.thrust, _manual_thr_min.get());
 				}
 			}

src/modules/navigator/mission.cpp

@@ -144,7 +144,7 @@ Mission::on_inactive()
 	check_mission_valid();
 
 	/* require takeoff after non-loiter or landing */
-	if (!_navigator->get_can_loiter_at_sp() || _navigator->get_vstatus()->condition_landed) {
+	if (!_navigator->get_can_loiter_at_sp() || _navigator->get_land_detected()->landed) {
 		_need_takeoff = true;
 	}
 }
@@ -278,7 +278,7 @@ Mission::update_offboard_mission()
 				_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_vstatus()->condition_landed);
+				_navigator->get_land_detected()->landed);
 
 		_navigator->get_mission_result()->valid = !failed;
 		if (!failed) {
@@ -413,7 +413,7 @@ Mission::set_mission_items()
 			 * https://en.wikipedia.org/wiki/Loiter_(aeronautics)
 			 */
 
-			if (_navigator->get_vstatus()->condition_landed) {
+			if (_navigator->get_land_detected()->landed) {
 				/* landed, refusing to take off without a mission */
 
 				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "no valid mission available, refusing takeoff");
@@ -585,7 +585,7 @@ Mission::set_mission_items()
 		if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION
 				&& _work_item_type != WORK_ITEM_TYPE_ALIGN
 				&& _navigator->get_vstatus()->is_rotary_wing
-				&& !_navigator->get_vstatus()->condition_landed
+				&& !_navigator->get_land_detected()->landed
 				&& has_next_position_item) {
 
 			new_work_item_type = WORK_ITEM_TYPE_ALIGN;
@@ -658,7 +658,7 @@ Mission::do_need_takeoff()
 		float takeoff_alt = calculate_takeoff_altitude(&_mission_item);
 
 		/* force takeoff if landed (additional protection) */
-		if (_navigator->get_vstatus()->condition_landed) {
+		if (_navigator->get_land_detected()->landed) {
 			_need_takeoff = true;
 
 		/* if in-air and already above takeoff height, don't do takeoff */
@@ -752,7 +752,7 @@ Mission::calculate_takeoff_altitude(struct mission_item_s *mission_item)
 	float takeoff_alt = get_absolute_altitude_for_item(*mission_item);
 
 	/* takeoff to at least NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */
-	if (_navigator->get_vstatus()->condition_landed) {
+	if (_navigator->get_land_detected()->landed) {
 		takeoff_alt = fmaxf(takeoff_alt, _navigator->get_global_position()->alt + _param_takeoff_alt.get());
 
 	} else {
@@ -1118,7 +1118,7 @@ Mission::check_mission_valid()
 				_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_vstatus()->condition_landed);
+				_navigator->get_land_detected()->landed);
 
 		_navigator->increment_mission_instance_count();
 		_navigator->set_mission_result_updated();

src/modules/navigator/mission_block.cpp

@@ -95,7 +95,7 @@ MissionBlock::is_mission_item_reached()
 
 		case NAV_CMD_LAND: /* fall through */
 		case NAV_CMD_VTOL_LAND:
-			return _navigator->get_vstatus()->condition_landed;
+			return _navigator->get_land_detected()->landed;
 
 		/* TODO: count turns */
 		/*_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||*/
@@ -413,7 +413,7 @@ MissionBlock::set_previous_pos_setpoint()
 void
 MissionBlock::set_loiter_item(struct mission_item_s *item, float min_clearance)
 {
-	if (_navigator->get_vstatus()->condition_landed) {
+	if (_navigator->get_land_detected()->landed) {
 		/* landed, don't takeoff, but switch to IDLE mode */
 		item->nav_cmd = NAV_CMD_IDLE;
 

src/modules/navigator/navigator.h

@@ -57,6 +57,7 @@
 #include <uORB/topics/mission_result.h>
 #include <uORB/topics/geofence_result.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_land_detected.h>
 
 #include "navigator_mode.h"
 #include "mission.h"
@@ -133,6 +134,7 @@ public:
 	 * Getters
 	 */
 	struct vehicle_status_s*	    get_vstatus() { return &_vstatus; }
+	struct vehicle_land_detected_s*	    get_land_detected() { return &_land_detected; }
 	struct vehicle_control_mode_s*	    get_control_mode() { return &_control_mode; }
 	struct vehicle_global_position_s*   get_global_position() { return &_global_pos; }
 	struct vehicle_gps_position_s*	    get_gps_position() { return &_gps_pos; }
@@ -200,6 +202,7 @@ private:
 	int		_sensor_combined_sub;		/**< sensor combined subscription */
 	int		_home_pos_sub;			/**< home position subscription */
 	int		_vstatus_sub;			/**< vehicle status subscription */
+	int		_land_detected_sub;		/**< vehicle land detected subscription */
 	int		_capabilities_sub;		/**< notification of vehicle capabilities updates */
 	int		_control_mode_sub;		/**< vehicle control mode subscription */
 	int		_onboard_mission_sub;		/**< onboard mission subscription */
@@ -215,6 +218,7 @@ private:
 							  when pos control is deactivated */
 
 	vehicle_status_s				_vstatus;		/**< vehicle status */
+	vehicle_land_detected_s				_land_detected;		/**< vehicle land_detected */
 	vehicle_control_mode_s				_control_mode;		/**< vehicle control mode */
 	vehicle_global_position_s			_global_pos;		/**< global vehicle position */
 	vehicle_gps_position_s				_gps_pos;		/**< gps position */
@@ -300,6 +304,11 @@ private:
 	 */
 	void		vehicle_status_update();
 
+	/**
+	 * Retrieve vehicle land detected
+	 */
+	void		vehicle_land_detected_update();
+
 	/**
 	 * Retrieve vehicle control mode
 	 */

src/modules/navigator/navigator_main.cpp

@@ -107,6 +107,7 @@ Navigator::Navigator() :
 	_gps_pos_sub(-1),
 	_home_pos_sub(-1),
 	_vstatus_sub(-1),
+	_land_detected_sub(-1),
 	_capabilities_sub(-1),
 	_control_mode_sub(-1),
 	_onboard_mission_sub(-1),
@@ -118,6 +119,7 @@ Navigator::Navigator() :
 	_geofence_result_pub(nullptr),
 	_att_sp_pub(nullptr),
 	_vstatus{},
+	_land_detected{},
 	_control_mode{},
 	_global_pos{},
 	_gps_pos{},
@@ -241,6 +243,12 @@ Navigator::vehicle_status_update()
 	}
 }
 
+void
+Navigator::vehicle_land_detected_update()
+{
+	orb_copy(ORB_ID(vehicle_land_detected), _land_detected_sub, &_land_detected);
+}
+
 void
 Navigator::vehicle_control_mode_update()
 {
@@ -290,6 +298,7 @@ Navigator::task_main()
 	_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
 	_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
 	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
+	_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
 	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_home_pos_sub = orb_subscribe(ORB_ID(home_position));
 	_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
@@ -299,6 +308,7 @@ Navigator::task_main()
 
 	/* copy all topics first time */
 	vehicle_status_update();
+	vehicle_land_detected_update();
 	vehicle_control_mode_update();
 	global_position_update();
 	gps_position_update();
@@ -374,6 +384,12 @@ Navigator::task_main()
 			vehicle_status_update();
 		}
 
+		/* vehicle land detected updated */
+		orb_check(_land_detected_sub, &updated);
+		if (updated) {
+			vehicle_land_detected_update();
+		}
+
 		/* navigation capabilities updated */
 		orb_check(_capabilities_sub, &updated);
 		if (updated) {

src/modules/navigator/rtl.cpp

@@ -94,7 +94,7 @@ RTL::on_activation()
 	/* decide where to enter the RTL procedure when we switch into it */
 	if (_rtl_state == RTL_STATE_NONE) {
 		/* for safety reasons don't go into RTL if landed */
-		if (_navigator->get_vstatus()->condition_landed) {
+		if (_navigator->get_land_detected()->landed) {
 			_rtl_state = RTL_STATE_LANDED;
 			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "no RTL when landed");
 

src/modules/sdlog2/sdlog2.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -40,6 +40,7 @@
  * @author Lorenz Meier <lm@inf.ethz.ch>
  * @author Anton Babushkin <anton.babushkin@me.com>
  * @author Ban Siesta <bansiesta@gmail.com>
+ * @author Julian Oes <julian@oes.ch>
  */
 
 #include <px4_config.h>
@@ -112,6 +113,7 @@
 #include <uORB/topics/ekf2_innovations.h>
 #include <uORB/topics/camera_trigger.h>
 #include <uORB/topics/ekf2_replay.h>
+#include <uORB/topics/vehicle_land_detected.h>
 
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
@@ -1156,6 +1158,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct ekf2_innovations_s innovations;
 		struct camera_trigger_s camera_trigger;
 		struct ekf2_replay_s replay;
+		struct vehicle_land_detected_s land_detected;
 	} buf;
 
 	memset(&buf, 0, sizeof(buf));
@@ -1213,6 +1216,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_EST6_s log_INO3;
 			struct log_RPL3_s log_RPL3;
 			struct log_RPL4_s log_RPL4;
+			struct log_LAND_s log_LAND;
 		} body;
 	} log_msg = {
 		LOG_PACKET_HEADER_INIT(0)
@@ -1260,6 +1264,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		int innov_sub;
 		int cam_trig_sub;
 		int replay_sub;
+		int land_detected_sub;
 	} subs;
 
 	subs.cmd_sub = -1;
@@ -1299,6 +1304,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 	subs.innov_sub = -1;
 	subs.cam_trig_sub = -1;
 	subs.replay_sub = -1;
+	subs.land_detected_sub = -1;
 
 	/* add new topics HERE */
 
@@ -1447,7 +1453,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_msg.body.log_STAT.main_state = buf_status.main_state;
 			log_msg.body.log_STAT.arming_state = buf_status.arming_state;
 			log_msg.body.log_STAT.failsafe = (uint8_t) buf_status.failsafe;
-			log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed;
 			log_msg.body.log_STAT.load = buf_status.load;
 			LOGBUFFER_WRITE_AND_COUNT(STAT);
 		}
@@ -2135,6 +2140,13 @@ int sdlog2_thread_main(int argc, char *argv[])
 			LOGBUFFER_WRITE_AND_COUNT(CAMT);
 		}
 
+		/* --- LAND DETECTED --- */
+		if (copy_if_updated(ORB_ID(vehicle_land_detected), &subs.land_detected_sub, &buf.land_detected)) {
+			log_msg.msg_type = LOG_LAND_MSG;
+			log_msg.body.log_LAND.landed = buf.land_detected.landed;
+			LOGBUFFER_WRITE_AND_COUNT(CTS);
+		}
+
 		pthread_mutex_lock(&logbuffer_mutex);
 
 		/* signal the other thread new data, but not yet unlock */

src/modules/sdlog2/sdlog2_messages.h

@@ -180,7 +180,6 @@ struct log_STAT_s {
 	uint8_t main_state;
 	uint8_t arming_state;
 	uint8_t failsafe;
-	uint8_t landed;
 	float load;
 };
 
@@ -582,6 +581,11 @@ struct log_CAMT_s {
 	uint32_t seq;
 };
 
+#define LOG_LAND_MSG 56
+struct log_LAND_s {
+	uint8_t landed;
+};
+
 /********** SYSTEM MESSAGES, ID > 0x80 **********/
 
 /* --- TIME - TIME STAMP --- */
@@ -623,7 +627,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(GPS, "QBffLLfffffBHHH",	"GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"),
 	LOG_FORMAT_S(ATTC, ATTC, "ffff",		"Roll,Pitch,Yaw,Thrust"),
 	LOG_FORMAT_S(ATC1, ATTC, "ffff",		"Roll,Pitch,Yaw,Thrust"),
-	LOG_FORMAT(STAT, "BBBBf",		"MainState,ArmS,Failsafe,Landed,Load"),
+	LOG_FORMAT(STAT, "BBBf",		"MainState,ArmS,Failsafe,Load"),
 	LOG_FORMAT(VTOL, "fBBB",		"Arsp,RwMode,TransMode,Failsafe"),
 	LOG_FORMAT(CTS, "fffffff", "Vx_b,Vy_b,Vz_b,Vinf,P,Q,R"),
 	LOG_FORMAT(RC, "ffffffffffffBBBL",		"C0,C1,C2,C3,C4,C5,C6,C7,C8,C9,C10,C11,RSSI,CNT,Lost,Drop"),
@@ -666,6 +670,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(RPL2, "QQLLiMMfffffffM", "Tpos,Tvel,lat,lon,alt,fix,nsats,eph,epv,sacc,v,vN,vE,vD,v_val"),
 	LOG_FORMAT(RPL3, "QffffIB", "Tflow,fx,fy,gx,gy,delT,qual"),
 	LOG_FORMAT(RPL4, "Qf", "Trng,rng"),
+	LOG_FORMAT(LAND, "B", "Landed"),
 	/* system-level messages, ID >= 0x80 */
 	/* FMT: don't write format of format message, it's useless */
 	LOG_FORMAT(TIME, "Q", "StartTime"),