Matthias Grob
/
FlyBed2
My fully self designed first stable working Quadrocopter Software.
IMU/IMU_10DOF.cpp@0:12950aa67f2a, 2013-09-02 (annotated)
- Committer:
- maetugr
- Date:
- Mon Sep 02 15:04:22 2013 +0000
- Revision:
- 0:12950aa67f2a
- Child:
- 4:b0a60b0b24a9
first commit of new version (only one axis for test until now); note: no more Ticker! (made problems because of interrupts)
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 | 0:12950aa67f2a | 3 | IMU_10DOF::IMU_10DOF(PinName sda, PinName scl) : 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 | 0:12950aa67f2a | 12 | LocalTimer.start(); |
maetugr | 0:12950aa67f2a | 13 | } |
maetugr | 0:12950aa67f2a | 14 | |
maetugr | 0:12950aa67f2a | 15 | void IMU_10DOF::readAngles() |
maetugr | 0:12950aa67f2a | 16 | { |
maetugr | 0:12950aa67f2a | 17 | time_for_dt_sensors = LocalTimer.read(); // start time for measuring sensors |
maetugr | 0:12950aa67f2a | 18 | Gyro.read(); // reading sensor data |
maetugr | 0:12950aa67f2a | 19 | Acc.read(); |
maetugr | 0:12950aa67f2a | 20 | Comp.read(); |
maetugr | 0:12950aa67f2a | 21 | dt_sensors = LocalTimer.read() - time_for_dt_sensors; // stop time for measuring sensors |
maetugr | 0:12950aa67f2a | 22 | |
maetugr | 0:12950aa67f2a | 23 | // meassure dt for the filter |
maetugr | 0:12950aa67f2a | 24 | dt = LocalTimer.read() - time_for_dt; // time in s since last loop |
maetugr | 0:12950aa67f2a | 25 | time_for_dt = LocalTimer.read(); // set new time for next measurement |
maetugr | 0:12950aa67f2a | 26 | |
maetugr | 0:12950aa67f2a | 27 | Filter.compute(dt, Gyro.data, Acc.data, Comp.data); |
maetugr | 0:12950aa67f2a | 28 | } |
maetugr | 0:12950aa67f2a | 29 | |
maetugr | 0:12950aa67f2a | 30 | void IMU_10DOF::readAltitude() |
maetugr | 0:12950aa67f2a | 31 | { |
maetugr | 0:12950aa67f2a | 32 | Alt.read(); |
maetugr | 0:12950aa67f2a | 33 | temperature = Alt.Temperature; // copy all resulting measurements |
maetugr | 0:12950aa67f2a | 34 | pressure = Alt.Pressure; |
maetugr | 0:12950aa67f2a | 35 | altitude = Alt.Altitude; |
maetugr | 0:12950aa67f2a | 36 | } |