Home | History | Annotate | Download | only in tilt_detection 
      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 
     71 static void algoInit()
     72 {
     73     // nothing here
     74 }
     75 
     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 
    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 
    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 
    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 
    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 
    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 
    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 
    219 static bool tiltDetectionFlush(void *cookie)
    220 {
    221     return osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_TILT),
    222                         SENSOR_DATA_EVENT_FLUSH, NULL);
    223 }
    224 
    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 
    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 
    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