Gyroscope.cpp

00001 #include "mbed.h"
00002 #include "MPU6050.h"
00003 
00004 #include "Gyroscope.h"
00005 
00006 #define LOG(args...)   // printf(args)     
00007 
00008 Gyroscope::Gyroscope(I2C &i2c): mpu(i2c, MPU6050_ADDRESS_AD0_LOW), timeout(10*1000) { 
00009     mpu.initialize();
00010 
00011     if( mpu.testConnection() ) {
00012         LOG("Giroscope Initialized !!\r\n");
00013     } else {
00014         LOG("There's an error trying initialize the Gyroscope !!\r\n");
00015     }
00016 
00017     value = new GyroscopeData();
00018 }
00019 
00020 Gyroscope::~Gyroscope() {
00021     finalizeTask();
00022     value->~GyroscopeData();
00023     // mpu.~MPU6050();
00024 }
00025 
00026 void Gyroscope::getMotion(GyroscopeData* data) {
00027     mpu.getMotion6(
00028         &data->ax, &data->ay, &data->az,
00029         &data->gx, &data->gy, &data->gz
00030     );
00031 }
00032 
00033 GyroscopeData Gyroscope::getValue() {
00034     getMotion(value);
00035     return *(value);
00036 }
00037 
00038 void Gyroscope::setCallbackReadOcurred( void (*callback_sensor)(GyroscopeData* value), uint32_t timeout = 10*1000 ) {
00039     Gyroscope::callback_sensor = callback_sensor;
00040     Gyroscope::timeout = timeout;
00041 }
00042 
00043 void Gyroscope::run(void const *self_context) {
00044     Gyroscope* self = (Gyroscope*)self_context;
00045 
00046     while(1) {
00047         GyroscopeData val = self->getValue();
00048         self->acceptDataEvent( &val );
00049         ThisThread::sleep_for(self->timeout);
00050     }
00051 }
00052 
00053 void Gyroscope::acceptDataEvent(GyroscopeData* data) {
00054     if(callback_sensor != NULL)
00055         callback_sensor(data);
00056 }
00057 
00058 void Gyroscope::initializeTask() { 
00059     t = new Thread();
00060     t->start(callback(run, this));
00061 }
00062 
00063 void Gyroscope::finalizeTask() {
00064     if(t != NULL)
00065         t->terminate();
00066  }