From 54636c764f423d3acb049a489329df2a2b90e992 Mon Sep 17 00:00:00 2001
From: Oskar Weigl <oskar.weigl@gmail.com>
Date: Thu, 3 Sep 2020 20:03:10 -0700
Subject: [PATCH 01/19] implement faster fmodf related functions

---
 Firmware/MotorControl/controller.cpp |  2 +-
 Firmware/MotorControl/encoder.cpp    |  4 ++--
 Firmware/MotorControl/utils.hpp      | 30 +++++++++++++---------------
 3 files changed, 17 insertions(+), 19 deletions(-)

diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp
index 1524dce6..ecea3c9c 100644
--- a/Firmware/MotorControl/controller.cpp
+++ b/Firmware/MotorControl/controller.cpp
@@ -233,7 +233,7 @@ bool Controller::update(float* torque_setpoint_output) {
             pos_setpoint_ = fmodf_pos(pos_setpoint_, *pos_wrap_src_);
             // Circular delta
             pos_err = pos_setpoint_ - *pos_estimate_circular;
-            pos_err = wrap_pm(pos_err, 0.5f * *pos_wrap_src_);
+            pos_err = wrap_pm(pos_err, *pos_wrap_src_);
         } else {
             if(!pos_estimate_linear) {
                 set_error(ERROR_INVALID_ESTIMATE);
diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp
index d2ce992b..f0c65445 100644
--- a/Firmware/MotorControl/encoder.cpp
+++ b/Firmware/MotorControl/encoder.cpp
@@ -546,7 +546,7 @@ bool Encoder::update() {
     // discrete phase detector
     float delta_pos_counts = (float)(shadow_count_ - (int32_t)std::floor(pos_estimate_counts_));
     float delta_pos_cpr_counts = (float)(count_in_cpr_ - (int32_t)std::floor(pos_cpr_counts_));
-    delta_pos_cpr_counts = wrap_pm(delta_pos_cpr_counts, 0.5f * (float)(config_.cpr));
+    delta_pos_cpr_counts = wrap_pm(delta_pos_cpr_counts, (float)(config_.cpr));
     // pll feedback
     pos_estimate_counts_ += current_meas_period * pll_kp_ * delta_pos_counts;
     pos_cpr_counts_ += current_meas_period * pll_kp_ * delta_pos_cpr_counts;
@@ -561,7 +561,7 @@ bool Encoder::update() {
     // Outputs from Encoder for Controller
     pos_estimate_ = pos_estimate_counts_ / (float)config_.cpr;
     vel_estimate_ = vel_estimate_counts_ / (float)config_.cpr;
-    pos_circular_ +=  wrap_pm((pos_cpr_counts_ - pos_cpr_counts_last) / (float)config_.cpr, 0.5f);
+    pos_circular_ +=  wrap_pm((pos_cpr_counts_ - pos_cpr_counts_last) / (float)config_.cpr, 1.0f);
     pos_circular_ = fmodf_pos(pos_circular_, axis_->controller_.config_.circular_setpoint_range);
 
     //// run encoder count interpolation
diff --git a/Firmware/MotorControl/utils.hpp b/Firmware/MotorControl/utils.hpp
index 49f9434d..70bd73be 100644
--- a/Firmware/MotorControl/utils.hpp
+++ b/Firmware/MotorControl/utils.hpp
@@ -84,25 +84,23 @@ static const float one_by_sqrt3 = 0.57735026919f;
 static const float two_by_sqrt3 = 1.15470053838f;
 static const float sqrt3_by_2 = 0.86602540378f;
 
-// like fmodf, but always positive
+// Result depends on rounding mode
+// With default rounding (round to nearest),
+// the result will be in range -y/2 to y/2
+static inline float wrap_pm(float x, float y) {
+    int q = (int)(x/y);
+    return x - (float)q * y;
+}
+
+// Same as fmodf but result is positive and y must be positive
 static inline float fmodf_pos(float x, float y) {
-    float out = fmodf(x, y);
-    if (out < 0.0f)
-        out += y;
-    return out;
+    float res = wrap_pm(x, y);
+    if (res < 0) res += y;
+    return res;
 }
 
-/**
- * @brief Similar to modulo operator, except that the output range is centered
- * around zero.
- * The returned value is always in the range [-pm_range, pm_range).
- */
-static inline float wrap_pm(float x, float pm_range) {
-    return fmodf_pos(x + pm_range, 2.0f * pm_range) - pm_range;
-}
-
-static inline float wrap_pm_pi(float theta) {
-    return wrap_pm(theta, M_PI);
+static inline float wrap_pm_pi(float x) {
+    return wrap_pm(x, 2*M_PI);
 }
 
 // Compute rising edge timings (0.0 - 1.0) as a function of alpha-beta

From 805b308a869cbb0a892d52b250d87176f2af529c Mon Sep 17 00:00:00 2001
From: Oskar Weigl <oskar.weigl@gmail.com>
Date: Fri, 4 Sep 2020 14:27:22 -0700
Subject: [PATCH 02/19] fix rounding mode, use vcvtr

---
 Firmware/.vscode/c_cpp_properties.json |  6 ++++--
 Firmware/MotorControl/utils.hpp        | 20 ++++++++++++++++----
 2 files changed, 20 insertions(+), 6 deletions(-)

diff --git a/Firmware/.vscode/c_cpp_properties.json b/Firmware/.vscode/c_cpp_properties.json
index 1d5ab7cf..be9f826f 100644
--- a/Firmware/.vscode/c_cpp_properties.json
+++ b/Firmware/.vscode/c_cpp_properties.json
@@ -6,12 +6,12 @@
                 "${workspaceFolder}/**"
             ],
             "defines": [
+                "__arm__",
                 "STM32F405xx",
                 "USE_HAL_DRIVER",
                 "HW_VERSION_MAJOR=3",
                 "HW_VERSION_MINOR=6",
                 "HW_VERSION_VOLTAGE=56",
-                "USB_PROTOCOL_NATIVE",
                 "__weak=\"__attribute__((weak))\"",
                 "__packed=\"__attribute__((__packed__))\"",
                 "__GNUC__"
@@ -27,6 +27,7 @@
                 "${workspaceFolder}/**"
             ],
             "defines": [
+                "__arm__",
                 "STM32F405xx",
                 "USE_HAL_DRIVER",
                 "HW_VERSION_MAJOR=3",
@@ -47,11 +48,12 @@
                 "${workspaceFolder}/**"
             ],
             "defines": [
+                "__arm__",
                 "STM32F405xx",
                 "USE_HAL_DRIVER",
                 "HW_VERSION_MAJOR=3",
                 "HW_VERSION_MINOR=4",
-                "HW_VERSION_VOLTAGE=24",
+                "HW_VERSION_VOLTAGE=56",
                 "__weak=\"__attribute__((weak))\"",
                 "__packed=\"__attribute__((__packed__))\"",
                 "__GNUC__"
diff --git a/Firmware/MotorControl/utils.hpp b/Firmware/MotorControl/utils.hpp
index 70bd73be..42e03c8f 100644
--- a/Firmware/MotorControl/utils.hpp
+++ b/Firmware/MotorControl/utils.hpp
@@ -84,12 +84,24 @@ static const float one_by_sqrt3 = 0.57735026919f;
 static const float two_by_sqrt3 = 1.15470053838f;
 static const float sqrt3_by_2 = 0.86602540378f;
 
-// Result depends on rounding mode
-// With default rounding (round to nearest),
+// Round to integer
+// Default rounding mode: round to nearest
+static inline int round_int(float x) {
+#ifdef __arm__
+    int res;
+    asm("vcvtr.s32.f32   %[res], %[x]" : [res] "=X" (res) : [x] "w" (x));
+    return res;
+#else
+    return (int)rint(x);
+#endif
+}
+
+// Wrap value to range.
+// With default rounding mode (round to nearest),
 // the result will be in range -y/2 to y/2
 static inline float wrap_pm(float x, float y) {
-    int q = (int)(x/y);
-    return x - (float)q * y;
+    int intval = round_int(x/y);
+    return x - (float)intval * y;
 }
 
 // Same as fmodf but result is positive and y must be positive

From fc726dfa3615bb2935965fe12552637be9447bbc Mon Sep 17 00:00:00 2001
From: Oskar Weigl <oskar.weigl@gmail.com>
Date: Fri, 4 Sep 2020 16:09:17 -0700
Subject: [PATCH 03/19] use nearbyint on VFPv5 as it is single instruction

---
 Firmware/.vscode/c_cpp_properties.json        |  3 +++
 Firmware/MotorControl/utils.hpp               | 10 +++++++---
 Firmware/Tests/test_trap_traj.cpp             |  2 +-
 Firmware/Tupfile.lua                          |  2 +-
 Firmware/fibre/cpp/include/fibre/protocol.hpp |  2 +-
 5 files changed, 13 insertions(+), 6 deletions(-)

diff --git a/Firmware/.vscode/c_cpp_properties.json b/Firmware/.vscode/c_cpp_properties.json
index be9f826f..3a82cf12 100644
--- a/Firmware/.vscode/c_cpp_properties.json
+++ b/Firmware/.vscode/c_cpp_properties.json
@@ -8,6 +8,7 @@
             "defines": [
                 "__arm__",
                 "STM32F405xx",
+                "FPU_FPV4",
                 "USE_HAL_DRIVER",
                 "HW_VERSION_MAJOR=3",
                 "HW_VERSION_MINOR=6",
@@ -29,6 +30,7 @@
             "defines": [
                 "__arm__",
                 "STM32F405xx",
+                "FPU_FPV4",
                 "USE_HAL_DRIVER",
                 "HW_VERSION_MAJOR=3",
                 "HW_VERSION_MINOR=6",
@@ -50,6 +52,7 @@
             "defines": [
                 "__arm__",
                 "STM32F405xx",
+                "FPU_FPV4",
                 "USE_HAL_DRIVER",
                 "HW_VERSION_MAJOR=3",
                 "HW_VERSION_MINOR=4",
diff --git a/Firmware/MotorControl/utils.hpp b/Firmware/MotorControl/utils.hpp
index 42e03c8f..e70a9ccc 100644
--- a/Firmware/MotorControl/utils.hpp
+++ b/Firmware/MotorControl/utils.hpp
@@ -92,7 +92,7 @@ static inline int round_int(float x) {
     asm("vcvtr.s32.f32   %[res], %[x]" : [res] "=X" (res) : [x] "w" (x));
     return res;
 #else
-    return (int)rint(x);
+    return (int)nearbyint(x);
 #endif
 }
 
@@ -100,8 +100,12 @@ static inline int round_int(float x) {
 // With default rounding mode (round to nearest),
 // the result will be in range -y/2 to y/2
 static inline float wrap_pm(float x, float y) {
-    int intval = round_int(x/y);
-    return x - (float)intval * y;
+#ifdef FPU_FPV4
+    float intval = (float)round_int(x/y);
+#else
+    float intval = nearbyint(x/y);
+#endif
+    return x - intval * y;
 }
 
 // Same as fmodf but result is positive and y must be positive
diff --git a/Firmware/Tests/test_trap_traj.cpp b/Firmware/Tests/test_trap_traj.cpp
index b5d30014..d95328dd 100644
--- a/Firmware/Tests/test_trap_traj.cpp
+++ b/Firmware/Tests/test_trap_traj.cpp
@@ -1,7 +1,7 @@
 
 #include <doctest.h>
 #include <limits.h>
-#include <cmath>
+#include <math.h>
 #include <iostream>
 #include <random>
 
diff --git a/Firmware/Tupfile.lua b/Firmware/Tupfile.lua
index 040db512..d2cf3cbf 100644
--- a/Firmware/Tupfile.lua
+++ b/Firmware/Tupfile.lua
@@ -35,7 +35,7 @@ tup.frule{
 board_v3 = {
     dir = 'Board/v3',
     sources = {'Drivers/DRV8301/drv8301.cpp', 'Board/v3/board.cpp'},
-    flags = {'-DSTM32F405xx', '-DARM_MATH_CM4', '-mcpu=cortex-m4', '-mfpu=fpv4-sp-d16'},
+    flags = {'-DSTM32F405xx', '-DARM_MATH_CM4', '-mcpu=cortex-m4', '-mfpu=fpv4-sp-d16', '-DFPU_FPV4'},
     ldflags = {'-TBoard/v3/STM32F405RGTx_FLASH.ld', '-LBoard/v3/Drivers/CMSIS/Lib', '-larm_cortexM4lf_math', '-mcpu=cortex-m4', '-mfpu=fpv4-sp-d16'}
 }
 
diff --git a/Firmware/fibre/cpp/include/fibre/protocol.hpp b/Firmware/fibre/cpp/include/fibre/protocol.hpp
index 2a01e220..5d529c39 100644
--- a/Firmware/fibre/cpp/include/fibre/protocol.hpp
+++ b/Firmware/fibre/cpp/include/fibre/protocol.hpp
@@ -10,7 +10,7 @@ see protocol.md for the protocol specification
 
 #include <functional>
 #include <limits>
-#include <cmath>
+#include <math.h>
 //#include <stdint.h>
 #include <stdio.h>
 #include <string.h>

From 711111e6fb76594fbebf3cb641ea7cd837b28f00 Mon Sep 17 00:00:00 2001
From: Oskar Weigl <oskar.weigl@gmail.com>
Date: Fri, 4 Sep 2020 18:23:33 -0700
Subject: [PATCH 04/19] change fmaxf to std max

---
 Firmware/MotorControl/low_level.cpp | 2 +-
 Firmware/MotorControl/motor.cpp     | 2 +-
 Firmware/MotorControl/trapTraj.cpp  | 2 +-
 Firmware/Tests/test_trap_traj.cpp   | 2 +-
 4 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/Firmware/MotorControl/low_level.cpp b/Firmware/MotorControl/low_level.cpp
index a40d2ac6..af258c29 100644
--- a/Firmware/MotorControl/low_level.cpp
+++ b/Firmware/MotorControl/low_level.cpp
@@ -494,7 +494,7 @@ void update_brake_current() {
     float brake_duty = brake_current * odrv.config_.brake_resistance / vbus_voltage;
     
     if (odrv.config_.enable_dc_bus_overvoltage_ramp && (odrv.config_.brake_resistance > 0.0f) && (odrv.config_.dc_bus_overvoltage_ramp_start < odrv.config_.dc_bus_overvoltage_ramp_end)) {
-        brake_duty += std::fmax((vbus_voltage - odrv.config_.dc_bus_overvoltage_ramp_start) / (odrv.config_.dc_bus_overvoltage_ramp_end - odrv.config_.dc_bus_overvoltage_ramp_start), 0.0f);
+        brake_duty += std::max((vbus_voltage - odrv.config_.dc_bus_overvoltage_ramp_start) / (odrv.config_.dc_bus_overvoltage_ramp_end - odrv.config_.dc_bus_overvoltage_ramp_start), 0.0f);
     }
 
     if (std::isnan(brake_duty)) {
diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp
index 9bdbaa75..0b0a2a31 100644
--- a/Firmware/MotorControl/motor.cpp
+++ b/Firmware/MotorControl/motor.cpp
@@ -416,7 +416,7 @@ bool Motor::update(float torque_setpoint, float phase, float phase_vel) {
     phase_vel *= config_.direction;
 
     if (config_.motor_type == MOTOR_TYPE_ACIM) {
-        current_setpoint = torque_setpoint / (config_.torque_constant * fmax(current_control_.acim_rotor_flux, config_.acim_gain_min_flux));
+        current_setpoint = torque_setpoint / (config_.torque_constant * std::max(current_control_.acim_rotor_flux, config_.acim_gain_min_flux));
     }
     else {
         current_setpoint = torque_setpoint / config_.torque_constant;
diff --git a/Firmware/MotorControl/trapTraj.cpp b/Firmware/MotorControl/trapTraj.cpp
index b3afd946..cb271fcd 100644
--- a/Firmware/MotorControl/trapTraj.cpp
+++ b/Firmware/MotorControl/trapTraj.cpp
@@ -42,7 +42,7 @@ bool TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, float Vi,
     // Are we displacing enough to reach cruising speed?
     if (s*dX < s*dXmin) {
         // Short move (triangle profile)
-        Vr_ = s * sqrtf(std::fmax((Dr_*SQ(Vi) + 2*Ar_*Dr_*dX) / (Dr_ - Ar_), 0.0f));
+        Vr_ = s * sqrtf(std::max((Dr_*SQ(Vi) + 2*Ar_*Dr_*dX) / (Dr_ - Ar_), 0.0f));
         Ta_ = std::max(0.0f, (Vr_ - Vi) / Ar_);
         Td_ = std::max(0.0f, -Vr_ / Dr_);
         Tv_ = 0.0f;
diff --git a/Firmware/Tests/test_trap_traj.cpp b/Firmware/Tests/test_trap_traj.cpp
index d95328dd..90d8275f 100644
--- a/Firmware/Tests/test_trap_traj.cpp
+++ b/Firmware/Tests/test_trap_traj.cpp
@@ -85,7 +85,7 @@ bool TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, float Vi,
     // Are we displacing enough to reach cruising speed?
     if (s*dX < s*dXmin) {
         // Short move (triangle profile)
-        Vr_ = s * sqrtf(std::fmax((Dr_*SQ(Vi) + 2*Ar_*Dr_*dX) / (Dr_ - Ar_), 0.0f));
+        Vr_ = s * sqrtf(std::max((Dr_*SQ(Vi) + 2*Ar_*Dr_*dX) / (Dr_ - Ar_), 0.0f));
         //Vr_ = s * sqrtf((Dr_*SQ(Vi) + 2*Ar_*Dr_*dX) / (Dr_ - Ar_));
         Ta_ = std::max(0.0f, (Vr_ - Vi) / Ar_);
         Td_ = std::max(0.0f, -Vr_ / Dr_);

From fb7d1d2961bba82e4ede5f32a07b5ca0b4b54967 Mon Sep 17 00:00:00 2001
From: Oskar Weigl <oskar.weigl@gmail.com>
Date: Fri, 4 Sep 2020 18:27:14 -0700
Subject: [PATCH 05/19] change fabsf to std abs

---
 Firmware/MotorControl/controller.cpp | 2 +-
 Firmware/MotorControl/motor.cpp      | 4 ++--
 Firmware/MotorControl/utils.cpp      | 4 ++--
 3 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp
index ecea3c9c..88906cf5 100644
--- a/Firmware/MotorControl/controller.cpp
+++ b/Firmware/MotorControl/controller.cpp
@@ -275,7 +275,7 @@ bool Controller::update(float* torque_setpoint_output) {
     if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_ACIM) {
         float effective_flux = axis_->motor_.current_control_.acim_rotor_flux;
         float minflux = axis_->motor_.config_.acim_gain_min_flux;
-        if (fabsf(effective_flux) < minflux)
+        if (std::abs(effective_flux) < minflux)
             effective_flux = std::copysignf(minflux, effective_flux);
         vel_gain /= effective_flux;
         vel_integrator_gain /= effective_flux;
diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp
index 0b0a2a31..b148ea7c 100644
--- a/Firmware/MotorControl/motor.cpp
+++ b/Firmware/MotorControl/motor.cpp
@@ -434,7 +434,7 @@ bool Motor::update(float torque_setpoint, float phase, float phase_vel) {
         // So we elect to write it as if the effect is immediate, to have cleaner code
 
         if (config_.acim_autoflux_enable) {
-            float abs_iq = fabsf(iq);
+            float abs_iq = std::abs(iq);
             float gain = abs_iq > id ? config_.acim_autoflux_attack_gain : config_.acim_autoflux_decay_gain;
             id += gain * (abs_iq - id) * current_meas_period;
             id = std::clamp(id, config_.acim_autoflux_min_Id, ilim);
@@ -446,7 +446,7 @@ bool Motor::update(float torque_setpoint, float phase, float phase_vel) {
         current_control_.acim_rotor_flux += dflux_by_dt * current_meas_period;
         float slip_velocity = config_.acim_slip_velocity * (iq / current_control_.acim_rotor_flux);
         // Check for issues with small denominator. Polarity of check to catch NaN too
-        bool acceptable_vel = fabsf(slip_velocity) <= 0.1f * (float)current_meas_hz;
+        bool acceptable_vel = std::abs(slip_velocity) <= 0.1f * (float)current_meas_hz;
         if (!acceptable_vel)
             slip_velocity = 0.0f;
         phase_vel += slip_velocity;
diff --git a/Firmware/MotorControl/utils.cpp b/Firmware/MotorControl/utils.cpp
index f4dc212d..3bf30baf 100644
--- a/Firmware/MotorControl/utils.cpp
+++ b/Firmware/MotorControl/utils.cpp
@@ -124,8 +124,8 @@ int SVM(float alpha, float beta, float* tA, float* tB, float* tC) {
 // based on https://math.stackexchange.com/a/1105038/81278
 float fast_atan2(float y, float x) {
     // a := min (|x|, |y|) / max (|x|, |y|)
-    float abs_y = fabsf(y);
-    float abs_x = fabsf(x);
+    float abs_y = std::abs(y);
+    float abs_x = std::abs(x);
     // inject FLT_MIN in denominator to avoid division by zero
     float a = MACRO_MIN(abs_x, abs_y) / (MACRO_MAX(abs_x, abs_y) + FLT_MIN);
     // s := a * a

From 412329d71a6eb7a42a93f804e783be5361f9cc62 Mon Sep 17 00:00:00 2001
From: Oskar Weigl <oskar.weigl@gmail.com>
Date: Fri, 4 Sep 2020 19:46:12 -0700
Subject: [PATCH 06/19] change to cmath by default

---
 Firmware/Drivers/DRV8301/drv8301.cpp          | 2 +-
 Firmware/MotorControl/low_level.cpp           | 2 +-
 Firmware/MotorControl/trapTraj.cpp            | 2 +-
 Firmware/MotorControl/utils.cpp               | 2 +-
 Firmware/MotorControl/utils.hpp               | 2 +-
 Firmware/Tests/test_trap_traj.cpp             | 2 +-
 Firmware/fibre/cpp/include/fibre/protocol.hpp | 2 +-
 7 files changed, 7 insertions(+), 7 deletions(-)

diff --git a/Firmware/Drivers/DRV8301/drv8301.cpp b/Firmware/Drivers/DRV8301/drv8301.cpp
index e2fb20c2..3e1cbf91 100644
--- a/Firmware/Drivers/DRV8301/drv8301.cpp
+++ b/Firmware/Drivers/DRV8301/drv8301.cpp
@@ -41,7 +41,7 @@
 #include "utils.hpp"
 
 #include "cmsis_os.h"
-#include <math.h>
+#include <cmath>
 #include <array>
 #include <algorithm>
 
diff --git a/Firmware/MotorControl/low_level.cpp b/Firmware/MotorControl/low_level.cpp
index af258c29..ea6b879b 100644
--- a/Firmware/MotorControl/low_level.cpp
+++ b/Firmware/MotorControl/low_level.cpp
@@ -3,7 +3,7 @@
 #include <board.h>
 
 #include <cmsis_os.h>
-#include <math.h>
+#include <cmath>
 #include <stdint.h>
 #include <stdlib.h>
 
diff --git a/Firmware/MotorControl/trapTraj.cpp b/Firmware/MotorControl/trapTraj.cpp
index cb271fcd..6bc73e18 100644
--- a/Firmware/MotorControl/trapTraj.cpp
+++ b/Firmware/MotorControl/trapTraj.cpp
@@ -1,4 +1,4 @@
-#include <math.h>
+#include <cmath>
 #include "odrive_main.h"
 #include "utils.hpp"
 
diff --git a/Firmware/MotorControl/utils.cpp b/Firmware/MotorControl/utils.cpp
index 3bf30baf..313b5da0 100644
--- a/Firmware/MotorControl/utils.cpp
+++ b/Firmware/MotorControl/utils.cpp
@@ -1,6 +1,6 @@
 
 #include <utils.hpp>
-#include <math.h>
+#include <cmath>
 #include <float.h>
 #include <cmsis_os.h>
 #include <board.h>
diff --git a/Firmware/MotorControl/utils.hpp b/Firmware/MotorControl/utils.hpp
index e70a9ccc..8c9143fa 100644
--- a/Firmware/MotorControl/utils.hpp
+++ b/Firmware/MotorControl/utils.hpp
@@ -3,7 +3,7 @@
 #define __UTILS_H
 
 #include <stdint.h>
-#include <math.h>
+#include <cmath>
 
 /**
  * @brief Flash size register address
diff --git a/Firmware/Tests/test_trap_traj.cpp b/Firmware/Tests/test_trap_traj.cpp
index 90d8275f..8d5399ff 100644
--- a/Firmware/Tests/test_trap_traj.cpp
+++ b/Firmware/Tests/test_trap_traj.cpp
@@ -1,7 +1,7 @@
 
 #include <doctest.h>
 #include <limits.h>
-#include <math.h>
+#include <cmath>
 #include <iostream>
 #include <random>
 
diff --git a/Firmware/fibre/cpp/include/fibre/protocol.hpp b/Firmware/fibre/cpp/include/fibre/protocol.hpp
index 5d529c39..2a01e220 100644
--- a/Firmware/fibre/cpp/include/fibre/protocol.hpp
+++ b/Firmware/fibre/cpp/include/fibre/protocol.hpp
@@ -10,7 +10,7 @@ see protocol.md for the protocol specification
 
 #include <functional>
 #include <limits>
-#include <math.h>
+#include <cmath>
 //#include <stdint.h>
 #include <stdio.h>
 #include <string.h>

From ec63f956f98efe7087d4dbd423de28d8c6f550e3 Mon Sep 17 00:00:00 2001
From: Oskar Weigl <oskar.weigl@gmail.com>
Date: Fri, 4 Sep 2020 20:17:35 -0700
Subject: [PATCH 07/19] no need for special fma function, the compiler knows

---
 Firmware/MotorControl/utils.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Firmware/MotorControl/utils.cpp b/Firmware/MotorControl/utils.cpp
index 313b5da0..d518fa56 100644
--- a/Firmware/MotorControl/utils.cpp
+++ b/Firmware/MotorControl/utils.cpp
@@ -151,7 +151,7 @@ float fast_atan2(float y, float x) {
 float horner_fma(float x, const float *coeffs, size_t count) {
     float result = 0.0f;
     for (size_t idx = 0; idx < count; ++idx)
-        result = fmaf(result, x, coeffs[idx]);
+        result = (result * x) + coeffs[idx];
     return result;
 }
 

From 600832f8cff4394ecec00899d791684e7fb0e957 Mon Sep 17 00:00:00 2001
From: Oskar Weigl <oskar.weigl@gmail.com>
Date: Fri, 4 Sep 2020 20:31:58 -0700
Subject: [PATCH 08/19] drop flag fno-finite-math-only, and implement explicit
 nan check function

---
 Firmware/MotorControl/low_level.cpp  | 14 ++++++++------
 Firmware/MotorControl/motor.cpp      |  9 ++++-----
 Firmware/MotorControl/thermistor.cpp |  2 +-
 Firmware/MotorControl/utils.cpp      |  5 ++---
 Firmware/MotorControl/utils.hpp      | 11 +++++++++--
 Firmware/Tupfile.lua                 |  2 +-
 6 files changed, 25 insertions(+), 18 deletions(-)

diff --git a/Firmware/MotorControl/low_level.cpp b/Firmware/MotorControl/low_level.cpp
index ea6b879b..9a6f9ca2 100644
--- a/Firmware/MotorControl/low_level.cpp
+++ b/Firmware/MotorControl/low_level.cpp
@@ -296,7 +296,7 @@ void start_general_purpose_adc() {
 // @brief Returns the ADC voltage associated with the specified pin.
 // This only works if the GPIO was not used for anything else since bootup, otherwise
 // it must be put to analog mode first.
-// Returns NaN if the pin has no associated ADC1 channel.
+// Returns -1.0f if the pin has no associated ADC1 channel.
 //
 // On ODrive 3.3 and 3.4 the following pins can be used with this function:
 //  GPIO_1, GPIO_2, GPIO_3, GPIO_4 and some pins that are connected to
@@ -359,13 +359,13 @@ uint16_t channel_from_gpio(Stm32Gpio gpio) {
 }
 
 // @brief Given an adc channel return the measured voltage.
-// returns NaN if the channel is not valid.
+// returns -1.0f if the channel is not valid.
 float get_adc_voltage_channel(uint16_t channel)
 {
     if (channel < ADC_CHANNEL_COUNT)
         return ((float)adc_measurements_[channel]) * (adc_ref_voltage / adc_full_scale);
     else
-        return 0.0f / 0.0f; // NaN
+        return -1.0f;
 }
 
 //--------------------------------
@@ -497,7 +497,7 @@ void update_brake_current() {
         brake_duty += std::max((vbus_voltage - odrv.config_.dc_bus_overvoltage_ramp_start) / (odrv.config_.dc_bus_overvoltage_ramp_end - odrv.config_.dc_bus_overvoltage_ramp_start), 0.0f);
     }
 
-    if (std::isnan(brake_duty)) {
+    if (is_nan(brake_duty)) {
         // Shuts off all motors AND brake resistor, sets error code on all motors.
         low_level_fault(Motor::ERROR_BRAKE_DUTY_CYCLE_NAN);
         return;
@@ -510,8 +510,10 @@ void update_brake_current() {
     // Duty limit at 95% to allow bootstrap caps to charge
     brake_duty = std::clamp(brake_duty, 0.0f, 0.95f);
 
-    // Special handling to avoid the case 0.0/0.0 == NaN.
-    Ibus_sum += brake_duty ? (brake_duty * vbus_voltage / odrv.config_.brake_resistance) : 0.0f;
+    // Special handling to avoid the case 0.0/0.0 == NaN, or divide by 0.
+    if (odrv.config_.brake_resistance > 0.0f) {
+        Ibus_sum += brake_duty * vbus_voltage / odrv.config_.brake_resistance;
+    }
 
     ibus_ += odrv.ibus_report_filter_k_ * (Ibus_sum - ibus_);
 
diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp
index b148ea7c..3e160dee 100644
--- a/Firmware/MotorControl/motor.cpp
+++ b/Firmware/MotorControl/motor.cpp
@@ -260,10 +260,10 @@ bool Motor::run_calibration() {
 }
 
 bool Motor::enqueue_modulation_timings(float mod_alpha, float mod_beta) {
-    if (std::isnan(mod_alpha) || std::isnan(mod_alpha))
+    if (is_nan(mod_alpha) || is_nan(mod_beta))
         return set_error(ERROR_MODULATION_IS_NAN), false;
     float tA, tB, tC;
-    if (SVM(mod_alpha, mod_beta, &tA, &tB, &tC) != 0)
+    if (!SVM(mod_alpha, mod_beta, &tA, &tB, &tC))
         return set_error(ERROR_MODULATION_MAGNITUDE), false;
     next_timings_[0] = (uint16_t)(tA * (float)TIM_1_8_PERIOD_CLOCKS);
     next_timings_[1] = (uint16_t)(tB * (float)TIM_1_8_PERIOD_CLOCKS);
@@ -445,9 +445,8 @@ bool Motor::update(float torque_setpoint, float phase, float phase_vel) {
         float dflux_by_dt = config_.acim_slip_velocity * (id - current_control_.acim_rotor_flux);
         current_control_.acim_rotor_flux += dflux_by_dt * current_meas_period;
         float slip_velocity = config_.acim_slip_velocity * (iq / current_control_.acim_rotor_flux);
-        // Check for issues with small denominator. Polarity of check to catch NaN too
-        bool acceptable_vel = std::abs(slip_velocity) <= 0.1f * (float)current_meas_hz;
-        if (!acceptable_vel)
+        // Check for issues with small denominator.
+        if (is_nan(slip_velocity) || std::abs(slip_velocity) > 0.1f * (float)current_meas_hz)
             slip_velocity = 0.0f;
         phase_vel += slip_velocity;
         // reporting only:
diff --git a/Firmware/MotorControl/thermistor.cpp b/Firmware/MotorControl/thermistor.cpp
index 3baf78a0..617dee75 100644
--- a/Firmware/MotorControl/thermistor.cpp
+++ b/Firmware/MotorControl/thermistor.cpp
@@ -42,7 +42,7 @@ float ThermistorCurrentLimiter::get_current_limit(float base_current_lim) const
     const float temp_margin = temp_limit_upper_ - temperature_;
     const float derating_range = temp_limit_upper_ - temp_limit_lower_;
     float thermal_current_lim = base_current_lim * (temp_margin / derating_range);
-    if (!(thermal_current_lim >= 0.0f)) { // Funny polarity to also catch NaN
+    if (thermal_current_lim < 0.0f || is_nan(thermal_current_lim)) {
         thermal_current_lim = 0.0f;
     }
 
diff --git a/Firmware/MotorControl/utils.cpp b/Firmware/MotorControl/utils.cpp
index d518fa56..24aa9569 100644
--- a/Firmware/MotorControl/utils.cpp
+++ b/Firmware/MotorControl/utils.cpp
@@ -5,7 +5,7 @@
 #include <cmsis_os.h>
 #include <board.h>
 
-int SVM(float alpha, float beta, float* tA, float* tB, float* tC) {
+bool SVM(float alpha, float beta, float* tA, float* tB, float* tC) {
     int Sextant;
 
     if (beta >= 0.0f) {
@@ -113,12 +113,11 @@ int SVM(float alpha, float beta, float* tA, float* tB, float* tC) {
         } break;
     }
 
-    // if any of the results becomes NaN, result_valid will evaluate to false
     int result_valid =
             *tA >= 0.0f && *tA <= 1.0f
          && *tB >= 0.0f && *tB <= 1.0f
          && *tC >= 0.0f && *tC <= 1.0f;
-    return result_valid ? 0 : -1;
+    return result_valid;
 }
 
 // based on https://math.stackexchange.com/a/1105038/81278
diff --git a/Firmware/MotorControl/utils.hpp b/Firmware/MotorControl/utils.hpp
index 8c9143fa..e31f27cf 100644
--- a/Firmware/MotorControl/utils.hpp
+++ b/Firmware/MotorControl/utils.hpp
@@ -77,6 +77,13 @@ std::array<T, 1 + sizeof...(Tail)> make_array(T head, Tail... tail)
     return std::array<T, 1 + sizeof...(Tail)>({ head, tail ... });
 }
 
+// To allow use of -ffast-math we need to have a special check for nan
+// that bypasses the "ignore nan" flag
+__attribute__((optimize("-fno-finite-math-only")))
+static inline bool is_nan(float x) {
+    return __builtin_isnan(x);;
+}
+
 extern "C" {
 #endif
 
@@ -122,8 +129,8 @@ static inline float wrap_pm_pi(float x) {
 // Compute rising edge timings (0.0 - 1.0) as a function of alpha-beta
 // as per the magnitude invariant clarke transform
 // The magnitude of the alpha-beta vector may not be larger than sqrt(3)/2
-// Returns 0 on success, and -1 if the input was out of range
-int SVM(float alpha, float beta, float* tA, float* tB, float* tC);
+// Returns true on success, and false if the input was out of range
+bool 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);
diff --git a/Firmware/Tupfile.lua b/Firmware/Tupfile.lua
index d2cf3cbf..f5a78ad8 100644
--- a/Firmware/Tupfile.lua
+++ b/Firmware/Tupfile.lua
@@ -149,7 +149,7 @@ else
 end
 
 -- common flags for ASM, C and C++
-OPT += '-ffast-math -fno-finite-math-only'
+OPT += '-ffast-math'
 tup.append_table(FLAGS, OPT)
 tup.append_table(LDFLAGS, OPT)
 

From 4815e5ed19a6d99df8930a2bf15be1386a9b955b Mon Sep 17 00:00:00 2001
From: Unknown <pfguenette@gmail.com>
Date: Sat, 5 Sep 2020 00:23:58 -0400
Subject: [PATCH 09/19] Change sqrtf to std::sqrt

---
 Firmware/MotorControl/motor.cpp    | 2 +-
 Firmware/MotorControl/trapTraj.cpp | 2 +-
 Firmware/Tests/test_trap_traj.cpp  | 4 ++--
 3 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp
index 3e160dee..bfee0047 100644
--- a/Firmware/MotorControl/motor.cpp
+++ b/Firmware/MotorControl/motor.cpp
@@ -349,7 +349,7 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float I_phase, float pwm_pha
 
     // Vector modulation saturation, lock integrator if saturated
     // TODO make maximum modulation configurable
-    float mod_scalefactor = 0.80f * sqrt3_by_2 * 1.0f / sqrtf(mod_d * mod_d + mod_q * mod_q);
+    float mod_scalefactor = 0.80f * sqrt3_by_2 * 1.0f / std::sqrt(mod_d * mod_d + mod_q * mod_q);
     if (mod_scalefactor < 1.0f) {
         mod_d *= mod_scalefactor;
         mod_q *= mod_scalefactor;
diff --git a/Firmware/MotorControl/trapTraj.cpp b/Firmware/MotorControl/trapTraj.cpp
index 6bc73e18..b4f94a37 100644
--- a/Firmware/MotorControl/trapTraj.cpp
+++ b/Firmware/MotorControl/trapTraj.cpp
@@ -42,7 +42,7 @@ bool TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, float Vi,
     // Are we displacing enough to reach cruising speed?
     if (s*dX < s*dXmin) {
         // Short move (triangle profile)
-        Vr_ = s * sqrtf(std::max((Dr_*SQ(Vi) + 2*Ar_*Dr_*dX) / (Dr_ - Ar_), 0.0f));
+        Vr_ = s * std::sqrt(std::max((Dr_*SQ(Vi) + 2*Ar_*Dr_*dX) / (Dr_ - Ar_), 0.0f));
         Ta_ = std::max(0.0f, (Vr_ - Vi) / Ar_);
         Td_ = std::max(0.0f, -Vr_ / Dr_);
         Tv_ = 0.0f;
diff --git a/Firmware/Tests/test_trap_traj.cpp b/Firmware/Tests/test_trap_traj.cpp
index 8d5399ff..8d5aad2d 100644
--- a/Firmware/Tests/test_trap_traj.cpp
+++ b/Firmware/Tests/test_trap_traj.cpp
@@ -85,8 +85,8 @@ bool TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, float Vi,
     // Are we displacing enough to reach cruising speed?
     if (s*dX < s*dXmin) {
         // Short move (triangle profile)
-        Vr_ = s * sqrtf(std::max((Dr_*SQ(Vi) + 2*Ar_*Dr_*dX) / (Dr_ - Ar_), 0.0f));
-        //Vr_ = s * sqrtf((Dr_*SQ(Vi) + 2*Ar_*Dr_*dX) / (Dr_ - Ar_));
+        Vr_ = s * std::sqrt(std::max((Dr_*SQ(Vi) + 2*Ar_*Dr_*dX) / (Dr_ - Ar_), 0.0f));
+        //Vr_ = s * std::sqrt((Dr_*SQ(Vi) + 2*Ar_*Dr_*dX) / (Dr_ - Ar_));
         Ta_ = std::max(0.0f, (Vr_ - Vi) / Ar_);
         Td_ = std::max(0.0f, -Vr_ / Dr_);
         Tv_ = 0.0f;

From 9e1e054f0a741efd7282ad1e981e07fd9b36a8f0 Mon Sep 17 00:00:00 2001
From: Unknown <pfguenette@gmail.com>
Date: Sat, 5 Sep 2020 00:30:12 -0400
Subject: [PATCH 10/19] Bring several functions from utils.cpp into utils.hpp
 for inlining

---
 Firmware/MotorControl/utils.cpp | 157 ------------------------
 Firmware/MotorControl/utils.hpp | 209 ++++++++++++++++++++++++++------
 2 files changed, 175 insertions(+), 191 deletions(-)

diff --git a/Firmware/MotorControl/utils.cpp b/Firmware/MotorControl/utils.cpp
index 24aa9569..8c041d55 100644
--- a/Firmware/MotorControl/utils.cpp
+++ b/Firmware/MotorControl/utils.cpp
@@ -1,164 +1,7 @@
 
 #include <utils.hpp>
-#include <cmath>
-#include <float.h>
-#include <cmsis_os.h>
 #include <board.h>
 
-bool SVM(float alpha, float beta, float* tA, float* tB, float* tC) {
-    int Sextant;
-
-    if (beta >= 0.0f) {
-        if (alpha >= 0.0f) {
-            //quadrant I
-            if (one_by_sqrt3 * beta > alpha)
-                Sextant = 2; //sextant v2-v3
-            else
-                Sextant = 1; //sextant v1-v2
-
-        } else {
-            //quadrant II
-            if (-one_by_sqrt3 * beta > alpha)
-                Sextant = 3; //sextant v3-v4
-            else
-                Sextant = 2; //sextant v2-v3
-        }
-    } else {
-        if (alpha >= 0.0f) {
-            //quadrant IV
-            if (-one_by_sqrt3 * beta > alpha)
-                Sextant = 5; //sextant v5-v6
-            else
-                Sextant = 6; //sextant v6-v1
-        } else {
-            //quadrant III
-            if (one_by_sqrt3 * beta > alpha)
-                Sextant = 4; //sextant v4-v5
-            else
-                Sextant = 5; //sextant v5-v6
-        }
-    }
-
-    switch (Sextant) {
-        // sextant v1-v2
-        case 1: {
-            // Vector on-times
-            float t1 = alpha - one_by_sqrt3 * beta;
-            float t2 = two_by_sqrt3 * beta;
-
-            // PWM timings
-            *tA = (1.0f - t1 - t2) * 0.5f;
-            *tB = *tA + t1;
-            *tC = *tB + t2;
-        } break;
-
-        // sextant v2-v3
-        case 2: {
-            // Vector on-times
-            float t2 = alpha + one_by_sqrt3 * beta;
-            float t3 = -alpha + one_by_sqrt3 * beta;
-
-            // PWM timings
-            *tB = (1.0f - t2 - t3) * 0.5f;
-            *tA = *tB + t3;
-            *tC = *tA + t2;
-        } break;
-
-        // sextant v3-v4
-        case 3: {
-            // Vector on-times
-            float t3 = two_by_sqrt3 * beta;
-            float t4 = -alpha - one_by_sqrt3 * beta;
-
-            // PWM timings
-            *tB = (1.0f - t3 - t4) * 0.5f;
-            *tC = *tB + t3;
-            *tA = *tC + t4;
-        } break;
-
-        // sextant v4-v5
-        case 4: {
-            // Vector on-times
-            float t4 = -alpha + one_by_sqrt3 * beta;
-            float t5 = -two_by_sqrt3 * beta;
-
-            // PWM timings
-            *tC = (1.0f - t4 - t5) * 0.5f;
-            *tB = *tC + t5;
-            *tA = *tB + t4;
-        } break;
-
-        // sextant v5-v6
-        case 5: {
-            // Vector on-times
-            float t5 = -alpha - one_by_sqrt3 * beta;
-            float t6 = alpha - one_by_sqrt3 * beta;
-
-            // PWM timings
-            *tC = (1.0f - t5 - t6) * 0.5f;
-            *tA = *tC + t5;
-            *tB = *tA + t6;
-        } break;
-
-        // sextant v6-v1
-        case 6: {
-            // Vector on-times
-            float t6 = -two_by_sqrt3 * beta;
-            float t1 = alpha + one_by_sqrt3 * beta;
-
-            // PWM timings
-            *tA = (1.0f - t6 - t1) * 0.5f;
-            *tC = *tA + t1;
-            *tB = *tC + t6;
-        } break;
-    }
-
-    int result_valid =
-            *tA >= 0.0f && *tA <= 1.0f
-         && *tB >= 0.0f && *tB <= 1.0f
-         && *tC >= 0.0f && *tC <= 1.0f;
-    return result_valid;
-}
-
-// based on https://math.stackexchange.com/a/1105038/81278
-float fast_atan2(float y, float x) {
-    // a := min (|x|, |y|) / max (|x|, |y|)
-    float abs_y = std::abs(y);
-    float abs_x = std::abs(x);
-    // inject FLT_MIN in denominator to avoid division by zero
-    float a = MACRO_MIN(abs_x, abs_y) / (MACRO_MAX(abs_x, abs_y) + FLT_MIN);
-    // s := a * a
-    float s = a * a;
-    // r := ((-0.0464964749 * s + 0.15931422) * s - 0.327622764) * s * a + a
-    float r = ((-0.0464964749f * s + 0.15931422f) * s - 0.327622764f) * s * a + a;
-    // if |y| > |x| then r := 1.57079637 - r
-    if (abs_y > abs_x)
-        r = 1.57079637f - r;
-    // if x < 0 then r := 3.14159274 - r
-    if (x < 0.0f)
-        r = 3.14159274f - r;
-    // if y < 0 then r := -r
-    if (y < 0.0f)
-        r = -r;
-
-    return r;
-}
-
-// Evaluate polynomials using Fused Multiply Add intrisic instruction.
-// coeffs[0] is highest order, as per numpy.polyfit
-// p(x) = coeffs[0] * x^deg + ... + coeffs[deg], for some degree "deg"
-float horner_fma(float x, const float *coeffs, size_t count) {
-    float result = 0.0f;
-    for (size_t idx = 0; idx < count; ++idx)
-        result = (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;
-    return (r < 0) ? (r + divisor) : r;
-}
 
 // @brief: Returns how much time is left until the deadline is reached.
 // If the deadline has already passed, the return value is 0 (except if
diff --git a/Firmware/MotorControl/utils.hpp b/Firmware/MotorControl/utils.hpp
index e31f27cf..9e978d3a 100644
--- a/Firmware/MotorControl/utils.hpp
+++ b/Firmware/MotorControl/utils.hpp
@@ -1,9 +1,8 @@
-
-#ifndef __UTILS_H
-#define __UTILS_H
+#pragma once
 
 #include <stdint.h>
-#include <cmath>
+#include <limits>
+#include <algorithm>
 
 /**
  * @brief Flash size register address
@@ -61,8 +60,6 @@
 
 #define SQ(x) ((x) * (x))
 
-#ifdef __cplusplus
-
 #include <array>
 
 /**
@@ -71,32 +68,30 @@
  * has to match exactly. Whereas initializer lists allow
  * less arguments.
  */
-template<class T, class... Tail>
-std::array<T, 1 + sizeof...(Tail)> make_array(T head, Tail... tail)
-{
-    return std::array<T, 1 + sizeof...(Tail)>({ head, tail ... });
+template <class T, class... Tail>
+std::array<T, 1 + sizeof...(Tail)> make_array(T head, Tail... tail) {
+    return std::array<T, 1 + sizeof...(Tail)>({head, tail...});
 }
 
 // To allow use of -ffast-math we need to have a special check for nan
 // that bypasses the "ignore nan" flag
-__attribute__((optimize("-fno-finite-math-only")))
-static inline bool is_nan(float x) {
-    return __builtin_isnan(x);;
+__attribute__((optimize("-fno-finite-math-only"))) static inline bool is_nan(float x) {
+    return __builtin_isnan(x);
+    ;
 }
 
-extern "C" {
-#endif
-
 static const float one_by_sqrt3 = 0.57735026919f;
 static const float two_by_sqrt3 = 1.15470053838f;
 static const float sqrt3_by_2 = 0.86602540378f;
 
 // Round to integer
 // Default rounding mode: round to nearest
-static inline int round_int(float x) {
+inline int round_int(float x) {
 #ifdef __arm__
     int res;
-    asm("vcvtr.s32.f32   %[res], %[x]" : [res] "=X" (res) : [x] "w" (x));
+    asm("vcvtr.s32.f32   %[res], %[x]"
+        : [ res ] "=X"(res)
+        : [ x ] "w"(x));
     return res;
 #else
     return (int)nearbyint(x);
@@ -106,35 +101,185 @@ static inline int round_int(float x) {
 // Wrap value to range.
 // With default rounding mode (round to nearest),
 // the result will be in range -y/2 to y/2
-static inline float wrap_pm(float x, float y) {
+inline float wrap_pm(float x, float y) {
 #ifdef FPU_FPV4
-    float intval = (float)round_int(x/y);
+    float intval = (float)round_int(x / y);
 #else
-    float intval = nearbyint(x/y);
+    float intval = nearbyint(x / y);
 #endif
     return x - intval * y;
 }
 
 // Same as fmodf but result is positive and y must be positive
-static inline float fmodf_pos(float x, float y) {
+inline float fmodf_pos(float x, float y) {
     float res = wrap_pm(x, y);
     if (res < 0) res += y;
     return res;
 }
 
-static inline float wrap_pm_pi(float x) {
-    return wrap_pm(x, 2*M_PI);
+inline float wrap_pm_pi(float x) {
+    return wrap_pm(x, 2 * M_PI);
 }
 
 // Compute rising edge timings (0.0 - 1.0) as a function of alpha-beta
 // as per the magnitude invariant clarke transform
 // The magnitude of the alpha-beta vector may not be larger than sqrt(3)/2
 // Returns true on success, and false if the input was out of range
-bool SVM(float alpha, float beta, float* tA, float* tB, float* tC);
+inline bool SVM(float alpha, float beta, float* tA, float* tB, float* tC) {
+    int Sextant;
 
-float fast_atan2(float y, float x);
-float horner_fma(float x, const float *coeffs, size_t count);
-int mod(int dividend, int divisor);
+    if (beta >= 0.0f) {
+        if (alpha >= 0.0f) {
+            //quadrant I
+            if (one_by_sqrt3 * beta > alpha)
+                Sextant = 2; //sextant v2-v3
+            else
+                Sextant = 1; //sextant v1-v2
+
+        } else {
+            //quadrant II
+            if (-one_by_sqrt3 * beta > alpha)
+                Sextant = 3; //sextant v3-v4
+            else
+                Sextant = 2; //sextant v2-v3
+        }
+    } else {
+        if (alpha >= 0.0f) {
+            //quadrant IV
+            if (-one_by_sqrt3 * beta > alpha)
+                Sextant = 5; //sextant v5-v6
+            else
+                Sextant = 6; //sextant v6-v1
+        } else {
+            //quadrant III
+            if (one_by_sqrt3 * beta > alpha)
+                Sextant = 4; //sextant v4-v5
+            else
+                Sextant = 5; //sextant v5-v6
+        }
+    }
+
+    switch (Sextant) {
+        // sextant v1-v2
+        case 1: {
+            // Vector on-times
+            float t1 = alpha - one_by_sqrt3 * beta;
+            float t2 = two_by_sqrt3 * beta;
+
+            // PWM timings
+            *tA = (1.0f - t1 - t2) * 0.5f;
+            *tB = *tA + t1;
+            *tC = *tB + t2;
+        } break;
+
+        // sextant v2-v3
+        case 2: {
+            // Vector on-times
+            float t2 = alpha + one_by_sqrt3 * beta;
+            float t3 = -alpha + one_by_sqrt3 * beta;
+
+            // PWM timings
+            *tB = (1.0f - t2 - t3) * 0.5f;
+            *tA = *tB + t3;
+            *tC = *tA + t2;
+        } break;
+
+        // sextant v3-v4
+        case 3: {
+            // Vector on-times
+            float t3 = two_by_sqrt3 * beta;
+            float t4 = -alpha - one_by_sqrt3 * beta;
+
+            // PWM timings
+            *tB = (1.0f - t3 - t4) * 0.5f;
+            *tC = *tB + t3;
+            *tA = *tC + t4;
+        } break;
+
+        // sextant v4-v5
+        case 4: {
+            // Vector on-times
+            float t4 = -alpha + one_by_sqrt3 * beta;
+            float t5 = -two_by_sqrt3 * beta;
+
+            // PWM timings
+            *tC = (1.0f - t4 - t5) * 0.5f;
+            *tB = *tC + t5;
+            *tA = *tB + t4;
+        } break;
+
+        // sextant v5-v6
+        case 5: {
+            // Vector on-times
+            float t5 = -alpha - one_by_sqrt3 * beta;
+            float t6 = alpha - one_by_sqrt3 * beta;
+
+            // PWM timings
+            *tC = (1.0f - t5 - t6) * 0.5f;
+            *tA = *tC + t5;
+            *tB = *tA + t6;
+        } break;
+
+        // sextant v6-v1
+        case 6: {
+            // Vector on-times
+            float t6 = -two_by_sqrt3 * beta;
+            float t1 = alpha + one_by_sqrt3 * beta;
+
+            // PWM timings
+            *tA = (1.0f - t6 - t1) * 0.5f;
+            *tC = *tA + t1;
+            *tB = *tC + t6;
+        } break;
+    }
+
+    int result_valid =
+            *tA >= 0.0f && *tA <= 1.0f
+         && *tB >= 0.0f && *tB <= 1.0f
+         && *tC >= 0.0f && *tC <= 1.0f;
+    return result_valid;
+}
+
+
+// based on https://math.stackexchange.com/a/1105038/81278
+inline float fast_atan2(const float y, const float x) {
+    // a := min (|x|, |y|) / max (|x|, |y|)
+    float abs_y = std::abs(y);
+    float abs_x = std::abs(x);
+    // inject FLT_MIN in denominator to avoid division by zero
+    float a = std::min(abs_x, abs_y) / (std::max(abs_x, abs_y) + std::numeric_limits<float>::min());
+    // s := a * a
+    float s = a * a;
+    // r := ((-0.0464964749 * s + 0.15931422) * s - 0.327622764) * s * a + a
+    float r = ((-0.0464964749f * s + 0.15931422f) * s - 0.327622764f) * s * a + a;
+    // if |y| > |x| then r := 1.57079637 - r
+    if (abs_y > abs_x)
+        r = 1.57079637f - r;
+    // if x < 0 then r := 3.14159274 - r
+    if (x < 0.0f)
+        r = 3.14159274f - r;
+    // if y < 0 then r := -r
+    if (y < 0.0f)
+        r = -r;
+
+    return r;
+}
+
+// Evaluate polynomials using Fused Multiply Add intrisic instruction.
+// coeffs[0] is highest order, as per numpy.polyfit
+// p(x) = coeffs[0] * x^deg + ... + coeffs[deg], for some degree "deg"
+inline float horner_fma(float x, const float *coeffs, size_t count) {
+    float result = 0.0f;
+    for (size_t idx = 0; idx < count; ++idx)
+        result = (result * x) + coeffs[idx];
+    return result;
+}
+
+// Modulo (as opposed to remainder), per https://stackoverflow.com/a/19288271
+inline int mod(const int dividend, const int divisor){
+    int r = dividend % divisor;
+    return (r < 0) ? (r + divisor) : r;
+}
 
 uint32_t deadline_to_timeout(uint32_t deadline_ms);
 uint32_t timeout_to_deadline(uint32_t timeout_ms);
@@ -143,11 +288,7 @@ int is_in_the_future(uint32_t time_ms);
 uint32_t micros(void);
 void delay_us(uint32_t us);
 
+extern "C" {
 float our_arm_sin_f32(float x);
 float our_arm_cos_f32(float x);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif  //__UTILS_H
+}
\ No newline at end of file

From fc5707a1a701cdd7969f5a21d73e80ccd142e57a Mon Sep 17 00:00:00 2001
From: Unknown <pfguenette@gmail.com>
Date: Sat, 5 Sep 2020 00:34:46 -0400
Subject: [PATCH 11/19] Remove unused macros

---
 Firmware/MotorControl/utils.hpp | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/Firmware/MotorControl/utils.hpp b/Firmware/MotorControl/utils.hpp
index 9e978d3a..e01afb7f 100644
--- a/Firmware/MotorControl/utils.hpp
+++ b/Firmware/MotorControl/utils.hpp
@@ -55,8 +55,7 @@
 #endif
 #define M_PI (3.14159265358979323846f)
 
-#define MACRO_MAX(x, y) (((x) > (y)) ? (x) : (y))
-#define MACRO_MIN(x, y) (((x) < (y)) ? (x) : (y))
+#include <array>
 
 #define SQ(x) ((x) * (x))
 

From 9d5e8614eb8323b8bfe8cc1f80cf6ab99013e714 Mon Sep 17 00:00:00 2001
From: Unknown <pfguenette@gmail.com>
Date: Sat, 5 Sep 2020 00:36:06 -0400
Subject: [PATCH 12/19] Change SQ macro to a constexpr function

---
 Firmware/MotorControl/utils.hpp | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/Firmware/MotorControl/utils.hpp b/Firmware/MotorControl/utils.hpp
index e01afb7f..d02c9fa6 100644
--- a/Firmware/MotorControl/utils.hpp
+++ b/Firmware/MotorControl/utils.hpp
@@ -3,6 +3,7 @@
 #include <stdint.h>
 #include <limits>
 #include <algorithm>
+#include <array>
 
 /**
  * @brief Flash size register address
@@ -55,11 +56,10 @@
 #endif
 #define M_PI (3.14159265358979323846f)
 
-#include <array>
-
-#define SQ(x) ((x) * (x))
-
-#include <array>
+template<typename T>
+constexpr T SQ(const T& x){
+    return x * x;
+}
 
 /**
  * @brief Small helper to make array with known size

From ea08c1fe77c35042f54edab26069b35ee5541af2 Mon Sep 17 00:00:00 2001
From: Unknown <pfguenette@gmail.com>
Date: Sat, 5 Sep 2020 00:43:40 -0400
Subject: [PATCH 13/19] Constexpr math constants

---
 Firmware/MotorControl/utils.hpp | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/Firmware/MotorControl/utils.hpp b/Firmware/MotorControl/utils.hpp
index d02c9fa6..7a663406 100644
--- a/Firmware/MotorControl/utils.hpp
+++ b/Firmware/MotorControl/utils.hpp
@@ -54,7 +54,12 @@
 #ifdef M_PI
 #undef M_PI
 #endif
-#define M_PI (3.14159265358979323846f)
+
+// Math Constants
+constexpr float M_PI = 3.14159265358979323846f;
+constexpr float one_by_sqrt3 = 0.57735026919f;
+constexpr float two_by_sqrt3 = 1.15470053838f;
+constexpr float sqrt3_by_2 = 0.86602540378f;
 
 template<typename T>
 constexpr T SQ(const T& x){
@@ -76,13 +81,8 @@ std::array<T, 1 + sizeof...(Tail)> make_array(T head, Tail... tail) {
 // that bypasses the "ignore nan" flag
 __attribute__((optimize("-fno-finite-math-only"))) static inline bool is_nan(float x) {
     return __builtin_isnan(x);
-    ;
 }
 
-static const float one_by_sqrt3 = 0.57735026919f;
-static const float two_by_sqrt3 = 1.15470053838f;
-static const float sqrt3_by_2 = 0.86602540378f;
-
 // Round to integer
 // Default rounding mode: round to nearest
 inline int round_int(float x) {

From 29332281be14e85a4d0e9ff8a8022e451225ff6f Mon Sep 17 00:00:00 2001
From: Unknown <pfguenette@gmail.com>
Date: Sat, 5 Sep 2020 00:56:59 -0400
Subject: [PATCH 14/19] Convert SVM to not use out parameters

---
 Firmware/MotorControl/motor.cpp |  4 +--
 Firmware/MotorControl/utils.hpp | 48 +++++++++++++++++----------------
 2 files changed, 27 insertions(+), 25 deletions(-)

diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp
index bfee0047..06493cd4 100644
--- a/Firmware/MotorControl/motor.cpp
+++ b/Firmware/MotorControl/motor.cpp
@@ -262,8 +262,8 @@ bool Motor::run_calibration() {
 bool Motor::enqueue_modulation_timings(float mod_alpha, float mod_beta) {
     if (is_nan(mod_alpha) || is_nan(mod_beta))
         return set_error(ERROR_MODULATION_IS_NAN), false;
-    float tA, tB, tC;
-    if (!SVM(mod_alpha, mod_beta, &tA, &tB, &tC))
+    auto [tA, tB, tC, success] = SVM(mod_alpha, mod_beta);
+    if(!success)
         return set_error(ERROR_MODULATION_MAGNITUDE), false;
     next_timings_[0] = (uint16_t)(tA * (float)TIM_1_8_PERIOD_CLOCKS);
     next_timings_[1] = (uint16_t)(tB * (float)TIM_1_8_PERIOD_CLOCKS);
diff --git a/Firmware/MotorControl/utils.hpp b/Firmware/MotorControl/utils.hpp
index 7a663406..251e2cbc 100644
--- a/Firmware/MotorControl/utils.hpp
+++ b/Firmware/MotorControl/utils.hpp
@@ -4,6 +4,7 @@
 #include <limits>
 #include <algorithm>
 #include <array>
+#include <tuple>
 
 /**
  * @brief Flash size register address
@@ -124,7 +125,8 @@ inline float wrap_pm_pi(float x) {
 // as per the magnitude invariant clarke transform
 // The magnitude of the alpha-beta vector may not be larger than sqrt(3)/2
 // Returns true on success, and false if the input was out of range
-inline bool SVM(float alpha, float beta, float* tA, float* tB, float* tC) {
+inline auto SVM(float alpha, float beta) {
+    float tA, tB, tC;
     int Sextant;
 
     if (beta >= 0.0f) {
@@ -166,9 +168,9 @@ inline bool SVM(float alpha, float beta, float* tA, float* tB, float* tC) {
             float t2 = two_by_sqrt3 * beta;
 
             // PWM timings
-            *tA = (1.0f - t1 - t2) * 0.5f;
-            *tB = *tA + t1;
-            *tC = *tB + t2;
+            tA = (1.0f - t1 - t2) * 0.5f;
+            tB = tA + t1;
+            tC = tB + t2;
         } break;
 
         // sextant v2-v3
@@ -178,9 +180,9 @@ inline bool SVM(float alpha, float beta, float* tA, float* tB, float* tC) {
             float t3 = -alpha + one_by_sqrt3 * beta;
 
             // PWM timings
-            *tB = (1.0f - t2 - t3) * 0.5f;
-            *tA = *tB + t3;
-            *tC = *tA + t2;
+            tB = (1.0f - t2 - t3) * 0.5f;
+            tA = tB + t3;
+            tC = tA + t2;
         } break;
 
         // sextant v3-v4
@@ -190,9 +192,9 @@ inline bool SVM(float alpha, float beta, float* tA, float* tB, float* tC) {
             float t4 = -alpha - one_by_sqrt3 * beta;
 
             // PWM timings
-            *tB = (1.0f - t3 - t4) * 0.5f;
-            *tC = *tB + t3;
-            *tA = *tC + t4;
+            tB = (1.0f - t3 - t4) * 0.5f;
+            tC = tB + t3;
+            tA = tC + t4;
         } break;
 
         // sextant v4-v5
@@ -202,9 +204,9 @@ inline bool SVM(float alpha, float beta, float* tA, float* tB, float* tC) {
             float t5 = -two_by_sqrt3 * beta;
 
             // PWM timings
-            *tC = (1.0f - t4 - t5) * 0.5f;
-            *tB = *tC + t5;
-            *tA = *tB + t4;
+            tC = (1.0f - t4 - t5) * 0.5f;
+            tB = tC + t5;
+            tA = tB + t4;
         } break;
 
         // sextant v5-v6
@@ -214,9 +216,9 @@ inline bool SVM(float alpha, float beta, float* tA, float* tB, float* tC) {
             float t6 = alpha - one_by_sqrt3 * beta;
 
             // PWM timings
-            *tC = (1.0f - t5 - t6) * 0.5f;
-            *tA = *tC + t5;
-            *tB = *tA + t6;
+            tC = (1.0f - t5 - t6) * 0.5f;
+            tA = tC + t5;
+            tB = tA + t6;
         } break;
 
         // sextant v6-v1
@@ -226,17 +228,17 @@ inline bool SVM(float alpha, float beta, float* tA, float* tB, float* tC) {
             float t1 = alpha + one_by_sqrt3 * beta;
 
             // PWM timings
-            *tA = (1.0f - t6 - t1) * 0.5f;
-            *tC = *tA + t1;
-            *tB = *tC + t6;
+            tA = (1.0f - t6 - t1) * 0.5f;
+            tC = tA + t1;
+            tB = tC + t6;
         } break;
     }
 
     int result_valid =
-            *tA >= 0.0f && *tA <= 1.0f
-         && *tB >= 0.0f && *tB <= 1.0f
-         && *tC >= 0.0f && *tC <= 1.0f;
-    return result_valid;
+            tA >= 0.0f && tA <= 1.0f
+         && tB >= 0.0f && tB <= 1.0f
+         && tC >= 0.0f && tC <= 1.0f;
+    return std::make_tuple(tA, tB, tC, result_valid);
 }
 
 

From b38fd2be9c87eb16bb08d5ddd4968d2796497fb4 Mon Sep 17 00:00:00 2001
From: Oskar Weigl <oskar.weigl@gmail.com>
Date: Sat, 5 Sep 2020 15:25:32 -0700
Subject: [PATCH 15/19] Only inline short functions

---
 Firmware/MotorControl/thermistor.cpp      |   2 +-
 Firmware/MotorControl/utils.cpp           | 146 ++++++++++++++++-
 Firmware/MotorControl/utils.hpp           | 188 +++-------------------
 Firmware/communication/interface_uart.cpp |   5 -
 4 files changed, 171 insertions(+), 170 deletions(-)

diff --git a/Firmware/MotorControl/thermistor.cpp b/Firmware/MotorControl/thermistor.cpp
index 617dee75..df3b1d0d 100644
--- a/Firmware/MotorControl/thermistor.cpp
+++ b/Firmware/MotorControl/thermistor.cpp
@@ -22,7 +22,7 @@ ThermistorCurrentLimiter::ThermistorCurrentLimiter(uint16_t adc_channel,
 void ThermistorCurrentLimiter::update() {
     const float voltage = get_adc_voltage_channel(adc_channel_);
     const float normalized_voltage = voltage / adc_ref_voltage;
-    temperature_ = horner_fma(normalized_voltage, coefficients_, num_coeffs_);
+    temperature_ = horner_poly_eval(normalized_voltage, coefficients_, num_coeffs_);
 }
 
 bool ThermistorCurrentLimiter::do_checks() {
diff --git a/Firmware/MotorControl/utils.cpp b/Firmware/MotorControl/utils.cpp
index 8c041d55..dad400fb 100644
--- a/Firmware/MotorControl/utils.cpp
+++ b/Firmware/MotorControl/utils.cpp
@@ -3,6 +3,150 @@
 #include <board.h>
 
 
+// Compute rising edge timings (0.0 - 1.0) as a function of alpha-beta
+// as per the magnitude invariant clarke transform
+// The magnitude of the alpha-beta vector may not be larger than sqrt(3)/2
+// Returns true on success, and false if the input was out of range
+std::tuple<float, float, float, bool> SVM(float alpha, float beta) {
+    float tA, tB, tC;
+    int Sextant;
+
+    if (beta >= 0.0f) {
+        if (alpha >= 0.0f) {
+            //quadrant I
+            if (one_by_sqrt3 * beta > alpha)
+                Sextant = 2; //sextant v2-v3
+            else
+                Sextant = 1; //sextant v1-v2
+
+        } else {
+            //quadrant II
+            if (-one_by_sqrt3 * beta > alpha)
+                Sextant = 3; //sextant v3-v4
+            else
+                Sextant = 2; //sextant v2-v3
+        }
+    } else {
+        if (alpha >= 0.0f) {
+            //quadrant IV
+            if (-one_by_sqrt3 * beta > alpha)
+                Sextant = 5; //sextant v5-v6
+            else
+                Sextant = 6; //sextant v6-v1
+        } else {
+            //quadrant III
+            if (one_by_sqrt3 * beta > alpha)
+                Sextant = 4; //sextant v4-v5
+            else
+                Sextant = 5; //sextant v5-v6
+        }
+    }
+
+    switch (Sextant) {
+        // sextant v1-v2
+        case 1: {
+            // Vector on-times
+            float t1 = alpha - one_by_sqrt3 * beta;
+            float t2 = two_by_sqrt3 * beta;
+
+            // PWM timings
+            tA = (1.0f - t1 - t2) * 0.5f;
+            tB = tA + t1;
+            tC = tB + t2;
+        } break;
+
+        // sextant v2-v3
+        case 2: {
+            // Vector on-times
+            float t2 = alpha + one_by_sqrt3 * beta;
+            float t3 = -alpha + one_by_sqrt3 * beta;
+
+            // PWM timings
+            tB = (1.0f - t2 - t3) * 0.5f;
+            tA = tB + t3;
+            tC = tA + t2;
+        } break;
+
+        // sextant v3-v4
+        case 3: {
+            // Vector on-times
+            float t3 = two_by_sqrt3 * beta;
+            float t4 = -alpha - one_by_sqrt3 * beta;
+
+            // PWM timings
+            tB = (1.0f - t3 - t4) * 0.5f;
+            tC = tB + t3;
+            tA = tC + t4;
+        } break;
+
+        // sextant v4-v5
+        case 4: {
+            // Vector on-times
+            float t4 = -alpha + one_by_sqrt3 * beta;
+            float t5 = -two_by_sqrt3 * beta;
+
+            // PWM timings
+            tC = (1.0f - t4 - t5) * 0.5f;
+            tB = tC + t5;
+            tA = tB + t4;
+        } break;
+
+        // sextant v5-v6
+        case 5: {
+            // Vector on-times
+            float t5 = -alpha - one_by_sqrt3 * beta;
+            float t6 = alpha - one_by_sqrt3 * beta;
+
+            // PWM timings
+            tC = (1.0f - t5 - t6) * 0.5f;
+            tA = tC + t5;
+            tB = tA + t6;
+        } break;
+
+        // sextant v6-v1
+        case 6: {
+            // Vector on-times
+            float t6 = -two_by_sqrt3 * beta;
+            float t1 = alpha + one_by_sqrt3 * beta;
+
+            // PWM timings
+            tA = (1.0f - t6 - t1) * 0.5f;
+            tC = tA + t1;
+            tB = tC + t6;
+        } break;
+    }
+
+    int result_valid =
+            tA >= 0.0f && tA <= 1.0f
+         && tB >= 0.0f && tB <= 1.0f
+         && tC >= 0.0f && tC <= 1.0f;
+    return {tA, tB, tC, result_valid};
+}
+
+// based on https://math.stackexchange.com/a/1105038/81278
+float fast_atan2(float y, float x) {
+    // a := min (|x|, |y|) / max (|x|, |y|)
+    float abs_y = std::abs(y);
+    float abs_x = std::abs(x);
+    // inject FLT_MIN in denominator to avoid division by zero
+    float a = std::min(abs_x, abs_y) / (std::max(abs_x, abs_y) + std::numeric_limits<float>::min());
+    // s := a * a
+    float s = a * a;
+    // r := ((-0.0464964749 * s + 0.15931422) * s - 0.327622764) * s * a + a
+    float r = ((-0.0464964749f * s + 0.15931422f) * s - 0.327622764f) * s * a + a;
+    // if |y| > |x| then r := 1.57079637 - r
+    if (abs_y > abs_x)
+        r = 1.57079637f - r;
+    // if x < 0 then r := 3.14159274 - r
+    if (x < 0.0f)
+        r = 3.14159274f - r;
+    // if y < 0 then r := -r
+    if (y < 0.0f)
+        r = -r;
+
+    return r;
+}
+
 // @brief: Returns how much time is left until the deadline is reached.
 // If the deadline has already passed, the return value is 0 (except if
 // the deadline is very far in the past)
@@ -41,6 +185,6 @@ void delay_us(uint32_t us)
 {
     uint32_t start = micros();
     while (micros() - start < (uint32_t) us) {
-        __ASM("nop");
+        asm volatile ("nop");
     }
 }
diff --git a/Firmware/MotorControl/utils.hpp b/Firmware/MotorControl/utils.hpp
index 251e2cbc..8eb044f9 100644
--- a/Firmware/MotorControl/utils.hpp
+++ b/Firmware/MotorControl/utils.hpp
@@ -62,6 +62,23 @@ constexpr float one_by_sqrt3 = 0.57735026919f;
 constexpr float two_by_sqrt3 = 1.15470053838f;
 constexpr float sqrt3_by_2 = 0.86602540378f;
 
+// Function prototypes for implementations in utils.cpp
+std::tuple<float, float, float, bool> SVM(float alpha, float beta);
+float fast_atan2(float y, float x);
+uint32_t deadline_to_timeout(uint32_t deadline_ms);
+uint32_t timeout_to_deadline(uint32_t timeout_ms);
+int is_in_the_future(uint32_t time_ms);
+uint32_t micros(void);
+void delay_us(uint32_t us);
+
+extern "C" {
+float our_arm_sin_f32(float x);
+float our_arm_cos_f32(float x);
+}
+
+// ----------------
+// Inline functions
+
 template<typename T>
 constexpr T SQ(const T& x){
     return x * x;
@@ -80,7 +97,8 @@ std::array<T, 1 + sizeof...(Tail)> make_array(T head, Tail... tail) {
 
 // To allow use of -ffast-math we need to have a special check for nan
 // that bypasses the "ignore nan" flag
-__attribute__((optimize("-fno-finite-math-only"))) static inline bool is_nan(float x) {
+__attribute__((optimize("-fno-finite-math-only")))
+inline bool is_nan(float x) {
     return __builtin_isnan(x);
 }
 
@@ -90,8 +108,8 @@ inline int round_int(float x) {
 #ifdef __arm__
     int res;
     asm("vcvtr.s32.f32   %[res], %[x]"
-        : [ res ] "=X"(res)
-        : [ x ] "w"(x));
+        : [res] "=X" (res)
+        : [x] "w" (x) );
     return res;
 #else
     return (int)nearbyint(x);
@@ -121,155 +139,10 @@ inline float wrap_pm_pi(float x) {
     return wrap_pm(x, 2 * M_PI);
 }
 
-// Compute rising edge timings (0.0 - 1.0) as a function of alpha-beta
-// as per the magnitude invariant clarke transform
-// The magnitude of the alpha-beta vector may not be larger than sqrt(3)/2
-// Returns true on success, and false if the input was out of range
-inline auto SVM(float alpha, float beta) {
-    float tA, tB, tC;
-    int Sextant;
-
-    if (beta >= 0.0f) {
-        if (alpha >= 0.0f) {
-            //quadrant I
-            if (one_by_sqrt3 * beta > alpha)
-                Sextant = 2; //sextant v2-v3
-            else
-                Sextant = 1; //sextant v1-v2
-
-        } else {
-            //quadrant II
-            if (-one_by_sqrt3 * beta > alpha)
-                Sextant = 3; //sextant v3-v4
-            else
-                Sextant = 2; //sextant v2-v3
-        }
-    } else {
-        if (alpha >= 0.0f) {
-            //quadrant IV
-            if (-one_by_sqrt3 * beta > alpha)
-                Sextant = 5; //sextant v5-v6
-            else
-                Sextant = 6; //sextant v6-v1
-        } else {
-            //quadrant III
-            if (one_by_sqrt3 * beta > alpha)
-                Sextant = 4; //sextant v4-v5
-            else
-                Sextant = 5; //sextant v5-v6
-        }
-    }
-
-    switch (Sextant) {
-        // sextant v1-v2
-        case 1: {
-            // Vector on-times
-            float t1 = alpha - one_by_sqrt3 * beta;
-            float t2 = two_by_sqrt3 * beta;
-
-            // PWM timings
-            tA = (1.0f - t1 - t2) * 0.5f;
-            tB = tA + t1;
-            tC = tB + t2;
-        } break;
-
-        // sextant v2-v3
-        case 2: {
-            // Vector on-times
-            float t2 = alpha + one_by_sqrt3 * beta;
-            float t3 = -alpha + one_by_sqrt3 * beta;
-
-            // PWM timings
-            tB = (1.0f - t2 - t3) * 0.5f;
-            tA = tB + t3;
-            tC = tA + t2;
-        } break;
-
-        // sextant v3-v4
-        case 3: {
-            // Vector on-times
-            float t3 = two_by_sqrt3 * beta;
-            float t4 = -alpha - one_by_sqrt3 * beta;
-
-            // PWM timings
-            tB = (1.0f - t3 - t4) * 0.5f;
-            tC = tB + t3;
-            tA = tC + t4;
-        } break;
-
-        // sextant v4-v5
-        case 4: {
-            // Vector on-times
-            float t4 = -alpha + one_by_sqrt3 * beta;
-            float t5 = -two_by_sqrt3 * beta;
-
-            // PWM timings
-            tC = (1.0f - t4 - t5) * 0.5f;
-            tB = tC + t5;
-            tA = tB + t4;
-        } break;
-
-        // sextant v5-v6
-        case 5: {
-            // Vector on-times
-            float t5 = -alpha - one_by_sqrt3 * beta;
-            float t6 = alpha - one_by_sqrt3 * beta;
-
-            // PWM timings
-            tC = (1.0f - t5 - t6) * 0.5f;
-            tA = tC + t5;
-            tB = tA + t6;
-        } break;
-
-        // sextant v6-v1
-        case 6: {
-            // Vector on-times
-            float t6 = -two_by_sqrt3 * beta;
-            float t1 = alpha + one_by_sqrt3 * beta;
-
-            // PWM timings
-            tA = (1.0f - t6 - t1) * 0.5f;
-            tC = tA + t1;
-            tB = tC + t6;
-        } break;
-    }
-
-    int result_valid =
-            tA >= 0.0f && tA <= 1.0f
-         && tB >= 0.0f && tB <= 1.0f
-         && tC >= 0.0f && tC <= 1.0f;
-    return std::make_tuple(tA, tB, tC, result_valid);
-}
-
-
-// based on https://math.stackexchange.com/a/1105038/81278
-inline float fast_atan2(const float y, const float x) {
-    // a := min (|x|, |y|) / max (|x|, |y|)
-    float abs_y = std::abs(y);
-    float abs_x = std::abs(x);
-    // inject FLT_MIN in denominator to avoid division by zero
-    float a = std::min(abs_x, abs_y) / (std::max(abs_x, abs_y) + std::numeric_limits<float>::min());
-    // s := a * a
-    float s = a * a;
-    // r := ((-0.0464964749 * s + 0.15931422) * s - 0.327622764) * s * a + a
-    float r = ((-0.0464964749f * s + 0.15931422f) * s - 0.327622764f) * s * a + a;
-    // if |y| > |x| then r := 1.57079637 - r
-    if (abs_y > abs_x)
-        r = 1.57079637f - r;
-    // if x < 0 then r := 3.14159274 - r
-    if (x < 0.0f)
-        r = 3.14159274f - r;
-    // if y < 0 then r := -r
-    if (y < 0.0f)
-        r = -r;
-
-    return r;
-}
-
-// Evaluate polynomials using Fused Multiply Add intrisic instruction.
+// Evaluate polynomials in an efficient way
 // coeffs[0] is highest order, as per numpy.polyfit
 // p(x) = coeffs[0] * x^deg + ... + coeffs[deg], for some degree "deg"
-inline float horner_fma(float x, const float *coeffs, size_t count) {
+inline float horner_poly_eval(float x, const float *coeffs, size_t count) {
     float result = 0.0f;
     for (size_t idx = 0; idx < count; ++idx)
         result = (result * x) + coeffs[idx];
@@ -279,17 +152,6 @@ inline float horner_fma(float x, const float *coeffs, size_t count) {
 // Modulo (as opposed to remainder), per https://stackoverflow.com/a/19288271
 inline int mod(const int dividend, const int divisor){
     int r = dividend % divisor;
-    return (r < 0) ? (r + divisor) : r;
+    if (r < 0) r += divisor;
+    return r;
 }
-
-uint32_t deadline_to_timeout(uint32_t deadline_ms);
-uint32_t timeout_to_deadline(uint32_t timeout_ms);
-int is_in_the_future(uint32_t time_ms);
-
-uint32_t micros(void);
-void delay_us(uint32_t us);
-
-extern "C" {
-float our_arm_sin_f32(float x);
-float our_arm_cos_f32(float x);
-}
\ No newline at end of file
diff --git a/Firmware/communication/interface_uart.cpp b/Firmware/communication/interface_uart.cpp
index 74fa02c0..6450eb50 100644
--- a/Firmware/communication/interface_uart.cpp
+++ b/Firmware/communication/interface_uart.cpp
@@ -18,9 +18,6 @@
 static uint8_t dma_rx_buffer[UART_RX_BUFFER_SIZE];
 static uint32_t dma_last_rcv_idx;
 
-// FIXME: the stdlib doesn't know about CMSIS threads, so this is just a global variable
-// static thread_local uint32_t deadline_ms = 0;
-
 osThreadId uart_thread = 0;
 extern UART_HandleTypeDef* uart0;
 static UART_HandleTypeDef* huart_ = uart0; // defined in board.cpp.
@@ -35,7 +32,6 @@ public:
             size_t chunk = length < UART_TX_BUFFER_SIZE ? length : UART_TX_BUFFER_SIZE;
             // wait for USB interface to become ready
             // TODO: implement ring buffer to get a more continuous stream of data
-            // if (osSemaphoreWait(sem_uart_dma, deadline_to_timeout(deadline_ms)) != osOK)
             if (osSemaphoreWait(sem_uart_dma, PROTOCOL_SERVER_TIMEOUT_MS) != osOK)
                 return -1;
             // transmit chunk
@@ -76,7 +72,6 @@ static void uart_server_thread(void * ctx) {
             continue;
         }
 
-        // deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS);
         // Process bytes in one or two chunks (two in case there was a wrap)
         if (new_rcv_idx < dma_last_rcv_idx) {
             uart_stream_input.process_bytes(dma_rx_buffer + dma_last_rcv_idx,

From 33987df7e1335f2deada3eb39d9c42f529fd40c1 Mon Sep 17 00:00:00 2001
From: Oskar Weigl <oskar.weigl@gmail.com>
Date: Sat, 5 Sep 2020 16:15:11 -0700
Subject: [PATCH 16/19] Avoid redundant normalizing of adc voltage

---
 Firmware/MotorControl/encoder.cpp    |  4 ++--
 Firmware/MotorControl/low_level.cpp  | 13 ++++++++-----
 Firmware/MotorControl/low_level.h    |  9 ++++++---
 Firmware/MotorControl/thermistor.cpp |  3 +--
 4 files changed, 17 insertions(+), 12 deletions(-)

diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp
index f0c65445..4fe75689 100644
--- a/Firmware/MotorControl/encoder.cpp
+++ b/Firmware/MotorControl/encoder.cpp
@@ -330,8 +330,8 @@ void Encoder::sample_now() {
         } break;
 
         case MODE_SINCOS: {
-            sincos_sample_s_ = (get_adc_voltage(get_gpio(config_.sincos_gpio_pin_sin)) / 3.3f) - 0.5f;
-            sincos_sample_c_ = (get_adc_voltage(get_gpio(config_.sincos_gpio_pin_cos)) / 3.3f) - 0.5f;
+            sincos_sample_s_ = get_adc_relative_voltage(get_gpio(config_.sincos_gpio_pin_sin)) - 0.5f;
+            sincos_sample_c_ = get_adc_relative_voltage(get_gpio(config_.sincos_gpio_pin_cos)) - 0.5f;
         } break;
 
         case MODE_SPI_ABS_AMS:
diff --git a/Firmware/MotorControl/low_level.cpp b/Firmware/MotorControl/low_level.cpp
index 9a6f9ca2..abd5fc09 100644
--- a/Firmware/MotorControl/low_level.cpp
+++ b/Firmware/MotorControl/low_level.cpp
@@ -311,8 +311,12 @@ void start_general_purpose_adc() {
 // The true frequency is slightly lower because of the injected vbus
 // measurements
 float get_adc_voltage(Stm32Gpio gpio) {
+    return get_adc_relative_voltage(gpio) * adc_ref_voltage;
+}
+
+float get_adc_relative_voltage(Stm32Gpio gpio) {
     const uint16_t channel = channel_from_gpio(gpio);
-    return get_adc_voltage_channel(channel);
+    return get_adc_relative_voltage_ch(channel);
 }
 
 // @brief Given a GPIO_port and pin return the associated adc_channel.
@@ -358,12 +362,11 @@ uint16_t channel_from_gpio(Stm32Gpio gpio) {
     return channel;
 }
 
-// @brief Given an adc channel return the measured voltage.
+// @brief Given an adc channel return the voltage as a ratio of adc_ref_voltage
 // returns -1.0f if the channel is not valid.
-float get_adc_voltage_channel(uint16_t channel)
-{
+float get_adc_relative_voltage_ch(uint16_t channel) {
     if (channel < ADC_CHANNEL_COUNT)
-        return ((float)adc_measurements_[channel]) * (adc_ref_voltage / adc_full_scale);
+        return (float)adc_measurements_[channel] / adc_full_scale;
     else
         return -1.0f;
 }
diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h
index e02ef5c2..fdae02a1 100644
--- a/Firmware/MotorControl/low_level.h
+++ b/Firmware/MotorControl/low_level.h
@@ -46,12 +46,15 @@ void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b,
                  uint16_t TIM_CLOCKSOURCE_ITRx, uint16_t count_offset,
                  TIM_HandleTypeDef* htim_refbase = nullptr);
 void start_general_purpose_adc();
-float get_adc_voltage(Stm32Gpio gpio);
-uint16_t channel_from_gpio(Stm32Gpio gpio);
-float get_adc_voltage_channel(uint16_t channel);
 void pwm_in_init();
 void start_analog_thread();
 
+// ADC getters
+uint16_t channel_from_gpio(Stm32Gpio gpio);
+float get_adc_voltage(Stm32Gpio gpio);
+float get_adc_relative_voltage(Stm32Gpio gpio);
+float get_adc_relative_voltage_ch(uint16_t channel);
+
 void update_brake_current();
 
 #ifdef __cplusplus
diff --git a/Firmware/MotorControl/thermistor.cpp b/Firmware/MotorControl/thermistor.cpp
index df3b1d0d..4d16ae61 100644
--- a/Firmware/MotorControl/thermistor.cpp
+++ b/Firmware/MotorControl/thermistor.cpp
@@ -20,8 +20,7 @@ ThermistorCurrentLimiter::ThermistorCurrentLimiter(uint16_t adc_channel,
 }
 
 void ThermistorCurrentLimiter::update() {
-    const float voltage = get_adc_voltage_channel(adc_channel_);
-    const float normalized_voltage = voltage / adc_ref_voltage;
+    const float normalized_voltage = get_adc_relative_voltage_ch(adc_channel_);
     temperature_ = horner_poly_eval(normalized_voltage, coefficients_, num_coeffs_);
 }
 

From 1e48a2b0e06d89f69a7a6af7573f8a270e0e78fd Mon Sep 17 00:00:00 2001
From: Oskar Weigl <oskar.weigl@gmail.com>
Date: Sat, 5 Sep 2020 16:40:59 -0700
Subject: [PATCH 17/19] fix result valid type

---
 Firmware/MotorControl/utils.cpp | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/Firmware/MotorControl/utils.cpp b/Firmware/MotorControl/utils.cpp
index dad400fb..cb1866db 100644
--- a/Firmware/MotorControl/utils.cpp
+++ b/Firmware/MotorControl/utils.cpp
@@ -18,7 +18,6 @@ std::tuple<float, float, float, bool> SVM(float alpha, float beta) {
                 Sextant = 2; //sextant v2-v3
             else
                 Sextant = 1; //sextant v1-v2
-
         } else {
             //quadrant II
             if (-one_by_sqrt3 * beta > alpha)
@@ -116,7 +115,7 @@ std::tuple<float, float, float, bool> SVM(float alpha, float beta) {
         } break;
     }
 
-    int result_valid =
+    bool result_valid =
             tA >= 0.0f && tA <= 1.0f
          && tB >= 0.0f && tB <= 1.0f
          && tC >= 0.0f && tC <= 1.0f;

From 062c3978474b2d46ca9f210913c20feebc1c3912 Mon Sep 17 00:00:00 2001
From: Samuel Sadok <samuel.sadok@bluewin.ch>
Date: Mon, 7 Sep 2020 20:54:12 +0200
Subject: [PATCH 18/19] implement CI check against std::isnan

---
 .github/workflows/compile.yaml | 19 ++++++++++++++++---
 1 file changed, 16 insertions(+), 3 deletions(-)

diff --git a/.github/workflows/compile.yaml b/.github/workflows/compile.yaml
index a433132f..f8468433 100644
--- a/.github/workflows/compile.yaml
+++ b/.github/workflows/compile.yaml
@@ -108,9 +108,22 @@ jobs:
           mv tup_build.sh tup_build.bat # in reality this is a .bat script on windows
           .\tup_build.bat
 
-  #code-checks:
-  #  runs-on: ubuntu-latest
-  #  steps:
+  code-checks:
+    strategy:
+      fail-fast: false
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v2
+
+      - name: Check that we're not using std::isnan
+        run: |
+          cd ${{ github.workspace }}/Firmware
+
+          if grep 'std::isnan' -R .; then
+            echo "Don't use std::isnan because it's not compatible with '-ffast-math'. Use is_nan() instead."
+            return 1;
+          fi
+
   #    TODO:
   #     - check if enums.py is consistent with yaml
   #     - clang-format check

From 074b017ab260d47fba20f1fb8d14083f1c59f181 Mon Sep 17 00:00:00 2001
From: PAJohnson <pajohns4@ncsu.edu>
Date: Sat, 26 Sep 2020 23:17:30 -0400
Subject: [PATCH 19/19] Update top level gitignore for gui artifacts

---
 .gitignore | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/.gitignore b/.gitignore
index 66a8990f..c94fc787 100644
--- a/.gitignore
+++ b/.gitignore
@@ -61,3 +61,8 @@ ODrive\.files
 ODrive\.includes
 
 Firmware/Tests/bin/
+
+# GUI
+GUI/dist_electron
+GUI/node_modules
+GUI/build
\ No newline at end of file