main.cpp

00001 /*   X- Configuration
00002         m2   m3                      --           >
00003           \ /                      /    \       /
00004           / \                            V     |
00005         m1   m0                                 \
00006                                     ROLL       PITCH */
00007 #include "mbed.h"
00008 #include "LED.h"        // LEDs framework for blinking ;)
00009 #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)
00010 
00011 #include "IMU_10DOF.h"  // Complete IMU class for 10DOF-Board (L3G4200D, ADXL345, HMC5883, BMP085)
00012 #include "RC_Channel.h" // RemoteControl Channels with PPM
00013 #include "PID.h"        // PID Library (slim, self written)
00014 #include "Servo.h"      // Motor PPM using any DigitalOut Pin
00015 
00016 #define PPM_FREQU       495     // Hz Frequency of PPM Signal for ESCs (maximum <500Hz)
00017 #define INTEGRAL_MAX    300     // maximal output offset that can result from integrating errors
00018 #define RC_SENSITIVITY  30      // maximal angle from horizontal that the PID is aming for
00019 #define YAWSPEED        1.0     // maximal speed of yaw rotation in degree per Rate
00020 #define AILERON         0       // RC
00021 #define ELEVATOR        1
00022 #define RUDDER          2
00023 #define THROTTLE        3
00024 #define CHANNEL8        4
00025 #define CHANNEL7        5
00026 #define CHANNEL6        6
00027 #define ROLL            0       // Axes
00028 #define PITCH           1
00029 #define YAW             2
00030 
00031 #define SQRT2           0.7071067811865
00032 
00033 bool  armed = false;                    // is for security (when false no motor rotates any more)
00034 bool  debug = true;                     // shows if we want output for the computer
00035 bool  level = false;                     // switches between self leveling and acro mode
00036 bool  RC_present = false;               // shows if an RC is present
00037 float P_R = 2.6, I_R = 0.3, D_R = 0;      // PID values for the rate controller
00038 float P_A = 1.9, I_A = 0.2, D_A = 0;        // PID values for the angle controller      P_A = 1.865, I_A = 1.765, D_A = 0
00039 float PY = 2.3, IY = 0, DY = 0;         // PID values for Yaw
00040 float RC_angle[] = {0,0,0};             // Angle of the RC Sticks, to steer the QC
00041 float Motor_speed[4] = {0,0,0,0};       // Mixed Motorspeeds, ready to send
00042 
00043 Timer LoopTimer;
00044 float Times[10] = {0,0,0,0,0,0,0,0,0,0};
00045 float control_frequency = 800;//PPM_FREQU;         // frequency for the main loop in Hz
00046 int counter = 0;
00047 int divider = 20;
00048 
00049 LED         LEDs;
00050 //PC          pc(USBTX, USBRX, 115200);   // USB
00051 PC          pc(p9, p10, 115200);       // Bluetooth PIN: 1234
00052 IMU_10DOF   IMU(p5, p6, p7, p19, p28, p27);
00053 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 !
00054 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
00055 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)};
00056 Servo       ESC[] = {Servo(p21,PPM_FREQU), Servo(p22,PPM_FREQU), Servo(p23,PPM_FREQU), Servo(p24,PPM_FREQU)};   // use any DigitalOit Pin
00057 
00058 extern "C" void mbed_reset();
00059 
00060 void loop() {
00061     LoopTimer.start();
00062     
00063     // IMU
00064     IMU.readAngles();
00065     Times[1] = LoopTimer.read(); // 197us
00066     
00067     // Arming / disarming
00068     RC_present = !(RC[AILERON].read() == -100 || RC[ELEVATOR].read() == -100 || RC[RUDDER].read() == -100 || RC[THROTTLE].read() == -100); // TODO: Failsafe
00069     if(RC[THROTTLE].read() < 20 && RC[RUDDER].read() > 850) {
00070         armed = true;
00071         RC_angle[YAW] = IMU.angle[YAW];
00072     }
00073     if((RC[THROTTLE].read() < 30 && RC[RUDDER].read() < 30) || !RC_present) {
00074         armed = false;
00075     }
00076     
00077     // Setting PID Values from auxiliary RC channels
00078     for(int i=0;i<3;i++)
00079         Controller_Angle[i].setPID(P_A,I_A,D_A);
00080     for(int i=0;i<2;i++)
00081         Controller_Rate[i].setPID(P_R,I_R,D_R); // give the new PID values to roll and pitch controller
00082     Controller_Rate[YAW].setPID(PY,IY,DY);
00083     Times[2] = LoopTimer.read(); // 7us
00084     
00085     // RC Angle ROLL-PITCH-Part
00086     for(int i=0;i<2;i++) {    // calculate new angle we want the QC to have
00087         if (RC_present)
00088             RC_angle[i] = (RC[i].read()-500)*RC_SENSITIVITY/500.0;
00089         else
00090             RC_angle[i] = 0;
00091     }
00092 
00093     // RC Angle YAW-Part
00094     float   RC_yaw_adding;                  // temporary variable to take the desired yaw adjustment
00095     if (RC_present && RC[THROTTLE].read() > 20)
00096         RC_yaw_adding = -(RC[RUDDER].read()-500)*YAWSPEED/500;  // the yaw angle is integrated from stick input
00097     else
00098         RC_yaw_adding = 0;
00099     
00100     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
00101     RC_angle[YAW] = RC_angle[YAW] + RC_yaw_adding > 180 ? RC_angle[YAW] - 360 + RC_yaw_adding : RC_angle[YAW] + RC_yaw_adding;
00102     Times[3] = LoopTimer.read(); // 6us
00103 
00104     // Controlling
00105     if (level) {
00106         for(int i=0;i<2;i++) { // LEVEL
00107             Controller_Angle[i].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying
00108             if (counter % 16 == 0)
00109                 Controller_Angle[i].compute(RC_angle[i], IMU.angle[i]); // give the controller the actual angles and get his advice to correct
00110             Controller_Rate[i].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying
00111             Controller_Rate[i].compute(-Controller_Angle[i].Value, /*IMU.mpu2.data_gyro[i]*/IMU.mpu.Gyro[i]); // give the controller the actual gyro values and get his advice to correct
00112             //Controller_Rate[i].compute(-Controller_Angle[i].Value, (IMU.mpu2.data_gyro[i] + IMU.mpu.Gyro[i])/2 );
00113         }
00114     } else {
00115         for(int i=0;i<2;i++) { // ACRO
00116             Controller_Rate[i].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying
00117             Controller_Rate[i].compute((RC[i].read()-500.0)*100.0/500.0, /*IMU.mpu2.data_gyro[i]*/IMU.mpu.Gyro[i]); // give the controller the actual gyro values and get his advice to correct
00118             //Controller_Rate[i].compute((RC[i].read()-500.0)*100.0/500.0, (IMU.mpu2.data_gyro[i] + IMU.mpu.Gyro[i])/2 );
00119         }
00120     }
00121     
00122     Controller_Rate[2].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying
00123     if (RC[THROTTLE].read() > 20)
00124         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
00125     else
00126         Controller_Rate[2].compute(0, IMU.mpu.Gyro[2]); // give the controller the actual gyro values and get his advice to correct
00127     
00128     float throttle = 100 + (RC[THROTTLE].read() * 500 / 1000);
00129     Times[4] = LoopTimer.read(); // 53us
00130     
00131     // Mixing
00132     Motor_speed[0] = throttle   +SQRT2*Controller_Rate[ROLL].Value  -SQRT2*Controller_Rate[PITCH].Value;  // X Configuration
00133     Motor_speed[1] = throttle   -SQRT2*Controller_Rate[ROLL].Value  -SQRT2*Controller_Rate[PITCH].Value;  // 
00134     Motor_speed[2] = throttle   -SQRT2*Controller_Rate[ROLL].Value  +SQRT2*Controller_Rate[PITCH].Value;  // 
00135     Motor_speed[3] = throttle   +SQRT2*Controller_Rate[ROLL].Value  +SQRT2*Controller_Rate[PITCH].Value;  // 
00136     
00137     Motor_speed[0] -= Controller_Rate[YAW].Value;
00138     Motor_speed[1] += Controller_Rate[YAW].Value;
00139     Motor_speed[2] -= Controller_Rate[YAW].Value;
00140     Motor_speed[3] += Controller_Rate[YAW].Value;
00141     Times[5] = LoopTimer.read(); // 17us
00142     
00143     if (armed) // for SECURITY!
00144     {       
00145             debug = false;
00146             // PITCH
00147             //ESC[0] = (int)Motor_speed[0]>50 ? (int)Motor_speed[0] : 50;
00148             //ESC[2] = (int)Motor_speed[2]>50 ? (int)Motor_speed[2] : 50;
00149             // ROLL
00150             //ESC[1] = (int)Motor_speed[1]>50 ? (int)Motor_speed[1] : 50;
00151             //ESC[3] = (int)Motor_speed[3]>50 ? (int)Motor_speed[3] : 50;
00152             for(int i=0;i<4;i++)   // Set new motorspeeds
00153                 ESC[i] = (int)Motor_speed[i]>100 ? (int)Motor_speed[i] : 100;
00154             
00155     } else {
00156         for(int i=0;i<4;i++) // for security reason, set every motor to zero speed
00157             ESC[i] = 0;
00158         debug = true;
00159     }
00160     Times[6] = LoopTimer.read(); // 6us
00161     
00162     LEDs.rollnext();
00163     
00164     /*if(counter % divider == 0) {
00165         pc.printf("%.3f,%.3f,%.3f\r\n", IMU.mpu.Gyro[ROLL], IMU.mpu.Gyro[PITCH], IMU.mpu.Gyro[YAW]);
00166     }*/
00167     counter++;
00168     
00169     Times[7] = LoopTimer.read(); // 7us TOTAL 297us
00170     while(LoopTimer.read() < 1/control_frequency); // Kill the rest of the time TODO: make a better solution so we can do misc things with these cycles
00171     Times[8] = LoopTimer.read();
00172     LoopTimer.stop();
00173     LoopTimer.reset();
00174     
00175     
00176     if (debug) {
00177         pc.printf("$STATE,%d,%d,%.f,%.3f,%.3f\r\n", armed, level, control_frequency, IMU.dt*1e3, IMU.dt_sensors*1e6);
00178         //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());
00179         pc.printf("$GYRO,%.3f,%.3f,%.3f\r\n", IMU.mpu.Gyro[ROLL], IMU.mpu.Gyro[PITCH], IMU.mpu.Gyro[YAW]);
00180         pc.printf("$GYRO2,%.3f,%.3f,%.3f\r\n", IMU.mpu2.data_gyro[ROLL], IMU.mpu2.data_gyro[PITCH], IMU.mpu2.data_gyro[YAW]);
00181         //pc.printf("$ACC,%.3f,%.3f,%.3f\r\n", IMU.mpu.Acc[ROLL], IMU.mpu.Acc[PITCH], IMU.mpu.Acc[YAW]);
00182         pc.printf("$ANG,%.3f,%.3f,%.3f\r\n", IMU.angle[ROLL], IMU.angle[PITCH], IMU.angle[YAW]);
00183         //pc.printf("$RCANG,%.3f,%.3f,%.3f\r\n", RC_angle[ROLL], RC_angle[PITCH], RC_angle[YAW]);
00184         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);
00185         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);
00186         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]);
00187         /*pc.printf("$TIMES");
00188         for(int i = 1; i < 10; i++)
00189             pc.printf(",%.3f", (Times[i]-Times[i-1])*1e6);
00190         pc.printf("\r\n");*/
00191         wait(0.1);
00192     }
00193 }
00194 
00195 void executer() {
00196     char command = pc.getc();
00197     if (command == 'X')
00198         mbed_reset();
00199     if (command == '-')
00200         debug = !debug;
00201         
00202     if (command == ':')
00203         armed = true;
00204     if (command == ' ')
00205         armed = false;
00206         
00207     if (command == 'q')
00208         level = true;
00209     if (command == 'a')
00210         level = false;
00211         
00212     if (command == 'w')
00213         P_R += 0.1;
00214     if (command == 's')
00215         P_R -= 0.1;
00216         
00217     if (command == 'e')
00218         I_R += 0.1;
00219     if (command == 'd')
00220         I_R -= 0.1;
00221         
00222     if (command == 'x')
00223         D_R += 0.001;
00224     if (command == 'c')
00225         D_R -= 0.001;
00226         
00227     if (command == 'r')
00228         P_A += 0.1;
00229     if (command == 'f')
00230         P_A -= 0.1;
00231         
00232     if (command == 't')
00233         I_A += 0.1;
00234     if (command == 'g')
00235         I_A -= 0.1;
00236         
00237     if (command == 'z')
00238         PY += 0.1;
00239     if (command == 'h')
00240         PY -= 0.1;
00241         
00242     if (command == 'o') {
00243         control_frequency += 100;
00244         
00245     }
00246     if (command == 'l') {
00247         control_frequency -= 100;
00248         
00249     }
00250         
00251         
00252     pc.putc(command);
00253     LEDs.tilt(2);
00254 }
00255 
00256 int main() {
00257     pc.attach(&executer);
00258     while(1) {
00259         loop();
00260     }
00261 }