mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-17 05:15:23 +03:00
538 lines
No EOL
16 KiB
C
538 lines
No EOL
16 KiB
C
/*
|
||
* 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 <http://www.gnu.org/licenses/>.
|
||
*/
|
||
|
||
#include <stdint.h>
|
||
#include <string.h>
|
||
#include <math.h>
|
||
|
||
#include "axis.h"
|
||
#include "maths.h"
|
||
#include "vector.h"
|
||
#include "quaternion.h"
|
||
#include "platform.h"
|
||
|
||
#ifdef USE_ARM_MATH
|
||
#include "arm_math.h"
|
||
#endif
|
||
|
||
FILE_COMPILE_FOR_SPEED
|
||
|
||
// 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_MATH) || defined(VERY_FAST_MATH)
|
||
#if defined(VERY_FAST_MATH)
|
||
#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));
|
||
}
|
||
|
||
// https://github.com/Crashpilot1000/HarakiriWebstore1/blob/396715f73c6fcf859e0db0f34e12fe44bace6483/src/mw.c#L1292
|
||
// http://http.developer.nvidia.com/Cg/atan2.html (not working correctly!)
|
||
// Poly coefficients by @ledvinap (https://github.com/cleanflight/cleanflight/pull/1107)
|
||
// Max absolute error 0,000027 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.
|
||
// Absolute error <= 6.7e-5
|
||
float acos_approx(float x)
|
||
{
|
||
float xa = fabsf(x);
|
||
float result = fast_fsqrtf(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
|
||
|
||
int gcd(int num, int denom)
|
||
{
|
||
if (denom == 0) {
|
||
return num;
|
||
}
|
||
|
||
return gcd(denom, num % denom);
|
||
}
|
||
|
||
int32_t wrap_18000(int32_t angle)
|
||
{
|
||
if (angle > 18000)
|
||
angle -= 36000;
|
||
if (angle < -18000)
|
||
angle += 36000;
|
||
return angle;
|
||
}
|
||
|
||
int32_t wrap_36000(int32_t angle)
|
||
{
|
||
if (angle > 36000)
|
||
angle -= 36000;
|
||
if (angle < 0)
|
||
angle += 36000;
|
||
return angle;
|
||
}
|
||
|
||
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;
|
||
}
|
||
|
||
int32_t applyDeadbandRescaled(int32_t value, int32_t deadband, int32_t min, int32_t max)
|
||
{
|
||
if (ABS(value) < deadband) {
|
||
value = 0;
|
||
} else if (value > 0) {
|
||
value = scaleRange(value - deadband, 0, max - deadband, 0, max);
|
||
} else if (value < 0) {
|
||
value = scaleRange(value + deadband, min + deadband, 0, min, 0);
|
||
}
|
||
return value;
|
||
}
|
||
|
||
int32_t constrain(int32_t amt, int32_t low, int32_t 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 fast_fsqrtf(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) + destMin);
|
||
}
|
||
|
||
float scaleRangef(float x, float srcMin, float srcMax, float destMin, float destMax) {
|
||
float a = (destMax - destMin) * (x - srcMin);
|
||
float b = srcMax - srcMin;
|
||
return ((a / b) + destMin);
|
||
}
|
||
|
||
// Build rMat from Tait–Bryan angles (convention X1, Y2, Z3)
|
||
void rotationMatrixFromAngles(fpMat3_t * rmat, const fp_angles_t * angles)
|
||
{
|
||
float cosx, sinx, cosy, siny, cosz, sinz;
|
||
float coszcosx, sinzcosx, coszsinx, sinzsinx;
|
||
|
||
cosx = cos_approx(angles->angles.roll);
|
||
sinx = sin_approx(angles->angles.roll);
|
||
cosy = cos_approx(angles->angles.pitch);
|
||
siny = sin_approx(angles->angles.pitch);
|
||
cosz = cos_approx(angles->angles.yaw);
|
||
sinz = sin_approx(angles->angles.yaw);
|
||
|
||
coszcosx = cosz * cosx;
|
||
sinzcosx = sinz * cosx;
|
||
coszsinx = sinx * cosz;
|
||
sinzsinx = sinx * sinz;
|
||
|
||
rmat->m[0][X] = cosz * cosy;
|
||
rmat->m[0][Y] = -cosy * sinz;
|
||
rmat->m[0][Z] = siny;
|
||
rmat->m[1][X] = sinzcosx + (coszsinx * siny);
|
||
rmat->m[1][Y] = coszcosx - (sinzsinx * siny);
|
||
rmat->m[1][Z] = -sinx * cosy;
|
||
rmat->m[2][X] = (sinzsinx) - (coszcosx * siny);
|
||
rmat->m[2][Y] = (coszsinx) + (sinzcosx * siny);
|
||
rmat->m[2][Z] = cosy * cosx;
|
||
}
|
||
|
||
void rotationMatrixFromAxisAngle(fpMat3_t * rmat, const fpAxisAngle_t * a)
|
||
{
|
||
const float sang = sin_approx(a->angle);
|
||
const float cang = cos_approx(a->angle);
|
||
const float C = 1.0f - cang;
|
||
|
||
const float xC = a->axis.x * C;
|
||
const float yC = a->axis.y * C;
|
||
const float zC = a->axis.z * C;
|
||
const float xxC = a->axis.x * xC;
|
||
const float yyC = a->axis.y * yC;
|
||
const float zzC = a->axis.z * zC;
|
||
const float xyC = a->axis.x * yC;
|
||
const float yzC = a->axis.y * zC;
|
||
const float zxC = a->axis.z * xC;
|
||
const float xs = a->axis.x * sang;
|
||
const float ys = a->axis.y * sang;
|
||
const float zs = a->axis.z * sang;
|
||
|
||
rmat->m[0][X] = xxC + cang;
|
||
rmat->m[0][Y] = xyC - zs;
|
||
rmat->m[0][Z] = zxC + ys;
|
||
|
||
rmat->m[1][X] = zxC + ys;
|
||
rmat->m[1][Y] = yyC + cang;
|
||
rmat->m[1][Z] = yzC - xs;
|
||
|
||
rmat->m[2][X] = zxC - ys;
|
||
rmat->m[2][Y] = yzC + xs;
|
||
rmat->m[2][Z] = zzC + cang;
|
||
}
|
||
|
||
// Quick median filter implementation
|
||
// (c) N. Devillard - 1998
|
||
// http://ndevilla.free.fr/median/median.pdf
|
||
#define QMF_SORT(type,a,b) { if ((a)>(b)) QMF_SWAP(type, (a),(b)); }
|
||
#define QMF_SWAP(type,a,b) { type temp=(a);(a)=(b);(b)=temp; }
|
||
|
||
int32_t quickMedianFilter3(int32_t * v)
|
||
{
|
||
int32_t p[3];
|
||
memcpy(p, v, sizeof(p));
|
||
|
||
QMF_SORT(int32_t, p[0], p[1]); QMF_SORT(int32_t, p[1], p[2]); QMF_SORT(int32_t, p[0], p[1]) ;
|
||
return p[1];
|
||
}
|
||
|
||
int16_t quickMedianFilter3_16(int16_t * v)
|
||
{
|
||
int16_t p[3];
|
||
memcpy(p, v, sizeof(p));
|
||
|
||
QMF_SORT(int16_t, p[0], p[1]); QMF_SORT(int16_t, p[1], p[2]); QMF_SORT(int16_t, p[0], p[1]) ;
|
||
return p[1];
|
||
}
|
||
|
||
int32_t quickMedianFilter5(int32_t * v)
|
||
{
|
||
int32_t p[5];
|
||
memcpy(p, v, sizeof(p));
|
||
|
||
QMF_SORT(int32_t, p[0], p[1]); QMF_SORT(int32_t, p[3], p[4]); QMF_SORT(int32_t, p[0], p[3]);
|
||
QMF_SORT(int32_t, p[1], p[4]); QMF_SORT(int32_t, p[1], p[2]); QMF_SORT(int32_t, p[2], p[3]);
|
||
QMF_SORT(int32_t, p[1], p[2]);
|
||
return p[2];
|
||
}
|
||
|
||
int16_t quickMedianFilter5_16(int16_t * v)
|
||
{
|
||
int16_t p[5];
|
||
memcpy(p, v, sizeof(p));
|
||
|
||
QMF_SORT(int16_t, p[0], p[1]); QMF_SORT(int16_t, p[3], p[4]); QMF_SORT(int16_t, p[0], p[3]);
|
||
QMF_SORT(int16_t, p[1], p[4]); QMF_SORT(int16_t, p[1], p[2]); QMF_SORT(int16_t, p[2], p[3]);
|
||
QMF_SORT(int16_t, p[1], p[2]);
|
||
return p[2];
|
||
}
|
||
|
||
int32_t quickMedianFilter7(int32_t * v)
|
||
{
|
||
int32_t p[7];
|
||
memcpy(p, v, sizeof(p));
|
||
|
||
QMF_SORT(int32_t, p[0], p[5]); QMF_SORT(int32_t, p[0], p[3]); QMF_SORT(int32_t, p[1], p[6]);
|
||
QMF_SORT(int32_t, p[2], p[4]); QMF_SORT(int32_t, p[0], p[1]); QMF_SORT(int32_t, p[3], p[5]);
|
||
QMF_SORT(int32_t, p[2], p[6]); QMF_SORT(int32_t, p[2], p[3]); QMF_SORT(int32_t, p[3], p[6]);
|
||
QMF_SORT(int32_t, p[4], p[5]); QMF_SORT(int32_t, p[1], p[4]); QMF_SORT(int32_t, p[1], p[3]);
|
||
QMF_SORT(int32_t, p[3], p[4]);
|
||
return p[3];
|
||
}
|
||
|
||
int32_t quickMedianFilter9(int32_t * v)
|
||
{
|
||
int32_t p[9];
|
||
memcpy(p, v, sizeof(p));
|
||
|
||
QMF_SORT(int32_t, p[1], p[2]); QMF_SORT(int32_t, p[4], p[5]); QMF_SORT(int32_t, p[7], p[8]);
|
||
QMF_SORT(int32_t, p[0], p[1]); QMF_SORT(int32_t, p[3], p[4]); QMF_SORT(int32_t, p[6], p[7]);
|
||
QMF_SORT(int32_t, p[1], p[2]); QMF_SORT(int32_t, p[4], p[5]); QMF_SORT(int32_t, p[7], p[8]);
|
||
QMF_SORT(int32_t, p[0], p[3]); QMF_SORT(int32_t, p[5], p[8]); QMF_SORT(int32_t, p[4], p[7]);
|
||
QMF_SORT(int32_t, p[3], p[6]); QMF_SORT(int32_t, p[1], p[4]); QMF_SORT(int32_t, p[2], p[5]);
|
||
QMF_SORT(int32_t, p[4], p[7]); QMF_SORT(int32_t, p[4], p[2]); QMF_SORT(int32_t, p[6], p[4]);
|
||
QMF_SORT(int32_t, p[4], p[2]);
|
||
return p[4];
|
||
}
|
||
|
||
void arraySubInt32(int32_t *dest, int32_t *array1, int32_t *array2, int count)
|
||
{
|
||
for (int i = 0; i < count; i++) {
|
||
dest[i] = array1[i] - array2[i];
|
||
}
|
||
}
|
||
|
||
/**
|
||
* Sensor offset calculation code based on Freescale's AN4246
|
||
* Initial implementation by @HaukeRa
|
||
* Modified to be re-usable by @DigitalEntity
|
||
*/
|
||
void sensorCalibrationResetState(sensorCalibrationState_t * state)
|
||
{
|
||
for (int i = 0; i < 4; i++){
|
||
for (int j = 0; j < 4; j++){
|
||
state->XtX[i][j] = 0;
|
||
}
|
||
|
||
state->XtY[i] = 0;
|
||
}
|
||
}
|
||
|
||
void sensorCalibrationPushSampleForOffsetCalculation(sensorCalibrationState_t * state, int32_t sample[3])
|
||
{
|
||
state->XtX[0][0] += (float)sample[0] * sample[0];
|
||
state->XtX[0][1] += (float)sample[0] * sample[1];
|
||
state->XtX[0][2] += (float)sample[0] * sample[2];
|
||
state->XtX[0][3] += (float)sample[0];
|
||
|
||
state->XtX[1][0] += (float)sample[1] * sample[0];
|
||
state->XtX[1][1] += (float)sample[1] * sample[1];
|
||
state->XtX[1][2] += (float)sample[1] * sample[2];
|
||
state->XtX[1][3] += (float)sample[1];
|
||
|
||
state->XtX[2][0] += (float)sample[2] * sample[0];
|
||
state->XtX[2][1] += (float)sample[2] * sample[1];
|
||
state->XtX[2][2] += (float)sample[2] * sample[2];
|
||
state->XtX[2][3] += (float)sample[2];
|
||
|
||
state->XtX[3][0] += (float)sample[0];
|
||
state->XtX[3][1] += (float)sample[1];
|
||
state->XtX[3][2] += (float)sample[2];
|
||
state->XtX[3][3] += 1;
|
||
|
||
float squareSum = ((float)sample[0] * sample[0]) + ((float)sample[1] * sample[1]) + ((float)sample[2] * sample[2]);
|
||
state->XtY[0] += sample[0] * squareSum;
|
||
state->XtY[1] += sample[1] * squareSum;
|
||
state->XtY[2] += sample[2] * squareSum;
|
||
state->XtY[3] += squareSum;
|
||
}
|
||
|
||
void sensorCalibrationPushSampleForScaleCalculation(sensorCalibrationState_t * state, int axis, int32_t sample[3], int target)
|
||
{
|
||
for (int i = 0; i < 3; i++) {
|
||
float scaledSample = (float)sample[i] / (float)target;
|
||
state->XtX[axis][i] += scaledSample * scaledSample;
|
||
state->XtX[3][i] += scaledSample * scaledSample;
|
||
}
|
||
|
||
state->XtX[axis][3] += 1;
|
||
state->XtY[axis] += 1;
|
||
state->XtY[3] += 1;
|
||
}
|
||
|
||
static void sensorCalibration_gaussLR(float mat[4][4]) {
|
||
uint8_t n = 4;
|
||
int i, j, k;
|
||
for (i = 0; i < 4; i++) {
|
||
// Determine R
|
||
for (j = i; j < 4; j++) {
|
||
for (k = 0; k < i; k++) {
|
||
mat[i][j] -= mat[i][k] * mat[k][j];
|
||
}
|
||
}
|
||
// Determine L
|
||
for (j = i + 1; j < n; j++) {
|
||
for (k = 0; k < i; k++) {
|
||
mat[j][i] -= mat[j][k] * mat[k][i];
|
||
}
|
||
mat[j][i] /= mat[i][i];
|
||
}
|
||
}
|
||
}
|
||
|
||
void sensorCalibration_ForwardSubstitution(float LR[4][4], float y[4], float b[4]) {
|
||
int i, k;
|
||
for (i = 0; i < 4; ++i) {
|
||
y[i] = b[i];
|
||
for (k = 0; k < i; ++k) {
|
||
y[i] -= LR[i][k] * y[k];
|
||
}
|
||
//y[i] /= MAT_ELEM_AT(LR,i,i); //Do not use, LR(i,i) is 1 anyways and not stored in this matrix
|
||
}
|
||
}
|
||
|
||
void sensorCalibration_BackwardSubstitution(float LR[4][4], float x[4], float y[4]) {
|
||
int i, k;
|
||
for (i = 3 ; i >= 0; --i) {
|
||
x[i] = y[i];
|
||
for (k = i + 1; k < 4; ++k) {
|
||
x[i] -= LR[i][k] * x[k];
|
||
}
|
||
x[i] /= LR[i][i];
|
||
}
|
||
}
|
||
|
||
// solve linear equation
|
||
// https://en.wikipedia.org/wiki/Gaussian_elimination
|
||
static void sensorCalibration_SolveLGS(float A[4][4], float x[4], float b[4]) {
|
||
int i;
|
||
float y[4];
|
||
|
||
sensorCalibration_gaussLR(A);
|
||
|
||
for (i = 0; i < 4; ++i) {
|
||
y[i] = 0;
|
||
}
|
||
|
||
sensorCalibration_ForwardSubstitution(A, y, b);
|
||
sensorCalibration_BackwardSubstitution(A, x, y);
|
||
}
|
||
|
||
bool sensorCalibrationValidateResult(const float result[3])
|
||
{
|
||
// Validate that result is not INF and not NAN
|
||
for (int i = 0; i < 3; i++) {
|
||
if (isnan(result[i]) && isinf(result[i])) {
|
||
return false;
|
||
}
|
||
}
|
||
|
||
return true;
|
||
}
|
||
|
||
bool sensorCalibrationSolveForOffset(sensorCalibrationState_t * state, float result[3])
|
||
{
|
||
float beta[4];
|
||
sensorCalibration_SolveLGS(state->XtX, beta, state->XtY);
|
||
|
||
for (int i = 0; i < 3; i++) {
|
||
result[i] = beta[i] / 2;
|
||
}
|
||
|
||
return sensorCalibrationValidateResult(result);
|
||
}
|
||
|
||
bool sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float result[3])
|
||
{
|
||
float beta[4];
|
||
sensorCalibration_SolveLGS(state->XtX, beta, state->XtY);
|
||
|
||
for (int i = 0; i < 3; i++) {
|
||
result[i] = fast_fsqrtf(beta[i]);
|
||
}
|
||
|
||
return sensorCalibrationValidateResult(result);
|
||
}
|
||
|
||
float bellCurve(const float x, const float curveWidth)
|
||
{
|
||
return powf(M_Ef, -sq(x) / (2.0f * sq(curveWidth)));
|
||
}
|
||
|
||
float fast_fsqrtf(const double value) {
|
||
float ret = 0.0f;
|
||
#ifdef USE_ARM_MATH
|
||
arm_sqrt_f32(value, &ret);
|
||
#else
|
||
ret = sqrtf(value);
|
||
#endif
|
||
if (isnan(ret))
|
||
{
|
||
return 0;
|
||
}
|
||
return ret;
|
||
} |