File content as of revision 0:ee11a5d3da00:

#include "mbed.h"
#include "HMC5883L.h"
#include "MPU6050.h"
#include <math.h>
 
#define SDA      D14
#define SCL      D15
#define PI       3.14159265
 
#define WAIT_COMMAND   1
#define WAIT_SETUP  2
#define SETUP  3
#define SAMPLING    4

DigitalOut myled(LED1);
Serial pc(D1, D0);
float x, y, z, heading;
int16_t raw[3];
float acc[3];
float gry[3];
float angel[3];
char buffer[128];
char command;
int state;

int main() {
    pc.printf("Inicializing...\r\n");
    //HMC5883L hmc5883l(SDA, SCL);
    MPU6050 mpu6050(SDA, SCL);
    //mpu6050.calibrate(accelBias,gyroBias);
    state = WAIT_COMMAND;
    pc.printf("OK...\r\n");
    
    wait(1);
    while(1) {
        switch(state) {
        case WAIT_COMMAND: {
            command = pc.getc();
            if (command == 's') 
                state = SETUP;
            else if (command == 'd')
                state = SAMPLING;
                
            } break;
        case WAIT_SETUP: {
            command = pc.getc();
            if (command == 's') 
                state = SETUP;
            } break;
        case SETUP: {
            //hmc5883l.getXYZ(raw);
            
            //pc.printf("$x=%d y=%d z=%d;", raw[0], raw[1], raw[2]);
            pc.printf("$Accelero: x=%d y=%d z=%d\n;", mpu6050.getAcceleroRawX(), mpu6050.getAcceleroRawY(), mpu6050.getAcceleroRawZ());
            state = WAIT_COMMAND;
            } break;
        case SAMPLING: {
            //hmc5883l.getXYZ(raw);
            //pc.printf("$x=%d y=%d z=%d;", raw[0], raw[1], raw[2]);
            //hmc58831.getHeadingXY()
            //pc.printf("$Accelero: x=%d y=%d z=%d\n;", mpu6050.getAcceleroRawX(), mpu6050.getAcceleroRawY(), mpu6050.getAcceleroRawZ());
            
            mpu6050.getAccelero(acc);
            mpu6050.getGyro(gry);
            mpu6050.getAcceleroAngle(angel);
            //pc.printf("ACC: %f %f %f\n",acc[0], acc[1], acc[2]);
            //pc.printf("GRY: %f %f %f\n",gry[0], gry[1], gry[2]);
            pc.printf("%f %f %f %f %f %f\n",gry[0], gry[1], gry[2], acc[0], acc[1], acc[2]);
            //pc.printf("%f %f %f %f %f %f\n", angel[0], angel[1], angel[2], acc[0], acc[1], acc[2]);
            wait_ms(20);
            //state = WAIT_COMMAND;
            } break;
        default: break;
        }
    }
}