esc_report: add actuator_function

msg/actuator_motors.msg

@@ -4,6 +4,8 @@ uint64 timestamp_sample	    # the timestamp the data this control response is ba
 
 uint16 reversible_flags     # bitset which motors are configured to be reversible
 
+uint8 ACTUATOR_FUNCTION_MOTOR1 = 101
+
 uint8 NUM_CONTROLS = 12
 float32[12] control # range: [-1, 1], where 1 means maximum positive thrust,
                     # -1 maximum negative (if not supported by the output, <0 maps to NaN),

msg/esc_report.msg

@@ -8,6 +8,8 @@ uint8 esc_address					# Address of current ESC (in most cases 1-8 / must be set
 
 uint8 esc_state					# State of ESC - depend on Vendor
 
+uint8 actuator_function				# actuator output function (one of Motor1...MotorN)
+
 uint16 failures					# Bitmask to indicate the internal ESC faults
 
 uint8 FAILURE_OVER_CURRENT = 0 			# (1 << 0)

src/drivers/dshot/DShot.cpp

@@ -247,7 +247,10 @@ void DShot::update_telemetry_num_motors()
 
 	if (_mixing_output.useDynamicMixing()) {
 		for (unsigned i = 0; i < _num_outputs; ++i) {
-			motor_count += _mixing_output.isFunctionSet(i);
+			if (_mixing_output.isFunctionSet(i)) {
+				_telemetry->actuator_functions[motor_count] = (uint8_t)_mixing_output.outputFunction(i);
+				++motor_count;
+			}
 		}
 
 	} else {
@@ -281,24 +284,26 @@ void DShot::init_telemetry(const char *device)
 	update_telemetry_num_motors();
 }
 
