mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-24 00:35:39 +03:00
Improve performance for ELRS SX1280 SPI callbacks and SPI/ELRS handlers. (#11460)
* SPI - Mark the SPI IRQ handler as FAST_IRQ_HANDLER. Since it's used by both the gyro code, at 8k (or 2x8k on dual gyro boards) having it in RAM removes a significant amount of potential flash latency. It is also used by the ELRS SPI code. The ELRS code runs at 500hz, but each cycle uses multiple SPI transfers, for clear irq, read, start-recieving, change-frequency, etc. I.e. invokd at least 1000hz in addition to gyro reads. * ELRS - Move some frequently used functions to RAM and mark some irq handlers as FAST. * SPI/LL - Move some frequently called code to FAST_CODE. Also remove some invalid characters that Eclipse with encoding as UTF-8 complained about. * SPI - Mark the spiRx and spiTx dma handlers as FAST_IRQ_HANDLER. * Filter - move nullFilterApply out of FAST_CODE. Since it doesn't do anything, it doesn't need to be fast. Instead we keep more `fast` RAM for other code that really benefits from being in fast RAM. There is a slight penalty to jump into slower RAM. * Gyro - Move `performGyroCalibration`out of 'fast' ram. On F7X2 it was being inlined, saved 478 bytes of ITCM. * Prevent handling of crash recovery handling, not detection of crash recovery, from being inlined to save ITCM for code that runs more frequently.
This commit is contained in:
parent
4494805202
commit
738ed32cff
6 changed files with 22 additions and 22 deletions
|
@ -33,7 +33,7 @@
|
||||||
|
|
||||||
// NULL filter
|
// NULL filter
|
||||||
|
|
||||||
FAST_CODE float nullFilterApply(filter_t *filter, float input)
|
float nullFilterApply(filter_t *filter, float input)
|
||||||
{
|
{
|
||||||
UNUSED(filter);
|
UNUSED(filter);
|
||||||
return input;
|
return input;
|
||||||
|
|
|
@ -378,7 +378,7 @@ uint32_t spiCalculateClock(uint16_t spiClkDivisor)
|
||||||
}
|
}
|
||||||
|
|
||||||
// Interrupt handler for SPI receive DMA completion
|
// Interrupt handler for SPI receive DMA completion
|
||||||
static void spiIrqHandler(const extDevice_t *dev)
|
FAST_IRQ_HANDLER static void spiIrqHandler(const extDevice_t *dev)
|
||||||
{
|
{
|
||||||
busDevice_t *bus = dev->bus;
|
busDevice_t *bus = dev->bus;
|
||||||
busSegment_t *nextSegment;
|
busSegment_t *nextSegment;
|
||||||
|
@ -447,7 +447,7 @@ static void spiIrqHandler(const extDevice_t *dev)
|
||||||
}
|
}
|
||||||
|
|
||||||
// Interrupt handler for SPI receive DMA completion
|
// Interrupt handler for SPI receive DMA completion
|
||||||
static void spiRxIrqHandler(dmaChannelDescriptor_t* descriptor)
|
FAST_IRQ_HANDLER static void spiRxIrqHandler(dmaChannelDescriptor_t* descriptor)
|
||||||
{
|
{
|
||||||
const extDevice_t *dev = (const extDevice_t *)descriptor->userParam;
|
const extDevice_t *dev = (const extDevice_t *)descriptor->userParam;
|
||||||
|
|
||||||
|
@ -484,7 +484,7 @@ static void spiRxIrqHandler(dmaChannelDescriptor_t* descriptor)
|
||||||
|
|
||||||
#if !defined(STM32G4) && !defined(STM32H7)
|
#if !defined(STM32G4) && !defined(STM32H7)
|
||||||
// Interrupt handler for SPI transmit DMA completion
|
// Interrupt handler for SPI transmit DMA completion
|
||||||
static void spiTxIrqHandler(dmaChannelDescriptor_t* descriptor)
|
FAST_IRQ_HANDLER static void spiTxIrqHandler(dmaChannelDescriptor_t* descriptor)
|
||||||
{
|
{
|
||||||
const extDevice_t *dev = (const extDevice_t *)descriptor->userParam;
|
const extDevice_t *dev = (const extDevice_t *)descriptor->userParam;
|
||||||
|
|
||||||
|
|
|
@ -225,7 +225,7 @@ void spiInternalResetStream(dmaChannelDescriptor_t *descriptor)
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
static bool spiInternalReadWriteBufPolled(SPI_TypeDef *instance, const uint8_t *txData, uint8_t *rxData, int len)
|
FAST_CODE static bool spiInternalReadWriteBufPolled(SPI_TypeDef *instance, const uint8_t *txData, uint8_t *rxData, int len)
|
||||||
{
|
{
|
||||||
#if defined(STM32H7)
|
#if defined(STM32H7)
|
||||||
LL_SPI_SetTransferSize(instance, len);
|
LL_SPI_SetTransferSize(instance, len);
|
||||||
|
@ -424,7 +424,7 @@ void spiInternalStartDMA(const extDevice_t *dev)
|
||||||
|
|
||||||
/* Note from AN4031
|
/* Note from AN4031
|
||||||
*
|
*
|
||||||
* If the user enables the used peripheral before the corresponding DMA stream, a “FEIF”
|
* If the user enables the used peripheral before the corresponding DMA stream, a FEIF
|
||||||
* (FIFO Error Interrupt Flag) may be set due to the fact the DMA is not ready to provide
|
* (FIFO Error Interrupt Flag) may be set due to the fact the DMA is not ready to provide
|
||||||
* the first required data to the peripheral (in case of memory-to-peripheral transfer).
|
* the first required data to the peripheral (in case of memory-to-peripheral transfer).
|
||||||
*/
|
*/
|
||||||
|
@ -464,7 +464,7 @@ void spiInternalStartDMA(const extDevice_t *dev)
|
||||||
|
|
||||||
/* Note from AN4031
|
/* Note from AN4031
|
||||||
*
|
*
|
||||||
* If the user enables the used peripheral before the corresponding DMA stream, a “FEIF”
|
* If the user enables the used peripheral before the corresponding DMA stream, a FEIF
|
||||||
* (FIFO Error Interrupt Flag) may be set due to the fact the DMA is not ready to provide
|
* (FIFO Error Interrupt Flag) may be set due to the fact the DMA is not ready to provide
|
||||||
* the first required data to the peripheral (in case of memory-to-peripheral transfer).
|
* the first required data to the peripheral (in case of memory-to-peripheral transfer).
|
||||||
*/
|
*/
|
||||||
|
@ -533,7 +533,7 @@ void spiInternalStopDMA (const extDevice_t *dev)
|
||||||
}
|
}
|
||||||
|
|
||||||
// DMA transfer setup and start
|
// DMA transfer setup and start
|
||||||
void spiSequenceStart(const extDevice_t *dev)
|
FAST_CODE void spiSequenceStart(const extDevice_t *dev)
|
||||||
{
|
{
|
||||||
busDevice_t *bus = dev->bus;
|
busDevice_t *bus = dev->bus;
|
||||||
SPI_TypeDef *instance = bus->busType_u.spi.instance;
|
SPI_TypeDef *instance = bus->busType_u.spi.instance;
|
||||||
|
|
|
@ -77,7 +77,7 @@ bool sx1280IsBusy(void)
|
||||||
return IORead(busy);
|
return IORead(busy);
|
||||||
}
|
}
|
||||||
|
|
||||||
static bool sx1280PollBusy(void)
|
FAST_CODE static bool sx1280PollBusy(void)
|
||||||
{
|
{
|
||||||
uint32_t startTime = micros();
|
uint32_t startTime = micros();
|
||||||
while (IORead(busy)) {
|
while (IORead(busy)) {
|
||||||
|
@ -90,7 +90,7 @@ static bool sx1280PollBusy(void)
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
static bool sx1280MarkBusy(void)
|
FAST_CODE static bool sx1280MarkBusy(void)
|
||||||
{
|
{
|
||||||
// Check that there isn't already a sequence of accesses to the SX1280 in progress
|
// Check that there isn't already a sequence of accesses to the SX1280 in progress
|
||||||
ATOMIC_BLOCK(NVIC_PRIO_MAX) {
|
ATOMIC_BLOCK(NVIC_PRIO_MAX) {
|
||||||
|
@ -110,7 +110,7 @@ static void sx1280ClearBusyFn(void)
|
||||||
}
|
}
|
||||||
|
|
||||||
// Switch to waiting for busy interrupt
|
// Switch to waiting for busy interrupt
|
||||||
static bool sx1280EnableBusy(void)
|
FAST_CODE static bool sx1280EnableBusy(void)
|
||||||
{
|
{
|
||||||
if (!sx1280MarkBusy()) {
|
if (!sx1280MarkBusy()) {
|
||||||
return false;
|
return false;
|
||||||
|
@ -569,7 +569,7 @@ static void sx1280SendTelemetryBuffer(extiCallbackRec_t *cb);
|
||||||
static busStatus_e sx1280TelemetryComplete(uint32_t arg);
|
static busStatus_e sx1280TelemetryComplete(uint32_t arg);
|
||||||
static void sx1280StartTransmittingDMA(extiCallbackRec_t *cb);
|
static void sx1280StartTransmittingDMA(extiCallbackRec_t *cb);
|
||||||
|
|
||||||
void sx1280ISR(void)
|
FAST_IRQ_HANDLER void sx1280ISR(void)
|
||||||
{
|
{
|
||||||
// Only attempt to access the SX1280 if it is currently idle to avoid any race condition
|
// Only attempt to access the SX1280 if it is currently idle to avoid any race condition
|
||||||
ATOMIC_BLOCK(NVIC_PRIO_MAX) {
|
ATOMIC_BLOCK(NVIC_PRIO_MAX) {
|
||||||
|
@ -584,7 +584,7 @@ void sx1280ISR(void)
|
||||||
|
|
||||||
// Next, the reason for the IRQ must be read
|
// Next, the reason for the IRQ must be read
|
||||||
|
|
||||||
static void sx1280IrqGetStatus(extiCallbackRec_t *cb)
|
FAST_IRQ_HANDLER static void sx1280IrqGetStatus(extiCallbackRec_t *cb)
|
||||||
{
|
{
|
||||||
extDevice_t *dev = rxSpiGetDevice();
|
extDevice_t *dev = rxSpiGetDevice();
|
||||||
|
|
||||||
|
@ -605,7 +605,7 @@ static void sx1280IrqGetStatus(extiCallbackRec_t *cb)
|
||||||
|
|
||||||
// Read the IRQ status, and save it to irqStatus variable
|
// Read the IRQ status, and save it to irqStatus variable
|
||||||
|
|
||||||
static busStatus_e sx1280IrqStatusRead(uint32_t arg)
|
FAST_IRQ_HANDLER static busStatus_e sx1280IrqStatusRead(uint32_t arg)
|
||||||
{
|
{
|
||||||
extDevice_t *dev = (extDevice_t *)arg;
|
extDevice_t *dev = (extDevice_t *)arg;
|
||||||
|
|
||||||
|
@ -625,7 +625,7 @@ static busStatus_e sx1280IrqStatusRead(uint32_t arg)
|
||||||
|
|
||||||
// Clear the IRQ bit in the Radio registers
|
// Clear the IRQ bit in the Radio registers
|
||||||
|
|
||||||
static void sx1280IrqClearStatus(extiCallbackRec_t *cb)
|
FAST_IRQ_HANDLER static void sx1280IrqClearStatus(extiCallbackRec_t *cb)
|
||||||
{
|
{
|
||||||
extDevice_t *dev = rxSpiGetDevice();
|
extDevice_t *dev = rxSpiGetDevice();
|
||||||
|
|
||||||
|
@ -647,7 +647,7 @@ static void sx1280IrqClearStatus(extiCallbackRec_t *cb)
|
||||||
}
|
}
|
||||||
|
|
||||||
// Callback follow clear of IRQ status
|
// Callback follow clear of IRQ status
|
||||||
static busStatus_e sx1280IrqCmdComplete(uint32_t arg)
|
FAST_IRQ_HANDLER static busStatus_e sx1280IrqCmdComplete(uint32_t arg)
|
||||||
{
|
{
|
||||||
UNUSED(arg);
|
UNUSED(arg);
|
||||||
|
|
||||||
|
@ -657,7 +657,7 @@ static busStatus_e sx1280IrqCmdComplete(uint32_t arg)
|
||||||
}
|
}
|
||||||
|
|
||||||
// Process IRQ status
|
// Process IRQ status
|
||||||
static void sx1280ProcessIrq(extiCallbackRec_t *cb)
|
FAST_IRQ_HANDLER static void sx1280ProcessIrq(extiCallbackRec_t *cb)
|
||||||
{
|
{
|
||||||
extDevice_t *dev = rxSpiGetDevice();
|
extDevice_t *dev = rxSpiGetDevice();
|
||||||
|
|
||||||
|
|
|
@ -460,7 +460,7 @@ STATIC_UNIT_TESTED FAST_CODE_NOINLINE float pidLevel(int axis, const pidProfile_
|
||||||
return currentPidSetpoint;
|
return currentPidSetpoint;
|
||||||
}
|
}
|
||||||
|
|
||||||
static void handleCrashRecovery(
|
static FAST_CODE_NOINLINE void handleCrashRecovery(
|
||||||
const pidCrashRecovery_e crash_recovery, const rollAndPitchTrims_t *angleTrim,
|
const pidCrashRecovery_e crash_recovery, const rollAndPitchTrims_t *angleTrim,
|
||||||
const int axis, const timeUs_t currentTimeUs, const float gyroRate, float *currentPidSetpoint, float *errorRate)
|
const int axis, const timeUs_t currentTimeUs, const float gyroRate, float *currentPidSetpoint, float *errorRate)
|
||||||
{
|
{
|
||||||
|
@ -502,7 +502,7 @@ static void handleCrashRecovery(
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
static void detectAndSetCrashRecovery(
|
static FAST_CODE_NOINLINE void detectAndSetCrashRecovery(
|
||||||
const pidCrashRecovery_e crash_recovery, const int axis,
|
const pidCrashRecovery_e crash_recovery, const int axis,
|
||||||
const timeUs_t currentTimeUs, const float delta, const float errorRate)
|
const timeUs_t currentTimeUs, const float delta, const float errorRate)
|
||||||
{
|
{
|
||||||
|
|
|
@ -133,12 +133,12 @@ void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig)
|
||||||
gyroConfig->simplified_gyro_filter_multiplier = SIMPLIFIED_TUNING_DEFAULT;
|
gyroConfig->simplified_gyro_filter_multiplier = SIMPLIFIED_TUNING_DEFAULT;
|
||||||
}
|
}
|
||||||
|
|
||||||
FAST_CODE bool isGyroSensorCalibrationComplete(const gyroSensor_t *gyroSensor)
|
bool isGyroSensorCalibrationComplete(const gyroSensor_t *gyroSensor)
|
||||||
{
|
{
|
||||||
return gyroSensor->calibration.cyclesRemaining == 0;
|
return gyroSensor->calibration.cyclesRemaining == 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
FAST_CODE bool gyroIsCalibrationComplete(void)
|
bool gyroIsCalibrationComplete(void)
|
||||||
{
|
{
|
||||||
switch (gyro.gyroToUse) {
|
switch (gyro.gyroToUse) {
|
||||||
default:
|
default:
|
||||||
|
@ -203,7 +203,7 @@ bool isFirstArmingGyroCalibrationRunning(void)
|
||||||
return firstArmingCalibrationWasStarted && !gyroIsCalibrationComplete();
|
return firstArmingCalibrationWasStarted && !gyroIsCalibrationComplete();
|
||||||
}
|
}
|
||||||
|
|
||||||
STATIC_UNIT_TESTED void performGyroCalibration(gyroSensor_t *gyroSensor, uint8_t gyroMovementCalibrationThreshold)
|
STATIC_UNIT_TESTED NOINLINE void performGyroCalibration(gyroSensor_t *gyroSensor, uint8_t gyroMovementCalibrationThreshold)
|
||||||
{
|
{
|
||||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||||
// Reset g[axis] at start of calibration
|
// Reset g[axis] at start of calibration
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue