ajout module_mouvement
Dependencies: mbed xbee_lib ADXL345_I2C IMUfilter ITG3200 Motor RangeFinder Servo mbos PID
Fork of Labo_TRSE_Drone by
Acc_Giro.cpp
00001 #include "Acc_Giro.h" 00002 00003 //extern Serial pc; 00004 00005 Acc_Giro::Acc_Giro() 00006 { 00007 imuFilter = new IMUfilter(FILTER_RATE, 0.3); 00008 accelerometer = new ADXL345_I2C(p9, p10); 00009 //i2c = new I2C(p9, p10); // sda, scl 00010 gyroscope = new ITG3200(p9, p10); 00011 00012 00013 calibrate = 0; 00014 calibrated = 0; 00015 start = 0; 00016 started = 0; 00017 alg_enable = 0; 00018 00019 a_xAccumulator = 0; 00020 a_yAccumulator = 0; 00021 a_zAccumulator = 0; 00022 w_xAccumulator = 0; 00023 w_yAccumulator = 0; 00024 w_zAccumulator = 0; 00025 00026 accelerometerSamples = 0; 00027 gyroscopeSamples = 0; 00028 } 00029 00030 void Acc_Giro::initializeAccelerometer(void) { 00031 00032 //Go into standby mode to configure the device. 00033 accelerometer->setPowerControl(0x00); 00034 //Full resolution, +/-16g, 4mg/LSB. 00035 accelerometer->setDataFormatControl(0x0B); 00036 //200Hz data rate. 00037 accelerometer->setDataRate(ADXL345_200HZ); 00038 //Measurement mode. 00039 accelerometer->setPowerControl(0x08); 00040 //See http://www.analog.com/static/imported-files/application_notes/AN-1077.pdf 00041 wait_ms(22); 00042 00043 } 00044 00045 void Acc_Giro::sampleAccelerometer(void) { 00046 //pc.printf("S"); 00047 //Have we taken enough samples? 00048 if (accelerometerSamples == SAMPLES) { 00049 00050 //Average the samples, remove the bias, and calculate the acceleration 00051 //in m/s/s. 00052 a_x = ((a_xAccumulator / SAMPLES) - a_xBias) * ACCELEROMETER_GAIN; 00053 a_y = ((a_yAccumulator / SAMPLES) - a_yBias) * ACCELEROMETER_GAIN; 00054 a_z = ((a_zAccumulator / SAMPLES) - a_zBias) * ACCELEROMETER_GAIN; 00055 00056 a_xAccumulator = 0; 00057 a_yAccumulator = 0; 00058 a_zAccumulator = 0; 00059 accelerometerSamples = 0; 00060 00061 } else { 00062 //Take another sample. 00063 accelerometer->getOutput(readings); 00064 00065 a_xAccumulator += (int16_t) readings[0]; 00066 a_yAccumulator += (int16_t) readings[1]; 00067 a_zAccumulator += (int16_t) readings[2]; 00068 00069 accelerometerSamples++; 00070 } 00071 } 00072 00073 void Acc_Giro::initializeGyroscope(void) { 00074 00075 //Low pass filter bandwidth of 42Hz. 00076 gyroscope->setLpBandwidth(LPFBW_42HZ); 00077 //Internal sample rate of 200Hz. (1kHz / 5). 00078 gyroscope->setSampleRateDivider(4); 00079 00080 } 00081 00082 void Acc_Giro::calibrateGyroscope(void) { 00083 00084 w_xAccumulator = 0; 00085 w_yAccumulator = 0; 00086 w_zAccumulator = 0; 00087 00088 //Take a number of readings and average them 00089 //to calculate the gyroscope bias offset. 00090 for (int i = 0; i < CALIBRATION_SAMPLES; i++) { 00091 00092 w_xAccumulator += gyroscope->getGyroX(); 00093 w_yAccumulator += gyroscope->getGyroY(); 00094 w_zAccumulator += gyroscope->getGyroZ(); 00095 wait(GYRO_RATE); 00096 00097 } 00098 00099 //Average the samples. 00100 w_xAccumulator /= CALIBRATION_SAMPLES; 00101 w_yAccumulator /= CALIBRATION_SAMPLES; 00102 w_zAccumulator /= CALIBRATION_SAMPLES; 00103 00104 w_xBias = w_xAccumulator; 00105 w_yBias = w_yAccumulator; 00106 w_zBias = w_zAccumulator; 00107 00108 w_xAccumulator = 0; 00109 w_yAccumulator = 0; 00110 w_zAccumulator = 0; 00111 00112 } 00113 00114 void Acc_Giro::calibrateAccelerometer(void) { 00115 00116 a_xAccumulator = 0; 00117 a_yAccumulator = 0; 00118 a_zAccumulator = 0; 00119 00120 //Take a number of readings and average them 00121 //to calculate the zero g offset. 00122 for (int i = 0; i < CALIBRATION_SAMPLES; i++) { 00123 00124 accelerometer->getOutput(readings); 00125 00126 a_xAccumulator += (int16_t) readings[0]; 00127 a_yAccumulator += (int16_t) readings[1]; 00128 a_zAccumulator += (int16_t) readings[2]; 00129 00130 wait(ACC_RATE); 00131 00132 } 00133 00134 a_xAccumulator /= CALIBRATION_SAMPLES; 00135 a_yAccumulator /= CALIBRATION_SAMPLES; 00136 a_zAccumulator /= CALIBRATION_SAMPLES; 00137 00138 //At 4mg/LSB, 250 LSBs is 1g. 00139 a_xBias = a_xAccumulator; 00140 a_yBias = a_yAccumulator; 00141 a_zBias = (a_zAccumulator - 250); 00142 00143 a_xAccumulator = 0; 00144 a_yAccumulator = 0; 00145 a_zAccumulator = 0; 00146 } 00147 00148 void Acc_Giro::sampleGyroscope(void) { 00149 00150 //Have we taken enough samples? 00151 if (gyroscopeSamples == SAMPLES) { 00152 00153 //Average the samples, remove the bias, and calculate the angular 00154 //velocity in rad/s. 00155 w_x = toRadians(((w_xAccumulator / SAMPLES) - w_xBias) * GYROSCOPE_GAIN); 00156 w_y = toRadians(((w_yAccumulator / SAMPLES) - w_yBias) * GYROSCOPE_GAIN); 00157 w_z = toRadians(((w_zAccumulator / SAMPLES) - w_zBias) * GYROSCOPE_GAIN); 00158 00159 w_xAccumulator = 0; 00160 w_yAccumulator = 0; 00161 w_zAccumulator = 0; 00162 gyroscopeSamples = 0; 00163 00164 } else { 00165 //Take another sample. 00166 w_xAccumulator += gyroscope->getGyroX(); 00167 w_yAccumulator += gyroscope->getGyroY(); 00168 w_zAccumulator += gyroscope->getGyroZ(); 00169 00170 gyroscopeSamples++; 00171 00172 } 00173 00174 } 00175 00176 void Acc_Giro::filter(void) { 00177 //Update the filter variables. 00178 imuFilter->updateFilter(w_y, w_x, w_z, a_y, a_x, a_z); 00179 //Calculate the new Euler angles. 00180 imuFilter->computeEuler(); 00181 alg_enable = 1; 00182 } 00183 00184 void Acc_Giro::GetI2CAddress() 00185 { 00186 int count = 1; 00187 for (int address=0; address<256; address+=2) { 00188 if (!i2c->write(address, NULL, 0)) { // 0 returned is ok 00189 char buffer [128]; 00190 sprintf (buffer, "%i: - %i\n",count, address); 00191 // tcp_send(buffer); 00192 count++; 00193 } 00194 } 00195 } 00196 00197 void Acc_Giro::dataSender(void) { 00198 char buffer [128]; 00199 //sprintf (buffer, "x:%f | y:%f | am1:%f | am3:%f\r\n",toDegrees(imuFilter.getRoll()),toDegrees(imuFilter.getPitch()),a_motor_1, a_motor_3); 00200 //tcp_send(buffer); 00201 }
Generated on Wed Jul 13 2022 00:16:44 by 1.7.2