for interfacing the sparkfun boards
Dependencies: ADXL345_I2C HMC5883L IMUfilter ITG3200_HelloWorld mbed
Diff: ITG3200HL.cpp
- Revision:
- 0:441caaf895d8
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ITG3200HL.cpp Tue Apr 29 00:04:48 2014 +0000 @@ -0,0 +1,73 @@ +#include "ITG3200HL.h" + +ITG3200HL::~ITG3200HL() +{ + delete[] output; + delete gyro; +} + +void ITG3200HL::init(int calibsampls, int readsampls, float samplrate) +{ + gyro=new ITG3200(p28,p27); + gyro->setLpBandwidth(LPFBW_42HZ); + output=new double[3]; + calibrationsamples=calibsampls; + samplerate=samplrate; + readsamples=readsampls; + output[0]=0; + output[1]=0; + output[2]=0; +} + +void ITG3200HL::calibrate() +{ + double a_xAccumulator = 0; + double a_yAccumulator = 0; + double a_zAccumulator = 0; + + //Take a number of readings and average them + //to calculate the zero g offset. + for (int i = 0; i < calibrationsamples; i++) { + int16_t readings[3]= {gyro->getGyroX(), gyro->getGyroY(), gyro->getGyroZ()}; + + a_xAccumulator += (int16_t) readings[0]; + a_yAccumulator += (int16_t) readings[1]; + a_zAccumulator += (int16_t) readings[2]; + + wait(samplerate); + + } + + a_xAccumulator /= calibrationsamples; + a_yAccumulator /= calibrationsamples; + a_zAccumulator /= calibrationsamples; + + //At 4mg/LSB, 250 LSBs is 1g. + xBias = a_xAccumulator; + yBias = a_yAccumulator; + zBias=a_zAccumulator; + //zBias = (a_zAccumulator - 250); + + a_xAccumulator = 0; + a_yAccumulator = 0; + a_zAccumulator = 0; +} + +double* ITG3200HL::sample() +{ + double a_xAccumulator = 0; + double a_yAccumulator = 0; + double a_zAccumulator = 0; + for (int i=0; i<readsamples; ++i) { + int16_t readings[3]= {gyro->getGyroX(), gyro->getGyroY(), gyro->getGyroZ()}; + a_xAccumulator += (int16_t) readings[0]; + a_yAccumulator += (int16_t) readings[1]; + a_zAccumulator += (int16_t) readings[2]; + } + //Average the samples, remove the bias, and calculate the rotation + //in rad/s + output[0] = ((a_xAccumulator / readsamples) - xBias) * GYRO_GAIN; + output[1] = ((a_yAccumulator / readsamples) - yBias) * GYRO_GAIN; + output[2] = ((a_zAccumulator / readsamples) - zBias) * GYRO_GAIN; + return output; +} \ No newline at end of file