Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-23 08:15:30 +03:00
Code Issues Wiki Activity

Added #define for imu debug output (+16 squashed commit)

Browse source 
Squashed local commits:

from       : e4265d4a13f63f82d5cf55eea2c091622f96660b
up to (inc): 72416dc74745fa8bae1aded79aa4b9ed0e389076
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2015-07-10 15:02:38 +10:00 • committed by ProDrone
parent c6f5b98a79
commit 45a4f11f92
21 changed files with 823 additions and 188 deletions
Download patch file Download diff file Expand all files Collapse all files

49
src/main/common/maths.c
View file

@ -21,12 +21,15 @@
#include "axis.h"
#include "maths.h"
#if defined(FAST_MATH) || defined(VERY_FAST_MATH)
#if defined(VERY_FAST_MATH)
// http://lolengine.net/blog/2011/12/21/better-function-approximations
// Chebyshev http://stackoverflow.com/questions/345085/how-do-trigonometric-functions-work/345117#345117
// Thanks for ledvinap for making such accuracy possible! See: https://github.com/cleanflight/cleanflight/issues/940#issuecomment-110323384
// https://github.com/Crashpilot1000/HarakiriWebstore1/blob/master/src/mw.c#L1235
#if defined(FAST_TRIGONOMETRY) || defined(EVEN_FASTER_TRIGONOMETRY)
#if defined(EVEN_FASTER_TRIGONOMETRY)
// sin_approx maximum absolute error = 2.305023e-06
// cos_approx maximum absolute error = 2.857298e-06
#define sinPolyCoef3 -1.666568107e-1f
#define sinPolyCoef5 8.312366210e-3f
#define sinPolyCoef7 -1.849218155e-4f
@ -37,7 +40,6 @@
#define sinPolyCoef7 -1.980661520e-4f // Double: -1.980661520135080504411629636078917643846e-4
#define sinPolyCoef9 2.600054768e-6f // Double: 2.600054767890361277123254766503271638682e-6
#endif
float sin_approx(float x)
{
int32_t xint = x;
@ -54,6 +56,47 @@ float cos_approx(float x)
{
return sin_approx(x + (0.5f * M_PIf));
}
// Initial implementation by Crashpilot1000 (https://github.com/Crashpilot1000/HarakiriWebstore1/blob/396715f73c6fcf859e0db0f34e12fe44bace6483/src/mw.c#L1292)
// Polynomial coefficients by Andor (http://www.dsprelated.com/showthread/comp.dsp/21872-1.php) optimized by Ledvinap to save one multiplication
// Max absolute error 0,000027 degree
// atan2_approx maximum absolute error = 7.152557e-07 rads (4.098114e-05 degree)
float atan2_approx(float y, float x)
{
#define atanPolyCoef1 3.14551665884836e-07f
#define atanPolyCoef2 0.99997356613987f
#define atanPolyCoef3 0.14744007058297684f
#define atanPolyCoef4 0.3099814292351353f
#define atanPolyCoef5 0.05030176425872175f
#define atanPolyCoef6 0.1471039133652469f
#define atanPolyCoef7 0.6444640676891548f
float res, absX, absY;
absX = fabsf(x);
absY = fabsf(y);
res = MAX(absX, absY);
if (res) res = MIN(absX, absY) / res;
else res = 0.0f;
res = -((((atanPolyCoef5 * res - atanPolyCoef4) * res - atanPolyCoef3) * res - atanPolyCoef2) * res - atanPolyCoef1) / ((atanPolyCoef7 * res + atanPolyCoef6) * res + 1.0f);
if (absY > absX) res = (M_PIf / 2.0f) - res;
if (x < 0) res = M_PIf - res;
if (y < 0) res = -res;
return res;
}
// http://http.developer.nvidia.com/Cg/acos.html
// Handbook of Mathematical Functions
// M. Abramowitz and I.A. Stegun, Ed.
// acos_approx maximum absolute error = 6.760856e-05 rads (3.873685e-03 degree)
float acos_approx(float x)
{
float xa = fabsf(x);
float result = sqrtf(1.0f - xa) * (1.5707288f + xa * (-0.2121144f + xa * (0.0742610f + (-0.0187293f * xa))));
if (x < 0.0f)
return M_PIf - result;
else
return result;
}
#endif
int32_t applyDeadband(int32_t value, int32_t deadband)