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?
User | Revision | Line number | New 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 |