Acc_Giro.cpp

00001 #include "Acc_Giro.h"
00002 
00003 //extern Serial pc;
00004 
00005 Acc_Giro::Acc_Giro()
00006 {
00007     imuFilter = new IMUfilter(FILTER_RATE, 0.3);
00008     accelerometer = new ADXL345_I2C(p9, p10);
00009     //i2c = new I2C(p9, p10); // sda, scl
00010     gyroscope = new ITG3200(p9, p10);  
00011 
00012  
00013     calibrate   = 0;
00014     calibrated  = 0;
00015     start       = 0;
00016     started     = 0;
00017     alg_enable  = 0;   
00018     
00019     a_xAccumulator = 0;
00020     a_yAccumulator = 0;
00021     a_zAccumulator = 0;
00022     w_xAccumulator = 0;
00023     w_yAccumulator = 0;
00024     w_zAccumulator = 0;
00025     
00026     accelerometerSamples = 0;
00027     gyroscopeSamples = 0;
00028 }
00029 
00030 void Acc_Giro::initializeAccelerometer(void) {
00031 
00032     //Go into standby mode to configure the device.
00033     accelerometer->setPowerControl(0x00);
00034     //Full resolution, +/-16g, 4mg/LSB.
00035     accelerometer->setDataFormatControl(0x0B);
00036     //200Hz data rate.
00037     accelerometer->setDataRate(ADXL345_200HZ);
00038     //Measurement mode.
00039     accelerometer->setPowerControl(0x08);
00040     //See http://www.analog.com/static/imported-files/application_notes/AN-1077.pdf
00041     wait_ms(22);
00042 
00043 }
00044 
00045 void Acc_Giro::sampleAccelerometer(void) {
00046 //pc.printf("S");
00047     //Have we taken enough samples?
00048     if (accelerometerSamples == SAMPLES) {
00049 
00050         //Average the samples, remove the bias, and calculate the acceleration
00051         //in m/s/s.
00052         a_x = ((a_xAccumulator / SAMPLES) - a_xBias) * ACCELEROMETER_GAIN;
00053         a_y = ((a_yAccumulator / SAMPLES) - a_yBias) * ACCELEROMETER_GAIN;
00054         a_z = ((a_zAccumulator / SAMPLES) - a_zBias) * ACCELEROMETER_GAIN;
00055 
00056         a_xAccumulator = 0;
00057         a_yAccumulator = 0;
00058         a_zAccumulator = 0;
00059         accelerometerSamples = 0;
00060 
00061     } else {
00062         //Take another sample.
00063         accelerometer->getOutput(readings);
00064 
00065         a_xAccumulator += (int16_t) readings[0];
00066         a_yAccumulator += (int16_t) readings[1];
00067         a_zAccumulator += (int16_t) readings[2];
00068 
00069         accelerometerSamples++;
00070     }
00071 }
00072 
00073 void Acc_Giro::initializeGyroscope(void) {
00074 
00075     //Low pass filter bandwidth of 42Hz.
00076     gyroscope->setLpBandwidth(LPFBW_42HZ);
00077     //Internal sample rate of 200Hz. (1kHz / 5).
00078     gyroscope->setSampleRateDivider(4);
00079 
00080 }
00081 
00082 void Acc_Giro::calibrateGyroscope(void) {
00083 
00084     w_xAccumulator = 0;
00085     w_yAccumulator = 0;
00086     w_zAccumulator = 0;
00087 
00088     //Take a number of readings and average them
00089     //to calculate the gyroscope bias offset.
00090     for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
00091 
00092         w_xAccumulator += gyroscope->getGyroX();
00093         w_yAccumulator += gyroscope->getGyroY();
00094         w_zAccumulator += gyroscope->getGyroZ();
00095         wait(GYRO_RATE);
00096 
00097     }
00098 
00099     //Average the samples.
00100     w_xAccumulator /= CALIBRATION_SAMPLES;
00101     w_yAccumulator /= CALIBRATION_SAMPLES;
00102     w_zAccumulator /= CALIBRATION_SAMPLES;
00103 
00104     w_xBias = w_xAccumulator;
00105     w_yBias = w_yAccumulator;
00106     w_zBias = w_zAccumulator;
00107 
00108     w_xAccumulator = 0;
00109     w_yAccumulator = 0;
00110     w_zAccumulator = 0;
00111 
00112 }
00113 
00114 void Acc_Giro::calibrateAccelerometer(void) {
00115 
00116     a_xAccumulator = 0;
00117     a_yAccumulator = 0;
00118     a_zAccumulator = 0;
00119 
00120     //Take a number of readings and average them
00121     //to calculate the zero g offset.
00122     for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
00123 
00124         accelerometer->getOutput(readings);
00125 
00126         a_xAccumulator += (int16_t) readings[0];
00127         a_yAccumulator += (int16_t) readings[1];
00128         a_zAccumulator += (int16_t) readings[2];
00129 
00130         wait(ACC_RATE);
00131 
00132     }
00133 
00134     a_xAccumulator /= CALIBRATION_SAMPLES;
00135     a_yAccumulator /= CALIBRATION_SAMPLES;
00136     a_zAccumulator /= CALIBRATION_SAMPLES;
00137 
00138     //At 4mg/LSB, 250 LSBs is 1g.
00139     a_xBias = a_xAccumulator;
00140     a_yBias = a_yAccumulator;
00141     a_zBias = (a_zAccumulator - 250);
00142 
00143     a_xAccumulator = 0;
00144     a_yAccumulator = 0;
00145     a_zAccumulator = 0;
00146 }
00147 
00148 void Acc_Giro::sampleGyroscope(void) {
00149 
00150     //Have we taken enough samples?
00151     if (gyroscopeSamples == SAMPLES) {
00152 
00153         //Average the samples, remove the bias, and calculate the angular
00154         //velocity in rad/s.
00155         w_x = toRadians(((w_xAccumulator / SAMPLES) - w_xBias) * GYROSCOPE_GAIN);
00156         w_y = toRadians(((w_yAccumulator / SAMPLES) - w_yBias) * GYROSCOPE_GAIN);
00157         w_z = toRadians(((w_zAccumulator / SAMPLES) - w_zBias) * GYROSCOPE_GAIN);
00158 
00159         w_xAccumulator = 0;
00160         w_yAccumulator = 0;
00161         w_zAccumulator = 0;
00162         gyroscopeSamples = 0;
00163 
00164     } else {
00165         //Take another sample.
00166         w_xAccumulator += gyroscope->getGyroX();
00167         w_yAccumulator += gyroscope->getGyroY();
00168         w_zAccumulator += gyroscope->getGyroZ();
00169 
00170         gyroscopeSamples++;
00171 
00172     }
00173 
00174 }
00175 
00176 void Acc_Giro::filter(void) {
00177     //Update the filter variables.
00178     imuFilter->updateFilter(w_y, w_x, w_z, a_y, a_x, a_z);
00179     //Calculate the new Euler angles.
00180     imuFilter->computeEuler();
00181     alg_enable = 1;
00182 }
00183 
00184 void Acc_Giro::GetI2CAddress()
00185 {
00186     int count = 1;
00187     for (int address=0; address<256; address+=2) {
00188         if (!i2c->write(address, NULL, 0)) { // 0 returned is ok    
00189             char buffer [128];
00190             sprintf (buffer, "%i: - %i\n",count, address);
00191            // tcp_send(buffer);
00192             count++;    
00193         }           
00194     }
00195 }
00196 
00197 void Acc_Giro::dataSender(void) {
00198     char buffer [128];
00199     //sprintf (buffer, "x:%f | y:%f | am1:%f | am3:%f\r\n",toDegrees(imuFilter.getRoll()),toDegrees(imuFilter.getPitch()),a_motor_1, a_motor_3);
00200     //tcp_send(buffer);
00201 }