From e30a0642640c3fc0e3e5b5f82d27fb7a0549d8ea Mon Sep 17 00:00:00 2001
From: Oskar Weigl <oskar.weigl@gmail.com>
Date: Sun, 1 Jul 2018 00:15:56 -0700
Subject: [PATCH] add horner_fma to utils

---
 Firmware/MotorControl/utils.c | 7 +++++++
 Firmware/MotorControl/utils.h | 1 +
 2 files changed, 8 insertions(+)

diff --git a/Firmware/MotorControl/utils.c b/Firmware/MotorControl/utils.c
index 5d3b9203..f70f5170 100644
--- a/Firmware/MotorControl/utils.c
+++ b/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;
diff --git a/Firmware/MotorControl/utils.h b/Firmware/MotorControl/utils.h
index eeb104b3..1cd63327 100644
--- a/Firmware/MotorControl/utils.h
+++ b/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);