Avoid reevaluating arguments.

_round, _constrain and _sign currently reevaluate their arguments, which
wastes CPU cycles. In torque/foc_current mode, this fix saves about
300ns/foc loop (about 2% of total CPU usage). In other modes, especially
estimated_current, it saves more.

Author: jaguilar

src/common/foc_utils.h

@@ -2,13 +2,41 @@
 #define FOCUTILS_LIB_H
 
 #include "Arduino.h"
+#include <type_traits>
+
+template<typename T>
+constexpr inline int _sign(T val) {
+  return __builtin_signbit(val);
+}
 
-// sign function
-#define _sign(a) ( ( (a) < 0 )  ?  -1   : ( (a) > 0 ) )
 #ifndef _round
-#define _round(x) ((x)>=0?(long)((x)+0.5f):(long)((x)-0.5f))
+// Use enable_if to select the roundf function for single precision floats.
+// This improves performance when -ffast-math is not set.
+template<typename T>
+constexpr inline typename std::enable_if<std::is_same<T, float>::value, long>::type _round(T x) {
+  return __builtin_roundf(x);
+}
+template<typename T>
+constexpr inline typename std::enable_if<std::is_same<T, double>::value, long>::type _round(T x) {
+  return __builtin_round(x);
+}
 #endif
-#define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
+
+// Use enable_if to select the fastest implementation according to the amt type.
+// Using __builtin_fXf is measurably faster than using the ternary approach.
+template<typename T, typename L, typename H>
+constexpr inline typename std::enable_if<std::is_integral<T>::value, T>::type _constrain(T amt, L low, H high) {
+  return (amt < low) ? low : (amt > high) ? high : amt;
+}
+template<typename T, typename L, typename H>
+constexpr inline typename std::enable_if<std::is_same<T, float>::value, T>::type _constrain(T amt, L low, H high) {
+  return __builtin_fmaxf(low, __builtin_fminf(high, amt));
+}
+template<typename T, typename L, typename H>
+constexpr inline typename std::enable_if<std::is_same<T, double>::value, T>::type _constrain(T amt, L low, H high) {
+  return __builtin_fmax(low, __builtin_fmin(high, amt));
+}
+
 #define _sqrt(a) (_sqrtApprox(a))
 #define _isset(a) ( (a) != (NOT_SET) )
 #define _UNUSED(v) (void) (v)
@@ -121,4 +149,4 @@ float _electricalAngle(float shaft_angle, int pole_pairs);
  */
 float _sqrtApprox(float value);
 
-#endif
+#endif