File content as of revision 0:fd289b2e6b74:

#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
}