jlpoole/MotionCal
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Unified MagCalibration_t data structure

Browse source
This commit is contained in:
PaulStoffregen 2016-03-11 07:19:03 -08:00
commit 9ed69c9e92
7 changed files with 260 additions and 278 deletions
Download patch file Download diff file Expand all files Collapse all files

2
Makefile
View file

@ -66,5 +66,7 @@ imuread.o: imuread.c imuread.h
visualize.o: visualize.c imuread.h
serialdata.o: serialdata.c imuread.h
rawdata.o: rawdata.c imuread.h
magcal.o: magcal.c imuread.h
matrix.o: matrix.c imuread.h

6
imuread.c
View file

@ -23,6 +23,12 @@ static void glut_display_callback(void)
int main(int argc, char *argv[])
{
memset(&magcal, 0, sizeof(magcal));
magcal.fV[2] = 80.0f;
magcal.finvW[0][0] = 1.0f;
magcal.finvW[1][1] = 1.0f;
magcal.finvW[2][2] = 1.0f;
glutInit(&argc, argv);
glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH);
glutInitWindowSize(600, 500);

47
imuread.h
View file

@ -39,9 +39,7 @@
#define TIMEOUT_MSEC 40
#define MAGBUFFSIZEX 14
#define MAGBUFFSIZEY 28
#define MAGBUFFSIZE (MAGBUFFSIZEX * MAGBUFFSIZEY)
#define MAGBUFFSIZE 500 // Freescale's lib needs at least 392
#ifdef __cplusplus
extern "C"{
@ -52,41 +50,27 @@ typedef struct {
float y;
float z;
int valid;
float distsqsum;
} magdata_t;
extern magdata_t caldata[MAGBUFFSIZE];
extern magdata_t hard_iron;
extern float soft_iron[9];
} Point_t;
typedef struct {
float w;
float x;
float y;
float z;
} quat_t;
extern quat_t current_orientation;
} Quaternion_t;
extern Quaternion_t current_orientation;
extern int open_port(const char *name);
extern int read_serial_data(void);
extern void close_port(void);
void raw_data(const int *data);
void raw_data(const int16_t *data);
void visualize_init(void);
void display_callback(void);
void resize_callback(int width, int height);
// magnetometer measurement buffer
struct MagneticBuffer
{
int16_t iBpFast[3][MAGBUFFSIZEX][MAGBUFFSIZEY]; // uncalibrated magnetometer readings
int32_t index[MAGBUFFSIZEX][MAGBUFFSIZEY]; // array of time indices
int16_t tanarray[MAGBUFFSIZEX - 1]; // array of tangents of (100 * angle)
int16_t iMagBufferCount; // number of magnetometer readings
};
// magnetic calibration structure
struct MagCalibration
{
// magnetic calibration & buffer structure
typedef struct {
float fV[3]; // current hard iron offset x, y, z, (uT)
float finvW[3][3]; // current inverse soft iron matrix
float fB; // current geomagnetic field magnitude (uT)
@ -102,14 +86,17 @@ struct MagCalibration
float fmatB[10][10]; // scratch 10x10 matrix used by calibration algorithms
float fvecA[10]; // scratch 10x1 vector used by calibration algorithms
float fvecB[4]; // scratch 4x1 vector used by calibration algorithms
int8_t iCalInProgress; // flag denoting that a calibration is in progress
int8_t iMagCalHasRun; // flag denoting that at least one calibration has been launched
int8_t iValidMagCal; // integer value 0, 4, 7, 10 denoting both valid calibration and solver used
};
int16_t iBpFast[3][MAGBUFFSIZE]; // uncalibrated magnetometer readings
int8_t valid[MAGBUFFSIZE]; // 1=has data, 0=empty slot
int16_t iMagBufferCount; // number of magnetometer readings
} MagCalibration_t;
void fUpdateCalibration4INV(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagBuffer);
void fUpdateCalibration7EIG(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagBuffer);
void fUpdateCalibration10EIG(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagBuffer);
extern MagCalibration_t magcal;
void fUpdateCalibration4INV(MagCalibration_t *MagCal);
void fUpdateCalibration7EIG(MagCalibration_t *MagCal);
void fUpdateCalibration10EIG(MagCalibration_t *MagCal);
void f3x3matrixAeqI(float A[][3]);

349
magcal.c
View file

