From 0e30739762f3086f493749c033a0a7f55cb245bf Mon Sep 17 00:00:00 2001 From: HalfManBear <89969229+halfmanbear@users.noreply.github.com> Date: Tue, 26 May 2026 23:05:54 +0100 Subject: [PATCH] Add bidirectional DShot telemetry and RPM gyro filtering Ports bidirectional DShot (GCR) telemetry from Betaflight, enabling RPM-based gyro filtering without a dedicated ESC telemetry UART. After each DShot output frame the motor pin is switched to timer input capture mode via per-channel DMA to receive the ESC's GCR-encoded eRPM response. The decoded eRPM feeds the existing RPM filter infrastructure. New settings: - dshot_bidir_enabled: enable bidirectional DShot telemetry - dshot_edt_enabled: enable Extended DShot Telemetry (temperature, voltage, current in addition to eRPM) Changes: - drivers/dshot.c/.h: GCR decode, EDT frame parsing, eRPM-to-RPM conversion, motor frequency LPF (ported from Betaflight, GPLv3) - drivers/pwm_output.c: per-channel DMA direction switching between DShot output and GCR input capture on STM32 (StdPeriph/HAL) and AT32 - sensors/esc_sensor.c/.h: DShot bidir data path into ESC sensor framework (escSensorSetDshotData, escSensorIsActive) - flight/rpm_filter.c: DShot telemetry as frequency source alongside serial ESC sensor path - target/common_post.h: USE_RPM_FILTER enabled for any USE_DSHOT target - fc/fc_init.c: call initDshotTelemetry() at startup; fix RPM filter init condition to accept DShot bidir as a valid source - blackbox/blackbox.c: fix NULL dereference when escSensorGetData() returns NULL; fix uninitialized blackboxSlowState_t comparison - telemetry/srxl.c: replace Betaflight-specific USE_DSHOT_TELEMETRY guards with INAV's USE_DSHOT/USE_ESC_SENSOR; use escSensorGetData() Tested on SPEEDYBEEF405V4 (STM32F405, StdPeriph) with Bluejay ESC firmware. eRPM telemetry confirmed in blackbox; RPM gyro filter updating notch frequencies with motor speed. Co-Authored-By: Claude Sonnet 4.6 --- src/main/CMakeLists.txt | 2 + src/main/blackbox/blackbox.c | 8 +- src/main/drivers/dshot.c | 237 ++++++++++++++++++++ src/main/drivers/dshot.h | 87 ++++++++ src/main/drivers/pwm_output.c | 393 ++++++++++++++++++++++++++++++++-- src/main/fc/fc_init.c | 4 +- src/main/fc/settings.yaml | 10 + src/main/flight/mixer.c | 6 +- src/main/flight/mixer.h | 2 + src/main/flight/rpm_filter.c | 23 +- src/main/sensors/esc_sensor.c | 59 ++++- src/main/sensors/esc_sensor.h | 3 + src/main/target/common_post.h | 2 +- src/main/telemetry/srxl.c | 22 +- 14 files changed, 804 insertions(+), 54 deletions(-) create mode 100644 src/main/drivers/dshot.c create mode 100644 src/main/drivers/dshot.h diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index c243d9215f3..dc38fb5c6be 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -233,6 +233,8 @@ main_sources(COMMON_SRC drivers/pwm_esc_detect.h drivers/pwm_mapping.c drivers/pwm_mapping.h + drivers/dshot.c + drivers/dshot.h drivers/pwm_output.c drivers/pwm_output.h drivers/pinio.c diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index f5225ca3e1e..42da98188a8 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1429,8 +1429,10 @@ static void loadSlowState(blackboxSlowState_t *slow) #ifdef USE_ESC_SENSOR escSensorData_t * escSensor = escSensorGetData(); - slow->escRPM = escSensor->rpm; - slow->escTemperature = escSensor->temperature; + if (escSensor) { + slow->escRPM = escSensor->rpm; + slow->escTemperature = escSensor->temperature; + } #endif } @@ -1448,7 +1450,7 @@ static bool writeSlowFrameIfNeeded(bool allowPeriodicWrite) if (shouldWrite) { loadSlowState(&slowHistory); } else { - blackboxSlowState_t newSlowState; + blackboxSlowState_t newSlowState = {0}; loadSlowState(&newSlowState); diff --git a/src/main/drivers/dshot.c b/src/main/drivers/dshot.c new file mode 100644 index 00000000000..c23b560c8a6 --- /dev/null +++ b/src/main/drivers/dshot.c @@ -0,0 +1,237 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include +#include + +#include "platform.h" +#include "common/utils.h" +#include "drivers/dshot.h" + +#ifdef USE_DSHOT + +#include "common/filter.h" +#include "common/maths.h" + +#include "flight/mixer.h" +#include "drivers/pwm_mapping.h" + +#define DSHOT_RPM_LPF_HZ 150 + +bool useDshotTelemetry = false; +dshotTelemetryState_t dshotTelemetryState; + +static pt1Filter_t motorFreqLpf[MAX_SUPPORTED_MOTORS]; +static float motorFrequencyHz[MAX_SUPPORTED_MOTORS]; +static float dshotRpm[MAX_SUPPORTED_MOTORS]; +static float dshotRpmAverage; +static float erpmToHz; +static bool edtAlwaysDecode; + +static const dshotTelemetryType_e extendedTelemetryLookup[8] = { + DSHOT_TELEMETRY_TYPE_ERPM, + DSHOT_TELEMETRY_TYPE_TEMPERATURE, + DSHOT_TELEMETRY_TYPE_VOLTAGE, + DSHOT_TELEMETRY_TYPE_CURRENT, + DSHOT_TELEMETRY_TYPE_DEBUG1, + DSHOT_TELEMETRY_TYPE_DEBUG2, + DSHOT_TELEMETRY_TYPE_DEBUG3, + DSHOT_TELEMETRY_TYPE_STATE_EVENTS, +}; + +static float erpmToRpm(uint32_t erpm) +{ + return erpm * erpmToHz * 60.0f; +} + +static uint32_t dshotDecodeErpmTelemetryValue(uint16_t value) +{ + if (value == 0x0fff) { + return 0; + } + + value = (value & 0x01ff) << ((value & 0xfe00) >> 9); + if (!value) { + return DSHOT_TELEMETRY_INVALID; + } + + return (1000000 * 60 / 100 + value / 2) / value; +} + +static void dshotDecodeTelemetryValue(uint8_t motorIndex, uint32_t *decoded, dshotTelemetryType_e *type) +{ + const uint16_t value = dshotTelemetryState.motorState[motorIndex].rawValue; + const bool edtEnabled = edtAlwaysDecode || (dshotTelemetryState.motorState[motorIndex].telemetryTypes & DSHOT_EXTENDED_TELEMETRY_MASK) != 0; + const unsigned telemetryType = (value & 0x0f00) >> 8; + const bool isErpm = !edtEnabled || (telemetryType & 0x01) || (telemetryType == 0); + + if (isErpm) { + *decoded = dshotDecodeErpmTelemetryValue(value); + *type = DSHOT_TELEMETRY_TYPE_ERPM; + } else { + const unsigned typeIndex = telemetryType >> 1; + *type = typeIndex < ARRAYLEN(extendedTelemetryLookup) ? extendedTelemetryLookup[typeIndex] : DSHOT_TELEMETRY_TYPE_STATE_EVENTS; + *decoded = value & 0x00ff; + } +} + +void dshotResetTelemetry(void) +{ + memset(&dshotTelemetryState, 0, sizeof(dshotTelemetryState)); + memset(dshotRpm, 0, sizeof(dshotRpm)); + memset(motorFrequencyHz, 0, sizeof(motorFrequencyHz)); + dshotRpmAverage = 0.0f; +} + +bool isDshotTelemetryConfigured(void) +{ + return useDshotTelemetry; +} + +bool isDshotTelemetryActive(void) +{ + return useDshotTelemetry; +} + +void initDshotTelemetry(timeUs_t looptimeUs) +{ + useDshotTelemetry = motorConfig()->useDshotTelemetry && (motorConfig()->motorPwmProtocol >= PWM_TYPE_DSHOT150); + edtAlwaysDecode = motorConfig()->useDshotEdt != 0; + + dshotResetTelemetry(); + + if (!useDshotTelemetry) { + return; + } + + erpmToHz = ERPM_PER_LSB / 60.0f / (motorConfig()->motorPoleCount / 2.0f); + + for (unsigned i = 0; i < MAX_SUPPORTED_MOTORS; i++) { + pt1FilterInit(&motorFreqLpf[i], DSHOT_RPM_LPF_HZ, looptimeUs * 1e-6f); + } +} + +uint16_t dshotProcessPacket(uint16_t rawValue, uint8_t motorIndex) +{ + if (!useDshotTelemetry || motorIndex >= MAX_SUPPORTED_MOTORS) { + return rawValue; + } + + if (rawValue == DSHOT_TELEMETRY_INVALID || rawValue == DSHOT_TELEMETRY_NOEDGE) { + if (rawValue == DSHOT_TELEMETRY_INVALID) { + dshotTelemetryState.invalidPacketCount++; + } + return rawValue; + } + + dshotTelemetryState.readCount++; + dshotTelemetryState.motorState[motorIndex].rawValue = rawValue; + + dshotTelemetryType_e type; + uint32_t decoded; + dshotDecodeTelemetryValue(motorIndex, &decoded, &type); + if (decoded == DSHOT_TELEMETRY_INVALID) { + dshotTelemetryState.invalidPacketCount++; + return DSHOT_TELEMETRY_INVALID; + } + + dshotTelemetryState.motorState[motorIndex].telemetryData[type] = decoded; + dshotTelemetryState.motorState[motorIndex].telemetryTypes |= (1 << type); + + if (type == DSHOT_TELEMETRY_TYPE_TEMPERATURE && decoded > dshotTelemetryState.motorState[motorIndex].maxTemp) { + dshotTelemetryState.motorState[motorIndex].maxTemp = decoded; + } + + if (type == DSHOT_TELEMETRY_TYPE_ERPM) { + dshotRpm[motorIndex] = erpmToRpm(decoded); + motorFrequencyHz[motorIndex] = pt1FilterApply(&motorFreqLpf[motorIndex], erpmToHz * decoded); + } + + float rpmTotal = 0.0f; + int rpmCount = 0; + for (unsigned i = 0; i < getMotorCount(); i++) { + if (dshotTelemetryState.motorState[i].telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_ERPM)) { + rpmTotal += dshotRpm[i]; + rpmCount++; + } + } + dshotRpmAverage = rpmCount ? rpmTotal / rpmCount : 0.0f; + dshotTelemetryState.rawValueState = DSHOT_RAW_VALUE_STATE_PROCESSED; + + return rawValue; +} + +uint16_t getDshotErpm(uint8_t motorIndex) +{ + return dshotTelemetryState.motorState[motorIndex].telemetryData[DSHOT_TELEMETRY_TYPE_ERPM]; +} + +float getDshotRpm(uint8_t motorIndex) +{ + return dshotRpm[motorIndex]; +} + +float getDshotRpmAverage(void) +{ + return dshotRpmAverage; +} + +float getMotorFrequencyHz(uint8_t motorIndex) +{ + return motorFrequencyHz[motorIndex]; +} + +bool getDshotEscSensorData(escSensorData_t *data, uint8_t motorIndex) +{ + if (!useDshotTelemetry || motorIndex >= MAX_SUPPORTED_MOTORS) { + return false; + } + + const dshotTelemetryMotorState_t *state = &dshotTelemetryState.motorState[motorIndex]; + if ((state->telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_ERPM)) == 0) { + return false; + } + + data->rpm = state->telemetryData[DSHOT_TELEMETRY_TYPE_ERPM]; + data->temperature = (state->telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_TEMPERATURE)) ? state->telemetryData[DSHOT_TELEMETRY_TYPE_TEMPERATURE] : 0; + data->voltage = (state->telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_VOLTAGE)) ? state->telemetryData[DSHOT_TELEMETRY_TYPE_VOLTAGE] * 250 : 0; + data->current = (state->telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_CURRENT)) ? state->telemetryData[DSHOT_TELEMETRY_TYPE_CURRENT] * 1000 : 0; + + return true; +} + +#endif + +#ifndef USE_DSHOT +bool useDshotTelemetry = false; +dshotTelemetryState_t dshotTelemetryState; + +void initDshotTelemetry(timeUs_t looptimeUs) { UNUSED(looptimeUs); } +void dshotResetTelemetry(void) {} +bool isDshotTelemetryConfigured(void) { return false; } +bool isDshotTelemetryActive(void) { return false; } +uint16_t dshotProcessPacket(uint16_t rawValue, uint8_t motorIndex) { UNUSED(motorIndex); return rawValue; } +float getDshotRpm(uint8_t motorIndex) { UNUSED(motorIndex); return 0.0f; } +uint16_t getDshotErpm(uint8_t motorIndex) { UNUSED(motorIndex); return 0; } +float getDshotRpmAverage(void) { return 0.0f; } +float getMotorFrequencyHz(uint8_t motorIndex) { UNUSED(motorIndex); return 0.0f; } +bool getDshotEscSensorData(escSensorData_t *data, uint8_t motorIndex) { UNUSED(data); UNUSED(motorIndex); return false; } +#endif diff --git a/src/main/drivers/dshot.h b/src/main/drivers/dshot.h new file mode 100644 index 00000000000..31061c76300 --- /dev/null +++ b/src/main/drivers/dshot.h @@ -0,0 +1,87 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#pragma once + +#include +#include + +#include "platform.h" + +#include "drivers/time.h" +#include "flight/mixer.h" + +#include "sensors/esc_sensor.h" + +#define DSHOT_TELEMETRY_NOEDGE (0xfffe) +#define DSHOT_TELEMETRY_INVALID (0xffff) + +#define MIN_GCR_EDGES (7) +#define MAX_GCR_EDGES (22) + +typedef enum { + DSHOT_TELEMETRY_TYPE_ERPM = 0, + DSHOT_TELEMETRY_TYPE_TEMPERATURE, + DSHOT_TELEMETRY_TYPE_VOLTAGE, + DSHOT_TELEMETRY_TYPE_CURRENT, + DSHOT_TELEMETRY_TYPE_DEBUG1, + DSHOT_TELEMETRY_TYPE_DEBUG2, + DSHOT_TELEMETRY_TYPE_DEBUG3, + DSHOT_TELEMETRY_TYPE_STATE_EVENTS, + DSHOT_TELEMETRY_TYPE_COUNT +} dshotTelemetryType_e; + +#define DSHOT_NORMAL_TELEMETRY_MASK (1 << DSHOT_TELEMETRY_TYPE_ERPM) +#define DSHOT_EXTENDED_TELEMETRY_MASK (~DSHOT_NORMAL_TELEMETRY_MASK) + +typedef enum { + DSHOT_RAW_VALUE_STATE_INVALID = 0, + DSHOT_RAW_VALUE_STATE_NOT_PROCESSED, + DSHOT_RAW_VALUE_STATE_PROCESSED, +} dshotRawValueState_e; + +typedef struct { + uint16_t rawValue; + uint16_t telemetryData[DSHOT_TELEMETRY_TYPE_COUNT]; + uint8_t telemetryTypes; + uint8_t maxTemp; +} dshotTelemetryMotorState_t; + +typedef struct { + uint32_t invalidPacketCount; + uint32_t readCount; + dshotTelemetryMotorState_t motorState[MAX_SUPPORTED_MOTORS]; + dshotRawValueState_e rawValueState; +} dshotTelemetryState_t; + +extern bool useDshotTelemetry; +extern dshotTelemetryState_t dshotTelemetryState; + +void initDshotTelemetry(timeUs_t looptimeUs); +void dshotResetTelemetry(void); +bool isDshotTelemetryConfigured(void); +bool isDshotTelemetryActive(void); +uint16_t dshotProcessPacket(uint16_t rawValue, uint8_t motorIndex); +float getDshotRpm(uint8_t motorIndex); +uint16_t getDshotErpm(uint8_t motorIndex); +float getDshotRpmAverage(void); +float getMotorFrequencyHz(uint8_t motorIndex); +bool getDshotEscSensorData(escSensorData_t *data, uint8_t motorIndex); diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index a4efb9e3008..8d0b9441064 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -25,12 +25,15 @@ #if !defined(SITL_BUILD) #include "build/debug.h" +#include "build/build_config.h" #include "common/log.h" #include "common/maths.h" #include "common/circular_queue.h" #include "drivers/io.h" +#include "drivers/dshot.h" +#include "drivers/nvic.h" #include "drivers/timer.h" #include "drivers/pwm_mapping.h" #include "drivers/pwm_output.h" @@ -59,6 +62,8 @@ #define DSHOT_MOTOR_BITLENGTH 20 #define DSHOT_DMA_BUFFER_SIZE 18 /* resolution + frame reset (2us) */ +#define DSHOT_TELEMETRY_DEADTIME_US 35 +#define GCR_TELEMETRY_INPUT_LEN MAX_GCR_EDGES #define MAX_DMA_TIMERS 8 #define DSHOT_COMMAND_DELAY_US 1000 @@ -85,10 +90,13 @@ typedef struct { #ifdef USE_DSHOT // DSHOT parameters - timerDMASafeType_t dmaBuffer[DSHOT_DMA_BUFFER_SIZE]; + timerDMASafeType_t dmaBuffer[GCR_TELEMETRY_INPUT_LEN]; #ifdef USE_DSHOT_DMAR timerDMASafeType_t *dmaBurstBuffer; #endif + bool telemetryInputActive; + timeUs_t telemetryInputStampUs; + timeUs_t dshotTelemetryDeadtimeUs; #endif } pwmOutputPort_t; @@ -120,6 +128,7 @@ static timeUs_t digitalMotorUpdateIntervalUs = 0; static timeUs_t digitalMotorLastUpdateUs; static timeUs_t lastCommandSent = 0; static timeUs_t commandPostDelay = 0; +static bool dshotTelemetryPending = false; static circularBuffer_t commandsCircularBuffer; static uint8_t commandsBuff[DHSOT_COMMAND_QUEUE_SIZE]; @@ -136,6 +145,13 @@ static void loadDmaBufferDshotStride(timerDMASafeType_t *dmaBuffer, int stride, burstDmaTimer_t burstDmaTimers[MAX_DMA_TIMERS]; uint8_t burstDmaTimersCount = 0; #endif + +// GCR decode algorithm ported from Betaflight (GPLv3) +static uint16_t dshotDecodeTelemetryPacket(const uint32_t buffer[], uint32_t count); +static bool pwmDshotDecodeTelemetry(void); +static void pwmDshotSetDirectionOutput(pwmOutputPort_t *port); +static void pwmDshotSetDirectionInput(pwmOutputPort_t *port); +static void pwmDshotDmaIrqHandler(DMA_t descriptor); #endif static void pwmOutConfigTimer(pwmOutputPort_t * p, TCH_t * tch, uint32_t hz, uint16_t period, uint16_t value) @@ -338,6 +354,319 @@ static uint8_t getBurstDmaTimerIndex(TIM_TypeDef *timer) } #endif +static uint32_t dshotDmaSource(const pwmOutputPort_t *port) +{ +#if defined(USE_HAL_DRIVER) || !defined(AT32F43x) + static const uint32_t sources[] = { TIM_DMA_CC1, TIM_DMA_CC2, TIM_DMA_CC3, TIM_DMA_CC4 }; +#else + static const uint32_t sources[] = { TMR_C1_DMA_REQUEST, TMR_C2_DMA_REQUEST, TMR_C3_DMA_REQUEST, TMR_C4_DMA_REQUEST }; +#endif + return sources[port->tch->timHw->channelIndex]; +} + +static uint32_t dshotDmaStream(const pwmOutputPort_t *port) +{ +#if defined(USE_HAL_DRIVER) + static const uint32_t streams[] = { + LL_DMA_STREAM_0, LL_DMA_STREAM_1, LL_DMA_STREAM_2, LL_DMA_STREAM_3, + LL_DMA_STREAM_4, LL_DMA_STREAM_5, LL_DMA_STREAM_6, LL_DMA_STREAM_7 + }; + return streams[DMATAG_GET_STREAM(port->tch->timHw->dmaTag)]; +#else + UNUSED(port); + return 0; +#endif +} + +static uint32_t dshotTimChannel(const pwmOutputPort_t *port) +{ +#if defined(USE_HAL_DRIVER) + static const uint32_t channels[] = { TIM_CHANNEL_1, TIM_CHANNEL_2, TIM_CHANNEL_3, TIM_CHANNEL_4 }; +#elif defined(AT32F43x) + static const uint32_t channels[] = { TMR_SELECT_CHANNEL_1, TMR_SELECT_CHANNEL_2, TMR_SELECT_CHANNEL_3, TMR_SELECT_CHANNEL_4 }; +#else + static const uint32_t channels[] = { TIM_Channel_1, TIM_Channel_2, TIM_Channel_3, TIM_Channel_4 }; +#endif + return channels[port->tch->timHw->channelIndex]; +} + +static uint16_t dshotDecodeTelemetryPacket(const uint32_t buffer[], uint32_t count) +{ + uint32_t value = 0; + uint32_t oldValue = buffer[0]; + int bits = 0; + + for (uint32_t i = 1; i <= count; i++) { + int len; + if (i < count) { + const int diff = buffer[i] - oldValue; + if (bits >= 21) { + break; + } + len = (diff + 8) / 16; + } else { + len = 21 - bits; + } + + value <<= len; + value |= 1 << (len - 1); + if (i < count) { + oldValue = buffer[i]; + } + bits += len; + } + + if (bits != 21) { + return DSHOT_TELEMETRY_INVALID; + } + + static const uint32_t decode[32] = { + 0, 0, 0, 0, 0, 0, 0, 0, 0, 9, 10, 11, 0, 13, 14, 15, + 0, 0, 2, 3, 0, 5, 6, 7, 0, 0, 8, 1, 0, 4, 12, 0 + }; + + uint32_t decodedValue = decode[value & 0x1f]; + decodedValue |= decode[(value >> 5) & 0x1f] << 4; + decodedValue |= decode[(value >> 10) & 0x1f] << 8; + decodedValue |= decode[(value >> 15) & 0x1f] << 12; + + uint32_t csum = decodedValue; + csum ^= csum >> 8; + csum ^= csum >> 4; + + if ((csum & 0xf) != 0xf) { + return DSHOT_TELEMETRY_INVALID; + } + + return decodedValue >> 4; +} + +static void pwmDshotSetDirectionOutput(pwmOutputPort_t *port) +{ +#if defined(USE_HAL_DRIVER) + TIM_OC_InitTypeDef init = {0}; + init.OCMode = TIM_OCMODE_PWM1; + init.OCIdleState = TIM_OCIDLESTATE_SET; + init.OCPolarity = TIM_OCPOLARITY_LOW; + init.OCNIdleState = TIM_OCNIDLESTATE_SET; + init.OCNPolarity = TIM_OCNPOLARITY_LOW; + init.Pulse = 0; + init.OCFastMode = TIM_OCFAST_DISABLE; + HAL_TIM_PWM_ConfigChannel(port->tch->timCtx->timHandle, &init, dshotTimChannel(port)); + if (port->tch->timHw->output & TIMER_OUTPUT_N_CHANNEL) { + HAL_TIMEx_PWMN_Start(port->tch->timCtx->timHandle, dshotTimChannel(port)); + } else { + HAL_TIM_PWM_Start(port->tch->timCtx->timHandle, dshotTimChannel(port)); + } + + const uint32_t streamLL = dshotDmaStream(port); + DMA_TypeDef *dmaBase = port->tch->dma->dma; + LL_DMA_DisableStream(dmaBase, streamLL); + while (LL_DMA_IsEnabledStream(dmaBase, streamLL)) { } + LL_DMA_ConfigAddresses(dmaBase, streamLL, (uint32_t)port->dmaBuffer, (uint32_t)port->ccr, LL_DMA_DIRECTION_MEMORY_TO_PERIPH); + LL_DMA_SetDataLength(dmaBase, streamLL, DSHOT_DMA_BUFFER_SIZE); + LL_DMA_EnableIT_TC(dmaBase, streamLL); + port->telemetryInputActive = false; +#elif defined(AT32F43x) + tmr_output_config_type output = {0}; + tmr_output_default_para_init(&output); + output.oc_mode = TMR_OUTPUT_CONTROL_PWM_MODE_A; + if (port->tch->timHw->output & TIMER_OUTPUT_N_CHANNEL) { + output.oc_output_state = FALSE; + output.occ_output_state = TRUE; + output.occ_polarity = TMR_OUTPUT_ACTIVE_LOW; + output.occ_idle_state = FALSE; + } else { + output.oc_output_state = TRUE; + output.occ_output_state = FALSE; + output.oc_polarity = TMR_OUTPUT_ACTIVE_LOW; + output.oc_idle_state = TRUE; + } + tmr_output_channel_config(port->tch->timHw->tim, dshotTimChannel(port), &output); + tmr_channel_value_set(port->tch->timHw->tim, dshotTimChannel(port), 0); + tmr_output_channel_buffer_enable(port->tch->timHw->tim, dshotTimChannel(port), TRUE); + dma_channel_enable(port->tch->dma->ref, FALSE); + port->tch->dma->ref->maddr = (uint32_t)port->dmaBuffer; + port->tch->dma->ref->dtcnt = DSHOT_DMA_BUFFER_SIZE; + port->telemetryInputActive = false; +#else + TIM_ARRPreloadConfig(port->tch->timHw->tim, DISABLE); + TIM_SetAutoreload(port->tch->timHw->tim, DSHOT_MOTOR_BITLENGTH - 1); + TIM_ARRPreloadConfig(port->tch->timHw->tim, ENABLE); + TIM_SetCounter(port->tch->timHw->tim, 0); + TIM_OCInitTypeDef init; + TIM_OCStructInit(&init); + init.TIM_OCMode = TIM_OCMode_PWM1; + init.TIM_Pulse = 0; + if (port->tch->timHw->output & TIMER_OUTPUT_N_CHANNEL) { + init.TIM_OutputState = TIM_OutputState_Disable; + init.TIM_OutputNState = TIM_OutputNState_Enable; + init.TIM_OCNPolarity = TIM_OCPolarity_Low; + init.TIM_OCNIdleState = TIM_OCIdleState_Reset; + } else { + init.TIM_OutputState = TIM_OutputState_Enable; + init.TIM_OutputNState = TIM_OutputNState_Disable; + init.TIM_OCPolarity = TIM_OCPolarity_Low; + init.TIM_OCIdleState = TIM_OCIdleState_Set; + } + switch (port->tch->timHw->channelIndex) { + case 0: TIM_OC1Init(port->tch->timHw->tim, &init); TIM_OC1PreloadConfig(port->tch->timHw->tim, TIM_OCPreload_Enable); break; + case 1: TIM_OC2Init(port->tch->timHw->tim, &init); TIM_OC2PreloadConfig(port->tch->timHw->tim, TIM_OCPreload_Enable); break; + case 2: TIM_OC3Init(port->tch->timHw->tim, &init); TIM_OC3PreloadConfig(port->tch->timHw->tim, TIM_OCPreload_Enable); break; + default: TIM_OC4Init(port->tch->timHw->tim, &init); TIM_OC4PreloadConfig(port->tch->timHw->tim, TIM_OCPreload_Enable); break; + } + DMA_Cmd(port->tch->dma->ref, DISABLE); + port->tch->dma->ref->CR = (port->tch->dma->ref->CR & ~(DMA_DIR_MemoryToPeripheral | DMA_DIR_MemoryToMemory)) | DMA_DIR_MemoryToPeripheral; + port->tch->dma->ref->M0AR = (uint32_t)port->dmaBuffer; + DMA_SetCurrDataCounter(port->tch->dma->ref, DSHOT_DMA_BUFFER_SIZE); + port->telemetryInputActive = false; +#endif +} + +static void pwmDshotSetDirectionInput(pwmOutputPort_t *port) +{ +#if defined(USE_HAL_DRIVER) + TIM_IC_InitTypeDef init = {0}; + init.ICPolarity = TIM_INPUTCHANNELPOLARITY_BOTHEDGE; + init.ICSelection = TIM_ICSELECTION_DIRECTTI; + init.ICPrescaler = TIM_ICPSC_DIV1; + init.ICFilter = 2; + HAL_TIM_IC_ConfigChannel(port->tch->timCtx->timHandle, &init, dshotTimChannel(port)); + HAL_TIM_IC_Start(port->tch->timCtx->timHandle, dshotTimChannel(port)); + + const uint32_t streamLL = dshotDmaStream(port); + DMA_TypeDef *dmaBase = port->tch->dma->dma; + LL_DMA_DisableStream(dmaBase, streamLL); + while (LL_DMA_IsEnabledStream(dmaBase, streamLL)) { } + LL_DMA_ConfigAddresses(dmaBase, streamLL, (uint32_t)port->ccr, (uint32_t)port->dmaBuffer, LL_DMA_DIRECTION_PERIPH_TO_MEMORY); + LL_DMA_SetDataLength(dmaBase, streamLL, GCR_TELEMETRY_INPUT_LEN); + LL_DMA_EnableIT_TC(dmaBase, streamLL); + LL_DMA_EnableStream(dmaBase, streamLL); + LL_TIM_EnableDMAReq_CCx(port->tch->timHw->tim, dshotDmaSource(port)); +#elif defined(AT32F43x) + tmr_input_config_type input; + tmr_input_default_para_init(&input); + input.input_channel_select = dshotTimChannel(port); + input.input_mapped_select = TMR_CC_CHANNEL_MAPPED_DIRECT; + input.input_filter_value = 2; + input.input_polarity_select = TMR_INPUT_BOTH_EDGE; + tmr_input_channel_init(port->tch->timHw->tim, &input, TMR_CHANNEL_INPUT_DIV_1); + port->tch->dma->ref->paddr = (uint32_t)port->ccr; + port->tch->dma->ref->maddr = (uint32_t)port->dmaBuffer; + port->tch->dma->ref->dtcnt = GCR_TELEMETRY_INPUT_LEN; + dma_channel_enable(port->tch->dma->ref, TRUE); + tmr_dma_request_enable(port->tch->timHw->tim, dshotDmaSource(port), TRUE); +#else + TIM_ARRPreloadConfig(port->tch->timHw->tim, ENABLE); + TIM_SetAutoreload(port->tch->timHw->tim, 0xffff); + TIM_ICInitTypeDef init; + TIM_ICStructInit(&init); + init.TIM_Channel = dshotTimChannel(port); + init.TIM_ICSelection = TIM_ICSelection_DirectTI; + init.TIM_ICPrescaler = TIM_ICPSC_DIV1; + init.TIM_ICFilter = 2; + init.TIM_ICPolarity = TIM_ICPolarity_BothEdge; + TIM_ICInit(port->tch->timHw->tim, &init); + DMA_Cmd(port->tch->dma->ref, DISABLE); + port->tch->dma->ref->CR &= ~(DMA_DIR_MemoryToPeripheral | DMA_DIR_MemoryToMemory); // P→M + port->tch->dma->ref->PAR = (uint32_t)port->ccr; + port->tch->dma->ref->M0AR = (uint32_t)port->dmaBuffer; + DMA_SetCurrDataCounter(port->tch->dma->ref, GCR_TELEMETRY_INPUT_LEN); + // Clear stale DMA flags (TCIF/HTIF from completed output DMA) before + // enabling input capture, otherwise a spurious TC fires immediately and + // kills the capture DMA before any edges can be recorded. + DMA_CLEAR_FLAG(port->tch->dma, DMA_IT_TCIF | DMA_IT_HTIF | DMA_IT_TEIF); + DMA_Cmd(port->tch->dma->ref, ENABLE); + TIM_DMACmd(port->tch->timHw->tim, dshotDmaSource(port), ENABLE); +#endif + port->telemetryInputStampUs = micros(); + port->telemetryInputActive = true; +} + +static void pwmDshotDmaIrqHandler(DMA_t descriptor) +{ + if (!DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) { + return; + } + + pwmOutputPort_t *port = &pwmOutputPorts[descriptor->userParam]; + +#if defined(USE_HAL_DRIVER) + const uint32_t streamLL = dshotDmaStream(port); + LL_DMA_DisableStream(port->tch->dma->dma, streamLL); + LL_TIM_DisableDMAReq_CCx(port->tch->timHw->tim, dshotDmaSource(port)); +#elif defined(AT32F43x) + dma_channel_enable(port->tch->dma->ref, FALSE); + tmr_dma_request_enable(port->tch->timHw->tim, dshotDmaSource(port), FALSE); +#else + DMA_Cmd(port->tch->dma->ref, DISABLE); + TIM_DMACmd(port->tch->timHw->tim, dshotDmaSource(port), DISABLE); +#endif + + if (useDshotTelemetry && !port->telemetryInputActive) { + pwmDshotSetDirectionInput(port); + dshotTelemetryPending = true; + } + + DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF); +} + +static bool pwmDshotDecodeTelemetry(void) +{ + if (!useDshotTelemetry || !dshotTelemetryPending) { + return true; + } + + const timeUs_t now = micros(); + for (int motorIndex = 0; motorIndex < getMotorCount(); motorIndex++) { + pwmOutputPort_t *port = motors[motorIndex].pwmPort; + if (!port || !port->configured || !port->telemetryInputActive) { + continue; + } + + if ((now - port->telemetryInputStampUs) < port->dshotTelemetryDeadtimeUs) { + return false; + } + + uint32_t edges = 0; +#if defined(USE_HAL_DRIVER) + const uint32_t streamLL = dshotDmaStream(port); + edges = GCR_TELEMETRY_INPUT_LEN - LL_DMA_GetDataLength(port->tch->dma->dma, streamLL); + LL_TIM_DisableDMAReq_CCx(port->tch->timHw->tim, dshotDmaSource(port)); + LL_DMA_DisableStream(port->tch->dma->dma, streamLL); +#elif defined(AT32F43x) + edges = GCR_TELEMETRY_INPUT_LEN - dma_data_number_get(port->tch->dma->ref); + tmr_dma_request_enable(port->tch->timHw->tim, dshotDmaSource(port), FALSE); + dma_channel_enable(port->tch->dma->ref, FALSE); +#else + edges = GCR_TELEMETRY_INPUT_LEN - DMA_GetCurrDataCounter(port->tch->dma->ref); + TIM_DMACmd(port->tch->timHw->tim, dshotDmaSource(port), DISABLE); + DMA_Cmd(port->tch->dma->ref, DISABLE); +#endif + + if (edges > MIN_GCR_EDGES) { + const uint16_t rawValue = dshotDecodeTelemetryPacket((const uint32_t *)port->dmaBuffer, edges); + const uint16_t processed = dshotProcessPacket(rawValue, motorIndex); + + if (processed != DSHOT_TELEMETRY_INVALID && processed != DSHOT_TELEMETRY_NOEDGE) { +#ifdef USE_ESC_SENSOR + escSensorData_t data; + if (getDshotEscSensorData(&data, motorIndex)) { + escSensorSetDshotData(motorIndex, computeRpm((int16_t)data.rpm), data.temperature, data.voltage, data.current); + } else { + escSensorSetDshotData(motorIndex, (uint32_t)getDshotRpm(motorIndex), 0, 0, 0); + } +#endif + } + } + + pwmDshotSetDirectionOutput(port); + } + + dshotTelemetryPending = false; + return true; +} + static pwmOutputPort_t * motorConfigDshot(const timerHardware_t * timerHardware, uint32_t dshotHz, bool enableOutput) { // Try allocating new port @@ -347,28 +676,37 @@ static pwmOutputPort_t * motorConfigDshot(const timerHardware_t * timerHardware, return NULL; } + if (enableOutput && useDshotTelemetry) { + IOConfigGPIOAF(IOGetByTag(timerHardware->tag), IOCFG_AF_PP_UP, timerHardware->alternateFunction); + } + // Configure timer DMA #ifdef USE_DSHOT_DMAR - //uint8_t timerIndex = lookupTimerIndex(timerHardware->tim); - uint8_t burstDmaTimerIndex = getBurstDmaTimerIndex(timerHardware->tim); - if (burstDmaTimerIndex >= MAX_DMA_TIMERS) { - return NULL; - } + if (!useDshotTelemetry) { + uint8_t burstDmaTimerIndex = getBurstDmaTimerIndex(timerHardware->tim); + if (burstDmaTimerIndex >= MAX_DMA_TIMERS) { + return NULL; + } - port->dmaBurstBuffer = &dmaBurstBuffer[burstDmaTimerIndex][0]; - burstDmaTimer_t *burstDmaTimer = &burstDmaTimers[burstDmaTimerIndex]; - burstDmaTimer->dmaBurstBuffer = port->dmaBurstBuffer; + port->dmaBurstBuffer = &dmaBurstBuffer[burstDmaTimerIndex][0]; + burstDmaTimer_t *burstDmaTimer = &burstDmaTimers[burstDmaTimerIndex]; + burstDmaTimer->dmaBurstBuffer = port->dmaBurstBuffer; - if (timerPWMConfigDMABurst(burstDmaTimer, port->tch, port->dmaBurstBuffer, sizeof(port->dmaBurstBuffer[0]), DSHOT_DMA_BUFFER_SIZE)) { - port->configured = true; - } -#else - if (timerPWMConfigChannelDMA(port->tch, port->dmaBuffer, sizeof(port->dmaBuffer[0]), DSHOT_DMA_BUFFER_SIZE)) { + if (timerPWMConfigDMABurst(burstDmaTimer, port->tch, port->dmaBurstBuffer, sizeof(port->dmaBurstBuffer[0]), DSHOT_DMA_BUFFER_SIZE)) { + port->configured = true; + } + } else +#endif + { + if (timerPWMConfigChannelDMA(port->tch, port->dmaBuffer, sizeof(port->dmaBuffer[0]), GCR_TELEMETRY_INPUT_LEN)) { // Only mark as DSHOT channel if DMA was set successfully ZERO_FARRAY(port->dmaBuffer); port->configured = true; + port->dshotTelemetryDeadtimeUs = DSHOT_TELEMETRY_DEADTIME_US + 1000000 * (16 * DSHOT_MOTOR_BITLENGTH) / dshotHz; + pwmDshotSetDirectionOutput(port); + dmaSetHandler(port->tch->dma, pwmDshotDmaIrqHandler, NVIC_PRIO_TIMER_DMA, allocatedOutputPortCount - 1); + } } -#endif return port; } @@ -391,11 +729,17 @@ static void loadDmaBufferDshot(timerDMASafeType_t *dmaBuffer, uint16_t packet) dmaBuffer[i] = (packet & 0x8000) ? DSHOT_MOTOR_BIT_1 : DSHOT_MOTOR_BIT_0; // MSB first packet <<= 1; } + dmaBuffer[16] = 0; + dmaBuffer[17] = 0; } #endif static uint16_t prepareDshotPacket(const uint16_t value, bool requestTelemetry) { + if (useDshotTelemetry) { + requestTelemetry = true; + } + uint16_t packet = (value << 1) | (requestTelemetry ? 1 : 0); // compute checksum @@ -405,6 +749,11 @@ static uint16_t prepareDshotPacket(const uint16_t value, bool requestTelemetry) csum ^= csum_data; // xor data by nibbles csum_data >>= 4; } +#ifdef USE_DSHOT + if (useDshotTelemetry) { + csum = ~csum; + } +#endif csum &= 0xf; // append checksum @@ -524,6 +873,10 @@ void pwmCompleteMotorUpdate(void) { return; } + if (useDshotTelemetry && !pwmDshotDecodeTelemetry()) { + return; + } + int motorCount = getMotorCount(); timeUs_t currentTimeUs = micros(); @@ -542,6 +895,7 @@ void pwmCompleteMotorUpdate(void) { } #ifdef USE_DSHOT_DMAR + if (!useDshotTelemetry) { for (int index = 0; index < motorCount; index++) { if (motors[index].pwmPort && motors[index].pwmPort->configured) { uint16_t packet = prepareDshotPacket(motors[index].value, motors[index].requestTelemetry); @@ -554,7 +908,9 @@ void pwmCompleteMotorUpdate(void) { burstDmaTimer_t *burstDmaTimer = &burstDmaTimers[burstDmaTimerIndex]; pwmBurstDMAStart(burstDmaTimer, DSHOT_DMA_BUFFER_SIZE * 4); } -#else + } else +#endif + { // Generate DMA buffers for (int index = 0; index < motorCount; index++) { if (motors[index].pwmPort && motors[index].pwmPort->configured) { @@ -571,7 +927,7 @@ void pwmCompleteMotorUpdate(void) { timerPWMStartDMA(motors[index].pwmPort->tch); } } -#endif + } } #endif } @@ -590,6 +946,9 @@ void pwmMotorPreconfigure(void) { // Keep track of initial motor protocol initMotorProtocol = motorConfig()->motorPwmProtocol; +#ifdef USE_DSHOT + useDshotTelemetry = motorConfig()->useDshotTelemetry && getMotorProtocolProperties(initMotorProtocol)->isDSHOT; +#endif #ifdef BRUSHED_MOTORS initMotorProtocol = PWM_TYPE_BRUSHED; // Override proto diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index a8ca6c0c199..223e2546a66 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -50,6 +50,7 @@ #include "drivers/compass/compass.h" #include "drivers/bus.h" #include "drivers/dma.h" +#include "drivers/dshot.h" #include "drivers/exti.h" #include "drivers/io.h" #include "drivers/flash.h" @@ -695,6 +696,7 @@ void init(void) #ifdef USE_DSHOT initDShotCommands(); + initDshotTelemetry(getLooptime()); #endif #ifdef USE_SERIAL_GIMBAL @@ -728,7 +730,7 @@ void init(void) #ifdef USE_RPM_FILTER disableRpmFilters(); - if (STATE(ESC_SENSOR_ENABLED) && (rpmFilterConfig()->gyro_filter_enabled || rpmFilterConfig()->dterm_filter_enabled)) { + if ((escSensorIsActive() || isDshotTelemetryActive()) && (rpmFilterConfig()->gyro_filter_enabled || rpmFilterConfig()->dterm_filter_enabled)) { rpmFiltersInit(); setTaskEnabled(TASK_RPM_FILTER, true); } diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 6ef471e3bb0..debdba477f1 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -851,6 +851,16 @@ groups: default_value: "ONESHOT125" field: motorPwmProtocol table: motor_pwm_protocol + - name: dshot_bidir_enabled + description: "Enable bidirectional DShot telemetry on motor outputs. Required for RPM filtering without a separate ESC telemetry UART" + field: useDshotTelemetry + type: bool + default_value: OFF + - name: dshot_edt_enabled + description: "Enable extended DShot telemetry decoding (temperature, voltage, current in addition to eRPM)" + field: useDshotEdt + type: bool + default_value: OFF - name: motor_poles field: motorPoleCount description: "The number of motor poles. Required to compute motor RPM" diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index a80992b772d..53c8255e8d1 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -86,11 +86,13 @@ PG_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig, .neutral = SETTING_3D_NEUTRAL_DEFAULT ); -PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 11); +PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 12); PG_RESET_TEMPLATE(motorConfig_t, motorConfig, .motorPwmProtocol = SETTING_MOTOR_PWM_PROTOCOL_DEFAULT, .motorPwmRate = SETTING_MOTOR_PWM_RATE_DEFAULT, + .useDshotTelemetry = 0, + .useDshotEdt = 0, .mincommand = SETTING_MIN_COMMAND_DEFAULT, .motorPoleCount = SETTING_MOTOR_POLES_DEFAULT, // Most brushless motors that we use are 14 poles ); @@ -737,4 +739,4 @@ uint16_t getMaxThrottle(void) { } return throttle; -} \ No newline at end of file +} diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 12688bd2c09..7a9e112c9d8 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -84,6 +84,8 @@ typedef struct motorConfig_s { uint16_t mincommand; // This is the value for the ESCs when they are not armed. In some cases, this value must be lowered down to 900 for some specific ESCs uint16_t motorPwmRate; // The update rate of motor outputs (50-498Hz) uint8_t motorPwmProtocol; + uint8_t useDshotTelemetry; + uint8_t useDshotEdt; uint16_t digitalIdleOffsetValue; uint8_t motorPoleCount; // Magnetic poles in the motors for calculating actual RPM from eRPM provided by ESC telemetry } motorConfig_t; diff --git a/src/main/flight/rpm_filter.c b/src/main/flight/rpm_filter.c index ffca6c8ea68..14da11482bb 100644 --- a/src/main/flight/rpm_filter.c +++ b/src/main/flight/rpm_filter.c @@ -36,13 +36,13 @@ #include "common/maths.h" #include "common/filter.h" #include "flight/mixer.h" +#include "drivers/dshot.h" #include "sensors/esc_sensor.h" #include "fc/config.h" #include "fc/settings.h" #ifdef USE_RPM_FILTER -#define HZ_TO_RPM 1/60.0f #define RPM_FILTER_RPM_LPF_HZ 150 #define RPM_FILTER_HARMONICS 3 @@ -66,7 +66,9 @@ typedef struct typedef float (*rpmFilterApplyFnPtr)(rpmFilterBank_t *filter, uint8_t axis, float input); typedef void (*rpmFilterUpdateFnPtr)(rpmFilterBank_t *filterBank, uint8_t motor, float baseFrequency); +#ifdef USE_ESC_SENSOR static EXTENDED_FASTRAM pt1Filter_t motorFrequencyFilter[MAX_SUPPORTED_MOTORS]; +#endif static EXTENDED_FASTRAM rpmFilterBank_t gyroRpmFilters; static EXTENDED_FASTRAM rpmFilterApplyFnPtr rpmGyroApplyFn; static EXTENDED_FASTRAM rpmFilterUpdateFnPtr rpmGyroUpdateFn; @@ -159,10 +161,11 @@ void rpmFilterUpdate(rpmFilterBank_t *filterBank, uint8_t motor, float baseFrequ void rpmFiltersInit(void) { - for (uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) - { +#ifdef USE_ESC_SENSOR + for (uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) { pt1FilterInit(&motorFrequencyFilter[i], RPM_FILTER_RPM_LPF_HZ, US2S(RPM_FILTER_UPDATE_RATE_US)); } +#endif rpmGyroUpdateFn = (rpmFilterUpdateFnPtr)nullRpmFilterUpdate; @@ -188,9 +191,17 @@ void rpmFilterUpdateTask(timeUs_t currentTimeUs) */ for (uint8_t i = 0; i < motorCount; i++) { - const escSensorData_t *escState = getEscTelemetry(i); //Get ESC telemetry - const float baseFrequency = pt1FilterApply(&motorFrequencyFilter[i], escState->rpm * HZ_TO_RPM); //Filter motor frequency - + float baseFrequency; + if (isDshotTelemetryActive()) { + baseFrequency = getMotorFrequencyHz(i); + } else { +#ifdef USE_ESC_SENSOR + const escSensorData_t *escState = getEscTelemetry(i); + baseFrequency = pt1FilterApply(&motorFrequencyFilter[i], (float)escState->rpm / 60.0f); +#else + baseFrequency = 0.0f; +#endif + } rpmGyroUpdateFn(&gyroRpmFilters, i, baseFrequency); } } diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c index bc77c281c79..3556f679938 100644 --- a/src/main/sensors/esc_sensor.c +++ b/src/main/sensors/esc_sensor.c @@ -41,6 +41,8 @@ #include "config/parameter_group_ids.h" #include "flight/mixer.h" +#include "drivers/dshot.h" +#include "drivers/pwm_mapping.h" #include "drivers/pwm_output.h" #include "sensors/esc_sensor.h" #include "io/serial.h" @@ -77,6 +79,7 @@ static int bufferPosition = 0; static escSensorData_t escSensorData[MAX_SUPPORTED_MOTORS]; static escSensorData_t escSensorDataCombined; static bool escSensorDataNeedsUpdate; +static bool escSensorDshotActive; PG_REGISTER_WITH_RESET_TEMPLATE(escSensorConfig_t, escSensorConfig, PG_ESC_SENSOR_CONFIG, 1); PG_RESET_TEMPLATE(escSensorConfig_t, escSensorConfig, @@ -154,9 +157,40 @@ escSensorData_t NOINLINE * getEscTelemetry(uint8_t esc) return &escSensorData[esc]; } +void escSensorInitData(void) +{ + for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) { + escSensorData[i].dataAge = ESC_DATA_INVALID; + escSensorData[i].temperature = 0; + escSensorData[i].voltage = 0; + escSensorData[i].current = 0; + escSensorData[i].rpm = 0; + } + escSensorDataNeedsUpdate = true; +} + +void escSensorSetDshotData(uint8_t esc, uint32_t rpm, int16_t temperature, int16_t voltage, int32_t current) +{ + if (esc >= MAX_SUPPORTED_MOTORS) { + return; + } + + escSensorData[esc].dataAge = 0; + escSensorData[esc].rpm = rpm; + escSensorData[esc].temperature = temperature; + escSensorData[esc].voltage = voltage; + escSensorData[esc].current = current; + escSensorDataNeedsUpdate = true; +} + +bool escSensorIsActive(void) +{ + return escSensorPort || escSensorDshotActive; +} + escSensorData_t * escSensorGetData(void) { - if (!escSensorPort) { + if (!escSensorIsActive()) { return NULL; } @@ -206,12 +240,21 @@ bool escSensorInitialize(void) { escSensorDataNeedsUpdate = true; escSensorPort = NULL; + escSensorDshotActive = false; // Fail immediately if motor output are disabled or motor outputs are not configured if (!feature(FEATURE_PWM_OUTPUT_ENABLE) || getMotorCount() == 0) { return false; } + escSensorInitData(); + + if (motorConfig()->useDshotTelemetry && (motorConfig()->motorPwmProtocol >= PWM_TYPE_DSHOT150)) { + escSensorDshotActive = true; + ENABLE_STATE(ESC_SENSOR_ENABLED); + return true; + } + // FUNCTION_ESCSERIAL is shared between SERIALSHOT and ESC_SENSOR telemetry // They are mutually exclusive serialPortConfig_t * portConfig = findSerialPortConfig(FUNCTION_ESCSERIAL); @@ -224,10 +267,6 @@ bool escSensorInitialize(void) return false; } - for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) { - escSensorData[i].dataAge = ESC_DATA_INVALID; - } - ENABLE_STATE(ESC_SENSOR_ENABLED); return true; @@ -293,3 +332,13 @@ void escSensorUpdate(timeUs_t currentTimeUs) } #endif + +#ifndef USE_ESC_SENSOR +bool escSensorIsActive(void) { return false; } +void escSensorInitData(void) {} +void escSensorSetDshotData(uint8_t esc, uint32_t rpm, int16_t temperature, int16_t voltage, int32_t current) +{ + UNUSED(esc); UNUSED(rpm); UNUSED(temperature); UNUSED(voltage); UNUSED(current); +} +escSensorData_t * escSensorGetData(void) { return NULL; } +#endif diff --git a/src/main/sensors/esc_sensor.h b/src/main/sensors/esc_sensor.h index a5958c85c16..ba86ccc66fc 100644 --- a/src/main/sensors/esc_sensor.h +++ b/src/main/sensors/esc_sensor.h @@ -48,3 +48,6 @@ void escSensorUpdate(timeUs_t currentTimeUs); escSensorData_t * escSensorGetData(void); escSensorData_t * getEscTelemetry(uint8_t esc); uint32_t computeRpm(int16_t erpm); +void escSensorInitData(void); +void escSensorSetDshotData(uint8_t esc, uint32_t rpm, int16_t temperature, int16_t voltage, int32_t current); +bool escSensorIsActive(void); diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h index ccd14d35a4a..102ec273e04 100644 --- a/src/main/target/common_post.h +++ b/src/main/target/common_post.h @@ -131,7 +131,7 @@ extern uint8_t __config_end; #endif -#ifdef USE_ESC_SENSOR +#if defined(USE_ESC_SENSOR) || defined(USE_DSHOT) #define USE_RPM_FILTER #endif diff --git a/src/main/telemetry/srxl.c b/src/main/telemetry/srxl.c index ae3dcf9b7f1..865e4551659 100644 --- a/src/main/telemetry/srxl.c +++ b/src/main/telemetry/srxl.c @@ -47,6 +47,7 @@ #include "flight/imu.h" #include "flight/mixer.h" +#include "drivers/dshot.h" #include "io/gps.h" @@ -64,7 +65,6 @@ #include "telemetry/srxl.h" #include "drivers/vtx_common.h" -//#include "drivers/dshot.h" #include "io/vtx_tramp.h" #include "io/vtx_smartaudio.h" @@ -184,30 +184,14 @@ typedef struct uint16_t getMotorAveragePeriod(void) { -#if defined( USE_ESC_SENSOR_TELEMETRY) || defined( USE_DSHOT_TELEMETRY) +#if defined(USE_ESC_SENSOR) || defined(USE_DSHOT) uint32_t rpm = 0; uint16_t period_us = SPEKTRUM_RPM_UNUSED; -#if defined( USE_ESC_SENSOR_TELEMETRY) - escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED); + escSensorData_t *escData = escSensorGetData(); if (escData != NULL) { rpm = escData->rpm; } -#endif - -#if defined(USE_DSHOT_TELEMETRY) - if (useDshotTelemetry) { - uint16_t motors = getMotorCount(); - - if (motors > 0) { - for (int motor = 0; motor < motors; motor++) { - rpm += getDshotTelemetry(motor); - } - rpm = 100.0f / (motorConfig()->motorPoleCount / 2.0f) * rpm; // convert erpm freq to RPM. - rpm /= motors; // Average combined rpm - } - } -#endif if (rpm > SPEKTRUM_MIN_RPM && rpm < SPEKTRUM_MAX_RPM) { period_us = MICROSEC_PER_MINUTE / rpm; // revs/minute -> microSeconds