From ac045cd350e4cc9d56d2d753c8e85f7f1554c012 Mon Sep 17 00:00:00 2001
From: tx_haggis <13982343+adbancroft@users.noreply.github.com>
Date: Thu, 6 Jun 2024 01:16:15 -0500
Subject: [PATCH] Performance: inline many functions

---
 speeduino/crankMaths.cpp             | 26 +++-------------
 speeduino/crankMaths.h               | 35 +++++++++++++++++++--
 speeduino/maths.h                    | 10 +++++-
 speeduino/pages.cpp                  |  2 +-
 speeduino/schedule_calcs.hpp         | 22 ++++++-------
 speeduino/schedule_state_machine.cpp |  2 +-
 speeduino/scheduler.cpp              | 18 -----------
 speeduino/scheduler.h                | 46 ++++++++++++++--------------
 speeduino/speeduino.ino              | 10 +++---
 speeduino/unit_testing.h             | 13 ++++++++
 test/test_fuel/test_PW.cpp           |  2 ++
 11 files changed, 101 insertions(+), 85 deletions(-)

diff --git a/speeduino/crankMaths.cpp b/speeduino/crankMaths.cpp
index 4e83b5c260..9a5696b141 100644
--- a/speeduino/crankMaths.cpp
+++ b/speeduino/crankMaths.cpp
@@ -9,20 +9,13 @@ byte deltaToothCount = 0; //The last tooth that was used with the deltaV calc
 int rpmDelta;
 #endif
 
-typedef uint32_t UQ24X8_t;
-static constexpr uint8_t UQ24X8_Shift = 8U;
-
 /** @brief µS per degree at current RPM in UQ24.8 fixed point 
  * 
  * Ranges between 
  *   * 1040649 (4065.039 µS/°) at MIN_RPM/MIN_REVOLUTION_TIME
  *   * 2370    (9.258333 µS/°) at MAX_RPM/MAX_REVOLUTION_TIME
 */
-static  UQ24X8_t microsPerDegree;
-static constexpr uint8_t microsPerDegree_Shift = UQ24X8_Shift;
-
-typedef uint16_t UQ1X15_t;
-static constexpr uint8_t UQ1X15_Shift = 15U;
+crank_math_detail::UQ24X8_t microsPerDegree; // cppcheck-suppress misra-c2012-8.4 ; To allow inlining in header
 
 /** @brief Degrees per µS in UQ1.15 fixed point.
  * 
@@ -30,8 +23,7 @@ static constexpr uint8_t UQ1X15_Shift = 15U;
  *   * 8    (0.000246 °/µS) at MIN_RPM/MIN_REVOLUTION_TIME
  *   * 3539 (0.108011 °/µS) at MAX_RPM/MAX_REVOLUTION_TIME
  */
-static UQ1X15_t degreesPerMicro;
-static constexpr uint8_t degreesPerMicro_Shift = UQ1X15_Shift;
+crank_math_detail::UQ1X15_t degreesPerMicro; // cppcheck-suppress misra-c2012-8.4 ; To allow inlining in header
 
 /** @brief The time in µS that one revolution would take at current speed */
 uint32_t revolutionTime = MIN_REVOLUTION_TIME; // cppcheck-suppress misra-c2012-8.4 ; To allow inlining in header
@@ -41,23 +33,13 @@ bool setRevolutionTime(uint32_t revTime) {
   
   if (hasChanged) {
     revolutionTime = revTime;
-    microsPerDegree = div360(lshift<microsPerDegree_Shift>(revTime));
-    degreesPerMicro = (uint16_t)UDIV_ROUND_CLOSEST(lshift<degreesPerMicro_Shift>(UINT32_C(360)), revTime, uint32_t);
+    microsPerDegree = div360(lshift<crank_math_detail::microsPerDegree_Shift>(revTime));
+    degreesPerMicro = (uint16_t)UDIV_ROUND_CLOSEST(lshift<crank_math_detail::degreesPerMicro_Shift>(UINT32_C(360)), revTime, uint32_t);
   }
 
   return hasChanged;
 }
 
