main.cpp

00001 // Calibrate and arm ESC and then sweep through motor speed range
00002 // To run: Hold down reset on mbed and power up ESC motor supply
00003 // Wait for three power up beeps from ESC, then release reset (or apply power) on mbed
00004 // See https://github.com/bitdump/BLHeli/blob/master/BLHeli_S%20SiLabs/BLHeli_S%20manual%20SiLabs%20Rev16.x.pdf
00005 // for info on beep codes and calibration
00006 #include "mbed.h"
00007 #include "Servo.h"
00008 #include "MPU6050.h"
00009 #include "PID.h"
00010 
00011 PwmOut ledf(LED1); //throttle up test led with PWM dimming
00012 PwmOut ledr(LED2); //throttle down test led with PWM dimming
00013 
00014 Servo ESC0(p21); //Back  Left
00015 Servo ESC1(p22); //Front Left
00016 Servo ESC2(p24); //Front Right
00017 Servo ESC3(p23); //Back  Right
00018 
00019 uint16_t motor_min = 1100; //Minimum motor speed
00020 
00021 DigitalOut led1(LED1);
00022 void Rx_interrupt(); //Establish Reciever interupt
00023 
00024 Serial pc(USBTX, USBRX); //Initalise PC serial comms
00025 Serial Receiver(p13, p14); //Initialize Receiver Serial comms
00026 MPU6050 mpu6050;           // class: MPU6050, object: mpu6050
00027 Ticker toggler1;
00028 
00029 float pitchAngle = 0;
00030 float rollAngle = 0;
00031 float yawAngle = 0;
00032 int arm = 0;
00033 uint16_t pitchTarget = 0;
00034 uint16_t rollTarget = 0;
00035 //float yawTarget = 0;
00036 float pitchOffset = 0;
00037 float rollOffset = 0;
00038 
00039 PID PitchPID(0.5,0,0.05);
00040 PID RollPID(0.5,0,0.05);
00041 PID YawPID(2,0.01,0);
00042 
00043 float Throttle = 0;
00044 
00045 // Receiver variables
00046 int16_t channel[14];
00047 
00048 float pid_setpoint_roll = 0;
00049 float pid_setpoint_pitch = 0;
00050 float pid_setpoint_yaw = 0;
00051 float pid_output_roll = 0;
00052 float pid_output_pitch = 0;
00053 float pid_output_yaw = 0;
00054 
00055 int16_t gr[3];
00056 int16_t acc[3];
00057 int16_t grC[3];
00058 int16_t accC[3];
00059 
00060 Timer t;
00061 Timer loop_time; //Loop timer initialisation
00062 Timer re;
00063 
00064 void Calculate_set_points(){
00065     
00066     // Throttle- CHannel[2]
00067     if (channel[2] > 1800){Throttle = 1800;}
00068     else{Throttle = channel[2];}
00069     // Roll - Y - Channel[0]
00070     if (channel[0] != 0){
00071         if (channel[0] > 1505){pid_setpoint_roll =  channel[0] - 1505;}     // max 495
00072         if (channel[0] < 1495){pid_setpoint_roll =  channel[0] - 1495;}     // max -495
00073         pid_setpoint_roll /= 1; // Max angular rate of 247 deg/s
00074     }
00075     else {
00076         pid_setpoint_roll = 0;
00077     }
00078     
00079     // Pitch - X - Channel[1]
00080     if (channel[1] != 0){
00081         if (channel[1] > 1505){pid_setpoint_pitch = 1505 - channel[1];}     // max 495
00082         if (channel[1] < 1495){pid_setpoint_pitch =  1495 - channel[1];}    // max -495
00083         pid_setpoint_pitch /= 1;    // Max angular rate of 247 deg/s
00084     }
00085     else {
00086         pid_setpoint_roll = 0;
00087     }
00088     
00089     //Yaw - Z - Channel[3]
00090     if (channel[3] != 0){
00091         if (channel[3] > 1505){pid_setpoint_yaw = 1505 - channel[3];} // max 495
00092         if (channel[3] < 1495){pid_setpoint_yaw = 1495 - channel[3];} // max -495
00093         pid_setpoint_roll /= 1;     // Max angular rate of 247 deg/s
00094     }
00095     else{
00096         pid_setpoint_yaw = 0;
00097         }
00098         
00099     //pc.printf("Pset: %.1f, Rset %.1f, Yset %.1f ",pid_setpoint_roll, pid_setpoint_roll, pid_setpoint_roll); 
00100 }
00101 
00102 // Interupt Routine to read in data from serial port
00103 void Rx_interrupt(){
00104     uint8_t byte1;
00105     uint8_t high_byte;
00106     uint8_t chksum_low_byte;
00107     uint8_t chksum_high_byte;
00108     if (Receiver.readable()) {
00109             if(Receiver.getc() == 0x20) {
00110                     if(Receiver.getc() == 0x40){
00111                     for (int i = 0; i<7; i++) {
00112                         byte1 = Receiver.getc();
00113                         //    printf("Throttle: %02X %02X Sum: %d \n\r", (byte1&0x000000FF), byte2, channel[i]);
00114                         channel[i] = (Receiver.getc() << 8);
00115                         channel[i] += (byte1&0x000000FF);
00116                     }
00117                     chksum_low_byte = Receiver.getc();
00118                     chksum_high_byte = Receiver.getc();
00119                     //pc.printf("CLB %u,CHB %u \n\r", chksum_low_byte, chksum_high_byte);
00120                     //pc.printf("CN1: %u, CN2: %u, CN3: %u \n\r", channel[1], channel[2], channel[3]);
00121                     }
00122             }
00123         }
00124 }
00125 
00126 void UpdateMotors(float Thrust, float Yaw, float Pitch, float Roll)
00127 {
00128     // ESC0 |  Back Left   | CCW
00129     // ESC1 |  Front Left  | CW
00130     // ESC2 |  Front Right | CCW
00131     // ESC3 |  Back Right  | CW
00132 
00133 
00134     //Back Left
00135     float SC0 = 0;
00136     SC0 = Thrust - Yaw - Pitch + Roll;
00137     if(SC0 < motor_min) {
00138         SC0 =  motor_min;
00139     }
00140     if(SC0 > 2000) {
00141         SC0 = 2000;
00142     }
00143     float NSC0 = (SC0-1003)/1003;
00144     ESC0 = NSC0;
00145 
00146     //Front Left
00147     float SC1 = Thrust + Yaw + Pitch + Roll;
00148     if(SC1 < motor_min) {
00149         SC1 =  motor_min;
00150     }
00151     if(SC1 > 2000) {
00152         SC1 = 2000;
00153     }
00154     float NSC1 = (SC1-1003)/1003;
00155     ESC1 = NSC1;
00156 
00157     //Front Right
00158     float SC2 = Thrust - Yaw + Pitch - Roll;
00159     if(SC2 < motor_min) {
00160         SC2 =  motor_min;
00161     }
00162     if(SC2 > 2000) {
00163         SC2 = 2000;
00164     }
00165     float NSC2 = (SC2-1003)/1003;
00166     ESC2 = NSC2;
00167 
00168     //Back Right
00169     float SC3 = Thrust + Yaw - Pitch - Roll;
00170     if(SC3 < motor_min) {
00171         SC3 =  motor_min;
00172     }
00173     if(SC3 > 2000) {
00174         SC3 = 2000;
00175     }
00176     float NSC3 = (SC3-1003)/1003;
00177     ESC3 = NSC3;
00178 
00179     //pc.printf("BL:%.3f, FL:%.3f, FR:%.3f, BR:%.3f ", SC0, SC1, SC2, SC3);
00180     //pc.printf("NBL:%.3f, NFL:%.3f, NFR:%.3f, NBR:%.3f ", NSC0, NSC1, NSC2, NSC3);
00181 }
00182 
00183 int main()
00184 {
00185     // Setup a serial interrupt function to receive data
00186     led1 = 1; //Turn on LED during calibration
00187     Receiver.baud(115200);
00188     pc.baud (115200);
00189     Receiver.attach(&Rx_interrupt); //Set up reciever serial bus interupt
00190 
00191     for (int c = 0; c<14; c++) {
00192         channel[c] = 0; //Reset all channels to 0 at initialisation
00193     }
00194     //Arm motors
00195     ESC0 = ESC1 = ESC2 = ESC3 = 0;
00196     wait(0.5);
00197     ESC0 = ESC1 = ESC2 = ESC3 = 1100;
00198     wait(0.5);
00199     ESC0 = ESC1 = ESC2 = ESC3 = 0;
00200     wait(0.5);
00201     
00202     // Check and calibrate IMU
00203     mpu6050.whoAmI(); // Communication test: WHO_AM_I register reading
00204     wait(1);
00205     mpu6050.init();    
00206     wait(0.5);                         // Initialize the sensor
00207     mpu6050.calibrate(accelBias,gyroBias);    // Calibrate MPU6050 and load biases into bias registers
00208     //pc.printf("Calibration is completed. \r\n");
00209     wait(1);
00210     //pc.printf("MPU6050 is initialized for operation.. \r\n\r\n");
00211     wait_ms(500);
00212     
00213     //pc.printf("%f ,%f",Roll Angle, pitchAngle)
00214 
00215     // Set PID limits
00216     PitchPID.setLimit(-250,250);
00217     RollPID.setLimit(-250,250);
00218     YawPID.setLimit(-250,250);
00219     //YawPID.setLimit(-0.25,0.25);
00220 
00221     //pc.printf("Ready to go...\n\r");
00222     led1 = 0; //Turn LED off after calibration
00223 
00224 
00225 
00226     while (1) {
00227         loop_time.start(); //Start loop timer
00228         t.start();
00229         
00230         // TO ADD
00231         // Timer timeout for reciving new commands, if after 10 s no new commands cut motor output
00232 // AUTO LEVEL 
00233         // Read MPU Values with complimantary filter allready applied
00234         //mpu6050.complementaryFilter(&pitchAngle, &rollAngle, &yawAngle);
00235         //pc.printf("Pitch: %.3f, Roll: %.3f, Yaw: %.3f \n\r", pitchAngle, rollAngle, yawAngle);
00236         
00237 // RATE MODE
00238         //Read IMU data
00239         mpu6050.readAccelData(acc);
00240         mpu6050.readGyroData(gr);
00241         //Apply bias'
00242         accC[0] = ((acc[0]-accelBias[0])/16384);
00243         grC[0] = ((gr[0]-gyroBias[0])/65.536);
00244         
00245         accC[1] = ((acc[1]-accelBias[1])/16384);
00246         grC[1] = ((gr[1]-gyroBias[1])/65.536);
00247         
00248         accC[2] = ((acc[2]-accelBias[2])/16384);
00249         grC[2] = ((gr[2]-gyroBias[2])/65.536)*-1;
00250         
00251         //Use gyro to calculate rate of change in angles
00252         pitchAngle =  (pitchAngle*0.7) + (grC[0]*0.3);
00253         rollAngle =  (rollAngle*0.7) + (grC[1]*0.3);
00254         yawAngle =  (yawAngle*0.7) + (grC[2]*0.3);
00255 
00256         //pc.printf("ac_x: %d ac_y %d ac_z %d gy_x %d gy_y %d gy_z %d  \r\n", accC[0], accC[1], accC[2], grC[0], grC[1], grC[2]);
00257         
00258         Calculate_set_points();//Calculate values to input to PID
00259 
00260         //pc.printf("T:%.1f R:%.1f P:%.1f ARM: %u \n\r",Throttle, pid_setpoint_roll, pid_setpoint_pitch, channel[6]);
00261         
00262         // Set PIDS
00263         pid_output_roll = RollPID.computeOutput(pid_setpoint_roll,rollAngle);
00264         pid_output_pitch = PitchPID.computeOutput(pid_setpoint_pitch,pitchAngle);
00265         pid_output_yaw = YawPID.computeOutput(pid_setpoint_yaw,yawAngle);
00266         //pc.printf("Radj: %.1f, Padj %.1f ",pid_output_roll, pid_output_pitch); 
00267         // Yaw angle currently constantly rises (Check MPU6050 complimentry filter function as it looks
00268         // like it doesnt apply the z axis config offset values)
00269         //yawAdjustment = YawPID.computeOutput(yawTarget,yawAngle);
00270         
00271         
00272         // Update Motor PWM if the arming switch is triggered
00273         if (channel[6] > 1600){UpdateMotors(Throttle,pid_output_yaw, pid_output_pitch, pid_output_roll);}
00274         else{
00275             ESC0 = 0;
00276             ESC1 = 0; 
00277             ESC2 = 0;
00278             ESC3 = 0;
00279         }
00280 
00281         t.stop(); //End loop timer
00282         //pc.printf("Loop time %f \n\r", t.read());
00283         if(t.read()*1000 > 3) { //If loop time is too long warn user
00284             //pc.printf("LOOP TIME TOO LONG!");
00285             led1 = 1;
00286         } else { //Otherwise ensure loop is consistantly timed
00287             wait_ms(3.04-t.read()*1000);
00288         }
00289         t.reset(); //Reset the loop timer
00290         loop_time.stop();
00291         //pc.printf("Loop time %f \n\r", loop_time.read());
00292         loop_time.reset();
00293     }
00294 }
00295