
Latest version of my quadcopter controller with an LPC1768 and MPU9250.
Currently running on a custom PCB with 30.5 x 30.5mm mounts. There are also 2 PC apps that go with the software; one to set up the PID controller and one to balance the motors and props. If anyone is interested, send me a message and I'll upload them.
main.cpp
- Committer:
- Anaesthetix
- Date:
- 2018-07-31
- Revision:
- 8:981f7e2365b6
- Parent:
- 7:d86c41443f6d
File content as of revision 8:981f7e2365b6:
// Coded by: Erik van de Coevering #include "mbed.h" #include "MadgwickAHRS.h" #include "MahonyAHRS.h" #include "MAfilter.h" #include "MPU9250_SPI.h" #include "calccomp.h" #include "SerialBuffered.h" #include "IAP.h" #include "LPfilter.h" #define MEM_SIZE 256 #define TARGET_SECTOR 29 // use sector 29 as target sector DigitalOut led2(LED2); // leds are active low DigitalOut ch_sw(p26); Serial pc(USBTX, USBRX); SerialBuffered *pc1 = new SerialBuffered( 100, USBTX, USBRX); SPI spi(p11, p12, p13); mpu9250_spi mpu9250(spi, p14); //Madgwick filter; Mahony filter; Timer timer; Timer tijd; Timer t; Timer print; LPfilter8 lp1, lp2, lp3; MAfilter10 MAgx, MAgy, MAgz; IAP iap; InterruptIn rx_rud(p5); InterruptIn rx_elev(p6); InterruptIn rx_thr(p7); InterruptIn rx_ail(p8); InterruptIn rx_p1(p29); InterruptIn rx_p2(p30); PwmOut motor4(p24); PwmOut motor3(p23); PwmOut motor1(p22); PwmOut motor2(p21); float KP_x, KI_x, KD_x; float KP_y, KI_y, KD_y; float KP_z, KI_z, KD_z; Timer trx0, trx1, trx2, trx3, trx4, trx5; int rx_data[6] = {0}; bool rxd = false; char mcommand; void rx0() { trx0.start(); } void rx1() { trx1.start(); trx0.stop(); if(trx0.read_us() > 900 && trx0.read_us() < 2200) rx_data[0] = trx0.read_us(); trx0.reset(); } void rx2() { trx2.start(); trx1.stop(); if(trx1.read_us() > 900 && trx1.read_us() < 2200) rx_data[1] = trx1.read_us(); trx1.reset(); } void rx3() { trx3.start(); trx2.stop(); if(trx2.read_us() > 900 && trx2.read_us() < 2200) rx_data[2] = trx2.read_us(); trx2.reset(); } void rx4() { trx4.start(); trx3.stop(); if(trx3.read_us() > 900 && trx3.read_us() < 2200) rx_data[3] = trx3.read_us(); trx3.reset(); } void rx5() { trx5.start(); trx4.stop(); if(trx4.read_us() > 900 && trx4.read_us() < 2200) rx_data[4] = trx4.read_us(); trx4.reset(); } void rx6() { trx5.stop(); if(trx5.read_us() > 900 && trx5.read_us() < 2200) rx_data[5] = trx5.read_us(); trx5.reset(); rxd = true; } int main() { ch_sw = 1; led1 = 1; led2 = 0; pc1->baud(230400); pc1->setTimeout(1); pc.baud(230400); spi.frequency(4000000); //IAP variables char* addr = sector_start_adress[TARGET_SECTOR]; char mem[ MEM_SIZE ]; // memory, it should be aligned to word boundary char PIDsetup = 0; int r; int tempval; //IMU variables float angles[6] = {0}; float time = 0; float samplef = 0; float gyrodata[3] = {0}; float acceldata[3] = {0}; float angles_temp[2] = {0}; float roll, pitch; float tempcomp[6] = {0}; float temp = 0; float temp2 = 0; float temp3 = 0; bool exit = true; float noise = 0; int count = 0; //Rx variables int motor[4] = {0}; // Init pwm motor1.period_ms(10); motor1.pulsewidth_us(1000); motor2.pulsewidth_us(1000); motor3.pulsewidth_us(1000); motor4.pulsewidth_us(1000); filter.begin(1500); pc.putc('c'); PIDsetup = pc1->getc(); if(PIDsetup == 'c') { while(1) { PIDsetup = pc1->getc(); if(PIDsetup == 'R') { for(int i=0; i<18; i++) { pc1->putc(addr[i]); wait_ms(1); } } if(PIDsetup == 'W') { for(int i=0; i<18; i++) { mem[i] = pc1->getc(); } iap.prepare( TARGET_SECTOR, TARGET_SECTOR ); r = iap.erase( TARGET_SECTOR, TARGET_SECTOR ); iap.prepare( TARGET_SECTOR, TARGET_SECTOR ); r = iap.write( mem, sector_start_adress[ TARGET_SECTOR ], MEM_SIZE ); } } } if(PIDsetup == 'W') { mpu9250.init2(1,BITS_DLPF_CFG_188HZ); pc1->setTimeout(0.01); rx_rud.rise(&rx0); rx_elev.rise(&rx1); rx_thr.rise(&rx2); rx_ail.rise(&rx3); rx_p1.rise(&rx4); rx_p2.rise(&rx5); rx_p2.fall(&rx6); mcommand = 'a'; while(exit) { wait_ms(1); if(mcommand == '5') { motor1.pulsewidth_us(rx_data[2] - 20); motor2.pulsewidth_us(1000); motor3.pulsewidth_us(1000); motor4.pulsewidth_us(1000); } else if(mcommand == '6') { motor1.pulsewidth_us(1000); motor2.pulsewidth_us(rx_data[2] - 20); motor3.pulsewidth_us(1000); motor4.pulsewidth_us(1000); } else if(mcommand == '3') { motor1.pulsewidth_us(1000); motor2.pulsewidth_us(1000); motor3.pulsewidth_us(rx_data[2] - 20); motor4.pulsewidth_us(1000); } else if(mcommand == '4') { motor1.pulsewidth_us(1000); motor2.pulsewidth_us(1000); motor3.pulsewidth_us(1000); motor4.pulsewidth_us(rx_data[2] - 20); } if(mcommand == 'E') exit = 0; if(rxd) { led2 = !led2; rxd = false; } mpu9250.read_all(); if(mpu9250.accelerometer_data[0] >= 0) noise = noise*0.99 + 0.01*mpu9250.accelerometer_data[0]; if(count>100) { count = 0; pc.printf("%.2f\n", noise); mcommand = pc1->getc(); } count++; } } tempval = addr[0]; tempval = tempval + (addr[1] << 8); KP_x = ((float)tempval) / 100; tempval = addr[2]; tempval = tempval + (addr[3] << 8); KI_x = ((float)tempval) / 100; tempval = addr[4]; tempval = tempval + (addr[5] << 8); KD_x = ((float)tempval) / 100; tempval = addr[6]; tempval = tempval + (addr[7] << 8); KP_y = ((float)tempval) / 100; tempval = addr[8]; tempval = tempval + (addr[9] << 8); KI_y = ((float)tempval) / 100; tempval = addr[10]; tempval = tempval + (addr[11] << 8); KD_y = ((float)tempval) / 100; tempval = addr[12]; tempval = tempval + (addr[13] << 8); KP_z = ((float)tempval) / 100; tempval = addr[14]; tempval = tempval + (addr[15] << 8); KI_z = ((float)tempval) / 100; tempval = addr[16]; tempval = tempval + (addr[17] << 8); KD_z = ((float)tempval) / 100; mpu9250.init(1,BITS_DLPF_CFG_188HZ); /* pc.printf("%.2f %.2f %.2f\r\n", KP_x, KI_x, KD_x); pc.printf("%.2f %.2f %.2f\r\n", KP_y, KI_y, KD_y); pc.printf("%.2f %.2f %.2f\r\n", KP_z, KI_z, KD_z); */ rx_rud.rise(&rx0); rx_elev.rise(&rx1); rx_thr.rise(&rx2); rx_ail.rise(&rx3); rx_p1.rise(&rx4); rx_p2.rise(&rx5); rx_p2.fall(&rx6); t.start(); while(1) { print.start(); t.stop(); time = (float)t.read(); t.reset(); t.start(); filter.invSampleFreq = time; samplef = 1/time; mpu9250.read_all(); if(mpu9250.Temperature < 6.0f) temp = 6.0f; else if(mpu9250.Temperature > 60.0f) temp = 60.0f; else temp = 0.999*temp + 0.001*mpu9250.Temperature; temp2 = temp*temp; temp3 = temp2*temp; // Temperature compensation tempcomp[0] = -1.77e-6*temp2 + 0.000233*temp + 0.02179; tempcomp[1] = 0.0005727*temp - 0.01488; tempcomp[2] = -2.181e-7*temp3 + 1.754e-5*temp2 - 0.0004955*temp; tempcomp[3] = -0.0002814*temp2 + 0.005545*temp - 1.4; tempcomp[4] = -3.011e-5*temp3 + 0.002823*temp2 - 0.1073*temp + 3.618; tempcomp[5] = 9.364e-5*temp2 + 0.009138*temp - 0.8939; // Low pass filters on accelerometer data (calculated with the help of Matlab's FDAtool) acceldata[0] = lp1.run(mpu9250.accelerometer_data[0] - tempcomp[0]); acceldata[1] = lp2.run(mpu9250.accelerometer_data[1] - tempcomp[1]); acceldata[2] = lp3.run((mpu9250.accelerometer_data[2] - tempcomp[2])*-1); // 10-term moving average to remove some noise gyrodata[0] = MAgx.run((mpu9250.gyroscope_data[0] - tempcomp[3])*-1); gyrodata[1] = MAgy.run((mpu9250.gyroscope_data[1] - tempcomp[4])*-1); gyrodata[2] = MAgz.run((mpu9250.gyroscope_data[2] - tempcomp[5])*-1); // Madgwick's quaternion algorithm filter.updateIMU(gyrodata[0], gyrodata[1], gyrodata[2], acceldata[0], acceldata[1], acceldata[2]); // filter.update(gyrodata[0], gyrodata[1], gyrodata[2], acceldata[0], acceldata[1], acceldata[2], mpu9250.Magnetometer[0], mpu9250.Magnetometer[1], mpu9250.Magnetometer[2]); // roll = filter.getRoll(); pitch = filter.getPitch(); angles[3] = gyrodata[1]; angles[4] = gyrodata[0]; angles[5] = gyrodata[2]; //Simple first order complementary filter -> TODO: CHECK STEP RESPONSE angles[0] = 0.95f*(angles[0] + (gyrodata[1]*time)) + 0.05f*pitch; angles[1] = 0.95f*(angles[1] + (gyrodata[0]*time)) + 0.05f*roll; // angles[0] = pitch; // angles[1] = roll; // If [VAR] is NaN use previous value if(angles[0] != angles[0]) angles[0] = angles_temp[0]; if(angles[1] != angles[1]) angles[1] = angles_temp[1]; if(angles[0] == angles[0]) angles_temp[0] = angles[0]; if(angles[1] == angles[1]) angles_temp[1] = angles[1]; tijd.stop(); tijd.reset(); tijd.start(); /* if(print.read_ms() > 100) { print.stop(); print.reset(); print.start(); //led2 = !led2; // pc.printf("X: %.2f Y: %.2f %.0f\r\n", angles[0], angles[1], samplef); pc.printf("%.2f %.0f\r\n", angles[1], samplef); } */ pc.printf("%.0f %.0f\r\n", pitch, roll); if(rxd) { led2 = !led2; rxd = false; // pc.printf("%d %d %d %d\r\n", rx_data[0], rx_data[1], rx_data[2], rx_data[3]); } // To change VTX channel/band/power with the remote if(rx_data[5] > 1650) ch_sw = 0; else ch_sw = 1; calccomp(rx_data, angles, motor); motor1.pulsewidth_us(motor[0]); motor2.pulsewidth_us(motor[1]); motor3.pulsewidth_us(motor[2]); motor4.pulsewidth_us(motor[3]); /* motor1.pulsewidth_us(rx_data[2]-20); motor2.pulsewidth_us(rx_data[2]-20); motor3.pulsewidth_us(rx_data[2]-20); motor4.pulsewidth_us(rx_data[2]-20); */ } }