Diff: Service/Acc_Giro.cpp

Revision:

37:e1ad11fe3fe4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Service/Acc_Giro.cpp	Wed Feb 26 08:46:22 2014 +0000
@@ -0,0 +1,201 @@
+#include "Acc_Giro.h"
+
+//extern Serial pc;
+
+Acc_Giro::Acc_Giro()
+{
+    imuFilter = new IMUfilter(FILTER_RATE, 0.3);
+    accelerometer = new ADXL345_I2C(p9, p10);
+    //i2c = new I2C(p9, p10); // sda, scl
+    gyroscope = new ITG3200(p9, p10);  
+
+ 
+    calibrate   = 0;
+    calibrated  = 0;
+    start       = 0;
+    started     = 0;
+    alg_enable  = 0;   
+    
+    a_xAccumulator = 0;
+    a_yAccumulator = 0;
+    a_zAccumulator = 0;
+    w_xAccumulator = 0;
+    w_yAccumulator = 0;
+    w_zAccumulator = 0;
+    
+    accelerometerSamples = 0;
+    gyroscopeSamples = 0;
+}
+
+void Acc_Giro::initializeAccelerometer(void) {
+
+    //Go into standby mode to configure the device.
+    accelerometer->setPowerControl(0x00);
+    //Full resolution, +/-16g, 4mg/LSB.
+    accelerometer->setDataFormatControl(0x0B);
+    //200Hz data rate.
+    accelerometer->setDataRate(ADXL345_200HZ);
+    //Measurement mode.
+    accelerometer->setPowerControl(0x08);
+    //See http://www.analog.com/static/imported-files/application_notes/AN-1077.pdf
+    wait_ms(22);
+
+}
+
+void Acc_Giro::sampleAccelerometer(void) {
+//pc.printf("S");
+    //Have we taken enough samples?
+    if (accelerometerSamples == SAMPLES) {
+
+        //Average the samples, remove the bias, and calculate the acceleration
+        //in m/s/s.
+        a_x = ((a_xAccumulator / SAMPLES) - a_xBias) * ACCELEROMETER_GAIN;
+        a_y = ((a_yAccumulator / SAMPLES) - a_yBias) * ACCELEROMETER_GAIN;
+        a_z = ((a_zAccumulator / SAMPLES) - a_zBias) * ACCELEROMETER_GAIN;
+
+        a_xAccumulator = 0;
+        a_yAccumulator = 0;
+        a_zAccumulator = 0;
+        accelerometerSamples = 0;
+
+    } else {
+        //Take another sample.
+        accelerometer->getOutput(readings);
+
+        a_xAccumulator += (int16_t) readings[0];
+        a_yAccumulator += (int16_t) readings[1];
+        a_zAccumulator += (int16_t) readings[2];
+
+        accelerometerSamples++;
+    }
+}
+
+void Acc_Giro::initializeGyroscope(void) {
+
+    //Low pass filter bandwidth of 42Hz.
+    gyroscope->setLpBandwidth(LPFBW_42HZ);
+    //Internal sample rate of 200Hz. (1kHz / 5).
+    gyroscope->setSampleRateDivider(4);
+
+}
+
+void Acc_Giro::calibrateGyroscope(void) {
+
+    w_xAccumulator = 0;
+    w_yAccumulator = 0;
+    w_zAccumulator = 0;
+
+    //Take a number of readings and average them
+    //to calculate the gyroscope bias offset.
+    for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
+
+        w_xAccumulator += gyroscope->getGyroX();
+        w_yAccumulator += gyroscope->getGyroY();
+        w_zAccumulator += gyroscope->getGyroZ();
+        wait(GYRO_RATE);
+
+    }
+
+    //Average the samples.
+    w_xAccumulator /= CALIBRATION_SAMPLES;
+    w_yAccumulator /= CALIBRATION_SAMPLES;
+    w_zAccumulator /= CALIBRATION_SAMPLES;
+
+    w_xBias = w_xAccumulator;
+    w_yBias = w_yAccumulator;
+    w_zBias = w_zAccumulator;
+
+    w_xAccumulator = 0;
+    w_yAccumulator = 0;
+    w_zAccumulator = 0;
+
+}
+
+void Acc_Giro::calibrateAccelerometer(void) {
+
+    a_xAccumulator = 0;
+    a_yAccumulator = 0;
+    a_zAccumulator = 0;
+
+    //Take a number of readings and average them
+    //to calculate the zero g offset.
+    for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
+
+        accelerometer->getOutput(readings);
+
+        a_xAccumulator += (int16_t) readings[0];
+        a_yAccumulator += (int16_t) readings[1];
+        a_zAccumulator += (int16_t) readings[2];
+
+        wait(ACC_RATE);
+
+    }
+
+    a_xAccumulator /= CALIBRATION_SAMPLES;
+    a_yAccumulator /= CALIBRATION_SAMPLES;
+    a_zAccumulator /= CALIBRATION_SAMPLES;
+
+    //At 4mg/LSB, 250 LSBs is 1g.
+    a_xBias = a_xAccumulator;
+    a_yBias = a_yAccumulator;
+    a_zBias = (a_zAccumulator - 250);
+
+    a_xAccumulator = 0;
+    a_yAccumulator = 0;
+    a_zAccumulator = 0;
+}
+
+void Acc_Giro::sampleGyroscope(void) {
+
+    //Have we taken enough samples?
+    if (gyroscopeSamples == SAMPLES) {
+
+        //Average the samples, remove the bias, and calculate the angular
+        //velocity in rad/s.
+        w_x = toRadians(((w_xAccumulator / SAMPLES) - w_xBias) * GYROSCOPE_GAIN);
+        w_y = toRadians(((w_yAccumulator / SAMPLES) - w_yBias) * GYROSCOPE_GAIN);
+        w_z = toRadians(((w_zAccumulator / SAMPLES) - w_zBias) * GYROSCOPE_GAIN);
+
+        w_xAccumulator = 0;
+        w_yAccumulator = 0;
+        w_zAccumulator = 0;
+        gyroscopeSamples = 0;
+
+    } else {
+        //Take another sample.
+        w_xAccumulator += gyroscope->getGyroX();
+        w_yAccumulator += gyroscope->getGyroY();
+        w_zAccumulator += gyroscope->getGyroZ();
+
+        gyroscopeSamples++;
+
+    }
+
+}
+
+void Acc_Giro::filter(void) {
+    //Update the filter variables.
+    imuFilter->updateFilter(w_y, w_x, w_z, a_y, a_x, a_z);
+    //Calculate the new Euler angles.
+    imuFilter->computeEuler();
+    alg_enable = 1;
+}
+
+void Acc_Giro::GetI2CAddress()
+{
+    int count = 1;
+    for (int address=0; address<256; address+=2) {
+        if (!i2c->write(address, NULL, 0)) { // 0 returned is ok    
+            char buffer [128];
+            sprintf (buffer, "%i: - %i\n",count, address);
+           // tcp_send(buffer);
+            count++;    
+        }           
+    }
+}
+
+void Acc_Giro::dataSender(void) {
+    char buffer [128];
+    //sprintf (buffer, "x:%f | y:%f | am1:%f | am3:%f\r\n",toDegrees(imuFilter.getRoll()),toDegrees(imuFilter.getPitch()),a_motor_1, a_motor_3);
+    //tcp_send(buffer);
+}
\ No newline at end of file