Diff: sensors/positionsensors.cpp

Revision:

54:e8d9bc885723

Parent:

33:e7b132029bae

--- a/sensors/positionsensors.cpp	Sat May 02 00:21:00 2015 +0000
+++ b/sensors/positionsensors.cpp	Tue May 19 08:50:42 2015 +0000
@@ -1,5 +1,6 @@
 #include "includes.h"
 #include "sensors.h"
+#include "filters.h"
 #include "lut.h"
 
 AnalogHallPositionSensor::AnalogHallPositionSensor(PinName pin_a, PinName pin_b, float cal1_a, float cal2_a, 
@@ -11,6 +12,12 @@
     _cal1_b = cal1_b;
     _cal2_b = cal2_b;
     _offset = offset;
+    _time_upd_ticker = new Ticker();
+    _time_upd_ticker->attach_us(this, &AnalogHallPositionSensor::upd_function, 50);
+    _last_time = 0.0f;
+    _last_position = 0.0f;
+    _speed = 0.0f;
+    GetPosition(); //first speed result is wrong, need to do this once
 }
 
 float AnalogHallPositionSensor::GetPosition() {
@@ -41,5 +48,11 @@
     if (angle < 0.0f) angle += 360.0f;
     if (angle >= 360.0f) angle -= 360.0f;
     
+    float delta_t = _time - _last_time;
+    float delta_angle = -(angle - _last_position); //backwards, for now
+    _last_time = _time;
+    _last_position = angle;
+    if (delta_angle < 0.0f) return angle;
+    _speed = delta_angle / delta_t;
     return angle;
 }
\ No newline at end of file