James Nagendran
/
4180_final_receiver
Receiver code for SLVM
Diff: VelocityTracker.cpp
- Revision:
- 0:fd289b2e6b74
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/VelocityTracker.cpp Tue Dec 09 01:15:37 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 +}