Bremen Team - Hangar / SML2

Implemented first Hangar-Service

Dependencies:   CalibrateMagneto QuaternionMath

Fork of SML2 by TobyRich GmbH

SensorFusion.h@44:5483079fa156, 2015-05-29 (annotated)

Committer:
uadhikari
Date:
Fri May 29 12:21:31 2015 +0000
Revision:
44:5483079fa156
Parent:
43:6251c0169f4f
Child:
45:9cd917ed413a
Gyro calibration and reset added and flag to check if calibrated

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 40:8e852115fe55 9 #include "Filter.h"
pvaibhav 15:4488660e1a3b 10
pvaibhav 40:8e852115fe55 11 class SensorFusion
pvaibhav 15:4488660e1a3b 12 {
pvaibhav 15:4488660e1a3b 13 public:
pvaibhav 20:503cbe360419 14 class Delegate
pvaibhav 20:503cbe360419 15 {
pvaibhav 21:5a0c9406e119 16 public:
pvaibhav 34:01dec68de3ed 17 virtual void sensorTick(float dt, Vector3 fused, Vector3 accel, Vector3 magneto, Vector3 gyro, Quaternion q) {}
pvaibhav 20:503cbe360419 18 };
pvaibhav 20:503cbe360419 19
pvaibhav 40:8e852115fe55 20 void setDelegate(Delegate &d) { delegate = &d; }
pvaibhav 40:8e852115fe55 21
pvaibhav 40:8e852115fe55 22 virtual bool start() { return false; }
pvaibhav 40:8e852115fe55 23 virtual void stop() {}
uadhikari 44:5483079fa156 24
uadhikari 44:5483079fa156 25 /* Calibrates the sensors: This needs to be separated out later for
uadhikari 44:5483079fa156 26 gyro and magneto because gyro need the plane to be at rest, while
uadhikari 44:5483079fa156 27 magneto needs it to be in motion
uadhikari 44:5483079fa156 28 */
uadhikari 44:5483079fa156 29 virtual void calibrate() = 0;
uadhikari 44:5483079fa156 30 virtual void reset() = 0;
uadhikari 44:5483079fa156 31 bool isGyroCalibrated() = 0;
pvaibhav 40:8e852115fe55 32 protected:
pvaibhav 40:8e852115fe55 33 // protected ctor so that base class cannot be instantiated directly
pvaibhav 40:8e852115fe55 34 SensorFusion() : delegate(&defaultDelegate), q(1, 0, 0, 0) {}
pvaibhav 40:8e852115fe55 35 Delegate defaultDelegate; // to avoid check for existence every time
pvaibhav 40:8e852115fe55 36 Delegate* delegate;
pvaibhav 40:8e852115fe55 37 Quaternion q;
pvaibhav 40:8e852115fe55 38 };
pvaibhav 15:4488660e1a3b 39
pvaibhav 40:8e852115fe55 40 class SensorFusion6 : public SensorFusion, public Sensor::Delegate
pvaibhav 40:8e852115fe55 41 {
pvaibhav 40:8e852115fe55 42 public:
pvaibhav 40:8e852115fe55 43 SensorFusion6(I2C &i2c);
pvaibhav 40:8e852115fe55 44 virtual void sensorUpdate(Vector3 gyro_degrees);
pvaibhav 40:8e852115fe55 45 virtual bool start();
pvaibhav 40:8e852115fe55 46 virtual void stop();
uadhikari 44:5483079fa156 47 virtual void calibrate();
uadhikari 44:5483079fa156 48 virtual void reset();
uadhikari 44:5483079fa156 49 virtual bool isGyroCalibrated();
uadhikari 44:5483079fa156 50
pvaibhav 40:8e852115fe55 51 protected:
pvaibhav 40:8e852115fe55 52 Accelerometer accel;
pvaibhav 40:8e852115fe55 53 Gyroscope gyro;
pvaibhav 40:8e852115fe55 54 float const deltat, beta;
pvaibhav 40:8e852115fe55 55
pvaibhav 40:8e852115fe55 56 LowPassFilter lowpassX;
pvaibhav 40:8e852115fe55 57 LowPassFilter lowpassY;
pvaibhav 40:8e852115fe55 58 LowPassFilter lowpassZ;
pvaibhav 40:8e852115fe55 59
pvaibhav 35:fb6e4601adf3 60 void updateFilter(float ax, float ay, float az, float gx, float gy, float gz);
pvaibhav 15:4488660e1a3b 61 };
pvaibhav 15:4488660e1a3b 62
pvaibhav 43:6251c0169f4f 63 class SensorFusion9 : public SensorFusion6
pvaibhav 43:6251c0169f4f 64 {
pvaibhav 43:6251c0169f4f 65 public:
pvaibhav 43:6251c0169f4f 66 SensorFusion9(I2C &i2c);
pvaibhav 43:6251c0169f4f 67 virtual void sensorUpdate(Vector3 gyro_degrees);
pvaibhav 43:6251c0169f4f 68 virtual bool start();
pvaibhav 43:6251c0169f4f 69 virtual void stop();
uadhikari 44:5483079fa156 70
pvaibhav 43:6251c0169f4f 71 protected:
pvaibhav 43:6251c0169f4f 72 Magnetometer magneto;
pvaibhav 43:6251c0169f4f 73 void updateFilter(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz);
pvaibhav 43:6251c0169f4f 74 };
pvaibhav 43:6251c0169f4f 75
pvaibhav 15:4488660e1a3b 76 #endif