电机控制DSP高效算法——1、正余弦

• 控制系统常用查表法加速。该段代码为正余弦的查表法实现，查找表角度范围为0-90°，共1024点。
• 适用于32位浮点MCU，精度跟math.h库函数几乎一致。
• 同一流程，修改其中常数，也适用于定点数。

extern const float *sin90d1024f32;
static float sinfpu(float x) /* x must have been normalized by 2pi */
{
    float s, y, y1;
    long  i;
    
    x -= (long)x;                /* (-1, 1) */
    x += x<0.0f ? 1.0f : 0.0f;   /* [0, 1)  */
    
    if(x >= 0.5f) {
        x -= 0.5f; s = -1.0f;    /* [0.5, 1)  */
    } else {
        x += 0.0f; s = +1.0f;    /* [0, 0.5)  */
    }
    if(x >= 0.25f) {
        x = 0.5f - x;            /* [0.25, 0.5) */
    } else {
        x = x;                   /* [0, 0.25) */
    }
    
    x *= 4092;  // 4096;  //
    i = (long)x;
    y = sin90d1024f32[i];        /* look up table */
    y1 = sin90d1024f32[i+1];
    y += (y1 - y) * (x-i);       /* Linear interpolation */
    y *= s;                      /* Signed */
    
    return y;
}
float sinfacc(float x)
{
    x =x * 0.159154943f; /* x / 2pi */
    return sinfpu(x);
}
float cosfacc(float x)
{
    x = x * 0.159154943f + 0.25f;
    return sinfpu(x);
}