Matthias Grob / Mbed 2 deprecated FlyBed3

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)

Dependencies:   mbed

main.cpp@7:90f876d47862, 2015-09-14 (annotated)

Committer:
maetugr
Date:
Mon Sep 14 12:49:08 2015 +0000
Revision:
7:90f876d47862
Parent:
6:f258093beed9
Child:
8:609a2ad4c30e
Changed a lot of Timing related things; controll frequency is now regulated to 495Hz becuase this is the output frequency anyway

Who changed what in which revision?

UserRevisionLine numberNew 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 6:f258093beed9 37 float P_R = 2.5, I_R = 0.3, D_R = 0; // PID values for the rate controller
maetugr 6:f258093beed9 38 float P_A = 2.1, I_A = 0.3, 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 7:90f876d47862 43 Timer LoopTimer;
maetugr 7:90f876d47862 44 float Times[10] = {0,0,0,0,0,0,0,0,0,0};
maetugr 7:90f876d47862 45 float control_frequency = PPM_FREQU; // frequency for the main loop in Hz
maetugr 7:90f876d47862 46
maetugr 0:37f0c1e8fa66 47 LED LEDs;
maetugr 7:90f876d47862 48 PC pc(USBTX, USBRX, 115200); // USB
maetugr 7:90f876d47862 49 //PC pc(p9, p10, 115200); // Bluetooth PIN: 1234
maetugr 0:37f0c1e8fa66 50 IMU_10DOF IMU(p5, p6, p7, p19);
maetugr 0:37f0c1e8fa66 51 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 52 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 53 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 54 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 55
maetugr 0:37f0c1e8fa66 56 extern "C" void mbed_reset();
maetugr 0:37f0c1e8fa66 57
maetugr 7:90f876d47862 58 void loop() {
maetugr 7:90f876d47862 59 LoopTimer.start();
maetugr 7:90f876d47862 60
maetugr 7:90f876d47862 61 // IMU
maetugr 7:90f876d47862 62 IMU.readAngles();
maetugr 7:90f876d47862 63 Times[1] = LoopTimer.read(); // 197us
maetugr 7:90f876d47862 64
maetugr 7:90f876d47862 65 // Arming / disarming
maetugr 7:90f876d47862 66 RC_present = !(RC[AILERON].read() == -100 || RC[ELEVATOR].read() == -100 || RC[RUDDER].read() == -100 || RC[THROTTLE].read() == -100); // TODO: Failsafe
maetugr 7:90f876d47862 67 if(RC[THROTTLE].read() < 20 && RC[RUDDER].read() > 850) {
maetugr 7:90f876d47862 68 armed = true;
maetugr 7:90f876d47862 69 RC_angle[YAW] = IMU.angle[YAW];
maetugr 7:90f876d47862 70 }
maetugr 7:90f876d47862 71 if((RC[THROTTLE].read() < 30 && RC[RUDDER].read() < 30) || !RC_present) {
maetugr 7:90f876d47862 72 armed = false;
maetugr 7:90f876d47862 73 }
maetugr 7:90f876d47862 74
maetugr 7:90f876d47862 75 // Setting PID Values from auxiliary RC channels
maetugr 7:90f876d47862 76 for(int i=0;i<3;i++)
maetugr 7:90f876d47862 77 Controller_Angle[i].setPID(P_A,I_A,D_A);
maetugr 7:90f876d47862 78 for(int i=0;i<2;i++)
maetugr 7:90f876d47862 79 Controller_Rate[i].setPID(P_R,I_R,D_R); // give the new PID values to roll and pitch controller
maetugr 7:90f876d47862 80 Controller_Rate[YAW].setPID(PY,IY,DY);
maetugr 7:90f876d47862 81 Times[2] = LoopTimer.read(); // 7us
maetugr 7:90f876d47862 82
maetugr 7:90f876d47862 83 // RC Angle ROLL-PITCH-Part
maetugr 7:90f876d47862 84 for(int i=0;i<2;i++) { // calculate new angle we want the QC to have
maetugr 7:90f876d47862 85 if (RC_present)
maetugr 7:90f876d47862 86 RC_angle[i] = (RC[i].read()-500)*RC_SENSITIVITY/500.0;
maetugr 7:90f876d47862 87 else
maetugr 7:90f876d47862 88 RC_angle[i] = 0;
maetugr 7:90f876d47862 89 }
maetugr 7:90f876d47862 90
maetugr 7:90f876d47862 91 // RC Angle YAW-Part
maetugr 7:90f876d47862 92 float RC_yaw_adding; // temporary variable to take the desired yaw adjustment
maetugr 7:90f876d47862 93 if (RC_present && RC[THROTTLE].read() > 20)
maetugr 7:90f876d47862 94 RC_yaw_adding = -(RC[RUDDER].read()-500)*YAWSPEED/500; // the yaw angle is integrated from stick input
maetugr 7:90f876d47862 95 else
maetugr 7:90f876d47862 96 RC_yaw_adding = 0;
maetugr 7:90f876d47862 97
maetugr 7:90f876d47862 98 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 7:90f876d47862 99 RC_angle[YAW] = RC_angle[YAW] + RC_yaw_adding > 180 ? RC_angle[YAW] - 360 + RC_yaw_adding : RC_angle[YAW] + RC_yaw_adding;
maetugr 7:90f876d47862 100 Times[3] = LoopTimer.read(); // 6us
maetugr 7:90f876d47862 101
maetugr 7:90f876d47862 102 // Controlling
maetugr 7:90f876d47862 103 if (level) {
maetugr 7:90f876d47862 104 for(int i=0;i<2;i++) { // LEVEL
maetugr 7:90f876d47862 105 Controller_Angle[i].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying
maetugr 7:90f876d47862 106 Controller_Angle[i].compute(RC_angle[i], IMU.angle[i]); // give the controller the actual gyro values and get his advice to correct
maetugr 7:90f876d47862 107 Controller_Rate[i].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying
maetugr 7:90f876d47862 108 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 7:90f876d47862 109 }
maetugr 7:90f876d47862 110 } else {
maetugr 7:90f876d47862 111 for(int i=0;i<2;i++) { // ACRO
maetugr 7:90f876d47862 112 Controller_Rate[i].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying
maetugr 7:90f876d47862 113 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 7:90f876d47862 114 }
maetugr 7:90f876d47862 115 }
maetugr 7:90f876d47862 116
maetugr 7:90f876d47862 117 Controller_Rate[2].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying
maetugr 7:90f876d47862 118 if (RC[THROTTLE].read() > 20)
maetugr 7:90f876d47862 119 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 7:90f876d47862 120 else
maetugr 7:90f876d47862 121 Controller_Rate[2].compute(0, IMU.mpu.Gyro[2]); // give the controller the actual gyro values and get his advice to correct
maetugr 7:90f876d47862 122
maetugr 7:90f876d47862 123 float throttle = 100 + (RC[THROTTLE].read() * 500 / 1000);
maetugr 7:90f876d47862 124 Times[4] = LoopTimer.read(); // 53us
maetugr 7:90f876d47862 125
maetugr 7:90f876d47862 126 // Mixing
maetugr 7:90f876d47862 127 Motor_speed[0] = throttle +SQRT2*Controller_Rate[ROLL].Value -SQRT2*Controller_Rate[PITCH].Value; // X Configuration
maetugr 7:90f876d47862 128 Motor_speed[1] = throttle -SQRT2*Controller_Rate[ROLL].Value -SQRT2*Controller_Rate[PITCH].Value; //
maetugr 7:90f876d47862 129 Motor_speed[2] = throttle -SQRT2*Controller_Rate[ROLL].Value +SQRT2*Controller_Rate[PITCH].Value; //
maetugr 7:90f876d47862 130 Motor_speed[3] = throttle +SQRT2*Controller_Rate[ROLL].Value +SQRT2*Controller_Rate[PITCH].Value; //
maetugr 7:90f876d47862 131
maetugr 7:90f876d47862 132 Motor_speed[0] -= Controller_Rate[YAW].Value;
maetugr 7:90f876d47862 133 Motor_speed[2] -= Controller_Rate[YAW].Value;
maetugr 7:90f876d47862 134 Motor_speed[3] += Controller_Rate[YAW].Value;
maetugr 7:90f876d47862 135 Motor_speed[1] += Controller_Rate[YAW].Value;
maetugr 7:90f876d47862 136 Times[5] = LoopTimer.read(); // 17us
maetugr 7:90f876d47862 137
maetugr 7:90f876d47862 138 if (armed) // for SECURITY!
maetugr 7:90f876d47862 139 {
maetugr 7:90f876d47862 140 debug = false;
maetugr 7:90f876d47862 141 // PITCH
maetugr 7:90f876d47862 142 ESC[0] = (int)Motor_speed[0]>50 ? (int)Motor_speed[0] : 50;
maetugr 7:90f876d47862 143 ESC[2] = (int)Motor_speed[2]>50 ? (int)Motor_speed[2] : 50;
maetugr 7:90f876d47862 144 // ROLL
maetugr 7:90f876d47862 145 //ESC[1] = (int)Motor_speed[1]>50 ? (int)Motor_speed[1] : 50;
maetugr 7:90f876d47862 146 //ESC[3] = (int)Motor_speed[3]>50 ? (int)Motor_speed[3] : 50;
maetugr 7:90f876d47862 147 /*for(int i=0;i<4;i++) // Set new motorspeeds
maetugr 7:90f876d47862 148 ESC[i] = (int)Motor_speed[i]>100 ? (int)Motor_speed[i] : 100;*/
maetugr 7:90f876d47862 149
maetugr 7:90f876d47862 150 } else {
maetugr 7:90f876d47862 151 for(int i=0;i<4;i++) // for security reason, set every motor to zero speed
maetugr 7:90f876d47862 152 ESC[i] = 0;
maetugr 7:90f876d47862 153 debug = true;
maetugr 7:90f876d47862 154 }
maetugr 7:90f876d47862 155 Times[6] = LoopTimer.read(); // 6us
maetugr 7:90f876d47862 156
maetugr 7:90f876d47862 157 LEDs.rollnext();
maetugr 7:90f876d47862 158
maetugr 7:90f876d47862 159 Times[7] = LoopTimer.read(); // 7us TOTAL 297us
maetugr 7:90f876d47862 160
maetugr 7:90f876d47862 161 while(LoopTimer.read() < 1/control_frequency);
maetugr 7:90f876d47862 162 Times[8] = LoopTimer.read();
maetugr 7:90f876d47862 163 LoopTimer.stop();
maetugr 7:90f876d47862 164 LoopTimer.reset();
maetugr 7:90f876d47862 165
maetugr 7:90f876d47862 166
maetugr 7:90f876d47862 167 if (debug) {
maetugr 7:90f876d47862 168 pc.printf("$STATE,%d,%d,%.f,%.3f,%.3f\r\n", armed, level, control_frequency, IMU.dt*1e3, IMU.dt_sensors*1e6);
maetugr 7:90f876d47862 169 //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 7:90f876d47862 170 //pc.printf("$GYRO,%.3f,%.3f,%.3f\r\n", IMU.mpu.Gyro[ROLL], IMU.mpu.Gyro[PITCH], IMU.mpu.Gyro[YAW]);
maetugr 7:90f876d47862 171 //pc.printf("$ACC,%.3f,%.3f,%.3f\r\n", IMU.mpu.Acc[ROLL], IMU.mpu.Acc[PITCH], IMU.mpu.Acc[YAW]);
maetugr 7:90f876d47862 172 pc.printf("$ANG,%.3f,%.3f,%.3f\r\n", IMU.angle[ROLL], IMU.angle[PITCH], IMU.angle[YAW]);
maetugr 7:90f876d47862 173 //pc.printf("$RCANG,%.3f,%.3f,%.3f\r\n", RC_angle[ROLL], RC_angle[PITCH], RC_angle[YAW]);
maetugr 7:90f876d47862 174 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 7:90f876d47862 175 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 7:90f876d47862 176 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 7:90f876d47862 177 pc.printf("$TIMES");
maetugr 7:90f876d47862 178 for(int i = 1; i < 10; i++)
maetugr 7:90f876d47862 179 pc.printf(",%.3f", (Times[i]-Times[i-1])*1e6);
maetugr 7:90f876d47862 180 pc.printf("\r\n");
maetugr 7:90f876d47862 181 wait(0.1);
maetugr 7:90f876d47862 182 }
maetugr 7:90f876d47862 183 }
maetugr 7:90f876d47862 184
maetugr 0:37f0c1e8fa66 185 void executer() {
maetugr 0:37f0c1e8fa66 186 char command = pc.getc();
maetugr 0:37f0c1e8fa66 187 if (command == 'X')
maetugr 0:37f0c1e8fa66 188 mbed_reset();
maetugr 0:37f0c1e8fa66 189 if (command == '-')
maetugr 0:37f0c1e8fa66 190 debug = !debug;
maetugr 2:f0a9ecb4d049 191
maetugr 3:3709be130495 192 if (command == ':')
maetugr 3:3709be130495 193 armed = true;
maetugr 3:3709be130495 194 if (command == ' ')
maetugr 3:3709be130495 195 armed = false;
maetugr 3:3709be130495 196
maetugr 3:3709be130495 197 if (command == 'q')
maetugr 3:3709be130495 198 level = true;
maetugr 3:3709be130495 199 if (command == 'a')
maetugr 3:3709be130495 200 level = false;
maetugr 3:3709be130495 201
maetugr 1:60882db03b0f 202 if (command == 'w')
maetugr 1:60882db03b0f 203 P_R += 0.1;
maetugr 1:60882db03b0f 204 if (command == 's')
maetugr 1:60882db03b0f 205 P_R -= 0.1;
maetugr 0:37f0c1e8fa66 206
maetugr 3:3709be130495 207 if (command == 'e')
maetugr 5:8ea99e98de73 208 I_R += 0.1;
maetugr 3:3709be130495 209 if (command == 'd')
maetugr 5:8ea99e98de73 210 I_R -= 0.1;
maetugr 3:3709be130495 211
maetugr 3:3709be130495 212 if (command == 'r')
maetugr 5:8ea99e98de73 213 P_A += 0.1;
maetugr 3:3709be130495 214 if (command == 'f')
maetugr 5:8ea99e98de73 215 P_A -= 0.1;
maetugr 2:f0a9ecb4d049 216
maetugr 4:b2efa7f03701 217 if (command == 't')
maetugr 4:b2efa7f03701 218 I_A += 0.1;
maetugr 4:b2efa7f03701 219 if (command == 'g')
maetugr 4:b2efa7f03701 220 I_A -= 0.1;
maetugr 4:b2efa7f03701 221
maetugr 5:8ea99e98de73 222 if (command == 'z')
maetugr 5:8ea99e98de73 223 PY += 0.1;
maetugr 5:8ea99e98de73 224 if (command == 'h')
maetugr 5:8ea99e98de73 225 PY -= 0.1;
maetugr 5:8ea99e98de73 226
maetugr 7:90f876d47862 227 /*if (command == 'o') {
maetugr 7:90f876d47862 228 control_frequency += 100;
maetugr 7:90f876d47862 229 }
maetugr 7:90f876d47862 230 if (command == 'l') {
maetugr 7:90f876d47862 231 control_frequency -= 100;
maetugr 7:90f876d47862 232 }*/
maetugr 7:90f876d47862 233
maetugr 0:37f0c1e8fa66 234 pc.putc(command);
maetugr 0:37f0c1e8fa66 235 LEDs.tilt(2);
maetugr 0:37f0c1e8fa66 236 }
maetugr 0:37f0c1e8fa66 237
maetugr 0:37f0c1e8fa66 238 int main() {
maetugr 0:37f0c1e8fa66 239 pc.attach(&executer);
maetugr 0:37f0c1e8fa66 240 while(1) {
maetugr 7:90f876d47862 241 loop();
maetugr 0:37f0c1e8fa66 242 }
maetugr 0:37f0c1e8fa66 243 }