Erik van de Coevering
/
Multicopter_2018
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Revision 6:033ad7377fee, committed 2018-07-14
- Comitter:
- Anaesthetix
- Date:
- Sat Jul 14 10:30:20 2018 +0000
- Parent:
- 5:0f4204941604
- Child:
- 7:d86c41443f6d
- Commit message:
- Changed overall PID gains to gains per axis.
Changed in this revision
| calccomp.h | Show annotated file Show diff for this revision Revisions of this file |
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/calccomp.h Fri Jul 13 14:27:30 2018 +0000
+++ b/calccomp.h Sat Jul 14 10:30:20 2018 +0000
@@ -1,12 +1,11 @@
// Coded by: Erik van de Coevering
// Axis are mixed up, will fix soon.
-
#include "mbed.h"
DigitalOut led1(LED1); // for stick arming (leds are active low)
-extern float Kp, Ki, Kd;
+extern float KP_x, KI_x, KD_x, KP_y, KI_y, KD_y, KP_z, KI_z, KD_z;
void calccomp(int* ctrl, float* angles, int* motor) //ctrl: 0-rud 1-elev 2-throttle 3-aileron angles: 0-ax 1-ay 2-az 3-gx 4-gy 5-gz
{
@@ -19,7 +18,6 @@
float xcomp = 0;
float ycomp = 0;
int xcomp2 = 0;
- float ruddercomp = 0;
int rud = 0;
int zcomp = 0;
int throttle = 0;
@@ -51,55 +49,55 @@
// Current values used on a 250 size mini racer (still needs tuning): P: 1.9, I: 2.4, D: 0.45
// Calc PID values and prevent integral windup on KIx
- KPx = (angles[0] - xcntrl) * Kp;
- KIx = KIx + ((angles[0] - xcntrl) * Ki * 0.001);
+ KPx = (angles[0] - xcntrl) * KP_y;
+ KIx = KIx + ((angles[0] - xcntrl) * KI_y * 0.001);
if(KIx > 50) KIx = 50;
if(KIx < -50) KIx = -50;
- KDx = (angles[3] + (angles[0] - xcntrl)) * Kd;
+ KDx = (angles[3] + (angles[0] - xcntrl)) * KD_y;
xcomp = KPx + KIx + KDx;
xcomp2 = xcomp*-1;
- //Mix aileron
+ //Mix pitch
m1 = m1 - xcomp2;
m2 = m2 + xcomp2;
m3 = m3 + xcomp2;
m4 = m4 - xcomp2;
// Calc PID values and prevent integral windup on KIy
- KPy = (angles[1] + ycntrl) * Kp * 0.8f;
- KIy = KIy + ((angles[1] + ycntrl) * Ki * 0.8f * 0.001);
+ KPy = (angles[1] + ycntrl) * KP_x;
+ KIy = KIy + ((angles[1] + ycntrl) * KI_x * 0.001);
if(KIy > 50) KIy = 50;
if(KIy < -50) KIy = -50;
- KDy = (angles[4] + (angles[1] + ycntrl)) * Kd * 0.8f;
+ KDy = (angles[4] + (angles[1] + ycntrl)) * KD_x;
ycomp = KPy + KIy + KDy;
ycomp = ycomp*-1;
- //Mix elevator
+ //Mix roll
m1 = m1 + ycomp;
m2 = m2 + ycomp;
m3 = m3 - ycomp;
m4 = m4 - ycomp;
- //Yaw PID gains hardcoded for now
+ //Calc yaw compensation and mix
error_z = angles[5] + rud;
- KPz = error_z * 1.5f;
- KIz = KIz + (error_z * 0.001f);
+ KPz = error_z * KP_z;
+ KIz = KIz + (error_z * 0.001f * KI_z);
- if(KIz > 50) KIz = 50;
- if(KIz < -50) KIz = -50;
+ if(KIz > 80) KIz = 80;
+ if(KIz < -80) KIz = -80;
- KDz = (error_z - error_z1) * 0.3f;
+ KDz = (error_z - error_z1) * KD_z;
error_z1 = error_z;
zcomp = (KPz + KIz + KDz) * -1.0f;
- //Mix rudder
+ //Mix yaw
m1 = m1 - zcomp;
m2 = m2 + zcomp;
m3 = m3 - zcomp;
@@ -130,4 +128,4 @@
motor[1] = m2;
motor[2] = m3;
motor[3] = m4;
-}
+}
\ No newline at end of file
--- 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);
+ */
}
}