From ece29213166581d6018106405dda55a9270db898 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Thu, 27 Sep 2018 21:18:38 -0700 Subject: [PATCH 1/2] add and rename our own copy of arm_sin_f32 and arm_cos_f32 --- Firmware/MotorControl/arm_cos_f32.c | 116 ++++++++++++++++++++++++++ Firmware/MotorControl/arm_sin_f32.c | 123 ++++++++++++++++++++++++++++ Firmware/MotorControl/encoder.cpp | 12 +-- Firmware/MotorControl/motor.cpp | 8 +- Firmware/MotorControl/utils.h | 5 +- Firmware/Tupfile.lua | 3 + ODrive_Workspace.code-workspace | 3 +- 7 files changed, 257 insertions(+), 13 deletions(-) create mode 100644 Firmware/MotorControl/arm_cos_f32.c create mode 100644 Firmware/MotorControl/arm_sin_f32.c diff --git a/Firmware/MotorControl/arm_cos_f32.c b/Firmware/MotorControl/arm_cos_f32.c new file mode 100644 index 00000000..559153e8 --- /dev/null +++ b/Firmware/MotorControl/arm_cos_f32.c @@ -0,0 +1,116 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_cos_f32.c + * Description: Fast cosine calculation for floating-point values + * + * $Date: 27. January 2017 + * $Revision: V.1.5.1 + * + * Target Processor: Cortex-M cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * + * SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the License); you may + * not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an AS IS BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#include // Sets up the correct chip specifc defines required by arm_math +#define ARM_MATH_CM4 // TODO: might change in future board versions +#include "arm_math.h" +#include "arm_common_tables.h" +/** + * @ingroup groupFastMath + */ + +/** + * @defgroup cos Cosine + * + * Computes the trigonometric cosine function using a combination of table lookup + * and linear interpolation. There are separate functions for + * Q15, Q31, and floating-point data types. + * The input to the floating-point version is in radians and in the range [0 2*pi) while the + * fixed-point Q15 and Q31 have a scaled input with the range + * [0 +0.9999] mapping to [0 2*pi). The fixed-point range is chosen so that a + * value of 2*pi wraps around to 0. + * + * The implementation is based on table lookup using 256 values together with linear interpolation. + * The steps used are: + * -# Calculation of the nearest integer table index + * -# Compute the fractional portion (fract) of the table index. + * -# The final result equals (1.0f-fract)*a + fract*b; + * + * where + * 
+ *    b=Table[index+0];
+ *    c=Table[index+1];
+ *
+ */ + + /** + * @addtogroup cos + * @{ + */ + +/** + * @brief Fast approximation to the trigonometric cosine function for floating-point data. + * @param[in] x input value in radians. + * @return cos(x). + */ + +float32_t our_arm_cos_f32( + float32_t x) +{ + float32_t cosVal, fract, in; /* Temporary variables for input, output */ + uint16_t index; /* Index variable */ + float32_t a, b; /* Two nearest output values */ + int32_t n; + float32_t findex; + + /* input x is in radians */ + /* Scale the input to [0 1] range from [0 2*PI] , divide input by 2*pi, add 0.25 (pi/2) to read sine table */ + in = x * 0.159154943092f + 0.25f; + + /* Calculation of floor value of input */ + n = (int32_t) in; + + /* Make negative values towards -infinity */ + if (in < 0.0f) + { + n--; + } + + /* Map input value to [0 1] */ + in = in - (float32_t) n; + + /* Calculation of index of the table */ + findex = (float32_t) FAST_MATH_TABLE_SIZE * in; + index = ((uint16_t)findex) & 0x1ff; + + /* fractional value calculation */ + fract = findex - (float32_t) index; + + /* Read two nearest values of input value from the cos table */ + a = sinTable_f32[index]; + b = sinTable_f32[index+1]; + + /* Linear interpolation process */ + cosVal = (1.0f-fract)*a + fract*b; + + /* Return the output value */ + return (cosVal); +} + +/** + * @} end of cos group + */ diff --git a/Firmware/MotorControl/arm_sin_f32.c b/Firmware/MotorControl/arm_sin_f32.c new file mode 100644 index 00000000..33d436e6 --- /dev/null +++ b/Firmware/MotorControl/arm_sin_f32.c @@ -0,0 +1,123 @@ +/* ---------------------------------------------------------------------- + * Project: CMSIS DSP Library + * Title: arm_sin_f32.c + * Description: Fast sine calculation for floating-point values + * + * $Date: 27. January 2017 + * $Revision: V.1.5.1 + * + * Target Processor: Cortex-M cores + * -------------------------------------------------------------------- */ +/* + * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. + * + * SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the License); you may + * not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an AS IS BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include // Sets up the correct chip specifc defines required by arm_math +#define ARM_MATH_CM4 // TODO: might change in future board versions +#include "arm_math.h" +#include "arm_common_tables.h" + +/** + * @ingroup groupFastMath + */ + +/** + * @defgroup sin Sine + * + * Computes the trigonometric sine function using a combination of table lookup + * and linear interpolation. There are separate functions for + * Q15, Q31, and floating-point data types. + * The input to the floating-point version is in radians and in the range [0 2*pi) while the + * fixed-point Q15 and Q31 have a scaled input with the range + * [0 +0.9999] mapping to [0 2*pi). The fixed-point range is chosen so that a + * value of 2*pi wraps around to 0. + * + * The implementation is based on table lookup using 256 values together with linear interpolation. + * The steps used are: + * -# Calculation of the nearest integer table index + * -# Compute the fractional portion (fract) of the table index. + * -# The final result equals (1.0f-fract)*a + fract*b; + * + * where + * 
+ *    b=Table[index+0];
+ *    c=Table[index+1];
+ *
+ */ + +/** + * @addtogroup sin + * @{ + */ + +/** + * @brief Fast approximation to the trigonometric sine function for floating-point data. + * @param[in] x input value in radians. + * @return sin(x). + */ + +float32_t our_arm_sin_f32( + float32_t x) +{ + float32_t sinVal, fract, in; /* Temporary variables for input, output */ + uint16_t index; /* Index variable */ + float32_t a, b; /* Two nearest output values */ + int32_t n; + float32_t findex; + + /* Special case for small negative inputs */ + if ((x < 0.0f) && (x >= -1.9e-7f)) { + return x; + } + + /* input x is in radians */ + /* Scale the input to [0 1] range from [0 2*PI] , divide input by 2*pi */ + in = x * 0.159154943092f; + + /* Calculation of floor value of input */ + n = (int32_t) in; + + /* Make negative values towards -infinity */ + if (x < 0.0f) + { + n--; + } + + /* Map input value to [0 1] */ + in = in - (float32_t) n; + + /* Calculation of index of the table */ + findex = (float32_t) FAST_MATH_TABLE_SIZE * in; + index = ((uint16_t)findex) & 0x1ff; + + /* fractional value calculation */ + fract = findex - (float32_t) index; + + /* Read two nearest values of input value from the sin table */ + a = sinTable_f32[index]; + b = sinTable_f32[index+1]; + + /* Linear interpolation process */ + sinVal = (1.0f-fract)*a + fract*b; + + /* Return the output value */ + return (sinVal); +} + +/** + * @} end of sin group + */ diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index 5b80ed8d..9cf2b5f9 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -109,8 +109,8 @@ bool Encoder::run_index_search() { axis_->run_control_loop([&](){ phase = wrap_pm_pi(phase + omega * current_meas_period); - float v_alpha = voltage_magnitude * arm_cos_f32(phase); - float v_beta = voltage_magnitude * arm_sin_f32(phase); + float v_alpha = voltage_magnitude * our_arm_cos_f32(phase); + float v_beta = voltage_magnitude * our_arm_sin_f32(phase); if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta)) return false; // error set inside enqueue_voltage_timings axis_->motor_.log_timing(Motor::TIMING_LOG_IDX_SEARCH); @@ -167,8 +167,8 @@ bool Encoder::run_offset_calibration() { i = 0; axis_->run_control_loop([&](){ float phase = wrap_pm_pi(scan_distance * (float)i / (float)num_steps - scan_distance / 2.0f); - float v_alpha = voltage_magnitude * arm_cos_f32(phase); - float v_beta = voltage_magnitude * arm_sin_f32(phase); + float v_alpha = voltage_magnitude * our_arm_cos_f32(phase); + float v_beta = voltage_magnitude * our_arm_sin_f32(phase); if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta)) return false; // error set inside enqueue_voltage_timings axis_->motor_.log_timing(Motor::TIMING_LOG_ENC_CALIB); @@ -208,8 +208,8 @@ bool Encoder::run_offset_calibration() { i = 0; axis_->run_control_loop([&](){ float phase = wrap_pm_pi(-scan_distance * (float)i / (float)num_steps + scan_distance / 2.0f); - float v_alpha = voltage_magnitude * arm_cos_f32(phase); - float v_beta = voltage_magnitude * arm_sin_f32(phase); + float v_alpha = voltage_magnitude * our_arm_cos_f32(phase); + float v_beta = voltage_magnitude * our_arm_sin_f32(phase); if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta)) return false; // error set inside enqueue_voltage_timings axis_->motor_.log_timing(Motor::TIMING_LOG_ENC_CALIB); diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp index 56817ee8..24982146 100644 --- a/Firmware/MotorControl/motor.cpp +++ b/Firmware/MotorControl/motor.cpp @@ -290,8 +290,8 @@ bool Motor::enqueue_voltage_timings(float v_alpha, float v_beta) { // TODO: This doesn't update brake current // We should probably make FOC Current call FOC Voltage to avoid duplication. bool Motor::FOC_voltage(float v_d, float v_q, float phase) { - float c = arm_cos_f32(phase); - float s = arm_sin_f32(phase); + float c = our_arm_cos_f32(phase); + float s = our_arm_sin_f32(phase); float v_alpha = c*v_d - s*v_q; float v_beta = c*v_q + s*v_d; return enqueue_voltage_timings(v_alpha, v_beta); @@ -308,8 +308,8 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float phase) { float Ibeta = one_by_sqrt3 * (current_meas_.phB - current_meas_.phC); // Park transform - float c = arm_cos_f32(phase); - float s = arm_sin_f32(phase); + float c = our_arm_cos_f32(phase); + float s = our_arm_sin_f32(phase); float Id = c * Ialpha + s * Ibeta; float Iq = c * Ibeta - s * Ialpha; ictrl->Iq_measured = Iq; diff --git a/Firmware/MotorControl/utils.h b/Firmware/MotorControl/utils.h index 1cd63327..e1ef74be 100644 --- a/Firmware/MotorControl/utils.h +++ b/Firmware/MotorControl/utils.h @@ -99,13 +99,14 @@ 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 } #endif diff --git a/Firmware/Tupfile.lua b/Firmware/Tupfile.lua index fe59828d..6f3ef34c 100644 --- a/Firmware/Tupfile.lua +++ b/Firmware/Tupfile.lua @@ -105,6 +105,7 @@ LDFLAGS += '-Wl,--undefined=uxTopUsedPriority' -- common flags for ASM, C and C++ OPT += '-Og' +-- OPT += '-O0' OPT += '-ffast-math -fno-finite-math-only' tup.append_table(FLAGS, OPT) tup.append_table(LDFLAGS, OPT) @@ -148,6 +149,8 @@ build{ sources={ 'Drivers/DRV8301/drv8301.c', 'MotorControl/utils.c', + 'MotorControl/arm_sin_f32.c', + 'MotorControl/arm_cos_f32.c', 'MotorControl/low_level.cpp', 'MotorControl/nvm.c', 'MotorControl/axis.cpp', diff --git a/ODrive_Workspace.code-workspace b/ODrive_Workspace.code-workspace index 250eb601..d858dcc3 100644 --- a/ODrive_Workspace.code-workspace +++ b/ODrive_Workspace.code-workspace @@ -44,7 +44,8 @@ "algorithm": "cpp", "chrono": "cpp", "condition_variable": "cpp", - "future": "cpp" + "future": "cpp", + "arm_math.h": "c" } } } From e4f506265278d079cb5578b71e3e9b363fdc237a Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Thu, 27 Sep 2018 21:29:21 -0700 Subject: [PATCH 2/2] fix float rounding issue in sincos index calculation --- Firmware/MotorControl/arm_cos_f32.c | 10 ++++++++-- Firmware/MotorControl/arm_sin_f32.c | 15 ++++++++------- 2 files changed, 16 insertions(+), 9 deletions(-) diff --git a/Firmware/MotorControl/arm_cos_f32.c b/Firmware/MotorControl/arm_cos_f32.c index 559153e8..a63d14cb 100644 --- a/Firmware/MotorControl/arm_cos_f32.c +++ b/Firmware/MotorControl/arm_cos_f32.c @@ -94,8 +94,14 @@ float32_t our_arm_cos_f32( in = in - (float32_t) n; /* Calculation of index of the table */ - findex = (float32_t) FAST_MATH_TABLE_SIZE * in; - index = ((uint16_t)findex) & 0x1ff; + findex = (float32_t)FAST_MATH_TABLE_SIZE * in; + index = (uint16_t)findex; + + /* when "in" is exactly 1, we need to rotate the index down to 0 */ + if (index >= FAST_MATH_TABLE_SIZE) { + index = 0; + findex -= (float32_t)FAST_MATH_TABLE_SIZE; + } /* fractional value calculation */ fract = findex - (float32_t) index; diff --git a/Firmware/MotorControl/arm_sin_f32.c b/Firmware/MotorControl/arm_sin_f32.c index 33d436e6..f037248f 100644 --- a/Firmware/MotorControl/arm_sin_f32.c +++ b/Firmware/MotorControl/arm_sin_f32.c @@ -79,11 +79,6 @@ float32_t our_arm_sin_f32( int32_t n; float32_t findex; - /* Special case for small negative inputs */ - if ((x < 0.0f) && (x >= -1.9e-7f)) { - return x; - } - /* input x is in radians */ /* Scale the input to [0 1] range from [0 2*PI] , divide input by 2*pi */ in = x * 0.159154943092f; @@ -101,8 +96,14 @@ float32_t our_arm_sin_f32( in = in - (float32_t) n; /* Calculation of index of the table */ - findex = (float32_t) FAST_MATH_TABLE_SIZE * in; - index = ((uint16_t)findex) & 0x1ff; + findex = (float32_t)FAST_MATH_TABLE_SIZE * in; + index = (uint16_t)findex; + + /* when "in" is exactly 1, we need to rotate the index down to 0 */ + if (index >= FAST_MATH_TABLE_SIZE) { + index = 0; + findex -= (float32_t)FAST_MATH_TABLE_SIZE; + } /* fractional value calculation */ fract = findex - (float32_t) index;