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