diff --git a/Tools/IO_Firmware/iofirmware_dshot_highpolh.bin b/Tools/IO_Firmware/iofirmware_dshot_highpolh.bin index a986d7dfa7480..210d3d86e1094 100755 Binary files a/Tools/IO_Firmware/iofirmware_dshot_highpolh.bin and b/Tools/IO_Firmware/iofirmware_dshot_highpolh.bin differ diff --git a/Tools/IO_Firmware/iofirmware_dshot_lowpolh.bin b/Tools/IO_Firmware/iofirmware_dshot_lowpolh.bin index 01e10a37e4e82..62a6c45273c2c 100755 Binary files a/Tools/IO_Firmware/iofirmware_dshot_lowpolh.bin and b/Tools/IO_Firmware/iofirmware_dshot_lowpolh.bin differ diff --git a/Tools/IO_Firmware/iofirmware_f103_8MHz_highpolh.bin b/Tools/IO_Firmware/iofirmware_f103_8MHz_highpolh.bin index 9d79171876dfd..b9db1d2cd273d 100755 Binary files a/Tools/IO_Firmware/iofirmware_f103_8MHz_highpolh.bin and b/Tools/IO_Firmware/iofirmware_f103_8MHz_highpolh.bin differ diff --git a/Tools/IO_Firmware/iofirmware_f103_8MHz_lowpolh.bin b/Tools/IO_Firmware/iofirmware_f103_8MHz_lowpolh.bin index cba2c729d057e..c25a1d1ca8d80 100755 Binary files a/Tools/IO_Firmware/iofirmware_f103_8MHz_lowpolh.bin and b/Tools/IO_Firmware/iofirmware_f103_8MHz_lowpolh.bin differ diff --git a/Tools/IO_Firmware/iofirmware_f103_dshot_highpolh.bin b/Tools/IO_Firmware/iofirmware_f103_dshot_highpolh.bin index e27740255e5a3..1b5104df03ad9 100755 Binary files a/Tools/IO_Firmware/iofirmware_f103_dshot_highpolh.bin and b/Tools/IO_Firmware/iofirmware_f103_dshot_highpolh.bin differ diff --git a/Tools/IO_Firmware/iofirmware_f103_dshot_lowpolh.bin b/Tools/IO_Firmware/iofirmware_f103_dshot_lowpolh.bin index a3e33ac45ea88..c0fb06d32219c 100755 Binary files a/Tools/IO_Firmware/iofirmware_f103_dshot_lowpolh.bin and b/Tools/IO_Firmware/iofirmware_f103_dshot_lowpolh.bin differ diff --git a/Tools/IO_Firmware/iofirmware_f103_highpolh.bin b/Tools/IO_Firmware/iofirmware_f103_highpolh.bin index c3d67598c1b11..788a7d2eb42b7 100755 Binary files a/Tools/IO_Firmware/iofirmware_f103_highpolh.bin and b/Tools/IO_Firmware/iofirmware_f103_highpolh.bin differ diff --git a/Tools/IO_Firmware/iofirmware_f103_lowpolh.bin b/Tools/IO_Firmware/iofirmware_f103_lowpolh.bin index 3a43e19918a21..d3f696f4793a1 100755 Binary files a/Tools/IO_Firmware/iofirmware_f103_lowpolh.bin and b/Tools/IO_Firmware/iofirmware_f103_lowpolh.bin differ diff --git a/Tools/IO_Firmware/iofirmware_highpolh.bin b/Tools/IO_Firmware/iofirmware_highpolh.bin index f03fc5273eb09..dcbdd81a9cf0a 100755 Binary files a/Tools/IO_Firmware/iofirmware_highpolh.bin and b/Tools/IO_Firmware/iofirmware_highpolh.bin differ diff --git a/Tools/IO_Firmware/iofirmware_lowpolh.bin b/Tools/IO_Firmware/iofirmware_lowpolh.bin index 648e4f962403c..d32dcb23a596e 100755 Binary files a/Tools/IO_Firmware/iofirmware_lowpolh.bin and b/Tools/IO_Firmware/iofirmware_lowpolh.bin differ diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp index e6f3bd523c392..e4daed9a097df 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp @@ -468,6 +468,15 @@ void AP_ESC_Telem::update_telem_data(const uint8_t esc_index, const AP_ESC_Telem telemdata.usage_s = new_data.usage_s; } +#if EDTV2_SUPPORT + 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(); @@ -499,6 +508,63 @@ void AP_ESC_Telem::update_rpm(const uint8_t esc_index, const float new_rpm, cons #endif } +#if EDTV2_SUPPORT + +// 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 stall event in Bluejay) +// - bit 6: the "warning" bit (e.g. the desync event in Bluejay) +// - bit 7: the "alert" bit (e.g. the demag 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 try to log them only once a new frame comes. +// 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; + +#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((status) & EDT2_ERROR_MASK) +#define EDT2_WARNING_BIT_FROM_STATUS(status) bool((status) & EDT2_WARNING_MASK) +#define EDT2_ALERT_BIT_FROM_STATUS(status) bool((status) & EDT2_ALERT_MASK) +#define EDT2_STRESS_FROM_STRESS(stress) uint8_t((stress) & EDT2_STRESS_FF_MASK) + +uint16_t AP_ESC_Telem::merge_edt2_status(uint16_t old_status, uint16_t new_status) +{ + if (EDT2_HAS_NEW_DATA(old_status)) { + new_status = uint16_t( + (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 + MAX(old_status & EDT2_STRESS_0F_MASK, new_status & EDT2_STRESS_0F_MASK) // the stress is maxed out + ); + } + return EDT2_TELEM_UPDATED | new_status; +} + +uint16_t AP_ESC_Telem::merge_edt2_stress(uint16_t old_stress, uint16_t new_stress) +{ + if (EDT2_HAS_NEW_DATA(old_stress)) { + new_stress = uint16_t( + MAX(old_stress & EDT2_STRESS_FF_MASK, new_stress & EDT2_STRESS_FF_MASK) // the stress is maxed out + ); + } + return EDT2_TELEM_UPDATED | new_stress; +} + +#endif // EDTV2_SUPPORT + void AP_ESC_Telem::update() { #if HAL_LOGGING_ENABLED @@ -507,7 +573,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] @@ -544,6 +610,51 @@ void AP_ESC_Telem::update() _last_telem_log_ms[i] = telemdata.last_update_ms; _last_rpm_log_us[i] = rpmdata.last_update_us; } + +#if EDTV2_SUPPORT + // Write an EDTv2 message, if there is any update + uint16_t edt2_status = telemdata.edt2_status; + uint16_t edt2_stress = telemdata.edt2_stress; + if (EDT2_HAS_NEW_DATA(edt2_status | edt2_stress)) { + // Could probably be faster/smaller with bitmasking, but not sure + uint8_t status = 0; + if (EDT2_HAS_NEW_DATA(edt2_stress)) { + status |= uint8_t(log_Edt2_Status::HAS_STRESS_DATA); + } + if (EDT2_HAS_NEW_DATA(edt2_status)) { + status |= uint8_t(log_Edt2_Status::HAS_STATUS_DATA); + } + if (EDT2_ALERT_BIT_FROM_STATUS(edt2_status)) { + status |= uint8_t(log_Edt2_Status::ALERT_BIT); + } + if (EDT2_WARNING_BIT_FROM_STATUS(edt2_status)) { + status |= uint8_t(log_Edt2_Status::WARNING_BIT); + } + if (EDT2_ERROR_BIT_FROM_STATUS(edt2_status)) { + status |= uint8_t(log_Edt2_Status::ERROR_BIT); + } + // An EDT2 status message is: + // id: starts from 0 + // stress: the current stress which comes from edt2_stress + // max_stress: the maximum stress which comes from edt2_status + // status: the status bits which come from both + const struct log_Edt2 pkt_edt2{ + LOG_PACKET_HEADER_INIT(uint8_t(LOG_EDT2_MSG)), + time_us : now_us64, + instance : i, + stress : EDT2_STRESS_FROM_STRESS(edt2_stress), + max_stress : EDT2_STRESS_FROM_STATUS(edt2_status), + status : status, + }; + if (AP::logger().WriteBlock_first_succeed(&pkt_edt2, sizeof(pkt_edt2))) { + // Only clean the telem_updated bits 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 &= ~EDT2_TELEM_UPDATED; + telemdata.edt2_stress &= ~EDT2_TELEM_UPDATED; + } + } +#endif // EDTV2_SUPPORT } } #endif // HAL_LOGGING_ENABLED @@ -608,4 +719,4 @@ AP_ESC_Telem &esc_telem() }; -#endif +#endif // HAL_WITH_ESC_TELEM diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.h b/libraries/AP_ESC_Telem/AP_ESC_Telem.h index ae40ede597079..792e5beff1b42 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); +#if EDTV2_SUPPORT + // helpers that aggregate data in EDTv2 messages + 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..62163fab8f46f 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 +#if EDTV2_SUPPORT + 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, +#if EDTV2_SUPPORT + 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..c50ac47b95e97 100644 --- a/libraries/AP_ESC_Telem/LogStructure.h +++ b/libraries/AP_ESC_Telem/LogStructure.h @@ -3,7 +3,8 @@ #include #define LOG_IDS_FROM_ESC_TELEM \ - LOG_ESC_MSG + LOG_ESC_MSG, \ + LOG_EDT2_MSG // @LoggerMessage: ESC // @Description: Feedback received from ESCs @@ -31,6 +32,35 @@ struct PACKED log_Esc { float error_rate; }; -#define LOG_STRUCTURE_FROM_ESC_TELEM \ +enum class log_Edt2_Status : uint8_t { + HAS_STRESS_DATA = 1U<<0, // true if the message contains up-to-date stress data + HAS_STATUS_DATA = 1U<<1, // true if the message contains up-to-date status data + ALERT_BIT = 1U<<2, // true if the last status had the "alert" bit (e.g. demag) + WARNING_BIT = 1U<<3, // true if the last status had the "warning" bit (e.g. desync) + ERROR_BIT = 1U<<4, // true if the last status had the "error" bit (e.g. stall) +}; + + +// @LoggerMessage: EDT2 +// @Description: Status received from ESCs via Extended DShot telemetry v2 +// @Field: TimeUS: microseconds since system startup +// @Field: Instance: ESC instance number +// @Field: Stress: current stress level (commutation effort), scaled to [0..255] +// @Field: MaxStress: maximum stress level (commutation effort) since arming, scaled to [0..15] +// @Field: Status: status bits +// @FieldBitmaskEnum: Status: log_Edt2_Status +struct PACKED log_Edt2 { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t instance; + uint8_t stress; + uint8_t max_stress; + uint8_t status; +}; + +#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 }, + "ESC", "QBffffcfcf", "TimeUS,Instance,RPM,RawRPM,Volt,Curr,Temp,CTot,MotTemp,Err", "s#qqvAOaO%", "F-00--BCB-" , true }, \ + { LOG_EDT2_MSG, sizeof(log_Edt2), \ + "EDT2", "QBBBB", "TimeUS,Instance,Stress,MaxStress,Status", "s#---", "F----" , true }, + diff --git a/libraries/AP_HAL/AP_HAL_Boards.h b/libraries/AP_HAL/AP_HAL_Boards.h index b2f38945cc68e..69340d20602ef 100644 --- a/libraries/AP_HAL/AP_HAL_Boards.h +++ b/libraries/AP_HAL/AP_HAL_Boards.h @@ -175,6 +175,19 @@ #define HAL_WITH_IO_MCU_DSHOT HAL_WITH_IO_MCU_BIDIR_DSHOT #endif +#ifndef HAL_REQUIRES_BDSHOT_SUPPORT +#define HAL_REQUIRES_BDSHOT_SUPPORT (defined(HAL_WITH_BIDIR_DSHOT) || HAL_WITH_IO_MCU_BIDIR_DSHOT) +#endif + +// support for Extended DShot Telemetry v2 is enabled only if any kind of such telemetry +// can in principle arrive, either from servo outputs or from IOMCU + +// if not desired, set to 0 - and if IOMCU has bidirectional DShot enabled, recompile it too, +// otherwise the communication to IOMCU breaks! +#ifndef EDTV2_SUPPORT +#define EDTV2_SUPPORT HAL_REQUIRES_BDSHOT_SUPPORT +#endif + // this is used as a general mechanism to make a 'small' build by // dropping little used features. We use this to allow us to keep // FMUv2 going for as long as possible diff --git a/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp b/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp index cb1c04e29d574..f16ed24221ead 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp @@ -755,12 +755,12 @@ bool RCOutput::bdshot_decode_telemetry_from_erpm(uint16_t encodederpm, uint8_t c uint16_t value = (encodederpm & 0x000001ffU); // 9bits #if HAL_WITH_ESC_TELEM uint8_t normalized_chan = chan; -#endif #if HAL_WITH_IO_MCU if (iomcu_enabled) { normalized_chan = chan + chan_offset; } #endif +#endif // HAL_WITH_ESC_TELEM: one can possibly imagine a FC with IOMCU but with ESC_TELEM compiled out... if (!(value & 0x100U) && (_dshot_esc_type == DSHOT_ESC_BLHELI_EDT || _dshot_esc_type == DSHOT_ESC_BLHELI_EDT_S)) { switch (expo) { @@ -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 && EDTV2_SUPPORT + 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 && EDTV2_SUPPORT + 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_IOMCU/AP_IOMCU.cpp b/libraries/AP_IOMCU/AP_IOMCU.cpp index 83ebd6e01a748..53bc42ace1f4f 100644 --- a/libraries/AP_IOMCU/AP_IOMCU.cpp +++ b/libraries/AP_IOMCU/AP_IOMCU.cpp @@ -453,7 +453,11 @@ void AP_IOMCU::read_telem() TelemetryData t { .temperature_cdeg = int16_t(telem->temperature_cdeg[i]), .voltage = float(telem->voltage_cvolts[i]) * 0.01, - .current = float(telem->current_camps[i]) * 0.01 + .current = float(telem->current_camps[i]) * 0.01, +#if EDTV2_SUPPORT + .edt2_status = telem->edt2_status[i], + .edt2_stress = telem->edt2_stress[i], +#endif }; update_telem_data(esc_group * 4 + i, t, telem->types[i]); } diff --git a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp index bb24b0dcad4c0..fb8885ce72a13 100644 --- a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp +++ b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp @@ -635,25 +635,34 @@ void AP_IOMCU_FW::telem_update() uint32_t now_ms = AP_HAL::millis(); for (uint8_t i = 0; i < IOMCU_MAX_TELEM_CHANNELS/4; i++) { + struct page_dshot_telem &dshot_i = dshot_telem[i]; for (uint8_t j = 0; j < 4; j++) { const uint8_t esc_id = (i * 4 + j); if (esc_id >= IOMCU_MAX_TELEM_CHANNELS) { continue; } - dshot_telem[i].error_rate[j] = uint16_t(roundf(hal.rcout->get_erpm_error_rate(esc_id) * 100.0)); + dshot_i.error_rate[j] = uint16_t(roundf(hal.rcout->get_erpm_error_rate(esc_id) * 100.0)); #if HAL_WITH_ESC_TELEM const volatile AP_ESC_Telem_Backend::TelemetryData& telem = esc_telem.get_telem_data(esc_id); // if data is stale then set to zero to avoid phantom data appearing in mavlink if (now_ms - telem.last_update_ms > ESC_TELEM_DATA_TIMEOUT_MS) { - dshot_telem[i].voltage_cvolts[j] = 0; - dshot_telem[i].current_camps[j] = 0; - dshot_telem[i].temperature_cdeg[j] = 0; + dshot_i.voltage_cvolts[j] = 0; + dshot_i.current_camps[j] = 0; + dshot_i.temperature_cdeg[j] = 0; +#if EDTV2_SUPPORT + dshot_i.edt2_status[j] = 0; + dshot_i.edt2_stress[j] = 0; +#endif continue; } - dshot_telem[i].voltage_cvolts[j] = uint16_t(roundf(telem.voltage * 100)); - dshot_telem[i].current_camps[j] = uint16_t(roundf(telem.current * 100)); - dshot_telem[i].temperature_cdeg[j] = telem.temperature_cdeg; - dshot_telem[i].types[j] = telem.types; + dshot_i.voltage_cvolts[j] = uint16_t(roundf(telem.voltage * 100)); + dshot_i.current_camps[j] = uint16_t(roundf(telem.current * 100)); + dshot_i.temperature_cdeg[j] = telem.temperature_cdeg; +#if EDTV2_SUPPORT + dshot_i.edt2_status[j] = uint8_t(telem.edt2_status); + dshot_i.edt2_stress[j] = uint8_t(telem.edt2_stress); +#endif + dshot_i.types[j] = telem.types; #endif } } diff --git a/libraries/AP_IOMCU/iofirmware/ioprotocol.h b/libraries/AP_IOMCU/iofirmware/ioprotocol.h index be00d927593f7..44f4eca67efa8 100644 --- a/libraries/AP_IOMCU/iofirmware/ioprotocol.h +++ b/libraries/AP_IOMCU/iofirmware/ioprotocol.h @@ -223,4 +223,9 @@ struct page_dshot_telem { uint16_t current_camps[4]; uint16_t temperature_cdeg[4]; uint16_t types[4]; +// if EDTv2 needs to be disabled, IOMCU firmware should be recompiled too, this is the reason +#if EDTV2_SUPPORT + uint8_t edt2_status[4]; + uint8_t edt2_stress[4]; +#endif };