mirror of https://github.com/thesofproject/sof.git
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>
This commit is contained in:
ShriramShastry
Liam Girdwood
parent
25a3fde242
commit
7de1744501
|
|
@ -33,10 +33,121 @@ typedef enum {
|
|||
void cordic_sin_cos(int32_t th_rad_fxp, cordic_cfg type, int32_t *sign, int32_t *b_yn, int32_t *xn,
|
||||
int32_t *th_cdc_fxp);
|
||||
/* Input is Q4.28, output is Q1.31 */
|
||||
int32_t sin_fixed_32b(int32_t th_rad_fxp);
|
||||
int32_t cos_fixed_32b(int32_t th_rad_fxp);
|
||||
/**
|
||||
* Compute fixed point cordicsine with table lookup and interpolation
|
||||
* The cordic sine algorithm converges, when the angle is in the range
|
||||
* [-pi/2, pi/2).If an angle is outside of this range, then a multiple of
|
||||
* pi/2 is added or subtracted from the angle until it is within the range
|
||||
* [-pi/2,pi/2).Start with the angle in the range [-2*pi, 2*pi) and output
|
||||
* has range in [-1.0 to 1.0]
|
||||
* +------------------+-----------------+--------+--------+
|
||||
* | thRadFxp | cdcsinth |thRadFxp|cdcsinth|
|
||||
* +----+-----+-------+----+----+-------+--------+--------+
|
||||
* |WLen| FLen|Signbit|WLen|FLen|Signbit| Qformat| Qformat|
|
||||
* +----+-----+-------+----+----+-------+--------+--------+
|
||||
* | 32 | 28 | 1 | 32 | 31 | 1 | 4.28 | 1.31 |
|
||||
* +------------------+-----------------+--------+--------+
|
||||
*/
|
||||
static inline int32_t sin_fixed_32b(int32_t th_rad_fxp)
|
||||
{
|
||||
int32_t sign;
|
||||
int32_t b_yn;
|
||||
int32_t xn;
|
||||
int32_t th_cdc_fxp;
|
||||
cordic_cfg type = EN_32B_CORDIC_SINE;
|
||||
cordic_sin_cos(th_rad_fxp, type, &sign, &b_yn, &xn, &th_cdc_fxp);
|
||||
|
||||
th_cdc_fxp = sign * b_yn;
|
||||
/*convert Q2.30 to Q1.31 format*/
|
||||
return sat_int32(Q_SHIFT_LEFT((int64_t)th_cdc_fxp, 30, 31));
|
||||
}
|
||||
/**
|
||||
* Compute fixed point cordicsine with table lookup and interpolation
|
||||
* The cordic cosine algorithm converges, when the angle is in the range
|
||||
* [-pi/2, pi/2).If an angle is outside of this range, then a multiple of
|
||||
* pi/2 is added or subtracted from the angle until it is within the range
|
||||
* [-pi/2,pi/2).Start with the angle in the range [-2*pi, 2*pi) and output
|
||||
* has range in [-1.0 to 1.0]
|
||||
* +------------------+-----------------+--------+--------+
|
||||
* | thRadFxp | cdccosth |thRadFxp|cdccosth|
|
||||
* +----+-----+-------+----+----+-------+--------+--------+
|
||||
* |WLen| FLen|Signbit|WLen|FLen|Signbit| Qformat| Qformat|
|
||||
* +----+-----+-------+----+----+-------+--------+--------+
|
||||
* | 32 | 28 | 1 | 32 | 31 | 1 | 4.28 | 1.31 |
|
||||
* +------------------+-----------------+--------+--------+
|
||||
*/
|
||||
static inline int32_t cos_fixed_32b(int32_t th_rad_fxp)
|
||||
{
|
||||
int32_t sign;
|
||||
int32_t b_yn;
|
||||
int32_t xn;
|
||||
int32_t th_cdc_fxp;
|
||||
cordic_cfg type = EN_32B_CORDIC_COSINE;
|
||||
cordic_sin_cos(th_rad_fxp, type, &sign, &b_yn, &xn, &th_cdc_fxp);
|
||||
|
||||
th_cdc_fxp = sign * xn;
|
||||
/*convert Q2.30 to Q1.31 format*/
|
||||
return sat_int32(Q_SHIFT_LEFT((int64_t)th_cdc_fxp, 30, 31));
|
||||
}
|
||||
|
||||
/* Input is Q4.28, output is Q1.15 */
|
||||
int16_t sin_fixed_16b(int32_t th_rad_fxp);
|
||||
int16_t cos_fixed_16b(int32_t th_rad_fxp);
|
||||
/**
|
||||
* Compute fixed point cordic sine with table lookup and interpolation
|
||||
* The cordic sine algorithm converges, when the angle is in the range
|
||||
* [-pi/2, pi/2).If an angle is outside of this range, then a multiple of
|
||||
* pi/2 is added or subtracted from the angle until it is within the range
|
||||
* [-pi/2,pi/2).Start with the angle in the range [-2*pi, 2*pi) and output
|
||||
* has range in [-1.0 to 1.0]
|
||||
* +------------------+-----------------+--------+------------+
|
||||
* | thRadFxp | cdcsinth |thRadFxp| cdcsinth|
|
||||
* +----+-----+-------+----+----+-------+--------+------------+
|
||||
* |WLen| FLen|Signbit|WLen|FLen|Signbit| Qformat| Qformat |
|
||||
* +----+-----+-------+----+----+-------+--------+------------+
|
||||
* | 32 | 28 | 1 | 32 | 15 | 1 | 4.28 | 1.15 |
|
||||
* +------------------+-----------------+--------+------------+
|
||||
*/
|
||||
static inline int16_t sin_fixed_16b(int32_t th_rad_fxp)
|
||||
{
|
||||
int32_t sign;
|
||||
int32_t b_yn;
|
||||
int32_t xn;
|
||||
int32_t th_cdc_fxp;
|
||||
cordic_cfg type = EN_16B_CORDIC_SINE;
|
||||
/* compute coeff from angles*/
|
||||
cordic_sin_cos(th_rad_fxp, type, &sign, &b_yn, &xn, &th_cdc_fxp);
|
||||
th_cdc_fxp = sign * b_yn;
|
||||
/*convert Q1.31 to Q1.15 format*/
|
||||
return sat_int16(Q_SHIFT_RND((sat_int32(Q_SHIFT_LEFT((int64_t)th_cdc_fxp, 30, 31))),
|
||||
31, 15));
|
||||
}
|
||||
/**
|
||||
* Compute fixed point cordic cosine with table lookup and interpolation
|
||||
* The cordic cos algorithm converges, when the angle is in the range
|
||||
* [-pi/2, pi/2).If an angle is outside of this range, then a multiple of
|
||||
* pi/2 is added or subtracted from the angle until it is within the range
|
||||
* [-pi/2,pi/2).Start with the angle in the range [-2*pi, 2*pi) and output
|
||||
* has range in [-1.0 to 1.0]
|
||||
* +------------------+-----------------+--------+------------+
|
||||
* | thRadFxp | cdccosth |thRadFxp| cdccosth|
|
||||
* +----+-----+-------+----+----+-------+--------+------------+
|
||||
* |WLen| FLen|Signbit|WLen|FLen|Signbit| Qformat| Qformat |
|
||||
* +----+-----+-------+----+----+-------+--------+------------+
|
||||
* | 32 | 28 | 1 | 32 | 15 | 1 | 4.28 | 1.15 |
|
||||
* +------------------+-----------------+--------+------------+
|
||||
*/
|
||||
static inline int16_t cos_fixed_16b(int32_t th_rad_fxp)
|
||||
{
|
||||
int32_t sign;
|
||||
int32_t b_yn;
|
||||
int32_t xn;
|
||||
int32_t th_cdc_fxp;
|
||||
cordic_cfg type = EN_16B_CORDIC_COSINE;
|
||||
/* compute coeff from angles*/
|
||||
cordic_sin_cos(th_rad_fxp, type, &sign, &b_yn, &xn, &th_cdc_fxp);
|
||||
th_cdc_fxp = sign * xn;
|
||||
/*convert Q1.31 to Q1.15 format*/
|
||||
return sat_int16(Q_SHIFT_RND((sat_int32(Q_SHIFT_LEFT((int64_t)th_cdc_fxp, 30, 31))),
|
||||
31, 15));
|
||||
}
|
||||
|
||||
#endif /* __SOF_MATH_TRIG_H__ */
|
||||
|
|
|
|||
122
src/math/trig.c
122
src/math/trig.c
|
|
@ -14,9 +14,10 @@
|
|||
#ifdef CONFIG_CORDIC_TRIGONOMETRY_FIXED
|
||||
/* Use a local definition to avoid adding a dependency on <math.h> */
|
||||
#define _M_PI 3.14159265358979323846 /* pi */
|
||||
/*cordic_atan2_lookup_table = atan(2.^-(0:N-1)) N = 31/16
|
||||
*CORDIC Gain is cordic_gain = prod(sqrt(1 + 2.^(-2*(0:31/16-1))))
|
||||
*Inverse CORDIC Gain,inverse_cordic_gain = 1 / cordic_gain
|
||||
/**
|
||||
* \cordic_atan2_lookup_table = atan(2.^-(0:N-1)) N = 31/16
|
||||
* \CORDIC Gain is cordic_gain = prod(sqrt(1 + 2.^(-2*(0:31/16-1))))
|
||||
* \Inverse CORDIC Gain,inverse_cordic_gain = 1 / cordic_gain
|
||||
*/
|
||||
static const int32_t cordic_lookup[CORDIC_31B_TABLE_SIZE] = { 843314857, 497837829,
|
||||
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);
|
|||
/* 843314857, deg = 90.000000 */
|
||||
const int32_t cord_sincos_piovertwo_q29fl = Q_CONVERT_FLOAT(_M_PI / 2, 29);
|
||||
|
||||
/**
|
||||
* \Compute fixed point cordicsine with table lookup and interpolation
|
||||
* \The cordic sine algorithm converges, when the angle is in the range
|
||||
* \[-pi/2, pi/2).If an angle is outside of this range, then a multiple of
|
||||
* \pi/2 is added or subtracted from the angle until it is within the range
|
||||
* \[-pi/2,pi/2).Start with the angle in the range [-2*pi, 2*pi) and output
|
||||
* \has range in [-1.0 to 1.0]
|
||||
* \+------------------+-----------------+--------+--------+
|
||||
* \| thRadFxp | cdcsinth |thRadFxp|cdcsinth|
|
||||
* \+----+-----+-------+----+----+-------+--------+--------+
|
||||
* \|WLen| FLen|Signbit|WLen|FLen|Signbit| Qformat| Qformat|
|
||||
* \+----+-----+-------+----+----+-------+--------+--------+
|
||||
* \| 32 | 28 | 1 | 32 | 31 | 1 | 4.28 | 1.31 |
|
||||
* \+------------------+-----------------+--------+--------+
|
||||
*/
|
||||
inline int32_t sin_fixed_32b(int32_t th_rad_fxp)
|
||||
{
|
||||
int32_t sign;
|
||||
int32_t b_yn;
|
||||
int32_t xn;
|
||||
int32_t th_cdc_fxp;
|
||||
cordic_cfg type = EN_32B_CORDIC_SINE;
|
||||
cordic_sin_cos(th_rad_fxp, type, &sign, &b_yn, &xn, &th_cdc_fxp);
|
||||
th_cdc_fxp = sign * b_yn;
|
||||
/*convert Q2.30 to Q1.31 format*/
|
||||
return sat_int32(Q_SHIFT_LEFT((int64_t)th_cdc_fxp, 30, 31));
|
||||
}
|
||||
/**
|
||||
* \Compute fixed point cordicsine with table lookup and interpolation
|
||||
* \The cordic cosine algorithm converges, when the angle is in the range
|
||||
* \[-pi/2, pi/2).If an angle is outside of this range, then a multiple of
|
||||
* \pi/2 is added or subtracted from the angle until it is within the range
|
||||
* \[-pi/2,pi/2).Start with the angle in the range [-2*pi, 2*pi) and output
|
||||
* \has range in [-1.0 to 1.0]
|
||||
* \+------------------+-----------------+--------+--------+
|
||||
* \| thRadFxp | cdccosth |thRadFxp|cdccosth|
|
||||
* \+----+-----+-------+----+----+-------+--------+--------+
|
||||
* \|WLen| FLen|Signbit|WLen|FLen|Signbit| Qformat| Qformat|
|
||||
* \+----+-----+-------+----+----+-------+--------+--------+
|
||||
* \| 32 | 28 | 1 | 32 | 31 | 1 | 4.28 | 1.31 |
|
||||
* \+------------------+-----------------+--------+--------+
|
||||
*/
|
||||
inline int32_t cos_fixed_32b(int32_t th_rad_fxp)
|
||||
{
|
||||
int32_t sign;
|
||||
int32_t b_yn;
|
||||
int32_t xn;
|
||||
int32_t th_cdc_fxp;
|
||||
cordic_cfg type = EN_32B_CORDIC_COSINE;
|
||||
cordic_sin_cos(th_rad_fxp, type, &sign, &b_yn, &xn, &th_cdc_fxp);
|
||||
th_cdc_fxp = sign * xn;
|
||||
/*convert Q2.30 to Q1.31 format*/
|
||||
return sat_int32(Q_SHIFT_LEFT((int64_t)th_cdc_fxp, 30, 31));
|
||||
}
|
||||
|
||||
/**
|
||||
* \Compute fixed point cordic sine with table lookup and interpolation
|
||||
* \The cordic sine algorithm converges, when the angle is in the range
|
||||
* \[-pi/2, pi/2).If an angle is outside of this range, then a multiple of
|
||||
* \pi/2 is added or subtracted from the angle until it is within the range
|
||||
* \[-pi/2,pi/2).Start with the angle in the range [-2*pi, 2*pi) and output
|
||||
* \has range in [-1.0 to 1.0]
|
||||
* \+------------------+-----------------+--------+------------+
|
||||
* \| thRadFxp | cdcsinth |thRadFxp| cdcsinth|
|
||||
* \+----+-----+-------+----+----+-------+--------+------------+
|
||||
* \|WLen| FLen|Signbit|WLen|FLen|Signbit| Qformat| Qformat |
|
||||
* \+----+-----+-------+----+----+-------+--------+------------+
|
||||
* \| 32 | 28 | 1 | 32 | 15 | 1 | 4.28 | 1.15 |
|
||||
* \+------------------+-----------------+--------+------------+
|
||||
*/
|
||||
inline int16_t sin_fixed_16b(int32_t th_rad_fxp)
|
||||
{
|
||||
int32_t sign;
|
||||
int32_t b_yn;
|
||||
int32_t xn;
|
||||
int32_t th_cdc_fxp;
|
||||
cordic_cfg type = EN_16B_CORDIC_SINE;
|
||||
/* compute coeff from angles*/
|
||||
cordic_sin_cos(th_rad_fxp, type, &sign, &b_yn, &xn, &th_cdc_fxp);
|
||||
th_cdc_fxp = sign * b_yn;
|
||||
/*convert Q1.31 to Q1.15 format*/
|
||||
return sat_int16(Q_SHIFT_RND((sat_int32(Q_SHIFT_LEFT((int64_t)th_cdc_fxp, 30, 31))),
|
||||
31, 15));
|
||||
}
|
||||
|
||||
/**
|
||||
* \Compute fixed point cordic cosine with table lookup and interpolation
|
||||
* \The cordic cos algorithm converges, when the angle is in the range
|
||||
* \[-pi/2, pi/2).If an angle is outside of this range, then a multiple of
|
||||
* \pi/2 is added or subtracted from the angle until it is within the range
|
||||
* \[-pi/2,pi/2).Start with the angle in the range [-2*pi, 2*pi) and output
|
||||
* \has range in [-1.0 to 1.0]
|
||||
* \+------------------+-----------------+--------+------------+
|
||||
* \| thRadFxp | cdccosth |thRadFxp| cdccosth|
|
||||
* \+----+-----+-------+----+----+-------+--------+------------+
|
||||
* \|WLen| FLen|Signbit|WLen|FLen|Signbit| Qformat| Qformat |
|
||||
* \+----+-----+-------+----+----+-------+--------+------------+
|
||||
* \| 32 | 28 | 1 | 32 | 15 | 1 | 4.28 | 1.15 |
|
||||
* \+------------------+-----------------+--------+------------+
|
||||
*/
|
||||
inline int16_t cos_fixed_16b(int32_t th_rad_fxp)
|
||||
{
|
||||
int32_t sign;
|
||||
int32_t b_yn;
|
||||
int32_t xn;
|
||||
int32_t th_cdc_fxp;
|
||||
cordic_cfg type = EN_16B_CORDIC_COSINE;
|
||||
/* compute coeff from angles*/
|
||||
cordic_sin_cos(th_rad_fxp, type, &sign, &b_yn, &xn, &th_cdc_fxp);
|
||||
th_cdc_fxp = sign * xn;
|
||||
/*convert Q1.31 to Q1.15 format*/
|
||||
return sat_int16(Q_SHIFT_RND((sat_int32(Q_SHIFT_LEFT((int64_t)th_cdc_fxp, 30, 31))),
|
||||
31, 15));
|
||||
}
|
||||
|
||||
/**
|
||||
* \CORDIC-based approximation of sine and cosine
|
||||
*/
|
||||
|
|
|
|||
Loading…
Reference in New Issue