1 /* ----------------------------------------------------------------------
2 * Project: CMSIS DSP Library
3 * Title: arm_sin_cos_f32.c
4 * Description: Sine and Cosine calculation for floating-point values
5 *
6 * $Date: 23 April 2021
7 * $Revision: V1.9.0
8 *
9 * Target Processor: Cortex-M and Cortex-A cores
10 * -------------------------------------------------------------------- */
11 /*
12 * Copyright (C) 2010-2021 ARM Limited or its affiliates. All rights reserved.
13 *
14 * SPDX-License-Identifier: Apache-2.0
15 *
16 * Licensed under the Apache License, Version 2.0 (the License); you may
17 * not use this file except in compliance with the License.
18 * You may obtain a copy of the License at
19 *
20 * www.apache.org/licenses/LICENSE-2.0
21 *
22 * Unless required by applicable law or agreed to in writing, software
23 * distributed under the License is distributed on an AS IS BASIS, WITHOUT
24 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
25 * See the License for the specific language governing permissions and
26 * limitations under the License.
27 */
28
29 #include "dsp/controller_functions.h"
30 #include "arm_common_tables.h"
31
32 /**
33 @ingroup groupController
34 */
35
36 /**
37 @defgroup SinCos Sine Cosine
38
39 Computes the trigonometric sine and cosine values using a combination of table lookup
40 and linear interpolation.
41 There are separate functions for Q31 and floating-point data types.
42 The input to the floating-point version is in degrees while the
43 fixed-point Q31 have a scaled input with the range
44 [-1 0.9999] mapping to [-180 +180] degrees.
45
46 The floating point function also allows values that are out of the usual range. When this happens, the function will
47 take extra time to adjust the input value to the range of [-180 180].
48
49 The result is accurate to 5 digits after the decimal point.
50
51 The implementation is based on table lookup using 360 values together with linear interpolation.
52 The steps used are:
53 -# Calculation of the nearest integer table index.
54 -# Compute the fractional portion (fract) of the input.
55 -# Fetch the value corresponding to \c index from sine table to \c y0 and also value from \c index+1 to \c y1.
56 -# Sine value is computed as <code> *psinVal = y0 + (fract * (y1 - y0))</code>.
57 -# Fetch the value corresponding to \c index from cosine table to \c y0 and also value from \c index+1 to \c y1.
58 -# Cosine value is computed as <code> *pcosVal = y0 + (fract * (y1 - y0))</code>.
59 */
60
61 /**
62 @addtogroup SinCos
63 @{
64 */
65
66 /**
67 @brief Floating-point sin_cos function.
68 @param[in] theta input value in degrees
69 @param[out] pSinVal points to processed sine output
70 @param[out] pCosVal points to processed cosine output
71 @return none
72 */
73
arm_sin_cos_f32(float32_t theta,float32_t * pSinVal,float32_t * pCosVal)74 void arm_sin_cos_f32(
75 float32_t theta,
76 float32_t * pSinVal,
77 float32_t * pCosVal)
78 {
79 float32_t fract, in; /* Temporary input, output variables */
80 uint16_t indexS, indexC; /* Index variable */
81 float32_t f1, f2, d1, d2; /* Two nearest output values */
82 float32_t Dn, Df;
83 float32_t temp, findex;
84
85 /* input x is in degrees */
86 /* Scale input, divide input by 360, for cosine add 0.25 (pi/2) to read sine table */
87 in = theta * 0.00277777777778f;
88
89 if (in < 0.0f)
90 {
91 in = -in;
92 }
93
94 in = in - (int32_t)in;
95
96 /* Calculate the nearest index */
97 findex = (float32_t)FAST_MATH_TABLE_SIZE * in;
98 indexS = ((uint16_t)findex) & 0x1ff;
99 indexC = (indexS + (FAST_MATH_TABLE_SIZE / 4)) & 0x1ff;
100
101 /* Calculation of fractional value */
102 fract = findex - (float32_t) indexS;
103
104 /* Read two nearest values of input value from the cos & sin tables */
105 f1 = sinTable_f32[indexC ];
106 f2 = sinTable_f32[indexC+1];
107 d1 = -sinTable_f32[indexS ];
108 d2 = -sinTable_f32[indexS+1];
109
110 temp = (1.0f - fract) * f1 + fract * f2;
111
112 Dn = 0.0122718463030f; /* delta between the two points (fixed), in this case 2*pi/FAST_MATH_TABLE_SIZE */
113 Df = f2 - f1; /* delta between the values of the functions */
114
115 temp = Dn * (d1 + d2) - 2 * Df;
116 temp = fract * temp + (3 * Df - (d2 + 2 * d1) * Dn);
117 temp = fract * temp + d1 * Dn;
118
119 /* Calculation of cosine value */
120 *pCosVal = fract * temp + f1;
121
122 /* Read two nearest values of input value from the cos & sin tables */
123 f1 = sinTable_f32[indexS ];
124 f2 = sinTable_f32[indexS+1];
125 d1 = sinTable_f32[indexC ];
126 d2 = sinTable_f32[indexC+1];
127
128 temp = (1.0f - fract) * f1 + fract * f2;
129
130 Df = f2 - f1; // delta between the values of the functions
131 temp = Dn * (d1 + d2) - 2 * Df;
132 temp = fract * temp + (3 * Df - (d2 + 2 * d1) * Dn);
133 temp = fract * temp + d1 * Dn;
134
135 /* Calculation of sine value */
136 *pSinVal = fract * temp + f1;
137
138 if (theta < 0.0f)
139 {
140 *pSinVal = -*pSinVal;
141 }
142 }
143
144 /**
145 @} end of SinCos group
146 */
147