Subsystem_info status flags & checks: Switch back to uORB for inter-process communication, handle GPS checks completely inside ekf2, add distance_sensor checks

msg/subsystem_info.msg

@@ -31,3 +31,5 @@ bool present
 bool enabled
 bool ok
 uint64 subsystem_type
+
+uint32 ORB_QUEUE_LENGTH = 25

src/drivers/distance_sensor/hc_sr04/hc_sr04.cpp

@@ -58,14 +58,12 @@
 
 #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>
 
 #define SR04_MAX_RANGEFINDERS 6
@@ -623,10 +621,6 @@ HC_SR04::start()
 		   (worker_t)&HC_SR04::cycle_trampoline,
 		   this,
 		   USEC2TICK(_cycling_rate));
-
-
-	/* notify about state change */
-	publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_SENSORPROXIMITY, true, true, true);
 }
 
 void

src/drivers/distance_sensor/mb12xx/mb12xx.cpp

@@ -61,14 +61,12 @@
 
 #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>
@@ -589,9 +587,6 @@ MB12XX::start()
 
 	/* schedule a cycle to start things */
 	work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 5);
-
-	/* notify about state change */
-	publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_SENSORPROXIMITY, true, true, true);
 }
 
 void

src/drivers/distance_sensor/sf0x/sf0x.cpp

@@ -643,8 +643,6 @@ SF0X::start()
 	/* schedule a cycle to start things */
 	work_queue(HPWORK, &_work, (worker_t)&SF0X::cycle_trampoline, this, 1);
 
-	// /* notify about state change */
-	// publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_SENSORPROXIMITY, true, true, true);
 }
 
 void

src/drivers/distance_sensor/sf1xx/sf1xx.cpp

@@ -64,14 +64,12 @@
 
 #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>
@@ -586,9 +584,6 @@ 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,14 +60,12 @@
 
 #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>
@@ -592,9 +590,6 @@ SRF02::start()
 
 	/* schedule a cycle to start things */
 	work_queue(HPWORK, &_work, (worker_t)&SRF02::cycle_trampoline, this, 5);
-
-	/* notify about state change */
-	publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_SENSORPROXIMITY, true, true, true);
 }
 
 void

src/drivers/distance_sensor/teraranger/teraranger.cpp

@@ -60,14 +60,12 @@
 
 #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>
@@ -669,9 +667,6 @@ TERARANGER::start()
 
 	/* schedule a cycle to start things */
 	work_queue(HPWORK, &_work, (worker_t)&TERARANGER::cycle_trampoline, this, 1);
-
-	/* notify about state change */
-	publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_SENSORPROXIMITY, true, true, true);
 }
 
 void

src/drivers/distance_sensor/tfmini/tfmini.cpp

@@ -61,7 +61,6 @@
 
 #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>
@@ -69,7 +68,6 @@
 #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>
@@ -608,9 +606,6 @@ TFMINI::start()
 
 	/* schedule a cycle to start things */
 	work_queue(HPWORK, &_work, (worker_t)&TFMINI::cycle_trampoline, this, 1);
-
-	/* notify about state change */
-	publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_SENSORPROXIMITY, true, true, true);
 }
 
 void

src/drivers/px4flow/px4flow.cpp

@@ -65,7 +65,6 @@
 #include <perf/perf_counter.h>
 #include <systemlib/err.h>
 #include <parameters/param.h>
-#include <systemlib/subsystem_info_pub.h>
 
 #include <conversion/rotation.h>
 
@@ -75,7 +74,6 @@
 #include <drivers/device/ringbuffer.h>
 
 #include <uORB/uORB.h>
-#include <uORB/topics/subsystem_info.h>
 #include <uORB/topics/optical_flow.h>
 #include <uORB/topics/distance_sensor.h>
 
@@ -588,9 +586,6 @@ PX4FLOW::start()
 
 	/* schedule a cycle to start things */
 	work_queue(HPWORK, &_work, (worker_t)&PX4FLOW::cycle_trampoline, this, 1);
-
-	/* notify about state change */
-	publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_OPTICALFLOW, true, true, true);
 }
 
 void

src/modules/commander/PreflightCheck.cpp

