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 #define kFileName           "/sys/class/i2c-adapter/i2c-3/3-001d/coord"
  12
  13 static int sampleIndex=0;
  14
  15 /**
  16  * Default constructor for Accelerometer class
  17  *
  18  */
  19 Accelerometer::Accelerometer()
  20 {
  21     calculate = new Calculate();
  22
  23     timer = new QTimer(this);
  24     connect(timer, SIGNAL(timeout()), this, SLOT(processData()));
  25     sampleRate = 40;
  26     initValues();
  27 }
  28
  29 /**
  30  * Constructor for Accelerometer class that takes accelerometer sample rate as parameter
  31  *
  32  * @param p_SampleRate the desired sample rate of accelerometer in milliseconds
  33  */
  34 Accelerometer::Accelerometer(int p_SampleRate)
  35 {
  36     calculate = new Calculate();
  37
  38     timer = new QTimer(this);
  39     connect(timer, SIGNAL(timeout()), this, SLOT(processData()));
  40     sampleRate = p_SampleRate;
  41     initValues();
  42 }
  43
  44 /**
  45  * Default destructor for Accelerometer class
  46  *
  47  */
  48 Accelerometer::~Accelerometer()
  49 {
  50 }
  51
  52 /**
  53  * Start measuring
  54  *
  55  */
  56 void Accelerometer::start()
  57 {
  58     initValues();
  59     calibrate();
  60     timer->start(sampleRate);
  61     now.restart();
  62 }
  63
  64 /**
  65  * Init class members
  66  *
  67  */
  68 void Accelerometer::initValues()
  69 {
  70     accelerationX=0;
  71     accelerationY=0;
  72     accelerationZ=0;
  73     trueAccelerationX=0;
  74     trueAccelerationY=0;
  75     trueAccelerationZ=0;
  76     previousAccelerationX=0;
  77     previousAccelerationY=0;
  78     previousAccelerationZ=0;
  79     previousSpeed=0;
  80     currentSpeed=0;
  81     currentAcceleration=0;
  82     previousAcceleration=0;
  83     totalAcceleration=0;
  84     intervalTime=0;
  85     totalTime=0;
  86     distanceTraveled=0;
  87     lastDistanceTraveled=0;
  88     averageSpeed=0;
  89     sampleRate=0;
  90     reverseAcceleration = false;
  91 }
  92
  93 /**
  94   * Calibrate. Purpose of this function is to calibrate
  95   * accelerometer when stationary.
  96   *
  97   */
  98 void Accelerometer::calibrate(void)
  99 {
 100     QFile file(kFileName);
 101     if(!file.open(QIODevice::ReadOnly | QIODevice::Text)) return;
 102
 103     unsigned int iteration = 0;
 104
 105     QByteArray line;
 106     QRegExp rx("([0-9-]+) ([0-9-]+) ([0-9-]+)");
 107
 108     do {
 109         // Read data, parse with regular expressions and process it
 110         line = file.readLine();
 111         rx.indexIn(line);
 112
 113         int sampleX = rx.cap(1).toInt();
 114         int sampleY = rx.cap(2).toInt();
 115         int sampleZ = rx.cap(3).toInt();
 116
 117         calibrationX += sampleX; // Accumulate Samples
 118         calibrationY += sampleY; // for all axes.
 119         calibrationZ += sampleZ;
 120
 121         iteration++;
 122     } while(iteration!=1024);       // 1024 times
 123
 124     calibrationX = calibrationX>>10;     // division by 1024
 125     calibrationY = calibrationY>>10;
 126     calibrationZ = calibrationZ>>10;
 127
 128     file.close();
 129 }
 130
 131 /**
 132  * Stop measuring
 133  *
 134  */
 135 void Accelerometer::stop()
 136 {
 137     timer->stop();
 138 }
 139
 140 /**
 141  * Set the sample rate of accelerometer
 142  *
 143  * @param pSampleRate desired sample rate
 144  */
 145 void Accelerometer::setSampleRate(int pSampleRate)
 146 {
 147     sampleRate = pSampleRate;
 148 }
 149
 150 /**
 151  * Get the sample rate of accelerometer
 152  *
 153  * @return sampleRate the sample rate of the accelerometer in milliseconds
 154  */
 155 int Accelerometer::getSampleRate()
 156 {
 157     return sampleRate;
 158 }
 159
 160 qreal Accelerometer::getCurrentAcceleration()
 161 {
 162     return currentAcceleration;
 163 }
 164
 165 qreal Accelerometer::getPreviousTotalAcceleration()
 166 {
 167     return previousAcceleration;
 168 }
 169
 170 qreal Accelerometer::getTotalAcceleration()
 171 {
 172     return totalAcceleration;
 173 }
 174
 175 qreal Accelerometer::getDistanceTraveled()
 176 {
 177     return distanceTraveled;
 178 }
 179
 180 qreal Accelerometer::getLastDistanceTraveled()
 181 {
 182     return lastDistanceTraveled;
 183 }
 184
 185 qreal Accelerometer::getAverageSpeed()
 186 {
 187     return averageSpeed;
 188 }
 189
 190 qreal Accelerometer::getTrueAccelerationX()
 191 {
 192     return trueAccelerationX;
 193 }
 194
 195 qreal Accelerometer::getTrueAccelerationY()
 196 {
 197     return trueAccelerationY;
 198 }
 199
 200 qreal Accelerometer::getTrueAccelerationZ()
 201 {
 202     return trueAccelerationZ;
 203 }
 204
 205 qreal Accelerometer::getPreviousSpeed()
 206 {
 207     return previousSpeed;
 208 }
 209
 210 qreal Accelerometer::getCurrentSpeed()
 211 {
 212     return currentSpeed;
 213 }
 214
 215 qreal Accelerometer::getIntervalTime()
 216 {
 217     return intervalTime;
 218 }
 219
 220 qreal Accelerometer::getTotalTime()
 221 {
 222     return totalTime;
 223 }
 224
 225 /**
 226  * Processes Accelerometer data
 227  *
 228  * Opens the accelerometer value file for reading and reads and parses accelerometer values.
 229  * Forwards data to Calculate class for processing
 230  *
 231  */
 232 void Accelerometer::processData()
 233 {
 234     QFile file(kFileName);
 235     if(!file.open(QIODevice::ReadOnly | QIODevice::Text)) return;
 236
 237     // Read data, parse with regular expressions and process it
 238     QByteArray line = file.readLine();
 239     QRegExp rx("([0-9-]+) ([0-9-]+) ([0-9-]+)");
 240     rx.indexIn(line);
 241
 242     smoothData(rx.cap(1).toInt(), rx.cap(2).toInt(), rx.cap(3).toInt());
 243
 244     // Apply calibration
 245     trueAccelerationX = accelerationX - calibrationX;
 246     trueAccelerationY = accelerationY - calibrationY;
 247     trueAccelerationZ = accelerationZ - calibrationZ;
 248
 249     // Discrimination window for acceleration values
 250     if (trueAccelerationX <= 30 && trueAccelerationX >= -30) { trueAccelerationX = 0; }
 251     if (trueAccelerationY <= 30 && trueAccelerationY >= -30) { trueAccelerationY = 0; }
 252     if (trueAccelerationZ <= 30 && trueAccelerationZ >= -30) { trueAccelerationZ = 0; }
 253
 254     trueAccelerationX = (accelerationX - previousAccelerationX) /*/ 1000 * kGravity*/;
 255     trueAccelerationY = (accelerationY - previousAccelerationY) /*/ 1000 * kGravity*/;
 256     trueAccelerationZ = (accelerationZ - previousAccelerationZ) /*/ 1000 * kGravity*/;
 257
 258     previousAccelerationX = accelerationX;
 259     previousAccelerationY = accelerationY;
 260     previousAccelerationZ = accelerationZ;
 261
 262     currentAcceleration = sqrt(trueAccelerationX * trueAccelerationX +
 263                                trueAccelerationY * trueAccelerationY +
 264                                trueAccelerationZ * trueAccelerationZ );
 265
 266     totalAcceleration = currentAcceleration - previousAcceleration;
 267     previousAcceleration = currentAcceleration;
 268
 269     // Measure time interval
 270     intervalTime = now.restart(); // millisecs to secs
 271     intervalTime = intervalTime/1000;
 272     totalTime = totalTime + intervalTime;
 273
 274     // Filter out acceleration caused by noise.
 275     if (fabs(currentAcceleration) < 0.09) {
 276         return;
 277     }
 278
 279     // Using calculate class to calculate velocity and distance etc.
 280     calculate->calculateParameters(currentAcceleration,intervalTime );
 281
 282     currentSpeed = calculate->getCurrentSpeed();
 283     distanceTraveled = calculate->getDistanceTraveled();
 284
 285     file.close();
 286 }
 287
 288 /**
 289  * Smooths Accelerometer data
 290  *
 291  * @param x accelerometer's x-axis input
 292  * @param y accelerometer's y-axis input
 293  * @param z accelerometer's z-axis input
 294  */
 295 void Accelerometer::smoothData(qreal x, qreal y, qreal z)
 296 {
 297     accelerationX = x;
 298     accelerationY = y;
 299     accelerationZ = z;
 300     if(sampleIndex>0) {
 301         accelerationX = previousAccelerationX + (accelerationX-previousAccelerationX) * kFilteringFactor;
 302         accelerationY = previousAccelerationY + (accelerationY-previousAccelerationY) * kFilteringFactor;
 303         accelerationZ = previousAccelerationZ + (accelerationZ-previousAccelerationZ) * kFilteringFactor;
 304     }
 305     sampleIndex++;
 306 }