diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 49df7747d7..97687857a8 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -33,7 +33,7 @@ // NULL filter -FAST_CODE float nullFilterApply(filter_t *filter, float input) +float nullFilterApply(filter_t *filter, float input) { UNUSED(filter); return input; diff --git a/src/main/drivers/bus_spi.c b/src/main/drivers/bus_spi.c index 3662cf8ce8..5de25e0c52 100644 --- a/src/main/drivers/bus_spi.c +++ b/src/main/drivers/bus_spi.c @@ -378,7 +378,7 @@ uint32_t spiCalculateClock(uint16_t spiClkDivisor) } // 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; busSegment_t *nextSegment; @@ -447,7 +447,7 @@ static void spiIrqHandler(const extDevice_t *dev) } // 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; @@ -484,7 +484,7 @@ static void spiRxIrqHandler(dmaChannelDescriptor_t* descriptor) #if !defined(STM32G4) && !defined(STM32H7) // 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; diff --git a/src/main/drivers/bus_spi_ll.c b/src/main/drivers/bus_spi_ll.c index dfcf91a08a..57919e7a7d 100644 --- a/src/main/drivers/bus_spi_ll.c +++ b/src/main/drivers/bus_spi_ll.c @@ -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) LL_SPI_SetTransferSize(instance, len); @@ -424,7 +424,7 @@ void spiInternalStartDMA(const extDevice_t *dev) /* 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 * 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 * - * 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 * 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 -void spiSequenceStart(const extDevice_t *dev) +FAST_CODE void spiSequenceStart(const extDevice_t *dev) { busDevice_t *bus = dev->bus; SPI_TypeDef *instance = bus->busType_u.spi.instance; diff --git a/src/main/drivers/rx/rx_sx1280.c b/src/main/drivers/rx/rx_sx1280.c index 670f894287..34546b8dff 100644 --- a/src/main/drivers/rx/rx_sx1280.c +++ b/src/main/drivers/rx/rx_sx1280.c @@ -77,7 +77,7 @@ bool sx1280IsBusy(void) return IORead(busy); } -static bool sx1280PollBusy(void) +FAST_CODE static bool sx1280PollBusy(void) { uint32_t startTime = micros(); while (IORead(busy)) { @@ -90,7 +90,7 @@ static bool sx1280PollBusy(void) 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 ATOMIC_BLOCK(NVIC_PRIO_MAX) { @@ -110,7 +110,7 @@ static void sx1280ClearBusyFn(void) } // Switch to waiting for busy interrupt -static bool sx1280EnableBusy(void) +FAST_CODE static bool sx1280EnableBusy(void) { if (!sx1280MarkBusy()) { return false; @@ -569,7 +569,7 @@ static void sx1280SendTelemetryBuffer(extiCallbackRec_t *cb); static busStatus_e sx1280TelemetryComplete(uint32_t arg); 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 ATOMIC_BLOCK(NVIC_PRIO_MAX) { @@ -584,7 +584,7 @@ void sx1280ISR(void) // 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(); @@ -605,7 +605,7 @@ static void sx1280IrqGetStatus(extiCallbackRec_t *cb) // 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; @@ -625,7 +625,7 @@ static busStatus_e sx1280IrqStatusRead(uint32_t arg) // 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(); @@ -647,7 +647,7 @@ static void sx1280IrqClearStatus(extiCallbackRec_t *cb) } // 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); @@ -657,7 +657,7 @@ static busStatus_e sx1280IrqCmdComplete(uint32_t arg) } // Process IRQ status -static void sx1280ProcessIrq(extiCallbackRec_t *cb) +FAST_IRQ_HANDLER static void sx1280ProcessIrq(extiCallbackRec_t *cb) { extDevice_t *dev = rxSpiGetDevice(); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 32d13466da..6feeed5ad1 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -460,7 +460,7 @@ STATIC_UNIT_TESTED FAST_CODE_NOINLINE float pidLevel(int axis, const pidProfile_ return currentPidSetpoint; } -static void handleCrashRecovery( +static FAST_CODE_NOINLINE void handleCrashRecovery( const pidCrashRecovery_e crash_recovery, const rollAndPitchTrims_t *angleTrim, 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 timeUs_t currentTimeUs, const float delta, const float errorRate) { diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 5ce7e35715..eec5508a31 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -133,12 +133,12 @@ void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig) 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; } -FAST_CODE bool gyroIsCalibrationComplete(void) +bool gyroIsCalibrationComplete(void) { switch (gyro.gyroToUse) { default: @@ -203,7 +203,7 @@ bool isFirstArmingGyroCalibrationRunning(void) 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++) { // Reset g[axis] at start of calibration