Successful acro and level mode now! Relying on MPU9250 as base sensor. I'm working continuously on tuning and features :) NEWEST VERSION ON: https://github.com/MaEtUgR/FlyBed (CODE 100% compatible/copyable)
IMU/IMU_10DOF.cpp@0:37f0c1e8fa66, 2015-09-08 (annotated)
- Committer:
- maetugr
- Date:
- Tue Sep 08 13:38:10 2015 +0000
- Revision:
- 0:37f0c1e8fa66
- Child:
- 1:60882db03b0f
MPU9250 on the new Board works fine over SPI; RC is broken and works with FlyBed2 :(
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
maetugr | 0:37f0c1e8fa66 | 1 | #include "IMU_10DOF.h" |
maetugr | 0:37f0c1e8fa66 | 2 | |
maetugr | 0:37f0c1e8fa66 | 3 | IMU_10DOF::IMU_10DOF(PinName MOSI, PinName MISO, PinName SCLK, PinName CS) : mpu(MOSI, MISO, SCLK, CS) |
maetugr | 0:37f0c1e8fa66 | 4 | { |
maetugr | 0:37f0c1e8fa66 | 5 | dt = 0; |
maetugr | 0:37f0c1e8fa66 | 6 | dt_sensors = 0; |
maetugr | 0:37f0c1e8fa66 | 7 | time_for_dt = 0; |
maetugr | 0:37f0c1e8fa66 | 8 | time_for_dt_sensors = 0; |
maetugr | 0:37f0c1e8fa66 | 9 | |
maetugr | 0:37f0c1e8fa66 | 10 | angle = Filter.angle; // initialize array pointer |
maetugr | 0:37f0c1e8fa66 | 11 | |
maetugr | 0:37f0c1e8fa66 | 12 | LocalTimer.start(); |
maetugr | 0:37f0c1e8fa66 | 13 | } |
maetugr | 0:37f0c1e8fa66 | 14 | |
maetugr | 0:37f0c1e8fa66 | 15 | void IMU_10DOF::readAngles() |
maetugr | 0:37f0c1e8fa66 | 16 | { |
maetugr | 0:37f0c1e8fa66 | 17 | time_for_dt_sensors = LocalTimer.read(); // start time for measuring sensors |
maetugr | 0:37f0c1e8fa66 | 18 | //mpu.readGyro(); // reading sensor data |
maetugr | 0:37f0c1e8fa66 | 19 | //mpu.readAcc(); |
maetugr | 0:37f0c1e8fa66 | 20 | dt_sensors = LocalTimer.read() - time_for_dt_sensors; // stop time for measuring sensors |
maetugr | 0:37f0c1e8fa66 | 21 | |
maetugr | 0:37f0c1e8fa66 | 22 | // meassure dt since last measurement for the filter |
maetugr | 0:37f0c1e8fa66 | 23 | dt = LocalTimer.read() - time_for_dt; // time in s since last loop |
maetugr | 0:37f0c1e8fa66 | 24 | time_for_dt = LocalTimer.read(); // set new time for next measurement |
maetugr | 0:37f0c1e8fa66 | 25 | |
maetugr | 0:37f0c1e8fa66 | 26 | Filter.compute(dt, mpu.Gyro, mpu.Acc, mpu.Acc); |
maetugr | 0:37f0c1e8fa66 | 27 | } |