Subsystem_info status flags & checks : Initial commit, updating the health flags in a centralized way mostly in commander and the votedSensorsUpdate function.

msg/subsystem_info.msg

@@ -11,10 +11,21 @@ uint64 SUBSYSTEM_TYPE_EXTERNALGROUNDTRUTH = 512
 uint64 SUBSYSTEM_TYPE_ANGULARRATECONTROL = 1024
 uint64 SUBSYSTEM_TYPE_ATTITUDESTABILIZATION = 2048
 uint64 SUBSYSTEM_TYPE_YAWPOSITION = 4096
-uint64 SUBSYSTEM_TYPE_ALTITUDECONTROL = 16384
+uint64 SUBSYSTEM_TYPE_ALTITUDECONTROL = 8192
-uint64 SUBSYSTEM_TYPE_POSITIONCONTROL = 32768
+uint64 SUBSYSTEM_TYPE_POSITIONCONTROL = 16384
-uint64 SUBSYSTEM_TYPE_MOTORCONTROL = 65536
+uint64 SUBSYSTEM_TYPE_MOTORCONTROL = 32768
-uint64 SUBSYSTEM_TYPE_RANGEFINDER = 131072
+uint64 SUBSYSTEM_TYPE_RCRECEIVER = 65536
+uint64 SUBSYSTEM_TYPE_GYRO2 = 131072
+uint64 SUBSYSTEM_TYPE_ACC2 = 262144
+uint64 SUBSYSTEM_TYPE_MAG2 = 524288
+uint64 SUBSYSTEM_TYPE_GEOFENCE = 1048576
+uint64 SUBSYSTEM_TYPE_AHRS = 2097152
+uint64 SUBSYSTEM_TYPE_TERRAIN = 4194304
+uint64 SUBSYSTEM_TYPE_REVERSEMOTOR = 8388608
+uint64 SUBSYSTEM_TYPE_LOGGING = 16777216
+uint64 SUBSYSTEM_TYPE_SENSORBATTERY = 33554432
+uint64 SUBSYSTEM_TYPE_SENSORPROXIMITY = 67108864
+uint64 SUBSYSTEM_TYPE_MISSION = 134217728
 
 bool present
 bool enabled

msg/vehicle_status_flags.msg

@@ -27,5 +27,6 @@ bool offboard_control_loss_timeout                # true if offboard is lost for
 
 bool rc_signal_found_once
 bool rc_input_blocked                                # set if RC input should be ignored temporarily
+bool rc_calibration_valid                            # set if RC calibration is valid
 bool vtol_transition_failure                        # Set to true if vtol transition failed
 bool usb_connected                                # status of the USB power supply

src/drivers/batt_smbus/batt_smbus.cpp

@@ -53,7 +53,6 @@
 #include <px4_workqueue.h>
 #include <perf/perf_counter.h>
 #include <uORB/topics/battery_status.h>
-#include <uORB/topics/subsystem_info.h>
 #include <uORB/uORB.h>
 
 #define BATT_SMBUS_ADDR_MIN             0x00	///< lowest possible address

src/drivers/differential_pressure/ets/ets_airspeed.cpp

@@ -54,7 +54,6 @@
 
 #include <uORB/uORB.h>
 #include <uORB/topics/differential_pressure.h>
-#include <uORB/topics/subsystem_info.h>
 #include <drivers/airspeed/airspeed.h>
 
 /* I2C bus address */

src/drivers/differential_pressure/ms4525/ms4525_airspeed.cpp

@@ -66,7 +66,6 @@
 
 #include <uORB/uORB.h>
 #include <uORB/topics/differential_pressure.h>
-#include <uORB/topics/subsystem_info.h>
 #include <uORB/topics/system_power.h>
 
 #include <drivers/airspeed/airspeed.h>

src/drivers/distance_sensor/hc_sr04/hc_sr04.cpp

@@ -58,6 +58,7 @@
 
 #include <perf/perf_counter.h>
 #include <systemlib/err.h>
+#include <systemlib/subsystem_info_pub.h>
 
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_range_finder.h>
@@ -73,7 +74,6 @@
 /* Configuration Constants */
 #define SR04_DEVICE_PATH	"/dev/hc_sr04"
 
-#define SUBSYSTEM_TYPE_RANGEFINDER 131072
 /* Device limits */
 #define SR04_MIN_DISTANCE 	(0.10f)
 #define SR04_MAX_DISTANCE 	(4.00f)
@@ -626,22 +626,7 @@ HC_SR04::start()
 
 
 	/* notify about state change */
-	struct subsystem_info_s info = {};
+	publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_SENSORPROXIMITY, true, true, true);
-	info.present = true;
-	info.enabled = true;
-	info.ok = true;
-	info.subsystem_type = SUBSYSTEM_TYPE_RANGEFINDER;
-
-	static orb_advert_t pub = nullptr;
-
-	if (pub != nullptr) {
-		orb_publish(ORB_ID(subsystem_info), pub, &info);
-
-
-	} else {
-		pub = orb_advertise(ORB_ID(subsystem_info), &info);
-
-	}
 }
 
 void

src/drivers/distance_sensor/mb12xx/mb12xx.cpp

@@ -61,6 +61,7 @@
 
 #include <perf/perf_counter.h>
 #include <systemlib/err.h>
+#include <systemlib/subsystem_info_pub.h>
 
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_range_finder.h>
@@ -590,22 +591,7 @@ MB12XX::start()
 	work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 5);
 
 	/* notify about state change */
-	struct subsystem_info_s info = {};
+	publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_SENSORPROXIMITY, true, true, true);
-	info.present = true;
-	info.enabled = true;
-	info.ok = true;
-	info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER;
-
-	static orb_advert_t pub = nullptr;
-
-	if (pub != nullptr) {
-		orb_publish(ORB_ID(subsystem_info), pub, &info);
-
-
-	} else {
-		pub = orb_advertise(ORB_ID(subsystem_info), &info);
-
-	}
 }
 
 void

src/drivers/distance_sensor/sf0x/sf0x.cpp

@@ -66,7 +66,6 @@
 #include <drivers/device/ringbuffer.h>
 
 #include <uORB/uORB.h>
-#include <uORB/topics/subsystem_info.h>
 #include <uORB/topics/distance_sensor.h>
 
 #include "sf0x_parser.h"
