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