42 lines
1.3 KiB
C++
42 lines
1.3 KiB
C++
#include <cstdint>
|
|
|
|
|
|
/*
|
|
Implements the 5-order polynomial approximation to sin(x).
|
|
@param i angle (with 2^15 units/circle)
|
|
@return 16 bit fixed point Sine value (4.12) (ie: +4096 = +1 & -4096 = -1)
|
|
|
|
The result is accurate to within +- 1 count. ie: +/-2.44e-4.
|
|
*/
|
|
int16_t fpsin(int16_t i)
|
|
{
|
|
/* Convert (signed) input to a value between 0 and 8192. (8192 is pi/2, which is the region of the curve fit). */
|
|
/* ------------------------------------------------------------------- */
|
|
i <<= 1;
|
|
uint8_t c = i<0; //set carry for output pos/neg
|
|
|
|
if(i == (i|0x4000)) // flip input value to corresponding value in range [0..8192)
|
|
i = (1<<15) - i;
|
|
i = (i & 0x7FFF) >> 1;
|
|
/* ------------------------------------------------------------------- */
|
|
|
|
/* The following section implements the formula:
|
|
= y * 2^-n * ( A1 - 2^(q-p)* y * 2^-n * y * 2^-n * [B1 - 2^-r * y * 2^-n * C1 * y]) * 2^(a-q)
|
|
Where the constants are defined as follows:
|
|
*/
|
|
enum {A1=3370945099UL, B1=2746362156UL, C1=292421UL};
|
|
enum {n=13, p=32, q=31, r=3, a=12};
|
|
|
|
uint32_t y = (C1*((uint32_t)i))>>n;
|
|
y = B1 - (((uint32_t)i*y)>>r);
|
|
y = (uint32_t)i * (y>>n);
|
|
y = (uint32_t)i * (y>>n);
|
|
y = A1 - (y>>(p-q));
|
|
y = (uint32_t)i * (y>>n);
|
|
y = (y+(1UL<<(q-a-1)))>>(q-a); // Rounding
|
|
|
|
return c ? -y : y;
|
|
}
|
|
|
|
//Cos(x) = sin(x + pi/2)
|
|
#define fpcos(i) fpsin((int16_t)(((uint16_t)(i)) + 8192U)) |