@@ -645,20 +644,7 @@ SF0X::start()
 	work_queue(HPWORK, &_work, (worker_t)&SF0X::cycle_trampoline, this, 1);
 
 	// /* notify about state change */
-	// struct subsystem_info_s info = {
+	// publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_SENSORPROXIMITY, true, true, true);
-	// 	true,
-	// 	true,
-	// 	true,
-	// 	SUBSYSTEM_TYPE_RANGEFINDER
-	// };
-	// static orb_advert_t pub = -1;
-
-	// if (pub > 0) {
-	// 	orb_publish(ORB_ID(subsystem_info), pub, &info);
-
-	// } else {
-	// 	pub = orb_advertise(ORB_ID(subsystem_info), &info);
-	// }
 }
 
 void

src/drivers/distance_sensor/sf1xx/sf1xx.cpp

@@ -64,12 +64,14 @@
 
 #include <perf/perf_counter.h>
 #include <systemlib/err.h>
+#include <systemlib/subsystem_info_pub.h>
 
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_range_finder.h>
 #include <drivers/device/ringbuffer.h>
 
 #include <uORB/uORB.h>
+#include <uORB/topics/subsystem_info.h>
 #include <uORB/topics/distance_sensor.h>
 
 #include <board_config.h>
@@ -584,6 +586,9 @@ SF1XX::start()
 
 	/* schedule a cycle to start things */
 	work_queue(HPWORK, &_work, (worker_t)&SF1XX::cycle_trampoline, this, USEC2TICK(_conversion_interval));
+
+	/* notify about state change */
+	publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_SENSORPROXIMITY, true, true, true);
 }
 
 void

src/drivers/distance_sensor/srf02/srf02.cpp

@@ -60,6 +60,7 @@
 
 #include <perf/perf_counter.h>
 #include <systemlib/err.h>
+#include <systemlib/subsystem_info_pub.h>
 
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_range_finder.h>
@@ -593,22 +594,7 @@ SRF02::start()
 	work_queue(HPWORK, &_work, (worker_t)&SRF02::cycle_trampoline, this, 5);
 
 	/* notify about state change */
-	struct subsystem_info_s info = {};
+	publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_SENSORPROXIMITY, true, true, true);
-	info.present = true;
-	info.enabled = true;
-	info.ok = true;
-	info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER;
-
-	static orb_advert_t pub = nullptr;
-
-	if (pub != nullptr) {
-		orb_publish(ORB_ID(subsystem_info), pub, &info);
-
-
-	} else {
-		pub = orb_advertise(ORB_ID(subsystem_info), &info);
-
-	}
 }
 
 void

src/drivers/distance_sensor/teraranger/teraranger.cpp

@@ -60,6 +60,7 @@
 
 #include <perf/perf_counter.h>
 #include <systemlib/err.h>
+#include <systemlib/subsystem_info_pub.h>
 
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_range_finder.h>
@@ -670,20 +671,7 @@ TERARANGER::start()
 	work_queue(HPWORK, &_work, (worker_t)&TERARANGER::cycle_trampoline, this, 1);
 
 	/* notify about state change */
-	struct subsystem_info_s info = {};
+	publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_SENSORPROXIMITY, true, true, true);
-	info.present = true;
-	info.enabled = true;
-	info.ok = true;
-	info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER;
-
-	static orb_advert_t pub = nullptr;
-
-	if (pub != nullptr) {
-		orb_publish(ORB_ID(subsystem_info), pub, &info);
-
-	} else {
-		pub = orb_advertise(ORB_ID(subsystem_info), &info);
-	}
 }
 
 void

src/drivers/distance_sensor/tfmini/tfmini.cpp

@@ -61,6 +61,7 @@
 
 #include <perf/perf_counter.h>
 #include <systemlib/err.h>
+#include <systemlib/subsystem_info_pub.h>
 
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_range_finder.h>
@@ -609,22 +610,7 @@ TFMINI::start()
 	work_queue(HPWORK, &_work, (worker_t)&TFMINI::cycle_trampoline, this, 1);
 
 	/* notify about state change */
-	struct subsystem_info_s info = {};
+	publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_SENSORPROXIMITY, true, true, true);
-	info.present = true;
-	info.enabled = true;
-	info.ok = true;
-	info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER;
-
-	static orb_advert_t pub = nullptr;
-
-	if (pub != nullptr) {
-		orb_publish(ORB_ID(subsystem_info), pub, &info);
-
-
-	} else {
-		pub = orb_advertise(ORB_ID(subsystem_info), &info);
-
-	}
 }
 
 void

src/drivers/pwm_input/pwm_input.cpp

@@ -73,7 +73,6 @@
 
 #include <uORB/uORB.h>
 #include <uORB/topics/pwm_input.h>
-#include <uORB/topics/subsystem_info.h>
 
 #include <drivers/drv_device.h>
 #include <drivers/device/device.h>

src/drivers/px4flow/px4flow.cpp

@@ -65,6 +65,7 @@
 #include <perf/perf_counter.h>
 #include <systemlib/err.h>
 #include <parameters/param.h>
+#include <systemlib/subsystem_info_pub.h>
 
 #include <conversion/rotation.h>
 
@@ -589,20 +590,7 @@ PX4FLOW::start()
 	work_queue(HPWORK, &_work, (worker_t)&PX4FLOW::cycle_trampoline, this, 1);
 
 	/* notify about state change */
-	struct subsystem_info_s info = {};
+	publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_OPTICALFLOW, true, true, true);
-	info.present = true;
-	info.enabled = true;
-	info.ok = true;
-	info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_OPTICALFLOW;
-
-	static orb_advert_t pub = nullptr;
-
-	if (pub != nullptr) {
-		orb_publish(ORB_ID(subsystem_info), pub, &info);
-
-	} else {
-		pub = orb_advertise(ORB_ID(subsystem_info), &info);
-	}
 }
 
 void

src/lib/drivers/airspeed/airspeed.cpp

@@ -54,20 +54,17 @@
 
 #include <uORB/uORB.h>
 #include <uORB/topics/differential_pressure.h>
-#include <uORB/topics/subsystem_info.h>
 
 #include <drivers/airspeed/airspeed.h>
 
 Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const char *path) :
 	I2C("Airspeed", path, bus, address, 100000),
 	_sensor_ok(false),
