a
[speedfreak] / 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