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

   1 /*
   2  * Accelerometer class to access the device accelerometer
   3  *
   4  * @author      Rikhard Kuutti <rikhard.kuutti@fudeco.com>
   5  * @author      Kai Rasilainen
   6  * @copyright   (c) 2010 Speed Freak team
   7  * @license     http://opensource.org/licenses/gpl-license.php GNU Public License
   8  */
   9
  10 #include "accelerometer.h"
  11 #include "math.h"
  12
  13 #include <QFile>
  14 #include <QString>
  15 #include <QRegExp>
  16 #include <QTimer>
  17
  18 #define kFilteringFactor    0.1
  19 #define kGravity            9.81
  20 #define kFileName           "/sys/class/i2c-adapter/i2c-3/3-001d/coord"
  21
  22 static int sampleIndex=0;
  23
  24 /**
  25  * Default constructor for Accelerometer class
  26  *
  27  */
  28 Accelerometer::Accelerometer()
  29 {
  30     calculate = new Calculate();
  31
  32     timer = new QTimer(this);
  33     connect(timer, SIGNAL(timeout()), this, SLOT(processData()));
  34     sampleRate = 40;
  35     initValues();
  36 }
  37
  38 /**
  39  * Constructor for Accelerometer class that takes accelerometer sample rate as parameter
  40  *
  41  * @param p_SampleRate the desired sample rate of accelerometer in milliseconds
  42  */
  43 Accelerometer::Accelerometer(int p_SampleRate)
  44 {
  45     calculate = new Calculate();
  46
  47     timer = new QTimer(this);
  48     connect(timer, SIGNAL(timeout()), this, SLOT(processData()));
  49     sampleRate = p_SampleRate;
  50     initValues();
  51 }
  52
  53 /**
  54  * Default destructor for Accelerometer class
  55  *
  56  */
  57 Accelerometer::~Accelerometer()
  58 {
  59 }
  60
  61 /**
  62  * Start measuring
  63  *
  64  */
  65 void Accelerometer::start()
  66 {
  67     initValues();
  68     calibrate();
  69     timer->start(sampleRate);
  70     now.restart();
  71 }
  72
  73 /**
  74  * Init class members
  75  *
  76  */
  77 void Accelerometer::initValues()
  78 {
  79     calculate->reset();
  80     accelerationX = 0;
  81     accelerationY = 0;
  82     accelerationZ = 0;
  83     trueAccelerationX = 0;
  84     trueAccelerationY = 0;
  85     trueAccelerationZ = 0;
  86     previousAccelerationX = 0;
  87     previousAccelerationY = 0;
  88     previousAccelerationZ = 0;
  89     previousSpeed = 0;
  90     currentSpeed = 0;
  91     currentAcceleration = 0;
  92     previousAcceleration = 0;
  93     totalAcceleration = 0;
  94     intervalTime = 0;
  95     totalTime = 0;
  96     distanceTraveled = 0;
  97     lastDistanceTraveled = 0;
  98     averageSpeed = 0;
  99     sampleRate = 0;
 100     firstRun = true;
 101     calibrationX = 0;
 102     calibrationY = 0;
 103     calibrationZ = 0;
 104 }
 105
 106 /**
 107   * Calibrate. Purpose of this function is to calibrate
 108   * accelerometer when stationary.
 109   *
 110   */
 111 void Accelerometer::calibrate(void)
 112 {
 113     unsigned int iteration = 0;
 114
 115     do {
 116         // Opening the file must be done inside the loop
 117         // or else the values obtained from file.readLine();
 118         // will be empty for all but the first iteration
 119         QFile file(kFileName);
 120         if(!file.open(QIODevice::ReadOnly | QIODevice::Text))
 121         {
 122             return;
 123         }
 124
 125         QByteArray line;
 126         QRegExp rx("([0-9-]+) ([0-9-]+) ([0-9-]+)");
 127
 128         // Read data, parse with regular expressions and process it
 129         line = file.readLine();
 130         rx.indexIn(line);
 131
 132         int sampleX = rx.cap(1).toInt();
 133         int sampleY = rx.cap(2).toInt();
 134         int sampleZ = rx.cap(3).toInt();
 135
 136         calibrationX = calibrationX + sampleX; // Accumulate Samples
 137         calibrationY = calibrationY + sampleY; // for all axes.
 138         calibrationZ = calibrationZ + sampleZ;
 139
 140         iteration++;
 141
 142         file.close();
 143     } while(iteration != 1024);       // 1024 times
 144
 145     calibrationX = calibrationX>>10;  // division by 1024
 146     calibrationY = calibrationY>>10;
 147     calibrationZ = calibrationZ>>10;
 148 }
 149
 150 /**
 151  * Stop measuring
 152  *
 153  */
 154 void Accelerometer::stop()
 155 {
 156     timer->stop();
 157 }
 158
 159 /**
 160  * Set the sample rate of accelerometer
 161  *
 162  * @param pSampleRate desired sample rate
 163  */
 164 void Accelerometer::setSampleRate(int pSampleRate)
 165 {
 166     sampleRate = pSampleRate;
 167 }
 168
 169 /**
 170  * Get the sample rate of accelerometer
 171  *
 172  * @return sampleRate the sample rate of the accelerometer in milliseconds
 173  */
 174 int Accelerometer::getSampleRate()
 175 {
 176     return sampleRate;
 177 }
 178
 179 qreal Accelerometer::getCurrentAcceleration()
 180 {
 181     return currentAcceleration;
 182 }
 183
 184 qreal Accelerometer::getPreviousTotalAcceleration()
 185 {
 186     return previousAcceleration;
 187 }
 188
 189 qreal Accelerometer::getTotalAcceleration()
 190 {
 191     return totalAcceleration;
 192 }
 193
 194 qreal Accelerometer::getDistanceTraveled()
 195 {
 196     return distanceTraveled;
 197 }
 198
 199 qreal Accelerometer::getLastDistanceTraveled()
 200 {
 201     return lastDistanceTraveled;
 202 }
 203
 204 qreal Accelerometer::getAverageSpeed()
 205 {
 206     return averageSpeed;
 207 }
 208
 209 qreal Accelerometer::getTrueAccelerationX()
 210 {
 211     return trueAccelerationX;
 212 }
 213
 214 qreal Accelerometer::getTrueAccelerationY()
 215 {
 216     return trueAccelerationY;
 217 }
 218
 219 qreal Accelerometer::getTrueAccelerationZ()
 220 {
 221     return trueAccelerationZ;
 222 }
 223
 224 qreal Accelerometer::getPreviousSpeed()
 225 {
 226     return previousSpeed;
 227 }
 228
 229 qreal Accelerometer::getCurrentSpeed()
 230 {
 231     return currentSpeed;
 232 }
 233
 234 qreal Accelerometer::getIntervalTime()
 235 {
 236     return intervalTime;
 237 }
 238
 239 qreal Accelerometer::getTotalTime()
 240 {
 241     return totalTime;
 242 }
 243
 244 int Accelerometer::getCalibrationX()
 245 {
 246     return calibrationX;
 247 }
 248
 249 int Accelerometer::getCalibrationY()
 250 {
 251     return calibrationY;
 252 }
 253
 254 int Accelerometer::getCalibrationZ()
 255 {
 256     return calibrationZ;
 257 }
 258
 259 /**
 260  * Processes Accelerometer data
 261  *
 262  * Opens the accelerometer value file for reading and reads and parses accelerometer values.
 263  * Forwards data to Calculate class for processing
 264  *
 265  */
 266 void Accelerometer::processData()
 267 {
 268     QFile file(kFileName);
 269     if(!file.open(QIODevice::ReadOnly | QIODevice::Text))
 270     {
 271         return;
 272     }
 273
 274     // Read data, parse with regular expressions and process it
 275     QByteArray line = file.readLine();
 276     QRegExp rx("([0-9-]+) ([0-9-]+) ([0-9-]+)");
 277     rx.indexIn(line);
 278
 279     smoothData(rx.cap(1).toInt(), rx.cap(2).toInt(), rx.cap(3).toInt());
 280
 281     // Apply calibration
 282     accelerationX = accelerationX - calibrationX;
 283     accelerationX = accelerationY - calibrationY;
 284     accelerationX = accelerationZ - calibrationZ;
 285
 286     // If the function is run the first time, make sure that there
 287     // are no differences with previous and current acceleration
 288     if (firstRun) {
 289         previousAccelerationX = accelerationX;
 290         previousAccelerationY = accelerationY;
 291         previousAccelerationZ = accelerationZ;
 292         firstRun = false;
 293     }
 294
 295     // Discrimination window for acceleration values
 296     /*if ( fabs(accelerationX) < 20 ) { accelerationX = 0; }
 297       if ( fabs(accelerationY) < 20 ) { accelerationY = 0; }
 298       if ( fabs(accelerationZ) < 20 ) { accelerationZ = 0; }*/
 299
 300     // Calculate the current acceleration for each axis
 301     trueAccelerationX = (accelerationX - previousAccelerationX) /*/ 1000 * kGravity*/;
 302     trueAccelerationY = (accelerationY - previousAccelerationY) /*/ 1000 * kGravity*/;
 303     trueAccelerationZ = (accelerationZ - previousAccelerationZ) /*/ 1000 * kGravity*/;
 304
 305     // Discrimination window for acceleration values
 306     if ( fabs(trueAccelerationX) < 20 ) { trueAccelerationX = 0; }
 307     if ( fabs(trueAccelerationY) < 20 ) { trueAccelerationY = 0; }
 308     if ( fabs(trueAccelerationZ) < 20 ) { trueAccelerationZ = 0; }
 309
 310     previousAccelerationX = accelerationX;
 311     previousAccelerationY = accelerationY;
 312     previousAccelerationZ = accelerationZ;
 313
 314     currentAcceleration = sqrt(trueAccelerationX * trueAccelerationX +
 315                                trueAccelerationY * trueAccelerationY +
 316                                trueAccelerationZ * trueAccelerationZ );
 317
 318     // Discrimination window for currentAcceleration
 319     if ( fabs(currentAcceleration) < 20 ) { currentAcceleration = 0; }
 320
 321     // Measure time interval
 322     intervalTime = now.restart();
 323     intervalTime = intervalTime/1000; // millisecs to secs
 324     totalTime = totalTime + intervalTime;
 325
 326     // Using calculate class to calculate velocity and distance etc.
 327     calculate->calculateParameters(currentAcceleration,intervalTime );
 328
 329     currentSpeed = calculate->getCurrentSpeed();
 330     distanceTraveled = calculate->getDistanceTraveled();
 331
 332     file.close();
 333 }
 334
 335 /**
 336  * Smooths Accelerometer data
 337  *
 338  * @param x accelerometer's x-axis input
 339  * @param y accelerometer's y-axis input
 340  * @param z accelerometer's z-axis input
 341  */
 342 void Accelerometer::smoothData(qreal x, qreal y, qreal z)
 343 {
 344     accelerationX = x;
 345     accelerationY = y;
 346     accelerationZ = z;
 347     if (sampleIndex > 0)
 348     {
 349         accelerationX = previousAccelerationX + (accelerationX-previousAccelerationX) * kFilteringFactor;
 350         accelerationY = previousAccelerationY + (accelerationY-previousAccelerationY) * kFilteringFactor;
 351         accelerationZ = previousAccelerationZ + (accelerationZ-previousAccelerationZ) * kFilteringFactor;
 352     }
 353     sampleIndex++;
 354 }
 355