Skip to content

Commit 7de1744

Browse files
ShriramShastrylgirdwood
ShriramShastry
authored and
lgirdwood
committed
Math: Trignometry: Cordic sine() and cos()
Add static inline for math sin() and cos() functions.This PR is just moving existing code to improve runtime performance due to less function calls Signed-off-by: ShriramShastry <malladi.sastry@intel.com>
1 parent 25a3fde commit 7de1744

2 files changed

Lines changed: 119 additions & 122 deletions

File tree

‎src/include/sof/math/trig.h‎

Lines changed: 115 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -33,10 +33,121 @@ typedef enum {
3333
void cordic_sin_cos(int32_t th_rad_fxp, cordic_cfg type, int32_t *sign, int32_t *b_yn, int32_t *xn,
3434
int32_t *th_cdc_fxp);
3535
/* Input is Q4.28, output is Q1.31 */
36-
int32_t sin_fixed_32b(int32_t th_rad_fxp);
37-
int32_t cos_fixed_32b(int32_t th_rad_fxp);
36+
/**
37+
* Compute fixed point cordicsine with table lookup and interpolation
38+
* The cordic sine algorithm converges, when the angle is in the range
39+
* [-pi/2, pi/2).If an angle is outside of this range, then a multiple of
40+
* pi/2 is added or subtracted from the angle until it is within the range
41+
* [-pi/2,pi/2).Start with the angle in the range [-2*pi, 2*pi) and output
42+
* has range in [-1.0 to 1.0]
43+
* +------------------+-----------------+--------+--------+
44+
* | thRadFxp | cdcsinth |thRadFxp|cdcsinth|
45+
* +----+-----+-------+----+----+-------+--------+--------+
46+
* |WLen| FLen|Signbit|WLen|FLen|Signbit| Qformat| Qformat|
47+
* +----+-----+-------+----+----+-------+--------+--------+
48+
* | 32 | 28 | 1 | 32 | 31 | 1 | 4.28 | 1.31 |
49+
* +------------------+-----------------+--------+--------+
50+
*/
51+
static inline int32_t sin_fixed_32b(int32_t th_rad_fxp)
52+
{
53+
int32_t sign;
54+
int32_t b_yn;
55+
int32_t xn;
56+
int32_t th_cdc_fxp;
57+
cordic_cfg type = EN_32B_CORDIC_SINE;
58+
cordic_sin_cos(th_rad_fxp, type, &sign, &b_yn, &xn, &th_cdc_fxp);
59+
60+
th_cdc_fxp = sign * b_yn;
61+
/*convert Q2.30 to Q1.31 format*/
62+
return sat_int32(Q_SHIFT_LEFT((int64_t)th_cdc_fxp, 30, 31));
63+
}
64+
/**
65+
* Compute fixed point cordicsine with table lookup and interpolation
66+
* The cordic cosine algorithm converges, when the angle is in the range
67+
* [-pi/2, pi/2).If an angle is outside of this range, then a multiple of
68+
* pi/2 is added or subtracted from the angle until it is within the range
69+
* [-pi/2,pi/2).Start with the angle in the range [-2*pi, 2*pi) and output
70+
* has range in [-1.0 to 1.0]
71+
* +------------------+-----------------+--------+--------+
72+
* | thRadFxp | cdccosth |thRadFxp|cdccosth|
73+
* +----+-----+-------+----+----+-------+--------+--------+
74+
* |WLen| FLen|Signbit|WLen|FLen|Signbit| Qformat| Qformat|
75+
* +----+-----+-------+----+----+-------+--------+--------+
76+
* | 32 | 28 | 1 | 32 | 31 | 1 | 4.28 | 1.31 |
77+
* +------------------+-----------------+--------+--------+
78+
*/
79+
static inline int32_t cos_fixed_32b(int32_t th_rad_fxp)
80+
{
81+
int32_t sign;
82+
int32_t b_yn;
83+
int32_t xn;
84+
int32_t th_cdc_fxp;
85+
cordic_cfg type = EN_32B_CORDIC_COSINE;
86+
cordic_sin_cos(th_rad_fxp, type, &sign, &b_yn, &xn, &th_cdc_fxp);
87+
88+
th_cdc_fxp = sign * xn;
89+
/*convert Q2.30 to Q1.31 format*/
90+
return sat_int32(Q_SHIFT_LEFT((int64_t)th_cdc_fxp, 30, 31));
91+
}
92+
3893
/* Input is Q4.28, output is Q1.15 */
39-
int16_t sin_fixed_16b(int32_t th_rad_fxp);
40-
int16_t cos_fixed_16b(int32_t th_rad_fxp);
94+
/**
95+
* Compute fixed point cordic sine with table lookup and interpolation
96+
* The cordic sine algorithm converges, when the angle is in the range
97+
* [-pi/2, pi/2).If an angle is outside of this range, then a multiple of
98+
* pi/2 is added or subtracted from the angle until it is within the range
99+
* [-pi/2,pi/2).Start with the angle in the range [-2*pi, 2*pi) and output
100+
* has range in [-1.0 to 1.0]
101+
* +------------------+-----------------+--------+------------+
102+
* | thRadFxp | cdcsinth |thRadFxp| cdcsinth|
103+
* +----+-----+-------+----+----+-------+--------+------------+
104+
* |WLen| FLen|Signbit|WLen|FLen|Signbit| Qformat| Qformat |
105+
* +----+-----+-------+----+----+-------+--------+------------+
106+
* | 32 | 28 | 1 | 32 | 15 | 1 | 4.28 | 1.15 |
107+
* +------------------+-----------------+--------+------------+
108+
*/
109+
static inline int16_t sin_fixed_16b(int32_t th_rad_fxp)
110+
{
111+
int32_t sign;
112+
int32_t b_yn;
113+
int32_t xn;
114+
int32_t th_cdc_fxp;
115+
cordic_cfg type = EN_16B_CORDIC_SINE;
116+
/* compute coeff from angles*/
117+
cordic_sin_cos(th_rad_fxp, type, &sign, &b_yn, &xn, &th_cdc_fxp);
118+
th_cdc_fxp = sign * b_yn;
119+
/*convert Q1.31 to Q1.15 format*/
120+
return sat_int16(Q_SHIFT_RND((sat_int32(Q_SHIFT_LEFT((int64_t)th_cdc_fxp, 30, 31))),
121+
31, 15));
122+
}
123+
/**
124+
* Compute fixed point cordic cosine with table lookup and interpolation
125+
* The cordic cos algorithm converges, when the angle is in the range
126+
* [-pi/2, pi/2).If an angle is outside of this range, then a multiple of
127+
* pi/2 is added or subtracted from the angle until it is within the range
128+
* [-pi/2,pi/2).Start with the angle in the range [-2*pi, 2*pi) and output
129+
* has range in [-1.0 to 1.0]
130+
* +------------------+-----------------+--------+------------+
131+
* | thRadFxp | cdccosth |thRadFxp| cdccosth|
132+
* +----+-----+-------+----+----+-------+--------+------------+
133+
* |WLen| FLen|Signbit|WLen|FLen|Signbit| Qformat| Qformat |
134+
* +----+-----+-------+----+----+-------+--------+------------+
135+
* | 32 | 28 | 1 | 32 | 15 | 1 | 4.28 | 1.15 |
136+
* +------------------+-----------------+--------+------------+
137+
*/
138+
static inline int16_t cos_fixed_16b(int32_t th_rad_fxp)
139+
{
140+
int32_t sign;
141+
int32_t b_yn;
142+
int32_t xn;
143+
int32_t th_cdc_fxp;
144+
cordic_cfg type = EN_16B_CORDIC_COSINE;
145+
/* compute coeff from angles*/
146+
cordic_sin_cos(th_rad_fxp, type, &sign, &b_yn, &xn, &th_cdc_fxp);
147+
th_cdc_fxp = sign * xn;
148+
/*convert Q1.31 to Q1.15 format*/
149+
return sat_int16(Q_SHIFT_RND((sat_int32(Q_SHIFT_LEFT((int64_t)th_cdc_fxp, 30, 31))),
150+
31, 15));
151+
}
41152

42153
#endif /* __SOF_MATH_TRIG_H__ */

‎src/math/trig.c‎

Lines changed: 4 additions & 118 deletions
Original file line numberDiff line numberDiff line change
@@ -14,9 +14,10 @@
1414
#ifdef CONFIG_CORDIC_TRIGONOMETRY_FIXED
1515
/* Use a local definition to avoid adding a dependency on <math.h> */
1616
#define _M_PI 3.14159265358979323846 /* pi */
17-
/*cordic_atan2_lookup_table = atan(2.^-(0:N-1)) N = 31/16
18-
*CORDIC Gain is cordic_gain = prod(sqrt(1 + 2.^(-2*(0:31/16-1))))
19-
*Inverse CORDIC Gain,inverse_cordic_gain = 1 / cordic_gain
17+
/**
18+
* \cordic_atan2_lookup_table = atan(2.^-(0:N-1)) N = 31/16
19+
* \CORDIC Gain is cordic_gain = prod(sqrt(1 + 2.^(-2*(0:31/16-1))))
20+
* \Inverse CORDIC Gain,inverse_cordic_gain = 1 / cordic_gain
2021
*/
2122
static const int32_t cordic_lookup[CORDIC_31B_TABLE_SIZE] = { 843314857, 497837829,
2223
263043837, 133525159, 67021687, 33543516, 16775851, 8388437, 4194283, 2097149,
@@ -32,121 +33,6 @@ const int32_t cord_sincos_piovertwo_q28fl = Q_CONVERT_FLOAT(_M_PI / 2, 28);
3233
/* 843314857, deg = 90.000000 */
3334
const int32_t cord_sincos_piovertwo_q29fl = Q_CONVERT_FLOAT(_M_PI / 2, 29);
3435

35-
/**
36-
* \Compute fixed point cordicsine with table lookup and interpolation
37-
* \The cordic sine algorithm converges, when the angle is in the range
38-
* \[-pi/2, pi/2).If an angle is outside of this range, then a multiple of
39-
* \pi/2 is added or subtracted from the angle until it is within the range
40-
* \[-pi/2,pi/2).Start with the angle in the range [-2*pi, 2*pi) and output
41-
* \has range in [-1.0 to 1.0]
42-
* \+------------------+-----------------+--------+--------+
43-
* \| thRadFxp | cdcsinth |thRadFxp|cdcsinth|
44-
* \+----+-----+-------+----+----+-------+--------+--------+
45-
* \|WLen| FLen|Signbit|WLen|FLen|Signbit| Qformat| Qformat|
46-
* \+----+-----+-------+----+----+-------+--------+--------+
47-
* \| 32 | 28 | 1 | 32 | 31 | 1 | 4.28 | 1.31 |
48-
* \+------------------+-----------------+--------+--------+
49-
*/
50-
inline int32_t sin_fixed_32b(int32_t th_rad_fxp)
51-
{
52-
int32_t sign;
53-
int32_t b_yn;
54-
int32_t xn;
55-
int32_t th_cdc_fxp;
56-
cordic_cfg type = EN_32B_CORDIC_SINE;
57-
cordic_sin_cos(th_rad_fxp, type, &sign, &b_yn, &xn, &th_cdc_fxp);
58-
th_cdc_fxp = sign * b_yn;
59-
/*convert Q2.30 to Q1.31 format*/
60-
return sat_int32(Q_SHIFT_LEFT((int64_t)th_cdc_fxp, 30, 31));
61-
}
62-
/**
63-
* \Compute fixed point cordicsine with table lookup and interpolation
64-
* \The cordic cosine algorithm converges, when the angle is in the range
65-
* \[-pi/2, pi/2).If an angle is outside of this range, then a multiple of
66-
* \pi/2 is added or subtracted from the angle until it is within the range
67-
* \[-pi/2,pi/2).Start with the angle in the range [-2*pi, 2*pi) and output
68-
* \has range in [-1.0 to 1.0]
69-
* \+------------------+-----------------+--------+--------+
70-
* \| thRadFxp | cdccosth |thRadFxp|cdccosth|
71-
* \+----+-----+-------+----+----+-------+--------+--------+
72-
* \|WLen| FLen|Signbit|WLen|FLen|Signbit| Qformat| Qformat|
73-
* \+----+-----+-------+----+----+-------+--------+--------+
74-
* \| 32 | 28 | 1 | 32 | 31 | 1 | 4.28 | 1.31 |
75-
* \+------------------+-----------------+--------+--------+
76-
*/
77-
inline int32_t cos_fixed_32b(int32_t th_rad_fxp)
78-
{
79-
int32_t sign;
80-
int32_t b_yn;
81-
int32_t xn;
82-
int32_t th_cdc_fxp;
83-
cordic_cfg type = EN_32B_CORDIC_COSINE;
84-
cordic_sin_cos(th_rad_fxp, type, &sign, &b_yn, &xn, &th_cdc_fxp);
85-
th_cdc_fxp = sign * xn;
86-
/*convert Q2.30 to Q1.31 format*/
87-
return sat_int32(Q_SHIFT_LEFT((int64_t)th_cdc_fxp, 30, 31));
88-
}
89-
90-
/**
91-
* \Compute fixed point cordic sine with table lookup and interpolation
92-
* \The cordic sine algorithm converges, when the angle is in the range
93-
* \[-pi/2, pi/2).If an angle is outside of this range, then a multiple of
94-
* \pi/2 is added or subtracted from the angle until it is within the range
95-
* \[-pi/2,pi/2).Start with the angle in the range [-2*pi, 2*pi) and output
96-
* \has range in [-1.0 to 1.0]
97-
* \+------------------+-----------------+--------+------------+
98-
* \| thRadFxp | cdcsinth |thRadFxp| cdcsinth|
99-
* \+----+-----+-------+----+----+-------+--------+------------+
100-
* \|WLen| FLen|Signbit|WLen|FLen|Signbit| Qformat| Qformat |
101-
* \+----+-----+-------+----+----+-------+--------+------------+
102-
* \| 32 | 28 | 1 | 32 | 15 | 1 | 4.28 | 1.15 |
103-
* \+------------------+-----------------+--------+------------+
104-
*/
105-
inline int16_t sin_fixed_16b(int32_t th_rad_fxp)
106-
{
107-
int32_t sign;
108-
int32_t b_yn;
109-
int32_t xn;
110-
int32_t th_cdc_fxp;
111-
cordic_cfg type = EN_16B_CORDIC_SINE;
112-
/* compute coeff from angles*/
113-
cordic_sin_cos(th_rad_fxp, type, &sign, &b_yn, &xn, &th_cdc_fxp);
114-
th_cdc_fxp = sign * b_yn;
115-
/*convert Q1.31 to Q1.15 format*/
116-
return sat_int16(Q_SHIFT_RND((sat_int32(Q_SHIFT_LEFT((int64_t)th_cdc_fxp, 30, 31))),
117-
31, 15));
118-
}
119-
120-
/**
121-
* \Compute fixed point cordic cosine with table lookup and interpolation
122-
* \The cordic cos algorithm converges, when the angle is in the range
123-
* \[-pi/2, pi/2).If an angle is outside of this range, then a multiple of
124-
* \pi/2 is added or subtracted from the angle until it is within the range
125-
* \[-pi/2,pi/2).Start with the angle in the range [-2*pi, 2*pi) and output
126-
* \has range in [-1.0 to 1.0]
127-
* \+------------------+-----------------+--------+------------+
128-
* \| thRadFxp | cdccosth |thRadFxp| cdccosth|
129-
* \+----+-----+-------+----+----+-------+--------+------------+
130-
* \|WLen| FLen|Signbit|WLen|FLen|Signbit| Qformat| Qformat |
131-
* \+----+-----+-------+----+----+-------+--------+------------+
132-
* \| 32 | 28 | 1 | 32 | 15 | 1 | 4.28 | 1.15 |
133-
* \+------------------+-----------------+--------+------------+
134-
*/
135-
inline int16_t cos_fixed_16b(int32_t th_rad_fxp)
136-
{
137-
int32_t sign;
138-
int32_t b_yn;
139-
int32_t xn;
140-
int32_t th_cdc_fxp;
141-
cordic_cfg type = EN_16B_CORDIC_COSINE;
142-
/* compute coeff from angles*/
143-
cordic_sin_cos(th_rad_fxp, type, &sign, &b_yn, &xn, &th_cdc_fxp);
144-
th_cdc_fxp = sign * xn;
145-
/*convert Q1.31 to Q1.15 format*/
146-
return sat_int16(Q_SHIFT_RND((sat_int32(Q_SHIFT_LEFT((int64_t)th_cdc_fxp, 30, 31))),
147-
31, 15));
148-
}
149-
15036
/**
15137
* \CORDIC-based approximation of sine and cosine
15238
*/

0 commit comments

Comments
 (0)