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