File content as of revision 38:7ab036d94678:

#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);
}