xs-android13
/
device
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
device/google/contexthub/firmware/os/drivers/orientation/orientation.c

889 lines
28 KiB
C
Raw Blame History

/*
* Copyright (C) 2016 The Android Open Source Project
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include <stdlib.h>
#include <string.h>
#include <timer.h>
#include <heap.h>
#include <plat/rtc.h>
#include <plat/syscfg.h>
#include <hostIntf.h>
#include <nanohubPacket.h>
#include <floatRt.h>
#include <seos.h>
#include <nanohub_math.h>
#include <algos/fusion.h>
#include <sensors.h>
#include <variant/sensType.h>
#include <limits.h>
#include <slab.h>
#define ORIENTATION_APP_VERSION 1
#define MAX_NUM_COMMS_EVENT_SAMPLES 15 // at most 15 samples can fit in one comms_event
#define NUM_COMMS_EVENTS_IN_FIFO 2 // This controls how often the hub needs to wake up
// in batching
// needs to be greater than max raw sensor rate ratio
#define FIFO_DEPTH (NUM_COMMS_EVENTS_IN_FIFO * MAX_NUM_COMMS_EVENT_SAMPLES)
/*
* FIFO_MARGIN: max raw sensor rate ratio is 8:1.
* If 2 batchs of high rate data comes before 1 low rate data, there can be at max 15 samples left
* in the FIFO
*/
#define FIFO_MARGIN 15
#define MAX_NUM_SAMPLES (FIFO_MARGIN + FIFO_DEPTH) // actual input sample fifo depth
#define EVT_SENSOR_ACC_DATA_RDY sensorGetMyEventType(SENS_TYPE_ACCEL)
#define EVT_SENSOR_GYR_DATA_RDY sensorGetMyEventType(SENS_TYPE_GYRO)
#define EVT_SENSOR_MAG_DATA_RDY sensorGetMyEventType(SENS_TYPE_MAG)
#define EVT_SENSOR_MAG_BIAS sensorGetMyEventType(SENS_TYPE_MAG_BIAS)
#define kGravityEarth 9.80665f
#define kRad2deg (180.0f / M_PI)
#define MIN_GYRO_RATE_HZ SENSOR_HZ(100.0f)
#define MAX_MAG_RATE_HZ SENSOR_HZ(50.0f)
enum
{
FUSION_FLAG_ENABLED = 0x01,
FUSION_FLAG_INITIALIZED = 0x08,
FUSION_FLAG_GAME_ENABLED = 0x10,
FUSION_FLAG_GAME_INITIALIZED = 0x20
};
enum RawSensorType
{
ACC,
GYR,
MAG,
NUM_OF_RAW_SENSOR
};
enum FusionSensorType
{
ORIENT,
GRAVITY,
GEOMAG,
LINEAR,
GAME,
ROTAT,
NUM_OF_FUSION_SENSOR
};
struct FusionSensorSample {
uint64_t time;
float x, y, z;
};
struct FusionSensor {
uint32_t handle;
struct TripleAxisDataEvent *ev;
uint64_t prev_time;
uint64_t latency;
uint32_t rate;
bool active;
bool use_gyro_data;
bool use_mag_data;
uint8_t idx;
};
struct FusionTask {
uint32_t tid;
uint32_t accelHandle;
uint32_t gyroHandle;
uint32_t magHandle;
struct Fusion fusion;
struct Fusion game;
struct FusionSensor sensors[NUM_OF_FUSION_SENSOR];
struct FusionSensorSample samples[NUM_OF_RAW_SENSOR][MAX_NUM_SAMPLES];
size_t sample_indices[NUM_OF_RAW_SENSOR];
size_t sample_counts[NUM_OF_RAW_SENSOR];
uint32_t counters[NUM_OF_RAW_SENSOR];
uint64_t ResamplePeriodNs[NUM_OF_RAW_SENSOR];
uint64_t last_time[NUM_OF_RAW_SENSOR];
struct TripleAxisDataPoint last_sample[NUM_OF_RAW_SENSOR];
uint32_t flags;
uint32_t raw_sensor_rate[NUM_OF_RAW_SENSOR];
uint64_t raw_sensor_latency;
uint8_t accel_client_cnt;
uint8_t gyro_client_cnt;
uint8_t mag_client_cnt;
};
static uint32_t FusionRates[] = {
SENSOR_HZ(12.5f),
SENSOR_HZ(25.0f),
SENSOR_HZ(50.0f),
SENSOR_HZ(100.0f),
SENSOR_HZ(200.0f),
0,
};
//should match "supported rates in length" and be the timer length for that rate in nanosecs
static const uint64_t rateTimerVals[] = {
1000000000ULL / 12.5f,
1000000000ULL / 25,
1000000000ULL / 50,
1000000000ULL / 100,
1000000000ULL / 200,
};
static struct FusionTask mTask;
#define DEC_INFO_RATE(name, rates, type, axis, inter, samples) \
.sensorName = name, \
.supportedRates = rates, \
.sensorType = type, \
.numAxis = axis, \
.interrupt = inter, \
.minSamples = samples
static const struct SensorInfo mSi[NUM_OF_FUSION_SENSOR] =
{
{ DEC_INFO_RATE("Orientation", FusionRates, SENS_TYPE_ORIENTATION, NUM_AXIS_THREE,
NANOHUB_INT_NONWAKEUP, 20) },
{ DEC_INFO_RATE("Gravity", FusionRates, SENS_TYPE_GRAVITY, NUM_AXIS_THREE,
NANOHUB_INT_NONWAKEUP, 20) },
{ DEC_INFO_RATE("Geomagnetic Rotation Vector", FusionRates, SENS_TYPE_GEO_MAG_ROT_VEC,
NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 20) },
{ DEC_INFO_RATE("Linear Acceleration", FusionRates, SENS_TYPE_LINEAR_ACCEL, NUM_AXIS_THREE,
NANOHUB_INT_NONWAKEUP, 20) },
{ DEC_INFO_RATE("Game Rotation Vector", FusionRates, SENS_TYPE_GAME_ROT_VECTOR, NUM_AXIS_THREE,
NANOHUB_INT_NONWAKEUP, 300) },
{ DEC_INFO_RATE("Rotation Vector", FusionRates, SENS_TYPE_ROTATION_VECTOR, NUM_AXIS_THREE,
NANOHUB_INT_NONWAKEUP, 20) },
};
static struct SlabAllocator *mDataSlab;
static void dataEvtFree(void *ptr)
{
slabAllocatorFree(mDataSlab, ptr);
}
static void fillSamples(struct TripleAxisDataEvent *ev, enum RawSensorType index)
{
bool bad_timestamp;
size_t i, w, n, num_samples;
struct TripleAxisDataPoint *curr_sample, *next_sample;
uint32_t counter;
uint64_t ResamplePeriodNs, curr_time, next_time;
uint64_t sample_spacing_ns;
float weight_next;
if (index == GYR && mTask.gyro_client_cnt == 0) {
return;
}
if (index == MAG && mTask.mag_client_cnt == 0) {
return;
}
n = mTask.sample_counts[index];
i = mTask.sample_indices[index];
counter = mTask.counters[index];
ResamplePeriodNs = mTask.ResamplePeriodNs[index];
w = (mTask.sample_indices[index] + n) % MAX_NUM_SAMPLES;
// check if this sensor was used before
if (mTask.last_time[index] == ULONG_LONG_MAX) {
curr_sample = ev->samples;
next_sample = curr_sample + 1;
num_samples = ev->samples[0].firstSample.numSamples;
curr_time = ev->referenceTime;
} else {
curr_sample = &mTask.last_sample[index];
next_sample = ev->samples;
num_samples = ev->samples[0].firstSample.numSamples + 1;
curr_time = mTask.last_time[index];
}
while (num_samples > 1) {
if (next_sample == ev->samples)
next_time = ev->referenceTime;
else
next_time = curr_time + next_sample->deltaTime;
// error handling for non-chronological accel timestamps
sample_spacing_ns = (next_time > curr_time) ? (next_time - curr_time) : 0;
// This can happen during sensor config changes
bad_timestamp = (sample_spacing_ns > 10 * ResamplePeriodNs);
// Check to see if we need to move the interpolation window or
// interpolate
if ((counter >= sample_spacing_ns) || bad_timestamp) {
num_samples--;
counter -= (bad_timestamp ? counter : sample_spacing_ns);
curr_sample = next_sample;
next_sample++;
curr_time = next_time;
} else {
weight_next = (float)counter / floatFromUint64(sample_spacing_ns);
mTask.samples[index][w].x = curr_sample->x + weight_next *
(next_sample->x - curr_sample->x);
mTask.samples[index][w].y = curr_sample->y + weight_next *
(next_sample->y - curr_sample->y);
mTask.samples[index][w].z = curr_sample->z + weight_next *
(next_sample->z - curr_sample->z);
mTask.samples[index][w].time = curr_time + counter;
// Move the read index when buffer is full
if (++n > MAX_NUM_SAMPLES) {
n = MAX_NUM_SAMPLES;
if (++i == MAX_NUM_SAMPLES) {
i = 0;
}
}
// Reset the write index
if (++w == MAX_NUM_SAMPLES) {
w = 0;
}
// Move to the next resample
counter += ResamplePeriodNs;
}
}
mTask.sample_counts[index] = n;
mTask.sample_indices[index] = i;
mTask.counters[index] = counter;
mTask.last_sample[index] = *curr_sample;
mTask.last_time[index] = curr_time;
}
static bool allocateDataEvt(struct FusionSensor *mSensor, uint64_t time)
{
mSensor->ev = slabAllocatorAlloc(mDataSlab);
if (mSensor->ev == NULL) {
// slab allocation failed, need to stop draining raw samples for now.
osLog(LOG_INFO, "ORIENTATION: slabAllocatorAlloc() Failed\n");
return false;
}
// delta time for the first sample is sample count
memset(&mSensor->ev->samples[0].firstSample, 0x00, sizeof(struct SensorFirstSample));
mSensor->ev->referenceTime = time;
mSensor->prev_time = time;
return true;
}
// returns false if addSample() fails
static bool addSample(struct FusionSensor *mSensor, uint64_t time, float x, float y, float z)
{
struct TripleAxisDataPoint *sample;
// Bypass processing this accel sample.
// This is needed after recovering from a slab shortage.
if (mSensor->prev_time == time) {
osLog(LOG_INFO, "Accel sample has been processed by fusion sensor %d\n",
mSensor->idx);
return true;
}
if (mSensor->ev == NULL) {
if (!allocateDataEvt(mSensor, time))
return false;
}
if (mSensor->ev->samples[0].firstSample.numSamples >= MAX_NUM_COMMS_EVENT_SAMPLES) {
osLog(LOG_ERROR, "ORIENTATION: BAD_INDEX\n");
return false;
}
sample = &mSensor->ev->samples[mSensor->ev->samples[0].firstSample.numSamples++];
if (mSensor->ev->samples[0].firstSample.numSamples > 1) {
sample->deltaTime = time > mSensor->prev_time ? (time - mSensor->prev_time) : 0;
mSensor->prev_time = time;
}
sample->x = x;
sample->y = y;
sample->z = z;
if (mSensor->ev->samples[0].firstSample.numSamples == MAX_NUM_COMMS_EVENT_SAMPLES) {
osEnqueueEvtOrFree(
EVENT_TYPE_BIT_DISCARDABLE | sensorGetMyEventType(mSi[mSensor->idx].sensorType),
mSensor->ev, dataEvtFree);
mSensor->ev = NULL;
}
return true;
}
// returns false if addSample fails for any fusion sensor
// (most likely due to slab allocation failure)
static bool updateOutput(ssize_t last_accel_sample_index, uint64_t last_sensor_time)
{
struct Vec4 attitude;
struct Vec3 g, a;
struct Mat33 R; // direction-cosine/rotation matrix, inertial -> device
bool rInited; // indicates if matrix R has been initialzed. for avoiding repeated computation
bool ret = true;
if (fusionHasEstimate(&mTask.game)) {
rInited = false;
if (mTask.sensors[GAME].active) {
fusionGetAttitude(&mTask.game, &attitude);
if (!addSample(&mTask.sensors[GAME],
last_sensor_time,
attitude.x,
attitude.y,
attitude.z)) {
ret = false;
}
}
if (mTask.sensors[GRAVITY].active) {
fusionGetRotationMatrix(&mTask.game, &R);
rInited = true;
initVec3(&g, R.elem[0][2], R.elem[1][2], R.elem[2][2]);
vec3ScalarMul(&g, kGravityEarth);
if (!addSample(&mTask.sensors[GRAVITY],
last_sensor_time,
g.x,
g.y,
g.z)) {
ret = false;
}
}
if (last_accel_sample_index >= 0
&& mTask.sensors[LINEAR].active) {
if (!rInited) {
fusionGetRotationMatrix(&mTask.game, &R);
}
initVec3(&g, R.elem[0][2], R.elem[1][2], R.elem[2][2]);
vec3ScalarMul(&g, kGravityEarth);
initVec3(&a,
mTask.samples[0][last_accel_sample_index].x,
mTask.samples[0][last_accel_sample_index].y,
mTask.samples[0][last_accel_sample_index].z);
if (!addSample(&mTask.sensors[LINEAR],
mTask.samples[0][last_accel_sample_index].time,
a.x - g.x,
a.y - g.y,
a.z - g.z)) {
ret = false;
}
}
}
if (fusionHasEstimate(&mTask.fusion)) {
fusionGetAttitude(&mTask.fusion, &attitude);
if (mTask.sensors[ORIENT].active) {
fusionGetRotationMatrix(&mTask.fusion, &R);
// x, y, z = yaw, pitch, roll
float x = atan2f(-R.elem[0][1], R.elem[0][0]) * kRad2deg;
float y = atan2f(-R.elem[1][2], R.elem[2][2]) * kRad2deg;
float z = asinf(R.elem[0][2]) * kRad2deg;
if (x < 0.0f) {
x += 360.0f;
}
if (!addSample(&mTask.sensors[ORIENT],
last_sensor_time,
x,
y,
z)) {
ret = false;
}
}
if (mTask.sensors[GEOMAG].active) {
if (!addSample(&mTask.sensors[GEOMAG],
last_sensor_time,
attitude.x,
attitude.y,
attitude.z)) {
ret = false;
}
}
if (mTask.sensors[ROTAT].active) {
if (!addSample(&mTask.sensors[ROTAT],
last_sensor_time,
attitude.x,
attitude.y,
attitude.z)) {
ret = false;
}
}
}
return ret;
}
static void drainSamples()
{
struct Vec3 a, w, m;
uint64_t a_time, g_time, m_time;
size_t i = mTask.sample_indices[ACC];
size_t j = 0;
size_t k = 0;
size_t which;
float dT;
bool success = true;
if (mTask.gyro_client_cnt > 0)
j = mTask.sample_indices[GYR];
if (mTask.mag_client_cnt > 0)
k = mTask.sample_indices[MAG];
// Keep draining raw samples and producing fusion samples only if
// 1) all raw sensors needed are present (to compare timestamp) and
// 2) updateOutput() succeeded (no slab shortage)
// Otherwise, wait till next raw sample event.
while (mTask.sample_counts[ACC] > 0
&& (!(mTask.gyro_client_cnt > 0) || mTask.sample_counts[GYR] > 0)
&& (!(mTask.mag_client_cnt > 0) || mTask.sample_counts[MAG] > 0)
&& success) {
a_time = mTask.samples[ACC][i].time;
g_time = mTask.gyro_client_cnt > 0 ? mTask.samples[GYR][j].time
: ULONG_LONG_MAX;
m_time = mTask.mag_client_cnt > 0 ? mTask.samples[MAG][k].time
: ULONG_LONG_MAX;
// priority with same timestamp: gyro > acc > mag
if (g_time <= a_time && g_time <= m_time) {
which = GYR;
} else if (a_time <= m_time) {
which = ACC;
} else {
which = MAG;
}
dT = floatFromUint64(mTask.ResamplePeriodNs[which]) * 1e-9f;
switch (which) {
case ACC:
initVec3(&a, mTask.samples[ACC][i].x, mTask.samples[ACC][i].y, mTask.samples[ACC][i].z);
if (mTask.flags & FUSION_FLAG_ENABLED)
fusionHandleAcc(&mTask.fusion, &a, dT);
if (mTask.flags & FUSION_FLAG_GAME_ENABLED)
fusionHandleAcc(&mTask.game, &a, dT);
success = updateOutput(i, mTask.samples[ACC][i].time);
// Do not remove the accel sample until all active fusion sesnsors
// successfully updated the output.
// Fusion sensors that have processed this accel sample will bypass
// it in addSample().
if (success) {
--mTask.sample_counts[ACC];
if (++i == MAX_NUM_SAMPLES) {
i = 0;
}
}
break;
case GYR:
initVec3(&w, mTask.samples[GYR][j].x, mTask.samples[GYR][j].y, mTask.samples[GYR][j].z);
if (mTask.flags & FUSION_FLAG_ENABLED)
fusionHandleGyro(&mTask.fusion, &w, dT);
if (mTask.flags & FUSION_FLAG_GAME_ENABLED)
fusionHandleGyro(&mTask.game, &w, dT);
--mTask.sample_counts[GYR];
if (++j == MAX_NUM_SAMPLES)
j = 0;
break;
case MAG:
initVec3(&m, mTask.samples[MAG][k].x, mTask.samples[MAG][k].y, mTask.samples[MAG][k].z);
fusionHandleMag(&mTask.fusion, &m, dT);
--mTask.sample_counts[MAG];
if (++k == MAX_NUM_SAMPLES)
k = 0;
break;
}
}
mTask.sample_indices[ACC] = i;
if (mTask.gyro_client_cnt > 0)
mTask.sample_indices[GYR] = j;
if (mTask.mag_client_cnt > 0)
mTask.sample_indices[MAG] = k;
for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++) {
if (mTask.sensors[i].ev != NULL) {
osEnqueueEvtOrFree(EVENT_TYPE_BIT_DISCARDABLE | sensorGetMyEventType(mSi[i].sensorType),
mTask.sensors[i].ev, dataEvtFree);
mTask.sensors[i].ev = NULL;
}
}
}
static void configureFusion()
{
if (mTask.sensors[ORIENT].active
|| mTask.sensors[ROTAT].active
|| mTask.sensors[GEOMAG].active) {
mTask.flags |= FUSION_FLAG_ENABLED;
initFusion(&mTask.fusion,
(mTask.mag_client_cnt > 0 ? FUSION_USE_MAG : 0) |
(mTask.gyro_client_cnt > 0 ? FUSION_USE_GYRO : 0) |
((mTask.flags & FUSION_FLAG_INITIALIZED) ? 0 : FUSION_REINITIALIZE));
mTask.flags |= FUSION_FLAG_INITIALIZED;
} else {
mTask.flags &= ~FUSION_FLAG_ENABLED;
mTask.flags &= ~FUSION_FLAG_INITIALIZED;
}
}
static void configureGame()
{
if (mTask.sensors[GAME].active || mTask.sensors[GRAVITY].active ||
mTask.sensors[LINEAR].active) {
mTask.flags |= FUSION_FLAG_GAME_ENABLED;
initFusion(&mTask.game, FUSION_USE_GYRO |
((mTask.flags & FUSION_FLAG_INITIALIZED) ? 0 : FUSION_REINITIALIZE));
mTask.flags |= FUSION_FLAG_GAME_INITIALIZED;
} else {
mTask.flags &= ~FUSION_FLAG_GAME_ENABLED;
mTask.flags &= ~FUSION_FLAG_GAME_INITIALIZED;
}
}
static void fusionSetRateAcc(void)
{
int i;
if (mTask.accelHandle == 0) {
mTask.sample_counts[ACC] = 0;
mTask.sample_indices[ACC] = 0;
mTask.counters[ACC] = 0;
mTask.last_time[ACC] = ULONG_LONG_MAX;
for (i = 0; sensorFind(SENS_TYPE_ACCEL, i, &mTask.accelHandle) != NULL; i++) {
if (sensorRequest(mTask.tid, mTask.accelHandle, mTask.raw_sensor_rate[ACC],
mTask.raw_sensor_latency)) {
osEventSubscribe(mTask.tid, EVT_SENSOR_ACC_DATA_RDY);
break;
}
}
} else {
sensorRequestRateChange(mTask.tid, mTask.accelHandle, mTask.raw_sensor_rate[ACC],
mTask.raw_sensor_latency);
}
}
static void fusionSetRateGyr(void)
{
int i;
if (mTask.gyroHandle == 0) {
mTask.sample_counts[GYR] = 0;
mTask.sample_indices[GYR] = 0;
mTask.counters[GYR] = 0;
mTask.last_time[GYR] = ULONG_LONG_MAX;
for (i = 0; sensorFind(SENS_TYPE_GYRO, i, &mTask.gyroHandle) != NULL; i++) {
if (sensorRequest(mTask.tid, mTask.gyroHandle, mTask.raw_sensor_rate[GYR],
mTask.raw_sensor_latency)) {
osEventSubscribe(mTask.tid, EVT_SENSOR_GYR_DATA_RDY);
break;
}
}
} else {
sensorRequestRateChange(mTask.tid, mTask.gyroHandle, mTask.raw_sensor_rate[GYR],
mTask.raw_sensor_latency);
}
}
static void fusionSetRateMag(void)
{
int i;
if (mTask.magHandle == 0) {
mTask.sample_counts[MAG] = 0;
mTask.sample_indices[MAG] = 0;
mTask.counters[MAG] = 0;
mTask.last_time[MAG] = ULONG_LONG_MAX;
for (i = 0; sensorFind(SENS_TYPE_MAG, i, &mTask.magHandle) != NULL; i++) {
if (sensorRequest(mTask.tid, mTask.magHandle, mTask.raw_sensor_rate[MAG],
mTask.raw_sensor_latency)) {
osEventSubscribe(mTask.tid, EVT_SENSOR_MAG_DATA_RDY);
osEventSubscribe(mTask.tid, EVT_SENSOR_MAG_BIAS);
break;
}
}
} else {
sensorRequestRateChange(mTask.tid, mTask.magHandle, mTask.raw_sensor_rate[MAG],
mTask.raw_sensor_latency);
}
}
static bool fusionSetRate(uint32_t rate, uint64_t latency, void *cookie)
{
struct FusionSensor *mSensor = &mTask.sensors[(int)cookie];
int i;
uint32_t max_rate = 0;
uint32_t gyr_rate, mag_rate;
uint64_t min_resample_period = ULONG_LONG_MAX;
mSensor->rate = rate;
mSensor->latency = latency;
for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++) {
if (mTask.sensors[i].active) {
max_rate = max_rate > mTask.sensors[i].rate ? max_rate : mTask.sensors[i].rate;
}
}
if (mTask.accel_client_cnt > 0) {
mTask.raw_sensor_rate[ACC] = max_rate;
mTask.ResamplePeriodNs[ACC] = sensorTimerLookupCommon(FusionRates, rateTimerVals, max_rate);
min_resample_period = mTask.ResamplePeriodNs[ACC] < min_resample_period ?
mTask.ResamplePeriodNs[ACC] : min_resample_period;
}
if (mTask.gyro_client_cnt > 0) {
gyr_rate = max_rate > MIN_GYRO_RATE_HZ ? max_rate : MIN_GYRO_RATE_HZ;
mTask.raw_sensor_rate[GYR] = gyr_rate;
mTask.ResamplePeriodNs[GYR] = sensorTimerLookupCommon(FusionRates, rateTimerVals, gyr_rate);
min_resample_period = mTask.ResamplePeriodNs[GYR] < min_resample_period ?
mTask.ResamplePeriodNs[GYR] : min_resample_period;
}
if (mTask.mag_client_cnt > 0) {
mag_rate = max_rate < MAX_MAG_RATE_HZ ? max_rate : MAX_MAG_RATE_HZ;
mTask.raw_sensor_rate[MAG] = mag_rate;
mTask.ResamplePeriodNs[MAG] = sensorTimerLookupCommon(FusionRates, rateTimerVals, mag_rate);
min_resample_period = mTask.ResamplePeriodNs[MAG] < min_resample_period ?
mTask.ResamplePeriodNs[MAG] : min_resample_period;
}
// This guarantees that local raw sensor FIFOs won't overflow.
mTask.raw_sensor_latency = min_resample_period * (FIFO_DEPTH - 1);
for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++) {
if (mTask.sensors[i].active) {
mTask.raw_sensor_latency = mTask.sensors[i].latency < mTask.raw_sensor_latency ?
mTask.sensors[i].latency : mTask.raw_sensor_latency;
}
}
if (mTask.accel_client_cnt > 0)
fusionSetRateAcc();
if (mTask.gyro_client_cnt > 0)
fusionSetRateGyr();
if (mTask.mag_client_cnt > 0)
fusionSetRateMag();
if (mSensor->rate > 0)
sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency);
return true;
}
static bool fusionPower(bool on, void *cookie)
{
struct FusionSensor *mSensor = &mTask.sensors[(int)cookie];
int idx;
mSensor->active = on;
if (on == false) {
mTask.accel_client_cnt--;
if (mSensor->use_gyro_data)
mTask.gyro_client_cnt--;
if (mSensor->use_mag_data)
mTask.mag_client_cnt--;
// if client_cnt == 0 and Handle == 0, nothing need to be done.
// if client_cnt > 0 and Handle == 0, something else is turning it on, all will be done.
if (mTask.accel_client_cnt == 0 && mTask.accelHandle != 0) {
sensorRelease(mTask.tid, mTask.accelHandle);
mTask.accelHandle = 0;
osEventUnsubscribe(mTask.tid, EVT_SENSOR_ACC_DATA_RDY);
}
if (mTask.gyro_client_cnt == 0 && mTask.gyroHandle != 0) {
sensorRelease(mTask.tid, mTask.gyroHandle);
mTask.gyroHandle = 0;
osEventUnsubscribe(mTask.tid, EVT_SENSOR_GYR_DATA_RDY);
}
if (mTask.mag_client_cnt == 0 && mTask.magHandle != 0) {
sensorRelease(mTask.tid, mTask.magHandle);
mTask.magHandle = 0;
osEventUnsubscribe(mTask.tid, EVT_SENSOR_MAG_DATA_RDY);
}
idx = mSensor->idx;
(void) fusionSetRate(0, ULONG_LONG_MAX, (void *)idx);
} else {
mTask.accel_client_cnt++;
if (mSensor->use_gyro_data)
mTask.gyro_client_cnt++;
if (mSensor->use_mag_data)
mTask.mag_client_cnt++;
}
configureFusion();
configureGame();
sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, on, 0);
return true;
}
static bool fusionFirmwareUpload(void *cookie)
{
struct FusionSensor *mSensor = &mTask.sensors[(int)cookie];
sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0);
return true;
}
static bool fusionFlush(void *cookie)
{
struct FusionSensor *mSensor = &mTask.sensors[(int)cookie];
uint32_t evtType = sensorGetMyEventType(mSi[mSensor->idx].sensorType);
osEnqueueEvt(evtType, SENSOR_DATA_EVENT_FLUSH, NULL);
return true;
}
static void fusionHandleEvent(uint32_t evtType, const void* evtData)
{
struct TripleAxisDataEvent *ev;
int i;
if (evtData == SENSOR_DATA_EVENT_FLUSH)
return;
switch (evtType) {
case EVT_APP_START:
// check for gyro and mag
osEventUnsubscribe(mTask.tid, EVT_APP_START);
if (!sensorFind(SENS_TYPE_GYRO, 0, &mTask.gyroHandle)) {
for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++)
mTask.sensors[i].use_gyro_data = false;
}
mTask.gyroHandle = 0;
if (!sensorFind(SENS_TYPE_MAG, 0, &mTask.magHandle)) {
for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++)
mTask.sensors[i].use_mag_data = false;
}
mTask.magHandle = 0;
break;
case EVT_SENSOR_ACC_DATA_RDY:
ev = (struct TripleAxisDataEvent *)evtData;
fillSamples(ev, ACC);
drainSamples();
break;
case EVT_SENSOR_GYR_DATA_RDY:
ev = (struct TripleAxisDataEvent *)evtData;
fillSamples(ev, GYR);
drainSamples();
break;
case EVT_SENSOR_MAG_BIAS:
ev = (struct TripleAxisDataEvent *)evtData;
if (ev->samples[0].firstSample.biasPresent && mTask.flags & FUSION_FLAG_ENABLED) {
//it is a user initiated mag cal event
fusionSetMagTrust(&mTask.fusion, MANUAL_MAG_CAL);
}
break;
case EVT_SENSOR_MAG_DATA_RDY:
ev = (struct TripleAxisDataEvent *)evtData;
fillSamples(ev, MAG);
drainSamples();
break;
}
}
static const struct SensorOps mSops =
{
.sensorPower = fusionPower,
.sensorFirmwareUpload = fusionFirmwareUpload,
.sensorSetRate = fusionSetRate,
.sensorFlush = fusionFlush,
};
static bool fusionStart(uint32_t tid)
{
size_t i, slabSize;
mTask.tid = tid;
mTask.flags = 0;
for (i = 0; i < NUM_OF_RAW_SENSOR; i++) {
mTask.sample_counts[i] = 0;
mTask.sample_indices[i] = 0;
}
for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++) {
mTask.sensors[i].handle = sensorRegister(&mSi[i], &mSops, (void *)i, true);
mTask.sensors[i].idx = i;
mTask.sensors[i].use_gyro_data = true;
mTask.sensors[i].use_mag_data = true;
}
mTask.sensors[GEOMAG].use_gyro_data = false;
mTask.sensors[GAME].use_mag_data = false;
mTask.sensors[GRAVITY].use_mag_data = false;
mTask.sensors[LINEAR].use_mag_data = false;
mTask.accel_client_cnt = 0;
mTask.gyro_client_cnt = 0;
mTask.mag_client_cnt = 0;
slabSize = sizeof(struct TripleAxisDataEvent)
+ MAX_NUM_COMMS_EVENT_SAMPLES * sizeof(struct TripleAxisDataPoint);
// worst case 6 output sensors * (N + 1) comms_events
mDataSlab = slabAllocatorNew(slabSize, 4, 6 * (NUM_COMMS_EVENTS_IN_FIFO + 1));
if (!mDataSlab) {
osLog(LOG_ERROR, "ORIENTATION: slabAllocatorNew() FAILED\n");
return false;
}
osEventSubscribe(mTask.tid, EVT_APP_START);
return true;
}
static void fusionEnd()
{
mTask.flags &= ~FUSION_FLAG_INITIALIZED;
mTask.flags &= ~FUSION_FLAG_GAME_INITIALIZED;
slabAllocatorDestroy(mDataSlab);
}
INTERNAL_APP_INIT(
APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 4),
ORIENTATION_APP_VERSION,
fusionStart,
fusionEnd,
fusionHandleEvent);
Reference in New Issue View Git Blame Copy Permalink