Arnaud Suire / Labo_TRSE_Drone

ajout module_mouvement

Dependencies:   mbed xbee_lib ADXL345_I2C IMUfilter ITG3200 Motor RangeFinder Servo mbos PID

Fork of Labo_TRSE_Drone by HERBERT Nicolas

Service/Acc_Giro.cpp@38:7ab036d94678, 2014-02-26 (annotated)

Committer:
arnaudsuire
Date:
Wed Feb 26 08:47:14 2014 +0000
Revision:
38:7ab036d94678
Parent:
37:e1ad11fe3fe4 
arnaud

Who changed what in which revision?

UserRevisionLine numberNew contents of line
arnaudsuire 37:e1ad11fe3fe4 1 #include "Acc_Giro.h"
arnaudsuire 37:e1ad11fe3fe4 2
arnaudsuire 37:e1ad11fe3fe4 3 //extern Serial pc;
arnaudsuire 37:e1ad11fe3fe4 4
arnaudsuire 37:e1ad11fe3fe4 5 Acc_Giro::Acc_Giro()
arnaudsuire 37:e1ad11fe3fe4 6 {
arnaudsuire 37:e1ad11fe3fe4 7 imuFilter = new IMUfilter(FILTER_RATE, 0.3);
arnaudsuire 37:e1ad11fe3fe4 8 accelerometer = new ADXL345_I2C(p9, p10);
arnaudsuire 37:e1ad11fe3fe4 9 //i2c = new I2C(p9, p10); // sda, scl
arnaudsuire 37:e1ad11fe3fe4 10 gyroscope = new ITG3200(p9, p10);
arnaudsuire 37:e1ad11fe3fe4 11
arnaudsuire 37:e1ad11fe3fe4 12
arnaudsuire 37:e1ad11fe3fe4 13 calibrate = 0;
arnaudsuire 37:e1ad11fe3fe4 14 calibrated = 0;
arnaudsuire 37:e1ad11fe3fe4 15 start = 0;
arnaudsuire 37:e1ad11fe3fe4 16 started = 0;
arnaudsuire 37:e1ad11fe3fe4 17 alg_enable = 0;
arnaudsuire 37:e1ad11fe3fe4 18
arnaudsuire 37:e1ad11fe3fe4 19 a_xAccumulator = 0;
arnaudsuire 37:e1ad11fe3fe4 20 a_yAccumulator = 0;
arnaudsuire 37:e1ad11fe3fe4 21 a_zAccumulator = 0;
arnaudsuire 37:e1ad11fe3fe4 22 w_xAccumulator = 0;
arnaudsuire 37:e1ad11fe3fe4 23 w_yAccumulator = 0;
arnaudsuire 37:e1ad11fe3fe4 24 w_zAccumulator = 0;
arnaudsuire 37:e1ad11fe3fe4 25
arnaudsuire 37:e1ad11fe3fe4 26 accelerometerSamples = 0;
arnaudsuire 37:e1ad11fe3fe4 27 gyroscopeSamples = 0;
arnaudsuire 37:e1ad11fe3fe4 28 }
arnaudsuire 37:e1ad11fe3fe4 29
arnaudsuire 37:e1ad11fe3fe4 30 void Acc_Giro::initializeAccelerometer(void) {
arnaudsuire 37:e1ad11fe3fe4 31
arnaudsuire 37:e1ad11fe3fe4 32 //Go into standby mode to configure the device.
arnaudsuire 37:e1ad11fe3fe4 33 accelerometer->setPowerControl(0x00);
arnaudsuire 37:e1ad11fe3fe4 34 //Full resolution, +/-16g, 4mg/LSB.
arnaudsuire 37:e1ad11fe3fe4 35 accelerometer->setDataFormatControl(0x0B);
arnaudsuire 37:e1ad11fe3fe4 36 //200Hz data rate.
arnaudsuire 37:e1ad11fe3fe4 37 accelerometer->setDataRate(ADXL345_200HZ);
arnaudsuire 37:e1ad11fe3fe4 38 //Measurement mode.
arnaudsuire 37:e1ad11fe3fe4 39 accelerometer->setPowerControl(0x08);
arnaudsuire 37:e1ad11fe3fe4 40 //See http://www.analog.com/static/imported-files/application_notes/AN-1077.pdf
arnaudsuire 37:e1ad11fe3fe4 41 wait_ms(22);
arnaudsuire 37:e1ad11fe3fe4 42
arnaudsuire 37:e1ad11fe3fe4 43 }
arnaudsuire 37:e1ad11fe3fe4 44
arnaudsuire 37:e1ad11fe3fe4 45 void Acc_Giro::sampleAccelerometer(void) {
arnaudsuire 37:e1ad11fe3fe4 46 //pc.printf("S");
arnaudsuire 37:e1ad11fe3fe4 47 //Have we taken enough samples?
arnaudsuire 37:e1ad11fe3fe4 48 if (accelerometerSamples == SAMPLES) {
arnaudsuire 37:e1ad11fe3fe4 49
arnaudsuire 37:e1ad11fe3fe4 50 //Average the samples, remove the bias, and calculate the acceleration
arnaudsuire 37:e1ad11fe3fe4 51 //in m/s/s.
arnaudsuire 37:e1ad11fe3fe4 52 a_x = ((a_xAccumulator / SAMPLES) - a_xBias) * ACCELEROMETER_GAIN;
arnaudsuire 37:e1ad11fe3fe4 53 a_y = ((a_yAccumulator / SAMPLES) - a_yBias) * ACCELEROMETER_GAIN;
arnaudsuire 37:e1ad11fe3fe4 54 a_z = ((a_zAccumulator / SAMPLES) - a_zBias) * ACCELEROMETER_GAIN;
arnaudsuire 37:e1ad11fe3fe4 55
arnaudsuire 37:e1ad11fe3fe4 56 a_xAccumulator = 0;
arnaudsuire 37:e1ad11fe3fe4 57 a_yAccumulator = 0;
arnaudsuire 37:e1ad11fe3fe4 58 a_zAccumulator = 0;
arnaudsuire 37:e1ad11fe3fe4 59 accelerometerSamples = 0;
arnaudsuire 37:e1ad11fe3fe4 60
arnaudsuire 37:e1ad11fe3fe4 61 } else {
arnaudsuire 37:e1ad11fe3fe4 62 //Take another sample.
arnaudsuire 37:e1ad11fe3fe4 63 accelerometer->getOutput(readings);
arnaudsuire 37:e1ad11fe3fe4 64
arnaudsuire 37:e1ad11fe3fe4 65 a_xAccumulator += (int16_t) readings[0];
arnaudsuire 37:e1ad11fe3fe4 66 a_yAccumulator += (int16_t) readings[1];
arnaudsuire 37:e1ad11fe3fe4 67 a_zAccumulator += (int16_t) readings[2];
arnaudsuire 37:e1ad11fe3fe4 68
arnaudsuire 37:e1ad11fe3fe4 69 accelerometerSamples++;
arnaudsuire 37:e1ad11fe3fe4 70 }
arnaudsuire 37:e1ad11fe3fe4 71 }
arnaudsuire 37:e1ad11fe3fe4 72
arnaudsuire 37:e1ad11fe3fe4 73 void Acc_Giro::initializeGyroscope(void) {
arnaudsuire 37:e1ad11fe3fe4 74
arnaudsuire 37:e1ad11fe3fe4 75 //Low pass filter bandwidth of 42Hz.
arnaudsuire 37:e1ad11fe3fe4 76 gyroscope->setLpBandwidth(LPFBW_42HZ);
arnaudsuire 37:e1ad11fe3fe4 77 //Internal sample rate of 200Hz. (1kHz / 5).
arnaudsuire 37:e1ad11fe3fe4 78 gyroscope->setSampleRateDivider(4);
arnaudsuire 37:e1ad11fe3fe4 79
arnaudsuire 37:e1ad11fe3fe4 80 }
arnaudsuire 37:e1ad11fe3fe4 81
arnaudsuire 37:e1ad11fe3fe4 82 void Acc_Giro::calibrateGyroscope(void) {
arnaudsuire 37:e1ad11fe3fe4 83
arnaudsuire 37:e1ad11fe3fe4 84 w_xAccumulator = 0;
arnaudsuire 37:e1ad11fe3fe4 85 w_yAccumulator = 0;
arnaudsuire 37:e1ad11fe3fe4 86 w_zAccumulator = 0;
arnaudsuire 37:e1ad11fe3fe4 87
arnaudsuire 37:e1ad11fe3fe4 88 //Take a number of readings and average them
arnaudsuire 37:e1ad11fe3fe4 89 //to calculate the gyroscope bias offset.
arnaudsuire 37:e1ad11fe3fe4 90 for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
arnaudsuire 37:e1ad11fe3fe4 91
arnaudsuire 37:e1ad11fe3fe4 92 w_xAccumulator += gyroscope->getGyroX();
arnaudsuire 37:e1ad11fe3fe4 93 w_yAccumulator += gyroscope->getGyroY();
arnaudsuire 37:e1ad11fe3fe4 94 w_zAccumulator += gyroscope->getGyroZ();
arnaudsuire 37:e1ad11fe3fe4 95 wait(GYRO_RATE);
arnaudsuire 37:e1ad11fe3fe4 96
arnaudsuire 37:e1ad11fe3fe4 97 }
arnaudsuire 37:e1ad11fe3fe4 98
arnaudsuire 37:e1ad11fe3fe4 99 //Average the samples.
arnaudsuire 37:e1ad11fe3fe4 100 w_xAccumulator /= CALIBRATION_SAMPLES;
arnaudsuire 37:e1ad11fe3fe4 101 w_yAccumulator /= CALIBRATION_SAMPLES;
arnaudsuire 37:e1ad11fe3fe4 102 w_zAccumulator /= CALIBRATION_SAMPLES;
arnaudsuire 37:e1ad11fe3fe4 103
arnaudsuire 37:e1ad11fe3fe4 104 w_xBias = w_xAccumulator;
arnaudsuire 37:e1ad11fe3fe4 105 w_yBias = w_yAccumulator;
arnaudsuire 37:e1ad11fe3fe4 106 w_zBias = w_zAccumulator;
arnaudsuire 37:e1ad11fe3fe4 107
arnaudsuire 37:e1ad11fe3fe4 108 w_xAccumulator = 0;
arnaudsuire 37:e1ad11fe3fe4 109 w_yAccumulator = 0;
arnaudsuire 37:e1ad11fe3fe4 110 w_zAccumulator = 0;
arnaudsuire 37:e1ad11fe3fe4 111
arnaudsuire 37:e1ad11fe3fe4 112 }
arnaudsuire 37:e1ad11fe3fe4 113
arnaudsuire 37:e1ad11fe3fe4 114 void Acc_Giro::calibrateAccelerometer(void) {
arnaudsuire 37:e1ad11fe3fe4 115
arnaudsuire 37:e1ad11fe3fe4 116 a_xAccumulator = 0;
arnaudsuire 37:e1ad11fe3fe4 117 a_yAccumulator = 0;
arnaudsuire 37:e1ad11fe3fe4 118 a_zAccumulator = 0;
arnaudsuire 37:e1ad11fe3fe4 119
arnaudsuire 37:e1ad11fe3fe4 120 //Take a number of readings and average them
arnaudsuire 37:e1ad11fe3fe4 121 //to calculate the zero g offset.
arnaudsuire 37:e1ad11fe3fe4 122 for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
arnaudsuire 37:e1ad11fe3fe4 123
arnaudsuire 37:e1ad11fe3fe4 124 accelerometer->getOutput(readings);
arnaudsuire 37:e1ad11fe3fe4 125
arnaudsuire 37:e1ad11fe3fe4 126 a_xAccumulator += (int16_t) readings[0];
arnaudsuire 37:e1ad11fe3fe4 127 a_yAccumulator += (int16_t) readings[1];
arnaudsuire 37:e1ad11fe3fe4 128 a_zAccumulator += (int16_t) readings[2];
arnaudsuire 37:e1ad11fe3fe4 129
arnaudsuire 37:e1ad11fe3fe4 130 wait(ACC_RATE);
arnaudsuire 37:e1ad11fe3fe4 131
arnaudsuire 37:e1ad11fe3fe4 132 }
arnaudsuire 37:e1ad11fe3fe4 133
arnaudsuire 37:e1ad11fe3fe4 134 a_xAccumulator /= CALIBRATION_SAMPLES;
arnaudsuire 37:e1ad11fe3fe4 135 a_yAccumulator /= CALIBRATION_SAMPLES;
arnaudsuire 37:e1ad11fe3fe4 136 a_zAccumulator /= CALIBRATION_SAMPLES;
arnaudsuire 37:e1ad11fe3fe4 137
arnaudsuire 37:e1ad11fe3fe4 138 //At 4mg/LSB, 250 LSBs is 1g.
arnaudsuire 37:e1ad11fe3fe4 139 a_xBias = a_xAccumulator;
arnaudsuire 37:e1ad11fe3fe4 140 a_yBias = a_yAccumulator;
arnaudsuire 37:e1ad11fe3fe4 141 a_zBias = (a_zAccumulator - 250);
arnaudsuire 37:e1ad11fe3fe4 142
arnaudsuire 37:e1ad11fe3fe4 143 a_xAccumulator = 0;
arnaudsuire 37:e1ad11fe3fe4 144 a_yAccumulator = 0;
arnaudsuire 37:e1ad11fe3fe4 145 a_zAccumulator = 0;
arnaudsuire 37:e1ad11fe3fe4 146 }
arnaudsuire 37:e1ad11fe3fe4 147
arnaudsuire 37:e1ad11fe3fe4 148 void Acc_Giro::sampleGyroscope(void) {
arnaudsuire 37:e1ad11fe3fe4 149
arnaudsuire 37:e1ad11fe3fe4 150 //Have we taken enough samples?
arnaudsuire 37:e1ad11fe3fe4 151 if (gyroscopeSamples == SAMPLES) {
arnaudsuire 37:e1ad11fe3fe4 152
arnaudsuire 37:e1ad11fe3fe4 153 //Average the samples, remove the bias, and calculate the angular
arnaudsuire 37:e1ad11fe3fe4 154 //velocity in rad/s.
arnaudsuire 37:e1ad11fe3fe4 155 w_x = toRadians(((w_xAccumulator / SAMPLES) - w_xBias) * GYROSCOPE_GAIN);
arnaudsuire 37:e1ad11fe3fe4 156 w_y = toRadians(((w_yAccumulator / SAMPLES) - w_yBias) * GYROSCOPE_GAIN);
arnaudsuire 37:e1ad11fe3fe4 157 w_z = toRadians(((w_zAccumulator / SAMPLES) - w_zBias) * GYROSCOPE_GAIN);
arnaudsuire 37:e1ad11fe3fe4 158
arnaudsuire 37:e1ad11fe3fe4 159 w_xAccumulator = 0;
arnaudsuire 37:e1ad11fe3fe4 160 w_yAccumulator = 0;
arnaudsuire 37:e1ad11fe3fe4 161 w_zAccumulator = 0;
arnaudsuire 37:e1ad11fe3fe4 162 gyroscopeSamples = 0;
arnaudsuire 37:e1ad11fe3fe4 163
arnaudsuire 37:e1ad11fe3fe4 164 } else {
arnaudsuire 37:e1ad11fe3fe4 165 //Take another sample.
arnaudsuire 37:e1ad11fe3fe4 166 w_xAccumulator += gyroscope->getGyroX();
arnaudsuire 37:e1ad11fe3fe4 167 w_yAccumulator += gyroscope->getGyroY();
arnaudsuire 37:e1ad11fe3fe4 168 w_zAccumulator += gyroscope->getGyroZ();
arnaudsuire 37:e1ad11fe3fe4 169
arnaudsuire 37:e1ad11fe3fe4 170 gyroscopeSamples++;
arnaudsuire 37:e1ad11fe3fe4 171
arnaudsuire 37:e1ad11fe3fe4 172 }
arnaudsuire 37:e1ad11fe3fe4 173
arnaudsuire 37:e1ad11fe3fe4 174 }
arnaudsuire 37:e1ad11fe3fe4 175
arnaudsuire 37:e1ad11fe3fe4 176 void Acc_Giro::filter(void) {
arnaudsuire 37:e1ad11fe3fe4 177 //Update the filter variables.
arnaudsuire 37:e1ad11fe3fe4 178 imuFilter->updateFilter(w_y, w_x, w_z, a_y, a_x, a_z);
arnaudsuire 37:e1ad11fe3fe4 179 //Calculate the new Euler angles.
arnaudsuire 37:e1ad11fe3fe4 180 imuFilter->computeEuler();
arnaudsuire 37:e1ad11fe3fe4 181 alg_enable = 1;
arnaudsuire 37:e1ad11fe3fe4 182 }
arnaudsuire 37:e1ad11fe3fe4 183
arnaudsuire 37:e1ad11fe3fe4 184 void Acc_Giro::GetI2CAddress()
arnaudsuire 37:e1ad11fe3fe4 185 {
arnaudsuire 37:e1ad11fe3fe4 186 int count = 1;
arnaudsuire 37:e1ad11fe3fe4 187 for (int address=0; address<256; address+=2) {
arnaudsuire 37:e1ad11fe3fe4 188 if (!i2c->write(address, NULL, 0)) { // 0 returned is ok
arnaudsuire 37:e1ad11fe3fe4 189 char buffer [128];
arnaudsuire 37:e1ad11fe3fe4 190 sprintf (buffer, "%i: - %i\n",count, address);
arnaudsuire 37:e1ad11fe3fe4 191 // tcp_send(buffer);
arnaudsuire 37:e1ad11fe3fe4 192 count++;
arnaudsuire 37:e1ad11fe3fe4 193 }
arnaudsuire 37:e1ad11fe3fe4 194 }
arnaudsuire 37:e1ad11fe3fe4 195 }
arnaudsuire 37:e1ad11fe3fe4 196
arnaudsuire 37:e1ad11fe3fe4 197 void Acc_Giro::dataSender(void) {
arnaudsuire 37:e1ad11fe3fe4 198 char buffer [128];
arnaudsuire 37:e1ad11fe3fe4 199 //sprintf (buffer, "x:%f | y:%f | am1:%f | am3:%f\r\n",toDegrees(imuFilter.getRoll()),toDegrees(imuFilter.getPitch()),a_motor_1, a_motor_3);
arnaudsuire 37:e1ad11fe3fe4 200 //tcp_send(buffer);
arnaudsuire 37:e1ad11fe3fe4 201 }