diff --git a/Makefile b/Makefile index ef8bf95..a3b8eba 100644 --- a/Makefile +++ b/Makefile @@ -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 diff --git a/imuread.c b/imuread.c index 424f230..ce5ecbc 100644 --- a/imuread.c +++ b/imuread.c @@ -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); diff --git a/imuread.h b/imuread.h index c789acb..5be0a99 100644 --- a/imuread.h +++ b/imuread.h @@ -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]); diff --git a/magcal.c b/magcal.c index 0045872..680820d 100644 --- a/magcal.c +++ b/magcal.c @@ -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, } + + + diff --git a/rawdata.c b/rawdata.c index fe1987e..acc27cc 100644 --- a/rawdata.c +++ b/rawdata.c @@ -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], diff --git a/serialdata.c b/serialdata.c index 777ce16..b86227b 100644 --- a/serialdata.c +++ b/serialdata.c @@ -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; diff --git a/visualize.c b/visualize.c index 18d7314..15a235f 100644 --- a/visualize.c +++ b/visualize.c @@ -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(¤t_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;