UAVCAN pre-arm checks on escs_status (cleanup of #12153)

msg/esc_status.msg

@@ -17,4 +17,14 @@ uint16 counter  					# incremented by the writing thread everytime new data is s
 uint8 esc_count						# number of connected ESCs
 uint8 esc_connectiontype				# how ESCs connected to the system
 
+uint8 esc_online_flags					# Bitmask indicating which ESC is online/offline
+# esc_online_flags bit 0 : Set to 1 if ESC0 is online
+# esc_online_flags bit 1 : Set to 1 if ESC1 is online
+# esc_online_flags bit 2 : Set to 1 if ESC2 is online
+# esc_online_flags bit 3 : Set to 1 if ESC3 is online
+# esc_online_flags bit 4 : Set to 1 if ESC4 is online
+# esc_online_flags bit 5 : Set to 1 if ESC5 is online
+# esc_online_flags bit 6 : Set to 1 if ESC6 is online
+# esc_online_flags bit 7 : Set to 1 if ESC7 is online
+
 esc_report[8] esc

msg/vehicle_status_flags.msg

@@ -14,7 +14,7 @@ bool condition_local_velocity_valid		# set to true by the commander app if the q
 bool condition_local_altitude_valid
 bool condition_power_input_valid                # set if input power is valid
 bool condition_battery_healthy                # set if battery is available and not low
-
+bool condition_escs_error		      # set to true if one or more ESCs reporting esc_status are offline
 bool circuit_breaker_engaged_power_check
 bool circuit_breaker_engaged_airspd_check
 bool circuit_breaker_engaged_enginefailure_check

src/drivers/uavcan/actuators/esc.cpp

@@ -43,6 +43,8 @@
 
 #define MOTOR_BIT(x) (1<<(x))
 
+using namespace time_literals;
+
 UavcanEscController::UavcanEscController(uavcan::INode &node) :
 	_node(node),
 	_uavcan_pub_raw_cmd(node),
@@ -202,13 +204,10 @@ void UavcanEscController::arm_single_esc(int num, bool arm)
 void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg)
 {
 	if (msg.esc_index < esc_status_s::CONNECTED_ESC_MAX) {
-		_esc_status.esc_count = uavcan::max<int>(_esc_status.esc_count, msg.esc_index + 1);
-		_esc_status.timestamp = hrt_absolute_time();
-
 		auto &ref = _esc_status.esc[msg.esc_index];
 
 		ref.esc_address = msg.getSrcNodeID().get();
-
+		ref.timestamp       = hrt_absolute_time();
 		ref.esc_voltage     = msg.voltage;
 		ref.esc_current     = msg.current;
 		ref.esc_temperature = msg.temperature;
@@ -220,8 +219,11 @@ void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<
 
 void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent &)
 {
+	_esc_status.timestamp = hrt_absolute_time();
+	_esc_status.esc_count = _rotor_count;
 	_esc_status.counter += 1;
 	_esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_CAN;
+	_esc_status.esc_online_flags = UavcanEscController::check_escs_status();
 
 	if (_esc_status_pub != nullptr) {
 		(void)orb_publish(ORB_ID(esc_status), _esc_status_pub, &_esc_status);
@@ -230,3 +232,19 @@ void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent &)
 		_esc_status_pub = orb_advertise(ORB_ID(esc_status), &_esc_status);
 	}
 }
+
+uint8_t UavcanEscController::check_escs_status()
+{
+	int esc_status_flags = 0;
+	hrt_abstime now = hrt_absolute_time();
+
+	for (int index = 0; index < esc_status_s::CONNECTED_ESC_MAX; index++) {
+
+		if (_esc_status.esc[index].timestamp > 0 && now - _esc_status.esc[index].timestamp < 800_ms) {
+			esc_status_flags |= (1 << index);
+		}
+
+	}
+
+	return esc_status_flags;
+}

src/drivers/uavcan/actuators/esc.hpp

@@ -50,6 +50,7 @@
 #include <perf/perf_counter.h>
 #include <uORB/topics/esc_status.h>
 #include <uORB/topics/actuator_outputs.h>
+#include <drivers/drv_hrt.h>
 
 
 class UavcanEscController
@@ -67,6 +68,11 @@ class UavcanEscController
 
 	void enable_idle_throttle_when_armed(bool value) { _run_at_idle_throttle_when_armed = value; }
 
+	/**
+	 * Sets the number of rotors
+	 */
+	void set_rotor_count(uint8_t count) { _rotor_count = count; }
+
 private:
 	/**
 	 * ESC status message reception will be reported via this callback.
@@ -78,6 +84,10 @@ class UavcanEscController
 	 */
 	void orb_pub_timer_cb(const uavcan::TimerEvent &event);
 
+	/**
+	 * Checks all the ESCs freshness based on timestamp, if an ESC exceeds the timeout then is flagged offline.
+	 */
+	uint8_t check_escs_status();
 
 	static constexpr unsigned MAX_RATE_HZ = 200;			///< XXX make this configurable
 	static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 10;
