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