drop flag fno-finite-math-only, and implement explicit nan check function

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_);
 

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:

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;
     }
 

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

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);

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)