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@8:981f7e2365b6, 2018-07-31 (annotated)

Committer:
Anaesthetix
Date:
Tue Jul 31 20:36:57 2018 +0000
Revision:
8:981f7e2365b6
Parent:
7:d86c41443f6d 
Switched from Madgwick to Mahony as I'm having trouble with slow oscillations caused by the madgwick filter. Fixed an error on the PID algorithm also.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Anaesthetix 7:d86c41443f6d 1 // Coded by: Erik van de Coevering
Anaesthetix 4:fab65ad01ab4 2
Anaesthetix 0:0929d3d566cf 3 #include "mbed.h"
Anaesthetix 0:0929d3d566cf 4 #include "MadgwickAHRS.h"
Anaesthetix 8:981f7e2365b6 5 #include "MahonyAHRS.h"
Anaesthetix 0:0929d3d566cf 6 #include "MAfilter.h"
Anaesthetix 0:0929d3d566cf 7 #include "MPU9250_SPI.h"
Anaesthetix 0:0929d3d566cf 8 #include "calccomp.h"
Anaesthetix 0:0929d3d566cf 9 #include "SerialBuffered.h"
Anaesthetix 0:0929d3d566cf 10 #include "IAP.h"
Anaesthetix 0:0929d3d566cf 11 #include "LPfilter.h"
Anaesthetix 0:0929d3d566cf 12
Anaesthetix 0:0929d3d566cf 13
Anaesthetix 0:0929d3d566cf 14 #define MEM_SIZE 256
Anaesthetix 0:0929d3d566cf 15 #define TARGET_SECTOR 29 // use sector 29 as target sector
Anaesthetix 0:0929d3d566cf 16
Anaesthetix 0:0929d3d566cf 17 DigitalOut led2(LED2); // leds are active low
Anaesthetix 0:0929d3d566cf 18 DigitalOut ch_sw(p26);
Anaesthetix 0:0929d3d566cf 19
Anaesthetix 0:0929d3d566cf 20 Serial pc(USBTX, USBRX);
Anaesthetix 0:0929d3d566cf 21 SerialBuffered *pc1 = new SerialBuffered( 100, USBTX, USBRX);
Anaesthetix 0:0929d3d566cf 22
Anaesthetix 0:0929d3d566cf 23 SPI spi(p11, p12, p13);
Anaesthetix 0:0929d3d566cf 24 mpu9250_spi mpu9250(spi, p14);
Anaesthetix 0:0929d3d566cf 25
Anaesthetix 8:981f7e2365b6 26 //Madgwick filter;
Anaesthetix 8:981f7e2365b6 27 Mahony filter;
Anaesthetix 0:0929d3d566cf 28
Anaesthetix 0:0929d3d566cf 29 Timer timer;
Anaesthetix 0:0929d3d566cf 30 Timer tijd;
Anaesthetix 0:0929d3d566cf 31 Timer t;
Anaesthetix 0:0929d3d566cf 32 Timer print;
Anaesthetix 0:0929d3d566cf 33
Anaesthetix 7:d86c41443f6d 34 LPfilter8 lp1, lp2, lp3;
Anaesthetix 8:981f7e2365b6 35 MAfilter10 MAgx, MAgy, MAgz;
Anaesthetix 0:0929d3d566cf 36
Anaesthetix 0:0929d3d566cf 37 IAP iap;
Anaesthetix 0:0929d3d566cf 38
Anaesthetix 0:0929d3d566cf 39 InterruptIn rx_rud(p5);
Anaesthetix 0:0929d3d566cf 40 InterruptIn rx_elev(p6);
Anaesthetix 0:0929d3d566cf 41 InterruptIn rx_thr(p7);
Anaesthetix 0:0929d3d566cf 42 InterruptIn rx_ail(p8);
Anaesthetix 0:0929d3d566cf 43 InterruptIn rx_p1(p29);
Anaesthetix 0:0929d3d566cf 44 InterruptIn rx_p2(p30);
Anaesthetix 0:0929d3d566cf 45
Anaesthetix 0:0929d3d566cf 46 PwmOut motor4(p24);
Anaesthetix 0:0929d3d566cf 47 PwmOut motor3(p23);
Anaesthetix 0:0929d3d566cf 48 PwmOut motor1(p22);
Anaesthetix 0:0929d3d566cf 49 PwmOut motor2(p21);
Anaesthetix 0:0929d3d566cf 50
Anaesthetix 6:033ad7377fee 51 float KP_x, KI_x, KD_x;
Anaesthetix 6:033ad7377fee 52 float KP_y, KI_y, KD_y;
Anaesthetix 6:033ad7377fee 53 float KP_z, KI_z, KD_z;
Anaesthetix 0:0929d3d566cf 54
Anaesthetix 0:0929d3d566cf 55 Timer trx0, trx1, trx2, trx3, trx4, trx5;
Anaesthetix 0:0929d3d566cf 56 int rx_data[6] = {0};
Anaesthetix 0:0929d3d566cf 57 bool rxd = false;
Anaesthetix 0:0929d3d566cf 58
Anaesthetix 0:0929d3d566cf 59 char mcommand;
Anaesthetix 0:0929d3d566cf 60
Anaesthetix 8:981f7e2365b6 61 void rx0()
Anaesthetix 8:981f7e2365b6 62 {
Anaesthetix 8:981f7e2365b6 63 trx0.start();
Anaesthetix 0:0929d3d566cf 64 }
Anaesthetix 0:0929d3d566cf 65
Anaesthetix 8:981f7e2365b6 66 void rx1()
Anaesthetix 8:981f7e2365b6 67 {
Anaesthetix 8:981f7e2365b6 68 trx1.start();
Anaesthetix 8:981f7e2365b6 69 trx0.stop();
Anaesthetix 8:981f7e2365b6 70 if(trx0.read_us() > 900 && trx0.read_us() < 2200) rx_data[0] = trx0.read_us();
Anaesthetix 8:981f7e2365b6 71 trx0.reset();
Anaesthetix 0:0929d3d566cf 72 }
Anaesthetix 0:0929d3d566cf 73
Anaesthetix 8:981f7e2365b6 74 void rx2()
Anaesthetix 8:981f7e2365b6 75 {
Anaesthetix 8:981f7e2365b6 76 trx2.start();
Anaesthetix 8:981f7e2365b6 77 trx1.stop();
Anaesthetix 8:981f7e2365b6 78 if(trx1.read_us() > 900 && trx1.read_us() < 2200) rx_data[1] = trx1.read_us();
Anaesthetix 8:981f7e2365b6 79 trx1.reset();
Anaesthetix 0:0929d3d566cf 80 }
Anaesthetix 0:0929d3d566cf 81
Anaesthetix 8:981f7e2365b6 82 void rx3()
Anaesthetix 8:981f7e2365b6 83 {
Anaesthetix 8:981f7e2365b6 84 trx3.start();
Anaesthetix 8:981f7e2365b6 85 trx2.stop();
Anaesthetix 8:981f7e2365b6 86 if(trx2.read_us() > 900 && trx2.read_us() < 2200) rx_data[2] = trx2.read_us();
Anaesthetix 8:981f7e2365b6 87 trx2.reset();
Anaesthetix 0:0929d3d566cf 88 }
Anaesthetix 0:0929d3d566cf 89
Anaesthetix 8:981f7e2365b6 90 void rx4()
Anaesthetix 8:981f7e2365b6 91 {
Anaesthetix 8:981f7e2365b6 92 trx4.start();
Anaesthetix 8:981f7e2365b6 93 trx3.stop();
Anaesthetix 8:981f7e2365b6 94 if(trx3.read_us() > 900 && trx3.read_us() < 2200) rx_data[3] = trx3.read_us();
Anaesthetix 8:981f7e2365b6 95 trx3.reset();
Anaesthetix 0:0929d3d566cf 96 }
Anaesthetix 0:0929d3d566cf 97
Anaesthetix 8:981f7e2365b6 98 void rx5()
Anaesthetix 8:981f7e2365b6 99 {
Anaesthetix 8:981f7e2365b6 100 trx5.start();
Anaesthetix 8:981f7e2365b6 101 trx4.stop();
Anaesthetix 8:981f7e2365b6 102 if(trx4.read_us() > 900 && trx4.read_us() < 2200) rx_data[4] = trx4.read_us();
Anaesthetix 8:981f7e2365b6 103 trx4.reset();
Anaesthetix 0:0929d3d566cf 104 }
Anaesthetix 0:0929d3d566cf 105
Anaesthetix 8:981f7e2365b6 106 void rx6()
Anaesthetix 8:981f7e2365b6 107 {
Anaesthetix 8:981f7e2365b6 108 trx5.stop();
Anaesthetix 8:981f7e2365b6 109 if(trx5.read_us() > 900 && trx5.read_us() < 2200) rx_data[5] = trx5.read_us();
Anaesthetix 8:981f7e2365b6 110 trx5.reset();
Anaesthetix 8:981f7e2365b6 111 rxd = true;
Anaesthetix 0:0929d3d566cf 112 }
Anaesthetix 0:0929d3d566cf 113
Anaesthetix 0:0929d3d566cf 114
Anaesthetix 0:0929d3d566cf 115 int main()
Anaesthetix 0:0929d3d566cf 116 {
Anaesthetix 8:981f7e2365b6 117 ch_sw = 1;
Anaesthetix 8:981f7e2365b6 118 led1 = 1;
Anaesthetix 8:981f7e2365b6 119 led2 = 0;
Anaesthetix 8:981f7e2365b6 120 pc1->baud(230400);
Anaesthetix 8:981f7e2365b6 121 pc1->setTimeout(1);
Anaesthetix 8:981f7e2365b6 122 pc.baud(230400);
Anaesthetix 8:981f7e2365b6 123 spi.frequency(4000000);
Anaesthetix 8:981f7e2365b6 124
Anaesthetix 8:981f7e2365b6 125
Anaesthetix 8:981f7e2365b6 126 //IAP variables
Anaesthetix 8:981f7e2365b6 127 char* addr = sector_start_adress[TARGET_SECTOR];
Anaesthetix 8:981f7e2365b6 128 char mem[ MEM_SIZE ]; // memory, it should be aligned to word boundary
Anaesthetix 8:981f7e2365b6 129 char PIDsetup = 0;
Anaesthetix 8:981f7e2365b6 130 int r;
Anaesthetix 8:981f7e2365b6 131 int tempval;
Anaesthetix 8:981f7e2365b6 132
Anaesthetix 8:981f7e2365b6 133 //IMU variables
Anaesthetix 8:981f7e2365b6 134 float angles[6] = {0};
Anaesthetix 8:981f7e2365b6 135 float time = 0;
Anaesthetix 8:981f7e2365b6 136 float samplef = 0;
Anaesthetix 8:981f7e2365b6 137 float gyrodata[3] = {0};
Anaesthetix 8:981f7e2365b6 138 float acceldata[3] = {0};
Anaesthetix 8:981f7e2365b6 139 float angles_temp[2] = {0};
Anaesthetix 8:981f7e2365b6 140 float roll, pitch;
Anaesthetix 8:981f7e2365b6 141 float tempcomp[6] = {0};
Anaesthetix 8:981f7e2365b6 142 float temp = 0;
Anaesthetix 8:981f7e2365b6 143 float temp2 = 0;
Anaesthetix 8:981f7e2365b6 144 float temp3 = 0;
Anaesthetix 8:981f7e2365b6 145 bool exit = true;
Anaesthetix 8:981f7e2365b6 146 float noise = 0;
Anaesthetix 8:981f7e2365b6 147 int count = 0;
Anaesthetix 8:981f7e2365b6 148
Anaesthetix 8:981f7e2365b6 149 //Rx variables
Anaesthetix 8:981f7e2365b6 150 int motor[4] = {0};
Anaesthetix 8:981f7e2365b6 151
Anaesthetix 8:981f7e2365b6 152 // Init pwm
Anaesthetix 8:981f7e2365b6 153 motor1.period_ms(10);
Anaesthetix 8:981f7e2365b6 154 motor1.pulsewidth_us(1000);
Anaesthetix 8:981f7e2365b6 155 motor2.pulsewidth_us(1000);
Anaesthetix 8:981f7e2365b6 156 motor3.pulsewidth_us(1000);
Anaesthetix 8:981f7e2365b6 157 motor4.pulsewidth_us(1000);
Anaesthetix 8:981f7e2365b6 158
Anaesthetix 8:981f7e2365b6 159 filter.begin(1500);
Anaesthetix 8:981f7e2365b6 160
Anaesthetix 8:981f7e2365b6 161 pc.putc('c');
Anaesthetix 8:981f7e2365b6 162 PIDsetup = pc1->getc();
Anaesthetix 8:981f7e2365b6 163 if(PIDsetup == 'c') {
Anaesthetix 8:981f7e2365b6 164 while(1) {
Anaesthetix 8:981f7e2365b6 165 PIDsetup = pc1->getc();
Anaesthetix 8:981f7e2365b6 166 if(PIDsetup == 'R') {
Anaesthetix 8:981f7e2365b6 167 for(int i=0; i<18; i++) {
Anaesthetix 8:981f7e2365b6 168 pc1->putc(addr[i]);
Anaesthetix 8:981f7e2365b6 169 wait_ms(1);
Anaesthetix 8:981f7e2365b6 170 }
Anaesthetix 8:981f7e2365b6 171 }
Anaesthetix 8:981f7e2365b6 172
Anaesthetix 8:981f7e2365b6 173 if(PIDsetup == 'W') {
Anaesthetix 8:981f7e2365b6 174 for(int i=0; i<18; i++) {
Anaesthetix 8:981f7e2365b6 175 mem[i] = pc1->getc();
Anaesthetix 8:981f7e2365b6 176 }
Anaesthetix 8:981f7e2365b6 177 iap.prepare( TARGET_SECTOR, TARGET_SECTOR );
Anaesthetix 8:981f7e2365b6 178 r = iap.erase( TARGET_SECTOR, TARGET_SECTOR );
Anaesthetix 8:981f7e2365b6 179 iap.prepare( TARGET_SECTOR, TARGET_SECTOR );
Anaesthetix 8:981f7e2365b6 180 r = iap.write( mem, sector_start_adress[ TARGET_SECTOR ], MEM_SIZE );
Anaesthetix 5:0f4204941604 181 }
Anaesthetix 5:0f4204941604 182 }
Anaesthetix 8:981f7e2365b6 183 }
Anaesthetix 5:0f4204941604 184
Anaesthetix 8:981f7e2365b6 185
Anaesthetix 8:981f7e2365b6 186 if(PIDsetup == 'W') {
Anaesthetix 8:981f7e2365b6 187 mpu9250.init2(1,BITS_DLPF_CFG_188HZ);
Anaesthetix 8:981f7e2365b6 188 pc1->setTimeout(0.01);
Anaesthetix 8:981f7e2365b6 189
Anaesthetix 8:981f7e2365b6 190 rx_rud.rise(&rx0);
Anaesthetix 8:981f7e2365b6 191 rx_elev.rise(&rx1);
Anaesthetix 8:981f7e2365b6 192 rx_thr.rise(&rx2);
Anaesthetix 8:981f7e2365b6 193 rx_ail.rise(&rx3);
Anaesthetix 8:981f7e2365b6 194 rx_p1.rise(&rx4);
Anaesthetix 8:981f7e2365b6 195 rx_p2.rise(&rx5);
Anaesthetix 8:981f7e2365b6 196 rx_p2.fall(&rx6);
Anaesthetix 8:981f7e2365b6 197 mcommand = 'a';
Anaesthetix 8:981f7e2365b6 198
Anaesthetix 8:981f7e2365b6 199 while(exit) {
Anaesthetix 8:981f7e2365b6 200 wait_ms(1);
Anaesthetix 8:981f7e2365b6 201 if(mcommand == '5') {
Anaesthetix 8:981f7e2365b6 202 motor1.pulsewidth_us(rx_data[2] - 20);
Anaesthetix 8:981f7e2365b6 203 motor2.pulsewidth_us(1000);
Anaesthetix 8:981f7e2365b6 204 motor3.pulsewidth_us(1000);
Anaesthetix 8:981f7e2365b6 205 motor4.pulsewidth_us(1000);
Anaesthetix 8:981f7e2365b6 206 } else if(mcommand == '6') {
Anaesthetix 8:981f7e2365b6 207 motor1.pulsewidth_us(1000);
Anaesthetix 8:981f7e2365b6 208 motor2.pulsewidth_us(rx_data[2] - 20);
Anaesthetix 8:981f7e2365b6 209 motor3.pulsewidth_us(1000);
Anaesthetix 8:981f7e2365b6 210 motor4.pulsewidth_us(1000);
Anaesthetix 8:981f7e2365b6 211 } else if(mcommand == '3') {
Anaesthetix 8:981f7e2365b6 212 motor1.pulsewidth_us(1000);
Anaesthetix 8:981f7e2365b6 213 motor2.pulsewidth_us(1000);
Anaesthetix 8:981f7e2365b6 214 motor3.pulsewidth_us(rx_data[2] - 20);
Anaesthetix 8:981f7e2365b6 215 motor4.pulsewidth_us(1000);
Anaesthetix 8:981f7e2365b6 216 } else if(mcommand == '4') {
Anaesthetix 8:981f7e2365b6 217 motor1.pulsewidth_us(1000);
Anaesthetix 8:981f7e2365b6 218 motor2.pulsewidth_us(1000);
Anaesthetix 8:981f7e2365b6 219 motor3.pulsewidth_us(1000);
Anaesthetix 8:981f7e2365b6 220 motor4.pulsewidth_us(rx_data[2] - 20);
Anaesthetix 5:0f4204941604 221 }
Anaesthetix 8:981f7e2365b6 222 if(mcommand == 'E') exit = 0;
Anaesthetix 8:981f7e2365b6 223
Anaesthetix 8:981f7e2365b6 224 if(rxd) {
Anaesthetix 8:981f7e2365b6 225 led2 = !led2;
Anaesthetix 8:981f7e2365b6 226 rxd = false;
Anaesthetix 8:981f7e2365b6 227 }
Anaesthetix 8:981f7e2365b6 228
Anaesthetix 8:981f7e2365b6 229 mpu9250.read_all();
Anaesthetix 8:981f7e2365b6 230 if(mpu9250.accelerometer_data[0] >= 0) noise = noise*0.99 + 0.01*mpu9250.accelerometer_data[0];
Anaesthetix 8:981f7e2365b6 231
Anaesthetix 8:981f7e2365b6 232 if(count>100) {
Anaesthetix 8:981f7e2365b6 233 count = 0;
Anaesthetix 8:981f7e2365b6 234 pc.printf("%.2f\n", noise);
Anaesthetix 8:981f7e2365b6 235 mcommand = pc1->getc();
Anaesthetix 8:981f7e2365b6 236 }
Anaesthetix 8:981f7e2365b6 237 count++;
Anaesthetix 5:0f4204941604 238 }
Anaesthetix 8:981f7e2365b6 239 }
Anaesthetix 8:981f7e2365b6 240
Anaesthetix 8:981f7e2365b6 241 tempval = addr[0];
Anaesthetix 5:0f4204941604 242 tempval = tempval + (addr[1] << 8);
Anaesthetix 6:033ad7377fee 243 KP_x = ((float)tempval) / 100;
Anaesthetix 8:981f7e2365b6 244 tempval = addr[2];
Anaesthetix 5:0f4204941604 245 tempval = tempval + (addr[3] << 8);
Anaesthetix 6:033ad7377fee 246 KI_x = ((float)tempval) / 100;
Anaesthetix 8:981f7e2365b6 247 tempval = addr[4];
Anaesthetix 5:0f4204941604 248 tempval = tempval + (addr[5] << 8);
Anaesthetix 6:033ad7377fee 249 KD_x = ((float)tempval) / 100;
Anaesthetix 8:981f7e2365b6 250
Anaesthetix 8:981f7e2365b6 251 tempval = addr[6];
Anaesthetix 6:033ad7377fee 252 tempval = tempval + (addr[7] << 8);
Anaesthetix 6:033ad7377fee 253 KP_y = ((float)tempval) / 100;
Anaesthetix 8:981f7e2365b6 254 tempval = addr[8];
Anaesthetix 6:033ad7377fee 255 tempval = tempval + (addr[9] << 8);
Anaesthetix 6:033ad7377fee 256 KI_y = ((float)tempval) / 100;
Anaesthetix 8:981f7e2365b6 257 tempval = addr[10];
Anaesthetix 6:033ad7377fee 258 tempval = tempval + (addr[11] << 8);
Anaesthetix 6:033ad7377fee 259 KD_y = ((float)tempval) / 100;
Anaesthetix 8:981f7e2365b6 260
Anaesthetix 8:981f7e2365b6 261 tempval = addr[12];
Anaesthetix 6:033ad7377fee 262 tempval = tempval + (addr[13] << 8);
Anaesthetix 6:033ad7377fee 263 KP_z = ((float)tempval) / 100;
Anaesthetix 8:981f7e2365b6 264 tempval = addr[14];
Anaesthetix 6:033ad7377fee 265 tempval = tempval + (addr[15] << 8);
Anaesthetix 6:033ad7377fee 266 KI_z = ((float)tempval) / 100;
Anaesthetix 8:981f7e2365b6 267 tempval = addr[16];
Anaesthetix 6:033ad7377fee 268 tempval = tempval + (addr[17] << 8);
Anaesthetix 6:033ad7377fee 269 KD_z = ((float)tempval) / 100;
Anaesthetix 8:981f7e2365b6 270
Anaesthetix 8:981f7e2365b6 271 mpu9250.init(1,BITS_DLPF_CFG_188HZ);
Anaesthetix 8:981f7e2365b6 272 /*
Anaesthetix 8:981f7e2365b6 273 pc.printf("%.2f %.2f %.2f\r\n", KP_x, KI_x, KD_x);
Anaesthetix 8:981f7e2365b6 274 pc.printf("%.2f %.2f %.2f\r\n", KP_y, KI_y, KD_y);
Anaesthetix 8:981f7e2365b6 275 pc.printf("%.2f %.2f %.2f\r\n", KP_z, KI_z, KD_z);
Anaesthetix 8:981f7e2365b6 276 */
Anaesthetix 8:981f7e2365b6 277 rx_rud.rise(&rx0);
Anaesthetix 8:981f7e2365b6 278 rx_elev.rise(&rx1);
Anaesthetix 8:981f7e2365b6 279 rx_thr.rise(&rx2);
Anaesthetix 8:981f7e2365b6 280 rx_ail.rise(&rx3);
Anaesthetix 8:981f7e2365b6 281 rx_p1.rise(&rx4);
Anaesthetix 8:981f7e2365b6 282 rx_p2.rise(&rx5);
Anaesthetix 8:981f7e2365b6 283 rx_p2.fall(&rx6);
Anaesthetix 8:981f7e2365b6 284
Anaesthetix 8:981f7e2365b6 285 t.start();
Anaesthetix 8:981f7e2365b6 286
Anaesthetix 5:0f4204941604 287 while(1) {
Anaesthetix 8:981f7e2365b6 288 print.start();
Anaesthetix 8:981f7e2365b6 289 t.stop();
Anaesthetix 8:981f7e2365b6 290 time = (float)t.read();
Anaesthetix 8:981f7e2365b6 291 t.reset();
Anaesthetix 8:981f7e2365b6 292 t.start();
Anaesthetix 8:981f7e2365b6 293 filter.invSampleFreq = time;
Anaesthetix 8:981f7e2365b6 294 samplef = 1/time;
Anaesthetix 8:981f7e2365b6 295
Anaesthetix 8:981f7e2365b6 296 mpu9250.read_all();
Anaesthetix 8:981f7e2365b6 297 if(mpu9250.Temperature < 6.0f) temp = 6.0f;
Anaesthetix 8:981f7e2365b6 298 else if(mpu9250.Temperature > 60.0f) temp = 60.0f;
Anaesthetix 8:981f7e2365b6 299 else temp = 0.999*temp + 0.001*mpu9250.Temperature;
Anaesthetix 8:981f7e2365b6 300 temp2 = temp*temp;
Anaesthetix 8:981f7e2365b6 301 temp3 = temp2*temp;
Anaesthetix 8:981f7e2365b6 302
Anaesthetix 8:981f7e2365b6 303 // Temperature compensation
Anaesthetix 8:981f7e2365b6 304 tempcomp[0] = -1.77e-6*temp2 + 0.000233*temp + 0.02179;
Anaesthetix 8:981f7e2365b6 305 tempcomp[1] = 0.0005727*temp - 0.01488;
Anaesthetix 8:981f7e2365b6 306 tempcomp[2] = -2.181e-7*temp3 + 1.754e-5*temp2 - 0.0004955*temp;
Anaesthetix 8:981f7e2365b6 307 tempcomp[3] = -0.0002814*temp2 + 0.005545*temp - 1.4;
Anaesthetix 8:981f7e2365b6 308 tempcomp[4] = -3.011e-5*temp3 + 0.002823*temp2 - 0.1073*temp + 3.618;
Anaesthetix 8:981f7e2365b6 309 tempcomp[5] = 9.364e-5*temp2 + 0.009138*temp - 0.8939;
Anaesthetix 8:981f7e2365b6 310
Anaesthetix 8:981f7e2365b6 311 // Low pass filters on accelerometer data (calculated with the help of Matlab's FDAtool)
Anaesthetix 8:981f7e2365b6 312 acceldata[0] = lp1.run(mpu9250.accelerometer_data[0] - tempcomp[0]);
Anaesthetix 8:981f7e2365b6 313 acceldata[1] = lp2.run(mpu9250.accelerometer_data[1] - tempcomp[1]);
Anaesthetix 8:981f7e2365b6 314 acceldata[2] = lp3.run((mpu9250.accelerometer_data[2] - tempcomp[2])*-1);
Anaesthetix 8:981f7e2365b6 315
Anaesthetix 8:981f7e2365b6 316 // 10-term moving average to remove some noise
Anaesthetix 8:981f7e2365b6 317 gyrodata[0] = MAgx.run((mpu9250.gyroscope_data[0] - tempcomp[3])*-1);
Anaesthetix 8:981f7e2365b6 318 gyrodata[1] = MAgy.run((mpu9250.gyroscope_data[1] - tempcomp[4])*-1);
Anaesthetix 8:981f7e2365b6 319 gyrodata[2] = MAgz.run((mpu9250.gyroscope_data[2] - tempcomp[5])*-1);
Anaesthetix 8:981f7e2365b6 320
Anaesthetix 8:981f7e2365b6 321 // Madgwick's quaternion algorithm
Anaesthetix 8:981f7e2365b6 322 filter.updateIMU(gyrodata[0], gyrodata[1], gyrodata[2], acceldata[0],
Anaesthetix 8:981f7e2365b6 323 acceldata[1], acceldata[2]);
Anaesthetix 8:981f7e2365b6 324
Anaesthetix 8:981f7e2365b6 325 // filter.update(gyrodata[0], gyrodata[1], gyrodata[2], acceldata[0], acceldata[1], acceldata[2], mpu9250.Magnetometer[0], mpu9250.Magnetometer[1], mpu9250.Magnetometer[2]);
Anaesthetix 8:981f7e2365b6 326 //
Anaesthetix 8:981f7e2365b6 327 roll = filter.getRoll();
Anaesthetix 8:981f7e2365b6 328 pitch = filter.getPitch();
Anaesthetix 8:981f7e2365b6 329
Anaesthetix 8:981f7e2365b6 330 angles[3] = gyrodata[1];
Anaesthetix 8:981f7e2365b6 331 angles[4] = gyrodata[0];
Anaesthetix 8:981f7e2365b6 332 angles[5] = gyrodata[2];
Anaesthetix 5:0f4204941604 333
Anaesthetix 8:981f7e2365b6 334 //Simple first order complementary filter -> TODO: CHECK STEP RESPONSE
Anaesthetix 8:981f7e2365b6 335 angles[0] = 0.95f*(angles[0] + (gyrodata[1]*time)) + 0.05f*pitch;
Anaesthetix 8:981f7e2365b6 336 angles[1] = 0.95f*(angles[1] + (gyrodata[0]*time)) + 0.05f*roll;
Anaesthetix 8:981f7e2365b6 337
Anaesthetix 8:981f7e2365b6 338 // angles[0] = pitch;
Anaesthetix 8:981f7e2365b6 339 // angles[1] = roll;
Anaesthetix 8:981f7e2365b6 340
Anaesthetix 8:981f7e2365b6 341 // If [VAR] is NaN use previous value
Anaesthetix 8:981f7e2365b6 342 if(angles[0] != angles[0]) angles[0] = angles_temp[0];
Anaesthetix 8:981f7e2365b6 343 if(angles[1] != angles[1]) angles[1] = angles_temp[1];
Anaesthetix 8:981f7e2365b6 344 if(angles[0] == angles[0]) angles_temp[0] = angles[0];
Anaesthetix 8:981f7e2365b6 345 if(angles[1] == angles[1]) angles_temp[1] = angles[1];
Anaesthetix 8:981f7e2365b6 346
Anaesthetix 8:981f7e2365b6 347 tijd.stop();
Anaesthetix 8:981f7e2365b6 348 tijd.reset();
Anaesthetix 8:981f7e2365b6 349 tijd.start();
Anaesthetix 8:981f7e2365b6 350
Anaesthetix 8:981f7e2365b6 351 /*
Anaesthetix 8:981f7e2365b6 352 if(print.read_ms() > 100) {
Anaesthetix 8:981f7e2365b6 353 print.stop();
Anaesthetix 8:981f7e2365b6 354 print.reset();
Anaesthetix 8:981f7e2365b6 355 print.start();
Anaesthetix 8:981f7e2365b6 356 //led2 = !led2;
Anaesthetix 8:981f7e2365b6 357 // pc.printf("X: %.2f Y: %.2f %.0f\r\n", angles[0], angles[1], samplef);
Anaesthetix 8:981f7e2365b6 358 pc.printf("%.2f %.0f\r\n", angles[1], samplef);
Anaesthetix 8:981f7e2365b6 359 }
Anaesthetix 8:981f7e2365b6 360 */
Anaesthetix 8:981f7e2365b6 361 pc.printf("%.0f %.0f\r\n", pitch, roll);
Anaesthetix 8:981f7e2365b6 362 if(rxd) {
Anaesthetix 8:981f7e2365b6 363 led2 = !led2;
Anaesthetix 8:981f7e2365b6 364 rxd = false;
Anaesthetix 8:981f7e2365b6 365 // pc.printf("%d %d %d %d\r\n", rx_data[0], rx_data[1], rx_data[2], rx_data[3]);
Anaesthetix 8:981f7e2365b6 366 }
Anaesthetix 8:981f7e2365b6 367
Anaesthetix 8:981f7e2365b6 368 // To change VTX channel/band/power with the remote
Anaesthetix 8:981f7e2365b6 369 if(rx_data[5] > 1650) ch_sw = 0;
Anaesthetix 8:981f7e2365b6 370 else ch_sw = 1;
Anaesthetix 8:981f7e2365b6 371
Anaesthetix 8:981f7e2365b6 372 calccomp(rx_data, angles, motor);
Anaesthetix 8:981f7e2365b6 373
Anaesthetix 8:981f7e2365b6 374 motor1.pulsewidth_us(motor[0]);
Anaesthetix 8:981f7e2365b6 375 motor2.pulsewidth_us(motor[1]);
Anaesthetix 8:981f7e2365b6 376 motor3.pulsewidth_us(motor[2]);
Anaesthetix 8:981f7e2365b6 377 motor4.pulsewidth_us(motor[3]);
Anaesthetix 8:981f7e2365b6 378 /*
Anaesthetix 8:981f7e2365b6 379 motor1.pulsewidth_us(rx_data[2]-20);
Anaesthetix 8:981f7e2365b6 380 motor2.pulsewidth_us(rx_data[2]-20);
Anaesthetix 8:981f7e2365b6 381 motor3.pulsewidth_us(rx_data[2]-20);
Anaesthetix 8:981f7e2365b6 382 motor4.pulsewidth_us(rx_data[2]-20);
Anaesthetix 8:981f7e2365b6 383 */
Anaesthetix 0:0929d3d566cf 384 }
Anaesthetix 0:0929d3d566cf 385 }