SparkFun_9DOF_SensorStick
Dependencies: FatFileSystem mbed
Fork of 9DOF_SensorStick by
main.cpp
- Committer:
- higedura
- Date:
- 2012-01-25
- Revision:
- 2:3ffce3e97527
- Parent:
- 1:29713f02de29
- Child:
- 3:5b192b38b3bb
File content as of revision 2:3ffce3e97527:
#include "ADXL345_I2C.h" #include "ITG3200.h" #include "HMC5883L.h" ADXL345_I2C accelerometer(p9, p10); ITG3200 gyro(p9, p10); HMC5883L compass(p9, p10); Serial pc(USBTX, USBRX); #define N 3 int main(){ int i = 0; float dt = 0.1; float t = 0; pc.baud(9600); int bitAcc[N] = {0}; // Buffer of the accelerometer int getMag[N] = {0}; // Buffer of the compass double Acc [N] = {0}; double Gyro [N] = {0}; int Mag [N] = {0}; // *** Setting up accelerometer *** // These are here to test whether any of the initialization fails. It will print the failure. if (accelerometer.setPowerControl(0x00)){ pc.printf("didn't intitialize power control\n\r"); return 0; } // Full resolution, +/-16g, 4mg/LSB. wait(.001); if(accelerometer.setDataFormatControl(0x0B)){ pc.printf("didn't set data format\n\r"); return 0; } wait(.001); // 3.2kHz data rate. if(accelerometer.setDataRate(ADXL345_3200HZ)){ pc.printf("didn't set data rate\n\r"); return 0; } wait(.001); if(accelerometer.setPowerControl(MeasurementMode)) { pc.printf("didn't set the power control to measurement\n\r"); return 0; } // *** Setting up accelerometer *** // *** Setting up gyro *** gyro.setLpBandwidth(LPFBW_42HZ); // *** Setting up compass *** compass.setDefault(); // Wait some time for all sensors (Need at least 5ms) wait(0.1); pc.printf("\n\rStarting ADXL345, ITG3200 and HMC5883L test...\n\r"); pc.printf(" Time AccX AccY AccZ GyroX GyroY GyroZ MagX MagY MagZ\n\r"); while(1){ // Updating accelerometer and compass accelerometer.getOutput(bitAcc); compass.readData(getMag); // Transfering units (Acc[g], Gyro[deg/s], Compass[Ga]) // Calibration YAL 9DOF Acc:X+6, Y-12, Z+44 // Calibration green Acc:X+1, Y-18, Z+45 Acc[0] = ((int16_t)bitAcc[0])*0.004; Acc[1] = ((int16_t)bitAcc[1])*0.004; Acc[2] = ((int16_t)bitAcc[2])*0.004; // Calibration YAL 9DOF Gyro:X+28, Y-45, Z+34 // Calibration green Gyro:X+135, Y-12, Z-53 Gyro[0] = (gyro.getGyroX())/14.375; Gyro[1] = (gyro.getGyroY())/14.375; Gyro[2] = (gyro.getGyroZ())/14.375; // Calibration YAL 9DOF Compass:X, Y, Z Mag[0] = (int16_t)getMag[0]; Mag[1] = (int16_t)getMag[1]; Mag[2] = (int16_t)getMag[2]; // Low pass filter for acc //for ( int i=0;i<N;i++ ){ // if( -0.05<Acc[i] && Acc[i]<0.05 ){ Acc[i]=0; } //} // Low pass filter for gyro //for ( int i=0;i<N;i++ ){ // if( -1.0<Gyro[i] && Gyro[i]<1.0 ){ Gyro[i]=0; } //} pc.printf("%7.2f, %7.3f, %7.3f, %7.3f, %7.1f, %7.1f, %7.1f, %5d, %5d, %5d\n\r", t, Acc[0], Acc[1], Acc[2], Gyro[0], Gyro[1], Gyro[2], Mag[0], Mag[1], Mag[2]); t += dt; wait(dt); } }