#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; }