-	_last_published_sensor_ok(true), /* initialize differently to force publication */
 	_measure_ticks(0),
 	_collect_phase(false),
 	_diff_pres_offset(0.0f),
 	_airspeed_pub(nullptr),
 	_airspeed_orb_class_instance(-1),
-	_subsys_pub(nullptr),
 	_class_instance(-1),
 	_conversion_interval(conversion_interval),
 	_sample_perf(perf_alloc(PC_ELAPSED, "aspd_read")),
@@ -248,33 +245,9 @@ Airspeed::stop()
 	work_cancel(HPWORK, &_work);
 }
 
-void
-Airspeed::update_status()
-{
-	if (_sensor_ok != _last_published_sensor_ok) {
-		/* notify about state change */
-		struct subsystem_info_s info = {};
-		info.present = true;
-		info.enabled = true;
-		info.ok = _sensor_ok;
-		info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_DIFFPRESSURE;
-
-		if (_subsys_pub != nullptr) {
-			orb_publish(ORB_ID(subsystem_info), _subsys_pub, &info);
-
-		} else {
-			_subsys_pub = orb_advertise(ORB_ID(subsystem_info), &info);
-		}
-
-		_last_published_sensor_ok = _sensor_ok;
-	}
-}
-
 void
 Airspeed::cycle_trampoline(void *arg)
 {
 	Airspeed *dev = (Airspeed *)arg;
 	dev->cycle();
-
-	dev->update_status();
 }

src/lib/drivers/airspeed/airspeed.h

@@ -43,7 +43,6 @@
 #include <systemlib/airspeed.h>
 #include <perf/perf_counter.h>
 #include <uORB/topics/differential_pressure.h>
-#include <uORB/topics/subsystem_info.h>
 #include <uORB/uORB.h>
 
 /* Default I2C bus */
@@ -75,14 +74,8 @@ protected:
 	virtual int	measure() = 0;
 	virtual int	collect() = 0;
 
-	/**
-	 * Update the subsystem status
-	 */
-	void update_status();
-
 	work_s			_work;
 	bool			_sensor_ok;
-	bool			_last_published_sensor_ok;
 	uint32_t		_measure_ticks;
 	bool			_collect_phase;
 	float			_diff_pres_offset;
@@ -90,8 +83,6 @@ protected:
 	orb_advert_t		_airspeed_pub;
 	int			_airspeed_orb_class_instance;
 
-	orb_advert_t		_subsys_pub;
-
 	int			_class_instance;
 
 	unsigned		_conversion_interval;

src/modules/commander/PreflightCheck.cpp

@@ -55,6 +55,7 @@
 #include <parameters/param.h>
 #include <systemlib/rc_check.h>
 #include <systemlib/mavlink_log.h>
+#include <systemlib/subsystem_info_pub.h>
 
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_mag.h>
@@ -69,6 +70,7 @@
 #include <uORB/topics/sensor_preflight.h>
 #include <uORB/topics/system_power.h>
 #include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/subsystem_info.h>
 
 #include "PreflightCheck.h"
 
@@ -118,7 +120,9 @@ static int check_calibration(DevHandle &h, const char *param_template, int &devi
 
 static bool magnometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool optional, int &device_id, bool report_fail)
 {
+	bool present = true;
 	bool success = true;
+	int ret = 0;
 
 	char s[30];
 	sprintf(s, "%s%u", MAG_BASE_DEVICE_PATH, instance);
@@ -131,11 +135,12 @@ static bool magnometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bo
 				mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: NO MAG SENSOR #%u", instance);
 			}
 		}
-
+		present = false;
-		return false;
+		success = false;
+		goto out;
 	}
 
-	int ret = check_calibration(h, "CAL_MAG%u_ID", device_id);
+	ret = check_calibration(h, "CAL_MAG%u_ID", device_id);
 
 	if (ret) {
 		if (report_fail) {
@@ -158,6 +163,9 @@ static bool magnometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bo
 	}
 
 out:
+	if(instance==0) publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_MAG, present, !optional, success);
+	if(instance==1) publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_MAG2, present, !optional, success);
+
 	DevMgr::releaseHandle(h);
 	return success;
 }
@@ -182,8 +190,9 @@ static bool imuConsistencyCheck(orb_advert_t *mavlink_log_pub, bool report_statu
 	if (sensors.accel_inconsistency_m_s_s > test_limit) {
 		if (report_status) {
 			mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCELS INCONSISTENT - CHECK CAL");
+			publish_subsystem_info_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC, false);
+			publish_subsystem_info_healthy(subsystem_info_s::SUBSYSTEM_TYPE_ACC2, false);
 		}
-
 		success = false;
 		goto out;
 
@@ -199,6 +208,8 @@ static bool imuConsistencyCheck(orb_advert_t *mavlink_log_pub, bool report_statu
 	if (sensors.gyro_inconsistency_rad_s > test_limit) {
 		if (report_status) {
 			mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GYROS INCONSISTENT - CHECK CAL");
+			publish_subsystem_info_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, false);
+			publish_subsystem_info_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GYRO2, false);
 		}
 
 		success = false;
@@ -238,7 +249,8 @@ static bool magConsistencyCheck(orb_advert_t *mavlink_log_pub, bool report_statu
 		if (report_status) {
 			mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: MAG SENSORS INCONSISTENT");
 		}
-
+		publish_subsystem_info_healthy(subsystem_info_s::SUBSYSTEM_TYPE_MAG, false);
+		publish_subsystem_info_healthy(subsystem_info_s::SUBSYSTEM_TYPE_MAG2, false);
 		return false;
 	}
 
@@ -247,7 +259,9 @@ static bool magConsistencyCheck(orb_advert_t *mavlink_log_pub, bool report_statu
 
 static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool optional, bool dynamic, int &device_id, bool report_fail)
 {
+	bool present = true;
 	bool success = true;
+	int ret = 0;
 
 	char s[30];
 	sprintf(s, "%s%u", ACCEL_BASE_DEVICE_PATH, instance);
@@ -260,11 +274,12 @@ static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance,
 				mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: NO ACCEL SENSOR #%u", instance);
 			}
 		}
-
+		present = false;
-		return false;
+		success = false;
+		goto out;
 	}
 
