Erik van de Coevering / Mbed 2 deprecated Multicopter_2018

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.

main.cpp

Committer:
Anaesthetix
Date:
2018-07-17
Revision:
7:d86c41443f6d
Parent:
6:033ad7377fee
Child:
8:981f7e2365b6

File content as of revision 7:d86c41443f6d:

// Coded by: Erik van de Coevering

#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;
LPfilter8 lp1, lp2, lp3;

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;
	
	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') {
            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 = 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("%.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);
				*/
    }
}