diff --git a/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp b/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp
index 9c5f5b7003..49a5ac9109 100644
--- a/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp
+++ b/libraries/AP_HAL_ChibiOS/RCOutput_bdshot.cpp
@@ -191,6 +191,8 @@ void RCOutput::bdshot_ic_dma_deallocate(Shared_DMA *ctx)
 
 // see https://github.com/betaflight/betaflight/pull/8554#issuecomment-512507625
 // called from the interrupt
+#pragma GCC push_options
+#pragma GCC optimize("O2")
 void RCOutput::bdshot_receive_pulses_DMAR(pwm_group* group)
 {
     // make sure the transaction finishes or times out, this function takes a little time to run so the most
@@ -584,6 +586,7 @@ uint32_t RCOutput::bdshot_decode_telemetry_packet(uint32_t* buffer, uint32_t cou
     uint32_t ret = (1000000 * 60 / 100 + decodedValue / 2) / decodedValue;
     return ret;
 }
+#pragma GCC pop_options
 
 
 #endif // HAL_WITH_BIDIR_DSHOT