Matthias Grob
/
FlyBed2
My fully self designed first stable working Quadrocopter Software.
IMU/IMU_10DOF.cpp@4:b0a60b0b24a9, 2014-02-15 (annotated)
- Committer:
- maetugr
- Date:
- Sat Feb 15 14:28:11 2014 +0000
- Revision:
- 4:b0a60b0b24a9
- Parent:
- 0:12950aa67f2a
- Child:
- 10:14390c90c3f5
After setting MPU6050 up
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
maetugr | 0:12950aa67f2a | 1 | #include "IMU_10DOF.h" |
maetugr | 0:12950aa67f2a | 2 | |
maetugr | 4:b0a60b0b24a9 | 3 | IMU_10DOF::IMU_10DOF(PinName sda, PinName scl) : Sensor(p28, p27), Gyro(sda, scl), Acc(sda, scl), Comp(sda, scl), Alt(sda,scl) |
maetugr | 0:12950aa67f2a | 4 | { |
maetugr | 0:12950aa67f2a | 5 | dt = 0; |
maetugr | 0:12950aa67f2a | 6 | dt_sensors = 0; |
maetugr | 0:12950aa67f2a | 7 | time_for_dt = 0; |
maetugr | 0:12950aa67f2a | 8 | time_for_dt_sensors = 0; |
maetugr | 0:12950aa67f2a | 9 | |
maetugr | 0:12950aa67f2a | 10 | angle = Filter.angle; // initialize array pointer |
maetugr | 0:12950aa67f2a | 11 | |
maetugr | 4:b0a60b0b24a9 | 12 | Sensor.calibrate(50, 0.02); |
maetugr | 4:b0a60b0b24a9 | 13 | |
maetugr | 0:12950aa67f2a | 14 | LocalTimer.start(); |
maetugr | 0:12950aa67f2a | 15 | } |
maetugr | 0:12950aa67f2a | 16 | |
maetugr | 0:12950aa67f2a | 17 | void IMU_10DOF::readAngles() |
maetugr | 0:12950aa67f2a | 18 | { |
maetugr | 0:12950aa67f2a | 19 | time_for_dt_sensors = LocalTimer.read(); // start time for measuring sensors |
maetugr | 4:b0a60b0b24a9 | 20 | Sensor.read(); |
maetugr | 4:b0a60b0b24a9 | 21 | //Gyro.read(); // reading sensor data |
maetugr | 4:b0a60b0b24a9 | 22 | //Acc.read(); |
maetugr | 4:b0a60b0b24a9 | 23 | //Comp.read(); |
maetugr | 0:12950aa67f2a | 24 | dt_sensors = LocalTimer.read() - time_for_dt_sensors; // stop time for measuring sensors |
maetugr | 0:12950aa67f2a | 25 | |
maetugr | 0:12950aa67f2a | 26 | // meassure dt for the filter |
maetugr | 0:12950aa67f2a | 27 | dt = LocalTimer.read() - time_for_dt; // time in s since last loop |
maetugr | 0:12950aa67f2a | 28 | time_for_dt = LocalTimer.read(); // set new time for next measurement |
maetugr | 0:12950aa67f2a | 29 | |
maetugr | 4:b0a60b0b24a9 | 30 | Filter.compute(dt, Sensor.data_gyro, Sensor.data_acc, Comp.data); |
maetugr | 4:b0a60b0b24a9 | 31 | //Filter.compute(dt, Gyro.data, Acc.data, Comp.data); |
maetugr | 0:12950aa67f2a | 32 | } |
maetugr | 0:12950aa67f2a | 33 | |
maetugr | 0:12950aa67f2a | 34 | void IMU_10DOF::readAltitude() |
maetugr | 0:12950aa67f2a | 35 | { |
maetugr | 0:12950aa67f2a | 36 | Alt.read(); |
maetugr | 0:12950aa67f2a | 37 | temperature = Alt.Temperature; // copy all resulting measurements |
maetugr | 0:12950aa67f2a | 38 | pressure = Alt.Pressure; |
maetugr | 0:12950aa67f2a | 39 | altitude = Alt.Altitude; |
maetugr | 0:12950aa67f2a | 40 | } |