From e4f506265278d079cb5578b71e3e9b363fdc237a Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Thu, 27 Sep 2018 21:29:21 -0700 Subject: [PATCH] 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;