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@7:d86c41443f6d, 2018-07-17 (annotated)

Committer:
Anaesthetix
Date:
Tue Jul 17 14:56:05 2018 +0000
Revision:
7:d86c41443f6d
Parent:
6:033ad7377fee
Child:
8:981f7e2365b6
Added 8th order low-pass IIR filters

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