add horner_fma to utils

Firmware/MotorControl/utils.c

@@ -153,6 +153,13 @@ float fast_atan2(float y, float x) {
     return r;
 }
 
+float horner_fma(float x, const float *coeffs, size_t count) {
+    float result = 0.0f;
+    for (int idx = count-1; idx >= 0; idx--)
+        result = fmaf(result, x, coeffs[idx]);
+    return result;
+}
+
 // Modulo (as opposed to remainder), per https://stackoverflow.com/a/19288271
 int mod(int dividend, int divisor){
     int r = dividend % divisor;

Firmware/MotorControl/utils.h

@@ -94,6 +94,7 @@ static inline float fmodf_pos(float x, float y) {
 int SVM(float alpha, float beta, float* tA, float* tB, float* tC);
 
 float fast_atan2(float y, float x);
+float horner_fma(float x, const float *coeffs, size_t count);
 int mod(int dividend, int divisor);
 
 uint32_t deadline_to_timeout(uint32_t deadline_ms);