Erik van de Coevering
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:
- 7:d86c41443f6d
- Parent:
- 6:033ad7377fee
- Child:
- 8:981f7e2365b6
diff -r 033ad7377fee -r d86c41443f6d main.cpp
--- a/main.cpp Sat Jul 14 10:30:20 2018 +0000
+++ b/main.cpp Tue Jul 17 14:56:05 2018 +0000
@@ -1,4 +1,4 @@
-// Coded by Erik van de Coevering
+// Coded by: Erik van de Coevering
#include "mbed.h"
#include "MadgwickAHRS.h"
@@ -30,8 +30,7 @@
Timer print;
MAfilter10 maGX, maGY, maGZ;
-LPfilter2 lp1, lp2, lp3;
-LPfilter2_1 test;
+LPfilter8 lp1, lp2, lp3;
IAP iap;
@@ -57,323 +56,321 @@
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;
-
- //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 );
+ 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);
}
}
- }
-
- 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(PIDsetup == 'W') {
+ for(int i=0; i<18; i++) {
+ mem[i] = pc1->getc();
}
- 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++;
+ 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 );
}
- }
-
- tempval = addr[0];
+ }
+ }
+
+
+ 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 = 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[19];
+ 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]);
+ 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;
- 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]);
- }
-
- // 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);
- */
+ // 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);
+ */
}
}