Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
SensorFusion.h@34:01dec68de3ed, 2015-05-06 (annotated)
- Committer:
- pvaibhav
- Date:
- Wed May 06 07:50:02 2015 +0000
- Revision:
- 34:01dec68de3ed
- Parent:
- 26:8f3e4e1a3acc
- Child:
- 35:fb6e4601adf3
motor voltage under 0.5V supported, magneto calibration initial values changed for smartplane2, complementary filter added to sensor fusion (default off), dt now sent with fusion parameters.
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); |
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 | 34:01dec68de3ed | 26 | void getMagnetometerCalibration(Vector3 &min, Vector3 &max); |
pvaibhav | 15:4488660e1a3b | 27 | |
pvaibhav | 15:4488660e1a3b | 28 | private: |
pvaibhav | 21:5a0c9406e119 | 29 | Delegate defaultDelegate; // to avoid check for existence every time |
pvaibhav | 20:503cbe360419 | 30 | Delegate* delegate; |
pvaibhav | 15:4488660e1a3b | 31 | Accelerometer accel; |
pvaibhav | 15:4488660e1a3b | 32 | Gyroscope gyro; |
pvaibhav | 15:4488660e1a3b | 33 | Magnetometer magneto; |
pvaibhav | 15:4488660e1a3b | 34 | Quaternion q; |
pvaibhav | 21:5a0c9406e119 | 35 | float const deltat, beta; |
pvaibhav | 34:01dec68de3ed | 36 | Vector3 fused; |
pvaibhav | 15:4488660e1a3b | 37 | void updateFilter(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz); |
pvaibhav | 15:4488660e1a3b | 38 | }; |
pvaibhav | 15:4488660e1a3b | 39 | |
pvaibhav | 15:4488660e1a3b | 40 | #endif |