-	int ret = check_calibration(h, "CAL_ACC%u_ID", device_id);
+	ret = check_calibration(h, "CAL_ACC%u_ID", device_id);
 
 	if (ret) {
 		if (report_fail) {
@@ -321,13 +336,18 @@ static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance,
 #endif
 
 out:
+	if(instance==0) publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_ACC, present, !optional, success);
+	if(instance==1) publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_ACC2, present, !optional, success);
+
 	DevMgr::releaseHandle(h);
 	return success;
 }
 
 static bool gyroCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool optional, int &device_id, bool report_fail)
 {
+	bool present = true;
 	bool success = true;
+	int ret = 0;
 
 	char s[30];
 	sprintf(s, "%s%u", GYRO_BASE_DEVICE_PATH, instance);
@@ -340,11 +360,12 @@ static bool gyroCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool opt
 				mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: NO GYRO SENSOR #%u", instance);
 			}
 		}
-
+		present = false;
-		return false;
+		success = false;
+		goto out;
 	}
 
-	int ret = check_calibration(h, "CAL_GYRO%u_ID", device_id);
+	ret = check_calibration(h, "CAL_GYRO%u_ID", device_id);
 
 	if (ret) {
 		if (report_fail) {
@@ -367,6 +388,9 @@ static bool gyroCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool opt
 	}
 
 out:
+	if(instance==0) publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, present, !optional, success);
+	if(instance==1) publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_GYRO2, present, !optional, success);
+
 	DevMgr::releaseHandle(h);
 	return success;
 }
@@ -386,7 +410,7 @@ static bool baroCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool opt
 				mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: NO BARO SENSOR #%u", instance);
 			}
 		}
-
+		publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE, false, !optional, false);
 		return false;
 	}
 
@@ -402,13 +426,14 @@ static bool baroCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool opt
 	// }
 
 //out:
-
+	publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE, true, !optional, success);
 	DevMgr::releaseHandle(h);
 	return success;
 }
 
 static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool report_fail, bool prearm)
 {
+	bool present = true;
 	bool success = true;
 
 	int fd_airspeed = orb_subscribe(ORB_ID(airspeed));
@@ -419,20 +444,20 @@ static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool rep
 
 	if ((orb_copy(ORB_ID(differential_pressure), fd_diffpres, &differential_pressure) != PX4_OK) ||
 	    (hrt_elapsed_time(&differential_pressure.timestamp) > 1000000)) {
-		if (report_fail) {
+		if (report_fail && !optional) {
 			mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING");
 		}
-
+		present = false;
 		success = false;
 		goto out;
 	}
 
 	if ((orb_copy(ORB_ID(airspeed), fd_airspeed, &airspeed) != PX4_OK) ||
 	    (hrt_elapsed_time(&airspeed.timestamp) > 1000000)) {
-		if (report_fail) {
+		if (report_fail && !optional) {
 			mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING");
 		}
-
+		present = false;
 		success = false;
 		goto out;
 	}
@@ -447,7 +472,7 @@ static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool rep
 		if (report_fail) {
 			mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED SENSOR STUCK");
 		}
-
+		present = true;
 		success = false;
 		goto out;
 	}
@@ -461,12 +486,13 @@ static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool rep
 		if (report_fail) {
 			mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: CHECK AIRSPEED CAL OR PITOT");
 		}
-
+		present = true;
 		success = false;
 		goto out;
 	}
 
 out:
+	publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_DIFFPRESSURE, present, !optional, success);
 	orb_unsubscribe(fd_airspeed);
 	orb_unsubscribe(fd_diffpres);
 	return success;
@@ -523,6 +549,7 @@ static bool powerCheck(orb_advert_t *mavlink_log_pub, bool report_fail, bool pre
 static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_fail, bool enforce_gps_required)
 {
 	bool success = true; // start with a pass and change to a fail if any test fails
+	bool present = true;
 	float test_limit = 1.0f; // pass limit re-used for each test
 
 	// Get estimator status data if available and exit with a fail recorded if not
@@ -530,6 +557,7 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_
 	estimator_status_s status = {};
 
 	if (orb_copy(ORB_ID(estimator_status), sub, &status) != PX4_OK) {
+		present = false;
 		goto out;
 	}
 
@@ -626,12 +654,13 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_
 			// The EKF is not using GPS
 			if (report_fail) {
 				if (ekf_gps_check_fail) {
-					// Poor GPS qulaity is the likely cause
+					// Poor GPS quality is the likely cause
 					mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GPS QUALITY POOR");
-
+					publish_subsystem_info_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GPS, false);
 				} else {
 					// Likely cause unknown
 					mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF NOT USING GPS");
+					publish_subsystem_info_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GPS, false);
 				}
 			}
 
@@ -649,6 +678,7 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_
 			if (gps_quality_fail) {
 				if (report_fail) {
 					mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GPS QUALITY POOR");
+					publish_subsystem_info_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GPS, false);
 				}
 
 				success = false;
@@ -658,15 +688,15 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_
 	}
 
 out:
+	publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, present, !optional, success && present);
 	orb_unsubscribe(sub);
 	return success;
 }
 
 bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status,
-		    const vehicle_status_flags_s &status_flags, bool checkGNSS, bool reportFailures, bool prearm,
+		    vehicle_status_flags_s &status_flags, bool checkGNSS, bool reportFailures, bool prearm,
 		    const hrt_abstime &time_since_boot)
 {
-
 	if (time_since_boot < 2000000) {
 		// the airspeed driver filter doesn't deliver the actual value yet
 		reportFailures = false;
@@ -737,7 +767,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &statu
 			if ((reportFailures && !failed)) {
 				mavlink_log_critical(mavlink_log_pub, "Primary compass not found");
 			}
-
+			publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_MAG, false, true, false);
 			failed = true;
 		}
 
@@ -775,7 +805,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &statu
 			if ((reportFailures && !failed)) {
 				mavlink_log_critical(mavlink_log_pub, "Primary accelerometer not found");
 			}
-
+			publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_ACC, false, true, false);
 			failed = true;
 		}
 	}
@@ -808,7 +838,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &statu
 			if ((reportFailures && !failed)) {
 				mavlink_log_critical(mavlink_log_pub, "Primary gyro not found");
 			}
-
+			publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_GYRO, false, true, false);
 			failed = true;
 		}
 	}
@@ -844,7 +874,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &statu
 			if (reportFailures && !failed) {
 				mavlink_log_critical(mavlink_log_pub, "Primary barometer not operational");
 			}
