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

MPU.h@0:54b67cd15a5b, 2012-12-23 (annotated)

Committer:
KarimAzzouz
Date:
Sun Dec 23 23:50:21 2012 +0000
Revision:
0:54b67cd15a5b
PPM interrupts clashing with i2c causing crashes!!

Who changed what in which revision?

UserRevisionLine numberNew contents of line
KarimAzzouz 0:54b67cd15a5b 1 // Contains all Sensor related functions : initializing the sensor, calibrating sensors, getting data , accelerometer angles , and filtering gyro/acc data
KarimAzzouz 0:54b67cd15a5b 2
KarimAzzouz 0:54b67cd15a5b 3 #define MPU_ADDRESS 0x68
KarimAzzouz 0:54b67cd15a5b 4 #define GyroScale 16.4
KarimAzzouz 0:54b67cd15a5b 5 #define AccScale 4096
KarimAzzouz 0:54b67cd15a5b 6
KarimAzzouz 0:54b67cd15a5b 7 I2C i2c(p9,p10);
KarimAzzouz 0:54b67cd15a5b 8
KarimAzzouz 0:54b67cd15a5b 9 float filtered_AccX[4],filtered_AccY[4],filtered_AccZ[4],filtered_GyroX[4],filtered_GyroY[4],filtered_GyroZ[4];
KarimAzzouz 0:54b67cd15a5b 10 float Scaled_GyroX,Scaled_GyroY,Scaled_GyroZ;
KarimAzzouz 0:54b67cd15a5b 11 float Scaled_AccX,Scaled_AccY,Scaled_AccZ;
KarimAzzouz 0:54b67cd15a5b 12 float GyroOffset[3],AccOffset[3];
KarimAzzouz 0:54b67cd15a5b 13 float accangle[2];
KarimAzzouz 0:54b67cd15a5b 14
KarimAzzouz 0:54b67cd15a5b 15 void write(char reg,char data){
KarimAzzouz 0:54b67cd15a5b 16
KarimAzzouz 0:54b67cd15a5b 17 char tx[2]={reg,data};
KarimAzzouz 0:54b67cd15a5b 18
KarimAzzouz 0:54b67cd15a5b 19 i2c.write((MPU_ADDRESS << 1)&0xFE, tx, 2);
KarimAzzouz 0:54b67cd15a5b 20
KarimAzzouz 0:54b67cd15a5b 21 }
KarimAzzouz 0:54b67cd15a5b 22
KarimAzzouz 0:54b67cd15a5b 23 int read (char reg){
KarimAzzouz 0:54b67cd15a5b 24
KarimAzzouz 0:54b67cd15a5b 25 char tx = reg;
KarimAzzouz 0:54b67cd15a5b 26 char rx[2];
KarimAzzouz 0:54b67cd15a5b 27
KarimAzzouz 0:54b67cd15a5b 28 i2c.write((MPU_ADDRESS << 1)&0xFE , &tx,1);
KarimAzzouz 0:54b67cd15a5b 29
KarimAzzouz 0:54b67cd15a5b 30 i2c.read((MPU_ADDRESS << 1)|0x01, rx, 2);
KarimAzzouz 0:54b67cd15a5b 31
KarimAzzouz 0:54b67cd15a5b 32
KarimAzzouz 0:54b67cd15a5b 33 int16_t output = ((int) rx[0] << 8) | ((int) rx[1]);
KarimAzzouz 0:54b67cd15a5b 34
KarimAzzouz 0:54b67cd15a5b 35 return output;
KarimAzzouz 0:54b67cd15a5b 36 }
KarimAzzouz 0:54b67cd15a5b 37
KarimAzzouz 0:54b67cd15a5b 38 void MPU_Setup(){
KarimAzzouz 0:54b67cd15a5b 39
KarimAzzouz 0:54b67cd15a5b 40
KarimAzzouz 0:54b67cd15a5b 41 write(0x6B,0x80); // Restart
KarimAzzouz 0:54b67cd15a5b 42 wait_ms(5);
KarimAzzouz 0:54b67cd15a5b 43 write(0x6B,0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 1 (PLL with Z Gyro reference)
KarimAzzouz 0:54b67cd15a5b 44 write(0x1B,0x18); //GYRO_CONFIG -- FS_SEL = 2 : Scale = +/-2000 deg/sec
KarimAzzouz 0:54b67cd15a5b 45 write(0x1C,0x10); //ACCEL_CONFIG -- AFS_SEL=2 : Scale = +/-8G
KarimAzzouz 0:54b67cd15a5b 46 write(0x1A,0x03); //DLPF Settings -- 44Hz Accelerometer Bandwidth 42Hz Gyroscope Bandwidth
KarimAzzouz 0:54b67cd15a5b 47 wait_ms(60);
KarimAzzouz 0:54b67cd15a5b 48 }
KarimAzzouz 0:54b67cd15a5b 49
KarimAzzouz 0:54b67cd15a5b 50 void ScaledGyro(){
KarimAzzouz 0:54b67cd15a5b 51
KarimAzzouz 0:54b67cd15a5b 52 Scaled_GyroX = -read(0x45)/GyroScale;
KarimAzzouz 0:54b67cd15a5b 53 Scaled_GyroY = read(0x43)/GyroScale;
KarimAzzouz 0:54b67cd15a5b 54 Scaled_GyroZ = read(0x47)/GyroScale;
KarimAzzouz 0:54b67cd15a5b 55 }
KarimAzzouz 0:54b67cd15a5b 56
KarimAzzouz 0:54b67cd15a5b 57 void ScaledAcc(){
KarimAzzouz 0:54b67cd15a5b 58
KarimAzzouz 0:54b67cd15a5b 59 Scaled_AccX = (float)read(0x3B)/AccScale;
KarimAzzouz 0:54b67cd15a5b 60 Scaled_AccY = (float)read(0x3D)/AccScale;
KarimAzzouz 0:54b67cd15a5b 61 Scaled_AccZ = (float)read(0x3F)/AccScale;
KarimAzzouz 0:54b67cd15a5b 62 }
KarimAzzouz 0:54b67cd15a5b 63
KarimAzzouz 0:54b67cd15a5b 64 void CalibrateGyro(){
KarimAzzouz 0:54b67cd15a5b 65
KarimAzzouz 0:54b67cd15a5b 66 for(int k=0;k<200;k++){
KarimAzzouz 0:54b67cd15a5b 67 ScaledGyro();
KarimAzzouz 0:54b67cd15a5b 68 GyroOffset[0]+=Scaled_GyroX;
KarimAzzouz 0:54b67cd15a5b 69 GyroOffset[1]+=Scaled_GyroY;
KarimAzzouz 0:54b67cd15a5b 70 GyroOffset[2]+=Scaled_GyroZ;
KarimAzzouz 0:54b67cd15a5b 71 wait_ms(1);
KarimAzzouz 0:54b67cd15a5b 72 }
KarimAzzouz 0:54b67cd15a5b 73 GyroOffset[0]/=200;
KarimAzzouz 0:54b67cd15a5b 74 GyroOffset[1]/=200;
KarimAzzouz 0:54b67cd15a5b 75 GyroOffset[2]/=200;
KarimAzzouz 0:54b67cd15a5b 76 }
KarimAzzouz 0:54b67cd15a5b 77
KarimAzzouz 0:54b67cd15a5b 78 void CalibrateAcc(){
KarimAzzouz 0:54b67cd15a5b 79
KarimAzzouz 0:54b67cd15a5b 80 for(int k=0;k<200;k++){
KarimAzzouz 0:54b67cd15a5b 81 ScaledAcc();
KarimAzzouz 0:54b67cd15a5b 82 AccOffset[0]+=Scaled_AccX;
KarimAzzouz 0:54b67cd15a5b 83 AccOffset[1]+=Scaled_AccY;
KarimAzzouz 0:54b67cd15a5b 84 AccOffset[2]+=Scaled_AccZ;
KarimAzzouz 0:54b67cd15a5b 85 wait_ms(1);
KarimAzzouz 0:54b67cd15a5b 86 }
KarimAzzouz 0:54b67cd15a5b 87 AccOffset[0]/=200;
KarimAzzouz 0:54b67cd15a5b 88 AccOffset[1]/=200;
KarimAzzouz 0:54b67cd15a5b 89 AccOffset[2]/=200;
KarimAzzouz 0:54b67cd15a5b 90 AccOffset[2]-=1;
KarimAzzouz 0:54b67cd15a5b 91 }
KarimAzzouz 0:54b67cd15a5b 92
KarimAzzouz 0:54b67cd15a5b 93 void GyroRate(){
KarimAzzouz 0:54b67cd15a5b 94
KarimAzzouz 0:54b67cd15a5b 95 ScaledGyro();
KarimAzzouz 0:54b67cd15a5b 96 Scaled_GyroX-=GyroOffset[0];
KarimAzzouz 0:54b67cd15a5b 97 Scaled_GyroY-=GyroOffset[1];
KarimAzzouz 0:54b67cd15a5b 98 Scaled_GyroZ-=GyroOffset[2];
KarimAzzouz 0:54b67cd15a5b 99 }
KarimAzzouz 0:54b67cd15a5b 100
KarimAzzouz 0:54b67cd15a5b 101 void Acc(){
KarimAzzouz 0:54b67cd15a5b 102
KarimAzzouz 0:54b67cd15a5b 103 ScaledAcc();
KarimAzzouz 0:54b67cd15a5b 104 Scaled_AccX-=AccOffset[0];
KarimAzzouz 0:54b67cd15a5b 105 Scaled_AccY-=AccOffset[1];
KarimAzzouz 0:54b67cd15a5b 106 Scaled_AccZ-=AccOffset[2];
KarimAzzouz 0:54b67cd15a5b 107 }
KarimAzzouz 0:54b67cd15a5b 108 void filterGyro(void){
KarimAzzouz 0:54b67cd15a5b 109
KarimAzzouz 0:54b67cd15a5b 110 GyroRate();
KarimAzzouz 0:54b67cd15a5b 111
KarimAzzouz 0:54b67cd15a5b 112 //////////////////////////////////////// Filter X Axis ////////////////////////////////////////////////////////////////
KarimAzzouz 0:54b67cd15a5b 113 filtered_GyroX[0] = Scaled_GyroX;
KarimAzzouz 0:54b67cd15a5b 114 Scaled_GyroX = filtered_GyroX[0]*0.25 + filtered_GyroX[1]*0.25 + filtered_GyroX[2]*0.25 + filtered_GyroX[3]*0.25;
KarimAzzouz 0:54b67cd15a5b 115 filtered_GyroX[3] = filtered_GyroX[2];
KarimAzzouz 0:54b67cd15a5b 116 filtered_GyroX[2] = filtered_GyroX[1];
KarimAzzouz 0:54b67cd15a5b 117 filtered_GyroX[1] = filtered_GyroX[0];
KarimAzzouz 0:54b67cd15a5b 118 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
KarimAzzouz 0:54b67cd15a5b 119
KarimAzzouz 0:54b67cd15a5b 120 /////////////////////////////////////// Filter Y Axis ////////////////////////////////////////////////////////////////
KarimAzzouz 0:54b67cd15a5b 121 filtered_GyroY[0] = Scaled_GyroY;
KarimAzzouz 0:54b67cd15a5b 122 Scaled_GyroY = filtered_GyroY[0]*0.25 + filtered_GyroY[1]*0.25 + filtered_GyroY[2]*0.25 + filtered_GyroY[3]*0.25;
KarimAzzouz 0:54b67cd15a5b 123 filtered_GyroY[3] = filtered_GyroY[2];
KarimAzzouz 0:54b67cd15a5b 124 filtered_GyroY[2] = filtered_GyroY[1];
KarimAzzouz 0:54b67cd15a5b 125 filtered_GyroY[1] = filtered_GyroY[0];
KarimAzzouz 0:54b67cd15a5b 126 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
KarimAzzouz 0:54b67cd15a5b 127
KarimAzzouz 0:54b67cd15a5b 128 /////////////////////////////////////// Filter Z Axis ////////////////////////////////////////////////////////////////
KarimAzzouz 0:54b67cd15a5b 129 filtered_GyroZ[0] = Scaled_GyroZ;
KarimAzzouz 0:54b67cd15a5b 130 Scaled_GyroZ = filtered_GyroZ[0]*0.25 + filtered_GyroZ[1]*0.25 + filtered_GyroZ[2]*0.25 + filtered_GyroZ[3]*0.25;
KarimAzzouz 0:54b67cd15a5b 131 filtered_GyroZ[3] = filtered_GyroZ[2];
KarimAzzouz 0:54b67cd15a5b 132 filtered_GyroZ[2] = filtered_GyroZ[1];
KarimAzzouz 0:54b67cd15a5b 133 filtered_GyroZ[1] = filtered_GyroZ[0];
KarimAzzouz 0:54b67cd15a5b 134 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
KarimAzzouz 0:54b67cd15a5b 135 }
KarimAzzouz 0:54b67cd15a5b 136
KarimAzzouz 0:54b67cd15a5b 137
KarimAzzouz 0:54b67cd15a5b 138 void filterAcc(void){
KarimAzzouz 0:54b67cd15a5b 139
KarimAzzouz 0:54b67cd15a5b 140 Acc();
KarimAzzouz 0:54b67cd15a5b 141
KarimAzzouz 0:54b67cd15a5b 142 //////////////////////////////////////// Filter X Axis///////////////////////////////////////////////////////////
KarimAzzouz 0:54b67cd15a5b 143 filtered_AccX[0] = Scaled_AccX;
KarimAzzouz 0:54b67cd15a5b 144 Scaled_AccX = filtered_AccX[0]*0.25 + filtered_AccX[1]*0.25 + filtered_AccX[2]*0.25 + filtered_AccX[3]*0.25;
KarimAzzouz 0:54b67cd15a5b 145 filtered_AccX[3] = filtered_AccX[2];
KarimAzzouz 0:54b67cd15a5b 146 filtered_AccX[2] = filtered_AccX[1];
KarimAzzouz 0:54b67cd15a5b 147 filtered_AccX[1] = filtered_AccX[0];
KarimAzzouz 0:54b67cd15a5b 148 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////
KarimAzzouz 0:54b67cd15a5b 149
KarimAzzouz 0:54b67cd15a5b 150 //////////////////////////////////////// Filter Y Axis //////////////////////////////////////////////////////////
KarimAzzouz 0:54b67cd15a5b 151 filtered_AccY[0] = Scaled_AccY;
KarimAzzouz 0:54b67cd15a5b 152 Scaled_AccY = filtered_AccY[0]*0.25 + filtered_AccY[1]*0.25 + filtered_AccY[2]*0.25 + filtered_AccY[3]*0.25;
KarimAzzouz 0:54b67cd15a5b 153 filtered_AccY[3] = filtered_AccY[2];
KarimAzzouz 0:54b67cd15a5b 154 filtered_AccY[2] = filtered_AccY[1];
KarimAzzouz 0:54b67cd15a5b 155 filtered_AccY[1] = filtered_AccY[0];
KarimAzzouz 0:54b67cd15a5b 156 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////
KarimAzzouz 0:54b67cd15a5b 157
KarimAzzouz 0:54b67cd15a5b 158 //////////////////////////////////////// Filter Z Axis//////////////////////////////////////////////////////////
KarimAzzouz 0:54b67cd15a5b 159 filtered_AccZ[0] = Scaled_AccZ;
KarimAzzouz 0:54b67cd15a5b 160 Scaled_AccZ = filtered_AccZ[0]*0.25 + filtered_AccZ[1]*0.25 + filtered_AccZ[2]*0.25 + filtered_AccZ[3]*0.25;
KarimAzzouz 0:54b67cd15a5b 161 filtered_AccZ[3] = filtered_AccZ[2];
KarimAzzouz 0:54b67cd15a5b 162 filtered_AccZ[2] = filtered_AccZ[1];
KarimAzzouz 0:54b67cd15a5b 163 filtered_AccZ[1] = filtered_AccZ[0];
KarimAzzouz 0:54b67cd15a5b 164 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
KarimAzzouz 0:54b67cd15a5b 165 }
KarimAzzouz 0:54b67cd15a5b 166
KarimAzzouz 0:54b67cd15a5b 167 void AccAngle(){
KarimAzzouz 0:54b67cd15a5b 168
KarimAzzouz 0:54b67cd15a5b 169 filterAcc();
KarimAzzouz 0:54b67cd15a5b 170
KarimAzzouz 0:54b67cd15a5b 171 accangle[0] = ToDeg(atan2(Scaled_AccX,sqrt(Scaled_AccY*Scaled_AccY+Scaled_AccZ*Scaled_AccZ)));
KarimAzzouz 0:54b67cd15a5b 172 accangle[1] = ToDeg(atan2(Scaled_AccY,sqrt(Scaled_AccX*Scaled_AccX+Scaled_AccZ*Scaled_AccZ)));
KarimAzzouz 0:54b67cd15a5b 173 }