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@5:8ea99e98de73, 2015-09-11 (annotated)
- Committer:
- maetugr
- Date:
- Fri Sep 11 08:43:35 2015 +0000
- Revision:
- 5:8ea99e98de73
- Parent:
- 4:b2efa7f03701
- Child:
- 6:f258093beed9
level mode with 12" props pretty stable indoors; some very small wobles remaining
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
maetugr | 0:37f0c1e8fa66 | 1 | /* X- Configuration |
maetugr | 2:f0a9ecb4d049 | 2 | m2 m3 -- > |
maetugr | 2:f0a9ecb4d049 | 3 | \ / / \ / |
maetugr | 2:f0a9ecb4d049 | 4 | / \ V | |
maetugr | 2:f0a9ecb4d049 | 5 | m1 m0 \ |
maetugr | 2:f0a9ecb4d049 | 6 | ROLL PITCH */ |
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 | 5:8ea99e98de73 | 18 | #define RC_SENSITIVITY 15 // 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 | bool armed = false; // is for security (when false no motor rotates any more) |
maetugr | 2:f0a9ecb4d049 | 34 | bool debug = true; // shows if we want output for the computer |
maetugr | 3:3709be130495 | 35 | bool level = false; // switches between self leveling and acro mode |
maetugr | 0:37f0c1e8fa66 | 36 | bool RC_present = false; // shows if an RC is present |
maetugr | 5:8ea99e98de73 | 37 | float P_R = 3.3, I_R = 1.1, D_R = 0; // PID values for the rate controller |
maetugr | 5:8ea99e98de73 | 38 | float P_A = 2.2, I_A = 0, D_A = 0; // PID values for the angle controller P_A = 1.865, I_A = 1.765, D_A = 0 |
maetugr | 4:b2efa7f03701 | 39 | float PY = 2.3, IY = 0, DY = 0; // PID values for Yaw |
maetugr | 0:37f0c1e8fa66 | 40 | float RC_angle[] = {0,0,0}; // Angle of the RC Sticks, to steer the QC |
maetugr | 0:37f0c1e8fa66 | 41 | float Motor_speed[4] = {0,0,0,0}; // Mixed Motorspeeds, ready to send |
maetugr | 0:37f0c1e8fa66 | 42 | |
maetugr | 0:37f0c1e8fa66 | 43 | LED LEDs; |
maetugr | 3:3709be130495 | 44 | //PC pc(USBTX, USBRX, 115200); // USB |
maetugr | 3:3709be130495 | 45 | PC pc(p9, p10, 115200); // Bluetooth PIN: 1234 |
maetugr | 0:37f0c1e8fa66 | 46 | IMU_10DOF IMU(p5, p6, p7, p19); |
maetugr | 0:37f0c1e8fa66 | 47 | 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 | 48 | 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 | 49 | 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 | 50 | 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 | 51 | |
maetugr | 0:37f0c1e8fa66 | 52 | extern "C" void mbed_reset(); |
maetugr | 0:37f0c1e8fa66 | 53 | |
maetugr | 0:37f0c1e8fa66 | 54 | void executer() { |
maetugr | 0:37f0c1e8fa66 | 55 | char command = pc.getc(); |
maetugr | 0:37f0c1e8fa66 | 56 | if (command == 'X') |
maetugr | 0:37f0c1e8fa66 | 57 | mbed_reset(); |
maetugr | 0:37f0c1e8fa66 | 58 | if (command == '-') |
maetugr | 0:37f0c1e8fa66 | 59 | debug = !debug; |
maetugr | 2:f0a9ecb4d049 | 60 | |
maetugr | 3:3709be130495 | 61 | if (command == ':') |
maetugr | 3:3709be130495 | 62 | armed = true; |
maetugr | 3:3709be130495 | 63 | if (command == ' ') |
maetugr | 3:3709be130495 | 64 | armed = false; |
maetugr | 3:3709be130495 | 65 | |
maetugr | 3:3709be130495 | 66 | if (command == 'q') |
maetugr | 3:3709be130495 | 67 | level = true; |
maetugr | 3:3709be130495 | 68 | if (command == 'a') |
maetugr | 3:3709be130495 | 69 | level = false; |
maetugr | 3:3709be130495 | 70 | |
maetugr | 1:60882db03b0f | 71 | if (command == 'w') |
maetugr | 1:60882db03b0f | 72 | P_R += 0.1; |
maetugr | 1:60882db03b0f | 73 | if (command == 's') |
maetugr | 1:60882db03b0f | 74 | P_R -= 0.1; |
maetugr | 0:37f0c1e8fa66 | 75 | |
maetugr | 3:3709be130495 | 76 | if (command == 'e') |
maetugr | 5:8ea99e98de73 | 77 | I_R += 0.1; |
maetugr | 3:3709be130495 | 78 | if (command == 'd') |
maetugr | 5:8ea99e98de73 | 79 | I_R -= 0.1; |
maetugr | 3:3709be130495 | 80 | |
maetugr | 3:3709be130495 | 81 | if (command == 'r') |
maetugr | 5:8ea99e98de73 | 82 | P_A += 0.1; |
maetugr | 3:3709be130495 | 83 | if (command == 'f') |
maetugr | 5:8ea99e98de73 | 84 | P_A -= 0.1; |
maetugr | 2:f0a9ecb4d049 | 85 | |
maetugr | 4:b2efa7f03701 | 86 | if (command == 't') |
maetugr | 4:b2efa7f03701 | 87 | I_A += 0.1; |
maetugr | 4:b2efa7f03701 | 88 | if (command == 'g') |
maetugr | 4:b2efa7f03701 | 89 | I_A -= 0.1; |
maetugr | 4:b2efa7f03701 | 90 | |
maetugr | 5:8ea99e98de73 | 91 | if (command == 'z') |
maetugr | 5:8ea99e98de73 | 92 | PY += 0.1; |
maetugr | 5:8ea99e98de73 | 93 | if (command == 'h') |
maetugr | 5:8ea99e98de73 | 94 | PY -= 0.1; |
maetugr | 5:8ea99e98de73 | 95 | |
maetugr | 0:37f0c1e8fa66 | 96 | pc.putc(command); |
maetugr | 0:37f0c1e8fa66 | 97 | LEDs.tilt(2); |
maetugr | 0:37f0c1e8fa66 | 98 | } |
maetugr | 0:37f0c1e8fa66 | 99 | |
maetugr | 0:37f0c1e8fa66 | 100 | int main() { |
maetugr | 0:37f0c1e8fa66 | 101 | pc.attach(&executer); |
maetugr | 0:37f0c1e8fa66 | 102 | while(1) { |
maetugr | 0:37f0c1e8fa66 | 103 | // IMU |
maetugr | 0:37f0c1e8fa66 | 104 | IMU.readAngles(); |
maetugr | 0:37f0c1e8fa66 | 105 | |
maetugr | 0:37f0c1e8fa66 | 106 | // Arming / disarming |
maetugr | 0:37f0c1e8fa66 | 107 | RC_present = !(RC[AILERON].read() == -100 || RC[ELEVATOR].read() == -100 || RC[RUDDER].read() == -100 || RC[THROTTLE].read() == -100); // TODO: Failsafe |
maetugr | 0:37f0c1e8fa66 | 108 | if(RC[THROTTLE].read() < 20 && RC[RUDDER].read() > 850) { |
maetugr | 0:37f0c1e8fa66 | 109 | armed = true; |
maetugr | 0:37f0c1e8fa66 | 110 | RC_angle[YAW] = IMU.angle[YAW]; |
maetugr | 0:37f0c1e8fa66 | 111 | } |
maetugr | 0:37f0c1e8fa66 | 112 | if((RC[THROTTLE].read() < 30 && RC[RUDDER].read() < 30) || !RC_present) { |
maetugr | 0:37f0c1e8fa66 | 113 | armed = false; |
maetugr | 0:37f0c1e8fa66 | 114 | } |
maetugr | 0:37f0c1e8fa66 | 115 | |
maetugr | 0:37f0c1e8fa66 | 116 | // Setting PID Values from auxiliary RC channels |
maetugr | 0:37f0c1e8fa66 | 117 | for(int i=0;i<3;i++) |
maetugr | 0:37f0c1e8fa66 | 118 | Controller_Angle[i].setPID(P_A,I_A,D_A); |
maetugr | 0:37f0c1e8fa66 | 119 | for(int i=0;i<2;i++) |
maetugr | 0:37f0c1e8fa66 | 120 | Controller_Rate[i].setPID(P_R,I_R,D_R); // give the new PID values to roll and pitch controller |
maetugr | 0:37f0c1e8fa66 | 121 | Controller_Rate[YAW].setPID(PY,IY,DY); |
maetugr | 0:37f0c1e8fa66 | 122 | |
maetugr | 0:37f0c1e8fa66 | 123 | // RC Angle ROLL-PITCH-Part |
maetugr | 0:37f0c1e8fa66 | 124 | for(int i=0;i<2;i++) { // calculate new angle we want the QC to have |
maetugr | 0:37f0c1e8fa66 | 125 | if (RC_present) |
maetugr | 0:37f0c1e8fa66 | 126 | RC_angle[i] = (RC[i].read()-500)*RC_SENSITIVITY/500.0; |
maetugr | 0:37f0c1e8fa66 | 127 | else |
maetugr | 0:37f0c1e8fa66 | 128 | RC_angle[i] = 0; |
maetugr | 0:37f0c1e8fa66 | 129 | } |
maetugr | 0:37f0c1e8fa66 | 130 | |
maetugr | 0:37f0c1e8fa66 | 131 | // RC Angle YAW-Part |
maetugr | 0:37f0c1e8fa66 | 132 | float RC_yaw_adding; // temporary variable to take the desired yaw adjustment |
maetugr | 0:37f0c1e8fa66 | 133 | if (RC_present && RC[THROTTLE].read() > 20) |
maetugr | 0:37f0c1e8fa66 | 134 | RC_yaw_adding = -(RC[RUDDER].read()-500)*YAWSPEED/500; // the yaw angle is integrated from stick input |
maetugr | 0:37f0c1e8fa66 | 135 | else |
maetugr | 0:37f0c1e8fa66 | 136 | RC_yaw_adding = 0; |
maetugr | 0:37f0c1e8fa66 | 137 | |
maetugr | 0:37f0c1e8fa66 | 138 | 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 | 139 | 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 | 140 | |
maetugr | 0:37f0c1e8fa66 | 141 | |
maetugr | 0:37f0c1e8fa66 | 142 | // Controlling |
maetugr | 3:3709be130495 | 143 | if (level) { |
maetugr | 3:3709be130495 | 144 | for(int i=0;i<2;i++) { // LEVEL |
maetugr | 3:3709be130495 | 145 | Controller_Angle[i].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying |
maetugr | 3:3709be130495 | 146 | Controller_Angle[i].compute(RC_angle[i], IMU.angle[i]); // give the controller the actual gyro values and get his advice to correct |
maetugr | 3:3709be130495 | 147 | Controller_Rate[i].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying |
maetugr | 3:3709be130495 | 148 | Controller_Rate[i].compute(-Controller_Angle[i].Value, IMU.mpu.Gyro[i]); // give the controller the actual gyro values and get his advice to correct |
maetugr | 3:3709be130495 | 149 | } |
maetugr | 3:3709be130495 | 150 | } else { |
maetugr | 3:3709be130495 | 151 | for(int i=0;i<2;i++) { // ACRO |
maetugr | 3:3709be130495 | 152 | Controller_Rate[i].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying |
maetugr | 3:3709be130495 | 153 | 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 | 3:3709be130495 | 154 | } |
maetugr | 0:37f0c1e8fa66 | 155 | } |
maetugr | 3:3709be130495 | 156 | |
maetugr | 0:37f0c1e8fa66 | 157 | Controller_Rate[2].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying |
maetugr | 0:37f0c1e8fa66 | 158 | if (RC[THROTTLE].read() > 20) |
maetugr | 0:37f0c1e8fa66 | 159 | 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 | 160 | else |
maetugr | 0:37f0c1e8fa66 | 161 | Controller_Rate[2].compute(0, IMU.mpu.Gyro[2]); // give the controller the actual gyro values and get his advice to correct |
maetugr | 4:b2efa7f03701 | 162 | |
maetugr | 4:b2efa7f03701 | 163 | float throttle = 100 + (RC[THROTTLE].read() * 500 / 1000); |
maetugr | 4:b2efa7f03701 | 164 | |
maetugr | 0:37f0c1e8fa66 | 165 | // Mixing |
maetugr | 4:b2efa7f03701 | 166 | Motor_speed[0] = throttle +SQRT2*Controller_Rate[ROLL].Value -SQRT2*Controller_Rate[PITCH].Value; // X Configuration |
maetugr | 4:b2efa7f03701 | 167 | Motor_speed[1] = throttle -SQRT2*Controller_Rate[ROLL].Value -SQRT2*Controller_Rate[PITCH].Value; // |
maetugr | 4:b2efa7f03701 | 168 | Motor_speed[2] = throttle -SQRT2*Controller_Rate[ROLL].Value +SQRT2*Controller_Rate[PITCH].Value; // |
maetugr | 4:b2efa7f03701 | 169 | Motor_speed[3] = throttle +SQRT2*Controller_Rate[ROLL].Value +SQRT2*Controller_Rate[PITCH].Value; // |
maetugr | 0:37f0c1e8fa66 | 170 | |
maetugr | 0:37f0c1e8fa66 | 171 | Motor_speed[0] -= Controller_Rate[YAW].Value; |
maetugr | 0:37f0c1e8fa66 | 172 | Motor_speed[2] -= Controller_Rate[YAW].Value; |
maetugr | 0:37f0c1e8fa66 | 173 | Motor_speed[3] += Controller_Rate[YAW].Value; |
maetugr | 0:37f0c1e8fa66 | 174 | Motor_speed[1] += Controller_Rate[YAW].Value; |
maetugr | 0:37f0c1e8fa66 | 175 | |
maetugr | 0:37f0c1e8fa66 | 176 | if (armed) // for SECURITY! |
maetugr | 0:37f0c1e8fa66 | 177 | { |
maetugr | 0:37f0c1e8fa66 | 178 | debug = false; |
maetugr | 0:37f0c1e8fa66 | 179 | // PITCH |
maetugr | 0:37f0c1e8fa66 | 180 | //ESC[0] = (int)Motor_speed[0]>50 ? (int)Motor_speed[0] : 50; |
maetugr | 0:37f0c1e8fa66 | 181 | //ESC[2] = (int)Motor_speed[2]>50 ? (int)Motor_speed[2] : 50; |
maetugr | 0:37f0c1e8fa66 | 182 | // ROLL |
maetugr | 0:37f0c1e8fa66 | 183 | //ESC[1] = (int)Motor_speed[1]>50 ? (int)Motor_speed[1] : 50; |
maetugr | 0:37f0c1e8fa66 | 184 | //ESC[3] = (int)Motor_speed[3]>50 ? (int)Motor_speed[3] : 50; |
maetugr | 0:37f0c1e8fa66 | 185 | for(int i=0;i<4;i++) // Set new motorspeeds |
maetugr | 5:8ea99e98de73 | 186 | ESC[i] = (int)Motor_speed[i]>100 ? (int)Motor_speed[i] : 100; |
maetugr | 0:37f0c1e8fa66 | 187 | |
maetugr | 0:37f0c1e8fa66 | 188 | } else { |
maetugr | 0:37f0c1e8fa66 | 189 | for(int i=0;i<4;i++) // for security reason, set every motor to zero speed |
maetugr | 0:37f0c1e8fa66 | 190 | ESC[i] = 0; |
maetugr | 1:60882db03b0f | 191 | debug = true; |
maetugr | 0:37f0c1e8fa66 | 192 | } |
maetugr | 0:37f0c1e8fa66 | 193 | |
maetugr | 0:37f0c1e8fa66 | 194 | if (debug) { |
maetugr | 3:3709be130495 | 195 | pc.printf("$STATE,%d,%d,%.3f,%.3f\r\n", armed, level, IMU.dt*1e3, IMU.dt_sensors*1e6); |
maetugr | 1:60882db03b0f | 196 | //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 | 3:3709be130495 | 197 | //pc.printf("$GYRO,%.3f,%.3f,%.3f\r\n", IMU.mpu.Gyro[ROLL], IMU.mpu.Gyro[PITCH], IMU.mpu.Gyro[YAW]); |
maetugr | 1:60882db03b0f | 198 | //pc.printf("$ACC,%.3f,%.3f,%.3f\r\n", IMU.mpu.Acc[ROLL], IMU.mpu.Acc[PITCH], IMU.mpu.Acc[YAW]); |
maetugr | 3:3709be130495 | 199 | pc.printf("$ANG,%.3f,%.3f,%.3f\r\n", IMU.angle[ROLL], IMU.angle[PITCH], IMU.angle[YAW]); |
maetugr | 4:b2efa7f03701 | 200 | //pc.printf("$RCANG,%.3f,%.3f,%.3f\r\n", RC_angle[ROLL], RC_angle[PITCH], RC_angle[YAW]); |
maetugr | 3:3709be130495 | 201 | pc.printf("$CONTR,%.3f,%.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, PY); |
maetugr | 3:3709be130495 | 202 | pc.printf("$CONTA,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\r\n", Controller_Angle[ROLL].Value, Controller_Angle[PITCH].Value, Controller_Angle[YAW].Value, P_A, I_A, D_A); |
maetugr | 2:f0a9ecb4d049 | 203 | 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 | 4:b2efa7f03701 | 204 | //pc.printf("\r\n"); |
maetugr | 4:b2efa7f03701 | 205 | wait(0.04); |
maetugr | 0:37f0c1e8fa66 | 206 | } |
maetugr | 0:37f0c1e8fa66 | 207 | |
maetugr | 0:37f0c1e8fa66 | 208 | LEDs.rollnext(); |
maetugr | 0:37f0c1e8fa66 | 209 | } |
maetugr | 0:37f0c1e8fa66 | 210 | } |