for interfacing the sparkfun boards
Dependencies: ADXL345_I2C HMC5883L IMUfilter ITG3200_HelloWorld mbed
ITG3200HL.cpp
00001 #include "ITG3200HL.h" 00002 00003 ITG3200HL::~ITG3200HL() 00004 { 00005 delete[] output; 00006 delete gyro; 00007 } 00008 00009 void ITG3200HL::init(int calibsampls, int readsampls, float samplrate) 00010 { 00011 gyro=new ITG3200(p28,p27); 00012 gyro->setLpBandwidth(LPFBW_42HZ); 00013 output=new double[3]; 00014 calibrationsamples=calibsampls; 00015 samplerate=samplrate; 00016 readsamples=readsampls; 00017 output[0]=0; 00018 output[1]=0; 00019 output[2]=0; 00020 } 00021 00022 void ITG3200HL::calibrate() 00023 { 00024 double a_xAccumulator = 0; 00025 double a_yAccumulator = 0; 00026 double a_zAccumulator = 0; 00027 00028 //Take a number of readings and average them 00029 //to calculate the zero g offset. 00030 for (int i = 0; i < calibrationsamples; i++) { 00031 int16_t readings[3]= {gyro->getGyroX(), gyro->getGyroY(), gyro->getGyroZ()}; 00032 00033 a_xAccumulator += (int16_t) readings[0]; 00034 a_yAccumulator += (int16_t) readings[1]; 00035 a_zAccumulator += (int16_t) readings[2]; 00036 00037 wait(samplerate); 00038 00039 } 00040 00041 a_xAccumulator /= calibrationsamples; 00042 a_yAccumulator /= calibrationsamples; 00043 a_zAccumulator /= calibrationsamples; 00044 00045 //At 4mg/LSB, 250 LSBs is 1g. 00046 xBias = a_xAccumulator; 00047 yBias = a_yAccumulator; 00048 zBias=a_zAccumulator; 00049 //zBias = (a_zAccumulator - 250); 00050 00051 a_xAccumulator = 0; 00052 a_yAccumulator = 0; 00053 a_zAccumulator = 0; 00054 } 00055 00056 double* ITG3200HL::sample() 00057 { 00058 double a_xAccumulator = 0; 00059 double a_yAccumulator = 0; 00060 double a_zAccumulator = 0; 00061 for (int i=0; i<readsamples; ++i) { 00062 int16_t readings[3]= {gyro->getGyroX(), gyro->getGyroY(), gyro->getGyroZ()}; 00063 a_xAccumulator += (int16_t) readings[0]; 00064 a_yAccumulator += (int16_t) readings[1]; 00065 a_zAccumulator += (int16_t) readings[2]; 00066 } 00067 //Average the samples, remove the bias, and calculate the rotation 00068 //in rad/s 00069 output[0] = ((a_xAccumulator / readsamples) - xBias) * GYRO_GAIN; 00070 output[1] = ((a_yAccumulator / readsamples) - yBias) * GYRO_GAIN; 00071 output[2] = ((a_zAccumulator / readsamples) - zBias) * GYRO_GAIN; 00072 return output; 00073 }
Generated on Tue Jul 12 2022 19:05:53 by 1.7.2