Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
Diff: SensorFusion.h
- Revision:
- 42:160a37bdaa64
- Parent:
- 41:731e3cfac19b
diff -r 731e3cfac19b -r 160a37bdaa64 SensorFusion.h --- a/SensorFusion.h Wed May 20 10:13:14 2015 +0000 +++ b/SensorFusion.h Wed May 20 17:20:27 2015 +0000 @@ -10,73 +10,58 @@ class SensorFusion : public Sensor::Delegate { public: - class Delegate - { + class Delegate{ public: - virtual void sensorTick(float dt, Vector3 fused, Vector3 accel, Vector3 magneto, Vector3 gyro, Quaternion q) {} + virtual void sensorTick(float dt, Vector3 fused, Vector3 accel, Vector3 magneto, Vector3 gyro, Quaternion q){}; + }; + + SensorFusion(I2C &i2c); + + void setDelegate(Delegate &d); + bool start(); + void stop(); + void getMagnetometerCalibration(Vector3 &min, Vector3 &max); + void enableMagnetometer(); + void disableMagnetometer(); + +private: + void updateFilter(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz); + void updateFilter(float ax, float ay, float az, float gx, float gy, float gz); + + class NineAxisSensorFusion : public Sensor::Delegate{ + private: + SensorFusion* senFuseRef; + public: + NineAxisSensorFusion(SensorFusion* _ref); + void sensorUpdate(Vector3 gyro_degrees); }; - SensorFusion(I2C &i2c); - SensorFusion(const SensorFusion& s); - virtual ~SensorFusion(); + class SixAxisSensorFusion : public Sensor::Delegate{ + private: + SensorFusion* senFuseRef; + public: + SixAxisSensorFusion(SensorFusion* _ref); + void sensorUpdate(Vector3 gyro_degrees); + }; - void setDelegate(Delegate &d); - void getMagnetometerCalibration(Vector3 &min, Vector3 &max); - + //So that these inner classes can acces the private variable of SensorFusion class + friend class NineAxisSensorFusion; + friend class SixAxisSensorFusion; - virtual bool start(); - virtual void stop(); - virtual void sensorUpdate(Vector3 gyro_degrees); // gyro's callback + bool magnetoMeterEnabled; - -protected: Delegate defaultDelegate; // to avoid check for existence every time Delegate* delegate; + + NineAxisSensorFusion gyroDelegateWithMagneto; + SixAxisSensorFusion gyroDelegateWithoutMagneto; + Accelerometer accel; - Gyroscope gyro; + Gyroscope gyro; Magnetometer magneto; Quaternion q; float const deltat, beta; Vector3 fused; - - void startAccelerometer(); - void startGyrometer(); - bool startMagnetometer(); - - void stopAccelerometer(); - void stopGyrometer(); - void stopMagnetometer(); }; -class NineAxesSensor : public SensorFusion -{ -public: - NineAxesSensor(I2C &i2c); - NineAxesSensor(const NineAxesSensor& c); - virtual ~NineAxesSensor(); - - virtual bool start(); - virtual void stop(); - virtual void sensorUpdate(Vector3 gyro_degrees); - -private: - void updateFilter(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz); -}; - -class SixAxesSensor : public SensorFusion -{ -public: - SixAxesSensor(I2C &i2c); - SixAxesSensor(const SixAxesSensor& c); - virtual ~SixAxesSensor(); - - virtual bool start(); - virtual void stop(); - virtual void sensorUpdate(Vector3 gyro_degrees); - -private: - void updateFilter(float ax, float ay, float az, float gx, float gy, float gz); -}; - - #endif \ No newline at end of file