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 Accelerometer::Accelerometer(int p_SampleRate)
  30 {
  31     calculate = new Calculate();
  32
  33     timer = new QTimer(this);
  34     connect(timer, SIGNAL(timeout()), this, SLOT(processData()));
  35     sampleRate = p_SampleRate;
  36     initValues();
  37 }
  38
  39 /**
  40  * Default destructor for Accelerometer class
  41  *
  42  */
  43 Accelerometer::~Accelerometer() {
  44 }
  45
  46 /**
  47  * Start measuring
  48  *
  49  */
  50 void Accelerometer::start() {
  51     initValues();
  52     calibrate();
  53     timer->start(sampleRate);
  54     now.restart();
  55 }
  56
  57 /**
  58  * Init class members
  59  *
  60  */
  61 void Accelerometer::initValues() {
  62     accelerationX=0;
  63     accelerationY=0;
  64     accelerationZ=0;
  65     trueAccelerationX=0;
  66     trueAccelerationY=0;
  67     trueAccelerationZ=0;
  68     previousAccelerationX=0;
  69     previousAccelerationY=0;
  70     previousAccelerationZ=0;
  71     previousSpeed=0;
  72     currentSpeed=0;
  73     currentAcceleration=0;
  74     previousAcceleration=0;
  75     totalAcceleration=0;
  76     intervalTime=0;
  77     totalTime=0;
  78     distanceTraveled=0;
  79     lastDistanceTraveled=0;
  80     averageSpeed=0;
  81     sampleRate=0;
  82     reverseAcceleration = false;
  83 }
  84
  85 /**
  86   * Calibrate. Purpose of this function is to calibrate
  87   * accelerometer when stationary.
  88   *
  89   */
  90 void Accelerometer::calibrate(void) {
  91     QFile file(kFileName);
  92     if(!file.open(QIODevice::ReadOnly | QIODevice::Text)) return;
  93
  94     unsigned int iteration;
  95     iteration = 0;
  96     QByteArray line;
  97     QRegExp rx("([0-9-]+) ([0-9-]+) ([0-9-]+)");
  98
  99     do{
 100         // Read data, parse with regular expressions and process it
 101         line = file.readLine();
 102         rx.indexIn(line);
 103
 104         int Sample_X = rx.cap(1).toInt();
 105         int Sample_Y = rx.cap(2).toInt();
 106         int Sample_Z = rx.cap(3).toInt();
 107
 108         sstatex += Sample_X; // Accumulate Samples
 109         sstatey += Sample_Y; // for all axes.
 110         sstatez += Sample_Z;
 111
 112         iteration++;
 113     }while(iteration!=1024);       // 1024 times
 114
 115     sstatex = sstatex>>10;           // division by 1024
 116     sstatey = sstatey>>10;
 117     sstatez = sstatez>>10;
 118
 119     file.close();
 120 }
 121
 122 /**
 123  * Stop measuring
 124  *
 125  */
 126 void Accelerometer::stop() {
 127     timer->stop();
 128 }
 129
 130 /**
 131  * Set the sample rate of accelerometer
 132  *
 133  * @param int pSampleRate desired sample rate
 134  */
 135 void Accelerometer::setSampleRate(int pSampleRate) {
 136     sampleRate = pSampleRate;
 137 }
 138
 139 int Accelerometer::getSampleRate() {
 140     return sampleRate;
 141 }
 142
 143 qreal Accelerometer::getCurrentAcceleration() {
 144     return currentAcceleration;
 145 }
 146
 147 qreal Accelerometer::getPreviousTotalAcceleration() {
 148     return previousAcceleration;
 149 }
 150
 151 qreal Accelerometer::getTotalAcceleration() {
 152     return totalAcceleration;
 153 }
 154
 155 qreal Accelerometer::getDistanceTraveled() {
 156     return distanceTraveled;
 157 }
 158
 159 qreal Accelerometer::getLastDistanceTraveled() {
 160     return lastDistanceTraveled;
 161 }
 162
 163 qreal Accelerometer::getAverageSpeed() {
 164     return averageSpeed;
 165 }
 166
 167 qreal Accelerometer::getTrueAccelerationX() {
 168     return trueAccelerationX;
 169 }
 170
 171 qreal Accelerometer::getTrueAccelerationY() {
 172     return trueAccelerationY;
 173 }
 174
 175 qreal Accelerometer::getTrueAccelerationZ() {
 176     return trueAccelerationZ;
 177 }
 178
 179 qreal Accelerometer::getPreviousSpeed() {
 180     return previousSpeed;
 181 }
 182
 183 qreal Accelerometer::getCurrentSpeed() {
 184     return currentSpeed;
 185 }
 186
 187 qreal Accelerometer::getIntervalTime() {
 188     return intervalTime;
 189 }
 190
 191 /**
 192  * Processes Accelerometer data
 193  *
 194  */
 195 void Accelerometer::processData()
 196 {
 197     QFile file(kFileName);
 198     if(!file.open(QIODevice::ReadOnly | QIODevice::Text)) return;
 199
 200     // Read data, parse with regular expressions and process it
 201     QByteArray line = file.readLine();
 202     //QByteArray line;
 203     QRegExp rx("([0-9-]+) ([0-9-]+) ([0-9-]+)");
 204     rx.indexIn(line);
 205
 206     smoothData(rx.cap(1).toInt(), rx.cap(2).toInt(), rx.cap(3).toInt());
 207
 208     // Add calibration
 209     trueAccelerationX = accelerationX - sstatex;
 210     trueAccelerationY = accelerationY - sstatey;
 211     trueAccelerationZ = accelerationZ - sstatez;
 212
 213     // Discrimination window for acceleration values
 214     if (trueAccelerationX <= 30 && trueAccelerationX >= -30) { trueAccelerationX = 0; }
 215     if (trueAccelerationY <= 30 && trueAccelerationY >= -30) { trueAccelerationY = 0; }
 216     if (trueAccelerationZ <= 30 && trueAccelerationZ >= -30) { trueAccelerationZ = 0; }
 217
 218     trueAccelerationX = (accelerationX - previousAccelerationX) / 1000 * kGravity;
 219     trueAccelerationY = (accelerationY - previousAccelerationY) / 1000 * kGravity;
 220     trueAccelerationZ = (accelerationZ - previousAccelerationZ) / 1000 * kGravity;
 221
 222     previousAccelerationX = accelerationX;
 223     previousAccelerationY = accelerationY;
 224     previousAccelerationZ = accelerationZ;
 225
 226     currentAcceleration = sqrt(trueAccelerationX * trueAccelerationX +
 227                                trueAccelerationY * trueAccelerationY +
 228                                trueAccelerationZ * trueAccelerationZ );
 229
 230     totalAcceleration = currentAcceleration - previousAcceleration;
 231     previousAcceleration = currentAcceleration;
 232
 233     // Measure time interval
 234     intervalTime = now.restart(); // millisecs to secs
 235     intervalTime = intervalTime/1000;
 236     totalTime = totalTime + intervalTime;
 237
 238     // Filter out acceleration caused by noise.
 239     if (fabs(currentAcceleration) < 0.09) {
 240         return;
 241     }
 242
 243     // Using calculate class to calculate velocity and distance etc.
 244     calculate->CalculateParameters(currentAcceleration,intervalTime );
 245
 246     currentSpeed = calculate->CurrentSpeed();
 247     distanceTraveled = calculate->DistanceTraveled();
 248     file.close();
 249 }
 250
 251 /**
 252  * Smooths Accelerometer data
 253  *
 254  * @param qreal x Accelerometers x-axis raw input
 255  * @param qreal y Accelerometers y-axis raw input
 256  * @param qreal z Accelerometers z-axis raw input
 257  */
 258 void Accelerometer::smoothData(qreal x, qreal y, qreal z) {
 259     accelerationX = x;
 260     accelerationY = y;
 261     accelerationZ = z;
 262     if(sampleIndex>0) {
 263         accelerationX = previousAccelerationX + (accelerationX-previousAccelerationX) * kFilteringFactor;
 264         accelerationY = previousAccelerationY + (accelerationY-previousAccelerationY) * kFilteringFactor;
 265         accelerationZ = previousAccelerationZ + (accelerationZ-previousAccelerationZ) * kFilteringFactor;
 266     }
 267     sampleIndex++;
 268 }