-
+			publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE, false, true, false);
 			failed = true;
 		}
 	}
@@ -858,7 +888,10 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &statu
 
 	/* ---- AIRSPEED ---- */
 	if (checkAirspeed) {
-		if (!airspeedCheck(mavlink_log_pub, true, reportFailures && !failed, prearm)) {
+		int32_t optional = 0; //Given checkAirspeed==true, assume the airspeed sensor is also required by default
+		param_get(param_find("FW_ARSP_MODE"), &optional);
+
+		if (!airspeedCheck(mavlink_log_pub, (bool)optional, reportFailures && !failed, prearm) && !(bool)optional) {
 			failed = true;
 		}
 	}
@@ -871,6 +904,13 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &statu
 			}
 
 			failed = true;
+			
+			publish_subsystem_info_enabled_healthy(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, false);
+			status_flags.rc_calibration_valid = false;
+		} else {
+			// The calibration is fine, but only set the overall health state to true if the signal is not currently lost
+			status_flags.rc_calibration_valid = true;
+			publish_subsystem_info_enabled_healthy(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, !status.rc_signal_lost);
 		}
 	}
 
@@ -881,6 +921,29 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &statu
 		}
 	}
 
+	/* ---- GPS ---- */
+	if (checkGNSS) {
+		int fd_gps = orb_subscribe(ORB_ID(vehicle_gps_position));
+		vehicle_gps_position_s gps = {};
+		bool present = true;
+
+		if ((orb_copy(ORB_ID(vehicle_gps_position), fd_gps, &gps) != PX4_OK) ||
+			(hrt_elapsed_time(&gps.timestamp) > 2000000)) {
+			if (reportFailures) {
+				mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GPS MODULE MISSING");
+			}
+			present = false;
+		}
+
+		// Mark GPS as required (given that checkGNSS=true) and indicate whether it is present. For now also assume it is healthy
+		// if there is a lock ... EKF2 will then set the healthy=false if its more extensive GPS checks fail in the next step.
+		publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_GPS, present, true, present && gps.fix_type>=3);
+	}
+
+	// TODO: Add rangefinder here. We have the SENS_EN_XXX params that tell us what we should have. This is currently completely done inside the driver.
+
+	// TODO: Add optical flow check here? This is currently completely done inside the driver.
+
 	/* ---- Navigation EKF ---- */
 	// only check EKF2 data if EKF2 is selected as the estimator and GNSS checking is enabled
 	int32_t estimator_type;
@@ -889,8 +952,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &statu
 	if (estimator_type == 2) {
 		// don't report ekf failures for the first 10 seconds to allow time for the filter to start
 		bool report_ekf_fail = (time_since_boot > 10 * 1000000);
-
+		if (!ekf2Check(mavlink_log_pub, false, reportFailures && report_ekf_fail && !failed, checkGNSS)) {
-		if (!ekf2Check(mavlink_log_pub, true, (reportFailures && report_ekf_fail && !failed), checkGNSS)) {
 			failed = true;
 		}
 	}

src/modules/commander/PreflightCheck.h

@@ -43,6 +43,8 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_status_flags.h>
 
+#include <uORB/topics/vehicle_status_flags.h>
+
 #pragma once
 
 namespace Preflight
@@ -73,7 +75,7 @@ namespace Preflight
 *   true if the system power should be checked
 **/
 bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status,
-		    const vehicle_status_flags_s &status_flags, bool checkGNSS, bool reportFailures, bool prearm,
+		    vehicle_status_flags_s &status_flags, bool checkGNSS, bool reportFailures, bool prearm,
 		    const hrt_abstime &time_since_boot);
 
 static constexpr unsigned max_mandatory_gyro_count = 1;

src/modules/commander/commander.cpp

@@ -79,6 +79,7 @@
 #include <parameters/param.h>
 #include <systemlib/rc_check.h>
 #include <systemlib/state_table.h>
+#include <systemlib/subsystem_info_pub.h>
 
 #include <cmath>
 #include <cfloat>
@@ -1256,6 +1257,7 @@ Commander::run()
 
 	status_flags.condition_power_input_valid = true;
 	status_flags.usb_connected = false;
+	status_flags.rc_calibration_valid = true;
 
 	// CIRCUIT BREAKERS
 	status_flags.circuit_breaker_engaged_power_check = false;
@@ -1270,6 +1272,11 @@ Commander::run()
 	status_flags.condition_local_velocity_valid = false;
 	status_flags.condition_local_altitude_valid = false;
 
+	bool status_changed = true;
+
+	/* initialize the vehicle status flag helper functions. This also initializes the sensor health flags*/
+	publish_subsystem_info_init(&status, &status_changed);
+
 	/* publish initial state */
 	status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
 
@@ -1310,7 +1317,6 @@ Commander::run()
 	bool emergency_battery_voltage_actions_done = false;
 	bool dangerous_battery_level_requests_poweroff = false;
 
-	bool status_changed = true;
 	bool param_init_forced = true;
 
 	bool updated = false;
@@ -1825,6 +1831,12 @@ Commander::run()
 			}
 		}
 
+		if((_last_condition_global_position_valid != status_flags.condition_global_position_valid) && status_flags.condition_global_position_valid) {
+			// If global position state changed and is now valid, set respective health flags to true. For now also assume GPS is OK if global pos is OK, but not vice versa.
+			publish_subsystem_info_healthy(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, true);
+			publish_subsystem_info_present_healthy(subsystem_info_s::SUBSYSTEM_TYPE_GPS, true, true);
+		}
+
 		check_valid(_local_position_sub.get().timestamp, _failsafe_pos_delay.get() * 1_s, _local_position_sub.get().z_valid, &(status_flags.condition_local_altitude_valid), &status_changed);
 
 		/* Update land detector */
@@ -1993,41 +2005,6 @@ Commander::run()
 			}
 		}
 
