James Nagendran / Mbed 2 deprecated 4180_final_receiver

Receiver code for SLVM

Dependencies:   mbed-rtos mbed

VelocityTracker.cpp@0:fd289b2e6b74, 2014-12-09 (annotated)

Committer:
jnagendran3
Date:
Tue Dec 09 01:15:37 2014 +0000
Revision:
0:fd289b2e6b74
first

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jnagendran3 0:fd289b2e6b74 1 #include "mbed.h"
jnagendran3 0:fd289b2e6b74 2 #include "math.h"
jnagendran3 0:fd289b2e6b74 3
jnagendran3 0:fd289b2e6b74 4 #include "LSM9DS0.h"
jnagendran3 0:fd289b2e6b74 5 #include "VelocityTracker.h"
jnagendran3 0:fd289b2e6b74 6
jnagendran3 0:fd289b2e6b74 7 // SDO_XM and SDO_G are both grounded, so our addresses are:
jnagendran3 0:fd289b2e6b74 8 #define LSM9DS0_XM 0x1D // Would be 0x1E if SDO_XM is LOW
jnagendran3 0:fd289b2e6b74 9 #define LSM9DS0_G 0x6B // Would be 0x6A if SDO_G is LOW
jnagendran3 0:fd289b2e6b74 10
jnagendran3 0:fd289b2e6b74 11 VelocityTracker::VelocityTracker(PinName sda, PinName scl, float updateRate, float biasRate) {
jnagendran3 0:fd289b2e6b74 12 imu = new LSM9DS0(sda, scl, LSM9DS0_G, LSM9DS0_XM);
jnagendran3 0:fd289b2e6b74 13 _updateRate = updateRate;
jnagendran3 0:fd289b2e6b74 14 _biasRate = biasRate;
jnagendran3 0:fd289b2e6b74 15 /*
jnagendran3 0:fd289b2e6b74 16 biasTicker.attach(&this, &VelocityTracker::updateBias, _biasRate);
jnagendran3 0:fd289b2e6b74 17 updateTicker.attach(&this, &VelocityTracker::updateVelocity, _updateRate);
jnagendran3 0:fd289b2e6b74 18 */
jnagendran3 0:fd289b2e6b74 19 _bias[0] = 0;
jnagendran3 0:fd289b2e6b74 20 _bias[1] = 0;
jnagendran3 0:fd289b2e6b74 21 _bias[2] = 0;
jnagendran3 0:fd289b2e6b74 22 }
jnagendran3 0:fd289b2e6b74 23
jnagendran3 0:fd289b2e6b74 24 void VelocityTracker::updateBias() {
jnagendran3 0:fd289b2e6b74 25 imu->readAccel();
jnagendran3 0:fd289b2e6b74 26 _bias[0] += imu->ax;
jnagendran3 0:fd289b2e6b74 27 _bias[1] += imu->ay;
jnagendran3 0:fd289b2e6b74 28 _bias[2] += imu->az;
jnagendran3 0:fd289b2e6b74 29 sampleCount++;
jnagendran3 0:fd289b2e6b74 30 }
jnagendran3 0:fd289b2e6b74 31
jnagendran3 0:fd289b2e6b74 32 void VelocityTracker::updateVelocity() {
jnagendran3 0:fd289b2e6b74 33 float ax = _bias[0] / sampleCount;
jnagendran3 0:fd289b2e6b74 34 float ay = _bias[1] / sampleCount;
jnagendran3 0:fd289b2e6b74 35 float az = _bias[2] / sampleCount;
jnagendran3 0:fd289b2e6b74 36
jnagendran3 0:fd289b2e6b74 37 // Multiplying by 9.8 to convert from g's
jnagendran3 0:fd289b2e6b74 38 _vx += ax * 9.8 * _updateRate;
jnagendran3 0:fd289b2e6b74 39 _vy += ay * 9.8 * _updateRate;
jnagendran3 0:fd289b2e6b74 40 _vz += az * 9.8 * _updateRate;
jnagendran3 0:fd289b2e6b74 41
jnagendran3 0:fd289b2e6b74 42 _bias[0] = 0;
jnagendran3 0:fd289b2e6b74 43 _bias[1] = 0;
jnagendran3 0:fd289b2e6b74 44 _bias[2] = 0;
jnagendran3 0:fd289b2e6b74 45 sampleCount = 0;
jnagendran3 0:fd289b2e6b74 46 }
jnagendran3 0:fd289b2e6b74 47
jnagendran3 0:fd289b2e6b74 48 float VelocityTracker::getVelocity() {
jnagendran3 0:fd289b2e6b74 49 return sqrt(_vx*_vx + _vy*_vy); // Assuming on a flat surface
jnagendran3 0:fd289b2e6b74 50 }