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@1:53628713ede9, 2018-07-09 (annotated)

Committer:
Anaesthetix
Date:
Mon Jul 09 16:37:24 2018 +0000
Revision:
1:53628713ede9
Parent:
0:0929d3d566cf
Child:
3:a49ab9168fb2
Typo

Who changed what in which revision?

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