Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
SensorFusion.h
- Committer:
- pvaibhav
- Date:
- 2015-05-26
- Revision:
- 40:8e852115fe55
- Parent:
- 35:fb6e4601adf3
- Child:
- 43:6251c0169f4f
File content as of revision 40:8e852115fe55:
#ifndef _H_SENSORFUSION_H #define _H_SENSORFUSION_H #include "I2CPeripheral.h" #include "Magnetometer.h" #include "Accelerometer.h" #include "Gyroscope.h" #include "Quaternion.h" #include "Filter.h" class SensorFusion { public: class Delegate { public: virtual void sensorTick(float dt, Vector3 fused, Vector3 accel, Vector3 magneto, Vector3 gyro, Quaternion q) {} }; void setDelegate(Delegate &d) { delegate = &d; } virtual bool start() { return false; } virtual void stop() {} protected: // protected ctor so that base class cannot be instantiated directly SensorFusion() : delegate(&defaultDelegate), q(1, 0, 0, 0) {} Delegate defaultDelegate; // to avoid check for existence every time Delegate* delegate; Quaternion q; }; class SensorFusion6 : public SensorFusion, public Sensor::Delegate { public: SensorFusion6(I2C &i2c); virtual void sensorUpdate(Vector3 gyro_degrees); virtual bool start(); virtual void stop(); protected: Accelerometer accel; Gyroscope gyro; float const deltat, beta; LowPassFilter lowpassX; LowPassFilter lowpassY; LowPassFilter lowpassZ; void updateFilter(float ax, float ay, float az, float gx, float gy, float gz); }; #endif