orientation.c revision 39bac2d9ad663f0d040aa2e9ba0c3b51831c9dd7
114986b090cf259d3d9e6762520dfc276e30de911Ashutosh Joshi/* 214986b090cf259d3d9e6762520dfc276e30de911Ashutosh Joshi * Copyright (C) 2016 The Android Open Source Project 314986b090cf259d3d9e6762520dfc276e30de911Ashutosh Joshi * 414986b090cf259d3d9e6762520dfc276e30de911Ashutosh Joshi * Licensed under the Apache License, Version 2.0 (the "License"); 514986b090cf259d3d9e6762520dfc276e30de911Ashutosh Joshi * you may not use this file except in compliance with the License. 614986b090cf259d3d9e6762520dfc276e30de911Ashutosh Joshi * You may obtain a copy of the License at 714986b090cf259d3d9e6762520dfc276e30de911Ashutosh Joshi * 814986b090cf259d3d9e6762520dfc276e30de911Ashutosh Joshi * http://www.apache.org/licenses/LICENSE-2.0 914986b090cf259d3d9e6762520dfc276e30de911Ashutosh Joshi * 1014986b090cf259d3d9e6762520dfc276e30de911Ashutosh Joshi * Unless required by applicable law or agreed to in writing, software 1114986b090cf259d3d9e6762520dfc276e30de911Ashutosh Joshi * distributed under the License is distributed on an "AS IS" BASIS, 1214986b090cf259d3d9e6762520dfc276e30de911Ashutosh Joshi * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 1314986b090cf259d3d9e6762520dfc276e30de911Ashutosh Joshi * See the License for the specific language governing permissions and 1414986b090cf259d3d9e6762520dfc276e30de911Ashutosh Joshi * limitations under the License. 1514986b090cf259d3d9e6762520dfc276e30de911Ashutosh Joshi */ 1614986b090cf259d3d9e6762520dfc276e30de911Ashutosh Joshi 17960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian#include <stdlib.h> 18960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian#include <string.h> 19960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian#include <timer.h> 20960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian#include <heap.h> 21960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian#include <plat/inc/rtc.h> 22960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian#include <plat/inc/syscfg.h> 23960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian#include <hostIntf.h> 247062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema#include <nanohubPacket.h> 25ae081f1f47013ddf08e5af685b9f6d02ac76421bDmitry Grinberg#include <floatRt.h> 26960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 27960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian#include <seos.h> 28960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 29960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian#include <nanohub_math.h> 3039bac2d9ad663f0d040aa2e9ba0c3b51831c9dd7Meng-hsuan Chung#include <algos/fusion.h> 31960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian#include <sensors.h> 32960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian#include <limits.h> 33e0e5b567748b8d46f60ede2ff12c4b2358ffe0dbZhengyin Qian#include <slab.h> 34960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 3546852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung#define MAX_NUM_COMMS_EVENT_SAMPLES 15 // at most 15 output samples can fit in one comms_event 3646852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung#define FIFO_MARGIN 10 // max raw sensor rate ratio is 8:1, there can be 7 samples left in FIFO. 3746852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung#define FIFO_DEPTH 15 // needs to be greater than max raw sensor rate ratio 3846852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung#define MAX_NUM_SAMPLES (FIFO_MARGIN + FIFO_DEPTH) 399105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema#define EVT_SENSOR_ACC_DATA_RDY sensorGetMyEventType(SENS_TYPE_ACCEL) 409105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema#define EVT_SENSOR_GYR_DATA_RDY sensorGetMyEventType(SENS_TYPE_GYRO) 419105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema#define EVT_SENSOR_MAG_DATA_RDY sensorGetMyEventType(SENS_TYPE_MAG) 4218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 439105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema#define kGravityEarth 9.80665f 4446852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung#define kRad2deg (180.0f / M_PI) 459105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema#define MIN_GYRO_RATE_HZ SENSOR_HZ(100.0f) 469105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema#define MAX_MAG_RATE_HZ SENSOR_HZ(50.0f) 47960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 481be10b6751f50675307716747bd4d729d9e7aff5Ben Fennemaenum 491be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema{ 501be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema FUSION_FLAG_ENABLED = 0x01, 511be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema FUSION_FLAG_INITIALIZED = 0x08, 521be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema FUSION_FLAG_GAME_ENABLED = 0x10, 531be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema FUSION_FLAG_GAME_INITIALIZED = 0x20 54960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian}; 55960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 5618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianenum RawSensorType 5718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian{ 5818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian ACC, 5918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian GYR, 6018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian MAG, 6118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian NUM_OF_RAW_SENSOR 6218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian}; 6318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 6418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianenum FusionSensorType 651be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema{ 6618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian ORIENT, 6718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian GRAVITY, 6818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian GEOMAG, 6918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian LINEAR, 7018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian GAME, 7118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian ROTAT, 7218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian NUM_OF_FUSION_SENSOR 731be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema}; 741be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema 751be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema 7618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianstruct FusionSensorSample { 77960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian uint64_t time; 78960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian float x, y, z; 79960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian}; 80960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 8118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianstruct FusionSensor { 82960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian uint32_t handle; 8318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian struct TripleAxisDataEvent *ev; 84c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian uint64_t prev_time; 8518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian uint64_t latency; 8618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian uint32_t rate; 8718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian bool active; 889105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema bool use_gyro_data; 8918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian bool use_mag_data; 9018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian uint8_t idx; 9118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian}; 9218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 9318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianstruct FusionTask { 9418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian uint32_t tid; 951be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema uint32_t accelHandle; 961be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema uint32_t gyroHandle; 971be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema uint32_t magHandle; 98960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 99960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian struct Fusion fusion; 10018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian struct Fusion game; 101960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 10218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian struct FusionSensor sensors[NUM_OF_FUSION_SENSOR]; 10318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian struct FusionSensorSample samples[NUM_OF_RAW_SENSOR][MAX_NUM_SAMPLES]; 10418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian size_t sample_indices[NUM_OF_RAW_SENSOR]; 10518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian size_t sample_counts[NUM_OF_RAW_SENSOR]; 10618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian uint32_t counters[NUM_OF_RAW_SENSOR]; 10718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian uint64_t ResamplePeriodNs[NUM_OF_RAW_SENSOR]; 10818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian uint64_t last_time[NUM_OF_RAW_SENSOR]; 10918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian struct TripleAxisDataPoint last_sample[NUM_OF_RAW_SENSOR]; 110960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 111960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian uint32_t flags; 112960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 113c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian uint32_t raw_sensor_rate[NUM_OF_RAW_SENSOR]; 114c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian uint64_t raw_sensor_latency; 115c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian 1169105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema uint8_t accel_cnt; 1179105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema uint8_t gyro_cnt; 11818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian uint8_t mag_cnt; 119960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian}; 120960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 12118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianstatic uint32_t FusionRates[] = { 122960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian SENSOR_HZ(12.5f), 123960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian SENSOR_HZ(25.0f), 124960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian SENSOR_HZ(50.0f), 125960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian SENSOR_HZ(100.0f), 126960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian SENSOR_HZ(200.0f), 127960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 0, 128960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian}; 129960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 130dd2f9808f0045a3eaa42cb488153eeb599eb97ffDmitry Grinbergstatic const uint64_t rateTimerVals[] = //should match "supported rates in length" and be the timer length for that rate in nanosecs 131dd2f9808f0045a3eaa42cb488153eeb599eb97ffDmitry Grinberg{ 132dd2f9808f0045a3eaa42cb488153eeb599eb97ffDmitry Grinberg 1000000000ULL / 12.5f, 133dd2f9808f0045a3eaa42cb488153eeb599eb97ffDmitry Grinberg 1000000000ULL / 25, 134dd2f9808f0045a3eaa42cb488153eeb599eb97ffDmitry Grinberg 1000000000ULL / 50, 135dd2f9808f0045a3eaa42cb488153eeb599eb97ffDmitry Grinberg 1000000000ULL / 100, 136dd2f9808f0045a3eaa42cb488153eeb599eb97ffDmitry Grinberg 1000000000ULL / 200, 137dd2f9808f0045a3eaa42cb488153eeb599eb97ffDmitry Grinberg}; 138dd2f9808f0045a3eaa42cb488153eeb599eb97ffDmitry Grinberg 13918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianstatic struct FusionTask mTask; 1404d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema 1414d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema#define DEC_INFO_RATE(name, rates, type, axis, inter, samples) \ 1424d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema .sensorName = name, \ 1434d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema .supportedRates = rates, \ 1444d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema .sensorType = type, \ 1454d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema .numAxis = axis, \ 1464d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema .interrupt = inter, \ 1474d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema .minSamples = samples 1484d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema 14918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianstatic const struct SensorInfo mSi[NUM_OF_FUSION_SENSOR] = 15064eb1702a319ad4bbc181ed1e5af0703aea7ea22Zhengyin Qian{ 1514d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema { DEC_INFO_RATE("Orientation", FusionRates, SENS_TYPE_ORIENTATION, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 20) }, 1524d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema { DEC_INFO_RATE("Gravity", FusionRates, SENS_TYPE_GRAVITY, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 20) }, 1534d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema { DEC_INFO_RATE("Geomagnetic Rotation Vector", FusionRates, SENS_TYPE_GEO_MAG_ROT_VEC, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 20) }, 1544d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema { DEC_INFO_RATE("Linear Acceleration", FusionRates, SENS_TYPE_LINEAR_ACCEL, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 20) }, 1554d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema { DEC_INFO_RATE("Game Rotation Vector", FusionRates, SENS_TYPE_GAME_ROT_VECTOR, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 300) }, 1564d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema { DEC_INFO_RATE("Rotation Vector", FusionRates, SENS_TYPE_ROTATION_VECTOR, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 20) }, 15764eb1702a319ad4bbc181ed1e5af0703aea7ea22Zhengyin Qian}; 158960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 159e0e5b567748b8d46f60ede2ff12c4b2358ffe0dbZhengyin Qianstatic struct SlabAllocator *mDataSlab; 160e0e5b567748b8d46f60ede2ff12c4b2358ffe0dbZhengyin Qian 161e0e5b567748b8d46f60ede2ff12c4b2358ffe0dbZhengyin Qianstatic void dataEvtFree(void *ptr) 162960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian{ 163e0e5b567748b8d46f60ede2ff12c4b2358ffe0dbZhengyin Qian slabAllocatorFree(mDataSlab, ptr); 164960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian} 165960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 16618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianstatic void fillSamples(struct TripleAxisDataEvent *ev, enum RawSensorType index) 167960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian{ 16818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian bool bad_timestamp; 16918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian size_t i, w, n, num_samples; 17018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian struct TripleAxisDataPoint *curr_sample, *next_sample; 171f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema uint32_t counter; 172f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema uint64_t ResamplePeriodNs, curr_time, next_time; 17318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian uint64_t sample_spacing_ns; 17418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian float weight_next; 17518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 1769105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema if (index == GYR && mTask.gyro_cnt == 0) { 177960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian return; 178960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 17918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (index == MAG && mTask.mag_cnt == 0) { 180960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian return; 181960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 182960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 18318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian n = mTask.sample_counts[index]; 18418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian i = mTask.sample_indices[index]; 18518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian counter = mTask.counters[index]; 18618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian ResamplePeriodNs = mTask.ResamplePeriodNs[index]; 18718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian w = (mTask.sample_indices[index] + n) % MAX_NUM_SAMPLES; 18818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 18918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian // check if this sensor was used before 19018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.last_time[index] == ULONG_LONG_MAX) { 19118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian curr_sample = ev->samples; 19218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian next_sample = curr_sample + 1; 193f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema num_samples = ev->samples[0].firstSample.numSamples; 19418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian curr_time = ev->referenceTime; 195960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } else { 19618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian curr_sample = &mTask.last_sample[index]; 19718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian next_sample = ev->samples; 198f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema num_samples = ev->samples[0].firstSample.numSamples + 1; 19918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian curr_time = mTask.last_time[index]; 20018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 201960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 20218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian while (num_samples > 1) { 203960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 20418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (next_sample == ev->samples) 20518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian next_time = ev->referenceTime; 20618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian else 20718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian next_time = curr_time + next_sample->deltaTime; 208960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 20918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian // error handling for non-chronological accel timestamps 21018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian sample_spacing_ns = (next_time > curr_time) ? (next_time - curr_time) : 0; 211960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 21246852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung // This can happen during sensor config changes 21318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian bad_timestamp = (sample_spacing_ns > 10 * ResamplePeriodNs); 214960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 21518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian // Check to see if we need to move the interpolation window or 21618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian // interpolate 21718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if ((counter >= sample_spacing_ns) || bad_timestamp) { 21818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian num_samples--; 21918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian counter -= (bad_timestamp ? counter : sample_spacing_ns); 22018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian curr_sample = next_sample; 22118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian next_sample++; 222960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 223c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian curr_time = next_time; 22418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } else { 225ae081f1f47013ddf08e5af685b9f6d02ac76421bDmitry Grinberg weight_next = (float)counter / floatFromUint64(sample_spacing_ns); 22618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 22718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.samples[index][w].x = curr_sample->x + weight_next * 22818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian (next_sample->x - curr_sample->x); 22918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.samples[index][w].y = curr_sample->y + weight_next * 23018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian (next_sample->y - curr_sample->y); 23118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.samples[index][w].z = curr_sample->z + weight_next * 23218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian (next_sample->z - curr_sample->z); 23318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.samples[index][w].time = curr_time + counter; 23418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 23518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian // Move the read index when buffer is full 23618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (++n > MAX_NUM_SAMPLES) { 23718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian n = MAX_NUM_SAMPLES; 23818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 23918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (++i == MAX_NUM_SAMPLES) { 24018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian i = 0; 24118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 24218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 24318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 24418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian // Reset the write index 24518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (++w == MAX_NUM_SAMPLES) { 24618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian w = 0; 24718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 24818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 24918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian // Move to the next resample 25018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian counter += ResamplePeriodNs; 251960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 252960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 25318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 25418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sample_counts[index] = n; 25518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sample_indices[index] = i; 25618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.counters[index] = counter; 25718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.last_sample[index] = *curr_sample; 25818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.last_time[index] = curr_time; 259960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian} 260960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 26118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianstatic void addSample(struct FusionSensor *mSensor, uint64_t time, float x, float y, float z) 262960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian{ 2637062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema struct TripleAxisDataPoint *sample; 264e0e5b567748b8d46f60ede2ff12c4b2358ffe0dbZhengyin Qian 26518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mSensor->ev == NULL) { 26618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mSensor->ev = slabAllocatorAlloc(mDataSlab); 26718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mSensor->ev == NULL) { 268e0e5b567748b8d46f60ede2ff12c4b2358ffe0dbZhengyin Qian // slaballocation failed 269c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian osLog(LOG_ERROR, "FUSION: Slab Allocation Failed\n"); 270e0e5b567748b8d46f60ede2ff12c4b2358ffe0dbZhengyin Qian return; 271e0e5b567748b8d46f60ede2ff12c4b2358ffe0dbZhengyin Qian } 272f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema mSensor->ev->samples[0].firstSample.numSamples = 0; 27318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mSensor->ev->referenceTime = time; 274c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian mSensor->prev_time = time; 275960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 276960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 277f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema if (mSensor->ev->samples[0].firstSample.numSamples >= MAX_NUM_COMMS_EVENT_SAMPLES) { 278960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian osLog(LOG_ERROR, "BAD_INDEX\n"); 279960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian return; 280960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 281960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 282f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema sample = &mSensor->ev->samples[mSensor->ev->samples[0].firstSample.numSamples++]; 283e0e5b567748b8d46f60ede2ff12c4b2358ffe0dbZhengyin Qian 284c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian if (mSensor->ev->samples[0].firstSample.numSamples > 1) { 28546852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung sample->deltaTime = time > mSensor->prev_time ? (time - mSensor->prev_time) : 0; 286c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian mSensor->prev_time = time; 287c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian } 288960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 289960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian sample->x = x; 290960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian sample->y = y; 291960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian sample->z = z; 292960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 293f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema if (mSensor->ev->samples[0].firstSample.numSamples == MAX_NUM_COMMS_EVENT_SAMPLES) { 29435339af82310e6a49884f5b845fb9557af360de7Zhengyin Qian osEnqueueEvt(EVENT_TYPE_BIT_DISCARDABLE | sensorGetMyEventType(mSi[mSensor->idx].sensorType), 29535339af82310e6a49884f5b845fb9557af360de7Zhengyin Qian mSensor->ev, dataEvtFree); 29618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mSensor->ev = NULL; 297960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 298960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian} 299960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 300960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qianstatic void updateOutput(ssize_t last_accel_sample_index, uint64_t last_sensor_time) 301960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian{ 30218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian struct Vec4 attitude; 30318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian struct Vec3 g, a; 30418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian struct Mat33 R; 30518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 30618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (fusionHasEstimate(&mTask.game)) { 30718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.sensors[GAME].active) { 30818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian fusionGetAttitude(&mTask.game, &attitude); 30918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian addSample(&mTask.sensors[GAME], 31018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian last_sensor_time, 31118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian attitude.x, 31218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian attitude.y, 31318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian attitude.z); 31418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 31518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 31618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.sensors[GRAVITY].active) { 31718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian fusionGetRotationMatrix(&mTask.game, &R); 31818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian initVec3(&g, R.elem[0][2], R.elem[1][2], R.elem[2][2]); 31918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian vec3ScalarMul(&g, kGravityEarth); 32018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian addSample(&mTask.sensors[GRAVITY], 32118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian last_sensor_time, 32218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian g.x, g.y, g.z); 32318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 32418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 32518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 326960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian if (fusionHasEstimate(&mTask.fusion)) { 327960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian fusionGetRotationMatrix(&mTask.fusion, &R); 328960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian fusionGetAttitude(&mTask.fusion, &attitude); 329960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 33018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.sensors[ORIENT].active) { 33118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian // x, y, z = yaw, pitch, roll 33218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian float x = atan2f(-R.elem[0][1], R.elem[0][0]) * kRad2deg; 33318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian float y = atan2f(-R.elem[1][2], R.elem[2][2]) * kRad2deg; 33418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian float z = asinf(R.elem[0][2]) * kRad2deg; 33518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 33618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (x < 0.0f) { 33718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian x += 360.0f; 33818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 33918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 34018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian addSample(&mTask.sensors[ORIENT], 34118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian last_sensor_time, x, y, z); 34218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 343960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 34418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.sensors[GEOMAG].active) { 34518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian addSample(&mTask.sensors[GEOMAG], 34618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian last_sensor_time, 34718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian attitude.x, 34818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian attitude.y, 34918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian attitude.z); 350960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 351960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 35218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.sensors[ROTAT].active) { 35318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian addSample(&mTask.sensors[ROTAT], 35418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian last_sensor_time, 35518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian attitude.x, 35618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian attitude.y, 35718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian attitude.z); 35818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 35918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 36018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (last_accel_sample_index >= 0 36118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian && mTask.sensors[LINEAR].active) { 36218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian initVec3(&g, R.elem[0][2], R.elem[1][2], R.elem[2][2]); 36318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian vec3ScalarMul(&g, kGravityEarth); 36418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian initVec3(&a, 36518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.samples[0][last_accel_sample_index].x, 36618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.samples[0][last_accel_sample_index].y, 36718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.samples[0][last_accel_sample_index].z); 36818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 36918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian addSample(&mTask.sensors[LINEAR], 37018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.samples[0][last_accel_sample_index].time, 37118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian a.x - g.x, 37218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian a.y - g.y, 37318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian a.z - g.z); 37418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 375960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 376960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian} 377960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 378960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qianstatic void drainSamples() 379960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian{ 38018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian size_t i = mTask.sample_indices[ACC]; 381960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian size_t j = 0; 382960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian size_t k = 0; 3831be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema size_t which; 3841be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema struct Vec3 a, w, m; 3851be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema float dT; 3861be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema uint64_t a_time, g_time, m_time; 3871be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema 3889105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema if (mTask.gyro_cnt > 0) 38918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian j = mTask.sample_indices[GYR]; 3901be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema 39118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.mag_cnt > 0) 39218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian k = mTask.sample_indices[MAG]; 3931be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema 39418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian while (mTask.sample_counts[ACC] > 0 3959105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema && (!mTask.gyro_cnt > 0 || mTask.sample_counts[GYR] > 0) 39618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian && (!mTask.mag_cnt > 0 || mTask.sample_counts[MAG] > 0)) { 39718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian a_time = mTask.samples[ACC][i].time; 3989105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema g_time = mTask.gyro_cnt > 0 ? mTask.samples[GYR][j].time 399960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian : ULONG_LONG_MAX; 40018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian m_time = mTask.mag_cnt > 0 ? mTask.samples[MAG][k].time 401960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian : ULONG_LONG_MAX; 402960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 403960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian // priority with same timestamp: gyro > acc > mag 404960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian if (g_time <= a_time && g_time <= m_time) { 40518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian which = GYR; 406960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } else if (a_time <= m_time) { 40718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian which = ACC; 408960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } else { 40918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian which = MAG; 410960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 411960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 412a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian dT = floatFromUint64(mTask.ResamplePeriodNs[which]) * 1e-9f; 4131be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema switch (which) { 41418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian case ACC: 41518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian initVec3(&a, mTask.samples[ACC][i].x, mTask.samples[ACC][i].y, mTask.samples[ACC][i].z); 41618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 41718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.flags & FUSION_FLAG_ENABLED) 41818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian fusionHandleAcc(&mTask.fusion, &a, dT); 419960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 42018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.flags & FUSION_FLAG_GAME_ENABLED) 42118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian fusionHandleAcc(&mTask.game, &a, dT); 422960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 42318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian updateOutput(i, mTask.samples[ACC][i].time); 424960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 42518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian --mTask.sample_counts[ACC]; 4261be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema if (++i == MAX_NUM_SAMPLES) 4271be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema i = 0; 4281be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema break; 42918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian case GYR: 43018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian initVec3(&w, mTask.samples[GYR][j].x, mTask.samples[GYR][j].y, mTask.samples[GYR][j].z); 431960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 43218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.flags & FUSION_FLAG_ENABLED) 43318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian fusionHandleGyro(&mTask.fusion, &w, dT); 434960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 43518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.flags & FUSION_FLAG_GAME_ENABLED) 43618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian fusionHandleGyro(&mTask.game, &w, dT); 43718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 43818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian --mTask.sample_counts[GYR]; 4391be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema if (++j == MAX_NUM_SAMPLES) 4401be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema j = 0; 4411be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema break; 44218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian case MAG: 44318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian initVec3(&m, mTask.samples[MAG][k].x, mTask.samples[MAG][k].y, mTask.samples[MAG][k].z); 444960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 4451be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema fusionHandleMag(&mTask.fusion, &m); 446960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 44718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian --mTask.sample_counts[MAG]; 4481be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema if (++k == MAX_NUM_SAMPLES) 4491be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema k = 0; 4501be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema break; 451960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 452960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 453960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 45418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sample_indices[ACC] = i; 455960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 4569105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema if (mTask.gyro_cnt > 0) 45718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sample_indices[GYR] = j; 458960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 45918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.mag_cnt > 0) 46018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sample_indices[MAG] = k; 461960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 46218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++) { 46318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.sensors[i].ev != NULL) { 46435339af82310e6a49884f5b845fb9557af360de7Zhengyin Qian osEnqueueEvt(EVENT_TYPE_BIT_DISCARDABLE | sensorGetMyEventType(mSi[i].sensorType), 46535339af82310e6a49884f5b845fb9557af360de7Zhengyin Qian mTask.sensors[i].ev, dataEvtFree); 46618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sensors[i].ev = NULL; 46718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 468960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 469960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian} 470960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 471960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qianstatic void configureFusion() 472960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian{ 47318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.sensors[ORIENT].active 47418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian || mTask.sensors[ROTAT].active 47518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian || mTask.sensors[LINEAR].active 47618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian || mTask.sensors[GEOMAG].active) { 47718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.flags |= FUSION_FLAG_ENABLED; 47818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian initFusion(&mTask.fusion, 47918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian (mTask.mag_cnt > 0 ? FUSION_USE_MAG : 0) | 4809105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema (mTask.gyro_cnt > 0 ? FUSION_USE_GYRO : 0) | 48118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian ((mTask.flags & FUSION_FLAG_INITIALIZED) ? 0 : FUSION_REINITIALIZE)); 48218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.flags |= FUSION_FLAG_INITIALIZED; 48318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } else { 48418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.flags &= ~FUSION_FLAG_ENABLED; 48518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.flags &= ~FUSION_FLAG_INITIALIZED; 48618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 487960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian} 488960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 48918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianstatic void configureGame() 490960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian{ 49118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.sensors[GAME].active || mTask.sensors[GRAVITY].active) { 49218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.flags |= FUSION_FLAG_GAME_ENABLED; 49318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian initFusion(&mTask.game, 49418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian FUSION_USE_GYRO | ((mTask.flags & FUSION_FLAG_INITIALIZED) ? 0 : FUSION_REINITIALIZE)); 49518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.flags |= FUSION_FLAG_GAME_INITIALIZED; 49618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } else { 49718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.flags &= ~FUSION_FLAG_GAME_ENABLED; 49818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.flags &= ~FUSION_FLAG_GAME_INITIALIZED; 499960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 500960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian} 501960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 502c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qianstatic void fusionSetRateAcc(void) 503960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian{ 5041be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema int i; 50518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.accelHandle == 0) { 50618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sample_counts[ACC] = 0; 50718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sample_indices[ACC] = 0; 50818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.counters[ACC] = 0; 50918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.last_time[ACC] = ULONG_LONG_MAX; 51046852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung for (i = 0; sensorFind(SENS_TYPE_ACCEL, i, &mTask.accelHandle) != NULL; i++) { 511c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian if (sensorRequest(mTask.tid, mTask.accelHandle, mTask.raw_sensor_rate[ACC], mTask.raw_sensor_latency)) { 5121be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema osEventSubscribe(mTask.tid, EVT_SENSOR_ACC_DATA_RDY); 5131be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema break; 5141be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema } 5151be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema } 5161be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema } else { 517c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian sensorRequestRateChange(mTask.tid, mTask.accelHandle, mTask.raw_sensor_rate[ACC], mTask.raw_sensor_latency); 5181be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema } 51918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian} 52018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 521c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qianstatic void fusionSetRateGyr(void) 52218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian{ 52318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian int i; 5241be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema if (mTask.gyroHandle == 0) { 52518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sample_counts[GYR] = 0; 52618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sample_indices[GYR] = 0; 52718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.counters[GYR] = 0; 52818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.last_time[GYR] = ULONG_LONG_MAX; 52946852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung for (i = 0; sensorFind(SENS_TYPE_GYRO, i, &mTask.gyroHandle) != NULL; i++) { 530c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian if (sensorRequest(mTask.tid, mTask.gyroHandle, mTask.raw_sensor_rate[GYR], mTask.raw_sensor_latency)) { 5311be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema osEventSubscribe(mTask.tid, EVT_SENSOR_GYR_DATA_RDY); 5321be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema break; 5331be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema } 5341be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema } 5351be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema } else { 536c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian sensorRequestRateChange(mTask.tid, mTask.gyroHandle, mTask.raw_sensor_rate[GYR], mTask.raw_sensor_latency); 5371be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema } 53818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian} 53918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 540c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qianstatic void fusionSetRateMag(void) 54118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian{ 54218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian int i; 5431be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema if (mTask.magHandle == 0) { 54418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sample_counts[MAG] = 0; 54518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sample_indices[MAG] = 0; 54618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.counters[MAG] = 0; 54718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.last_time[MAG] = ULONG_LONG_MAX; 54846852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung for (i = 0; sensorFind(SENS_TYPE_MAG, i, &mTask.magHandle) != NULL; i++) { 549c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian if (sensorRequest(mTask.tid, mTask.magHandle, mTask.raw_sensor_rate[MAG], mTask.raw_sensor_latency)) { 5501be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema osEventSubscribe(mTask.tid, EVT_SENSOR_MAG_DATA_RDY); 5511be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema break; 5521be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema } 5531be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema } 5541be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema } else { 555c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian sensorRequestRateChange(mTask.tid, mTask.magHandle, mTask.raw_sensor_rate[MAG], mTask.raw_sensor_latency); 556c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian } 557c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian} 558c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian 559e723815b378ee08afad27d0fe84253e22c050528Ben Fennemastatic bool fusionSetRate(uint32_t rate, uint64_t latency, void *cookie) 560c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian{ 561e723815b378ee08afad27d0fe84253e22c050528Ben Fennema struct FusionSensor *mSensor = &mTask.sensors[(int)cookie]; 562c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian int i; 563c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian uint32_t max_rate = 0; 564c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian uint32_t gyr_rate, mag_rate; 56546852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung uint64_t min_resample_period = ULONG_LONG_MAX; 566c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian 567c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian mSensor->rate = rate; 568c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian mSensor->latency = latency; 569c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian 570c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++) { 571c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian if (mTask.sensors[i].active) { 572c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian max_rate = max_rate > mTask.sensors[i].rate ? max_rate : mTask.sensors[i].rate; 573c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian } 574c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian } 575c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian 5769105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema if (mTask.accel_cnt > 0) { 577c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian mTask.raw_sensor_rate[ACC] = max_rate; 57846852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung mTask.ResamplePeriodNs[ACC] = sensorTimerLookupCommon(FusionRates, rateTimerVals, max_rate); 57946852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung min_resample_period = mTask.ResamplePeriodNs[ACC] < min_resample_period ? 58046852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung mTask.ResamplePeriodNs[ACC] : min_resample_period; 581c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian } 582c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian 5839105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema if (mTask.gyro_cnt > 0) { 5849105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema gyr_rate = max_rate > MIN_GYRO_RATE_HZ ? max_rate : MIN_GYRO_RATE_HZ; 585c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian mTask.raw_sensor_rate[GYR] = gyr_rate; 58646852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung mTask.ResamplePeriodNs[GYR] = sensorTimerLookupCommon(FusionRates, rateTimerVals, gyr_rate); 58746852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung min_resample_period = mTask.ResamplePeriodNs[GYR] < min_resample_period ? 58846852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung mTask.ResamplePeriodNs[GYR] : min_resample_period; 589c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian } 590c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian 591c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian if (mTask.mag_cnt > 0) { 5929105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema mag_rate = max_rate < MAX_MAG_RATE_HZ ? max_rate : MAX_MAG_RATE_HZ; 593c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian mTask.raw_sensor_rate[MAG] = mag_rate; 59446852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung mTask.ResamplePeriodNs[MAG] = sensorTimerLookupCommon(FusionRates, rateTimerVals, mag_rate); 59546852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung min_resample_period = mTask.ResamplePeriodNs[MAG] < min_resample_period ? 59646852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung mTask.ResamplePeriodNs[MAG] : min_resample_period; 597c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian } 598c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian 59946852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung // This guarantees that local raw sensor FIFOs won't overflow. 60046852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung mTask.raw_sensor_latency = min_resample_period * (FIFO_DEPTH - 1); 60146852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung 602c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++) { 603c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian if (mTask.sensors[i].active) { 604c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian mTask.raw_sensor_latency = mTask.sensors[i].latency < mTask.raw_sensor_latency ? 605c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian mTask.sensors[i].latency : mTask.raw_sensor_latency; 606c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian } 60718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 608c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian 6099105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema if (mTask.accel_cnt > 0) 610c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian fusionSetRateAcc(); 6119105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema if (mTask.gyro_cnt > 0) 612c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian fusionSetRateGyr(); 613c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian if (mTask.mag_cnt > 0) 614c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian fusionSetRateMag(); 615c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian if (mSensor->rate > 0) 616c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency); 617c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian 618c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian return true; 61918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian} 62018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 621e723815b378ee08afad27d0fe84253e22c050528Ben Fennemastatic bool fusionPower(bool on, void *cookie) 62218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian{ 623e723815b378ee08afad27d0fe84253e22c050528Ben Fennema struct FusionSensor *mSensor = &mTask.sensors[(int)cookie]; 624e723815b378ee08afad27d0fe84253e22c050528Ben Fennema int idx; 625e723815b378ee08afad27d0fe84253e22c050528Ben Fennema 62618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mSensor->active = on; 62718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (on == false) { 6289105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema mTask.accel_cnt--; 6299105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema if (mSensor->use_gyro_data) 6309105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema mTask.gyro_cnt--; 6319105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema if (mSensor->use_mag_data) 63218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.mag_cnt--; 63318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 63418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian // if cnt == 0 and Handle == 0, nothing need to be done. 63518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian // if cnt > 0 and Handle == 0, something else is turning it on, all will be done. 6369105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema if (mTask.accel_cnt == 0 && mTask.accelHandle != 0) { 63718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian sensorRelease(mTask.tid, mTask.accelHandle); 63818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.accelHandle = 0; 63918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian osEventUnsubscribe(mTask.tid, EVT_SENSOR_ACC_DATA_RDY); 64018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 64118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 6429105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema if (mTask.gyro_cnt == 0 && mTask.gyroHandle != 0) { 64318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian sensorRelease(mTask.tid, mTask.gyroHandle); 64418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.gyroHandle = 0; 64518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian osEventUnsubscribe(mTask.tid, EVT_SENSOR_GYR_DATA_RDY); 64618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 64718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 64818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.mag_cnt == 0 && mTask.magHandle != 0) { 64918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian sensorRelease(mTask.tid, mTask.magHandle); 65018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.magHandle = 0; 65118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian osEventUnsubscribe(mTask.tid, EVT_SENSOR_MAG_DATA_RDY); 65218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 653c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian 654e723815b378ee08afad27d0fe84253e22c050528Ben Fennema idx = mSensor->idx; 655e723815b378ee08afad27d0fe84253e22c050528Ben Fennema (void) fusionSetRate(0, ULONG_LONG_MAX, (void *)idx); 65618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } else { 6579105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema mTask.accel_cnt++; 6589105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema if (mSensor->use_gyro_data) 6599105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema mTask.gyro_cnt++; 6609105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema if (mSensor->use_mag_data) 66118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.mag_cnt++; 6621be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema } 6631be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema 66418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian configureFusion(); 66518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian configureGame(); 66618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, on, 0); 667960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 668960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian return true; 669960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian} 670960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 671e723815b378ee08afad27d0fe84253e22c050528Ben Fennemastatic bool fusionFirmwareUpload(void *cookie) 672960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian{ 673e723815b378ee08afad27d0fe84253e22c050528Ben Fennema struct FusionSensor *mSensor = &mTask.sensors[(int)cookie]; 674960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 67518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0); 67618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian return true; 67718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian} 67818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 679e723815b378ee08afad27d0fe84253e22c050528Ben Fennemastatic bool fusionFlush(void *cookie) 68018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian{ 681e723815b378ee08afad27d0fe84253e22c050528Ben Fennema struct FusionSensor *mSensor = &mTask.sensors[(int)cookie]; 68218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian uint32_t evtType = sensorGetMyEventType(mSi[mSensor->idx].sensorType); 683e723815b378ee08afad27d0fe84253e22c050528Ben Fennema 68418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian osEnqueueEvt(evtType, SENSOR_DATA_EVENT_FLUSH, NULL); 68518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian return true; 68618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian} 68718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 68818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianstatic void fusionHandleEvent(uint32_t evtType, const void* evtData) 689960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian{ 6907062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema struct TripleAxisDataEvent *ev; 6919105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema int i; 692960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 6930bcb1a3baec93492e58afff27f2ef92afcbf1698Ben Fennema if (evtData == SENSOR_DATA_EVENT_FLUSH) 6940bcb1a3baec93492e58afff27f2ef92afcbf1698Ben Fennema return; 6950bcb1a3baec93492e58afff27f2ef92afcbf1698Ben Fennema 696960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian switch (evtType) { 6979105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema case EVT_APP_START: 6989105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema // check for gyro and mag 6999105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema osEventUnsubscribe(mTask.tid, EVT_APP_START); 7009105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema if (!sensorFind(SENS_TYPE_GYRO, 0, &mTask.gyroHandle)) { 7019105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++) 7029105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema mTask.sensors[i].use_gyro_data = false; 7039105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema } 7049105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema mTask.gyroHandle = 0; 7059105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema if (!sensorFind(SENS_TYPE_MAG, 0, &mTask.magHandle)) { 7069105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++) 7079105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema mTask.sensors[i].use_mag_data = false; 7089105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema } 7099105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema mTask.magHandle = 0; 7109105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema break; 7111be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema case EVT_SENSOR_ACC_DATA_RDY: 7121be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema ev = (struct TripleAxisDataEvent *)evtData; 71318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian fillSamples(ev, ACC); 7141be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema drainSamples(); 7151be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema break; 7161be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema case EVT_SENSOR_GYR_DATA_RDY: 7171be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema ev = (struct TripleAxisDataEvent *)evtData; 71818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian fillSamples(ev, GYR); 7191be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema drainSamples(); 7201be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema break; 7211be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema case EVT_SENSOR_MAG_DATA_RDY: 7221be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema ev = (struct TripleAxisDataEvent *)evtData; 72318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian fillSamples(ev, MAG); 7241be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema drainSamples(); 7251be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema break; 726960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 727960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian} 728960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 729e723815b378ee08afad27d0fe84253e22c050528Ben Fennemastatic const struct SensorOps mSops = 73064eb1702a319ad4bbc181ed1e5af0703aea7ea22Zhengyin Qian{ 7314d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema .sensorPower = fusionPower, 7324d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema .sensorFirmwareUpload = fusionFirmwareUpload, 7334d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema .sensorSetRate = fusionSetRate, 7344d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema .sensorFlush = fusionFlush, 73564eb1702a319ad4bbc181ed1e5af0703aea7ea22Zhengyin Qian}; 73664eb1702a319ad4bbc181ed1e5af0703aea7ea22Zhengyin Qian 73718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianstatic bool fusionStart(uint32_t tid) 738960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian{ 739960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian osLog(LOG_INFO, " ORIENTATION: %ld\n", tid); 740e0e5b567748b8d46f60ede2ff12c4b2358ffe0dbZhengyin Qian size_t i, slabSize; 741960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 742960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian mTask.tid = tid; 743960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian mTask.flags = 0; 744960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 74518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian for (i = 0; i < NUM_OF_RAW_SENSOR; i++) { 746960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian mTask.sample_counts[i] = 0; 747960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian mTask.sample_indices[i] = 0; 748960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 749960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 75018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++) { 751e723815b378ee08afad27d0fe84253e22c050528Ben Fennema mTask.sensors[i].handle = sensorRegister(&mSi[i], &mSops, (void *)i, true); 75218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sensors[i].idx = i; 7539105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema mTask.sensors[i].use_gyro_data = true; 75418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sensors[i].use_mag_data = true; 75518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 756960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 7579105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema mTask.sensors[GEOMAG].use_gyro_data = false; 75818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sensors[GAME].use_mag_data = false; 75918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sensors[GRAVITY].use_mag_data = false; 760960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 76146852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung mTask.accel_cnt = 0; 76246852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung mTask.gyro_cnt = 0; 76346852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung mTask.mag_cnt = 0; 76446852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung 7657062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema slabSize = sizeof(struct TripleAxisDataEvent) 7667062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema + MAX_NUM_COMMS_EVENT_SAMPLES * sizeof(struct TripleAxisDataPoint); 76746852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung mDataSlab = slabAllocatorNew(slabSize, 4, 10); // 10 slots for now, worst case 6 output sensors * 2 comms_events = 12 768c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian if (!mDataSlab) { 769c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian osLog(LOG_ERROR, "FUSION SLAB ALLOCATION FAILED\n"); 770c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian return false; 771c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian } 772e0e5b567748b8d46f60ede2ff12c4b2358ffe0dbZhengyin Qian 7739105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema osEventSubscribe(mTask.tid, EVT_APP_START); 7749105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema 775960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian return true; 776960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian} 777960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 77818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianstatic void fusionEnd() 779960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian{ 780960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian mTask.flags &= ~FUSION_FLAG_INITIALIZED; 78146852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung mTask.flags &= ~FUSION_FLAG_GAME_INITIALIZED; 782e0e5b567748b8d46f60ede2ff12c4b2358ffe0dbZhengyin Qian slabAllocatorDestroy(mDataSlab); 783960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian} 784960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 785960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin QianINTERNAL_APP_INIT( 786dca61fcfcc85a8240bc46a650c71008ded2d32ddBen Fennema APP_ID_MAKE(APP_ID_VENDOR_GOOGLE, 4), 787cc77708f80db23714a92638ddee64f59dbdf5483Dmitry Grinberg 0, 78818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian fusionStart, 78918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian fusionEnd, 79018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian fusionHandleEvent); 791