File content as of revision 45:9cd917ed413a:

#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() {}
    
    /* Calibrates the sensors: This needs to be separated out later for 
       gyro and magneto because gyro need the plane to be at rest, while
       magneto needs it to be in motion
    */
    virtual void calibrate() = 0;
    virtual void reset() = 0;
    virtual bool isGyroCalibrated() = 0;
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();
    virtual void calibrate();
    virtual void reset();
    virtual bool isGyroCalibrated();

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);
};

class SensorFusion9 : public SensorFusion6
{
public:
    SensorFusion9(I2C &i2c);
    virtual void sensorUpdate(Vector3 gyro_degrees);
    virtual bool start();
    virtual void stop();

protected:
    Magnetometer    magneto;
    void updateFilter(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz);
};

#endif