From 9ae9c87574691a69e0229b22c01de30abb61edbc Mon Sep 17 00:00:00 2001
From: dschin <dschin>
Date: Tue, 17 Aug 2010 19:02:07 +0000
Subject: [PATCH] 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
---
 flight/AHRS/MagOrAccelSensorCal.c | 144 ++++++++++++++++++++++++++++++
 1 file changed, 144 insertions(+)
 create mode 100644 flight/AHRS/MagOrAccelSensorCal.c

diff --git a/flight/AHRS/MagOrAccelSensorCal.c b/flight/AHRS/MagOrAccelSensorCal.c
new file mode 100644
index 000000000..3f51f0a37
--- /dev/null
+++ b/flight/AHRS/MagOrAccelSensorCal.c
@@ -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;
+}
+