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@1:60882db03b0f, 2015-09-09 (annotated)
- Committer:
- maetugr
- Date:
- Wed Sep 09 09:58:41 2015 +0000
- Revision:
- 1:60882db03b0f
- Parent:
- 0:37f0c1e8fa66
- Child:
- 7:90f876d47862
Gyro P Controller on one axis tested; everything should work again
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 | 1:60882db03b0f | 18 | mpu.readGyro(); // reading sensor data |
maetugr | 1:60882db03b0f | 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 | } |