commander: check if baro is healthy

2 files changed, 21 insertions, 0 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 0bf5cfe34..ddfbd65a1 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1081,6 +1081,25 @@ int commander_thread_main(int argc, char *argv[])
 
 		if (updated) {
 			orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors);
+			/* Check if the barometer is healthy and issue a warning in the GCS if not so.
+			 * Because the barometer is used for calculating AMSL altitude which is used to ensure
+			 * vertical separation from other airtraffic the operator has to know when the
+			 * barometer is inoperational.
+			 * */
+			if (hrt_elapsed_time(&sensors.baro_timestamp) < FAILSAFE_DEFAULT_TIMEOUT) {
+				/* handle the case where baro was regained */
+				if (status.barometer_failure) {
+					status.barometer_failure = false;
+					status_changed = true;
+					mavlink_log_critical(mavlink_fd, "baro healthy");
+				}
+			} else {
+				if (!status.barometer_failure) {
+					status.barometer_failure = true;
+					status_changed = true;
+					mavlink_log_critical(mavlink_fd, "baro failed");
+				}
+			}
 		}
 
 		orb_check(diff_pres_sub, &updated);
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index b465f8407..ad92f5b26 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -211,6 +211,8 @@ struct vehicle_status_s {
 	bool gps_failure;				/** Set to true if a gps failure is detected */
 	bool gps_failure_cmd;				/** Set to true if a gps failure mode is commanded */
 
+	bool barometer_failure;				/** Set to true if a barometer failure is detected */
+
 	bool offboard_control_signal_found_once;
 	bool offboard_control_signal_lost;
 	bool offboard_control_signal_weak;