/* * 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/inc/rtc.h> #include <plat/inc/syscfg.h> #include <hostIntf.h> #include <nanohubPacket.h> #include <floatRt.h> #include <seos.h> #include <nanohub_math.h> #include <algos/fusion.h> #include <sensors.h> #include <variant/inc/sensType.h> #include <limits.h> #include <slab.h> #define ORIENTATION_APP_VERSION 1 #define MAX_NUM_COMMS_EVENT_SAMPLES 15 // at most 15 samples can fit in one comms_event #define NUM_COMMS_EVENTS_IN_FIFO 2 // This controls how often the hub needs to wake up // in batching // needs to be greater than max raw sensor rate ratio #define FIFO_DEPTH (NUM_COMMS_EVENTS_IN_FIFO * MAX_NUM_COMMS_EVENT_SAMPLES) /* * FIFO_MARGIN: max raw sensor rate ratio is 8:1. * If 2 batchs of high rate data comes before 1 low rate data, there can be at max 15 samples left * in the FIFO */ #define FIFO_MARGIN 15 #define MAX_NUM_SAMPLES (FIFO_MARGIN + FIFO_DEPTH) // actual input sample fifo depth #define EVT_SENSOR_ACC_DATA_RDY sensorGetMyEventType(SENS_TYPE_ACCEL) #define EVT_SENSOR_GYR_DATA_RDY sensorGetMyEventType(SENS_TYPE_GYRO) #define EVT_SENSOR_MAG_DATA_RDY sensorGetMyEventType(SENS_TYPE_MAG) #define EVT_SENSOR_MAG_BIAS sensorGetMyEventType(SENS_TYPE_MAG_BIAS) #define kGravityEarth 9.80665f #define kRad2deg (180.0f / M_PI) #define MIN_GYRO_RATE_HZ SENSOR_HZ(100.0f) #define MAX_MAG_RATE_HZ SENSOR_HZ(50.0f) enum { FUSION_FLAG_ENABLED = 0x01, FUSION_FLAG_INITIALIZED = 0x08, FUSION_FLAG_GAME_ENABLED = 0x10, FUSION_FLAG_GAME_INITIALIZED = 0x20 }; enum RawSensorType { ACC, GYR, MAG, NUM_OF_RAW_SENSOR }; enum FusionSensorType { ORIENT, GRAVITY, GEOMAG, LINEAR, GAME, ROTAT, NUM_OF_FUSION_SENSOR }; struct FusionSensorSample { uint64_t time; float x, y, z; }; struct FusionSensor { uint32_t handle; struct TripleAxisDataEvent *ev; uint64_t prev_time; uint64_t latency; uint32_t rate; bool active; bool use_gyro_data; bool use_mag_data; uint8_t idx; }; struct FusionTask { uint32_t tid; uint32_t accelHandle; uint32_t gyroHandle; uint32_t magHandle; struct Fusion fusion; struct Fusion game; struct FusionSensor sensors[NUM_OF_FUSION_SENSOR]; struct FusionSensorSample samples[NUM_OF_RAW_SENSOR][MAX_NUM_SAMPLES]; size_t sample_indices[NUM_OF_RAW_SENSOR]; size_t sample_counts[NUM_OF_RAW_SENSOR]; uint32_t counters[NUM_OF_RAW_SENSOR]; uint64_t ResamplePeriodNs[NUM_OF_RAW_SENSOR]; uint64_t last_time[NUM_OF_RAW_SENSOR]; struct TripleAxisDataPoint last_sample[NUM_OF_RAW_SENSOR]; uint32_t flags; uint32_t raw_sensor_rate[NUM_OF_RAW_SENSOR]; uint64_t raw_sensor_latency; uint8_t accel_client_cnt; uint8_t gyro_client_cnt; uint8_t mag_client_cnt; }; static uint32_t FusionRates[] = { SENSOR_HZ(12.5f), SENSOR_HZ(25.0f), SENSOR_HZ(50.0f), SENSOR_HZ(100.0f), SENSOR_HZ(200.0f), 0, }; //should match "supported rates in length" and be the timer length for that rate in nanosecs static const uint64_t rateTimerVals[] = { 1000000000ULL / 12.5f, 1000000000ULL / 25, 1000000000ULL / 50, 1000000000ULL / 100, 1000000000ULL / 200, }; static struct FusionTask mTask; #define DEC_INFO_RATE(name, rates, type, axis, inter, samples) \ .sensorName = name, \ .supportedRates = rates, \ .sensorType = type, \ .numAxis = axis, \ .interrupt = inter, \ .minSamples = samples static const struct SensorInfo mSi[NUM_OF_FUSION_SENSOR] = { { DEC_INFO_RATE("Orientation", FusionRates, SENS_TYPE_ORIENTATION, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 20) }, { DEC_INFO_RATE("Gravity", FusionRates, SENS_TYPE_GRAVITY, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 20) }, { DEC_INFO_RATE("Geomagnetic Rotation Vector", FusionRates, SENS_TYPE_GEO_MAG_ROT_VEC, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 20) }, { DEC_INFO_RATE("Linear Acceleration", FusionRates, SENS_TYPE_LINEAR_ACCEL, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 20) }, { DEC_INFO_RATE("Game Rotation Vector", FusionRates, SENS_TYPE_GAME_ROT_VECTOR, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 300) }, { DEC_INFO_RATE("Rotation Vector", FusionRates, SENS_TYPE_ROTATION_VECTOR, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 20) }, }; static struct SlabAllocator *mDataSlab; static void dataEvtFree(void *ptr) { slabAllocatorFree(mDataSlab, ptr); } static void fillSamples(struct TripleAxisDataEvent *ev, enum RawSensorType index) { bool bad_timestamp; size_t i, w, n, num_samples; struct TripleAxisDataPoint *curr_sample, *next_sample; uint32_t counter; uint64_t ResamplePeriodNs, curr_time, next_time; uint64_t sample_spacing_ns; float weight_next; if (index == GYR && mTask.gyro_client_cnt == 0) { return; } if (index == MAG && mTask.mag_client_cnt == 0) { return; } n = mTask.sample_counts[index]; i = mTask.sample_indices[index]; counter = mTask.counters[index]; ResamplePeriodNs = mTask.ResamplePeriodNs[index]; w = (mTask.sample_indices[index] + n) % MAX_NUM_SAMPLES; // check if this sensor was used before if (mTask.last_time[index] == ULONG_LONG_MAX) { curr_sample = ev->samples; next_sample = curr_sample + 1; num_samples = ev->samples[0].firstSample.numSamples; curr_time = ev->referenceTime; } else { curr_sample = &mTask.last_sample[index]; next_sample = ev->samples; num_samples = ev->samples[0].firstSample.numSamples + 1; curr_time = mTask.last_time[index]; } while (num_samples > 1) { if (next_sample == ev->samples) next_time = ev->referenceTime; else next_time = curr_time + next_sample->deltaTime; // error handling for non-chronological accel timestamps sample_spacing_ns = (next_time > curr_time) ? (next_time - curr_time) : 0; // This can happen during sensor config changes bad_timestamp = (sample_spacing_ns > 10 * ResamplePeriodNs); // Check to see if we need to move the interpolation window or // interpolate if ((counter >= sample_spacing_ns) || bad_timestamp) { num_samples--; counter -= (bad_timestamp ? counter : sample_spacing_ns); curr_sample = next_sample; next_sample++; curr_time = next_time; } else { weight_next = (float)counter / floatFromUint64(sample_spacing_ns); mTask.samples[index][w].x = curr_sample->x + weight_next * (next_sample->x - curr_sample->x); mTask.samples[index][w].y = curr_sample->y + weight_next * (next_sample->y - curr_sample->y); mTask.samples[index][w].z = curr_sample->z + weight_next * (next_sample->z - curr_sample->z); mTask.samples[index][w].time = curr_time + counter; // Move the read index when buffer is full if (++n > MAX_NUM_SAMPLES) { n = MAX_NUM_SAMPLES; if (++i == MAX_NUM_SAMPLES) { i = 0; } } // Reset the write index if (++w == MAX_NUM_SAMPLES) { w = 0; } // Move to the next resample counter += ResamplePeriodNs; } } mTask.sample_counts[index] = n; mTask.sample_indices[index] = i; mTask.counters[index] = counter; mTask.last_sample[index] = *curr_sample; mTask.last_time[index] = curr_time; } static bool allocateDataEvt(struct FusionSensor *mSensor, uint64_t time) { mSensor->ev = slabAllocatorAlloc(mDataSlab); if (mSensor->ev == NULL) { // slab allocation failed osLog(LOG_ERROR, "ORIENTATION: slabAllocatorAlloc() Failed\n"); return false; } // delta time for the first sample is sample count memset(&mSensor->ev->samples[0].firstSample, 0x00, sizeof(struct SensorFirstSample)); mSensor->ev->referenceTime = time; mSensor->prev_time = time; return true; } static void addSample(struct FusionSensor *mSensor, uint64_t time, float x, float y, float z) { struct TripleAxisDataPoint *sample; if (mSensor->ev == NULL) { if (!allocateDataEvt(mSensor, time)) return; } if (mSensor->ev->samples[0].firstSample.numSamples >= MAX_NUM_COMMS_EVENT_SAMPLES) { osLog(LOG_ERROR, "ORIENTATION: BAD_INDEX\n"); return; } sample = &mSensor->ev->samples[mSensor->ev->samples[0].firstSample.numSamples++]; if (mSensor->ev->samples[0].firstSample.numSamples > 1) { sample->deltaTime = time > mSensor->prev_time ? (time - mSensor->prev_time) : 0; mSensor->prev_time = time; } sample->x = x; sample->y = y; sample->z = z; if (mSensor->ev->samples[0].firstSample.numSamples == MAX_NUM_COMMS_EVENT_SAMPLES) { osEnqueueEvtOrFree( EVENT_TYPE_BIT_DISCARDABLE | sensorGetMyEventType(mSi[mSensor->idx].sensorType), mSensor->ev, dataEvtFree); mSensor->ev = NULL; } } static void updateOutput(ssize_t last_accel_sample_index, uint64_t last_sensor_time) { struct Vec4 attitude; struct Vec3 g, a; struct Mat33 R; // direction-cosine/rotation matrix, inertial -> device bool rInited; // indicates if matrix R has been initialzed. for avoiding repeated computation if (fusionHasEstimate(&mTask.game)) { rInited = false; if (mTask.sensors[GAME].active) { fusionGetAttitude(&mTask.game, &attitude); addSample(&mTask.sensors[GAME], last_sensor_time, attitude.x, attitude.y, attitude.z); } if (mTask.sensors[GRAVITY].active) { fusionGetRotationMatrix(&mTask.game, &R); rInited = true; initVec3(&g, R.elem[0][2], R.elem[1][2], R.elem[2][2]); vec3ScalarMul(&g, kGravityEarth); addSample(&mTask.sensors[GRAVITY], last_sensor_time, g.x, g.y, g.z); } if (last_accel_sample_index >= 0 && mTask.sensors[LINEAR].active) { if (!rInited) { fusionGetRotationMatrix(&mTask.game, &R); } initVec3(&g, R.elem[0][2], R.elem[1][2], R.elem[2][2]); vec3ScalarMul(&g, kGravityEarth); initVec3(&a, mTask.samples[0][last_accel_sample_index].x, mTask.samples[0][last_accel_sample_index].y, mTask.samples[0][last_accel_sample_index].z); addSample(&mTask.sensors[LINEAR], mTask.samples[0][last_accel_sample_index].time, a.x - g.x, a.y - g.y, a.z - g.z); } } if (fusionHasEstimate(&mTask.fusion)) { fusionGetAttitude(&mTask.fusion, &attitude); if (mTask.sensors[ORIENT].active) { fusionGetRotationMatrix(&mTask.fusion, &R); // x, y, z = yaw, pitch, roll float x = atan2f(-R.elem[0][1], R.elem[0][0]) * kRad2deg; float y = atan2f(-R.elem[1][2], R.elem[2][2]) * kRad2deg; float z = asinf(R.elem[0][2]) * kRad2deg; if (x < 0.0f) { x += 360.0f; } addSample(&mTask.sensors[ORIENT], last_sensor_time, x, y, z); } if (mTask.sensors[GEOMAG].active) { addSample(&mTask.sensors[GEOMAG], last_sensor_time, attitude.x, attitude.y, attitude.z); } if (mTask.sensors[ROTAT].active) { addSample(&mTask.sensors[ROTAT], last_sensor_time, attitude.x, attitude.y, attitude.z); } } } static void drainSamples() { size_t i = mTask.sample_indices[ACC]; size_t j = 0; size_t k = 0; size_t which; struct Vec3 a, w, m; float dT; uint64_t a_time, g_time, m_time; if (mTask.gyro_client_cnt > 0) j = mTask.sample_indices[GYR]; if (mTask.mag_client_cnt > 0) k = mTask.sample_indices[MAG]; while (mTask.sample_counts[ACC] > 0 && (!(mTask.gyro_client_cnt > 0) || mTask.sample_counts[GYR] > 0) && (!(mTask.mag_client_cnt > 0) || mTask.sample_counts[MAG] > 0)) { a_time = mTask.samples[ACC][i].time; g_time = mTask.gyro_client_cnt > 0 ? mTask.samples[GYR][j].time : ULONG_LONG_MAX; m_time = mTask.mag_client_cnt > 0 ? mTask.samples[MAG][k].time : ULONG_LONG_MAX; // priority with same timestamp: gyro > acc > mag if (g_time <= a_time && g_time <= m_time) { which = GYR; } else if (a_time <= m_time) { which = ACC; } else { which = MAG; } dT = floatFromUint64(mTask.ResamplePeriodNs[which]) * 1e-9f; switch (which) { case ACC: initVec3(&a, mTask.samples[ACC][i].x, mTask.samples[ACC][i].y, mTask.samples[ACC][i].z); if (mTask.flags & FUSION_FLAG_ENABLED) fusionHandleAcc(&mTask.fusion, &a, dT); if (mTask.flags & FUSION_FLAG_GAME_ENABLED) fusionHandleAcc(&mTask.game, &a, dT); updateOutput(i, mTask.samples[ACC][i].time); --mTask.sample_counts[ACC]; if (++i == MAX_NUM_SAMPLES) i = 0; break; case GYR: initVec3(&w, mTask.samples[GYR][j].x, mTask.samples[GYR][j].y, mTask.samples[GYR][j].z); if (mTask.flags & FUSION_FLAG_ENABLED) fusionHandleGyro(&mTask.fusion, &w, dT); if (mTask.flags & FUSION_FLAG_GAME_ENABLED) fusionHandleGyro(&mTask.game, &w, dT); --mTask.sample_counts[GYR]; if (++j == MAX_NUM_SAMPLES) j = 0; break; case MAG: initVec3(&m, mTask.samples[MAG][k].x, mTask.samples[MAG][k].y, mTask.samples[MAG][k].z); fusionHandleMag(&mTask.fusion, &m, dT); --mTask.sample_counts[MAG]; if (++k == MAX_NUM_SAMPLES) k = 0; break; } } mTask.sample_indices[ACC] = i; if (mTask.gyro_client_cnt > 0) mTask.sample_indices[GYR] = j; if (mTask.mag_client_cnt > 0) mTask.sample_indices[MAG] = k; for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++) { if (mTask.sensors[i].ev != NULL) { osEnqueueEvtOrFree(EVENT_TYPE_BIT_DISCARDABLE | sensorGetMyEventType(mSi[i].sensorType), mTask.sensors[i].ev, dataEvtFree); mTask.sensors[i].ev = NULL; } } } static void configureFusion() { if (mTask.sensors[ORIENT].active || mTask.sensors[ROTAT].active || mTask.sensors[GEOMAG].active) { mTask.flags |= FUSION_FLAG_ENABLED; initFusion(&mTask.fusion, (mTask.mag_client_cnt > 0 ? FUSION_USE_MAG : 0) | (mTask.gyro_client_cnt > 0 ? FUSION_USE_GYRO : 0) | ((mTask.flags & FUSION_FLAG_INITIALIZED) ? 0 : FUSION_REINITIALIZE)); mTask.flags |= FUSION_FLAG_INITIALIZED; } else { mTask.flags &= ~FUSION_FLAG_ENABLED; mTask.flags &= ~FUSION_FLAG_INITIALIZED; } } static void configureGame() { if (mTask.sensors[GAME].active || mTask.sensors[GRAVITY].active || mTask.sensors[LINEAR].active) { mTask.flags |= FUSION_FLAG_GAME_ENABLED; initFusion(&mTask.game, FUSION_USE_GYRO | ((mTask.flags & FUSION_FLAG_INITIALIZED) ? 0 : FUSION_REINITIALIZE)); mTask.flags |= FUSION_FLAG_GAME_INITIALIZED; } else { mTask.flags &= ~FUSION_FLAG_GAME_ENABLED; mTask.flags &= ~FUSION_FLAG_GAME_INITIALIZED; } } static void fusionSetRateAcc(void) { int i; if (mTask.accelHandle == 0) { mTask.sample_counts[ACC] = 0; mTask.sample_indices[ACC] = 0; mTask.counters[ACC] = 0; mTask.last_time[ACC] = ULONG_LONG_MAX; for (i = 0; sensorFind(SENS_TYPE_ACCEL, i, &mTask.accelHandle) != NULL; i++) { if (sensorRequest(mTask.tid, mTask.accelHandle, mTask.raw_sensor_rate[ACC], mTask.raw_sensor_latency)) { osEventSubscribe(mTask.tid, EVT_SENSOR_ACC_DATA_RDY); break; } } } else { sensorRequestRateChange(mTask.tid, mTask.accelHandle, mTask.raw_sensor_rate[ACC], mTask.raw_sensor_latency); } } static void fusionSetRateGyr(void) { int i; if (mTask.gyroHandle == 0) { mTask.sample_counts[GYR] = 0; mTask.sample_indices[GYR] = 0; mTask.counters[GYR] = 0; mTask.last_time[GYR] = ULONG_LONG_MAX; for (i = 0; sensorFind(SENS_TYPE_GYRO, i, &mTask.gyroHandle) != NULL; i++) { if (sensorRequest(mTask.tid, mTask.gyroHandle, mTask.raw_sensor_rate[GYR], mTask.raw_sensor_latency)) { osEventSubscribe(mTask.tid, EVT_SENSOR_GYR_DATA_RDY); break; } } } else { sensorRequestRateChange(mTask.tid, mTask.gyroHandle, mTask.raw_sensor_rate[GYR], mTask.raw_sensor_latency); } } static void fusionSetRateMag(void) { int i; if (mTask.magHandle == 0) { mTask.sample_counts[MAG] = 0; mTask.sample_indices[MAG] = 0; mTask.counters[MAG] = 0; mTask.last_time[MAG] = ULONG_LONG_MAX; for (i = 0; sensorFind(SENS_TYPE_MAG, i, &mTask.magHandle) != NULL; i++) { if (sensorRequest(mTask.tid, mTask.magHandle, mTask.raw_sensor_rate[MAG], mTask.raw_sensor_latency)) { osEventSubscribe(mTask.tid, EVT_SENSOR_MAG_DATA_RDY); osEventSubscribe(mTask.tid, EVT_SENSOR_MAG_BIAS); break; } } } else { sensorRequestRateChange(mTask.tid, mTask.magHandle, mTask.raw_sensor_rate[MAG], mTask.raw_sensor_latency); } } static bool fusionSetRate(uint32_t rate, uint64_t latency, void *cookie) { struct FusionSensor *mSensor = &mTask.sensors[(int)cookie]; int i; uint32_t max_rate = 0; uint32_t gyr_rate, mag_rate; uint64_t min_resample_period = ULONG_LONG_MAX; mSensor->rate = rate; mSensor->latency = latency; for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++) { if (mTask.sensors[i].active) { max_rate = max_rate > mTask.sensors[i].rate ? max_rate : mTask.sensors[i].rate; } } if (mTask.accel_client_cnt > 0) { mTask.raw_sensor_rate[ACC] = max_rate; mTask.ResamplePeriodNs[ACC] = sensorTimerLookupCommon(FusionRates, rateTimerVals, max_rate); min_resample_period = mTask.ResamplePeriodNs[ACC] < min_resample_period ? mTask.ResamplePeriodNs[ACC] : min_resample_period; } if (mTask.gyro_client_cnt > 0) { gyr_rate = max_rate > MIN_GYRO_RATE_HZ ? max_rate : MIN_GYRO_RATE_HZ; mTask.raw_sensor_rate[GYR] = gyr_rate; mTask.ResamplePeriodNs[GYR] = sensorTimerLookupCommon(FusionRates, rateTimerVals, gyr_rate); min_resample_period = mTask.ResamplePeriodNs[GYR] < min_resample_period ? mTask.ResamplePeriodNs[GYR] : min_resample_period; } if (mTask.mag_client_cnt > 0) { mag_rate = max_rate < MAX_MAG_RATE_HZ ? max_rate : MAX_MAG_RATE_HZ; mTask.raw_sensor_rate[MAG] = mag_rate; mTask.ResamplePeriodNs[MAG] = sensorTimerLookupCommon(FusionRates, rateTimerVals, mag_rate); min_resample_period = mTask.ResamplePeriodNs[MAG] < min_resample_period ? mTask.ResamplePeriodNs[MAG] : min_resample_period; } // This guarantees that local raw sensor FIFOs won't overflow. mTask.raw_sensor_latency = min_resample_period * (FIFO_DEPTH - 1); for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++) { if (mTask.sensors[i].active) { mTask.raw_sensor_latency = mTask.sensors[i].latency < mTask.raw_sensor_latency ? mTask.sensors[i].latency : mTask.raw_sensor_latency; } } if (mTask.accel_client_cnt > 0) fusionSetRateAcc(); if (mTask.gyro_client_cnt > 0) fusionSetRateGyr(); if (mTask.mag_client_cnt > 0) fusionSetRateMag(); if (mSensor->rate > 0) sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency); return true; } static bool fusionPower(bool on, void *cookie) { struct FusionSensor *mSensor = &mTask.sensors[(int)cookie]; int idx; mSensor->active = on; if (on == false) { mTask.accel_client_cnt--; if (mSensor->use_gyro_data) mTask.gyro_client_cnt--; if (mSensor->use_mag_data) mTask.mag_client_cnt--; // if client_cnt == 0 and Handle == 0, nothing need to be done. // if client_cnt > 0 and Handle == 0, something else is turning it on, all will be done. if (mTask.accel_client_cnt == 0 && mTask.accelHandle != 0) { sensorRelease(mTask.tid, mTask.accelHandle); mTask.accelHandle = 0; osEventUnsubscribe(mTask.tid, EVT_SENSOR_ACC_DATA_RDY); } if (mTask.gyro_client_cnt == 0 && mTask.gyroHandle != 0) { sensorRelease(mTask.tid, mTask.gyroHandle); mTask.gyroHandle = 0; osEventUnsubscribe(mTask.tid, EVT_SENSOR_GYR_DATA_RDY); } if (mTask.mag_client_cnt == 0 && mTask.magHandle != 0) { sensorRelease(mTask.tid, mTask.magHandle); mTask.magHandle = 0; osEventUnsubscribe(mTask.tid, EVT_SENSOR_MAG_DATA_RDY); } idx = mSensor->idx; (void) fusionSetRate(0, ULONG_LONG_MAX, (void *)idx); } else { mTask.accel_client_cnt++; if (mSensor->use_gyro_data) mTask.gyro_client_cnt++; if (mSensor->use_mag_data) mTask.mag_client_cnt++; } configureFusion(); configureGame(); sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, on, 0); return true; } static bool fusionFirmwareUpload(void *cookie) { struct FusionSensor *mSensor = &mTask.sensors[(int)cookie]; sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0); return true; } static bool fusionFlush(void *cookie) { struct FusionSensor *mSensor = &mTask.sensors[(int)cookie]; uint32_t evtType = sensorGetMyEventType(mSi[mSensor->idx].sensorType); osEnqueueEvt(evtType, SENSOR_DATA_EVENT_FLUSH, NULL); return true; } static void fusionHandleEvent(uint32_t evtType, const void* evtData) { struct TripleAxisDataEvent *ev; int i; if (evtData == SENSOR_DATA_EVENT_FLUSH) return; switch (evtType) { case EVT_APP_START: // check for gyro and mag osEventUnsubscribe(mTask.tid, EVT_APP_START); if (!sensorFind(SENS_TYPE_GYRO, 0, &mTask.gyroHandle)) { for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++) mTask.sensors[i].use_gyro_data = false; } mTask.gyroHandle = 0; if (!sensorFind(SENS_TYPE_MAG, 0, &mTask.magHandle)) { for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++) mTask.sensors[i].use_mag_data = false; } mTask.magHandle = 0; break; case EVT_SENSOR_ACC_DATA_RDY: ev = (struct TripleAxisDataEvent *)evtData; fillSamples(ev, ACC); drainSamples(); break; case EVT_SENSOR_GYR_DATA_RDY: ev = (struct TripleAxisDataEvent *)evtData; fillSamples(ev, GYR); drainSamples(); break; case EVT_SENSOR_MAG_BIAS: ev = (struct TripleAxisDataEvent *)evtData; if (ev->samples[0].firstSample.biasPresent && mTask.flags & FUSION_FLAG_ENABLED) { //it is a user initiated mag cal event fusionSetMagTrust(&mTask.fusion, MANUAL_MAG_CAL); } break; case EVT_SENSOR_MAG_DATA_RDY: ev = (struct TripleAxisDataEvent *)evtData; fillSamples(ev, MAG); drainSamples(); break; } } static const struct SensorOps mSops = { .sensorPower = fusionPower, .sensorFirmwareUpload = fusionFirmwareUpload, .sensorSetRate = fusionSetRate, .sensorFlush = fusionFlush, }; static bool fusionStart(uint32_t tid) { size_t i, slabSize; mTask.tid = tid; mTask.flags = 0; for (i = 0; i < NUM_OF_RAW_SENSOR; i++) { mTask.sample_counts[i] = 0; mTask.sample_indices[i] = 0; } for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++) { mTask.sensors[i].handle = sensorRegister(&mSi[i], &mSops, (void *)i, true); mTask.sensors[i].idx = i; mTask.sensors[i].use_gyro_data = true; mTask.sensors[i].use_mag_data = true; } mTask.sensors[GEOMAG].use_gyro_data = false; mTask.sensors[GAME].use_mag_data = false; mTask.sensors[GRAVITY].use_mag_data = false; mTask.sensors[LINEAR].use_mag_data = false; mTask.accel_client_cnt = 0; mTask.gyro_client_cnt = 0; mTask.mag_client_cnt = 0; slabSize = sizeof(struct TripleAxisDataEvent) + MAX_NUM_COMMS_EVENT_SAMPLES * sizeof(struct TripleAxisDataPoint); // worst case 6 output sensors * (N + 1) comms_events mDataSlab = slabAllocatorNew(slabSize, 4, 6 * (NUM_COMMS_EVENTS_IN_FIFO + 1)); if (!mDataSlab) { osLog(LOG_ERROR, "ORIENTATION: slabAllocatorNew() FAILED\n"); return false; } osEventSubscribe(mTask.tid, EVT_APP_START); return true; } static void fusionEnd() { mTask.flags &= ~FUSION_FLAG_INITIALIZED; mTask.flags &= ~FUSION_FLAG_GAME_INITIALIZED; slabAllocatorDestroy(mDataSlab); } INTERNAL_APP_INIT( APP_ID_MAKE(APP_ID_VENDOR_GOOGLE, 4), ORIENTATION_APP_VERSION, fusionStart, fusionEnd, fusionHandleEvent);