make wrapAngle a function not macro

firmware/controllers/math/engine_math.cpp

@@ -30,11 +30,6 @@
 extern bool verboseMode;
 #endif /* EFI_UNIT_TEST */
 
-angle_t wrapAngleMethod(angle_t param, const char *msg, ObdCode code) {
-	wrapAngle(param, msg, code);
-	return param;
-}
-
 floatms_t getEngineCycleDuration(int rpm) {
 	return getCrankshaftRevolutionTimeMs(rpm) * (getEngineRotationState()->getOperationMode() == TWO_STROKE ? 1 : 2);
 }

firmware/controllers/math/engine_math.h

@@ -13,9 +13,6 @@ void setAlgorithm(engine_load_mode_e algo);
 
 void setFlatInjectorLag(float value);
 
-// proper method avoids un-wrapped state of variables
-angle_t wrapAngleMethod(angle_t param, const char *msg, ObdCode code);
-
 /**
  * @return time needed to rotate crankshaft by one degree, in milliseconds.
  * @deprecated use at least Us, maybe even Nt

firmware/controllers/trigger/decoders/trigger_structure.h

@@ -13,6 +13,7 @@
 #include "state_sequence.h"
 #include "engine_configuration_generated_structures.h"
 #include <rusefi/isnan.h>
+#include "engine_state.h"
 
 #define FOUR_STROKE_ENGINE_CYCLE 720
 
@@ -26,28 +27,32 @@
 #define assertAngleRange(angle, msg, code) {}
 #endif
 
-/**
- * @brief Shifts angle into the [0..720) range for four stroke and [0..360) for two stroke
- * I guess this implementation would be faster than 'angle % engineCycle'
- * See also wrapVvt
- */
-#define wrapAngle(angle, msg, code)			   	    	    \
-	{																		\
-   	    if (cisnan(angle)) {                                                \
-		   firmwareError(ObdCode::CUSTOM_ERR_ANGLE, "a NaN %s", msg);       \
-		   angle = 0;                                                       \
-	    }                                                                   \
-		assertAngleRange(angle, msg, code);	   					            \
-		float engineCycle = getEngineState()->engineCycle;	                \
-		/* todo: split this method into 'fixAngleUp' and 'fixAngleDown'*/   \
-		/*       as a performance optimization?*/                           \
-		while (angle < 0)                       							\
-			angle += engineCycle;   										\
-			/* todo: would 'if' work as good as 'while'? */                 \
-		while (angle >= engineCycle)										\
-			angle -= engineCycle;   										\
+// Shifts angle into the [0..720) range for four stroke and [0..360) for two stroke
+// See also wrapVvt
+static inline void wrapAngle(angle_t& angle, const char* msg, ObdCode code) {
+	if (cisnan(angle)) {
+		firmwareError(ObdCode::CUSTOM_ERR_ANGLE, "a NaN %s", msg);
+		angle = 0;
 	}
 
+	assertAngleRange(angle, msg, code);
+	float engineCycle = getEngineState()->engineCycle;
+
+	while (angle < 0) {
+		angle += engineCycle;
+	}
+
+	while (angle >= engineCycle) {
+		angle -= engineCycle;
+	}
+}
+
+// proper method avoids un-wrapped state of variables
+static inline angle_t wrapAngleMethod(angle_t param, const char *msg, ObdCode code) {
+	wrapAngle(param, msg, code);
+	return param;
+}
+
 class TriggerDecoderBase;
 class TriggerFormDetails;
 class TriggerConfiguration;