double sensorFusion::getTiltProportionalA()
{
double T_total = RAD2DEG((acos(cos(RPY(0))*cos(RPY(1)))));
T_total = constrainn<double>(T_total, 0.0f, SFUS_COMPF_RP_LIM_TILT);
return mapp<double>(T_total, 0.0f, SFUS_COMPF_RP_LIM_TILT, rollPitchA_minimum, rollPitchA_maximum);
}
double sensorFusion::getEulerYawFromMagne(Vector3d magneBodyFrame)
{
// tilt compensation without yaw
// Source: http://www.chrobotics.com/library/understanding-euler-angles
// Source: http://cache.freescale.com/files/sensors/doc/app_note/AN4248.pdf
Vector3d magneEarthFrame = getVectorFromBody2EarthFrame(magneBodyFrame, Vector3d(RPY(0), RPY(1), 0.0f));
// Source: https://www.adafruit.com/datasheets/AN203_Compass_Heading_Using_Magnetometers.pdf
double orientation_tmp;
if(magneEarthFrame(1) == 0.0)
{
if(magneEarthFrame(0) > 0.0)
orientation_tmp = 0.0;
else if(magneEarthFrame(0) < 0.0)
orientation_tmp = Pi;
}
else if(magneEarthFrame(1) > 0.0)
{
orientation_tmp = Pi/2.0 - atan(magneEarthFrame(0)/magneEarthFrame(1));
}
else if(magneEarthFrame(2) < 0.0)
{
orientation_tmp = 1.5*Pi - atan(magneEarthFrame(0)/magneEarthFrame(1));
}
// transform from 0-2*Pi to +/-Pi
return wrap_Pi(-orientation_tmp);
}
void sensorFusion::writeData(Vector3d accel, Vector3d gyros, Vector3d magne, Vector3d barom)
{
// compensate chip tilt for all sensors
accel = getVectorFromBody2EarthFrame(accel, sensorTiltCalib);
gyros = getRatesFromBody2EarthFrame(gyros-gyrosCalib, sensorTiltCalib);
magne = getVectorFromBody2EarthFrame(magne, sensorTiltCalib);
// push tilt compensated values to container
accelData.pushNewElem(accel);
gyrosData.pushNewElem(gyros);
magneData.pushNewElem(magne);
baromData.pushNewElem(barom);
// transform input from sensors into earth frame
Vector3d accelEulertmp = getEulerAnglesFromAccel(accel);
Vector3d gyrosEulerRatestmp = getRatesFromBody2EarthFrame(gyros, RPY);
Vector3d accelAccelerationtmp = getVectorFromBody2EarthFrame(accel, RPY);
// adjust complementary filter
double currentA = getTiltProportionalA();
cf[ROLL].setTauViaA(currentA);
cf[PITCH].setTauViaA(currentA);
// filtering for roll and pitch
RPY(0) = cf[ROLL].getCombinedEstimation(accelEulertmp(0), gyrosEulerRatestmp(0));
RPY(1) = cf[PITCH].getCombinedEstimation(accelEulertmp(1), gyrosEulerRatestmp(1));
RPY(2) = cf[YAW].getCombinedEstimation(getEulerYawFromMagne(magne), gyrosEulerRatestmp(2));
// yaw is +/- Pi
RPY(2) = wrap_Pi(RPY(2));
cf[YAW].setCombinedEstimation(RPY(2));
// rotation rates from calibrated gyroscope
RPYDot = gyrosEulerRatestmp;
// apply moving average filter for height
height = 0.0;
double height_old = 0.0;
for(int i=0; i<SFUS_MA_HEIGHT; i++)
{
Vector3d tmp;
baromData.getNthElem(tmp, i);
height += tmp(2);
baromData.getNthElem(tmp, i+1);
height_old += tmp(2);
}
height /= SFUS_MA_HEIGHT;
height_old /= SFUS_MA_HEIGHT;
// vertical speed
heightDot = cf[HEIGHTDOT].getCombinedEstimation((height - height_old)/dT, accelAccelerationtmp(2) - GRAVITY);
// vertical acceleration
heightDotDot = accelAccelerationtmp(2) - GRAVITY;
}
/*---------------------------------------------------------------------------*/
int CGFMMmrhs(MKL_INT *solution, double *ShellSphs, double *rhs, double *nRhs, MKL_INT n)
{
/*---------------------------------------------------------------------------*/
/* Define arrays for the upper triangle of the coefficient matrix and rhs vector */
/* Compressed sparse row storage is used for sparse representation */
/*---------------------------------------------------------------------------*/
MKL_INT rci_request, expected_itercount = 20, i, j;
MKL_INT itercount[nRhs];
/* Fill all arrays containing matrix data. */
/*---------------------------------------------------------------------------*/
/* Allocate storage for the solver ?par and temporary storage tmp */
/*---------------------------------------------------------------------------*/
MKL_INT length = 128, method = 1;
/*---------------------------------------------------------------------------*/
/* Some additional variables to use with the RCI (P)CG solver */
/*---------------------------------------------------------------------------*/
MKL_INT ipar[128 + 2 * nRhs];
double euclidean_norm, dpar[128 + 2 * nRhs];
double *tmp;
tmp = (double*)calloc(n * (3 + nRhs),sizeof(double));
double eone = -1.E0;
MKL_INT ione = 1;
/*---------------------------------------------------------------------------*/
/* Initialize the initial guess */
/*---------------------------------------------------------------------------*/
for (i = 0; i < n*nRhs; i++)
solution[i] = 1.E0;
/*---------------------------------------------------------------------------*/
/* Initialize the solver */
/*---------------------------------------------------------------------------*/
for (i = 0; i < (length + 2 * nRhs); i++)
ipar[i] = 0;
for (i = 0; i < (length + 2 * nRhs); i++)
dpar[i] = 0.E0;
dcgmrhs_init (&n, solution, &nRhs, rhs, &method, &rci_request, ipar, dpar, tmp);
if (rci_request != 0)
goto failure;
/*---------------------------------------------------------------------------*/
/* Set the desired parameters: */
/* LOGICAL parameters: */
/* do residual stopping test */
/* do not request for the user defined stopping test */
/* DOUBLE parameters */
/* set the relative tolerance to 1.0D-5 instead of default value 1.0D-6 */
/*---------------------------------------------------------------------------*/
ipar[8] = 1;
ipar[9] = 0;
dpar[0] = 1.E-5;
/*---------------------------------------------------------------------------*/
/* Compute the solution by RCI (P)CG solver without preconditioning */
/* Reverse Communications starts here */
/*---------------------------------------------------------------------------*/
rci:dcgmrhs (&n, solution, &nRhs, rhs, &rci_request, ipar, dpar, tmp);
/*---------------------------------------------------------------------------*/
/* If rci_request=0, then the solution was found with the required precision */
/*---------------------------------------------------------------------------*/
if (rci_request == 0)
goto getsln;
/*---------------------------------------------------------------------------*/
/* If rci_request=1, then compute the vector A*tmp[0] */
/* and put the result in vector tmp[n] */
/*---------------------------------------------------------------------------*/
if (rci_request == 1)
{
//mkl_dcsrsymv (&tr, &n, a, ia, ja, tmp, &tmp[n]);
//debug
RPY(n, ShellSphs,tmp); // SPMV by 4 calls of FMM
goto rci;
}
/*---------------------------------------------------------------------------*/
/* If rci_request=anything else, then dcg subroutine failed */
/* to compute the solution vector: solution[n] */
/*---------------------------------------------------------------------------*/
goto failure;
/*---------------------------------------------------------------------------*/
/* Reverse Communication ends here */
/* Get the current iteration number into itercount */
/*---------------------------------------------------------------------------*/
getsln:dcgmrhs_get (&n, solution, &nRhs, rhs, &rci_request, ipar, dpar, tmp, itercount);
/*---------------------------------------------------------------------------*/
/* Print solution vector: solution[n] and number of iterations: itercount */
/*---------------------------------------------------------------------------*/
printf ("The system has been solved\n");
printf ("The following solution obtained\n");
for (i = 0; i < n / 2; i++)
printf ("%6.3f ", solution[i]);
printf ("\n");
for (i = n / 2; i < n; i++)
printf ("%6.3f ", solution[i]);
printf ("\n");
for (i = 0; i < n / 2; i++)
printf ("%6.3f ", solution[n + i]);
printf ("\n");
for (i = n / 2; i < n; i++)
printf ("%6.3f ", solution[n + i]);
printf ("\nExpected solution is\n");
for (i = 0; i < n / 2; i++)
{
printf ("%6.3f ", expected_sol[i]);
expected_sol[i] -= solution[i];
}
printf ("\n");
for (i = n / 2; i < n; i++)
{
printf ("%6.3f ", expected_sol[i]);
expected_sol[i] -= solution[i];
}
printf ("\n");
for (i = 0; i < n / 2; i++)
{
printf ("%6.3f ", expected_sol[n + i]);
expected_sol[n + i] -= solution[n + i];
}
printf ("\n");
for (i = n / 2; i < n; i++)
{
printf ("%6.3f ", expected_sol[n + i]);
expected_sol[n + i] -= solution[n + i];
}
printf ("\n");
i = 1;
j = n * nRhs;
euclidean_norm = dnrm2 (&j, expected_sol, &i);
/*-------------------------------------------------------------------------*/
/* Release internal MKL memory that might be used for computations */
/* NOTE: It is important to call the routine below to avoid memory leaks */
/* unless you disable MKL Memory Manager */
/*-------------------------------------------------------------------------*/
MKL_Free_Buffers ();
if (euclidean_norm < 1.0e-12)
{
printf ("This example has successfully PASSED through all steps of computation!\n");
return 0;
}
else
{
printf ("This example may have FAILED as the computed solution differs\n");
printf ("much from the expected solution (Euclidean norm is %e).\n", euclidean_norm);
return 1;
}
/*-------------------------------------------------------------------------*/
/* Release internal MKL memory that might be used for computations */
/* NOTE: It is important to call the routine below to avoid memory leaks */
/* unless you disable MKL Memory Manager */
/*-------------------------------------------------------------------------*/
failure:printf ("This example FAILED as the solver has returned the ERROR code %d", rci_request);
MKL_Free_Buffers ();
return 1;
}
bool sensorFusion::writeInitData(Vector3d accel, Vector3d gyros, Vector3d magne, Vector3d barom)
{
initAccelReadsCnt++;
if(initAccelReadsCnt > SFUS_INIT_ACCEL_DROPS && initAccelReadsCnt <= (SFUS_INIT_ACCEL_DROPS + SFUS_INIT_ACCEL_READS))
initAccelSum += accel;
initGyrosReadsCnt++;
if(initGyrosReadsCnt > SFUS_INIT_GYROS_DROPS && initGyrosReadsCnt <= (SFUS_INIT_GYROS_DROPS + SFUS_INIT_GYROS_READS))
initGyrosSum += gyros;
initMagneReadsCnt++;
if(initMagneReadsCnt > SFUS_INIT_MAGNE_DROPS && initMagneReadsCnt <= (SFUS_INIT_MAGNE_DROPS + SFUS_INIT_MAGNE_READS))
initMagneSum += magne;
initBaromReadsCnt++;
if(initBaromReadsCnt > SFUS_INIT_BAROM_DROPS && initBaromReadsCnt <= (SFUS_INIT_BAROM_DROPS + SFUS_INIT_BAROM_READS))
initBaromSum += barom;
// data collection finished
if(initAccelReadsCnt >= (SFUS_INIT_ACCEL_DROPS + SFUS_INIT_ACCEL_READS)
&& initGyrosReadsCnt >= (SFUS_INIT_GYROS_DROPS + SFUS_INIT_GYROS_READS)
&& initMagneReadsCnt >= (SFUS_INIT_MAGNE_DROPS + SFUS_INIT_MAGNE_READS)
&& initBaromReadsCnt >= (SFUS_INIT_BAROM_DROPS + SFUS_INIT_BAROM_READS))
{
initAccelSum /= SFUS_INIT_ACCEL_READS;
initGyrosSum /= SFUS_INIT_GYROS_READS;
initMagneSum /= SFUS_INIT_MAGNE_READS;
initBaromSum /= SFUS_INIT_BAROM_READS;
// compensate chip tilt
initAccelSum = getVectorFromBody2EarthFrame(initAccelSum, sensorTiltCalib);
initMagneSum = getVectorFromBody2EarthFrame(initMagneSum, sensorTiltCalib);
// determine initial values
RPY = getEulerAnglesFromAccel(initAccelSum);
RPY(2) = getEulerYawFromMagne(initMagneSum);
RPYDot = Vector3d(0.0, 0.0, 0.0);
height = initBaromSum(2);
heightDot = 0.0;
heightDotDot = 0.0;
#ifdef QS_DEBUG_WITH_CONSOLE
printf("*********************************************\n");
printf("Initial attitude:\n");
printf("roll: %f\n", RAD2DEG(RPY(0)));
printf("pitch: %f\n", RAD2DEG(RPY(1)));
printf("yaw: %f\n\n", RAD2DEG(RPY(2)));
#endif
// the gyroscope is recalibrated at each start up
#ifdef SFUS_USE_GYROS_CALIB
gyrosCalib = initGyrosSum;
#else
gyrosCalib = Vector3d(0.0, 0.0, 0.0);
#endif
#ifdef QS_DEBUG_WITH_CONSOLE
printf("*********************************************\n");
printf("Gyroscope calibrated in sensor fusion:\n");
printf("gyros x offset: %f\n", RAD2DEG(gyrosCalib(0)));
printf("gyros x offset: %f\n", RAD2DEG(gyrosCalib(1)));
printf("gyros x offset: %f\n\n", RAD2DEG(gyrosCalib(2)));
#endif
// push values into buffers
for(int i=0; i<SFUS_ACCEL_BUF; i++) accelData.pushNewElem(initAccelSum);
for(int i=0; i<SFUS_GYROS_BUF; i++) gyrosData.pushNewElem(initGyrosSum);
for(int i=0; i<SFUS_MAGNE_BUF; i++) magneData.pushNewElem(initMagneSum);
for(int i=0; i<SFUS_BAROM_BUF; i++) baromData.pushNewElem(initBaromSum);
// init complementary filter A/Tau factor
float currentA = getTiltProportionalA();
cf[ROLL].setTauViaA(currentA);
cf[PITCH].setTauViaA(currentA);
// init complementary filter angles
cf[ROLL].setCombinedEstimation(RPY(0));
cf[PITCH].setCombinedEstimation(RPY(1));
cf[YAW].setCombinedEstimation(RPY(2));
cf[HEIGHTDOT].setCombinedEstimation(heightDot);
return true;
}
return false;
}
//! Gives back a rotation matrix specified with EulerZYX convention :
//! First rotate around Z with alfa,
//! then around the new Y with beta, then around
//! new X with gamma.
//!
//! closely related to RPY-convention
inline static Rotation EulerZYX(double Alfa,double Beta,double Gamma) {
return RPY(Gamma,Beta,Alfa);
}
