rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Interrupt/DMA driven SX1280 interaction for ELRS

This commit is contained in:
Steve Evans 2022-02-02 21:05:58 +00:00
parent 7b4415f062
commit fc8640154a
39 changed files with 1003 additions and 438 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/main/drivers/accgyro/accgyro_mpu.c
View File

@ -221,7 +221,7 @@ bool mpuAccReadSPI(accDev_t *acc)
busSegment_t segments[] = {
{.u.buffers = {NULL, NULL}, 7, true, NULL},
{.u.buffers = {NULL, NULL}, 0, true, NULL},
{.u.link = {NULL, NULL}, 0, true, NULL},
};
segments[0].u.buffers.txData = acc->gyro->dev.txBuf;
segments[0].u.buffers.rxData = &acc->gyro->dev.rxBuf[1];
@ -298,7 +298,7 @@ bool mpuGyroReadSPI(gyroDev_t *gyro)
busSegment_t segments[] = {
{.u.buffers = {NULL, NULL}, 7, true, NULL},
{.u.buffers = {NULL, NULL}, 0, true, NULL},
{.u.link = {NULL, NULL}, 0, true, NULL},
};
segments[0].u.buffers.txData = gyro->dev.txBuf;
segments[0].u.buffers.rxData = &gyro->dev.rxBuf[1];

4
src/main/drivers/accgyro/accgyro_spi_bmi270.c
View File

@ -320,7 +320,7 @@ static bool bmi270AccRead(accDev_t *acc)
busSegment_t segments[] = {
{.u.buffers = {NULL, NULL}, 8, true, NULL},
{.u.buffers = {NULL, NULL}, 0, true, NULL},
{.u.link = {NULL, NULL}, 0, true, NULL},
};
segments[0].u.buffers.txData = acc->gyro->dev.txBuf;
segments[0].u.buffers.rxData = acc->gyro->dev.rxBuf;
@ -398,7 +398,7 @@ static bool bmi270GyroReadRegister(gyroDev_t *gyro)
busSegment_t segments[] = {
{.u.buffers = {NULL, NULL}, 8, true, NULL},
{.u.buffers = {NULL, NULL}, 0, true, NULL},
{.u.link = {NULL, NULL}, 0, true, NULL},
};
segments[0].u.buffers.txData = gyro->dev.txBuf;
segments[0].u.buffers.rxData = gyro->dev.rxBuf;

9
src/main/drivers/bus.h
View File

