check avoidance status in commander and set sys status

remove mavlink log

msg/subsystem_info.msg

@@ -28,6 +28,7 @@ uint64 SUBSYSTEM_TYPE_LOGGING = 16777216
 uint64 SUBSYSTEM_TYPE_SENSORBATTERY = 33554432
 uint64 SUBSYSTEM_TYPE_SENSORPROXIMITY = 67108864
 uint64 SUBSYSTEM_TYPE_SATCOM = 134217728
+uint64 SUBSYSTEM_TYPE_OBSTACLE_AVOIDANCE = 536870912
 
 bool present
 bool enabled

src/modules/commander/Commander.cpp

@@ -1976,6 +1976,8 @@ Commander::run()
 		// data link checks which update the status
 		data_link_check();
 
+		avoidance_check();
+
 		// engine failure detection
 		// TODO: move out of commander
 		if (_actuator_controls_sub.updated()) {
@@ -3614,7 +3616,6 @@ void Commander::data_link_check()
 					_datalink_last_status_avoidance_system = telemetry.remote_system_status;
 
 					if (_avoidance_system_lost) {
-						mavlink_log_info(&mavlink_log_pub, "Avoidance system regained");
 						_status_changed = true;
 						_avoidance_system_lost = false;
 						status_flags.avoidance_system_valid = true;
@@ -3654,42 +3655,29 @@ void Commander::data_link_check()
 	// AVOIDANCE SYSTEM state check (only if it is enabled)
 	if (status_flags.avoidance_system_required && !_onboard_controller_lost) {
 
-		//if avoidance never started
-		if (_datalink_last_heartbeat_avoidance_system == 0
-		    && hrt_elapsed_time(&_datalink_last_heartbeat_avoidance_system) > _param_com_oa_boot_t.get() * 1_s) {
-			if (!_print_avoidance_msg_once) {
-				mavlink_log_critical(&mavlink_log_pub, "Avoidance system not available");
-				_print_avoidance_msg_once = true;
-
-			}
-		}
-
 		//if heartbeats stop
 		if (!_avoidance_system_lost && (_datalink_last_heartbeat_avoidance_system > 0)
 		    && (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;
-			_print_avoidance_msg_once = false;
 		}
 
 		//if status changed
 		if (_avoidance_system_status_change) {
 			if (_datalink_last_status_avoidance_system == telemetry_status_s::MAV_STATE_BOOT) {
-				mavlink_log_info(&mavlink_log_pub, "Avoidance system starting");
+				status_flags.avoidance_system_valid = false;
 			}
 
 			if (_datalink_last_status_avoidance_system == telemetry_status_s::MAV_STATE_ACTIVE) {
-				mavlink_log_info(&mavlink_log_pub, "Avoidance system connected");
 				status_flags.avoidance_system_valid = true;
 			}
 
 			if (_datalink_last_status_avoidance_system == telemetry_status_s::MAV_STATE_CRITICAL) {
-				mavlink_log_info(&mavlink_log_pub, "Avoidance system timed out");
+				status_flags.avoidance_system_valid = false;
+				_status_changed = true;
 			}
 
 			if (_datalink_last_status_avoidance_system == telemetry_status_s::MAV_STATE_FLIGHT_TERMINATION) {
-				mavlink_log_critical(&mavlink_log_pub, "Avoidance system rejected");
 				status_flags.avoidance_system_valid = false;
 				_status_changed = true;
 			}
@@ -3712,6 +3700,46 @@ void Commander::data_link_check()
 	}
 }
 
+void Commander::avoidance_check()
+{
+
+	for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
+		if (_sub_distance_sensor[i].updated()) {
+			distance_sensor_s distance_sensor {};
+			_sub_distance_sensor[i].copy(&distance_sensor);
+
+			if ((distance_sensor.orientation != distance_sensor_s::ROTATION_DOWNWARD_FACING) &&
+			    (distance_sensor.orientation != distance_sensor_s::ROTATION_UPWARD_FACING)) {
+				_valid_distance_sensor_time_us = distance_sensor.timestamp;
+			}
+		}
+	}
+
+	const bool cp_enabled =  _param_cp_dist.get() > 0.f;
+
+	const bool distance_sensor_valid = hrt_elapsed_time(&_valid_distance_sensor_time_us) < 500_ms;
+	const bool cp_healthy = status_flags.avoidance_system_valid || distance_sensor_valid;
+
+	const bool sensor_oa_present = cp_healthy || status_flags.avoidance_system_required || cp_enabled;
+
+	const bool auto_mode = _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_MISSION
+			       || _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER
+			       || _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL
+			       || _internal_state.main_state == commander_state_s::MAIN_STATE_OFFBOARD
+			       || _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF
+			       || _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND;
+	const bool pos_ctl_mode = _internal_state.main_state == commander_state_s::MAIN_STATE_POSCTL;
+
+	const bool sensor_oa_enabled = ((auto_mode && status_flags.avoidance_system_required) || (pos_ctl_mode
+					&& cp_enabled)) ? true : false;
+	const bool sensor_oa_healthy = ((auto_mode && status_flags.avoidance_system_valid) || (pos_ctl_mode
+					&& cp_healthy)) ? true : false;
+
+	set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_OBSTACLE_AVOIDANCE, sensor_oa_present, sensor_oa_enabled,
+			 sensor_oa_healthy, status);
+
+}
+
 void Commander::battery_status_check()
 {
 	bool battery_sub_updated = false;

src/modules/commander/Commander.hpp

@@ -61,6 +61,7 @@
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/cpuload.h>
+#include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/esc_status.h>
 #include <uORB/topics/estimator_status.h>
 #include <uORB/topics/geofence_result.h>
@@ -132,6 +133,8 @@ private:
 	 */
 	void data_link_check();
 
+	void avoidance_check();
+
 	void esc_status_check(const esc_status_s &esc_status);
 
 	void estimator_check();
@@ -196,7 +199,6 @@ private:
 		(ParamFloat<px4::params::COM_DISARM_PRFLT>) _param_com_disarm_preflight,
 
 		(ParamBool<px4::params::COM_OBS_AVOID>) _param_com_obs_avoid,
-		(ParamInt<px4::params::COM_OA_BOOT_T>) _param_com_oa_boot_t,
 
 		(ParamInt<px4::params::COM_FLT_PROFILE>) _param_com_flt_profile,
 
@@ -251,7 +253,9 @@ private:
 		// Mavlink
 		(ParamInt<px4::params::MAV_COMP_ID>) _param_mav_comp_id,
 		(ParamInt<px4::params::MAV_SYS_ID>) _param_mav_sys_id,
-		(ParamInt<px4::params::MAV_TYPE>) _param_mav_type
+		(ParamInt<px4::params::MAV_TYPE>) _param_mav_type,
+
+		(ParamFloat<px4::params::CP_DIST>) _param_cp_dist
 	)
 
 	enum class PrearmedMode {
@@ -282,6 +286,8 @@ private:
 
 	PreFlightCheck::arm_requirements_t	_arm_requirements{};
 
+	hrt_abstime	_valid_distance_sensor_time_us{0}; /**< Last time that distance sensor data arrived (usec) */
+
 	hrt_abstime	_last_gpos_fail_time_us{0};	/**< Last time that the global position validity recovery check failed (usec) */
 	hrt_abstime	_last_lpos_fail_time_us{0};	/**< Last time that the local position validity recovery check failed (usec) */
 	hrt_abstime	_last_lvel_fail_time_us{0};	/**< Last time that the local velocity validity recovery check failed (usec) */
@@ -325,8 +331,6 @@ private:
 	Hysteresis	_auto_disarm_landed{false};
 	Hysteresis	_auto_disarm_killed{false};
 
-	bool		_print_avoidance_msg_once{false};
-
 	hrt_abstime	_last_print_mode_reject_time{0};	///< To remember when last notification was sent
 
 	float		_eph_threshold_adj{INFINITY};	///< maximum allowable horizontal position uncertainty after adjustment for flight condition
@@ -388,6 +392,7 @@ private:
 #endif
 	uORB::Subscription					_cmd_sub {ORB_ID(vehicle_command)};
 	uORB::Subscription					_cpuload_sub{ORB_ID(cpuload)};
+	uORB::Subscription					_sub_distance_sensor[ORB_MULTI_MAX_INSTANCES] {{ORB_ID(distance_sensor), 0}, {ORB_ID(distance_sensor), 1}, {ORB_ID(distance_sensor), 2}, {ORB_ID(distance_sensor), 3}}; /**< distance data received from onboard rangefinders */
 	uORB::Subscription					_esc_status_sub{ORB_ID(esc_status)};
 	uORB::Subscription					_geofence_result_sub{ORB_ID(geofence_result)};
 	uORB::Subscription					_iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)};

src/modules/commander/commander_params.c

@@ -844,19 +844,6 @@ PARAM_DEFINE_INT32(NAV_RCL_ACT, 2);
  */
 PARAM_DEFINE_INT32(COM_OBS_AVOID, 0);
 
-/**
- * Set avoidance system bootup timeout.
- *
- * The avoidance system running on the companion computer is expected to boot
- * within this time and start providing trajectory points.
- * If no avoidance system is detected a MAVLink warning message is sent.
- * @group Commander
- * @unit s
- * @min 0
- * @max 200
- */
-PARAM_DEFINE_INT32(COM_OA_BOOT_T, 100);
-
 /**
  * User Flight Profile
  *