File content as of revision 20:503cbe360419:

#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
    {
        virtual void sensorTick(Vector3 fused, Vector3 accel, Vector3 magneto, Vector3 gyro, Quaternion q) = 0;
    };
    
    SensorFusion(I2C &i2c);
    void setDelegate(Delegate &d);

    bool start();
    void stop();

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

private:
    Delegate* delegate;
    Accelerometer accel;
    Gyroscope gyro;
    Magnetometer magneto;
    Quaternion q;
    float deltat, beta;
    void updateFilter(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz);
    Vector3 eulerAngles(Quaternion const &q) const;
};

#endif