-uint32_t angleToTimeMicroSecPerDegree(uint16_t angle) {
-  UQ24X8_t micros = (uint32_t)angle * (uint32_t)microsPerDegree;
-  return rshift_round<microsPerDegree_Shift>(micros);
-}
-
-uint16_t timeToAngleDegPerMicroSec(uint32_t time) {
-  uint32_t degFixed = time * (uint32_t)degreesPerMicro;
-  return rshift_round<degreesPerMicro_Shift>(degFixed);
-}
-
 #if SECOND_DERIV_ENABLED!=0
 void doCrankSpeedCalcs(void)
 {
diff --git a/speeduino/crankMaths.h b/speeduino/crankMaths.h
index d5aa9fb813..d9e6a262ca 100644
--- a/speeduino/crankMaths.h
+++ b/speeduino/crankMaths.h
@@ -47,7 +47,7 @@ static constexpr uint32_t MAX_REVOLUTION_TIME = MICROS_PER_MIN/MIN_RPM;
  * @param angle A crank angle in degrees
  * @return int16_t 
  */
-static inline int16_t ignitionLimits(int16_t angle) {
+static CRITICAL_INLINE int16_t ignitionLimits(int16_t angle) {
     return nudge(0, CRANK_ANGLE_MAX_IGN-1, angle, CRANK_ANGLE_MAX_IGN);
 }
 
@@ -87,6 +87,16 @@ static inline uint32_t getRevolutionTime(void) {
   return revolutionTime;
 }
 
+/// @cond
+namespace crank_math_detail {
+  // Private to crankMath
+  typedef uint32_t UQ24X8_t;
+  static constexpr uint8_t UQ24X8_Shift = 8U;
+  static constexpr uint8_t microsPerDegree_Shift = UQ24X8_Shift;
+}
+/// @endcond
+
+
 /**
  * @brief Converts angular degrees to the time interval that amount of rotation
  * will take at the current crank revolution time (@ref setRevolutionTime).
@@ -97,7 +107,21 @@ static inline uint32_t getRevolutionTime(void) {
  * @param angle Angle in degrees
  * @return Time interval in µS
  */
-uint32_t angleToTimeMicroSecPerDegree(uint16_t angle);
+// uint32_t angleToTimeMicroSecPerDegree(uint16_t angle);
+static CRITICAL_INLINE uint32_t angleToTimeMicroSecPerDegree(uint16_t angle) {
+  extern crank_math_detail::UQ24X8_t microsPerDegree;
+  crank_math_detail::UQ24X8_t micros = (uint32_t)angle * (uint32_t)microsPerDegree;
+  return rshift_round<crank_math_detail::microsPerDegree_Shift>(micros);
+}
+
+/// @cond
+namespace crank_math_detail {
+  // Private to crankMath
+  typedef uint16_t UQ1X15_t;
+  static constexpr uint8_t UQ1X15_Shift = 15U;
+  static constexpr uint8_t degreesPerMicro_Shift = UQ1X15_Shift;
+}
+/// @endcond
 
 /**
  * @brief Converts a time interval in µS to the equivalent degrees of angular (crank)
@@ -108,7 +132,12 @@ uint32_t angleToTimeMicroSecPerDegree(uint16_t angle);
  * @param time Time interval in µS
  * @return Angle in degrees
  */
-uint16_t timeToAngleDegPerMicroSec(uint32_t time);
+static CRITICAL_INLINE uint16_t timeToAngleDegPerMicroSec(uint32_t time) {
+    extern crank_math_detail::UQ1X15_t degreesPerMicro;
+    uint32_t degFixed = time * (uint32_t)degreesPerMicro;
+    return rshift_round<crank_math_detail::degreesPerMicro_Shift>(degFixed);
+}
+
 
 /** @brief Calculate RPM based on the current crank revolution time (@ref setRevolutionTime). */
 static inline uint16_t rpmFromRevolutionTime(void) {
diff --git a/speeduino/maths.h b/speeduino/maths.h
index 6df9664768..c26b388c1f 100644
--- a/speeduino/maths.h
+++ b/speeduino/maths.h
@@ -14,7 +14,15 @@
 #include "src/libdivide/constant_fast_div.h"
 #endif
 
-uint8_t random1to100(void);
+/**
+ * @brief When a function really must be inline.
+ * 
+ * Usually best to let the compiler optimize, so use sparingly & only after
+ * comparing performance with & without applying it.
+ */
+#define CRITICAL_INLINE inline __attribute__((always_inline))
+
+extern uint8_t random1to100(void);
 
 /**
  * @defgroup group-rounded-div Rounding integer division
diff --git a/speeduino/pages.cpp b/speeduino/pages.cpp
index 0d95f2ed01..151a5b5f20 100644
--- a/speeduino/pages.cpp
+++ b/speeduino/pages.cpp
@@ -295,7 +295,7 @@ inline const page_iterator_t create_raw_iterator(void *pBuffer, uint8_t pageNum,
 //
 // Alternative implementation would be to encode the mapping into data structures
 // That uses flash memory, which is scarce. And it was too slow.
-static inline __attribute__((always_inline)) // <-- this is critical for performance
+static CRITICAL_INLINE // <-- this is critical for performance
 page_iterator_t map_page_offset_to_entity(uint8_t pageNumber, uint16_t offset)
 {
   // The start address of the 1st entity in any page.
diff --git a/speeduino/schedule_calcs.hpp b/speeduino/schedule_calcs.hpp
index 0d6df65c97..b0a84e802c 100644
--- a/speeduino/schedule_calcs.hpp
+++ b/speeduino/schedule_calcs.hpp
@@ -6,7 +6,7 @@
 #include "maths.h"
 #include "timers.h"
 
-static SCHEDULE_INLINE void setOpenAngle(FuelSchedule &schedule, uint16_t pwDegrees, uint16_t injAngle)
+static CRITICAL_INLINE void setOpenAngle(FuelSchedule &schedule, uint16_t pwDegrees, uint16_t injAngle)
 {
   // 0<=injAngle<=720°
   // 0<=injChannelDegrees<=720°
@@ -23,7 +23,7 @@ static SCHEDULE_INLINE void setOpenAngle(FuelSchedule &schedule, uint16_t pwDegr
   while (schedule.openAngle>(uint16_t)CRANK_ANGLE_MAX_INJ) { schedule.openAngle = schedule.openAngle - (uint16_t)CRANK_ANGLE_MAX_INJ; }
 }
 
-static SCHEDULE_INLINE uint32_t _calculateAngularTime(const Schedule &schedule, uint16_t eventAngle, uint16_t crankAngle, uint16_t maxAngle) {
+static CRITICAL_INLINE uint32_t _calculateAngularTime(const Schedule &schedule, uint16_t eventAngle, uint16_t crankAngle, uint16_t maxAngle) {
   int16_t delta = eventAngle - crankAngle;
   if (delta<0 && isRunning(schedule)) {
     delta = delta + (int16_t)maxAngle; 
@@ -31,13 +31,13 @@ static SCHEDULE_INLINE uint32_t _calculateAngularTime(const Schedule &schedule,
   return delta<0 ? 0U : angleToTimeMicroSecPerDegree((uint16_t)delta);
 }
 
-static SCHEDULE_INLINE uint16_t _adjustToTDC(int16_t angle, uint16_t angleOffset, uint16_t maxAngle) {
+static CRITICAL_INLINE uint16_t _adjustToTDC(int16_t angle, uint16_t angleOffset, uint16_t maxAngle) {
   angle = angle - (int)angleOffset;
   if( angle < 0) { return angle + (int)maxAngle; }
   return angle;
 }
 
-static SCHEDULE_INLINE uint32_t _calculateAngularTime(const Schedule &schedule, uint16_t angleOffset, uint16_t eventAngle, uint16_t crankAngle, uint16_t maxAngle) {
+static CRITICAL_INLINE uint32_t _calculateAngularTime(const Schedule &schedule, uint16_t angleOffset, uint16_t eventAngle, uint16_t crankAngle, uint16_t maxAngle) {
   if (angleOffset==0U) { // Optimize for zero channel angle - no need to adjust start & crank angles
     return _calculateAngularTime(schedule, eventAngle, crankAngle, maxAngle);
   }
@@ -53,38 +53,38 @@ static SCHEDULE_INLINE uint32_t _calculateAngularTime(const Schedule &schedule,
             maxAngle);
 }
 
-static SCHEDULE_INLINE uint32_t _calculateInjectorTimeout(const FuelSchedule &schedule, int16_t crankAngle)
+static CRITICAL_INLINE uint32_t _calculateInjectorTimeout(const FuelSchedule &schedule, int16_t crankAngle)
 {
   return _calculateAngularTime(schedule, schedule.channelDegrees, schedule.openAngle, crankAngle, CRANK_ANGLE_MAX_INJ);
 }
 
-static SCHEDULE_INLINE int16_t _calculateSparkAngle(const IgnitionSchedule &schedule, int8_t advance) {
+static CRITICAL_INLINE int16_t _calculateSparkAngle(const IgnitionSchedule &schedule, int8_t advance) {
   int16_t angle = (schedule.channelDegrees==0U ? CRANK_ANGLE_MAX_IGN : (int16_t)schedule.channelDegrees) - advance;
   if(angle > CRANK_ANGLE_MAX_IGN) {angle -= CRANK_ANGLE_MAX_IGN;}
   return angle;
 }
 
-static SCHEDULE_INLINE int16_t _calculateCoilChargeAngle(uint16_t dwellAngle, int16_t dischargeAngle) {
+static CRITICAL_INLINE int16_t _calculateCoilChargeAngle(uint16_t dwellAngle, int16_t dischargeAngle) {
   if (dischargeAngle>(int16_t)dwellAngle) {
     return dischargeAngle - (int16_t)dwellAngle;
   }
   return dischargeAngle + CRANK_ANGLE_MAX_IGN - (int16_t)dwellAngle;
 }
 
-static SCHEDULE_INLINE void calculateIgnitionAngles(IgnitionSchedule &schedule, uint16_t dwellAngle, int8_t advance)
+static CRITICAL_INLINE void calculateIgnitionAngles(IgnitionSchedule &schedule, uint16_t dwellAngle, int8_t advance)
 {
   schedule.dischargeAngle = _calculateSparkAngle(schedule,  advance);
   schedule.chargeAngle = _calculateCoilChargeAngle(dwellAngle, schedule.dischargeAngle);
 }
 
 
-static SCHEDULE_INLINE void calculateIgnitionTrailingRotary(IgnitionSchedule &leading, uint16_t dwellAngle, int16_t rotarySplitDegrees, IgnitionSchedule &trailing) 
+static CRITICAL_INLINE void calculateIgnitionTrailingRotary(IgnitionSchedule &leading, uint16_t dwellAngle, int16_t rotarySplitDegrees, IgnitionSchedule &trailing) 
 {
   trailing.dischargeAngle = (int16_t)ignitionLimits(leading.dischargeAngle + rotarySplitDegrees);
   trailing.chargeAngle = (int16_t)ignitionLimits(trailing.dischargeAngle - dwellAngle); 
 }
 
-static SCHEDULE_INLINE uint32_t _calculateIgnitionTimeout(const IgnitionSchedule &schedule, int16_t crankAngle)
+static CRITICAL_INLINE uint32_t _calculateIgnitionTimeout(const IgnitionSchedule &schedule, int16_t crankAngle)
 {
   return _calculateAngularTime(schedule, schedule.channelDegrees, schedule.chargeAngle, crankAngle, CRANK_ANGLE_MAX_IGN);
 }
@@ -94,7 +94,7 @@ static SCHEDULE_INLINE uint32_t _calculateIgnitionTimeout(const IgnitionSchedule
 // So the timing to begin & end charging the coil is based on crank angle.
 // With a more accurate crank angle, we can increase the precision of the
 // spark timing.
-static SCHEDULE_INLINE void adjustCrankAngle(IgnitionSchedule &schedule, int16_t crankAngle) {
+static CRITICAL_INLINE void adjustCrankAngle(IgnitionSchedule &schedule, int16_t crankAngle) {
   constexpr uint8_t MIN_CYCLES_FOR_CORRECTION = 6U;
 
   ATOMIC() {
diff --git a/speeduino/schedule_state_machine.cpp b/speeduino/schedule_state_machine.cpp
index 26e4764bef..1846c3edd9 100644
--- a/speeduino/schedule_state_machine.cpp
+++ b/speeduino/schedule_state_machine.cpp
@@ -18,7 +18,7 @@ void defaultRunningToPending(Schedule *schedule) {
   schedule->_status = PENDING;
 }
 
-static inline bool hasNextSchedule(const Schedule &schedule) {
+static CRITICAL_INLINE bool hasNextSchedule(const Schedule &schedule) {
   return schedule._status==RUNNING_WITHNEXT;
 }
 
diff --git a/speeduino/scheduler.cpp b/speeduino/scheduler.cpp
index a8b750979e..acd1210359 100644
--- a/speeduino/scheduler.cpp
+++ b/speeduino/scheduler.cpp
@@ -1033,24 +1033,6 @@ void setCallbacks(Schedule &schedule, voidVoidCallback pStartCallback, voidVoidC
   schedule._pEndCallback = pEndCallback;
 }
 
-void _setSchedulePending(Schedule &schedule, uint32_t timeout, uint32_t duration)
-{
-  //The following must be enclosed in the noInterupts block to avoid contention caused if the relevant interrupt fires before the state is fully set
-  schedule._duration = uS_TO_TIMER_COMPARE(duration);
-  schedule._compare = schedule._counter + uS_TO_TIMER_COMPARE(timeout);
-  schedule._status = PENDING; //Turn this schedule on
-}
-
-void _setScheduleNext(Schedule &schedule, uint32_t timeout, uint32_t duration)
-{
-  //If the schedule is already running, we can set the next schedule so it is ready to go
-  //This is required in cases of high rpm and high DC where there otherwise would not be enough time to set the schedule
-  schedule._nextStartCompare = schedule._counter + uS_TO_TIMER_COMPARE(timeout);
-  // Schedule must already be running, so safe to reuse this.
-  schedule._duration = uS_TO_TIMER_COMPARE(duration);
-  schedule._status = RUNNING_WITHNEXT;
-}
-
 static inline void applyChannelOverDwellProtection(IgnitionSchedule &schedule, uint32_t targetOverdwellTime) {
   ATOMIC() {  
     if (isRunning(schedule)) {
diff --git a/speeduino/scheduler.h b/speeduino/scheduler.h
index 5c0c9a6c51..c1fa06baec 100644
--- a/speeduino/scheduler.h
+++ b/speeduino/scheduler.h
@@ -44,13 +44,8 @@ See page 136 of the processors datasheet: http://www.atmel.com/Images/doc2549.pd
 #include "board_definition.h"
 #include "scheduledIO.h"
 #include "table3d.h"
-
-// Inlining seems to be very important for AVR performance
-#if defined(CORE_AVR)
-#define SCHEDULE_INLINE inline __attribute__((always_inline))
-#else
-#define SCHEDULE_INLINE inline
-#endif
+#include "timers.h"
+#include "maths.h"
 
 #define USE_IGN_REFRESH
 #define IGNITION_REFRESH_THRESHOLD  30 //Time in uS that the refresh functions will check to ensure there is enough time before changing the end compare
@@ -164,7 +159,7 @@ struct Schedule {
   compare_t &_compare;       ///< **Reference**to the compare register. E.g. OCR3A
 };
 
-static SCHEDULE_INLINE bool isRunning(const Schedule &schedule) {
+static CRITICAL_INLINE bool isRunning(const Schedule &schedule) {
   // Using flags and bitwise AND (&) to check multiple states is much quicker
   // than a logical or (||) (one less branch & 30% less instructions)
   static constexpr uint8_t flags = RUNNING | RUNNING_WITHNEXT;
@@ -173,16 +168,21 @@ static SCHEDULE_INLINE bool isRunning(const Schedule &schedule) {
 
 /// @cond 
 // Private function - not for use external to the scheduler code
-void _setScheduleNext(Schedule &schedule, uint32_t timeout, uint32_t duration);
-void _setSchedulePending(Schedule &schedule, uint32_t timeout, uint32_t duration);
-
-static SCHEDULE_INLINE void _setSchedule(Schedule &schedule, uint32_t timeout, uint32_t duration) {
+static CRITICAL_INLINE void _setSchedule(Schedule &schedule, uint32_t timeout, uint32_t duration) {
   if(timeout<MAX_TIMER_PERIOD && duration<MAX_TIMER_PERIOD) {
-      if(!isRunning(schedule)) { //Check that we're not already part way through a schedule
-        _setSchedulePending(schedule, timeout, duration);
-      }
-      else {
-        _setScheduleNext(schedule, timeout, duration);
+    if(!isRunning(schedule)) { //Check that we're not already part way through a schedule
+      //The following must be enclosed in the noInterupts block to avoid contention caused if the relevant interrupt fires before the state is fully set
+      schedule._duration = uS_TO_TIMER_COMPARE(duration);
+      SET_COMPARE(schedule._compare, schedule._counter + (COMPARE_TYPE)uS_TO_TIMER_COMPARE(timeout));
+      schedule._status = PENDING; //Turn this schedule on        
+    }
+    else {
+      //If the schedule is already running, we can set the next schedule so it is ready to go
+      //This is required in cases of high rpm and high DC where there otherwise would not be enough time to set the schedule
+      schedule._nextStartCompare = schedule._counter + uS_TO_TIMER_COMPARE(timeout);
+      // Schedule must already be running, so safe to reuse this.
+      schedule._duration = uS_TO_TIMER_COMPARE(duration);
+      schedule._status = RUNNING_WITHNEXT;
     }
   }
 }
@@ -202,7 +202,7 @@ void setCallbacks(Schedule &schedule, voidVoidCallback pStartCallback, voidVoidC
  * @brief Is the schedule in pending state?
  * I.e. waiting for a timer interrupt to start the scheduled action. E.g. open an injector
  */
-static SCHEDULE_INLINE bool isPending(const Schedule &schedule) {
+static CRITICAL_INLINE bool isPending(const Schedule &schedule) {
   static constexpr uint8_t flags = PENDING | PENDING_WITH_OVERRIDE;
   return (bool)(schedule._status & flags);
 }
@@ -243,7 +243,7 @@ struct IgnitionSchedule : public Schedule {
  * @param crankAngle The current crank angle
  * @return uint32_t 
  */
-static SCHEDULE_INLINE uint32_t _calculateIgnitionTimeout(const IgnitionSchedule &schedule, int16_t crankAngle);
+static CRITICAL_INLINE uint32_t _calculateIgnitionTimeout(const IgnitionSchedule &schedule, int16_t crankAngle);
 /// @endcond
 
 /** @brief Set the next schedule for the ignition channel.
@@ -252,7 +252,7 @@ static SCHEDULE_INLINE uint32_t _calculateIgnitionTimeout(const IgnitionSchedule
  * @param crankAngle The current crank angle
  * @param dwellDuration The coil dwell time in µS
  */
-static SCHEDULE_INLINE void setIgnitionSchedule(IgnitionSchedule &schedule, int16_t crankAngle, uint32_t dwellDuration) {
+static CRITICAL_INLINE void setIgnitionSchedule(IgnitionSchedule &schedule, int16_t crankAngle, uint32_t dwellDuration) {
   // Do not override the per-tooth timing - quick & dirty check
   if (schedule._status!=PENDING_WITH_OVERRIDE) {
     uint32_t delay = _calculateIgnitionTimeout(schedule, crankAngle);
@@ -335,7 +335,7 @@ struct FuelSchedule : public Schedule {
 
 /// @cond
 // Private function - not for use external to the scheduler code
-static SCHEDULE_INLINE uint32_t _calculateInjectorTimeout(const FuelSchedule &schedule, int16_t crankAngle);
+static CRITICAL_INLINE uint32_t _calculateInjectorTimeout(const FuelSchedule &schedule, int16_t crankAngle);
 /// @endcond
 
 /** @brief Set the next schedule for the fuel channel. 
@@ -344,7 +344,7 @@ static SCHEDULE_INLINE uint32_t _calculateInjectorTimeout(const FuelSchedule &sc
  * @param schedule The fuel channel
  * @param crankAngle The current crank angle
  */
-static SCHEDULE_INLINE void setFuelSchedule(FuelSchedule &schedule, int16_t crankAngle) {
+static CRITICAL_INLINE void setFuelSchedule(FuelSchedule &schedule, int16_t crankAngle) {
   uint32_t delay = _calculateInjectorTimeout(schedule, crankAngle);  
 
   ATOMIC() {
@@ -369,7 +369,7 @@ void moveToNextState(FuelSchedule &schedule);
  * @param pwDegrees How many crank degrees the calculated PW will take at the current speed
  * @param injAngle The requested injection angle
   */
-static SCHEDULE_INLINE void setOpenAngle(FuelSchedule &schedule, uint16_t pwDegrees, uint16_t injAngle);
+static CRITICAL_INLINE void setOpenAngle(FuelSchedule &schedule, uint16_t pwDegrees, uint16_t injAngle);
 
 extern FuelSchedule fuelSchedule1;
 extern FuelSchedule fuelSchedule2;
diff --git a/speeduino/speeduino.ino b/speeduino/speeduino.ino
index d92a087ceb..266b0814f4 100644
--- a/speeduino/speeduino.ino
+++ b/speeduino/speeduino.ino
@@ -154,7 +154,7 @@ static inline byte getVE1(void)
  * @param injOpen Injector opening time. The time the injector take to open minus the time it takes to close (Both in uS)
  * @return uint16_t The injector pulse width in uS
  */
-TESTABLE_INLINE_STATIC uint16_t PW(int REQ_FUEL, byte VE, long MAP, uint16_t corrections, int injOpen)
+TESTABLE_ALWAYS_INLINE_STATIC uint16_t PW(int REQ_FUEL, byte VE, long MAP, uint16_t corrections, int injOpen)
 {
   //Standard float version of the calculation
   //return (REQ_FUEL * (float)(VE/100.0) * (float)(MAP/100.0) * (float)(TPS/100.0) * (float)(corrections/100.0) + injOpen);
@@ -242,13 +242,13 @@ static inline void applyFuelTrimToPW(FuelSchedule &schedule, int16_t fuelLoad, i
     if (pw1percent != 100U) { schedule.pw = percentage(pw1percent, schedule.pw); }
 }
 
-static void setIgnitionSchedule(IgnitionSchedule &schedule, uint8_t index, uint16_t crankAngle, uint16_t totalDwell) {
+static CRITICAL_INLINE void setIgnitionSchedule(IgnitionSchedule &schedule, uint8_t index, uint16_t crankAngle, uint16_t totalDwell) {
   if ((maxIgnOutputs>index) && (BIT_CHECK(ignitionChannelsOn, (IGN1_CMD_BIT+index))) ) {
     setIgnitionSchedule(schedule, crankAngle, totalDwell);
   }
 }
 
-static inline __attribute__((flatten, always_inline)) void setIgnitionSchedules(uint16_t crankAngle, uint16_t totalDwell) {
+static CRITICAL_INLINE void setIgnitionSchedules(uint16_t crankAngle, uint16_t totalDwell) {
   setIgnitionSchedule(ignitionSchedule1, 0, crankAngle, totalDwell);
 #if defined(USE_IGN_REFRESH)
   if( isRunning(ignitionSchedule1) && (ignitionSchedule1.dischargeAngle > (int16_t)crankAngle) && (configPage4.StgCycles == 0) && (configPage2.perToothIgn != true) )
@@ -288,13 +288,13 @@ static inline __attribute__((flatten, always_inline)) void setIgnitionSchedules(
 #endif
 } 
 
-static void setFuelSchedule(FuelSchedule &schedule, uint8_t index, uint16_t crankAngle) {
+static CRITICAL_INLINE void setFuelSchedule(FuelSchedule &schedule, uint8_t index, uint16_t crankAngle) {
   if( (maxInjOutputs > index) && (schedule.pw >= inj_opentime_uS) && (BIT_CHECK(fuelChannelsOn, (INJ1_CMD_BIT+index))) ) {
     setFuelSchedule(schedule, crankAngle);
   }
 }
 
-static inline __attribute__((flatten)) void setFuelSchedules(uint16_t crankAngle) {
+static CRITICAL_INLINE void setFuelSchedules(uint16_t crankAngle) {
   setFuelSchedule(fuelSchedule1, 0, crankAngle);
   setFuelSchedule(fuelSchedule2, 1, crankAngle);
   setFuelSchedule(fuelSchedule3, 2, crankAngle);
diff --git a/speeduino/unit_testing.h b/speeduino/unit_testing.h
index fc81d9645a..7700c2f51a 100644
--- a/speeduino/unit_testing.h
+++ b/speeduino/unit_testing.h
@@ -31,3 +31,16 @@
 #else
 #define TESTABLE_INLINE_STATIC
 #endif
+
+#if !defined(UNIT_TEST)
+/** 
+ * @brief Mark an entity as having static (internal) linkage & inlined, unless a unit
+ * test is in progress - then the entity is given external linkage so the test can access it.
+ * 
+ * Most useful for translation unit scoped functions. I.e. "static inline" within a CPP file 
+ * 
+ */
+#define TESTABLE_ALWAYS_INLINE_STATIC static inline __attribute__((always_inline)) 
+#else
+#define TESTABLE_ALWAYS_INLINE_STATIC
+#endif
diff --git a/test/test_fuel/test_PW.cpp b/test/test_fuel/test_PW.cpp
index 5e351ae90d..9c337cb084 100644
--- a/test/test_fuel/test_PW.cpp
+++ b/test/test_fuel/test_PW.cpp
@@ -8,6 +8,8 @@
 
 #define PW_ALLOWED_ERROR  30
 
+extern uint16_t PW(int REQ_FUEL, byte VE, long MAP, uint16_t corrections, int injOpen);
+
 void testPW(void)
 {
   SET_UNITY_FILENAME() {