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     calculate->reset();
  71     accelerationX = 0;
  72     accelerationY = 0;
  73     accelerationZ = 0;
  74     trueAccelerationX = 0;
  75     trueAccelerationY = 0;
  76     trueAccelerationZ = 0;
  77     previousAccelerationX = 0;
  78     previousAccelerationY = 0;
  79     previousAccelerationZ = 0;
  80     previousSpeed = 0;
  81     currentSpeed = 0;
  82     currentAcceleration = 0;
  83     previousAcceleration = 0;
  84     totalAcceleration = 0;
  85     intervalTime = 0;
  86     totalTime = 0;
  87     distanceTraveled = 0;
  88     lastDistanceTraveled = 0;
  89     averageSpeed = 0;
  90     sampleRate = 0;
  91     firstRun = true;
  92     calibrationX = 0;
  93     calibrationY = 0;
  94     calibrationZ = 0;
  95 }
  96
  97 /**
  98   * Calibrate. Purpose of this function is to calibrate
  99   * accelerometer when stationary.
 100   *
 101   */
 102 void Accelerometer::calibrate(void)
 103 {
 104     unsigned int iteration = 0;
 105
 106     do {
 107         // Opening the file must be done inside the loop
 108         // or else the values obtained from file.readLine();
 109         // will be empty for all but the first iteration
 110         QFile file(kFileName);
 111         if(!file.open(QIODevice::ReadOnly | QIODevice::Text))
 112         {
 113             return;
 114         }
 115
 116         QByteArray line;
 117         QRegExp rx("([0-9-]+) ([0-9-]+) ([0-9-]+)");
 118
 119         // Read data, parse with regular expressions and process it
 120         line = file.readLine();
 121         rx.indexIn(line);
 122
 123         int sampleX = rx.cap(1).toInt();
 124         int sampleY = rx.cap(2).toInt();
 125         int sampleZ = rx.cap(3).toInt();
 126
 127         calibrationX = calibrationX + sampleX; // Accumulate Samples
 128         calibrationY = calibrationY + sampleY; // for all axes.
 129         calibrationZ = calibrationZ + sampleZ;
 130
 131         iteration++;
 132
 133         file.close();
 134     } while(iteration != 1024);       // 1024 times
 135
 136     calibrationX = calibrationX>>10;  // division by 1024
 137     calibrationY = calibrationY>>10;
 138     calibrationZ = calibrationZ>>10;
 139 }
 140
 141 /**
 142  * Stop measuring
 143  *
 144  */
 145 void Accelerometer::stop()
 146 {
 147     timer->stop();
 148 }
 149
 150 /**
 151  * Set the sample rate of accelerometer
 152  *
 153  * @param pSampleRate desired sample rate
 154  */
 155 void Accelerometer::setSampleRate(int pSampleRate)
 156 {
 157     sampleRate = pSampleRate;
 158 }
 159
 160 /**
 161  * Get the sample rate of accelerometer
 162  *
 163  * @return sampleRate the sample rate of the accelerometer in milliseconds
 164  */
 165 int Accelerometer::getSampleRate()
 166 {
 167     return sampleRate;
 168 }
 169
 170 qreal Accelerometer::getCurrentAcceleration()
 171 {
 172     return currentAcceleration;
 173 }
 174
 175 qreal Accelerometer::getPreviousTotalAcceleration()
 176 {
 177     return previousAcceleration;
 178 }
 179
 180 qreal Accelerometer::getTotalAcceleration()
 181 {
 182     return totalAcceleration;
 183 }
 184
 185 qreal Accelerometer::getDistanceTraveled()
 186 {
 187     return distanceTraveled;
 188 }
 189
 190 qreal Accelerometer::getLastDistanceTraveled()
 191 {
 192     return lastDistanceTraveled;
 193 }
 194
 195 qreal Accelerometer::getAverageSpeed()
 196 {
 197     return averageSpeed;
 198 }
 199
 200 qreal Accelerometer::getTrueAccelerationX()
 201 {
 202     return trueAccelerationX;
 203 }
 204
 205 qreal Accelerometer::getTrueAccelerationY()
 206 {
 207     return trueAccelerationY;
 208 }
 209
 210 qreal Accelerometer::getTrueAccelerationZ()
 211 {
 212     return trueAccelerationZ;
 213 }
 214
 215 qreal Accelerometer::getPreviousSpeed()
 216 {
 217     return previousSpeed;
 218 }
 219
 220 qreal Accelerometer::getCurrentSpeed()
 221 {
 222     return currentSpeed;
 223 }
 224
 225 qreal Accelerometer::getIntervalTime()
 226 {
 227     return intervalTime;
 228 }
 229
 230 qreal Accelerometer::getTotalTime()
 231 {
 232     return totalTime;
 233 }
 234
 235 int Accelerometer::getCalibrationX()
 236 {
 237     return calibrationX;
 238 }
 239
 240 int Accelerometer::getCalibrationY()
 241 {
 242     return calibrationY;
 243 }
 244
 245 int Accelerometer::getCalibrationZ()
 246 {
 247     return calibrationZ;
 248 }
 249
 250 /**
 251  * Processes Accelerometer data
 252  *
 253  * Opens the accelerometer value file for reading and reads and parses accelerometer values.
 254  * Forwards data to Calculate class for processing
 255  *
 256  */
 257 void Accelerometer::processData()
 258 {
 259     QFile file(kFileName);
 260     if(!file.open(QIODevice::ReadOnly | QIODevice::Text))
 261     {
 262         return;
 263     }
 264
 265     // Read data, parse with regular expressions and process it
 266     QByteArray line = file.readLine();
 267     QRegExp rx("([0-9-]+) ([0-9-]+) ([0-9-]+)");
 268     rx.indexIn(line);
 269
 270     smoothData(rx.cap(1).toInt(), rx.cap(2).toInt(), rx.cap(3).toInt());
 271
 272     // Apply calibration
 273     accelerationX = accelerationX - calibrationX;
 274     accelerationX = accelerationY - calibrationY;
 275     accelerationX = accelerationZ - calibrationZ;
 276
 277     // If the function is run the first time, make sure that there
 278     // are no differences with previous and current acceleration
 279     if (firstRun) {
 280         previousAccelerationX = accelerationX;
 281         previousAccelerationY = accelerationY;
 282         previousAccelerationZ = accelerationZ;
 283         firstRun = false;
 284     }
 285
 286     // Discrimination window for acceleration values
 287     /*if ( fabs(accelerationX) < 20 ) { accelerationX = 0; }
 288       if ( fabs(accelerationY) < 20 ) { accelerationY = 0; }
 289       if ( fabs(accelerationZ) < 20 ) { accelerationZ = 0; }*/
 290
 291     // Calculate the current acceleration for each axis
 292     trueAccelerationX = (accelerationX - previousAccelerationX) /*/ 1000 * kGravity*/;
 293     trueAccelerationY = (accelerationY - previousAccelerationY) /*/ 1000 * kGravity*/;
 294     trueAccelerationZ = (accelerationZ - previousAccelerationZ) /*/ 1000 * kGravity*/;
 295
 296     // Discrimination window for acceleration values
 297     if ( fabs(trueAccelerationX) < 20 ) { trueAccelerationX = 0; }
 298     if ( fabs(trueAccelerationY) < 20 ) { trueAccelerationY = 0; }
 299     if ( fabs(trueAccelerationZ) < 20 ) { trueAccelerationZ = 0; }
 300
 301     previousAccelerationX = accelerationX;
 302     previousAccelerationY = accelerationY;
 303     previousAccelerationZ = accelerationZ;
 304
 305     currentAcceleration = sqrt(trueAccelerationX * trueAccelerationX +
 306                                trueAccelerationY * trueAccelerationY +
 307                                trueAccelerationZ * trueAccelerationZ );
 308
 309     // Discrimination window for currentAcceleration
 310     if ( fabs(currentAcceleration) < 20 ) { currentAcceleration = 0; }
 311
 312     // Measure time interval
 313     intervalTime = now.restart();
 314     intervalTime = intervalTime/1000; // millisecs to secs
 315     totalTime = totalTime + intervalTime;
 316
 317     // Using calculate class to calculate velocity and distance etc.
 318     calculate->calculateParameters(currentAcceleration,intervalTime );
 319
 320     currentSpeed = calculate->getCurrentSpeed();
 321     distanceTraveled = calculate->getDistanceTraveled();
 322
 323     file.close();
 324 }
 325
 326 /**
 327  * Smooths Accelerometer data
 328  *
 329  * @param x accelerometer's x-axis input
 330  * @param y accelerometer's y-axis input
 331  * @param z accelerometer's z-axis input
 332  */
 333 void Accelerometer::smoothData(qreal x, qreal y, qreal z)
 334 {
 335     accelerationX = x;
 336     accelerationY = y;
 337     accelerationZ = z;
 338     if (sampleIndex > 0)
 339     {
 340         accelerationX = previousAccelerationX + (accelerationX-previousAccelerationX) * kFilteringFactor;
 341         accelerationY = previousAccelerationY + (accelerationY-previousAccelerationY) * kFilteringFactor;
 342         accelerationZ = previousAccelerationZ + (accelerationZ-previousAccelerationZ) * kFilteringFactor;
 343     }
 344     sampleIndex++;
 345 }
 346