main.cpp

00001 #include "mbed.h"       // Standard Library
00002 #include "LED.h"        // LEDs framework for blinking ;)
00003 #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)
00004 #include "MPU6050.h"    // Combined Gyro and Acc
00005 //#include "L3G4200D.h"   // Gyro (Gyroscope)
00006 //#include "ADXL345.h"    // Acc (Accelerometer)
00007 //#include "HMC5883.h"    // Comp (Compass)
00008 //#include "BMP085_old.h"     // Alt (Altitude sensor)
00009 #include "RC_Channel.h" // RemoteControl Channels with PPM
00010 #include "Servo_PWM.h"  // Motor PPM using PwmOut
00011 #include "PID.h"        // PID Library (slim, self written)
00012 #include "IMU_Filter.h" // Class to calculate position angles
00013 #include "Mixer.h"      // Class to calculate motorspeeds from Angles, Regulation and RC-Signals
00014 
00015 #define RATE            0.002                               // speed of the interrupt for Sensors and PID
00016 #define PPM_FREQU       495                                 // Hz Frequency of PPM Signal for ESCs (maximum <500Hz)
00017 #define RC_SENSITIVITY  30                                  // maximal angle from horizontal that the PID is aming for
00018 #define YAWSPEED        0.2                                 // maximal speed of yaw rotation in degree per Rate
00019 #define INTEGRAL_MAX    300
00020 
00021 // RC
00022 #define AILERON         0
00023 #define ELEVATOR        1
00024 #define RUDDER          2
00025 #define THROTTLE        3
00026 // Axes
00027 #define ROLL            0
00028 #define PITCH           1
00029 #define YAW             2
00030 
00031 #define PC_CONNECTED // decomment if you want to debug per USB/Bluetooth and your PC
00032 
00033 // Global variables
00034 bool    armed = false;                  // this variable is for security (when false no motor rotates any more)
00035 bool    RC_present = false;             // this variable shows if an RC is present
00036 float   dt = 0;
00037 float   time_for_dt = 0;
00038 float   dt_read_sensors = 0;
00039 float   time_read_sensors = 0;
00040 float   controller_value[] = {0,0,0};   // The calculated answer form the Controller
00041 float   RC_angle[] = {0,0,0};           // Angle of the RC Sticks, to steer the QC
00042 float   RC_yaw_adding;                  // temporary variable to take the desired yaw adjustment
00043 
00044 float P = 4.0;                          // PID values
00045 float I = 0;
00046 float D = 0.1;
00047 
00048 float PY = 0;                          // PID values for YAW
00049 float IY = 0;
00050 float DY = 0;
00051 
00052 Timer GlobalTimer;                      // global time to calculate processing speed
00053 Ticker Dutycycler;                      // timecontrolled interrupt for exact timed control loop
00054 
00055 // Initialisation of hardware (see includes for more info)
00056 LED         LEDs;
00057 #ifdef PC_CONNECTED
00058     PC          pc(USBTX, USBRX, 115200);    // USB
00059     //PC          pc(p9, p10, 115200);       // Bluetooth
00060 #endif
00061 MPU6050     Sensor(p28, p27);
00062 //L3G4200D    Gyro(p28, p27);
00063 //ADXL345     Acc(p28, p27);
00064 //HMC5883     Comp(p28, p27);
00065 //BMP085_old      Alt(p28, p27);
00066 RC_Channel  RC[] = {RC_Channel(p5,1), RC_Channel(p6,2), RC_Channel(p8,4), RC_Channel(p7,3)};                                // no p19/p20 !
00067 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!
00068 IMU_Filter  IMU;    // (don't write () after constructor for no arguments!)
00069 Mixer       MIX(1); // 0 for +-Formation, 1 for X-Formation 
00070 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
00071 
00072 void dutycycle() { // method which is called by the Ticker Dutycycler every RATE seconds
00073     time_read_sensors = GlobalTimer.read(); // start time measure for sensors
00074     
00075     // read data from sensors // ATTENTION! the I2C option repeated true is important because otherwise interrupts while bus communications cause crashes
00076     Sensor.read();
00077     //Gyro.read();
00078     //Acc.read();
00079     //Comp.read(); // TODO: not every loop every sensor? altitude not that important
00080     //Alt.Update(); // TODO needs very long to read because of waits
00081     
00082     //pc.printf("%6.1f,%6.1f,%6.1f,%6.1f,%6.1f,%6.1f\r\n", Gyro.data[0], Gyro.data[1], Gyro.data[2], Acc.data[0], Acc.data[1], Acc.data[2]);
00083     
00084     
00085     
00086     // meassure dt for the filter
00087     dt = GlobalTimer.read() - time_for_dt; // time in us since last loop
00088     time_for_dt = GlobalTimer.read();      // set new time for next measurement
00089     
00090     IMU.compute(dt, Sensor.data_gyro, Sensor.data_acc);
00091     //IMU.compute(dt, Gyro.data, Acc.data);
00092     //pc.printf("%f,%f,%f,%3.5fs,%3.5fs\r\n", IMU.angle[0], IMU.angle[1], IMU.angle[2], dt, dt_read_sensors);
00093 
00094     if(RC[AILERON].read() == -100 || RC[ELEVATOR].read() == -100 || RC[RUDDER].read() == -100 || RC[THROTTLE].read() == -100)
00095         RC_present = false;
00096     else
00097         RC_present = true;
00098 
00099     // Arming / disarming
00100     if(RC[THROTTLE].read() < 20 && RC[RUDDER].read() > 850) {
00101         armed = true;
00102         RC_angle[YAW] = IMU.angle[YAW];
00103     }
00104     if((RC[THROTTLE].read() < 30 && RC[RUDDER].read() < 30) || !RC_present) {
00105         armed = false;
00106     }
00107     
00108     // RC Angle ROLL-PITCH-Part
00109     for(int i=0;i<2;i++) {    // calculate new angle we want the QC to have
00110         if (RC_present)
00111             RC_angle[i] = (RC[i].read()-500)*RC_SENSITIVITY/500.0;
00112         else
00113             RC_angle[i] = 0;
00114     }
00115     
00116     // RC Angle YAW-Part
00117     if (RC_present && RC[THROTTLE].read() > 20)
00118         RC_yaw_adding = (RC[RUDDER].read()-500)*YAWSPEED/500;
00119     else
00120         RC_yaw_adding = 0;
00121         
00122     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
00123         if(RC_angle[YAW] + RC_yaw_adding < -180)
00124             RC_yaw_adding += 360;
00125         if(RC_angle[YAW] + RC_yaw_adding > 180)
00126             RC_yaw_adding -= 360;
00127     }
00128     RC_angle[YAW] += RC_yaw_adding;  // for yaw angle it's integrated
00129     
00130     // PID controlling
00131     for(int i=0;i<2;i++) {
00132         Controller[i].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying
00133         controller_value[i] = Controller[i].compute(RC_angle[i], IMU.angle[i]); // give the controller the actual angle and get his advice to correct
00134     }
00135     Controller[YAW].setIntegrate(armed); // same for YAW
00136     if (abs(RC_angle[YAW] - IMU.angle[YAW]) > 180)  // for YAW a special calculation because of range -180 to 180
00137          if (RC_angle[YAW] > IMU.angle[YAW])
00138             controller_value[YAW] = Controller[YAW].compute(RC_angle[YAW] - 360, IMU.angle[YAW]);
00139          else
00140             controller_value[YAW] = Controller[YAW].compute(RC_angle[YAW] + 360, IMU.angle[YAW]);
00141     else
00142         controller_value[YAW] = Controller[YAW].compute(RC_angle[YAW], IMU.angle[YAW]);
00143     
00144     if (armed) // for SECURITY!
00145     {       
00146             MIX.compute(RC[THROTTLE].read(), controller_value); // let the Mixer compute motorspeeds based on throttle and controller output
00147             for(int i=0;i<4;i++)   // Set new motorspeeds
00148                 ESC[i] = (int)MIX.Motor_speed[i];
00149             
00150     } else {
00151         for(int i=0;i<4;i++) // for security reason, set every motor to zero speed
00152             ESC[i] = 0;
00153     }
00154     /*pc.printf("%d,%f,%f,  %f,%f,%f,  %f,%f,%f,  %f,%f,%f,  %f,%f,%f,%f\r\n",
00155         armed,
00156         dt,
00157         dt_read_sensors,
00158         IMU.angle[ROLL], 
00159         IMU.angle[PITCH], 
00160         IMU.angle[YAW], 
00161         RC_angle[ROLL], 
00162         RC_angle[PITCH], 
00163         RC_angle[YAW], 
00164         controller_value[ROLL], 
00165         controller_value[PITCH], 
00166         controller_value[YAW], 
00167         MIX.Motor_speed[0], 
00168         MIX.Motor_speed[1], 
00169         MIX.Motor_speed[2], 
00170         MIX.Motor_speed[3]);*/
00171         
00172     pc.printf("%f,%f,%f,  %f,%f,%f,  %f,%f,%f,  %f,%f,%f,  %f,%f,%f,%f\r\n",
00173         IMU.angle[ROLL], 
00174         IMU.angle[PITCH], 
00175         IMU.angle[YAW],
00176         Sensor.data_gyro[0],
00177         Sensor.data_gyro[1],
00178         Sensor.data_gyro[2],
00179         Sensor.data_acc[0],
00180         Sensor.data_acc[1],
00181         Sensor.data_acc[2],  
00182         controller_value[ROLL], 
00183         controller_value[PITCH], 
00184         controller_value[YAW], 
00185         MIX.Motor_speed[0], 
00186         MIX.Motor_speed[1], 
00187         MIX.Motor_speed[2], 
00188         MIX.Motor_speed[3]);
00189                                     
00190     dt_read_sensors = GlobalTimer.read() - time_read_sensors; // stop time for loop
00191 }
00192 
00193 void commandexecuter(char* command) {  // take new PID values on the fly
00194     if (command[0] == 'p')
00195         if (command[1] == 'y')
00196             PY = atof(&command[2]);
00197         else
00198             P = atof(&command[1]);
00199     if (command[0] == 'i')
00200         if (command[1] == 'y')
00201             IY = atof(&command[2]);
00202         else
00203             I = atof(&command[1]);
00204     if (command[0] == 'd')
00205         if (command[1] == 'y')
00206             DY = atof(&command[2]);
00207         else
00208             D = atof(&command[1]);
00209     for(int i=0;i<2;i++) {
00210         Controller[i].setPID(P,I,D); // give the controller the new PID values
00211     }
00212     Controller[YAW].setPID(PY,IY,DY); // give the controller the new PID values
00213 }
00214 
00215 int main() { // main programm for initialisation and debug output
00216     NVIC_SetPriority(TIMER3_IRQn, 1); // set priorty of tickers below hardware interrupts (standard priority is 0)(this is to prevent the RC interrupt from waiting until ticker is finished)
00217     
00218     #ifdef PC_CONNECTED
00219         // init screen
00220         pc.locate(10,5);
00221         pc.printf("Flybed v0.2");
00222     #endif
00223     LEDs.roll(2);
00224     
00225     Sensor.calibrate(50, 0.02);
00226     //Gyro.calibrate(50, 0.02);
00227     //Acc.calibrate(50, 0.02);
00228     
00229     // Start!
00230     GlobalTimer.start();
00231     Dutycycler.attach(&dutycycle, RATE);     // start to process all RATE seconds
00232     
00233     while(1) { 
00234         #ifdef PC_CONNECTED
00235             if (pc.readable())  // Get Serial input (polled because interrupts disturb I2C)
00236                 pc.readcommand(&commandexecuter);
00237             //pc.printf("%f %f %f %f %f %f\r\n", IMU.angle[0], IMU.angle[1], IMU.angle[2], controller_value[0], controller_value[1], controller_value[2]); // For live plot in MATLAB of IMU
00238             //pc.printf("%f,%f,%f,%f,%f,%f\r\n", IMU.angle[0], IMU.angle[1], IMU.angle[2], controller_value[0], controller_value[1], controller_value[2]);
00239             #if 0 //pc.cls();
00240                 pc.locate(20,0); // PC output
00241                 pc.printf("dt:%3.5fs   dt_sensors:%3.5fs    Altitude:%6.1fm   ", dt, dt_read_sensors, Alt.CalcAltitude(Alt.Pressure));
00242                 pc.locate(5,1);
00243                 if(armed)
00244                     pc.printf("ARMED!!!!!!!!!!!!!");
00245                 else
00246                     pc.printf("DIS_ARMED            ");
00247                 pc.locate(5,3);
00248                 pc.printf("Roll:%6.1f   Pitch:%6.1f   Yaw:%6.1f    ", IMU.angle[0], IMU.angle[1], IMU.angle[2]);
00249                 pc.locate(5,4);
00250                 pc.printf("q0:%6.1f   q1:%6.1f   q2:%6.1f   q3:%6.1f       ", IMU.q0, IMU.q1, IMU.q2, IMU.q3);
00251                 pc.locate(5,5);
00252                 pc.printf("Gyro.data: X:%6.1f  Y:%6.1f  Z:%6.1f", Gyro.data[0], Gyro.data[1], Gyro.data[2]);
00253                 pc.locate(5,6);
00254                 pc.printf("Acc.data:  X:%6.1f  Y:%6.1f  Z:%6.1f", Acc.data[0], Acc.data[1], Acc.data[2]);
00255                 
00256                 pc.locate(5,8);
00257                 pc.printf("P :%6.1f   I :%6.1f   D :%6.1f    ", P, I, D);
00258                 pc.locate(5,9);
00259                 pc.printf("PY:%6.1f   IY:%6.1f   DY:%6.1f    ", PY, IY, DY);
00260                 
00261                 pc.locate(5,11);
00262                 pc.printf("PID Result:");
00263                 for(int i=0;i<3;i++)
00264                     pc.printf("  %d: %6.1f", i, controller_value[i]);
00265                 pc.locate(5,14);
00266                 pc.printf("RC angle: roll: %f     pitch: %f      yaw: %f    ", RC_angle[0], RC_angle[1], RC_angle[2]);
00267                 pc.locate(5,16);
00268                 pc.printf("Motor: 0:%d 1:%d 2:%d 3:%d    ", (int)MIX.Motor_speed[0], (int)MIX.Motor_speed[1], (int)MIX.Motor_speed[2], (int)MIX.Motor_speed[3]);
00269                 
00270                 // RC
00271                 pc.locate(10,19);
00272                 pc.printf("RC0: %4d   RC1: %4d   RC2: %4d   RC3: %4d   ", RC[0].read(), RC[1].read(), RC[2].read(), RC[3].read());
00273                 
00274                 pc.locate(10,21);
00275                 pc.printf("Commandline: %s                                  ", pc.command);
00276             #endif
00277         #endif
00278         if(armed){
00279             LEDs.rollnext();
00280         } else {
00281             for(int i=1;i<=4;i++)
00282                 LEDs.set(i);
00283         }
00284         wait(0.05);
00285     }
00286 }