From f8f2a9c8862f761713ca0aebb95d47b12104da23 Mon Sep 17 00:00:00 2001 From: Maxim Buzdalov Date: Sun, 25 Feb 2024 16:22:57 +0000 Subject: [PATCH] AP_ESC_Telem: support Extended DShot Telemetry v2 --- libraries/AP_ESC_Telem/AP_ESC_Telem.cpp | 111 +++++++++++++++++- libraries/AP_ESC_Telem/AP_ESC_Telem.h | 6 + libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h | 8 ++ libraries/AP_ESC_Telem/LogStructure.h | 40 ++++++- libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp | 22 +++- libraries/AP_Logger/LogStructure.h | 1 + 6 files changed, 184 insertions(+), 4 deletions(-) diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp index a82a014dedd64..a905e27408339 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp @@ -465,9 +465,19 @@ void AP_ESC_Telem::update_telem_data(const uint8_t esc_index, const AP_ESC_Telem telemdata.usage_s = new_data.usage_s; } +#ifdef HAL_WITH_BIDIR_DSHOT + if (data_mask & AP_ESC_Telem_Backend::TelemetryType::EDT2_STATUS) { + telemdata.edt2_status = merge_edt2_status(telemdata.edt2_status, new_data.edt2_status); + } + if (data_mask & AP_ESC_Telem_Backend::TelemetryType::EDT2_STRESS) { + telemdata.edt2_stress = merge_edt2_stress(telemdata.edt2_stress, new_data.edt2_stress); + } +#endif + telemdata.count++; telemdata.types |= data_mask; telemdata.last_update_ms = AP_HAL::millis(); + } // record an update to the RPM together with timestamp, this allows the notch values to be slewed @@ -496,6 +506,59 @@ void AP_ESC_Telem::update_rpm(const uint8_t esc_index, const float new_rpm, cons #endif } +#ifdef HAL_WITH_BIDIR_DSHOT + +// The following is based on https://github.com/bird-sanctuary/extended-dshot-telemetry. +// For the following part we explain the bits of Extended DShot Telemetry v2 status telemetry: +// - bits 0-3: the "stress level" +// - bit 5: the "error" bit (e.g. the demag event in Bluejay) +// - bit 6: the "warning" bit (e.g. the desync event in Bluejay) +// - bit 7: the "alert" bit (e.g. the stall event in Bluejay) + +// Since logger can read out telemetry values less frequently than they are updated, +// it makes sense to aggregate these status bits, and to collect the maximum observed stress level. +// To reduce the logging rate of the EDT2 messages, we will log them only once something has changed. +// To track this, we are going to (ab)use bit 15 of the field: 1 means there is something to write. + +// EDTv2 also features separate "stress" messages. +// These come more frequently, and are scaled differently (the allowed range is from 0 to 255), +// so we have to log them separately. + +constexpr uint16_t EDT2_TELEM_UPDATED = 0x8000U; +constexpr uint16_t EDT2_STRESS_0F_MASK = 0xfU; +constexpr uint16_t EDT2_STRESS_FF_MASK = 0xffU; +constexpr uint16_t EDT2_ERROR_MASK = 0x20U; +constexpr uint16_t EDT2_WARNING_MASK = 0x40U; +constexpr uint16_t EDT2_ALERT_MASK = 0x80U; +constexpr uint16_t EDT2_ALL_BITS = EDT2_ERROR_MASK | EDT2_WARNING_MASK | EDT2_ALERT_MASK; + +uint16_t AP_ESC_Telem::merge_edt2_status(uint16_t old_status, uint16_t new_status) +{ + return uint16_t( + EDT2_TELEM_UPDATED | // the highest bit "telemetry is updated" is always set + (old_status & ~EDT2_STRESS_0F_MASK) | // old status except for the stress is preserved + (new_status & EDT2_ALL_BITS) | // all new status bits are included + std::max(old_status & EDT2_STRESS_0F_MASK, new_status & EDT2_STRESS_0F_MASK) // the stress is maxed out + ); +} + +uint16_t AP_ESC_Telem::merge_edt2_stress(uint16_t old_stress, uint16_t new_stress) +{ + return uint16_t( + EDT2_TELEM_UPDATED | // the highest bit "telemetry is updated" is always set + std::max(old_stress & EDT2_STRESS_FF_MASK, new_stress & EDT2_STRESS_FF_MASK) // the stress is maxed out + ); +} + +#define EDT2_HAS_NEW_DATA(status) bool(status & EDT2_TELEM_UPDATED) +#define EDT2_STRESS_FROM_STATUS(status) uint8_t(status & EDT2_STRESS_0F_MASK) +#define EDT2_ERROR_BIT_FROM_STATUS(status) bool(edt2_status & EDT2_ERROR_MASK) +#define EDT2_WARNING_BIT_FROM_STATUS(status) bool(edt2_status & EDT2_WARNING_MASK) +#define EDT2_ALERT_BIT_FROM_STATUS(status) bool(edt2_status & EDT2_ALERT_MASK) +#define EDT2_STRESS_FROM_STRESS(stress) uint8_t(stress & EDT2_STRESS_FF_MASK) + +#endif //HAL_WITH_BIDIR_DSHOT + void AP_ESC_Telem::update() { #if HAL_LOGGING_ENABLED @@ -504,7 +567,7 @@ void AP_ESC_Telem::update() for (uint8_t i = 0; i < ESC_TELEM_MAX_ESCS; i++) { const volatile AP_ESC_Telem_Backend::RpmData &rpmdata = _rpm_data[i]; - const volatile AP_ESC_Telem_Backend::TelemetryData &telemdata = _telem_data[i]; + volatile AP_ESC_Telem_Backend::TelemetryData &telemdata = _telem_data[i]; // Push received telemetry data into the logging system if (logger && logger->logging_enabled()) { if (telemdata.last_update_ms != _last_telem_log_ms[i] @@ -541,6 +604,52 @@ void AP_ESC_Telem::update() _last_telem_log_ms[i] = telemdata.last_update_ms; _last_rpm_log_us[i] = rpmdata.last_update_us; } + +#ifdef HAL_WITH_BIDIR_DSHOT + // Write an EDTv2 status message, if there is any update + uint16_t edt2_status = telemdata.edt2_status; + if (EDT2_HAS_NEW_DATA(edt2_status)) { + // An EDT2 status message is: + // id starts from 0 + // alert, warning, error are flags (so 0 or 1) + // stress is in [0; 15] + const struct log_Edt2 pkt_edt2{ + LOG_PACKET_HEADER_INIT(uint8_t(LOG_EDT2_MSG)), + time_us : now_us64, + instance : i, + alert : EDT2_ALERT_BIT_FROM_STATUS(edt2_status), + warning : EDT2_WARNING_BIT_FROM_STATUS(edt2_status), + error : EDT2_ERROR_BIT_FROM_STATUS(edt2_status), + max_stress : EDT2_STRESS_FROM_STATUS(edt2_status) + }; + if (AP::logger().WriteBlock_first_succeed(&pkt_edt2, sizeof(pkt_edt2))) { + // Only clean the status if the write succeeded. + // This is important because, if rate limiting is enabled, + // the log-on-change behavior may lose a lot of entries + telemdata.edt2_status = 0; + } + } + + // Write an EDTv2 stress value, if there is any update + uint16_t edt2_stress = telemdata.edt2_stress; + if (EDT2_HAS_NEW_DATA(edt2_stress)) { + // An EDTS stress message is: + // id starts from 0 + // stress is in [0; 255] + const struct log_Edts pkt_edts{ + LOG_PACKET_HEADER_INIT(uint8_t(LOG_EDTS_MSG)), + time_us : now_us64, + instance : i, + stress : EDT2_STRESS_FROM_STRESS(edt2_stress) + }; + if (AP::logger().WriteBlock_first_succeed(&pkt_edts, sizeof(pkt_edts))) { + // Only clean the stress if the write succeeded. + // This is important because, if rate limiting is enabled, + // the log-on-change behavior may lose a lot of entries + telemdata.edt2_stress = 0; + } + } +#endif // HAL_WITH_BIDIR_DSHOT } } #endif // HAL_LOGGING_ENABLED diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.h b/libraries/AP_ESC_Telem/AP_ESC_Telem.h index ae40ede597079..dafd52c94feaf 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.h +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.h @@ -118,6 +118,12 @@ class AP_ESC_Telem { static bool rpm_data_within_timeout (const volatile AP_ESC_Telem_Backend::RpmData &instance, const uint32_t now_us, const uint32_t timeout_us); static bool was_rpm_data_ever_reported (const volatile AP_ESC_Telem_Backend::RpmData &instance); + // helpers that aggregate data in EDTv2 messages +#ifdef HAL_WITH_BIDIR_DSHOT + static uint16_t merge_edt2_status(uint16_t old_status, uint16_t new_status); + static uint16_t merge_edt2_stress(uint16_t old_stress, uint16_t new_stress); +#endif + // rpm data volatile AP_ESC_Telem_Backend::RpmData _rpm_data[ESC_TELEM_MAX_ESCS]; // telemetry data diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h b/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h index b6d6725a55fa1..a4d8e2df17780 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h @@ -19,6 +19,10 @@ class AP_ESC_Telem_Backend { uint32_t last_update_ms; // last update time in milliseconds, determines whether active uint16_t types; // telemetry types present uint16_t count; // number of times updated +#ifdef HAL_WITH_BIDIR_DSHOT + uint16_t edt2_status; // status reported by Extended DShot Telemetry v2 + uint16_t edt2_stress; // stress reported in dedicated messages by Extended DShot Telemetry v2 +#endif // return true if the data is stale bool stale(uint32_t now_ms=0) const volatile; @@ -42,6 +46,10 @@ class AP_ESC_Telem_Backend { USAGE = 1 << 5, TEMPERATURE_EXTERNAL = 1 << 6, MOTOR_TEMPERATURE_EXTERNAL = 1 << 7, +#ifdef HAL_WITH_BIDIR_DSHOT + EDT2_STATUS = 1 << 8, + EDT2_STRESS = 1 << 9, +#endif }; diff --git a/libraries/AP_ESC_Telem/LogStructure.h b/libraries/AP_ESC_Telem/LogStructure.h index a3c0dc791cd3d..0380230105f30 100644 --- a/libraries/AP_ESC_Telem/LogStructure.h +++ b/libraries/AP_ESC_Telem/LogStructure.h @@ -3,7 +3,9 @@ #include #define LOG_IDS_FROM_ESC_TELEM \ - LOG_ESC_MSG + LOG_ESC_MSG, \ + LOG_EDT2_MSG, \ + LOG_EDTS_MSG // @LoggerMessage: ESC // @Description: Feedback received from ESCs @@ -34,3 +36,39 @@ struct PACKED log_Esc { #define LOG_STRUCTURE_FROM_ESC_TELEM \ { LOG_ESC_MSG, sizeof(log_Esc), \ "ESC", "QBffffcfcf", "TimeUS,Instance,RPM,RawRPM,Volt,Curr,Temp,CTot,MotTemp,Err", "s#qqvAOaO%", "F-00--BCB-" , true }, + +// @LoggerMessage: EDT2 +// @Description: Status received from ESCs via Extended DShot telemetry v2 +// @Field: TimeUS: microseconds since system startup +// @Field: Instance: ESC instance number +// @Field: Alert: the "alert" status bit +// @Field: Warning: the "warning" status bit +// @Field: Error: the "error" status bit +// @Field: MaxStress: the maximum stress level observed for this ESC, scaled to [0..15] +struct PACKED log_Edt2 { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t instance; + uint8_t alert; + uint8_t warning; + uint8_t error; + uint8_t max_stress; +}; + +// @LoggerMessage: EDTS +// @Description: Stress values received from ESCs via Extended DShot telemetry v2 +// @Field: TimeUS: microseconds since system startup +// @Field: Instance: ESC instance number +// @Field: Stress: the stress value observed for this ESC, scaled to [0..255] +struct PACKED log_Edts { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t instance; + uint8_t stress; +}; + +#define LOG_STRUCTURE_FROM_EDT2_TELEM \ + { LOG_EDT2_MSG, sizeof(log_Edt2), \ + "EDT2", "QBBBBB", "TimeUS,Instance,Alert,Warning,Error,MaxStress", "s#----", "F-----" , true }, \ + { LOG_EDTS_MSG, sizeof(log_Edts), \ + "EDTS", "QBB", "TimeUS,Instance,Stress", "s#-", "F--", true }, diff --git a/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp b/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp index d8fd73fc8f752..1502c3ba5fe30 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp @@ -796,10 +796,28 @@ bool RCOutput::bdshot_decode_telemetry_from_erpm(uint16_t encodederpm, uint8_t c break; case 0b100: // Debug 1 case 0b101: // Debug 2 - case 0b110: // Stress level - case 0b111: // Status return false; break; + case 0b110: { // Stress level + #if HAL_WITH_ESC_TELEM && defined(HAL_WITH_BIDIR_DSHOT) + TelemetryData t { + .edt2_stress = value + }; + update_telem_data(normalized_chan, t, AP_ESC_Telem_Backend::TelemetryType::EDT2_STRESS); + #endif + return false; + } + break; + case 0b111: { // Status + #if HAL_WITH_ESC_TELEM && defined(HAL_WITH_BIDIR_DSHOT) + TelemetryData t { + .edt2_status = value + }; + update_telem_data(normalized_chan, t, AP_ESC_Telem_Backend::TelemetryType::EDT2_STATUS); + #endif + return false; + } + break; default: // eRPM break; } diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index 54dd754e6200d..bb2a64d108e1b 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -1297,6 +1297,7 @@ LOG_STRUCTURE_FROM_AVOIDANCE \ { LOG_TERRAIN_MSG, sizeof(log_TERRAIN), \ "TERR","QBLLHffHHf","TimeUS,Status,Lat,Lng,Spacing,TerrH,CHeight,Pending,Loaded,ROfs", "s-DU-mm--m", "F-GG-00--0", true }, \ LOG_STRUCTURE_FROM_ESC_TELEM \ +LOG_STRUCTURE_FROM_EDT2_TELEM \ { LOG_CSRV_MSG, sizeof(log_CSRV), \ "CSRV","QBfffBfffffB","TimeUS,Id,Pos,Force,Speed,Pow,PosCmd,V,A,MotT,PCBT,Err", "s#---%dvAOO-", "F-000000000-", true }, \ { LOG_PIDR_MSG, sizeof(log_PID), \