-void DShot::handle_new_telemetry_data(const int motor_index, const DShotTelemetry::EscData &data)
+void DShot::handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data)
 {
 	// fill in new motor data
 	esc_status_s &esc_status = _telemetry->esc_status_pub.get();
 
-	if (motor_index < esc_status_s::CONNECTED_ESC_MAX) {
+	if (telemetry_index < esc_status_s::CONNECTED_ESC_MAX) {
-		esc_status.esc_online_flags |= 1 << motor_index;
+		esc_status.esc_online_flags |= 1 << telemetry_index;
 
-		esc_status.esc[motor_index].timestamp       = data.time;
+		esc_status.esc[telemetry_index].actuator_function = _telemetry->actuator_functions[telemetry_index];
-		esc_status.esc[motor_index].esc_rpm         = (static_cast<int>(data.erpm) * 100) / (_param_mot_pole_count.get() / 2);
+		esc_status.esc[telemetry_index].timestamp       = data.time;
-		esc_status.esc[motor_index].esc_voltage     = static_cast<float>(data.voltage) * 0.01f;
+		esc_status.esc[telemetry_index].esc_rpm         = (static_cast<int>(data.erpm) * 100) /
-		esc_status.esc[motor_index].esc_current     = static_cast<float>(data.current) * 0.01f;
+				(_param_mot_pole_count.get() / 2);
-		esc_status.esc[motor_index].esc_temperature = static_cast<float>(data.temperature);
+		esc_status.esc[telemetry_index].esc_voltage     = static_cast<float>(data.voltage) * 0.01f;
+		esc_status.esc[telemetry_index].esc_current     = static_cast<float>(data.current) * 0.01f;
+		esc_status.esc[telemetry_index].esc_temperature = static_cast<float>(data.temperature);
 		// TODO: accumulate consumption and use for battery estimation
 	}
 
 	// publish when motor index wraps (which is robust against motor timeouts)
-	if (motor_index <= _telemetry->last_motor_index) {
+	if (telemetry_index <= _telemetry->last_telemetry_index) {
 		esc_status.timestamp = hrt_absolute_time();
 		esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_DSHOT;
 		esc_status.esc_count = _telemetry->handler.numMotors();
@@ -314,7 +319,7 @@ void DShot::handle_new_telemetry_data(const int motor_index, const DShotTelemetr
 		esc_status.esc_online_flags = 0;
 	}
 
-	_telemetry->last_motor_index = motor_index;
+	_telemetry->last_telemetry_index = telemetry_index;
 }
 
 int DShot::send_command_thread_safe(const dshot_command_t command, const int num_repetitions, const int motor_index)

src/drivers/dshot/DShot.h

@@ -127,14 +127,15 @@ private:
 	struct Telemetry {
 		DShotTelemetry handler{};
 		uORB::PublicationMultiData<esc_status_s> esc_status_pub{ORB_ID(esc_status)};
-		int last_motor_index{-1};
+		int last_telemetry_index{-1};
+		uint8_t actuator_functions[esc_status_s::CONNECTED_ESC_MAX] {};
 	};
 
 	void enable_dshot_outputs(const bool enabled);
 
 	void init_telemetry(const char *device);
 
-	void handle_new_telemetry_data(const int motor_index, const DShotTelemetry::EscData &data);
+	void handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data);
 
 	int request_esc_info();
 

src/drivers/tap_esc/TAP_ESC.cpp

@@ -290,6 +290,8 @@ bool TAP_ESC::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], u
 
 				if (feed_back_data.channelID < esc_status_s::CONNECTED_ESC_MAX) {
 					_esc_feedback.esc[feed_back_data.channelID].timestamp = hrt_absolute_time();
+					_esc_feedback.esc[feed_back_data.channelID].actuator_function = (uint8_t)_mixing_output.outputFunction(
+								feed_back_data.channelID);
 					_esc_feedback.esc[feed_back_data.channelID].esc_errorcount = 0;
 					_esc_feedback.esc[feed_back_data.channelID].esc_rpm = feed_back_data.speed;
 #if defined(ESC_HAVE_VOLTAGE_SENSOR)

src/drivers/telemetry/hott/messages.cpp

@@ -48,6 +48,7 @@
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/esc_status.h>
+#include <uORB/topics/actuator_motors.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/vehicle_air_data.h>
 #include <uORB/topics/vehicle_gps_position.h>
@@ -114,6 +115,7 @@ publish_gam_message(const uint8_t *buffer)
 	esc.esc_count = 1;
 	esc.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_PPM;
 
+	esc.esc[0].actuator_function = actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1;
 	esc.esc[0].esc_rpm = (uint16_t)((msg.rpm_H << 8) | (msg.rpm_L & 0xff)) * 10;
 	esc.esc[0].esc_temperature = static_cast<float>(msg.temperature1 - 20);
 	esc.esc[0].esc_voltage = static_cast<float>((msg.main_voltage_H << 8) | (msg.main_voltage_L & 0xff)) * 0.1F;

src/drivers/uavcan/actuators/esc.hpp

@@ -78,6 +78,8 @@ public:
 
 	static int max_output_value() { return uavcan::equipment::esc::RawCommand::FieldTypes::cmd::RawValueType::max(); }
 
+	esc_status_s &esc_status() { return _esc_status; }
+
 private:
 	/**
 	 * ESC status message reception will be reported via this callback.

src/drivers/uavcan/uavcan_main.cpp

@@ -1003,6 +1003,10 @@ void UavcanMixingInterfaceESC::mixerChanged()
 	if (_mixing_output.useDynamicMixing()) {
 		for (unsigned i = 0; i < MAX_ACTUATORS; ++i) {
 			rotor_count += _mixing_output.isFunctionSet(i);
+
+			if (i < esc_status_s::CONNECTED_ESC_MAX) {
+				_esc_controller.esc_status().esc[i].actuator_function = (uint8_t)_mixing_output.outputFunction(i);
+			}
 		}
 
 	} else {

src/lib/mixer_module/functions.hpp

@@ -116,6 +116,8 @@ public:
 	static_assert(actuator_motors_s::NUM_CONTROLS == (int)OutputFunction::MotorMax - (int)OutputFunction::Motor1 + 1,
 		      "Unexpected num motors");
 
+	static_assert(actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1 == (int)OutputFunction::Motor1, "Unexpected motor idx");
+
 	FunctionMotors(const Context &context);
 	static FunctionProviderBase *allocate(const Context &context) { return new FunctionMotors(context); }