-		/* update subsystem */
-		orb_check(subsys_sub, &updated);
-
-		if (updated) {
-			orb_copy(ORB_ID(subsystem_info), subsys_sub, &info);
-
-			//warnx("subsystem changed: %d\n", (int)info.subsystem_type);
-
-			/* mark / unmark as present */
-			if (info.present) {
-				status.onboard_control_sensors_present |= info.subsystem_type;
-
-			} else {
-				status.onboard_control_sensors_present &= ~info.subsystem_type;
-			}
-
-			/* mark / unmark as enabled */
-			if (info.enabled) {
-				status.onboard_control_sensors_enabled |= info.subsystem_type;
-
-			} else {
-				status.onboard_control_sensors_enabled &= ~info.subsystem_type;
-			}
-
-			/* mark / unmark as ok */
-			if (info.ok) {
-				status.onboard_control_sensors_health |= info.subsystem_type;
-
-			} else {
-				status.onboard_control_sensors_health &= ~info.subsystem_type;
-			}
-
-			status_changed = true;
-		}
-
 		/* If in INIT state, try to proceed to STANDBY state */
 		if (!status_flags.condition_calibration_enabled && status.arming_state == vehicle_status_s::ARMING_STATE_INIT) {
 
@@ -2223,11 +2200,13 @@ Commander::run()
 			/* handle the case where RC signal was regained */
 			if (!status_flags.rc_signal_found_once) {
 				status_flags.rc_signal_found_once = true;
+				publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, true && status_flags.rc_calibration_valid);
 				status_changed = true;
 
 			} else {
 				if (status.rc_signal_lost) {
 					mavlink_log_info(&mavlink_log_pub, "MANUAL CONTROL REGAINED after %llums", hrt_elapsed_time(&rc_signal_lost_timestamp) / 1000);
+					publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, true && status_flags.rc_calibration_valid);
 					status_changed = true;
 				}
 			}
@@ -2375,6 +2354,7 @@ Commander::run()
 				mavlink_log_critical(&mavlink_log_pub, "MANUAL CONTROL LOST (at t=%llums)", hrt_absolute_time() / 1000);
 				status.rc_signal_lost = true;
 				rc_signal_lost_timestamp = sp_man.timestamp;
+				publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, false);
 				status_changed = true;
 			}
 		}
@@ -4096,6 +4076,9 @@ void Commander::poll_telemetry_status()
 				    (mission_result.instance_count > 0) && !mission_result.valid) {
 
 					mavlink_log_critical(&mavlink_log_pub, "Planned mission fails check. Please upload again.");
+					//publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_MISSION, true, true, false); // TODO
+				} else {
+					//publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_MISSION, true, true, true); // TODO
 				}
 			}
 

src/modules/sensors/voted_sensors_update.cpp

@@ -40,6 +40,7 @@
 #include "voted_sensors_update.h"
 
 #include <systemlib/mavlink_log.h>
+#include <systemlib/subsystem_info_pub.h>
 
 #include <conversion/rotation.h>
 #include <ecl/geo/geo.h>
