File content as of revision 46:fd5a62296b12:

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

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