Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
SensorFusion.h@21:5a0c9406e119, 2015-04-09 (annotated)
- Committer:
- pvaibhav
- Date:
- Thu Apr 09 11:54:48 2015 +0000
- Revision:
- 21:5a0c9406e119
- Parent:
- 20:503cbe360419
- Child:
- 26:8f3e4e1a3acc
Refactoring complete (not heavily tested, compiles)
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 | 21:5a0c9406e119 | 16 | virtual void sensorTick(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); |
pvaibhav | 20:503cbe360419 | 20 | void setDelegate(Delegate &d); |
pvaibhav | 15:4488660e1a3b | 21 | |
pvaibhav | 15:4488660e1a3b | 22 | bool start(); |
pvaibhav | 15:4488660e1a3b | 23 | void stop(); |
pvaibhav | 15:4488660e1a3b | 24 | |
pvaibhav | 15:4488660e1a3b | 25 | virtual void sensorUpdate(Vector3 gyro_degrees); // gyro's callback |
pvaibhav | 15:4488660e1a3b | 26 | |
pvaibhav | 15:4488660e1a3b | 27 | private: |
pvaibhav | 21:5a0c9406e119 | 28 | Delegate defaultDelegate; // to avoid check for existence every time |
pvaibhav | 20:503cbe360419 | 29 | Delegate* delegate; |
pvaibhav | 15:4488660e1a3b | 30 | Accelerometer accel; |
pvaibhav | 15:4488660e1a3b | 31 | Gyroscope gyro; |
pvaibhav | 15:4488660e1a3b | 32 | Magnetometer magneto; |
pvaibhav | 15:4488660e1a3b | 33 | Quaternion q; |
pvaibhav | 21:5a0c9406e119 | 34 | float const deltat, beta; |
pvaibhav | 15:4488660e1a3b | 35 | void updateFilter(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz); |
pvaibhav | 17:e9d42864c8a1 | 36 | Vector3 eulerAngles(Quaternion const &q) const; |
pvaibhav | 15:4488660e1a3b | 37 | }; |
pvaibhav | 15:4488660e1a3b | 38 | |
pvaibhav | 15:4488660e1a3b | 39 | #endif |