Judah Okeleye / Mbed 2 deprecated SLVM

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed-rtos mbed

VelocityTracker.cpp@0:354a8831107d, 2014-12-03 (annotated)

Committer:
jerziboi732
Date:
Wed Dec 03 22:33:03 2014 +0000
Revision:
0:354a8831107d
Initial commit

Who changed what in which revision?

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