void Cfilterbegin(){ //Turning on the accelerometer Accel.init(ADXL345_ADDR_ALT_LOW); Accel.set_bw(ADXL345_BW_12); gyro.reset(); // Use ITG3200_ADDR_AD0_HIGH or ITG3200_ADDR_AD0_LOW as the ITG3200 address // depending on how AD0 is connected on your breakout board, check its schematics for details gyro.init(ITG3200_ADDR_AD0_LOW); Serial.print("zeroCalibrating..."); gyro.zeroCalibrate(2500,2); Serial.println("done."); alpha = float(TIME_CONSTANT) / (float(TIME_CONSTANT) + float(SAMPLE_RATE)); // calculate the scaling coefficent Serial.print("Scaling Coefficent: "); Serial.println(alpha); delay(100); }
int main(int argc, char **argv) { const char *fileName = "/dev/i2c-1"; int countdown = 0; int blink = 1; float timeStep = 83.33; int printTerminal = 1; int fd;// File description string buttonInput; float lastBlink = 0.0f; bool isOnLED = false; float timeStamp = 0.0f; int delay = 0; CTimeClass timer; Logger logger; ADXL345 acc; BMP085 baro; L3G4200D gyro; HMC5883L compass; GPIOClass gpioLED("17"); GPIOClass gpioButton("4"); // init GPIOs gpioLED.export_gpio(); gpioButton.export_gpio(); gpioLED.setdir_gpio("out"); //GPIO4 set to output gpioButton.setdir_gpio("in"); // GPIO17 set to input // switch off LED just in case gpioLED.setval_gpio("0"); // parse input arguments if( argc > 1 ) { parseArgv( countdown, blink, timeStep, printTerminal, argc, argv ); printf("c = %d, b = %d, t = %f, p = %d\n", countdown, blink, timeStep, printTerminal); } // init i2c if(!init(&fd, fileName)) { exit(1); } // wait for a little usleep(100000); // init imu acc.init(&fd, ((int) ((1000/timeStep)+0.5)), timeStep); baro.init(&fd); compass.init(&fd); gyro.init(&fd, ((int) ((1000/timeStep)+0.5))); if( countdown > 0 ) { waitUntilButton(gpioButton, gpioLED, countdown, blink ); } timer.init(); if( logger.openLogFile( timeStep, timer ) == false ) { exit( 1 ); } printf("GO\n"); for(;;) { // read current Time timer.computeStartTime(); getIMUReadings( acc, baro, compass, gyro ); timer.computeTime( timeStamp ); // print data to file MATLAB: 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 fprintf(logger.fp, "%d, %4d %4d %4d %.2f %.2f %.2f %.2f %5.3f %5.3f %5.3f %5.3f %5.3f %d %d %d %5.3f %5.3f %5.3f %5.3f %5.3f\n", ((int) timeStamp), gyro.x, gyro.y, gyro.z, compass.heading, baro.altitude, baro.pressure / 100, baro.temperature, acc.accKalman[ X ], acc.accKalman[ Y ], acc.accKalman[ Z ], acc.acc[ Z ], acc.magnitude, acc.zeroX[X], acc.zeroX[Y], acc.zeroX[Z], acc.pitch, acc.roll, acc.deriv2[X], acc.deriv2[Y], acc.deriv2[Z]); //fprintf(logger.fp, "%d %5d %5d %5d %5.3f %5.3f %5.3f %5.3f %5.3f %5.3f\n", ((int) timeStamp), acc.raw[0], acc.raw[1], acc.raw[2], acc.acc[ 0 ], acc.acc[ 1 ], acc.acc[ 2 ], acc.accKalman[ 0 ], acc.accKalman[ 1 ], acc.accKalman[ 2 ]); //, acc.movingAverage[ 0 ], acc.movingAverage[ 1 ],acc.movingAverage[ 2 ]); if( printTerminal == 1 ) { printf("%d, %4d %4d %4d %.2f %.2f %.2f %.2f %5.3f %5.3f %5.3f %5.3f %5.3f %d %d %d %5.3f %5.3f\n", ((int) timeStamp), gyro.x, gyro.y, gyro.z, compass.heading, baro.altitude, baro.pressure / 100, baro.temperature, acc.accKalman[ X ], acc.accKalman[ Y ], acc.accKalman[ Z ], acc.acc[ Z ], acc.magnitude,acc.zeroX[X], acc.zeroX[Y], acc.zeroX[Z], acc.pitch, acc.roll); } // see if user wants to quit gpioButton.getval_gpio(buttonInput); // button pressed, so exit program if( buttonInput == "0" ) { printf("Exiting\n"); break; } timer.computeEndTime(); // delay in microseconds delay = ( timeStep * 1000 ) - timer.getElapsedMilliseconds() * 1000; // sleep by 'delay' in order to have periodic readings if( delay > 0 ) { usleep(delay); } // sleep 1ms else { usleep(1000); } // if blink is set to ON if( blink == 1 ){ static float endTime; endTime = timer.getEndTime(); blinkLED( gpioLED, endTime, lastBlink, isOnLED, 888 ); } } gpioLED.setval_gpio("0"); printf("bye bye\n"); return 0; }