diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index 6f704279e8..0e2f647a37 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp @@ -539,6 +539,7 @@ void RCOutput::set_dshot_esc_type(DshotEscType dshot_esc_type) _dshot_esc_type = dshot_esc_type; switch (_dshot_esc_type) { case DSHOT_ESC_BLHELI_S: + case DSHOT_ESC_BLHELI_EDT_S: DSHOT_BIT_WIDTH_TICKS = DSHOT_BIT_WIDTH_TICKS_S; DSHOT_BIT_0_TICKS = DSHOT_BIT_0_TICKS_S; DSHOT_BIT_1_TICKS = DSHOT_BIT_1_TICKS_S; @@ -1531,15 +1532,9 @@ void RCOutput::dshot_send(pwm_group &group, uint64_t time_out_us) uint8_t chan = group.chan[i]; if (group.is_chan_enabled(i)) { #ifdef HAL_WITH_BIDIR_DSHOT - // retrieve the last erpm values - const uint16_t erpm = group.bdshot.erpm[i]; -#if HAL_WITH_ESC_TELEM - // update the ESC telemetry data - if (erpm < 0xFFFF && group.bdshot.enabled) { - update_rpm(chan, erpm * 200 / _bdshot.motor_poles, get_erpm_error_rate(chan)); + if (group.bdshot.enabled) { + bdshot_decode_telemetry_from_erpm(group.bdshot.erpm[i], chan); } -#endif - _bdshot.erpm[chan] = erpm; #endif if (safety_on && !(safety_mask & (1U<<(chan+chan_offset)))) { // safety is on, don't output anything diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.h b/libraries/AP_HAL_ChibiOS/RCOutput.h index a6e7a9fa54..c8a97bbf80 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.h +++ b/libraries/AP_HAL_ChibiOS/RCOutput.h @@ -222,7 +222,7 @@ public: Send a dshot command, if command timout is 0 then 10 commands are sent chan is the servo channel to send the command to */ - void send_dshot_command(uint8_t command, uint8_t chan, uint32_t command_timeout_ms = 0, uint16_t repeat_count = 10, bool priority = false) override; + void send_dshot_command(uint8_t command, uint8_t chan = ALL_CHANNELS, uint32_t command_timeout_ms = 0, uint16_t repeat_count = 10, bool priority = false) override; /* * Update channel masks at 1Hz allowing for actions such as dshot commands to be sent @@ -644,6 +644,7 @@ private: void bdshot_ic_dma_allocate(Shared_DMA *ctx); void bdshot_ic_dma_deallocate(Shared_DMA *ctx); static uint32_t bdshot_decode_telemetry_packet(uint32_t* buffer, uint32_t count); + bool bdshot_decode_telemetry_from_erpm(uint16_t erpm, uint8_t chan); bool bdshot_decode_dshot_telemetry(pwm_group& group, uint8_t chan); static uint8_t bdshot_find_next_ic_channel(const pwm_group& group); static void bdshot_dma_ic_irq_callback(void *p, uint32_t flags); diff --git a/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp b/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp index 33d6f2808c..91eb91ae39 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp @@ -595,17 +595,84 @@ uint32_t RCOutput::bdshot_decode_telemetry_packet(uint32_t* buffer, uint32_t cou } decodedValue >>= 4; - if (decodedValue == 0x0fff) { - return 0; - } - decodedValue = (decodedValue & 0x000001ff) << ((decodedValue & 0xfffffe00) >> 9); - if (!decodedValue) { - return 0xffff; - } - uint32_t ret = (1000000 * 60 / 100 + decodedValue / 2) / decodedValue; - return ret; + return decodedValue; } #pragma GCC pop_options +// update ESC telemetry information. Returns true if valid eRPM data was decoded. +bool RCOutput::bdshot_decode_telemetry_from_erpm(uint16_t encodederpm, uint8_t chan) +{ + if (encodederpm == 0xFFFF) { + return false; + } + + // eRPM = m << e (see https://github.com/bird-sanctuary/extended-dshot-telemetry) + uint8_t expo = ((encodederpm & 0xfffffe00) >> 9) & 0xFF; + uint16_t value = (encodederpm & 0x000001ff); + + if (!(value & 0x100) && (_dshot_esc_type == DSHOT_ESC_BLHELI_EDT || _dshot_esc_type == DSHOT_ESC_BLHELI_EDT_S)) { + switch (expo) { + case 0b001: { // Temperature C + #if HAL_WITH_ESC_TELEM + TelemetryData t { + .temperature_cdeg = int16_t(value * 100) + }; + update_telem_data(chan, t, AP_ESC_Telem_Backend::TelemetryType::TEMPERATURE); + #endif + return false; + } + break; + case 0b010: { // Voltage 0.25v + #if HAL_WITH_ESC_TELEM + TelemetryData t { + .voltage = 0.25f * value + }; + update_telem_data(chan, t, AP_ESC_Telem_Backend::TelemetryType::VOLTAGE); + #endif + return false; + } + break; + case 0b011: { // Current A + #if HAL_WITH_ESC_TELEM + TelemetryData t { + .current = float(value) + }; + update_telem_data(chan, t, AP_ESC_Telem_Backend::TelemetryType::CURRENT); + #endif + return false; + } + break; + case 0b100: // Debug 1 + case 0b101: // Debug 2 + case 0b110: // Stress level + case 0b111: // Status + return false; + break; + default: // eRPM + break; + } + } + + uint16_t erpm = value << expo; + + if (!erpm) { // decoded as 0 is an error + return false; + } + + erpm = (1000000 * 60 / 100 + erpm / 2) / erpm; + + if (encodederpm == 0x0fff) { // the special 0 encoding + erpm = 0; + } + + // update the ESC telemetry data + if (erpm < 0xFFFF) { + _bdshot.erpm[chan] = erpm; +#if HAL_WITH_ESC_TELEM + update_rpm(chan, erpm * 200 / _bdshot.motor_poles, get_erpm_error_rate(chan)); +#endif + } + return erpm < 0xFFFF; +} #endif // HAL_WITH_BIDIR_DSHOT diff --git a/libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp b/libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp index 67a162fb87..733a7e44b5 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput_serial.cpp @@ -147,6 +147,8 @@ void RCOutput::update_channel_masks() { switch (_dshot_esc_type) { case DSHOT_ESC_BLHELI: case DSHOT_ESC_BLHELI_S: + case DSHOT_ESC_BLHELI_EDT: + case DSHOT_ESC_BLHELI_EDT_S: if (_reversible_mask & (1U<