/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see .
*/
#include
#include
#include "axis.h"
#include "maths.h"
// 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)
#define sinPolyCoef3 -1.666568107e-1f
#define sinPolyCoef5 8.312366210e-3f
#define sinPolyCoef7 -1.849218155e-4f
#define sinPolyCoef9 0
#else
#define sinPolyCoef3 -1.666665710e-1f // Double: -1.666665709650470145824129400050267289858e-1
#define sinPolyCoef5 8.333017292e-3f // Double: 8.333017291562218127986291618761571373087e-3
#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;
if (xint < -32 || xint > 32) return 0.0f; // Stop here on error input (5 * 360 Deg)
while (x > M_PIf) x -= (2.0f * M_PIf); // always wrap input angle to -PI..PI
while (x < -M_PIf) x += (2.0f * M_PIf);
if (x > (0.5f * M_PIf)) x = (0.5f * M_PIf) - (x - (0.5f * M_PIf)); // We just pick -90..+90 Degree
else if (x < -(0.5f * M_PIf)) x = -(0.5f * M_PIf) - ((0.5f * M_PIf) + x);
float x2 = x * x;
return x + x * x2 * (sinPolyCoef3 + x2 * (sinPolyCoef5 + x2 * (sinPolyCoef7 + x2 * sinPolyCoef9)));
}
float cos_approx(float x)
{
return sin_approx(x + (0.5f * M_PIf));
}
#endif
int32_t applyDeadband(int32_t value, int32_t deadband)
{
if (ABS(value) < deadband) {
value = 0;
} else if (value > 0) {
value -= deadband;
} else if (value < 0) {
value += deadband;
}
return value;
}
int constrain(int amt, int low, int high)
{
if (amt < low)
return low;
else if (amt > high)
return high;
else
return amt;
}
float constrainf(float amt, float low, float high)
{
if (amt < low)
return low;
else if (amt > high)
return high;
else
return amt;
}
void devClear(stdev_t *dev)
{
dev->m_n = 0;
}
void devPush(stdev_t *dev, float x)
{
dev->m_n++;
if (dev->m_n == 1) {
dev->m_oldM = dev->m_newM = x;
dev->m_oldS = 0.0f;
} else {
dev->m_newM = dev->m_oldM + (x - dev->m_oldM) / dev->m_n;
dev->m_newS = dev->m_oldS + (x - dev->m_oldM) * (x - dev->m_newM);
dev->m_oldM = dev->m_newM;
dev->m_oldS = dev->m_newS;
}
}
float devVariance(stdev_t *dev)
{
return ((dev->m_n > 1) ? dev->m_newS / (dev->m_n - 1) : 0.0f);
}
float devStandardDeviation(stdev_t *dev)
{
return sqrtf(devVariance(dev));
}
float degreesToRadians(int16_t degrees)
{
return degrees * RAD;
}
int scaleRange(int x, int srcMin, int srcMax, int destMin, int destMax) {
long int a = ((long int) destMax - (long int) destMin) * ((long int) x - (long int) srcMin);
long int b = (long int) srcMax - (long int) srcMin;
return ((a / b) - (destMax - destMin)) + destMax;
}
// Normalize a vector
void normalizeV(struct fp_vector *src, struct fp_vector *dest)
{
float length;
length = sqrtf(src->X * src->X + src->Y * src->Y + src->Z * src->Z);
if (length != 0) {
dest->X = src->X / length;
dest->Y = src->Y / length;
dest->Z = src->Z / length;
}
}
void buildRotationMatrix(fp_angles_t *delta, float matrix[3][3])
{
float cosx, sinx, cosy, siny, cosz, sinz;
float coszcosx, sinzcosx, coszsinx, sinzsinx;
cosx = cos_approx(delta->angles.roll);
sinx = sin_approx(delta->angles.roll);
cosy = cos_approx(delta->angles.pitch);
siny = sin_approx(delta->angles.pitch);
cosz = cos_approx(delta->angles.yaw);
sinz = sin_approx(delta->angles.yaw);
coszcosx = cosz * cosx;
sinzcosx = sinz * cosx;
coszsinx = sinx * cosz;
sinzsinx = sinx * sinz;
matrix[0][X] = cosz * cosy;
matrix[0][Y] = -cosy * sinz;
matrix[0][Z] = siny;
matrix[1][X] = sinzcosx + (coszsinx * siny);
matrix[1][Y] = coszcosx - (sinzsinx * siny);
matrix[1][Z] = -sinx * cosy;
matrix[2][X] = (sinzsinx) - (coszcosx * siny);
matrix[2][Y] = (coszsinx) + (sinzcosx * siny);
matrix[2][Z] = cosy * cosx;
}
// Rotate a vector *v by the euler angles defined by the 3-vector *delta.
void rotateV(struct fp_vector *v, fp_angles_t *delta)
{
struct fp_vector v_tmp = *v;
float matrix[3][3];
buildRotationMatrix(delta, matrix);
v->X = v_tmp.X * matrix[0][X] + v_tmp.Y * matrix[1][X] + v_tmp.Z * matrix[2][X];
v->Y = v_tmp.X * matrix[0][Y] + v_tmp.Y * matrix[1][Y] + v_tmp.Z * matrix[2][Y];
v->Z = v_tmp.X * matrix[0][Z] + v_tmp.Y * matrix[1][Z] + v_tmp.Z * matrix[2][Z];
}
// Quick median filter implementation
// (c) N. Devillard - 1998
// http://ndevilla.free.fr/median/median.pdf
#define QMF_SORT(a,b) { if ((a)>(b)) QMF_SWAP((a),(b)); }
#define QMF_SWAP(a,b) { int32_t temp=(a);(a)=(b);(b)=temp; }
#define QMF_COPY(p,v,n) { int32_t i; for (i=0; i