@@ -95,6 +105,7 @@ class UavcanEscController
 	esc_status_s	_esc_status = {};
 	orb_advert_t	_esc_status_pub = nullptr;
 	orb_advert_t _actuator_outputs_pub = nullptr;
+	uint8_t		_rotor_count = 0;
 
 	/*
 	 * libuavcan related things

src/drivers/uavcan/uavcan_main.cpp

@@ -1123,6 +1123,11 @@ UavcanNode::ioctl(file *filp, int cmd, unsigned long arg)
 				} else {
 
 					_mixers->groups_required(_groups_required);
+					PX4_INFO("Groups required %d", _groups_required);
+
+					int rotor_count = _mixers->get_multirotor_count();
+					_esc_controller.set_rotor_count(rotor_count);
+					PX4_INFO("Number of rotors %d", rotor_count);
 				}
 			}
 		}

src/lib/mixer/mixer.h

@@ -254,6 +254,8 @@ class Mixer
 	 */
 	virtual void set_airmode(Airmode airmode) {};
 
+	virtual unsigned get_multirotor_count()  {return 0;}
+
 protected:
 	/** client-supplied callback used when fetching control values */
 	ControlCallback			_control_cb;
@@ -450,6 +452,8 @@ class MixerGroup : public Mixer
 
 	void 	set_airmode(Airmode airmode) override;
 
+	unsigned get_multirotor_count() override;
+
 private:
 	Mixer				*_first;	/**< linked list of mixers */
 
@@ -715,6 +719,8 @@ class MultirotorMixer : public Mixer
 
 	void 			set_airmode(Airmode airmode) override;
 
+	unsigned get_multirotor_count() override {return _rotor_count;}
+
 	union saturation_status {
 		struct {
 			uint16_t valid		: 1; // 0 - true when the saturation status is used

src/lib/mixer/mixer_group.cpp

@@ -198,6 +198,24 @@ MixerGroup::set_airmode(Airmode airmode)
 	}
 }
 
+unsigned
+MixerGroup::get_multirotor_count()
+{
+	Mixer	*mixer = _first;
+	unsigned rotor_count = 0;
+
+	while (mixer != nullptr) {
+
+		rotor_count = mixer->get_multirotor_count();
+
+		if (rotor_count > 0) { break; }
+
+		mixer = mixer -> _next;
+	}
+
+	return rotor_count;
+}
+
 uint16_t
 MixerGroup::get_saturation_status()
 {

src/modules/commander/Commander.cpp

@@ -95,6 +95,7 @@
 #include <uORB/topics/telemetry_status.h>
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vtol_vehicle_status.h>
+#include <uORB/topics/esc_status.h>
 
 typedef enum VEHICLE_MODE_FLAG {
 	VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */
@@ -1198,6 +1199,7 @@ Commander::run()
 	param_t _param_arm_switch_is_button = param_find("COM_ARM_SWISBTN");
 	param_t _param_rc_override = param_find("COM_RC_OVERRIDE");
 	param_t _param_arm_mission_required = param_find("COM_ARM_MIS_REQ");
+	param_t _param_escs_checks_required = param_find("COM_ARM_CHK_ESCS");
 	param_t _param_flight_uuid = param_find("COM_FLIGHT_UUID");
 	param_t _param_takeoff_finished_action = param_find("COM_TAKEOFF_ACT");
 
@@ -1243,6 +1245,7 @@ Commander::run()
 		PX4_ERR("Failed to register power notification callback");
 	}
 
+
 	get_circuit_breaker_params();
 
 	/* armed topic */
@@ -1275,6 +1278,7 @@ Commander::run()
 	uORB::Subscription subsys_sub{ORB_ID(subsystem_info)};
 	uORB::Subscription system_power_sub{ORB_ID(system_power)};
 	uORB::Subscription vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
+	uORB::Subscription esc_status_sub{ORB_ID(esc_status)};
 
 	geofence_result_s geofence_result {};
 
@@ -1329,6 +1333,10 @@ Commander::run()
 	param_get(_param_arm_mission_required, &arm_mission_required_param);
 	arm_requirements |= (arm_mission_required_param & (ARM_REQ_MISSION_BIT | ARM_REQ_ARM_AUTH_BIT));
 
+	int32_t arm_escs_checks_required_param = 0;
+	param_get(_param_escs_checks_required, &arm_escs_checks_required_param);
+	arm_requirements |= (arm_escs_checks_required_param == 0) ? ARM_REQ_NONE : ARM_REQ_ESCS_CHECK_BIT;
+
 	status.rc_input_mode = rc_in_off;
 
 	// user adjustable duration required to assert arm/disarm via throttle/rudder stick
@@ -1458,6 +1466,9 @@ Commander::run()
 			arm_requirements = (arm_without_gps_param == 1) ? ARM_REQ_NONE : ARM_REQ_GPS_BIT;
 			param_get(_param_arm_mission_required, &arm_mission_required_param);
 			arm_requirements |= (arm_mission_required_param & (ARM_REQ_MISSION_BIT | ARM_REQ_ARM_AUTH_BIT));
+			param_get(_param_escs_checks_required, &arm_escs_checks_required_param);
+			arm_requirements |= (arm_escs_checks_required_param == 0) ? ARM_REQ_NONE : ARM_REQ_ESCS_CHECK_BIT;
+
 
 			/* flight mode slots */
 			param_get(_param_fmode_1, &_flight_mode_slots[0]);
@@ -1610,6 +1621,14 @@ Commander::run()
 			}
 		}
 
