diff --git a/docs/Blackbox.md b/docs/Blackbox.md index 55b0f19c103..732428e1b97 100644 --- a/docs/Blackbox.md +++ b/docs/Blackbox.md @@ -194,6 +194,7 @@ Available debug modes include: - `GPS` - GPS debugging - `ALTITUDE` - Altitude estimation debugging - `MAG` - Raw, uncalibrated magnetometer samples (useful for external compass calibration) +- `RPM_FILTER` - Raw per-motor eRPM/100 (same unit as Betaflight's `eRPM[]` blackbox field, before the RPM filter's smoothing), motors 1-8 in `debug[0]`-`debug[7]`; RPM = value * 100 / (`motor_poles` / 2). Needs the RPM filter enabled, so bidirectional DShot or ESC telemetry. `src/utils/blackbox_rpm_to_erpm.py` renames the fields to `eRPM[]` for PIDtoolbox - And 20+ other modes for specific subsystems To use debug mode logging: diff --git a/docs/Settings.md b/docs/Settings.md index a0bdc187935..b0f66f277aa 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -803,6 +803,7 @@ Defines debug values exposed in debug variables (developer / debugging setting) | ESC | | | FW_TURN | | | MAG | | +| RPM_FILTER | | --- @@ -944,6 +945,26 @@ Sets the DShot beeper tone --- +### dshot_bidir_enabled + +Enable bidirectional DShot telemetry on motor outputs. Required for RPM filtering without a separate ESC telemetry UART + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + +### dshot_edt_enabled + +Enable extended DShot telemetry (temperature, voltage, current in addition to eRPM). The ESC is asked for it on every arm, so the values appear after the first arm + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + ### dterm_lpf2_hz Dterm pre-differentiation LPF cutoff (Hz). Filters gyro before differentiation to reduce noise amplification. Higher = less delay, more noise. 0 = disabled. Values around 200-250Hz can add smoothing with small delay. diff --git a/docs/development/msp/README.md b/docs/development/msp/README.md index ec6054fb51b..4c5fcf89b5f 100644 --- a/docs/development/msp/README.md +++ b/docs/development/msp/README.md @@ -4180,7 +4180,7 @@ When the MSP JSON specification changes, bump `msp_messages.json` version: **Notes:** Requires `USE_PROGRAMMING_FRAMEWORK`. Fixed 8-byte reply carrying one 64-bit mask as two `uint32_t` halves, low half first. Only the first `MIN(MAX_LOGIC_CONDITIONS, 64)` bits are evaluated. A condition counts as configured when any of `enabled`, `activatorId` (default -1), `operation`, `operandA.type`, `operandA.value`, `operandB.type`, `operandB.value` or `flags` differs from its default. ## `MSP2_INAV_ESC_RPM (8256 / 0x2040)` -**Description:** Retrieves the RPM reported by each ESC via telemetry. +**Description:** Retrieves the RPM and telemetry link quality reported by each ESC. **Request Payload:** **None** @@ -4188,8 +4188,10 @@ When the MSP JSON specification changes, bump `msp_messages.json` version: |Field|C Type|Size (Bytes)|Units|Description| |---|---|---|---|---| | `escRpm` | `uint32_t` | 4 | RPM | RPM reported by the ESC | +| `frameSuccess` | `uint8_t` | 1 | % | Share of telemetry replies decoded in the last closed window. 255 when no reply was expected in that window | +| `frameTotal` | `uint16_t` | 2 | frames | Telemetry replies expected in the last closed window, missing ones included | -**Notes:** Requires `USE_ESC_SENSOR`. Payload size depends on the number of detected motors with telemetry. +**Notes:** Requires `USE_ESC_SENSOR` or `USE_DSHOT_BIDIR`. 7 bytes per motor, `getMotorCount()` entries. Source follows the RPM filter: bidirectional DSHOT when `isDshotTelemetryActive()`, serial ESC telemetry otherwise. The frame statistics come from fixed back-to-back windows of `ESC_FRAME_WINDOW_MS` (1 s), so they change once per window. A reply counts as missing when the ESC stays silent, fails CRC/GCR decoding or (serial) times out. Sources without frame counting (SRXL2) always report 255 / 0. ## `MSP2_INAV_ESC_TELEM (8257 / 0x2041)` **Description:** Retrieves the full telemetry data structure reported by each ESC. diff --git a/docs/development/msp/msp_messages.json b/docs/development/msp/msp_messages.json index 50d6d0b27d0..0f69ef03788 100644 --- a/docs/development/msp/msp_messages.json +++ b/docs/development/msp/msp_messages.json @@ -9847,13 +9847,25 @@ "ctype": "uint32_t", "desc": "RPM reported by the ESC", "units": "RPM" + }, + { + "name": "frameSuccess", + "ctype": "uint8_t", + "desc": "Share of telemetry replies decoded in the last closed window. 255 when no reply was expected in that window", + "units": "%" + }, + { + "name": "frameTotal", + "ctype": "uint16_t", + "desc": "Telemetry replies expected in the last closed window, missing ones included", + "units": "frames" } ], "repeating": "getMotorCount()" }, "variable_len": "getMotorCount()", - "notes": "Requires `USE_ESC_SENSOR`. Payload size depends on the number of detected motors with telemetry.", - "description": "Retrieves the RPM reported by each ESC via telemetry." + "notes": "Requires `USE_ESC_SENSOR` or `USE_DSHOT_BIDIR`. 7 bytes per motor, `getMotorCount()` entries. Source follows the RPM filter: bidirectional DSHOT when `isDshotTelemetryActive()`, serial ESC telemetry otherwise. The frame statistics come from fixed back-to-back windows of `ESC_FRAME_WINDOW_MS` (1 s), so they change once per window. A reply counts as missing when the ESC stays silent, fails CRC/GCR decoding or (serial) times out. Sources without frame counting (SRXL2) always report 255 / 0.", + "description": "Retrieves the RPM and telemetry link quality reported by each ESC." }, "MSP2_INAV_ESC_TELEM": { "code": 8257, diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 1cfd028c3f9..d637af2d53f 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -252,6 +252,8 @@ main_sources(COMMON_SRC drivers/pwm_esc_detect.h drivers/pwm_mapping.c drivers/pwm_mapping.h + drivers/bidir_dshot.c + drivers/bidir_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 fc6b8c11d3b..0975b163f85 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1573,8 +1573,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 #ifdef USE_DRONECAN @@ -1600,7 +1602,7 @@ static bool writeSlowFrameIfNeeded(bool allowPeriodicWrite) if (shouldWrite) { loadSlowState(&slowHistory); } else { - blackboxSlowState_t newSlowState; + blackboxSlowState_t newSlowState = {0}; loadSlowState(&newSlowState); @@ -2152,6 +2154,7 @@ static bool blackboxWriteSysinfo(void) #endif BLACKBOX_PRINT_HEADER_LINE("serialrx_provider", "%d", rxConfig()->serialrx_provider); BLACKBOX_PRINT_HEADER_LINE("motor_pwm_protocol", "%d", motorConfig()->motorPwmProtocol); + BLACKBOX_PRINT_HEADER_LINE("motor_poles", "%d", motorConfig()->motorPoleCount); BLACKBOX_PRINT_HEADER_LINE("motor_pwm_rate", "%d", getEscUpdateFrequency()); BLACKBOX_PRINT_HEADER_LINE("debug_mode", "%d", systemConfig()->debug_mode); BLACKBOX_PRINT_HEADER_LINE("features", "%d", featureConfig()->enabledFeatures); diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 23caa6b6fa7..6adfd58cf72 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -87,6 +87,7 @@ typedef enum { DEBUG_ESC, DEBUG_FW_TURN, DEBUG_MAG, + DEBUG_RPM_FILTER, DEBUG_COUNT // also update debugModeNames in cli.c } debugType_e; diff --git a/src/main/drivers/bidir_dshot.c b/src/main/drivers/bidir_dshot.c new file mode 100644 index 00000000000..d99a2ed87f9 --- /dev/null +++ b/src/main/drivers/bidir_dshot.c @@ -0,0 +1,179 @@ +/* + * 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 "platform.h" +#include "drivers/bidir_dshot.h" + +// Bidirectional DSHOT telemetry only: eRPM / EDT decoding, RPM state for the RPM filter and +// the ESC sensor. Frame generation, DMA and DSHOT commands for plain DSHOT live in +// pwm_output.c under USE_DSHOT. Every user of this API is guarded by USE_DSHOT_BIDIR. +// +// Values are published as decoded, unfiltered: frames arrive at the DSHOT frame rate, which +// follows the scheduler's idle time rather than any fixed period, so smoothing belongs to the +// consumer that samples them at a known rate (rpmFilterUpdateTask()). + +#ifdef USE_DSHOT_BIDIR + +#include "flight/mixer.h" + +// Only eRPM, temperature, voltage and current are kept; the EDT debug and status frames +// have no consumer and just mark their type as seen +#define DSHOT_TELEMETRY_STORED_TYPE_COUNT (DSHOT_TELEMETRY_TYPE_CURRENT + 1) + +// A silent ESC (reset, brownout, desync) must not leave the RPM notches on its last speed. +// Replies come with every frame, so this is far longer than any regular gap, EDT included +#define DSHOT_ERPM_TIMEOUT_MS 100 + +typedef struct { + uint16_t telemetryData[DSHOT_TELEMETRY_STORED_TYPE_COUNT]; + uint8_t telemetryTypes; // bit per dshotTelemetryType_e seen since boot + timeMs_t lastErpmMs; // millis(), not micros(): a dead ESC must not turn fresh on wrap +} dshotTelemetryMotorState_t; + +bool useDshotTelemetry = false; + +static dshotTelemetryMotorState_t motorTelemetry[MAX_SUPPORTED_MOTORS]; +static float erpmToHz; + +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(uint16_t value, uint8_t motorIndex, uint32_t *decoded, dshotTelemetryType_e *type) +{ + const bool edtEnabled = motorConfig()->useDshotEdt || (motorTelemetry[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 { + // EDT frame types 2, 4, ... 14 are the enum values 1..7 in order + *type = telemetryType >> 1; + *decoded = value & 0x00ff; + } +} + +bool isDshotTelemetryActive(void) +{ + return useDshotTelemetry; +} + +// useDshotTelemetry itself is set by the motor driver (pwmMotorPreconfigure()), which +// knows whether the outputs were configured for DSHOT at all +void initDshotTelemetry(void) +{ + if (!useDshotTelemetry) { + return; + } + + erpmToHz = ERPM_PER_LSB / 60.0f / (motorConfig()->motorPoleCount / 2.0f); +} + +uint16_t dshotProcessPacket(uint16_t rawValue, uint8_t motorIndex) +{ + if (!useDshotTelemetry || motorIndex >= MAX_SUPPORTED_MOTORS) { + return rawValue; + } + + // As in Betaflight, a window without a reply is not counted: an ESC may skip replies + // when busy, so only replies that were at least partly captured count + if (rawValue == DSHOT_TELEMETRY_NOEDGE) { + return rawValue; + } + + escFrameCounter_t *frames = escSensorFrameCounter(motorIndex); + frames->total++; + + if (rawValue == DSHOT_TELEMETRY_INVALID) { + return rawValue; + } + + dshotTelemetryType_e type; + uint32_t decoded; + dshotDecodeTelemetryValue(rawValue, motorIndex, &decoded, &type); + if (decoded == DSHOT_TELEMETRY_INVALID) { + return DSHOT_TELEMETRY_INVALID; + } + + frames->valid++; + + motorTelemetry[motorIndex].telemetryTypes |= (1 << type); + if (type < DSHOT_TELEMETRY_STORED_TYPE_COUNT) { + motorTelemetry[motorIndex].telemetryData[type] = decoded; + } + if (type == DSHOT_TELEMETRY_TYPE_ERPM) { + motorTelemetry[motorIndex].lastErpmMs = millis(); + } + + return rawValue; +} + +float getMotorFrequencyHz(uint8_t motorIndex) +{ + const dshotTelemetryMotorState_t *state = &motorTelemetry[motorIndex]; + if (millis() - state->lastErpmMs > DSHOT_ERPM_TIMEOUT_MS) { + return 0.0f; + } + + return state->telemetryData[DSHOT_TELEMETRY_TYPE_ERPM] * erpmToHz; +} + +float getDshotRpm(uint8_t motorIndex) +{ + return getMotorFrequencyHz(motorIndex) * 60.0f; +} + +bool getDshotEscSensorData(escSensorData_t *data, uint8_t motorIndex) +{ + if (!useDshotTelemetry || motorIndex >= MAX_SUPPORTED_MOTORS) { + return false; + } + + const dshotTelemetryMotorState_t *state = &motorTelemetry[motorIndex]; + if ((state->telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_ERPM)) == 0) { + return false; + } + + // escSensorData_t units: rpm (mechanical), degrees C, 0.01 V, 0.01 A. EDT reports + // the temperature in degrees C, the voltage in 0.25 V and the current in 1 A steps. + data->rpm = lrintf(getDshotRpm(motorIndex)); + 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] * 25 : 0; + data->current = (state->telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_CURRENT)) ? state->telemetryData[DSHOT_TELEMETRY_TYPE_CURRENT] * 100 : 0; + + return true; +} + +#endif diff --git a/src/main/drivers/bidir_dshot.h b/src/main/drivers/bidir_dshot.h new file mode 100644 index 00000000000..4ecdd37115e --- /dev/null +++ b/src/main/drivers/bidir_dshot.h @@ -0,0 +1,65 @@ +/* + * 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" // timeUs_t for sensors/esc_sensor.h, which has no includes of its own +#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) + +#ifdef USE_DSHOT_BIDIR +extern bool useDshotTelemetry; + +void initDshotTelemetry(void); +bool isDshotTelemetryActive(void); +uint16_t dshotProcessPacket(uint16_t rawValue, uint8_t motorIndex); +float getDshotRpm(uint8_t motorIndex); +// Mechanical frequency from the last decoded eRPM frame, unfiltered (see bidir_dshot.c); 0 once it is stale +float getMotorFrequencyHz(uint8_t motorIndex); +bool getDshotEscSensorData(escSensorData_t *data, uint8_t motorIndex); +#endif diff --git a/src/main/drivers/dma.h b/src/main/drivers/dma.h index 10ece999253..3a866f6eae8 100644 --- a/src/main/drivers/dma.h +++ b/src/main/drivers/dma.h @@ -100,8 +100,8 @@ typedef void (*dmaCallbackHandlerFuncPtr)(DMA_t channelDescriptor); dmaDescriptors[i].irqHandlerCallback(&dmaDescriptors[i]);\ } -#define DMA_CLEAR_FLAG(d, flag) if (d->flagsShift > 31) d->dma->HIFCR = (flag << (d->flagsShift - 32)); else d->dma->LIFCR = (flag << d->flagsShift) -#define DMA_GET_FLAG_STATUS(d, flag) (d->flagsShift > 31 ? d->dma->HISR & (flag << (d->flagsShift - 32)): d->dma->LISR & (flag << d->flagsShift)) +#define DMA_CLEAR_FLAG(d, flag) if (d->flagsShift > 31) d->dma->HIFCR = ((flag) << (d->flagsShift - 32)); else d->dma->LIFCR = ((flag) << d->flagsShift) +#define DMA_GET_FLAG_STATUS(d, flag) (d->flagsShift > 31 ? d->dma->HISR & ((flag) << (d->flagsShift - 32)): d->dma->LISR & ((flag) << d->flagsShift)) @@ -129,8 +129,8 @@ DMA_t dmaGetByRef(const DMA_Stream_TypeDef * ref); } //Same as dma_flag_clear/dma_flag_get -#define DMA_CLEAR_FLAG(d, flag) d->dma->clr = (flag << d->flagsShift) -#define DMA_GET_FLAG_STATUS(d, flag) (d->dma->sts & (flag << d->flagsShift)) +#define DMA_CLEAR_FLAG(d, flag) d->dma->clr = ((flag) << d->flagsShift) +#define DMA_GET_FLAG_STATUS(d, flag) (d->dma->sts & ((flag) << d->flagsShift)) DMA_t dmaGetByRef(const dma_channel_type * ref); diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 20aed6c7264..303f090881d 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -26,13 +26,16 @@ #include "build/atomic.h" #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/io_impl.h" #include "drivers/nvic.h" +#include "drivers/bidir_dshot.h" #include "drivers/time.h" #include "drivers/timer.h" #include "drivers/pwm_mapping.h" @@ -70,6 +73,20 @@ #define DSHOT_DMA_BUFFER_SIZE 18 /* resolution + frame reset (2us) */ #define MAX_DMA_TIMERS 8 +#ifdef USE_DSHOT_BIDIR +/* Telemetry reply window, measured from the frame start: the 18-slot frame, then the ESC's + * ~30 us turnaround and its 21-bit GCR reply at 5/4 of the DSHOT bit rate (17 slots). + * DSHOT_TELEMETRY_DEADTIME_US covers the turnaround plus margin for ESC timing variation. */ +#define DSHOT_TELEMETRY_DEADTIME_US 45 +#define DSHOT_TELEMETRY_WINDOW_SLOTS (DSHOT_DMA_BUFFER_SIZE + 17) +#define GCR_TELEMETRY_INPUT_LEN MAX_GCR_EDGES +/* A bidir port captures the GCR edges of the ESC reply into the same buffer it sends the + * frame from, so the buffer is sized for the larger of the two */ +#define DSHOT_PORT_DMA_BUFFER_SIZE GCR_TELEMETRY_INPUT_LEN +#else +#define DSHOT_PORT_DMA_BUFFER_SIZE DSHOT_DMA_BUFFER_SIZE +#endif + /* Keep-alive frame replayed by circular DMA while the CPU is stalled by a flash write: * 16 data bits followed by an idle (line low) gap of DSHOT_KEEPALIVE_GAP_US. * One DMA slot is one DSHOT bit period, so the gap needs gapUs * dshotHz / bitLength slots. */ @@ -115,10 +132,13 @@ typedef struct { #ifdef USE_DSHOT // DSHOT parameters - timerDMASafeType_t dmaBuffer[DSHOT_DMA_BUFFER_SIZE]; + timerDMASafeType_t dmaBuffer[DSHOT_PORT_DMA_BUFFER_SIZE]; #ifdef USE_DSHOT_DMAR timerDMASafeType_t *dmaBurstBuffer; #endif +#ifdef USE_DSHOT_BIDIR + bool telemetryInputActive; +#endif #endif } pwmOutputPort_t; @@ -150,7 +170,19 @@ static timeUs_t digitalMotorUpdateIntervalUs = 0; static timeUs_t digitalMotorLastUpdateUs; static timeUs_t lastCommandSent = 0; static timeUs_t commandPostDelay = 0; - +#ifdef USE_DSHOT_BIDIR +static bool dshotTelemetryPending = false; +/* One stamp for the whole frame, taken in the main loop just before the frames start, so + * the completion ISR has no clock to read. Turning a port back to output resets the + * counter and period of its timer, which every other port on that timer shares, so no + * port may switch back while a sibling could still be capturing: the reply window is + * therefore waited out once, for all ports, before any of them is touched */ +static timeUs_t dshotTelemetryFrameStampUs = 0; +static timeUs_t dshotTelemetryDeadtimeUs = 0; +// DEBUG_ESC: cycle counter at the frame start, debug[motor] = cycles until its capture is armed +static uint32_t dshotFrameStartCycles = 0; +#endif + static circularBuffer_t commandsCircularBuffer; static uint8_t commandsBuff[DHSOT_COMMAND_QUEUE_SIZE]; static currentExecutingCommand_t currentExecutingCommand; @@ -167,6 +199,19 @@ static void loadDmaBufferDshotStride(timerDMASafeType_t *dmaBuffer, int stride, burstDmaTimer_t burstDmaTimers[MAX_DMA_TIMERS]; uint8_t burstDmaTimersCount = 0; #endif + +#ifdef USE_DSHOT_BIDIR +// 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); +// Set while bidir pins are plain GPIO, disconnected from their timer: parked low until +// the first frame, or held by the pull only during a flash write. The next frame +// reconnects them (dshotConnectOutputs()). +static bool dshotPinsDisconnected = false; +#endif #endif static void pwmOutConfigTimer(pwmOutputPort_t * p, TCH_t * tch, uint32_t hz, uint16_t period, uint16_t value) @@ -295,11 +340,39 @@ void pwmSetMotorDMACircular(bool circular) int motorCount = getMotorCount(); const uint32_t dshotHz = getDshotHz(initMotorProtocol); const uint32_t keepaliveSlots = DSHOT_KEEPALIVE_SLOTS(dshotHz); + // A frame started by pwmCompleteMotorUpdate() may still be in flight for this long + const uint32_t frameUs = DSHOT_DMA_BUFFER_SIZE * DSHOT_MOTOR_BITLENGTH * 1000000UL / dshotHz; + +#ifdef USE_DSHOT_BIDIR + // No keep-alive frames for bidir: the ESC sees a frame gap for the flash write, as in + // Betaflight, with the line held at its idle level only by the pull. + if (useDshotTelemetry) { + if (circular && !dshotPinsDisconnected) { + // Complete the DSHOT exchange before the write: let a frame in flight finish, + // with the deadtime as margin for its completion IRQ, then wait out and decode + // the reply, which turns every port back to output + delayMicroseconds(frameUs + DSHOT_TELEMETRY_DEADTIME_US); + while (!pwmDshotDecodeTelemetry()) { } + + // Disconnect the pins from the timers so nothing drives the line during the + // write; the next frame after it reconnects them + for (int i = 0; i < motorCount; i++) { + const pwmOutputPort_t *port = motors[i].pwmPort; + if (port && port->configured) { + const bool baseInverted = port->tch->timHw->output & TIMER_OUTPUT_INVERTED; + IOConfigGPIO(IOGetByTag(port->tch->timHw->tag), baseInverted ? IOCFG_IPD : IOCFG_IPU); + } + } + dshotPinsDisconnected = true; + } + return; + } +#endif if (circular) { // A frame started by pwmCompleteMotorUpdate() may still be in flight: let it finish // and keep the line low for one full gap before the keep-alive stream starts - delayMicroseconds(DSHOT_DMA_BUFFER_SIZE * DSHOT_MOTOR_BITLENGTH * 1000000UL / dshotHz + DSHOT_KEEPALIVE_GAP_US); + delayMicroseconds(frameUs + DSHOT_KEEPALIVE_GAP_US); // Load a zero-throttle packet into the shared keep-alive buffer. The padding slots // must be zero (line low between frames); DMA_RAM is NOLOAD and not cleared at @@ -420,8 +493,407 @@ static uint8_t getBurstDmaTimerIndex(TIM_TypeDef *timer) } #endif -static pwmOutputPort_t * motorConfigDshot(const timerHardware_t * timerHardware, uint32_t dshotHz, bool enableOutput) +#ifdef USE_DSHOT_BIDIR +// The CC1..CC4 DMA request bits sit next to each other in DIER on every supported MCU +static uint32_t dshotDmaSource(const pwmOutputPort_t *port) { +#if !defined(AT32F43x) + STATIC_ASSERT(TIM_DMA_CC2 == TIM_DMA_CC1 << 1 && TIM_DMA_CC4 == TIM_DMA_CC1 << 3, tim_dma_cc_bits_adjacent); + return TIM_DMA_CC1 << port->tch->timHw->channelIndex; +#else + STATIC_ASSERT(TMR_C2_DMA_REQUEST == TMR_C1_DMA_REQUEST << 1 && TMR_C4_DMA_REQUEST == TMR_C1_DMA_REQUEST << 3, tmr_dma_request_bits_adjacent); + return TMR_C1_DMA_REQUEST << port->tch->timHw->channelIndex; +#endif +} + +#if !defined(AT32F43x) +// Per-channel DMA request bit in DIER; the LL and SPL drivers only offer fixed-channel variants +static inline void dshotDmaReqEnable(TIM_TypeDef *tim, uint32_t dmaSource) +{ + SET_BIT(tim->DIER, dmaSource); +} + +static inline void dshotDmaReqDisable(TIM_TypeDef *tim, uint32_t dmaSource) +{ + CLEAR_BIT(tim->DIER, dmaSource); +} + +// Register slices of this port's timer channel for the direction switch: the channel's +// byte in CCMR1/CCMR2 and its nibble in CCER. Derived on the fly, the shifts cost a few cycles. +typedef struct { + __typeof__(((TIM_TypeDef *)0)->CCMR1) *ccmr; // CCMRx is 16 bits wide on F4, 32 on F7/H7 + uint32_t ccmrShift; + uint32_t ccmrMask; // the channel's byte plus its OCxM[3] bit + uint32_t ccerShift; + uint32_t ccerMask; +} dshotChannelRegs_t; + +static dshotChannelRegs_t dshotChannelRegs(const pwmOutputPort_t *port) +{ + const uint32_t channelIndex = port->tch->timHw->channelIndex; + TIM_TypeDef *tim = port->tch->timHw->tim; + dshotChannelRegs_t regs; + regs.ccmr = (channelIndex < 2) ? &tim->CCMR1 : &tim->CCMR2; + regs.ccmrShift = 8 * (channelIndex & 1); + regs.ccmrMask = 0xFF << regs.ccmrShift; +#ifdef TIM_CCMR1_OC1M_3 + // F7/H7 keep a fourth OCxM bit outside the channel byte; F4 has none + regs.ccmrMask |= TIM_CCMR1_OC1M_3 << regs.ccmrShift; +#endif + regs.ccerShift = 4 * channelIndex; + regs.ccerMask = 0xF << regs.ccerShift; + return regs; +} + +// CCMRx channel byte for capture: CCxS=01 (TI direct), ICxPSC=0, ICxF=0010 (fDTS, N=4) +#define DSHOT_CCMR_INPUT (TIM_CCMR1_CC1S_0 | TIM_CCMR1_IC1F_1) +// CCMRx channel byte for output: CCxS=00, OCxM=110 (PWM1); preload is added once CCR is zeroed +#define DSHOT_CCMR_OUTPUT (TIM_CCMR1_OC1M_2 | TIM_CCMR1_OC1M_1) +// CCER channel nibble for capture: both edges, channel on +#define DSHOT_CCER_INPUT (TIM_CCER_CC1E | TIM_CCER_CC1P | TIM_CCER_CC1NP) +#else +// The channel identifiers are evenly spaced, so channelIndex maps onto them linearly +static uint32_t dshotTimChannel(const pwmOutputPort_t *port) +{ + STATIC_ASSERT(TMR_SELECT_CHANNEL_4 == TMR_SELECT_CHANNEL_1 + 3 * (TMR_SELECT_CHANNEL_2 - TMR_SELECT_CHANNEL_1), tmr_channel_ids_linear); + return TMR_SELECT_CHANNEL_1 + port->tch->timHw->channelIndex * (TMR_SELECT_CHANNEL_2 - TMR_SELECT_CHANNEL_1); +} +#endif + +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; + if (len < 1 || len > 21) { + // Edges closer than half a GCR bit or further apart than a whole frame + return DSHOT_TELEMETRY_INVALID; + } + } else { + len = 21 - bits; + if (len < 1) { + // All bits accounted for, no trailing run left to infer + break; + } + } + + 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) +{ + // Bidirectional DSHOT is carried on inverted (idle-high) signalling - flip the + // channel's normal polarity whenever telemetry is active, matching Betaflight. + const bool inverted = ((port->tch->timHw->output & TIMER_OUTPUT_INVERTED) != 0) ^ useDshotTelemetry; + +#if !defined(AT32F43x) + // Telemetry input capture widens ARR to 0xffff (see pwmDshotSetDirectionInput); + // restore the DSHOT bit period now, bypassing the ARR preload shadow so it + // takes effect immediately instead of after the next update event. + TIM_TypeDef *tim = port->tch->timHw->tim; + CLEAR_BIT(tim->CR1, TIM_CR1_ARPE); + tim->ARR = DSHOT_MOTOR_BITLENGTH - 1; + SET_BIT(tim->CR1, TIM_CR1_ARPE); + tim->CNT = 0; + + // Channel back to PWM1 output. CCxS is only writable with the channel off, and CCR is + // zeroed before OCxPE goes back on so the stale capture value can't give one bit period + // of output before the first update event. Idle state (CR2 OISx) is untouched. + const dshotChannelRegs_t regs = dshotChannelRegs(port); + CLEAR_BIT(tim->CCER, regs.ccerMask); + MODIFY_REG(*regs.ccmr, regs.ccmrMask, DSHOT_CCMR_OUTPUT << regs.ccmrShift); + *port->ccr = 0; + SET_BIT(*regs.ccmr, TIM_CCMR1_OC1PE << regs.ccmrShift); + uint32_t ccer; + if (port->tch->timHw->output & TIMER_OUTPUT_N_CHANNEL) { + ccer = TIM_CCER_CC1NE | (inverted ? TIM_CCER_CC1NP : 0); + } else { + ccer = TIM_CCER_CC1E | (inverted ? TIM_CCER_CC1P : 0); + } + MODIFY_REG(tim->CCER, regs.ccerMask, ccer << regs.ccerShift); + + // Stream back to memory-to-peripheral; timerPWMPrepareDMA() sets count and addresses and + // enables it for the next frame. The capture may still be draining (the decode loop only + // cleared EN), so wait for EN to read 0 before touching CR, then clear all five event + // flags: EN=1 is ignored while any of them is set (RM0433). + DMA_Stream_TypeDef *stream = port->tch->dma->ref; + for (uint32_t timeout = 10000; timeout && (stream->CR & DMA_SxCR_EN); timeout--) { } + DMA_CLEAR_FLAG(port->tch->dma, DMA_IT_TCIF | DMA_IT_HTIF | DMA_IT_TEIF | DMA_IT_DMEIF | DMA_IT_FEIF); + stream->CR = (stream->CR & ~(DMA_SxCR_DIR | DMA_SxCR_EN)) | DMA_SxCR_DIR_0; + port->telemetryInputActive = false; +#else + // Telemetry input capture widens the period to 0xffff (see pwmDshotSetDirectionInput); + // restore the DSHOT bit period now, bypassing the period buffer so it takes effect + // immediately, and reset the counter so a stale count above the new period doesn't + // have to run all the way round (TMR2/TMR5 are 32-bit) before the first compare. + tmr_period_buffer_enable(port->tch->timHw->tim, FALSE); + tmr_period_value_set(port->tch->timHw->tim, DSHOT_MOTOR_BITLENGTH - 1); + tmr_period_buffer_enable(port->tch->timHw->tim, TRUE); + tmr_counter_value_set(port->tch->timHw->tim, 0); + + 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 = inverted ? TMR_OUTPUT_ACTIVE_LOW : TMR_OUTPUT_ACTIVE_HIGH; + output.occ_idle_state = FALSE; + } else { + output.oc_output_state = TRUE; + output.occ_output_state = FALSE; + output.oc_polarity = inverted ? TMR_OUTPUT_ACTIVE_LOW : TMR_OUTPUT_ACTIVE_HIGH; + 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; +#endif +} + +FAST_CODE static void pwmDshotSetDirectionInput(pwmOutputPort_t *port) +{ +#if !defined(AT32F43x) + // Runs in the DMA completion ISR for every motor in turn, so it only writes registers: + // no HAL/LL init calls, and the stream is reconfigured in place (hardware clears its EN + // bit at end of transfer, the wait below only covers the flag/EN ordering). + // pwmOutputPorts lives in non-cacheable DMA_RAM, so read tch once. + TCH_t *tch = port->tch; + TIM_TypeDef *tim = tch->timHw->tim; + const dshotChannelRegs_t regs = dshotChannelRegs(port); + + // Widen ARR so the free-running counter doesn't wrap every DSHOT bit period + // (20 ticks) while timing GCR edges, which span ~21 bits per telemetry frame. + // ARPE is set, so this lands at the next update event, within one bit period. + tim->ARR = 0xffff; + +#if defined(STM32H7) + // H7 errata workaround (matches Betaflight): reconfiguring the channel from output + // compare to input capture can glitch the pin for a cycle while CCMR/CCER are mid-update. + // Drive it as a plain GPIO output at the preset ODR level meanwhile. Only MODER changes; + // pull, speed and AF stay as dshotConnectOutputs() set them. + const IO_t io = IOGetByTag(tch->timHw->tag); + GPIO_TypeDef *gpio = IO_GPIO(io); + const uint32_t moderShift = 2 * IO_GPIOPinIdx(io); + MODIFY_REG(gpio->MODER, GPIO_MODER_MODE0_Msk << moderShift, GPIO_MODER_MODE0_0 << moderShift); +#endif + + // Channel to input capture, both edges, filter N=4. CCxS is only writable with the channel off. + CLEAR_BIT(tim->CCER, regs.ccerMask); + MODIFY_REG(*regs.ccmr, regs.ccmrMask, DSHOT_CCMR_INPUT << regs.ccmrShift); + MODIFY_REG(tim->CCER, regs.ccerMask, DSHOT_CCER_INPUT << regs.ccerShift); + +#if defined(STM32H7) + MODIFY_REG(gpio->MODER, GPIO_MODER_MODE0_Msk << moderShift, GPIO_MODER_MODE0_1 << moderShift); +#endif + + // Stream to peripheral-to-memory: addresses, FIFO and request stay from the output frame. + // Clear all five event flags first, EN=1 is ignored while any of them is set (RM0433). + DMA_Stream_TypeDef *stream = tch->dma->ref; + for (uint32_t timeout = 1000; timeout && (stream->CR & DMA_SxCR_EN); timeout--) { } + DMA_CLEAR_FLAG(tch->dma, DMA_IT_TCIF | DMA_IT_HTIF | DMA_IT_TEIF | DMA_IT_DMEIF | DMA_IT_FEIF); + const uint32_t cr = (stream->CR & ~(DMA_SxCR_DIR | DMA_SxCR_EN)) | DMA_SxCR_TCIE; + stream->CR = cr; + stream->NDTR = GCR_TELEMETRY_INPUT_LEN; + stream->CR = cr | DMA_SxCR_EN; + dshotDmaReqEnable(tim, dshotDmaSource(port)); +#else + // Widen the period so the free-running counter doesn't wrap every DSHOT bit period + // (20 ticks) while timing GCR edges, which span ~21 bits per telemetry frame. + tmr_period_buffer_enable(port->tch->timHw->tim, TRUE); + tmr_period_value_set(port->tch->timHw->tim, 0xffff); + + 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); +#endif + port->telemetryInputActive = true; +} + +// Connect bidir DSHOT pins to their timer AF (idle-high). Called on the first +// real motor update so the line only goes high microseconds before frames start, +// keeping it low through the ESC's boot-time bootloader-entry window. +static void dshotConnectOutputs(void) +{ + for (int index = 0; index < getMotorCount(); index++) { + pwmOutputPort_t *port = motors[index].pwmPort; + if (port && port->configured) { + const timerHardware_t *timHw = port->tch->timHw; + const IO_t io = IOGetByTag(timHw->tag); + // The pull must match this channel's *un-flipped* polarity + // (pwmDshotSetDirectionOutput flips OCPolarity for bidir, but the GCR + // listen phase still idles toward the same physical level either way). + const bool baseInverted = timHw->output & TIMER_OUTPUT_INVERTED; +#if defined(STM32H7) + // Preset ODR to the bidir idle level once - the H7 OC->IC erratum workaround + // in pwmDshotSetDirectionInput() disconnects the pin from the timer and relies + // on this cached ODR bit (matches Betaflight) instead of redriving the level. + baseInverted ? IOLo(io) : IOHi(io); +#endif + IOConfigGPIOAF(io, baseInverted ? IOCFG_AF_PP_PD : IOCFG_AF_PP_UP, timHw->alternateFunction); + } + } +} + +// Takes over the stream's completion interrupt from the timer driver on bidir ports: once +// the frame is out the port is turned round to capture the ESC reply, and +// pwmDshotDecodeTelemetry() turns it back before the next frame. Runs once per motor per +// frame, back to back, so it lives in ITCM where available. +FAST_CODE static void pwmDshotDmaIrqHandler(DMA_t descriptor) +{ + if (!DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) { + return; + } + + // userParam is the motor index (see motorConfigDshot()) + pwmOutputPort_t *port = motors[descriptor->userParam].pwmPort; + +#if defined(AT32F43x) + dma_channel_enable(port->tch->dma->ref, FALSE); + tmr_dma_request_enable(port->tch->timHw->tim, dshotDmaSource(port), FALSE); +#else + // The stream disabled itself at end of transfer (normal mode). The DMA request must go + // too: CCR is 0 after the frame, so compare matches keep a request pending and the stream + // would serve it as a bogus first capture the moment it is re-enabled for input. + dshotDmaReqDisable(port->tch->timHw->tim, dshotDmaSource(port)); +#endif + + // Same bookkeeping as the timer driver's handler (timerPWMDMAInProgress()) + if (port->tch->dmaState == TCH_DMA_ACTIVE) { + port->tch->dmaState = TCH_DMA_IDLE; + } + + if (!port->telemetryInputActive) { + pwmDshotSetDirectionInput(port); + dshotTelemetryPending = true; + + // DEBUG_ESC: debug[motor] = cycles from the frame start to this capture being armed + // (H7: /480 = us, F7: /216) + if (debugMode == DEBUG_ESC && descriptor->userParam < DEBUG32_VALUE_COUNT) { + debug[descriptor->userParam] = ticks() - dshotFrameStartCycles; + } + } + + DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF); +} + +// DMA has already captured the telemetry waveform before this runs. Keep the +// decode path out of the fast motor-update caller on constrained F7 targets. +static bool NOINLINE pwmDshotDecodeTelemetry(void) +{ + if (!dshotTelemetryPending) { + return true; + } + + // Wait out the reply window of the frame before switching any port back (see + // dshotTelemetryFrameStampUs) + if ((micros() - dshotTelemetryFrameStampUs) < dshotTelemetryDeadtimeUs) { + return false; + } + + // Cleared before the loop: a completion IRQ landing while the loop runs re-arms it, so a + // port the loop has already passed is picked up by the next call instead of being left + // in input for the following frame + dshotTelemetryPending = false; + + for (int motorIndex = 0; motorIndex < getMotorCount(); motorIndex++) { + pwmOutputPort_t *port = motors[motorIndex].pwmPort; + if (!port || !port->configured || !port->telemetryInputActive) { + continue; + } + + uint32_t edges = 0; +#if 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 - port->tch->dma->ref->NDTR; + dshotDmaReqDisable(port->tch->timHw->tim, dshotDmaSource(port)); + CLEAR_BIT(port->tch->dma->ref->CR, DMA_SxCR_EN); +#endif + + // Too few edges is no reply at all + uint16_t rawValue = DSHOT_TELEMETRY_NOEDGE; + if (edges > MIN_GCR_EDGES) { + // No cache maintenance: pwmOutputPorts is DMA_RAM, which the H7 MPU maps non-cacheable + 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 + // Nothing to publish before the first eRPM value (an EDT frame may come first) + escSensorData_t data; + if (getDshotEscSensorData(&data, motorIndex)) { + escSensorSetDshotData(motorIndex, data.rpm, data.temperature, data.voltage, data.current); + } +#endif + } + + pwmDshotSetDirectionOutput(port); + } + + // Re-armed during the loop: some port is in input again, hold the frame until it is back + return !dshotTelemetryPending; +} +#endif // USE_DSHOT_BIDIR + +static pwmOutputPort_t * motorConfigDshot(const timerHardware_t * timerHardware, uint32_t dshotHz, bool enableOutput, uint8_t motorIndex) +{ + UNUSED(motorIndex); // Try allocating new port pwmOutputPort_t * port = pwmOutConfig(timerHardware, OWNER_MOTOR, dshotHz, DSHOT_MOTOR_BITLENGTH, 0, enableOutput); @@ -429,9 +901,26 @@ static pwmOutputPort_t * motorConfigDshot(const timerHardware_t * timerHardware, return NULL; } +#ifdef USE_DSHOT_BIDIR + if (enableOutput && useDshotTelemetry) { + // Bidir signalling idles high, but holding the line high with no edges + // during ESC boot triggers the BLHeli/Bluejay bootloader-entry check + // (~150ms of continuous high right after the ESC's startup melody). The + // first INAV frame goes out seconds after this pin is configured, so the + // ESC would enter its bootloader, time out and reset - replaying the + // startup melody. Park the pin so the ESC sees low, like normal DSHOT + // idle, and defer the AF connect until the first frame goes out + // (dshotConnectOutputs()). On TIMER_OUTPUT_INVERTED hardware the pin + // level is re-inverted downstream, so park high there. + const IO_t io = IOGetByTag(timerHardware->tag); + IOConfigGPIO(io, IOCFG_OUT_PP); + (timerHardware->output & TIMER_OUTPUT_INVERTED) ? IOHi(io) : IOLo(io); + dshotPinsDisconnected = true; + } +#endif + // 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; @@ -449,6 +938,25 @@ static pwmOutputPort_t * motorConfigDshot(const timerHardware_t * timerHardware, // Only mark as DSHOT channel if DMA was set successfully ZERO_FARRAY(port->dmaBuffer); port->configured = true; +#ifdef USE_DSHOT_BIDIR + if (useDshotTelemetry) { + // Inverted signalling and per-frame direction switching; the port takes the + // stream's completion interrupt over from the timer driver + dshotTelemetryDeadtimeUs = DSHOT_TELEMETRY_DEADTIME_US + 1000000 * (DSHOT_TELEMETRY_WINDOW_SLOTS * DSHOT_MOTOR_BITLENGTH) / dshotHz; +#if defined(USE_HAL_DRIVER) + // FIFO threshold 1/4 for both directions (the F7/H7 timer driver set FULL), so + // captured edges reach memory one word at a time and the decode never finds a + // partial reply still sitting in the FIFO. Set once here; the direction switch + // leaves FCR alone. The F4 driver runs the stream in direct mode, which already + // writes every word straight away. + port->tch->dma->ref->FCR = DMA_SxFCR_DMDIS; +#endif + pwmDshotSetDirectionOutput(port); + // The handler gets the motor index: it reaches the port through motors[] and uses + // the index straight as the debug slot + dmaSetHandler(port->tch->dma, pwmDshotDmaIrqHandler, NVIC_PRIO_TIMER_DMA, motorIndex); + } +#endif } #endif @@ -473,11 +981,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; } + // Frame reset slots: rewritten every time, a bidir port captures edges into this buffer + dmaBuffer[16] = 0; + dmaBuffer[17] = 0; } #endif static uint16_t prepareDshotPacket(const uint16_t value, bool requestTelemetry) { + // The telemetry bit asks for a serial telemetry packet on the ESC's telemetry wire; + // the eRPM reply on the motor line is triggered by the inverted checksum alone, so + // bidir leaves the bit to the serial ESC sensor's round-robin requests uint16_t packet = (value << 1) | (requestTelemetry ? 1 : 0); // compute checksum @@ -487,6 +1001,11 @@ static uint16_t prepareDshotPacket(const uint16_t value, bool requestTelemetry) csum ^= csum_data; // xor data by nibbles csum_data >>= 4; } +#ifdef USE_DSHOT_BIDIR + if (useDshotTelemetry) { + csum = ~csum; + } +#endif csum &= 0xf; // append checksum @@ -551,6 +1070,7 @@ static int getDShotCommandRepeats(dshotCommands_e cmd) { int repeats = 1; switch (cmd) { + case DSHOT_CMD_EXTENDED_TELEMETRY_ENABLE: case DSHOT_CMD_SPIN_DIRECTION_NORMAL: case DSHOT_CMD_SPIN_DIRECTION_REVERSED: repeats = 10; @@ -562,7 +1082,8 @@ static int getDShotCommandRepeats(dshotCommands_e cmd) { return repeats; } -static bool executeDShotCommands(void){ +// NOINLINE: pwmCompleteMotorUpdate() is inlined into the ITCM scheduler; commands are rare +static bool NOINLINE executeDShotCommands(void){ timeUs_t tNow = micros(); @@ -606,9 +1127,21 @@ void pwmCompleteMotorUpdate(void) { return; } +#ifdef USE_DSHOT_BIDIR + if (useDshotTelemetry && !pwmDshotDecodeTelemetry()) { + return; + } +#endif + int motorCount = getMotorCount(); timeUs_t currentTimeUs = micros(); +#ifdef USE_DSHOT_BIDIR + if (useDshotTelemetry) { + escSensorFrameWindowUpdate(currentTimeUs / 1000); + } +#endif + // Enforce motor update rate if ((digitalMotorUpdateIntervalUs == 0) || ((currentTimeUs - digitalMotorLastUpdateUs) <= digitalMotorUpdateIntervalUs)) { return; @@ -619,6 +1152,13 @@ void pwmCompleteMotorUpdate(void) { #ifdef USE_DSHOT if (isMotorProtocolDshot()) { +#ifdef USE_DSHOT_BIDIR + if (dshotPinsDisconnected) { + dshotConnectOutputs(); + dshotPinsDisconnected = false; + } +#endif + if (!executeDShotCommands()) { return; } @@ -647,6 +1187,16 @@ void pwmCompleteMotorUpdate(void) { } } +#ifdef USE_DSHOT_BIDIR + if (useDshotTelemetry) { + // Reference for the reply window; the completion ISRs don't read the clock + dshotTelemetryFrameStampUs = micros(); + if (debugMode == DEBUG_ESC) { + dshotFrameStartCycles = ticks(); + } + } +#endif + // Start DMA on all timers for (int index = 0; index < motorCount; index++) { if (motors[index].pwmPort && motors[index].pwmPort->configured) { @@ -672,6 +1222,9 @@ void pwmMotorPreconfigure(void) { // Keep track of initial motor protocol initMotorProtocol = motorConfig()->motorPwmProtocol; +#ifdef USE_DSHOT_BIDIR + useDshotTelemetry = motorConfig()->useDshotTelemetry && getMotorProtocolProperties(initMotorProtocol)->isDSHOT; +#endif #ifdef BRUSHED_MOTORS initMotorProtocol = PWM_TYPE_BRUSHED; // Override proto @@ -764,7 +1317,7 @@ bool pwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, bo case PWM_TYPE_DSHOT600: case PWM_TYPE_DSHOT300: case PWM_TYPE_DSHOT150: - motors[motorIndex].pwmPort = motorConfigDshot(timerHardware, getDshotHz(initMotorProtocol), enableOutput); + motors[motorIndex].pwmPort = motorConfigDshot(timerHardware, getDshotHz(initMotorProtocol), enableOutput, motorIndex); break; #endif diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index afe1a1916e3..fec7d8cd0d0 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -27,6 +27,7 @@ #define MAX_PWM_OUTPUTS (MAX_PWM_OUTPUT_PORTS) #endif typedef enum { + DSHOT_CMD_EXTENDED_TELEMETRY_ENABLE = 13, DSHOT_CMD_SPIN_DIRECTION_NORMAL = 20, DSHOT_CMD_SPIN_DIRECTION_REVERSED = 21, } dshotCommands_e; diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 15339c4f39e..228d872e3d4 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -238,7 +238,8 @@ static const char *debugModeNames[DEBUG_COUNT] = { "TERRAIN_NAV", "ESC", "FW_TURN", - "MAG" + "MAG", + "RPM_FILTER" }; /* Sensor names (used in lookup tables for *_hardware settings and in status diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 778dce2e511..a79c29b5555 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -37,6 +37,7 @@ #include "drivers/time.h" #include "drivers/system.h" #include "drivers/pwm_output.h" +#include "drivers/bidir_dshot.h" #include "drivers/pwm_mapping.h" #include "sensors/sensors.h" @@ -593,6 +594,17 @@ bool emergInflightRearmEnabled(void) return false; // craft doesn't appear to be flying, don't allow emergency rearm } +#ifdef USE_DSHOT_BIDIR +// As in Betaflight: the ESC sends EDT frames only after being asked and forgets it on power-up, +// so ask on every arm, while the motors are still stopped and the ESC accepts commands +static void requestDshotExtendedTelemetry(void) +{ + if (isDshotTelemetryActive() && motorConfig()->useDshotEdt) { + sendDShotCommand(DSHOT_CMD_EXTENDED_TELEMETRY_ENABLE); + } +} +#endif + void tryArm(void) { updateArmingStatus(); @@ -608,6 +620,9 @@ void tryArm(void) const bool turtleIsActive = IS_RC_MODE_ACTIVE(BOXTURTLE); #endif if (STATE(MULTIROTOR) && turtleIsActive && !FLIGHT_MODE(TURTLE_MODE) && emergencyArmingCanOverrideArmingDisabled() && isMotorProtocolDshot()) { +#ifdef USE_DSHOT_BIDIR + requestDshotExtendedTelemetry(); +#endif sendDShotCommand(DSHOT_CMD_SPIN_DIRECTION_REVERSED); ENABLE_ARMING_FLAG(ARMED); ENABLE_FLIGHT_MODE(TURTLE_MODE); @@ -633,6 +648,9 @@ void tryArm(void) lastDisarmReason = DISARM_NONE; +#ifdef USE_DSHOT_BIDIR + requestDshotExtendedTelemetry(); +#endif ENABLE_ARMING_FLAG(ARMED); ENABLE_ARMING_FLAG(WAS_EVER_ARMED); //It is required to inform the mixer that arming was executed and it has to switch to the FORWARD direction diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 9ba5a8e442c..b83297e9744 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/bidir_dshot.h" #include "drivers/exti.h" #include "drivers/io.h" #include "drivers/flash.h" @@ -765,6 +766,9 @@ void init(void) #ifdef USE_DSHOT initDShotCommands(); #endif +#ifdef USE_DSHOT_BIDIR + initDshotTelemetry(); +#endif #ifdef USE_SERIAL_GIMBAL gimbalCommonInit(); @@ -797,7 +801,11 @@ void init(void) #ifdef USE_RPM_FILTER disableRpmFilters(); - if (STATE(ESC_SENSOR_ENABLED) && (rpmFilterConfig()->gyro_filter_enabled || rpmFilterConfig()->dterm_filter_enabled)) { + bool rpmSourceActive = STATE(ESC_SENSOR_ENABLED); +#ifdef USE_DSHOT_BIDIR + rpmSourceActive = rpmSourceActive || isDshotTelemetryActive(); +#endif + if (rpmSourceActive && (rpmFilterConfig()->gyro_filter_enabled || rpmFilterConfig()->dterm_filter_enabled)) { rpmFiltersInit(); setTaskEnabled(TASK_RPM_FILTER, true); } diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 3bf520b977a..efa95e668c1 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -47,6 +47,7 @@ #include "drivers/compass/compass.h" #include "drivers/compass/compass_msp.h" #include "drivers/barometer/barometer_msp.h" +#include "drivers/bidir_dshot.h" #include "drivers/pitotmeter/pitotmeter_msp.h" #include "sensors/battery_sensor_fake.h" #include "drivers/bus_i2c.h" @@ -1957,18 +1958,40 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF break; #endif -#ifdef USE_ESC_SENSOR +#if defined(USE_ESC_SENSOR) || defined(USE_DSHOT_BIDIR) case MSP2_INAV_ESC_RPM: { uint8_t motorCount = getMotorCount(); + // Same source selection as rpmFilterUpdateTask(): bidirectional DSHOT keeps its + // own per-motor RPM, so it works on targets without USE_ESC_SENSOR too for (uint8_t i = 0; i < motorCount; i++){ - const escSensorData_t *escState = getEscTelemetry(i); //Get ESC telemetry - sbufWriteU32(dst, escState->rpm); + uint32_t rpm; +#ifdef USE_DSHOT_BIDIR + if (isDshotTelemetryActive()) { + rpm = lrintf(getDshotRpm(i)); + } else +#endif + { +#ifdef USE_ESC_SENSOR + rpm = getEscTelemetry(i)->rpm; +#else + rpm = 0; +#endif + } + sbufWriteU32(dst, rpm); + // Link quality of the last closed window; 255 when no reply was expected in it + // (boot, no telemetry source, a source without frame counting such as SRXL2) + const escFrameCounter_t *frames = escSensorFrameCounter(i); + const bool framesKnown = frames->lastWindowTotal > 0; + sbufWriteU8(dst, framesKnown ? frames->lastWindowSuccess : 255); + sbufWriteU16(dst, framesKnown ? frames->lastWindowTotal : 0); } } break; +#endif +#ifdef USE_ESC_SENSOR case MSP2_INAV_ESC_TELEM: { uint8_t motorCount = getMotorCount(); diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 142f5f06c9b..400bbeca6d6 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -93,7 +93,7 @@ tables: "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING", "POS_EST", "ADAPTIVE_FILTER", "HEADTRACKER", "GPS", "LULU", "SBUS2", "OSD_REFRESH", "MAG_CALIB", "VTOL_TRANSITION", "VTOL_MC_PROTECT", "TERRAIN_NAV", "ESC", - "FW_TURN", "MAG"] + "FW_TURN", "MAG", "RPM_FILTER"] - name: vtol_mc_protection_mode values: ["OFF", "NAV", "NAV_AND_STABILIZED"] - name: aux_operator @@ -896,6 +896,18 @@ groups: condition: USE_MOTOR_SRXL2 min: 0 max: 9 + - name: dshot_bidir_enabled + description: "Enable bidirectional DShot telemetry on motor outputs. Required for RPM filtering without a separate ESC telemetry UART" + field: useDshotTelemetry + condition: USE_DSHOT_BIDIR + type: bool + default_value: OFF + - name: dshot_edt_enabled + description: "Enable extended DShot telemetry (temperature, voltage, current in addition to eRPM). The ESC is asked for it on every arm, so the values appear after the first arm" + field: useDshotEdt + condition: USE_DSHOT_BIDIR + 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 2dccb22e3b2..0ec3ac21994 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -89,11 +89,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 #ifdef USE_MOTOR_SRXL2 diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 43c31c9132d..d11fe1ae3af 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -93,6 +93,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 #ifdef USE_MOTOR_SRXL2 diff --git a/src/main/flight/rpm_filter.c b/src/main/flight/rpm_filter.c index ffca6c8ea68..7e3c98e7d75 100644 --- a/src/main/flight/rpm_filter.c +++ b/src/main/flight/rpm_filter.c @@ -22,6 +22,8 @@ * along with this program. If not, see http://www.gnu.org/licenses/. */ +#include + #include "platform.h" #include "flight/rpm_filter.h" @@ -36,14 +38,17 @@ #include "common/maths.h" #include "common/filter.h" #include "flight/mixer.h" +#include "drivers/bidir_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 +// Serial ESC telemetry ages by one per missed reply of that motor (SRXL2 only once its own +// stale timeout passed): a single CRC error keeps the value, a silent ESC drops to 0 +#define RPM_FILTER_MAX_ESC_DATA_AGE 2 #define RPM_FILTER_HARMONICS 3 PG_REGISTER_WITH_RESET_TEMPLATE(rpmFilterConfig_t, rpmFilterConfig, PG_RPM_FILTER_CONFIG, 1); @@ -159,8 +164,7 @@ void rpmFilterUpdate(rpmFilterBank_t *filterBank, uint8_t motor, float baseFrequ void rpmFiltersInit(void) { - for (uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) - { + for (uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) { pt1FilterInit(&motorFrequencyFilter[i], RPM_FILTER_RPM_LPF_HZ, US2S(RPM_FILTER_UPDATE_RATE_US)); } @@ -183,14 +187,35 @@ void rpmFilterUpdateTask(timeUs_t currentTimeUs) UNUSED(currentTimeUs); uint8_t motorCount = getMotorCount(); + // Debug holds eRPM/100 like Betaflight's eRPM[] blackbox field, so log tools can reuse it + const float hzToErpm100 = 60.0f * (motorConfig()->motorPoleCount / 2.0f) / ERPM_PER_LSB; /* - * For each motor, read ERPM, filter it and update motor frequency + * For each motor, read its frequency, filter it and update the notches. Both sources + * hand over their latest raw value; the LPF runs here, at this task's fixed rate, so its + * cutoff does not depend on how often new frames happened to arrive */ 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 motorFrequency; +#ifdef USE_DSHOT_BIDIR + if (isDshotTelemetryActive()) { + motorFrequency = getMotorFrequencyHz(i); + } else +#endif + { +#ifdef USE_ESC_SENSOR + const escSensorData_t *escData = getEscTelemetry(i); + motorFrequency = escData->dataAge <= RPM_FILTER_MAX_ESC_DATA_AGE ? (float)escData->rpm / 60.0f : 0.0f; +#else + motorFrequency = 0.0f; +#endif + } + // Raw telemetry before the LPF; blackbox has no per-motor field + if (i < DEBUG32_VALUE_COUNT) { + DEBUG_SET(DEBUG_RPM_FILTER, i, lrintf(motorFrequency * hzToErpm100)); + } + const float baseFrequency = pt1FilterApply(&motorFrequencyFilter[i], motorFrequency); rpmGyroUpdateFn(&gyroRpmFilters, i, baseFrequency); } } diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c index 0611dd0ad44..88c60921e61 100644 --- a/src/main/sensors/esc_sensor.c +++ b/src/main/sensors/esc_sensor.c @@ -41,6 +41,7 @@ #include "config/parameter_group_ids.h" #include "flight/mixer.h" +#include "drivers/bidir_dshot.h" #include "drivers/pwm_output.h" #include "sensors/esc_sensor.h" #include "drivers/pwm_mapping.h" @@ -51,6 +52,30 @@ #include "fc/runtime_config.h" #include "fc/settings.h" +static escFrameCounter_t escSensorFrames[MAX_SUPPORTED_MOTORS]; +static timeMs_t escFrameWindowStartMs; + +escFrameCounter_t * escSensorFrameCounter(uint8_t esc) +{ + return &escSensorFrames[esc]; +} + +// NOINLINE: escSensorUpdate() is inlined into the ITCM scheduler; this runs once per window +void NOINLINE escSensorFrameWindowUpdate(timeMs_t currentTimeMs) +{ + if (currentTimeMs - escFrameWindowStartMs < ESC_FRAME_WINDOW_MS) { + return; + } + + escFrameWindowStartMs = currentTimeMs; + for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) { + escFrameCounter_t *counter = &escSensorFrames[i]; + counter->lastWindowTotal = counter->total; + counter->lastWindowSuccess = counter->total ? (uint32_t)counter->valid * 100 / counter->total : 0; + counter->total = 0; + counter->valid = 0; + } +} #if defined(USE_ESC_SENSOR) @@ -130,6 +155,7 @@ static bool escSensorDecodeFrame(void) if (bufferPosition >= TELEMETRY_FRAME_SIZE) { uint8_t checksum = crc8_update(0, telemetryBuffer, TELEMETRY_FRAME_SIZE - 1); if (checksum == telemetryBuffer[TELEMETRY_FRAME_SIZE - 1]) { + escSensorFrames[escSensorMotor].valid++; escSensorData[escSensorMotor].dataAge = 0; escSensorData[escSensorMotor].temperature = telemetryBuffer[0]; escSensorData[escSensorMotor].voltage = ((uint16_t)telemetryBuffer[1]) << 8 | telemetryBuffer[2]; @@ -157,6 +183,32 @@ 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; +} + escSensorData_t * escSensorGetData(void) { /* @@ -249,6 +301,16 @@ bool escSensorInitialize(void) } #endif + escSensorInitData(); + +#ifdef USE_DSHOT_BIDIR + // Telemetry on the motor line: fed by the motor driver, no port to open + if (isDshotTelemetryActive()) { + ENABLE_STATE(ESC_SENSOR_ENABLED); + return true; + } +#endif + // FUNCTION_ESCSERIAL is shared between SERIALSHOT and ESC_SENSOR telemetry // They are mutually exclusive serialPortConfig_t * portConfig = findSerialPortConfig(FUNCTION_ESCSERIAL); @@ -261,10 +323,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; @@ -272,6 +330,25 @@ bool escSensorInitialize(void) void escSensorUpdate(timeUs_t currentTimeUs) { +#ifdef USE_DSHOT_BIDIR + if (isDshotTelemetryActive()) { + // The motor driver refreshes the data with every decoded frame. Age it at the serial + // poll rate, so a silent ESC drops out of the combined values the same way (the serial + // poll timer doubles as the aging clock, nothing polls here) + const timeMs_t currentTimeMs = currentTimeUs / 1000; + if (currentTimeMs - escTriggerTimeMs >= ESC_REQUEST_TIMEOUT_MS) { + escTriggerTimeMs = currentTimeMs; + for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) { + if (escSensorData[i].dataAge < ESC_DATA_INVALID) { + escSensorData[i].dataAge++; + escSensorDataNeedsUpdate = true; + } + } + } + return; + } +#endif + #ifdef USE_MOTOR_SRXL2 if (motorConfig()->motorPwmProtocol == PWM_TYPE_SRXL2) { /* One ESC per port, so escSensorData[i] belongs to the i-th assigned @@ -326,6 +403,8 @@ void escSensorUpdate(timeUs_t currentTimeUs) const timeMs_t currentTimeMs = currentTimeUs / 1000; + escSensorFrameWindowUpdate(currentTimeMs); + switch (escSensorState) { case ESC_SENSOR_WAIT_STARTUP: if (currentTimeMs > ESC_BOOTTIME_MS) { @@ -338,6 +417,9 @@ void escSensorUpdate(timeUs_t currentTimeUs) if (!escSensorConfig()->listenOnly) { pwmRequestMotorTelemetry(escSensorMotor); } + // One reply expected per request (per listen window with listenOnly); a timeout + // or a CRC error just never makes it to valid + escSensorFrames[escSensorMotor].total++; bufferPosition = 0; escTriggerTimeMs = currentTimeMs; escSensorState = ESC_SENSOR_WAITING; @@ -378,3 +460,12 @@ void escSensorUpdate(timeUs_t currentTimeUs) } #endif + +#ifndef USE_ESC_SENSOR +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 6bccd340960..702f68938d5 100644 --- a/src/main/sensors/esc_sensor.h +++ b/src/main/sensors/esc_sensor.h @@ -32,6 +32,21 @@ typedef struct { uint32_t rpm; } escSensorData_t; +#define ESC_FRAME_WINDOW_MS 1000 + +// Telemetry link quality of one ESC over fixed back-to-back windows, shared by the serial +// ESC sensor and bidirectional DSHOT (only one source is ever active). total counts every +// reply expected (serial: a missing one too; bidir DSHOT: only replies with edges, as in +// Betaflight), valid those that decoded. The last closed window is kept, so the reported +// value moves once per window; lastWindowTotal == 0 means nothing was expected (boot, a +// source without frame counting such as SRXL2) and lastWindowSuccess is meaningless then +typedef struct { + uint16_t total; + uint16_t valid; + uint16_t lastWindowTotal; + uint8_t lastWindowSuccess; // percent +} escFrameCounter_t; + typedef struct escSensorConfig_s { uint16_t currentOffset; // offset consumed by the flight controller / VTX / cam / ... in mA uint8_t listenOnly; @@ -47,5 +62,11 @@ bool escSensorInitialize(void); void escSensorUpdate(timeUs_t currentTimeUs); escSensorData_t * escSensorGetData(void); escSensorData_t * getEscTelemetry(uint8_t esc); +escFrameCounter_t * escSensorFrameCounter(uint8_t esc); +// Closes the counting window of every ESC once ESC_FRAME_WINDOW_MS have passed; the active +// source calls it from wherever it runs regularly +void escSensorFrameWindowUpdate(timeMs_t currentTimeMs); 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); int getTelemetryMotorCount(void); diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h index 183b44a3908..80629eb05c6 100644 --- a/src/main/target/common_post.h +++ b/src/main/target/common_post.h @@ -185,7 +185,26 @@ extern uint8_t __config_end; #endif #endif -#ifdef USE_ESC_SENSOR +// DSHOT variants, all on top of USE_DSHOT: +// - USE_DSHOT_DMAR: one burst DMA stream per timer (target choice) +// - USE_DSHOT_BIDIR: bidirectional DSHOT (eRPM telemetry on the motor line, +// dshot_bidir_enabled). Needs a DMA stream per motor channel for the direction +// switching, so it excludes USE_DSHOT_DMAR. Implemented by the timer/DMA motor +// driver (pwm_output.c); RP2350 drives its motors from a PIO program that only +// transmits, so the setting is hidden there and stays off. STM32F7 is excluded +// as well: bidirectional DSHOT is only supported on STM32H7 and AT32. +#if defined(USE_DSHOT_DMAR) && !defined(USE_DSHOT) +#error "USE_DSHOT_DMAR requires USE_DSHOT" +#endif +#if defined(USE_DSHOT) && !defined(USE_DSHOT_DMAR) && !defined(RP2350) && !defined(STM32F7) +#define USE_DSHOT_BIDIR +#endif +#if defined(USE_DSHOT_BIDIR) && (!defined(USE_DSHOT) || defined(USE_DSHOT_DMAR)) +#error "USE_DSHOT_BIDIR requires USE_DSHOT and excludes USE_DSHOT_DMAR" +#endif + +// RPM filter sources: serial ESC telemetry or bidirectional DSHOT +#if defined(USE_ESC_SENSOR) || defined(USE_DSHOT_BIDIR) #define USE_RPM_FILTER #endif diff --git a/src/main/telemetry/srxl.c b/src/main/telemetry/srxl.c index ae3dcf9b7f1..cb6e5c750d8 100644 --- a/src/main/telemetry/srxl.c +++ b/src/main/telemetry/srxl.c @@ -64,7 +64,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 +183,14 @@ typedef struct uint16_t getMotorAveragePeriod(void) { -#if defined( USE_ESC_SENSOR_TELEMETRY) || defined( USE_DSHOT_TELEMETRY) +#if defined(USE_ESC_SENSOR) 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