Latest version of my quadcopter controller with an LPC1768 and MPU9250.
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?
User | Revision | Line number | New 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 | } |