Erik van de Coevering
/
Multicopter_2018
Latest version of my quadcopter controller with an LPC1768 and MPU9250.
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 // Coded by: Erik van de Coevering 00002 00003 #include "mbed.h" 00004 #include "MadgwickAHRS.h" 00005 #include "MahonyAHRS.h" 00006 #include "MAfilter.h" 00007 #include "MPU9250_SPI.h" 00008 #include "calccomp.h" 00009 #include "SerialBuffered.h" 00010 #include "IAP.h" 00011 #include "LPfilter.h" 00012 00013 00014 #define MEM_SIZE 256 00015 #define TARGET_SECTOR 29 // use sector 29 as target sector 00016 00017 DigitalOut led2(LED2); // leds are active low 00018 DigitalOut ch_sw(p26); 00019 00020 Serial pc(USBTX, USBRX); 00021 SerialBuffered *pc1 = new SerialBuffered( 100, USBTX, USBRX); 00022 00023 SPI spi(p11, p12, p13); 00024 mpu9250_spi mpu9250(spi, p14); 00025 00026 //Madgwick filter; 00027 Mahony filter; 00028 00029 Timer timer; 00030 Timer tijd; 00031 Timer t; 00032 Timer print; 00033 00034 LPfilter8 lp1, lp2, lp3; 00035 MAfilter10 MAgx, MAgy, MAgz; 00036 00037 IAP iap; 00038 00039 InterruptIn rx_rud(p5); 00040 InterruptIn rx_elev(p6); 00041 InterruptIn rx_thr(p7); 00042 InterruptIn rx_ail(p8); 00043 InterruptIn rx_p1(p29); 00044 InterruptIn rx_p2(p30); 00045 00046 PwmOut motor4(p24); 00047 PwmOut motor3(p23); 00048 PwmOut motor1(p22); 00049 PwmOut motor2(p21); 00050 00051 float KP_x, KI_x, KD_x; 00052 float KP_y, KI_y, KD_y; 00053 float KP_z, KI_z, KD_z; 00054 00055 Timer trx0, trx1, trx2, trx3, trx4, trx5; 00056 int rx_data[6] = {0}; 00057 bool rxd = false; 00058 00059 char mcommand; 00060 00061 void rx0() 00062 { 00063 trx0.start(); 00064 } 00065 00066 void rx1() 00067 { 00068 trx1.start(); 00069 trx0.stop(); 00070 if(trx0.read_us() > 900 && trx0.read_us() < 2200) rx_data[0] = trx0.read_us(); 00071 trx0.reset(); 00072 } 00073 00074 void rx2() 00075 { 00076 trx2.start(); 00077 trx1.stop(); 00078 if(trx1.read_us() > 900 && trx1.read_us() < 2200) rx_data[1] = trx1.read_us(); 00079 trx1.reset(); 00080 } 00081 00082 void rx3() 00083 { 00084 trx3.start(); 00085 trx2.stop(); 00086 if(trx2.read_us() > 900 && trx2.read_us() < 2200) rx_data[2] = trx2.read_us(); 00087 trx2.reset(); 00088 } 00089 00090 void rx4() 00091 { 00092 trx4.start(); 00093 trx3.stop(); 00094 if(trx3.read_us() > 900 && trx3.read_us() < 2200) rx_data[3] = trx3.read_us(); 00095 trx3.reset(); 00096 } 00097 00098 void rx5() 00099 { 00100 trx5.start(); 00101 trx4.stop(); 00102 if(trx4.read_us() > 900 && trx4.read_us() < 2200) rx_data[4] = trx4.read_us(); 00103 trx4.reset(); 00104 } 00105 00106 void rx6() 00107 { 00108 trx5.stop(); 00109 if(trx5.read_us() > 900 && trx5.read_us() < 2200) rx_data[5] = trx5.read_us(); 00110 trx5.reset(); 00111 rxd = true; 00112 } 00113 00114 00115 int main() 00116 { 00117 ch_sw = 1; 00118 led1 = 1; 00119 led2 = 0; 00120 pc1->baud(230400); 00121 pc1->setTimeout(1); 00122 pc.baud(230400); 00123 spi.frequency(4000000); 00124 00125 00126 //IAP variables 00127 char* addr = sector_start_adress[TARGET_SECTOR]; 00128 char mem[ MEM_SIZE ]; // memory, it should be aligned to word boundary 00129 char PIDsetup = 0; 00130 int r; 00131 int tempval; 00132 00133 //IMU variables 00134 float angles[6] = {0}; 00135 float time = 0; 00136 float samplef = 0; 00137 float gyrodata[3] = {0}; 00138 float acceldata[3] = {0}; 00139 float angles_temp[2] = {0}; 00140 float roll, pitch; 00141 float tempcomp[6] = {0}; 00142 float temp = 0; 00143 float temp2 = 0; 00144 float temp3 = 0; 00145 bool exit = true; 00146 float noise = 0; 00147 int count = 0; 00148 00149 //Rx variables 00150 int motor[4] = {0}; 00151 00152 // Init pwm 00153 motor1.period_ms(10); 00154 motor1.pulsewidth_us(1000); 00155 motor2.pulsewidth_us(1000); 00156 motor3.pulsewidth_us(1000); 00157 motor4.pulsewidth_us(1000); 00158 00159 filter.begin(1500); 00160 00161 pc.putc('c'); 00162 PIDsetup = pc1->getc(); 00163 if(PIDsetup == 'c') { 00164 while(1) { 00165 PIDsetup = pc1->getc(); 00166 if(PIDsetup == 'R') { 00167 for(int i=0; i<18; i++) { 00168 pc1->putc(addr[i]); 00169 wait_ms(1); 00170 } 00171 } 00172 00173 if(PIDsetup == 'W') { 00174 for(int i=0; i<18; i++) { 00175 mem[i] = pc1->getc(); 00176 } 00177 iap.prepare( TARGET_SECTOR, TARGET_SECTOR ); 00178 r = iap.erase( TARGET_SECTOR, TARGET_SECTOR ); 00179 iap.prepare( TARGET_SECTOR, TARGET_SECTOR ); 00180 r = iap.write( mem, sector_start_adress[ TARGET_SECTOR ], MEM_SIZE ); 00181 } 00182 } 00183 } 00184 00185 00186 if(PIDsetup == 'W') { 00187 mpu9250.init2(1,BITS_DLPF_CFG_188HZ); 00188 pc1->setTimeout(0.01); 00189 00190 rx_rud.rise(&rx0); 00191 rx_elev.rise(&rx1); 00192 rx_thr.rise(&rx2); 00193 rx_ail.rise(&rx3); 00194 rx_p1.rise(&rx4); 00195 rx_p2.rise(&rx5); 00196 rx_p2.fall(&rx6); 00197 mcommand = 'a'; 00198 00199 while(exit) { 00200 wait_ms(1); 00201 if(mcommand == '5') { 00202 motor1.pulsewidth_us(rx_data[2] - 20); 00203 motor2.pulsewidth_us(1000); 00204 motor3.pulsewidth_us(1000); 00205 motor4.pulsewidth_us(1000); 00206 } else if(mcommand == '6') { 00207 motor1.pulsewidth_us(1000); 00208 motor2.pulsewidth_us(rx_data[2] - 20); 00209 motor3.pulsewidth_us(1000); 00210 motor4.pulsewidth_us(1000); 00211 } else if(mcommand == '3') { 00212 motor1.pulsewidth_us(1000); 00213 motor2.pulsewidth_us(1000); 00214 motor3.pulsewidth_us(rx_data[2] - 20); 00215 motor4.pulsewidth_us(1000); 00216 } else if(mcommand == '4') { 00217 motor1.pulsewidth_us(1000); 00218 motor2.pulsewidth_us(1000); 00219 motor3.pulsewidth_us(1000); 00220 motor4.pulsewidth_us(rx_data[2] - 20); 00221 } 00222 if(mcommand == 'E') exit = 0; 00223 00224 if(rxd) { 00225 led2 = !led2; 00226 rxd = false; 00227 } 00228 00229 mpu9250.read_all(); 00230 if(mpu9250.accelerometer_data[0] >= 0) noise = noise*0.99 + 0.01*mpu9250.accelerometer_data[0]; 00231 00232 if(count>100) { 00233 count = 0; 00234 pc.printf("%.2f\n", noise); 00235 mcommand = pc1->getc(); 00236 } 00237 count++; 00238 } 00239 } 00240 00241 tempval = addr[0]; 00242 tempval = tempval + (addr[1] << 8); 00243 KP_x = ((float)tempval) / 100; 00244 tempval = addr[2]; 00245 tempval = tempval + (addr[3] << 8); 00246 KI_x = ((float)tempval) / 100; 00247 tempval = addr[4]; 00248 tempval = tempval + (addr[5] << 8); 00249 KD_x = ((float)tempval) / 100; 00250 00251 tempval = addr[6]; 00252 tempval = tempval + (addr[7] << 8); 00253 KP_y = ((float)tempval) / 100; 00254 tempval = addr[8]; 00255 tempval = tempval + (addr[9] << 8); 00256 KI_y = ((float)tempval) / 100; 00257 tempval = addr[10]; 00258 tempval = tempval + (addr[11] << 8); 00259 KD_y = ((float)tempval) / 100; 00260 00261 tempval = addr[12]; 00262 tempval = tempval + (addr[13] << 8); 00263 KP_z = ((float)tempval) / 100; 00264 tempval = addr[14]; 00265 tempval = tempval + (addr[15] << 8); 00266 KI_z = ((float)tempval) / 100; 00267 tempval = addr[16]; 00268 tempval = tempval + (addr[17] << 8); 00269 KD_z = ((float)tempval) / 100; 00270 00271 mpu9250.init(1,BITS_DLPF_CFG_188HZ); 00272 /* 00273 pc.printf("%.2f %.2f %.2f\r\n", KP_x, KI_x, KD_x); 00274 pc.printf("%.2f %.2f %.2f\r\n", KP_y, KI_y, KD_y); 00275 pc.printf("%.2f %.2f %.2f\r\n", KP_z, KI_z, KD_z); 00276 */ 00277 rx_rud.rise(&rx0); 00278 rx_elev.rise(&rx1); 00279 rx_thr.rise(&rx2); 00280 rx_ail.rise(&rx3); 00281 rx_p1.rise(&rx4); 00282 rx_p2.rise(&rx5); 00283 rx_p2.fall(&rx6); 00284 00285 t.start(); 00286 00287 while(1) { 00288 print.start(); 00289 t.stop(); 00290 time = (float)t.read(); 00291 t.reset(); 00292 t.start(); 00293 filter.invSampleFreq = time; 00294 samplef = 1/time; 00295 00296 mpu9250.read_all(); 00297 if(mpu9250.Temperature < 6.0f) temp = 6.0f; 00298 else if(mpu9250.Temperature > 60.0f) temp = 60.0f; 00299 else temp = 0.999*temp + 0.001*mpu9250.Temperature; 00300 temp2 = temp*temp; 00301 temp3 = temp2*temp; 00302 00303 // Temperature compensation 00304 tempcomp[0] = -1.77e-6*temp2 + 0.000233*temp + 0.02179; 00305 tempcomp[1] = 0.0005727*temp - 0.01488; 00306 tempcomp[2] = -2.181e-7*temp3 + 1.754e-5*temp2 - 0.0004955*temp; 00307 tempcomp[3] = -0.0002814*temp2 + 0.005545*temp - 1.4; 00308 tempcomp[4] = -3.011e-5*temp3 + 0.002823*temp2 - 0.1073*temp + 3.618; 00309 tempcomp[5] = 9.364e-5*temp2 + 0.009138*temp - 0.8939; 00310 00311 // Low pass filters on accelerometer data (calculated with the help of Matlab's FDAtool) 00312 acceldata[0] = lp1.run(mpu9250.accelerometer_data[0] - tempcomp[0]); 00313 acceldata[1] = lp2.run(mpu9250.accelerometer_data[1] - tempcomp[1]); 00314 acceldata[2] = lp3.run((mpu9250.accelerometer_data[2] - tempcomp[2])*-1); 00315 00316 // 10-term moving average to remove some noise 00317 gyrodata[0] = MAgx.run((mpu9250.gyroscope_data[0] - tempcomp[3])*-1); 00318 gyrodata[1] = MAgy.run((mpu9250.gyroscope_data[1] - tempcomp[4])*-1); 00319 gyrodata[2] = MAgz.run((mpu9250.gyroscope_data[2] - tempcomp[5])*-1); 00320 00321 // Madgwick's quaternion algorithm 00322 filter.updateIMU(gyrodata[0], gyrodata[1], gyrodata[2], acceldata[0], 00323 acceldata[1], acceldata[2]); 00324 00325 // filter.update(gyrodata[0], gyrodata[1], gyrodata[2], acceldata[0], acceldata[1], acceldata[2], mpu9250.Magnetometer[0], mpu9250.Magnetometer[1], mpu9250.Magnetometer[2]); 00326 // 00327 roll = filter.getRoll(); 00328 pitch = filter.getPitch(); 00329 00330 angles[3] = gyrodata[1]; 00331 angles[4] = gyrodata[0]; 00332 angles[5] = gyrodata[2]; 00333 00334 //Simple first order complementary filter -> TODO: CHECK STEP RESPONSE 00335 angles[0] = 0.95f*(angles[0] + (gyrodata[1]*time)) + 0.05f*pitch; 00336 angles[1] = 0.95f*(angles[1] + (gyrodata[0]*time)) + 0.05f*roll; 00337 00338 // angles[0] = pitch; 00339 // angles[1] = roll; 00340 00341 // If [VAR] is NaN use previous value 00342 if(angles[0] != angles[0]) angles[0] = angles_temp[0]; 00343 if(angles[1] != angles[1]) angles[1] = angles_temp[1]; 00344 if(angles[0] == angles[0]) angles_temp[0] = angles[0]; 00345 if(angles[1] == angles[1]) angles_temp[1] = angles[1]; 00346 00347 tijd.stop(); 00348 tijd.reset(); 00349 tijd.start(); 00350 00351 /* 00352 if(print.read_ms() > 100) { 00353 print.stop(); 00354 print.reset(); 00355 print.start(); 00356 //led2 = !led2; 00357 // pc.printf("X: %.2f Y: %.2f %.0f\r\n", angles[0], angles[1], samplef); 00358 pc.printf("%.2f %.0f\r\n", angles[1], samplef); 00359 } 00360 */ 00361 pc.printf("%.0f %.0f\r\n", pitch, roll); 00362 if(rxd) { 00363 led2 = !led2; 00364 rxd = false; 00365 // pc.printf("%d %d %d %d\r\n", rx_data[0], rx_data[1], rx_data[2], rx_data[3]); 00366 } 00367 00368 // To change VTX channel/band/power with the remote 00369 if(rx_data[5] > 1650) ch_sw = 0; 00370 else ch_sw = 1; 00371 00372 calccomp(rx_data, angles, motor); 00373 00374 motor1.pulsewidth_us(motor[0]); 00375 motor2.pulsewidth_us(motor[1]); 00376 motor3.pulsewidth_us(motor[2]); 00377 motor4.pulsewidth_us(motor[3]); 00378 /* 00379 motor1.pulsewidth_us(rx_data[2]-20); 00380 motor2.pulsewidth_us(rx_data[2]-20); 00381 motor3.pulsewidth_us(rx_data[2]-20); 00382 motor4.pulsewidth_us(rx_data[2]-20); 00383 */ 00384 } 00385 }
Generated on Fri Jul 15 2022 02:25:27 by 1.7.2