17 #include <propeller.h>
26 void imu_calibrateAG()
28 unsigned char data[6] = {0, 0, 0, 0, 0, 0};
29 unsigned char samples = 0;
31 int ax, ay, az, gx, gy, gz;
32 int aBiasRawTemp[3] = {0, 0, 0};
33 int gBiasRawTemp[3] = {0, 0, 0};
37 imu_SPIreadBytes(__pinAG, CTRL_REG9, &tempF, 1);
39 imu_SPIwriteByte(__pinAG, CTRL_REG9, tempF);
40 imu_SPIwriteByte(__pinAG, FIFO_CTRL, (((FIFO_THS & 0x7) << 5) | 0x1F));
43 while (samples < 0x1F)
46 imu_SPIreadBytes(__pinAG, FIFO_SRC, &tempS, 1);
47 samples = tempS & 0x3F;
50 for(ii = 0; ii < samples ; ii++)
53 imu_readGyro(&gx, &gy, &gz);
54 gBiasRawTemp[0] += gx;
55 gBiasRawTemp[1] += gy;
56 gBiasRawTemp[2] += gz;
57 imu_readAccel(&ax, &ay, &az);
58 aBiasRawTemp[0] += ax;
59 aBiasRawTemp[1] += ay;
60 aBiasRawTemp[2] += az - ((int) __aRes);
63 for (ii = 0; ii < 3; ii++)
65 gBiasRawTemp[ii] = gBiasRawTemp[ii] / samples;
66 aBiasRawTemp[ii] = aBiasRawTemp[ii] / samples;
69 imu_setGyroCalibration(gBiasRawTemp[0], gBiasRawTemp[1], gBiasRawTemp[2]);
70 imu_setAccelCalibration(aBiasRawTemp[0], aBiasRawTemp[1], aBiasRawTemp[2]);
75 imu_SPIreadBytes(__pinAG, CTRL_REG9, &tempF, 1);
77 imu_SPIwriteByte(__pinAG, CTRL_REG9, tempF);
78 imu_SPIwriteByte(__pinAG, FIFO_CTRL, ((FIFO_OFF & 0x7) << 5));