orientation.c revision f17db6a82e2e2f3c929c08726574456b97136b2f
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 3641f1d787265e609c81b4906df20c725b053b4e94Meng-hsuan Chung#define NUM_COMMS_EVENTS_IN_FIFO 2 // This controls how often the hub needs to wake up in batching 3741f1d787265e609c81b4906df20c725b053b4e94Meng-hsuan Chung#define FIFO_DEPTH (NUM_COMMS_EVENTS_IN_FIFO * MAX_NUM_COMMS_EVENT_SAMPLES) // needs to be greater than max raw sensor rate ratio 38f17db6a82e2e2f3c929c08726574456b97136b2fZhengyin Qian/* 39f17db6a82e2e2f3c929c08726574456b97136b2fZhengyin Qian * FIFO_MARGIN: max raw sensor rate ratio is 8:1. 40f17db6a82e2e2f3c929c08726574456b97136b2fZhengyin Qian * If 2 batchs of high rate data comes before 1 low rate data, there can be at max 15 samples left in the FIFO 41f17db6a82e2e2f3c929c08726574456b97136b2fZhengyin Qian */ 42f17db6a82e2e2f3c929c08726574456b97136b2fZhengyin Qian#define FIFO_MARGIN 15 4341f1d787265e609c81b4906df20c725b053b4e94Meng-hsuan Chung#define MAX_NUM_SAMPLES (FIFO_MARGIN + FIFO_DEPTH) // actual input sample fifo depth 449105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema#define EVT_SENSOR_ACC_DATA_RDY sensorGetMyEventType(SENS_TYPE_ACCEL) 459105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema#define EVT_SENSOR_GYR_DATA_RDY sensorGetMyEventType(SENS_TYPE_GYRO) 469105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema#define EVT_SENSOR_MAG_DATA_RDY sensorGetMyEventType(SENS_TYPE_MAG) 4718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 489105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema#define kGravityEarth 9.80665f 4946852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung#define kRad2deg (180.0f / M_PI) 509105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema#define MIN_GYRO_RATE_HZ SENSOR_HZ(100.0f) 519105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema#define MAX_MAG_RATE_HZ SENSOR_HZ(50.0f) 52960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 531be10b6751f50675307716747bd4d729d9e7aff5Ben Fennemaenum 541be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema{ 551be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema FUSION_FLAG_ENABLED = 0x01, 561be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema FUSION_FLAG_INITIALIZED = 0x08, 571be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema FUSION_FLAG_GAME_ENABLED = 0x10, 581be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema FUSION_FLAG_GAME_INITIALIZED = 0x20 59960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian}; 60960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 6118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianenum RawSensorType 6218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian{ 6318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian ACC, 6418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian GYR, 6518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian MAG, 6618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian NUM_OF_RAW_SENSOR 6718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian}; 6818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 6918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianenum FusionSensorType 701be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema{ 7118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian ORIENT, 7218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian GRAVITY, 7318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian GEOMAG, 7418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian LINEAR, 7518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian GAME, 7618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian ROTAT, 7718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian NUM_OF_FUSION_SENSOR 781be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema}; 791be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema 801be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema 8118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianstruct FusionSensorSample { 82960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian uint64_t time; 83960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian float x, y, z; 84960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian}; 85960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 8618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianstruct FusionSensor { 87960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian uint32_t handle; 8818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian struct TripleAxisDataEvent *ev; 89c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian uint64_t prev_time; 9018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian uint64_t latency; 9118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian uint32_t rate; 9218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian bool active; 939105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema bool use_gyro_data; 9418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian bool use_mag_data; 9518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian uint8_t idx; 9618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian}; 9718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 9818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianstruct FusionTask { 9918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian uint32_t tid; 1001be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema uint32_t accelHandle; 1011be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema uint32_t gyroHandle; 1021be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema uint32_t magHandle; 103960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 104960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian struct Fusion fusion; 10518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian struct Fusion game; 106960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 10718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian struct FusionSensor sensors[NUM_OF_FUSION_SENSOR]; 10818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian struct FusionSensorSample samples[NUM_OF_RAW_SENSOR][MAX_NUM_SAMPLES]; 10918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian size_t sample_indices[NUM_OF_RAW_SENSOR]; 11018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian size_t sample_counts[NUM_OF_RAW_SENSOR]; 11118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian uint32_t counters[NUM_OF_RAW_SENSOR]; 11218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian uint64_t ResamplePeriodNs[NUM_OF_RAW_SENSOR]; 11318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian uint64_t last_time[NUM_OF_RAW_SENSOR]; 11418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian struct TripleAxisDataPoint last_sample[NUM_OF_RAW_SENSOR]; 115960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 116960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian uint32_t flags; 117960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 118c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian uint32_t raw_sensor_rate[NUM_OF_RAW_SENSOR]; 119c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian uint64_t raw_sensor_latency; 120c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian 121cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung uint8_t accel_client_cnt; 122cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung uint8_t gyro_client_cnt; 123cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung uint8_t mag_client_cnt; 124960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian}; 125960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 12618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianstatic uint32_t FusionRates[] = { 127960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian SENSOR_HZ(12.5f), 128960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian SENSOR_HZ(25.0f), 129960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian SENSOR_HZ(50.0f), 130960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian SENSOR_HZ(100.0f), 131960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian SENSOR_HZ(200.0f), 132960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 0, 133960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian}; 134960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 135dd2f9808f0045a3eaa42cb488153eeb599eb97ffDmitry Grinbergstatic const uint64_t rateTimerVals[] = //should match "supported rates in length" and be the timer length for that rate in nanosecs 136dd2f9808f0045a3eaa42cb488153eeb599eb97ffDmitry Grinberg{ 137dd2f9808f0045a3eaa42cb488153eeb599eb97ffDmitry Grinberg 1000000000ULL / 12.5f, 138dd2f9808f0045a3eaa42cb488153eeb599eb97ffDmitry Grinberg 1000000000ULL / 25, 139dd2f9808f0045a3eaa42cb488153eeb599eb97ffDmitry Grinberg 1000000000ULL / 50, 140dd2f9808f0045a3eaa42cb488153eeb599eb97ffDmitry Grinberg 1000000000ULL / 100, 141dd2f9808f0045a3eaa42cb488153eeb599eb97ffDmitry Grinberg 1000000000ULL / 200, 142dd2f9808f0045a3eaa42cb488153eeb599eb97ffDmitry Grinberg}; 143dd2f9808f0045a3eaa42cb488153eeb599eb97ffDmitry Grinberg 14418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianstatic struct FusionTask mTask; 1454d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema 1464d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema#define DEC_INFO_RATE(name, rates, type, axis, inter, samples) \ 1474d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema .sensorName = name, \ 1484d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema .supportedRates = rates, \ 1494d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema .sensorType = type, \ 1504d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema .numAxis = axis, \ 1514d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema .interrupt = inter, \ 1524d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema .minSamples = samples 1534d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema 15418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianstatic const struct SensorInfo mSi[NUM_OF_FUSION_SENSOR] = 15564eb1702a319ad4bbc181ed1e5af0703aea7ea22Zhengyin Qian{ 1564d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema { DEC_INFO_RATE("Orientation", FusionRates, SENS_TYPE_ORIENTATION, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 20) }, 1574d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema { DEC_INFO_RATE("Gravity", FusionRates, SENS_TYPE_GRAVITY, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 20) }, 1584d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema { DEC_INFO_RATE("Geomagnetic Rotation Vector", FusionRates, SENS_TYPE_GEO_MAG_ROT_VEC, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 20) }, 1594d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema { DEC_INFO_RATE("Linear Acceleration", FusionRates, SENS_TYPE_LINEAR_ACCEL, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 20) }, 1604d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema { DEC_INFO_RATE("Game Rotation Vector", FusionRates, SENS_TYPE_GAME_ROT_VECTOR, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 300) }, 1614d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema { DEC_INFO_RATE("Rotation Vector", FusionRates, SENS_TYPE_ROTATION_VECTOR, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 20) }, 16264eb1702a319ad4bbc181ed1e5af0703aea7ea22Zhengyin Qian}; 163960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 164e0e5b567748b8d46f60ede2ff12c4b2358ffe0dbZhengyin Qianstatic struct SlabAllocator *mDataSlab; 165e0e5b567748b8d46f60ede2ff12c4b2358ffe0dbZhengyin Qian 166e0e5b567748b8d46f60ede2ff12c4b2358ffe0dbZhengyin Qianstatic void dataEvtFree(void *ptr) 167960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian{ 168e0e5b567748b8d46f60ede2ff12c4b2358ffe0dbZhengyin Qian slabAllocatorFree(mDataSlab, ptr); 169960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian} 170960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 17118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianstatic void fillSamples(struct TripleAxisDataEvent *ev, enum RawSensorType index) 172960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian{ 17318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian bool bad_timestamp; 17418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian size_t i, w, n, num_samples; 17518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian struct TripleAxisDataPoint *curr_sample, *next_sample; 176f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema uint32_t counter; 177f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema uint64_t ResamplePeriodNs, curr_time, next_time; 17818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian uint64_t sample_spacing_ns; 17918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian float weight_next; 18018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 181cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung if (index == GYR && mTask.gyro_client_cnt == 0) { 182960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian return; 183960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 184cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung if (index == MAG && mTask.mag_client_cnt == 0) { 185960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian return; 186960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 187960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 18818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian n = mTask.sample_counts[index]; 18918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian i = mTask.sample_indices[index]; 19018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian counter = mTask.counters[index]; 19118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian ResamplePeriodNs = mTask.ResamplePeriodNs[index]; 19218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian w = (mTask.sample_indices[index] + n) % MAX_NUM_SAMPLES; 19318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 19418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian // check if this sensor was used before 19518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.last_time[index] == ULONG_LONG_MAX) { 19618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian curr_sample = ev->samples; 19718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian next_sample = curr_sample + 1; 198f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema num_samples = ev->samples[0].firstSample.numSamples; 19918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian curr_time = ev->referenceTime; 200960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } else { 20118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian curr_sample = &mTask.last_sample[index]; 20218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian next_sample = ev->samples; 203f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema num_samples = ev->samples[0].firstSample.numSamples + 1; 20418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian curr_time = mTask.last_time[index]; 20518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 206960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 20718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian while (num_samples > 1) { 208960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 20918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (next_sample == ev->samples) 21018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian next_time = ev->referenceTime; 21118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian else 21218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian next_time = curr_time + next_sample->deltaTime; 213960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 21418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian // error handling for non-chronological accel timestamps 21518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian sample_spacing_ns = (next_time > curr_time) ? (next_time - curr_time) : 0; 216960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 21746852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung // This can happen during sensor config changes 21818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian bad_timestamp = (sample_spacing_ns > 10 * ResamplePeriodNs); 219960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 22018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian // Check to see if we need to move the interpolation window or 22118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian // interpolate 22218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if ((counter >= sample_spacing_ns) || bad_timestamp) { 22318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian num_samples--; 22418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian counter -= (bad_timestamp ? counter : sample_spacing_ns); 22518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian curr_sample = next_sample; 22618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian next_sample++; 227960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 228c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian curr_time = next_time; 22918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } else { 230ae081f1f47013ddf08e5af685b9f6d02ac76421bDmitry Grinberg weight_next = (float)counter / floatFromUint64(sample_spacing_ns); 23118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 23218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.samples[index][w].x = curr_sample->x + weight_next * 23318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian (next_sample->x - curr_sample->x); 23418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.samples[index][w].y = curr_sample->y + weight_next * 23518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian (next_sample->y - curr_sample->y); 23618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.samples[index][w].z = curr_sample->z + weight_next * 23718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian (next_sample->z - curr_sample->z); 23818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.samples[index][w].time = curr_time + counter; 23918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 24018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian // Move the read index when buffer is full 24118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (++n > MAX_NUM_SAMPLES) { 24218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian n = MAX_NUM_SAMPLES; 24318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 24418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (++i == MAX_NUM_SAMPLES) { 24518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian i = 0; 24618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 24718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 24818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 24918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian // Reset the write index 25018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (++w == MAX_NUM_SAMPLES) { 25118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian w = 0; 25218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 25318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 25418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian // Move to the next resample 25518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian counter += ResamplePeriodNs; 256960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 257960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 25818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 25918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sample_counts[index] = n; 26018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sample_indices[index] = i; 26118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.counters[index] = counter; 26218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.last_sample[index] = *curr_sample; 26318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.last_time[index] = curr_time; 264960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian} 265960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 26618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianstatic void addSample(struct FusionSensor *mSensor, uint64_t time, float x, float y, float z) 267960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian{ 2687062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema struct TripleAxisDataPoint *sample; 269e0e5b567748b8d46f60ede2ff12c4b2358ffe0dbZhengyin Qian 27018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mSensor->ev == NULL) { 27118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mSensor->ev = slabAllocatorAlloc(mDataSlab); 27218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mSensor->ev == NULL) { 273e0e5b567748b8d46f60ede2ff12c4b2358ffe0dbZhengyin Qian // slaballocation failed 27441f1d787265e609c81b4906df20c725b053b4e94Meng-hsuan Chung osLog(LOG_ERROR, "ORIENTATION: slabAllocatorAlloc() Failed\n"); 275e0e5b567748b8d46f60ede2ff12c4b2358ffe0dbZhengyin Qian return; 276e0e5b567748b8d46f60ede2ff12c4b2358ffe0dbZhengyin Qian } 277f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema mSensor->ev->samples[0].firstSample.numSamples = 0; 27818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mSensor->ev->referenceTime = time; 279c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian mSensor->prev_time = time; 280960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 281960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 282f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema if (mSensor->ev->samples[0].firstSample.numSamples >= MAX_NUM_COMMS_EVENT_SAMPLES) { 28341f1d787265e609c81b4906df20c725b053b4e94Meng-hsuan Chung osLog(LOG_ERROR, "ORIENTATION: BAD_INDEX\n"); 284960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian return; 285960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 286960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 287f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema sample = &mSensor->ev->samples[mSensor->ev->samples[0].firstSample.numSamples++]; 288e0e5b567748b8d46f60ede2ff12c4b2358ffe0dbZhengyin Qian 289c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian if (mSensor->ev->samples[0].firstSample.numSamples > 1) { 29046852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung sample->deltaTime = time > mSensor->prev_time ? (time - mSensor->prev_time) : 0; 291c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian mSensor->prev_time = time; 292c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian } 293960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 294960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian sample->x = x; 295960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian sample->y = y; 296960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian sample->z = z; 297960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 298f45dac303dc3756b80036a68f23cdc48f34d870eBen Fennema if (mSensor->ev->samples[0].firstSample.numSamples == MAX_NUM_COMMS_EVENT_SAMPLES) { 299d4cfbfe80cdb23453458697fb57463d0ab3b3432Alexey Polyudov osEnqueueEvtOrFree(EVENT_TYPE_BIT_DISCARDABLE | sensorGetMyEventType(mSi[mSensor->idx].sensorType), 300d4cfbfe80cdb23453458697fb57463d0ab3b3432Alexey Polyudov mSensor->ev, dataEvtFree); 30118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mSensor->ev = NULL; 302960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 303960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian} 304960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 305960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qianstatic void updateOutput(ssize_t last_accel_sample_index, uint64_t last_sensor_time) 306960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian{ 30718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian struct Vec4 attitude; 30818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian struct Vec3 g, a; 30918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian struct Mat33 R; 31018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 31118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (fusionHasEstimate(&mTask.game)) { 31218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.sensors[GAME].active) { 31318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian fusionGetAttitude(&mTask.game, &attitude); 31418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian addSample(&mTask.sensors[GAME], 31518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian last_sensor_time, 31618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian attitude.x, 31718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian attitude.y, 31818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian attitude.z); 31918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 32018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 32118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.sensors[GRAVITY].active) { 32218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian fusionGetRotationMatrix(&mTask.game, &R); 32318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian initVec3(&g, R.elem[0][2], R.elem[1][2], R.elem[2][2]); 32418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian vec3ScalarMul(&g, kGravityEarth); 32518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian addSample(&mTask.sensors[GRAVITY], 32618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian last_sensor_time, 32718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian g.x, g.y, g.z); 32818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 32918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 33018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 331960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian if (fusionHasEstimate(&mTask.fusion)) { 332960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian fusionGetRotationMatrix(&mTask.fusion, &R); 333960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian fusionGetAttitude(&mTask.fusion, &attitude); 334960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 33518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.sensors[ORIENT].active) { 33618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian // x, y, z = yaw, pitch, roll 33718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian float x = atan2f(-R.elem[0][1], R.elem[0][0]) * kRad2deg; 33818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian float y = atan2f(-R.elem[1][2], R.elem[2][2]) * kRad2deg; 33918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian float z = asinf(R.elem[0][2]) * kRad2deg; 34018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 34118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (x < 0.0f) { 34218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian x += 360.0f; 34318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 34418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 34518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian addSample(&mTask.sensors[ORIENT], 34618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian last_sensor_time, x, y, z); 34718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 348960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 34918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.sensors[GEOMAG].active) { 35018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian addSample(&mTask.sensors[GEOMAG], 35118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian last_sensor_time, 35218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian attitude.x, 35318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian attitude.y, 35418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian attitude.z); 355960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 356960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 35718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.sensors[ROTAT].active) { 35818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian addSample(&mTask.sensors[ROTAT], 35918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian last_sensor_time, 36018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian attitude.x, 36118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian attitude.y, 36218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian attitude.z); 36318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 36418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 36518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (last_accel_sample_index >= 0 36618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian && mTask.sensors[LINEAR].active) { 36718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian initVec3(&g, R.elem[0][2], R.elem[1][2], R.elem[2][2]); 36818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian vec3ScalarMul(&g, kGravityEarth); 36918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian initVec3(&a, 37018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.samples[0][last_accel_sample_index].x, 37118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.samples[0][last_accel_sample_index].y, 37218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.samples[0][last_accel_sample_index].z); 37318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 37418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian addSample(&mTask.sensors[LINEAR], 37518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.samples[0][last_accel_sample_index].time, 37618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian a.x - g.x, 37718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian a.y - g.y, 37818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian a.z - g.z); 37918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 380960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 381960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian} 382960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 383960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qianstatic void drainSamples() 384960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian{ 38518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian size_t i = mTask.sample_indices[ACC]; 386960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian size_t j = 0; 387960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian size_t k = 0; 3881be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema size_t which; 3891be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema struct Vec3 a, w, m; 3901be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema float dT; 3911be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema uint64_t a_time, g_time, m_time; 3921be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema 393cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung if (mTask.gyro_client_cnt > 0) 39418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian j = mTask.sample_indices[GYR]; 3951be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema 396cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung if (mTask.mag_client_cnt > 0) 39718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian k = mTask.sample_indices[MAG]; 3981be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema 39918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian while (mTask.sample_counts[ACC] > 0 400cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung && (!(mTask.gyro_client_cnt > 0) || mTask.sample_counts[GYR] > 0) 401cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung && (!(mTask.mag_client_cnt > 0) || mTask.sample_counts[MAG] > 0)) { 40218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian a_time = mTask.samples[ACC][i].time; 403cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung g_time = mTask.gyro_client_cnt > 0 ? mTask.samples[GYR][j].time 404960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian : ULONG_LONG_MAX; 405cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung m_time = mTask.mag_client_cnt > 0 ? mTask.samples[MAG][k].time 406960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian : ULONG_LONG_MAX; 407960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 408960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian // priority with same timestamp: gyro > acc > mag 409960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian if (g_time <= a_time && g_time <= m_time) { 41018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian which = GYR; 411960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } else if (a_time <= m_time) { 41218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian which = ACC; 413960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } else { 41418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian which = MAG; 415960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 416960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 417a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian dT = floatFromUint64(mTask.ResamplePeriodNs[which]) * 1e-9f; 4181be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema switch (which) { 41918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian case ACC: 42018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian initVec3(&a, mTask.samples[ACC][i].x, mTask.samples[ACC][i].y, mTask.samples[ACC][i].z); 42118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 42218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.flags & FUSION_FLAG_ENABLED) 42318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian fusionHandleAcc(&mTask.fusion, &a, dT); 424960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 42518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.flags & FUSION_FLAG_GAME_ENABLED) 42618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian fusionHandleAcc(&mTask.game, &a, dT); 427960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 42818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian updateOutput(i, mTask.samples[ACC][i].time); 429960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 43018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian --mTask.sample_counts[ACC]; 4311be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema if (++i == MAX_NUM_SAMPLES) 4321be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema i = 0; 4331be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema break; 43418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian case GYR: 43518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian initVec3(&w, mTask.samples[GYR][j].x, mTask.samples[GYR][j].y, mTask.samples[GYR][j].z); 436960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 43718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.flags & FUSION_FLAG_ENABLED) 43818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian fusionHandleGyro(&mTask.fusion, &w, dT); 439960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 44018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.flags & FUSION_FLAG_GAME_ENABLED) 44118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian fusionHandleGyro(&mTask.game, &w, dT); 44218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 44318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian --mTask.sample_counts[GYR]; 4441be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema if (++j == MAX_NUM_SAMPLES) 4451be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema j = 0; 4461be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema break; 44718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian case MAG: 44818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian initVec3(&m, mTask.samples[MAG][k].x, mTask.samples[MAG][k].y, mTask.samples[MAG][k].z); 449960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 4501be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema fusionHandleMag(&mTask.fusion, &m); 451960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 45218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian --mTask.sample_counts[MAG]; 4531be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema if (++k == MAX_NUM_SAMPLES) 4541be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema k = 0; 4551be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema break; 456960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 457960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 458960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 45918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sample_indices[ACC] = i; 460960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 461cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung if (mTask.gyro_client_cnt > 0) 46218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sample_indices[GYR] = j; 463960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 464cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung if (mTask.mag_client_cnt > 0) 46518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sample_indices[MAG] = k; 466960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 46718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++) { 46818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.sensors[i].ev != NULL) { 469d4cfbfe80cdb23453458697fb57463d0ab3b3432Alexey Polyudov osEnqueueEvtOrFree(EVENT_TYPE_BIT_DISCARDABLE | sensorGetMyEventType(mSi[i].sensorType), 470d4cfbfe80cdb23453458697fb57463d0ab3b3432Alexey Polyudov mTask.sensors[i].ev, dataEvtFree); 47118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sensors[i].ev = NULL; 47218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 473960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 474960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian} 475960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 476960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qianstatic void configureFusion() 477960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian{ 47818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.sensors[ORIENT].active 47918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian || mTask.sensors[ROTAT].active 48018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian || mTask.sensors[LINEAR].active 48118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian || mTask.sensors[GEOMAG].active) { 48218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.flags |= FUSION_FLAG_ENABLED; 48318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian initFusion(&mTask.fusion, 484cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung (mTask.mag_client_cnt > 0 ? FUSION_USE_MAG : 0) | 485cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung (mTask.gyro_client_cnt > 0 ? FUSION_USE_GYRO : 0) | 48618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian ((mTask.flags & FUSION_FLAG_INITIALIZED) ? 0 : FUSION_REINITIALIZE)); 48718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.flags |= FUSION_FLAG_INITIALIZED; 48818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } else { 48918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.flags &= ~FUSION_FLAG_ENABLED; 49018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.flags &= ~FUSION_FLAG_INITIALIZED; 49118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 492960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian} 493960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 49418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianstatic void configureGame() 495960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian{ 49618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.sensors[GAME].active || mTask.sensors[GRAVITY].active) { 49718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.flags |= FUSION_FLAG_GAME_ENABLED; 49818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian initFusion(&mTask.game, 49918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian FUSION_USE_GYRO | ((mTask.flags & FUSION_FLAG_INITIALIZED) ? 0 : FUSION_REINITIALIZE)); 50018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.flags |= FUSION_FLAG_GAME_INITIALIZED; 50118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } else { 50218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.flags &= ~FUSION_FLAG_GAME_ENABLED; 50318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.flags &= ~FUSION_FLAG_GAME_INITIALIZED; 504960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 505960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian} 506960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 507c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qianstatic void fusionSetRateAcc(void) 508960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian{ 5091be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema int i; 51018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (mTask.accelHandle == 0) { 51118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sample_counts[ACC] = 0; 51218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sample_indices[ACC] = 0; 51318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.counters[ACC] = 0; 51418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.last_time[ACC] = ULONG_LONG_MAX; 51546852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung for (i = 0; sensorFind(SENS_TYPE_ACCEL, i, &mTask.accelHandle) != NULL; i++) { 516c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian if (sensorRequest(mTask.tid, mTask.accelHandle, mTask.raw_sensor_rate[ACC], mTask.raw_sensor_latency)) { 5171be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema osEventSubscribe(mTask.tid, EVT_SENSOR_ACC_DATA_RDY); 5181be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema break; 5191be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema } 5201be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema } 5211be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema } else { 522c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian sensorRequestRateChange(mTask.tid, mTask.accelHandle, mTask.raw_sensor_rate[ACC], mTask.raw_sensor_latency); 5231be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema } 52418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian} 52518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 526c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qianstatic void fusionSetRateGyr(void) 52718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian{ 52818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian int i; 5291be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema if (mTask.gyroHandle == 0) { 53018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sample_counts[GYR] = 0; 53118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sample_indices[GYR] = 0; 53218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.counters[GYR] = 0; 53318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.last_time[GYR] = ULONG_LONG_MAX; 53446852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung for (i = 0; sensorFind(SENS_TYPE_GYRO, i, &mTask.gyroHandle) != NULL; i++) { 535c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian if (sensorRequest(mTask.tid, mTask.gyroHandle, mTask.raw_sensor_rate[GYR], mTask.raw_sensor_latency)) { 5361be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema osEventSubscribe(mTask.tid, EVT_SENSOR_GYR_DATA_RDY); 5371be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema break; 5381be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema } 5391be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema } 5401be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema } else { 541c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian sensorRequestRateChange(mTask.tid, mTask.gyroHandle, mTask.raw_sensor_rate[GYR], mTask.raw_sensor_latency); 5421be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema } 54318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian} 54418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 545c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qianstatic void fusionSetRateMag(void) 54618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian{ 54718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian int i; 5481be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema if (mTask.magHandle == 0) { 54918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sample_counts[MAG] = 0; 55018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sample_indices[MAG] = 0; 55118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.counters[MAG] = 0; 55218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.last_time[MAG] = ULONG_LONG_MAX; 55346852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung for (i = 0; sensorFind(SENS_TYPE_MAG, i, &mTask.magHandle) != NULL; i++) { 554c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian if (sensorRequest(mTask.tid, mTask.magHandle, mTask.raw_sensor_rate[MAG], mTask.raw_sensor_latency)) { 5551be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema osEventSubscribe(mTask.tid, EVT_SENSOR_MAG_DATA_RDY); 5561be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema break; 5571be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema } 5581be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema } 5591be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema } else { 560c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian sensorRequestRateChange(mTask.tid, mTask.magHandle, mTask.raw_sensor_rate[MAG], mTask.raw_sensor_latency); 561c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian } 562c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian} 563c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian 564e723815b378ee08afad27d0fe84253e22c050528Ben Fennemastatic bool fusionSetRate(uint32_t rate, uint64_t latency, void *cookie) 565c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian{ 566e723815b378ee08afad27d0fe84253e22c050528Ben Fennema struct FusionSensor *mSensor = &mTask.sensors[(int)cookie]; 567c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian int i; 568c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian uint32_t max_rate = 0; 569c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian uint32_t gyr_rate, mag_rate; 57046852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung uint64_t min_resample_period = ULONG_LONG_MAX; 571c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian 572c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian mSensor->rate = rate; 573c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian mSensor->latency = latency; 574c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian 575c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++) { 576c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian if (mTask.sensors[i].active) { 577c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian max_rate = max_rate > mTask.sensors[i].rate ? max_rate : mTask.sensors[i].rate; 578c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian } 579c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian } 580c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian 581cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung if (mTask.accel_client_cnt > 0) { 582c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian mTask.raw_sensor_rate[ACC] = max_rate; 58346852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung mTask.ResamplePeriodNs[ACC] = sensorTimerLookupCommon(FusionRates, rateTimerVals, max_rate); 58446852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung min_resample_period = mTask.ResamplePeriodNs[ACC] < min_resample_period ? 58546852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung mTask.ResamplePeriodNs[ACC] : min_resample_period; 586c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian } 587c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian 588cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung if (mTask.gyro_client_cnt > 0) { 5899105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema gyr_rate = max_rate > MIN_GYRO_RATE_HZ ? max_rate : MIN_GYRO_RATE_HZ; 590c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian mTask.raw_sensor_rate[GYR] = gyr_rate; 59146852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung mTask.ResamplePeriodNs[GYR] = sensorTimerLookupCommon(FusionRates, rateTimerVals, gyr_rate); 59246852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung min_resample_period = mTask.ResamplePeriodNs[GYR] < min_resample_period ? 59346852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung mTask.ResamplePeriodNs[GYR] : min_resample_period; 594c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian } 595c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian 596cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung if (mTask.mag_client_cnt > 0) { 5979105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema mag_rate = max_rate < MAX_MAG_RATE_HZ ? max_rate : MAX_MAG_RATE_HZ; 598c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian mTask.raw_sensor_rate[MAG] = mag_rate; 59946852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung mTask.ResamplePeriodNs[MAG] = sensorTimerLookupCommon(FusionRates, rateTimerVals, mag_rate); 60046852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung min_resample_period = mTask.ResamplePeriodNs[MAG] < min_resample_period ? 60146852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung mTask.ResamplePeriodNs[MAG] : min_resample_period; 602c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian } 603c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian 60446852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung // This guarantees that local raw sensor FIFOs won't overflow. 60546852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung mTask.raw_sensor_latency = min_resample_period * (FIFO_DEPTH - 1); 60646852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung 607c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++) { 608c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian if (mTask.sensors[i].active) { 609c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian mTask.raw_sensor_latency = mTask.sensors[i].latency < mTask.raw_sensor_latency ? 610c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian mTask.sensors[i].latency : mTask.raw_sensor_latency; 611c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian } 61218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 613c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian 614cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung if (mTask.accel_client_cnt > 0) 615c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian fusionSetRateAcc(); 616cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung if (mTask.gyro_client_cnt > 0) 617c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian fusionSetRateGyr(); 618cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung if (mTask.mag_client_cnt > 0) 619c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian fusionSetRateMag(); 620c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian if (mSensor->rate > 0) 621c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency); 622c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian 623c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian return true; 62418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian} 62518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 626e723815b378ee08afad27d0fe84253e22c050528Ben Fennemastatic bool fusionPower(bool on, void *cookie) 62718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian{ 628e723815b378ee08afad27d0fe84253e22c050528Ben Fennema struct FusionSensor *mSensor = &mTask.sensors[(int)cookie]; 629e723815b378ee08afad27d0fe84253e22c050528Ben Fennema int idx; 630e723815b378ee08afad27d0fe84253e22c050528Ben Fennema 63118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mSensor->active = on; 63218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian if (on == false) { 633cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung mTask.accel_client_cnt--; 6349105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema if (mSensor->use_gyro_data) 635cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung mTask.gyro_client_cnt--; 6369105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema if (mSensor->use_mag_data) 637cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung mTask.mag_client_cnt--; 63818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 639cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung // if client_cnt == 0 and Handle == 0, nothing need to be done. 640cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung // if client_cnt > 0 and Handle == 0, something else is turning it on, all will be done. 641cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung if (mTask.accel_client_cnt == 0 && mTask.accelHandle != 0) { 64218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian sensorRelease(mTask.tid, mTask.accelHandle); 64318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.accelHandle = 0; 64418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian osEventUnsubscribe(mTask.tid, EVT_SENSOR_ACC_DATA_RDY); 64518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 64618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 647cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung if (mTask.gyro_client_cnt == 0 && mTask.gyroHandle != 0) { 64818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian sensorRelease(mTask.tid, mTask.gyroHandle); 64918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.gyroHandle = 0; 65018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian osEventUnsubscribe(mTask.tid, EVT_SENSOR_GYR_DATA_RDY); 65118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 65218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 653cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung if (mTask.mag_client_cnt == 0 && mTask.magHandle != 0) { 65418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian sensorRelease(mTask.tid, mTask.magHandle); 65518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.magHandle = 0; 65618040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian osEventUnsubscribe(mTask.tid, EVT_SENSOR_MAG_DATA_RDY); 65718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 658c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian 659e723815b378ee08afad27d0fe84253e22c050528Ben Fennema idx = mSensor->idx; 660e723815b378ee08afad27d0fe84253e22c050528Ben Fennema (void) fusionSetRate(0, ULONG_LONG_MAX, (void *)idx); 66118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } else { 662cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung mTask.accel_client_cnt++; 6639105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema if (mSensor->use_gyro_data) 664cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung mTask.gyro_client_cnt++; 6659105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema if (mSensor->use_mag_data) 666cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung mTask.mag_client_cnt++; 6671be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema } 6681be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema 66918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian configureFusion(); 67018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian configureGame(); 67118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, on, 0); 672960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 673960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian return true; 674960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian} 675960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 676e723815b378ee08afad27d0fe84253e22c050528Ben Fennemastatic bool fusionFirmwareUpload(void *cookie) 677960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian{ 678e723815b378ee08afad27d0fe84253e22c050528Ben Fennema struct FusionSensor *mSensor = &mTask.sensors[(int)cookie]; 679960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 68018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0); 68118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian return true; 68218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian} 68318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 684e723815b378ee08afad27d0fe84253e22c050528Ben Fennemastatic bool fusionFlush(void *cookie) 68518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian{ 686e723815b378ee08afad27d0fe84253e22c050528Ben Fennema struct FusionSensor *mSensor = &mTask.sensors[(int)cookie]; 68718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian uint32_t evtType = sensorGetMyEventType(mSi[mSensor->idx].sensorType); 688e723815b378ee08afad27d0fe84253e22c050528Ben Fennema 68918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian osEnqueueEvt(evtType, SENSOR_DATA_EVENT_FLUSH, NULL); 69018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian return true; 69118040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian} 69218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian 69318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianstatic void fusionHandleEvent(uint32_t evtType, const void* evtData) 694960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian{ 6957062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema struct TripleAxisDataEvent *ev; 6969105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema int i; 697960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 6980bcb1a3baec93492e58afff27f2ef92afcbf1698Ben Fennema if (evtData == SENSOR_DATA_EVENT_FLUSH) 6990bcb1a3baec93492e58afff27f2ef92afcbf1698Ben Fennema return; 7000bcb1a3baec93492e58afff27f2ef92afcbf1698Ben Fennema 701960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian switch (evtType) { 7029105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema case EVT_APP_START: 7039105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema // check for gyro and mag 7049105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema osEventUnsubscribe(mTask.tid, EVT_APP_START); 7059105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema if (!sensorFind(SENS_TYPE_GYRO, 0, &mTask.gyroHandle)) { 7069105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++) 7079105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema mTask.sensors[i].use_gyro_data = false; 7089105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema } 7099105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema mTask.gyroHandle = 0; 7109105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema if (!sensorFind(SENS_TYPE_MAG, 0, &mTask.magHandle)) { 7119105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++) 7129105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema mTask.sensors[i].use_mag_data = false; 7139105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema } 7149105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema mTask.magHandle = 0; 7159105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema break; 7161be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema case EVT_SENSOR_ACC_DATA_RDY: 7171be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema ev = (struct TripleAxisDataEvent *)evtData; 71818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian fillSamples(ev, ACC); 7191be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema drainSamples(); 7201be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema break; 7211be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema case EVT_SENSOR_GYR_DATA_RDY: 7221be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema ev = (struct TripleAxisDataEvent *)evtData; 72318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian fillSamples(ev, GYR); 7241be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema drainSamples(); 7251be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema break; 7261be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema case EVT_SENSOR_MAG_DATA_RDY: 7271be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema ev = (struct TripleAxisDataEvent *)evtData; 72818040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian fillSamples(ev, MAG); 7291be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema drainSamples(); 7301be10b6751f50675307716747bd4d729d9e7aff5Ben Fennema break; 731960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 732960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian} 733960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 734e723815b378ee08afad27d0fe84253e22c050528Ben Fennemastatic const struct SensorOps mSops = 73564eb1702a319ad4bbc181ed1e5af0703aea7ea22Zhengyin Qian{ 7364d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema .sensorPower = fusionPower, 7374d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema .sensorFirmwareUpload = fusionFirmwareUpload, 7384d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema .sensorSetRate = fusionSetRate, 7394d17cb52d252591db12cee09b08eed4ee173b939Ben Fennema .sensorFlush = fusionFlush, 74064eb1702a319ad4bbc181ed1e5af0703aea7ea22Zhengyin Qian}; 74164eb1702a319ad4bbc181ed1e5af0703aea7ea22Zhengyin Qian 74218040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianstatic bool fusionStart(uint32_t tid) 743960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian{ 744960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian osLog(LOG_INFO, " ORIENTATION: %ld\n", tid); 745e0e5b567748b8d46f60ede2ff12c4b2358ffe0dbZhengyin Qian size_t i, slabSize; 746960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 747960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian mTask.tid = tid; 748960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian mTask.flags = 0; 749960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 75018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian for (i = 0; i < NUM_OF_RAW_SENSOR; i++) { 751960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian mTask.sample_counts[i] = 0; 752960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian mTask.sample_indices[i] = 0; 753960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian } 754960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 75518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++) { 756e723815b378ee08afad27d0fe84253e22c050528Ben Fennema mTask.sensors[i].handle = sensorRegister(&mSi[i], &mSops, (void *)i, true); 75718040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sensors[i].idx = i; 7589105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema mTask.sensors[i].use_gyro_data = true; 75918040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sensors[i].use_mag_data = true; 76018040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian } 761960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 7629105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema mTask.sensors[GEOMAG].use_gyro_data = false; 76318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sensors[GAME].use_mag_data = false; 76418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian mTask.sensors[GRAVITY].use_mag_data = false; 765960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 766cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung mTask.accel_client_cnt = 0; 767cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung mTask.gyro_client_cnt = 0; 768cb2d2403d5ad1a58e85b4296c96bd8ac1a1055ecMeng-hsuan Chung mTask.mag_client_cnt = 0; 76946852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung 7707062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema slabSize = sizeof(struct TripleAxisDataEvent) 7717062239aaa63d8a5260d7d325483755acbb8c35cBen Fennema + MAX_NUM_COMMS_EVENT_SAMPLES * sizeof(struct TripleAxisDataPoint); 77241f1d787265e609c81b4906df20c725b053b4e94Meng-hsuan Chung mDataSlab = slabAllocatorNew(slabSize, 4, 6 * (NUM_COMMS_EVENTS_IN_FIFO + 1)); // worst case 6 output sensors * (N + 1) comms_events 773c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian if (!mDataSlab) { 77441f1d787265e609c81b4906df20c725b053b4e94Meng-hsuan Chung osLog(LOG_ERROR, "ORIENTATION: slabAllocatorNew() FAILED\n"); 775c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian return false; 776c900c013a76aa73bd612da12000c76e0d4169595Zhengyin Qian } 777e0e5b567748b8d46f60ede2ff12c4b2358ffe0dbZhengyin Qian 7789105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema osEventSubscribe(mTask.tid, EVT_APP_START); 7799105ba155c563959955fa3b4ebe49809025c1df3Ben Fennema 780960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian return true; 781960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian} 782960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 78318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qianstatic void fusionEnd() 784960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian{ 785960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian mTask.flags &= ~FUSION_FLAG_INITIALIZED; 78646852dd4246dcd6b44de1e5ab294f516d7a95f18Meng-hsuan Chung mTask.flags &= ~FUSION_FLAG_GAME_INITIALIZED; 787e0e5b567748b8d46f60ede2ff12c4b2358ffe0dbZhengyin Qian slabAllocatorDestroy(mDataSlab); 788960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian} 789960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin Qian 790960c2ec577b48937271d3d951bfbc66b070283a1Zhengyin QianINTERNAL_APP_INIT( 791dca61fcfcc85a8240bc46a650c71008ded2d32ddBen Fennema APP_ID_MAKE(APP_ID_VENDOR_GOOGLE, 4), 792cc77708f80db23714a92638ddee64f59dbdf5483Dmitry Grinberg 0, 79318040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian fusionStart, 79418040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian fusionEnd, 79518040c9213658bbcc2ccc9e6cdb654d7a228eb66Zhengyin Qian fusionHandleEvent); 796