@@ -897,18 +898,15 @@ bool VotedSensorsUpdate::check_failover(SensorData &sensor, const char *sensor_n
 	if (sensor.last_failover_count != sensor.voter.failover_count()) {
 
 		uint32_t flags = sensor.voter.failover_state();
+		int failover_index = sensor.voter.failover_index();
 
 		if (flags == DataValidator::ERROR_FLAG_NO_ERROR) {
-			int failover_index = sensor.voter.failover_index();
-
 			if (failover_index != -1) {
 				//we switched due to a non-critical reason. No need to panic.
 				PX4_INFO("%s sensor switch from #%i", sensor_name, failover_index);
 			}
 
 		} else {
-			int failover_index = sensor.voter.failover_index();
-
 			if (failover_index != -1) {
 				mavlink_log_emergency(&_mavlink_log_pub, "%s #%i fail: %s%s%s%s%s!",
 						      sensor_name,
@@ -921,6 +919,40 @@ bool VotedSensorsUpdate::check_failover(SensorData &sensor, const char *sensor_n
 
 				// reduce priority of failed sensor to the minimum
 				sensor.priority[failover_index] = 1;
+
+				// Update the subsystem_info uORB given that a sensor failed
+				int ctr_valid = 0;
+
+				for (uint8_t i = 0; i < sensor.subscription_count; i++) {
+					if (sensor.priority[i] > 1) { ctr_valid++; }
+
+					PX4_WARN("FAILOVER event (idx=%u)! Sensor %s: Nr. %u Priority: %u", failover_index, sensor_name, i, sensor.priority[i]);
+				}
+
+				PX4_INFO("%s sensor switch from #%i", sensor_name, failover_index);
+
+				if (ctr_valid < 2) { // subsystem_info only contains flags for the first two sensors
+					uint64_t subsystem_type = 0;
+
+					if (ctr_valid == 0) { // There are no valid sensors left!
+						if (strcmp(sensor_name, "Gyro") == 0) { subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_GYRO; }
+
+						if (strcmp(sensor_name, "Accel") == 0) { subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_ACC; }
+
+						if (strcmp(sensor_name, "Mag") == 0) { subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_MAG; }
+
+						if (strcmp(sensor_name, "Baro") == 0) { subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE; }
+
+					} else if (ctr_valid == 1) { // A single valid sensor remains, set secondary sensor health to false
+						if (strcmp(sensor_name, "Gyro") == 0) { subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_GYRO2; }
+
+						if (strcmp(sensor_name, "Accel") == 0) { subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_ACC2; }
+
+						if (strcmp(sensor_name, "Mag") == 0) { subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_MAG2; }
+					}
+
+					publish_subsystem_info_healthy(subsystem_type, false);
+				}
 			}
 		}
 
@@ -951,6 +983,30 @@ void VotedSensorsUpdate::init_sensor_class(const struct orb_metadata *meta, Sens
 					PX4_ERR("failed to add validator for sensor %s %i", meta->o_name, i);
 				}
 			}
+
+			// Update the subsystem_info uORB to indicate the amount of valid sensors
+			if (i < 2) { // subsystem_info only contains flags for the first two sensors
+				uint64_t subsystem_type = 0;
+
+				if (i == 0) { // First sensor valid
+					if (strcmp(meta->o_name, "sensor_gyro") == 0) { subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_GYRO; }
+
+					if (strcmp(meta->o_name, "sensor_accel") == 0) { subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_ACC; }
+
+					if (strcmp(meta->o_name, "sensor_mag") == 0) { subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_MAG; }
+
+					if (strcmp(meta->o_name, "sensor_baro") == 0) { subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_ABSPRESSURE; }
+
+				} else if (i == 1) { // We also have a second sensor
+					if (strcmp(meta->o_name, "sensor_gyro") == 0) { subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_GYRO2; }
+
+					if (strcmp(meta->o_name, "sensor_accel") == 0) { subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_ACC2; }
+
+					if (strcmp(meta->o_name, "sensor_mag") == 0) { subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_MAG2; }
+				}
+
+				publish_subsystem_info(subsystem_type, true, true, true);
+			}
 		}
 	}
 

src/modules/sensors/voted_sensors_update.h

@@ -58,6 +58,7 @@
 #include <uORB/topics/sensor_selection.h>
 #include <uORB/topics/vehicle_air_data.h>
 #include <uORB/topics/vehicle_magnetometer.h>
+#include <uORB/topics/subsystem_info.h>
 
 #include <DevMgr.hpp>
 

src/modules/simulator/airspeedsim/airspeedsim.cpp

@@ -65,7 +65,6 @@
 
 #include <uORB/uORB.h>
 #include <uORB/topics/differential_pressure.h>
-#include <uORB/topics/subsystem_info.h>
 
 #include <simulator/simulator.h>
 
@@ -76,12 +75,10 @@ AirspeedSim::AirspeedSim(int bus, int address, unsigned conversion_interval, con
 	_reports(nullptr),
 	_retries(0),
 	_sensor_ok(false),
-	_last_published_sensor_ok(true), /* initialize differently to force publication */
 	_measure_ticks(0),
 	_collect_phase(false),
 	_diff_pres_offset(0.0f),
 	_airspeed_pub(nullptr),
-	_subsys_pub(nullptr),
 	_class_instance(-1),
 	_conversion_interval(conversion_interval),
 	_sample_perf(perf_alloc(PC_ELAPSED, "airspeed_read")),
@@ -360,38 +357,12 @@ AirspeedSim::stop()
 	work_cancel(HPWORK, &_work);
 }
 
-void
-AirspeedSim::update_status()
-{
-	if (_sensor_ok != _last_published_sensor_ok) {
-		/* notify about state change */
-		struct subsystem_info_s info = {};
-		info.present = true;
-		info.enabled = true;
-		info.ok = _sensor_ok;
-		info.subsystem_type = subsystem_info_s::SUBSYSTEM_TYPE_DIFFPRESSURE;
-
-		if (_subsys_pub != nullptr) {
-			orb_publish(ORB_ID(subsystem_info), _subsys_pub, &info);
-
-		} else {
-			_subsys_pub = orb_advertise(ORB_ID(subsystem_info), &info);
-		}
-
-		_last_published_sensor_ok = _sensor_ok;
-	}
-}
-
 void
 AirspeedSim::cycle_trampoline(void *arg)
 {
 	AirspeedSim *dev = (AirspeedSim *)arg;
 
 	dev->cycle();
-	// XXX we do not know if this is
-	// really helping - do not update the
-	// subsys state right now
-	//dev->update_status();
 }
 
 void

src/modules/simulator/airspeedsim/airspeedsim.h

@@ -69,7 +69,6 @@
 
 #include <uORB/uORB.h>
 #include <uORB/topics/differential_pressure.h>
-#include <uORB/topics/subsystem_info.h>
 
 /* Default I2C bus */
 #define PX4_I2C_BUS_DEFAULT		PX4_I2C_BUS_EXPANSION
@@ -117,20 +116,13 @@ protected:
 	virtual int	transfer(const uint8_t *send, unsigned send_len,
 				 uint8_t *recv, unsigned recv_len);
 
-	/**
-	 * Update the subsystem status
-	 */
-	void update_status();
-
 	struct work_s			_work;
 	bool			_sensor_ok;
-	bool			_last_published_sensor_ok;
 	unsigned			_measure_ticks;
 	bool			_collect_phase;
 	float			_diff_pres_offset;
 
 	orb_advert_t		_airspeed_pub;
-	orb_advert_t		_subsys_pub;
 
 	int			_class_instance;
 

src/modules/simulator/airspeedsim/meas_airspeed_sim.cpp

@@ -71,7 +71,6 @@
 
 #include <uORB/uORB.h>
 #include <uORB/topics/differential_pressure.h>
-#include <uORB/topics/subsystem_info.h>
 #include <uORB/topics/system_power.h>
 
 #include "airspeedsim.h"

src/modules/systemlib/CMakeLists.txt

@@ -44,6 +44,7 @@ set(SRCS
 	pid/pid.c
 	pwm_limit/pwm_limit.c
 	rc_check.c
+	subsystem_info_pub.cpp
 	)
 
 if(${OS} STREQUAL "nuttx")

src/modules/systemlib/subsystem_info_pub.cpp

@@ -0,0 +1,148 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file subsystem_info_pub.cpp
+ *
+ * Contains helper functions to efficiently publish the subsystem_info messages from various locations inside the code. It is basically a
+ * helper function for commander. Approach:
+ * 	- Before commander starts (which happens after some of the drivers have already published the respective subsystem_info), this helper
+ * 	  code stores all requests for a publish_subsystem_info in the internal_status variable
+ * 	- When commander starts up, it calls the publish_subsystem_info_init function. This 1) copies the internal_status into commander's
+ * 	  vehicle status variable and 2) assigns the status pointer to commanders vehicle status
+ * 	- After that, all requests to publish_subsystem_info are directly written to commander's vehicle status such that it is always up
+ * 	  to date. Commander then publishes the vehicle_status uORB (and is in fact the only app that does that, which is why this approach works)
+ *
+ * @author Philipp Oettershagen (philipp.oettershagen@mavt.ethz.ch)
+ */
+
+#include "subsystem_info_pub.h"
+
+vehicle_status_s internal_status = {};
+vehicle_status_s *status = &internal_status;
+bool *status_changed = nullptr;
+
+/* initialize pointer to commander's vehicle status variable */
+void publish_subsystem_info_init(vehicle_status_s *commander_vehicle_status_ptr, bool *commander_status_changed_ptr)
+{
+	status = commander_vehicle_status_ptr;
+	status_changed = commander_status_changed_ptr;
+
+	status->onboard_control_sensors_present = internal_status.onboard_control_sensors_present;
+	status->onboard_control_sensors_enabled = internal_status.onboard_control_sensors_enabled;
+	status->onboard_control_sensors_health = internal_status.onboard_control_sensors_health;
+	*status_changed = true;
+}
+
+/* Writes the full state information for a specific subsystem type, either directly into commander's vehicle
+ * status variable or into an internal variable that is later copied to commander's vehicle status variable*/
+void publish_subsystem_info(uint64_t subsystem_type, bool present, bool enabled, bool ok)
+{
+	PX4_DEBUG("publish_subsystem_info (ext:%u): Type %llu pres=%u enabl=%u ok=%u", status != &internal_status,
+		  subsystem_type, present, enabled, ok);
+
+	if (present) {
+		status->onboard_control_sensors_present |= (uint32_t)subsystem_type;
+
+	} else {
+		status->onboard_control_sensors_present &= ~(uint32_t)subsystem_type;
+	}
+
+	if (enabled) {
+		status->onboard_control_sensors_enabled |= (uint32_t)subsystem_type;
+
+	} else {
+		status->onboard_control_sensors_enabled &= ~(uint32_t)subsystem_type;
+	}
+
+	if (ok) {
+		status->onboard_control_sensors_health |= (uint32_t)subsystem_type;
+
+	} else {
+		status->onboard_control_sensors_health &= ~(uint32_t)subsystem_type;
+	}
+
+	if (status != &internal_status) { *status_changed = true; }
+}
+
+void publish_subsystem_info_present_healthy(uint64_t subsystem_type, bool present, bool healthy)
+{
+	publish_subsystem_info(subsystem_type, present, getEnabled(subsystem_type), healthy);
+}
+
+void publish_subsystem_info_present_enabled(uint64_t subsystem_type, bool present, bool enabled)
+{
+	publish_subsystem_info(subsystem_type, present, enabled, getHealthy(subsystem_type));
+}
+
+void publish_subsystem_info_enabled_healthy(uint64_t subsystem_type, bool enabled, bool ok)
+{
+	publish_subsystem_info(subsystem_type, getPresent(subsystem_type), enabled, ok);
+}
+
+void publish_subsystem_info_enabled(uint64_t subsystem_type, bool enabled)
+{
+	publish_subsystem_info(subsystem_type, getPresent(subsystem_type), enabled, getHealthy(subsystem_type));
+}
+
+void publish_subsystem_info_healthy(uint64_t subsystem_type, bool ok)
+{
+	publish_subsystem_info(subsystem_type, getPresent(subsystem_type), getEnabled(subsystem_type), ok);
+}
+
+void publish_subsystem_info_print()
+{
+	uint64_t type = 1;
+
+	for (int i = 1; i < 31; i++) {
+		PX4_DEBUG("subsystem_info: Type %llu pres=%u enabl=%u ok=%u", type,
+			  (status->onboard_control_sensors_present & (uint32_t)type) > 0,
+			  (status->onboard_control_sensors_enabled & (uint32_t)type) > 0,
+			  (status->onboard_control_sensors_health & (uint32_t)type) > 0);
+		type = type * 2;
+	}
+}
+
+// Local helper functions
+bool getPresent(uint64_t subsystem_type)
+{
+	return status->onboard_control_sensors_present & (uint32_t)subsystem_type;
+}
+bool getEnabled(uint64_t subsystem_type)
+{
+	return status->onboard_control_sensors_enabled & (uint32_t)subsystem_type;
+}
+bool getHealthy(uint64_t subsystem_type)
+{
+	return status->onboard_control_sensors_health & (uint32_t)subsystem_type;
+}

