Diff: peripheral/impl/Gyroscope.cpp

Revision:

5:caecc2426bbb

Parent:

4:f21aab30658a

--- a/peripheral/impl/Gyroscope.cpp	Mon Jun 03 23:33:00 2019 -0300
+++ b/peripheral/impl/Gyroscope.cpp	Thu Jun 06 23:54:07 2019 -0300
@@ -5,18 +5,22 @@
 
 #define LOG(args...)   // printf(args)     
 
-Gyroscope::Gyroscope(I2C i2c): mpu(i2c, MPU6050_ADDRESS_AD0_LOW) { 
+Gyroscope::Gyroscope(I2C &i2c): mpu(i2c, MPU6050_ADDRESS_AD0_LOW), timeout(10*1000) { 
     mpu.initialize();
 
-    if(mpu.testConnection()) {
-        LOG("MPU6050 test passed !!\r\n");
+    if( mpu.testConnection() ) {
+        LOG("Giroscope Initialized !!\r\n");
     } else {
-        LOG("MPU6050 test failed !!\r\n");
+        LOG("There's an error trying initialize the Gyroscope !!\r\n");
     }
+
+    value = new GyroscopeData();
 }
 
 Gyroscope::~Gyroscope() {
-    // mpu.
+    finalizeTask();
+    value->~GyroscopeData();
+    // mpu.~MPU6050();
 }
 
 void Gyroscope::getMotion(GyroscopeData* data) {
@@ -27,19 +31,36 @@
 }
 
 GyroscopeData Gyroscope::getValue() {
-    getMotion(Gyroscope::value);
-    return *(Gyroscope::value);
+    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::setCallbackReadOcurred( void* (*callback)(GyroscopeData* value) ) {
-    Gyroscope::callback = callback;
-}
+void Gyroscope::run(void const *self_context) {
+    Gyroscope* self = (Gyroscope*)self_context;
 
-void Gyroscope::run(float timeout) {
-    // TODO: every timeout, calls acceptData !!
+    while(1) {
+        GyroscopeData val = self->getValue();
+        self->acceptDataEvent( &val );
+        ThisThread::sleep_for(self->timeout);
+    }
 }
 
 void Gyroscope::acceptDataEvent(GyroscopeData* data) {
-    if(Gyroscope::callback != NULL)
-        Gyroscope::callback(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();
+ }