From efbc9e64a4cb01b71ec7aff3adc55396d692108e Mon Sep 17 00:00:00 2001 From: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> Date: Wed, 19 Nov 2025 08:51:42 -0900 Subject: [PATCH] mavlink: esc: fix ESC_STATUS and ESC_INFO message emission. (#25849) * mavlink: esc: fix ESC_STATUS and ESC_INFO message emission. Fixes mavlink messages emission for ESC messages. Actuator --> MotorNumber mapping was not respected, the mavlink messages should be reporting the ESC status in motor number order not actuator order. * Update src/modules/mavlink/streams/ESC_STATUS.hpp Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> * Update src/modules/mavlink/streams/ESC_INFO.hpp Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> * remove dependency on mixer_module/output_functions.hpp * add actuator function definitions to EscReport.msg * clean up * add missing header --------- Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- msg/EscReport.msg | 13 +++ src/modules/mavlink/mavlink_main.cpp | 5 +- src/modules/mavlink/streams/ESC_INFO.hpp | 125 ++++++++++++++++----- src/modules/mavlink/streams/ESC_STATUS.hpp | 88 +++++++++++---- 4 files changed, 180 insertions(+), 51 deletions(-) diff --git a/msg/EscReport.msg b/msg/EscReport.msg index 9a75c3d7cc..ca80753f14 100644 --- a/msg/EscReport.msg +++ b/msg/EscReport.msg @@ -11,6 +11,19 @@ uint8 esc_state # State of ESC - depend on Vendor uint8 actuator_function # actuator output function (one of Motor1...MotorN) +uint8 ACTUATOR_FUNCTION_MOTOR1 = 101 +uint8 ACTUATOR_FUNCTION_MOTOR2 = 102 +uint8 ACTUATOR_FUNCTION_MOTOR3 = 103 +uint8 ACTUATOR_FUNCTION_MOTOR4 = 104 +uint8 ACTUATOR_FUNCTION_MOTOR5 = 105 +uint8 ACTUATOR_FUNCTION_MOTOR6 = 106 +uint8 ACTUATOR_FUNCTION_MOTOR7 = 107 +uint8 ACTUATOR_FUNCTION_MOTOR8 = 108 +uint8 ACTUATOR_FUNCTION_MOTOR9 = 109 +uint8 ACTUATOR_FUNCTION_MOTOR10 = 110 +uint8 ACTUATOR_FUNCTION_MOTOR11 = 111 +uint8 ACTUATOR_FUNCTION_MOTOR12 = 112 + uint16 failures # Bitmask to indicate the internal ESC faults int8 esc_power # Applied power 0-100 in % (negative values reserved) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 479fa5a0a9..b81b1af9d5 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1813,9 +1813,8 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("GIMBAL_DEVICE_ATTITUDE_STATUS", 1.0f); configure_stream_local("GIMBAL_MANAGER_STATUS", 0.5f); configure_stream_local("GIMBAL_DEVICE_SET_ATTITUDE", 2.0f); - configure_stream_local("ESC_INFO", 0.2f); - configure_stream_local("ESC_STATUS", 0.5f); - + configure_stream_local("ESC_INFO", 1.0f); + configure_stream_local("ESC_STATUS", 2.0f); configure_stream_local("ADSB_VEHICLE", 1.0f); configure_stream_local("ATTITUDE_TARGET", 0.5f); configure_stream_local("AVAILABLE_MODES", 0.3f); diff --git a/src/modules/mavlink/streams/ESC_INFO.hpp b/src/modules/mavlink/streams/ESC_INFO.hpp index 609112ed0a..d4066a2332 100644 --- a/src/modules/mavlink/streams/ESC_INFO.hpp +++ b/src/modules/mavlink/streams/ESC_INFO.hpp @@ -34,7 +34,9 @@ #ifndef ESC_INFO_HPP #define ESC_INFO_HPP +#include #include +#include class MavlinkStreamESCInfo : public MavlinkStream { @@ -49,50 +51,121 @@ public: unsigned get_size() override { - static constexpr unsigned size_per_batch = MAVLINK_MSG_ID_ESC_INFO_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; - return _esc_status_sub.advertised() ? size_per_batch * _number_of_batches : 0; + static constexpr unsigned message_size = MAVLINK_MSG_ID_ESC_INFO_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + return _esc_status_subs.advertised_count() * message_size; } private: explicit MavlinkStreamESCInfo(Mavlink *mavlink) : MavlinkStream(mavlink) {} - uORB::Subscription _esc_status_sub{ORB_ID(esc_status)}; - uint8_t _number_of_batches{0}; + uORB::SubscriptionMultiArray _esc_status_subs{ORB_ID::esc_status}; + + static constexpr uint8_t MAX_ESC_OUTPUTS = 12; // See output_functions.hpp + static constexpr uint8_t ESCS_PER_MSG = MAVLINK_MSG_ESC_INFO_FIELD_TEMPERATURE_LEN; + static constexpr uint8_t MAX_NUM_MSGS = MAX_ESC_OUTPUTS / ESCS_PER_MSG; + + static constexpr hrt_abstime ESC_TIMEOUT = 100000; + + struct EscOutputInterfaceInfo { + uint16_t counter; + uint8_t esc_count; + uint8_t esc_connectiontype; + uint8_t esc_online_flags; + }; + + struct EscInfo { + hrt_abstime timestamp; + uint16_t failure_flags; + uint32_t error_count; + int16_t temperature; + bool online; + }; + + int _total_esc_count = {}; + EscOutputInterfaceInfo _interface[MAX_NUM_MSGS] = {}; + EscInfo _escs[MAX_ESC_OUTPUTS] = {}; + + void update_data() override + { + int subscriber_count = math::min(_esc_status_subs.size(), MAX_NUM_MSGS); + + for (int i = 0; i < subscriber_count; i++) { + esc_status_s esc = {}; + + if (_esc_status_subs[i].update(&esc)) { + _interface[i].counter = esc.counter; + _interface[i].esc_count = esc.esc_count; + _interface[i].esc_connectiontype = esc.esc_connectiontype; + + // Capture online_flags, we will map from index to motor number + uint8_t online_flags = esc.esc_online_flags; + _interface[i].esc_online_flags = 0; + + for (int j = 0; j < esc_status_s::CONNECTED_ESC_MAX; j++) { + bool is_motor = ((int)esc.esc[j].actuator_function >= esc_report_s::ACTUATOR_FUNCTION_MOTOR1) && + ((int)esc.esc[j].actuator_function <= esc_report_s::ACTUATOR_FUNCTION_MOTOR12); + + if (is_motor) { + // Map OutputFunction number to index + int index = (int)esc.esc[j].actuator_function - esc_report_s::ACTUATOR_FUNCTION_MOTOR1; + _escs[index].online = online_flags & (1 << j); + _escs[index].failure_flags = esc.esc[j].failures; + _escs[index].error_count = esc.esc[j].esc_errorcount; + _escs[index].timestamp = esc.esc[j].timestamp; + _escs[index].temperature = esc.esc[j].esc_temperature * 100.f; + } + } + } + } + + int count = 0; + + for (int i = 0; i < MAX_NUM_MSGS; i++) { + count += _interface[i].esc_count; + } + + _total_esc_count = count; + } bool send() override { - static constexpr uint8_t batch_size = MAVLINK_MSG_ESC_INFO_FIELD_TEMPERATURE_LEN; - esc_status_s esc_status; + bool updated = false; - if (_esc_status_sub.update(&esc_status)) { - mavlink_esc_info_t msg{}; + for (int i = 0; i < MAX_NUM_MSGS; i++) { - msg.time_usec = esc_status.timestamp; - msg.counter = esc_status.counter; - msg.count = esc_status.esc_count; - msg.connection_type = esc_status.esc_connectiontype; - msg.info = esc_status.esc_online_flags; + hrt_abstime now = hrt_absolute_time(); - // Ceil value of integer division. For 1-4 esc => 1 batch, 5-8 esc => 2 batches etc - _number_of_batches = ceilf((float)esc_status.esc_count / batch_size); + mavlink_esc_info_t msg = {}; + msg.index = i * ESCS_PER_MSG; + msg.time_usec = now; + msg.counter = _interface[i].counter; + msg.count = _total_esc_count; + msg.connection_type = _interface[i].esc_connectiontype; + msg.info = _interface[i].esc_online_flags; - for (int batch_number = 0; batch_number < _number_of_batches; batch_number++) { - msg.index = batch_number * batch_size; + bool atleast_one_esc_updated = false; - for (int esc_index = 0; esc_index < batch_size ; esc_index++) { - msg.failure_flags[esc_index] = esc_status.esc[esc_index].failures; - msg.error_count[esc_index] = esc_status.esc[esc_index].esc_errorcount; - msg.temperature[esc_index] = static_cast(esc_status.esc[esc_index].esc_temperature * - 100.f); // convert to centiDegrees + for (int j = 0; j < ESCS_PER_MSG; j++) { + + EscInfo &esc = _escs[i * ESCS_PER_MSG + j]; + + msg.info |= (esc.online << j); + + if ((esc.timestamp != 0) && (esc.timestamp + ESC_TIMEOUT) > now) { + msg.failure_flags[j] = esc.failure_flags; + msg.error_count[j] = esc.error_count; + msg.temperature[j] = esc.temperature; + atleast_one_esc_updated = true; } - - mavlink_msg_esc_info_send_struct(_mavlink->get_channel(), &msg); } - return true; + if (atleast_one_esc_updated) { + mavlink_msg_esc_info_send_struct(_mavlink->get_channel(), &msg); + updated = true; + } } - return false; + return updated; } }; diff --git a/src/modules/mavlink/streams/ESC_STATUS.hpp b/src/modules/mavlink/streams/ESC_STATUS.hpp index 54c11cbd8e..925f77ab81 100644 --- a/src/modules/mavlink/streams/ESC_STATUS.hpp +++ b/src/modules/mavlink/streams/ESC_STATUS.hpp @@ -34,7 +34,9 @@ #ifndef ESC_STATUS_HPP #define ESC_STATUS_HPP +#include #include +#include class MavlinkStreamESCStatus : public MavlinkStream { @@ -49,46 +51,88 @@ public: unsigned get_size() override { - static constexpr unsigned size_per_batch = MAVLINK_MSG_ID_ESC_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; - return _esc_status_sub.advertised() ? size_per_batch * _number_of_batches : 0; + static constexpr unsigned message_size = MAVLINK_MSG_ID_ESC_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + return _esc_status_subs.advertised_count() * message_size; } private: explicit MavlinkStreamESCStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} - uORB::Subscription _esc_status_sub{ORB_ID(esc_status)}; - uint8_t _number_of_batches{0}; + uORB::SubscriptionMultiArray _esc_status_subs{ORB_ID::esc_status}; + + static constexpr uint8_t MAX_ESC_OUTPUTS = 12; // See output_functions.hpp + static constexpr uint8_t ESCS_PER_MSG = MAVLINK_MSG_ESC_STATUS_FIELD_RPM_LEN; + static constexpr uint8_t MAX_NUM_MSGS = MAX_ESC_OUTPUTS / ESCS_PER_MSG; + static constexpr hrt_abstime ESC_TIMEOUT = 100000; + + struct EscStatus { + hrt_abstime timestamp; + int32_t rpm; + float voltage; + float current; + }; + + EscStatus _escs[MAX_ESC_OUTPUTS] = {}; + + void update_data() override + { + int subscriber_count = math::min(_esc_status_subs.size(), MAX_NUM_MSGS); + + for (int i = 0; i < subscriber_count; i++) { + esc_status_s esc = {}; + + if (_esc_status_subs[i].update(&esc)) { + for (int j = 0; j < esc_status_s::CONNECTED_ESC_MAX; j++) { + + bool is_motor = ((int)esc.esc[j].actuator_function >= esc_report_s::ACTUATOR_FUNCTION_MOTOR1) && + ((int)esc.esc[j].actuator_function <= esc_report_s::ACTUATOR_FUNCTION_MOTOR12); + + if (is_motor) { + // Map OutputFunction number to index + int index = (int)esc.esc[j].actuator_function - esc_report_s::ACTUATOR_FUNCTION_MOTOR1; + _escs[index].timestamp = esc.esc[j].timestamp; + _escs[index].rpm = esc.esc[j].esc_rpm; + _escs[index].voltage = esc.esc[j].esc_voltage; + _escs[index].current = esc.esc[j].esc_current; + } + } + } + } + } bool send() override { - static constexpr uint8_t batch_size = MAVLINK_MSG_ESC_STATUS_FIELD_RPM_LEN; - esc_status_s esc_status; + bool updated = false; - if (_esc_status_sub.update(&esc_status)) { - mavlink_esc_status_t msg{}; + for (int i = 0; i < MAX_NUM_MSGS; i++) { - msg.time_usec = esc_status.timestamp; + hrt_abstime now = hrt_absolute_time(); - // Ceil value of integer division. For 1-4 esc => 1 batch, 5-8 esc => 2 batches etc - _number_of_batches = ceilf((float)esc_status.esc_count / batch_size); + mavlink_esc_status_t msg = {}; + msg.index = i * ESCS_PER_MSG; + msg.time_usec = now; - for (int batch_number = 0; batch_number < _number_of_batches; batch_number++) { - msg.index = batch_number * batch_size; + bool atleast_one_esc_updated = false; - for (int esc_index = 0; esc_index < batch_size - && msg.index + esc_index < esc_status_s::CONNECTED_ESC_MAX; esc_index++) { - msg.rpm[esc_index] = esc_status.esc[msg.index + esc_index].esc_rpm; - msg.voltage[esc_index] = esc_status.esc[msg.index + esc_index].esc_voltage; - msg.current[esc_index] = esc_status.esc[msg.index + esc_index].esc_current; + for (int j = 0; j < ESCS_PER_MSG; j++) { + + EscStatus &esc = _escs[i * ESCS_PER_MSG + j]; + + if ((esc.timestamp != 0) && (esc.timestamp + ESC_TIMEOUT) > now) { + msg.rpm[j] = esc.rpm; + msg.voltage[j] = esc.voltage; + msg.current[j] = esc.current; + atleast_one_esc_updated = true; } - - mavlink_msg_esc_status_send_struct(_mavlink->get_channel(), &msg); } - return true; + if (atleast_one_esc_updated) { + mavlink_msg_esc_status_send_struct(_mavlink->get_channel(), &msg); + updated = true; + } } - return false; + return updated; } };