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@5:0f4204941604, 2018-07-13 (annotated)

Committer:
Anaesthetix
Date:
Fri Jul 13 14:27:30 2018 +0000
Revision:
5:0f4204941604
Parent:
4:fab65ad01ab4
Child:
6:033ad7377fee
Yaw PID added

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 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 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 5:0f4204941604 166 for(int i=0; i<6; 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 5:0f4204941604 173 for(int i=0; i<6; 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 5:0f4204941604 242 Kp = ((float)tempval) / 100;
Anaesthetix 5:0f4204941604 243 tempval = addr[2];
Anaesthetix 5:0f4204941604 244 tempval = tempval + (addr[3] << 8);
Anaesthetix 5:0f4204941604 245 Ki = ((float)tempval) / 100;
Anaesthetix 5:0f4204941604 246 tempval = addr[4];
Anaesthetix 5:0f4204941604 247 tempval = tempval + (addr[5] << 8);
Anaesthetix 5:0f4204941604 248 Kd = ((float)tempval) / 100;
Anaesthetix 5:0f4204941604 249
Anaesthetix 5:0f4204941604 250 mpu9250.init(1,BITS_DLPF_CFG_188HZ);
Anaesthetix 5:0f4204941604 251
Anaesthetix 5:0f4204941604 252 pc.printf("%.2f %.2f %.2f\r\n", Kp, Ki, Kd);
Anaesthetix 5:0f4204941604 253
Anaesthetix 5:0f4204941604 254 rx_rud.rise(&rx0);
Anaesthetix 5:0f4204941604 255 rx_elev.rise(&rx1);
Anaesthetix 5:0f4204941604 256 rx_thr.rise(&rx2);
Anaesthetix 5:0f4204941604 257 rx_ail.rise(&rx3);
Anaesthetix 5:0f4204941604 258 rx_p1.rise(&rx4);
Anaesthetix 5:0f4204941604 259 rx_p2.rise(&rx5);
Anaesthetix 5:0f4204941604 260 rx_p2.fall(&rx6);
Anaesthetix 5:0f4204941604 261
Anaesthetix 5:0f4204941604 262 t.start();
Anaesthetix 5:0f4204941604 263
Anaesthetix 5:0f4204941604 264 while(1) {
Anaesthetix 5:0f4204941604 265 print.start();
Anaesthetix 5:0f4204941604 266 t.stop();
Anaesthetix 5:0f4204941604 267 time = (float)t.read();
Anaesthetix 5:0f4204941604 268 t.reset();
Anaesthetix 5:0f4204941604 269 t.start();
Anaesthetix 5:0f4204941604 270 filter.invSampleFreq = time;
Anaesthetix 5:0f4204941604 271 samplef = 1/time;
Anaesthetix 5:0f4204941604 272
Anaesthetix 5:0f4204941604 273 mpu9250.read_all();
Anaesthetix 5:0f4204941604 274 if(mpu9250.Temperature < 6.0f) temp = 6.0f;
Anaesthetix 5:0f4204941604 275 else if(mpu9250.Temperature > 60.0f) temp = 60.0f;
Anaesthetix 5:0f4204941604 276 else temp = mpu9250.Temperature;
Anaesthetix 5:0f4204941604 277 temp2 = temp*temp;
Anaesthetix 5:0f4204941604 278 temp3 = temp2*temp;
Anaesthetix 5:0f4204941604 279
Anaesthetix 5:0f4204941604 280 // Temperature compensation
Anaesthetix 5:0f4204941604 281 // Derived from datalogging gyro data over a wide temperature range and using the fitting tool in Matlab's plotter
Anaesthetix 5:0f4204941604 282 tempcomp[0] = -1.77e-6*temp2 + 0.000233*temp + 0.02179;
Anaesthetix 5:0f4204941604 283 tempcomp[1] = 0.0005727*temp - 0.01488;
Anaesthetix 5:0f4204941604 284 tempcomp[2] = -2.181e-7*temp3 + 1.754e-5*temp2 - 0.0004955*temp;
Anaesthetix 5:0f4204941604 285 tempcomp[3] = -0.0002814*temp2 + 0.005545*temp - 3.018;
Anaesthetix 5:0f4204941604 286 tempcomp[4] = -3.011e-5*temp3 + 0.002823*temp2 - 0.1073*temp + 3.618;
Anaesthetix 5:0f4204941604 287 tempcomp[5] = 9.364e-5*temp2 + 0.009138*temp - 0.8939;
Anaesthetix 5:0f4204941604 288
Anaesthetix 5:0f4204941604 289 // Low pass filters on accelerometer data (calculated with the help of Matlab's FDAtool)
Anaesthetix 5:0f4204941604 290 acceldata[0] = lp1.run(mpu9250.accelerometer_data[0] - tempcomp[0]);
Anaesthetix 5:0f4204941604 291 acceldata[1] = lp2.run(mpu9250.accelerometer_data[1] - tempcomp[1]);
Anaesthetix 5:0f4204941604 292 acceldata[2] = lp3.run((mpu9250.accelerometer_data[2] - tempcomp[2])*-1);
Anaesthetix 5:0f4204941604 293
Anaesthetix 5:0f4204941604 294 // 10-term moving average to remove some noise
Anaesthetix 5:0f4204941604 295 gyrodata[0] = maGX.run((mpu9250.gyroscope_data[0] - tempcomp[3])*-1);
Anaesthetix 5:0f4204941604 296 gyrodata[1] = maGY.run((mpu9250.gyroscope_data[1] - tempcomp[4])*-1);
Anaesthetix 5:0f4204941604 297 gyrodata[2] = maGZ.run((mpu9250.gyroscope_data[2] - tempcomp[5])*-1);
Anaesthetix 5:0f4204941604 298
Anaesthetix 5:0f4204941604 299 // Madgwick's quaternion algorithm
Anaesthetix 5:0f4204941604 300 filter.updateIMU(gyrodata[0], gyrodata[1], gyrodata[2], acceldata[0],
Anaesthetix 5:0f4204941604 301 acceldata[1], acceldata[2]);
Anaesthetix 5:0f4204941604 302
Anaesthetix 5:0f4204941604 303 roll = filter.getRoll();
Anaesthetix 5:0f4204941604 304 pitch = filter.getPitch();
Anaesthetix 5:0f4204941604 305
Anaesthetix 5:0f4204941604 306 angles[3] = gyrodata[1];
Anaesthetix 5:0f4204941604 307 angles[4] = gyrodata[0];
Anaesthetix 5:0f4204941604 308 angles[5] = gyrodata[2];
Anaesthetix 5:0f4204941604 309
Anaesthetix 5:0f4204941604 310 //Simple first order complementary filter -> TODO: CHECK STEP RESPONSE
Anaesthetix 5:0f4204941604 311 angles[0] = 0.99f*(angles[0] + (gyrodata[1]*time)) + 0.01f*pitch;
Anaesthetix 5:0f4204941604 312 angles[1] = 0.99f*(angles[1] + (gyrodata[0]*time)) + 0.01f*roll;
Anaesthetix 5:0f4204941604 313
Anaesthetix 5:0f4204941604 314 // If [VAR] is NaN use previous value
Anaesthetix 5:0f4204941604 315 if(angles[0] != angles[0]) angles[0] = angles_temp[0];
Anaesthetix 5:0f4204941604 316 if(angles[1] != angles[1]) angles[1] = angles_temp[1];
Anaesthetix 5:0f4204941604 317 if(angles[0] == angles[0]) angles_temp[0] = angles[0];
Anaesthetix 5:0f4204941604 318 if(angles[1] == angles[1]) angles_temp[1] = angles[1];
Anaesthetix 5:0f4204941604 319
Anaesthetix 5:0f4204941604 320 tijd.stop();
Anaesthetix 5:0f4204941604 321 tijd.reset();
Anaesthetix 5:0f4204941604 322 tijd.start();
Anaesthetix 5:0f4204941604 323
Anaesthetix 5:0f4204941604 324 /*
Anaesthetix 5:0f4204941604 325 if(print.read_ms() > 100) {
Anaesthetix 5:0f4204941604 326 print.stop();
Anaesthetix 5:0f4204941604 327 print.reset();
Anaesthetix 5:0f4204941604 328 print.start();
Anaesthetix 5:0f4204941604 329 //led2 = !led2;
Anaesthetix 5:0f4204941604 330 // pc.printf("X: %.2f Y: %.2f %.0f\r\n", angles[0], angles[1], samplef);
Anaesthetix 5:0f4204941604 331 pc.printf("%.2f %.0f\r\n", angles[1], samplef);
Anaesthetix 5:0f4204941604 332 }
Anaesthetix 5:0f4204941604 333 */
Anaesthetix 5:0f4204941604 334 pc.printf("%.2f %.0f\r\n", angles[0], samplef);
Anaesthetix 5:0f4204941604 335 if(rxd) {
Anaesthetix 5:0f4204941604 336 led2 = !led2;
Anaesthetix 5:0f4204941604 337 rxd = false;
Anaesthetix 5:0f4204941604 338 // pc.printf("%d %d %d %d\r\n", motor[0], motor[1], motor[2], motor[3]);
Anaesthetix 0:0929d3d566cf 339 }
Anaesthetix 0:0929d3d566cf 340
Anaesthetix 5:0f4204941604 341 if(rx_data[5] > 1650) ch_sw = 0;
Anaesthetix 5:0f4204941604 342 else ch_sw = 1;
Anaesthetix 5:0f4204941604 343
Anaesthetix 5:0f4204941604 344 calccomp(rx_data, angles, motor);
Anaesthetix 0:0929d3d566cf 345
Anaesthetix 5:0f4204941604 346 motor1.pulsewidth_us(motor[0]);
Anaesthetix 5:0f4204941604 347 motor2.pulsewidth_us(motor[1]);
Anaesthetix 5:0f4204941604 348 motor3.pulsewidth_us(motor[2]);
Anaesthetix 5:0f4204941604 349 motor4.pulsewidth_us(motor[3]);
Anaesthetix 0:0929d3d566cf 350 }
Anaesthetix 0:0929d3d566cf 351 }