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.
Diff: main.cpp
- Revision:
- 6:033ad7377fee
- Parent:
- 5:0f4204941604
- Child:
- 7:d86c41443f6d
--- a/main.cpp Fri Jul 13 14:27:30 2018 +0000 +++ b/main.cpp Sat Jul 14 10:30:20 2018 +0000 @@ -47,9 +47,9 @@ PwmOut motor1(p22); PwmOut motor2(p21); -float Kp = 1.05f; -float Ki = 0; -float Kd = 0.45; +float KP_x, KI_x, KD_x; +float KP_y, KI_y, KD_y; +float KP_z, KI_z, KD_z; Timer trx0, trx1, trx2, trx3, trx4, trx5; int rx_data[6] = {0}; @@ -163,14 +163,14 @@ while(1) { PIDsetup = pc1->getc(); if(PIDsetup == 'R') { - for(int i=0; i<6; i++) { + for(int i=0; i<18; i++) { pc1->putc(addr[i]); wait_ms(1); } } if(PIDsetup == 'W') { - for(int i=0; i<6; i++) { + for(int i=0; i<18; i++) { mem[i] = pc1->getc(); } iap.prepare( TARGET_SECTOR, TARGET_SECTOR ); @@ -239,18 +239,40 @@ tempval = addr[0]; tempval = tempval + (addr[1] << 8); - Kp = ((float)tempval) / 100; + KP_x = ((float)tempval) / 100; tempval = addr[2]; tempval = tempval + (addr[3] << 8); - Ki = ((float)tempval) / 100; + KI_x = ((float)tempval) / 100; tempval = addr[4]; tempval = tempval + (addr[5] << 8); - Kd = ((float)tempval) / 100; + KD_x = ((float)tempval) / 100; + + tempval = addr[6]; + tempval = tempval + (addr[7] << 8); + KP_y = ((float)tempval) / 100; + tempval = addr[8]; + tempval = tempval + (addr[9] << 8); + KI_y = ((float)tempval) / 100; + tempval = addr[19]; + tempval = tempval + (addr[11] << 8); + KD_y = ((float)tempval) / 100; + + tempval = addr[12]; + tempval = tempval + (addr[13] << 8); + KP_z = ((float)tempval) / 100; + tempval = addr[14]; + tempval = tempval + (addr[15] << 8); + KI_z = ((float)tempval) / 100; + tempval = addr[16]; + tempval = tempval + (addr[17] << 8); + KD_z = ((float)tempval) / 100; mpu9250.init(1,BITS_DLPF_CFG_188HZ); - - pc.printf("%.2f %.2f %.2f\r\n", Kp, Ki, Kd); - + /* + pc.printf("%.2f %.2f %.2f\r\n", KP_x, KI_x, KD_x); + pc.printf("%.2f %.2f %.2f\r\n", KP_y, KI_y, KD_y); + pc.printf("%.2f %.2f %.2f\r\n", KP_z, KI_z, KD_z); + */ rx_rud.rise(&rx0); rx_elev.rise(&rx1); rx_thr.rise(&rx2); @@ -278,7 +300,6 @@ temp3 = temp2*temp; // Temperature compensation - // Derived from datalogging gyro data over a wide temperature range and using the fitting tool in Matlab's plotter tempcomp[0] = -1.77e-6*temp2 + 0.000233*temp + 0.02179; tempcomp[1] = 0.0005727*temp - 0.01488; tempcomp[2] = -2.181e-7*temp3 + 1.754e-5*temp2 - 0.0004955*temp; @@ -338,6 +359,7 @@ // pc.printf("%d %d %d %d\r\n", motor[0], motor[1], motor[2], motor[3]); } + // To change VTX channel/band/power with the remote if(rx_data[5] > 1650) ch_sw = 0; else ch_sw = 1; @@ -347,5 +369,11 @@ motor2.pulsewidth_us(motor[1]); motor3.pulsewidth_us(motor[2]); motor4.pulsewidth_us(motor[3]); + /* + motor1.pulsewidth_us(rx_data[2]-20); + motor2.pulsewidth_us(rx_data[2]-20); + motor3.pulsewidth_us(rx_data[2]-20); + motor4.pulsewidth_us(rx_data[2]-20); + */ } }