diff options
Diffstat (limited to 'tmk_core/tool/mbed/mbed-sdk/libraries/dsp/cmsis_dsp/ControllerFunctions')
8 files changed, 1274 insertions, 0 deletions
diff --git a/tmk_core/tool/mbed/mbed-sdk/libraries/dsp/cmsis_dsp/ControllerFunctions/arm_pid_init_f32.c b/tmk_core/tool/mbed/mbed-sdk/libraries/dsp/cmsis_dsp/ControllerFunctions/arm_pid_init_f32.c new file mode 100644 index 000000000..cc1fc99a0 --- /dev/null +++ b/tmk_core/tool/mbed/mbed-sdk/libraries/dsp/cmsis_dsp/ControllerFunctions/arm_pid_init_f32.c @@ -0,0 +1,87 @@ +/* ---------------------------------------------------------------------- +* Copyright (C) 2010-2013 ARM Limited. All rights reserved. +* +* $Date: 17. January 2013 +* $Revision: V1.4.1 +* +* Project: CMSIS DSP Library +* Title: arm_pid_init_f32.c +* +* Description: Floating-point PID Control initialization function +* +* +* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0 +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* - Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* - Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in +* the documentation and/or other materials provided with the +* distribution. +* - Neither the name of ARM LIMITED nor the names of its contributors +* may be used to endorse or promote products derived from this +* software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* ------------------------------------------------------------------- */ + +#include "arm_math.h" + + /** + * @addtogroup PID + * @{ + */ + +/** + * @brief Initialization function for the floating-point PID Control. + * @param[in,out] *S points to an instance of the PID structure. + * @param[in] resetStateFlag flag to reset the state. 0 = no change in state & 1 = reset the state. + * @return none. + * \par Description: + * \par + * The <code>resetStateFlag</code> specifies whether to set state to zero or not. \n + * The function computes the structure fields: <code>A0</code>, <code>A1</code> <code>A2</code> + * using the proportional gain( \c Kp), integral gain( \c Ki) and derivative gain( \c Kd) + * also sets the state variables to all zeros. + */ + +void arm_pid_init_f32( + arm_pid_instance_f32 * S, + int32_t resetStateFlag) +{ + + /* Derived coefficient A0 */ + S->A0 = S->Kp + S->Ki + S->Kd; + + /* Derived coefficient A1 */ + S->A1 = (-S->Kp) - ((float32_t) 2.0 * S->Kd); + + /* Derived coefficient A2 */ + S->A2 = S->Kd; + + /* Check whether state needs reset or not */ + if(resetStateFlag) + { + /* Clear the state buffer. The size will be always 3 samples */ + memset(S->state, 0, 3u * sizeof(float32_t)); + } + +} + +/** + * @} end of PID group + */ diff --git a/tmk_core/tool/mbed/mbed-sdk/libraries/dsp/cmsis_dsp/ControllerFunctions/arm_pid_init_q15.c b/tmk_core/tool/mbed/mbed-sdk/libraries/dsp/cmsis_dsp/ControllerFunctions/arm_pid_init_q15.c new file mode 100644 index 000000000..8f293f6e3 --- /dev/null +++ b/tmk_core/tool/mbed/mbed-sdk/libraries/dsp/cmsis_dsp/ControllerFunctions/arm_pid_init_q15.c @@ -0,0 +1,122 @@ +/* ---------------------------------------------------------------------- +* Copyright (C) 2010-2013 ARM Limited. All rights reserved. +* +* $Date: 17. January 2013 +* $Revision: V1.4.1 +* +* Project: CMSIS DSP Library +* Title: arm_pid_init_q15.c +* +* Description: Q15 PID Control initialization function +* +* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0 +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* - Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* - Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in +* the documentation and/or other materials provided with the +* distribution. +* - Neither the name of ARM LIMITED nor the names of its contributors +* may be used to endorse or promote products derived from this +* software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* -------------------------------------------------------------------- */ + +#include "arm_math.h" + + /** + * @addtogroup PID + * @{ + */ + +/** + * @details + * @param[in,out] *S points to an instance of the Q15 PID structure. + * @param[in] resetStateFlag flag to reset the state. 0 = no change in state 1 = reset the state. + * @return none. + * \par Description: + * \par + * The <code>resetStateFlag</code> specifies whether to set state to zero or not. \n + * The function computes the structure fields: <code>A0</code>, <code>A1</code> <code>A2</code> + * using the proportional gain( \c Kp), integral gain( \c Ki) and derivative gain( \c Kd) + * also sets the state variables to all zeros. + */ + +void arm_pid_init_q15( + arm_pid_instance_q15 * S, + int32_t resetStateFlag) +{ + +#ifndef ARM_MATH_CM0_FAMILY + + /* Run the below code for Cortex-M4 and Cortex-M3 */ + + /* Derived coefficient A0 */ + S->A0 = __QADD16(__QADD16(S->Kp, S->Ki), S->Kd); + + /* Derived coefficients and pack into A1 */ + +#ifndef ARM_MATH_BIG_ENDIAN + + S->A1 = __PKHBT(-__QADD16(__QADD16(S->Kd, S->Kd), S->Kp), S->Kd, 16); + +#else + + S->A1 = __PKHBT(S->Kd, -__QADD16(__QADD16(S->Kd, S->Kd), S->Kp), 16); + +#endif /* #ifndef ARM_MATH_BIG_ENDIAN */ + + /* Check whether state needs reset or not */ + if(resetStateFlag) + { + /* Clear the state buffer. The size will be always 3 samples */ + memset(S->state, 0, 3u * sizeof(q15_t)); + } + +#else + + /* Run the below code for Cortex-M0 */ + + q31_t temp; /*to store the sum */ + + /* Derived coefficient A0 */ + temp = S->Kp + S->Ki + S->Kd; + S->A0 = (q15_t) __SSAT(temp, 16); + + /* Derived coefficients and pack into A1 */ + temp = -(S->Kd + S->Kd + S->Kp); + S->A1 = (q15_t) __SSAT(temp, 16); + S->A2 = S->Kd; + + + + /* Check whether state needs reset or not */ + if(resetStateFlag) + { + /* Clear the state buffer. The size will be always 3 samples */ + memset(S->state, 0, 3u * sizeof(q15_t)); + } + +#endif /* #ifndef ARM_MATH_CM0_FAMILY */ + +} + +/** + * @} end of PID group + */ diff --git a/tmk_core/tool/mbed/mbed-sdk/libraries/dsp/cmsis_dsp/ControllerFunctions/arm_pid_init_q31.c b/tmk_core/tool/mbed/mbed-sdk/libraries/dsp/cmsis_dsp/ControllerFunctions/arm_pid_init_q31.c new file mode 100644 index 000000000..b492cf79f --- /dev/null +++ b/tmk_core/tool/mbed/mbed-sdk/libraries/dsp/cmsis_dsp/ControllerFunctions/arm_pid_init_q31.c @@ -0,0 +1,107 @@ +/* ---------------------------------------------------------------------- +* Copyright (C) 2010-2013 ARM Limited. All rights reserved. +* +* $Date: 17. January 2013 +* $Revision: V1.4.1 +* +* Project: CMSIS DSP Library +* Title: arm_pid_init_q31.c +* +* Description: Q31 PID Control initialization function +* +* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0 +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* - Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* - Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in +* the documentation and/or other materials provided with the +* distribution. +* - Neither the name of ARM LIMITED nor the names of its contributors +* may be used to endorse or promote products derived from this +* software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* ------------------------------------------------------------------- */ + +#include "arm_math.h" + + /** + * @addtogroup PID + * @{ + */ + +/** + * @brief Initialization function for the Q31 PID Control. + * @param[in,out] *S points to an instance of the Q31 PID structure. + * @param[in] resetStateFlag flag to reset the state. 0 = no change in state 1 = reset the state. + * @return none. + * \par Description: + * \par + * The <code>resetStateFlag</code> specifies whether to set state to zero or not. \n + * The function computes the structure fields: <code>A0</code>, <code>A1</code> <code>A2</code> + * using the proportional gain( \c Kp), integral gain( \c Ki) and derivative gain( \c Kd) + * also sets the state variables to all zeros. + */ + +void arm_pid_init_q31( + arm_pid_instance_q31 * S, + int32_t resetStateFlag) +{ + +#ifndef ARM_MATH_CM0_FAMILY + + /* Run the below code for Cortex-M4 and Cortex-M3 */ + + /* Derived coefficient A0 */ + S->A0 = __QADD(__QADD(S->Kp, S->Ki), S->Kd); + + /* Derived coefficient A1 */ + S->A1 = -__QADD(__QADD(S->Kd, S->Kd), S->Kp); + + +#else + + /* Run the below code for Cortex-M0 */ + + q31_t temp; + + /* Derived coefficient A0 */ + temp = clip_q63_to_q31((q63_t) S->Kp + S->Ki); + S->A0 = clip_q63_to_q31((q63_t) temp + S->Kd); + + /* Derived coefficient A1 */ + temp = clip_q63_to_q31((q63_t) S->Kd + S->Kd); + S->A1 = -clip_q63_to_q31((q63_t) temp + S->Kp); + +#endif /* #ifndef ARM_MATH_CM0_FAMILY */ + + /* Derived coefficient A2 */ + S->A2 = S->Kd; + + /* Check whether state needs reset or not */ + if(resetStateFlag) + { + /* Clear the state buffer. The size will be always 3 samples */ + memset(S->state, 0, 3u * sizeof(q31_t)); + } + +} + +/** + * @} end of PID group + */ diff --git a/tmk_core/tool/mbed/mbed-sdk/libraries/dsp/cmsis_dsp/ControllerFunctions/arm_pid_reset_f32.c b/tmk_core/tool/mbed/mbed-sdk/libraries/dsp/cmsis_dsp/ControllerFunctions/arm_pid_reset_f32.c new file mode 100644 index 000000000..c6753b1b5 --- /dev/null +++ b/tmk_core/tool/mbed/mbed-sdk/libraries/dsp/cmsis_dsp/ControllerFunctions/arm_pid_reset_f32.c @@ -0,0 +1,65 @@ +/* ---------------------------------------------------------------------- +* Copyright (C) 2010-2013 ARM Limited. All rights reserved. +* +* $Date: 17. January 2013 +* $Revision: V1.4.1 +* +* Project: CMSIS DSP Library +* Title: arm_pid_reset_f32.c +* +* Description: Floating-point PID Control reset function +* +* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0 +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* - Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* - Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in +* the documentation and/or other materials provided with the +* distribution. +* - Neither the name of ARM LIMITED nor the names of its contributors +* may be used to endorse or promote products derived from this +* software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* ------------------------------------------------------------------- */ + +#include "arm_math.h" + + /** + * @addtogroup PID + * @{ + */ + +/** +* @brief Reset function for the floating-point PID Control. +* @param[in] *S Instance pointer of PID control data structure. +* @return none. +* \par Description: +* The function resets the state buffer to zeros. +*/ +void arm_pid_reset_f32( + arm_pid_instance_f32 * S) +{ + + /* Clear the state buffer. The size will be always 3 samples */ + memset(S->state, 0, 3u * sizeof(float32_t)); +} + +/** + * @} end of PID group + */ diff --git a/tmk_core/tool/mbed/mbed-sdk/libraries/dsp/cmsis_dsp/ControllerFunctions/arm_pid_reset_q15.c b/tmk_core/tool/mbed/mbed-sdk/libraries/dsp/cmsis_dsp/ControllerFunctions/arm_pid_reset_q15.c new file mode 100644 index 000000000..410339e59 --- /dev/null +++ b/tmk_core/tool/mbed/mbed-sdk/libraries/dsp/cmsis_dsp/ControllerFunctions/arm_pid_reset_q15.c @@ -0,0 +1,64 @@ +/* ---------------------------------------------------------------------- +* Copyright (C) 2010-2013 ARM Limited. All rights reserved. +* +* $Date: 17. January 2013 +* $Revision: V1.4.1 +* +* Project: CMSIS DSP Library +* Title: arm_pid_reset_q15.c +* +* Description: Q15 PID Control reset function +* +* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0 +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* - Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* - Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in +* the documentation and/or other materials provided with the +* distribution. +* - Neither the name of ARM LIMITED nor the names of its contributors +* may be used to endorse or promote products derived from this +* software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* -------------------------------------------------------------------- */ + +#include "arm_math.h" + + /** + * @addtogroup PID + * @{ + */ + +/** +* @brief Reset function for the Q15 PID Control. +* @param[in] *S Instance pointer of PID control data structure. +* @return none. +* \par Description: +* The function resets the state buffer to zeros. +*/ +void arm_pid_reset_q15( + arm_pid_instance_q15 * S) +{ + /* Reset state to zero, The size will be always 3 samples */ + memset(S->state, 0, 3u * sizeof(q15_t)); +} + +/** + * @} end of PID group + */ diff --git a/tmk_core/tool/mbed/mbed-sdk/libraries/dsp/cmsis_dsp/ControllerFunctions/arm_pid_reset_q31.c b/tmk_core/tool/mbed/mbed-sdk/libraries/dsp/cmsis_dsp/ControllerFunctions/arm_pid_reset_q31.c new file mode 100644 index 000000000..fd8208008 --- /dev/null +++ b/tmk_core/tool/mbed/mbed-sdk/libraries/dsp/cmsis_dsp/ControllerFunctions/arm_pid_reset_q31.c @@ -0,0 +1,65 @@ +/* ---------------------------------------------------------------------- +* Copyright (C) 2010-2013 ARM Limited. All rights reserved. +* +* $Date: 17. January 2013 +* $Revision: V1.4.1 +* +* Project: CMSIS DSP Library +* Title: arm_pid_reset_q31.c +* +* Description: Q31 PID Control reset function +* +* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0 +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* - Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* - Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in +* the documentation and/or other materials provided with the +* distribution. +* - Neither the name of ARM LIMITED nor the names of its contributors +* may be used to endorse or promote products derived from this +* software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* ------------------------------------------------------------------- */ + +#include "arm_math.h" + + /** + * @addtogroup PID + * @{ + */ + +/** +* @brief Reset function for the Q31 PID Control. +* @param[in] *S Instance pointer of PID control data structure. +* @return none. +* \par Description: +* The function resets the state buffer to zeros. +*/ +void arm_pid_reset_q31( + arm_pid_instance_q31 * S) +{ + + /* Clear the state buffer. The size will be always 3 samples */ + memset(S->state, 0, 3u * sizeof(q31_t)); +} + +/** + * @} end of PID group + */ diff --git a/tmk_core/tool/mbed/mbed-sdk/libraries/dsp/cmsis_dsp/ControllerFunctions/arm_sin_cos_f32.c b/tmk_core/tool/mbed/mbed-sdk/libraries/dsp/cmsis_dsp/ControllerFunctions/arm_sin_cos_f32.c new file mode 100644 index 000000000..4658f9a2a --- /dev/null +++ b/tmk_core/tool/mbed/mbed-sdk/libraries/dsp/cmsis_dsp/ControllerFunctions/arm_sin_cos_f32.c @@ -0,0 +1,436 @@ +/* ---------------------------------------------------------------------- +* Copyright (C) 2010-2013 ARM Limited. All rights reserved. +* +* $Date: 17. January 2013 +* $Revision: V1.4.1 +* +* Project: CMSIS DSP Library +* Title: arm_sin_cos_f32.c +* +* Description: Sine and Cosine calculation for floating-point values. +* +* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0 +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* - Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* - Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in +* the documentation and/or other materials provided with the +* distribution. +* - Neither the name of ARM LIMITED nor the names of its contributors +* may be used to endorse or promote products derived from this +* software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* -------------------------------------------------------------------- */ + +#include "arm_math.h" + +/** + * @ingroup groupController + */ + +/** + * @defgroup SinCos Sine Cosine + * + * Computes the trigonometric sine and cosine values using a combination of table lookup + * and linear interpolation. + * There are separate functions for Q31 and floating-point data types. + * The input to the floating-point version is in degrees while the + * fixed-point Q31 have a scaled input with the range + * [-1 0.9999] mapping to [-180 179] degrees. + * + * The implementation is based on table lookup using 360 values together with linear interpolation. + * The steps used are: + * -# Calculation of the nearest integer table index. + * -# Compute the fractional portion (fract) of the input. + * -# Fetch the value corresponding to \c index from sine table to \c y0 and also value from \c index+1 to \c y1. + * -# Sine value is computed as <code> *psinVal = y0 + (fract * (y1 - y0))</code>. + * -# Fetch the value corresponding to \c index from cosine table to \c y0 and also value from \c index+1 to \c y1. + * -# Cosine value is computed as <code> *pcosVal = y0 + (fract * (y1 - y0))</code>. + */ + + /** + * @addtogroup SinCos + * @{ + */ + + +/** +* \par +* Cosine Table is generated from following loop +* <pre>for(i = 0; i < 360; i++) +* { +* cosTable[i]= cos((i-180) * PI/180.0); +* } </pre> +*/ + +static const float32_t cosTable[360] = { + -0.999847695156391270f, -0.999390827019095760f, -0.998629534754573830f, + -0.997564050259824200f, -0.996194698091745550f, -0.994521895368273290f, + -0.992546151641321980f, -0.990268068741570250f, + -0.987688340595137660f, -0.984807753012208020f, -0.981627183447663980f, + -0.978147600733805690f, -0.974370064785235250f, -0.970295726275996470f, + -0.965925826289068200f, -0.961261695938318670f, + -0.956304755963035440f, -0.951056516295153530f, -0.945518575599316740f, + -0.939692620785908320f, -0.933580426497201740f, -0.927183854566787310f, + -0.920504853452440150f, -0.913545457642600760f, + -0.906307787036649940f, -0.898794046299167040f, -0.891006524188367790f, + -0.882947592858926770f, -0.874619707139395740f, -0.866025403784438710f, + -0.857167300702112220f, -0.848048096156425960f, + -0.838670567945424160f, -0.829037572555041620f, -0.819152044288991580f, + -0.809016994374947340f, -0.798635510047292940f, -0.788010753606721900f, + -0.777145961456970680f, -0.766044443118977900f, + -0.754709580222772010f, -0.743144825477394130f, -0.731353701619170460f, + -0.719339800338651300f, -0.707106781186547460f, -0.694658370458997030f, + -0.681998360062498370f, -0.669130606358858240f, + -0.656059028990507500f, -0.642787609686539360f, -0.629320391049837280f, + -0.615661475325658290f, -0.601815023152048380f, -0.587785252292473030f, + -0.573576436351045830f, -0.559192903470746680f, + -0.544639035015027080f, -0.529919264233204790f, -0.515038074910054270f, + -0.499999999999999780f, -0.484809620246337000f, -0.469471562785890530f, + -0.453990499739546750f, -0.438371146789077510f, + -0.422618261740699330f, -0.406736643075800100f, -0.390731128489273600f, + -0.374606593415912070f, -0.358367949545300270f, -0.342020143325668710f, + -0.325568154457156420f, -0.309016994374947340f, + -0.292371704722736660f, -0.275637355816999050f, -0.258819045102520850f, + -0.241921895599667790f, -0.224951054343864810f, -0.207911690817759120f, + -0.190808995376544800f, -0.173648177666930300f, + -0.156434465040231040f, -0.139173100960065350f, -0.121869343405147370f, + -0.104528463267653330f, -0.087155742747658235f, -0.069756473744125330f, + -0.052335956242943620f, -0.034899496702500733f, + -0.017452406437283477f, 0.000000000000000061f, 0.017452406437283376f, + 0.034899496702501080f, 0.052335956242943966f, 0.069756473744125455f, + 0.087155742747658138f, 0.104528463267653460f, + 0.121869343405147490f, 0.139173100960065690f, 0.156434465040230920f, + 0.173648177666930410f, 0.190808995376544920f, 0.207911690817759450f, + 0.224951054343864920f, 0.241921895599667900f, + 0.258819045102520740f, 0.275637355816999160f, 0.292371704722736770f, + 0.309016994374947450f, 0.325568154457156760f, 0.342020143325668820f, + 0.358367949545300380f, 0.374606593415911960f, + 0.390731128489273940f, 0.406736643075800210f, 0.422618261740699440f, + 0.438371146789077460f, 0.453990499739546860f, 0.469471562785890860f, + 0.484809620246337110f, 0.500000000000000110f, + 0.515038074910054380f, 0.529919264233204900f, 0.544639035015027200f, + 0.559192903470746790f, 0.573576436351046050f, 0.587785252292473140f, + 0.601815023152048270f, 0.615661475325658290f, + 0.629320391049837500f, 0.642787609686539360f, 0.656059028990507280f, + 0.669130606358858240f, 0.681998360062498480f, 0.694658370458997370f, + 0.707106781186547570f, 0.719339800338651190f, + 0.731353701619170570f, 0.743144825477394240f, 0.754709580222772010f, + 0.766044443118978010f, 0.777145961456970900f, 0.788010753606722010f, + 0.798635510047292830f, 0.809016994374947450f, + 0.819152044288991800f, 0.829037572555041620f, 0.838670567945424050f, + 0.848048096156425960f, 0.857167300702112330f, 0.866025403784438710f, + 0.874619707139395740f, 0.882947592858926990f, + 0.891006524188367900f, 0.898794046299167040f, 0.906307787036649940f, + 0.913545457642600870f, 0.920504853452440370f, 0.927183854566787420f, + 0.933580426497201740f, 0.939692620785908430f, + 0.945518575599316850f, 0.951056516295153530f, 0.956304755963035440f, + 0.961261695938318890f, 0.965925826289068310f, 0.970295726275996470f, + 0.974370064785235250f, 0.978147600733805690f, + 0.981627183447663980f, 0.984807753012208020f, 0.987688340595137770f, + 0.990268068741570360f, 0.992546151641321980f, 0.994521895368273290f, + 0.996194698091745550f, 0.997564050259824200f, + 0.998629534754573830f, 0.999390827019095760f, 0.999847695156391270f, + 1.000000000000000000f, 0.999847695156391270f, 0.999390827019095760f, + 0.998629534754573830f, 0.997564050259824200f, + 0.996194698091745550f, 0.994521895368273290f, 0.992546151641321980f, + 0.990268068741570360f, 0.987688340595137770f, 0.984807753012208020f, + 0.981627183447663980f, 0.978147600733805690f, + 0.974370064785235250f, 0.970295726275996470f, 0.965925826289068310f, + 0.961261695938318890f, 0.956304755963035440f, 0.951056516295153530f, + 0.945518575599316850f, 0.939692620785908430f, + 0.933580426497201740f, 0.927183854566787420f, 0.920504853452440370f, + 0.913545457642600870f, 0.906307787036649940f, 0.898794046299167040f, + 0.891006524188367900f, 0.882947592858926990f, + 0.874619707139395740f, 0.866025403784438710f, 0.857167300702112330f, + 0.848048096156425960f, 0.838670567945424050f, 0.829037572555041620f, + 0.819152044288991800f, 0.809016994374947450f, + 0.798635510047292830f, 0.788010753606722010f, 0.777145961456970900f, + 0.766044443118978010f, 0.754709580222772010f, 0.743144825477394240f, + 0.731353701619170570f, 0.719339800338651190f, + 0.707106781186547570f, 0.694658370458997370f, 0.681998360062498480f, + 0.669130606358858240f, 0.656059028990507280f, 0.642787609686539360f, + 0.629320391049837500f, 0.615661475325658290f, + 0.601815023152048270f, 0.587785252292473140f, 0.573576436351046050f, + 0.559192903470746790f, 0.544639035015027200f, 0.529919264233204900f, + 0.515038074910054380f, 0.500000000000000110f, + 0.484809620246337110f, 0.469471562785890860f, 0.453990499739546860f, + 0.438371146789077460f, 0.422618261740699440f, 0.406736643075800210f, + 0.390731128489273940f, 0.374606593415911960f, + 0.358367949545300380f, 0.342020143325668820f, 0.325568154457156760f, + 0.309016994374947450f, 0.292371704722736770f, 0.275637355816999160f, + 0.258819045102520740f, 0.241921895599667900f, + 0.224951054343864920f, 0.207911690817759450f, 0.190808995376544920f, + 0.173648177666930410f, 0.156434465040230920f, 0.139173100960065690f, + 0.121869343405147490f, 0.104528463267653460f, + 0.087155742747658138f, 0.069756473744125455f, 0.052335956242943966f, + 0.034899496702501080f, 0.017452406437283376f, 0.000000000000000061f, + -0.017452406437283477f, -0.034899496702500733f, + -0.052335956242943620f, -0.069756473744125330f, -0.087155742747658235f, + -0.104528463267653330f, -0.121869343405147370f, -0.139173100960065350f, + -0.156434465040231040f, -0.173648177666930300f, + -0.190808995376544800f, -0.207911690817759120f, -0.224951054343864810f, + -0.241921895599667790f, -0.258819045102520850f, -0.275637355816999050f, + -0.292371704722736660f, -0.309016994374947340f, + -0.325568154457156420f, -0.342020143325668710f, -0.358367949545300270f, + -0.374606593415912070f, -0.390731128489273600f, -0.406736643075800100f, + -0.422618261740699330f, -0.438371146789077510f, + -0.453990499739546750f, -0.469471562785890530f, -0.484809620246337000f, + -0.499999999999999780f, -0.515038074910054270f, -0.529919264233204790f, + -0.544639035015027080f, -0.559192903470746680f, + -0.573576436351045830f, -0.587785252292473030f, -0.601815023152048380f, + -0.615661475325658290f, -0.629320391049837280f, -0.642787609686539360f, + -0.656059028990507500f, -0.669130606358858240f, + -0.681998360062498370f, -0.694658370458997030f, -0.707106781186547460f, + -0.719339800338651300f, -0.731353701619170460f, -0.743144825477394130f, + -0.754709580222772010f, -0.766044443118977900f, + -0.777145961456970680f, -0.788010753606721900f, -0.798635510047292940f, + -0.809016994374947340f, -0.819152044288991580f, -0.829037572555041620f, + -0.838670567945424160f, -0.848048096156425960f, + -0.857167300702112220f, -0.866025403784438710f, -0.874619707139395740f, + -0.882947592858926770f, -0.891006524188367790f, -0.898794046299167040f, + -0.906307787036649940f, -0.913545457642600760f, + -0.920504853452440150f, -0.927183854566787310f, -0.933580426497201740f, + -0.939692620785908320f, -0.945518575599316740f, -0.951056516295153530f, + -0.956304755963035440f, -0.961261695938318670f, + -0.965925826289068200f, -0.970295726275996470f, -0.974370064785235250f, + -0.978147600733805690f, -0.981627183447663980f, -0.984807753012208020f, + -0.987688340595137660f, -0.990268068741570250f, + -0.992546151641321980f, -0.994521895368273290f, -0.996194698091745550f, + -0.997564050259824200f, -0.998629534754573830f, -0.999390827019095760f, + -0.999847695156391270f, -1.000000000000000000f +}; + +/** +* \par +* Sine Table is generated from following loop +* <pre>for(i = 0; i < 360; i++) +* { +* sinTable[i]= sin((i-180) * PI/180.0); +* } </pre> +*/ + + +static const float32_t sinTable[360] = { + -0.017452406437283439f, -0.034899496702500699f, -0.052335956242943807f, + -0.069756473744125524f, -0.087155742747658638f, -0.104528463267653730f, + -0.121869343405147550f, -0.139173100960065740f, + -0.156434465040230980f, -0.173648177666930280f, -0.190808995376544970f, + -0.207911690817759310f, -0.224951054343864780f, -0.241921895599667730f, + -0.258819045102521020f, -0.275637355816999660f, + -0.292371704722737050f, -0.309016994374947510f, -0.325568154457156980f, + -0.342020143325668880f, -0.358367949545300210f, -0.374606593415912240f, + -0.390731128489274160f, -0.406736643075800430f, + -0.422618261740699500f, -0.438371146789077290f, -0.453990499739546860f, + -0.469471562785891080f, -0.484809620246337170f, -0.499999999999999940f, + -0.515038074910054380f, -0.529919264233204900f, + -0.544639035015026860f, -0.559192903470746900f, -0.573576436351046380f, + -0.587785252292473250f, -0.601815023152048160f, -0.615661475325658400f, + -0.629320391049837720f, -0.642787609686539470f, + -0.656059028990507280f, -0.669130606358858350f, -0.681998360062498590f, + -0.694658370458997140f, -0.707106781186547570f, -0.719339800338651410f, + -0.731353701619170570f, -0.743144825477394240f, + -0.754709580222771790f, -0.766044443118978010f, -0.777145961456971010f, + -0.788010753606722010f, -0.798635510047292720f, -0.809016994374947450f, + -0.819152044288992020f, -0.829037572555041740f, + -0.838670567945424050f, -0.848048096156426070f, -0.857167300702112330f, + -0.866025403784438710f, -0.874619707139395850f, -0.882947592858927100f, + -0.891006524188367900f, -0.898794046299166930f, + -0.906307787036650050f, -0.913545457642600980f, -0.920504853452440370f, + -0.927183854566787420f, -0.933580426497201740f, -0.939692620785908430f, + -0.945518575599316850f, -0.951056516295153640f, + -0.956304755963035550f, -0.961261695938318890f, -0.965925826289068310f, + -0.970295726275996470f, -0.974370064785235250f, -0.978147600733805690f, + -0.981627183447663980f, -0.984807753012208020f, + -0.987688340595137660f, -0.990268068741570360f, -0.992546151641322090f, + -0.994521895368273400f, -0.996194698091745550f, -0.997564050259824200f, + -0.998629534754573830f, -0.999390827019095760f, + -0.999847695156391270f, -1.000000000000000000f, -0.999847695156391270f, + -0.999390827019095760f, -0.998629534754573830f, -0.997564050259824200f, + -0.996194698091745550f, -0.994521895368273290f, + -0.992546151641321980f, -0.990268068741570250f, -0.987688340595137770f, + -0.984807753012208020f, -0.981627183447663980f, -0.978147600733805580f, + -0.974370064785235250f, -0.970295726275996470f, + -0.965925826289068310f, -0.961261695938318890f, -0.956304755963035440f, + -0.951056516295153530f, -0.945518575599316740f, -0.939692620785908320f, + -0.933580426497201740f, -0.927183854566787420f, + -0.920504853452440260f, -0.913545457642600870f, -0.906307787036649940f, + -0.898794046299167040f, -0.891006524188367790f, -0.882947592858926880f, + -0.874619707139395740f, -0.866025403784438600f, + -0.857167300702112220f, -0.848048096156426070f, -0.838670567945423940f, + -0.829037572555041740f, -0.819152044288991800f, -0.809016994374947450f, + -0.798635510047292830f, -0.788010753606722010f, + -0.777145961456970790f, -0.766044443118978010f, -0.754709580222772010f, + -0.743144825477394240f, -0.731353701619170460f, -0.719339800338651080f, + -0.707106781186547460f, -0.694658370458997250f, + -0.681998360062498480f, -0.669130606358858240f, -0.656059028990507160f, + -0.642787609686539250f, -0.629320391049837390f, -0.615661475325658180f, + -0.601815023152048270f, -0.587785252292473140f, + -0.573576436351046050f, -0.559192903470746900f, -0.544639035015027080f, + -0.529919264233204900f, -0.515038074910054160f, -0.499999999999999940f, + -0.484809620246337060f, -0.469471562785890810f, + -0.453990499739546750f, -0.438371146789077400f, -0.422618261740699440f, + -0.406736643075800150f, -0.390731128489273720f, -0.374606593415912010f, + -0.358367949545300270f, -0.342020143325668710f, + -0.325568154457156640f, -0.309016994374947400f, -0.292371704722736770f, + -0.275637355816999160f, -0.258819045102520740f, -0.241921895599667730f, + -0.224951054343865000f, -0.207911690817759310f, + -0.190808995376544800f, -0.173648177666930330f, -0.156434465040230870f, + -0.139173100960065440f, -0.121869343405147480f, -0.104528463267653460f, + -0.087155742747658166f, -0.069756473744125302f, + -0.052335956242943828f, -0.034899496702500969f, -0.017452406437283512f, + 0.000000000000000000f, 0.017452406437283512f, 0.034899496702500969f, + 0.052335956242943828f, 0.069756473744125302f, + 0.087155742747658166f, 0.104528463267653460f, 0.121869343405147480f, + 0.139173100960065440f, 0.156434465040230870f, 0.173648177666930330f, + 0.190808995376544800f, 0.207911690817759310f, + 0.224951054343865000f, 0.241921895599667730f, 0.258819045102520740f, + 0.275637355816999160f, 0.292371704722736770f, 0.309016994374947400f, + 0.325568154457156640f, 0.342020143325668710f, + 0.358367949545300270f, 0.374606593415912010f, 0.390731128489273720f, + 0.406736643075800150f, 0.422618261740699440f, 0.438371146789077400f, + 0.453990499739546750f, 0.469471562785890810f, + 0.484809620246337060f, 0.499999999999999940f, 0.515038074910054160f, + 0.529919264233204900f, 0.544639035015027080f, 0.559192903470746900f, + 0.573576436351046050f, 0.587785252292473140f, + 0.601815023152048270f, 0.615661475325658180f, 0.629320391049837390f, + 0.642787609686539250f, 0.656059028990507160f, 0.669130606358858240f, + 0.681998360062498480f, 0.694658370458997250f, + 0.707106781186547460f, 0.719339800338651080f, 0.731353701619170460f, + 0.743144825477394240f, 0.754709580222772010f, 0.766044443118978010f, + 0.777145961456970790f, 0.788010753606722010f, + 0.798635510047292830f, 0.809016994374947450f, 0.819152044288991800f, + 0.829037572555041740f, 0.838670567945423940f, 0.848048096156426070f, + 0.857167300702112220f, 0.866025403784438600f, + 0.874619707139395740f, 0.882947592858926880f, 0.891006524188367790f, + 0.898794046299167040f, 0.906307787036649940f, 0.913545457642600870f, + 0.920504853452440260f, 0.927183854566787420f, + 0.933580426497201740f, 0.939692620785908320f, 0.945518575599316740f, + 0.951056516295153530f, 0.956304755963035440f, 0.961261695938318890f, + 0.965925826289068310f, 0.970295726275996470f, + 0.974370064785235250f, 0.978147600733805580f, 0.981627183447663980f, + 0.984807753012208020f, 0.987688340595137770f, 0.990268068741570250f, + 0.992546151641321980f, 0.994521895368273290f, + 0.996194698091745550f, 0.997564050259824200f, 0.998629534754573830f, + 0.999390827019095760f, 0.999847695156391270f, 1.000000000000000000f, + 0.999847695156391270f, 0.999390827019095760f, + 0.998629534754573830f, 0.997564050259824200f, 0.996194698091745550f, + 0.994521895368273400f, 0.992546151641322090f, 0.990268068741570360f, + 0.987688340595137660f, 0.984807753012208020f, + 0.981627183447663980f, 0.978147600733805690f, 0.974370064785235250f, + 0.970295726275996470f, 0.965925826289068310f, 0.961261695938318890f, + 0.956304755963035550f, 0.951056516295153640f, + 0.945518575599316850f, 0.939692620785908430f, 0.933580426497201740f, + 0.927183854566787420f, 0.920504853452440370f, 0.913545457642600980f, + 0.906307787036650050f, 0.898794046299166930f, + 0.891006524188367900f, 0.882947592858927100f, 0.874619707139395850f, + 0.866025403784438710f, 0.857167300702112330f, 0.848048096156426070f, + 0.838670567945424050f, 0.829037572555041740f, + 0.819152044288992020f, 0.809016994374947450f, 0.798635510047292720f, + 0.788010753606722010f, 0.777145961456971010f, 0.766044443118978010f, + 0.754709580222771790f, 0.743144825477394240f, + 0.731353701619170570f, 0.719339800338651410f, 0.707106781186547570f, + 0.694658370458997140f, 0.681998360062498590f, 0.669130606358858350f, + 0.656059028990507280f, 0.642787609686539470f, + 0.629320391049837720f, 0.615661475325658400f, 0.601815023152048160f, + 0.587785252292473250f, 0.573576436351046380f, 0.559192903470746900f, + 0.544639035015026860f, 0.529919264233204900f, + 0.515038074910054380f, 0.499999999999999940f, 0.484809620246337170f, + 0.469471562785891080f, 0.453990499739546860f, 0.438371146789077290f, + 0.422618261740699500f, 0.406736643075800430f, + 0.390731128489274160f, 0.374606593415912240f, 0.358367949545300210f, + 0.342020143325668880f, 0.325568154457156980f, 0.309016994374947510f, + 0.292371704722737050f, 0.275637355816999660f, + 0.258819045102521020f, 0.241921895599667730f, 0.224951054343864780f, + 0.207911690817759310f, 0.190808995376544970f, 0.173648177666930280f, + 0.156434465040230980f, 0.139173100960065740f, + 0.121869343405147550f, 0.104528463267653730f, 0.087155742747658638f, + 0.069756473744125524f, 0.052335956242943807f, 0.034899496702500699f, + 0.017452406437283439f, 0.000000000000000122f +}; + + +/** + * @brief Floating-point sin_cos function. + * @param[in] theta input value in degrees + * @param[out] *pSinVal points to the processed sine output. + * @param[out] *pCosVal points to the processed cos output. + * @return none. + */ + + +void arm_sin_cos_f32( + float32_t theta, + float32_t * pSinVal, + float32_t * pCosVal) +{ + int32_t i; /* Index for reading nearwst output values */ + float32_t x1 = -179.0f; /* Initial input value */ + float32_t y0, y1; /* nearest output values */ + float32_t y2, y3; + float32_t fract; /* fractional part of input */ + + /* Calculation of fractional part */ + if(theta > 0.0f) + { + fract = theta - (float32_t) ((int32_t) theta); + } + else + { + fract = (theta - (float32_t) ((int32_t) theta)) + 1.0f; + } + + /* index calculation for reading nearest output values */ + i = (uint32_t) (theta - x1); + + /* Checking min and max index of table */ + if(i < 0) + { + i = 0; + } + else if(i >= 359) + { + i = 358; + } + + /* reading nearest sine output values */ + y0 = sinTable[i]; + y1 = sinTable[i + 1u]; + + /* reading nearest cosine output values */ + y2 = cosTable[i]; + y3 = cosTable[i + 1u]; + + y1 = y1 - y0; + y3 = y3 - y2; + + y1 = fract * y1; + y3 = fract * y3; + + /* Calculation of sine value */ + *pSinVal = y0 + y1; + + /* Calculation of cosine value */ + *pCosVal = y2 + y3; + +} + +/** + * @} end of SinCos group + */ diff --git a/tmk_core/tool/mbed/mbed-sdk/libraries/dsp/cmsis_dsp/ControllerFunctions/arm_sin_cos_q31.c b/tmk_core/tool/mbed/mbed-sdk/libraries/dsp/cmsis_dsp/ControllerFunctions/arm_sin_cos_q31.c new file mode 100644 index 000000000..370b7b6ef --- /dev/null +++ b/tmk_core/tool/mbed/mbed-sdk/libraries/dsp/cmsis_dsp/ControllerFunctions/arm_sin_cos_q31.c @@ -0,0 +1,328 @@ +/* ---------------------------------------------------------------------- +* Copyright (C) 2010-2013 ARM Limited. All rights reserved. +* +* $Date: 17. January 2013 +* $Revision: V1.4.1 +* +* Project: CMSIS DSP Library +* Title: arm_sin_cos_q31.c +* +* Description: Cosine & Sine calculation for Q31 values. +* +* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0 +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* - Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* - Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in +* the documentation and/or other materials provided with the +* distribution. +* - Neither the name of ARM LIMITED nor the names of its contributors +* may be used to endorse or promote products derived from this +* software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* -------------------------------------------------------------------- */ + +#include "arm_math.h" + +/** + * @ingroup groupController + */ + + /** + * @addtogroup SinCos + * @{ + */ + +/** +* \par +* Sine Table is generated from following loop +* <pre>for(i = 0; i < 360; i++) +* { +* sinTable[i]= sin((i-180) * PI/180.0); +* } </pre> +* Convert above coefficients to fixed point 1.31 format. +*/ + +static const int32_t sinTableQ31[360] = { + + 0x0, 0xfdc41e9b, 0xfb8869ce, 0xf94d0e2e, 0xf7123849, 0xf4d814a4, 0xf29ecfb2, + 0xf06695da, + 0xee2f9369, 0xebf9f498, 0xe9c5e582, 0xe7939223, 0xe5632654, 0xe334cdc9, + 0xe108b40d, 0xdedf047d, + 0xdcb7ea46, 0xda939061, 0xd8722192, 0xd653c860, 0xd438af17, 0xd220ffc0, + 0xd00ce422, 0xcdfc85bb, + 0xcbf00dbe, 0xc9e7a512, 0xc7e3744b, 0xc5e3a3a9, 0xc3e85b18, 0xc1f1c224, + 0xc0000000, 0xbe133b7c, + 0xbc2b9b05, 0xba4944a2, 0xb86c5df0, 0xb6950c1e, 0xb4c373ee, 0xb2f7b9af, + 0xb1320139, 0xaf726def, + 0xadb922b7, 0xac0641fb, 0xaa59eda4, 0xa8b4471a, 0xa7156f3c, 0xa57d8666, + 0xa3ecac65, 0xa263007d, + 0xa0e0a15f, 0x9f65ad2d, 0x9df24175, 0x9c867b2c, 0x9b2276b0, 0x99c64fc5, + 0x98722192, 0x9726069c, + 0x95e218c9, 0x94a6715d, 0x937328f5, 0x92485786, 0x9126145f, 0x900c7621, + 0x8efb92c2, 0x8df37f8b, + 0x8cf45113, 0x8bfe1b3f, 0x8b10f144, 0x8a2ce59f, 0x89520a1a, 0x88806fc4, + 0x87b826f7, 0x86f93f50, + 0x8643c7b3, 0x8597ce46, 0x84f56073, 0x845c8ae3, 0x83cd5982, 0x8347d77b, + 0x82cc0f36, 0x825a0a5b, + 0x81f1d1ce, 0x81936daf, 0x813ee55b, 0x80f43f69, 0x80b381ac, 0x807cb130, + 0x804fd23a, 0x802ce84c, + 0x8013f61d, 0x8004fda0, 0x80000000, 0x8004fda0, 0x8013f61d, 0x802ce84c, + 0x804fd23a, 0x807cb130, + 0x80b381ac, 0x80f43f69, 0x813ee55b, 0x81936daf, 0x81f1d1ce, 0x825a0a5b, + 0x82cc0f36, 0x8347d77b, + 0x83cd5982, 0x845c8ae3, 0x84f56073, 0x8597ce46, 0x8643c7b3, 0x86f93f50, + 0x87b826f7, 0x88806fc4, + 0x89520a1a, 0x8a2ce59f, 0x8b10f144, 0x8bfe1b3f, 0x8cf45113, 0x8df37f8b, + 0x8efb92c2, 0x900c7621, + 0x9126145f, 0x92485786, 0x937328f5, 0x94a6715d, 0x95e218c9, 0x9726069c, + 0x98722192, 0x99c64fc5, + 0x9b2276b0, 0x9c867b2c, 0x9df24175, 0x9f65ad2d, 0xa0e0a15f, 0xa263007d, + 0xa3ecac65, 0xa57d8666, + 0xa7156f3c, 0xa8b4471a, 0xaa59eda4, 0xac0641fb, 0xadb922b7, 0xaf726def, + 0xb1320139, 0xb2f7b9af, + 0xb4c373ee, 0xb6950c1e, 0xb86c5df0, 0xba4944a2, 0xbc2b9b05, 0xbe133b7c, + 0xc0000000, 0xc1f1c224, + 0xc3e85b18, 0xc5e3a3a9, 0xc7e3744b, 0xc9e7a512, 0xcbf00dbe, 0xcdfc85bb, + 0xd00ce422, 0xd220ffc0, + 0xd438af17, 0xd653c860, 0xd8722192, 0xda939061, 0xdcb7ea46, 0xdedf047d, + 0xe108b40d, 0xe334cdc9, + 0xe5632654, 0xe7939223, 0xe9c5e582, 0xebf9f498, 0xee2f9369, 0xf06695da, + 0xf29ecfb2, 0xf4d814a4, + 0xf7123849, 0xf94d0e2e, 0xfb8869ce, 0xfdc41e9b, 0x0, 0x23be165, 0x4779632, + 0x6b2f1d2, + 0x8edc7b7, 0xb27eb5c, 0xd61304e, 0xf996a26, 0x11d06c97, 0x14060b68, + 0x163a1a7e, 0x186c6ddd, + 0x1a9cd9ac, 0x1ccb3237, 0x1ef74bf3, 0x2120fb83, 0x234815ba, 0x256c6f9f, + 0x278dde6e, 0x29ac37a0, + 0x2bc750e9, 0x2ddf0040, 0x2ff31bde, 0x32037a45, 0x340ff242, 0x36185aee, + 0x381c8bb5, 0x3a1c5c57, + 0x3c17a4e8, 0x3e0e3ddc, 0x40000000, 0x41ecc484, 0x43d464fb, 0x45b6bb5e, + 0x4793a210, 0x496af3e2, + 0x4b3c8c12, 0x4d084651, 0x4ecdfec7, 0x508d9211, 0x5246dd49, 0x53f9be05, + 0x55a6125c, 0x574bb8e6, + 0x58ea90c4, 0x5a82799a, 0x5c13539b, 0x5d9cff83, 0x5f1f5ea1, 0x609a52d3, + 0x620dbe8b, 0x637984d4, + 0x64dd8950, 0x6639b03b, 0x678dde6e, 0x68d9f964, 0x6a1de737, 0x6b598ea3, + 0x6c8cd70b, 0x6db7a87a, + 0x6ed9eba1, 0x6ff389df, 0x71046d3e, 0x720c8075, 0x730baeed, 0x7401e4c1, + 0x74ef0ebc, 0x75d31a61, + 0x76adf5e6, 0x777f903c, 0x7847d909, 0x7906c0b0, 0x79bc384d, 0x7a6831ba, + 0x7b0a9f8d, 0x7ba3751d, + 0x7c32a67e, 0x7cb82885, 0x7d33f0ca, 0x7da5f5a5, 0x7e0e2e32, 0x7e6c9251, + 0x7ec11aa5, 0x7f0bc097, + 0x7f4c7e54, 0x7f834ed0, 0x7fb02dc6, 0x7fd317b4, 0x7fec09e3, 0x7ffb0260, + 0x7fffffff, 0x7ffb0260, + 0x7fec09e3, 0x7fd317b4, 0x7fb02dc6, 0x7f834ed0, 0x7f4c7e54, 0x7f0bc097, + 0x7ec11aa5, 0x7e6c9251, + 0x7e0e2e32, 0x7da5f5a5, 0x7d33f0ca, 0x7cb82885, 0x7c32a67e, 0x7ba3751d, + 0x7b0a9f8d, 0x7a6831ba, + 0x79bc384d, 0x7906c0b0, 0x7847d909, 0x777f903c, 0x76adf5e6, 0x75d31a61, + 0x74ef0ebc, 0x7401e4c1, + 0x730baeed, 0x720c8075, 0x71046d3e, 0x6ff389df, 0x6ed9eba1, 0x6db7a87a, + 0x6c8cd70b, 0x6b598ea3, + 0x6a1de737, 0x68d9f964, 0x678dde6e, 0x6639b03b, 0x64dd8950, 0x637984d4, + 0x620dbe8b, 0x609a52d3, + 0x5f1f5ea1, 0x5d9cff83, 0x5c13539b, 0x5a82799a, 0x58ea90c4, 0x574bb8e6, + 0x55a6125c, 0x53f9be05, + 0x5246dd49, 0x508d9211, 0x4ecdfec7, 0x4d084651, 0x4b3c8c12, 0x496af3e2, + 0x4793a210, 0x45b6bb5e, + 0x43d464fb, 0x41ecc484, 0x40000000, 0x3e0e3ddc, 0x3c17a4e8, 0x3a1c5c57, + 0x381c8bb5, 0x36185aee, + 0x340ff242, 0x32037a45, 0x2ff31bde, 0x2ddf0040, 0x2bc750e9, 0x29ac37a0, + 0x278dde6e, 0x256c6f9f, + 0x234815ba, 0x2120fb83, 0x1ef74bf3, 0x1ccb3237, 0x1a9cd9ac, 0x186c6ddd, + 0x163a1a7e, 0x14060b68, + 0x11d06c97, 0xf996a26, 0xd61304e, 0xb27eb5c, 0x8edc7b7, 0x6b2f1d2, + 0x4779632, 0x23be165, + + +}; + +/** +* \par +* Cosine Table is generated from following loop +* <pre>for(i = 0; i < 360; i++) +* { +* cosTable[i]= cos((i-180) * PI/180.0); +* } </pre> +* \par +* Convert above coefficients to fixed point 1.31 format. +*/ +static const int32_t cosTableQ31[360] = { + 0x80000000, 0x8004fda0, 0x8013f61d, 0x802ce84c, 0x804fd23a, 0x807cb130, + 0x80b381ac, 0x80f43f69, + 0x813ee55b, 0x81936daf, 0x81f1d1ce, 0x825a0a5b, 0x82cc0f36, 0x8347d77b, + 0x83cd5982, 0x845c8ae3, + 0x84f56073, 0x8597ce46, 0x8643c7b3, 0x86f93f50, 0x87b826f7, 0x88806fc4, + 0x89520a1a, 0x8a2ce59f, + 0x8b10f144, 0x8bfe1b3f, 0x8cf45113, 0x8df37f8b, 0x8efb92c2, 0x900c7621, + 0x9126145f, 0x92485786, + 0x937328f5, 0x94a6715d, 0x95e218c9, 0x9726069c, 0x98722192, 0x99c64fc5, + 0x9b2276b0, 0x9c867b2c, + 0x9df24175, 0x9f65ad2d, 0xa0e0a15f, 0xa263007d, 0xa3ecac65, 0xa57d8666, + 0xa7156f3c, 0xa8b4471a, + 0xaa59eda4, 0xac0641fb, 0xadb922b7, 0xaf726def, 0xb1320139, 0xb2f7b9af, + 0xb4c373ee, 0xb6950c1e, + 0xb86c5df0, 0xba4944a2, 0xbc2b9b05, 0xbe133b7c, 0xc0000000, 0xc1f1c224, + 0xc3e85b18, 0xc5e3a3a9, + 0xc7e3744b, 0xc9e7a512, 0xcbf00dbe, 0xcdfc85bb, 0xd00ce422, 0xd220ffc0, + 0xd438af17, 0xd653c860, + 0xd8722192, 0xda939061, 0xdcb7ea46, 0xdedf047d, 0xe108b40d, 0xe334cdc9, + 0xe5632654, 0xe7939223, + 0xe9c5e582, 0xebf9f498, 0xee2f9369, 0xf06695da, 0xf29ecfb2, 0xf4d814a4, + 0xf7123849, 0xf94d0e2e, + 0xfb8869ce, 0xfdc41e9b, 0x0, 0x23be165, 0x4779632, 0x6b2f1d2, 0x8edc7b7, + 0xb27eb5c, + 0xd61304e, 0xf996a26, 0x11d06c97, 0x14060b68, 0x163a1a7e, 0x186c6ddd, + 0x1a9cd9ac, 0x1ccb3237, + 0x1ef74bf3, 0x2120fb83, 0x234815ba, 0x256c6f9f, 0x278dde6e, 0x29ac37a0, + 0x2bc750e9, 0x2ddf0040, + 0x2ff31bde, 0x32037a45, 0x340ff242, 0x36185aee, 0x381c8bb5, 0x3a1c5c57, + 0x3c17a4e8, 0x3e0e3ddc, + 0x40000000, 0x41ecc484, 0x43d464fb, 0x45b6bb5e, 0x4793a210, 0x496af3e2, + 0x4b3c8c12, 0x4d084651, + 0x4ecdfec7, 0x508d9211, 0x5246dd49, 0x53f9be05, 0x55a6125c, 0x574bb8e6, + 0x58ea90c4, 0x5a82799a, + 0x5c13539b, 0x5d9cff83, 0x5f1f5ea1, 0x609a52d3, 0x620dbe8b, 0x637984d4, + 0x64dd8950, 0x6639b03b, + 0x678dde6e, 0x68d9f964, 0x6a1de737, 0x6b598ea3, 0x6c8cd70b, 0x6db7a87a, + 0x6ed9eba1, 0x6ff389df, + 0x71046d3e, 0x720c8075, 0x730baeed, 0x7401e4c1, 0x74ef0ebc, 0x75d31a61, + 0x76adf5e6, 0x777f903c, + 0x7847d909, 0x7906c0b0, 0x79bc384d, 0x7a6831ba, 0x7b0a9f8d, 0x7ba3751d, + 0x7c32a67e, 0x7cb82885, + 0x7d33f0ca, 0x7da5f5a5, 0x7e0e2e32, 0x7e6c9251, 0x7ec11aa5, 0x7f0bc097, + 0x7f4c7e54, 0x7f834ed0, + 0x7fb02dc6, 0x7fd317b4, 0x7fec09e3, 0x7ffb0260, 0x7fffffff, 0x7ffb0260, + 0x7fec09e3, 0x7fd317b4, + 0x7fb02dc6, 0x7f834ed0, 0x7f4c7e54, 0x7f0bc097, 0x7ec11aa5, 0x7e6c9251, + 0x7e0e2e32, 0x7da5f5a5, + 0x7d33f0ca, 0x7cb82885, 0x7c32a67e, 0x7ba3751d, 0x7b0a9f8d, 0x7a6831ba, + 0x79bc384d, 0x7906c0b0, + 0x7847d909, 0x777f903c, 0x76adf5e6, 0x75d31a61, 0x74ef0ebc, 0x7401e4c1, + 0x730baeed, 0x720c8075, + 0x71046d3e, 0x6ff389df, 0x6ed9eba1, 0x6db7a87a, 0x6c8cd70b, 0x6b598ea3, + 0x6a1de737, 0x68d9f964, + 0x678dde6e, 0x6639b03b, 0x64dd8950, 0x637984d4, 0x620dbe8b, 0x609a52d3, + 0x5f1f5ea1, 0x5d9cff83, + 0x5c13539b, 0x5a82799a, 0x58ea90c4, 0x574bb8e6, 0x55a6125c, 0x53f9be05, + 0x5246dd49, 0x508d9211, + 0x4ecdfec7, 0x4d084651, 0x4b3c8c12, 0x496af3e2, 0x4793a210, 0x45b6bb5e, + 0x43d464fb, 0x41ecc484, + 0x40000000, 0x3e0e3ddc, 0x3c17a4e8, 0x3a1c5c57, 0x381c8bb5, 0x36185aee, + 0x340ff242, 0x32037a45, + 0x2ff31bde, 0x2ddf0040, 0x2bc750e9, 0x29ac37a0, 0x278dde6e, 0x256c6f9f, + 0x234815ba, 0x2120fb83, + 0x1ef74bf3, 0x1ccb3237, 0x1a9cd9ac, 0x186c6ddd, 0x163a1a7e, 0x14060b68, + 0x11d06c97, 0xf996a26, + 0xd61304e, 0xb27eb5c, 0x8edc7b7, 0x6b2f1d2, 0x4779632, 0x23be165, 0x0, + 0xfdc41e9b, + 0xfb8869ce, 0xf94d0e2e, 0xf7123849, 0xf4d814a4, 0xf29ecfb2, 0xf06695da, + 0xee2f9369, 0xebf9f498, + 0xe9c5e582, 0xe7939223, 0xe5632654, 0xe334cdc9, 0xe108b40d, 0xdedf047d, + 0xdcb7ea46, 0xda939061, + 0xd8722192, 0xd653c860, 0xd438af17, 0xd220ffc0, 0xd00ce422, 0xcdfc85bb, + 0xcbf00dbe, 0xc9e7a512, + 0xc7e3744b, 0xc5e3a3a9, 0xc3e85b18, 0xc1f1c224, 0xc0000000, 0xbe133b7c, + 0xbc2b9b05, 0xba4944a2, + 0xb86c5df0, 0xb6950c1e, 0xb4c373ee, 0xb2f7b9af, 0xb1320139, 0xaf726def, + 0xadb922b7, 0xac0641fb, + 0xaa59eda4, 0xa8b4471a, 0xa7156f3c, 0xa57d8666, 0xa3ecac65, 0xa263007d, + 0xa0e0a15f, 0x9f65ad2d, + 0x9df24175, 0x9c867b2c, 0x9b2276b0, 0x99c64fc5, 0x98722192, 0x9726069c, + 0x95e218c9, 0x94a6715d, + 0x937328f5, 0x92485786, 0x9126145f, 0x900c7621, 0x8efb92c2, 0x8df37f8b, + 0x8cf45113, 0x8bfe1b3f, + 0x8b10f144, 0x8a2ce59f, 0x89520a1a, 0x88806fc4, 0x87b826f7, 0x86f93f50, + 0x8643c7b3, 0x8597ce46, + 0x84f56073, 0x845c8ae3, 0x83cd5982, 0x8347d77b, 0x82cc0f36, 0x825a0a5b, + 0x81f1d1ce, 0x81936daf, + 0x813ee55b, 0x80f43f69, 0x80b381ac, 0x807cb130, 0x804fd23a, 0x802ce84c, + 0x8013f61d, 0x8004fda0, + +}; + + +/** + * @brief Q31 sin_cos function. + * @param[in] theta scaled input value in degrees + * @param[out] *pSinVal points to the processed sine output. + * @param[out] *pCosVal points to the processed cosine output. + * @return none. + * + * The Q31 input value is in the range [-1 0.999999] and is mapped to a degree value in the range [-180 179]. + * + */ + + +void arm_sin_cos_q31( + q31_t theta, + q31_t * pSinVal, + q31_t * pCosVal) +{ + q31_t x0; /* Nearest input value */ + q31_t y0, y1; /* Nearest output values */ + q31_t xSpacing = INPUT_SPACING; /* Spaing between inputs */ + uint32_t i; /* Index */ + q31_t oneByXSpacing; /* 1/ xSpacing value */ + q31_t out; /* temporary variable */ + uint32_t sign_bits; /* No.of sign bits */ + uint32_t firstX = 0x80000000; /* First X value */ + + /* Calculation of index */ + i = ((uint32_t) theta - firstX) / (uint32_t) xSpacing; + + /* Checking min and max index of table */ + if(i >= 359) + { + i = 358; + } + + /* Calculation of first nearest input value */ + x0 = (q31_t) firstX + ((q31_t) i * xSpacing); + + /* Reading nearest sine output values from table */ + y0 = sinTableQ31[i]; + y1 = sinTableQ31[i + 1u]; + + /* Calculation of 1/(x1-x0) */ + /* (x1-x0) is xSpacing which is fixed value */ + sign_bits = 8u; + oneByXSpacing = 0x5A000000; + + /* Calculation of (theta - x0)/(x1-x0) */ + out = + (((q31_t) (((q63_t) (theta - x0) * oneByXSpacing) >> 32)) << sign_bits); + + /* Calculation of y0 + (y1 - y0) * ((theta - x0)/(x1-x0)) */ + *pSinVal = __QADD(y0, ((q31_t) (((q63_t) (y1 - y0) * out) >> 30))); + + /* Reading nearest cosine output values from table */ + y0 = cosTableQ31[i]; + y1 = cosTableQ31[i + 1u]; + + /* Calculation of y0 + (y1 - y0) * ((theta - x0)/(x1-x0)) */ + *pCosVal = __QADD(y0, ((q31_t) (((q63_t) (y1 - y0) * out) >> 30))); + +} + +/** + * @} end of SinCos group + */ |