mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-20 10:54:14 +01:00
Six point sensor cal for accels and/or mags
Uploaded here for now - belongs in GCS probably. git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1310 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
dschin
dschin
parent
3061ddc98c
commit
9ae9c87574
144
flight/AHRS/MagOrAccelSensorCal.c
Normal file
144
flight/AHRS/MagOrAccelSensorCal.c
Normal file
@ -0,0 +1,144 @@
|
||||
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file MagOrAccelSensorCal.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief 3 axis sensor cal from six measurements taken in a constant field.
|
||||
* Call SixPointInConstFieldCal(FieldMagnitude, X, Y, Z, S, b)
|
||||
* - FieldMagnitude is the constant field, e.g. 9.81 for accels
|
||||
* - X, Y, Z are vectors of six measurements from different orientations
|
||||
* - returns, S[3] and b[3], that are the scale and offsett for the axes
|
||||
* - i.e. Measurementx = S[0]*Sensorx + b[0]
|
||||
*
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program 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.
|
||||
*
|
||||
* This program 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 this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
|
||||
#include <math.h>
|
||||
#include "stdint.h"
|
||||
|
||||
//Function Prototypes
|
||||
int16_t SixPointInConstFieldCal( double ConstMag, double x[6], double y[6], double z[6], double S[3], double b[3]);
|
||||
int16_t LinearEquationsSolving(int16_t nDim, double* pfMatr, double* pfVect, double* pfSolution);
|
||||
|
||||
|
||||
int16_t SixPointInConstFieldCal( double ConstMag, double x[6], double y[6], double z[6], double S[3], double b[3] )
|
||||
{
|
||||
int16_t i;
|
||||
double A[5][5];
|
||||
double f[5], c[5];
|
||||
double xp, yp, zp, Sx;
|
||||
|
||||
// Fill in matrix A -
|
||||
// write six difference-in-magnitude equations of the form
|
||||
// Sx^2(x2^2-x1^2) + 2*Sx*bx*(x2-x1) + Sy^2(y2^2-y1^2) + 2*Sy*by*(y2-y1) + Sz^2(z2^2-z1^2) + 2*Sz*bz*(z2-z1) = 0
|
||||
// or in other words
|
||||
// 2*Sx*bx*(x2-x1)/Sx^2 + Sy^2(y2^2-y1^2)/Sx^2 + 2*Sy*by*(y2-y1)/Sx^2 + Sz^2(z2^2-z1^2)/Sx^2 + 2*Sz*bz*(z2-z1)/Sx^2 = (x1^2-x2^2)
|
||||
for (i=0;i<5;i++){
|
||||
A[i][0] = 2.0 * (x[i+1] - x[i]);
|
||||
A[i][1] = y[i+1]*y[i+1] - y[i]*y[i];
|
||||
A[i][2] = 2.0 * (y[i+1] - y[i]);
|
||||
A[i][3] = z[i+1]*z[i+1] - z[i]*z[i];
|
||||
A[i][4] = 2.0 * (z[i+1] - z[i]);
|
||||
f[i] = x[i]*x[i] - x[i+1]*x[i+1];
|
||||
}
|
||||
|
||||
// solve for c0=bx/Sx, c1=Sy^2/Sx^2; c2=Sy*by/Sx^2, c3=Sz^2/Sx^2, c4=Sz*bz/Sx^2
|
||||
if ( !LinearEquationsSolving( 5, (double *)A, f, c) ) return 0;
|
||||
|
||||
// use one magnitude equation and c's to find Sx - doesn't matter which - all give the same answer
|
||||
xp = x[0]; yp = y[0]; zp = z[0];
|
||||
Sx = sqrt(ConstMag*ConstMag / (xp*xp + 2*c[0]*xp + c[0]*c[0] + c[1]*yp*yp + 2*c[2]*yp + c[2]*c[2]/c[1] + c[3]*zp*zp + 2*c[4]*zp + c[4]*c[4]/c[3]));
|
||||
|
||||
S[0] = Sx;
|
||||
b[0] = Sx*c[0];
|
||||
S[1] = sqrt(c[1]*Sx*Sx);
|
||||
b[1] = c[2]*Sx*Sx/S[1];
|
||||
S[2] = sqrt(c[3]*Sx*Sx);
|
||||
b[2] = c[4]*Sx*Sx/S[2];
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
//*****************************************************************
|
||||
|
||||
int16_t LinearEquationsSolving(int16_t nDim, double* pfMatr, double* pfVect, double* pfSolution)
|
||||
{
|
||||
double fMaxElem;
|
||||
double fAcc;
|
||||
|
||||
int16_t i , j, k, m;
|
||||
|
||||
|
||||
for(k=0; k<(nDim-1); k++) // base row of matrix
|
||||
{
|
||||
// search of line with max element
|
||||
fMaxElem = fabs( pfMatr[k*nDim + k] );
|
||||
m = k;
|
||||
for(i=k+1; i<nDim; i++)
|
||||
{
|
||||
if(fMaxElem < fabs(pfMatr[i*nDim + k]) )
|
||||
{
|
||||
fMaxElem = pfMatr[i*nDim + k];
|
||||
m = i;
|
||||
}
|
||||
}
|
||||
|
||||
// permutation of base line (index k) and max element line(index m)
|
||||
if(m != k)
|
||||
{
|
||||
for(i=k; i<nDim; i++)
|
||||
{
|
||||
fAcc = pfMatr[k*nDim + i];
|
||||
pfMatr[k*nDim + i] = pfMatr[m*nDim + i];
|
||||
pfMatr[m*nDim + i] = fAcc;
|
||||
}
|
||||
fAcc = pfVect[k];
|
||||
pfVect[k] = pfVect[m];
|
||||
pfVect[m] = fAcc;
|
||||
}
|
||||
|
||||
if( pfMatr[k*nDim + k] == 0.) return 0; // needs improvement !!!
|
||||
|
||||
// triangulation of matrix with coefficients
|
||||
for(j=(k+1); j<nDim; j++) // current row of matrix
|
||||
{
|
||||
fAcc = - pfMatr[j*nDim + k] / pfMatr[k*nDim + k];
|
||||
for(i=k; i<nDim; i++)
|
||||
{
|
||||
pfMatr[j*nDim + i] = pfMatr[j*nDim + i] + fAcc*pfMatr[k*nDim + i];
|
||||
}
|
||||
pfVect[j] = pfVect[j] + fAcc*pfVect[k]; // free member recalculation
|
||||
}
|
||||
}
|
||||
|
||||
for(k=(nDim-1); k>=0; k--)
|
||||
{
|
||||
pfSolution[k] = pfVect[k];
|
||||
for(i=(k+1); i<nDim; i++)
|
||||
{
|
||||
pfSolution[k] -= (pfMatr[k*nDim + i]*pfSolution[i]);
|
||||
}
|
||||
pfSolution[k] = pfSolution[k] / pfMatr[k*nDim + k];
|
||||
}
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user