KL46 Accelerometer + Magnetometer, with 3-axis calibration. Readout through OpenSDA CDC.
Dependencies: MAG3110 MMA8451Q USBDevice mbed
Revision 0:c569d820861b, committed 2014-04-10
- Comitter:
- wue
- Date:
- Thu Apr 10 07:48:46 2014 +0000
- Commit message:
- KL46Z Accelerometer & Magnetometer readout over OpenSDA CDC.; 3-axis 4-value calibration taken from eCompass.
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MAG3110.lib Thu Apr 10 07:48:46 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/SomeRandomBloke/code/MAG3110/#cf40601402b7
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MMA8451Q.lib Thu Apr 10 07:48:46 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/JoKer/code/MMA8451Q/#2d14600116fc
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/USBDevice.lib Thu Apr 10 07:48:46 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/USBDevice/#5b7d31d9d3f3
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/magnetic.cpp Thu Apr 10 07:48:46 2014 +0000 @@ -0,0 +1,410 @@ +// Source: eCompass v3 + +#include "magnetic.h" +#include "mbed.h" + +float xftmpA4x4[4][4], *ftmpA4x4[4]; // scratch 4x4 matrix +float xftmpB4x4[4][4], *ftmpB4x4[4]; // scratch 4x4 matrix +float xftmpA4x1[4][1], *ftmpA4x1[4]; // scratch 4x1 matrix +float xftmpB4x1[4][1], *ftmpB4x1[4]; // scratch 4x1 matrix + +int32 xicolind[MAXMATINV][1], *icolind[MAXMATINV]; +int32 xirowind[MAXMATINV][1], *irowind[MAXMATINV]; +int32 xipivot[MAXMATINV][1], *ipivot[MAXMATINV]; + + +void magUpdateCalibration(struct MagCalibration *pthisMagCal, + struct MagneticBuffer *pthisMagneticBuffer) { + + int i; + + //while(1); + // 4 row arrays + for (i = 0; i < 4; i++) + { + ftmpA4x4[i] = xftmpA4x4[i]; + ftmpB4x4[i] = xftmpB4x4[i]; + ftmpA4x1[i] = xftmpA4x1[i]; + ftmpB4x1[i] = xftmpB4x1[i]; + }; + + // MAXMATINV row arrays + for (i = 0; i < MAXMATINV; i++) { + icolind[i] = xicolind[i]; + irowind[i] = xirowind[i]; + ipivot[i] = xipivot[i]; + }; + + + fUpdateCalibration4INV(pthisMagCal, pthisMagneticBuffer, ftmpA4x4, ftmpB4x4, ftmpA4x1, ftmpB4x1, icolind, irowind, ipivot); + +}; + + + + +// function calculates the matrix product A = B x C +void fmatrixAeqBxC(float **A, float **B, float **C, int32 rB, int32 cBrC, int32 cC) +{ + // rB = rows in B + // cBrC = columns in B = rows in C + // cC = columns in C + // A has dimension rB rows x cC columns + + int32 i, j, k; // counters + + for (i = 0; i < rB; i++) + { + for (j = 0; j < cC; j++) + { + A[i][j] = 0.0F; + for (k = 0; k < cBrC; k++) + A[i][j] += B[i][k] * C[k][j]; + } + } + return; +} + + +// function sets the matrix A to the identity matrix +void fmatrixAeqI(float **A, int32 rc) +{ + // rc = rows and columns in A + + int32 i, j; // loop counters + + for (i = 0; i < rc; i++) + { + for (j = 0; j < rc; j++) + { + A[i][j] = 0.0F; + } + A[i][i] = 1.0F; + } + return; +} + + +/* function uses Gauss-Jordan elimination to compute the inverse of matrix A in situ */ +/* on exit, A is replaced with its inverse */ +void fmatrixAeqInvA(float **A, int32 isize, int32 **icolind, int32 **irowind, int32 **ipivot) +{ + int32 i, j, k, l, m; // index counters + int32 ipivotrow, ipivotcol; // row and column of pivot element + float largest; // largest element used for pivoting + float scaling; // scaling factor in pivoting + float recippiv; // reciprocal of pivot element + float ftmp; // temporary variable used in swaps + + // initialize the pivot array to 0 + for (j = 0; j < isize; j++) + { + ipivot[j][0] = 0; + } + + // main loop i over the dimensions of the square matrix A + for (i = 0; i < isize; i++) + { + // zero the largest element found for pivoting + largest = 0.0F; + // loop over candidate rows j + for (j = 0; j < isize; j++) + { + // check if row j has been previously pivoted + if (ipivot[j][0] != 1) + { + // loop over candidate columns k + for (k = 0; k < isize; k++) + { + // check if column k has previously been pivoted + if (ipivot[k][0] == 0) + { + // check if the pivot element is the largest found so far + if (fabs(A[j][k]) >= largest) + { + // and store this location as the current best candidate for pivoting + ipivotrow = j; + ipivotcol = k; + largest = (float) fabs(A[ipivotrow][ipivotcol]); + } + } + else if (ipivot[k][0] > 1) + { + // zero determinant situation: exit with identity matrix + fmatrixAeqI(A, 3); + return; + } + } + } + } + // increment the entry in ipivot to denote it has been selected for pivoting + ipivot[ipivotcol][0]++; + + // check the pivot rows ipivotrow and ipivotcol are not the same before swapping + if (ipivotrow != ipivotcol) + { + // loop over columns l + for (l = 0; l < isize; l++) + { + // and swap all elements of rows ipivotrow and ipivotcol + ftmp = A[ipivotrow][l]; + A[ipivotrow][l] = A[ipivotcol][l]; + A[ipivotcol][l] = ftmp; + } + } + + // record that on the i-th iteration rows ipivotrow and ipivotcol were swapped + irowind[i][0] = ipivotrow; + icolind[i][0] = ipivotcol; + + // check for zero on-diagonal element (singular matrix) and return with identity matrix if detected + if (A[ipivotcol][ipivotcol] == 0.0F) + { + // zero determinant situation: exit with identity matrix + fmatrixAeqI(A, 3); + return; + } + + // calculate the reciprocal of the pivot element knowing it's non-zero + recippiv = 1.0F / A[ipivotcol][ipivotcol]; + // by definition, the diagonal element normalizes to 1 + A[ipivotcol][ipivotcol] = 1.0F; + // multiply all of row ipivotcol by the reciprocal of the pivot element including the diagonal element + // the diagonal element A[ipivotcol][ipivotcol] now has value equal to the reciprocal of its previous value + for (l = 0; l < isize; l++) + { + A[ipivotcol][l] *= recippiv; + } + // loop over all rows m of A + for (m = 0; m < isize; m++) + { + if (m != ipivotcol) + { + // scaling factor for this row m is in column ipivotcol + scaling = A[m][ipivotcol]; + // zero this element + A[m][ipivotcol] = 0.0F; + // loop over all columns l of A and perform elimination + for (l = 0; l < isize; l++) + { + A[m][l] -= A[ipivotcol][l] * scaling; + } + } + } + } // end of loop i over the matrix dimensions + + // finally, loop in inverse order to apply the missing column swaps + for (l = isize - 1; l >= 0; l--) + { + // set i and j to the two columns to be swapped + i = irowind[l][0]; + j = icolind[l][0]; + + // check that the two columns i and j to be swapped are not the same + if (i != j) + { + // loop over all rows k to swap columns i and j of A + for (k = 0; k < isize; k++) + { + ftmp = A[k][i]; + A[k][i] = A[k][j]; + A[k][j] = ftmp; + } + } + } + + return; +} + + +void ResetMagCalibration(struct MagCalibration *pthisMagCal/*, struct MagneticBuffer *pthisMagneticBuffer*/) +{ + int32 i, j, k, l; // loop counters + + for (i = 0; i < 3; i++) { + pthisMagCal->finvW[i] = pthisMagCal->xfinvW[i]; + pthisMagCal->ftrinvW[i] = pthisMagCal->xftrinvW[i]; + pthisMagCal->fA[i] = pthisMagCal->xfA[i]; + pthisMagCal->finvA[i] = pthisMagCal->xinvA[i]; + }; + + // initialize the calibration hard and soft iron estimate to null + fmatrixAeqI(pthisMagCal->finvW, 3); + pthisMagCal->fVx = pthisMagCal->fVy = pthisMagCal->fVz = 0.0F; + pthisMagCal->fB = 0.0F; + pthisMagCal->fFitErrorpc = 1000.0F; + pthisMagCal->iValidMagCal = 0; + + // set magnetic buffer index to invalid value -1 to denote invalid + /*pthisMagneticBuffer->iMagBufferCount = 0; + for (j = 0; j < MAGBUFFSIZE; j++) + { + for (k = 0; k < MAGBUFFSIZE; k++) + { + for (l = 0; l < MAGBUFFSIZE; l++) + { + pthisMagneticBuffer->index[j][k][l] = -1; + } + } + }*/ + + return; +} + +// 4 element calibration using 4x4 matrix inverse +void fUpdateCalibration4INV(struct MagCalibration *pthisMagCal, + struct MagneticBuffer *pthisMagneticBuffer, + float **ftmpA4x4, float **ftmpB4x4, float **ftmpA4x1, + float **ftmpB4x1, int32 **icolind, int32 **irowind, int32 **ipivot) +{ + int32 i, j, /*k, l,*/ m, n; // loop counters + int32 ilocalMagBufferCount; // local count of measurements for this process + float fOffsetx, fOffsety, fOffsetz; // offset to remove large DC hard iron bias in matrix + float ftmpBpx, ftmpBpy, ftmpBpz; // x, y, z magnetometer readings (uT) + float ftmpBpxsq, ftmpBpysq, ftmpBpzsq; // squares of x, y, z magnetometer readings (uT) + float fy; // dependent variable + float fYTY; // scalar equal to Y^T.Y + float fscaling; // set to FUTPERCOUNT * FMATRIXSCALING + float fP; // performance function = r^T.r + + // compute fscaling to reduce multiplications later + fscaling = FUTPERCOUNT * FMATRIXSCALING; + + // set trial inverse soft iron matrix invW to the identity matrix for 4 element calibration + pthisMagCal->ftrinvW[0][0] = pthisMagCal->ftrinvW[1][1] = pthisMagCal->ftrinvW[2][2] = 1.0F; + pthisMagCal->ftrinvW[0][1] = pthisMagCal->ftrinvW[0][2] = pthisMagCal->ftrinvW[1][0] = 0.0F; + pthisMagCal->ftrinvW[1][2] = pthisMagCal->ftrinvW[2][0] = pthisMagCal->ftrinvW[2][1] = 0.0F; + + // zero fYTY=Y^T.Y, ftmpA4x1=X^T.Y and on and above diagonal elements of ftmpA4x4=X^T*X + fYTY = 0.0F; + for (m = 0; m < 4; m++) + { + ftmpA4x1[m][0] = 0.0F; + for (n = m; n < 4; n++) + { + ftmpA4x4[m][n] = 0.0F; + } + } + + // the offsets are guaranteed to be set from the first element but to avoid compiler error + fOffsetx = fOffsety = fOffsetz = 0.0F; + + // use from MINEQUATIONS up to MAXEQUATIONS entries from magnetic buffer to compute matrices + i = 0; + for (j = 0; j < MAGBUFFSIZE; j++) + { + //for (k = 0; k < MAGBUFFSIZE; k++) + //{ + //for (l = 0; l < MAGBUFFSIZE; l++) + //{ + //if (pthisMagneticBuffer->index[j][k][l] != -1) + //{ + // use first valid magnetic buffer entry as estimate (in counts) for offset + printf("."); + if (i == 0) + { + fOffsetx = (float)pthisMagneticBuffer->iBx[j]/*[k][l]*/; + fOffsety = (float)pthisMagneticBuffer->iBy[j]/*[k][l]*/; + fOffsetz = (float)pthisMagneticBuffer->iBz[j]/*[k][l]*/; + } + + // calculate offset and scaled magnetic buffer vector data Bx, By, Bz (scaled uT) + ftmpBpx = ((float)pthisMagneticBuffer->iBx[j]/*[k][l]*/ - fOffsetx) * fscaling; + ftmpBpy = ((float)pthisMagneticBuffer->iBy[j]/*[k][l]*/ - fOffsety) * fscaling; + ftmpBpz = ((float)pthisMagneticBuffer->iBz[j]/*[k][l]*/ - fOffsetz) * fscaling; + + // calculate y = Bx^2 + By^2 + Bz^2 (scaled uT^2) and accumulate Y^T.Y + ftmpBpxsq = ftmpBpx * ftmpBpx; + ftmpBpysq = ftmpBpy * ftmpBpy; + ftmpBpzsq = ftmpBpz * ftmpBpz; + fy = ftmpBpxsq + ftmpBpysq + ftmpBpzsq; + fYTY += fy * fy; + + // accumulate ftmpA4x1 = X^T.Y + ftmpA4x1[0][0] += ftmpBpx * fy; + ftmpA4x1[1][0] += ftmpBpy * fy; + ftmpA4x1[2][0] += ftmpBpz * fy; + ftmpA4x1[3][0] += fy; + + // accumulate on and above-diagonal terms of ftmpA4x4 = X^T.X + ftmpA4x4[0][0] += ftmpBpxsq; + ftmpA4x4[0][1] += ftmpBpx * ftmpBpy; + ftmpA4x4[0][2] += ftmpBpx * ftmpBpz; + ftmpA4x4[0][3] += ftmpBpx; + ftmpA4x4[1][1] += ftmpBpysq; + ftmpA4x4[1][2] += ftmpBpy * ftmpBpz; + ftmpA4x4[1][3] += ftmpBpy; + ftmpA4x4[2][2] += ftmpBpzsq; + ftmpA4x4[2][3] += ftmpBpz; + + // increment the counter for next iteration + i++; + //} + //} + //} + } + //printf("[dbg1]"); + + + // store the number of measurements accumulated + ilocalMagBufferCount = i; + + // set the last element of the measurement matrix to the number of buffer elements used + ftmpA4x4[3][3] = (float) i; + + // use above diagonal elements of symmetric matrix ftmpA4x4 to create ftmpB4x4 = ftmpA4x4 = X^T.X + for (m = 0; m < 4; m++) + { + for (n = m; n < 4; n++) + { + ftmpB4x4[m][n] = ftmpB4x4[n][m] = ftmpA4x4[n][m] = ftmpA4x4[m][n]; + } + } + + //printf("[dbg2]"); + // calculate in situ inverse of ftmpB4x4 = inv(X^T.X) (4x4) + fmatrixAeqInvA(ftmpB4x4, 4, icolind, irowind, ipivot); + + // calculate ftmpB4x1 = solution vector beta (4x1) = inv(X^T.X).X^T.Y = ftmpB4x4 * ftmpA4x1 + fmatrixAeqBxC(ftmpB4x1, ftmpB4x4, ftmpA4x1, 4, 4, 1); + + // calculate P = r^T.r = Y^T.Y - 2 * beta^T.(X^T.Y) + beta^T.(X^T.X).beta + // = fYTY - 2 * ftmpB4x1^T.ftmpA4x1 + ftmpB4x1^T.ftmpA4x4.ftmpB4x1 + // first set P = Y^T.Y - 2 * beta^T.(X^T.Y) = fYTY - 2 * ftmpB4x1^T.ftmpA4x1 + fP = fYTY - 2.0F * (ftmpA4x1[0][0] * ftmpB4x1[0][0] + ftmpA4x1[1][0] * ftmpB4x1[1][0] + + ftmpA4x1[2][0] * ftmpB4x1[2][0] + ftmpA4x1[3][0] * ftmpB4x1[3][0]); + // set ftmpA4x1 = (X^T.X).beta = ftmpA4x4.ftmpB4x1 + fmatrixAeqBxC(ftmpA4x1, ftmpA4x4, ftmpB4x1, 4, 4, 1); + // add beta^T.(X^T.X).beta = ftmpB4x1^T * ftmpA4x1 to P + fP += ftmpA4x1[0][0] * ftmpB4x1[0][0] + ftmpA4x1[1][0] * ftmpB4x1[1][0] + + ftmpA4x1[2][0] * ftmpB4x1[2][0] + ftmpA4x1[3][0] * ftmpB4x1[3][0]; + + // compute the hard iron vector (in uT but offset and scaled by FMATRIXSCALING) + pthisMagCal->ftrVx = 0.5F * ftmpB4x1[0][0]; + pthisMagCal->ftrVy = 0.5F * ftmpB4x1[1][0]; + pthisMagCal->ftrVz = 0.5F * ftmpB4x1[2][0]; + + // compute the scaled geomagnetic field strength B (in uT but scaled by FMATRIXSCALING) + pthisMagCal->ftrB = (float)sqrt(ftmpB4x1[3][0] + pthisMagCal->ftrVx * pthisMagCal->ftrVx + + pthisMagCal->ftrVy * pthisMagCal->ftrVy + pthisMagCal->ftrVz * pthisMagCal->ftrVz); + + // calculate the trial fit error (percent) normalized to number of measurements and scaled geomagnetic field strength + pthisMagCal->ftrFitErrorpc = (float)sqrt(fP / (float) ilocalMagBufferCount) * 100.0F / + (2.0F * pthisMagCal->ftrB * pthisMagCal->ftrB); + //printf("\n\nTrial new calibration fit error=%9.4f%% versus previous %9.4f%%", pthisMagCal->ftrFitErrorpc, pthisMagCal->fFitErrorpc); + + // correct the hard iron estimate for FMATRIXSCALING and the offsets applied (result in uT) + pthisMagCal->ftrVx = pthisMagCal->ftrVx * FINVMATRIXSCALING + fOffsetx * FUTPERCOUNT; + pthisMagCal->ftrVy = pthisMagCal->ftrVy * FINVMATRIXSCALING + fOffsety * FUTPERCOUNT; + pthisMagCal->ftrVz = pthisMagCal->ftrVz * FINVMATRIXSCALING + fOffsetz * FUTPERCOUNT; + //printf("\n\nTrial new calibration hard iron (uT) Vx=%9.3f Vy=%9.3f Vz=%9.3f", pthisMagCal->ftrVx, pthisMagCal->ftrVy, pthisMagCal->ftrVz); + + // correct the geomagnetic field strength B to correct scaling (result in uT) + pthisMagCal->ftrB *= FINVMATRIXSCALING; + //printf("\n\nTrial new calibration geomagnetic field (uT) B=%9.3f", pthisMagCal->ftrB); + + // set the valid calibration flag to true + pthisMagCal->iValidMagCal = 1; + + return; +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/magnetic.h Thu Apr 10 07:48:46 2014 +0000 @@ -0,0 +1,63 @@ +// Source: eCompass v3 + +#include "mbed.h" + +#ifndef _MAGNETIC_H_ +#define _MAGNETIC_H_ + +typedef int16_t int16; +typedef uint16_t uint16; +typedef int32_t int32; +typedef uint32_t uint32; + +#define MAGBUFFSIZE (216) // magnetic buffer size: 6 implies 6^3 = 216 entries +#define FINVMATRIXSCALING (50.0F) // inverse of FMATRIXSCALING +#define FUTPERCOUNT (0.1F) // MAG3110 and FXOS8700 provide 0.1uT per count resolution +#define FMATRIXSCALING (0.02F) // approx normalizes geomagnetic field 50uT +#define MAXMATINV 6 // maximum size supported in matrix inverse function + +// 3d arrays, dude... + +struct MagneticBuffer { + int16 iBx[MAGBUFFSIZE]/*[MAGBUFFSIZE][MAGBUFFSIZE]*/; // array of x magnetic fields + int16 iBy[MAGBUFFSIZE]/*[MAGBUFFSIZE][MAGBUFFSIZE]*/; // array of y magnetic fields + int16 iBz[MAGBUFFSIZE]/*[MAGBUFFSIZE][MAGBUFFSIZE]*/; // array of z magnetic fields + //int32 index[MAGBUFFSIZE][MAGBUFFSIZE][MAGBUFFSIZE]; // array of time indices + //int32 iMagBufferCount; // number of magnetometer readings +}; + +// magnetic calibration structure +struct MagCalibration +{ + float fVx; // x component of computed hard iron offset + float fVy; // y component of computed hard iron offset + float fVz; // z component of computed hard iron offset + float fB; // computed geomagnetic field magnitude in uT + float fFitErrorpc; // computed fit error % + float ftrVx; // trial value of x component of hard iron offset + float ftrVy; // trial value of y component of hard iron offset + float ftrVz; // trial value of z component of hard iron offset + float ftrB; // trial value of geomagnetic field magnitude in uT + float ftrFitErrorpc; // trial value of fit error % + int32 iValidMagCal; // valid magnetic calibration flag + float xfinvW[3][3]; // estimated inverse soft iron matrix size + float *finvW[3]; + float xfA[3][3]; // estimated ellipsoid matrix A + float *fA[3]; + float xinvA[3][3]; // inverse of ellipsoid matrix A + float *finvA[3]; + float xftrinvW[3][3]; // trial computed inverse soft iron matrix size + float *ftrinvW[3]; +}; + +void fUpdateCalibration4INV(struct MagCalibration *pthisMagCal, + struct MagneticBuffer *pthisMagneticBuffer, + float **ftmpA4x4, float **ftmpB4x4, float **ftmpA4x1, + float **ftmpB4x1, int32 **icolind, int32 **irowind, int32 **ipivot); + +void ResetMagCalibration(struct MagCalibration *pthisMagCal/*, struct MagneticBuffer *pthisMagneticBuffer*/); + +void magUpdateCalibration(struct MagCalibration *pthisMagCal, + struct MagneticBuffer *pthisMagneticBuffer); + +#endif \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu Apr 10 07:48:46 2014 +0000 @@ -0,0 +1,91 @@ +#include "mbed.h" +//#include "USBSerial.h" +#include "MAG3110.h" +#include "MMA8451Q.h" + +#include "magnetic.h" + +#define MMA8451_I2C_ADDRESS (0x1d<<1) +MMA8451Q acc(PTE25, PTE24, MMA8451_I2C_ADDRESS); +MAG3110 mag(PTE25, PTE24); +Serial usb_cdc(USBTX, USBRX); + +DigitalOut gLED(LED_GREEN); //PTD5 +DigitalOut rLED(LED_RED); //PTE29 + + +struct MagneticBuffer mbuf; +struct MagCalibration mcal; + +typedef struct tagV3i { + int x; + int y; + int z; +} v3i, *pv3i; + +typedef struct tagV3f { + float x; + float y; + float z; +} v3f, *pv3f; + +int main(void) { + + int i; + v3i mv; + v3f av; + + wait(0.2); + usb_cdc.baud(115200); + + //printf("Started.\r\n"); + + gLED = 1; + rLED = 1; + wait(0.2); + + //usb_cdc.printf("Reading %d mag vectors... ", MAGBUFFSIZE); + gLED = 0; + + // sample MAGBUFFSIZE vectors for cal. + for(i=0; i<MAGBUFFSIZE; i++) { + mag.getValues(&mv.x, &mv.y, &mv.z); + mbuf.iBx[i] = mv.x; + mbuf.iBy[i] = mv.y; + mbuf.iBz[i] = mv.z; + wait(0.04); + }; + + gLED = 1; + //usb_cdc.printf("done\r\n"); + wait(0.2); + + rLED = 0; + + //usb_cdc.printf("Calibrating... "); + ResetMagCalibration(&mcal); + magUpdateCalibration(&mcal, &mbuf); + //usb_cdc.printf("done\r\n"); + rLED = 1; + //printf("cal. result: % 3.2f % 3.2f % 3.2f\r\n", mcal.ftrVx, mcal.ftrVy, mcal.ftrVz); + + while(1) { + mag.getValues(&(mv.x), &(mv.y), &(mv.z)); + if(mv.x>=32768) mv.x |= 0xFFFF0000; + if(mv.y>=32768) mv.y |= 0xFFFF0000; + if(mv.z>=32768) mv.z |= 0xFFFF0000; + + av.x = acc.getAccX(); + av.y = acc.getAccY(); + av.z = acc.getAccZ(); + printf("% 3.3f % 3.3f % 3.3f % 3.3f % 3.3f % 3.3f\r\n", + mv.x*FUTPERCOUNT-mcal.ftrVx, + mv.y*FUTPERCOUNT-mcal.ftrVy, + mv.z*FUTPERCOUNT-mcal.ftrVz, + av.x, + av.y, + av.z); + wait(0.02); + + }; +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Thu Apr 10 07:48:46 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/6473597d706e \ No newline at end of file