quadcopter cufe
/
MPU_6050_nadafa
mpu6050 nadafa
main.cpp@0:8fe8d6dd7cd6, 2015-06-23 (annotated)
- Committer:
- khaledelmadawi
- Date:
- Tue Jun 23 14:47:52 2015 +0000
- Revision:
- 0:8fe8d6dd7cd6
mpu nadafa
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
khaledelmadawi | 0:8fe8d6dd7cd6 | 1 | #include "mbed.h" |
khaledelmadawi | 0:8fe8d6dd7cd6 | 2 | #include "AngularDataAcquizition.h" |
khaledelmadawi | 0:8fe8d6dd7cd6 | 3 | AngularDataAcquizition Angle(p9, p10); |
khaledelmadawi | 0:8fe8d6dd7cd6 | 4 | float AngleYaw,AngleRoll,AnglePitch,AngleOffset[3]; |
khaledelmadawi | 0:8fe8d6dd7cd6 | 5 | void Calibration(); |
khaledelmadawi | 0:8fe8d6dd7cd6 | 6 | DigitalOut myled(LED1); |
khaledelmadawi | 0:8fe8d6dd7cd6 | 7 | Serial pc(p13, p14); |
khaledelmadawi | 0:8fe8d6dd7cd6 | 8 | |
khaledelmadawi | 0:8fe8d6dd7cd6 | 9 | int main() { |
khaledelmadawi | 0:8fe8d6dd7cd6 | 10 | pc.baud(115200); |
khaledelmadawi | 0:8fe8d6dd7cd6 | 11 | |
khaledelmadawi | 0:8fe8d6dd7cd6 | 12 | AngleOffset[0]=0; |
khaledelmadawi | 0:8fe8d6dd7cd6 | 13 | AngleOffset[1]=0; |
khaledelmadawi | 0:8fe8d6dd7cd6 | 14 | AngleOffset[2]=0; |
khaledelmadawi | 0:8fe8d6dd7cd6 | 15 | // Angle.BeginInterrupt(0.05); |
khaledelmadawi | 0:8fe8d6dd7cd6 | 16 | |
khaledelmadawi | 0:8fe8d6dd7cd6 | 17 | while(1) { |
khaledelmadawi | 0:8fe8d6dd7cd6 | 18 | Calibration(); |
khaledelmadawi | 0:8fe8d6dd7cd6 | 19 | |
khaledelmadawi | 0:8fe8d6dd7cd6 | 20 | Angle.callMeanFilteredReadings(); |
khaledelmadawi | 0:8fe8d6dd7cd6 | 21 | AngleYaw=(Angle.Meanypr[0]-AngleOffset[0]); |
khaledelmadawi | 0:8fe8d6dd7cd6 | 22 | AnglePitch=(Angle.Meanypr[1]-AngleOffset[1]); |
khaledelmadawi | 0:8fe8d6dd7cd6 | 23 | AngleRoll=(Angle.Meanypr[2]-AngleOffset[2]); |
khaledelmadawi | 0:8fe8d6dd7cd6 | 24 | pc.printf("A1:%f A2:%f OP:%f\r\n",AnglePitch*180/3.14,AngleRoll*180/3.14,AngleYaw*180/3.14); |
khaledelmadawi | 0:8fe8d6dd7cd6 | 25 | myled = 1; |
khaledelmadawi | 0:8fe8d6dd7cd6 | 26 | wait(0.2); |
khaledelmadawi | 0:8fe8d6dd7cd6 | 27 | myled = 0; |
khaledelmadawi | 0:8fe8d6dd7cd6 | 28 | wait(0.2); |
khaledelmadawi | 0:8fe8d6dd7cd6 | 29 | } |
khaledelmadawi | 0:8fe8d6dd7cd6 | 30 | } |
khaledelmadawi | 0:8fe8d6dd7cd6 | 31 | void Calibration(){ |
khaledelmadawi | 0:8fe8d6dd7cd6 | 32 | if(pc.readable()) { |
khaledelmadawi | 0:8fe8d6dd7cd6 | 33 | switch (pc.getc()) { |
khaledelmadawi | 0:8fe8d6dd7cd6 | 34 | case 'c': |
khaledelmadawi | 0:8fe8d6dd7cd6 | 35 | AngleOffset[0]=Angle.Meanypr[0]; |
khaledelmadawi | 0:8fe8d6dd7cd6 | 36 | AngleOffset[1]=Angle.Meanypr[1]; |
khaledelmadawi | 0:8fe8d6dd7cd6 | 37 | AngleOffset[2]=Angle.Meanypr[2]; |
khaledelmadawi | 0:8fe8d6dd7cd6 | 38 | |
khaledelmadawi | 0:8fe8d6dd7cd6 | 39 | break; |
khaledelmadawi | 0:8fe8d6dd7cd6 | 40 | } |
khaledelmadawi | 0:8fe8d6dd7cd6 | 41 | } |
khaledelmadawi | 0:8fe8d6dd7cd6 | 42 | } |