Add Support for DroneCAN Status Extended Message

msg/CMakeLists.txt

@@ -74,6 +74,8 @@ set(msg_files
 	DifferentialPressure.msg
 	DistanceSensor.msg
 	DistanceSensorModeChangeRequest.msg
+	DronecanEscStatusExtended.msg
+	DronecanEscStatusExtendedData.msg
 	Ekf2Timestamps.msg
 	EscReport.msg
 	EscStatus.msg

msg/DronecanEscStatusExtended.msg

@@ -0,0 +1,5 @@
+uint64 timestamp						# time since system start (microseconds)
+
+uint8 CONNECTED_ESC_MAX = 8					# The number of ESCs supported. To be consistent with ESC Status, limit it to 8.
+
+DronecanEscStatusExtendedData[8] extended_esc_status_data	# An array of up to CONNECTED_ESC_MAX DronecanEscStatusExtendedData instances

msg/DronecanEscStatusExtendedData.msg

@@ -0,0 +1,9 @@
+uint64 timestamp					# time since system start (microseconds)
+
+# From the StatusExtended.uavcan message
+uint8 input_percent					# Input command to ESC, in percent, which is commanded using the setpoint messages. Range 0% to 100%.
+uint8 output_percent					# Output command from ESC to motor, in percent. Range 0% to 100%.
+int16 motor_temperature_deg_c  				# Temperature of connected motor, in Celsius. Range is -256 to +255 C.
+uint16 motor_angle             				# Measured angle of connected angle sensor, in degrees. Range is 0 to 360.
+uint32 status_flags           				# Manufacturer-specific status flags currently active.
+uint8 esc_index               				# Index of currently reporting ESC.

src/drivers/uavcan/actuators/esc.cpp

@@ -48,7 +48,8 @@ using namespace time_literals;
 UavcanEscController::UavcanEscController(uavcan::INode &node) :
 	_node(node),
 	_uavcan_pub_raw_cmd(node),
-	_uavcan_sub_status(node)
+	_uavcan_sub_status(node),
+	_uavcan_sub_status_extended(node)
 {
 	_uavcan_pub_raw_cmd.setPriority(uavcan::TransferPriority::NumericallyMin); // Highest priority
 }
@@ -64,7 +65,16 @@ UavcanEscController::init()
 		return res;
 	}
 
+	//ESC Status Extended subscription
+	res = _uavcan_sub_status_extended.start(StatusExtendedCbBinder(this, &UavcanEscController::esc_status_extended_sub_cb));
+
+	if (res < 0) {
+		PX4_ERR("ESC status extended sub failed %i", res);
+		return res;
+	}
+
 	_esc_status_pub.advertise();
+	_status_extended_pub.advertise();  //Make sure people are listening
 
 	return res;
 }
@@ -154,6 +164,32 @@ UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavca
 	}
 }
 
+void
+UavcanEscController::esc_status_extended_sub_cb(const
+		uavcan::ReceivedDataStructure<uavcan::equipment::esc::StatusExtended> &received_status_extended_msg)
+{
+	//Make sure it's an ESC we can handle
+	if (received_status_extended_msg.esc_index < dronecan_esc_status_extended_s::CONNECTED_ESC_MAX) {
+		//Grab the ESC we're talking about
+		auto &esc_reference = _status_extended.extended_esc_status_data[received_status_extended_msg.esc_index];
+
+		//Fill in the data
+		esc_reference.input_percent = received_status_extended_msg.input_pct;
+		esc_reference.output_percent = received_status_extended_msg.output_pct;
+		esc_reference.motor_temperature_deg_c = received_status_extended_msg.motor_temperature_degC;
+		esc_reference.motor_angle = received_status_extended_msg.motor_angle;
+		esc_reference.status_flags = received_status_extended_msg.status_flags;
+		esc_reference.esc_index = received_status_extended_msg.esc_index;
+		esc_reference.timestamp = hrt_absolute_time();
+
+		//Make sure to update the timestamp of our top level status
+		_status_extended.timestamp = hrt_absolute_time();
+
+		//Put the data into the world
+		_status_extended_pub.publish(_status_extended);
+	}
+}
+
 uint8_t
 UavcanEscController::check_escs_status()
 {

src/drivers/uavcan/actuators/esc.hpp

@@ -47,12 +47,14 @@
 #include <uavcan/uavcan.hpp>
 #include <uavcan/equipment/esc/RawCommand.hpp>
 #include <uavcan/equipment/esc/Status.hpp>
+#include <uavcan/equipment/esc/StatusExtended.hpp>
 #include <lib/perf/perf_counter.h>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/esc_status.h>
 #include <drivers/drv_hrt.h>
 #include <lib/mixer_module/mixer_module.hpp>
+#include <uORB/topics/dronecan_esc_status_extended.h>
 
 class UavcanEscController
 {
@@ -86,6 +88,12 @@ class UavcanEscController
 	 */
 	void esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg);
 
+	/**
+	 * ESC status extended message reception will be reported via this callback.
+	 */
+	void esc_status_extended_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::StatusExtended>
+					&received_status_extended_msg);
+
 	/**
 	 * Checks all the ESCs freshness based on timestamp, if an ESC exceeds the timeout then is flagged offline.
 	 */
@@ -94,12 +102,18 @@ class UavcanEscController
 	typedef uavcan::MethodBinder<UavcanEscController *,
 		void (UavcanEscController::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status>&)> StatusCbBinder;
 
+	typedef uavcan::MethodBinder<UavcanEscController *,
+		void (UavcanEscController::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::StatusExtended>&)>
+		StatusExtendedCbBinder;
+
 	typedef uavcan::MethodBinder<UavcanEscController *,
 		void (UavcanEscController::*)(const uavcan::TimerEvent &)> TimerCbBinder;
 
 	esc_status_s	_esc_status{};
+	dronecan_esc_status_extended_s _status_extended{};
 
 	uORB::PublicationMulti<esc_status_s> _esc_status_pub{ORB_ID(esc_status)};
+	uORB::Publication<dronecan_esc_status_extended_s> _status_extended_pub{ORB_ID(dronecan_esc_status_extended)};
 
 	uint8_t		_rotor_count{0};
 
@@ -110,6 +124,7 @@ class UavcanEscController
 	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;
 
 	/*
 	 * ESC states

src/modules/logger/logged_topics.cpp

@@ -59,6 +59,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("config_overrides");
 	add_topic("cpuload");
 	add_topic("distance_sensor_mode_change_request");
+	add_optional_topic("dronecan_esc_status_extended", 250);
 	add_optional_topic("external_ins_attitude");
 	add_optional_topic("external_ins_global_position");
 	add_optional_topic("external_ins_local_position");