File content as of revision 1:ba2d31e3112d:

#include "mbed.h"
#include "L3GD20.h"
#include "LSM303DLHC.h"
#include "math.h"
#include "arm_math.h"
#include "sensfusion9.h"
L3GD20 gyro(D14, D15);
Serial debug(USBTX,USBRX);
LSM303DLHC compass(D14, D15);

float ax, ay, az;
float mx, my, mz;
float gx, gy, gz;
float roll, pitch, yaw;
Ticker kalmanTimer;
float dt = 0.01;

void attitudeUpdate(void) { 
}

int main() {
    //inizializzo filtro di kalman
    sensfusion9Init();
    //kalmanTimer.attach(&attitudeUpdate, dt);
    
    debug.format(8, Serial::None, 1);
    debug.baud(115200);
    
    //lunghezza asta
    float l = 1.0f;
    while(1) {
        compass.read(&ax, &ay, &az, &mx, &my, &mz);
        gyro.read(&gx, &gy, &gz);
        sensfusion9UpdateQ(gx+(ax/l)*dt, gy+(ay/l)*dt, gz+(az/l)*dt, ax, ay, az, mx, my, mz, dt);
        sensfusion9GetEulerRPY(&roll, &pitch, &yaw);
        //debug.printf("%.10f %.10f %.10f\n\r", mx, my, mz);
        //debug.printf("a %.4f %.4f %.4f m %.4f %.4f %.4f m %.4f %.4f %.4f R:%.4f P:%.4f Y:%.4f\n\r",ax,ay,az,mx,my,mz,gx,gy,gz,roll, pitch, yaw);
        //debug.printf("R:%.4f P:%.4f Y:%.4f\n\r", roll, pitch, yaw);
        debug.printf("%.4f %.4f %.4f\n\r", roll, pitch+3, yaw);
        wait(dt);
    }
}