File content as of revision 5:caecc2426bbb:

#include "mbed.h"
#include "MPU6050.h"

#include "Gyroscope.h"

#define LOG(args...)   // printf(args)     

Gyroscope::Gyroscope(I2C &i2c): mpu(i2c, MPU6050_ADDRESS_AD0_LOW), timeout(10*1000) { 
    mpu.initialize();

    if( mpu.testConnection() ) {
        LOG("Giroscope Initialized !!\r\n");
    } else {
        LOG("There's an error trying initialize the Gyroscope !!\r\n");
    }

    value = new GyroscopeData();
}

Gyroscope::~Gyroscope() {
    finalizeTask();
    value->~GyroscopeData();
    // mpu.~MPU6050();
}

void Gyroscope::getMotion(GyroscopeData* data) {
    mpu.getMotion6(
        &data->ax, &data->ay, &data->az,
        &data->gx, &data->gy, &data->gz
    );
}

GyroscopeData Gyroscope::getValue() {
    getMotion(value);
    return *(value);
}

void Gyroscope::setCallbackReadOcurred( void (*callback_sensor)(GyroscopeData* value), uint32_t timeout = 10*1000 ) {
    Gyroscope::callback_sensor = callback_sensor;
    Gyroscope::timeout = timeout;
}

void Gyroscope::run(void const *self_context) {
    Gyroscope* self = (Gyroscope*)self_context;

    while(1) {
        GyroscopeData val = self->getValue();
        self->acceptDataEvent( &val );
        ThisThread::sleep_for(self->timeout);
    }
}

void Gyroscope::acceptDataEvent(GyroscopeData* data) {
    if(callback_sensor != NULL)
        callback_sensor(data);
}

void Gyroscope::initializeTask() { 
    t = new Thread();
    t->start(callback(run, this));
}

void Gyroscope::finalizeTask() {
    if(t != NULL)
        t->terminate();
 }