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:
- 8:981f7e2365b6
- Parent:
- 7:d86c41443f6d
--- a/main.cpp Tue Jul 17 14:56:05 2018 +0000 +++ b/main.cpp Tue Jul 31 20:36:57 2018 +0000 @@ -2,6 +2,7 @@ #include "mbed.h" #include "MadgwickAHRS.h" +#include "MahonyAHRS.h" #include "MAfilter.h" #include "MPU9250_SPI.h" #include "calccomp.h" @@ -22,15 +23,16 @@ SPI spi(p11, p12, p13); mpu9250_spi mpu9250(spi, p14); -Madgwick filter; +//Madgwick filter; +Mahony filter; Timer timer; Timer tijd; Timer t; Timer print; -MAfilter10 maGX, maGY, maGZ; LPfilter8 lp1, lp2, lp3; +MAfilter10 MAgx, MAgy, MAgz; IAP iap; @@ -56,321 +58,328 @@ char mcommand; -void rx0() { - trx0.start(); +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 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 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 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 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 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; +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; - - float filtertest = 1.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); + 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') { - for(int i=0; i<18; i++) { - mem[i] = pc1->getc(); + + 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); } - 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(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++; } - } - } - - - 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 = addr[0]; tempval = tempval + (addr[1] << 8); KP_x = ((float)tempval) / 100; - tempval = addr[2]; + tempval = addr[2]; tempval = tempval + (addr[3] << 8); KI_x = ((float)tempval) / 100; - tempval = addr[4]; + tempval = addr[4]; tempval = tempval + (addr[5] << 8); KD_x = ((float)tempval) / 100; - - tempval = addr[6]; + + tempval = addr[6]; tempval = tempval + (addr[7] << 8); KP_y = ((float)tempval) / 100; - tempval = addr[8]; + tempval = addr[8]; tempval = tempval + (addr[9] << 8); KI_y = ((float)tempval) / 100; - tempval = addr[10]; + tempval = addr[10]; tempval = tempval + (addr[11] << 8); KD_y = ((float)tempval) / 100; - - tempval = addr[12]; + + tempval = addr[12]; tempval = tempval + (addr[13] << 8); KP_z = ((float)tempval) / 100; - tempval = addr[14]; + tempval = addr[14]; tempval = tempval + (addr[15] << 8); KI_z = ((float)tempval) / 100; - tempval = addr[16]; + 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(); - + + 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 = 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; + 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]; - // 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("%.1f %.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]); - } - - // 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); - */ + //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); + */ } }