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.

Committer:
Anaesthetix
Date:
Sat Jul 14 10:30:20 2018 +0000
Revision:
6:033ad7377fee
Parent:
5:0f4204941604
Child:
7:d86c41443f6d
Changed overall PID gains to gains per axis.

Who changed what in which revision?

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