@ -26,9 +26,11 @@
//
// This file contains magnetic calibration functions. It is STRONGLY RECOMMENDED
// that the casual developer NOT TOUCH THIS FILE. The mathematics behind this file
// is extremely complex, and it will be very easy (almost inevitable) that you screw it
// up.
// is extremely complex, and it will be very easy (almost inevitable) that you screw
// it up.
//
// Haha - This file has been edited! Please do not blame or pester NXP (formerly
// Freescale) about the "almost inevitable" issues!
#include "imuread.h"
@ -43,17 +45,15 @@
// 4 element calibration using 4x4 matrix inverse
void fUpdateCalibration4INV(struct MagCalibration *MagCal,
struct MagneticBuffer *MagBuffer)
void fUpdateCalibration4INV(MagCalibration_t *MagCal)
{
// local variables
float fBp2; // fBp[X]^2+fBp[Y]^2+fBp[Z]^2
float fSumBp4; // sum of fBp2
float fscaling; // set to FUTPERCOUNT * FMATRIXSCALING
float fE; // error function = r^T.r
int16_t iOffset[3]; // offset to remove large DC hard iron bias in matrix
int16_t iCount; // number of measurements counted
int8_t i, j, k, l; // loop counters
int8_t i, j, k; // loop counters
// working arrays for 4x4 matrix inversion
float *pfRows[4];
@ -83,59 +83,57 @@ void fUpdateCalibration4INV(struct MagCalibration *MagCal,
// use from MINEQUATIONS up to MAXEQUATIONS entries from magnetic buffer to compute matrices
iCount = 0;
for (j = 0; j < MAGBUFFSIZEX; j++) {
for (k = 0; k < MAGBUFFSIZEY; k++) {
if (MagBuffer->index[j][k] != -1) {
// use first valid magnetic buffer entry as estimate (in counts) for offset
if (iCount == 0) {
for (l = X; l <= Z; l++) {
iOffset[l] = MagBuffer->iBpFast[l][j][k];
}
for (j = 0; j < MAGBUFFSIZE; j++) {
if (MagCal->valid[j]) {
// use first valid magnetic buffer entry as estimate (in counts) for offset
if (iCount == 0) {
for (k = X; k <= Z; k++) {
iOffset[k] = MagCal->iBpFast[k][j];
}
// store scaled and offset fBp[XYZ] in fvecA[0-2] and fBp[XYZ]^2 in fvecA[3-5]
for (l = X; l <= Z; l++) {
MagCal->fvecA[l] = (float)((int32_t)MagBuffer->iBpFast[l][j][k]
- (int32_t)iOffset[l]) * fscaling;
MagCal->fvecA[l + 3] = MagCal->fvecA[l] * MagCal->fvecA[l];
}
// calculate fBp2 = fBp[X]^2 + fBp[Y]^2 + fBp[Z]^2 (scaled uT^2)
fBp2 = MagCal->fvecA[3] + MagCal->fvecA[4] + MagCal->fvecA[5];
// accumulate fBp^4 over all measurements into fSumBp4=Y^T.Y
fSumBp4 += fBp2 * fBp2;
// now we have fBp2, accumulate fvecB[0-2] = X^T.Y =sum(fBp2.fBp[XYZ])
for (l = X; l <= Z; l++) {
MagCal->fvecB[l] += MagCal->fvecA[l] * fBp2;
}
//accumulate fvecB[3] = X^T.Y =sum(fBp2)
MagCal->fvecB[3] += fBp2;
// accumulate on and above-diagonal terms of fmatA = X^T.X ignoring fmatA[3][3]
MagCal->fmatA[0][0] += MagCal->fvecA[X + 3];
MagCal->fmatA[0][1] += MagCal->fvecA[X] * MagCal->fvecA[Y];
MagCal->fmatA[0][2] += MagCal->fvecA[X] * MagCal->fvecA[Z];
MagCal->fmatA[0][3] += MagCal->fvecA[X];
MagCal->fmatA[1][1] += MagCal->fvecA[Y + 3];
MagCal->fmatA[1][2] += MagCal->fvecA[Y] * MagCal->fvecA[Z];
MagCal->fmatA[1][3] += MagCal->fvecA[Y];
MagCal->fmatA[2][2] += MagCal->fvecA[Z + 3];
MagCal->fmatA[2][3] += MagCal->fvecA[Z];
// increment the counter for next iteration
iCount++;
}
// store scaled and offset fBp[XYZ] in fvecA[0-2] and fBp[XYZ]^2 in fvecA[3-5]
for (k = X; k <= Z; k++) {
MagCal->fvecA[k] = (float)((int32_t)MagCal->iBpFast[k][j]
- (int32_t)iOffset[k]) * fscaling;
MagCal->fvecA[k + 3] = MagCal->fvecA[k] * MagCal->fvecA[k];
}
// calculate fBp2 = fBp[X]^2 + fBp[Y]^2 + fBp[Z]^2 (scaled uT^2)
fBp2 = MagCal->fvecA[3] + MagCal->fvecA[4] + MagCal->fvecA[5];
// accumulate fBp^4 over all measurements into fSumBp4=Y^T.Y
fSumBp4 += fBp2 * fBp2;
// now we have fBp2, accumulate fvecB[0-2] = X^T.Y =sum(fBp2.fBp[XYZ])
for (k = X; k <= Z; k++) {
MagCal->fvecB[k] += MagCal->fvecA[k] * fBp2;
}
//accumulate fvecB[3] = X^T.Y =sum(fBp2)
MagCal->fvecB[3] += fBp2;
// accumulate on and above-diagonal terms of fmatA = X^T.X ignoring fmatA[3][3]
MagCal->fmatA[0][0] += MagCal->fvecA[X + 3];
MagCal->fmatA[0][1] += MagCal->fvecA[X] * MagCal->fvecA[Y];
MagCal->fmatA[0][2] += MagCal->fvecA[X] * MagCal->fvecA[Z];
MagCal->fmatA[0][3] += MagCal->fvecA[X];
MagCal->fmatA[1][1] += MagCal->fvecA[Y + 3];
MagCal->fmatA[1][2] += MagCal->fvecA[Y] * MagCal->fvecA[Z];
MagCal->fmatA[1][3] += MagCal->fvecA[Y];
MagCal->fmatA[2][2] += MagCal->fvecA[Z + 3];
MagCal->fmatA[2][3] += MagCal->fvecA[Z];
// increment the counter for next iteration
iCount++;
}
}
// set the last element of the measurement matrix to the number of buffer elements used
MagCal->fmatA[3][3] = (float) iCount;
// store the number of measurements accumulated (defensive programming, should never be needed)
MagBuffer->iMagBufferCount = iCount;
// store the number of measurements accumulated
MagCal->iMagBufferCount = iCount;
// use above diagonal elements of symmetric fmatA to set both fmatB and fmatA to X^T.X
for (i = 0; i < 4; i++) {
@ -182,22 +180,23 @@ void fUpdateCalibration4INV(struct MagCalibration *MagCal,
}
// compute the hard iron vector (in uT but offset and scaled by FMATRIXSCALING)
for (l = X; l <= Z; l++) {
MagCal->ftrV[l] = 0.5F * MagCal->fvecA[l];
for (k = X; k <= Z; k++) {
MagCal->ftrV[k] = 0.5F * MagCal->fvecA[k];
}
// compute the scaled geomagnetic field strength B (in uT but scaled by FMATRIXSCALING)
MagCal->ftrB = sqrtf(MagCal->fvecA[3] + MagCal->ftrV[X] * MagCal->ftrV[X] +
MagCal->ftrV[Y] * MagCal->ftrV[Y] + MagCal->ftrV[Z] * MagCal->ftrV[Z]);
// calculate the trial fit error (percent) normalized to number of measurements and scaled geomagnetic field strength
MagCal->ftrFitErrorpc = sqrtf(fE / (float) MagBuffer->iMagBufferCount) * 100.0F /
// calculate the trial fit error (percent) normalized to number of measurements
// and scaled geomagnetic field strength
MagCal->ftrFitErrorpc = sqrtf(fE / (float) MagCal->iMagBufferCount) * 100.0F /
(2.0F * MagCal->ftrB * MagCal->ftrB);
// correct the hard iron estimate for FMATRIXSCALING and the offsets applied (result in uT)
for (l = X; l <= Z; l++) {
MagCal->ftrV[l] = MagCal->ftrV[l] * DEFAULTB
+ (float)iOffset[l] * FXOS8700_UTPERCOUNT;
for (k = X; k <= Z; k++) {
MagCal->ftrV[k] = MagCal->ftrV[k] * DEFAULTB
+ (float)iOffset[k] * FXOS8700_UTPERCOUNT;
}
// correct the geomagnetic field strength B to correct scaling (result in uT)
@ -214,16 +213,14 @@ void fUpdateCalibration4INV(struct MagCalibration *MagCal,
// 7 element calibration using direct eigen-decomposition
void fUpdateCalibration7EIG(struct MagCalibration *MagCal,
struct MagneticBuffer *MagBuffer)
void fUpdateCalibration7EIG(MagCalibration_t *MagCal)
{
// local variables
float det; // matrix determinant
float fscaling; // set to FUTPERCOUNT * FMATRIXSCALING
float ftmp; // scratch variable
int16_t iOffset[3]; // offset to remove large DC hard iron bias
int16_t iCount; // number of measurements counted
int8_t i, j, k, l, m, n; // loop counters
int8_t i, j, k, m, n; // loop counters
// compute fscaling to reduce multiplications later
fscaling = FXOS8700_UTPERCOUNT / DEFAULTB;
@ -240,47 +237,47 @@ void fUpdateCalibration7EIG(struct MagCalibration *MagCal,
// place from MINEQUATIONS to MAXEQUATIONS entries into product matrix fmatA
iCount = 0;
for (j = 0; j < MAGBUFFSIZEX; j++) {
for (k = 0; k < MAGBUFFSIZEY; k++) {
if (MagBuffer->index[j][k] != -1) {
// use first valid magnetic buffer entry as offset estimate (bit counts)
if (iCount == 0) {
for (l = X; l <= Z; l++) {
iOffset[l] = MagBuffer->iBpFast[l][j][k];
}
for (j = 0; j < MAGBUFFSIZE; j++) {
if (MagCal->valid[j]) {
// use first valid magnetic buffer entry as offset estimate (bit counts)
if (iCount == 0) {
for (k = X; k <= Z; k++) {
iOffset[k] = MagCal->iBpFast[k][j];
}
// apply the offset and scaling and store in fvecA
for (l = X; l <= Z; l++) {
MagCal->fvecA[l + 3] = (float)((int32_t)MagBuffer->iBpFast[l][j][k] - (int32_t)iOffset[l]) * fscaling;
MagCal->fvecA[l] = MagCal->fvecA[l + 3] * MagCal->fvecA[l + 3];
}
// accumulate the on-and above-diagonal terms of MagCal->fmatA=Sigma{fvecA^T * fvecA}
// with the exception of fmatA[6][6] which will sum to the number of measurements
// and remembering that fvecA[6] equals 1.0F
// update the right hand column [6] of fmatA except for fmatA[6][6]
for (m = 0; m < 6; m++) {
MagCal->fmatA[m][6] += MagCal->fvecA[m];
}
// update the on and above diagonal terms except for right hand column 6
for (m = 0; m < 6; m++) {
for (n = m; n < 6; n++) {
MagCal->fmatA[m][n] += MagCal->fvecA[m] * MagCal->fvecA[n];
}
}
// increment the measurement counter for the next iteration
iCount++;
}
// apply the offset and scaling and store in fvecA
for (k = X; k <= Z; k++) {
MagCal->fvecA[k + 3] = (float)((int32_t)MagCal->iBpFast[k][j]
- (int32_t)iOffset[k]) * fscaling;
MagCal->fvecA[k] = MagCal->fvecA[k + 3] * MagCal->fvecA[k + 3];
}
// accumulate the on-and above-diagonal terms of
// MagCal->fmatA=Sigma{fvecA^T * fvecA}
// with the exception of fmatA[6][6] which will sum to the number
// of measurements and remembering that fvecA[6] equals 1.0F
// update the right hand column [6] of fmatA except for fmatA[6][6]
for (m = 0; m < 6; m++) {
MagCal->fmatA[m][6] += MagCal->fvecA[m];
}
// update the on and above diagonal terms except for right hand column 6
for (m = 0; m < 6; m++) {
for (n = m; n < 6; n++) {
MagCal->fmatA[m][n] += MagCal->fvecA[m] * MagCal->fvecA[n];
}
}
// increment the measurement counter for the next iteration
iCount++;
}
}
// finally set the last element fmatA[6][6] to the number of measurements
MagCal->fmatA[6][6] = (float) iCount;
// store the number of measurements accumulated (defensive programming, should never be needed)
MagBuffer->iMagBufferCount = iCount;
// store the number of measurements accumulated
MagCal->iMagBufferCount = iCount;
// copy the above diagonal elements of fmatA to below the diagonal
for (m = 1; m < 7; m++) {
@ -300,14 +297,14 @@ void fUpdateCalibration7EIG(struct MagCalibration *MagCal,
}
}
// set ellipsoid matrix A to the solution vector with smallest eigenvalue, compute its determinant
// and the hard iron offset (scaled and offset)
// set ellipsoid matrix A to the solution vector with smallest eigenvalue,
// compute its determinant and the hard iron offset (scaled and offset)
f3x3matrixAeqScalar(MagCal->fA, 0.0F);
det = 1.0F;
for (l = X; l <= Z; l++) {
MagCal->fA[l][l] = MagCal->fmatB[l][j];
det *= MagCal->fA[l][l];
MagCal->ftrV[l] = -0.5F * MagCal->fmatB[l + 3][j] / MagCal->fA[l][l];
for (k = X; k <= Z; k++) {
MagCal->fA[k][k] = MagCal->fmatB[k][j];
det *= MagCal->fA[k][k];
MagCal->ftrV[k] = -0.5F * MagCal->fmatB[k + 3][j] / MagCal->fA[k][k];
}
// negate A if it has negative determinant
@ -317,43 +314,46 @@ void fUpdateCalibration7EIG(struct MagCalibration *MagCal,
det = -det;
}
// set ftmp to the square of the trial geomagnetic field strength B (counts times FMATRIXSCALING)
// set ftmp to the square of the trial geomagnetic field strength B
// (counts times FMATRIXSCALING)
ftmp = -MagCal->fmatB[6][j];
for (l = X; l <= Z; l++) {
ftmp += MagCal->fA[l][l] * MagCal->ftrV[l] * MagCal->ftrV[l];
for (k = X; k <= Z; k++) {
ftmp += MagCal->fA[k][k] * MagCal->ftrV[k] * MagCal->ftrV[k];
}
// calculate the trial normalized fit error as a percentage
MagCal->ftrFitErrorpc = 50.0F * sqrtf(fabs(MagCal->fvecA[j]) / (float) MagBuffer->iMagBufferCount) / fabs(ftmp);
MagCal->ftrFitErrorpc = 50.0F *
sqrtf(fabs(MagCal->fvecA[j]) / (float) MagCal->iMagBufferCount) / fabs(ftmp);
// normalize the ellipsoid matrix A to unit determinant
f3x3matrixAeqAxScalar(MagCal->fA, powf(det, -(ONETHIRD)));
// convert the geomagnetic field strength B into uT for normalized soft iron matrix A and normalize
// convert the geomagnetic field strength B into uT for normalized
// soft iron matrix A and normalize
MagCal->ftrB = sqrtf(fabs(ftmp)) * DEFAULTB * powf(det, -(ONESIXTH));
// compute trial invW from the square root of A also with normalized determinant and hard iron offset in uT
// compute trial invW from the square root of A also with normalized
// determinant and hard iron offset in uT
f3x3matrixAeqI(MagCal->ftrinvW);
for (l = X; l <= Z; l++) {
MagCal->ftrinvW[l][l] = sqrtf(fabs(MagCal->fA[l][l]));
MagCal->ftrV[l] = MagCal->ftrV[l] * DEFAULTB
+ (float)iOffset[l] * FXOS8700_UTPERCOUNT;
for (k = X; k <= Z; k++) {
MagCal->ftrinvW[k][k] = sqrtf(fabs(MagCal->fA[k][k]));
MagCal->ftrV[k] = MagCal->ftrV[k] * DEFAULTB + (float)iOffset[k] * FXOS8700_UTPERCOUNT;
}
return;
}
// 10 element calibration using direct eigen-decomposition
void fUpdateCalibration10EIG(struct MagCalibration *MagCal,
struct MagneticBuffer *MagBuffer)
void fUpdateCalibration10EIG(MagCalibration_t *MagCal)
{
// local variables
float det; // matrix determinant
float fscaling; // set to FUTPERCOUNT * FMATRIXSCALING
float ftmp; // scratch variable
int16_t iOffset[3]; // offset to remove large DC hard iron bias in matrix
int16_t iCount; // number of measurements counted
int8_t i, j, k, l, m, n; // loop counters
int8_t i, j, k, m, n; // loop counters
// compute fscaling to reduce multiplications later
fscaling = FXOS8700_UTPERCOUNT / DEFAULTB;
@ -370,53 +370,53 @@ void fUpdateCalibration10EIG(struct MagCalibration *MagCal,
// sum between MINEQUATIONS to MAXEQUATIONS entries into the 10x10 product matrix fmatA
iCount = 0;
for (j = 0; j < MAGBUFFSIZEX; j++) {
for (k = 0; k < MAGBUFFSIZEY; k++) {
if (MagBuffer->index[j][k] != -1) {
// use first valid magnetic buffer entry as estimate for offset to help solution (bit counts)
if (iCount == 0) {
for (l = X; l <= Z; l++) {
iOffset[l] = MagBuffer->iBpFast[l][j][k];
}
for (j = 0; j < MAGBUFFSIZE; j++) {
if (MagCal->valid[j] != -1) {
// use first valid magnetic buffer entry as estimate for offset
// to help solution (bit counts)
if (iCount == 0) {
for (k = X; k <= Z; k++) {
iOffset[k] = MagCal->iBpFast[k][j];
}
// apply the fixed offset and scaling and enter into fvecA[6-8]
for (l = X; l <= Z; l++) {
MagCal->fvecA[l + 6] = (float)((int32_t)MagBuffer->iBpFast[l][j][k] - (int32_t)iOffset[l]) * fscaling;
}
// compute measurement vector elements fvecA[0-5] from fvecA[6-8]
MagCal->fvecA[0] = MagCal->fvecA[6] * MagCal->fvecA[6];
MagCal->fvecA[1] = 2.0F * MagCal->fvecA[6] * MagCal->fvecA[7];
MagCal->fvecA[2] = 2.0F * MagCal->fvecA[6] * MagCal->fvecA[8];
MagCal->fvecA[3] = MagCal->fvecA[7] * MagCal->fvecA[7];
MagCal->fvecA[4] = 2.0F * MagCal->fvecA[7] * MagCal->fvecA[8];
MagCal->fvecA[5] = MagCal->fvecA[8] * MagCal->fvecA[8];
// accumulate the on-and above-diagonal terms of fmatA=Sigma{fvecA^T * fvecA}
// with the exception of fmatA[9][9] which equals the number of measurements
// update the right hand column [9] of fmatA[0-8][9] ignoring fmatA[9][9]
for (m = 0; m < 9; m++) {
MagCal->fmatA[m][9] += MagCal->fvecA[m];
}
// update the on and above diagonal terms of fmatA ignoring right hand column 9
for (m = 0; m < 9; m++) {
for (n = m; n < 9; n++) {
MagCal->fmatA[m][n] += MagCal->fvecA[m] * MagCal->fvecA[n];
}
}
// increment the measurement counter for the next iteration
iCount++;
}
// apply the fixed offset and scaling and enter into fvecA[6-8]
for (k = X; k <= Z; k++) {
MagCal->fvecA[k + 6] = (float)((int32_t)MagCal->iBpFast[k][j]
- (int32_t)iOffset[k]) * fscaling;
}
// compute measurement vector elements fvecA[0-5] from fvecA[6-8]
MagCal->fvecA[0] = MagCal->fvecA[6] * MagCal->fvecA[6];
MagCal->fvecA[1] = 2.0F * MagCal->fvecA[6] * MagCal->fvecA[7];
MagCal->fvecA[2] = 2.0F * MagCal->fvecA[6] * MagCal->fvecA[8];
MagCal->fvecA[3] = MagCal->fvecA[7] * MagCal->fvecA[7];
MagCal->fvecA[4] = 2.0F * MagCal->fvecA[7] * MagCal->fvecA[8];
MagCal->fvecA[5] = MagCal->fvecA[8] * MagCal->fvecA[8];
// accumulate the on-and above-diagonal terms of fmatA=Sigma{fvecA^T * fvecA}
// with the exception of fmatA[9][9] which equals the number of measurements
// update the right hand column [9] of fmatA[0-8][9] ignoring fmatA[9][9]
for (m = 0; m < 9; m++) {
MagCal->fmatA[m][9] += MagCal->fvecA[m];
}
// update the on and above diagonal terms of fmatA ignoring right hand column 9
for (m = 0; m < 9; m++) {
for (n = m; n < 9; n++) {
MagCal->fmatA[m][n] += MagCal->fvecA[m] * MagCal->fvecA[n];
}
}
// increment the measurement counter for the next iteration
iCount++;
}
}
// set the last element fmatA[9][9] to the number of measurements
MagCal->fmatA[9][9] = (float) iCount;
// store the number of measurements accumulated (defensive programming, should never be needed)
MagBuffer->iMagBufferCount = iCount;
// store the number of measurements accumulated
MagCal->iMagBufferCount = iCount;
// copy the above diagonal elements of symmetric product matrix fmatA to below the diagonal
for (m = 1; m < 10; m++) {
@ -425,10 +425,12 @@ void fUpdateCalibration10EIG(struct MagCalibration *MagCal,
}
}
// set MagCal->fvecA to the unsorted eigenvalues and fmatB to the unsorted normalized eigenvectors of fmatA
// set MagCal->fvecA to the unsorted eigenvalues and fmatB to the unsorted
// normalized eigenvectors of fmatA
eigencompute(MagCal->fmatA, MagCal->fvecA, MagCal->fmatB, 10);
// set ellipsoid matrix A from elements of the solution vector column j with smallest eigenvalue
// set ellipsoid matrix A from elements of the solution vector column j with
// smallest eigenvalue
j = 0;
for (i = 1; i < 10; i++) {
if (MagCal->fvecA[i] < MagCal->fvecA[j]) {
@ -457,12 +459,12 @@ void fUpdateCalibration10EIG(struct MagCalibration *MagCal,
f3x3matrixAeqInvSymB(MagCal->finvA, MagCal->fA);
// compute the trial hard iron vector in offset bit counts times FMATRIXSCALING
for (l = X; l <= Z; l++) {
MagCal->ftrV[l] = 0.0F;
for (k = X; k <= Z; k++) {
MagCal->ftrV[k] = 0.0F;
for (m = X; m <= Z; m++) {
MagCal->ftrV[l] += MagCal->finvA[l][m] * MagCal->fmatB[m + 6][j];
MagCal->ftrV[k] += MagCal->finvA[k][m] * MagCal->fmatB[m + 6][j];
}
MagCal->ftrV[l] *= -0.5F;
MagCal->ftrV[k] *= -0.5F;
}
// compute the trial geomagnetic field strength B in bit counts times FMATRIXSCALING
@ -475,20 +477,21 @@ void fUpdateCalibration10EIG(struct MagCalibration *MagCal,
// calculate the trial normalized fit error as a percentage
MagCal->ftrFitErrorpc = 50.0F * sqrtf(
fabs(MagCal->fvecA[j]) / (float) MagBuffer->iMagBufferCount) /
fabs(MagCal->fvecA[j]) / (float) MagCal->iMagBufferCount) /
(MagCal->ftrB * MagCal->ftrB);
// correct for the measurement matrix offset and scaling and
// get the computed hard iron offset in uT
for (l = X; l <= Z; l++) {
MagCal->ftrV[l] = MagCal->ftrV[l] * DEFAULTB
+ (float)iOffset[l] * FXOS8700_UTPERCOUNT;
for (k = X; k <= Z; k++) {
MagCal->ftrV[k] = MagCal->ftrV[k] * DEFAULTB + (float)iOffset[k] * FXOS8700_UTPERCOUNT;
}
// convert the trial geomagnetic field strength B into uT for un-normalized soft iron matrix A
// convert the trial geomagnetic field strength B into uT for
// un-normalized soft iron matrix A
MagCal->ftrB *= DEFAULTB;
// normalize the ellipsoid matrix A to unit determinant and correct B by root of this multiplicative factor
// normalize the ellipsoid matrix A to unit determinant and
// correct B by root of this multiplicative factor
f3x3matrixAeqAxScalar(MagCal->fA, powf(det, -(ONETHIRD)));
MagCal->ftrB *= powf(det, -(ONESIXTH));
@ -502,7 +505,8 @@ void fUpdateCalibration10EIG(struct MagCalibration *MagCal,
}
eigencompute(MagCal->fmatA, MagCal->fvecA, MagCal->fmatB, 3);
// set MagCal->fmatB to be eigenvectors . diag(sqrt(sqrt(eigenvalues))) = fmatB . diag(sqrt(sqrt(fvecA))
// set MagCal->fmatB to be eigenvectors . diag(sqrt(sqrt(eigenvalues))) =
// fmatB . diag(sqrt(sqrt(fvecA))
for (j = 0; j < 3; j++) { // loop over columns j
ftmp = sqrtf(sqrtf(fabs(MagCal->fvecA[j])));
for (i = 0; i < 3; i++) { // loop over rows i
@ -510,8 +514,8 @@ void fUpdateCalibration10EIG(struct MagCalibration *MagCal,
}
}
// set ftrinvW to eigenvectors * diag(sqrt(eigenvalues)) * eigenvectors^T
// = fmatB * fmatB^T = sqrt(fA) (guaranteed symmetric)
// set ftrinvW to eigenvectors * diag(sqrt(eigenvalues)) * eigenvectors^T =
// fmatB * fmatB^T = sqrt(fA) (guaranteed symmetric)
// loop over rows
for (i = 0; i < 3; i++) {
// loop over on and above diagonal columns
@ -528,3 +532,6 @@ void fUpdateCalibration10EIG(struct MagCalibration *MagCal,
}

34
rawdata.c
View file

@ -13,27 +13,26 @@ void raw_data_reset(void)
}
//#undef MAGBUFFSIZE
//#define MAGBUFFSIZE 6
static void add_magcal_data(const int *data)
static void add_magcal_data(const int16_t *data)
{
float distsq, dx, dy, dz, magscale;
float minsum=1e30f;
int32_t dx, dy, dz;
uint64_t distsq, minsum=0xFFFFFFFFFFFFFFFF;
int i, j, minindex=0;
// first look for an unused caldata slot
for (i=0; i < MAGBUFFSIZE; i++) {
if (!caldata[i].valid) break;
if (!magcal.valid[i]) break;
}
// if no unused, find the ones closest to each other
if (i >= MAGBUFFSIZE) {
for (i=0; i < MAGBUFFSIZE; i++) {
for (j=i+1; j < MAGBUFFSIZE; j++) {
dx = caldata[i].x - caldata[j].x;
dy = caldata[i].y - caldata[j].y;
dz = caldata[i].z - caldata[j].z;
distsq = dx * dx + dy * dy + dz * dz;
dx = magcal.iBpFast[0][i] - magcal.iBpFast[0][j];
dy = magcal.iBpFast[1][i] - magcal.iBpFast[1][j];
dz = magcal.iBpFast[2][i] - magcal.iBpFast[2][j];
distsq = (int64_t)dx * (int64_t)dx;
distsq += (int64_t)dy * (int64_t)dy;
distsq += (int64_t)dz * (int64_t)dz;
if (distsq < minsum) {
minsum = distsq;
minindex = (random() & 1) ? i : j;
@ -42,17 +41,16 @@ static void add_magcal_data(const int *data)
}
i = minindex;
}
printf("i = %d\n", i);
// add it to the cal buffer
magscale = 0.1f;
caldata[i].x = (float)data[6] * magscale;
caldata[i].y = (float)data[7] * magscale;
caldata[i].z = (float)data[8] * magscale;
caldata[i].valid = 1;
magcal.iBpFast[0][i] = data[6];
magcal.iBpFast[1][i] = data[7];
magcal.iBpFast[2][i] = data[8];
magcal.valid[i] = 1;
}
void raw_data(const int *data)
void raw_data(const int16_t *data)
{
//printf("raw_data: %d %d %d %d %d %d %d %d %d\n", data[0], data[1], data[2],

54
serialdata.c
View file

@ -42,8 +42,7 @@ static int packet_primary_data(const unsigned char *data)
static int packet_magnetic_cal(const unsigned char *data)
{
int16_t id, x, y, z;
magdata_t *cal;
float newx, newy, newz;
int n;
id = (data[7] << 8) | data[6];
x = (data[9] << 8) | data[8];
@ -51,33 +50,32 @@ static int packet_magnetic_cal(const unsigned char *data)
z = (data[13] << 8) | data[12];
if (id == 1) {
cal = &hard_iron;
cal->x = (float)x / 10.0f;
cal->y = (float)y / 10.0f;
cal->z = (float)z / 10.0f;
magcal.fV[0] = (float)x * 0.1f;
magcal.fV[1] = (float)y * 0.1f;
magcal.fV[2] = (float)z * 0.1f;
return 1;
} else if (id == 2) {
soft_iron[0] = (float)x / 1000.0f;
soft_iron[4] = (float)y / 1000.0f;
soft_iron[8] = (float)z / 1000.0f;
magcal.finvW[0][0] = (float)x * 0.001f;
magcal.finvW[1][1] = (float)y * 0.001f;
magcal.finvW[2][2] = (float)z * 0.001f;
return 1;
} else if (id == 3) {
soft_iron[1] = soft_iron[3] = (float)x / 1000.0f; // finvW[X][Y]
soft_iron[2] = soft_iron[6] = (float)y / 1000.0f; // finvW[X][Z]
soft_iron[5] = soft_iron[7] = (float)z / 1000.0f; // finvW[Y][Z]
cal->valid = 1;
magcal.finvW[0][1] = (float)x / 1000.0f;
magcal.finvW[1][0] = (float)x / 1000.0f; // TODO: check this assignment
magcal.finvW[0][2] = (float)y / 1000.0f;
magcal.finvW[1][2] = (float)y / 1000.0f; // TODO: check this assignment
magcal.finvW[1][2] = (float)z / 1000.0f;
magcal.finvW[2][1] = (float)z / 1000.0f; // TODO: check this assignment
return 1;
} else if (id >= 10 && id < MAGBUFFSIZE+10) {
newx = (float)x / 10.0f;
newy = (float)y / 10.0f;
newz = (float)z / 10.0f;
cal = &caldata[id - 10];
if (!cal->valid || cal->x != newx || cal->y != newy || cal->z != newz) {
cal->x = newx;
cal->y = newy;
cal->z = newz;
cal->valid = 1;
printf("mag cal, id=%3d: %5d %5d %5d\n", id, x, y, z);
n = id - 10;
if (magcal.valid[n] == 0 || x != magcal.iBpFast[0][n]
|| y != magcal.iBpFast[1][n] || z != magcal.iBpFast[2][n]) {
magcal.iBpFast[0][n] = x;
magcal.iBpFast[1][n] = y;
magcal.iBpFast[2][n] = z;
magcal.valid[n] = 1;
printf("mag cal, n=%3d: %5d %5d %5d\n", n, x, y, z);
}
return 1;
}
@ -179,18 +177,10 @@ static int packet_parse(const unsigned char *data, int len)
return ret;
}
/*
void raw_data(const int *data)
{
//printf("raw_data: %d %d %d %d %d %d %d %d %d\n", data[0], data[1], data[2],
//data[3], data[4], data[5], data[6], data[7], data[8]);
}
*/
static int ascii_parse(const unsigned char *data, int len)
{
static int ascii_num=0, ascii_neg=0, ascii_count=0;
static int ascii_raw_data[9];
static int16_t ascii_raw_data[9];
static unsigned int ascii_raw_data_count=0;
const char *p, *end;
int ret=0;

46
visualize.c
View file

@ -1,10 +1,8 @@
#include "imuread.h"
magdata_t caldata[MAGBUFFSIZE];
magdata_t hard_iron = {0.0, 0.0, 80.0, 1};
float soft_iron[9] = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
quat_t current_orientation;
MagCalibration_t magcal;
Quaternion_t current_orientation;
// longitude = 0 to 2pi (meaning 0 to 360 degrees)
// latitude = -pi/2 to +pi/2 (meaning -90 to +90 degrees)
@ -48,20 +46,20 @@ static int sphere_region(float longitude, float latitude)
}
static void apply_calibration(const magdata_t *in, magdata_t *out)
static void apply_calibration(int16_t rawx, int16_t rawy, int16_t rawz, Point_t *out)
{
float x, y, z;
x = in->x - hard_iron.x;
y = in->y - hard_iron.y;
z = in->z - hard_iron.z;
out->x = x * soft_iron[0] + y * soft_iron[1] + z * soft_iron[2];
out->y = x * soft_iron[3] + y * soft_iron[4] + z * soft_iron[5];
out->z = x * soft_iron[6] + y * soft_iron[7] + z * soft_iron[8];
x = ((float)rawx * 0.1) - magcal.fV[0];
y = ((float)rawy * 0.1) - magcal.fV[1];
z = ((float)rawz * 0.1) - magcal.fV[2];
out->x = x * magcal.finvW[0][0] + y * magcal.finvW[0][1] + z * magcal.finvW[0][2];
out->y = x * magcal.finvW[1][0] + y * magcal.finvW[1][1] + z * magcal.finvW[1][2];
out->z = x * magcal.finvW[2][0] + y * magcal.finvW[2][1] + z * magcal.finvW[2][2];
out->valid = 1;
}
static void quad_to_rotation(const quat_t *quat, float *rmatrix)
static void quad_to_rotation(const Quaternion_t *quat, float *rmatrix)
{
float qx = quat->x;
float qy = quat->y;
@ -79,7 +77,7 @@ static void quad_to_rotation(const quat_t *quat, float *rmatrix)
rmatrix[8] = 1.0f - 2.0f * qx * qx - 2.0f * qy * qy;
}
static void rotate(const magdata_t *in, magdata_t *out, const float *rmatrix)
static void rotate(const Point_t *in, Point_t *out, const float *rmatrix)
{
if (out == NULL) return;
if (in == NULL || in->valid == 0) {
@ -90,19 +88,9 @@ static void rotate(const magdata_t *in, magdata_t *out, const float *rmatrix)
out->x = in->x * rmatrix[0] + in->y * rmatrix[1] + in->z * rmatrix[2];
out->y = in->x * rmatrix[3] + in->y * rmatrix[4] + in->z * rmatrix[5];
out->z = in->x * rmatrix[6] + in->y * rmatrix[7] + in->z * rmatrix[8];
out->valid = 1;
}
/*
typedef struct {
float x;
float y;
float z;
int valid;
} magdata_t;
magdata_t caldata[MAGBUFFSIZE];
*/
static GLuint spherelist;
static GLuint spherelowreslist;
@ -114,7 +102,7 @@ void display_callback(void)
float xscale, yscale, zscale;
float xoff, yoff, zoff;
float rotation[9];
magdata_t point, draw;
Point_t point, draw;
float magnitude[MAGBUFFSIZE];
float latitude[MAGBUFFSIZE]; // 0 to PI
float longitude[MAGBUFFSIZE]; // -PI to +PI
@ -132,11 +120,15 @@ void display_callback(void)
yoff = 0.0;
zoff = -7.0;
if (hard_iron.valid) {
//if (hard_iron.valid) {
if (1) {
quad_to_rotation(&current_orientation, rotation);
for (i=0; i < MAGBUFFSIZE; i++) {
if (caldata[i].valid) {
apply_calibration(&caldata[i], &point);
//if (caldata[i].valid) {
if (magcal.valid[i]) {
//apply_calibration(&caldata[i], &point);
apply_calibration(magcal.iBpFast[0][i], magcal.iBpFast[1][i],
magcal.iBpFast[2][i], &point);
sumx += point.x;
sumy += point.y;
sumz += point.z;