Diff: VelocityTracker.cpp

Revision:

0:354a8831107d

diff -r 000000000000 -r 354a8831107d VelocityTracker.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/VelocityTracker.cpp	Wed Dec 03 22:33:03 2014 +0000
@@ -0,0 +1,50 @@
+#include "mbed.h"
+#include "math.h"
+
+#include "LSM9DS0.h"
+#include "VelocityTracker.h"
+
+// SDO_XM and SDO_G are both grounded, so our addresses are:
+#define LSM9DS0_XM  0x1D // Would be 0x1E if SDO_XM is LOW
+#define LSM9DS0_G   0x6B // Would be 0x6A if SDO_G is LOW
+
+VelocityTracker::VelocityTracker(PinName sda, PinName scl, float updateRate, float biasRate) {
+    imu = new LSM9DS0(sda, scl, LSM9DS0_G, LSM9DS0_XM);
+    _updateRate = updateRate;
+    _biasRate = biasRate;
+    /*
+    biasTicker.attach(&this, &VelocityTracker::updateBias, _biasRate);
+    updateTicker.attach(&this, &VelocityTracker::updateVelocity, _updateRate);
+    */
+    _bias[0] = 0;
+    _bias[1] = 0;
+    _bias[2] = 0;
+}
+
+void VelocityTracker::updateBias() {
+    imu->readAccel();
+    _bias[0] += imu->ax;
+    _bias[1] += imu->ay;
+    _bias[2] += imu->az;
+    sampleCount++;
+}
+
+void VelocityTracker::updateVelocity() {
+    float ax = _bias[0] / sampleCount;
+    float ay = _bias[1] / sampleCount;
+    float az = _bias[2] / sampleCount;
+    
+    // Multiplying by 9.8 to convert from g's
+    _vx += ax * 9.8 * _updateRate;
+    _vy += ay * 9.8 * _updateRate;
+    _vz += az * 9.8 * _updateRate;
+    
+    _bias[0] = 0;
+    _bias[1] = 0;
+    _bias[2] = 0;
+    sampleCount = 0;
+}
+
+float VelocityTracker::getVelocity() {
+    return sqrt(_vx*_vx + _vy*_vy);             // Assuming on a flat surface
+}
\ No newline at end of file