Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
SensorFusion.h@45:9cd917ed413a, 2015-05-29 (annotated)
- Committer:
- uadhikari
- Date:
- Fri May 29 15:07:33 2015 +0000
- Revision:
- 45:9cd917ed413a
- Parent:
- 44:5483079fa156
isGyroCalibrate in SensorFusion is made virtual
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
pvaibhav | 15:4488660e1a3b | 1 | #ifndef _H_SENSORFUSION_H |
pvaibhav | 15:4488660e1a3b | 2 | #define _H_SENSORFUSION_H |
pvaibhav | 15:4488660e1a3b | 3 | |
pvaibhav | 15:4488660e1a3b | 4 | #include "I2CPeripheral.h" |
pvaibhav | 15:4488660e1a3b | 5 | #include "Magnetometer.h" |
pvaibhav | 15:4488660e1a3b | 6 | #include "Accelerometer.h" |
pvaibhav | 15:4488660e1a3b | 7 | #include "Gyroscope.h" |
pvaibhav | 15:4488660e1a3b | 8 | #include "Quaternion.h" |
pvaibhav | 40:8e852115fe55 | 9 | #include "Filter.h" |
pvaibhav | 15:4488660e1a3b | 10 | |
pvaibhav | 40:8e852115fe55 | 11 | class SensorFusion |
pvaibhav | 15:4488660e1a3b | 12 | { |
pvaibhav | 15:4488660e1a3b | 13 | public: |
pvaibhav | 20:503cbe360419 | 14 | class Delegate |
pvaibhav | 20:503cbe360419 | 15 | { |
pvaibhav | 21:5a0c9406e119 | 16 | public: |
pvaibhav | 34:01dec68de3ed | 17 | virtual void sensorTick(float dt, Vector3 fused, Vector3 accel, Vector3 magneto, Vector3 gyro, Quaternion q) {} |
pvaibhav | 20:503cbe360419 | 18 | }; |
pvaibhav | 20:503cbe360419 | 19 | |
pvaibhav | 40:8e852115fe55 | 20 | void setDelegate(Delegate &d) { delegate = &d; } |
pvaibhav | 40:8e852115fe55 | 21 | |
pvaibhav | 40:8e852115fe55 | 22 | virtual bool start() { return false; } |
pvaibhav | 40:8e852115fe55 | 23 | virtual void stop() {} |
uadhikari | 44:5483079fa156 | 24 | |
uadhikari | 44:5483079fa156 | 25 | /* Calibrates the sensors: This needs to be separated out later for |
uadhikari | 44:5483079fa156 | 26 | gyro and magneto because gyro need the plane to be at rest, while |
uadhikari | 44:5483079fa156 | 27 | magneto needs it to be in motion |
uadhikari | 44:5483079fa156 | 28 | */ |
uadhikari | 44:5483079fa156 | 29 | virtual void calibrate() = 0; |
uadhikari | 44:5483079fa156 | 30 | virtual void reset() = 0; |
uadhikari | 45:9cd917ed413a | 31 | virtual bool isGyroCalibrated() = 0; |
pvaibhav | 40:8e852115fe55 | 32 | protected: |
pvaibhav | 40:8e852115fe55 | 33 | // protected ctor so that base class cannot be instantiated directly |
pvaibhav | 40:8e852115fe55 | 34 | SensorFusion() : delegate(&defaultDelegate), q(1, 0, 0, 0) {} |
uadhikari | 45:9cd917ed413a | 35 | Delegate defaultDelegate; // to avoid check for existence every time |
uadhikari | 45:9cd917ed413a | 36 | Delegate* delegate; |
pvaibhav | 40:8e852115fe55 | 37 | Quaternion q; |
pvaibhav | 40:8e852115fe55 | 38 | }; |
pvaibhav | 15:4488660e1a3b | 39 | |
pvaibhav | 40:8e852115fe55 | 40 | class SensorFusion6 : public SensorFusion, public Sensor::Delegate |
pvaibhav | 40:8e852115fe55 | 41 | { |
pvaibhav | 40:8e852115fe55 | 42 | public: |
pvaibhav | 40:8e852115fe55 | 43 | SensorFusion6(I2C &i2c); |
pvaibhav | 40:8e852115fe55 | 44 | virtual void sensorUpdate(Vector3 gyro_degrees); |
pvaibhav | 40:8e852115fe55 | 45 | virtual bool start(); |
pvaibhav | 40:8e852115fe55 | 46 | virtual void stop(); |
uadhikari | 44:5483079fa156 | 47 | virtual void calibrate(); |
uadhikari | 44:5483079fa156 | 48 | virtual void reset(); |
uadhikari | 44:5483079fa156 | 49 | virtual bool isGyroCalibrated(); |
uadhikari | 44:5483079fa156 | 50 | |
pvaibhav | 40:8e852115fe55 | 51 | protected: |
pvaibhav | 40:8e852115fe55 | 52 | Accelerometer accel; |
pvaibhav | 40:8e852115fe55 | 53 | Gyroscope gyro; |
pvaibhav | 40:8e852115fe55 | 54 | float const deltat, beta; |
pvaibhav | 40:8e852115fe55 | 55 | |
pvaibhav | 40:8e852115fe55 | 56 | LowPassFilter lowpassX; |
pvaibhav | 40:8e852115fe55 | 57 | LowPassFilter lowpassY; |
pvaibhav | 40:8e852115fe55 | 58 | LowPassFilter lowpassZ; |
pvaibhav | 40:8e852115fe55 | 59 | |
pvaibhav | 35:fb6e4601adf3 | 60 | void updateFilter(float ax, float ay, float az, float gx, float gy, float gz); |
pvaibhav | 15:4488660e1a3b | 61 | }; |
pvaibhav | 15:4488660e1a3b | 62 | |
pvaibhav | 43:6251c0169f4f | 63 | class SensorFusion9 : public SensorFusion6 |
pvaibhav | 43:6251c0169f4f | 64 | { |
pvaibhav | 43:6251c0169f4f | 65 | public: |
pvaibhav | 43:6251c0169f4f | 66 | SensorFusion9(I2C &i2c); |
pvaibhav | 43:6251c0169f4f | 67 | virtual void sensorUpdate(Vector3 gyro_degrees); |
pvaibhav | 43:6251c0169f4f | 68 | virtual bool start(); |
pvaibhav | 43:6251c0169f4f | 69 | virtual void stop(); |
uadhikari | 44:5483079fa156 | 70 | |
pvaibhav | 43:6251c0169f4f | 71 | protected: |
pvaibhav | 43:6251c0169f4f | 72 | Magnetometer magneto; |
pvaibhav | 43:6251c0169f4f | 73 | void updateFilter(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz); |
pvaibhav | 43:6251c0169f4f | 74 | }; |
pvaibhav | 43:6251c0169f4f | 75 | |
pvaibhav | 15:4488660e1a3b | 76 | #endif |