Added obstacle avoidance healthiness topic in vehicle status msg.

This allows to perform pre-arm checks and prevent arming if obstacle avoidance is enabled but not yet running.
Added a print once flag to prevent excessive message spamming in QGC.

Signed-off-by: Claudio Micheli <claudio@auterion.com>

msg/vehicle_status_flags.msg

@@ -33,3 +33,5 @@ bool rc_input_blocked                                # set if RC input should be
 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
+
+bool avoidance_system_valid                       # status of the obstacle avoidance system

src/modules/commander/Commander.cpp

@@ -580,6 +580,7 @@ Commander::Commander() :
 
 	status_flags.condition_power_input_valid = true;
 	status_flags.rc_calibration_valid = true;
+	status_flags.avoidance_system_valid = false;
 }
 
 Commander::~Commander()
@@ -3918,6 +3919,7 @@ void Commander::data_link_check(bool &status_changed)
 						mavlink_log_info(&mavlink_log_pub, "Avoidance system regained");
 						status_changed = true;
 						_avoidance_system_lost = false;
+						status_flags.avoidance_system_valid = true;
 					}
 				}
 
@@ -3965,6 +3967,7 @@ void Commander::data_link_check(bool &status_changed)
 		    && (hrt_elapsed_time(&_datalink_last_heartbeat_avoidance_system) > 5_s)) {
 			_avoidance_system_lost = true;
 			mavlink_log_critical(&mavlink_log_pub, "Avoidance system lost");
+			status_flags.avoidance_system_valid = false;
 		}
 
 		//if status changed
@@ -3975,6 +3978,7 @@ void Commander::data_link_check(bool &status_changed)
 
 			if (_datalink_last_status_avoidance_system == telemetry_status_s::MAV_STATE_ACTIVE) {
 				mavlink_log_info(&mavlink_log_pub, "Avoidance system healthy");
+				status_flags.avoidance_system_valid = true;
 			}
 
 			if (_datalink_last_status_avoidance_system == telemetry_status_s::MAV_STATE_CRITICAL) {
@@ -3983,6 +3987,7 @@ void Commander::data_link_check(bool &status_changed)
 
 			if (_datalink_last_status_avoidance_system == telemetry_status_s::MAV_STATE_FLIGHT_TERMINATION) {
 				mavlink_log_critical(&mavlink_log_pub, "Avoidance system abort");
+				status_flags.avoidance_system_valid = false;
 			}
 
 			_avoidance_system_status_change = false;

src/modules/commander/state_machine_helper.cpp

@@ -46,6 +46,7 @@
 #include <systemlib/mavlink_log.h>
 #include <drivers/drv_hrt.h>
 
+
 #include "state_machine_helper.h"
 #include "commander_helper.h"
 #include "PreflightCheck.h"
@@ -915,6 +916,7 @@ bool prearm_check(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &s
 	bool reportFailures = true;
 	bool prearm_ok = true;
 
+
 	// USB not connected
 	if (!status_flags.circuit_breaker_engaged_usb_check && status_flags.usb_connected) {
 		if (reportFailures) {
@@ -997,6 +999,15 @@ bool prearm_check(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &s
 		}
 	}
 
+	if (!status_flags.avoidance_system_valid) {
+		if (prearm_ok && reportFailures) {
+			mavlink_log_critical(mavlink_log_pub, "ARMING DENIED: AVOIDANCE SYSTEM NOT READY");
+		}
+
+		prearm_ok = false;
+
+	}
+
 	return prearm_ok;
 }