@@ -71,6 +71,7 @@
 #include <uORB/topics/system_power.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/subsystem_info.h>
+#include <uORB/topics/distance_sensor.h>
 
 #include "PreflightCheck.h"
 
@@ -552,6 +553,9 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_
 	bool present = true;
 	float test_limit = 1.0f; // pass limit re-used for each test
 
+	bool gps_success = true;
+	bool gps_present = true;
+
 	// Get estimator status data if available and exit with a fail recorded if not
 	int sub = orb_subscribe(ORB_ID(estimator_status));
 	estimator_status_s status = {};
@@ -652,16 +656,15 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_
 
 		if (!ekf_gps_fusion) {
 			// The EKF is not using GPS
-			if (report_fail) {
-				if (ekf_gps_check_fail) {
-					// 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);
-				}
+			if (ekf_gps_check_fail) {
+				// Poor GPS quality is the likely cause
+				if (report_fail) mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GPS QUALITY POOR");
+				gps_success = false;
+			} else {
+				// Likely cause unknown
+				if (report_fail) mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: EKF NOT USING GPS");
+				gps_success = false;
+				gps_present = false;
 			}
 
 			success = false;
@@ -676,11 +679,8 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, bool optional, bool report_
 						  + (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_SPD_ERR))) > 0);
 
 			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);
-				}
-
+				if (report_fail) mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GPS QUALITY POOR");
+				gps_success = false;
 				success = false;
 				goto out;
 			}
@@ -689,6 +689,7 @@ 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);
+	publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_GPS, gps_present, enforce_gps_required, gps_success);
 	orb_unsubscribe(sub);
 	return success;
 }
@@ -709,6 +710,20 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &statu
 	const bool checkDynamic = !hil_enabled;
 	const bool checkPower = (status_flags.condition_power_input_valid && !status_flags.circuit_breaker_engaged_power_check);
 
+	bool checkDistanceSensors = false;
+	int32_t distSensorEnabled[7];
+	param_get(param_find("SENS_EN_LEDDAR1"), &(distSensorEnabled[0]));
+	param_get(param_find("SENS_EN_LL40LS"), &distSensorEnabled[1]);
+	param_get(param_find("SENS_EN_MB12XX"), &distSensorEnabled[2]);
+	param_get(param_find("SENS_EN_SF0X"), &distSensorEnabled[3]);
+	param_get(param_find("SENS_EN_SF1XX"), &distSensorEnabled[4]);
+	param_get(param_find("SENS_EN_TFMINI"), &distSensorEnabled[5]);
+	param_get(param_find("SENS_EN_TRANGER"), &distSensorEnabled[6]);
+	if(distSensorEnabled[0]>0 || distSensorEnabled[1]>0 || distSensorEnabled[2]>0 || distSensorEnabled[3]>0 ||
+			distSensorEnabled[4]>0 || distSensorEnabled[5]>0 || distSensorEnabled[6]>0) {
+		checkDistanceSensors=true;
+	}
+
 	bool checkAirspeed = false;
 
 	/* Perform airspeed check only if circuit breaker is not
@@ -921,29 +936,23 @@ 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 = {};
+	/* ---- DISTANCE SENSORS ---- */
+	if (checkDistanceSensors && time_since_boot > 10 * 1000000) {
+		int fd_distance_sensor = orb_subscribe(ORB_ID(distance_sensor));
+		distance_sensor_s dist_sensor = {};
 		bool present = true;
 
-		if ((orb_copy(ORB_ID(vehicle_gps_position), fd_gps, &gps) != PX4_OK) ||
-			(hrt_elapsed_time(&gps.timestamp) > 2000000)) {
+		if ((orb_copy(ORB_ID(distance_sensor), fd_distance_sensor, &dist_sensor) != PX4_OK) ||
+			(hrt_elapsed_time(&dist_sensor.timestamp) > 2000000)) {
 			if (reportFailures) {
-				mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GPS MODULE MISSING");
+				mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: DISTANCE SENSOR 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);
+		publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_SENSORPROXIMITY, present, true, present);
+		orb_unsubscribe(fd_distance_sensor);
 	}
 
-	// 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;

src/modules/commander/commander.cpp

@@ -1274,9 +1274,6 @@ Commander::run()
 
 	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);
 
