|
|
@@ -79,24 +79,24 @@ static int32_t taylor(uint32_t ph) |
|
|
|
{ |
|
|
|
int32_t angle, sum, p1, p2, p3, p5, p7, p9, p11; |
|
|
|
|
|
|
|
if (ph >= 0xC0000000 || ph < 0x40000000) { |
|
|
|
if (ph >= 0xC0000000 || ph < 0x40000000) { // ph: 0.32 |
|
|
|
angle = (int32_t)ph; // valid from -90 to +90 degrees |
|
|
|
} else { |
|
|
|
angle = (int32_t)(0x80000000u - ph); |
|
|
|
angle = (int32_t)(0x80000000u - ph); // angle: 2.30 |
|
|
|
} |
|
|
|
p1 = multiply_32x32_rshift32_rounded(angle << 1, 1686629713); |
|
|
|
p2 = multiply_32x32_rshift32_rounded(p1, p1) << 3; |
|
|
|
p3 = multiply_32x32_rshift32_rounded(p2, p1) << 3; |
|
|
|
sum = multiply_subtract_32x32_rshift32_rounded(p1 << 1, p3, 1431655765); |
|
|
|
p5 = multiply_32x32_rshift32_rounded(p3, p2) << 1; |
|
|
|
p1 = multiply_32x32_rshift32_rounded(angle, 1686629713) << 2; // p1: 2.30 |
|
|
|
p2 = multiply_32x32_rshift32_rounded(p1, p1) << 1; // p2: 3.29 |
|
|
|
p3 = multiply_32x32_rshift32_rounded(p2, p1) << 2; // p3: 3.29 |
|
|
|
sum = multiply_subtract_32x32_rshift32_rounded(p1, p3, 1431655765); // sum: 2.30 |
|
|
|
p5 = multiply_32x32_rshift32_rounded(p3, p2) << 1; // p5: 5.27 |
|
|
|
sum = multiply_accumulate_32x32_rshift32_rounded(sum, p5, 286331153); |
|
|
|
p7 = multiply_32x32_rshift32_rounded(p5, p2); |
|
|
|
p7 = multiply_32x32_rshift32_rounded(p5, p2); // p7: 8.24 |
|
|
|
sum = multiply_subtract_32x32_rshift32_rounded(sum, p7, 54539267); |
|
|
|
p9 = multiply_32x32_rshift32_rounded(p7, p2); |
|
|
|
p9 = multiply_32x32_rshift32_rounded(p7, p2); // p9: 11.21 |
|
|
|
sum = multiply_accumulate_32x32_rshift32_rounded(sum, p9, 6059919); |
|
|
|
p11 = multiply_32x32_rshift32_rounded(p9, p2); |
|
|
|
p11 = multiply_32x32_rshift32_rounded(p9, p2); // p11: 14.18 |
|
|
|
sum = multiply_subtract_32x32_rshift32_rounded(sum, p11, 440721); |
|
|
|
return sum <<= 1; |
|
|
|
return sum <<= 1; // return: 1.31 |
|
|
|
} |
|
|
|
#endif |
|
|
|
|