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@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?
User | Revision | Line number | New 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 | } |