343 lines
8.9 KiB
C
343 lines
8.9 KiB
C
|
#include "maths.h"
|
||
|
|
|
||
|
|
|
||
|
|
//#include "config_param.h"
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
#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
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
float map(long x, long in_min, long in_max, float out_min, float out_max)
|
||
|
|
{
|
||
|
|
long divisor = (in_max - in_min);
|
||
|
|
if(divisor == 0){
|
||
|
|
return -1; //AVR returns -1, SAM returns 0
|
||
|
|
}
|
||
|
|
return (x - in_min) * (out_max - out_min) / divisor + out_min;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
float mapFloat(float x, float in_min, float in_max, float out_min, float out_max)
|
||
|
|
{
|
||
|
|
float divisor = (in_max - in_min);
|
||
|
|
if(divisor == 0){
|
||
|
|
return -1; //AVR returns -1, SAM returns 0
|
||
|
|
}
|
||
|
|
return (x - in_min) * (out_max - out_min) / divisor + out_min;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
float constrainFloat(float data,float min,float max)
|
||
|
|
{
|
||
|
|
if(data > max)
|
||
|
|
data = max;
|
||
|
|
else if(data < min)
|
||
|
|
data = min;
|
||
|
|
|
||
|
|
return data;
|
||
|
|
}
|
||
|
|
|
||
|
|
int16_t constrainInt16_t(int16_t data,int16_t min,int16_t max)
|
||
|
|
{
|
||
|
|
if(data > max)
|
||
|
|
data = max;
|
||
|
|
else if(data < min)
|
||
|
|
data = min;
|
||
|
|
|
||
|
|
return data;
|
||
|
|
}
|
||
|
|
|
||
|
|
uint16_t constrainUint16_t(uint16_t data,uint16_t min,uint16_t max)
|
||
|
|
{
|
||
|
|
if(data > max)
|
||
|
|
data = max;
|
||
|
|
else if(data < min)
|
||
|
|
data = min;
|
||
|
|
|
||
|
|
return data;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
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 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 = 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;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
/**
|
||
|
|
* 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;
|
||
|
|
}
|
||
|
|
|
||
|
|
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);
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
void 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;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
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;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
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;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
float applyDeadbandF(float value, float deadband)
|
||
|
|
{
|
||
|
|
if (ABS(value) < deadband)
|
||
|
|
{
|
||
|
|
value = 0;
|
||
|
|
}
|
||
|
|
else if (value > 0)
|
||
|
|
{
|
||
|
|
value -= deadband;
|
||
|
|
}
|
||
|
|
else if (value < 0)
|
||
|
|
{
|
||
|
|
value += deadband;
|
||
|
|
}
|
||
|
|
return value;
|
||
|
|
}
|
||
|
|
|