git.maemo.org Git - speedfreak/blob - Client/accelerometer.cpp

   1 #include "accelerometer.h"
   2 #include "math.h"
   3
   4 #include <QFile>
   5 #include <QString>
   6 #include <QRegExp>
   7 #include <QTimer>
   8
   9 #define kFilteringFactor    0.1
  10 #define kGravity            9.81
  11
  12 static int sampleIndex=0;
  13
  14 Accelerometer::Accelerometer()
  15 {
  16     QTimer *timer = new QTimer(this);
  17     connect(timer, SIGNAL(timeout()), this, SLOT(processData()));
  18     sampleRate = 100;
  19     timer->start(sampleRate);
  20     now.restart();
  21
  22     initValues();
  23 }
  24
  25 Accelerometer::~Accelerometer() {
  26 }
  27
  28 void Accelerometer::start() {
  29     timer->start(sampleRate);
  30     now.restart();
  31 }
  32
  33 void Accelerometer::initValues() {
  34     accelerationX=0;
  35     accelerationY=0;
  36     accelerationZ=0;
  37     trueAccelerationX=0;
  38     trueAccelerationY=0;
  39     trueAccelerationZ=0;
  40     previousAccelerationX=0;
  41     previousAccelerationY=0;
  42     previousAccelerationZ=0;
  43     previousSpeed=0;
  44     currentSpeed=0;
  45     currentAcceleration=0;
  46     previousAcceleration=0;
  47     totalAcceleration=0;
  48     intervalTime=0;
  49     totalTime=0;
  50     distanceTraveled=0;
  51     lastDistanceTraveled=0;
  52     averageSpeed=0;
  53     sampleRate=0;
  54 }
  55
  56 void Accelerometer::stop() {
  57     timer->stop();
  58 }
  59
  60 void Accelerometer::setSampleRate(int pSampleRate) {
  61     sampleRate = pSampleRate;
  62 }
  63
  64 int Accelerometer::getSampleRate() {
  65     return sampleRate;
  66 }
  67
  68 qreal Accelerometer::getCurrentAcceleration() {
  69     return currentAcceleration;
  70 }
  71
  72 qreal Accelerometer::getPreviousTotalAcceleration() {
  73     return previousAcceleration;
  74 }
  75
  76 qreal Accelerometer::getTotalAcceleration() {
  77     return totalAcceleration;
  78 }
  79
  80 qreal Accelerometer::getDistanceTraveled() {
  81     return distanceTraveled;
  82 }
  83
  84 qreal Accelerometer::getLastDistanceTraveled() {
  85     return lastDistanceTraveled;
  86 }
  87
  88 qreal Accelerometer::getAverageSpeed() {
  89     return averageSpeed;
  90 }
  91
  92 qreal Accelerometer::getTrueAccelerationX() {
  93     return trueAccelerationX;
  94 }
  95
  96 qreal Accelerometer::getTrueAccelerationY() {
  97     return trueAccelerationY;
  98 }
  99
 100 qreal Accelerometer::getTrueAccelerationZ() {
 101     return trueAccelerationZ;
 102 }
 103
 104 qreal Accelerometer::getPreviousSpeed() {
 105     return previousSpeed;
 106 }
 107
 108 qreal Accelerometer::getCurrentSpeed() {
 109     return currentSpeed;
 110 }
 111
 112 qreal Accelerometer::getintervalTime() {
 113     return intervalTime;
 114 }
 115
 116 /**
 117  * Processes Accelerometer data
 118  *
 119  */
 120 void Accelerometer::processData()
 121 {
 122     QFile file("/sys/class/i2c-adapter/i2c-3/3-001d/coord");
 123     if (!file.open(QIODevice::ReadOnly | QIODevice::Text))
 124         return;
 125
 126     // Read data, parse with regular expressions and process it
 127     QByteArray line = file.readLine();
 128     QRegExp rx("([0-9-]+) ([0-9-]+) ([0-9-]+)");
 129     rx.indexIn(line);
 130
 131     smoothData(rx.cap(1).toInt(), rx.cap(2).toInt(), rx.cap(3).toInt());
 132
 133     trueAccelerationX = (accelerationX - previousAccelerationX)/1000*kGravity;
 134     trueAccelerationY = (accelerationY - previousAccelerationY)/1000*kGravity;
 135     trueAccelerationZ = (accelerationZ - previousAccelerationZ)/1000*kGravity;
 136
 137     previousAccelerationX = accelerationX;
 138     previousAccelerationY = accelerationY;
 139     previousAccelerationZ = accelerationZ;
 140
 141     currentAcceleration = sqrt(trueAccelerationX * trueAccelerationX +
 142                            trueAccelerationY * trueAccelerationY +
 143                            trueAccelerationZ * trueAccelerationZ );
 144
 145     totalAcceleration = currentAcceleration - previousAcceleration;
 146
 147     totalAcceleration = fabs(totalAcceleration);
 148
 149     previousAcceleration = currentAcceleration;
 150
 151     // v = v0 + at
 152     // x = x0 + v0t + (at^2)/2
 153     // v = (v + v0)/2
 154
 155     intervalTime = now.restart();
 156     intervalTime = intervalTime/1000; // millisecs to secs
 157     totalTime = totalTime + intervalTime;
 158
 159     // filter noise
 160     // TODO: do this in smoothdata: implement a better filter.
 161     if (totalAcceleration > 0.02) {
 162         currentSpeed = ( previousSpeed + ( totalAcceleration * intervalTime ) / 2 );
 163     } else {
 164         currentSpeed = 0;
 165     }
 166
 167     // filter noise
 168     if (currentSpeed > 0.02) {
 169         distanceTraveled = ( lastDistanceTraveled + ( ( currentSpeed + previousSpeed ) * intervalTime) / 2 );
 170     } else {
 171         //distanceTraveled = 0;
 172     }
 173
 174     averageSpeed = distanceTraveled / totalTime;
 175
 176     previousSpeed = currentSpeed;
 177     lastDistanceTraveled = distanceTraveled;
 178
 179     file.close();
 180 }
 181
 182 /**
 183  * Smooths Accelerometer data
 184  *
 185  * @param x Accelerometers x-axis raw input
 186  * @param y Accelerometers y-axis raw input
 187  * @param z Accelerometers z-axis raw input
 188  */
 189 void Accelerometer::smoothData(qreal x, qreal y, qreal z) {
 190     accelerationX = x;
 191     accelerationY = y;
 192     accelerationZ = z;
 193     if(sampleIndex>0) {
 194         accelerationX = previousAccelerationX + (accelerationX-previousAccelerationX) * kFilteringFactor;
 195         accelerationY = previousAccelerationY + (accelerationY-previousAccelerationY) * kFilteringFactor;
 196         accelerationZ = previousAccelerationZ + (accelerationZ-previousAccelerationZ) * kFilteringFactor;
 197     }
 198     sampleIndex++;
 199 }