uavcan esc handling review suggestions

msg/DeviceInformation.msg

@@ -23,7 +23,7 @@ uint8 DEVICE_TYPE_DIFFERENTIAL_PRESSURE = 13  # Differential pressure
 uint8 DEVICE_TYPE_BATTERY = 14                # Battery
 uint8 DEVICE_TYPE_HYGROMETER = 15             # Hygrometer
 
-char[80] name  # Name of node
+char[80] name # Name of device e.g. DroneCAN node name
 
 uint32   device_id         # [-] [@invalid 0 if not available] Unique device ID for the sensor. Does not change between power cycles.
 char[24] firmware_version  # [-] [@invalid empty if not available] Firmware version.

src/drivers/uavcan/actuators/esc.cpp

@@ -129,8 +129,7 @@ void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<
 		_esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_CAN;
 		_esc_status.esc_online_flags = check_escs_status();
 		_esc_status.esc_armed_flags = (1 << _rotor_count) - 1;
-		_esc_status.timestamp = hrt_absolute_time();
+		_esc_status.timestamp = esc_report.timestamp;
-
 		_esc_status_pub.publish(_esc_status);
 	}
 
@@ -172,29 +171,15 @@ uint8_t UavcanEscController::check_escs_status()
 
 uint32_t UavcanEscController::get_failures(uint8_t esc_index)
 {
-	esc_report_s &esc_report = _esc_status.esc[esc_index];
+	// Check DoneCAN node health of the ESC
-	uint32_t failures = 0;
+	dronecan_node_status_s node_status{};
-
+	uint8_t esc_node_id = _esc_status.esc[esc_index].esc_address;
-	// Update device vendor/model information from device_information topic
+	uint8_t node_health = dronecan_node_status_s::HEALTH_OK;
-	device_information_s device_information{};
+	uint16_t vendor_specific_status_code = 0;
-
-	char esc_name[80] {};
-
-	if (_device_information_sub.copy(&device_information)
-	    && device_information.device_type == device_information_s::DEVICE_TYPE_ESC
-	    && device_information.device_id == esc_index) {
-		strncpy(esc_name, device_information.name, sizeof(esc_name) - 1);
-		esc_name[sizeof(esc_name) - 1] = '\0';
-	}
-
-	// Update node health from all available dronecan_node_status messages
-	dronecan_node_status_s node_status {};
-	uint8_t node_health{dronecan_node_status_s::HEALTH_OK};
-	uint16_t vendor_specific_status_code{0};
 
 	for (auto &dronecan_node_status_sub : _dronecan_node_status_subs) {
 		if (dronecan_node_status_sub.copy(&node_status)) {
-			if (node_status.node_id == esc_report.esc_address) {
+			if (node_status.node_id == esc_node_id) {
 				node_health = node_status.health;
 				vendor_specific_status_code = node_status.vendor_specific_status_code;
 				break;
@@ -202,12 +187,20 @@ uint32_t UavcanEscController::get_failures(uint8_t esc_index)
 		}
 	}
 
-	if (esc_report.esc_address <= uavcan::NodeID::Max && (node_health == dronecan_node_status_s::HEALTH_OK ||
+	const bool failure = (node_health == dronecan_node_status_s::HEALTH_ERROR)
-			node_health == dronecan_node_status_s::HEALTH_WARNING)) {
+			     || (node_health == dronecan_node_status_s::HEALTH_CRITICAL);
-		failures = 0;
 
-	} else if (strstr(esc_name, "iq_motion") != nullptr) {
+	if (!failure) {
-		// Parse iq_motion ESC errors
+		return 0;
+	}
+
+	// Parse VertiQ = iq_motion ESC error flags
+	device_information_s device_information{};
+
+	if (_device_information_sub.copy(&device_information)
+	    && device_information.device_type == device_information_s::DEVICE_TYPE_ESC
+	    && device_information.device_id == esc_index
+	    && strstr(device_information.name, "iq_motion") != nullptr) {
 		static const struct {
 			uint8_t bit;
 			uint8_t failure_type;
@@ -226,16 +219,17 @@ uint32_t UavcanEscController::get_failures(uint8_t esc_index)
 			{11, esc_report_s::FAILURE_OVER_VOLTAGE},
 		};
 
+		uint32_t failures = 0;
+
 		for (const auto &mapping : bit_to_failure_map) {
 			if (vendor_specific_status_code & (1 << mapping.bit)) {
 				failures |= (1 << mapping.failure_type);
 			}
 		}
 
-	} else {
+		return failures;
-		// Generic parsing
-		failures |= (1 << esc_report_s::FAILURE_GENERIC);
 	}
 
-	return failures;
+	// Generic parsing
+	return (1 << esc_report_s::FAILURE_GENERIC);
 }

src/drivers/uavcan/actuators/esc.hpp

@@ -74,6 +74,7 @@ public:
 	bool initialized() { return _initialized; };
 
 	void update_outputs(uint16_t outputs[MAX_ACTUATORS], uint8_t output_array_size);
+
 	/**
 	 * Sets the number of rotors and enable timer
 	 */
@@ -100,7 +101,6 @@ private:
 	 */
 	uint8_t check_escs_status();
 
-
 	/**
 	 * Gets failure flags for a specific ESC
 	 */
@@ -110,13 +110,12 @@ private:
 		void (UavcanEscController::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status>&)> StatusCbBinder;
 
 	typedef uavcan::MethodBinder<UavcanEscController *,
-		void (UavcanEscController::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::StatusExtended>&)>
+		void (UavcanEscController::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::StatusExtended>&)> StatusExtendedCbBinder;
-		StatusExtendedCbBinder;
 
 	typedef uavcan::MethodBinder<UavcanEscController *,
 		void (UavcanEscController::*)(const uavcan::TimerEvent &)> TimerCbBinder;
 
-	bool _initialized{};
+	bool _initialized = false;
 
 	esc_status_s	_esc_status{};
 
@@ -133,7 +132,7 @@ private:
 	uavcan::INode								&_node;
 	uavcan::Publisher<uavcan::equipment::esc::RawCommand>			_uavcan_pub_raw_cmd;
 	uavcan::Subscriber<uavcan::equipment::esc::Status, StatusCbBinder>	_uavcan_sub_status;
-	uavcan::Subscriber<uavcan::equipment::esc::StatusExtended, StatusExtendedCbBinder>	_uavcan_sub_status_extended;
+	uavcan::Subscriber<uavcan::equipment::esc::StatusExtended, StatusExtendedCbBinder> _uavcan_sub_status_extended;
 
 	NodeInfoPublisher *_node_info_publisher{nullptr};
 };

src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp

@@ -132,7 +132,7 @@ void EscChecks::checkEscStatus(const Context &context, Report &reporter, const e
 			mavlink_log_critical(reporter.mavlink_log_pub(), "%soffline. %s\t", esc_fail_msg, context.isArmed() ? "Land now!" : "");
 		}
 
-		for (int index = 0; index <  esc_status_s::CONNECTED_ESC_MAX; ++index) {
+		for (int index = 0; index < esc_status_s::CONNECTED_ESC_MAX; ++index) {
 
 			if (esc_status.esc[index].failures != 0) {
 

src/modules/commander/failure_detector/FailureDetector.cpp

@@ -183,6 +183,10 @@ void FailureDetector::updateEscsStatus(const vehicle_status_s &vehicle_status, c
 
 		bool is_esc_failure = !is_all_escs_armed;
 
+		for (int i = 0; i < limited_esc_count; i++) {
+			is_esc_failure = is_esc_failure || (esc_status.esc[i].failures > 0);
+		}
+
 		_esc_failure_hysteresis.set_hysteresis_time_from(false, 300_ms);
 		_esc_failure_hysteresis.set_state_and_update(is_esc_failure, now);