[232] | 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 | }
|
---|