@ -73,7 +73,7 @@ typedef struct busDevice_s {
DMA_InitTypeDef *initRx;
#endif
#endif // UNIT_TEST
struct busSegment_s* volatile curSegment;
volatile struct busSegment_s* volatile curSegment;
bool initSegment;
} busDevice_t;
@ -112,7 +112,10 @@ typedef struct extDevice_s {
} extDevice_t;
/* Each SPI access may comprise multiple parts, for example, wait/write enable/write/data each of which
* is defined by a segment, with optional callback after each is completed
* is defined by a segment, with optional callback after each is completed.
*
* If there are more than one segments, or a single segment with negateCS negated then DMA will be used irrespective of length
*
*/
typedef struct busSegment_s {
union {
@ -126,7 +129,7 @@ typedef struct busSegment_s {
// Link to the device associated with the next transfer
const extDevice_t *dev;
// Segments to process in the next transfer.
struct busSegment_s *segments;
volatile struct busSegment_s *segments;
} link;
} u;
int len;

43
src/main/drivers/bus_spi.c
View File

@ -172,7 +172,7 @@ void spiReadWriteBuf(const extDevice_t *dev, uint8_t *txData, uint8_t *rxData, i
// This routine blocks so no need to use static data
busSegment_t segments[] = {
{.u.buffers = {txData, rxData}, len, true, NULL},
{.u.buffers = {NULL, NULL}, 0, true, NULL},
{.u.link = {NULL, NULL}, 0, true, NULL},
};
spiSequence(dev, &segments[0]);
@ -201,7 +201,7 @@ uint8_t spiReadWrite(const extDevice_t *dev, uint8_t data)
// This routine blocks so no need to use static data
busSegment_t segments[] = {
{.u.buffers = {&data, &retval}, sizeof(data), true, NULL},
{.u.buffers = {NULL, NULL}, 0, true, NULL},
{.u.link = {NULL, NULL}, 0, true, NULL},
};
spiSequence(dev, &segments[0]);
@ -220,7 +220,7 @@ uint8_t spiReadWriteReg(const extDevice_t *dev, uint8_t reg, uint8_t data)
busSegment_t segments[] = {
{.u.buffers = {&reg, NULL}, sizeof(reg), false, NULL},
{.u.buffers = {&data, &retval}, sizeof(data), true, NULL},
{.u.buffers = {NULL, NULL}, 0, true, NULL},
{.u.link = {NULL, NULL}, 0, true, NULL},
};
spiSequence(dev, &segments[0]);
@ -236,7 +236,7 @@ void spiWrite(const extDevice_t *dev, uint8_t data)
// This routine blocks so no need to use static data
busSegment_t segments[] = {
{.u.buffers = {&data, NULL}, sizeof(data), true, NULL},
{.u.buffers = {NULL, NULL}, 0, true, NULL},
{.u.link = {NULL, NULL}, 0, true, NULL},
};
spiSequence(dev, &segments[0]);
@ -251,7 +251,7 @@ void spiWriteReg(const extDevice_t *dev, uint8_t reg, uint8_t data)
busSegment_t segments[] = {
{.u.buffers = {&reg, NULL}, sizeof(reg), false, NULL},
{.u.buffers = {&data, NULL}, sizeof(data), true, NULL},
{.u.buffers = {NULL, NULL}, 0, true, NULL},
{.u.link = {NULL, NULL}, 0, true, NULL},
};
spiSequence(dev, &segments[0]);
@ -279,7 +279,7 @@ void spiReadRegBuf(const extDevice_t *dev, uint8_t reg, uint8_t *data, uint8_t l
busSegment_t segments[] = {
{.u.buffers = {&reg, NULL}, sizeof(reg), false, NULL},
{.u.buffers = {NULL, data}, length, true, NULL},
{.u.buffers = {NULL, NULL}, 0, true, NULL},
{.u.link = {NULL, NULL}, 0, true, NULL},
};
spiSequence(dev, &segments[0]);
@ -313,7 +313,7 @@ void spiWriteRegBuf(const extDevice_t *dev, uint8_t reg, uint8_t *data, uint32_t
busSegment_t segments[] = {
{.u.buffers = {&reg, NULL}, sizeof(reg), false, NULL},
{.u.buffers = {data, NULL}, length, true, NULL},
{.u.buffers = {NULL, NULL}, 0, true, NULL},
{.u.link = {NULL, NULL}, 0, true, NULL},
};
spiSequence(dev, &segments[0]);
@ -329,7 +329,7 @@ uint8_t spiReadReg(const extDevice_t *dev, uint8_t reg)
busSegment_t segments[] = {
{.u.buffers = {&reg, NULL}, sizeof(reg), false, NULL},
{.u.buffers = {NULL, &data}, sizeof(data), true, NULL},
{.u.buffers = {NULL, NULL}, 0, true, NULL},
{.u.link = {NULL, NULL}, 0, true, NULL},
};
spiSequence(dev, &segments[0]);
@ -404,13 +404,19 @@ static void spiIrqHandler(const extDevice_t *dev)
}
// Advance through the segment list
nextSegment = bus->curSegment + 1;
// OK to discard the volatile qualifier here
nextSegment = (busSegment_t *)bus->curSegment + 1;
if (nextSegment->len == 0) {
if (!bus->curSegment->negateCS) {
// Negate Chip Select if not done so already
IOHi(dev->busType_u.spi.csnPin);
}
// If a following transaction has been linked, start it
if (nextSegment->u.link.dev) {
const extDevice_t *nextDev = nextSegment->u.link.dev;
busSegment_t *nextSegments = nextSegment->u.link.segments;
busSegment_t *nextSegments = (busSegment_t *)nextSegment->u.link.segments;
// The end of the segment list has been reached
bus->curSegment = nextSegments;
nextSegment->u.link.dev = NULL;
@ -420,6 +426,9 @@ static void spiIrqHandler(const extDevice_t *dev)
bus->curSegment = (busSegment_t *)BUS_SPI_FREE;
}
} else {
// Do as much processing as possible before asserting CS to avoid violating minimum high time
bool negateCS = bus->curSegment->negateCS;
bus->curSegment = nextSegment;
// After the completion of the first segment setup the init structure for the subsequent segment
@ -428,6 +437,11 @@ static void spiIrqHandler(const extDevice_t *dev)
bus->initSegment = false;
}
if (negateCS) {
// Assert Chip Select - it's costly so only do so if necessary
IOLo(dev->busType_u.spi.csnPin);
}
// Launch the next transfer
spiInternalStartDMA(dev);
@ -713,10 +727,11 @@ void spiSequence(const extDevice_t *dev, busSegment_t *segments)
// Defer this transfer to be triggered upon completion of the current transfer
// Find the last segment of the current transfer
// Find the last segment of the new transfer
for (endSegment = segments; endSegment->len; endSegment++);
busSegment_t *endCmpSegment = bus->curSegment;
// Safe to discard the volatile qualifier as we're in an atomic block
busSegment_t *endCmpSegment = (busSegment_t *)bus->curSegment;
if (endCmpSegment) {
while (true) {
@ -736,7 +751,8 @@ void spiSequence(const extDevice_t *dev, busSegment_t *segments)
break;
} else {
// Follow the link to the next queued segment list
endCmpSegment = endCmpSegment->u.link.segments;
endCmpSegment = (busSegment_t *)endCmpSegment->u.link.segments;
}
}
}
@ -745,7 +761,6 @@ void spiSequence(const extDevice_t *dev, busSegment_t *segments)
endCmpSegment->u.link.segments = segments;
return;
}
} else {
// Claim the bus with this list of segments
bus->curSegment = segments;

29
src/main/drivers/bus_spi_ll.c
View File

@ -290,7 +290,7 @@ void spiInternalInitStream(const extDevice_t *dev, bool preInit)
STATIC_DMA_DATA_AUTO uint8_t dummyRxByte;
busDevice_t *bus = dev->bus;
busSegment_t *segment = bus->curSegment;
busSegment_t *segment = (busSegment_t *)bus->curSegment;
if (preInit) {
// Prepare the init structure for the next segment to reduce inter-segment interval
@ -368,9 +368,6 @@ void spiInternalStartDMA(const extDevice_t *dev)
{
busDevice_t *bus = dev->bus;
// Assert Chip Select
IOLo(dev->busType_u.spi.csnPin);
dmaChannelDescriptor_t *dmaTx = bus->dmaTx;
dmaChannelDescriptor_t *dmaRx = bus->dmaRx;
@ -584,7 +581,7 @@ void spiSequenceStart(const extDevice_t *dev)
*/
// Check that any reads are cache aligned and of multiple cache lines in length
for (busSegment_t *checkSegment = bus->curSegment; checkSegment->len; checkSegment++) {
for (busSegment_t *checkSegment = (busSegment_t *)bus->curSegment; checkSegment->len; checkSegment++) {
// Check there is no receive data as only transmit DMA is available
if ((checkSegment->u.buffers.rxData) && (bus->dmaRx == (dmaChannelDescriptor_t *)NULL)) {
dmaSafe = false;
@ -635,17 +632,25 @@ void spiSequenceStart(const extDevice_t *dev)
}
// Use DMA if possible
if (bus->useDMA && dmaSafe && ((segmentCount > 1) || (xferLen >= 8))) {
// If there are more than one segments, or a single segment with negateCS negated then force DMA irrespective of length
if (bus->useDMA && dmaSafe && ((segmentCount > 1) || (xferLen >= 8) || !bus->curSegment->negateCS)) {
// Intialise the init structures for the first transfer
spiInternalInitStream(dev, false);
// Assert Chip Select
IOLo(dev->busType_u.spi.csnPin);
// Start the transfers
spiInternalStartDMA(dev);
} else {
busSegment_t *lastSegment = NULL;
// Manually work through the segment list performing a transfer for each
while (bus->curSegment->len) {
// Assert Chip Select
if (!lastSegment || lastSegment->negateCS) {
// Assert Chip Select if necessary - it's costly so only do so if necessary
IOLo(dev->busType_u.spi.csnPin);
}
spiInternalReadWriteBufPolled(
bus->busType_u.spi.instance,
@ -675,14 +680,20 @@ void spiSequenceStart(const extDevice_t *dev)
break;
}
}
lastSegment = (busSegment_t *)bus->curSegment;
bus->curSegment++;
}
if (lastSegment && !lastSegment->negateCS) {
// Negate Chip Select if not done so already
IOHi(dev->busType_u.spi.csnPin);
}
// If a following transaction has been linked, start it
if (bus->curSegment->u.link.dev) {
const extDevice_t *nextDev = bus->curSegment->u.link.dev;
busSegment_t *nextSegments = bus->curSegment->u.link.segments;
busSegment_t *endSegment = bus->curSegment;
busSegment_t *nextSegments = (busSegment_t *)bus->curSegment->u.link.segments;
busSegment_t *endSegment = (busSegment_t *)bus->curSegment;
bus->curSegment = nextSegments;
endSegment->u.link.dev = NULL;
spiSequenceStart(nextDev);

27
src/main/drivers/bus_spi_stdperiph.c
View File

@ -215,9 +215,6 @@ void spiInternalInitStream(const extDevice_t *dev, bool preInit)
void spiInternalStartDMA(const extDevice_t *dev)
{
// Assert Chip Select
IOLo(dev->busType_u.spi.csnPin);
dmaChannelDescriptor_t *dmaTx = dev->bus->dmaTx;
dmaChannelDescriptor_t *dmaRx = dev->bus->dmaRx;
DMA_Stream_TypeDef *streamRegsTx = (DMA_Stream_TypeDef *)dmaTx->ref;
@ -355,7 +352,7 @@ void spiSequenceStart(const extDevice_t *dev)
SPI_Cmd(instance, ENABLE);
// Check that any there are no attempts to DMA to/from CCD SRAM
for (busSegment_t *checkSegment = bus->curSegment; checkSegment->len; checkSegment++) {
for (busSegment_t *checkSegment = (busSegment_t *)bus->curSegment; checkSegment->len; checkSegment++) {
// Check there is no receive data as only transmit DMA is available
if (((checkSegment->u.buffers.rxData) && (IS_CCM(checkSegment->u.buffers.rxData) || (bus->dmaRx == (dmaChannelDescriptor_t *)NULL))) ||
((checkSegment->u.buffers.txData) && IS_CCM(checkSegment->u.buffers.txData))) {
@ -367,17 +364,25 @@ void spiSequenceStart(const extDevice_t *dev)
xferLen += checkSegment->len;
}
// Use DMA if possible
if (bus->useDMA && dmaSafe && ((segmentCount > 1) || (xferLen >= 8))) {
// If there are more than one segments, or a single segment with negateCS negated then force DMA irrespective of length
if (bus->useDMA && dmaSafe && ((segmentCount > 1) || (xferLen >= 8) || !bus->curSegment->negateCS)) {
// Intialise the init structures for the first transfer
spiInternalInitStream(dev, false);
// Assert Chip Select
IOLo(dev->busType_u.spi.csnPin);
// Start the transfers
spiInternalStartDMA(dev);
} else {
busSegment_t *lastSegment = NULL;
// Manually work through the segment list performing a transfer for each
while (bus->curSegment->len) {
// Assert Chip Select
if (!lastSegment || lastSegment->negateCS) {
// Assert Chip Select if necessary - it's costly so only do so if necessary
IOLo(dev->busType_u.spi.csnPin);
}
spiInternalReadWriteBufPolled(
bus->busType_u.spi.instance,
@ -407,14 +412,20 @@ void spiSequenceStart(const extDevice_t *dev)
break;
}
}
lastSegment = (busSegment_t *)bus->curSegment;
bus->curSegment++;
}
if (lastSegment && !lastSegment->negateCS) {
// Negate Chip Select if not done so already
IOHi(dev->busType_u.spi.csnPin);
}
// If a following transaction has been linked, start it
if (bus->curSegment->u.link.dev) {
const extDevice_t *nextDev = bus->curSegment->u.link.dev;
busSegment_t *nextSegments = bus->curSegment->u.link.segments;
busSegment_t *endSegment = bus->curSegment;
busSegment_t *nextSegments = (busSegment_t *)bus->curSegment->u.link.segments;
busSegment_t *endSegment = (busSegment_t *)bus->curSegment;
bus->curSegment = nextSegments;
endSegment->u.link.dev = NULL;
spiSequenceStart(nextDev);

12
src/main/drivers/flash_m25p16.c
View File

@ -284,7 +284,7 @@ static void m25p16_eraseSector(flashDevice_t *fdevice, uint32_t address)
{.u.buffers = {readStatus, readyStatus}, sizeof(readStatus), true, m25p16_callbackReady},
{.u.buffers = {writeEnable, NULL}, sizeof(writeEnable), true, m25p16_callbackWriteEnable},
{.u.buffers = {sectorErase, NULL}, fdevice->isLargeFlash ? 5 : 4, true, NULL},
{.u.buffers = {NULL, NULL}, 0, true, NULL},
{.u.link = {NULL, NULL}, 0, true, NULL},
};
// Ensure any prior DMA has completed before continuing
@ -309,7 +309,7 @@ static void m25p16_eraseCompletely(flashDevice_t *fdevice)
{.u.buffers = {readStatus, readyStatus}, sizeof(readStatus), true, m25p16_callbackReady},
{.u.buffers = {writeEnable, NULL}, sizeof(writeEnable), true, m25p16_callbackWriteEnable},
{.u.buffers = {bulkErase, NULL}, sizeof(bulkErase), true, NULL},
{.u.buffers = {NULL, NULL}, 0, true, NULL},
{.u.link = {NULL, NULL}, 0, true, NULL},
};
spiSequence(fdevice->io.handle.dev, segments);
@ -337,9 +337,9 @@ static uint32_t m25p16_pageProgramContinue(flashDevice_t *fdevice, uint8_t const
{.u.buffers = {readStatus, readyStatus}, sizeof(readStatus), true, m25p16_callbackReady},
{.u.buffers = {writeEnable, NULL}, sizeof(writeEnable), true, m25p16_callbackWriteEnable},
{.u.buffers = {pageProgram, NULL}, 0, false, NULL},
{.u.buffers = {NULL, NULL}, 0, true, NULL},
{.u.buffers = {NULL, NULL}, 0, true, NULL},
{.u.buffers = {NULL, NULL}, 0, true, NULL},
{.u.link = {NULL, NULL}, 0, true, NULL},
{.u.link = {NULL, NULL}, 0, true, NULL},
{.u.link = {NULL, NULL}, 0, true, NULL},
};
// Ensure any prior DMA has completed before continuing
@ -430,7 +430,7 @@ static int m25p16_readBytes(flashDevice_t *fdevice, uint32_t address, uint8_t *b
{.u.buffers = {readStatus, readyStatus}, sizeof(readStatus), true, m25p16_callbackReady},
{.u.buffers = {readBytes, NULL}, fdevice->isLargeFlash ? 5 : 4, false, NULL},
{.u.buffers = {NULL, buffer}, length, true, NULL},
{.u.buffers = {NULL, NULL}, 0, true, NULL},
{.u.link = {NULL, NULL}, 0, true, NULL},
};
// Patch the readBytes command

2
src/main/drivers/flash_w25m.c
View File

@ -74,7 +74,7 @@ static void w25m_dieSelect(const extDevice_t *dev, int die)
busSegment_t segments[] = {
{.u.buffers = {command, NULL}, sizeof(command), true, NULL},
{.u.buffers = {NULL, NULL}, 0, true, NULL},
{.u.link = {NULL, NULL}, 0, true, NULL},
};
// Ensure any prior DMA has completed before continuing

20
src/main/drivers/flash_w25n01g.c
View File

@ -151,7 +151,7 @@ static void w25n01g_performOneByteCommand(flashDeviceIO_t *io, uint8_t command)
busSegment_t segments[] = {
{.u.buffers = {&command, NULL}, sizeof(command), true, NULL},
{.u.buffers = {NULL, NULL}, 0, true, NULL},
{.u.link = {NULL, NULL}, 0, true, NULL},
};
spiSequence(dev, &segments[0]);
@ -176,7 +176,7 @@ static void w25n01g_performCommandWithPageAddress(flashDeviceIO_t *io, uint8_t c
busSegment_t segments[] = {
{.u.buffers = {cmd, NULL}, sizeof(cmd), true, NULL},
{.u.buffers = {NULL, NULL}, 0, true, NULL},
{.u.link = {NULL, NULL}, 0, true, NULL},
};
spiSequence(dev, &segments[0]);
@ -203,7 +203,7 @@ static uint8_t w25n01g_readRegister(flashDeviceIO_t *io, uint8_t reg)
busSegment_t segments[] = {
{.u.buffers = {cmd, in}, sizeof(cmd), true, NULL},
{.u.buffers = {NULL, NULL}, 0, true, NULL},
{.u.link = {NULL, NULL}, 0, true, NULL},
};
// Ensure any prior DMA has completed before continuing
@ -238,7 +238,7 @@ static void w25n01g_writeRegister(flashDeviceIO_t *io, uint8_t reg, uint8_t data
busSegment_t segments[] = {
{.u.buffers = {cmd, NULL}, sizeof(cmd), true, NULL},
{.u.buffers = {NULL, NULL}, 0, true, NULL},
{.u.link = {NULL, NULL}, 0, true, NULL},
};
// Ensure any prior DMA has completed before continuing
@ -410,7 +410,7 @@ static void w25n01g_programDataLoad(flashDevice_t *fdevice, uint16_t columnAddre
busSegment_t segments[] = {
{.u.buffers = {cmd, NULL}, sizeof(cmd), false, NULL},
{.u.buffers = {(uint8_t *)data, NULL}, length, true, NULL},
{.u.buffers = {NULL, NULL}, 0, true, NULL},
{.u.link = {NULL, NULL}, 0, true, NULL},
};
spiSequence(dev, &segments[0]);
@ -441,7 +441,7 @@ static void w25n01g_randomProgramDataLoad(flashDevice_t *fdevice, uint16_t colum
busSegment_t segments[] = {
{.u.buffers = {cmd, NULL}, sizeof(cmd), false, NULL},
{.u.buffers = {(uint8_t *)data, NULL}, length, true, NULL},
{.u.buffers = {NULL, NULL}, 0, true, NULL},
{.u.link = {NULL, NULL}, 0, true, NULL},
};
spiSequence(dev, &segments[0]);
@ -684,7 +684,7 @@ int w25n01g_readBytes(flashDevice_t *fdevice, uint32_t address, uint8_t *buffer,
busSegment_t segments[] = {
{.u.buffers = {cmd, NULL}, sizeof(cmd), false, NULL},
{.u.buffers = {NULL, buffer}, length, true, NULL},
{.u.buffers = {NULL, NULL}, 0, true, NULL},
{.u.link = {NULL, NULL}, 0, true, NULL},
};
spiSequence(dev, &segments[0]);
@ -749,7 +749,7 @@ int w25n01g_readExtensionBytes(flashDevice_t *fdevice, uint32_t address, uint8_t
busSegment_t segments[] = {
{.u.buffers = {cmd, NULL}, sizeof(cmd), false, NULL},
{.u.buffers = {NULL, buffer}, length, true, NULL},
{.u.buffers = {NULL, NULL}, 0, true, NULL},
{.u.link = {NULL, NULL}, 0, true, NULL},
};
// Ensure any prior DMA has completed before continuing
@ -849,7 +849,7 @@ void w25n01g_readBBLUT(flashDevice_t *fdevice, bblut_t *bblut, int lutsize)
busSegment_t segments[] = {
{.u.buffers = {cmd, NULL}, sizeof(cmd), false, NULL},
{.u.buffers = {NULL, in}, sizeof(in), true, w25n01g_readBBLUTCallback},
{.u.buffers = {NULL, NULL}, 0, true, NULL},
{.u.link = {NULL, NULL}, 0, true, NULL},
};
spiSequence(dev, &segments[0]);
@ -888,7 +888,7 @@ void w25n01g_writeBBLUT(flashDevice_t *fdevice, uint16_t lba, uint16_t pba)
busSegment_t segments[] = {
{.u.buffers = {cmd, NULL}, sizeof(cmd), true, NULL},
{.u.buffers = {NULL, NULL}, 0, true, NULL},
{.u.link = {NULL, NULL}, 0, true, NULL},
};
// Ensure any prior DMA has completed before continuing

4
src/main/drivers/max7456.c
View File

@ -616,8 +616,8 @@ bool max7456DrawScreen(void)
static uint16_t pos = 0;
// This routine doesn't block so need to use static data
static busSegment_t segments[] = {
{.u.buffers = {NULL, NULL}, 0, true, NULL},
{.u.buffers = {NULL, NULL}, 0, true, NULL},
{.u.link = {NULL, NULL}, 0, true, NULL},
{.u.link = {NULL, NULL}, 0, true, NULL},
};
if (!fontIsLoading) {

3
src/main/drivers/nvic.h
View File

@ -28,6 +28,8 @@
#define NVIC_PRIO_SONAR_EXTI NVIC_BUILD_PRIORITY(2, 0) // maybe increase slightly
#define NVIC_PRIO_DSHOT_DMA NVIC_BUILD_PRIORITY(2, 1)
#define NVIC_PRIO_TRANSPONDER_DMA NVIC_BUILD_PRIORITY(3, 0)
#define NVIC_PRIO_RX_INT_EXTI NVIC_BUILD_PRIORITY(0x0f, 0x0f)
#define NVIC_PRIO_RX_BUSY_EXTI NVIC_BUILD_PRIORITY(3, 0)
#define NVIC_PRIO_MPU_INT_EXTI NVIC_BUILD_PRIORITY(0x0f, 0x0f)
#define NVIC_PRIO_MAG_INT_EXTI NVIC_BUILD_PRIORITY(0x0f, 0x0f)
#define NVIC_PRIO_RX_SPI_INT_EXTI NVIC_BUILD_PRIORITY(0x0f, 0x0f)
@ -74,7 +76,6 @@
#define NVIC_PRIO_MPU_DATA_READY NVIC_BUILD_PRIORITY(0, 1)
#define NVIC_PRIO_MAG_DATA_READY NVIC_BUILD_PRIORITY(0x0f, 0x0f)
#define NVIC_PRIO_CALLBACK NVIC_BUILD_PRIORITY(0x0f, 0x0f)
#define NVIC_PRIO_MAX7456_DMA NVIC_BUILD_PRIORITY(3, 0)
#define NVIC_PRIO_SPI_DMA NVIC_BUILD_PRIORITY(0, 0)
#define NVIC_PRIO_SDIO_DMA NVIC_BUILD_PRIORITY(0, 0)

31
src/main/drivers/rx/rx_spi.c
View File

@ -43,6 +43,11 @@
#include "rx_spi.h"
#ifdef USE_RX_EXPRESSLRS
#include "rx/rx_spi.h"
#include "rx/expresslrs.h"
#endif
// 13.5 MHz max SPI frequency
#define RX_MAX_SPI_CLK_HZ 13500000
// 6.5 MHz max SPI frequency during startup
@ -52,14 +57,19 @@ static extDevice_t rxSpiDevice;
static extDevice_t *dev = &rxSpiDevice;
static IO_t extiPin = IO_NONE;
static extiCallbackRec_t rxSpiExtiCallbackRec;
static bool extiLevel = true;
static extiCallbackRec_t rxSpiExtiCallbackRec;
static volatile bool extiHasOccurred = false;
static volatile timeUs_t lastExtiTimeUs = 0;
static uint32_t spiNormalSpeedMhz = RX_MAX_SPI_CLK_HZ;
extDevice_t *rxSpiGetDevice(void)
{
return dev;
}
void rxSpiDevicePreInit(const rxSpiConfig_t *rxSpiConfig)
{
spiPreinitRegister(rxSpiConfig->csnTag, IOCFG_IPU, 1);
@ -69,12 +79,12 @@ void rxSpiExtiHandler(extiCallbackRec_t* callback)
{
UNUSED(callback);
const timeUs_t extiTimeUs = microsISR();
if (IORead(extiPin) == extiLevel) {
lastExtiTimeUs = extiTimeUs;
lastExtiTimeUs = microsISR();
extiHasOccurred = true;
}
#ifdef USE_RX_EXPRESSLRS
expressLrsISR(true);
#endif
}
void rxSpiSetNormalSpeedMhz(uint32_t mhz)
@ -121,12 +131,19 @@ bool rxSpiDeviceInit(const rxSpiConfig_t *rxSpiConfig)
void rxSpiExtiInit(ioConfig_t rxSpiExtiPinConfig, extiTrigger_t rxSpiExtiPinTrigger)
{
if (extiPin) {
// Use interrupts on the EXTI pin
if (rxSpiExtiPinTrigger == BETAFLIGHT_EXTI_TRIGGER_FALLING) {
extiLevel = false;
}
EXTIHandlerInit(&rxSpiExtiCallbackRec, rxSpiExtiHandler);
EXTIConfig(extiPin, &rxSpiExtiCallbackRec, NVIC_PRIO_RX_SPI_INT_EXTI, rxSpiExtiPinConfig, rxSpiExtiPinTrigger);
EXTIConfig(extiPin, &rxSpiExtiCallbackRec, NVIC_PRIO_RX_INT_EXTI, rxSpiExtiPinConfig, rxSpiExtiPinTrigger);
EXTIEnable(extiPin, true);
// Check that we've not missed the rising edge on the interrupt line
if (rxSpiGetExtiState()) {
rxSpiExtiHandler(NULL);
}
}
}

3
src/main/drivers/rx/rx_spi.h
View File

@ -24,12 +24,14 @@
#include "common/time.h"
#include "drivers/bus.h"
#include "drivers/exti.h"
#define RX_SPI_MAX_PAYLOAD_SIZE 35
struct rxSpiConfig_s;
extDevice_t *rxSpiGetDevice(void);
void rxSpiDevicePreInit(const struct rxSpiConfig_s *rxSpiConfig);
bool rxSpiDeviceInit(const struct rxSpiConfig_s *rxSpiConfig);
void rxSpiSetNormalSpeedMhz(uint32_t mhz);
@ -43,6 +45,7 @@ void rxSpiWriteCommandMulti(uint8_t command, const uint8_t *data, uint8_t length
uint8_t rxSpiReadCommand(uint8_t command, uint8_t commandData);
void rxSpiReadCommandMulti(uint8_t command, uint8_t commandData, uint8_t *retData, uint8_t length);
void rxSpiExtiInit(ioConfig_t rxSpiExtiPinConfig, extiTrigger_t rxSpiExtiPinTrigger);
void rxSpiEnableExti(void);
bool rxSpiExtiConfigured(void);
bool rxSpiGetExtiState(void);
bool rxSpiPollExti(void);

625
src/main/drivers/rx/rx_sx1280.c
View File

@ -33,6 +33,7 @@
#ifdef USE_RX_SX1280
#include "build/atomic.h"
#include "build/debug.h"
#include "drivers/bus_spi.h"
#include "drivers/io.h"
@ -42,10 +43,35 @@
#include "drivers/rx/rx_spi.h"
#include "drivers/time.h"
#define SX1280_MAX_SPI_MHZ 10000000
#include "rx/rx_spi.h"
#include "rx/expresslrs.h"
#include "rx/expresslrs_common.h"
#include "rx/expresslrs_impl.h"
#define SX1280_MAX_SPI_MHZ 18000000
// The following global variables are accessed from interrupt context to process the sequence of steps in packet processing
// As there is only ever one device, no need to add a device context; globals will do
static volatile dioReason_e irqReason; // Used to pass irq status from sx1280IrqStatusRead() to sx1280ProcessIrq()
static volatile uint8_t packetStats[2];
static volatile uint8_t FIFOaddr; // Used to pass data from sx1280GotFIFOAddr() to sx1280DoReadBuffer()
static IO_t busy;
typedef struct busyIntContext_s {
extiCallbackRec_t exti;
} busyIntContext_t;
static busyIntContext_t busyIntContext;
static volatile timeUs_t sx1280Processing;
static volatile bool pendingISR = false;
static volatile bool pendingDoFHSS = false;
#define SX1280_BUSY_TIMEOUT_US 1000
bool sx1280IsBusy(void)
{
return IORead(busy);
@ -55,7 +81,7 @@ static bool sx1280PollBusy(void)
{
uint32_t startTime = micros();
while (IORead(busy)) {
if ((micros() - startTime) > 1000) {
if ((micros() - startTime) > SX1280_BUSY_TIMEOUT_US) {
return false;
} else {
__asm__("nop");
@ -64,9 +90,108 @@ static bool sx1280PollBusy(void)
return true;
}
static bool sx1280MarkBusy(void)
{
// Check that there isn't already a sequence of accesses to the SX1280 in progress
ATOMIC_BLOCK(NVIC_PRIO_MAX) {
if (sx1280Processing) {
return false;
}
sx1280Processing = micros();
}
return true;
}
static void sx1280ClearBusyFn(void)
{
EXTIEnable(busy, false);
}
// Switch to waiting for busy interrupt
static bool sx1280EnableBusy(void)
{
if (!sx1280MarkBusy()) {
return false;
}
/* Ensure BUSY EXTI is enabled
*
* This is needed because the BETAFPV F4SX1280 target defines the following resources which cannot be
* simultaneously used with the EXTI15_10_IRQHandler. Fortunately we can enable RX_SPI_EXTI until an
* interrupt is received, then enable RX_SPI_EXPRESSLRS_BUSY with the call below until data transfers
* are complete and then switch back with a call to sx1280EnableExti().
*
* resource RX_SPI_EXTI 1 C13
* resource RX_SPI_EXPRESSLRS_BUSY 1 A13
*
*/
EXTIConfig(busy, &busyIntContext.exti, NVIC_PRIO_RX_BUSY_EXTI, IOCFG_IN_FLOATING, BETAFLIGHT_EXTI_TRIGGER_FALLING);
return true;
}
// waitingFn() must call sx1280ClearBusyFn() to prevent repeated calls
static void sx1280SetBusyFn(extiHandlerCallback *waitingFn)
{
bool sx1280Busy;
ATOMIC_BLOCK(NVIC_PRIO_RX_BUSY_EXTI) {
sx1280Busy = IORead(busy);
if (sx1280Busy) {
EXTIHandlerInit(&busyIntContext.exti, waitingFn);
EXTIEnable(busy, true);
} else {
EXTIEnable(busy, false);
}
}
if (!sx1280Busy) {
waitingFn(&busyIntContext.exti);
}
}
static void sx1280MarkFree(void)
{
// Mark that current sequence of accesses is concluded
sx1280Processing = (timeUs_t)0;
}
// Switch to waiting for EXTI interrupt
static void sx1280EnableExti(void)
{
sx1280MarkFree();
rxSpiEnableExti();
}
// Unlikely as it is for the code to lock up waiting on a busy SX1280, we can't afford the risk
// If this routine is called twice in succession whilst waiting on the same busy, force the code to advance
// Called from the Tick timer
bool sx1280HandleFromTick(void)
{
// Grab a copy to prevent a race condition
timeUs_t startTime = sx1280Processing;
if (startTime) {
// No operation should take SX1280_BUSY_TIMEOUT_US us
if (cmpTimeUs(micros(), startTime) > SX1280_BUSY_TIMEOUT_US) {
// Brute force abandon the current sequence of operations
sx1280ClearBusyFn();
// Renable EXTI
sx1280EnableExti();
return true;
}
}
return false;
}
bool sx1280Init(IO_t resetPin, IO_t busyPin)
{
if (!rxSpiExtiConfigured()) {
return false;
}
@ -83,7 +208,6 @@ bool sx1280Init(IO_t resetPin, IO_t busyPin)
if (busyPin) {
IOInit(busyPin, OWNER_RX_SPI_EXPRESSLRS_BUSY, 0);
IOConfigGPIO(busyPin, IOCFG_IPU);
} else {
busyPin = IO_NONE;
}
@ -101,33 +225,13 @@ bool sx1280Init(IO_t resetPin, IO_t busyPin)
return false;
}
// Record the dev pointer for callbacks
extDevice_t *dev = rxSpiGetDevice();
dev->callbackArg = (uint32_t)dev;
return true;
}
uint8_t sx1280ISR(timeUs_t *timeStamp)
{
bool extiTriggered = false;
timeUs_t extiTimestamp;
ATOMIC_BLOCK(NVIC_PRIO_RX_SPI_INT_EXTI) {
// prevent a data-race that can occur if a new EXTI ISR occurs during this block.
extiTriggered = rxSpiPollExti();
extiTimestamp = rxSpiGetLastExtiTimeUs();
if (extiTriggered) {
rxSpiResetExti();
}
}
if (extiTriggered) {
uint8_t irqReason = sx1280GetIrqReason();
if (extiTimestamp) {
*timeStamp = extiTimestamp;
}
return irqReason;
}
return 0;
}
void sx1280WriteCommand(const uint8_t address, const uint8_t data)
{
sx1280PollBusy();
@ -242,8 +346,8 @@ void sx1280ConfigLoraDefaults(void)
sx1280WriteCommand(SX1280_RADIO_SET_AUTOFS, 0x01); //enable auto FS
sx1280WriteRegister(0x0891, (sx1280ReadRegister(0x0891) | 0xC0)); //default is low power mode, switch to high sensitivity instead
sx1280SetPacketParams(12, SX1280_LORA_PACKET_IMPLICIT, 8, SX1280_LORA_CRC_OFF, SX1280_LORA_IQ_NORMAL); //default params
sx1280SetFrequencyHZ(2400000000); //Step 3: Set Freq
sx1280SetFIFOaddr(0x00, 0x00); //Step 4: Config FIFO addr
sx1280SetFrequencyReg(fhssGetInitialFreq(0)); //Step 3: Set Freq
sx1280SetFifoAddr(0x00, 0x00); //Step 4: Config FIFO addr
sx1280SetDioIrqParams(SX1280_IRQ_RADIO_ALL, SX1280_IRQ_TX_DONE | SX1280_IRQ_RX_DONE, SX1280_IRQ_RADIO_NONE, SX1280_IRQ_RADIO_NONE); //set IRQ to both RXdone/TXdone on DIO1
}
@ -352,25 +456,13 @@ void sx1280ConfigLoraModParams(const sx1280LoraBandwidths_e bw, const sx1280Lora
}
}
void sx1280SetFrequencyHZ(const uint32_t reqFreq)
void sx1280SetFrequencyReg(const uint32_t freqReg)
{
uint8_t buf[3] = {0};
uint32_t freq = (uint32_t)(reqFreq / SX1280_FREQ_STEP);
buf[0] = (uint8_t)((freq >> 16) & 0xFF);
buf[1] = (uint8_t)((freq >> 8) & 0xFF);
buf[2] = (uint8_t)(freq & 0xFF);
sx1280WriteCommandBurst(SX1280_RADIO_SET_RFFREQUENCY, buf, 3);
}
void sx1280SetFrequencyReg(const uint32_t freq)
{
uint8_t buf[3] = {0};
buf[0] = (uint8_t)((freq >> 16) & 0xFF);
buf[1] = (uint8_t)((freq >> 8) & 0xFF);
buf[2] = (uint8_t)(freq & 0xFF);
buf[0] = (uint8_t)((freqReg >> 16) & 0xFF);
buf[1] = (uint8_t)((freqReg >> 8) & 0xFF);
buf[2] = (uint8_t)(freqReg & 0xFF);
sx1280WriteCommandBurst(SX1280_RADIO_SET_RFFREQUENCY, buf, 3);
}
@ -382,7 +474,7 @@ void sx1280AdjustFrequency(int32_t offset, const uint32_t freq)
UNUSED(freq);
}
void sx1280SetFIFOaddr(const uint8_t txBaseAddr, const uint8_t rxBaseAddr)
void sx1280SetFifoAddr(const uint8_t txBaseAddr, const uint8_t rxBaseAddr)
{
uint8_t buf[2];
@ -407,39 +499,6 @@ void sx1280SetDioIrqParams(const uint16_t irqMask, const uint16_t dio1Mask, cons
sx1280WriteCommandBurst(SX1280_RADIO_SET_DIOIRQPARAMS, buf, 8);
}
uint16_t sx1280GetIrqStatus(void)
{
uint8_t status[2];
sx1280ReadCommandBurst(SX1280_RADIO_GET_IRQSTATUS, status, 2);
return status[0] << 8 | status[1];
}
void sx1280ClearIrqStatus(const uint16_t irqMask)
{
uint8_t buf[2];
buf[0] = (uint8_t)(((uint16_t)irqMask >> 8) & 0x00FF);
buf[1] = (uint8_t)((uint16_t)irqMask & 0x00FF);
sx1280WriteCommandBurst(SX1280_RADIO_CLR_IRQSTATUS, buf, 2);
}
uint8_t sx1280GetIrqReason(void)
{
uint16_t irqStatus = sx1280GetIrqStatus();
sx1280ClearIrqStatus(SX1280_IRQ_RADIO_ALL);
if ((irqStatus & SX1280_IRQ_TX_DONE) && (irqStatus & SX1280_IRQ_RX_DONE)) {
return 3;
} else if ((irqStatus & SX1280_IRQ_TX_DONE)) {
return 2;
} else if ((irqStatus & SX1280_IRQ_RX_DONE)) {
return 1;
}
return 0;
}
void sx1280TransmitData(const uint8_t *data, const uint8_t length)
{
sx1280WriteBuffer(0x00, data, length);
@ -461,18 +520,414 @@ void sx1280ReceiveData(uint8_t *data, const uint8_t length)
void sx1280StartReceiving(void)
{
if (sx1280MarkBusy()) {
sx1280SetMode(SX1280_MODE_RX);
sx1280MarkFree();
}
}
void sx1280GetLastPacketStats(int8_t *rssi, int8_t *snr)
{
uint8_t status[2];
sx1280ReadCommandBurst(SX1280_RADIO_GET_PACKETSTATUS, status, 2);
*rssi = -(int8_t)(status[0] / 2);
*snr = ((int8_t) status[1]) / 4;
*rssi = -(int8_t)(packetStats[0] / 2);
*snr = ((int8_t) packetStats[1]) / 4;
int8_t negOffset = (*snr < 0) ? *snr : 0;
*rssi += negOffset;
}
void sx1280DoFHSS(void)
{
return;
}
void sx1280ClearIrqStatus(const uint16_t irqMask)
{
uint8_t buf[2];
buf[0] = (uint8_t)(((uint16_t)irqMask >> 8) & 0x00FF);
buf[1] = (uint8_t)((uint16_t)irqMask & 0x00FF);
sx1280WriteCommandBurst(SX1280_RADIO_CLR_IRQSTATUS, buf, 2);
}
// Forward Definitions for DMA Chain //
static void sx1280IrqGetStatus(extiCallbackRec_t *cb);
static busStatus_e sx1280IrqStatusRead(uint32_t arg);
static void sx1280IrqClearStatus(extiCallbackRec_t *cb);
static busStatus_e sx1280IrqCmdComplete(uint32_t arg);
static void sx1280ProcessIrq(extiCallbackRec_t *cb);
static busStatus_e sx1280GotFIFOAddr(uint32_t arg);
static void sx1280DoReadBuffer(extiCallbackRec_t *cb);
static busStatus_e sx1280ReadBufferComplete(uint32_t arg);
static void sx1280GetPacketStats(extiCallbackRec_t *cb);
static busStatus_e sx1280GetStatsCmdComplete(uint32_t arg);
static busStatus_e sx1280IsFhssReq(uint32_t arg);
static void sx1280SetFrequency(extiCallbackRec_t *cb);
static busStatus_e sx1280SetFreqComplete(uint32_t arg);
static void sx1280StartReceivingDMA(extiCallbackRec_t *cb);
static busStatus_e sx1280EnableIRQs(uint32_t arg);
static void sx1280SendTelemetryBuffer(extiCallbackRec_t *cb);
static busStatus_e sx1280TelemetryComplete(uint32_t arg);
static void sx1280StartTransmittingDMA(extiCallbackRec_t *cb);
void sx1280ISR(void)
{
// Only attempt to access the SX1280 if it is currently idle to avoid any race condition
ATOMIC_BLOCK(NVIC_PRIO_MAX) {
if (sx1280EnableBusy()) {
pendingISR = false;
sx1280SetBusyFn(sx1280IrqGetStatus);
} else {
pendingISR = true;
}
}
}
// Next, the reason for the IRQ must be read
static void sx1280IrqGetStatus(extiCallbackRec_t *cb)
{
extDevice_t *dev = rxSpiGetDevice();
UNUSED(cb);
sx1280ClearBusyFn();
STATIC_DMA_DATA_AUTO uint8_t irqStatusCmd[] = {SX1280_RADIO_GET_IRQSTATUS, 0, 0, 0};
STATIC_DMA_DATA_AUTO uint8_t irqStatus[sizeof(irqStatusCmd)];
static busSegment_t segments[] = {
{.u.buffers = {irqStatusCmd, irqStatus}, sizeof(irqStatusCmd), false, sx1280IrqStatusRead},
{.u.link = {NULL, NULL}, 0, true, NULL},
};
spiSequence(dev, segments);
}
// Read the IRQ status, and save it to irqStatus variable
static busStatus_e sx1280IrqStatusRead(uint32_t arg)
{
extDevice_t *dev = (extDevice_t *)arg;
uint16_t irqStatus = (dev->bus->curSegment->u.buffers.rxData[2] << 8) | dev->bus->curSegment->u.buffers.rxData[3];
if (irqStatus & SX1280_IRQ_TX_DONE) {
irqReason = ELRS_DIO_TX_DONE;
} else if (irqStatus & SX1280_IRQ_RX_DONE) {
irqReason = ELRS_DIO_RX_DONE;
} else {
irqReason = ELRS_DIO_UNKNOWN;
}
sx1280SetBusyFn(sx1280IrqClearStatus);
return BUS_READY;
}
// Clear the IRQ bit in the Radio registers
static void sx1280IrqClearStatus(extiCallbackRec_t *cb)
{
extDevice_t *dev = rxSpiGetDevice();
UNUSED(cb);
sx1280ClearBusyFn();
STATIC_DMA_DATA_AUTO uint8_t irqCmd[] = {SX1280_RADIO_CLR_IRQSTATUS, 0, 0};
irqCmd[1] = (uint8_t)(((uint16_t)SX1280_IRQ_RADIO_ALL >> 8) & 0x00FF);
irqCmd[2] = (uint8_t)((uint16_t)SX1280_IRQ_RADIO_ALL & 0x00FF);
static busSegment_t segments[] = {
{.u.buffers = {irqCmd, NULL}, sizeof(irqCmd), false, sx1280IrqCmdComplete},
{.u.link = {NULL, NULL}, 0, true, NULL},
};
spiSequence(dev, segments);
}
// Callback follow clear of IRQ status
static busStatus_e sx1280IrqCmdComplete(uint32_t arg)
{
UNUSED(arg);
sx1280SetBusyFn(sx1280ProcessIrq);
return BUS_READY;
}
// Process IRQ status
static void sx1280ProcessIrq(extiCallbackRec_t *cb)
{
extDevice_t *dev = rxSpiGetDevice();
UNUSED(cb);
sx1280ClearBusyFn();
if (irqReason == ELRS_DIO_RX_DONE || irqReason == ELRS_DIO_UNKNOWN) {
// Fire off the chain to read and decode the packet from the radio
// Get the buffer status to determine the FIFO address
STATIC_DMA_DATA_AUTO uint8_t cmdBufStatusCmd[] = {SX1280_RADIO_GET_RXBUFFERSTATUS, 0, 0, 0};
STATIC_DMA_DATA_AUTO uint8_t bufStatus[sizeof(cmdBufStatusCmd)];
static busSegment_t segments[] = {
{.u.buffers = {cmdBufStatusCmd, bufStatus}, sizeof(cmdBufStatusCmd), false, sx1280GotFIFOAddr},
{.u.link = {NULL, NULL}, 0, true, NULL},
};
spiSequence(dev, segments);
} else {
// return to RX mode immediately, the next packet will be an RX and we won't need to FHSS
STATIC_DMA_DATA_AUTO uint8_t irqSetRxCmd[] = {SX1280_RADIO_SET_RX, 0, 0xff, 0xff};
static busSegment_t segments[] = {
{.u.buffers = {irqSetRxCmd, NULL}, sizeof(irqSetRxCmd), false, sx1280EnableIRQs},
{.u.link = {NULL, NULL}, 0, true, NULL},
};
spiSequence(dev, segments);
}
}
// First we read from the FIFO address register to determine the FIFO address
static busStatus_e sx1280GotFIFOAddr(uint32_t arg)
{
extDevice_t *dev = (extDevice_t *)arg;
FIFOaddr = dev->bus->curSegment->u.buffers.rxData[3];
// Wait until no longer busy and read the buffer
sx1280SetBusyFn(sx1280DoReadBuffer);
return BUS_READY;
}
// Using the addr val stored to the global varable FIFOaddr, read the buffer
static void sx1280DoReadBuffer(extiCallbackRec_t *cb)
{
extDevice_t *dev = rxSpiGetDevice();
UNUSED(cb);
sx1280ClearBusyFn();
STATIC_DMA_DATA_AUTO uint8_t cmdReadBuf[] = {SX1280_RADIO_READ_BUFFER, 0, 0};
cmdReadBuf[1] = FIFOaddr;
static busSegment_t segments[] = {
{.u.buffers = {cmdReadBuf, NULL}, sizeof(cmdReadBuf), false, NULL},
{.u.buffers = {NULL, NULL}, ELRS_RX_TX_BUFF_SIZE, true, sx1280ReadBufferComplete},
{.u.link = {NULL, NULL}, 0, true, NULL},
};
segments[1].u.buffers.rxData = (uint8_t *)expressLrsGetRxBuffer();
spiSequence(dev, segments);
}
// Get the Packet Status and RSSI
static busStatus_e sx1280ReadBufferComplete(uint32_t arg)
{
UNUSED(arg);
sx1280SetBusyFn(sx1280GetPacketStats);
return BUS_READY;
}
// Save the Packet Stats to the global variables
static void sx1280GetPacketStats(extiCallbackRec_t *cb)
{
UNUSED(cb);
extDevice_t *dev = rxSpiGetDevice();
sx1280ClearBusyFn();
STATIC_DMA_DATA_AUTO uint8_t getStatsCmd[] = {SX1280_RADIO_GET_PACKETSTATUS, 0, 0, 0};
STATIC_DMA_DATA_AUTO uint8_t stats[sizeof(getStatsCmd)];
static busSegment_t segments[] = {
{.u.buffers = {getStatsCmd, stats}, sizeof(getStatsCmd), false, sx1280GetStatsCmdComplete},
{.u.link = {NULL, NULL}, 0, true, NULL},
};
spiSequence(dev, segments);
}
// Process and decode the RF packet
static busStatus_e sx1280GetStatsCmdComplete(uint32_t arg)
{
extDevice_t *dev = (extDevice_t *)arg;
volatile uint8_t *payload = expressLrsGetPayloadBuffer();
packetStats[0] = dev->bus->curSegment->u.buffers.rxData[2];
packetStats[1] = dev->bus->curSegment->u.buffers.rxData[3];
expressLrsSetRfPacketStatus(processRFPacket(payload, rxSpiGetLastExtiTimeUs()));
return sx1280IsFhssReq(arg);
}
void sx1280HandleFromTock(void)
{
ATOMIC_BLOCK(NVIC_PRIO_MAX) {
if (expressLrsIsFhssReq()) {
if (sx1280EnableBusy()) {
pendingDoFHSS = false;
sx1280SetBusyFn(sx1280SetFrequency);
} else {
pendingDoFHSS = true;
}
}
}
}
// Next we need to check if we need to FHSS and then do so if needed
static busStatus_e sx1280IsFhssReq(uint32_t arg)
{
UNUSED(arg);
if (expressLrsIsFhssReq()) {
sx1280SetBusyFn(sx1280SetFrequency);
} else {
sx1280SetFreqComplete(arg);
}
return BUS_READY;
}
// Set the frequency
static void sx1280SetFrequency(extiCallbackRec_t *cb)
{
UNUSED(cb);
extDevice_t *dev = rxSpiGetDevice();
uint32_t currentFreq = expressLrsGetCurrentFreq();
sx1280ClearBusyFn();
STATIC_DMA_DATA_AUTO uint8_t setFreqCmd[] = {SX1280_RADIO_SET_RFFREQUENCY, 0, 0, 0};
setFreqCmd[1] = (uint8_t)((currentFreq >> 16) & 0xFF);
setFreqCmd[2] = (uint8_t)((currentFreq >> 8) & 0xFF);
setFreqCmd[3] = (uint8_t)(currentFreq & 0xFF);
static busSegment_t segments[] = {
{.u.buffers = {setFreqCmd, NULL}, sizeof(setFreqCmd), false, sx1280SetFreqComplete},
{.u.link = {NULL, NULL}, 0, true, NULL},
};
spiSequence(dev, segments);
}
// Determine if we need to go back to RX or if we need to send TLM data
static busStatus_e sx1280SetFreqComplete(uint32_t arg)
{
UNUSED(arg);
if (expressLrsTelemRespReq()) {
expressLrsDoTelem();
// if it's time to do TLM and we have enough to do so
sx1280SetBusyFn(sx1280SendTelemetryBuffer);
} else {
// we don't need to send TLM and we've already FHSS so just hop back into RX mode
sx1280SetBusyFn(sx1280StartReceivingDMA);
}
return BUS_READY;
}
// Go back into RX mode
static void sx1280StartReceivingDMA(extiCallbackRec_t *cb)
{
UNUSED(cb);
extDevice_t *dev = rxSpiGetDevice();
sx1280ClearBusyFn();
// Issue command to start receiving
// periodBase = 1ms, page 71 datasheet, set to FF for cont RX
STATIC_DMA_DATA_AUTO uint8_t irqSetRxCmd[] = {SX1280_RADIO_SET_RX, 0, 0xff, 0xff};
static busSegment_t segments[] = {
{.u.buffers = {irqSetRxCmd, NULL}, sizeof(irqSetRxCmd), false, sx1280EnableIRQs},
{.u.link = {NULL, NULL}, 0, true, NULL},
};
spiSequence(dev, segments);
}
static busStatus_e sx1280EnableIRQs(uint32_t arg)
{
UNUSED(arg);
// Handle any queued interrupt processing
if (pendingISR) {
pendingISR = false;
sx1280SetBusyFn(sx1280IrqGetStatus);
} else if (pendingDoFHSS) {
pendingDoFHSS = false;
sx1280SetBusyFn(sx1280SetFrequency);
} else {
// Switch back to waiting for EXTI interrupt
sx1280EnableExti();
}
return BUS_READY;
}
// Send telemetry response
static void sx1280SendTelemetryBuffer(extiCallbackRec_t *cb)
{
UNUSED(cb);
extDevice_t *dev = rxSpiGetDevice();
sx1280ClearBusyFn();
STATIC_DMA_DATA_AUTO uint8_t writeBufferCmd[] = {SX1280_RADIO_WRITE_BUFFER, 0};
static busSegment_t segments[] = {
{.u.buffers = {writeBufferCmd, NULL}, sizeof(writeBufferCmd), false, NULL},
{.u.buffers = {NULL, NULL}, ELRS_RX_TX_BUFF_SIZE, true, sx1280TelemetryComplete},
{.u.link = {NULL, NULL}, 0, true, NULL},
};
segments[1].u.buffers.txData = (uint8_t *)expressLrsGetTelemetryBuffer();
spiSequence(dev, segments);
}
static busStatus_e sx1280TelemetryComplete(uint32_t arg)
{
UNUSED(arg);
sx1280SetBusyFn(sx1280StartTransmittingDMA);
return BUS_READY;
}
static void sx1280StartTransmittingDMA(extiCallbackRec_t *cb)
{
UNUSED(cb);
extDevice_t *dev = rxSpiGetDevice();
sx1280ClearBusyFn();
//uses timeout Time-out duration = periodBase * periodBaseCount
// periodBase = 1ms, page 71 datasheet
// no timeout set for now
// TODO dynamic timeout based on expected onairtime
STATIC_DMA_DATA_AUTO uint8_t irqSetRxCmd[] = {SX1280_RADIO_SET_TX, 0, 0xff, 0xff};
static busSegment_t segments[] = {
{.u.buffers = {irqSetRxCmd, NULL}, sizeof(irqSetRxCmd), false, sx1280EnableIRQs},
{.u.link = {NULL, NULL}, 0, true, NULL},
};
spiSequence(dev, segments);
}
#endif /* USE_RX_SX1280 */

15
src/main/drivers/rx/rx_sx1280.h
View File

@ -25,6 +25,8 @@
#pragma once
#include "common/time.h"
#define REG_LR_FIRMWARE_VERSION_MSB 0x0153 //The address of the register holding the firmware version MSB
#define SX1280_REG_LR_ESTIMATED_FREQUENCY_ERROR_MSB 0x0954
#define SX1280_REG_LR_ESTIMATED_FREQUENCY_ERROR_MASK 0x0FFFFF
@ -247,7 +249,8 @@ typedef enum {
} sx1280TickSizes_e;
bool sx1280Init(IO_t resetPin, IO_t busyPin);
uint8_t sx1280ISR(uint32_t *timeStamp);
void sx1280ISR(void);
bool sx1280HandleFromTick(void);
bool sx1280IsBusy(void);
void sx1280WriteCommand(const uint8_t address, const uint8_t data);
void sx1280WriteCommandBurst(const uint8_t address, const uint8_t *data, const uint8_t length);
@ -266,14 +269,14 @@ void sx1280SetOutputPower(const int8_t power);
void sx1280SetPacketParams(const uint8_t preambleLength, const sx1280LoraPacketLengthsModes_e headerType, const uint8_t payloadLength, const sx1280LoraCrcModes_e crc, const sx1280LoraIqModes_e invertIQ);
void sx1280SetMode(const sx1280OperatingModes_e opMode);
void sx1280ConfigLoraModParams(const sx1280LoraBandwidths_e bw, const sx1280LoraSpreadingFactors_e sf, const sx1280LoraCodingRates_e cr);
void sx1280SetFrequencyHZ(const uint32_t reqFreq);
void sx1280SetFrequencyReg(const uint32_t freq);
void sx1280SetFrequencyReg(const uint32_t freqReg);
void sx1280AdjustFrequency(int32_t offset, const uint32_t freq);
void sx1280SetFIFOaddr(const uint8_t txBaseAddr, const uint8_t rxBaseAddr);
void sx1280SetFifoAddr(const uint8_t txBaseAddr, const uint8_t rxBaseAddr);
void sx1280SetDioIrqParams(const uint16_t irqMask, const uint16_t dio1Mask, const uint16_t dio2Mask, const uint16_t dio3Mask);
uint16_t sx1280GetIrqStatus(void);
void sx1280ClearIrqStatus(const uint16_t irqMask);
uint8_t sx1280GetIrqReason(void);
void sx1280GetIrqReason(void);
void sx1280HandleFromTock();
void sx1280TransmitData(const uint8_t *data, const uint8_t length);
void sx1280ReceiveData(uint8_t *data, const uint8_t length);

9
src/main/fc/core.c
View File

@ -767,7 +767,6 @@ bool processRx(timeUs_t currentTimeUs)
if (currentTimeUs > FAILSAFE_POWER_ON_DELAY_US && !failsafeIsMonitoring()) {
failsafeStartMonitoring();
}
failsafeUpdateState();
const throttleStatus_e throttleStatus = calculateThrottleStatus();
const uint8_t throttlePercent = calculateThrottlePercentAbs();
@ -1269,6 +1268,14 @@ FAST_CODE bool pidLoopReady(void)
return false;
}
FAST_CODE bool rxFrameReady(void)
{
if ((activePidLoopDenom == 1) || (pidUpdateCounter % activePidLoopDenom == 0)) {
return true;
}
return false;
}
FAST_CODE void taskFiltering(timeUs_t currentTimeUs)
{
gyroFiltering(currentTimeUs);

1
src/main/fc/core.h
View File

@ -82,6 +82,7 @@ void updateArmingStatus(void);
void taskGyroSample(timeUs_t currentTimeUs);
bool gyroFilterReady(void);
bool pidLoopReady(void);
bool rxFrameReady(void);
void taskFiltering(timeUs_t currentTimeUs);
void taskMainPidLoop(timeUs_t currentTimeUs);

11
src/main/fc/rc_controls.c
View File

@ -144,6 +144,7 @@ void processRcStickPositions()
// an extra guard for disarming through switch to prevent that one frame can disarm it
static uint8_t rcDisarmTicks;
static bool doNotRepeat;
static bool pendingApplyRollAndPitchTrimDeltaSave = false;
// checking sticks positions
uint8_t stTmp = 0;
@ -306,6 +307,12 @@ void processRcStickPositions()
rollAndPitchTrims_t accelerometerTrimsDelta;
memset(&accelerometerTrimsDelta, 0, sizeof(accelerometerTrimsDelta));
if (pendingApplyRollAndPitchTrimDeltaSave && ((rcSticks & THR_MASK) != THR_HI)) {
saveConfigAndNotify();
pendingApplyRollAndPitchTrimDeltaSave = false;
return;
}
bool shouldApplyRollAndPitchTrimDelta = false;
switch (rcSticks) {
case THR_HI + YAW_CE + PIT_HI + ROL_CE:
@ -329,7 +336,9 @@ void processRcStickPositions()
#if defined(USE_ACC)
applyAccelerometerTrimsDelta(&accelerometerTrimsDelta);
#endif
saveConfigAndNotify();
pendingApplyRollAndPitchTrimDeltaSave = true;
beeperConfirmationBeeps(1);
repeatAfter(STICK_AUTOREPEAT_MS);

1
src/main/fc/rc_controls.h
View File

@ -70,6 +70,7 @@ typedef enum {
#define THR_LO (1 << (2 * THROTTLE))
#define THR_CE (3 << (2 * THROTTLE))
#define THR_HI (2 << (2 * THROTTLE))
#define THR_MASK (3 << (2 * THROTTLE))
#define CONTROL_RATE_CONFIG_RC_EXPO_MAX 100

5
src/main/fc/tasks.c
View File

@ -167,7 +167,6 @@ static void taskUpdateAccelerometer(timeUs_t currentTimeUs)
typedef enum {
RX_STATE_CHECK,
RX_STATE_PROCESS,
RX_STATE_MODES,
RX_STATE_UPDATE,
RX_STATE_COUNT
@ -195,10 +194,6 @@ static void taskUpdateRxMain(timeUs_t currentTimeUs)
switch (rxState) {
default:
case RX_STATE_CHECK:
rxState = RX_STATE_PROCESS;
break;
case RX_STATE_PROCESS:
if (!processRx(currentTimeUs)) {
rxState = RX_STATE_CHECK;
break;

10
src/main/flight/failsafe.c
View File

@ -192,7 +192,15 @@ void failsafeOnValidDataFailed(void)
}
}
void failsafeUpdateState(void)
void failsafeCheckDataFailurePeriod(void)
{
if (cmp32(millis(), failsafeState.validRxDataReceivedAt) > (int32_t)failsafeState.rxDataFailurePeriod) {
setArmingDisabled(ARMING_DISABLED_RX_FAILSAFE); // To prevent arming with no RX link
failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN;
}
}
FAST_CODE_NOINLINE void failsafeUpdateState(void)
{
if (!failsafeIsMonitoring()) {
return;

1
src/main/flight/failsafe.h
View File

@ -103,6 +103,7 @@ failsafePhase_e failsafePhase(void);
bool failsafeIsMonitoring(void);
bool failsafeIsActive(void);
bool failsafeIsReceivingRxData(void);
void failsafeCheckDataFailurePeriod(void);
void failsafeOnRxSuspend(uint32_t suspendPeriod);
void failsafeOnRxResume(void);

312
src/main/rx/expresslrs.c
View File

@ -69,7 +69,7 @@
#include "rx/expresslrs_impl.h"
#include "rx/expresslrs_telemetry.h"
STATIC_UNIT_TESTED elrsReceiver_t receiver;
UNIT_TESTED elrsReceiver_t receiver;
static const uint8_t BindingUID[6] = {0,1,2,3,4,5}; // Special binding UID values
static uint16_t crcInitializer = 0;
static uint8_t bindingRateIndex = 0;
@ -79,9 +79,14 @@ static uint8_t wideSwitchIndex = 0;
static simpleLowpassFilter_t rssiFilter;
static volatile DMA_DATA uint8_t dmaBuffer[ELRS_RX_TX_BUFF_SIZE];
static volatile DMA_DATA uint8_t telemetryPacket[ELRS_RX_TX_BUFF_SIZE];
static volatile rx_spi_received_e rfPacketStatus = RX_SPI_RECEIVED_NONE;
static volatile uint8_t *payload;
static void rssiFilterReset(void)
{
simpleLPFilterInit(&rssiFilter, 3, 5);
simpleLPFilterInit(&rssiFilter, 2, 5);
}
#define PACKET_HANDLING_TO_TOCK_ISR_DELAY_US 250
@ -107,28 +112,28 @@ eprState_t eprState = {
.eventRecorded = {0},
};
static void expressLrsEPRRecordEvent(eprEvent_e event, uint32_t currentTimeUs)
static void phaseLockEprEvent(eprEvent_e event, uint32_t currentTimeUs)
{
eprState.eventAtUs[event] = currentTimeUs;
eprState.eventRecorded[event] = true;
}
static bool expressLrsEPRHaveBothEvents(void)
static bool phaseLockEprHaveBothEvents(void)
{
bool bothEventsRecorded = eprState.eventRecorded[EPR_SECOND] && eprState.eventRecorded[EPR_FIRST];
return bothEventsRecorded;
}
static int32_t expressLrsEPRGetResult(void)
static int32_t phaseLockEprResult(void)
{
if (!expressLrsEPRHaveBothEvents()) {
if (!phaseLockEprHaveBothEvents()) {
return 0;
}
return (int32_t)(eprState.eventAtUs[EPR_SECOND] - eprState.eventAtUs[EPR_FIRST]);
}
static void expressLrsEPRReset(void)
static void phaseLockEprReset(void)
{
memset(&eprState, 0, sizeof(eprState_t));
}
@ -161,7 +166,7 @@ static void expressLrsPhaseLockReset(void)
simpleLPFilterInit(&pl.offsetFilter, 2, 5);
simpleLPFilterInit(&pl.offsetDxFilter, 4, 5);
expressLrsEPRReset();
phaseLockEprReset();
}
static uint8_t nextTelemetryType = ELRS_TELEMETRY_TYPE_LINK;
@ -290,12 +295,6 @@ static void unpackChannelDataHybridWide(uint16_t *rcData, const uint8_t *payload
setRssiChannelData(rcData);
}
static void startReceiving(void)
{
dbgPinLo(1);
receiver.startReceiving();
}
static uint8_t minLqForChaos(void)
{
// Determine the most number of CRC-passing packets we could receive on
@ -307,12 +306,12 @@ static uint8_t minLqForChaos(void)
// FHSShopInterval * ceil(100 / FHSShopInterval * numfhss) or
// FHSShopInterval * trunc((100 + (FHSShopInterval * numfhss) - 1) / (FHSShopInterval * numfhss))
// With a interval of 4 this works out to: 2.4=4, FCC915=4, AU915=8, EU868=8, EU/AU433=36
const uint32_t numfhss = getFHSSNumEntries();
const uint32_t numfhss = fhssGetNumEntries();
const uint8_t interval = receiver.modParams->fhssHopInterval;
return interval * ((interval * numfhss + 99) / (interval * numfhss));
}
static void setRFLinkRate(const uint8_t index)
static void setRfLinkRate(const uint8_t index)
{
#if defined(USE_RX_SX1280) && defined(USE_RX_SX127X)
receiver.modParams = (rxExpressLrsSpiConfig()->domain == ISM2400) ? &airRateConfig[1][index] : &airRateConfig[0][index];
@ -321,9 +320,9 @@ static void setRFLinkRate(const uint8_t index)
receiver.modParams = &airRateConfig[0][index];
receiver.rfPerfParams = &rfPerfConfig[0][index];
#endif
receiver.currentFreq = getInitialFreq(receiver.freqOffset);
receiver.currentFreq = fhssGetInitialFreq(receiver.freqOffset);
// Wait for (11/10) 110% of time it takes to cycle through all freqs in FHSS table (in ms)
receiver.cycleIntervalMs = ((uint32_t)11U * getFHSSNumEntries() * receiver.modParams->fhssHopInterval * receiver.modParams->interval) / (10U * 1000U);
receiver.cycleIntervalMs = ((uint32_t)11U * fhssGetNumEntries() * receiver.modParams->fhssHopInterval * receiver.modParams->interval) / (10U * 1000U);
receiver.config(receiver.modParams->bw, receiver.modParams->sf, receiver.modParams->cr, receiver.currentFreq, receiver.modParams->preambleLen, receiver.UID[5] & 0x01);
@ -338,57 +337,73 @@ static void setRFLinkRate(const uint8_t index)
#endif
}
static bool handleFHSS(void)
uint32_t expressLrsGetCurrentFreq(void)
{
return receiver.currentFreq;
}
void expressLrsSetRfPacketStatus(rx_spi_received_e status)
{
rfPacketStatus = status;
}
volatile uint8_t *expressLrsGetRxBuffer(void) {
return dmaBuffer;
}
volatile uint8_t *expressLrsGetTelemetryBuffer(void)
{
return telemetryPacket;
}
volatile uint8_t *expressLrsGetPayloadBuffer(void)
{
return payload;
}
bool expressLrsIsFhssReq(void)
{
uint8_t modresultFHSS = (receiver.nonceRX + 1) % receiver.modParams->fhssHopInterval;
if ((receiver.modParams->fhssHopInterval == 0) || receiver.alreadyFHSS == true || receiver.inBindingMode || (modresultFHSS != 0) || (receiver.connectionState == ELRS_DISCONNECTED)) {
if ((receiver.modParams->fhssHopInterval == 0) || receiver.alreadyFhss == true || receiver.inBindingMode || (modresultFHSS != 0) || (receiver.connectionState == ELRS_DISCONNECTED)) {
return false;
}
receiver.alreadyFHSS = true;
receiver.currentFreq = FHSSgetNextFreq(receiver.freqOffset);
receiver.setFrequency(receiver.currentFreq);
receiver.alreadyFhss = true;
receiver.currentFreq = fhssGetNextFreq(receiver.freqOffset);
uint8_t modresultTLM = (receiver.nonceRX + 1) % (tlmRatioEnumToValue(receiver.modParams->tlmInterval));
if (modresultTLM != 0 || receiver.modParams->tlmInterval == TLM_RATIO_NO_TLM) { // if we are about to send a tlm response don't bother going back to rx
startReceiving();
}
return true;
}
static bool shouldSendTelemetryResponse(void)
bool expressLrsTelemRespReq(void)
{
uint8_t modresult = (receiver.nonceRX + 1) % tlmRatioEnumToValue(receiver.modParams->tlmInterval);
if ((receiver.connectionState == ELRS_DISCONNECTED) || (receiver.modParams->tlmInterval == TLM_RATIO_NO_TLM) || (receiver.alreadyTLMresp == true) || (modresult != 0)) {
if (receiver.inBindingMode || (receiver.connectionState == ELRS_DISCONNECTED) || (receiver.modParams->tlmInterval == TLM_RATIO_NO_TLM) || (modresult != 0)) {
return false; // don't bother sending tlm if disconnected or TLM is off
} else {
return true;
}
}
static void handleSendTelemetryResponse(void)
static void expressLrsSendTelemResp(void)
{
uint8_t packet[8];
uint8_t *data;
uint8_t maxLength;
uint8_t packageIndex;
receiver.alreadyTLMresp = true;
packet[0] = ELRS_TLM_PACKET;
receiver.alreadyTelemResp = true;
telemetryPacket[0] = ELRS_TLM_PACKET;
if (nextTelemetryType == ELRS_TELEMETRY_TYPE_LINK || !isTelemetrySenderActive()) {
packet[1] = ELRS_TELEMETRY_TYPE_LINK;
packet[2] = receiver.rssiFiltered > 0 ? 0 : -receiver.rssiFiltered; //diversity not supported
packet[3] = connectionHasModelMatch << 7;
packet[4] = receiver.snr;
packet[5] = receiver.uplinkLQ;
telemetryPacket[1] = ELRS_TELEMETRY_TYPE_LINK;
telemetryPacket[2] = receiver.rssiFiltered > 0 ? 0 : -receiver.rssiFiltered; //diversity not supported
telemetryPacket[3] = connectionHasModelMatch << 7;
telemetryPacket[4] = receiver.snr;
telemetryPacket[5] = receiver.uplinkLQ;
#ifdef USE_MSP_OVER_TELEMETRY
packet[6] = getCurrentMspConfirm() ? 1 : 0;
telemetryPacket[6] = getCurrentMspConfirm() ? 1 : 0;
#else
packet[6] = 0;
telemetryPacket[6] = 0;
#endif
nextTelemetryType = ELRS_TELEMETRY_TYPE_DATA;
// Start the count at 1 because the next will be DATA and doing +1 before checking
@ -402,27 +417,24 @@ static void handleSendTelemetryResponse(void)
}
getCurrentTelemetryPayload(&packageIndex, &maxLength, &data);
packet[1] = (packageIndex << ELRS_TELEMETRY_SHIFT) + ELRS_TELEMETRY_TYPE_DATA;
packet[2] = maxLength > 0 ? *data : 0;
packet[3] = maxLength >= 1 ? *(data + 1) : 0;
packet[4] = maxLength >= 2 ? *(data + 2) : 0;
packet[5] = maxLength >= 3 ? *(data + 3) : 0;
packet[6] = maxLength >= 4 ? *(data + 4) : 0;
telemetryPacket[1] = (packageIndex << ELRS_TELEMETRY_SHIFT) + ELRS_TELEMETRY_TYPE_DATA;
telemetryPacket[2] = maxLength > 0 ? *data : 0;
telemetryPacket[3] = maxLength >= 1 ? *(data + 1) : 0;
telemetryPacket[4] = maxLength >= 2 ? *(data + 2) : 0;
telemetryPacket[5] = maxLength >= 3 ? *(data + 3) : 0;
telemetryPacket[6] = maxLength >= 4 ? *(data + 4) : 0;
}
uint16_t crc = calcCrc14(packet, 7, crcInitializer);
packet[0] |= (crc >> 6) & 0xFC;
packet[7] = crc & 0xFF;
dbgPinHi(1);
receiver.transmitData(packet, ELRS_RX_TX_BUFF_SIZE);
uint16_t crc = calcCrc14((uint8_t *)telemetryPacket, 7, crcInitializer);
telemetryPacket[0] |= (crc >> 6) & 0xFC;
telemetryPacket[7] = crc & 0xFF;
}
static void updatePhaseLock(void)
{
if (receiver.connectionState != ELRS_DISCONNECTED && expressLrsEPRHaveBothEvents()) {
if (receiver.connectionState != ELRS_DISCONNECTED && phaseLockEprHaveBothEvents()) {
int32_t maxOffset = receiver.modParams->interval / 4;
pl.rawOffsetUs = constrain(expressLrsEPRGetResult(), -maxOffset, maxOffset);
pl.rawOffsetUs = constrain(phaseLockEprResult(), -maxOffset, maxOffset);
pl.offsetUs = simpleLPFilterUpdate(&pl.offsetFilter, pl.rawOffsetUs);
pl.offsetDeltaUs = simpleLPFilterUpdate(&pl.offsetDxFilter, pl.rawOffsetUs - pl.previousRawOffsetUs);
@ -452,7 +464,7 @@ static void updatePhaseLock(void)
DEBUG_SET(DEBUG_RX_EXPRESSLRS_PHASELOCK, 1, pl.offsetUs);
}
expressLrsEPRReset();
phaseLockEprReset();
}
//hwTimerCallbackTick
@ -464,12 +476,14 @@ void expressLrsOnTimerTickISR(void) // this is 180 out of phase with the other c
// Save the LQ value before the inc() reduces it by 1
receiver.uplinkLQ = lqGet();
// Only advance the LQI period counter if we didn't send Telemetry this period
if (!receiver.alreadyTLMresp) {
if (!receiver.alreadyTelemResp) {
lqNewPeriod();
}
receiver.alreadyTLMresp = false;
receiver.alreadyFHSS = false;
receiver.alreadyTelemResp = false;
receiver.alreadyFhss = false;
receiver.rxHandleFromTick();
}
//hwTimerCallbackTock
@ -477,9 +491,9 @@ void expressLrsOnTimerTockISR(void)
{
uint32_t currentTimeUs = micros();
expressLrsEPRRecordEvent(EPR_INTERNAL, currentTimeUs);
phaseLockEprEvent(EPR_INTERNAL, currentTimeUs);
receiver.fhssRequired = true; //Rest of the code is moved to expressLrsDataReceived to avoid race condition
receiver.rxHandleFromTock();
}
static uint16_t lostConnectionCounter = 0;
@ -501,14 +515,13 @@ void lostConnection(void)
receiver.uplinkLQ = 0;
lqReset();
expressLrsPhaseLockReset();
receiver.alreadyTLMresp = false;
receiver.alreadyFHSS = false;
receiver.alreadyTelemResp = false;
receiver.alreadyFhss = false;
if (!receiver.inBindingMode) {
//while (micros() - expressLrsEPRGetResult() > 250); // time it just after the tock() TODO this currently breaks and is blocking, not a fan of this.
expressLrsTimerStop();
setRFLinkRate(receiver.nextRateIndex); // also sets to initialFreq
startReceiving();
setRfLinkRate(receiver.nextRateIndex); // also sets to initialFreq
receiver.startReceiving();
}
}
@ -548,7 +561,7 @@ static void gotConnection(const uint32_t timeStampMs)
//setup radio
static void initializeReceiver(void)
{
FHSSrandomiseFHSSsequence(receiver.UID, rxExpressLrsSpiConfig()->domain);
fhssGenSequence(receiver.UID, rxExpressLrsSpiConfig()->domain);
lqReset();
receiver.nonceRX = 0;
receiver.freqOffset = 0;
@ -558,11 +571,11 @@ static void initializeReceiver(void)
receiver.snr = 0;
receiver.uplinkLQ = 0;
receiver.rateIndex = receiver.inBindingMode ? bindingRateIndex : rxExpressLrsSpiConfig()->rateIndex;
setRFLinkRate(receiver.rateIndex);
setRfLinkRate(receiver.rateIndex);
receiver.started = false;
receiver.alreadyFHSS = false;
receiver.alreadyTLMresp = false;
receiver.alreadyFhss = false;
receiver.alreadyTelemResp = false;
receiver.lockRFmode = false;
receiver.timerState = ELRS_TIM_DISCONNECTED;
receiver.connectionState = ELRS_DISCONNECTED;
@ -579,7 +592,7 @@ static void initializeReceiver(void)
receiver.rfModeCycleMultiplier = 1;
}
static void unpackBindPacket(uint8_t *packet)
static void unpackBindPacket(volatile uint8_t *packet)
{
rxExpressLrsSpiConfigMutable()->UID[2] = packet[3];
rxExpressLrsSpiConfigMutable()->UID[3] = packet[4];
@ -589,16 +602,13 @@ static void unpackBindPacket(uint8_t *packet)
receiver.UID = rxExpressLrsSpiConfigMutable()->UID;
crcInitializer = (receiver.UID[4] << 8) | receiver.UID[5];
receiver.inBindingMode = false;
initializeReceiver();
receiver.configChanged = true; //after initialize as it sets it to false
}
/**
* Process the assembled MSP packet in mspBuffer[]
**/
static void processRFMspPacket(uint8_t *packet)
static void processRFMspPacket(volatile uint8_t *packet)
{
// Always examine MSP packets for bind information if in bind mode
// [1] is the package index, first packet of the MSP
@ -635,7 +645,7 @@ static void processRFMspPacket(uint8_t *packet)
#endif
}
static bool processRFSyncPacket(uint8_t *packet, const uint32_t timeStampMs)
static bool processRFSyncPacket(volatile uint8_t *packet, const uint32_t timeStampMs)
{
// Verify the first two of three bytes of the binding ID, which should always match
if (packet[4] != receiver.UID[3] || packet[5] != receiver.UID[4]) {
@ -672,8 +682,8 @@ static bool processRFSyncPacket(uint8_t *packet, const uint32_t timeStampMs)
uint8_t modelXor = (~rxExpressLrsSpiConfig()->modelId) & ELRS_MODELMATCH_MASK;
bool modelMatched = packet[6] == (receiver.UID[5] ^ modelXor);
if (receiver.connectionState == ELRS_DISCONNECTED || receiver.nonceRX != packet[2] || FHSSgetCurrIndex() != packet[1] || connectionHasModelMatch != modelMatched) {
FHSSsetCurrIndex(packet[1]);
if (receiver.connectionState == ELRS_DISCONNECTED || receiver.nonceRX != packet[2] || fhssGetCurrIndex() != packet[1] || connectionHasModelMatch != modelMatched) {
fhssSetCurrIndex(packet[1]);
receiver.nonceRX = packet[2];
tentativeConnection(timeStampMs);
@ -687,30 +697,26 @@ static bool processRFSyncPacket(uint8_t *packet, const uint32_t timeStampMs)
return false;
}
static rx_spi_received_e processRFPacket(uint8_t *payload, uint32_t timeStampUs)
rx_spi_received_e processRFPacket(volatile uint8_t *payload, uint32_t timeStampUs)
{
uint8_t packet[ELRS_RX_TX_BUFF_SIZE];
receiver.receiveData(packet, ELRS_RX_TX_BUFF_SIZE);
elrsPacketType_e type = packet[0] & 0x03;
uint16_t inCRC = (((uint16_t)(packet[0] & 0xFC)) << 6 ) | packet[7];
elrsPacketType_e type = dmaBuffer[0] & 0x03;
uint16_t inCRC = (((uint16_t)(dmaBuffer[0] & 0xFC)) << 6 ) | dmaBuffer[7];
// For SM_HYBRID the CRC only has the packet type in byte 0
// For SM_HYBRID_WIDE the FHSS slot is added to the CRC in byte 0 on RC_DATA_PACKETs
if (type != ELRS_RC_DATA_PACKET || rxExpressLrsSpiConfig()->switchMode != SM_HYBRID_WIDE) {
packet[0] = type;
dmaBuffer[0] = type;
} else {
uint8_t nonceFHSSresult = receiver.nonceRX % receiver.modParams->fhssHopInterval;
packet[0] = type | (nonceFHSSresult << 2);
dmaBuffer[0] = type | (nonceFHSSresult << 2);
}
uint16_t calculatedCRC = calcCrc14(packet, 7, crcInitializer);
uint16_t calculatedCRC = calcCrc14((uint8_t *)dmaBuffer, 7, crcInitializer);
if (inCRC != calculatedCRC) {
return RX_SPI_RECEIVED_NONE;
}
expressLrsEPRRecordEvent(EPR_EXTERNAL, timeStampUs + PACKET_HANDLING_TO_TOCK_ISR_DELAY_US);
phaseLockEprEvent(EPR_EXTERNAL, timeStampUs + PACKET_HANDLING_TO_TOCK_ISR_DELAY_US);
bool shouldStartTimer = false;
uint32_t timeStampMs = millis();
@ -725,31 +731,32 @@ static rx_spi_received_e processRFPacket(uint8_t *payload, uint32_t timeStampUs)
if (rxExpressLrsSpiConfig()->switchMode == SM_HYBRID_WIDE) {
wideSwitchIndex = hybridWideNonceToSwitchIndex(receiver.nonceRX);
if ((tlmRatioEnumToValue(receiver.modParams->tlmInterval) < 8) || wideSwitchIndex == 7) {
confirmCurrentTelemetryPayload((packet[6] & 0x40) >> 6);
confirmCurrentTelemetryPayload((dmaBuffer[6] & 0x40) >> 6);
}
} else {
confirmCurrentTelemetryPayload(packet[6] & (1 << 7));
confirmCurrentTelemetryPayload(dmaBuffer[6] & (1 << 7));
}
memcpy(payload, &packet[1], 6); // stick data handling is done in expressLrsSetRcDataFromPayload
memcpy((uint8_t *)payload, (uint8_t *)&dmaBuffer[1], 6); // stick data handling is done in expressLrsSetRcDataFromPayload
}
break;
case ELRS_MSP_DATA_PACKET:
processRFMspPacket(packet);
processRFMspPacket(dmaBuffer);
break;
case ELRS_TLM_PACKET:
//not implemented
break;
case ELRS_SYNC_PACKET:
shouldStartTimer = processRFSyncPacket(packet, timeStampMs) && !receiver.inBindingMode;
shouldStartTimer = processRFSyncPacket(dmaBuffer, timeStampMs) && !receiver.inBindingMode;
break;
default:
return RX_SPI_RECEIVED_NONE;
}
// Store the LQ/RSSI/Antenna
receiver.getRFlinkInfo(&receiver.rssi, &receiver.snr);
receiver.getRfLinkInfo(&receiver.rssi, &receiver.snr);
// Received a packet, that's the definition of LQ
lqIncrease();
// Extend sync duration since we've received a packet at this rate
// but do not extend it indefinitely
receiver.rfModeCycleMultiplier = ELRS_MODE_CYCLE_MULTIPLIER_SLOW; //RFModeCycleMultiplierSlow
@ -758,8 +765,6 @@ static rx_spi_received_e processRFPacket(uint8_t *payload, uint32_t timeStampUs)
expressLrsTimerResume();
}
receiver.fhssRequired = true;
return RX_SPI_RECEIVED_DATA;
}
@ -798,10 +803,10 @@ static void cycleRfMode(const uint32_t timeStampMs)
receiver.rfModeCycledAtMs = timeStampMs;
receiver.lastSyncPacketMs = timeStampMs; // reset this variable
receiver.rateIndex = (receiver.rateIndex + 1) % ELRS_RATE_MAX;
setRFLinkRate(receiver.rateIndex); // switch between rates
setRfLinkRate(receiver.rateIndex); // switch between rates
receiver.statsUpdatedAtMs = timeStampMs;
lqReset();
startReceiving();
receiver.startReceiving();
// Switch to FAST_SYNC if not already in it (won't be if was just connected)
receiver.rfModeCycleMultiplier = 1;
@ -815,10 +820,9 @@ static inline void configureReceiverForSX1280(void)
receiver.config = (elrsRxConfigFnPtr) sx1280Config;
receiver.startReceiving = (elrsRxStartReceivingFnPtr) sx1280StartReceiving;
receiver.rxISR = (elrsRxISRFnPtr) sx1280ISR;
receiver.transmitData = (elrsRxTransmitDataFnPtr) sx1280TransmitData;
receiver.receiveData = (elrsRxReceiveDataFnPtr) sx1280ReceiveData;
receiver.getRFlinkInfo = (elrsRxGetRFlinkInfoFnPtr) sx1280GetLastPacketStats;
receiver.setFrequency = (elrsRxSetFrequencyFnPtr) sx1280SetFrequencyReg;
receiver.rxHandleFromTock = (elrsRxHandleFromTockFnPtr) sx1280HandleFromTock;
receiver.rxHandleFromTick = (elrsRxBusyTimeoutFnPtr) sx1280HandleFromTick;
receiver.getRfLinkInfo = (elrsRxgetRfLinkInfoFnPtr) sx1280GetLastPacketStats;
receiver.handleFreqCorrection = (elrsRxHandleFreqCorrectionFnPtr) sx1280AdjustFrequency;
}
#endif
@ -830,10 +834,7 @@ static inline void configureReceiverForSX127x(void)
receiver.config = (elrsRxConfigFnPtr) sx127xConfig;
receiver.startReceiving = (elrsRxStartReceivingFnPtr) sx127xStartReceiving;
receiver.rxISR = (elrsRxISRFnPtr) sx127xISR;
receiver.transmitData = (elrsRxTransmitDataFnPtr) sx127xTransmitData;
receiver.receiveData = (elrsRxReceiveDataFnPtr) sx127xReceiveData;
receiver.getRFlinkInfo = (elrsRxGetRFlinkInfoFnPtr) sx127xGetLastPacketStats;
receiver.setFrequency = (elrsRxSetFrequencyFnPtr) sx127xSetFrequencyReg;
receiver.getRfLinkInfo = (elrsRxgetRfLinkInfoFnPtr) sx127xGetLastPacketStats;
receiver.handleFreqCorrection = (elrsRxHandleFreqCorrectionFnPtr) sx127xAdjustFrequency;
}
#endif
@ -981,7 +982,8 @@ static void handleConfigUpdate(const uint32_t timeStampMs)
if ((timeStampMs - receiver.configCheckedAtMs) > ELRS_CONFIG_CHECK_MS) {
receiver.configCheckedAtMs = timeStampMs;
if (receiver.configChanged) {
writeEEPROM();
saveConfigAndNotify();
receiver.initializeReceiverPending = true;
receiver.configChanged = false;
}
}
@ -1018,7 +1020,7 @@ static void handleLinkStatsUpdate(const uint32_t timeStampMs)
}
}
static void handleTelemetryUpdate(void)
void expressLrsHandleTelemetryUpdate(void)
{
if (receiver.connectionState != ELRS_CONNECTED || (receiver.modParams->tlmInterval == TLM_RATIO_NO_TLM)) {
return;
@ -1041,75 +1043,48 @@ void expressLrsSetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload)
static void enterBindingMode(void)
{
if ((receiver.connectionState == ELRS_CONNECTED) || receiver.inBindingMode) {
// Don't enter binding if:
// - we're already connected
// - we're already binding
return;
}
// Set UID to special binding values
receiver.UID = BindingUID;
crcInitializer = 0;
receiver.inBindingMode = true;
setRFLinkRate(bindingRateIndex);
startReceiving();
setRfLinkRate(bindingRateIndex);
receiver.startReceiving();
}
static uint32_t isrTimeStampUs;
rx_spi_received_e expressLrsDataReceived(uint8_t *payload)
void expressLrsDoTelem(void)
{
rx_spi_received_e result = RX_SPI_RECEIVED_NONE;
expressLrsHandleTelemetryUpdate();
expressLrsSendTelemResp();
if (rxExpressLrsSpiConfig()->domain != ISM2400 && !receiver.didFhss && !expressLrsTelemRespReq() && lqPeriodIsSet()) {
// TODO No need to handle this on SX1280, but will on SX127x
// TODO this needs to be DMA aswell, SX127x unlikely to work right now
receiver.handleFreqCorrection(receiver.freqOffset, receiver.currentFreq); //corrects for RX freq offset
}
}
rx_spi_received_e expressLrsDataReceived(uint8_t *payloadBuffer)
{
payload = payloadBuffer;
rx_spi_received_e rfPacketReturnStatus = RX_SPI_RECEIVED_NONE;
if (!receiver.started && (systemState & SYSTEM_STATE_READY)) {
receiver.started = true;
startReceiving(); // delay receiving after initialization to ensure a clean connect
receiver.startReceiving(); // delay receiving after initialization to ensure a clean connect
}
if (receiver.initializeReceiverPending) {
initializeReceiver();
receiver.initializeReceiverPending = false;
}
if (rxSpiCheckBindRequested(true)) {
enterBindingMode();
}
uint8_t irqReason = receiver.rxISR(&isrTimeStampUs);
if (irqReason == ELRS_DIO_RX_AND_TX_DONE) {
startReceiving();
} else if (irqReason == ELRS_DIO_TX_DONE) {
startReceiving();
} else if (irqReason == ELRS_DIO_RX_DONE) {
result = processRFPacket(payload, isrTimeStampUs);
}
if (receiver.fhssRequired) {
receiver.fhssRequired = false;
bool didFHSS = false;
bool tlmReq = false;
ATOMIC_BLOCK(NVIC_PRIO_TIMER) { // prevent from updating nonce in TICK
didFHSS = handleFHSS();
tlmReq = shouldSendTelemetryResponse();
}
if (tlmReq) {
// in case we miss a packet before TLM we still need to estimate processing time using %
uint32_t processingTime = (micros() - isrTimeStampUs) % receiver.modParams->interval;
if (processingTime < PACKET_HANDLING_TO_TOCK_ISR_DELAY_US && receiver.timerState == ELRS_TIM_LOCKED) {
handleSendTelemetryResponse();
} else {
receiver.alreadyTLMresp = true;
startReceiving();
}
}
if (rxExpressLrsSpiConfig()->domain != ISM2400 && !didFHSS && !tlmReq && lqPeriodIsSet()) {
receiver.handleFreqCorrection(receiver.freqOffset, receiver.currentFreq); //corrects for RX freq offset
}
}
handleTelemetryUpdate();
const uint32_t timeStampMs = millis();
handleConnectionStateUpdate(timeStampMs);
handleConfigUpdate(timeStampMs);
handleLinkStatsUpdate(timeStampMs);
@ -1119,9 +1094,14 @@ rx_spi_received_e expressLrsDataReceived(uint8_t *payload)
DEBUG_SET(DEBUG_RX_EXPRESSLRS_SPI, 2, receiver.snr);
DEBUG_SET(DEBUG_RX_EXPRESSLRS_SPI, 3, receiver.uplinkLQ);
receiver.inBindingMode ? rxSpiLedBlinkBind() : rxSpiLedBlinkRxLoss(result);
receiver.inBindingMode ? rxSpiLedBlinkBind() : rxSpiLedBlinkRxLoss(rfPacketStatus);
return result;
if (rfPacketStatus != RX_SPI_RECEIVED_NONE) {
// A packet has been received since last time
rfPacketReturnStatus = rfPacketStatus;
rfPacketStatus = RX_SPI_RECEIVED_NONE;
}
return rfPacketReturnStatus;
}
void expressLrsStop(void)
@ -1131,4 +1111,10 @@ void expressLrsStop(void)
}
}
void expressLrsISR(bool runAlways)
{
if (runAlways || !expressLrsTimerIsRunning()) {
receiver.rxISR();
}
}
#endif /* USE_RX_EXPRESSLRS */

11
src/main/rx/expresslrs.h
View File

@ -34,4 +34,15 @@
bool expressLrsSpiInit(const struct rxSpiConfig_s *rxConfig, struct rxRuntimeState_s *rxRuntimeState, rxSpiExtiConfig_t *extiConfig);
void expressLrsSetRcDataFromPayload(uint16_t *rcData, const uint8_t *payload);
rx_spi_received_e expressLrsDataReceived(uint8_t *payload);
rx_spi_received_e processRFPacket(volatile uint8_t *payload, uint32_t timeStampUs);
bool expressLrsIsFhssReq(void);
void expressLrsDoTelem(void);
bool expressLrsTelemRespReq(void);
void expressLrsSetRfPacketStatus(rx_spi_received_e status);
uint32_t expressLrsGetCurrentFreq(void);
volatile uint8_t *expressLrsGetRxBuffer(void);
volatile uint8_t *expressLrsGetTelemetryBuffer(void);
volatile uint8_t *expressLrsGetPayloadBuffer(void);
void expressLrsHandleTelemetryUpdate(void);
void expressLrsStop(void);
void expressLrsISR(bool runAlways);

88
src/main/rx/expresslrs_common.c
View File

@ -38,9 +38,9 @@
STATIC_UNIT_TESTED uint16_t crc14tab[ELRS_CRC_LEN] = {0};
static uint8_t volatile FHSSptr = 0;
STATIC_UNIT_TESTED uint8_t FHSSsequence[ELRS_NR_SEQUENCE_ENTRIES] = {0};
static const uint32_t *FHSSfreqs;
static uint8_t volatile fhssIndex = 0;
STATIC_UNIT_TESTED uint8_t fhssSequence[ELRS_NR_SEQUENCE_ENTRIES] = {0};
static const uint32_t *fhssFreqs;
static uint8_t numFreqs = 0; // The number of FHSS frequencies in the table
static uint8_t seqCount = 0;
static uint8_t syncChannel = 0;
@ -96,12 +96,12 @@ elrsRfPerfParams_t rfPerfConfig[][ELRS_RATE_MAX] = {
};
#ifdef USE_RX_SX127X
const uint32_t FHSSfreqsAU433[] = {
const uint32_t fhssFreqsAU433[] = {
FREQ_HZ_TO_REG_VAL_900(433420000),
FREQ_HZ_TO_REG_VAL_900(433920000),
FREQ_HZ_TO_REG_VAL_900(434420000)};
const uint32_t FHSSfreqsAU915[] = {
const uint32_t fhssFreqsAU915[] = {
FREQ_HZ_TO_REG_VAL_900(915500000),
FREQ_HZ_TO_REG_VAL_900(916100000),
FREQ_HZ_TO_REG_VAL_900(916700000),
@ -135,7 +135,7 @@ const uint32_t FHSSfreqsAU915[] = {
* Therefore we simply maximize the usage of available spectrum so laboratory testing of the software won't disturb existing
* 868MHz ISM band traffic too much.
*/
const uint32_t FHSSfreqsEU868[] = {
const uint32_t fhssFreqsEU868[] = {
FREQ_HZ_TO_REG_VAL_900(863275000), // band H1, 863 - 865MHz, 0.1% duty cycle or CSMA techniques, 25mW EIRP
FREQ_HZ_TO_REG_VAL_900(863800000),
FREQ_HZ_TO_REG_VAL_900(864325000),
@ -157,7 +157,7 @@ const uint32_t FHSSfreqsEU868[] = {
* There is currently no mention of Direct-sequence spread spectrum,
* So these frequencies are a subset of Regulatory_Domain_EU_868 frequencies.
*/
const uint32_t FHSSfreqsIN866[] = {
const uint32_t fhssFreqsIN866[] = {
FREQ_HZ_TO_REG_VAL_900(865375000),
FREQ_HZ_TO_REG_VAL_900(865900000),
FREQ_HZ_TO_REG_VAL_900(866425000),
@ -167,14 +167,14 @@ const uint32_t FHSSfreqsIN866[] = {
* Note: As is the case with the 868Mhz band, these frequencies only comply to the license free portion
* of the spectrum, nothing else. As such, these are likely illegal to use.
*/
const uint32_t FHSSfreqsEU433[] = {
const uint32_t fhssFreqsEU433[] = {
FREQ_HZ_TO_REG_VAL_900(433100000),
FREQ_HZ_TO_REG_VAL_900(433925000),
FREQ_HZ_TO_REG_VAL_900(434450000)};
/* Very definitely not fully checked. An initial pass at increasing the hops
*/
const uint32_t FHSSfreqsFCC915[] = {
const uint32_t fhssFreqsFCC915[] = {
FREQ_HZ_TO_REG_VAL_900(903500000),
FREQ_HZ_TO_REG_VAL_900(904100000),
FREQ_HZ_TO_REG_VAL_900(904700000),
@ -226,7 +226,7 @@ const uint32_t FHSSfreqsFCC915[] = {
FREQ_HZ_TO_REG_VAL_900(926900000)};
#endif
#ifdef USE_RX_SX1280
const uint32_t FHSSfreqsISM2400[] = {
const uint32_t fhssFreqsISM2400[] = {
FREQ_HZ_TO_REG_VAL_24(2400400000),
FREQ_HZ_TO_REG_VAL_24(2401400000),
FREQ_HZ_TO_REG_VAL_24(2402400000),
@ -344,70 +344,70 @@ uint16_t calcCrc14(uint8_t *data, uint8_t len, uint16_t crc)
return crc & 0x3FFF;
}
static void initializeFHSSFrequencies(const elrsFreqDomain_e dom) {
static void initializeFhssFrequencies(const elrsFreqDomain_e dom) {
switch (dom) {
#ifdef USE_RX_SX127X
case AU433:
FHSSfreqs = FHSSfreqsAU433;
numFreqs = sizeof(FHSSfreqsAU433) / sizeof(uint32_t);
fhssFreqs = fhssFreqsAU433;
numFreqs = sizeof(fhssFreqsAU433) / sizeof(uint32_t);
break;
case AU915:
FHSSfreqs = FHSSfreqsAU915;
numFreqs = sizeof(FHSSfreqsAU915) / sizeof(uint32_t);
fhssFreqs = fhssFreqsAU915;
numFreqs = sizeof(fhssFreqsAU915) / sizeof(uint32_t);
break;
case EU433:
FHSSfreqs = FHSSfreqsEU433;
numFreqs = sizeof(FHSSfreqsEU433) / sizeof(uint32_t);
fhssFreqs = fhssFreqsEU433;
numFreqs = sizeof(fhssFreqsEU433) / sizeof(uint32_t);
break;
case EU868:
FHSSfreqs = FHSSfreqsEU868;
numFreqs = sizeof(FHSSfreqsEU868) / sizeof(uint32_t);
fhssFreqs = fhssFreqsEU868;
numFreqs = sizeof(fhssFreqsEU868) / sizeof(uint32_t);
break;
case IN866:
FHSSfreqs = FHSSfreqsIN866;
numFreqs = sizeof(FHSSfreqsIN866) / sizeof(uint32_t);
fhssFreqs = fhssFreqsIN866;
numFreqs = sizeof(fhssFreqsIN866) / sizeof(uint32_t);
break;
case FCC915:
FHSSfreqs = FHSSfreqsFCC915;
numFreqs = sizeof(FHSSfreqsFCC915) / sizeof(uint32_t);
fhssFreqs = fhssFreqsFCC915;
numFreqs = sizeof(fhssFreqsFCC915) / sizeof(uint32_t);
break;
#endif
#ifdef USE_RX_SX1280
case ISM2400:
FHSSfreqs = FHSSfreqsISM2400;
numFreqs = sizeof(FHSSfreqsISM2400) / sizeof(uint32_t);
fhssFreqs = fhssFreqsISM2400;
numFreqs = sizeof(fhssFreqsISM2400) / sizeof(uint32_t);
break;
#endif
default:
FHSSfreqs = NULL;
fhssFreqs = NULL;
numFreqs = 0;
}
}
uint32_t getInitialFreq(const int32_t freqCorrection)
uint32_t fhssGetInitialFreq(const int32_t freqCorrection)
{
return FHSSfreqs[syncChannel] - freqCorrection;
return fhssFreqs[syncChannel] - freqCorrection;
}
uint8_t getFHSSNumEntries(void)
uint8_t fhssGetNumEntries(void)
{
return numFreqs;
}
uint8_t FHSSgetCurrIndex(void)
uint8_t fhssGetCurrIndex(void)
{
return FHSSptr;
return fhssIndex;
}
void FHSSsetCurrIndex(const uint8_t value)
void fhssSetCurrIndex(const uint8_t value)
{
FHSSptr = value % seqCount;
fhssIndex = value % seqCount;
}
uint32_t FHSSgetNextFreq(const int32_t freqCorrection)
uint32_t fhssGetNextFreq(const int32_t freqCorrection)
{
FHSSptr = (FHSSptr + 1) % seqCount;
return FHSSfreqs[FHSSsequence[FHSSptr]] - freqCorrection;
fhssIndex = (fhssIndex + 1) % seqCount;
return fhssFreqs[fhssSequence[fhssIndex]] - freqCorrection;
}
static uint32_t seed = 0;
@ -435,11 +435,11 @@ Approach:
another random entry, excluding the sync channel.
*/
void FHSSrandomiseFHSSsequence(const uint8_t UID[], const elrsFreqDomain_e dom)
void fhssGenSequence(const uint8_t UID[], const elrsFreqDomain_e dom)
{
seed = ((long)UID[2] << 24) + ((long)UID[3] << 16) + ((long)UID[4] << 8) + UID[5];
initializeFHSSFrequencies(dom);
initializeFhssFrequencies(dom);
seqCount = (256 / MAX(numFreqs, 1)) * numFreqs;
@ -448,11 +448,11 @@ void FHSSrandomiseFHSSsequence(const uint8_t UID[], const elrsFreqDomain_e dom)
// initialize the sequence array
for (uint8_t i = 0; i < seqCount; i++) {
if (i % numFreqs == 0) {
FHSSsequence[i] = syncChannel;
fhssSequence[i] = syncChannel;
} else if (i % numFreqs == syncChannel) {
FHSSsequence[i] = 0;
fhssSequence[i] = 0;
} else {
FHSSsequence[i] = i % numFreqs;
fhssSequence[i] = i % numFreqs;
}
}
@ -463,9 +463,9 @@ void FHSSrandomiseFHSSsequence(const uint8_t UID[], const elrsFreqDomain_e dom)
uint8_t rand = rngN(numFreqs - 1) + 1; // random number between 1 and numFreqs
// switch this entry and another random entry in the same block
uint8_t temp = FHSSsequence[i];
FHSSsequence[i] = FHSSsequence[offset + rand];
FHSSsequence[offset + rand] = temp;
uint8_t temp = fhssSequence[i];
fhssSequence[i] = fhssSequence[offset + rand];
fhssSequence[offset + rand] = temp;
}
}
}

18
src/main/rx/expresslrs_common.h
View File

@ -133,10 +133,12 @@ typedef struct elrsRfPerfParams_s {
typedef bool (*elrsRxInitFnPtr)(IO_t resetPin, IO_t busyPin);
typedef void (*elrsRxConfigFnPtr)(const uint8_t bw, const uint8_t sf, const uint8_t cr, const uint32_t freq, const uint8_t preambleLen, const bool iqInverted);
typedef void (*elrsRxStartReceivingFnPtr)(void);
typedef uint8_t (*elrsRxISRFnPtr)(timeUs_t *timeStamp);
typedef void (*elrsRxISRFnPtr)(void);
typedef void (*elrsRxHandleFromTockFnPtr)(void);
typedef bool (*elrsRxBusyTimeoutFnPtr)(void);
typedef void (*elrsRxTransmitDataFnPtr)(const uint8_t *data, const uint8_t length);
typedef void (*elrsRxReceiveDataFnPtr)(uint8_t *data, const uint8_t length);
typedef void (*elrsRxGetRFlinkInfoFnPtr)(int8_t *rssi, int8_t *snr);
typedef void (*elrsRxgetRfLinkInfoFnPtr)(int8_t *rssi, int8_t *snr);
typedef void (*elrsRxSetFrequencyFnPtr)(const uint32_t freq);
typedef void (*elrsRxHandleFreqCorrectionFnPtr)(int32_t offset, const uint32_t freq);
@ -146,12 +148,12 @@ extern elrsRfPerfParams_t rfPerfConfig[][ELRS_RATE_MAX];
void generateCrc14Table(void);
uint16_t calcCrc14(uint8_t *data, uint8_t len, uint16_t crc);
uint32_t getInitialFreq(const int32_t freqCorrection);
uint8_t getFHSSNumEntries(void);
uint8_t FHSSgetCurrIndex(void);
void FHSSsetCurrIndex(const uint8_t value);
uint32_t FHSSgetNextFreq(const int32_t freqCorrection);
void FHSSrandomiseFHSSsequence(const uint8_t UID[], const elrsFreqDomain_e dom);
uint32_t fhssGetInitialFreq(const int32_t freqCorrection);
uint8_t fhssGetNumEntries(void);
uint8_t fhssGetCurrIndex(void);
void fhssSetCurrIndex(const uint8_t value);
uint32_t fhssGetNextFreq(const int32_t freqCorrection);
void fhssGenSequence(const uint8_t UID[], const elrsFreqDomain_e dom);
uint8_t tlmRatioEnumToValue(const elrsTlmRatio_e enumval);
uint16_t rateEnumToHz(const elrsRfRate_e eRate);
uint16_t txPowerIndexToValue(const uint8_t index);

12
src/main/rx/expresslrs_impl.h
View File

@ -71,8 +71,8 @@ typedef struct elrsReceiver_s {
uint8_t uplinkLQ;
bool alreadyFHSS;
bool alreadyTLMresp;
bool alreadyFhss;
bool alreadyTelemResp;
bool lockRFmode;
bool started;
@ -93,7 +93,9 @@ typedef struct elrsReceiver_s {
bool configChanged;
bool inBindingMode;
volatile bool initializeReceiverPending;
volatile bool fhssRequired;
volatile bool didFhss;
uint32_t statsUpdatedAtMs;
@ -101,9 +103,9 @@ typedef struct elrsReceiver_s {
elrsRxConfigFnPtr config;
elrsRxStartReceivingFnPtr startReceiving;
elrsRxISRFnPtr rxISR;
elrsRxTransmitDataFnPtr transmitData;
elrsRxReceiveDataFnPtr receiveData;
elrsRxGetRFlinkInfoFnPtr getRFlinkInfo;
elrsRxHandleFromTockFnPtr rxHandleFromTock;
elrsRxBusyTimeoutFnPtr rxHandleFromTick;
elrsRxgetRfLinkInfoFnPtr getRfLinkInfo;
elrsRxSetFrequencyFnPtr setFrequency;
elrsRxHandleFreqCorrectionFnPtr handleFreqCorrection;

14
src/main/rx/rx.c
View File

@ -469,13 +469,21 @@ void rxSetUplinkTxPwrMw(uint16_t uplinkTxPwrMwValue)
#endif
bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs)
{
UNUSED(currentTimeUs);
UNUSED(currentDeltaTimeUs);
return taskUpdateRxMainInProgress() || rxDataProcessingRequired || auxiliaryProcessingRequired || !failsafeIsReceivingRxData();
}
FAST_CODE_NOINLINE void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs)
{
bool signalReceived = false;
bool useDataDrivenProcessing = true;
if (taskUpdateRxMainInProgress()) {
// There are more states to process
return true;
// No need to check for new data as a packet is being processed already
return;
}
switch (rxRuntimeState.rxProvider) {
@ -535,8 +543,6 @@ bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs)
if ((signalReceived && useDataDrivenProcessing) || cmpTimeUs(currentTimeUs, rxNextUpdateAtUs) > 0) {
rxDataProcessingRequired = true;
}
return rxDataProcessingRequired || auxiliaryProcessingRequired; // data driven or 50Hz
}
#if defined(USE_PWM) || defined(USE_PPM)

1
src/main/rx/rx.h
View File

@ -177,6 +177,7 @@ extern rxRuntimeState_t rxRuntimeState; //!!TODO remove this extern, only needed
void rxInit(void);
void rxProcessPending(bool state);
bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs);
void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs);
bool rxIsReceivingSignal(void);
bool rxAreFlightChannelsValid(void);
bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs);

24
src/main/rx/rx_spi.c
View File

@ -71,6 +71,11 @@ static protocolProcessFrameFnPtr protocolProcessFrame;
static protocolSetRcDataFromPayloadFnPtr protocolSetRcDataFromPayload;
static protocolStopFnPtr protocolStop = nullProtocolStop;
static rxSpiExtiConfig_t extiConfig = {
.ioConfig = IOCFG_IN_FLOATING,
.trigger = BETAFLIGHT_EXTI_TRIGGER_RISING,
};
STATIC_UNIT_TESTED float rxSpiReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t channel)
{
STATIC_ASSERT(NRF24L01_MAX_PAYLOAD_SIZE <= RX_SPI_MAX_PAYLOAD_SIZE, NRF24L01_MAX_PAYLOAD_SIZE_larger_than_RX_SPI_MAX_PAYLOAD_SIZE);
@ -194,10 +199,8 @@ STATIC_UNIT_TESTED bool rxSpiSetProtocol(rx_spi_protocol_e protocol)
return true;
}
/*
/* Called by scheduler immediately after real-time tasks
* Returns true if the RX has received new data.
* Called from updateRx in rx.c, updateRx called from taskUpdateRxCheck.
* If taskUpdateRxCheck returns true, then taskUpdateRxMain will shortly be called.
*/
static uint8_t rxSpiFrameStatus(rxRuntimeState_t *rxRuntimeState)
{
@ -218,7 +221,10 @@ static uint8_t rxSpiFrameStatus(rxRuntimeState_t *rxRuntimeState)
return status;
}
/* Called from updateRx in rx.c, updateRx called from taskUpdateRxCheck.
* If taskUpdateRxCheck returns true, then taskUpdateRxMain will shortly be called.
*
*/
static bool rxSpiProcessFrame(const rxRuntimeState_t *rxRuntimeState)
{
UNUSED(rxRuntimeState);
@ -253,11 +259,6 @@ bool rxSpiInit(const rxSpiConfig_t *rxSpiConfig, rxRuntimeState_t *rxRuntimeStat
return false;
}
rxSpiExtiConfig_t extiConfig = {
.ioConfig = IOCFG_IN_FLOATING,
.trigger = BETAFLIGHT_EXTI_TRIGGER_RISING,
};
ret = protocolInit(rxSpiConfig, rxRuntimeState, &extiConfig);
if (rxSpiExtiConfigured()) {
@ -276,6 +277,11 @@ bool rxSpiInit(const rxSpiConfig_t *rxSpiConfig, rxRuntimeState_t *rxRuntimeStat
return ret;
}
void rxSpiEnableExti(void)
{
rxSpiExtiInit(extiConfig.ioConfig, extiConfig.trigger);
}
void rxSpiStop(void)
{
protocolStop();

27
src/main/scheduler/scheduler.c
View File

@ -42,15 +42,17 @@
#include "fc/core.h"
#include "fc/tasks.h"
#include "rx/rx.h"
#include "flight/failsafe.h"
#include "scheduler.h"
#include "sensors/gyro_init.h"
// DEBUG_SCHEDULER, timings for:
// 0 - gyroUpdate()
// 1 - pidController()
// 0 - Average time spent executing check function
// 1 - Time spent priortising
// 2 - time spent in scheduler
// 3 - time spent executing check function
// DEBUG_SCHEDULER_DETERMINISM, requires USE_LATE_TASK_STATISTICS to be defined
// 0 - Gyro task start cycle time in 10th of a us
@ -104,6 +106,8 @@ static int16_t taskCount = 0;
static uint32_t nextTimingCycles;
#endif
static timeMs_t lastFailsafeCheckMs = 0;
// No need for a linked list for the queue, since items are only inserted at startup
STATIC_UNIT_TESTED FAST_DATA_ZERO_INIT task_t* taskQueueArray[TASK_COUNT + 1]; // extra item for NULL pointer at end of queue
@ -498,6 +502,20 @@ FAST_CODE void scheduler(void)
if (pidLoopReady()) {
taskExecutionTimeUs += schedulerExecuteTask(getTask(TASK_PID), currentTimeUs);
}
if (rxFrameReady()) {
// Check to for incoming RX data. Don't do this in the checker as that is called repeatedly within
// a given gyro loop, and ELRS takes a long time to process this and so can only be safely processed
// before the checkers
rxFrameCheck(currentTimeUs, cmpTimeUs(currentTimeUs, getTask(TASK_RX)->lastExecutedAtUs));
}
// Check for failsafe conditions without reliance on the RX task being well behaved
if (cmp32(millis(), lastFailsafeCheckMs) > PERIOD_RXDATA_FAILURE) {
// This is very low cost taking less that 4us every 200ms
failsafeCheckDataFailurePeriod();
failsafeUpdateState();
lastFailsafeCheckMs = millis();
}
#if defined(USE_LATE_TASK_STATISTICS)
// % CPU busy
@ -587,9 +605,6 @@ FAST_CODE void scheduler(void)
task->dynamicPriority = 1 + task->attribute->staticPriority * task->taskAgePeriods;
} else if (task->attribute->checkFunc(currentTimeUs, cmpTimeUs(currentTimeUs, task->lastExecutedAtUs))) {
const uint32_t checkFuncExecutionTimeUs = cmpTimeUs(micros(), currentTimeUs);
#if !defined(UNIT_TEST)
DEBUG_SET(DEBUG_SCHEDULER, 3, checkFuncExecutionTimeUs);
#endif
checkFuncMovingSumExecutionTimeUs += checkFuncExecutionTimeUs - checkFuncMovingSumExecutionTimeUs / TASK_STATS_MOVING_SUM_COUNT;
checkFuncMovingSumDeltaTimeUs += task->taskLatestDeltaTimeUs - checkFuncMovingSumDeltaTimeUs / TASK_STATS_MOVING_SUM_COUNT;
checkFuncTotalExecutionTimeUs += checkFuncExecutionTimeUs; // time consumed by scheduler + task

0
src/main/startup/startup_stm32g474xx.s Executable file → Normal file
View File

2
src/main/target/CRAZYBEEF4SX1280/target.h
View File

@ -125,7 +125,7 @@
#define USE_LED_STRIP
#define ENABLE_DSHOT_DMAR DSHOT_DMAR_AUTO
#define DSHOT_BITBANG_DEFAULT DSHOT_BITBANG_ON
#define DSHOT_BITBANG_DEFAULT DSHOT_BITBANG_OFF
#define USE_PINIO
//#define PINIO1_PIN PB5 // VTX switcher

1
src/test/unit/link_quality_unittest.cc
View File

@ -494,6 +494,7 @@ extern "C" {
bool areMotorsRunning(void){ return true; }
bool pidOsdAntiGravityActive(void) { return false; }
bool failsafeIsActive(void) { return false; }
bool failsafeIsReceivingRxData(void) { return true; }
bool gpsIsHealthy(void) { return true; }
bool gpsRescueIsConfigured(void) { return false; }
int8_t calculateThrottlePercent(void) { return 0; }

1
src/test/unit/rx_ranges_unittest.cc
View File

@ -107,6 +107,7 @@ extern "C" {
void failsafeOnRxSuspend(uint32_t ) {}
void failsafeOnRxResume(void) {}
bool failsafeIsReceivingRxData(void) { return true; }
bool taskUpdateRxMainInProgress() { return true; }
uint32_t micros(void) { return 0; }

1
src/test/unit/rx_rx_unittest.cc
View File

@ -210,6 +210,7 @@ extern "C" {
void failsafeOnRxSuspend(uint32_t ) {}
void failsafeOnRxResume(void) {}
bool failsafeIsReceivingRxData(void) { return true; }
uint32_t micros(void) { return 0; }
uint32_t millis(void) { return 0; }

23
src/test/unit/rx_spi_expresslrs_unittest.cc
View File

@ -45,7 +45,7 @@ extern "C" {
#include "drivers/rx/rx_sx127x.h"
#include "drivers/rx/rx_sx1280.h"
extern uint8_t FHSSsequence[ELRS_NR_SEQUENCE_ENTRIES];
extern uint8_t fhssSequence[ELRS_NR_SEQUENCE_ENTRIES];
extern uint16_t crc14tab[ELRS_CRC_LEN];
extern elrsReceiver_t receiver;
@ -164,14 +164,14 @@ TEST(RxSpiExpressLrsUnitTest, TestFHSSTable)
}
};
FHSSrandomiseFHSSsequence(UID, ISM2400);
fhssGenSequence(UID, ISM2400);
for (int i = 0; i < ELRS_NR_SEQUENCE_ENTRIES; i++) {
EXPECT_EQ(expectedSequence[0][i], FHSSsequence[i]);
EXPECT_EQ(expectedSequence[0][i], fhssSequence[i]);
}
FHSSrandomiseFHSSsequence(UID, FCC915);
fhssGenSequence(UID, FCC915);
for (int i = 0; i < ELRS_NR_SEQUENCE_ENTRIES; i++) {
EXPECT_EQ(expectedSequence[1][i], FHSSsequence[i]);
EXPECT_EQ(expectedSequence[1][i], fhssSequence[i]);
}
}
@ -390,7 +390,8 @@ extern "C" {
IO_t IOGetByTag(ioTag_t ) { return (IO_t)1; }
void IOHi(IO_t ) {}
void IOLo(IO_t ) {}
void writeEEPROM(void) {}
void saveConfigAndNotify(void) {}
void rxSpiCommonIOInit(const rxSpiConfig_t *) {}
void rxSpiLedBlinkRxLoss(rx_spi_received_e ) {}
@ -404,11 +405,10 @@ extern "C" {
bool sx1280IsBusy(void) { return false; }
void sx1280Config(const sx1280LoraBandwidths_e , const sx1280LoraSpreadingFactors_e , const sx1280LoraCodingRates_e , const uint32_t , const uint8_t , const bool ) {}
void sx1280StartReceiving(void) {}
uint8_t sx1280ISR(uint32_t *timestamp)
{
*timestamp = 0;
return 0;
}
void sx1280ISR(void) {}
bool rxSpiGetExtiState(void) { return false; }
void sx1280HandleFromTock(void) {}
bool sx1280HandleFromTick(void) { return false; }
void sx1280TransmitData(const uint8_t *, const uint8_t ) {}
void sx1280ReceiveData(uint8_t *, const uint8_t ) {}
void sx1280SetFrequencyReg(const uint32_t ) {}
@ -469,5 +469,4 @@ extern "C" {
void getCurrentTelemetryPayload(uint8_t *, uint8_t *, uint8_t **) {}
void confirmCurrentTelemetryPayload(const bool ) {}
void updateTelemetryRate(const uint16_t , const uint8_t , const uint8_t ) {}
}

6
src/test/unit/scheduler_unittest.cc
View File

@ -67,9 +67,13 @@ extern "C" {
int16_t debug[1];
uint8_t debugMode = 0;
bool rxFrameReady(void) { return 0; }
void rxFrameCheck(timeUs_t, timeDelta_t) {}
// set up micros() to simulate time
uint32_t simulatedTime = 0;
uint32_t micros(void) { return simulatedTime; }
uint32_t millis(void) { return simulatedTime/1000; } // Note simplistic mapping suitable only for short unit tests
uint32_t clockCyclesToMicros(uint32_t x) { return x/10;}
int32_t clockCyclesTo10thMicros(int32_t x) { return x;}
uint32_t clockMicrosToCycles(uint32_t x) { return x*10;}
@ -78,6 +82,8 @@ extern "C" {
// set up tasks to take a simulated representative time to execute
bool gyroFilterReady(void) { return taskFilterReady; }
bool pidLoopReady(void) { return taskPidReady; }
void failsafeCheckDataFailurePeriod(void) {}
void failsafeUpdateState(void) {}
void taskGyroSample(timeUs_t) { simulatedTime += TEST_GYRO_SAMPLE_TIME; taskGyroRan = true; }
void taskFiltering(timeUs_t) { simulatedTime += TEST_FILTERING_TIME; taskFilterRan = true; }
void taskMainPidLoop(timeUs_t) { simulatedTime += TEST_PID_LOOP_TIME; taskPidRan = true; }

13
unified_targets/configs/CRAZYBEEF4SX1280.config
View File

@ -1,13 +0,0 @@
board_name CRAZYBEEF4SX1280
manufacturer_id HAMO
resource RX_SPI_CS 1 A15
resource RX_SPI_EXTI 1 C14
resource RX_SPI_BIND 1 B02
resource RX_SPI_LED 1 B09
resource RX_SPI_EXPRESSLRS_RESET 1 A08
resource RX_SPI_EXPRESSLRS_BUSY 1 A13
set expresslrs_domain = ISM2400
set expresslrs_rate_index = 0
set expresslrs_switch_mode = HYBRID