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 +// 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) { + 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::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 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 constexpr T SQ(const T& x){ return x * x; @@ -80,7 +97,8 @@ std::array 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::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,