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