firmware/os/drivers/tilt_detection/tilt_detection.c - device/google/contexthub - Git at Google

 /*
  * Copyright (C) 2016 The Android Open Source Project
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  *
  *      http://www.apache.org/licenses/LICENSE-2.0
  *
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  */

 #include <stdlib.h>
 #include <string.h>
 #include <timer.h>
 #include <heap.h>
 #include <plat/rtc.h>
 #include <plat/syscfg.h>
 #include <hostIntf.h>
 #include <nanohubPacket.h>

 #include <seos.h>

 #include <nanohub_math.h>
 #include <sensors.h>
 #include <limits.h>

 #define TILT_APP_VERSION 1

 #define EVT_SENSOR_ANY_MOTION sensorGetMyEventType(SENS_TYPE_ANY_MOTION)
 #define EVT_SENSOR_NO_MOTION sensorGetMyEventType(SENS_TYPE_NO_MOTION)
 #define EVT_SENSOR_ACCEL sensorGetMyEventType(SENS_TYPE_ACCEL)

 #define ACCEL_MIN_RATE    SENSOR_HZ(50)
 #define ACCEL_MAX_LATENCY 250000000ull   // 250 ms

 #define BATCH_TIME      2000000000ull // 2.0 seconds
 #define ANGLE_THRESH    (0.819 * 9.81 * 9.81) // ~cos(35) * (1G in m/s^2)^2

 struct TiltAlgoState {
     uint64_t this_batch_init_ts;
     uint32_t this_batch_num_samples;
     float this_batch_sample_sum[3];
     float this_batch_g[3];
     float last_ref_g_vector[3];
     bool last_ref_g_vector_valid;
     bool anamoly_this_batch;
     bool tilt_detected;
 };

 static struct TiltDetectionTask {
     struct TiltAlgoState algoState;
     uint32_t taskId;
     uint32_t handle;
     uint32_t anyMotionHandle;
     uint32_t noMotionHandle;
     uint32_t accelHandle;
     enum {
         STATE_DISABLED,
         STATE_AWAITING_ANY_MOTION,
         STATE_AWAITING_TILT,
     } taskState;
 } mTask;

 // *****************************************************************************

 static void algoInit()
 {
     // nothing here
 }

 static bool algoUpdate(struct TripleAxisDataEvent *ev)
 {
     float dotProduct = 0.0f;
     uint64_t dt;
     bool latch_g_vector = false;
     bool tilt_detected = false;
     struct TiltAlgoState *state = &mTask.algoState;
     uint64_t sample_ts = ev->referenceTime;
     uint32_t numSamples = ev->samples[0].firstSample.numSamples;
     uint32_t i;
     struct TripleAxisDataPoint *sample;
     float invN;

     for (i = 0; i < numSamples; i++) {
         sample = &ev->samples[i];
         if (i > 0)
             sample_ts += sample->deltaTime;

         if (state->this_batch_init_ts == 0) {
             state->this_batch_init_ts = sample_ts;
         }

         state->this_batch_sample_sum[0] += sample->x;
         state->this_batch_sample_sum[1] += sample->y;
         state->this_batch_sample_sum[2] += sample->z;

         state->this_batch_num_samples++;

         dt = (sample_ts - state->this_batch_init_ts);

         if (dt > BATCH_TIME) {
             invN = 1.0f / state->this_batch_num_samples;
             state->this_batch_g[0] = state->this_batch_sample_sum[0] * invN;
             state->this_batch_g[1] = state->this_batch_sample_sum[1] * invN;
             state->this_batch_g[2] = state->this_batch_sample_sum[2] * invN;

             if (state->last_ref_g_vector_valid) {
                 dotProduct = state->this_batch_g[0] * state->last_ref_g_vector[0] +
                     state->this_batch_g[1] * state->last_ref_g_vector[1] +
                     state->this_batch_g[2] * state->last_ref_g_vector[2];

                 if (dotProduct < ANGLE_THRESH) {
                     tilt_detected = true;
                     latch_g_vector = true;
                 }
             } else { // reference g vector not valid, first time computing
                 latch_g_vector = true;
                 state->last_ref_g_vector_valid = true;
             }

             // latch the first batch or when dotProduct < ANGLE_THRESH
             if (latch_g_vector) {
                 state->last_ref_g_vector[0] = state->this_batch_g[0];
                 state->last_ref_g_vector[1] = state->this_batch_g[1];
                 state->last_ref_g_vector[2] = state->this_batch_g[2];
             }

             // Seed the next batch
             state->this_batch_init_ts = 0;
             state->this_batch_num_samples = 0;
             state->this_batch_sample_sum[0] = 0;
             state->this_batch_sample_sum[1] = 0;
             state->this_batch_sample_sum[2] = 0;
         }
     }

     return tilt_detected;
 }

 static void configAnyMotion(bool on) {
     if (on) {
         sensorRequest(mTask.taskId, mTask.anyMotionHandle, SENSOR_RATE_ONCHANGE, 0);
         osEventSubscribe(mTask.taskId, EVT_SENSOR_ANY_MOTION);
     } else {
         sensorRelease(mTask.taskId, mTask.anyMotionHandle);
         osEventUnsubscribe(mTask.taskId, EVT_SENSOR_ANY_MOTION);
     }
 }

 static void configNoMotion(bool on) {
     if (on) {
         sensorRequest(mTask.taskId, mTask.noMotionHandle, SENSOR_RATE_ONCHANGE, 0);
         osEventSubscribe(mTask.taskId, EVT_SENSOR_NO_MOTION);
     } else {
         sensorRelease(mTask.taskId, mTask.noMotionHandle);
         osEventUnsubscribe(mTask.taskId, EVT_SENSOR_NO_MOTION);
     }
 }

 static void configAccel(bool on) {
     if (on) {
         sensorRequest(mTask.taskId, mTask.accelHandle, ACCEL_MIN_RATE,
                       ACCEL_MAX_LATENCY);
         osEventSubscribe(mTask.taskId, EVT_SENSOR_ACCEL);
     } else {
         sensorRelease(mTask.taskId, mTask.accelHandle);
         osEventUnsubscribe(mTask.taskId, EVT_SENSOR_ACCEL);
     }

 }

 // *****************************************************************************

 static const struct SensorInfo mSi =
 {
     .sensorName = "Tilt Detection",
     .sensorType = SENS_TYPE_TILT,
     .numAxis = NUM_AXIS_EMBEDDED,
     .interrupt = NANOHUB_INT_WAKEUP,
     .minSamples = 20
 };

 static bool tiltDetectionPower(bool on, void *cookie)
 {
     if (on) {
         configAnyMotion(true);
         mTask.taskState = STATE_AWAITING_ANY_MOTION;
     } else {
         configAnyMotion(false);
         configNoMotion(false);
         configAccel(false);
         mTask.taskState = STATE_DISABLED;
     }

     sensorSignalInternalEvt(mTask.handle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG,
                             on, 0);
     return true;
 }

 static bool tiltDetectionSetRate(uint32_t rate, uint64_t latency, void *cookie)
 {
     sensorSignalInternalEvt(mTask.handle, SENSOR_INTERNAL_EVT_RATE_CHG, rate,
                             latency);
     return true;
 }

 static bool tiltDetectionFirmwareUpload(void *cookie)
 {
     sensorSignalInternalEvt(mTask.handle, SENSOR_INTERNAL_EVT_FW_STATE_CHG,
             1, 0);
     return true;
 }

 static bool tiltDetectionFlush(void *cookie)
 {
     return osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_TILT),
                         SENSOR_DATA_EVENT_FLUSH, NULL);
 }

 static void tiltDetectionHandleEvent(uint32_t evtType, const void* evtData)
 {
     if (evtData == SENSOR_DATA_EVENT_FLUSH)
         return;

     switch (evtType) {
     case EVT_APP_START:
         osEventUnsubscribe(mTask.taskId, EVT_APP_START);
         sensorFind(SENS_TYPE_ANY_MOTION, 0, &mTask.anyMotionHandle);
         sensorFind(SENS_TYPE_NO_MOTION, 0, &mTask.noMotionHandle);
         sensorFind(SENS_TYPE_ACCEL, 0, &mTask.accelHandle);
         break;

     case EVT_SENSOR_ANY_MOTION:
         if (mTask.taskState == STATE_AWAITING_ANY_MOTION) {
             configAnyMotion(false);
             configNoMotion(true);
             configAccel(true);

             mTask.taskState = STATE_AWAITING_TILT;
         }
         break;

     case EVT_SENSOR_NO_MOTION:
         if (mTask.taskState == STATE_AWAITING_TILT) {
             configNoMotion(false);
             configAccel(false);
             configAnyMotion(true);

             mTask.taskState = STATE_AWAITING_ANY_MOTION;
         }
         break;

     case EVT_SENSOR_ACCEL:
         if (mTask.taskState == STATE_AWAITING_TILT) {
             if (algoUpdate((struct TripleAxisDataEvent *)evtData)) {
                 union EmbeddedDataPoint sample;
                 sample.idata = 1;
                 osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_TILT), sample.vptr, NULL);
             }
         }
         break;
     }
 }

 static const struct SensorOps mSops =
 {
     .sensorPower = tiltDetectionPower,
     .sensorFirmwareUpload = tiltDetectionFirmwareUpload,
     .sensorSetRate = tiltDetectionSetRate,
     .sensorFlush = tiltDetectionFlush,
 };

 static bool tiltDetectionStart(uint32_t taskId)
 {
     mTask.taskId = taskId;
     mTask.handle = sensorRegister(&mSi, &mSops, NULL, true);
     algoInit();
     osEventSubscribe(taskId, EVT_APP_START);
     return true;
 }

 static void tiltDetectionEnd()
 {
 }

 INTERNAL_APP_INIT(
         APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 8),
         TILT_APP_VERSION,
         tiltDetectionStart,
         tiltDetectionEnd,
         tiltDetectionHandleEvent);
	/*
	* Copyright (C) 2016 The Android Open Source Project
	*
	* Licensed under the Apache License, Version 2.0 (the "License");
	* you may not use this file except in compliance with the License.
	* You may obtain a copy of the License at
	*
	* http://www.apache.org/licenses/LICENSE-2.0
	*
	* Unless required by applicable law or agreed to in writing, software
	* distributed under the License is distributed on an "AS IS" BASIS,
	* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
	* See the License for the specific language governing permissions and
	* limitations under the License.
	*/

	#include <stdlib.h>
	#include <string.h>
	#include <timer.h>
	#include <heap.h>
	#include <plat/rtc.h>
	#include <plat/syscfg.h>
	#include <hostIntf.h>
	#include <nanohubPacket.h>

	#include <seos.h>

	#include <nanohub_math.h>
	#include <sensors.h>
	#include <limits.h>

	#define TILT_APP_VERSION 1

	#define EVT_SENSOR_ANY_MOTION sensorGetMyEventType(SENS_TYPE_ANY_MOTION)
	#define EVT_SENSOR_NO_MOTION sensorGetMyEventType(SENS_TYPE_NO_MOTION)
	#define EVT_SENSOR_ACCEL sensorGetMyEventType(SENS_TYPE_ACCEL)

	#define ACCEL_MIN_RATE SENSOR_HZ(50)
	#define ACCEL_MAX_LATENCY 250000000ull // 250 ms

	#define BATCH_TIME 2000000000ull // 2.0 seconds
	#define ANGLE_THRESH (0.819 * 9.81 * 9.81) // ~cos(35) * (1G in m/s^2)^2

	struct TiltAlgoState {
	uint64_t this_batch_init_ts;
	uint32_t this_batch_num_samples;
	float this_batch_sample_sum[3];
	float this_batch_g[3];
	float last_ref_g_vector[3];
	bool last_ref_g_vector_valid;
	bool anamoly_this_batch;
	bool tilt_detected;
	};

	static struct TiltDetectionTask {
	struct TiltAlgoState algoState;
	uint32_t taskId;
	uint32_t handle;
	uint32_t anyMotionHandle;
	uint32_t noMotionHandle;
	uint32_t accelHandle;
	enum {
	STATE_DISABLED,
	STATE_AWAITING_ANY_MOTION,
	STATE_AWAITING_TILT,
	} taskState;
	} mTask;

	// *****************************************************************************

	static void algoInit()
	{
	// nothing here
	}

	static bool algoUpdate(struct TripleAxisDataEvent *ev)
	{
	float dotProduct = 0.0f;
	uint64_t dt;
	bool latch_g_vector = false;
	bool tilt_detected = false;
	struct TiltAlgoState *state = &mTask.algoState;
	uint64_t sample_ts = ev->referenceTime;
	uint32_t numSamples = ev->samples[0].firstSample.numSamples;
	uint32_t i;
	struct TripleAxisDataPoint *sample;
	float invN;

	for (i = 0; i < numSamples; i++) {
	sample = &ev->samples[i];
	if (i > 0)
	sample_ts += sample->deltaTime;

	if (state->this_batch_init_ts == 0) {
	state->this_batch_init_ts = sample_ts;
	}

	state->this_batch_sample_sum[0] += sample->x;
	state->this_batch_sample_sum[1] += sample->y;
	state->this_batch_sample_sum[2] += sample->z;

	state->this_batch_num_samples++;

	dt = (sample_ts - state->this_batch_init_ts);

	if (dt > BATCH_TIME) {
	invN = 1.0f / state->this_batch_num_samples;
	state->this_batch_g[0] = state->this_batch_sample_sum[0] * invN;
	state->this_batch_g[1] = state->this_batch_sample_sum[1] * invN;
	state->this_batch_g[2] = state->this_batch_sample_sum[2] * invN;

	if (state->last_ref_g_vector_valid) {
	dotProduct = state->this_batch_g[0] * state->last_ref_g_vector[0] +
	state->this_batch_g[1] * state->last_ref_g_vector[1] +
	state->this_batch_g[2] * state->last_ref_g_vector[2];

	if (dotProduct < ANGLE_THRESH) {
	tilt_detected = true;
	latch_g_vector = true;
	}
	} else { // reference g vector not valid, first time computing
	latch_g_vector = true;
	state->last_ref_g_vector_valid = true;
	}

	// latch the first batch or when dotProduct < ANGLE_THRESH
	if (latch_g_vector) {
	state->last_ref_g_vector[0] = state->this_batch_g[0];
	state->last_ref_g_vector[1] = state->this_batch_g[1];
	state->last_ref_g_vector[2] = state->this_batch_g[2];
	}

	// Seed the next batch
	state->this_batch_init_ts = 0;
	state->this_batch_num_samples = 0;
	state->this_batch_sample_sum[0] = 0;
	state->this_batch_sample_sum[1] = 0;
	state->this_batch_sample_sum[2] = 0;
	}
	}

	return tilt_detected;
	}

	static void configAnyMotion(bool on) {
	if (on) {
	sensorRequest(mTask.taskId, mTask.anyMotionHandle, SENSOR_RATE_ONCHANGE, 0);
	osEventSubscribe(mTask.taskId, EVT_SENSOR_ANY_MOTION);
	} else {
	sensorRelease(mTask.taskId, mTask.anyMotionHandle);
	osEventUnsubscribe(mTask.taskId, EVT_SENSOR_ANY_MOTION);
	}
	}

	static void configNoMotion(bool on) {
	if (on) {
	sensorRequest(mTask.taskId, mTask.noMotionHandle, SENSOR_RATE_ONCHANGE, 0);
	osEventSubscribe(mTask.taskId, EVT_SENSOR_NO_MOTION);
	} else {
	sensorRelease(mTask.taskId, mTask.noMotionHandle);
	osEventUnsubscribe(mTask.taskId, EVT_SENSOR_NO_MOTION);
	}
	}

	static void configAccel(bool on) {
	if (on) {
	sensorRequest(mTask.taskId, mTask.accelHandle, ACCEL_MIN_RATE,
	ACCEL_MAX_LATENCY);
	osEventSubscribe(mTask.taskId, EVT_SENSOR_ACCEL);
	} else {
	sensorRelease(mTask.taskId, mTask.accelHandle);
	osEventUnsubscribe(mTask.taskId, EVT_SENSOR_ACCEL);
	}

	}

	// *****************************************************************************

	static const struct SensorInfo mSi =
	{
	.sensorName = "Tilt Detection",
	.sensorType = SENS_TYPE_TILT,
	.numAxis = NUM_AXIS_EMBEDDED,
	.interrupt = NANOHUB_INT_WAKEUP,
	.minSamples = 20
	};

	static bool tiltDetectionPower(bool on, void *cookie)
	{
	if (on) {
	configAnyMotion(true);
	mTask.taskState = STATE_AWAITING_ANY_MOTION;
	} else {
	configAnyMotion(false);
	configNoMotion(false);
	configAccel(false);
	mTask.taskState = STATE_DISABLED;
	}

	sensorSignalInternalEvt(mTask.handle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG,
	on, 0);
	return true;
	}

	static bool tiltDetectionSetRate(uint32_t rate, uint64_t latency, void *cookie)
	{
	sensorSignalInternalEvt(mTask.handle, SENSOR_INTERNAL_EVT_RATE_CHG, rate,
	latency);
	return true;
	}

	static bool tiltDetectionFirmwareUpload(void *cookie)
	{
	sensorSignalInternalEvt(mTask.handle, SENSOR_INTERNAL_EVT_FW_STATE_CHG,
	1, 0);
	return true;
	}

	static bool tiltDetectionFlush(void *cookie)
	{
	return osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_TILT),
	SENSOR_DATA_EVENT_FLUSH, NULL);
	}

	static void tiltDetectionHandleEvent(uint32_t evtType, const void* evtData)
	{
	if (evtData == SENSOR_DATA_EVENT_FLUSH)
	return;

	switch (evtType) {
	case EVT_APP_START:
	osEventUnsubscribe(mTask.taskId, EVT_APP_START);
	sensorFind(SENS_TYPE_ANY_MOTION, 0, &mTask.anyMotionHandle);
	sensorFind(SENS_TYPE_NO_MOTION, 0, &mTask.noMotionHandle);
	sensorFind(SENS_TYPE_ACCEL, 0, &mTask.accelHandle);
	break;

	case EVT_SENSOR_ANY_MOTION:
	if (mTask.taskState == STATE_AWAITING_ANY_MOTION) {
	configAnyMotion(false);
	configNoMotion(true);
	configAccel(true);

	mTask.taskState = STATE_AWAITING_TILT;
	}
	break;

	case EVT_SENSOR_NO_MOTION:
	if (mTask.taskState == STATE_AWAITING_TILT) {
	configNoMotion(false);
	configAccel(false);
	configAnyMotion(true);

	mTask.taskState = STATE_AWAITING_ANY_MOTION;
	}
	break;

	case EVT_SENSOR_ACCEL:
	if (mTask.taskState == STATE_AWAITING_TILT) {
	if (algoUpdate((struct TripleAxisDataEvent *)evtData)) {
	union EmbeddedDataPoint sample;
	sample.idata = 1;
	osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_TILT), sample.vptr, NULL);
	}
	}
	break;
	}
	}

	static const struct SensorOps mSops =
	{
	.sensorPower = tiltDetectionPower,
	.sensorFirmwareUpload = tiltDetectionFirmwareUpload,
	.sensorSetRate = tiltDetectionSetRate,
	.sensorFlush = tiltDetectionFlush,
	};

	static bool tiltDetectionStart(uint32_t taskId)
	{
	mTask.taskId = taskId;
	mTask.handle = sensorRegister(&mSi, &mSops, NULL, true);
	algoInit();
	osEventSubscribe(taskId, EVT_APP_START);
	return true;
	}

	static void tiltDetectionEnd()
	{
	}

	INTERNAL_APP_INIT(
	APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 8),
	TILT_APP_VERSION,
	tiltDetectionStart,
	tiltDetectionEnd,
	tiltDetectionHandleEvent);