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 diff --git a/Firmware/.vscode/c_cpp_properties.json b/Firmware/.vscode/c_cpp_properties.json index 1d700720..82c77a74 100644 --- a/Firmware/.vscode/c_cpp_properties.json +++ b/Firmware/.vscode/c_cpp_properties.json @@ -6,12 +6,13 @@ "${workspaceFolder}/**" ], "defines": [ + "__arm__", "STM32F405xx", + "FPU_FPV4", "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__" @@ -37,7 +38,9 @@ "${workspaceFolder}/**" ], "defines": [ + "__arm__", "STM32F405xx", + "FPU_FPV4", "USE_HAL_DRIVER", "HW_VERSION_MAJOR=3", "HW_VERSION_MINOR=6", @@ -57,11 +60,13 @@ "${workspaceFolder}/**" ], "defines": [ + "__arm__", "STM32F405xx", + "FPU_FPV4", "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/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 +#include #include #include diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index 1524dce6..88906cf5 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); @@ -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/encoder.cpp b/Firmware/MotorControl/encoder.cpp index d2ce992b..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: @@ -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/low_level.cpp b/Firmware/MotorControl/low_level.cpp index 3a611971..55b5b887 100644 --- a/Firmware/MotorControl/low_level.cpp +++ b/Firmware/MotorControl/low_level.cpp @@ -3,7 +3,7 @@ #include #include -#include +#include #include #include @@ -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 @@ -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,14 +362,13 @@ uint16_t channel_from_gpio(Stm32Gpio gpio) { return channel; } -// @brief Given an adc channel return the measured voltage. -// returns NaN if the channel is not valid. -float get_adc_voltage_channel(uint16_t channel) -{ +// @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_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 0.0f / 0.0f; // NaN + return -1.0f; } //-------------------------------- @@ -494,10 +497,10 @@ 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)) { + 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 +513,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/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/motor.cpp b/Firmware/MotorControl/motor.cpp index 355067f5..95526d1d 100644 --- a/Firmware/MotorControl/motor.cpp +++ b/Firmware/MotorControl/motor.cpp @@ -261,10 +261,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) + 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); @@ -350,7 +350,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; @@ -417,7 +417,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; @@ -435,7 +435,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,9 +446,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 = fabsf(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..4d16ae61 100644 --- a/Firmware/MotorControl/thermistor.cpp +++ b/Firmware/MotorControl/thermistor.cpp @@ -20,9 +20,8 @@ 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_); + const float normalized_voltage = get_adc_relative_voltage_ch(adc_channel_); + temperature_ = horner_poly_eval(normalized_voltage, coefficients_, num_coeffs_); } bool ThermistorCurrentLimiter::do_checks() { @@ -42,7 +41,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/trapTraj.cpp b/Firmware/MotorControl/trapTraj.cpp index b3afd946..b4f94a37 100644 --- a/Firmware/MotorControl/trapTraj.cpp +++ b/Firmware/MotorControl/trapTraj.cpp @@ -1,4 +1,4 @@ -#include +#include #include "odrive_main.h" #include "utils.hpp" @@ -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 * 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/MotorControl/utils.cpp b/Firmware/MotorControl/utils.cpp index f4dc212d..cb1866db 100644 --- a/Firmware/MotorControl/utils.cpp +++ b/Firmware/MotorControl/utils.cpp @@ -1,11 +1,14 @@ #include -#include -#include -#include #include -int SVM(float alpha, float beta, float* tA, float* tB, float* tC) { + +// 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 SVM(float alpha, float beta) { + float tA, tB, tC; int Sextant; if (beta >= 0.0f) { @@ -15,7 +18,6 @@ int SVM(float alpha, float beta, float* tA, float* tB, float* tC) { Sextant = 2; //sextant v2-v3 else Sextant = 1; //sextant v1-v2 - } else { //quadrant II if (-one_by_sqrt3 * beta > alpha) @@ -47,9 +49,9 @@ int 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 @@ -59,9 +61,9 @@ int 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 @@ -71,9 +73,9 @@ int 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 @@ -83,9 +85,9 @@ int 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 @@ -95,9 +97,9 @@ int 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 @@ -107,27 +109,26 @@ int 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; } - // 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; + bool 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 = 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); + float a = std::min(abs_x, abs_y) / (std::max(abs_x, abs_y) + std::numeric_limits::min()); // s := a * a float s = a * a; // r := ((-0.0464964749 * s + 0.15931422) * s - 0.327622764) * s * a + a @@ -145,22 +146,6 @@ float fast_atan2(float y, float x) { 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 = 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; - 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 // the deadline is very far in the past) @@ -199,6 +184,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 49f9434d..8eb044f9 100644 --- a/Firmware/MotorControl/utils.hpp +++ b/Firmware/MotorControl/utils.hpp @@ -1,9 +1,10 @@ - -#ifndef __UTILS_H -#define __UTILS_H +#pragma once #include -#include +#include +#include +#include +#include /** * @brief Flash size register address @@ -54,16 +55,34 @@ #ifdef M_PI #undef M_PI #endif -#define M_PI (3.14159265358979323846f) -#define MACRO_MAX(x, y) (((x) > (y)) ? (x) : (y)) -#define MACRO_MIN(x, y) (((x) < (y)) ? (x) : (y)) +// 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; -#define SQ(x) ((x) * (x)) +// Function prototypes for implementations in utils.cpp +std::tuple 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); -#ifdef __cplusplus +extern "C" { +float our_arm_sin_f32(float x); +float our_arm_cos_f32(float x); +} -#include +// ---------------- +// Inline functions + +template +constexpr T SQ(const T& x){ + return x * x; +} /** * @brief Small helper to make array with known size @@ -71,62 +90,68 @@ * has to match exactly. Whereas initializer lists allow * less arguments. */ -template -std::array make_array(T head, Tail... tail) -{ - return std::array({ head, tail ... }); +template +std::array make_array(T head, Tail... tail) { + return std::array({head, tail...}); } -extern "C" { +// 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"))) +inline bool is_nan(float x) { + return __builtin_isnan(x); +} + +// Round to integer +// Default rounding mode: round to nearest +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)nearbyint(x); #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; - -// like fmodf, but always positive -static inline float fmodf_pos(float x, float y) { - float out = fmodf(x, y); - if (out < 0.0f) - out += y; - return out; } -/** - * @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); -} - -// 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); - -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); -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); - -float our_arm_sin_f32(float x); -float our_arm_cos_f32(float x); - -#ifdef __cplusplus -} +// Wrap value to range. +// With default rounding mode (round to nearest), +// the result will be in range -y/2 to y/2 +inline float wrap_pm(float x, float y) { +#ifdef FPU_FPV4 + float intval = (float)round_int(x / y); +#else + float intval = nearbyint(x / y); #endif + return x - intval * y; +} -#endif //__UTILS_H +// Same as fmodf but result is positive and y must be positive +inline float fmodf_pos(float x, float y) { + float res = wrap_pm(x, y); + if (res < 0) res += y; + return res; +} + +inline float wrap_pm_pi(float x) { + return wrap_pm(x, 2 * M_PI); +} + +// 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_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]; + 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; + if (r < 0) r += divisor; + return r; +} diff --git a/Firmware/Tests/test_trap_traj.cpp b/Firmware/Tests/test_trap_traj.cpp index b5d30014..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::fmax((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; diff --git a/Firmware/Tupfile.lua b/Firmware/Tupfile.lua index 62a8acb8..8677fb1b 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'} } @@ -155,7 +155,7 @@ if tup.getconfig("USE_LTO") == "true" then 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) 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,