#include "mbed.h"

#define UM6_GYRO_PROC_XY                0x5C
#define UM6_GYRO_PROC_Z                 0x5D
#define UM6_ACCEL_PROC_XY               0x5E
#define UM6_ACCEL_PROC_Z                0x5F

#define UM6_EULER_PHI_THETA             0x62
#define UM6_EULER_PSI                   0x63

#define PI 3.14159265359
#define DEG2RAD ((2.0*PI)/360.0)
#define RAD2DEG (1/(DEG2RAD))

#define RAW2ANG     0.0109863  //(1.0/91.02222)
#define RAW2ANGDOT  0.0610352    //(1.0/16.0)
#define RAW2ACC     0.000183105
#define NELEMS(x)  (sizeof(x) / sizeof((x)[0]))


extern float eulerTime;
extern double pitch;
extern double roll;
extern double yaw;
extern double rollDot;
extern double pitchDot;
extern double yawDot;
extern double accX;
extern double accY;
extern double accZ;

typedef struct UM6_packet_struct{
    uint8_t Address;
    uint8_t PT;
    uint16_t Checksum;
    uint8_t data_length;
    uint8_t data[42];
}UM6_packet;
    
uint8_t parse_serial_data( uint8_t* rx_data6, uint8_t rx_length, UM6_packet* packet);

//IMU helper functions
void setupIMU();
float readFloat(uint8_t * buffer, int offset);
void readIMU();
void readIMUAsync();