Bremen Team - Hangar / SML2

Implemented first Hangar-Service

Dependencies:   CalibrateMagneto QuaternionMath

Fork of SML2 by TobyRich GmbH

SensorFusion.h@42:160a37bdaa64, 2015-05-20 (annotated)

Committer:
uadhikari
Date:
Wed May 20 17:20:27 2015 +0000
Revision:
42:160a37bdaa64
Parent:
41:731e3cfac19b 
added runtime enable/disable of  Magnetometer, some warnings persist

Who changed what in which revision?

UserRevisionLine numberNew 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:
uadhikari 42:160a37bdaa64 13 class Delegate{
pvaibhav 21:5a0c9406e119 14 public:
uadhikari 42:160a37bdaa64 15 virtual void sensorTick(float dt, Vector3 fused, Vector3 accel, Vector3 magneto, Vector3 gyro, Quaternion q){};
uadhikari 42:160a37bdaa64 16 };
uadhikari 42:160a37bdaa64 17
uadhikari 42:160a37bdaa64 18 SensorFusion(I2C &i2c);
uadhikari 42:160a37bdaa64 19
uadhikari 42:160a37bdaa64 20 void setDelegate(Delegate &d);
uadhikari 42:160a37bdaa64 21 bool start();
uadhikari 42:160a37bdaa64 22 void stop();
uadhikari 42:160a37bdaa64 23 void getMagnetometerCalibration(Vector3 &min, Vector3 &max);
uadhikari 42:160a37bdaa64 24 void enableMagnetometer();
uadhikari 42:160a37bdaa64 25 void disableMagnetometer();
uadhikari 42:160a37bdaa64 26
uadhikari 42:160a37bdaa64 27 private:
uadhikari 42:160a37bdaa64 28 void updateFilter(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz);
uadhikari 42:160a37bdaa64 29 void updateFilter(float ax, float ay, float az, float gx, float gy, float gz);
uadhikari 42:160a37bdaa64 30
uadhikari 42:160a37bdaa64 31 class NineAxisSensorFusion : public Sensor::Delegate{
uadhikari 42:160a37bdaa64 32 private:
uadhikari 42:160a37bdaa64 33 SensorFusion* senFuseRef;
uadhikari 42:160a37bdaa64 34 public:
uadhikari 42:160a37bdaa64 35 NineAxisSensorFusion(SensorFusion* _ref);
uadhikari 42:160a37bdaa64 36 void sensorUpdate(Vector3 gyro_degrees);
pvaibhav 20:503cbe360419 37 };
pvaibhav 20:503cbe360419 38
uadhikari 42:160a37bdaa64 39 class SixAxisSensorFusion : public Sensor::Delegate{
uadhikari 42:160a37bdaa64 40 private:
uadhikari 42:160a37bdaa64 41 SensorFusion* senFuseRef;
uadhikari 42:160a37bdaa64 42 public:
uadhikari 42:160a37bdaa64 43 SixAxisSensorFusion(SensorFusion* _ref);
uadhikari 42:160a37bdaa64 44 void sensorUpdate(Vector3 gyro_degrees);
uadhikari 42:160a37bdaa64 45 };
uadhikari 41:731e3cfac19b 46
uadhikari 42:160a37bdaa64 47 //So that these inner classes can acces the private variable of SensorFusion class
uadhikari 42:160a37bdaa64 48 friend class NineAxisSensorFusion;
uadhikari 42:160a37bdaa64 49 friend class SixAxisSensorFusion;
uadhikari 41:731e3cfac19b 50
uadhikari 42:160a37bdaa64 51 bool magnetoMeterEnabled;
uadhikari 41:731e3cfac19b 52
pvaibhav 21:5a0c9406e119 53 Delegate defaultDelegate; // to avoid check for existence every time
pvaibhav 20:503cbe360419 54 Delegate* delegate;
uadhikari 42:160a37bdaa64 55
uadhikari 42:160a37bdaa64 56 NineAxisSensorFusion gyroDelegateWithMagneto;
uadhikari 42:160a37bdaa64 57 SixAxisSensorFusion gyroDelegateWithoutMagneto;
uadhikari 42:160a37bdaa64 58
pvaibhav 15:4488660e1a3b 59 Accelerometer accel;
uadhikari 42:160a37bdaa64 60 Gyroscope gyro;
pvaibhav 15:4488660e1a3b 61 Magnetometer magneto;
pvaibhav 15:4488660e1a3b 62 Quaternion q;
pvaibhav 21:5a0c9406e119 63 float const deltat, beta;
pvaibhav 34:01dec68de3ed 64 Vector3 fused;
uadhikari 41:731e3cfac19b 65 };
uadhikari 41:731e3cfac19b 66
pvaibhav 15:4488660e1a3b 67 #endif