1 | #include <Wire.h>
|
---|
2 | #include <LSM303.h>
|
---|
3 | #include <RunningAverage.h>
|
---|
4 | #include <Accelerometer.h>
|
---|
5 |
|
---|
6 | // class Accelerometer -- member function definitions
|
---|
7 |
|
---|
8 | // enable accelerometer only
|
---|
9 | // to enable both accelerometer and magnetometer, call enableDefault() instead
|
---|
10 | void Accelerometer::enable(void)
|
---|
11 | {
|
---|
12 | // Enable Accelerometer
|
---|
13 | // 0x27 = 0b00100111
|
---|
14 | // Normal power mode, all axes enabled
|
---|
15 | writeAccReg(LSM303::CTRL_REG1_A, 0x27);
|
---|
16 |
|
---|
17 | if (getDeviceType() == LSM303::device_DLHC)
|
---|
18 | writeAccReg(LSM303::CTRL_REG4_A, 0x08); // DLHC: enable high resolution mode
|
---|
19 | }
|
---|
20 |
|
---|
21 | void Accelerometer::getLogHeader(void)
|
---|
22 | {
|
---|
23 | Serial.print("millis x y len dir | len_avg dir_avg | avg_len");
|
---|
24 | Serial.println();
|
---|
25 | }
|
---|
26 |
|
---|
27 | void Accelerometer::readAcceleration(unsigned long timestamp)
|
---|
28 | {
|
---|
29 | readAcc();
|
---|
30 | if (a.x == last.x && a.y == last.y) return;
|
---|
31 |
|
---|
32 | last.timestamp = timestamp;
|
---|
33 | last.x = a.x;
|
---|
34 | last.y = a.y;
|
---|
35 |
|
---|
36 | ra_x.addValue(last.x);
|
---|
37 | ra_y.addValue(last.y);
|
---|
38 |
|
---|
39 | #ifdef LOG_SERIAL
|
---|
40 | Serial.print(last.timestamp);
|
---|
41 | Serial.print(" ");
|
---|
42 | Serial.print(last.x);
|
---|
43 | Serial.print(" ");
|
---|
44 | Serial.print(last.y);
|
---|
45 | Serial.print(" ");
|
---|
46 | Serial.print(len_xy());
|
---|
47 | Serial.print(" ");
|
---|
48 | Serial.print(dir_xy());
|
---|
49 | Serial.print(" | ");
|
---|
50 | Serial.print(sqrt(static_cast<float>(ss_xy_avg())));
|
---|
51 | Serial.print(" ");
|
---|
52 | Serial.print(dir_xy_avg());
|
---|
53 | Serial.println();
|
---|
54 | #endif
|
---|
55 | }
|
---|
56 |
|
---|
57 | float Accelerometer::len_xy() const
|
---|
58 | {
|
---|
59 | return sqrt(last.x*a.x + last.y*a.y);
|
---|
60 | }
|
---|
61 |
|
---|
62 | float Accelerometer::dir_xy() const
|
---|
63 | {
|
---|
64 | return atan2(last.x, last.y) * 180.0 / M_PI;
|
---|
65 | }
|
---|
66 |
|
---|
67 | int Accelerometer::x_avg(void) const
|
---|
68 | {
|
---|
69 | return ra_x.getAverage();
|
---|
70 | }
|
---|
71 |
|
---|
72 | int Accelerometer::y_avg(void) const
|
---|
73 | {
|
---|
74 | return ra_y.getAverage();
|
---|
75 | }
|
---|
76 |
|
---|
77 | long Accelerometer::ss_xy_avg(void) const
|
---|
78 | {
|
---|
79 | long x_avg_long = static_cast<long>(x_avg());
|
---|
80 | long y_avg_long = static_cast<long>(y_avg());
|
---|
81 | return x_avg_long*x_avg_long + y_avg_long*y_avg_long;
|
---|
82 | }
|
---|
83 |
|
---|
84 | float Accelerometer::dir_xy_avg(void) const
|
---|
85 | {
|
---|
86 | return atan2(static_cast<float>(x_avg()), static_cast<float>(y_avg())) * 180.0 / M_PI;
|
---|
87 | }
|
---|