jetfishteam / Mbed 2 deprecated sparkfun6dof

for interfacing the sparkfun boards

Dependencies:   ADXL345_I2C HMC5883L IMUfilter ITG3200_HelloWorld mbed

ITG3200HL.cpp@0:441caaf895d8, 2014-04-29 (annotated)

Committer:
sandwich
Date:
Tue Apr 29 00:04:48 2014 +0000
Revision:
0:441caaf895d8
imufilter works. 6dof

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sandwich 0:441caaf895d8 1 #include "ITG3200HL.h"
sandwich 0:441caaf895d8 2
sandwich 0:441caaf895d8 3 ITG3200HL::~ITG3200HL()
sandwich 0:441caaf895d8 4 {
sandwich 0:441caaf895d8 5 delete[] output;
sandwich 0:441caaf895d8 6 delete gyro;
sandwich 0:441caaf895d8 7 }
sandwich 0:441caaf895d8 8
sandwich 0:441caaf895d8 9 void ITG3200HL::init(int calibsampls, int readsampls, float samplrate)
sandwich 0:441caaf895d8 10 {
sandwich 0:441caaf895d8 11 gyro=new ITG3200(p28,p27);
sandwich 0:441caaf895d8 12 gyro->setLpBandwidth(LPFBW_42HZ);
sandwich 0:441caaf895d8 13 output=new double[3];
sandwich 0:441caaf895d8 14 calibrationsamples=calibsampls;
sandwich 0:441caaf895d8 15 samplerate=samplrate;
sandwich 0:441caaf895d8 16 readsamples=readsampls;
sandwich 0:441caaf895d8 17 output[0]=0;
sandwich 0:441caaf895d8 18 output[1]=0;
sandwich 0:441caaf895d8 19 output[2]=0;
sandwich 0:441caaf895d8 20 }
sandwich 0:441caaf895d8 21
sandwich 0:441caaf895d8 22 void ITG3200HL::calibrate()
sandwich 0:441caaf895d8 23 {
sandwich 0:441caaf895d8 24 double a_xAccumulator = 0;
sandwich 0:441caaf895d8 25 double a_yAccumulator = 0;
sandwich 0:441caaf895d8 26 double a_zAccumulator = 0;
sandwich 0:441caaf895d8 27
sandwich 0:441caaf895d8 28 //Take a number of readings and average them
sandwich 0:441caaf895d8 29 //to calculate the zero g offset.
sandwich 0:441caaf895d8 30 for (int i = 0; i < calibrationsamples; i++) {
sandwich 0:441caaf895d8 31 int16_t readings[3]= {gyro->getGyroX(), gyro->getGyroY(), gyro->getGyroZ()};
sandwich 0:441caaf895d8 32
sandwich 0:441caaf895d8 33 a_xAccumulator += (int16_t) readings[0];
sandwich 0:441caaf895d8 34 a_yAccumulator += (int16_t) readings[1];
sandwich 0:441caaf895d8 35 a_zAccumulator += (int16_t) readings[2];
sandwich 0:441caaf895d8 36
sandwich 0:441caaf895d8 37 wait(samplerate);
sandwich 0:441caaf895d8 38
sandwich 0:441caaf895d8 39 }
sandwich 0:441caaf895d8 40
sandwich 0:441caaf895d8 41 a_xAccumulator /= calibrationsamples;
sandwich 0:441caaf895d8 42 a_yAccumulator /= calibrationsamples;
sandwich 0:441caaf895d8 43 a_zAccumulator /= calibrationsamples;
sandwich 0:441caaf895d8 44
sandwich 0:441caaf895d8 45 //At 4mg/LSB, 250 LSBs is 1g.
sandwich 0:441caaf895d8 46 xBias = a_xAccumulator;
sandwich 0:441caaf895d8 47 yBias = a_yAccumulator;
sandwich 0:441caaf895d8 48 zBias=a_zAccumulator;
sandwich 0:441caaf895d8 49 //zBias = (a_zAccumulator - 250);
sandwich 0:441caaf895d8 50
sandwich 0:441caaf895d8 51 a_xAccumulator = 0;
sandwich 0:441caaf895d8 52 a_yAccumulator = 0;
sandwich 0:441caaf895d8 53 a_zAccumulator = 0;
sandwich 0:441caaf895d8 54 }
sandwich 0:441caaf895d8 55
sandwich 0:441caaf895d8 56 double* ITG3200HL::sample()
sandwich 0:441caaf895d8 57 {
sandwich 0:441caaf895d8 58 double a_xAccumulator = 0;
sandwich 0:441caaf895d8 59 double a_yAccumulator = 0;
sandwich 0:441caaf895d8 60 double a_zAccumulator = 0;
sandwich 0:441caaf895d8 61 for (int i=0; i<readsamples; ++i) {
sandwich 0:441caaf895d8 62 int16_t readings[3]= {gyro->getGyroX(), gyro->getGyroY(), gyro->getGyroZ()};
sandwich 0:441caaf895d8 63 a_xAccumulator += (int16_t) readings[0];
sandwich 0:441caaf895d8 64 a_yAccumulator += (int16_t) readings[1];
sandwich 0:441caaf895d8 65 a_zAccumulator += (int16_t) readings[2];
sandwich 0:441caaf895d8 66 }
sandwich 0:441caaf895d8 67 //Average the samples, remove the bias, and calculate the rotation
sandwich 0:441caaf895d8 68 //in rad/s
sandwich 0:441caaf895d8 69 output[0] = ((a_xAccumulator / readsamples) - xBias) * GYRO_GAIN;
sandwich 0:441caaf895d8 70 output[1] = ((a_yAccumulator / readsamples) - yBias) * GYRO_GAIN;
sandwich 0:441caaf895d8 71 output[2] = ((a_zAccumulator / readsamples) - zBias) * GYRO_GAIN;
sandwich 0:441caaf895d8 72 return output;
sandwich 0:441caaf895d8 73 }