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@4:fab65ad01ab4, 2018-07-12 (annotated)

Committer:
Anaesthetix
Date:
Thu Jul 12 13:53:55 2018 +0000
Revision:
4:fab65ad01ab4
Parent:
3:a49ab9168fb2
Child:
5:0f4204941604
Minor changes and comments

Who changed what in which revision?

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