From dfd934fbdbf2da59e46cf67a82d086f86d122a53 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 24 Jan 2022 11:20:27 +0100 Subject: [PATCH] esc_report: add actuator_function --- msg/actuator_motors.msg | 2 ++ msg/esc_report.msg | 2 ++ src/drivers/dshot/DShot.cpp | 27 +++++++++++++++---------- src/drivers/dshot/DShot.h | 5 +++-- src/drivers/tap_esc/TAP_ESC.cpp | 2 ++ src/drivers/telemetry/hott/messages.cpp | 2 ++ src/drivers/uavcan/actuators/esc.hpp | 2 ++ src/drivers/uavcan/uavcan_main.cpp | 4 ++++ src/lib/mixer_module/functions.hpp | 2 ++ 9 files changed, 35 insertions(+), 13 deletions(-) diff --git a/msg/actuator_motors.msg b/msg/actuator_motors.msg index acaeed88d3..e74566f1f7 100644 --- a/msg/actuator_motors.msg +++ b/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), diff --git a/msg/esc_report.msg b/msg/esc_report.msg index c5874d6480..1b18450380 100644 --- a/msg/esc_report.msg +++ b/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) diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp index fec51e4afc..013faad440 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/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) { - esc_status.esc_online_flags |= 1 << motor_index; + if (telemetry_index < esc_status_s::CONNECTED_ESC_MAX) { + esc_status.esc_online_flags |= 1 << telemetry_index; - esc_status.esc[motor_index].timestamp = data.time; - esc_status.esc[motor_index].esc_rpm = (static_cast(data.erpm) * 100) / (_param_mot_pole_count.get() / 2); - esc_status.esc[motor_index].esc_voltage = static_cast(data.voltage) * 0.01f; - esc_status.esc[motor_index].esc_current = static_cast(data.current) * 0.01f; - esc_status.esc[motor_index].esc_temperature = static_cast(data.temperature); + esc_status.esc[telemetry_index].actuator_function = _telemetry->actuator_functions[telemetry_index]; + esc_status.esc[telemetry_index].timestamp = data.time; + esc_status.esc[telemetry_index].esc_rpm = (static_cast(data.erpm) * 100) / + (_param_mot_pole_count.get() / 2); + esc_status.esc[telemetry_index].esc_voltage = static_cast(data.voltage) * 0.01f; + esc_status.esc[telemetry_index].esc_current = static_cast(data.current) * 0.01f; + esc_status.esc[telemetry_index].esc_temperature = static_cast(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) diff --git a/src/drivers/dshot/DShot.h b/src/drivers/dshot/DShot.h index d76b8f091c..6e9fff8d31 100644 --- a/src/drivers/dshot/DShot.h +++ b/src/drivers/dshot/DShot.h @@ -127,14 +127,15 @@ private: struct Telemetry { DShotTelemetry handler{}; uORB::PublicationMultiData 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(); diff --git a/src/drivers/tap_esc/TAP_ESC.cpp b/src/drivers/tap_esc/TAP_ESC.cpp index 7598f97a4f..9891108f83 100644 --- a/src/drivers/tap_esc/TAP_ESC.cpp +++ b/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) diff --git a/src/drivers/telemetry/hott/messages.cpp b/src/drivers/telemetry/hott/messages.cpp index 92ecd9a162..f8e9192570 100644 --- a/src/drivers/telemetry/hott/messages.cpp +++ b/src/drivers/telemetry/hott/messages.cpp @@ -48,6 +48,7 @@ #include #include #include +#include #include #include #include @@ -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(msg.temperature1 - 20); esc.esc[0].esc_voltage = static_cast((msg.main_voltage_H << 8) | (msg.main_voltage_L & 0xff)) * 0.1F; diff --git a/src/drivers/uavcan/actuators/esc.hpp b/src/drivers/uavcan/actuators/esc.hpp index ae97ea140c..4ff9c02ed8 100644 --- a/src/drivers/uavcan/actuators/esc.hpp +++ b/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. diff --git a/src/drivers/uavcan/uavcan_main.cpp b/src/drivers/uavcan/uavcan_main.cpp index 4c035a01aa..e1bfc231b0 100644 --- a/src/drivers/uavcan/uavcan_main.cpp +++ b/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 { diff --git a/src/lib/mixer_module/functions.hpp b/src/lib/mixer_module/functions.hpp index 5fba447bdf..1059baee2a 100644 --- a/src/lib/mixer_module/functions.hpp +++ b/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); }