ajout module_mouvement
Dependencies: mbed xbee_lib ADXL345_I2C IMUfilter ITG3200 Motor RangeFinder Servo mbos PID
Fork of Labo_TRSE_Drone by
Service/Acc_Giro.cpp
- Committer:
- arnaudsuire
- Date:
- 2014-02-26
- Revision:
- 38:7ab036d94678
- Parent:
- 37:e1ad11fe3fe4
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); }