src/modules/systemlib/subsystem_info_pub.h

@@ -0,0 +1,62 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file subsystem_info_pub.h
+ *
+ * Contains helper functions to efficiently publish the subsystem_info topic from various locations inside the code.
+ *
+ * @author Philipp Oettershagen (philipp.oettershagen@mavt.ethz.ch)
+ */
+
+#pragma once
+
+#include <px4_log.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_status.h>
+
+void publish_subsystem_info_init(vehicle_status_s *commander_vehicle_status_ptr, bool *commander_status_changed_ptr);
+void publish_subsystem_info(uint64_t subsystem_type, bool present, bool enabled, bool ok);
+
+void publish_subsystem_info_present_healthy(uint64_t subsystem_type, bool present, bool healthy);
+void publish_subsystem_info_present_enabled(uint64_t subsystem_type, bool present, bool enabled);
+void publish_subsystem_info_enabled_healthy(uint64_t subsystem_type, bool enabled, bool ok);
+void publish_subsystem_info_enabled(uint64_t subsystem_type, bool enabled);
+void publish_subsystem_info_healthy(uint64_t subsystem_type, bool ok);
+
+void publish_subsystem_info_print();
+
+// Local helper functions
+bool getPresent(uint64_t subsystem_type);
+bool getEnabled(uint64_t subsystem_type);
+bool getHealthy(uint64_t subsystem_type);

src/modules/uORB/uORBDevices.cpp

@@ -148,8 +148,13 @@ uORB::DeviceNode::open(device::file_t *filp)
 			return -ENOMEM;
 		}
 
-		/* default to no pending update */
+		/* If queue size >1, allow the subscriber to read the data in the queue. Otherwise, assume subscriber is up to date.*/
-		sd->generation = _generation;
+		if (_queue_size <= 1) {
+			sd->generation = _generation;
+
+		} else {
+			sd->generation = _generation - (_queue_size < _generation ? _queue_size : _generation);
+		}
 
 		/* set priority */
 		sd->set_priority(_priority);

src/platforms/posix/drivers/df_bebop_rangefinder_wrapper/df_bebop_rangefinder_wrapper.cpp

@@ -57,7 +57,6 @@
 #include <drivers/drv_hrt.h>
 
 #include <uORB/uORB.h>
-#include <uORB/topics/subsystem_info.h>
 #include <uORB/topics/distance_sensor.h>
 
 #include <board_config.h>

src/platforms/posix/drivers/df_isl29501_wrapper/df_isl29501_wrapper.cpp

@@ -60,7 +60,6 @@
 #include <drivers/drv_hrt.h>
 
 #include <uORB/uORB.h>
-#include <uORB/topics/subsystem_info.h>
 #include <uORB/topics/distance_sensor.h>
 
 #include <board_config.h>

src/platforms/posix/drivers/df_trone_wrapper/df_trone_wrapper.cpp

@@ -59,7 +59,6 @@
 #include <drivers/drv_hrt.h>
 
 #include <uORB/uORB.h>
-#include <uORB/topics/subsystem_info.h>
 #include <uORB/topics/distance_sensor.h>
 
 #include <board_config.h>