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 #include -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 make_array(T head, Tail... tail) return std::array({ 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)