1 /*
2 * Copyright (C) 2016 The Android Open Source Project
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17 #include <stdlib.h>
18 #include <string.h>
19 #include <timer.h>
20 #include <heap.h>
21 #include <plat/rtc.h>
22 #include <plat/syscfg.h>
23 #include <hostIntf.h>
24 #include <nanohubPacket.h>
25
26 #include <seos.h>
27
28 #include <nanohub_math.h>
29 #include <sensors.h>
30 #include <limits.h>
31
32 #define TILT_APP_VERSION 1
33
34 #define EVT_SENSOR_ANY_MOTION sensorGetMyEventType(SENS_TYPE_ANY_MOTION)
35 #define EVT_SENSOR_NO_MOTION sensorGetMyEventType(SENS_TYPE_NO_MOTION)
36 #define EVT_SENSOR_ACCEL sensorGetMyEventType(SENS_TYPE_ACCEL)
37
38 #define ACCEL_MIN_RATE SENSOR_HZ(50)
39 #define ACCEL_MAX_LATENCY 250000000ull // 250 ms
40
41 #define BATCH_TIME 2000000000ull // 2.0 seconds
42 #define ANGLE_THRESH (0.819 * 9.81 * 9.81) // ~cos(35) * (1G in m/s^2)^2
43
44 struct TiltAlgoState {
45 uint64_t this_batch_init_ts;
46 uint32_t this_batch_num_samples;
47 float this_batch_sample_sum[3];
48 float this_batch_g[3];
49 float last_ref_g_vector[3];
50 bool last_ref_g_vector_valid;
51 bool anamoly_this_batch;
52 bool tilt_detected;
53 };
54
55 static struct TiltDetectionTask {
56 struct TiltAlgoState algoState;
57 uint32_t taskId;
58 uint32_t handle;
59 uint32_t anyMotionHandle;
60 uint32_t noMotionHandle;
61 uint32_t accelHandle;
62 enum {
63 STATE_DISABLED,
64 STATE_AWAITING_ANY_MOTION,
65 STATE_AWAITING_TILT,
66 } taskState;
67 } mTask;
68
69 // *****************************************************************************
70
algoInit()71 static void algoInit()
72 {
73 // nothing here
74 }
75
algoUpdate(struct TripleAxisDataEvent * ev)76 static bool algoUpdate(struct TripleAxisDataEvent *ev)
77 {
78 float dotProduct = 0.0f;
79 uint64_t dt;
80 bool latch_g_vector = false;
81 bool tilt_detected = false;
82 struct TiltAlgoState *state = &mTask.algoState;
83 uint64_t sample_ts = ev->referenceTime;
84 uint32_t numSamples = ev->samples[0].firstSample.numSamples;
85 uint32_t i;
86 struct TripleAxisDataPoint *sample;
87 float invN;
88
89 for (i = 0; i < numSamples; i++) {
90 sample = &ev->samples[i];
91 if (i > 0)
92 sample_ts += sample->deltaTime;
93
94 if (state->this_batch_init_ts == 0) {
95 state->this_batch_init_ts = sample_ts;
96 }
97
98 state->this_batch_sample_sum[0] += sample->x;
99 state->this_batch_sample_sum[1] += sample->y;
100 state->this_batch_sample_sum[2] += sample->z;
101
102 state->this_batch_num_samples++;
103
104 dt = (sample_ts - state->this_batch_init_ts);
105
106 if (dt > BATCH_TIME) {
107 invN = 1.0f / state->this_batch_num_samples;
108 state->this_batch_g[0] = state->this_batch_sample_sum[0] * invN;
109 state->this_batch_g[1] = state->this_batch_sample_sum[1] * invN;
110 state->this_batch_g[2] = state->this_batch_sample_sum[2] * invN;
111
112 if (state->last_ref_g_vector_valid) {
113 dotProduct = state->this_batch_g[0] * state->last_ref_g_vector[0] +
114 state->this_batch_g[1] * state->last_ref_g_vector[1] +
115 state->this_batch_g[2] * state->last_ref_g_vector[2];
116
117 if (dotProduct < ANGLE_THRESH) {
118 tilt_detected = true;
119 latch_g_vector = true;
120 }
121 } else { // reference g vector not valid, first time computing
122 latch_g_vector = true;
123 state->last_ref_g_vector_valid = true;
124 }
125
126 // latch the first batch or when dotProduct < ANGLE_THRESH
127 if (latch_g_vector) {
128 state->last_ref_g_vector[0] = state->this_batch_g[0];
129 state->last_ref_g_vector[1] = state->this_batch_g[1];
130 state->last_ref_g_vector[2] = state->this_batch_g[2];
131 }
132
133 // Seed the next batch
134 state->this_batch_init_ts = 0;
135 state->this_batch_num_samples = 0;
136 state->this_batch_sample_sum[0] = 0;
137 state->this_batch_sample_sum[1] = 0;
138 state->this_batch_sample_sum[2] = 0;
139 }
140 }
141
142 return tilt_detected;
143 }
144
configAnyMotion(bool on)145 static void configAnyMotion(bool on) {
146 if (on) {
147 sensorRequest(mTask.taskId, mTask.anyMotionHandle, SENSOR_RATE_ONCHANGE, 0);
148 osEventSubscribe(mTask.taskId, EVT_SENSOR_ANY_MOTION);
149 } else {
150 sensorRelease(mTask.taskId, mTask.anyMotionHandle);
151 osEventUnsubscribe(mTask.taskId, EVT_SENSOR_ANY_MOTION);
152 }
153 }
154
configNoMotion(bool on)155 static void configNoMotion(bool on) {
156 if (on) {
157 sensorRequest(mTask.taskId, mTask.noMotionHandle, SENSOR_RATE_ONCHANGE, 0);
158 osEventSubscribe(mTask.taskId, EVT_SENSOR_NO_MOTION);
159 } else {
160 sensorRelease(mTask.taskId, mTask.noMotionHandle);
161 osEventUnsubscribe(mTask.taskId, EVT_SENSOR_NO_MOTION);
162 }
163 }
164
configAccel(bool on)165 static void configAccel(bool on) {
166 if (on) {
167 sensorRequest(mTask.taskId, mTask.accelHandle, ACCEL_MIN_RATE,
168 ACCEL_MAX_LATENCY);
169 osEventSubscribe(mTask.taskId, EVT_SENSOR_ACCEL);
170 } else {
171 sensorRelease(mTask.taskId, mTask.accelHandle);
172 osEventUnsubscribe(mTask.taskId, EVT_SENSOR_ACCEL);
173 }
174
175 }
176
177 // *****************************************************************************
178
179 static const struct SensorInfo mSi =
180 {
181 .sensorName = "Tilt Detection",
182 .sensorType = SENS_TYPE_TILT,
183 .numAxis = NUM_AXIS_EMBEDDED,
184 .interrupt = NANOHUB_INT_WAKEUP,
185 .minSamples = 20
186 };
187
tiltDetectionPower(bool on,void * cookie)188 static bool tiltDetectionPower(bool on, void *cookie)
189 {
190 if (on) {
191 configAnyMotion(true);
192 mTask.taskState = STATE_AWAITING_ANY_MOTION;
193 } else {
194 configAnyMotion(false);
195 configNoMotion(false);
196 configAccel(false);
197 mTask.taskState = STATE_DISABLED;
198 }
199
200 sensorSignalInternalEvt(mTask.handle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG,
201 on, 0);
202 return true;
203 }
204
tiltDetectionSetRate(uint32_t rate,uint64_t latency,void * cookie)205 static bool tiltDetectionSetRate(uint32_t rate, uint64_t latency, void *cookie)
206 {
207 sensorSignalInternalEvt(mTask.handle, SENSOR_INTERNAL_EVT_RATE_CHG, rate,
208 latency);
209 return true;
210 }
211
tiltDetectionFirmwareUpload(void * cookie)212 static bool tiltDetectionFirmwareUpload(void *cookie)
213 {
214 sensorSignalInternalEvt(mTask.handle, SENSOR_INTERNAL_EVT_FW_STATE_CHG,
215 1, 0);
216 return true;
217 }
218
tiltDetectionFlush(void * cookie)219 static bool tiltDetectionFlush(void *cookie)
220 {
221 return osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_TILT),
222 SENSOR_DATA_EVENT_FLUSH, NULL);
223 }
224
tiltDetectionHandleEvent(uint32_t evtType,const void * evtData)225 static void tiltDetectionHandleEvent(uint32_t evtType, const void* evtData)
226 {
227 if (evtData == SENSOR_DATA_EVENT_FLUSH)
228 return;
229
230 switch (evtType) {
231 case EVT_APP_START:
232 osEventUnsubscribe(mTask.taskId, EVT_APP_START);
233 sensorFind(SENS_TYPE_ANY_MOTION, 0, &mTask.anyMotionHandle);
234 sensorFind(SENS_TYPE_NO_MOTION, 0, &mTask.noMotionHandle);
235 sensorFind(SENS_TYPE_ACCEL, 0, &mTask.accelHandle);
236 break;
237
238 case EVT_SENSOR_ANY_MOTION:
239 if (mTask.taskState == STATE_AWAITING_ANY_MOTION) {
240 configAnyMotion(false);
241 configNoMotion(true);
242 configAccel(true);
243
244 mTask.taskState = STATE_AWAITING_TILT;
245 }
246 break;
247
248 case EVT_SENSOR_NO_MOTION:
249 if (mTask.taskState == STATE_AWAITING_TILT) {
250 configNoMotion(false);
251 configAccel(false);
252 configAnyMotion(true);
253
254 mTask.taskState = STATE_AWAITING_ANY_MOTION;
255 }
256 break;
257
258 case EVT_SENSOR_ACCEL:
259 if (mTask.taskState == STATE_AWAITING_TILT) {
260 if (algoUpdate((struct TripleAxisDataEvent *)evtData)) {
261 union EmbeddedDataPoint sample;
262 sample.idata = 1;
263 osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_TILT), sample.vptr, NULL);
264 }
265 }
266 break;
267 }
268 }
269
270 static const struct SensorOps mSops =
271 {
272 .sensorPower = tiltDetectionPower,
273 .sensorFirmwareUpload = tiltDetectionFirmwareUpload,
274 .sensorSetRate = tiltDetectionSetRate,
275 .sensorFlush = tiltDetectionFlush,
276 };
277
tiltDetectionStart(uint32_t taskId)278 static bool tiltDetectionStart(uint32_t taskId)
279 {
280 mTask.taskId = taskId;
281 mTask.handle = sensorRegister(&mSi, &mSops, NULL, true);
282 algoInit();
283 osEventSubscribe(taskId, EVT_APP_START);
284 return true;
285 }
286
tiltDetectionEnd()287 static void tiltDetectionEnd()
288 {
289 }
290
291 INTERNAL_APP_INIT(
292 APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 8),
293 TILT_APP_VERSION,
294 tiltDetectionStart,
295 tiltDetectionEnd,
296 tiltDetectionHandleEvent);
297