| /* |
| * 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 <accelerometer.h> |
| |
| #include <nanohub_math.h> |
| #include <fusion/fusion.h> |
| #include <sensors.h> |
| #include <limits.h> |
| #include <slab.h> |
| |
| #define MAX_NUM_COMMS_EVENT_SAMPLES 15 |
| #define FIFO_MARGIN 10 |
| #define MAX_NUM_SAMPLES (MAX_NUM_COMMS_EVENT_SAMPLES + FIFO_MARGIN) |
| #define MAX_NUM_RAW_DATA_SAMPLES 50000000000ull |
| #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 kGravityEarth 9.80665f |
| #define DEFAULT_GYRO_RATE_HZ SENSOR_HZ(100.0f) |
| #define DEFAULT_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_gyr_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 acc_cnt; |
| uint8_t gyr_cnt; |
| uint8_t mag_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, |
| }; |
| |
| static const uint64_t rateTimerVals[] = //should match "supported rates in length" and be the timer length for that rate in nanosecs |
| { |
| 1000000000ULL / 12.5f, |
| 1000000000ULL / 25, |
| 1000000000ULL / 50, |
| 1000000000ULL / 100, |
| 1000000000ULL / 200, |
| }; |
| |
| static struct FusionTask mTask; |
| static const struct SensorInfo mSi[NUM_OF_FUSION_SENSOR] = |
| { |
| {"Orientation", FusionRates, SENS_TYPE_ORIENTATION, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 20 }, |
| {"Gravity", FusionRates, SENS_TYPE_GRAVITY, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 20 }, |
| {"Geomagnetic Rotation Vector", FusionRates, SENS_TYPE_GEO_MAG_ROT_VEC, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 20 }, |
| {"Linear Acceleration", FusionRates, SENS_TYPE_LINEAR_ACCEL, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 20 }, |
| {"Game Rotation Vector", FusionRates, SENS_TYPE_GAME_ROT_VECTOR, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 300 }, |
| {"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.gyr_cnt == 0) { |
| return; |
| } |
| if (index == MAG && mTask.mag_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 happens during BMI160 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 void addSample(struct FusionSensor *mSensor, uint64_t time, float x, float y, float z) |
| { |
| struct TripleAxisDataPoint *sample; |
| uint32_t deltaTime = 0; |
| |
| if (mSensor->ev == NULL) { |
| mSensor->ev = slabAllocatorAlloc(mDataSlab); |
| if (mSensor->ev == NULL) { |
| // slaballocation failed |
| osLog(LOG_ERROR, "FUSION: Slab Allocation Failed\n"); |
| return; |
| } |
| mSensor->ev->samples[0].firstSample.numSamples = 0; |
| mSensor->ev->referenceTime = time; |
| mSensor->prev_time = time; |
| } |
| |
| if (mSensor->ev->samples[0].firstSample.numSamples >= MAX_NUM_COMMS_EVENT_SAMPLES) { |
| osLog(LOG_ERROR, "BAD_INDEX\n"); |
| return; |
| } |
| |
| sample = &mSensor->ev->samples[mSensor->ev->samples[0].firstSample.numSamples++]; |
| |
| if (mSensor->ev->samples[0].firstSample.numSamples > 1) { |
| deltaTime = time - mSensor->prev_time; |
| deltaTime = deltaTime < 0 ? 0 : deltaTime; |
| sample->deltaTime = deltaTime; |
| 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) { |
| osEnqueueEvt(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; |
| |
| if (fusionHasEstimate(&mTask.game)) { |
| 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); |
| 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 (fusionHasEstimate(&mTask.fusion)) { |
| fusionGetRotationMatrix(&mTask.fusion, &R); |
| fusionGetAttitude(&mTask.fusion, &attitude); |
| |
| if (mTask.sensors[ORIENT].active) { |
| // x, y, z = yaw, pitch, roll |
| static const float kRad2deg = 180.0f / M_PI; |
| 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); |
| } |
| |
| if (last_accel_sample_index >= 0 |
| && mTask.sensors[LINEAR].active) { |
| 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); |
| } |
| } |
| } |
| |
| 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.gyr_cnt > 0) |
| j = mTask.sample_indices[GYR]; |
| |
| if (mTask.mag_cnt > 0) |
| k = mTask.sample_indices[MAG]; |
| |
| while (mTask.sample_counts[ACC] > 0 |
| && (!mTask.gyr_cnt > 0 || mTask.sample_counts[GYR] > 0) |
| && (!mTask.mag_cnt > 0 || mTask.sample_counts[MAG] > 0)) { |
| a_time = mTask.samples[ACC][i].time; |
| g_time = mTask.gyr_cnt > 0 ? mTask.samples[GYR][j].time |
| : ULONG_LONG_MAX; |
| m_time = mTask.mag_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); |
| |
| --mTask.sample_counts[MAG]; |
| if (++k == MAX_NUM_SAMPLES) |
| k = 0; |
| break; |
| } |
| } |
| |
| mTask.sample_indices[ACC] = i; |
| |
| if (mTask.gyr_cnt > 0) |
| mTask.sample_indices[GYR] = j; |
| |
| if (mTask.mag_cnt > 0) |
| mTask.sample_indices[MAG] = k; |
| |
| for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++) { |
| if (mTask.sensors[i].ev != NULL) { |
| osEnqueueEvt(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[LINEAR].active |
| || mTask.sensors[GEOMAG].active) { |
| mTask.flags |= FUSION_FLAG_ENABLED; |
| initFusion(&mTask.fusion, |
| (mTask.mag_cnt > 0 ? FUSION_USE_MAG : 0) | |
| (mTask.gyr_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.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); |
| 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; |
| uint32_t total_sample = 0; |
| |
| 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.acc_cnt > 0) { |
| mTask.ResamplePeriodNs[ACC] = sensorTimerLookupCommon(FusionRates, rateTimerVals, max_rate); |
| mTask.raw_sensor_rate[ACC] = max_rate; |
| total_sample += mTask.raw_sensor_rate[ACC]; |
| } |
| |
| if (mTask.gyr_cnt > 0) { |
| mTask.ResamplePeriodNs[GYR] = sensorTimerLookupCommon(FusionRates, rateTimerVals, max_rate); |
| gyr_rate = max_rate > DEFAULT_GYRO_RATE_HZ ? max_rate : DEFAULT_GYRO_RATE_HZ; |
| mTask.raw_sensor_rate[GYR] = gyr_rate; |
| total_sample += mTask.raw_sensor_rate[GYR]; |
| } |
| |
| if (mTask.mag_cnt > 0) { |
| mag_rate = max_rate < DEFAULT_MAG_RATE_HZ ? max_rate : DEFAULT_MAG_RATE_HZ; |
| mTask.ResamplePeriodNs[MAG] = sensorTimerLookupCommon(FusionRates, rateTimerVals, mag_rate); |
| mTask.raw_sensor_rate[MAG] = mag_rate; |
| total_sample += mTask.raw_sensor_rate[MAG]; |
| } |
| |
| //Note: |
| // Previous code here: |
| // mTask.raw_sensor_latency = total_sample ? (MAX_NUM_RAW_DATA_SAMPLES * 1024ull / total_sample) : 0; |
| // |
| // At this point total_sample = sum of 0, 1, 2, or 3 values from FusionRates[]. |
| // But also note that for all N, FusionRates[N] = SENSOR_HZ(2^ N * 12.5f), |
| // Thus means that total_samples here is sure to be an integer multiple of |
| // SENSOR_HZ(12.5f). We'll use this here to avoid a uint64 division. Also of |
| // note is that when they use 1024ull here, they really mean SENSOR_HZ(1) just |
| // to remove the SENSOR_HZ() bias. So we'll decompose this into a few pieces. |
| // For now we'll ingore the zero-check on total_sample. Just pretend it is there. |
| // First we'll divide both top and bottom by SENSOR_HZ(1), and thus get |
| // mTask.raw_sensor_latency = MAX_NUM_RAW_DATA_SAMPLES / (total_sample / SENSOR_HZ(1)) |
| // this has not lost us any precision, as we know total_sample is divisible |
| // by SENSOR_HZ(12.5f), and thus certainly by SENSOR_HZ(1). But wait, there is |
| // more. We can actually divide both top and bottom by SENSOR_HZ(12.5) to reduce |
| // the size of that constant. We thus get: |
| // mTask.raw_sensor_latency = (MAX_NUM_RAW_DATA_SAMPLES / 12.5) / (total_sample / SENSOR_HZ(12.5)) |
| // Why all this? Well, MAX_NUM_RAW_DATA_SAMPLES is used just here, so it just gets |
| // redefined from 50000000000ull to 4000000000ull. And that, my friends is a |
| // 32-bit number. woohoo! |
| |
| mTask.raw_sensor_latency = total_sample ? ((uint32_t)(MAX_NUM_RAW_DATA_SAMPLES / 12.5f) / (total_sample / SENSOR_HZ(12.5))) : 0; |
| 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.acc_cnt > 0) |
| fusionSetRateAcc(); |
| if (mTask.gyr_cnt > 0) |
| fusionSetRateGyr(); |
| if (mTask.mag_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.acc_cnt--; |
| if (mSensor->idx != GEOMAG) |
| mTask.gyr_cnt--; |
| if (mSensor->idx != GAME && mSensor->idx != GRAVITY) |
| mTask.mag_cnt--; |
| |
| // if cnt == 0 and Handle == 0, nothing need to be done. |
| // if cnt > 0 and Handle == 0, something else is turning it on, all will be done. |
| if (mTask.acc_cnt == 0 && mTask.accelHandle != 0) { |
| sensorRelease(mTask.tid, mTask.accelHandle); |
| mTask.accelHandle = 0; |
| osEventUnsubscribe(mTask.tid, EVT_SENSOR_ACC_DATA_RDY); |
| } |
| |
| if (mTask.gyr_cnt == 0 && mTask.gyroHandle != 0) { |
| sensorRelease(mTask.tid, mTask.gyroHandle); |
| mTask.gyroHandle = 0; |
| osEventUnsubscribe(mTask.tid, EVT_SENSOR_GYR_DATA_RDY); |
| } |
| |
| if (mTask.mag_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.acc_cnt++; |
| if (mSensor->idx != GEOMAG) |
| mTask.gyr_cnt++; |
| if (mSensor->idx != GAME && mSensor->idx != GRAVITY) |
| mTask.mag_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; |
| |
| if (evtData == SENSOR_DATA_EVENT_FLUSH) |
| return; |
| |
| switch (evtType) { |
| 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_DATA_RDY: |
| ev = (struct TripleAxisDataEvent *)evtData; |
| fillSamples(ev, MAG); |
| drainSamples(); |
| break; |
| } |
| } |
| |
| static const struct SensorOps mSops = |
| { |
| fusionPower, |
| fusionFirmwareUpload, |
| fusionSetRate, |
| fusionFlush, |
| NULL, |
| NULL, |
| }; |
| |
| static bool fusionStart(uint32_t tid) |
| { |
| osLog(LOG_INFO, " ORIENTATION: %ld\n", 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_gyr_data = true; |
| mTask.sensors[i].use_mag_data = true; |
| } |
| |
| mTask.sensors[GEOMAG].use_gyr_data = false; |
| mTask.sensors[GAME].use_mag_data = false; |
| mTask.sensors[GRAVITY].use_mag_data = false; |
| |
| |
| slabSize = sizeof(struct TripleAxisDataEvent) |
| + MAX_NUM_COMMS_EVENT_SAMPLES * sizeof(struct TripleAxisDataPoint); |
| mDataSlab = slabAllocatorNew(slabSize, 4, 10); // 10 slots for now.. |
| if (!mDataSlab) { |
| osLog(LOG_ERROR, "FUSION SLAB ALLOCATION FAILED\n"); |
| return false; |
| } |
| |
| return true; |
| } |
| |
| static void fusionEnd() |
| { |
| mTask.flags &= ~FUSION_FLAG_INITIALIZED; |
| slabAllocatorDestroy(mDataSlab); |
| } |
| |
| INTERNAL_APP_INIT( |
| APP_ID_MAKE(APP_ID_VENDOR_GOOGLE, 4), |
| 0, |
| fusionStart, |
| fusionEnd, |
| fusionHandleEvent); |
