main.cpp

00001 #include "mbed.h"
00002 #include "PPM.h"
00003 
00004 I2C i2c(PB_11, PB_10);  // sda, scl
00005 Serial pc(PA_9, PA_10, 115200); // tx, rx
00006 DigitalOut led_green(PB_3);
00007 DigitalOut led_red(PB_4);
00008 PPM ppm(PA_0);
00009 
00010 DigitalOut motorPin1(PA_8);
00011 DigitalOut motorPin2(PA_11);
00012 DigitalOut motorPin3(PB_6);
00013 DigitalOut motorPin4(PB_7);
00014 DigitalOut motorPin5(PB_8);
00015 DigitalOut motorPin6(PB_9);
00016 
00017 DigitalOut buzzer(PA_12);
00018 
00019 #define ROLL  0
00020 #define PITCH 1
00021 #define YAW   2
00022 
00023 #define RC_ROLL      (ppm.channels[0])
00024 #define RC_PITCH     (ppm.channels[1])
00025 #define RC_THROTTLE  (ppm.channels[2])
00026 #define RC_YAW       (ppm.channels[3])
00027 #define RC_DEADBAND_US 8
00028 
00029 #define GYRO_CALIB_SAMPLES 4000
00030 #define MPU6050 (0x68 << 1)
00031 
00032 float pid_p_gain_roll = 1.3;               //Gain setting for the roll P-controller
00033 float pid_i_gain_roll = 0.04;              //Gain setting for the roll I-controller
00034 float pid_d_gain_roll = 18.0;              //Gain setting for the roll D-controller
00035 int pid_max_roll = 400;                    //Maximum output of the PID-controller (+/-)
00036 
00037 float pid_p_gain_pitch = pid_p_gain_roll;  //Gain setting for the pitch P-controller.
00038 float pid_i_gain_pitch = pid_i_gain_roll;  //Gain setting for the pitch I-controller.
00039 float pid_d_gain_pitch = pid_d_gain_roll;  //Gain setting for the pitch D-controller.
00040 int pid_max_pitch = pid_max_roll;          //Maximum output of the PID-controller (+/-)
00041 
00042 float pid_p_gain_yaw = 4.0;                //Gain setting for the pitch P-controller. //4.0
00043 float pid_i_gain_yaw = 0.02;               //Gain setting for the pitch I-controller. //0.02
00044 float pid_d_gain_yaw = 0.0;                //Gain setting for the pitch D-controller.
00045 int pid_max_yaw = 400;                     //Maximum output of the PID-controller (+/-)
00046 
00047 int cal_int;
00048 double gyro_cal[3];
00049 int16_t acc_raw[3], gyro_raw[3];
00050 float acc_smoothed[3], gyro_smoothed[3];
00051 
00052 Timer armingTimer;
00053 
00054 Timer timer;
00055 
00056 float angle_roll_acc, angle_pitch_acc, angle_pitch, angle_roll;
00057 float acc_total_vector;
00058 
00059 float roll_level_adjust, pitch_level_adjust;
00060 float pid_i_mem_roll, pid_roll_setpoint, pid_output_roll, pid_last_roll_d_error;
00061 float pid_i_mem_pitch, pid_pitch_setpoint, pid_output_pitch, pid_last_pitch_d_error;
00062 float pid_i_mem_yaw, pid_yaw_setpoint, pid_output_yaw, pid_last_yaw_d_error;
00063 
00064 int constrain( int val, int lower, int upper )
00065 {
00066     if ( val < lower ) return lower;
00067     if ( val > upper ) return upper;
00068     return val;
00069 }
00070 
00071 void beepStart() 
00072 {
00073     buzzer = 0;
00074 }
00075 
00076 void beepStop() 
00077 {
00078     buzzer = 1;
00079 }
00080 
00081 void beep(int ms) 
00082 {
00083     beepStart();
00084     wait_ms(ms);
00085     beepStop();
00086 }
00087 
00088 void setIMURegisters() 
00089 {    
00090     char cmd[2];
00091     
00092     cmd[0] = 0x6B;                                                          //We want to write to the PWR_MGMT_1 register (6B hex)
00093     cmd[1] = 0x00;                                                          //Set the register bits as 00000000 to activate the gyro
00094     i2c.write(MPU6050, cmd, 2);
00095     
00096     cmd[0] = 0x1B;                                                          //We want to write to the GYRO_CONFIG register (1B hex)
00097     cmd[1] = 0x08;                                                          //Set the register bits as 00001000 (500dps full scale)
00098     i2c.write(MPU6050, cmd, 2);
00099     
00100     cmd[0] = 0x1C;                                                          //We want to write to the ACCEL_CONFIG register (1A hex)
00101     cmd[1] = 0x10;                                                          //Set the register bits as 00010000 (+/- 8g full scale range)
00102     i2c.write(MPU6050, cmd, 2);
00103     
00104     //Let's perform a random register check to see if the values are written correct
00105     cmd[0] = 0x1B;                                                          //Start reading at register 0x1B
00106     i2c.write(MPU6050, cmd, 1);    
00107     i2c.read(MPU6050, cmd, 1);
00108     if (cmd[0] != 0x08) {                                                   //Check if the value is 0x08
00109         led_red = 0;                                                        //Turn on the warning led
00110         pc.printf("Gyro init failed! (%d)\n", cmd[0]);
00111         while (true)
00112             wait(1);                                                      //Stay in this loop for ever
00113     }
00114     
00115     cmd[0] = 0x1A;                                                          //We want to write to the CONFIG register (1A hex)
00116     cmd[1] = 0x03;                                                          //Set the register bits as 00000011 (Set Digital Low Pass Filter to ~43Hz)
00117     i2c.write(MPU6050, cmd, 2);
00118 }
00119 
00120 void readIMU(bool calibrated = true)
00121 {    
00122     char cmd[14];
00123 
00124     cmd[0] = 0x3B;                                                          //Start reading at register 43h and auto increment with every read.
00125     i2c.write(MPU6050, cmd, 1);
00126     i2c.read(MPU6050, cmd, 14);                                             //Read 14 bytes from the gyro.
00127     
00128     acc_raw[0] = (cmd[0] << 8) | cmd[1];                               //Add the low and high byte to the acc_x variable.
00129     acc_raw[1] = (cmd[2] << 8) | cmd[3];                               //Add the low and high byte to the acc_y variable.
00130     acc_raw[2] = (cmd[4] << 8) | cmd[5];                               //Add the low and high byte to the acc_z variable.
00131     
00132     gyro_raw[0] = (cmd[8] << 8) |  cmd[9];                              //Read high and low part of the angular data.
00133     gyro_raw[1] = (cmd[10] << 8) | cmd[11];                              //Read high and low part of the angular data.
00134     gyro_raw[2] = (cmd[12] << 8) | cmd[13];                              //Read high and low part of the angular data.
00135     
00136     if ( calibrated ) {
00137         for (int i = 0; i < 3; i++)
00138             gyro_raw[i] -= gyro_cal[i];                                       
00139     }
00140 }
00141 
00142 void calibrateGyro() 
00143 {
00144     for (int i = 0; i < 3; i++) 
00145         gyro_cal[i] = 0;
00146     
00147     for (int s = 0; s < GYRO_CALIB_SAMPLES; s++) {                            //Take GYRO_CALIB_SAMPLES readings for calibration.
00148         if (s % 50 == 0)
00149             led_green = 1 - led_green;                                          //Change the led status to indicate calibration.
00150         readIMU(false);                                                        //Read the gyro output.
00151         for (int i = 0; i < 3; i++)
00152             gyro_cal[i] += gyro_raw[i];                                       //Ad roll value to gyro_roll_cal.
00153         wait_us(3);                                                               //Wait 3 milliseconds before the next loop.
00154     }
00155     
00156     for (int i = 0; i < 3; i++)
00157         gyro_cal[i] /= (float)GYRO_CALIB_SAMPLES;                                                 //Divide the roll total by GYRO_CALIB_SAMPLES.
00158 }
00159 
00160 bool checkArmStateChange(bool armed) 
00161 {
00162     static int armingState = 0;
00163     
00164     if ( RC_THROTTLE > 1100 )
00165         return false;
00166     
00167     if ( ! armed && RC_YAW > 1900 ) {
00168         if ( armingState == 0 ) {
00169             armingState = 1;
00170             armingTimer.reset();
00171             armingTimer.start();
00172             pc.printf("Started arming\n");
00173         }
00174         else {
00175             uint16_t time = armingTimer.read_ms();
00176             if ( time > 2000 ) {
00177                 armed = true;
00178                 armingState = 0;
00179                 pc.printf("ARMED\n");
00180                 //beep(300);
00181                 return true;
00182             }
00183         }
00184     }
00185     else if ( armed && RC_YAW < 1100 ) {
00186         if ( armingState == 0 ) {
00187             armingState = -1;
00188             armingTimer.reset();
00189             armingTimer.start();
00190             pc.printf("Started disarming\n");
00191         }
00192         else {
00193             uint16_t time = armingTimer.read_ms();
00194             if ( time > 2000 ) {
00195                 armed = false;
00196                 armingState = 0;
00197                 pc.printf("DISARMED\n");
00198                 //beep(50);
00199                 //wait_ms(50);
00200                 //beep(50);
00201                 return false;
00202             }
00203         }
00204     }
00205     else {
00206         armingState = 0;
00207         armingTimer.reset();
00208     }
00209     
00210     return armed;
00211 }
00212 
00213 bool getAutoLevelState()
00214 {
00215     return ppm.channels[5] > 1600;
00216 }
00217 
00218 int afterDeadband(int in)
00219 {
00220     int lower = 1500 - RC_DEADBAND_US;
00221     int upper = 1500 + RC_DEADBAND_US;
00222     if ( in < lower )
00223         return in - lower;
00224     if ( in > upper )
00225         return in - upper;
00226     return 0;
00227 }
00228 
00229 void resetPID()
00230 {
00231     pid_i_mem_roll = 0;
00232     pid_last_roll_d_error = 0;
00233     pid_i_mem_pitch = 0;
00234     pid_last_pitch_d_error = 0;
00235     pid_i_mem_yaw = 0;
00236     pid_last_yaw_d_error = 0;
00237 }
00238 
00239 void calculatePID()
00240 {
00241     //Roll calculations
00242     float pid_error_temp = gyro_smoothed[ROLL] - pid_roll_setpoint;
00243     pid_i_mem_roll += pid_i_gain_roll * pid_error_temp;
00244     if(pid_i_mem_roll > pid_max_roll)pid_i_mem_roll = pid_max_roll;
00245     else if(pid_i_mem_roll < pid_max_roll * -1)pid_i_mem_roll = pid_max_roll * -1;
00246     
00247     pid_output_roll = pid_p_gain_roll * pid_error_temp + pid_i_mem_roll + pid_d_gain_roll * (pid_error_temp - pid_last_roll_d_error);
00248     if(pid_output_roll > pid_max_roll)pid_output_roll = pid_max_roll;
00249     else if(pid_output_roll < pid_max_roll * -1)pid_output_roll = pid_max_roll * -1;
00250     
00251     pid_last_roll_d_error = pid_error_temp;
00252     
00253     //Pitch calculations
00254     pid_error_temp = gyro_smoothed[PITCH] - pid_pitch_setpoint;
00255     pid_i_mem_pitch += pid_i_gain_pitch * pid_error_temp;
00256     if(pid_i_mem_pitch > pid_max_pitch)pid_i_mem_pitch = pid_max_pitch;
00257     else if(pid_i_mem_pitch < pid_max_pitch * -1)pid_i_mem_pitch = pid_max_pitch * -1;
00258     
00259     pid_output_pitch = pid_p_gain_pitch * pid_error_temp + pid_i_mem_pitch + pid_d_gain_pitch * (pid_error_temp - pid_last_pitch_d_error);
00260     if(pid_output_pitch > pid_max_pitch)pid_output_pitch = pid_max_pitch;
00261     else if(pid_output_pitch < pid_max_pitch * -1)pid_output_pitch = pid_max_pitch * -1;
00262     
00263     pid_last_pitch_d_error = pid_error_temp;
00264     
00265     //Yaw calculations
00266     pid_error_temp = gyro_smoothed[YAW] - pid_yaw_setpoint;
00267     pid_i_mem_yaw += pid_i_gain_yaw * pid_error_temp;
00268     if(pid_i_mem_yaw > pid_max_yaw)pid_i_mem_yaw = pid_max_yaw;
00269     else if(pid_i_mem_yaw < pid_max_yaw * -1)pid_i_mem_yaw = pid_max_yaw * -1;
00270     
00271     pid_output_yaw = pid_p_gain_yaw * pid_error_temp + pid_i_mem_yaw + pid_d_gain_yaw * (pid_error_temp - pid_last_yaw_d_error);
00272     if(pid_output_yaw > pid_max_yaw)pid_output_yaw = pid_max_yaw;
00273     else if(pid_output_yaw < pid_max_yaw * -1)pid_output_yaw = pid_max_yaw * -1;
00274     
00275     pid_last_yaw_d_error = pid_error_temp;
00276 }
00277 
00278 int main() {
00279     
00280     i2c.frequency(400000);
00281     
00282     buzzer = 1;
00283     led_red = 1;
00284     led_green = 1;
00285     
00286     motorPin1 = 0;
00287     motorPin2 = 0;
00288     motorPin3 = 0;
00289     motorPin4 = 0;
00290     motorPin5 = 0;
00291     motorPin6 = 0;
00292     
00293     armingTimer.reset();
00294     
00295     beep(50);
00296         
00297     pc.printf("setIMURegisters\n");    
00298     setIMURegisters();
00299     
00300     pc.printf("calibrateGyro\n");
00301     calibrateGyro();
00302     
00303     pc.printf("Finished gyro calibration (%f, %f, %f)\n", gyro_cal[0], gyro_cal[1], gyro_cal[2]);
00304     
00305     timer.reset();
00306     timer.start();
00307     
00308     for (int i = 0; i < 3; i++)
00309         gyro_smoothed[i] = 0;
00310     angle_pitch = 0;
00311     angle_roll = 0;
00312         
00313     beep(50);
00314     wait_ms(50);
00315     beep(50);
00316     
00317     bool armed = false;
00318     bool armedLastTime = armed;
00319     
00320     bool autoLevel = getAutoLevelState();
00321     bool autoLevelLastTime = autoLevel;
00322     
00323     unsigned long oneShotCounter = 0;
00324     
00325     unsigned long loopCounter = 0;
00326     
00327     float initialAccUptake = 0.8;
00328     
00329     while (true) {
00330         
00331         //pc.printf("Gyro (%f, %f, %f)\n", gyro_smoothed[0], gyro_smoothed[1], gyro_smoothed[2]);
00332         //pc.printf("Accel (%f, %f)\n", angle_pitch, angle_roll);
00333         
00334         armed = checkArmStateChange(armed);
00335         led_green = ! armed;
00336         
00337         if ( ! armedLastTime && armed )
00338             resetPID();
00339         if ( armed != armedLastTime ) {
00340             oneShotCounter = 20;
00341             beepStart();
00342         }
00343         armedLastTime = armed;
00344         
00345         autoLevel = getAutoLevelState();
00346         if ( autoLevel != autoLevelLastTime ) {
00347             oneShotCounter = 4;
00348             beepStart();
00349         }
00350         autoLevelLastTime = autoLevel;
00351         
00352         if ( oneShotCounter > 0 ) {
00353             if ( --oneShotCounter == 0 ) {
00354                 beepStop();
00355             }
00356         }
00357         
00358         const float uptake = 0.3;
00359         for (int i = 0; i < 3; i++)
00360             gyro_smoothed[i] = (gyro_smoothed[i] * (1-uptake)) + ((gyro_raw[i] / 65.5) * uptake);   //Gyro pid input is deg/sec.
00361             
00362             
00363         //Gyro angle calculations
00364         //0.0000611 = 1 / (250Hz / 65.5)
00365         angle_pitch += gyro_raw[ROLL] * 0.0000611;                                    //Calculate the traveled pitch angle and add this to the angle_pitch variable.
00366         angle_roll += gyro_raw[PITCH] * 0.0000611;                                      //Calculate the traveled roll angle and add this to the angle_roll variable.
00367         
00368         //0.000001066 = 0.0000611 * (3.142(PI) / 180degr) The Arduino sin function is in radians
00369         float new_angle_pitch = angle_pitch + angle_roll * sin(gyro_raw[YAW] * 0.000001066);                  //If the IMU has yawed transfer the roll angle to the pitch angel.
00370         angle_roll -= angle_pitch * sin(gyro_raw[YAW] * 0.000001066);                  //If the IMU has yawed transfer the pitch angle to the roll angel.
00371         angle_pitch = new_angle_pitch;
00372         
00373         //Accelerometer angle calculations
00374         acc_total_vector = sqrtf((acc_raw[0]*acc_raw[0])+(acc_raw[1]*acc_raw[1])+(acc_raw[2]*acc_raw[2]));       //Calculate the total accelerometer vector.
00375         
00376         if (abs(acc_raw[PITCH]) < acc_total_vector) {                                        //Prevent the asin function to produce a NaN
00377             angle_pitch_acc = asin((float)acc_raw[PITCH]/acc_total_vector)* 57.296;          //Calculate the pitch angle.
00378         }
00379         if (abs(acc_raw[ROLL]) < acc_total_vector) {                                        //Prevent the asin function to produce a NaN
00380             angle_roll_acc = asin((float)acc_raw[ROLL]/acc_total_vector)* -57.296;          //Calculate the roll angle.
00381         }
00382         
00383         //Place the MPU-6050 spirit level and note the values in the following two lines for calibration.
00384         angle_pitch_acc -= 0.0;                                                   //Accelerometer calibration value for pitch.
00385         angle_roll_acc -= 0.0;                                                    //Accelerometer calibration value for roll.
00386         
00387         initialAccUptake *= 0.98;
00388         
00389         float acc_uptake = 0.0004 + initialAccUptake;
00390         angle_pitch = angle_pitch * (1-acc_uptake) + angle_pitch_acc * acc_uptake;            //Correct the drift of the gyro pitch angle with the accelerometer pitch angle.
00391         angle_roll = angle_roll * (1-acc_uptake) + angle_roll_acc * acc_uptake;               //Correct the drift of the gyro roll angle with the accelerometer roll angle.
00392         
00393         pitch_level_adjust = angle_pitch * 15;                                    //Calculate the pitch angle correction
00394         roll_level_adjust = angle_roll * 15;                                      //Calculate the roll angle correction
00395         
00396         if ( ! autoLevel ){                                                          //If the quadcopter is not in auto-level mode
00397             pitch_level_adjust = 0;                                                 //Set the pitch angle correction to zero.
00398             roll_level_adjust = 0;                                                  //Set the roll angle correcion to zero.
00399         }
00400             
00401            
00402            
00403         //The PID set point in degrees per second is determined by the roll receiver input.
00404         //In the case of deviding by 3 the max roll rate is aprox 164 degrees per second ( (500-8)/3 = 164d/s ).
00405         pid_roll_setpoint = afterDeadband(RC_ROLL);        
00406         pid_roll_setpoint -= roll_level_adjust;                                   //Subtract the angle correction from the standardized receiver roll input value.
00407         pid_roll_setpoint /= 3.0;                                                 //Divide the setpoint for the PID roll controller by 3 to get angles in degrees.
00408         
00409         //The PID set point in degrees per second is determined by the pitch receiver input.
00410         //In the case of deviding by 3 the max pitch rate is aprox 164 degrees per second ( (500-8)/3 = 164d/s ).
00411         pid_pitch_setpoint = afterDeadband(RC_PITCH);        
00412         pid_pitch_setpoint -= pitch_level_adjust;                                  //Subtract the angle correction from the standardized receiver pitch input value.
00413         pid_pitch_setpoint /= 3.0;                                                 //Divide the setpoint for the PID pitch controller by 3 to get angles in degrees.
00414         
00415         //The PID set point in degrees per second is determined by the yaw receiver input.
00416         //In the case of dividing by 3 the max yaw rate is aprox 164 degrees per second ( (500-8)/3 = 164d/s ).
00417         pid_yaw_setpoint = 0;
00418         //We need a little dead band of 16us for better results.
00419         if (RC_THROTTLE > 1050) { //Do not yaw when turning off the motors.
00420             pid_yaw_setpoint = afterDeadband(RC_YAW) / 3.0;
00421         }
00422         
00423         calculatePID();           
00424            
00425         int pwm[6];
00426         
00427         if (armed) {                                                          //The motors are started.
00428             int throttle = RC_THROTTLE;
00429             if (throttle > 1800) 
00430                 throttle = 1800;                                   //We need some room to keep full control at full throttle.
00431             pwm[0] = throttle - pid_output_pitch + pid_output_roll - pid_output_yaw; //Calculate the pulse for esc 1 (front-right - CCW)
00432             pwm[1] = throttle + pid_output_pitch + pid_output_roll + pid_output_yaw; //Calculate the pulse for esc 2 (rear-right - CW)
00433             pwm[2] = throttle + pid_output_pitch - pid_output_roll - pid_output_yaw; //Calculate the pulse for esc 3 (rear-left - CCW)
00434             pwm[3] = throttle - pid_output_pitch - pid_output_roll + pid_output_yaw; //Calculate the pulse for esc 4 (front-left - CW)
00435             pwm[4] = 1000;
00436             pwm[5] = 1000;
00437             
00438             for (int i = 0; i < 6; i++)
00439                 pwm[i] = constrain( pwm[i], 1100, 2000);                                         //Keep the motors running.
00440         }        
00441         else {
00442             for (int i = 0; i < 6; i++)
00443                 pwm[i] = 1000;                                                           //If start is not 2 keep a 1000us pulse for ess-1.
00444         }
00445     
00446     
00447     
00448         
00449         while ( timer.read_us() < 4000 )
00450             ; // wait
00451             
00452         motorPin1 = motorPin2 = motorPin3 = motorPin4 = 1;
00453         timer.reset();
00454         
00455         readIMU();
00456         
00457         if ( loopCounter++ % 20 == 0 ) {
00458             //pc.printf("%d %d %d %d\n", pwm[0], pwm[1], pwm[2], pwm[3]);
00459             pc.printf("%f %f\n", angle_roll, angle_pitch );
00460             //pc.printf("%d\n", pwm[0] );
00461             //pc.printf("Gyro (%f, %f, %f)\n", gyro_smoothed[0], gyro_smoothed[1], gyro_smoothed[2]);
00462             //pc.printf("%d, %d, %d, %d, %d, %d\n", ppm.channels[0], ppm.channels[1], ppm.channels[2], ppm.channels[3], ppm.channels[4], ppm.channels[5]);
00463         }
00464         
00465         bool doneOutput = false;
00466         while ( ! doneOutput ) {
00467             int now = timer.read_us();
00468             int c = 0;
00469             if ( now >= pwm[0] ) { motorPin1 = 0; c++; }
00470             if ( now >= pwm[1] ) { motorPin2 = 0; c++; }
00471             if ( now >= pwm[2] ) { motorPin3 = 0; c++; }
00472             if ( now >= pwm[3] ) { motorPin4 = 0; c++; }
00473             if ( now >= pwm[4] ) { motorPin5 = 0; c++; }
00474             if ( now >= pwm[5] ) { motorPin6 = 0; c++; }
00475             doneOutput = c >= 6;
00476         }
00477         
00478     }
00479     
00480     /*while (true) {
00481         pc.printf("%d, %d, %d, %d, %d, %d\n", ppm.channels[0], ppm.channels[1], ppm.channels[2], ppm.channels[3], ppm.channels[4], ppm.channels[5]);
00482     }*/
00483 }