orientation.c revision 5fed15d587cf904753f8b86bcd552a1ddb0c39fb
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 3541f1d787265e609c81b4906df20c725b053b4e94Meng-hsuan Chung#define MAX_NUM_COMMS_EVENT_SAMPLES 15 // at most 15 samples can fit in one comms_event 365fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu#define NUM_COMMS_EVENTS_IN_FIFO 2 // This controls how often the hub needs to wake up 375fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu // in batching 385fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu 395fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu// needs to be greater than max raw sensor rate ratio 405fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu#define FIFO_DEPTH (NUM_COMMS_EVENTS_IN_FIFO * MAX_NUM_COMMS_EVENT_SAMPLES) 415fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu 42f17db6a82e2e2f3c929c08726574456b97136b2fZhengyin Qian/* 43f17db6a82e2e2f3c929c08726574456b97136b2fZhengyin Qian * FIFO_MARGIN: max raw sensor rate ratio is 8:1. 445fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu * If 2 batchs of high rate data comes before 1 low rate data, there can be at max 15 samples left 455fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu * in the FIFO 46f17db6a82e2e2f3c929c08726574456b97136b2fZhengyin Qian */ 47f17db6a82e2e2f3c929c08726574456b97136b2fZhengyin Qian#define FIFO_MARGIN 15 4841f1d787265e609c81b4906df20c725b053b4e94Meng-hsuan Chung#define MAX_NUM_SAMPLES (FIFO_MARGIN + FIFO_DEPTH) // actual input sample fifo depth 499105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema#define EVT_SENSOR_ACC_DATA_RDY sensorGetMyEventType(SENS_TYPE_ACCEL) 509105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema#define EVT_SENSOR_GYR_DATA_RDY sensorGetMyEventType(SENS_TYPE_GYRO) 519105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema#define EVT_SENSOR_MAG_DATA_RDY sensorGetMyEventType(SENS_TYPE_MAG) 5218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 539105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema#define kGravityEarth 9.80665f 5446852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung#define kRad2deg (180.0f / M_PI) 559105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema#define MIN_GYRO_RATE_HZ SENSOR_HZ(100.0f) 569105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema#define MAX_MAG_RATE_HZ SENSOR_HZ(50.0f) 57960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 581be10b6751f50675307716747bd4d729d9e7aff5Ben Fennemaenum 591be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema{ 601be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema FUSION_FLAG_ENABLED = 0x01, 611be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema FUSION_FLAG_INITIALIZED = 0x08, 621be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema FUSION_FLAG_GAME_ENABLED = 0x10, 631be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema FUSION_FLAG_GAME_INITIALIZED = 0x20 64960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian}; 65960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 6618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianenum RawSensorType 6718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian{ 6818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian ACC, 6918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian GYR, 7018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian MAG, 7118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian NUM_OF_RAW_SENSOR 7218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian}; 7318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 7418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianenum FusionSensorType 751be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema{ 7618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian ORIENT, 7718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian GRAVITY, 7818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian GEOMAG, 7918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian LINEAR, 8018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian GAME, 8118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian ROTAT, 8218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian NUM_OF_FUSION_SENSOR 831be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema}; 841be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema 851be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema 8618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianstruct FusionSensorSample { 87960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian uint64_t time; 88960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian float x, y, z; 89960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian}; 90960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 9118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianstruct FusionSensor { 92960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian uint32_t handle; 9318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian struct TripleAxisDataEvent *ev; 94c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian uint64_t prev_time; 9518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian uint64_t latency; 9618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian uint32_t rate; 9718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian bool active; 989105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema bool use_gyro_data; 9918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian bool use_mag_data; 10018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian uint8_t idx; 10118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian}; 10218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 10318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianstruct FusionTask { 10418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian uint32_t tid; 1051be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema uint32_t accelHandle; 1061be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema uint32_t gyroHandle; 1071be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema uint32_t magHandle; 108960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 109960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian struct Fusion fusion; 11018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian struct Fusion game; 111960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 11218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian struct FusionSensor sensors[NUM_OF_FUSION_SENSOR]; 11318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian struct FusionSensorSample samples[NUM_OF_RAW_SENSOR][MAX_NUM_SAMPLES]; 11418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian size_t sample_indices[NUM_OF_RAW_SENSOR]; 11518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian size_t sample_counts[NUM_OF_RAW_SENSOR]; 11618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian uint32_t counters[NUM_OF_RAW_SENSOR]; 11718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian uint64_t ResamplePeriodNs[NUM_OF_RAW_SENSOR]; 11818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian uint64_t last_time[NUM_OF_RAW_SENSOR]; 11918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian struct TripleAxisDataPoint last_sample[NUM_OF_RAW_SENSOR]; 120960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 121960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian uint32_t flags; 122960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 123c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian uint32_t raw_sensor_rate[NUM_OF_RAW_SENSOR]; 124c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian uint64_t raw_sensor_latency; 125c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian 126cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung uint8_t accel_client_cnt; 127cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung uint8_t gyro_client_cnt; 128cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung uint8_t mag_client_cnt; 129960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian}; 130960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 13118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianstatic uint32_t FusionRates[] = { 132960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian SENSOR_HZ(12.5f), 133960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian SENSOR_HZ(25.0f), 134960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian SENSOR_HZ(50.0f), 135960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian SENSOR_HZ(100.0f), 136960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian SENSOR_HZ(200.0f), 137960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 0, 138960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian}; 139960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 1405fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu//should match "supported rates in length" and be the timer length for that rate in nanosecs 1415fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xustatic const uint64_t rateTimerVals[] = { 142dd2f9808f0045a3eaa42cb488153eeb599eb97ffDmitry Grinberg 1000000000ULL / 12.5f, 143dd2f9808f0045a3eaa42cb488153eeb599eb97ffDmitry Grinberg 1000000000ULL / 25, 144dd2f9808f0045a3eaa42cb488153eeb599eb97ffDmitry Grinberg 1000000000ULL / 50, 145dd2f9808f0045a3eaa42cb488153eeb599eb97ffDmitry Grinberg 1000000000ULL / 100, 146dd2f9808f0045a3eaa42cb488153eeb599eb97ffDmitry Grinberg 1000000000ULL / 200, 147dd2f9808f0045a3eaa42cb488153eeb599eb97ffDmitry Grinberg}; 148dd2f9808f0045a3eaa42cb488153eeb599eb97ffDmitry Grinberg 14918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianstatic struct FusionTask mTask; 1504d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema 1514d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema#define DEC_INFO_RATE(name, rates, type, axis, inter, samples) \ 1524d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema .sensorName = name, \ 1534d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema .supportedRates = rates, \ 1544d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema .sensorType = type, \ 1554d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema .numAxis = axis, \ 1564d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema .interrupt = inter, \ 1574d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema .minSamples = samples 1584d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema 15918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianstatic const struct SensorInfo mSi[NUM_OF_FUSION_SENSOR] = 16064eb1702a319ad4bbc181ed1e5af0703aea7ea22Zhengyin Qian{ 1615fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu { DEC_INFO_RATE("Orientation", FusionRates, SENS_TYPE_ORIENTATION, NUM_AXIS_THREE, 1625fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu NANOHUB_INT_NONWAKEUP, 20) }, 1635fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu { DEC_INFO_RATE("Gravity", FusionRates, SENS_TYPE_GRAVITY, NUM_AXIS_THREE, 1645fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu NANOHUB_INT_NONWAKEUP, 20) }, 1655fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu { DEC_INFO_RATE("Geomagnetic Rotation Vector", FusionRates, SENS_TYPE_GEO_MAG_ROT_VEC, 1665fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 20) }, 1675fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu { DEC_INFO_RATE("Linear Acceleration", FusionRates, SENS_TYPE_LINEAR_ACCEL, NUM_AXIS_THREE, 1685fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu NANOHUB_INT_NONWAKEUP, 20) }, 1695fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu { DEC_INFO_RATE("Game Rotation Vector", FusionRates, SENS_TYPE_GAME_ROT_VECTOR, NUM_AXIS_THREE, 1705fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu NANOHUB_INT_NONWAKEUP, 300) }, 1715fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu { DEC_INFO_RATE("Rotation Vector", FusionRates, SENS_TYPE_ROTATION_VECTOR, NUM_AXIS_THREE, 1725fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu NANOHUB_INT_NONWAKEUP, 20) }, 17364eb1702a319ad4bbc181ed1e5af0703aea7ea22Zhengyin Qian}; 174960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 175e0e5b567748b8d46f60ede2ff12c4b2358ffe0dbZhengyin Qianstatic struct SlabAllocator *mDataSlab; 176e0e5b567748b8d46f60ede2ff12c4b2358ffe0dbZhengyin Qian 177e0e5b567748b8d46f60ede2ff12c4b2358ffe0dbZhengyin Qianstatic void dataEvtFree(void *ptr) 178960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian{ 179e0e5b567748b8d46f60ede2ff12c4b2358ffe0dbZhengyin Qian slabAllocatorFree(mDataSlab, ptr); 180960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian} 181960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 18218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianstatic void fillSamples(struct TripleAxisDataEvent *ev, enum RawSensorType index) 183960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian{ 18418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian bool bad_timestamp; 18518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian size_t i, w, n, num_samples; 18618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian struct TripleAxisDataPoint *curr_sample, *next_sample; 187f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema uint32_t counter; 188f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema uint64_t ResamplePeriodNs, curr_time, next_time; 18918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian uint64_t sample_spacing_ns; 19018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian float weight_next; 19118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 192cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung if (index == GYR && mTask.gyro_client_cnt == 0) { 193960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian return; 194960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 195cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung if (index == MAG && mTask.mag_client_cnt == 0) { 196960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian return; 197960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 198960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 19918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian n = mTask.sample_counts[index]; 20018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian i = mTask.sample_indices[index]; 20118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian counter = mTask.counters[index]; 20218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian ResamplePeriodNs = mTask.ResamplePeriodNs[index]; 20318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian w = (mTask.sample_indices[index] + n) % MAX_NUM_SAMPLES; 20418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 20518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian // check if this sensor was used before 20618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.last_time[index] == ULONG_LONG_MAX) { 20718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian curr_sample = ev->samples; 20818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian next_sample = curr_sample + 1; 209f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema num_samples = ev->samples[0].firstSample.numSamples; 21018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian curr_time = ev->referenceTime; 211960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } else { 21218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian curr_sample = &mTask.last_sample[index]; 21318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian next_sample = ev->samples; 214f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema num_samples = ev->samples[0].firstSample.numSamples + 1; 21518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian curr_time = mTask.last_time[index]; 21618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 217960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 21818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian while (num_samples > 1) { 219960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 22018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (next_sample == ev->samples) 22118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian next_time = ev->referenceTime; 22218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian else 22318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian next_time = curr_time + next_sample->deltaTime; 224960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 22518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian // error handling for non-chronological accel timestamps 22618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian sample_spacing_ns = (next_time > curr_time) ? (next_time - curr_time) : 0; 227960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 22846852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung // This can happen during sensor config changes 22918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian bad_timestamp = (sample_spacing_ns > 10 * ResamplePeriodNs); 230960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 23118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian // Check to see if we need to move the interpolation window or 23218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian // interpolate 23318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if ((counter >= sample_spacing_ns) || bad_timestamp) { 23418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian num_samples--; 23518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian counter -= (bad_timestamp ? counter : sample_spacing_ns); 23618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian curr_sample = next_sample; 23718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian next_sample++; 238960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 239c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian curr_time = next_time; 24018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } else { 241ae081f1f47013ddf08e5af685b9f6d02ac76421bDmitry Grinberg weight_next = (float)counter / floatFromUint64(sample_spacing_ns); 24218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 24318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.samples[index][w].x = curr_sample->x + weight_next * 24418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian (next_sample->x - curr_sample->x); 24518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.samples[index][w].y = curr_sample->y + weight_next * 24618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian (next_sample->y - curr_sample->y); 24718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.samples[index][w].z = curr_sample->z + weight_next * 24818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian (next_sample->z - curr_sample->z); 24918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.samples[index][w].time = curr_time + counter; 25018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 25118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian // Move the read index when buffer is full 25218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (++n > MAX_NUM_SAMPLES) { 25318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian n = MAX_NUM_SAMPLES; 25418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 25518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (++i == MAX_NUM_SAMPLES) { 25618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian i = 0; 25718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 25818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 25918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 26018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian // Reset the write index 26118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (++w == MAX_NUM_SAMPLES) { 26218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian w = 0; 26318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 26418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 26518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian // Move to the next resample 26618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian counter += ResamplePeriodNs; 267960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 268960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 26918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 27018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sample_counts[index] = n; 27118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sample_indices[index] = i; 27218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.counters[index] = counter; 27318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.last_sample[index] = *curr_sample; 27418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.last_time[index] = curr_time; 275960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian} 276960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 2774f3270dd3a919e485bb0d6c4db6e0f4d56f7d5ceBen Fennemastatic bool allocateDataEvt(struct FusionSensor *mSensor, uint64_t time) 2784f3270dd3a919e485bb0d6c4db6e0f4d56f7d5ceBen Fennema{ 2794f3270dd3a919e485bb0d6c4db6e0f4d56f7d5ceBen Fennema mSensor->ev = slabAllocatorAlloc(mDataSlab); 2804f3270dd3a919e485bb0d6c4db6e0f4d56f7d5ceBen Fennema if (mSensor->ev == NULL) { 2814f3270dd3a919e485bb0d6c4db6e0f4d56f7d5ceBen Fennema // slab allocation failed 2824f3270dd3a919e485bb0d6c4db6e0f4d56f7d5ceBen Fennema osLog(LOG_ERROR, "ORIENTATION: slabAllocatorAlloc() Failed\n"); 2834f3270dd3a919e485bb0d6c4db6e0f4d56f7d5ceBen Fennema return false; 2844f3270dd3a919e485bb0d6c4db6e0f4d56f7d5ceBen Fennema } 2854f3270dd3a919e485bb0d6c4db6e0f4d56f7d5ceBen Fennema 2864f3270dd3a919e485bb0d6c4db6e0f4d56f7d5ceBen Fennema // delta time for the first sample is sample count 2874f3270dd3a919e485bb0d6c4db6e0f4d56f7d5ceBen Fennema memset(&mSensor->ev->samples[0].firstSample, 0x00, sizeof(struct SensorFirstSample)); 2884f3270dd3a919e485bb0d6c4db6e0f4d56f7d5ceBen Fennema mSensor->ev->referenceTime = time; 2894f3270dd3a919e485bb0d6c4db6e0f4d56f7d5ceBen Fennema mSensor->prev_time = time; 2904f3270dd3a919e485bb0d6c4db6e0f4d56f7d5ceBen Fennema 2914f3270dd3a919e485bb0d6c4db6e0f4d56f7d5ceBen Fennema return true; 2924f3270dd3a919e485bb0d6c4db6e0f4d56f7d5ceBen Fennema} 2934f3270dd3a919e485bb0d6c4db6e0f4d56f7d5ceBen Fennema 29418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianstatic void addSample(struct FusionSensor *mSensor, uint64_t time, float x, float y, float z) 295960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian{ 2967062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema struct TripleAxisDataPoint *sample; 297e0e5b567748b8d46f60ede2ff12c4b2358ffe0dbZhengyin Qian 29818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mSensor->ev == NULL) { 2994f3270dd3a919e485bb0d6c4db6e0f4d56f7d5ceBen Fennema if (!allocateDataEvt(mSensor, time)) 300e0e5b567748b8d46f60ede2ff12c4b2358ffe0dbZhengyin Qian return; 301960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 302960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 303f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema if (mSensor->ev->samples[0].firstSample.numSamples >= MAX_NUM_COMMS_EVENT_SAMPLES) { 30441f1d787265e609c81b4906df20c725b053b4e94Meng-hsuan Chung osLog(LOG_ERROR, "ORIENTATION: BAD_INDEX\n"); 305960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian return; 306960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 307960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 308f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema sample = &mSensor->ev->samples[mSensor->ev->samples[0].firstSample.numSamples++]; 309e0e5b567748b8d46f60ede2ff12c4b2358ffe0dbZhengyin Qian 310c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian if (mSensor->ev->samples[0].firstSample.numSamples > 1) { 31146852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung sample->deltaTime = time > mSensor->prev_time ? (time - mSensor->prev_time) : 0; 312c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian mSensor->prev_time = time; 313c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian } 314960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 315960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian sample->x = x; 316960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian sample->y = y; 317960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian sample->z = z; 318960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 319f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema if (mSensor->ev->samples[0].firstSample.numSamples == MAX_NUM_COMMS_EVENT_SAMPLES) { 3205fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu osEnqueueEvtOrFree( 3215fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu EVENT_TYPE_BIT_DISCARDABLE | sensorGetMyEventType(mSi[mSensor->idx].sensorType), 3225fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu mSensor->ev, dataEvtFree); 32318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mSensor->ev = NULL; 324960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 325960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian} 326960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 327960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qianstatic void updateOutput(ssize_t last_accel_sample_index, uint64_t last_sensor_time) 328960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian{ 32918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian struct Vec4 attitude; 33018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian struct Vec3 g, a; 3315fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu struct Mat33 R; // direction-cosine/rotation matrix, inertial -> device 3325fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu bool rInited; // indicates if matrix R has been initialzed. for avoiding repeated computation 33318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 33418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (fusionHasEstimate(&mTask.game)) { 3355fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu rInited = false; 33618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.sensors[GAME].active) { 33718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian fusionGetAttitude(&mTask.game, &attitude); 33818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian addSample(&mTask.sensors[GAME], 33918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian last_sensor_time, 34018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian attitude.x, 34118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian attitude.y, 34218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian attitude.z); 34318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 34418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 34518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.sensors[GRAVITY].active) { 34618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian fusionGetRotationMatrix(&mTask.game, &R); 3475fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu rInited = true; 34818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian initVec3(&g, R.elem[0][2], R.elem[1][2], R.elem[2][2]); 34918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian vec3ScalarMul(&g, kGravityEarth); 35018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian addSample(&mTask.sensors[GRAVITY], 35118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian last_sensor_time, 35218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian g.x, g.y, g.z); 35318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 3545fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu 3555fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu if (last_accel_sample_index >= 0 3565fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu && mTask.sensors[LINEAR].active) { 3575fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu if (!rInited) { 3585fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu fusionGetRotationMatrix(&mTask.game, &R); 3595fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu } 3605fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu initVec3(&g, R.elem[0][2], R.elem[1][2], R.elem[2][2]); 3615fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu vec3ScalarMul(&g, kGravityEarth); 3625fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu initVec3(&a, 3635fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu mTask.samples[0][last_accel_sample_index].x, 3645fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu mTask.samples[0][last_accel_sample_index].y, 3655fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu mTask.samples[0][last_accel_sample_index].z); 3665fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu 3675fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu addSample(&mTask.sensors[LINEAR], 3685fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu mTask.samples[0][last_accel_sample_index].time, 3695fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu a.x - g.x, 3705fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu a.y - g.y, 3715fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu a.z - g.z); 3725fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu } 37318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 37418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 375960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian if (fusionHasEstimate(&mTask.fusion)) { 376960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian fusionGetAttitude(&mTask.fusion, &attitude); 377960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 37818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.sensors[ORIENT].active) { 3795fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu fusionGetRotationMatrix(&mTask.fusion, &R); 38018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian // x, y, z = yaw, pitch, roll 38118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian float x = atan2f(-R.elem[0][1], R.elem[0][0]) * kRad2deg; 38218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian float y = atan2f(-R.elem[1][2], R.elem[2][2]) * kRad2deg; 38318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian float z = asinf(R.elem[0][2]) * kRad2deg; 38418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 38518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (x < 0.0f) { 38618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian x += 360.0f; 38718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 38818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 38918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian addSample(&mTask.sensors[ORIENT], 39018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian last_sensor_time, x, y, z); 39118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 392960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 39318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.sensors[GEOMAG].active) { 39418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian addSample(&mTask.sensors[GEOMAG], 39518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian last_sensor_time, 39618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian attitude.x, 39718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian attitude.y, 39818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian attitude.z); 399960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 400960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 40118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.sensors[ROTAT].active) { 40218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian addSample(&mTask.sensors[ROTAT], 40318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian last_sensor_time, 40418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian attitude.x, 40518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian attitude.y, 40618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian attitude.z); 40718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 40818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 409960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 410960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian} 411960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 412960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qianstatic void drainSamples() 413960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian{ 41418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian size_t i = mTask.sample_indices[ACC]; 415960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian size_t j = 0; 416960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian size_t k = 0; 4171be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema size_t which; 4181be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema struct Vec3 a, w, m; 4191be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema float dT; 4201be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema uint64_t a_time, g_time, m_time; 4211be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema 422cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung if (mTask.gyro_client_cnt > 0) 42318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian j = mTask.sample_indices[GYR]; 4241be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema 425cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung if (mTask.mag_client_cnt > 0) 42618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian k = mTask.sample_indices[MAG]; 4271be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema 42818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian while (mTask.sample_counts[ACC] > 0 429cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung && (!(mTask.gyro_client_cnt > 0) || mTask.sample_counts[GYR] > 0) 430cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung && (!(mTask.mag_client_cnt > 0) || mTask.sample_counts[MAG] > 0)) { 43118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian a_time = mTask.samples[ACC][i].time; 432cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung g_time = mTask.gyro_client_cnt > 0 ? mTask.samples[GYR][j].time 433960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian : ULONG_LONG_MAX; 434cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung m_time = mTask.mag_client_cnt > 0 ? mTask.samples[MAG][k].time 435960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian : ULONG_LONG_MAX; 436960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 437960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian // priority with same timestamp: gyro > acc > mag 438960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian if (g_time <= a_time && g_time <= m_time) { 43918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian which = GYR; 440960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } else if (a_time <= m_time) { 44118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian which = ACC; 442960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } else { 44318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian which = MAG; 444960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 445960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 446a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian dT = floatFromUint64(mTask.ResamplePeriodNs[which]) * 1e-9f; 4471be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema switch (which) { 44818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian case ACC: 44918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian initVec3(&a, mTask.samples[ACC][i].x, mTask.samples[ACC][i].y, mTask.samples[ACC][i].z); 45018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 45118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.flags & FUSION_FLAG_ENABLED) 45218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian fusionHandleAcc(&mTask.fusion, &a, dT); 453960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 45418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.flags & FUSION_FLAG_GAME_ENABLED) 45518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian fusionHandleAcc(&mTask.game, &a, dT); 456960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 45718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian updateOutput(i, mTask.samples[ACC][i].time); 458960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 45918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian --mTask.sample_counts[ACC]; 4601be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema if (++i == MAX_NUM_SAMPLES) 4611be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema i = 0; 4621be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema break; 46318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian case GYR: 46418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian initVec3(&w, mTask.samples[GYR][j].x, mTask.samples[GYR][j].y, mTask.samples[GYR][j].z); 465960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 46618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.flags & FUSION_FLAG_ENABLED) 46718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian fusionHandleGyro(&mTask.fusion, &w, dT); 468960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 46918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.flags & FUSION_FLAG_GAME_ENABLED) 47018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian fusionHandleGyro(&mTask.game, &w, dT); 47118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 47218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian --mTask.sample_counts[GYR]; 4731be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema if (++j == MAX_NUM_SAMPLES) 4741be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema j = 0; 4751be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema break; 47618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian case MAG: 47718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian initVec3(&m, mTask.samples[MAG][k].x, mTask.samples[MAG][k].y, mTask.samples[MAG][k].z); 478960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 4791be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema fusionHandleMag(&mTask.fusion, &m); 480960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 48118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian --mTask.sample_counts[MAG]; 4821be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema if (++k == MAX_NUM_SAMPLES) 4831be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema k = 0; 4841be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema break; 485960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 486960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 487960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 48818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sample_indices[ACC] = i; 489960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 490cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung if (mTask.gyro_client_cnt > 0) 49118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sample_indices[GYR] = j; 492960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 493cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung if (mTask.mag_client_cnt > 0) 49418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sample_indices[MAG] = k; 495960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 49618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++) { 49718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.sensors[i].ev != NULL) { 498d4cfbfe80cdb23453458697fb57463d0ab3b3432Alexey Polyudov osEnqueueEvtOrFree(EVENT_TYPE_BIT_DISCARDABLE | sensorGetMyEventType(mSi[i].sensorType), 499d4cfbfe80cdb23453458697fb57463d0ab3b3432Alexey Polyudov mTask.sensors[i].ev, dataEvtFree); 50018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sensors[i].ev = NULL; 50118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 502960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 503960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian} 504960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 505960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qianstatic void configureFusion() 506960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian{ 50718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.sensors[ORIENT].active 50818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian || mTask.sensors[ROTAT].active 50918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian || mTask.sensors[GEOMAG].active) { 51018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.flags |= FUSION_FLAG_ENABLED; 51118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian initFusion(&mTask.fusion, 512cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung (mTask.mag_client_cnt > 0 ? FUSION_USE_MAG : 0) | 513cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung (mTask.gyro_client_cnt > 0 ? FUSION_USE_GYRO : 0) | 51418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian ((mTask.flags & FUSION_FLAG_INITIALIZED) ? 0 : FUSION_REINITIALIZE)); 51518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.flags |= FUSION_FLAG_INITIALIZED; 51618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } else { 51718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.flags &= ~FUSION_FLAG_ENABLED; 51818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.flags &= ~FUSION_FLAG_INITIALIZED; 51918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 520960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian} 521960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 52218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianstatic void configureGame() 523960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian{ 5245fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu if (mTask.sensors[GAME].active || mTask.sensors[GRAVITY].active || 5255fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu mTask.sensors[LINEAR].active) { 52618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.flags |= FUSION_FLAG_GAME_ENABLED; 5275fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu initFusion(&mTask.game, FUSION_USE_GYRO | 5285fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu ((mTask.flags & FUSION_FLAG_INITIALIZED) ? 0 : FUSION_REINITIALIZE)); 52918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.flags |= FUSION_FLAG_GAME_INITIALIZED; 53018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } else { 53118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.flags &= ~FUSION_FLAG_GAME_ENABLED; 53218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.flags &= ~FUSION_FLAG_GAME_INITIALIZED; 533960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 534960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian} 535960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 536c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qianstatic void fusionSetRateAcc(void) 537960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian{ 5381be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema int i; 53918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.accelHandle == 0) { 54018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sample_counts[ACC] = 0; 54118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sample_indices[ACC] = 0; 54218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.counters[ACC] = 0; 54318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.last_time[ACC] = ULONG_LONG_MAX; 54446852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung for (i = 0; sensorFind(SENS_TYPE_ACCEL, i, &mTask.accelHandle) != NULL; i++) { 5455fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu if (sensorRequest(mTask.tid, mTask.accelHandle, mTask.raw_sensor_rate[ACC], 5465fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu mTask.raw_sensor_latency)) { 5471be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema osEventSubscribe(mTask.tid, EVT_SENSOR_ACC_DATA_RDY); 5481be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema break; 5491be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema } 5501be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema } 5511be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema } else { 5525fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu sensorRequestRateChange(mTask.tid, mTask.accelHandle, mTask.raw_sensor_rate[ACC], 5535fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu mTask.raw_sensor_latency); 5541be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema } 55518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian} 55618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 557c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qianstatic void fusionSetRateGyr(void) 55818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian{ 55918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian int i; 5601be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema if (mTask.gyroHandle == 0) { 56118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sample_counts[GYR] = 0; 56218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sample_indices[GYR] = 0; 56318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.counters[GYR] = 0; 56418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.last_time[GYR] = ULONG_LONG_MAX; 56546852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung for (i = 0; sensorFind(SENS_TYPE_GYRO, i, &mTask.gyroHandle) != NULL; i++) { 5665fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu if (sensorRequest(mTask.tid, mTask.gyroHandle, mTask.raw_sensor_rate[GYR], 5675fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu mTask.raw_sensor_latency)) { 5681be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema osEventSubscribe(mTask.tid, EVT_SENSOR_GYR_DATA_RDY); 5691be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema break; 5701be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema } 5711be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema } 5721be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema } else { 5735fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu sensorRequestRateChange(mTask.tid, mTask.gyroHandle, mTask.raw_sensor_rate[GYR], 5745fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu mTask.raw_sensor_latency); 5751be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema } 57618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian} 57718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 578c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qianstatic void fusionSetRateMag(void) 57918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian{ 58018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian int i; 5811be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema if (mTask.magHandle == 0) { 58218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sample_counts[MAG] = 0; 58318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sample_indices[MAG] = 0; 58418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.counters[MAG] = 0; 58518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.last_time[MAG] = ULONG_LONG_MAX; 58646852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung for (i = 0; sensorFind(SENS_TYPE_MAG, i, &mTask.magHandle) != NULL; i++) { 5875fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu if (sensorRequest(mTask.tid, mTask.magHandle, mTask.raw_sensor_rate[MAG], 5885fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu mTask.raw_sensor_latency)) { 5891be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema osEventSubscribe(mTask.tid, EVT_SENSOR_MAG_DATA_RDY); 5901be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema break; 5911be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema } 5921be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema } 5931be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema } else { 5945fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu sensorRequestRateChange(mTask.tid, mTask.magHandle, mTask.raw_sensor_rate[MAG], 5955fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu mTask.raw_sensor_latency); 596c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian } 597c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian} 598c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian 599e723815b378ee08afad27d0fe84253e22c050528Ben Fennemastatic bool fusionSetRate(uint32_t rate, uint64_t latency, void *cookie) 600c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian{ 601e723815b378ee08afad27d0fe84253e22c050528Ben Fennema struct FusionSensor *mSensor = &mTask.sensors[(int)cookie]; 602c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian int i; 603c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian uint32_t max_rate = 0; 604c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian uint32_t gyr_rate, mag_rate; 60546852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung uint64_t min_resample_period = ULONG_LONG_MAX; 606c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian 607c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian mSensor->rate = rate; 608c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian mSensor->latency = latency; 609c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian 610c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++) { 611c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian if (mTask.sensors[i].active) { 612c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian max_rate = max_rate > mTask.sensors[i].rate ? max_rate : mTask.sensors[i].rate; 613c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian } 614c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian } 615c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian 616cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung if (mTask.accel_client_cnt > 0) { 617c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian mTask.raw_sensor_rate[ACC] = max_rate; 61846852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung mTask.ResamplePeriodNs[ACC] = sensorTimerLookupCommon(FusionRates, rateTimerVals, max_rate); 61946852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung min_resample_period = mTask.ResamplePeriodNs[ACC] < min_resample_period ? 62046852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung mTask.ResamplePeriodNs[ACC] : min_resample_period; 621c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian } 622c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian 623cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung if (mTask.gyro_client_cnt > 0) { 6249105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema gyr_rate = max_rate > MIN_GYRO_RATE_HZ ? max_rate : MIN_GYRO_RATE_HZ; 625c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian mTask.raw_sensor_rate[GYR] = gyr_rate; 62646852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung mTask.ResamplePeriodNs[GYR] = sensorTimerLookupCommon(FusionRates, rateTimerVals, gyr_rate); 62746852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung min_resample_period = mTask.ResamplePeriodNs[GYR] < min_resample_period ? 62846852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung mTask.ResamplePeriodNs[GYR] : min_resample_period; 629c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian } 630c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian 631cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung if (mTask.mag_client_cnt > 0) { 6329105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema mag_rate = max_rate < MAX_MAG_RATE_HZ ? max_rate : MAX_MAG_RATE_HZ; 633c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian mTask.raw_sensor_rate[MAG] = mag_rate; 63446852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung mTask.ResamplePeriodNs[MAG] = sensorTimerLookupCommon(FusionRates, rateTimerVals, mag_rate); 63546852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung min_resample_period = mTask.ResamplePeriodNs[MAG] < min_resample_period ? 63646852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung mTask.ResamplePeriodNs[MAG] : min_resample_period; 637c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian } 638c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian 63946852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung // This guarantees that local raw sensor FIFOs won't overflow. 64046852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung mTask.raw_sensor_latency = min_resample_period * (FIFO_DEPTH - 1); 64146852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung 642c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++) { 643c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian if (mTask.sensors[i].active) { 644c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian mTask.raw_sensor_latency = mTask.sensors[i].latency < mTask.raw_sensor_latency ? 645c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian mTask.sensors[i].latency : mTask.raw_sensor_latency; 646c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian } 64718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 648c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian 649cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung if (mTask.accel_client_cnt > 0) 650c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian fusionSetRateAcc(); 651cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung if (mTask.gyro_client_cnt > 0) 652c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian fusionSetRateGyr(); 653cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung if (mTask.mag_client_cnt > 0) 654c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian fusionSetRateMag(); 655c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian if (mSensor->rate > 0) 656c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency); 657c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian 658c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian return true; 65918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian} 66018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 661e723815b378ee08afad27d0fe84253e22c050528Ben Fennemastatic bool fusionPower(bool on, void *cookie) 66218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian{ 663e723815b378ee08afad27d0fe84253e22c050528Ben Fennema struct FusionSensor *mSensor = &mTask.sensors[(int)cookie]; 664e723815b378ee08afad27d0fe84253e22c050528Ben Fennema int idx; 665e723815b378ee08afad27d0fe84253e22c050528Ben Fennema 66618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mSensor->active = on; 66718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (on == false) { 668cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung mTask.accel_client_cnt--; 6699105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema if (mSensor->use_gyro_data) 670cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung mTask.gyro_client_cnt--; 6719105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema if (mSensor->use_mag_data) 672cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung mTask.mag_client_cnt--; 67318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 674cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung // if client_cnt == 0 and Handle == 0, nothing need to be done. 675cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung // if client_cnt > 0 and Handle == 0, something else is turning it on, all will be done. 676cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung if (mTask.accel_client_cnt == 0 && mTask.accelHandle != 0) { 67718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian sensorRelease(mTask.tid, mTask.accelHandle); 67818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.accelHandle = 0; 67918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian osEventUnsubscribe(mTask.tid, EVT_SENSOR_ACC_DATA_RDY); 68018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 68118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 682cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung if (mTask.gyro_client_cnt == 0 && mTask.gyroHandle != 0) { 68318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian sensorRelease(mTask.tid, mTask.gyroHandle); 68418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.gyroHandle = 0; 68518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian osEventUnsubscribe(mTask.tid, EVT_SENSOR_GYR_DATA_RDY); 68618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 68718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 688cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung if (mTask.mag_client_cnt == 0 && mTask.magHandle != 0) { 68918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian sensorRelease(mTask.tid, mTask.magHandle); 69018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.magHandle = 0; 69118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian osEventUnsubscribe(mTask.tid, EVT_SENSOR_MAG_DATA_RDY); 69218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 693c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian 694e723815b378ee08afad27d0fe84253e22c050528Ben Fennema idx = mSensor->idx; 695e723815b378ee08afad27d0fe84253e22c050528Ben Fennema (void) fusionSetRate(0, ULONG_LONG_MAX, (void *)idx); 69618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } else { 697cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung mTask.accel_client_cnt++; 6989105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema if (mSensor->use_gyro_data) 699cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung mTask.gyro_client_cnt++; 7009105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema if (mSensor->use_mag_data) 701cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung mTask.mag_client_cnt++; 7021be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema } 7031be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema 70418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian configureFusion(); 70518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian configureGame(); 70618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, on, 0); 707960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 708960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian return true; 709960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian} 710960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 711e723815b378ee08afad27d0fe84253e22c050528Ben Fennemastatic bool fusionFirmwareUpload(void *cookie) 712960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian{ 713e723815b378ee08afad27d0fe84253e22c050528Ben Fennema struct FusionSensor *mSensor = &mTask.sensors[(int)cookie]; 714960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 71518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0); 71618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian return true; 71718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian} 71818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 719e723815b378ee08afad27d0fe84253e22c050528Ben Fennemastatic bool fusionFlush(void *cookie) 72018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian{ 721e723815b378ee08afad27d0fe84253e22c050528Ben Fennema struct FusionSensor *mSensor = &mTask.sensors[(int)cookie]; 72218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian uint32_t evtType = sensorGetMyEventType(mSi[mSensor->idx].sensorType); 723e723815b378ee08afad27d0fe84253e22c050528Ben Fennema 72418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian osEnqueueEvt(evtType, SENSOR_DATA_EVENT_FLUSH, NULL); 72518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian return true; 72618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian} 72718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 72818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianstatic void fusionHandleEvent(uint32_t evtType, const void* evtData) 729960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian{ 7307062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema struct TripleAxisDataEvent *ev; 7319105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema int i; 732960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 7330bcb1a3baec93492e58afff27f2ef92afcbf1698Ben Fennema if (evtData == SENSOR_DATA_EVENT_FLUSH) 7340bcb1a3baec93492e58afff27f2ef92afcbf1698Ben Fennema return; 7350bcb1a3baec93492e58afff27f2ef92afcbf1698Ben Fennema 736960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian switch (evtType) { 7379105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema case EVT_APP_START: 7389105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema // check for gyro and mag 7399105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema osEventUnsubscribe(mTask.tid, EVT_APP_START); 7409105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema if (!sensorFind(SENS_TYPE_GYRO, 0, &mTask.gyroHandle)) { 7419105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++) 7429105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema mTask.sensors[i].use_gyro_data = false; 7439105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema } 7449105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema mTask.gyroHandle = 0; 7459105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema if (!sensorFind(SENS_TYPE_MAG, 0, &mTask.magHandle)) { 7469105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++) 7479105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema mTask.sensors[i].use_mag_data = false; 7489105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema } 7499105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema mTask.magHandle = 0; 7509105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema break; 7511be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema case EVT_SENSOR_ACC_DATA_RDY: 7521be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema ev = (struct TripleAxisDataEvent *)evtData; 75318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian fillSamples(ev, ACC); 7541be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema drainSamples(); 7551be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema break; 7561be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema case EVT_SENSOR_GYR_DATA_RDY: 7571be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema ev = (struct TripleAxisDataEvent *)evtData; 75818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian fillSamples(ev, GYR); 7591be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema drainSamples(); 7601be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema break; 7611be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema case EVT_SENSOR_MAG_DATA_RDY: 7621be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema ev = (struct TripleAxisDataEvent *)evtData; 76318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian fillSamples(ev, MAG); 7641be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema drainSamples(); 7651be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema break; 766960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 767960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian} 768960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 769e723815b378ee08afad27d0fe84253e22c050528Ben Fennemastatic const struct SensorOps mSops = 77064eb1702a319ad4bbc181ed1e5af0703aea7ea22Zhengyin Qian{ 7714d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema .sensorPower = fusionPower, 7724d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema .sensorFirmwareUpload = fusionFirmwareUpload, 7734d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema .sensorSetRate = fusionSetRate, 7744d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema .sensorFlush = fusionFlush, 77564eb1702a319ad4bbc181ed1e5af0703aea7ea22Zhengyin Qian}; 77664eb1702a319ad4bbc181ed1e5af0703aea7ea22Zhengyin Qian 77718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianstatic bool fusionStart(uint32_t tid) 778960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian{ 779960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian osLog(LOG_INFO, " ORIENTATION: %ld\n", tid); 780e0e5b567748b8d46f60ede2ff12c4b2358ffe0dbZhengyin Qian size_t i, slabSize; 781960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 782960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian mTask.tid = tid; 783960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian mTask.flags = 0; 784960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 78518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian for (i = 0; i < NUM_OF_RAW_SENSOR; i++) { 786960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian mTask.sample_counts[i] = 0; 787960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian mTask.sample_indices[i] = 0; 788960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 789960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 79018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++) { 791e723815b378ee08afad27d0fe84253e22c050528Ben Fennema mTask.sensors[i].handle = sensorRegister(&mSi[i], &mSops, (void *)i, true); 79218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sensors[i].idx = i; 7939105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema mTask.sensors[i].use_gyro_data = true; 79418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sensors[i].use_mag_data = true; 79518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 796960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 7979105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema mTask.sensors[GEOMAG].use_gyro_data = false; 79818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sensors[GAME].use_mag_data = false; 79918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sensors[GRAVITY].use_mag_data = false; 8005fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu mTask.sensors[LINEAR].use_mag_data = false; 801960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 802cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung mTask.accel_client_cnt = 0; 803cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung mTask.gyro_client_cnt = 0; 804cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung mTask.mag_client_cnt = 0; 80546852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung 8067062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema slabSize = sizeof(struct TripleAxisDataEvent) 8077062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema + MAX_NUM_COMMS_EVENT_SAMPLES * sizeof(struct TripleAxisDataPoint); 8085fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu 8095fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu // worst case 6 output sensors * (N + 1) comms_events 8105fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu mDataSlab = slabAllocatorNew(slabSize, 4, 6 * (NUM_COMMS_EVENTS_IN_FIFO + 1)); 811c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian if (!mDataSlab) { 81241f1d787265e609c81b4906df20c725b053b4e94Meng-hsuan Chung osLog(LOG_ERROR, "ORIENTATION: slabAllocatorNew() FAILED\n"); 813c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian return false; 814c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian } 815e0e5b567748b8d46f60ede2ff12c4b2358ffe0dbZhengyin Qian 8169105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema osEventSubscribe(mTask.tid, EVT_APP_START); 8179105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema 818960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian return true; 819960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian} 820960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 82118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianstatic void fusionEnd() 822960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian{ 823960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian mTask.flags &= ~FUSION_FLAG_INITIALIZED; 82446852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung mTask.flags &= ~FUSION_FLAG_GAME_INITIALIZED; 825e0e5b567748b8d46f60ede2ff12c4b2358ffe0dbZhengyin Qian slabAllocatorDestroy(mDataSlab); 826960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian} 827960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 828960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin QianINTERNAL_APP_INIT( 829dca61fcfcc85a8240bc46a650c71008ded2d32ddBen Fennema APP_ID_MAKE(APP_ID_VENDOR_GOOGLE, 4), 830cc77708f80db23714a92638ddee64f59dbdf5483Dmitry Grinberg 0, 83118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian fusionStart, 83218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian fusionEnd, 83318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian fusionHandleEvent); 8345fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu 835