@@ -2005,6 +2002,42 @@ Commander::run()
 			}
 		}
 
+		/* update subsystem */
+		do {
+			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;
+			}
+		} while(updated);
+
 		/* 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) {
 
@@ -2392,6 +2425,7 @@ Commander::run()
 						status_changed = true;
 
 						PX4_ERR("Engine Failure");
+						publish_subsystem_info(subsystem_info_s::SUBSYSTEM_TYPE_MOTORCONTROL,true,true,false);
 					}
 				}
 

src/modules/systemlib/subsystem_info_pub.cpp

@@ -34,65 +34,59 @@
 /**
  * @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)
+ * Contains helper functions to efficiently publish the subsystem_info messages from various locations inside the code.
  *
  * @author Philipp Oettershagen (philipp.oettershagen@mavt.ethz.ch)
  */
 
 #include "subsystem_info_pub.h"
+#include <uORB/topics/subsystem_info.h>
 
-vehicle_status_s internal_status = {};
-vehicle_status_s *status = &internal_status;
-bool *status_changed = nullptr;
+static orb_advert_t pub = nullptr;
+struct subsystem_info_s info = {};
+struct vehicle_status_s status;
 
-/* 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);
+	PX4_INFO("publish_subsystem_info: Type %llu pres=%u enabl=%u ok=%u", subsystem_type, present, enabled, ok);
 
+	// Keep a local copy of the status flags. Because we use queuing, it could be that the flags in the vehicle_status topics are
+	// not up to date. When using those publish_subsystem_info_xxx functions that only write a subset of health flags but leave others
+	// unchanged, we'd write outdated health flags to vehicle_status. Having an up to date local copy resolves that issue.
 	if (present) {
-		status->onboard_control_sensors_present |= (uint32_t)subsystem_type;
+		status.onboard_control_sensors_present |= (uint32_t)subsystem_type;
 
 	} else {
-		status->onboard_control_sensors_present &= ~(uint32_t)subsystem_type;
+		status.onboard_control_sensors_present &= ~(uint32_t)subsystem_type;
 	}
 
 	if (enabled) {
-		status->onboard_control_sensors_enabled |= (uint32_t)subsystem_type;
+		status.onboard_control_sensors_enabled |= (uint32_t)subsystem_type;
 
 	} else {
-		status->onboard_control_sensors_enabled &= ~(uint32_t)subsystem_type;
+		status.onboard_control_sensors_enabled &= ~(uint32_t)subsystem_type;
 	}
 
 	if (ok) {
-		status->onboard_control_sensors_health |= (uint32_t)subsystem_type;
+		status.onboard_control_sensors_health |= (uint32_t)subsystem_type;
 
 	} else {
-		status->onboard_control_sensors_health &= ~(uint32_t)subsystem_type;
+		status.onboard_control_sensors_health &= ~(uint32_t)subsystem_type;
 	}
 
-	if (status != &internal_status) { *status_changed = true; }
+	/* Publish the subsystem_info message */
+	info.present = present;
+	info.enabled = enabled;
+	info.ok = ok;
+	info.subsystem_type = subsystem_type;
+
+	if (pub != nullptr) {
+		orb_publish(ORB_ID(subsystem_info), pub, &info);
+
+	} else {
+		pub = orb_advertise_queue(ORB_ID(subsystem_info), &info, subsystem_info_s::ORB_QUEUE_LENGTH);
+	}
 }
 
 void publish_subsystem_info_present_healthy(uint64_t subsystem_type, bool present, bool healthy)
@@ -120,29 +114,16 @@ 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;
+	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;
+	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;
+	return status.onboard_control_sensors_health & (uint32_t)subsystem_type;
 }

src/modules/systemlib/subsystem_info_pub.h

@@ -45,7 +45,6 @@
 #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);
@@ -54,9 +53,7 @@ void publish_subsystem_info_enabled_healthy(uint64_t subsystem_type, bool enable
 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
+//// Local helper functions
 bool getPresent(uint64_t subsystem_type);
 bool getEnabled(uint64_t subsystem_type);
 bool getHealthy(uint64_t subsystem_type);