File content as of revision 26:8f3e4e1a3acc:

#ifndef _H_SENSORFUSION_H
#define _H_SENSORFUSION_H

#include "I2CPeripheral.h"
#include "Magnetometer.h"
#include "Accelerometer.h"
#include "Gyroscope.h"
#include "Quaternion.h"

class SensorFusion : public Sensor::Delegate
{
public:
    class Delegate
    {
    public:
        virtual void sensorTick(Vector3 fused, Vector3 accel, Vector3 magneto, Vector3 gyro, Quaternion q) {}
    };
    
    SensorFusion(I2C &i2c);
    void setDelegate(Delegate &d);

    bool start();
    void stop();

    virtual void sensorUpdate(Vector3 gyro_degrees); // gyro's callback

private:
    Delegate defaultDelegate; // to avoid check for existence every time
    Delegate* delegate;
    Accelerometer accel;
    Gyroscope gyro;
    Magnetometer magneto;
    Quaternion q;
    float const deltat, beta;
    void updateFilter(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz);
};

#endif