+		if (esc_status_sub.updated()) {
+			/* ESCs status changed */
+			esc_status_s esc_status = {};
+
+			esc_status_sub.copy(&esc_status);
+			esc_status_check(esc_status);
+		}
+
 		estimator_check(&status_changed);
 		airspeed_use_check();
 
@@ -4347,3 +4366,39 @@ Commander::offboard_control_update(bool &status_changed)
 	}
 
 }
+
+void Commander::esc_status_check(const esc_status_s &esc_status)
+{
+	char esc_fail_msg[50];
+	esc_fail_msg[0] = '\0';
+
+	int online_bitmask = (1 << esc_status.esc_count) - 1;
+
+	// Check if ALL the ESCs are online
+	if (online_bitmask == esc_status.esc_online_flags) {
+		status_flags.condition_escs_error = false;
+		_last_esc_online_flags = esc_status.esc_online_flags;
+
+	} else if (_last_esc_online_flags == esc_status.esc_online_flags || esc_status.esc_count == 0)  {
+
+		// Avoid checking the status if the flags are the same or if the mixer has not yet been loaded in the ESC driver
+
+		status_flags.condition_escs_error = true;
+
+	} else if (esc_status.esc_online_flags < _last_esc_online_flags) {
+
+		// Only warn the user when an ESC goes from ONLINE to OFFLINE. This is done to prevent showing Offline ESCs warnings at boot
+
+		for (int index = 0; index < esc_status.esc_count; index++) {
+			if ((esc_status.esc_online_flags & (1 << index)) == 0) {
+				snprintf(esc_fail_msg + strlen(esc_fail_msg), sizeof(esc_fail_msg) - strlen(esc_fail_msg), "ESC%d ", index + 1);
+				esc_fail_msg[sizeof(esc_fail_msg) - 1] = '\0';
+			}
+		}
+
+		mavlink_log_critical(&mavlink_log_pub, "%soffline", esc_fail_msg);
+
+		_last_esc_online_flags = esc_status.esc_online_flags;
+		status_flags.condition_escs_error = true;
+	}
+}

src/modules/commander/Commander.hpp

@@ -65,6 +65,7 @@
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/esc_status.h>
 
 using math::constrain;
 
@@ -217,6 +218,8 @@ class Commander : public ModuleBase<Commander>, public ModuleParams
 
 	void battery_status_check();
 
+	void esc_status_check(const esc_status_s &esc_status);
+
 	/**
 	 * Checks the status of all available data links and handles switching between different system telemetry states.
 	 */
@@ -240,6 +243,8 @@ class Commander : public ModuleBase<Commander>, public ModuleParams
 	hrt_abstime	_high_latency_datalink_heartbeat{0};
 	hrt_abstime	_high_latency_datalink_lost{0};
 
+	int  _last_esc_online_flags{-1};
+
 	uORB::Subscription _battery_sub{ORB_ID(battery_status)};
 	uint8_t _battery_warning{battery_status_s::BATTERY_WARNING_NONE};
 	float _battery_current{0.0f};

src/modules/commander/commander_params.c

@@ -927,3 +927,13 @@ PARAM_DEFINE_INT32(COM_ASPD_FS_DLY, 0);
  * @group Commander
  */
 PARAM_DEFINE_INT32(COM_FLT_PROFILE, 0);
+
+/**
+ * Require all the ESCs to be detected to arm.
+ *
+ * This param is specific for ESCs reporting status. Normal ESCs configurations are not affected by the change of this param.
+ *
+ * @group Commander
+ * @boolean
+ */
+PARAM_DEFINE_INT32(COM_ARM_CHK_ESCS, 1);

src/modules/commander/state_machine_helper.cpp

@@ -1010,6 +1010,14 @@ bool prearm_check(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &s
 
 	}
 
+	if (status_flags.condition_escs_error && (arm_requirements & ARM_REQ_ESCS_CHECK_BIT)) {
+		if (prearm_ok && reportFailures) {
+			mavlink_log_critical(mavlink_log_pub, "Arming denied! One or more ESCs are offline");
+			prearm_ok = false;
+		}
+	}
+
+
 	return prearm_ok;
 }
 

src/modules/commander/state_machine_helper.h

@@ -73,6 +73,7 @@ typedef enum {
 	ARM_REQ_MISSION_BIT = (1 << 0),
 	ARM_REQ_ARM_AUTH_BIT = (1 << 1),
 	ARM_REQ_GPS_BIT = (1 << 2),
+	ARM_REQ_ESCS_CHECK_BIT = (1 << 3)
 } arm_requirements_t;
 
 extern const char *const arming_state_names[];