From 9e1e054f0a741efd7282ad1e981e07fd9b36a8f0 Mon Sep 17 00:00:00 2001 From: Unknown Date: Sat, 5 Sep 2020 00:30:12 -0400 Subject: [PATCH] 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 -#include -#include -#include #include -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 -#include +#include +#include /** * @brief Flash size register address @@ -61,8 +60,6 @@ #define SQ(x) ((x) * (x)) -#ifdef __cplusplus - #include /** @@ -71,32 +68,30 @@ * 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...}); } // 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::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