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.
Diff: main.cpp
- Revision:
- 0:0929d3d566cf
- Child:
- 1:53628713ede9
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon Jul 09 16:31:40 2018 +0000 @@ -0,0 +1,345 @@ +#include "mbed.h" +#include "MadgwickAHRS.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; + +Timer timer; +Timer tijd; +Timer t; +Timer print; + +MAfilter10 maGX, maGY, maGZ; +LPfilter2 lp1, lp2, lp3; +LPfilter2_1 test; + +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 = 1.05f; +float Ki = 0; +float Kd = 0.45; + +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; + float lptest = 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<6; i++) { + pc1->putc(addr[i]); + wait_ms(1); + } + } + + if(PIDsetup == 'W') { + for(int i=0; i<6; 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 = ((float)tempval) / 100; + tempval = addr[2]; + tempval = tempval + (addr[3] << 8); + Ki = ((float)tempval) / 100; + tempval = addr[4]; + tempval = tempval + (addr[5] << 8); + Kd = ((float)tempval) / 100; + + mpu9250.init(1,BITS_DLPF_CFG_188HZ); + + pc.printf("%.2f %.2f %.2f\r\n", Kp, Ki, Kd); + + 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 = 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 - 3.018; + 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]); + + 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.99f*(angles[0] + (gyrodata[1]*time)) + 0.01f*pitch; + angles[1] = 0.99f*(angles[1] + (gyrodata[0]*time)) + 0.01f*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("%.2f %.0f\r\n", angles[0], samplef); + if(rxd) { + led2 = !led2; + rxd = false; +// pc.printf("%d %d %d %d\r\n", motor[0], motor[1], motor[2], motor[3]); + } + + 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]); + } +}