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 }