Karim Azzouz / Mbed 2 deprecated A-Quad

A quadcopter control Software (Still in development). achieved single axis stability!!!!! released for others benefit. if you'd like to help co-develop this code, then please let me know

Dependencies:   MovingAverageFilter MyI2C PID RC mbed-rtos mbed

Currently on hold, due to the fact that i don't own a RX/TX system

main.cpp@1:e08a4f517989, 2013-08-27 (annotated)

Committer:
KarimAzzouz
Date:
Tue Aug 27 09:38:49 2013 +0000
Revision:
1:e08a4f517989
Parent:
0:54b67cd15a5b 
initial commit, achieved single axis stability

Who changed what in which revision?

UserRevisionLine numberNew contents of line
KarimAzzouz 1:e08a4f517989 1 /*
KarimAzzouz 1:e08a4f517989 2 F +X
KarimAzzouz 1:e08a4f517989 3 | |
KarimAzzouz 1:e08a4f517989 4 | |
KarimAzzouz 1:e08a4f517989 5 L ------|------R -Y------|------+Y
KarimAzzouz 1:e08a4f517989 6 | |
KarimAzzouz 1:e08a4f517989 7 | |
KarimAzzouz 1:e08a4f517989 8 B -X
KarimAzzouz 1:e08a4f517989 9 */
KarimAzzouz 1:e08a4f517989 10 #include "mbed.h"
KarimAzzouz 0:54b67cd15a5b 11 #include "rtos.h"
KarimAzzouz 0:54b67cd15a5b 12 #include "GlobalDefines.h"
KarimAzzouz 1:e08a4f517989 13 #include "MPU6050.h"
KarimAzzouz 1:e08a4f517989 14 #include "BMP085.h"
KarimAzzouz 1:e08a4f517989 15 //#include "DCM.h" Drifts a lot, needs tuning
KarimAzzouz 1:e08a4f517989 16 #include "PID.h"
KarimAzzouz 0:54b67cd15a5b 17 #include "Motors.h"
KarimAzzouz 0:54b67cd15a5b 18
KarimAzzouz 1:e08a4f517989 19 #define Debugging
KarimAzzouz 1:e08a4f517989 20
KarimAzzouz 1:e08a4f517989 21 typedef enum{Armed=0,
KarimAzzouz 1:e08a4f517989 22 UnArmed
KarimAzzouz 1:e08a4f517989 23 }State;
KarimAzzouz 1:e08a4f517989 24
KarimAzzouz 1:e08a4f517989 25 State MotorState = Armed;
KarimAzzouz 1:e08a4f517989 26 bool print = false;
KarimAzzouz 1:e08a4f517989 27 bool P_readingReady=false;
KarimAzzouz 1:e08a4f517989 28 bool T_readingReady=false;
KarimAzzouz 1:e08a4f517989 29 char c ;
KarimAzzouz 0:54b67cd15a5b 30
KarimAzzouz 1:e08a4f517989 31 uint8 count=0;
KarimAzzouz 1:e08a4f517989 32 int16 Temperature=0;
KarimAzzouz 1:e08a4f517989 33 int32 Pressure=0;
KarimAzzouz 1:e08a4f517989 34
KarimAzzouz 0:54b67cd15a5b 35
KarimAzzouz 1:e08a4f517989 36 PIDClass PitchPID(4,5,2.5,0.1); // This is a nested PID loop
KarimAzzouz 1:e08a4f517989 37 // Kp,Ki,Kp,Kd,iLim..... best till now 4,0,2.5,0.1
KarimAzzouz 1:e08a4f517989 38 MPU6050 Sensor;
KarimAzzouz 1:e08a4f517989 39 BMP085 BMP;
KarimAzzouz 1:e08a4f517989 40 //DCM IMU;
KarimAzzouz 1:e08a4f517989 41
KarimAzzouz 1:e08a4f517989 42 void FlightControl(void const *args){
KarimAzzouz 0:54b67cd15a5b 43
KarimAzzouz 1:e08a4f517989 44 //Get Sensor Data
KarimAzzouz 1:e08a4f517989 45 Sensor.GyroRate();
KarimAzzouz 1:e08a4f517989 46 Sensor.AccAngle();
KarimAzzouz 1:e08a4f517989 47
KarimAzzouz 1:e08a4f517989 48 ///////////////////////BMP085 Contorol switch statement(Work in Progress!!!)//////////////////////////////////////
KarimAzzouz 1:e08a4f517989 49 switch(count){
KarimAzzouz 1:e08a4f517989 50
KarimAzzouz 1:e08a4f517989 51 case 0:
KarimAzzouz 1:e08a4f517989 52 BMP.readUT_Flag();
KarimAzzouz 1:e08a4f517989 53 count+=1;
KarimAzzouz 1:e08a4f517989 54 break;
KarimAzzouz 1:e08a4f517989 55
KarimAzzouz 1:e08a4f517989 56
KarimAzzouz 1:e08a4f517989 57 case 1:
KarimAzzouz 1:e08a4f517989 58 Temperature=BMP.read_Temperature();
KarimAzzouz 1:e08a4f517989 59 T_readingReady=true;
KarimAzzouz 1:e08a4f517989 60 BMP.readUP_Flag();
KarimAzzouz 1:e08a4f517989 61 count+=1;
KarimAzzouz 1:e08a4f517989 62 break;
KarimAzzouz 1:e08a4f517989 63
KarimAzzouz 1:e08a4f517989 64
KarimAzzouz 1:e08a4f517989 65 case 7:
KarimAzzouz 1:e08a4f517989 66 Pressure = BMP.read_Pressure();
KarimAzzouz 1:e08a4f517989 67 P_readingReady=true;
KarimAzzouz 1:e08a4f517989 68 count=0;
KarimAzzouz 1:e08a4f517989 69 break;
KarimAzzouz 1:e08a4f517989 70
KarimAzzouz 1:e08a4f517989 71 default: count+=1; break;
KarimAzzouz 1:e08a4f517989 72 }
KarimAzzouz 1:e08a4f517989 73 ///////////////////////////////////////////////////////////////////////////////////////////
KarimAzzouz 1:e08a4f517989 74
KarimAzzouz 1:e08a4f517989 75 //Gotta Keep Time
KarimAzzouz 1:e08a4f517989 76 deltaTime = Global.read();
KarimAzzouz 1:e08a4f517989 77 Global.reset();
KarimAzzouz 0:54b67cd15a5b 78
KarimAzzouz 1:e08a4f517989 79 pitch = (0.99)*(pitch+Sensor.Scaled_GyroX*deltaTime) + (0.01)*(Sensor.accangle[0]);
KarimAzzouz 1:e08a4f517989 80 roll = (0.99)*(roll+ Sensor.Scaled_GyroY*deltaTime) + (0.01)*(Sensor.accangle[1]);
KarimAzzouz 1:e08a4f517989 81
KarimAzzouz 1:e08a4f517989 82
KarimAzzouz 1:e08a4f517989 83 //IMU.Update_DCM(deltaTime,ToRad(Sensor.Scaled_GyroX),ToRad(Sensor.Scaled_GyroY),ToRad(Sensor.Scaled_GyroZ),Sensor.Scaled_AccX,Sensor.Scaled_AccY,Sensor.Scaled_AccZ);
KarimAzzouz 1:e08a4f517989 84
KarimAzzouz 1:e08a4f517989 85 //Update PID
KarimAzzouz 1:e08a4f517989 86 if(Throttle>=1200){
KarimAzzouz 1:e08a4f517989 87 PitchCorrection= PitchPID.update(PitchSetpoint,pitch,Sensor.Scaled_GyroX,deltaTime);
KarimAzzouz 1:e08a4f517989 88 }
KarimAzzouz 1:e08a4f517989 89
KarimAzzouz 1:e08a4f517989 90 //Motor Commands
KarimAzzouz 1:e08a4f517989 91 if(MotorState==Armed){
KarimAzzouz 1:e08a4f517989 92 updateMotors(PitchCorrection,0);
KarimAzzouz 1:e08a4f517989 93 }
KarimAzzouz 1:e08a4f517989 94
KarimAzzouz 1:e08a4f517989 95 print = true;
KarimAzzouz 0:54b67cd15a5b 96 }
KarimAzzouz 0:54b67cd15a5b 97
KarimAzzouz 1:e08a4f517989 98
KarimAzzouz 0:54b67cd15a5b 99 int main(){
KarimAzzouz 1:e08a4f517989 100
KarimAzzouz 1:e08a4f517989 101 #ifdef Debugging
KarimAzzouz 1:e08a4f517989 102 pc.baud(115200);
KarimAzzouz 1:e08a4f517989 103 #endif
KarimAzzouz 0:54b67cd15a5b 104
KarimAzzouz 1:e08a4f517989 105 //Start-Up functions
KarimAzzouz 1:e08a4f517989 106 __disable_irq();
KarimAzzouz 1:e08a4f517989 107 CalibrateESC(); // Throttle Sweep on startup
KarimAzzouz 1:e08a4f517989 108 two=1; // check sensor connection
KarimAzzouz 1:e08a4f517989 109 while(!Sensor.CheckConnection()); //check sensor connection
KarimAzzouz 1:e08a4f517989 110 two=0; // sensor okay
KarimAzzouz 1:e08a4f517989 111 BMP.Calibrate(); // Calibrations sequence for BMP... TODO: store values in LocalFileSystem
KarimAzzouz 1:e08a4f517989 112 Sensor.MPU_Setup(); // Setup MPU6050
KarimAzzouz 1:e08a4f517989 113 Sensor.CalibrateGyro(); // Calibrate Gyros (averaging 100 readings)
KarimAzzouz 1:e08a4f517989 114 Sensor.CalibrateAcc(); // Calibrate ACC (averaging 100 readings)
KarimAzzouz 1:e08a4f517989 115 one=1; // System okay to start
KarimAzzouz 1:e08a4f517989 116 __enable_irq();
KarimAzzouz 0:54b67cd15a5b 117
KarimAzzouz 1:e08a4f517989 118 RtosTimer Fly(FlightControl,osTimerPeriodic,NULL); // Initialize RTOS Timer
KarimAzzouz 1:e08a4f517989 119 Global.start(); // Start timer
KarimAzzouz 1:e08a4f517989 120 Fly.start(5); // RTOS Timer (200Hz Control Loop)
KarimAzzouz 0:54b67cd15a5b 121
KarimAzzouz 0:54b67cd15a5b 122 while(true){
KarimAzzouz 1:e08a4f517989 123
KarimAzzouz 1:e08a4f517989 124 if(pc.readable()){
KarimAzzouz 1:e08a4f517989 125 c = pc.getc();
KarimAzzouz 1:e08a4f517989 126 if(c == 'a'||c=='A') MotorState = Armed;
KarimAzzouz 1:e08a4f517989 127 else if(c == 's'||c=='S') MotorState = UnArmed;
KarimAzzouz 1:e08a4f517989 128 else if(c == 'u'||c=='U') Throttle+=50 ;
KarimAzzouz 1:e08a4f517989 129 else if(c == 'i'||c=='I') Throttle-=50;
KarimAzzouz 1:e08a4f517989 130 else if(c=='m'||c=='M') PitchSetpoint+=10;
KarimAzzouz 1:e08a4f517989 131 else if(c=='n'||c=='N') PitchSetpoint-=10;
KarimAzzouz 0:54b67cd15a5b 132
KarimAzzouz 1:e08a4f517989 133 //print=true;
KarimAzzouz 1:e08a4f517989 134 }
KarimAzzouz 1:e08a4f517989 135
KarimAzzouz 1:e08a4f517989 136 #ifdef Debugging
KarimAzzouz 1:e08a4f517989 137 if(print){
KarimAzzouz 1:e08a4f517989 138 //pc.printf("%d\n",Temperature);
KarimAzzouz 1:e08a4f517989 139 pc.printf("%6.2f\n",pitch);
KarimAzzouz 1:e08a4f517989 140 //T_readingReady=false;
KarimAzzouz 1:e08a4f517989 141 print=false;
KarimAzzouz 1:e08a4f517989 142 }
KarimAzzouz 1:e08a4f517989 143 #endif
KarimAzzouz 1:e08a4f517989 144
KarimAzzouz 1:e08a4f517989 145 Thread::wait(20);
KarimAzzouz 0:54b67cd15a5b 146 }
KarimAzzouz 0:54b67cd15a5b 147 }