Latest version of my quadcopter controller with an LPC1768 and MPU9250.

Dependencies:   mbed

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.

Revision:
7:d86c41443f6d
Parent:
6:033ad7377fee
Child:
8:981f7e2365b6
--- 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);
+				*/
     }
 }