Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
SensorFusion.h@41:731e3cfac19b, 2015-05-20 (annotated)
- Committer:
- uadhikari
- Date:
- Wed May 20 10:13:14 2015 +0000
- Revision:
- 41:731e3cfac19b
- Parent:
- 35:fb6e4601adf3
- Child:
- 42:160a37bdaa64
Refactored SensorFusion class
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 | 15:4488660e1a3b | 9 | |
pvaibhav | 15:4488660e1a3b | 10 | class SensorFusion : public Sensor::Delegate |
pvaibhav | 15:4488660e1a3b | 11 | { |
pvaibhav | 15:4488660e1a3b | 12 | public: |
pvaibhav | 20:503cbe360419 | 13 | class Delegate |
pvaibhav | 20:503cbe360419 | 14 | { |
pvaibhav | 21:5a0c9406e119 | 15 | public: |
pvaibhav | 34:01dec68de3ed | 16 | virtual void sensorTick(float dt, Vector3 fused, Vector3 accel, Vector3 magneto, Vector3 gyro, Quaternion q) {} |
pvaibhav | 20:503cbe360419 | 17 | }; |
pvaibhav | 20:503cbe360419 | 18 | |
pvaibhav | 15:4488660e1a3b | 19 | SensorFusion(I2C &i2c); |
uadhikari | 41:731e3cfac19b | 20 | SensorFusion(const SensorFusion& s); |
uadhikari | 41:731e3cfac19b | 21 | virtual ~SensorFusion(); |
uadhikari | 41:731e3cfac19b | 22 | |
uadhikari | 41:731e3cfac19b | 23 | void setDelegate(Delegate &d); |
uadhikari | 41:731e3cfac19b | 24 | void getMagnetometerCalibration(Vector3 &min, Vector3 &max); |
uadhikari | 41:731e3cfac19b | 25 | |
uadhikari | 41:731e3cfac19b | 26 | |
uadhikari | 41:731e3cfac19b | 27 | virtual bool start(); |
uadhikari | 41:731e3cfac19b | 28 | virtual void stop(); |
uadhikari | 41:731e3cfac19b | 29 | virtual void sensorUpdate(Vector3 gyro_degrees); // gyro's callback |
uadhikari | 41:731e3cfac19b | 30 | |
pvaibhav | 15:4488660e1a3b | 31 | |
uadhikari | 41:731e3cfac19b | 32 | protected: |
pvaibhav | 21:5a0c9406e119 | 33 | Delegate defaultDelegate; // to avoid check for existence every time |
pvaibhav | 20:503cbe360419 | 34 | Delegate* delegate; |
pvaibhav | 15:4488660e1a3b | 35 | Accelerometer accel; |
pvaibhav | 15:4488660e1a3b | 36 | Gyroscope gyro; |
pvaibhav | 15:4488660e1a3b | 37 | Magnetometer magneto; |
pvaibhav | 15:4488660e1a3b | 38 | Quaternion q; |
pvaibhav | 21:5a0c9406e119 | 39 | float const deltat, beta; |
pvaibhav | 34:01dec68de3ed | 40 | Vector3 fused; |
uadhikari | 41:731e3cfac19b | 41 | |
uadhikari | 41:731e3cfac19b | 42 | void startAccelerometer(); |
uadhikari | 41:731e3cfac19b | 43 | void startGyrometer(); |
uadhikari | 41:731e3cfac19b | 44 | bool startMagnetometer(); |
uadhikari | 41:731e3cfac19b | 45 | |
uadhikari | 41:731e3cfac19b | 46 | void stopAccelerometer(); |
uadhikari | 41:731e3cfac19b | 47 | void stopGyrometer(); |
uadhikari | 41:731e3cfac19b | 48 | void stopMagnetometer(); |
uadhikari | 41:731e3cfac19b | 49 | }; |
uadhikari | 41:731e3cfac19b | 50 | |
uadhikari | 41:731e3cfac19b | 51 | class NineAxesSensor : public SensorFusion |
uadhikari | 41:731e3cfac19b | 52 | { |
uadhikari | 41:731e3cfac19b | 53 | public: |
uadhikari | 41:731e3cfac19b | 54 | NineAxesSensor(I2C &i2c); |
uadhikari | 41:731e3cfac19b | 55 | NineAxesSensor(const NineAxesSensor& c); |
uadhikari | 41:731e3cfac19b | 56 | virtual ~NineAxesSensor(); |
uadhikari | 41:731e3cfac19b | 57 | |
uadhikari | 41:731e3cfac19b | 58 | virtual bool start(); |
uadhikari | 41:731e3cfac19b | 59 | virtual void stop(); |
uadhikari | 41:731e3cfac19b | 60 | virtual void sensorUpdate(Vector3 gyro_degrees); |
uadhikari | 41:731e3cfac19b | 61 | |
uadhikari | 41:731e3cfac19b | 62 | private: |
pvaibhav | 15:4488660e1a3b | 63 | void updateFilter(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz); |
pvaibhav | 15:4488660e1a3b | 64 | }; |
pvaibhav | 15:4488660e1a3b | 65 | |
uadhikari | 41:731e3cfac19b | 66 | class SixAxesSensor : public SensorFusion |
uadhikari | 41:731e3cfac19b | 67 | { |
uadhikari | 41:731e3cfac19b | 68 | public: |
uadhikari | 41:731e3cfac19b | 69 | SixAxesSensor(I2C &i2c); |
uadhikari | 41:731e3cfac19b | 70 | SixAxesSensor(const SixAxesSensor& c); |
uadhikari | 41:731e3cfac19b | 71 | virtual ~SixAxesSensor(); |
uadhikari | 41:731e3cfac19b | 72 | |
uadhikari | 41:731e3cfac19b | 73 | virtual bool start(); |
uadhikari | 41:731e3cfac19b | 74 | virtual void stop(); |
uadhikari | 41:731e3cfac19b | 75 | virtual void sensorUpdate(Vector3 gyro_degrees); |
uadhikari | 41:731e3cfac19b | 76 | |
uadhikari | 41:731e3cfac19b | 77 | private: |
uadhikari | 41:731e3cfac19b | 78 | void updateFilter(float ax, float ay, float az, float gx, float gy, float gz); |
uadhikari | 41:731e3cfac19b | 79 | }; |
uadhikari | 41:731e3cfac19b | 80 | |
uadhikari | 41:731e3cfac19b | 81 | |
pvaibhav | 15:4488660e1a3b | 82 | #endif |