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
  12 #include "math.h"
  13
  14 #include <QFile>
  15 #include <QString>
  16 #include <QRegExp>
  17 #include <QTimer>
  18 #include <QDBusConnection>
  19 #include <QDBusInterface>
  20 #include <QDBusPendingReply>
  21
  22 #define kFilteringFactor    0.2
  23
  24 static int sampleIndex = 0;
  25
  26 /**
  27  * Default constructor for Accelerometer class
  28  *
  29  */
  30 Accelerometer::Accelerometer()
  31 {
  32     calculate = new Calculate();
  33
  34     timer = new QTimer(this);
  35     connect(timer, SIGNAL(timeout()), this, SLOT(processData()));
  36     sampleRate = 40;
  37     initValues();
  38 }
  39
  40 /**
  41  * Constructor for Accelerometer class that takes accelerometer sample rate as parameter
  42  *
  43  * @param p_SampleRate the desired sample rate of accelerometer in milliseconds
  44  */
  45 Accelerometer::Accelerometer(int p_SampleRate)
  46 {
  47     calculate = new Calculate();
  48
  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 /**
 138  * Stop measuring
 139  *
 140  */
 141 void Accelerometer::stop()
 142 {
 143     timer->stop();
 144 }
 145
 146 /**
 147  * Processes Accelerometer data
 148  *
 149  * Opens the accelerometer value file for reading and reads and parses accelerometer values.
 150  * Forwards data to Calculate class for processing
 151  *
 152  */
 153 /*
 154 void Accelerometer::processData()
 155 {
 156     getAcceleration(accelerationX, accelerationY, accelerationZ);
 157     //smoothData(accelerationX, accelerationY, accelerationZ);
 158
 159     // Apply calibration
 160     accelerationX = accelerationX - calibrationX;
 161     accelerationY = accelerationY - calibrationY;
 162     accelerationZ = accelerationZ - calibrationZ;
 163
 164     // If the function is run the first time, make sure that there
 165     // are no differences with previous and current acceleration
 166     if (firstRun) {
 167         previousAccelerationX = accelerationX;
 168         previousAccelerationY = accelerationY;
 169         previousAccelerationZ = accelerationZ;
 170         firstRun = false;
 171     }
 172
 173     // Calculate the current acceleration for each axis
 174     trueAccelerationX = (accelerationX - previousAccelerationX);
 175     trueAccelerationY = (accelerationY - previousAccelerationY);
 176     trueAccelerationZ = (accelerationZ - previousAccelerationZ);
 177
 178     previousAccelerationX = accelerationX;
 179     previousAccelerationY = accelerationY;
 180     previousAccelerationZ = accelerationZ;
 181
 182     currentAcceleration = sqrt((trueAccelerationX * trueAccelerationX) +
 183                                (trueAccelerationY * trueAccelerationY) +
 184                                (trueAccelerationZ * trueAccelerationZ));
 185
 186     // Discrimination window for currentAcceleration
 187     if ( fabs(currentAcceleration) < 0.10 ) { currentAcceleration = 0; }
 188
 189     // Measure time interval
 190     intervalTime = now.restart();
 191     intervalTime = intervalTime/1000; // millisecs to secs
 192     totalTime = totalTime + intervalTime;
 193
 194     // Using calculate class to calculate velocity and distance etc.
 195     calculate->calculateParameters(currentAcceleration,intervalTime );
 196
 197     currentSpeed = calculate->getCurrentSpeed();
 198     distanceTraveled = calculate->getDistanceTraveled();
 199 }
 200 */
 201 /**
 202  * Smooths Accelerometer data
 203  *
 204  * @param x accelerometer's x-axis input
 205  * @param y accelerometer's y-axis input
 206  * @param z accelerometer's z-axis input
 207  */
 208 void Accelerometer::smoothData(qreal &x, qreal &y, qreal &z)
 209 {
 210     accelerationX = x;
 211     accelerationY = y;
 212     accelerationZ = z;
 213
 214     if (sampleIndex > 0)
 215     {
 216         accelerationX = ((previousAccelerationX) * (1-kFilteringFactor)) + (accelerationX * kFilteringFactor);
 217         accelerationY = ((previousAccelerationY) * (1-kFilteringFactor)) + (accelerationY * kFilteringFactor);
 218         accelerationZ = ((previousAccelerationZ) * (1-kFilteringFactor)) + (accelerationZ * kFilteringFactor);
 219     }
 220     sampleIndex++;
 221 }
 222
 223 void Accelerometer::getAcceleration(qreal &x, qreal &y, qreal &z)
 224 {
 225     QDBusConnection connection(QDBusConnection::systemBus());
 226     if (connection.isConnected()) {
 227         QDBusInterface interface("com.nokia.mce", "/com/nokia/icd", QString(), connection);
 228         QDBusPendingReply<QString, QString, QString, int, int, int> reply;
 229         reply = interface.asyncCall("get_device_orientation");
 230         reply.waitForFinished();
 231         x = static_cast<qreal>(reply.argumentAt<3>()) / 1000;
 232         y = static_cast<qreal>(reply.argumentAt<4>()) / 1000;
 233         z = static_cast<qreal>(reply.argumentAt<5>()) / 1000;
 234     }
 235 }
 236
 237 /**
 238  * Set the sample rate of accelerometer
 239  *
 240  * @param pSampleRate desired sample rate
 241  */
 242 void Accelerometer::setSampleRate(int pSampleRate)
 243 {
 244     sampleRate = pSampleRate;
 245 }
 246
 247 /**
 248  * Get the sample rate of accelerometer
 249  *
 250  * @return sampleRate the sample rate of the accelerometer in milliseconds
 251  */
 252 int Accelerometer::getSampleRate()
 253 {
 254     return sampleRate;
 255 }
 256
 257 qreal Accelerometer::getCurrentAcceleration()
 258 {
 259     return currentAcceleration;
 260 }
 261
 262 qreal Accelerometer::getPreviousTotalAcceleration()
 263 {
 264     return previousAcceleration;
 265 }
 266
 267 qreal Accelerometer::getTotalAcceleration()
 268 {
 269     return totalAcceleration;
 270 }
 271
 272 qreal Accelerometer::getDistanceTraveled()
 273 {
 274     return distanceTraveled;
 275 }
 276
 277 qreal Accelerometer::getLastDistanceTraveled()
 278 {
 279     return lastDistanceTraveled;
 280 }
 281
 282 qreal Accelerometer::getAverageSpeed()
 283 {
 284     return averageSpeed;
 285 }
 286
 287 qreal Accelerometer::getTrueAccelerationX()
 288 {
 289     return trueAccelerationX;
 290 }
 291
 292 qreal Accelerometer::getTrueAccelerationY()
 293 {
 294     return trueAccelerationY;
 295 }
 296
 297 qreal Accelerometer::getTrueAccelerationZ()
 298 {
 299     return trueAccelerationZ;
 300 }
 301
 302 qreal Accelerometer::getPreviousSpeed()
 303 {
 304     return previousSpeed;
 305 }
 306
 307 qreal Accelerometer::getCurrentSpeed()
 308 {
 309     return currentSpeed;
 310 }
 311
 312 qreal Accelerometer::getIntervalTime()
 313 {
 314     return intervalTime;
 315 }
 316
 317 qreal Accelerometer::getTotalTime()
 318 {
 319     return totalTime;
 320 }
 321
 322 qreal Accelerometer::getCalibrationX()
 323 {
 324     return calibrationX;
 325 }
 326
 327 qreal Accelerometer::getCalibrationY()
 328 {
 329     return calibrationY;
 330 }
 331
 332 qreal Accelerometer::getCalibrationZ()
 333 {
 334     return calibrationZ;
 335 }