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)) return;
 242
 243     // Read data, parse with regular expressions and process it
 244     QByteArray line = file.readLine();
 245     QRegExp rx("([0-9-]+) ([0-9-]+) ([0-9-]+)");
 246     rx.indexIn(line);
 247
 248     smoothData(rx.cap(1).toInt(), rx.cap(2).toInt(), rx.cap(3).toInt());
 249
 250     // Apply calibration
 251     trueAccelerationX = accelerationX - calibrationX;
 252     trueAccelerationY = accelerationY - calibrationY;
 253     trueAccelerationZ = accelerationZ - calibrationZ;
 254
 255     // Discrimination window for acceleration values
 256     if (trueAccelerationX <= 30 && trueAccelerationX >= -30) { trueAccelerationX = 0; }
 257     if (trueAccelerationY <= 30 && trueAccelerationY >= -30) { trueAccelerationY = 0; }
 258     if (trueAccelerationZ <= 30 && trueAccelerationZ >= -30) { trueAccelerationZ = 0; }
 259
 260     trueAccelerationX = (accelerationX - previousAccelerationX) /*/ 1000 * kGravity*/;
 261     trueAccelerationY = (accelerationY - previousAccelerationY) /*/ 1000 * kGravity*/;
 262     trueAccelerationZ = (accelerationZ - previousAccelerationZ) /*/ 1000 * kGravity*/;
 263
 264     previousAccelerationX = accelerationX;
 265     previousAccelerationY = accelerationY;
 266     previousAccelerationZ = accelerationZ;
 267
 268     currentAcceleration = sqrt(trueAccelerationX * trueAccelerationX +
 269                                trueAccelerationY * trueAccelerationY +
 270                                trueAccelerationZ * trueAccelerationZ );
 271
 272     totalAcceleration = currentAcceleration - previousAcceleration;
 273     previousAcceleration = currentAcceleration;
 274
 275     // Measure time interval
 276     intervalTime = now.restart(); // millisecs to secs
 277     intervalTime = intervalTime/1000;
 278     totalTime = totalTime + intervalTime;
 279
 280     // Filter out acceleration caused by noise.
 281     if (fabs(currentAcceleration) < 0.09) {
 282         return;
 283     }
 284
 285     // Using calculate class to calculate velocity and distance etc.
 286     calculate->calculateParameters(currentAcceleration,intervalTime );
 287
 288     currentSpeed = calculate->getCurrentSpeed();
 289     distanceTraveled = calculate->getDistanceTraveled();
 290
 291     file.close();
 292 }
 293
 294 /**
 295  * Smooths Accelerometer data
 296  *
 297  * @param x accelerometer's x-axis input
 298  * @param y accelerometer's y-axis input
 299  * @param z accelerometer's z-axis input
 300  */
 301 void Accelerometer::smoothData(qreal x, qreal y, qreal z)
 302 {
 303     accelerationX = x;
 304     accelerationY = y;
 305     accelerationZ = z;
 306     if(sampleIndex>0) {
 307         accelerationX = previousAccelerationX + (accelerationX-previousAccelerationX) * kFilteringFactor;
 308         accelerationY = previousAccelerationY + (accelerationY-previousAccelerationY) * kFilteringFactor;
 309         accelerationZ = previousAccelerationZ + (accelerationZ-previousAccelerationZ) * kFilteringFactor;
 310     }
 311     sampleIndex++;
 312 }