Matthias Grob / Mbed 2 deprecated FlyBed2

My fully self designed first stable working Quadrocopter Software.

Dependencies:   mbed

Dependents:   fluy343

/media/uploads/maetugr/dsc09031.jpg

main.cpp@4:b0a60b0b24a9, 2014-02-15 (annotated)

Committer:
maetugr
Date:
Sat Feb 15 14:28:11 2014 +0000
Revision:
4:b0a60b0b24a9
Parent:
3:e277653258ab
Child:
5:06e978fd147a
After setting MPU6050 up

Who changed what in which revision?

UserRevisionLine numberNew contents of line
maetugr 0:12950aa67f2a 1 #include "mbed.h"
maetugr 0:12950aa67f2a 2 #include "LED.h" // LEDs framework for blinking ;)
maetugr 0:12950aa67f2a 3 #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:12950aa67f2a 4
maetugr 0:12950aa67f2a 5 #include "IMU_10DOF.h" // Complete IMU class for 10DOF-Board (L3G4200D, ADXL345, HMC5883, BMP085)
maetugr 0:12950aa67f2a 6 #include "RC_Channel.h" // RemoteControl Channels with PPM
maetugr 0:12950aa67f2a 7 #include "PID.h" // PID Library (slim, self written)
maetugr 0:12950aa67f2a 8 #include "Servo_PWM.h" // Motor PPM using PwmOut
maetugr 0:12950aa67f2a 9
maetugr 0:12950aa67f2a 10 #define PPM_FREQU 495 // Hz Frequency of PPM Signal for ESCs (maximum <500Hz)
maetugr 0:12950aa67f2a 11 #define INTEGRAL_MAX 300 // maximal output offset that can result from integrating errors
maetugr 2:03e5f7ab473f 12 #define RC_SENSITIVITY 30 // maximal angle from horizontal that the PID is aming for
maetugr 2:03e5f7ab473f 13 #define YAWSPEED 0.2 // maximal speed of yaw rotation in degree per Rate
maetugr 0:12950aa67f2a 14 #define AILERON 0 // RC
maetugr 0:12950aa67f2a 15 #define ELEVATOR 1
maetugr 0:12950aa67f2a 16 #define RUDDER 2
maetugr 0:12950aa67f2a 17 #define THROTTLE 3
maetugr 1:5e2b81f2d0b4 18 #define CHANNEL8 4
maetugr 1:5e2b81f2d0b4 19 #define CHANNEL7 5
maetugr 1:5e2b81f2d0b4 20 #define CHANNEL6 6
maetugr 0:12950aa67f2a 21 #define ROLL 0 // Axes
maetugr 0:12950aa67f2a 22 #define PITCH 1
maetugr 0:12950aa67f2a 23 #define YAW 2
maetugr 0:12950aa67f2a 24
maetugr 0:12950aa67f2a 25 bool armed = false; // this variable is for security (when false no motor rotates any more)
maetugr 2:03e5f7ab473f 26 bool RC_present = false; // this variable shows if an RC is present
maetugr 3:e277653258ab 27 float P = 13.16, I = 8, D = 2.73; // PID values
maetugr 2:03e5f7ab473f 28 float PY = 5.37, IY = 0, DY = 3; // PID values for Yaw
maetugr 2:03e5f7ab473f 29 float RC_angle[] = {0,0,0}; // Angle of the RC Sticks, to steer the QC
maetugr 0:12950aa67f2a 30 float Motor_speed[4] = {0,0,0,0}; // Mixed Motorspeeds, ready to send
maetugr 0:12950aa67f2a 31
maetugr 0:12950aa67f2a 32 LED LEDs;
maetugr 0:12950aa67f2a 33 PC pc(USBTX, USBRX, 921600); // USB
maetugr 1:5e2b81f2d0b4 34 //PC pc(p9, p10, 115200); // Bluetooth
maetugr 0:12950aa67f2a 35 IMU_10DOF IMU(p28, p27);
maetugr 1:5e2b81f2d0b4 36 RC_Channel RC[] = {RC_Channel(p8,1), RC_Channel(p7,2), RC_Channel(p5,4), RC_Channel(p6,3), RC_Channel(p15,2), RC_Channel(p16,4), RC_Channel(p17,3)}; // no p19/p20 !
maetugr 2:03e5f7ab473f 37 PID Controller[] = {PID(P, I, D, INTEGRAL_MAX), PID(P, I, D, INTEGRAL_MAX), PID(PY, IY, DY, INTEGRAL_MAX)}; // 0:X:Roll 1:Y:Pitch 2:Z:Yaw
maetugr 0:12950aa67f2a 38 Servo_PWM ESC[] = {Servo_PWM(p21,PPM_FREQU), Servo_PWM(p22,PPM_FREQU), Servo_PWM(p23,PPM_FREQU), Servo_PWM(p24,PPM_FREQU)}; // p21 - p26 only because PWM needed!
maetugr 0:12950aa67f2a 39
maetugr 1:5e2b81f2d0b4 40 extern "C" void mbed_reset();
maetugr 1:5e2b81f2d0b4 41
maetugr 0:12950aa67f2a 42 void executer() {
maetugr 1:5e2b81f2d0b4 43 char command = pc.getc();
maetugr 1:5e2b81f2d0b4 44 if (command == 'X')
maetugr 1:5e2b81f2d0b4 45 mbed_reset();
maetugr 2:03e5f7ab473f 46 if (command == 'A') {
maetugr 2:03e5f7ab473f 47 IMU.Acc.calibrate(100,0.05);
maetugr 2:03e5f7ab473f 48 pc.printf("\r\n***A***%.3f,%.3f,%.3f***\r\n", IMU.Acc.offset[ROLL], IMU.Acc.offset[PITCH], IMU.Acc.offset[YAW]);
maetugr 2:03e5f7ab473f 49 wait(10);
maetugr 2:03e5f7ab473f 50 }
maetugr 2:03e5f7ab473f 51 if (command == 'C') {
maetugr 2:03e5f7ab473f 52 IMU.Comp.calibrate(60);
maetugr 2:03e5f7ab473f 53 pc.printf("\r\n***C***%.3f,%.3f,%.3f***\r\n", IMU.Comp.offset[ROLL], IMU.Comp.offset[PITCH], IMU.Comp.offset[YAW]);
maetugr 2:03e5f7ab473f 54 wait(20);
maetugr 2:03e5f7ab473f 55 }
maetugr 2:03e5f7ab473f 56
maetugr 2:03e5f7ab473f 57 pc.putc(command);
maetugr 0:12950aa67f2a 58 LEDs.tilt(2);
maetugr 0:12950aa67f2a 59 }
maetugr 0:12950aa67f2a 60
maetugr 0:12950aa67f2a 61 int main() {
maetugr 0:12950aa67f2a 62 pc.attach(&executer);
maetugr 0:12950aa67f2a 63 while(1) {
maetugr 0:12950aa67f2a 64 // IMU
maetugr 0:12950aa67f2a 65 IMU.readAngles();
maetugr 2:03e5f7ab473f 66 //IMU.readAltitude(); // TODO: reading altitude takes much more time than the angles -> don't do this in your fast loop, Ticker?
maetugr 0:12950aa67f2a 67 //pc.printf("%.1f,%.1f,%.1f,%.1f'C,%.1fhPa,%.1fmaS,%.5fs,%.5fs\r\n", IMU.angle[0], IMU.angle[1], IMU.angle[2], IMU.temperature, IMU.pressure, IMU.altitude, IMU.dt, IMU.dt_sensors); // Output for Python
maetugr 0:12950aa67f2a 68
maetugr 0:12950aa67f2a 69 // Arming / disarming
maetugr 2:03e5f7ab473f 70 RC_present = !(RC[AILERON].read() == -100 || RC[ELEVATOR].read() == -100 || RC[RUDDER].read() == -100 || RC[THROTTLE].read() == -100); // TODO: Failsafe
maetugr 1:5e2b81f2d0b4 71 if(RC[THROTTLE].read() < 20 && RC[RUDDER].read() > 850) {
maetugr 1:5e2b81f2d0b4 72 armed = true;
maetugr 2:03e5f7ab473f 73 RC_angle[YAW] = IMU.angle[YAW];
maetugr 1:5e2b81f2d0b4 74 }
maetugr 2:03e5f7ab473f 75 if((RC[THROTTLE].read() < 30 && RC[RUDDER].read() < 30) || !RC_present) {
maetugr 0:12950aa67f2a 76 armed = false;
maetugr 0:12950aa67f2a 77 }
maetugr 0:12950aa67f2a 78
maetugr 2:03e5f7ab473f 79 // Setting PID Values from auxiliary RC channels
maetugr 1:5e2b81f2d0b4 80 if (RC[CHANNEL8].read() > 0 && RC[CHANNEL8].read() < 1000)
maetugr 3:e277653258ab 81 P = 13 + (((float)RC[CHANNEL8].read()) * 7 / 1000);
maetugr 1:5e2b81f2d0b4 82 if (RC[CHANNEL7].read() > 0 && RC[CHANNEL7].read() < 1000)
maetugr 3:e277653258ab 83 D = 2 + (((float)RC[CHANNEL7].read()) * 4 / 1000);
maetugr 2:03e5f7ab473f 84 for(int i=0;i<2;i++)
maetugr 2:03e5f7ab473f 85 Controller[i].setPID(P,I,D); // give the new PID values to roll and pitch controller
maetugr 2:03e5f7ab473f 86 Controller[YAW].setPID(PY,IY,DY);
maetugr 1:5e2b81f2d0b4 87
maetugr 2:03e5f7ab473f 88 // RC Angle ROLL-PITCH-Part
maetugr 2:03e5f7ab473f 89 for(int i=0;i<2;i++) { // calculate new angle we want the QC to have
maetugr 2:03e5f7ab473f 90 if (RC_present)
maetugr 2:03e5f7ab473f 91 RC_angle[i] = (RC[i].read()-500)*RC_SENSITIVITY/500.0;
maetugr 2:03e5f7ab473f 92 else
maetugr 2:03e5f7ab473f 93 RC_angle[i] = 0;
maetugr 2:03e5f7ab473f 94 }
maetugr 2:03e5f7ab473f 95
maetugr 2:03e5f7ab473f 96 // RC Angle YAW-Part
maetugr 2:03e5f7ab473f 97 float RC_yaw_adding; // temporary variable to take the desired yaw adjustment
maetugr 2:03e5f7ab473f 98 if (RC_present && RC[THROTTLE].read() > 20)
maetugr 2:03e5f7ab473f 99 RC_yaw_adding = -(RC[RUDDER].read()-500)*YAWSPEED/500;
maetugr 1:5e2b81f2d0b4 100 else
maetugr 2:03e5f7ab473f 101 RC_yaw_adding = 0;
maetugr 2:03e5f7ab473f 102
maetugr 2:03e5f7ab473f 103 while(RC_angle[YAW] + RC_yaw_adding < -180 || RC_angle[YAW] + RC_yaw_adding > 180) { // make shure it's in the cycle -180 to 180
maetugr 2:03e5f7ab473f 104 if(RC_angle[YAW] + RC_yaw_adding < -180)
maetugr 2:03e5f7ab473f 105 RC_yaw_adding += 360;
maetugr 2:03e5f7ab473f 106 if(RC_angle[YAW] + RC_yaw_adding > 180)
maetugr 2:03e5f7ab473f 107 RC_yaw_adding -= 360;
maetugr 2:03e5f7ab473f 108 }
maetugr 2:03e5f7ab473f 109 RC_angle[YAW] += RC_yaw_adding; // the yaw angle is integrated from stick input
maetugr 1:5e2b81f2d0b4 110
maetugr 0:12950aa67f2a 111 // Controlling
maetugr 2:03e5f7ab473f 112 for(int i=0;i<2;i++) {
maetugr 2:03e5f7ab473f 113 Controller[i].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying
maetugr 4:b0a60b0b24a9 114 Controller[i].compute(RC_angle[i], IMU.angle[i], IMU.Sensor.data_gyro[i]); // give the controller the actual gyro values for D and angle for P,I and get his advice to correct
maetugr 2:03e5f7ab473f 115 }
maetugr 2:03e5f7ab473f 116 Controller[YAW].setIntegrate(armed); // same for YAW
maetugr 2:03e5f7ab473f 117 if (abs(RC_angle[YAW] - IMU.angle[YAW]) > 180) // for YAW a special calculation because of range -180 to 180
maetugr 2:03e5f7ab473f 118 if (RC_angle[YAW] > IMU.angle[YAW])
maetugr 4:b0a60b0b24a9 119 Controller[YAW].compute(RC_angle[YAW] - 360, IMU.angle[YAW], IMU.Sensor.data_gyro[YAW]);
maetugr 2:03e5f7ab473f 120 else
maetugr 4:b0a60b0b24a9 121 Controller[YAW].compute(RC_angle[YAW] + 360, IMU.angle[YAW], IMU.Sensor.data_gyro[YAW]);
maetugr 2:03e5f7ab473f 122 else
maetugr 4:b0a60b0b24a9 123 Controller[YAW].compute(RC_angle[YAW], IMU.angle[YAW], IMU.Sensor.data_gyro[YAW]);
maetugr 0:12950aa67f2a 124
maetugr 0:12950aa67f2a 125 // Mixing
maetugr 0:12950aa67f2a 126 if (armed) // for SECURITY!
maetugr 0:12950aa67f2a 127 {
maetugr 2:03e5f7ab473f 128 Motor_speed[0] = RC[THROTTLE].read() + Controller[PITCH].Value;
maetugr 2:03e5f7ab473f 129 Motor_speed[2] = RC[THROTTLE].read() - Controller[PITCH].Value;
maetugr 2:03e5f7ab473f 130 Motor_speed[3] = RC[THROTTLE].read() + Controller[ROLL].Value;
maetugr 2:03e5f7ab473f 131 Motor_speed[1] = RC[THROTTLE].read() - Controller[ROLL].Value;
maetugr 2:03e5f7ab473f 132
maetugr 2:03e5f7ab473f 133 Motor_speed[0] -= Controller[YAW].Value;
maetugr 2:03e5f7ab473f 134 Motor_speed[2] -= Controller[YAW].Value;
maetugr 2:03e5f7ab473f 135 Motor_speed[3] += Controller[YAW].Value;
maetugr 2:03e5f7ab473f 136 Motor_speed[1] += Controller[YAW].Value;
maetugr 2:03e5f7ab473f 137
maetugr 2:03e5f7ab473f 138
maetugr 0:12950aa67f2a 139 for(int i=0;i<4;i++) // Set new motorspeeds
maetugr 0:12950aa67f2a 140 ESC[i] = (int)Motor_speed[i];
maetugr 0:12950aa67f2a 141
maetugr 0:12950aa67f2a 142 } else {
maetugr 0:12950aa67f2a 143 for(int i=0;i<4;i++) // for security reason, set every motor to zero speed
maetugr 0:12950aa67f2a 144 ESC[i] = 0;
maetugr 0:12950aa67f2a 145 }
maetugr 0:12950aa67f2a 146
maetugr 4:b0a60b0b24a9 147 //pc.printf("%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\r\n", IMU.Acc.data[0], IMU.Acc.data[1], IMU.Acc.data[2], D, IMU.angle[PITCH], Controller[PITCH].Value, RC_angle[YAW], IMU.dt);
maetugr 4:b0a60b0b24a9 148 pc.printf("%d,%.1f,%.1f,%.1f,%.3f,%.3f,%.3f,%.2f,%.2f\r\n", armed, IMU.angle[ROLL], IMU.angle[PITCH], IMU.angle[YAW], Controller[ROLL].Value, Controller[PITCH].Value, Controller[YAW].Value, P, D); // RC[0].read(), RC[1].read(), RC[2].read(), RC[3].read()
maetugr 3:e277653258ab 149 //pc.printf("%d,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\r\n", armed, P, PY, D, IMU.angle[PITCH], Controller[PITCH].Value, RC_angle[YAW], IMU.dt);
maetugr 3:e277653258ab 150 //pc.printf("%d,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\r\n", armed, P, PY, D, IMU.angle[PITCH], Controller[PITCH].Value, RC_angle[YAW], IMU.dt);
maetugr 0:12950aa67f2a 151 //pc.printf("%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.5f\r\n", IMU.angle[0], IMU.angle[1], IMU.angle[2], IMU.Gyro.data[0], IMU.Gyro.data[1], IMU.Gyro.data[2], IMU.dt);
maetugr 0:12950aa67f2a 152
maetugr 0:12950aa67f2a 153 //wait(0.01);
maetugr 0:12950aa67f2a 154
maetugr 0:12950aa67f2a 155 LEDs.rollnext();
maetugr 0:12950aa67f2a 156 }
maetugr 0:12950aa67f2a 157 }