File content as of revision 5:1faeeab28bd1:

#include "LSM6DSLSensor.h" 
#include "accgyro.h"
#include "mbed.h"
#include "behaviour.h"

int i;
int32_t ACCGYRO_gyro_axes[3];
int32_t ACCGYRO_acc_axes[3];
int32_t gyro_x[5];
int32_t GYRO_Xf = 0;
int32_t GYRO_Yf = 0;
int32_t GYRO_Zf = 0;
int32_t GYRO_Xfold;
int32_t GYRO_Yfold;
int32_t GYRO_Zfold;
float GYRO_X_D;
float GYRO_Y_D;
float GYRO_Z_D;
float GYRO_X_Dold;
float GYRO_Y_Dold;
float GYRO_Z_Dold;
float GYRO_X_Time;
float GYRO_Y_Time;
float GYRO_Z_Time;

static DevI2C devI2c(PB_11,PB_10);
static LSM6DSLSensor acc_gyro(&devI2c,LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW,PD_11);

Timer t;
Ticker getdata;
EventQueue ACCGYRO_queue;
extern EventQueue BEHAVIOUR_queue;

//Mutex ACCGYRO_mutex;
int32_t ACCGYRO_get_gyro_x()
{
    int32_t temp;
    temp = ACCGYRO_gyro_axes[0];
    return temp;
}

int32_t ACCGYRO_get_gyro_y()
{
    int32_t temp;
    temp = ACCGYRO_gyro_axes[1];
    return temp;
}

int32_t ACCGYRO_get_gyro_z()
{
    int32_t temp;
    temp = ACCGYRO_gyro_axes[2];
    return temp;
}
/*
void GYRO_get()
{
    
    for (i=0; i<5; i++) 
    {
        gyro_x[i] = ACCGYRO_gyro_axes[0];
    }
    printf("%61d\n", &i);
}
*/

void ACCGYRO_task()
{
    //int i;
    acc_gyro.get_x_axes(ACCGYRO_acc_axes);
    acc_gyro.get_g_axes(ACCGYRO_gyro_axes);
    BEHAVIOUR_queue.call(BEHAVIOUR_gyro_x_set,ACCGYRO_gyro_axes[0]);
    BEHAVIOUR_queue.call(BEHAVIOUR_gyro_y_set,ACCGYRO_gyro_axes[1]);
    BEHAVIOUR_queue.call(BEHAVIOUR_gyro_z_set,ACCGYRO_gyro_axes[2]);
    BEHAVIOUR_queue.call(BEHAVIOUR_acc_x_set,ACCGYRO_acc_axes[0]);
    BEHAVIOUR_queue.call(BEHAVIOUR_acc_y_set,ACCGYRO_acc_axes[1]);
    BEHAVIOUR_queue.call(BEHAVIOUR_acc_z_set,ACCGYRO_acc_axes[2]);
    t. start();
    for (i=0; i<5; i++)
    {
        gyro_x[i] = ACCGYRO_gyro_axes[0];
    }
    t.stop();
    GYRO_X_Time = t.read();
    //printf("The time taken was %f seconds\n", GYRO_X_Time);
    t.reset();
    GYRO_Xf = gyro_x[0];
    GYRO_Xfold = gyro_x[4];
    GYRO_X_D = (GYRO_Xf  + GYRO_Xfold)*0.5 * GYRO_X_Time *1000+ GYRO_X_Dold;
    printf("%f\r\n", GYRO_X_D);
/*
    if(GYRO_X_D>180.0)
        GYRO_X_D = GYRO_X_D - 360.0;
    else
    if(GYRO_X_D<-180.0)
        GYRO_X_D = GYRO_X_D + 360.0;
*/
};

void ACCGYRO_init()
{
    printf("Initing LSM6DSL\r\n");
    acc_gyro.init(NULL);
    acc_gyro.enable_x();
    acc_gyro.enable_g();
    printf("Init done\r\n");
    ACCGYRO_queue.call_every(100, ACCGYRO_task);
    ACCGYRO_queue.dispatch();
};