1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
|
#include "interfaces.h"
#include "legato.h"
#include "util.h"
static const char FormatStr[] =
"/sys/bus/i2c/devices/3-0068/iio:device0/in_%s_%s";
static const char AccType[] = "accel";
static const char GyroType[] = "anglvel";
static const char CompX[] = "x_raw";
static const char CompY[] = "y_raw";
static const char CompZ[] = "z_raw";
static const char CompScale[] = "scale";
typedef struct {
double x;
double y;
double z;
} Acceleration;
bool hasSuddenImpact = false;
Acceleration suddenImpact = {0, 0, 0};
/**
* Reports the x, y and z accelerometer readings in meters per second squared.
*/
le_result_t brnkl_motion_getCurrentAcceleration(double* xAcc,
double* yAcc,
double* zAcc) {
le_result_t r;
char path[256];
double scaling = 0.0;
int pathLen = snprintf(path, sizeof(path), FormatStr, AccType, CompScale);
LE_ASSERT(pathLen < sizeof(path));
r = ioutil_readDoubleFromFile(path, &scaling);
if (r != LE_OK) {
goto done;
}
pathLen = snprintf(path, sizeof(path), FormatStr, AccType, CompX);
LE_ASSERT(pathLen < sizeof(path));
r = ioutil_readDoubleFromFile(path, xAcc);
if (r != LE_OK) {
goto done;
}
*xAcc *= scaling;
pathLen = snprintf(path, sizeof(path), FormatStr, AccType, CompY);
LE_ASSERT(pathLen < sizeof(path));
r = ioutil_readDoubleFromFile(path, yAcc);
if (r != LE_OK) {
goto done;
}
*yAcc *= scaling;
pathLen = snprintf(path, sizeof(path), FormatStr, AccType, CompZ);
LE_ASSERT(pathLen < sizeof(path));
r = ioutil_readDoubleFromFile(path, zAcc);
*zAcc *= scaling;
done:
LE_INFO("Showing accel: x: %f, y: %f, z: %f", *xAcc, *yAcc, *zAcc);
return r;
}
le_result_t brnkl_motion_getSuddenImpact(double* xAcc,
double* yAcc,
double* zAcc) {
*xAcc = suddenImpact.x;
*yAcc = suddenImpact.y;
*zAcc = suddenImpact.z;
suddenImpact.x = 0;
suddenImpact.y = 0;
suddenImpact.z = 0;
hasSuddenImpact = false;
return LE_OK;
}
int8_t brnkl_motion_hasSuddenImpact() {
return hasSuddenImpact;
}
// take a reading but make sure its "larger" than
// whatever is already in the buffer
void interruptChangeHandler(bool state, void* ctx) {
double x, y, z;
le_result_t r = brnkl_motion_getCurrentAcceleration(&x, &y, &z);
LE_INFO("TRIGGERED x:%f y:%f z:%f", x, y, z);
if (r == LE_OK) {
bool updateX = fabs(x) > fabs(suddenImpact.x);
bool updateY = fabs(y) > fabs(suddenImpact.y);
bool updateZ = fabs(z) > fabs(suddenImpact.z);
if (updateX) {
suddenImpact.x = x;
}
if (updateY) {
suddenImpact.y = y;
}
if (updateZ) {
suddenImpact.z = z;
}
if (updateX || updateY || updateZ) {
hasSuddenImpact = true;
}
}
}
void initGpio() {
interrupt_SetInput(INTERRUPT_ACTIVE_HIGH);
interrupt_AddChangeEventHandler(INTERRUPT_EDGE_RISING, interruptChangeHandler,
NULL, 0);
}
COMPONENT_INIT {
initGpio();
}
|
