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