Successful acro and level mode now! Relying on MPU9250 as base sensor. I'm working continuously on tuning and features :) NEWEST VERSION ON: https://github.com/MaEtUgR/FlyBed (CODE 100% compatible/copyable)
main.cpp@1:60882db03b0f, 2015-09-09 (annotated)
- Committer:
- maetugr
- Date:
- Wed Sep 09 09:58:41 2015 +0000
- Revision:
- 1:60882db03b0f
- Parent:
- 0:37f0c1e8fa66
- Child:
- 2:f0a9ecb4d049
Gyro P Controller on one axis tested; everything should work again
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
maetugr | 0:37f0c1e8fa66 | 1 | /* X- Configuration |
maetugr | 0:37f0c1e8fa66 | 2 | m3 m0 -- > |
maetugr | 0:37f0c1e8fa66 | 3 | \ / / \ / |
maetugr | 0:37f0c1e8fa66 | 4 | / \ V | |
maetugr | 0:37f0c1e8fa66 | 5 | m2 m1 \ |
maetugr | 0:37f0c1e8fa66 | 6 | PITCH ROLL*/ |
maetugr | 0:37f0c1e8fa66 | 7 | #include "mbed.h" |
maetugr | 0:37f0c1e8fa66 | 8 | #include "LED.h" // LEDs framework for blinking ;) |
maetugr | 0:37f0c1e8fa66 | 9 | #include "PC.h" // Serial Port via USB by Roland Elmiger for debugging with Terminal (driver needed: https://mbed.org/media/downloads/drivers/mbedWinSerial_16466.exe) |
maetugr | 0:37f0c1e8fa66 | 10 | |
maetugr | 0:37f0c1e8fa66 | 11 | #include "IMU_10DOF.h" // Complete IMU class for 10DOF-Board (L3G4200D, ADXL345, HMC5883, BMP085) |
maetugr | 0:37f0c1e8fa66 | 12 | #include "RC_Channel.h" // RemoteControl Channels with PPM |
maetugr | 0:37f0c1e8fa66 | 13 | #include "PID.h" // PID Library (slim, self written) |
maetugr | 0:37f0c1e8fa66 | 14 | #include "Servo.h" // Motor PPM using any DigitalOut Pin |
maetugr | 0:37f0c1e8fa66 | 15 | |
maetugr | 0:37f0c1e8fa66 | 16 | #define PPM_FREQU 495 // Hz Frequency of PPM Signal for ESCs (maximum <500Hz) |
maetugr | 0:37f0c1e8fa66 | 17 | #define INTEGRAL_MAX 300 // maximal output offset that can result from integrating errors |
maetugr | 0:37f0c1e8fa66 | 18 | #define RC_SENSITIVITY 30 // maximal angle from horizontal that the PID is aming for |
maetugr | 0:37f0c1e8fa66 | 19 | #define YAWSPEED 1.0 // maximal speed of yaw rotation in degree per Rate |
maetugr | 0:37f0c1e8fa66 | 20 | #define AILERON 0 // RC |
maetugr | 0:37f0c1e8fa66 | 21 | #define ELEVATOR 1 |
maetugr | 0:37f0c1e8fa66 | 22 | #define RUDDER 2 |
maetugr | 0:37f0c1e8fa66 | 23 | #define THROTTLE 3 |
maetugr | 0:37f0c1e8fa66 | 24 | #define CHANNEL8 4 |
maetugr | 0:37f0c1e8fa66 | 25 | #define CHANNEL7 5 |
maetugr | 0:37f0c1e8fa66 | 26 | #define CHANNEL6 6 |
maetugr | 0:37f0c1e8fa66 | 27 | #define ROLL 0 // Axes |
maetugr | 0:37f0c1e8fa66 | 28 | #define PITCH 1 |
maetugr | 0:37f0c1e8fa66 | 29 | #define YAW 2 |
maetugr | 0:37f0c1e8fa66 | 30 | |
maetugr | 0:37f0c1e8fa66 | 31 | #define SQRT2 0.7071067811865 |
maetugr | 0:37f0c1e8fa66 | 32 | |
maetugr | 0:37f0c1e8fa66 | 33 | //#define CONSTRAIN(VAL,LIMIT) ((VAL)<(-LIMIT)?(-LIMIT):((VAL)>(LIMIT)?(LIMIT):(VAL))) |
maetugr | 0:37f0c1e8fa66 | 34 | |
maetugr | 0:37f0c1e8fa66 | 35 | bool armed = false; // is for security (when false no motor rotates any more) |
maetugr | 0:37f0c1e8fa66 | 36 | bool debug = true; // shows if we want output for the computer |
maetugr | 0:37f0c1e8fa66 | 37 | bool RC_present = false; // shows if an RC is present |
maetugr | 1:60882db03b0f | 38 | //float P_R = 2.5, I_R = 3.7, D_R = 0; |
maetugr | 1:60882db03b0f | 39 | float P_R = 6, I_R = 0, D_R = 0; |
maetugr | 0:37f0c1e8fa66 | 40 | float P_A = 1.865, I_A = 1.765, D_A = 0; |
maetugr | 0:37f0c1e8fa66 | 41 | //float P = 13.16, I = 8, D = 2.73; // PID values |
maetugr | 0:37f0c1e8fa66 | 42 | float PY = 3.2, IY = 0, DY = 0; |
maetugr | 0:37f0c1e8fa66 | 43 | //float PY = 5.37, IY = 0, DY = 3; // PID values for Yaw |
maetugr | 0:37f0c1e8fa66 | 44 | float RC_angle[] = {0,0,0}; // Angle of the RC Sticks, to steer the QC |
maetugr | 0:37f0c1e8fa66 | 45 | float Motor_speed[4] = {0,0,0,0}; // Mixed Motorspeeds, ready to send |
maetugr | 0:37f0c1e8fa66 | 46 | //float * command_pointer = &D; // TODO: pointer to varible that's going to be changed by UART command |
maetugr | 0:37f0c1e8fa66 | 47 | |
maetugr | 0:37f0c1e8fa66 | 48 | /*float max[3] = {-10000,-10000,-10000}; |
maetugr | 0:37f0c1e8fa66 | 49 | float min[3] = {10000,10000,10000};*/ |
maetugr | 0:37f0c1e8fa66 | 50 | LED LEDs; |
maetugr | 0:37f0c1e8fa66 | 51 | PC pc(USBTX, USBRX, 115200); // USB |
maetugr | 0:37f0c1e8fa66 | 52 | //PC pc(p9, p10, 115200); // Bluetooth |
maetugr | 0:37f0c1e8fa66 | 53 | IMU_10DOF IMU(p5, p6, p7, p19); |
maetugr | 0:37f0c1e8fa66 | 54 | RC_Channel RC[] = {RC_Channel(p8,1), RC_Channel(p15,2), RC_Channel(p17,4), RC_Channel(p16,3), RC_Channel(p25,2), RC_Channel(p26,4), RC_Channel(p29,3)}; // no p19/p20 ! |
maetugr | 0:37f0c1e8fa66 | 55 | PID Controller_Rate[] = {PID(P_R, I_R, D_R, INTEGRAL_MAX), PID(P_R, I_R, D_R, INTEGRAL_MAX), PID(PY, IY, DY, INTEGRAL_MAX)}; // 0:X:Roll 1:Y:Pitch 2:Z:Yaw |
maetugr | 0:37f0c1e8fa66 | 56 | PID Controller_Angle[] = {PID(P_A, I_A, D_A, INTEGRAL_MAX), PID(P_A, I_A, D_A, INTEGRAL_MAX), PID(0, 0, 0, INTEGRAL_MAX)}; |
maetugr | 0:37f0c1e8fa66 | 57 | Servo ESC[] = {Servo(p21,PPM_FREQU), Servo(p22,PPM_FREQU), Servo(p23,PPM_FREQU), Servo(p24,PPM_FREQU)}; // use any DigitalOit Pin |
maetugr | 0:37f0c1e8fa66 | 58 | |
maetugr | 0:37f0c1e8fa66 | 59 | extern "C" void mbed_reset(); |
maetugr | 0:37f0c1e8fa66 | 60 | |
maetugr | 0:37f0c1e8fa66 | 61 | void executer() { |
maetugr | 0:37f0c1e8fa66 | 62 | char command = pc.getc(); |
maetugr | 0:37f0c1e8fa66 | 63 | if (command == 'X') |
maetugr | 0:37f0c1e8fa66 | 64 | mbed_reset(); |
maetugr | 0:37f0c1e8fa66 | 65 | if (command == '-') |
maetugr | 0:37f0c1e8fa66 | 66 | debug = !debug; |
maetugr | 1:60882db03b0f | 67 | if (command == 'w') |
maetugr | 1:60882db03b0f | 68 | P_R += 0.1; |
maetugr | 1:60882db03b0f | 69 | if (command == 's') |
maetugr | 1:60882db03b0f | 70 | P_R -= 0.1; |
maetugr | 0:37f0c1e8fa66 | 71 | |
maetugr | 0:37f0c1e8fa66 | 72 | pc.putc(command); |
maetugr | 0:37f0c1e8fa66 | 73 | LEDs.tilt(2); |
maetugr | 0:37f0c1e8fa66 | 74 | } |
maetugr | 0:37f0c1e8fa66 | 75 | |
maetugr | 0:37f0c1e8fa66 | 76 | int main() { |
maetugr | 0:37f0c1e8fa66 | 77 | pc.attach(&executer); |
maetugr | 0:37f0c1e8fa66 | 78 | while(1) { |
maetugr | 0:37f0c1e8fa66 | 79 | // IMU |
maetugr | 0:37f0c1e8fa66 | 80 | IMU.readAngles(); |
maetugr | 0:37f0c1e8fa66 | 81 | |
maetugr | 0:37f0c1e8fa66 | 82 | // Arming / disarming |
maetugr | 0:37f0c1e8fa66 | 83 | RC_present = !(RC[AILERON].read() == -100 || RC[ELEVATOR].read() == -100 || RC[RUDDER].read() == -100 || RC[THROTTLE].read() == -100); // TODO: Failsafe |
maetugr | 0:37f0c1e8fa66 | 84 | if(RC[THROTTLE].read() < 20 && RC[RUDDER].read() > 850) { |
maetugr | 0:37f0c1e8fa66 | 85 | armed = true; |
maetugr | 0:37f0c1e8fa66 | 86 | RC_angle[YAW] = IMU.angle[YAW]; |
maetugr | 0:37f0c1e8fa66 | 87 | } |
maetugr | 0:37f0c1e8fa66 | 88 | if((RC[THROTTLE].read() < 30 && RC[RUDDER].read() < 30) || !RC_present) { |
maetugr | 0:37f0c1e8fa66 | 89 | armed = false; |
maetugr | 0:37f0c1e8fa66 | 90 | } |
maetugr | 0:37f0c1e8fa66 | 91 | |
maetugr | 0:37f0c1e8fa66 | 92 | // Setting PID Values from auxiliary RC channels |
maetugr | 0:37f0c1e8fa66 | 93 | //if (RC[CHANNEL8].read() > 0 && RC[CHANNEL8].read() < 1000) |
maetugr | 0:37f0c1e8fa66 | 94 | // P_R = 0 + (((float)RC[CHANNEL8].read()) * 3 / 1000); |
maetugr | 0:37f0c1e8fa66 | 95 | /*if (RC[CHANNEL7].read() > 0 && RC[CHANNEL7].read() < 1000) |
maetugr | 0:37f0c1e8fa66 | 96 | I_R = 0 + (((float)RC[CHANNEL7].read()) * 12 / 1000);*/ |
maetugr | 0:37f0c1e8fa66 | 97 | for(int i=0;i<3;i++) |
maetugr | 0:37f0c1e8fa66 | 98 | Controller_Angle[i].setPID(P_A,I_A,D_A); |
maetugr | 0:37f0c1e8fa66 | 99 | for(int i=0;i<2;i++) |
maetugr | 0:37f0c1e8fa66 | 100 | Controller_Rate[i].setPID(P_R,I_R,D_R); // give the new PID values to roll and pitch controller |
maetugr | 0:37f0c1e8fa66 | 101 | Controller_Rate[YAW].setPID(PY,IY,DY); |
maetugr | 0:37f0c1e8fa66 | 102 | |
maetugr | 0:37f0c1e8fa66 | 103 | // RC Angle ROLL-PITCH-Part |
maetugr | 0:37f0c1e8fa66 | 104 | for(int i=0;i<2;i++) { // calculate new angle we want the QC to have |
maetugr | 0:37f0c1e8fa66 | 105 | if (RC_present) |
maetugr | 0:37f0c1e8fa66 | 106 | RC_angle[i] = (RC[i].read()-500)*RC_SENSITIVITY/500.0; |
maetugr | 0:37f0c1e8fa66 | 107 | else |
maetugr | 0:37f0c1e8fa66 | 108 | RC_angle[i] = 0; |
maetugr | 0:37f0c1e8fa66 | 109 | } |
maetugr | 0:37f0c1e8fa66 | 110 | |
maetugr | 0:37f0c1e8fa66 | 111 | // RC Angle YAW-Part |
maetugr | 0:37f0c1e8fa66 | 112 | float RC_yaw_adding; // temporary variable to take the desired yaw adjustment |
maetugr | 0:37f0c1e8fa66 | 113 | if (RC_present && RC[THROTTLE].read() > 20) |
maetugr | 0:37f0c1e8fa66 | 114 | RC_yaw_adding = -(RC[RUDDER].read()-500)*YAWSPEED/500; // the yaw angle is integrated from stick input |
maetugr | 0:37f0c1e8fa66 | 115 | else |
maetugr | 0:37f0c1e8fa66 | 116 | RC_yaw_adding = 0; |
maetugr | 0:37f0c1e8fa66 | 117 | |
maetugr | 0:37f0c1e8fa66 | 118 | RC_angle[YAW] = RC_angle[YAW] + RC_yaw_adding < -180 ? RC_angle[YAW] + 360 + RC_yaw_adding : RC_angle[YAW] + RC_yaw_adding; // make shure it's in the cycle -180 to 180 |
maetugr | 0:37f0c1e8fa66 | 119 | RC_angle[YAW] = RC_angle[YAW] + RC_yaw_adding > 180 ? RC_angle[YAW] - 360 + RC_yaw_adding : RC_angle[YAW] + RC_yaw_adding; |
maetugr | 0:37f0c1e8fa66 | 120 | |
maetugr | 0:37f0c1e8fa66 | 121 | |
maetugr | 0:37f0c1e8fa66 | 122 | // Controlling |
maetugr | 0:37f0c1e8fa66 | 123 | for(int i=0;i<2;i++) { |
maetugr | 0:37f0c1e8fa66 | 124 | Controller_Rate[i].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying |
maetugr | 0:37f0c1e8fa66 | 125 | Controller_Rate[i].compute((RC[i].read()-500.0)*100.0/500.0, IMU.mpu.Gyro[i]); // give the controller the actual gyro values and get his advice to correct |
maetugr | 0:37f0c1e8fa66 | 126 | } |
maetugr | 0:37f0c1e8fa66 | 127 | Controller_Rate[2].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying |
maetugr | 0:37f0c1e8fa66 | 128 | if (RC[THROTTLE].read() > 20) |
maetugr | 0:37f0c1e8fa66 | 129 | Controller_Rate[2].compute(-(RC[2].read()-500.0)*100.0/500.0, IMU.mpu.Gyro[2]); // give the controller the actual gyro values and get his advice to correct |
maetugr | 0:37f0c1e8fa66 | 130 | else |
maetugr | 0:37f0c1e8fa66 | 131 | Controller_Rate[2].compute(0, IMU.mpu.Gyro[2]); // give the controller the actual gyro values and get his advice to correct |
maetugr | 0:37f0c1e8fa66 | 132 | |
maetugr | 0:37f0c1e8fa66 | 133 | |
maetugr | 0:37f0c1e8fa66 | 134 | // Mixing |
maetugr | 0:37f0c1e8fa66 | 135 | Motor_speed[0] = RC[THROTTLE].read() +SQRT2*Controller_Rate[ROLL].Value -SQRT2*Controller_Rate[PITCH].Value; // X Configuration |
maetugr | 0:37f0c1e8fa66 | 136 | Motor_speed[1] = RC[THROTTLE].read() -SQRT2*Controller_Rate[ROLL].Value -SQRT2*Controller_Rate[PITCH].Value; // |
maetugr | 0:37f0c1e8fa66 | 137 | Motor_speed[2] = RC[THROTTLE].read() -SQRT2*Controller_Rate[ROLL].Value +SQRT2*Controller_Rate[PITCH].Value; // |
maetugr | 0:37f0c1e8fa66 | 138 | Motor_speed[3] = RC[THROTTLE].read() +SQRT2*Controller_Rate[ROLL].Value +SQRT2*Controller_Rate[PITCH].Value; // |
maetugr | 0:37f0c1e8fa66 | 139 | |
maetugr | 0:37f0c1e8fa66 | 140 | Motor_speed[0] -= Controller_Rate[YAW].Value; |
maetugr | 0:37f0c1e8fa66 | 141 | Motor_speed[2] -= Controller_Rate[YAW].Value; |
maetugr | 0:37f0c1e8fa66 | 142 | Motor_speed[3] += Controller_Rate[YAW].Value; |
maetugr | 0:37f0c1e8fa66 | 143 | Motor_speed[1] += Controller_Rate[YAW].Value; |
maetugr | 0:37f0c1e8fa66 | 144 | |
maetugr | 0:37f0c1e8fa66 | 145 | if (armed) // for SECURITY! |
maetugr | 0:37f0c1e8fa66 | 146 | { |
maetugr | 0:37f0c1e8fa66 | 147 | debug = false; |
maetugr | 0:37f0c1e8fa66 | 148 | // PITCH |
maetugr | 0:37f0c1e8fa66 | 149 | //ESC[0] = (int)Motor_speed[0]>50 ? (int)Motor_speed[0] : 50; |
maetugr | 0:37f0c1e8fa66 | 150 | //ESC[2] = (int)Motor_speed[2]>50 ? (int)Motor_speed[2] : 50; |
maetugr | 0:37f0c1e8fa66 | 151 | // ROLL |
maetugr | 0:37f0c1e8fa66 | 152 | //ESC[1] = (int)Motor_speed[1]>50 ? (int)Motor_speed[1] : 50; |
maetugr | 0:37f0c1e8fa66 | 153 | //ESC[3] = (int)Motor_speed[3]>50 ? (int)Motor_speed[3] : 50; |
maetugr | 0:37f0c1e8fa66 | 154 | for(int i=0;i<4;i++) // Set new motorspeeds |
maetugr | 0:37f0c1e8fa66 | 155 | ESC[i] = (int)Motor_speed[i]>50 ? (int)Motor_speed[i] : 50; |
maetugr | 0:37f0c1e8fa66 | 156 | |
maetugr | 0:37f0c1e8fa66 | 157 | } else { |
maetugr | 0:37f0c1e8fa66 | 158 | for(int i=0;i<4;i++) // for security reason, set every motor to zero speed |
maetugr | 0:37f0c1e8fa66 | 159 | ESC[i] = 0; |
maetugr | 1:60882db03b0f | 160 | debug = true; |
maetugr | 0:37f0c1e8fa66 | 161 | } |
maetugr | 0:37f0c1e8fa66 | 162 | |
maetugr | 0:37f0c1e8fa66 | 163 | if (debug) { |
maetugr | 1:60882db03b0f | 164 | pc.printf("$STATE,%d,%.3f\r\n", armed, IMU.dt); |
maetugr | 1:60882db03b0f | 165 | //pc.printf("$RC,%d,%d,%d,%d,%d,%d,%d\r\n", RC[AILERON].read(), RC[ELEVATOR].read(), RC[RUDDER].read(), RC[THROTTLE].read(), RC[CHANNEL6].read(), RC[CHANNEL7].read(), RC[CHANNEL8].read()); |
maetugr | 1:60882db03b0f | 166 | //pc.printf("$GYRO,%.3f,%.3f,%.3f\r\n", IMU.mpu.Gyro[ROLL], IMU.mpu.Gyro[PITCH], IMU.mpu.Gyro[YAW]); |
maetugr | 1:60882db03b0f | 167 | //pc.printf("$ACC,%.3f,%.3f,%.3f\r\n", IMU.mpu.Acc[ROLL], IMU.mpu.Acc[PITCH], IMU.mpu.Acc[YAW]); |
maetugr | 1:60882db03b0f | 168 | //pc.printf("$ANG,%.3f,%.3f,%.3f\r\n", IMU.angle[ROLL], IMU.angle[PITCH], IMU.angle[YAW]); |
maetugr | 0:37f0c1e8fa66 | 169 | //pc.printf("$RCANG,%.3f,%.3f,%.3f\r\n", RC_angle[ROLL], RC_angle[PITCH], RC_angle[YAW]); |
maetugr | 1:60882db03b0f | 170 | pc.printf("$CONT,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\r\n", Controller_Rate[ROLL].Value, Controller_Rate[PITCH].Value, Controller_Rate[YAW].Value, P_R, I_R, D_R); |
maetugr | 0:37f0c1e8fa66 | 171 | //pc.printf("$MOT,%d,%d,%d,%d\r\n", (int)Motor_speed[0], (int)Motor_speed[1], (int)Motor_speed[2], (int)Motor_speed[3]); |
maetugr | 0:37f0c1e8fa66 | 172 | |
maetugr | 1:60882db03b0f | 173 | wait(0.04); |
maetugr | 0:37f0c1e8fa66 | 174 | } |
maetugr | 0:37f0c1e8fa66 | 175 | |
maetugr | 0:37f0c1e8fa66 | 176 | LEDs.rollnext(); |
maetugr | 0:37f0c1e8fa66 | 177 | } |
maetugr | 0:37f0c1e8fa66 | 178 | } |