mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-16 04:45:24 +03:00
If DSHOT telemetry is still being received, wait (#12612)
This commit is contained in:
Steve Evans
GitHub
parent
af97415da2
commit
b7c5f84d2f
5 changed files with 130 additions and 46 deletions
|
|
@ -112,4 +112,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
|
|||
"ANGLE_MODE",
|
||||
"ANGLE_TARGET",
|
||||
"CURRENT_ANGLE",
|
||||
"DSHOT_TELEMETRY_COUNTS",
|
||||
};
|
||||
|
|
|
|||
|
|
@ -110,6 +110,7 @@ typedef enum {
|
|||
DEBUG_ANGLE_MODE,
|
||||
DEBUG_ANGLE_TARGET,
|
||||
DEBUG_CURRENT_ANGLE,
|
||||
DEBUG_DSHOT_TELEMETRY_COUNTS,
|
||||
DEBUG_COUNT
|
||||
} debugType_e;
|
||||
|
||||
|
|
|
|||
|
|
@ -1,3 +1,23 @@
|
|||
/*
|
||||
* This file is part of Cleanflight and Betaflight.
|
||||
*
|
||||
* Cleanflight and Betaflight are 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.
|
||||
*
|
||||
* Cleanflight and Betaflight are distributed in the hope that they
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
|
|
@ -8,6 +28,7 @@
|
|||
|
||||
#include "common/maths.h"
|
||||
#include "common/utils.h"
|
||||
#include "build/debug.h"
|
||||
#include "drivers/dshot.h"
|
||||
#include "drivers/dshot_bitbang_decode.h"
|
||||
|
||||
|
|
@ -28,7 +49,8 @@ uint16_t bbBuffer[134];
|
|||
#define BITBAND_SRAM_BASE 0x22000000
|
||||
#define BITBAND_SRAM(a,b) ((BITBAND_SRAM_BASE + (((a)-BITBAND_SRAM_REF)<<5) + ((b)<<2))) // Convert SRAM address
|
||||
|
||||
|
||||
#define DSHOT_TELEMETRY_START_MARGIN 10
|
||||
static uint8_t preambleSkip = 0;
|
||||
|
||||
typedef struct bitBandWord_s {
|
||||
uint32_t value;
|
||||
|
|
@ -84,6 +106,8 @@ static uint32_t decode_bb_value(uint32_t value, uint16_t buffer[], uint32_t coun
|
|||
|
||||
uint32_t decode_bb_bitband( uint16_t buffer[], uint32_t count, uint32_t bit)
|
||||
{
|
||||
uint8_t startMargin;
|
||||
|
||||
#ifdef DEBUG_BBDECODE
|
||||
memset(sequence, 0, sizeof(sequence));
|
||||
sequenceIndex = 0;
|
||||
|
|
@ -94,6 +118,9 @@ uint32_t decode_bb_bitband( uint16_t buffer[], uint32_t count, uint32_t bit)
|
|||
bitBandWord_t* b = p;
|
||||
bitBandWord_t* endP = p + (count - MIN_VALID_BBSAMPLES);
|
||||
|
||||
// Jump forward in the buffer to just before where we anticipate the first zero
|
||||
p += preambleSkip;
|
||||
|
||||
// Eliminate leading high signal level by looking for first zero bit in data stream.
|
||||
// Manual loop unrolling and branch hinting to produce faster code.
|
||||
while (p < endP) {
|
||||
|
|
@ -105,6 +132,9 @@ uint32_t decode_bb_bitband( uint16_t buffer[], uint32_t count, uint32_t bit)
|
|||
}
|
||||
}
|
||||
|
||||
startMargin = p - b;
|
||||
DEBUG_SET(DEBUG_DSHOT_TELEMETRY_COUNTS, 3, startMargin);
|
||||
|
||||
if (p >= endP) {
|
||||
// not returning telemetry is ok if the esc cpu is
|
||||
// overburdened. in that case no edge will be found and
|
||||
|
|
@ -188,6 +218,10 @@ uint32_t decode_bb_bitband( uint16_t buffer[], uint32_t count, uint32_t bit)
|
|||
sequence[sequenceIndex] = sequence[sequenceIndex] + (nlen) * 3;
|
||||
sequenceIndex++;
|
||||
#endif
|
||||
|
||||
// The anticipated edges were observed
|
||||
preambleSkip = startMargin - DSHOT_TELEMETRY_START_MARGIN;
|
||||
|
||||
if (nlen > 0) {
|
||||
value <<= nlen;
|
||||
value |= 1 << (nlen - 1);
|
||||
|
|
@ -198,6 +232,8 @@ uint32_t decode_bb_bitband( uint16_t buffer[], uint32_t count, uint32_t bit)
|
|||
|
||||
FAST_CODE uint32_t decode_bb( uint16_t buffer[], uint32_t count, uint32_t bit)
|
||||
{
|
||||
uint8_t startMargin;
|
||||
|
||||
#ifdef DEBUG_BBDECODE
|
||||
memset(sequence, 0, sizeof(sequence));
|
||||
sequenceIndex = 0;
|
||||
|
|
@ -209,12 +245,15 @@ FAST_CODE uint32_t decode_bb( uint16_t buffer[], uint32_t count, uint32_t bit)
|
|||
memset(sequence, 0, sizeof(sequence));
|
||||
int sequenceIndex = 0;
|
||||
#endif
|
||||
|
||||
uint16_t lastValue = 0;
|
||||
uint32_t value = 0;
|
||||
|
||||
uint16_t* p = buffer;
|
||||
uint16_t* endP = p + count - MIN_VALID_BBSAMPLES;
|
||||
|
||||
// Jump forward in the buffer to just before where we anticipate the first zero
|
||||
p += preambleSkip;
|
||||
|
||||
// Eliminate leading high signal level by looking for first zero bit in data stream.
|
||||
// Manual loop unrolling and branch hinting to produce faster code.
|
||||
while (p < endP) {
|
||||
|
|
@ -226,10 +265,15 @@ FAST_CODE uint32_t decode_bb( uint16_t buffer[], uint32_t count, uint32_t bit)
|
|||
}
|
||||
}
|
||||
|
||||
startMargin = p - buffer;
|
||||
DEBUG_SET(DEBUG_DSHOT_TELEMETRY_COUNTS, 3, startMargin);
|
||||
|
||||
if(*p & mask) {
|
||||
// not returning telemetry is ok if the esc cpu is
|
||||
// overburdened. in that case no edge will be found and
|
||||
// BB_NOEDGE indicates the condition to caller
|
||||
// Increase the start margin
|
||||
preambleSkip--;
|
||||
return DSHOT_TELEMETRY_NOEDGE;
|
||||
}
|
||||
|
||||
|
|
@ -268,6 +312,8 @@ FAST_CODE uint32_t decode_bb( uint16_t buffer[], uint32_t count, uint32_t bit)
|
|||
|
||||
// length of last sequence has to be inferred since the last bit with inverted dshot is high
|
||||
if (bits < 18) {
|
||||
// Increase the start margin
|
||||
preambleSkip--;
|
||||
return DSHOT_TELEMETRY_NOEDGE;
|
||||
}
|
||||
|
||||
|
|
@ -278,9 +324,14 @@ FAST_CODE uint32_t decode_bb( uint16_t buffer[], uint32_t count, uint32_t bit)
|
|||
#endif
|
||||
|
||||
if (nlen < 0) {
|
||||
// Increase the start margin
|
||||
preambleSkip--;
|
||||
return DSHOT_TELEMETRY_NOEDGE;
|
||||
}
|
||||
|
||||
// The anticipated edges were observed
|
||||
preambleSkip = startMargin - DSHOT_TELEMETRY_START_MARGIN;
|
||||
|
||||
if (nlen > 0) {
|
||||
value <<= nlen;
|
||||
value |= 1 << (nlen - 1);
|
||||
|
|
|
|||
|
|
@ -167,6 +167,7 @@ typedef struct bbPort_s {
|
|||
uint16_t *portInputBuffer;
|
||||
uint32_t portInputCount;
|
||||
bool inputActive;
|
||||
volatile bool telemetryPending;
|
||||
|
||||
// Misc
|
||||
#ifdef DEBUG_COUNT_INTERRUPT
|
||||
|
|
|
|||
|
|
@ -47,6 +47,15 @@
|
|||
|
||||
#include "pg/motor.h"
|
||||
|
||||
// DEBUG_DSHOT_TELEMETRY_COUNTS
|
||||
// 0 - Count of telemetry packets read
|
||||
// 1 - Count of missing edge
|
||||
// 2 - Count of reception not complete in time
|
||||
// 3 - Number of high bits before telemetry start
|
||||
|
||||
// Maximum time to wait for telemetry reception to complete
|
||||
#define DSHOT_TELEMETRY_TIMEOUT 2000
|
||||
|
||||
FAST_DATA_ZERO_INIT bbPacer_t bbPacers[MAX_MOTOR_PACERS]; // TIM1 or TIM8
|
||||
FAST_DATA_ZERO_INIT int usedMotorPacers = 0;
|
||||
|
||||
|
|
@ -324,6 +333,7 @@ FAST_IRQ_HANDLER void bbDMAIrqHandler(dmaChannelDescriptor_t *descriptor)
|
|||
#ifdef USE_DSHOT_TELEMETRY
|
||||
if (useDshotTelemetry) {
|
||||
if (bbPort->direction == DSHOT_BITBANG_DIRECTION_INPUT) {
|
||||
bbPort->telemetryPending = false;
|
||||
#ifdef DEBUG_COUNT_INTERRUPT
|
||||
bbPort->inputIrq++;
|
||||
#endif
|
||||
|
|
@ -335,6 +345,7 @@ FAST_IRQ_HANDLER void bbDMAIrqHandler(dmaChannelDescriptor_t *descriptor)
|
|||
// Switch to input
|
||||
|
||||
bbSwitchToInput(bbPort);
|
||||
bbPort->telemetryPending = true;
|
||||
|
||||
bbTIM_DMACmd(bbPort->timhw->tim, bbPort->dmaSource, ENABLE);
|
||||
}
|
||||
|
|
@ -500,62 +511,81 @@ static bool bbUpdateStart(void)
|
|||
#ifdef USE_DSHOT_TELEMETRY_STATS
|
||||
const timeMs_t currentTimeMs = millis();
|
||||
#endif
|
||||
timeUs_t currentUs = micros();
|
||||
|
||||
// don't send while telemetry frames might still be incoming
|
||||
if (cmpTimeUs(currentUs, lastSendUs) < (timeDelta_t)(40 + 2 * dshotFrameUs)) {
|
||||
return false;
|
||||
}
|
||||
// Wait for telemetry reception to complete before decode
|
||||
bool telemetryPending;
|
||||
bool telemetryWait = false;
|
||||
const timeUs_t startTimeUs = micros();
|
||||
|
||||
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
|
||||
do {
|
||||
telemetryPending = false;
|
||||
for (int i = 0; i < usedMotorPorts; i++) {
|
||||
telemetryPending |= bbPorts[i].telemetryPending;
|
||||
}
|
||||
|
||||
telemetryWait |= telemetryPending;
|
||||
|
||||
if (cmpTimeUs(micros(), startTimeUs) > DSHOT_TELEMETRY_TIMEOUT) {
|
||||
return false;
|
||||
}
|
||||
} while (telemetryPending);
|
||||
|
||||
if (telemetryWait) {
|
||||
DEBUG_SET(DEBUG_DSHOT_TELEMETRY_COUNTS, 2, debug[2] + 1);
|
||||
} else {
|
||||
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
|
||||
#ifdef USE_DSHOT_CACHE_MGMT
|
||||
// Only invalidate the buffer once. If all motors are on a common port they'll share a buffer.
|
||||
bool invalidated = false;
|
||||
for (int i = 0; i < motorIndex; i++) {
|
||||
if (bbMotors[motorIndex].bbPort->portInputBuffer == bbMotors[i].bbPort->portInputBuffer) {
|
||||
invalidated = true;
|
||||
// Only invalidate the buffer once. If all motors are on a common port they'll share a buffer.
|
||||
bool invalidated = false;
|
||||
for (int i = 0; i < motorIndex; i++) {
|
||||
if (bbMotors[motorIndex].bbPort->portInputBuffer == bbMotors[i].bbPort->portInputBuffer) {
|
||||
invalidated = true;
|
||||
}
|
||||
}
|
||||
if (!invalidated) {
|
||||
SCB_InvalidateDCache_by_Addr((uint32_t *)bbMotors[motorIndex].bbPort->portInputBuffer,
|
||||
DSHOT_BB_PORT_IP_BUF_CACHE_ALIGN_BYTES);
|
||||
}
|
||||
}
|
||||
if (!invalidated) {
|
||||
SCB_InvalidateDCache_by_Addr((uint32_t *)bbMotors[motorIndex].bbPort->portInputBuffer,
|
||||
DSHOT_BB_PORT_IP_BUF_CACHE_ALIGN_BYTES);
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef STM32F4
|
||||
uint32_t rawValue = decode_bb_bitband(
|
||||
bbMotors[motorIndex].bbPort->portInputBuffer,
|
||||
bbMotors[motorIndex].bbPort->portInputCount - bbDMA_Count(bbMotors[motorIndex].bbPort),
|
||||
bbMotors[motorIndex].pinIndex);
|
||||
uint32_t rawValue = decode_bb_bitband(
|
||||
bbMotors[motorIndex].bbPort->portInputBuffer,
|
||||
bbMotors[motorIndex].bbPort->portInputCount - bbDMA_Count(bbMotors[motorIndex].bbPort),
|
||||
bbMotors[motorIndex].pinIndex);
|
||||
#else
|
||||
uint32_t rawValue = decode_bb(
|
||||
bbMotors[motorIndex].bbPort->portInputBuffer,
|
||||
bbMotors[motorIndex].bbPort->portInputCount - bbDMA_Count(bbMotors[motorIndex].bbPort),
|
||||
bbMotors[motorIndex].pinIndex);
|
||||
uint32_t rawValue = decode_bb(
|
||||
bbMotors[motorIndex].bbPort->portInputBuffer,
|
||||
bbMotors[motorIndex].bbPort->portInputCount - bbDMA_Count(bbMotors[motorIndex].bbPort),
|
||||
bbMotors[motorIndex].pinIndex);
|
||||
#endif
|
||||
if (rawValue == DSHOT_TELEMETRY_NOEDGE) {
|
||||
continue;
|
||||
}
|
||||
dshotTelemetryState.readCount++;
|
||||
|
||||
if (rawValue != DSHOT_TELEMETRY_INVALID) {
|
||||
// Check EDT enable or store raw value
|
||||
if ((rawValue == 0x0E00) && (dshotCommandGetCurrent(motorIndex) == DSHOT_CMD_EXTENDED_TELEMETRY_ENABLE)) {
|
||||
dshotTelemetryState.motorState[motorIndex].telemetryTypes = 1 << DSHOT_TELEMETRY_TYPE_STATE_EVENTS;
|
||||
} else {
|
||||
dshotTelemetryState.motorState[motorIndex].rawValue = rawValue;
|
||||
if (rawValue == DSHOT_TELEMETRY_NOEDGE) {
|
||||
DEBUG_SET(DEBUG_DSHOT_TELEMETRY_COUNTS, 1, debug[1] + 1);
|
||||
continue;
|
||||
}
|
||||
} else {
|
||||
dshotTelemetryState.invalidPacketCount++;
|
||||
}
|
||||
#ifdef USE_DSHOT_TELEMETRY_STATS
|
||||
updateDshotTelemetryQuality(&dshotTelemetryQuality[motorIndex], rawValue != DSHOT_TELEMETRY_INVALID, currentTimeMs);
|
||||
#endif
|
||||
}
|
||||
DEBUG_SET(DEBUG_DSHOT_TELEMETRY_COUNTS, 0, debug[0] + 1);
|
||||
dshotTelemetryState.readCount++;
|
||||
|
||||
dshotTelemetryState.rawValueState = DSHOT_RAW_VALUE_STATE_NOT_PROCESSED;
|
||||
}
|
||||
if (rawValue != DSHOT_TELEMETRY_INVALID) {
|
||||
// Check EDT enable or store raw value
|
||||
if ((rawValue == 0x0E00) && (dshotCommandGetCurrent(motorIndex) == DSHOT_CMD_EXTENDED_TELEMETRY_ENABLE)) {
|
||||
dshotTelemetryState.motorState[motorIndex].telemetryTypes = 1 << DSHOT_TELEMETRY_TYPE_STATE_EVENTS;
|
||||
} else {
|
||||
dshotTelemetryState.motorState[motorIndex].rawValue = rawValue;
|
||||
}
|
||||
} else {
|
||||
dshotTelemetryState.invalidPacketCount++;
|
||||
}
|
||||
#ifdef USE_DSHOT_TELEMETRY_STATS
|
||||
updateDshotTelemetryQuality(&dshotTelemetryQuality[motorIndex], rawValue != DSHOT_TELEMETRY_INVALID, currentTimeMs);
|
||||
#endif
|
||||
}
|
||||
|
||||
dshotTelemetryState.rawValueState = DSHOT_RAW_VALUE_STATE_NOT_PROCESSED;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
for (int i = 0; i < usedMotorPorts; i++) {
|
||||
bbDMA_Cmd(&bbPorts[i], DISABLE);
|
||||
bbOutputDataClear(bbPorts[i].portOutputBuffer);
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue