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/MPU6050.cpp@1:e08a4f517989, 2013-08-27 (annotated)

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

Who changed what in which revision?

UserRevisionLine numberNew contents of line
KarimAzzouz 1:e08a4f517989 1 #include "MPU6050.h"
KarimAzzouz 1:e08a4f517989 2
KarimAzzouz 1:e08a4f517989 3 MPU6050::MPU6050(){
KarimAzzouz 1:e08a4f517989 4
KarimAzzouz 1:e08a4f517989 5 Panic=false;
KarimAzzouz 1:e08a4f517989 6 }
KarimAzzouz 1:e08a4f517989 7
KarimAzzouz 1:e08a4f517989 8 void MPU6050::write(char reg,char data){
KarimAzzouz 1:e08a4f517989 9
KarimAzzouz 1:e08a4f517989 10 char tx[2]={reg,data};
KarimAzzouz 1:e08a4f517989 11
KarimAzzouz 1:e08a4f517989 12 I2C0.blockwrite((MPU_ADDRESS << 1)&0xFE, tx, 2);
KarimAzzouz 1:e08a4f517989 13
KarimAzzouz 1:e08a4f517989 14 }
KarimAzzouz 1:e08a4f517989 15
KarimAzzouz 1:e08a4f517989 16 int MPU6050::read (char reg){
KarimAzzouz 1:e08a4f517989 17
KarimAzzouz 1:e08a4f517989 18 char tx = reg;
KarimAzzouz 1:e08a4f517989 19 char rx[2];
KarimAzzouz 1:e08a4f517989 20
KarimAzzouz 1:e08a4f517989 21 I2C0.blockwrite((MPU_ADDRESS << 1)&0xFE, &tx, 1);
KarimAzzouz 1:e08a4f517989 22
KarimAzzouz 1:e08a4f517989 23 I2C0.blockread((MPU_ADDRESS << 1)|0x01, rx, 2);
KarimAzzouz 1:e08a4f517989 24
KarimAzzouz 1:e08a4f517989 25 int16_t output = ((int) rx[0] << 8) | ((int) rx[1]);
KarimAzzouz 1:e08a4f517989 26 return output;
KarimAzzouz 1:e08a4f517989 27 }
KarimAzzouz 1:e08a4f517989 28
KarimAzzouz 1:e08a4f517989 29 bool MPU6050::CheckConnection(void){
KarimAzzouz 1:e08a4f517989 30
KarimAzzouz 1:e08a4f517989 31 char tx = 0x75; // Who Am I Register
KarimAzzouz 1:e08a4f517989 32 char rx;
KarimAzzouz 1:e08a4f517989 33
KarimAzzouz 1:e08a4f517989 34 I2C0.blockwrite((MPU_ADDRESS << 1)&0xFE, &tx, 1);
KarimAzzouz 1:e08a4f517989 35
KarimAzzouz 1:e08a4f517989 36 I2C0.blockread((MPU_ADDRESS << 1)|0x01, &rx, 1);
KarimAzzouz 1:e08a4f517989 37
KarimAzzouz 1:e08a4f517989 38 if(rx==0x68) return true;
KarimAzzouz 1:e08a4f517989 39
KarimAzzouz 1:e08a4f517989 40 else return false ;
KarimAzzouz 1:e08a4f517989 41 }
KarimAzzouz 1:e08a4f517989 42
KarimAzzouz 1:e08a4f517989 43 void MPU6050::MPU_Setup(void){
KarimAzzouz 1:e08a4f517989 44
KarimAzzouz 1:e08a4f517989 45 write(0x6B,0x80); // Restart
KarimAzzouz 1:e08a4f517989 46 wait_ms(5);
KarimAzzouz 1:e08a4f517989 47 write(0x6B,0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
KarimAzzouz 1:e08a4f517989 48 write(0x1B,0x18); //GYRO_CONFIG -- FS_SEL =3 : Scale = +/-2000 deg/sec
KarimAzzouz 1:e08a4f517989 49 write(0x1C,0x10); //ACCEL_CONFIG -- AFS_SEL= 2 : Scale = +/-8G
KarimAzzouz 1:e08a4f517989 50 write(0x1A,0x03); //DLPF Settings -- 44Hz Accelerometer Bandwidth 42Hz Gyroscope Bandwidth
KarimAzzouz 1:e08a4f517989 51 wait_ms(60);
KarimAzzouz 1:e08a4f517989 52 }
KarimAzzouz 1:e08a4f517989 53
KarimAzzouz 1:e08a4f517989 54 void MPU6050::ScaledGyro(void){
KarimAzzouz 1:e08a4f517989 55
KarimAzzouz 1:e08a4f517989 56 Scaled_GyroX = -(float)read(0x45)/GyroScale;
KarimAzzouz 1:e08a4f517989 57 Scaled_GyroY = (float)read(0x43)/GyroScale;
KarimAzzouz 1:e08a4f517989 58 Scaled_GyroZ = (float)read(0x47)/GyroScale;
KarimAzzouz 1:e08a4f517989 59 }
KarimAzzouz 1:e08a4f517989 60
KarimAzzouz 1:e08a4f517989 61 void MPU6050::ScaledAcc(void){
KarimAzzouz 1:e08a4f517989 62
KarimAzzouz 1:e08a4f517989 63 Scaled_AccX = (float)read(0x3B)/AccScale;
KarimAzzouz 1:e08a4f517989 64 Scaled_AccY = (float)read(0x3D)/AccScale;
KarimAzzouz 1:e08a4f517989 65 Scaled_AccZ = (float)read(0x3F)/AccScale;
KarimAzzouz 1:e08a4f517989 66 }
KarimAzzouz 1:e08a4f517989 67
KarimAzzouz 1:e08a4f517989 68 void MPU6050::RawAcc(void){
KarimAzzouz 1:e08a4f517989 69
KarimAzzouz 1:e08a4f517989 70 Raw_AccX = read(0x3B);
KarimAzzouz 1:e08a4f517989 71 Raw_AccY = read(0x3D);
KarimAzzouz 1:e08a4f517989 72 Raw_AccZ = read(0x3F);
KarimAzzouz 1:e08a4f517989 73 }
KarimAzzouz 1:e08a4f517989 74
KarimAzzouz 1:e08a4f517989 75 void MPU6050::CalibrateGyro(void){
KarimAzzouz 1:e08a4f517989 76
KarimAzzouz 1:e08a4f517989 77 for(int k=0;k<100;k++){
KarimAzzouz 1:e08a4f517989 78 ScaledGyro();
KarimAzzouz 1:e08a4f517989 79 GyroOffset[0]+=Scaled_GyroX;
KarimAzzouz 1:e08a4f517989 80 GyroOffset[1]+=Scaled_GyroY;
KarimAzzouz 1:e08a4f517989 81 GyroOffset[2]+=Scaled_GyroZ;
KarimAzzouz 1:e08a4f517989 82 wait_ms(5);
KarimAzzouz 1:e08a4f517989 83 }
KarimAzzouz 1:e08a4f517989 84 GyroOffset[0]/=100;
KarimAzzouz 1:e08a4f517989 85 GyroOffset[1]/=100;
KarimAzzouz 1:e08a4f517989 86 GyroOffset[2]/=100;
KarimAzzouz 1:e08a4f517989 87
KarimAzzouz 1:e08a4f517989 88 }
KarimAzzouz 1:e08a4f517989 89
KarimAzzouz 1:e08a4f517989 90 void MPU6050::CalibrateAcc(void){
KarimAzzouz 1:e08a4f517989 91
KarimAzzouz 1:e08a4f517989 92 for(int k=0;k<100;k++){
KarimAzzouz 1:e08a4f517989 93 ScaledAcc();
KarimAzzouz 1:e08a4f517989 94 AccOffset[0]+=Scaled_AccX;
KarimAzzouz 1:e08a4f517989 95 AccOffset[1]+=Scaled_AccY;
KarimAzzouz 1:e08a4f517989 96 AccOffset[2]+=Scaled_AccZ;
KarimAzzouz 1:e08a4f517989 97 wait_ms(5);
KarimAzzouz 1:e08a4f517989 98 }
KarimAzzouz 1:e08a4f517989 99 AccOffset[0]/=100;
KarimAzzouz 1:e08a4f517989 100 AccOffset[1]/=100;
KarimAzzouz 1:e08a4f517989 101 AccOffset[2]/=100;
KarimAzzouz 1:e08a4f517989 102 AccOffset[2]-=1;
KarimAzzouz 1:e08a4f517989 103
KarimAzzouz 1:e08a4f517989 104 }
KarimAzzouz 1:e08a4f517989 105
KarimAzzouz 1:e08a4f517989 106 void MPU6050::GyroRate(void){
KarimAzzouz 1:e08a4f517989 107
KarimAzzouz 1:e08a4f517989 108 ScaledGyro();
KarimAzzouz 1:e08a4f517989 109 Scaled_GyroX-=GyroOffset[0];
KarimAzzouz 1:e08a4f517989 110 Scaled_GyroY-=GyroOffset[1];
KarimAzzouz 1:e08a4f517989 111 Scaled_GyroZ-=GyroOffset[2];
KarimAzzouz 1:e08a4f517989 112 }
KarimAzzouz 1:e08a4f517989 113
KarimAzzouz 1:e08a4f517989 114 void MPU6050::Acc(void){
KarimAzzouz 1:e08a4f517989 115
KarimAzzouz 1:e08a4f517989 116 ScaledAcc();
KarimAzzouz 1:e08a4f517989 117 Scaled_AccX-=AccOffset[0];
KarimAzzouz 1:e08a4f517989 118 Scaled_AccY-=AccOffset[1];
KarimAzzouz 1:e08a4f517989 119 Scaled_AccZ-=AccOffset[2];
KarimAzzouz 1:e08a4f517989 120
KarimAzzouz 1:e08a4f517989 121 }
KarimAzzouz 1:e08a4f517989 122
KarimAzzouz 1:e08a4f517989 123 void MPU6050::filterGyro(void){
KarimAzzouz 1:e08a4f517989 124
KarimAzzouz 1:e08a4f517989 125 GyroRate();
KarimAzzouz 1:e08a4f517989 126
KarimAzzouz 1:e08a4f517989 127 // Filter X Axis //
KarimAzzouz 1:e08a4f517989 128 filtered_Gyro[0] = filter_GyroX.update(Scaled_GyroX);
KarimAzzouz 1:e08a4f517989 129
KarimAzzouz 1:e08a4f517989 130 //Filter Y Axis //
KarimAzzouz 1:e08a4f517989 131 filtered_Gyro[1] = filter_GyroY.update(Scaled_GyroY);
KarimAzzouz 1:e08a4f517989 132
KarimAzzouz 1:e08a4f517989 133 // Filter Z Axis //
KarimAzzouz 1:e08a4f517989 134 filtered_Gyro[2] = filter_GyroZ.update(Scaled_GyroZ);
KarimAzzouz 1:e08a4f517989 135 }
KarimAzzouz 1:e08a4f517989 136
KarimAzzouz 1:e08a4f517989 137
KarimAzzouz 1:e08a4f517989 138 void MPU6050::filterAcc(void){
KarimAzzouz 1:e08a4f517989 139
KarimAzzouz 1:e08a4f517989 140 Acc();
KarimAzzouz 1:e08a4f517989 141
KarimAzzouz 1:e08a4f517989 142 // Filter X Axis//
KarimAzzouz 1:e08a4f517989 143 filtered_Acc[0] = filter_AccX.update(Scaled_AccX);
KarimAzzouz 1:e08a4f517989 144
KarimAzzouz 1:e08a4f517989 145 // Filter Y Axis //
KarimAzzouz 1:e08a4f517989 146 filtered_Acc[1] = filter_AccY.update(Scaled_AccY);
KarimAzzouz 1:e08a4f517989 147
KarimAzzouz 1:e08a4f517989 148 // Filter Z Axis//
KarimAzzouz 1:e08a4f517989 149 filtered_Acc[2] = filter_AccZ.update(Scaled_AccZ);
KarimAzzouz 1:e08a4f517989 150
KarimAzzouz 1:e08a4f517989 151 Acceleration_Magnitude = sqrt(filtered_Acc[0]*filtered_Acc[0] + filtered_Acc[1]*filtered_Acc[1] + filtered_Acc[2]*filtered_Acc[2]);
KarimAzzouz 1:e08a4f517989 152
KarimAzzouz 1:e08a4f517989 153 if(Acceleration_Magnitude==0) Panic = true; // Free-Fall detection;
KarimAzzouz 1:e08a4f517989 154 else Panic= false;
KarimAzzouz 1:e08a4f517989 155 }
KarimAzzouz 1:e08a4f517989 156
KarimAzzouz 1:e08a4f517989 157 void MPU6050::AccAngle(void){
KarimAzzouz 1:e08a4f517989 158
KarimAzzouz 1:e08a4f517989 159 //filterAcc();
KarimAzzouz 1:e08a4f517989 160 Acc();
KarimAzzouz 1:e08a4f517989 161
KarimAzzouz 1:e08a4f517989 162 //accangle[0] = ToDeg(atan2(filtered_Acc[0],sqrt(filtered_Acc[1]*filtered_Acc[1]+filtered_Acc[2]*filtered_Acc[2])));
KarimAzzouz 1:e08a4f517989 163 accangle[0] = ToDeg(atan2(Scaled_AccX,sqrt(Scaled_AccY*Scaled_AccY+Scaled_AccZ*Scaled_AccZ)));
KarimAzzouz 1:e08a4f517989 164 //accangle[1] = ToDeg(atan2(filtered_Acc[1],sqrt(filtered_Acc[0]*filtered_Acc[0]+filtered_Acc[2]*filtered_Acc[2])));
KarimAzzouz 1:e08a4f517989 165 accangle[1] = ToDeg(atan2(Scaled_AccY,sqrt(Scaled_AccX*Scaled_AccX+Scaled_AccZ*Scaled_AccZ)));
KarimAzzouz 1:e08a4f517989 166 }
KarimAzzouz 1:e08a4f517989 167