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> 21f805306b53d82eef67d8891a5dd5c32d3794a3abAlexey Polyudov#include <plat/rtc.h> 22f805306b53d82eef67d8891a5dd5c32d3794a3abAlexey Polyudov#include <plat/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> 32dbaf10bfdddf0a0b9f536524d4f6b08894a98118Alexey Polyudov#include <variant/sensType.h> 33960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian#include <limits.h> 34e0e5b567748b8d46f60ede2ff12c4b2358ffe0dbZhengyin Qian#include <slab.h> 35960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 36474093786e1adfd5e5fcbd84859f5152ca675af4Meng-hsuan Chung#define ORIENTATION_APP_VERSION 1 37474093786e1adfd5e5fcbd84859f5152ca675af4Meng-hsuan Chung 3841f1d787265e609c81b4906df20c725b053b4e94Meng-hsuan Chung#define MAX_NUM_COMMS_EVENT_SAMPLES 15 // at most 15 samples can fit in one comms_event 395fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu#define NUM_COMMS_EVENTS_IN_FIFO 2 // This controls how often the hub needs to wake up 405fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu // in batching 415fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu 425fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu// needs to be greater than max raw sensor rate ratio 435fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu#define FIFO_DEPTH (NUM_COMMS_EVENTS_IN_FIFO * MAX_NUM_COMMS_EVENT_SAMPLES) 445fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu 45f17db6a82e2e2f3c929c08726574456b97136b2fZhengyin Qian/* 46f17db6a82e2e2f3c929c08726574456b97136b2fZhengyin Qian * FIFO_MARGIN: max raw sensor rate ratio is 8:1. 475fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu * If 2 batchs of high rate data comes before 1 low rate data, there can be at max 15 samples left 485fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu * in the FIFO 49f17db6a82e2e2f3c929c08726574456b97136b2fZhengyin Qian */ 50f17db6a82e2e2f3c929c08726574456b97136b2fZhengyin Qian#define FIFO_MARGIN 15 5141f1d787265e609c81b4906df20c725b053b4e94Meng-hsuan Chung#define MAX_NUM_SAMPLES (FIFO_MARGIN + FIFO_DEPTH) // actual input sample fifo depth 529105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema#define EVT_SENSOR_ACC_DATA_RDY sensorGetMyEventType(SENS_TYPE_ACCEL) 539105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema#define EVT_SENSOR_GYR_DATA_RDY sensorGetMyEventType(SENS_TYPE_GYRO) 549105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema#define EVT_SENSOR_MAG_DATA_RDY sensorGetMyEventType(SENS_TYPE_MAG) 556ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu#define EVT_SENSOR_MAG_BIAS sensorGetMyEventType(SENS_TYPE_MAG_BIAS) 5618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 579105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema#define kGravityEarth 9.80665f 5846852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung#define kRad2deg (180.0f / M_PI) 599105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema#define MIN_GYRO_RATE_HZ SENSOR_HZ(100.0f) 609105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema#define MAX_MAG_RATE_HZ SENSOR_HZ(50.0f) 61960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 621be10b6751f50675307716747bd4d729d9e7aff5Ben Fennemaenum 631be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema{ 641be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema FUSION_FLAG_ENABLED = 0x01, 651be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema FUSION_FLAG_INITIALIZED = 0x08, 661be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema FUSION_FLAG_GAME_ENABLED = 0x10, 671be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema FUSION_FLAG_GAME_INITIALIZED = 0x20 68960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian}; 69960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 7018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianenum RawSensorType 7118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian{ 7218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian ACC, 7318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian GYR, 7418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian MAG, 7518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian NUM_OF_RAW_SENSOR 7618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian}; 7718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 7818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianenum FusionSensorType 791be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema{ 8018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian ORIENT, 8118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian GRAVITY, 8218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian GEOMAG, 8318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian LINEAR, 8418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian GAME, 8518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian ROTAT, 8618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian NUM_OF_FUSION_SENSOR 871be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema}; 881be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema 891be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema 9018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianstruct FusionSensorSample { 91960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian uint64_t time; 92960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian float x, y, z; 93960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian}; 94960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 9518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianstruct FusionSensor { 96960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian uint32_t handle; 9718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian struct TripleAxisDataEvent *ev; 98c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian uint64_t prev_time; 9918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian uint64_t latency; 10018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian uint32_t rate; 10118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian bool active; 1029105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema bool use_gyro_data; 10318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian bool use_mag_data; 10418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian uint8_t idx; 10518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian}; 10618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 10718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianstruct FusionTask { 10818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian uint32_t tid; 1091be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema uint32_t accelHandle; 1101be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema uint32_t gyroHandle; 1111be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema uint32_t magHandle; 112960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 113960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian struct Fusion fusion; 11418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian struct Fusion game; 115960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 11618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian struct FusionSensor sensors[NUM_OF_FUSION_SENSOR]; 11718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian struct FusionSensorSample samples[NUM_OF_RAW_SENSOR][MAX_NUM_SAMPLES]; 11818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian size_t sample_indices[NUM_OF_RAW_SENSOR]; 11918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian size_t sample_counts[NUM_OF_RAW_SENSOR]; 12018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian uint32_t counters[NUM_OF_RAW_SENSOR]; 12118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian uint64_t ResamplePeriodNs[NUM_OF_RAW_SENSOR]; 12218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian uint64_t last_time[NUM_OF_RAW_SENSOR]; 12318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian struct TripleAxisDataPoint last_sample[NUM_OF_RAW_SENSOR]; 124960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 125960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian uint32_t flags; 126960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 127c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian uint32_t raw_sensor_rate[NUM_OF_RAW_SENSOR]; 128c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian uint64_t raw_sensor_latency; 129c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian 130cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung uint8_t accel_client_cnt; 131cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung uint8_t gyro_client_cnt; 132cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung uint8_t mag_client_cnt; 133960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian}; 134960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 13518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianstatic uint32_t FusionRates[] = { 136960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian SENSOR_HZ(12.5f), 137960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian SENSOR_HZ(25.0f), 138960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian SENSOR_HZ(50.0f), 139960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian SENSOR_HZ(100.0f), 140960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian SENSOR_HZ(200.0f), 141960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 0, 142960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian}; 143960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 1445fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu//should match "supported rates in length" and be the timer length for that rate in nanosecs 1455fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xustatic const uint64_t rateTimerVals[] = { 146dd2f9808f0045a3eaa42cb488153eeb599eb97ffDmitry Grinberg 1000000000ULL / 12.5f, 147dd2f9808f0045a3eaa42cb488153eeb599eb97ffDmitry Grinberg 1000000000ULL / 25, 148dd2f9808f0045a3eaa42cb488153eeb599eb97ffDmitry Grinberg 1000000000ULL / 50, 149dd2f9808f0045a3eaa42cb488153eeb599eb97ffDmitry Grinberg 1000000000ULL / 100, 150dd2f9808f0045a3eaa42cb488153eeb599eb97ffDmitry Grinberg 1000000000ULL / 200, 151dd2f9808f0045a3eaa42cb488153eeb599eb97ffDmitry Grinberg}; 152dd2f9808f0045a3eaa42cb488153eeb599eb97ffDmitry Grinberg 15318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianstatic struct FusionTask mTask; 1544d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema 1554d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema#define DEC_INFO_RATE(name, rates, type, axis, inter, samples) \ 1564d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema .sensorName = name, \ 1574d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema .supportedRates = rates, \ 1584d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema .sensorType = type, \ 1594d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema .numAxis = axis, \ 1604d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema .interrupt = inter, \ 1614d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema .minSamples = samples 1624d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema 16318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianstatic const struct SensorInfo mSi[NUM_OF_FUSION_SENSOR] = 16464eb1702a319ad4bbc181ed1e5af0703aea7ea22Zhengyin Qian{ 1655fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu { DEC_INFO_RATE("Orientation", FusionRates, SENS_TYPE_ORIENTATION, NUM_AXIS_THREE, 1665fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu NANOHUB_INT_NONWAKEUP, 20) }, 1675fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu { DEC_INFO_RATE("Gravity", FusionRates, SENS_TYPE_GRAVITY, NUM_AXIS_THREE, 1685fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu NANOHUB_INT_NONWAKEUP, 20) }, 1695fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu { DEC_INFO_RATE("Geomagnetic Rotation Vector", FusionRates, SENS_TYPE_GEO_MAG_ROT_VEC, 1705fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 20) }, 1715fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu { DEC_INFO_RATE("Linear Acceleration", FusionRates, SENS_TYPE_LINEAR_ACCEL, NUM_AXIS_THREE, 1725fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu NANOHUB_INT_NONWAKEUP, 20) }, 1735fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu { DEC_INFO_RATE("Game Rotation Vector", FusionRates, SENS_TYPE_GAME_ROT_VECTOR, NUM_AXIS_THREE, 1745fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu NANOHUB_INT_NONWAKEUP, 300) }, 1755fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu { DEC_INFO_RATE("Rotation Vector", FusionRates, SENS_TYPE_ROTATION_VECTOR, NUM_AXIS_THREE, 1765fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu NANOHUB_INT_NONWAKEUP, 20) }, 17764eb1702a319ad4bbc181ed1e5af0703aea7ea22Zhengyin Qian}; 178960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 179e0e5b567748b8d46f60ede2ff12c4b2358ffe0dbZhengyin Qianstatic struct SlabAllocator *mDataSlab; 180e0e5b567748b8d46f60ede2ff12c4b2358ffe0dbZhengyin Qian 181e0e5b567748b8d46f60ede2ff12c4b2358ffe0dbZhengyin Qianstatic void dataEvtFree(void *ptr) 182960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian{ 183e0e5b567748b8d46f60ede2ff12c4b2358ffe0dbZhengyin Qian slabAllocatorFree(mDataSlab, ptr); 184960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian} 185960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 18618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianstatic void fillSamples(struct TripleAxisDataEvent *ev, enum RawSensorType index) 187960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian{ 18818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian bool bad_timestamp; 18918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian size_t i, w, n, num_samples; 19018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian struct TripleAxisDataPoint *curr_sample, *next_sample; 191f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema uint32_t counter; 192f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema uint64_t ResamplePeriodNs, curr_time, next_time; 19318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian uint64_t sample_spacing_ns; 19418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian float weight_next; 19518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 196cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung if (index == GYR && mTask.gyro_client_cnt == 0) { 197960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian return; 198960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 199cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung if (index == MAG && mTask.mag_client_cnt == 0) { 200960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian return; 201960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 202960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 20318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian n = mTask.sample_counts[index]; 20418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian i = mTask.sample_indices[index]; 20518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian counter = mTask.counters[index]; 20618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian ResamplePeriodNs = mTask.ResamplePeriodNs[index]; 20718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian w = (mTask.sample_indices[index] + n) % MAX_NUM_SAMPLES; 20818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 20918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian // check if this sensor was used before 21018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.last_time[index] == ULONG_LONG_MAX) { 21118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian curr_sample = ev->samples; 21218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian next_sample = curr_sample + 1; 213f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema num_samples = ev->samples[0].firstSample.numSamples; 21418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian curr_time = ev->referenceTime; 215960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } else { 21618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian curr_sample = &mTask.last_sample[index]; 21718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian next_sample = ev->samples; 218f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema num_samples = ev->samples[0].firstSample.numSamples + 1; 21918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian curr_time = mTask.last_time[index]; 22018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 221960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 22218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian while (num_samples > 1) { 223960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 22418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (next_sample == ev->samples) 22518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian next_time = ev->referenceTime; 22618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian else 22718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian next_time = curr_time + next_sample->deltaTime; 228960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 22918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian // error handling for non-chronological accel timestamps 23018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian sample_spacing_ns = (next_time > curr_time) ? (next_time - curr_time) : 0; 231960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 23246852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung // This can happen during sensor config changes 23318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian bad_timestamp = (sample_spacing_ns > 10 * ResamplePeriodNs); 234960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 23518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian // Check to see if we need to move the interpolation window or 23618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian // interpolate 23718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if ((counter >= sample_spacing_ns) || bad_timestamp) { 23818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian num_samples--; 23918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian counter -= (bad_timestamp ? counter : sample_spacing_ns); 24018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian curr_sample = next_sample; 24118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian next_sample++; 242960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 243c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian curr_time = next_time; 24418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } else { 245ae081f1f47013ddf08e5af685b9f6d02ac76421bDmitry Grinberg weight_next = (float)counter / floatFromUint64(sample_spacing_ns); 24618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 24718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.samples[index][w].x = curr_sample->x + weight_next * 24818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian (next_sample->x - curr_sample->x); 24918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.samples[index][w].y = curr_sample->y + weight_next * 25018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian (next_sample->y - curr_sample->y); 25118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.samples[index][w].z = curr_sample->z + weight_next * 25218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian (next_sample->z - curr_sample->z); 25318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.samples[index][w].time = curr_time + counter; 25418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 25518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian // Move the read index when buffer is full 25618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (++n > MAX_NUM_SAMPLES) { 25718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian n = MAX_NUM_SAMPLES; 25818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 25918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (++i == MAX_NUM_SAMPLES) { 26018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian i = 0; 26118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 26218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 26318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 26418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian // Reset the write index 26518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (++w == MAX_NUM_SAMPLES) { 26618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian w = 0; 26718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 26818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 26918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian // Move to the next resample 27018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian counter += ResamplePeriodNs; 271960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 272960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 27318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 27418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sample_counts[index] = n; 27518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sample_indices[index] = i; 27618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.counters[index] = counter; 27718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.last_sample[index] = *curr_sample; 27818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.last_time[index] = curr_time; 279960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian} 280960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 2814f3270dd3a919e485bb0d6c4db6e0f4d56f7d5ceBen Fennemastatic bool allocateDataEvt(struct FusionSensor *mSensor, uint64_t time) 2824f3270dd3a919e485bb0d6c4db6e0f4d56f7d5ceBen Fennema{ 2834f3270dd3a919e485bb0d6c4db6e0f4d56f7d5ceBen Fennema mSensor->ev = slabAllocatorAlloc(mDataSlab); 2844f3270dd3a919e485bb0d6c4db6e0f4d56f7d5ceBen Fennema if (mSensor->ev == NULL) { 285faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung // slab allocation failed, need to stop draining raw samples for now. 286faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung osLog(LOG_INFO, "ORIENTATION: slabAllocatorAlloc() Failed\n"); 2874f3270dd3a919e485bb0d6c4db6e0f4d56f7d5ceBen Fennema return false; 2884f3270dd3a919e485bb0d6c4db6e0f4d56f7d5ceBen Fennema } 2894f3270dd3a919e485bb0d6c4db6e0f4d56f7d5ceBen Fennema 2904f3270dd3a919e485bb0d6c4db6e0f4d56f7d5ceBen Fennema // delta time for the first sample is sample count 2914f3270dd3a919e485bb0d6c4db6e0f4d56f7d5ceBen Fennema memset(&mSensor->ev->samples[0].firstSample, 0x00, sizeof(struct SensorFirstSample)); 2924f3270dd3a919e485bb0d6c4db6e0f4d56f7d5ceBen Fennema mSensor->ev->referenceTime = time; 2934f3270dd3a919e485bb0d6c4db6e0f4d56f7d5ceBen Fennema mSensor->prev_time = time; 2944f3270dd3a919e485bb0d6c4db6e0f4d56f7d5ceBen Fennema 2954f3270dd3a919e485bb0d6c4db6e0f4d56f7d5ceBen Fennema return true; 2964f3270dd3a919e485bb0d6c4db6e0f4d56f7d5ceBen Fennema} 2974f3270dd3a919e485bb0d6c4db6e0f4d56f7d5ceBen Fennema 298faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung// returns false if addSample() fails 299faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chungstatic bool addSample(struct FusionSensor *mSensor, uint64_t time, float x, float y, float z) 300960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian{ 3017062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema struct TripleAxisDataPoint *sample; 302e0e5b567748b8d46f60ede2ff12c4b2358ffe0dbZhengyin Qian 303faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung // Bypass processing this accel sample. 304faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung // This is needed after recovering from a slab shortage. 305faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung if (mSensor->prev_time == time) { 306faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung osLog(LOG_INFO, "Accel sample has been processed by fusion sensor %d\n", 307faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung mSensor->idx); 308faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung return true; 309faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung } 310faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung 31118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mSensor->ev == NULL) { 3124f3270dd3a919e485bb0d6c4db6e0f4d56f7d5ceBen Fennema if (!allocateDataEvt(mSensor, time)) 313faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung return false; 314960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 315960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 316f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema if (mSensor->ev->samples[0].firstSample.numSamples >= MAX_NUM_COMMS_EVENT_SAMPLES) { 31741f1d787265e609c81b4906df20c725b053b4e94Meng-hsuan Chung osLog(LOG_ERROR, "ORIENTATION: BAD_INDEX\n"); 318faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung return false; 319960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 320960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 321f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema sample = &mSensor->ev->samples[mSensor->ev->samples[0].firstSample.numSamples++]; 322e0e5b567748b8d46f60ede2ff12c4b2358ffe0dbZhengyin Qian 323c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian if (mSensor->ev->samples[0].firstSample.numSamples > 1) { 32446852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung sample->deltaTime = time > mSensor->prev_time ? (time - mSensor->prev_time) : 0; 325c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian mSensor->prev_time = time; 326c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian } 327960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 328960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian sample->x = x; 329960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian sample->y = y; 330960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian sample->z = z; 331960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 332f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema if (mSensor->ev->samples[0].firstSample.numSamples == MAX_NUM_COMMS_EVENT_SAMPLES) { 3335fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu osEnqueueEvtOrFree( 3345fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu EVENT_TYPE_BIT_DISCARDABLE | sensorGetMyEventType(mSi[mSensor->idx].sensorType), 3355fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu mSensor->ev, dataEvtFree); 33618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mSensor->ev = NULL; 337960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 338faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung return true; 339960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian} 340960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 341faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung// returns false if addSample fails for any fusion sensor 342faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung// (most likely due to slab allocation failure) 343faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chungstatic bool updateOutput(ssize_t last_accel_sample_index, uint64_t last_sensor_time) 344960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian{ 34518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian struct Vec4 attitude; 34618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian struct Vec3 g, a; 3475fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu struct Mat33 R; // direction-cosine/rotation matrix, inertial -> device 3485fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu bool rInited; // indicates if matrix R has been initialzed. for avoiding repeated computation 349faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung bool ret = true; 35018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 35118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (fusionHasEstimate(&mTask.game)) { 3525fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu rInited = false; 35318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.sensors[GAME].active) { 35418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian fusionGetAttitude(&mTask.game, &attitude); 355faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung if (!addSample(&mTask.sensors[GAME], 35618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian last_sensor_time, 35718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian attitude.x, 35818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian attitude.y, 359faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung attitude.z)) { 360faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung ret = false; 361faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung } 36218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 36318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 36418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.sensors[GRAVITY].active) { 36518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian fusionGetRotationMatrix(&mTask.game, &R); 3665fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu rInited = true; 36718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian initVec3(&g, R.elem[0][2], R.elem[1][2], R.elem[2][2]); 36818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian vec3ScalarMul(&g, kGravityEarth); 369faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung if (!addSample(&mTask.sensors[GRAVITY], 37018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian last_sensor_time, 371faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung g.x, 372faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung g.y, 373faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung g.z)) { 374faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung ret = false; 375faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung } 37618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 3775fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu 3785fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu if (last_accel_sample_index >= 0 3795fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu && mTask.sensors[LINEAR].active) { 3805fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu if (!rInited) { 3815fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu fusionGetRotationMatrix(&mTask.game, &R); 3825fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu } 3835fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu initVec3(&g, R.elem[0][2], R.elem[1][2], R.elem[2][2]); 3845fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu vec3ScalarMul(&g, kGravityEarth); 3855fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu initVec3(&a, 3865fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu mTask.samples[0][last_accel_sample_index].x, 3875fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu mTask.samples[0][last_accel_sample_index].y, 3885fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu mTask.samples[0][last_accel_sample_index].z); 3895fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu 390faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung if (!addSample(&mTask.sensors[LINEAR], 3915fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu mTask.samples[0][last_accel_sample_index].time, 3925fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu a.x - g.x, 3935fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu a.y - g.y, 394faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung a.z - g.z)) { 395faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung ret = false; 396faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung } 3975fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu } 39818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 39918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 400960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian if (fusionHasEstimate(&mTask.fusion)) { 401960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian fusionGetAttitude(&mTask.fusion, &attitude); 402960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 40318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.sensors[ORIENT].active) { 4045fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu fusionGetRotationMatrix(&mTask.fusion, &R); 40518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian // x, y, z = yaw, pitch, roll 40618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian float x = atan2f(-R.elem[0][1], R.elem[0][0]) * kRad2deg; 40718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian float y = atan2f(-R.elem[1][2], R.elem[2][2]) * kRad2deg; 40818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian float z = asinf(R.elem[0][2]) * kRad2deg; 40918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 41018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (x < 0.0f) { 41118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian x += 360.0f; 41218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 41318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 414faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung if (!addSample(&mTask.sensors[ORIENT], 415faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung last_sensor_time, 416faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung x, 417faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung y, 418faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung z)) { 419faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung ret = false; 420faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung } 42118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 422960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 42318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.sensors[GEOMAG].active) { 424faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung if (!addSample(&mTask.sensors[GEOMAG], 42518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian last_sensor_time, 42618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian attitude.x, 42718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian attitude.y, 428faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung attitude.z)) { 429faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung ret = false; 430faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung } 431960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 432960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 43318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.sensors[ROTAT].active) { 434faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung if (!addSample(&mTask.sensors[ROTAT], 43518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian last_sensor_time, 43618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian attitude.x, 43718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian attitude.y, 438faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung attitude.z)) { 439faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung ret = false; 440faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung } 44118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 44218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 443960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 444faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung return ret; 445960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian} 446960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 447960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qianstatic void drainSamples() 448960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian{ 449faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung struct Vec3 a, w, m; 450faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung uint64_t a_time, g_time, m_time; 45118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian size_t i = mTask.sample_indices[ACC]; 452960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian size_t j = 0; 453960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian size_t k = 0; 4541be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema size_t which; 4551be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema float dT; 456faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung bool success = true; 4571be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema 458cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung if (mTask.gyro_client_cnt > 0) 45918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian j = mTask.sample_indices[GYR]; 4601be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema 461cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung if (mTask.mag_client_cnt > 0) 46218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian k = mTask.sample_indices[MAG]; 4631be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema 464faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung // Keep draining raw samples and producing fusion samples only if 465faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung // 1) all raw sensors needed are present (to compare timestamp) and 466faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung // 2) updateOutput() succeeded (no slab shortage) 467faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung // Otherwise, wait till next raw sample event. 46818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian while (mTask.sample_counts[ACC] > 0 469cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung && (!(mTask.gyro_client_cnt > 0) || mTask.sample_counts[GYR] > 0) 470faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung && (!(mTask.mag_client_cnt > 0) || mTask.sample_counts[MAG] > 0) 471faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung && success) { 47218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian a_time = mTask.samples[ACC][i].time; 473cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung g_time = mTask.gyro_client_cnt > 0 ? mTask.samples[GYR][j].time 474960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian : ULONG_LONG_MAX; 475cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung m_time = mTask.mag_client_cnt > 0 ? mTask.samples[MAG][k].time 476960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian : ULONG_LONG_MAX; 477960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 478960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian // priority with same timestamp: gyro > acc > mag 479960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian if (g_time <= a_time && g_time <= m_time) { 48018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian which = GYR; 481960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } else if (a_time <= m_time) { 48218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian which = ACC; 483960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } else { 48418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian which = MAG; 485960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 486960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 487a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian dT = floatFromUint64(mTask.ResamplePeriodNs[which]) * 1e-9f; 4881be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema switch (which) { 48918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian case ACC: 49018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian initVec3(&a, mTask.samples[ACC][i].x, mTask.samples[ACC][i].y, mTask.samples[ACC][i].z); 49118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 49218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.flags & FUSION_FLAG_ENABLED) 49318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian fusionHandleAcc(&mTask.fusion, &a, dT); 494960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 49518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.flags & FUSION_FLAG_GAME_ENABLED) 49618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian fusionHandleAcc(&mTask.game, &a, dT); 497960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 498faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung success = updateOutput(i, mTask.samples[ACC][i].time); 499960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 500faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung // Do not remove the accel sample until all active fusion sesnsors 501faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung // successfully updated the output. 502faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung // Fusion sensors that have processed this accel sample will bypass 503faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung // it in addSample(). 504faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung if (success) { 505faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung --mTask.sample_counts[ACC]; 506faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung if (++i == MAX_NUM_SAMPLES) { 507faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung i = 0; 508faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung } 509faec9a1e0e8ede8f9ec0875a258e2a994b473675Meng-hsuan Chung } 5101be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema break; 51118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian case GYR: 51218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian initVec3(&w, mTask.samples[GYR][j].x, mTask.samples[GYR][j].y, mTask.samples[GYR][j].z); 513960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 51418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.flags & FUSION_FLAG_ENABLED) 51518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian fusionHandleGyro(&mTask.fusion, &w, dT); 516960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 51718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.flags & FUSION_FLAG_GAME_ENABLED) 51818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian fusionHandleGyro(&mTask.game, &w, dT); 51918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 52018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian --mTask.sample_counts[GYR]; 5211be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema if (++j == MAX_NUM_SAMPLES) 5221be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema j = 0; 5231be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema break; 52418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian case MAG: 52518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian initVec3(&m, mTask.samples[MAG][k].x, mTask.samples[MAG][k].y, mTask.samples[MAG][k].z); 526960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 5276ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu fusionHandleMag(&mTask.fusion, &m, dT); 528960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 52918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian --mTask.sample_counts[MAG]; 5301be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema if (++k == MAX_NUM_SAMPLES) 5311be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema k = 0; 5321be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema break; 533960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 534960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 535960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 53618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sample_indices[ACC] = i; 537960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 538cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung if (mTask.gyro_client_cnt > 0) 53918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sample_indices[GYR] = j; 540960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 541cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung if (mTask.mag_client_cnt > 0) 54218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sample_indices[MAG] = k; 543960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 54418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++) { 54518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.sensors[i].ev != NULL) { 546d4cfbfe80cdb23453458697fb57463d0ab3b3432Alexey Polyudov osEnqueueEvtOrFree(EVENT_TYPE_BIT_DISCARDABLE | sensorGetMyEventType(mSi[i].sensorType), 547d4cfbfe80cdb23453458697fb57463d0ab3b3432Alexey Polyudov mTask.sensors[i].ev, dataEvtFree); 54818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sensors[i].ev = NULL; 54918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 550960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 551960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian} 552960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 553960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qianstatic void configureFusion() 554960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian{ 55518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.sensors[ORIENT].active 55618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian || mTask.sensors[ROTAT].active 55718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian || mTask.sensors[GEOMAG].active) { 55818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.flags |= FUSION_FLAG_ENABLED; 55918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian initFusion(&mTask.fusion, 560cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung (mTask.mag_client_cnt > 0 ? FUSION_USE_MAG : 0) | 561cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung (mTask.gyro_client_cnt > 0 ? FUSION_USE_GYRO : 0) | 56218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian ((mTask.flags & FUSION_FLAG_INITIALIZED) ? 0 : FUSION_REINITIALIZE)); 56318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.flags |= FUSION_FLAG_INITIALIZED; 56418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } else { 56518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.flags &= ~FUSION_FLAG_ENABLED; 56618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.flags &= ~FUSION_FLAG_INITIALIZED; 56718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 568960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian} 569960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 57018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianstatic void configureGame() 571960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian{ 5725fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu if (mTask.sensors[GAME].active || mTask.sensors[GRAVITY].active || 5735fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu mTask.sensors[LINEAR].active) { 57418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.flags |= FUSION_FLAG_GAME_ENABLED; 5755fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu initFusion(&mTask.game, FUSION_USE_GYRO | 5765fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu ((mTask.flags & FUSION_FLAG_INITIALIZED) ? 0 : FUSION_REINITIALIZE)); 57718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.flags |= FUSION_FLAG_GAME_INITIALIZED; 57818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } else { 57918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.flags &= ~FUSION_FLAG_GAME_ENABLED; 58018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.flags &= ~FUSION_FLAG_GAME_INITIALIZED; 581960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 582960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian} 583960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 584c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qianstatic void fusionSetRateAcc(void) 585960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian{ 5861be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema int i; 58718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.accelHandle == 0) { 58818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sample_counts[ACC] = 0; 58918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sample_indices[ACC] = 0; 59018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.counters[ACC] = 0; 59118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.last_time[ACC] = ULONG_LONG_MAX; 59246852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung for (i = 0; sensorFind(SENS_TYPE_ACCEL, i, &mTask.accelHandle) != NULL; i++) { 5935fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu if (sensorRequest(mTask.tid, mTask.accelHandle, mTask.raw_sensor_rate[ACC], 5945fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu mTask.raw_sensor_latency)) { 5951be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema osEventSubscribe(mTask.tid, EVT_SENSOR_ACC_DATA_RDY); 5961be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema break; 5971be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema } 5981be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema } 5991be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema } else { 6005fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu sensorRequestRateChange(mTask.tid, mTask.accelHandle, mTask.raw_sensor_rate[ACC], 6015fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu mTask.raw_sensor_latency); 6021be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema } 60318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian} 60418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 605c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qianstatic void fusionSetRateGyr(void) 60618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian{ 60718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian int i; 6081be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema if (mTask.gyroHandle == 0) { 60918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sample_counts[GYR] = 0; 61018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sample_indices[GYR] = 0; 61118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.counters[GYR] = 0; 61218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.last_time[GYR] = ULONG_LONG_MAX; 61346852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung for (i = 0; sensorFind(SENS_TYPE_GYRO, i, &mTask.gyroHandle) != NULL; i++) { 6145fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu if (sensorRequest(mTask.tid, mTask.gyroHandle, mTask.raw_sensor_rate[GYR], 6155fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu mTask.raw_sensor_latency)) { 6161be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema osEventSubscribe(mTask.tid, EVT_SENSOR_GYR_DATA_RDY); 6171be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema break; 6181be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema } 6191be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema } 6201be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema } else { 6215fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu sensorRequestRateChange(mTask.tid, mTask.gyroHandle, mTask.raw_sensor_rate[GYR], 6225fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu mTask.raw_sensor_latency); 6231be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema } 62418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian} 62518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 626c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qianstatic void fusionSetRateMag(void) 62718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian{ 62818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian int i; 6291be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema if (mTask.magHandle == 0) { 63018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sample_counts[MAG] = 0; 63118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sample_indices[MAG] = 0; 63218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.counters[MAG] = 0; 63318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.last_time[MAG] = ULONG_LONG_MAX; 63446852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung for (i = 0; sensorFind(SENS_TYPE_MAG, i, &mTask.magHandle) != NULL; i++) { 6355fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu if (sensorRequest(mTask.tid, mTask.magHandle, mTask.raw_sensor_rate[MAG], 6365fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu mTask.raw_sensor_latency)) { 6371be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema osEventSubscribe(mTask.tid, EVT_SENSOR_MAG_DATA_RDY); 6386ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu osEventSubscribe(mTask.tid, EVT_SENSOR_MAG_BIAS); 6391be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema break; 6401be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema } 6411be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema } 6421be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema } else { 6435fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu sensorRequestRateChange(mTask.tid, mTask.magHandle, mTask.raw_sensor_rate[MAG], 6445fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu mTask.raw_sensor_latency); 645c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian } 646c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian} 647c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian 648e723815b378ee08afad27d0fe84253e22c050528Ben Fennemastatic bool fusionSetRate(uint32_t rate, uint64_t latency, void *cookie) 649c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian{ 650e723815b378ee08afad27d0fe84253e22c050528Ben Fennema struct FusionSensor *mSensor = &mTask.sensors[(int)cookie]; 651c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian int i; 652c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian uint32_t max_rate = 0; 653c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian uint32_t gyr_rate, mag_rate; 65446852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung uint64_t min_resample_period = ULONG_LONG_MAX; 655c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian 656c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian mSensor->rate = rate; 657c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian mSensor->latency = latency; 658c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian 659c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++) { 660c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian if (mTask.sensors[i].active) { 661c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian max_rate = max_rate > mTask.sensors[i].rate ? max_rate : mTask.sensors[i].rate; 662c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian } 663c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian } 664c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian 665cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung if (mTask.accel_client_cnt > 0) { 666c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian mTask.raw_sensor_rate[ACC] = max_rate; 66746852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung mTask.ResamplePeriodNs[ACC] = sensorTimerLookupCommon(FusionRates, rateTimerVals, max_rate); 66846852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung min_resample_period = mTask.ResamplePeriodNs[ACC] < min_resample_period ? 66946852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung mTask.ResamplePeriodNs[ACC] : min_resample_period; 670c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian } 671c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian 672cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung if (mTask.gyro_client_cnt > 0) { 6739105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema gyr_rate = max_rate > MIN_GYRO_RATE_HZ ? max_rate : MIN_GYRO_RATE_HZ; 674c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian mTask.raw_sensor_rate[GYR] = gyr_rate; 67546852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung mTask.ResamplePeriodNs[GYR] = sensorTimerLookupCommon(FusionRates, rateTimerVals, gyr_rate); 67646852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung min_resample_period = mTask.ResamplePeriodNs[GYR] < min_resample_period ? 67746852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung mTask.ResamplePeriodNs[GYR] : min_resample_period; 678c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian } 679c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian 680cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung if (mTask.mag_client_cnt > 0) { 6819105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema mag_rate = max_rate < MAX_MAG_RATE_HZ ? max_rate : MAX_MAG_RATE_HZ; 682c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian mTask.raw_sensor_rate[MAG] = mag_rate; 68346852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung mTask.ResamplePeriodNs[MAG] = sensorTimerLookupCommon(FusionRates, rateTimerVals, mag_rate); 68446852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung min_resample_period = mTask.ResamplePeriodNs[MAG] < min_resample_period ? 68546852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung mTask.ResamplePeriodNs[MAG] : min_resample_period; 686c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian } 687c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian 68846852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung // This guarantees that local raw sensor FIFOs won't overflow. 68946852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung mTask.raw_sensor_latency = min_resample_period * (FIFO_DEPTH - 1); 69046852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung 691c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++) { 692c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian if (mTask.sensors[i].active) { 693c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian mTask.raw_sensor_latency = mTask.sensors[i].latency < mTask.raw_sensor_latency ? 694c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian mTask.sensors[i].latency : mTask.raw_sensor_latency; 695c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian } 69618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 697c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian 698cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung if (mTask.accel_client_cnt > 0) 699c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian fusionSetRateAcc(); 700cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung if (mTask.gyro_client_cnt > 0) 701c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian fusionSetRateGyr(); 702cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung if (mTask.mag_client_cnt > 0) 703c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian fusionSetRateMag(); 704c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian if (mSensor->rate > 0) 705c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency); 706c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian 707c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian return true; 70818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian} 70918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 710e723815b378ee08afad27d0fe84253e22c050528Ben Fennemastatic bool fusionPower(bool on, void *cookie) 71118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian{ 712e723815b378ee08afad27d0fe84253e22c050528Ben Fennema struct FusionSensor *mSensor = &mTask.sensors[(int)cookie]; 713e723815b378ee08afad27d0fe84253e22c050528Ben Fennema int idx; 714e723815b378ee08afad27d0fe84253e22c050528Ben Fennema 71518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mSensor->active = on; 71618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (on == false) { 717cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung mTask.accel_client_cnt--; 7189105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema if (mSensor->use_gyro_data) 719cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung mTask.gyro_client_cnt--; 7209105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema if (mSensor->use_mag_data) 721cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung mTask.mag_client_cnt--; 72218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 723cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung // if client_cnt == 0 and Handle == 0, nothing need to be done. 724cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung // if client_cnt > 0 and Handle == 0, something else is turning it on, all will be done. 725cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung if (mTask.accel_client_cnt == 0 && mTask.accelHandle != 0) { 72618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian sensorRelease(mTask.tid, mTask.accelHandle); 72718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.accelHandle = 0; 72818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian osEventUnsubscribe(mTask.tid, EVT_SENSOR_ACC_DATA_RDY); 72918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 73018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 731cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung if (mTask.gyro_client_cnt == 0 && mTask.gyroHandle != 0) { 73218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian sensorRelease(mTask.tid, mTask.gyroHandle); 73318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.gyroHandle = 0; 73418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian osEventUnsubscribe(mTask.tid, EVT_SENSOR_GYR_DATA_RDY); 73518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 73618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 737cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung if (mTask.mag_client_cnt == 0 && mTask.magHandle != 0) { 73818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian sensorRelease(mTask.tid, mTask.magHandle); 73918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.magHandle = 0; 74018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian osEventUnsubscribe(mTask.tid, EVT_SENSOR_MAG_DATA_RDY); 74118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 742c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian 743e723815b378ee08afad27d0fe84253e22c050528Ben Fennema idx = mSensor->idx; 744e723815b378ee08afad27d0fe84253e22c050528Ben Fennema (void) fusionSetRate(0, ULONG_LONG_MAX, (void *)idx); 74518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } else { 746cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung mTask.accel_client_cnt++; 7479105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema if (mSensor->use_gyro_data) 748cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung mTask.gyro_client_cnt++; 7499105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema if (mSensor->use_mag_data) 750cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung mTask.mag_client_cnt++; 7511be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema } 7521be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema 75318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian configureFusion(); 75418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian configureGame(); 75518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, on, 0); 756960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 757960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian return true; 758960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian} 759960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 760e723815b378ee08afad27d0fe84253e22c050528Ben Fennemastatic bool fusionFirmwareUpload(void *cookie) 761960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian{ 762e723815b378ee08afad27d0fe84253e22c050528Ben Fennema struct FusionSensor *mSensor = &mTask.sensors[(int)cookie]; 763960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 76418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0); 76518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian return true; 76618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian} 76718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 768e723815b378ee08afad27d0fe84253e22c050528Ben Fennemastatic bool fusionFlush(void *cookie) 76918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian{ 770e723815b378ee08afad27d0fe84253e22c050528Ben Fennema struct FusionSensor *mSensor = &mTask.sensors[(int)cookie]; 77118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian uint32_t evtType = sensorGetMyEventType(mSi[mSensor->idx].sensorType); 772e723815b378ee08afad27d0fe84253e22c050528Ben Fennema 77318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian osEnqueueEvt(evtType, SENSOR_DATA_EVENT_FLUSH, NULL); 77418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian return true; 77518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian} 77618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 77718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianstatic void fusionHandleEvent(uint32_t evtType, const void* evtData) 778960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian{ 7797062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema struct TripleAxisDataEvent *ev; 7809105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema int i; 781960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 7820bcb1a3baec93492e58afff27f2ef92afcbf1698Ben Fennema if (evtData == SENSOR_DATA_EVENT_FLUSH) 7830bcb1a3baec93492e58afff27f2ef92afcbf1698Ben Fennema return; 7840bcb1a3baec93492e58afff27f2ef92afcbf1698Ben Fennema 785960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian switch (evtType) { 7869105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema case EVT_APP_START: 7879105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema // check for gyro and mag 7889105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema osEventUnsubscribe(mTask.tid, EVT_APP_START); 7899105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema if (!sensorFind(SENS_TYPE_GYRO, 0, &mTask.gyroHandle)) { 7909105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++) 7919105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema mTask.sensors[i].use_gyro_data = false; 7929105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema } 7939105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema mTask.gyroHandle = 0; 7949105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema if (!sensorFind(SENS_TYPE_MAG, 0, &mTask.magHandle)) { 7959105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++) 7969105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema mTask.sensors[i].use_mag_data = false; 7979105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema } 7989105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema mTask.magHandle = 0; 7999105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema break; 8001be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema case EVT_SENSOR_ACC_DATA_RDY: 8011be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema ev = (struct TripleAxisDataEvent *)evtData; 80218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian fillSamples(ev, ACC); 8031be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema drainSamples(); 8041be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema break; 8051be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema case EVT_SENSOR_GYR_DATA_RDY: 8061be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema ev = (struct TripleAxisDataEvent *)evtData; 80718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian fillSamples(ev, GYR); 8081be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema drainSamples(); 8091be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema break; 8106ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu case EVT_SENSOR_MAG_BIAS: 8116ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu ev = (struct TripleAxisDataEvent *)evtData; 8126ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu if (ev->samples[0].firstSample.biasPresent && mTask.flags & FUSION_FLAG_ENABLED) { 8136ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu //it is a user initiated mag cal event 8146ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu fusionSetMagTrust(&mTask.fusion, MANUAL_MAG_CAL); 8156ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu } 8166ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu break; 8171be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema case EVT_SENSOR_MAG_DATA_RDY: 8181be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema ev = (struct TripleAxisDataEvent *)evtData; 81918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian fillSamples(ev, MAG); 8201be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema drainSamples(); 8211be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema break; 822960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 823960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian} 824960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 825e723815b378ee08afad27d0fe84253e22c050528Ben Fennemastatic const struct SensorOps mSops = 82664eb1702a319ad4bbc181ed1e5af0703aea7ea22Zhengyin Qian{ 8274d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema .sensorPower = fusionPower, 8284d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema .sensorFirmwareUpload = fusionFirmwareUpload, 8294d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema .sensorSetRate = fusionSetRate, 8304d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema .sensorFlush = fusionFlush, 83164eb1702a319ad4bbc181ed1e5af0703aea7ea22Zhengyin Qian}; 83264eb1702a319ad4bbc181ed1e5af0703aea7ea22Zhengyin Qian 83318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianstatic bool fusionStart(uint32_t tid) 834960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian{ 835e0e5b567748b8d46f60ede2ff12c4b2358ffe0dbZhengyin Qian size_t i, slabSize; 836960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 837960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian mTask.tid = tid; 838960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian mTask.flags = 0; 839960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 84018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian for (i = 0; i < NUM_OF_RAW_SENSOR; i++) { 841960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian mTask.sample_counts[i] = 0; 842960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian mTask.sample_indices[i] = 0; 843960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 844960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 84518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++) { 846e723815b378ee08afad27d0fe84253e22c050528Ben Fennema mTask.sensors[i].handle = sensorRegister(&mSi[i], &mSops, (void *)i, true); 84718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sensors[i].idx = i; 8489105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema mTask.sensors[i].use_gyro_data = true; 84918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sensors[i].use_mag_data = true; 85018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 851960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 8529105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema mTask.sensors[GEOMAG].use_gyro_data = false; 85318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sensors[GAME].use_mag_data = false; 85418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sensors[GRAVITY].use_mag_data = false; 8555fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu mTask.sensors[LINEAR].use_mag_data = false; 856960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 857cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung mTask.accel_client_cnt = 0; 858cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung mTask.gyro_client_cnt = 0; 859cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung mTask.mag_client_cnt = 0; 86046852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung 8617062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema slabSize = sizeof(struct TripleAxisDataEvent) 8627062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema + MAX_NUM_COMMS_EVENT_SAMPLES * sizeof(struct TripleAxisDataPoint); 8635fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu 8645fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu // worst case 6 output sensors * (N + 1) comms_events 8655fed15d587cf904753f8b86bcd552a1ddb0c39fbPeng Xu mDataSlab = slabAllocatorNew(slabSize, 4, 6 * (NUM_COMMS_EVENTS_IN_FIFO + 1)); 866c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian if (!mDataSlab) { 86741f1d787265e609c81b4906df20c725b053b4e94Meng-hsuan Chung osLog(LOG_ERROR, "ORIENTATION: slabAllocatorNew() FAILED\n"); 868c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian return false; 869c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian } 870e0e5b567748b8d46f60ede2ff12c4b2358ffe0dbZhengyin Qian 8719105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema osEventSubscribe(mTask.tid, EVT_APP_START); 8729105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema 873960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian return true; 874960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian} 875960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 87618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianstatic void fusionEnd() 877960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian{ 878960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian mTask.flags &= ~FUSION_FLAG_INITIALIZED; 87946852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung mTask.flags &= ~FUSION_FLAG_GAME_INITIALIZED; 880e0e5b567748b8d46f60ede2ff12c4b2358ffe0dbZhengyin Qian slabAllocatorDestroy(mDataSlab); 881960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian} 882960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 883960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin QianINTERNAL_APP_INIT( 88454794375c7f69172894e9f365db835440c2bfdf6Alexey Polyudov APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 4), 885474093786e1adfd5e5fcbd84859f5152ca675af4Meng-hsuan Chung ORIENTATION_APP_VERSION, 88618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian fusionStart, 88718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian fusionEnd, 88818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian fusionHandleEvent); 889