1 /*
2 * Copyright (C) 2016 The Android Open Source Project
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17 #include <stdlib.h>
18 #include <string.h>
19 #include <timer.h>
20 #include <heap.h>
21 #include <plat/rtc.h>
22 #include <plat/syscfg.h>
23 #include <hostIntf.h>
24 #include <nanohubPacket.h>
25 #include <floatRt.h>
26
27 #include <seos.h>
28
29 #include <nanohub_math.h>
30 #include <algos/fusion.h>
31 #include <sensors.h>
32 #include <variant/sensType.h>
33 #include <limits.h>
34 #include <slab.h>
35
36 #define ORIENTATION_APP_VERSION 1
37
38 #define MAX_NUM_COMMS_EVENT_SAMPLES 15 // at most 15 samples can fit in one comms_event
39 #define NUM_COMMS_EVENTS_IN_FIFO 2 // This controls how often the hub needs to wake up
40 // in batching
41
42 // needs to be greater than max raw sensor rate ratio
43 #define FIFO_DEPTH (NUM_COMMS_EVENTS_IN_FIFO * MAX_NUM_COMMS_EVENT_SAMPLES)
44
45 /*
46 * FIFO_MARGIN: max raw sensor rate ratio is 8:1.
47 * If 2 batchs of high rate data comes before 1 low rate data, there can be at max 15 samples left
48 * in the FIFO
49 */
50 #define FIFO_MARGIN 15
51 #define MAX_NUM_SAMPLES (FIFO_MARGIN + FIFO_DEPTH) // actual input sample fifo depth
52 #define EVT_SENSOR_ACC_DATA_RDY sensorGetMyEventType(SENS_TYPE_ACCEL)
53 #define EVT_SENSOR_GYR_DATA_RDY sensorGetMyEventType(SENS_TYPE_GYRO)
54 #define EVT_SENSOR_MAG_DATA_RDY sensorGetMyEventType(SENS_TYPE_MAG)
55 #define EVT_SENSOR_MAG_BIAS sensorGetMyEventType(SENS_TYPE_MAG_BIAS)
56
57 #define kGravityEarth 9.80665f
58 #define kRad2deg (180.0f / M_PI)
59 #define MIN_GYRO_RATE_HZ SENSOR_HZ(100.0f)
60 #define MAX_MAG_RATE_HZ SENSOR_HZ(50.0f)
61
62 enum
63 {
64 FUSION_FLAG_ENABLED = 0x01,
65 FUSION_FLAG_INITIALIZED = 0x08,
66 FUSION_FLAG_GAME_ENABLED = 0x10,
67 FUSION_FLAG_GAME_INITIALIZED = 0x20
68 };
69
70 enum RawSensorType
71 {
72 ACC,
73 GYR,
74 MAG,
75 NUM_OF_RAW_SENSOR
76 };
77
78 enum FusionSensorType
79 {
80 ORIENT,
81 GRAVITY,
82 GEOMAG,
83 LINEAR,
84 GAME,
85 ROTAT,
86 NUM_OF_FUSION_SENSOR
87 };
88
89
90 struct FusionSensorSample {
91 uint64_t time;
92 float x, y, z;
93 };
94
95 struct FusionSensor {
96 uint32_t handle;
97 struct TripleAxisDataEvent *ev;
98 uint64_t prev_time;
99 uint64_t latency;
100 uint32_t rate;
101 bool active;
102 bool use_gyro_data;
103 bool use_mag_data;
104 uint8_t idx;
105 };
106
107 struct FusionTask {
108 uint32_t tid;
109 uint32_t accelHandle;
110 uint32_t gyroHandle;
111 uint32_t magHandle;
112
113 struct Fusion fusion;
114 struct Fusion game;
115
116 struct FusionSensor sensors[NUM_OF_FUSION_SENSOR];
117 struct FusionSensorSample samples[NUM_OF_RAW_SENSOR][MAX_NUM_SAMPLES];
118 size_t sample_indices[NUM_OF_RAW_SENSOR];
119 size_t sample_counts[NUM_OF_RAW_SENSOR];
120 uint32_t counters[NUM_OF_RAW_SENSOR];
121 uint64_t ResamplePeriodNs[NUM_OF_RAW_SENSOR];
122 uint64_t last_time[NUM_OF_RAW_SENSOR];
123 struct TripleAxisDataPoint last_sample[NUM_OF_RAW_SENSOR];
124
125 uint32_t flags;
126
127 uint32_t raw_sensor_rate[NUM_OF_RAW_SENSOR];
128 uint64_t raw_sensor_latency;
129
130 uint8_t accel_client_cnt;
131 uint8_t gyro_client_cnt;
132 uint8_t mag_client_cnt;
133 };
134
135 static uint32_t FusionRates[] = {
136 SENSOR_HZ(12.5f),
137 SENSOR_HZ(25.0f),
138 SENSOR_HZ(50.0f),
139 SENSOR_HZ(100.0f),
140 SENSOR_HZ(200.0f),
141 0,
142 };
143
144 //should match "supported rates in length" and be the timer length for that rate in nanosecs
145 static const uint64_t rateTimerVals[] = {
146 1000000000ULL / 12.5f,
147 1000000000ULL / 25,
148 1000000000ULL / 50,
149 1000000000ULL / 100,
150 1000000000ULL / 200,
151 };
152
153 static struct FusionTask mTask;
154
155 #define DEC_INFO_RATE(name, rates, type, axis, inter, samples) \
156 .sensorName = name, \
157 .supportedRates = rates, \
158 .sensorType = type, \
159 .numAxis = axis, \
160 .interrupt = inter, \
161 .minSamples = samples
162
163 static const struct SensorInfo mSi[NUM_OF_FUSION_SENSOR] =
164 {
165 { DEC_INFO_RATE("Orientation", FusionRates, SENS_TYPE_ORIENTATION, NUM_AXIS_THREE,
166 NANOHUB_INT_NONWAKEUP, 20) },
167 { DEC_INFO_RATE("Gravity", FusionRates, SENS_TYPE_GRAVITY, NUM_AXIS_THREE,
168 NANOHUB_INT_NONWAKEUP, 20) },
169 { DEC_INFO_RATE("Geomagnetic Rotation Vector", FusionRates, SENS_TYPE_GEO_MAG_ROT_VEC,
170 NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 20) },
171 { DEC_INFO_RATE("Linear Acceleration", FusionRates, SENS_TYPE_LINEAR_ACCEL, NUM_AXIS_THREE,
172 NANOHUB_INT_NONWAKEUP, 20) },
173 { DEC_INFO_RATE("Game Rotation Vector", FusionRates, SENS_TYPE_GAME_ROT_VECTOR, NUM_AXIS_THREE,
174 NANOHUB_INT_NONWAKEUP, 300) },
175 { DEC_INFO_RATE("Rotation Vector", FusionRates, SENS_TYPE_ROTATION_VECTOR, NUM_AXIS_THREE,
176 NANOHUB_INT_NONWAKEUP, 20) },
177 };
178
179 static struct SlabAllocator *mDataSlab;
180
dataEvtFree(void * ptr)181 static void dataEvtFree(void *ptr)
182 {
183 slabAllocatorFree(mDataSlab, ptr);
184 }
185
fillSamples(struct TripleAxisDataEvent * ev,enum RawSensorType index)186 static void fillSamples(struct TripleAxisDataEvent *ev, enum RawSensorType index)
187 {
188 bool bad_timestamp;
189 size_t i, w, n, num_samples;
190 struct TripleAxisDataPoint *curr_sample, *next_sample;
191 uint32_t counter;
192 uint64_t ResamplePeriodNs, curr_time, next_time;
193 uint64_t sample_spacing_ns;
194 float weight_next;
195
196 if (index == GYR && mTask.gyro_client_cnt == 0) {
197 return;
198 }
199 if (index == MAG && mTask.mag_client_cnt == 0) {
200 return;
201 }
202
203 n = mTask.sample_counts[index];
204 i = mTask.sample_indices[index];
205 counter = mTask.counters[index];
206 ResamplePeriodNs = mTask.ResamplePeriodNs[index];
207 w = (mTask.sample_indices[index] + n) % MAX_NUM_SAMPLES;
208
209 // check if this sensor was used before
210 if (mTask.last_time[index] == ULONG_LONG_MAX) {
211 curr_sample = ev->samples;
212 next_sample = curr_sample + 1;
213 num_samples = ev->samples[0].firstSample.numSamples;
214 curr_time = ev->referenceTime;
215 } else {
216 curr_sample = &mTask.last_sample[index];
217 next_sample = ev->samples;
218 num_samples = ev->samples[0].firstSample.numSamples + 1;
219 curr_time = mTask.last_time[index];
220 }
221
222 while (num_samples > 1) {
223
224 if (next_sample == ev->samples)
225 next_time = ev->referenceTime;
226 else
227 next_time = curr_time + next_sample->deltaTime;
228
229 // error handling for non-chronological accel timestamps
230 sample_spacing_ns = (next_time > curr_time) ? (next_time - curr_time) : 0;
231
232 // This can happen during sensor config changes
233 bad_timestamp = (sample_spacing_ns > 10 * ResamplePeriodNs);
234
235 // Check to see if we need to move the interpolation window or
236 // interpolate
237 if ((counter >= sample_spacing_ns) || bad_timestamp) {
238 num_samples--;
239 counter -= (bad_timestamp ? counter : sample_spacing_ns);
240 curr_sample = next_sample;
241 next_sample++;
242
243 curr_time = next_time;
244 } else {
245 weight_next = (float)counter / floatFromUint64(sample_spacing_ns);
246
247 mTask.samples[index][w].x = curr_sample->x + weight_next *
248 (next_sample->x - curr_sample->x);
249 mTask.samples[index][w].y = curr_sample->y + weight_next *
250 (next_sample->y - curr_sample->y);
251 mTask.samples[index][w].z = curr_sample->z + weight_next *
252 (next_sample->z - curr_sample->z);
253 mTask.samples[index][w].time = curr_time + counter;
254
255 // Move the read index when buffer is full
256 if (++n > MAX_NUM_SAMPLES) {
257 n = MAX_NUM_SAMPLES;
258
259 if (++i == MAX_NUM_SAMPLES) {
260 i = 0;
261 }
262 }
263
264 // Reset the write index
265 if (++w == MAX_NUM_SAMPLES) {
266 w = 0;
267 }
268
269 // Move to the next resample
270 counter += ResamplePeriodNs;
271 }
272 }
273
274 mTask.sample_counts[index] = n;
275 mTask.sample_indices[index] = i;
276 mTask.counters[index] = counter;
277 mTask.last_sample[index] = *curr_sample;
278 mTask.last_time[index] = curr_time;
279 }
280
allocateDataEvt(struct FusionSensor * mSensor,uint64_t time)281 static bool allocateDataEvt(struct FusionSensor *mSensor, uint64_t time)
282 {
283 mSensor->ev = slabAllocatorAlloc(mDataSlab);
284 if (mSensor->ev == NULL) {
285 // slab allocation failed, need to stop draining raw samples for now.
286 osLog(LOG_INFO, "ORIENTATION: slabAllocatorAlloc() Failed\n");
287 return false;
288 }
289
290 // delta time for the first sample is sample count
291 memset(&mSensor->ev->samples[0].firstSample, 0x00, sizeof(struct SensorFirstSample));
292 mSensor->ev->referenceTime = time;
293 mSensor->prev_time = time;
294
295 return true;
296 }
297
298 // returns false if addSample() fails
addSample(struct FusionSensor * mSensor,uint64_t time,float x,float y,float z)299 static bool addSample(struct FusionSensor *mSensor, uint64_t time, float x, float y, float z)
300 {
301 struct TripleAxisDataPoint *sample;
302
303 // Bypass processing this accel sample.
304 // This is needed after recovering from a slab shortage.
305 if (mSensor->prev_time == time) {
306 osLog(LOG_INFO, "Accel sample has been processed by fusion sensor %d\n",
307 mSensor->idx);
308 return true;
309 }
310
311 if (mSensor->ev == NULL) {
312 if (!allocateDataEvt(mSensor, time))
313 return false;
314 }
315
316 if (mSensor->ev->samples[0].firstSample.numSamples >= MAX_NUM_COMMS_EVENT_SAMPLES) {
317 osLog(LOG_ERROR, "ORIENTATION: BAD_INDEX\n");
318 return false;
319 }
320
321 sample = &mSensor->ev->samples[mSensor->ev->samples[0].firstSample.numSamples++];
322
323 if (mSensor->ev->samples[0].firstSample.numSamples > 1) {
324 sample->deltaTime = time > mSensor->prev_time ? (time - mSensor->prev_time) : 0;
325 mSensor->prev_time = time;
326 }
327
328 sample->x = x;
329 sample->y = y;
330 sample->z = z;
331
332 if (mSensor->ev->samples[0].firstSample.numSamples == MAX_NUM_COMMS_EVENT_SAMPLES) {
333 osEnqueueEvtOrFree(
334 EVENT_TYPE_BIT_DISCARDABLE | sensorGetMyEventType(mSi[mSensor->idx].sensorType),
335 mSensor->ev, dataEvtFree);
336 mSensor->ev = NULL;
337 }
338 return true;
339 }
340
341 // returns false if addSample fails for any fusion sensor
342 // (most likely due to slab allocation failure)
updateOutput(ssize_t last_accel_sample_index,uint64_t last_sensor_time)343 static bool updateOutput(ssize_t last_accel_sample_index, uint64_t last_sensor_time)
344 {
345 struct Vec4 attitude;
346 struct Vec3 g, a;
347 struct Mat33 R; // direction-cosine/rotation matrix, inertial -> device
348 bool rInited; // indicates if matrix R has been initialzed. for avoiding repeated computation
349 bool ret = true;
350
351 if (fusionHasEstimate(&mTask.game)) {
352 rInited = false;
353 if (mTask.sensors[GAME].active) {
354 fusionGetAttitude(&mTask.game, &attitude);
355 if (!addSample(&mTask.sensors[GAME],
356 last_sensor_time,
357 attitude.x,
358 attitude.y,
359 attitude.z)) {
360 ret = false;
361 }
362 }
363
364 if (mTask.sensors[GRAVITY].active) {
365 fusionGetRotationMatrix(&mTask.game, &R);
366 rInited = true;
367 initVec3(&g, R.elem[0][2], R.elem[1][2], R.elem[2][2]);
368 vec3ScalarMul(&g, kGravityEarth);
369 if (!addSample(&mTask.sensors[GRAVITY],
370 last_sensor_time,
371 g.x,
372 g.y,
373 g.z)) {
374 ret = false;
375 }
376 }
377
378 if (last_accel_sample_index >= 0
379 && mTask.sensors[LINEAR].active) {
380 if (!rInited) {
381 fusionGetRotationMatrix(&mTask.game, &R);
382 }
383 initVec3(&g, R.elem[0][2], R.elem[1][2], R.elem[2][2]);
384 vec3ScalarMul(&g, kGravityEarth);
385 initVec3(&a,
386 mTask.samples[0][last_accel_sample_index].x,
387 mTask.samples[0][last_accel_sample_index].y,
388 mTask.samples[0][last_accel_sample_index].z);
389
390 if (!addSample(&mTask.sensors[LINEAR],
391 mTask.samples[0][last_accel_sample_index].time,
392 a.x - g.x,
393 a.y - g.y,
394 a.z - g.z)) {
395 ret = false;
396 }
397 }
398 }
399
400 if (fusionHasEstimate(&mTask.fusion)) {
401 fusionGetAttitude(&mTask.fusion, &attitude);
402
403 if (mTask.sensors[ORIENT].active) {
404 fusionGetRotationMatrix(&mTask.fusion, &R);
405 // x, y, z = yaw, pitch, roll
406 float x = atan2f(-R.elem[0][1], R.elem[0][0]) * kRad2deg;
407 float y = atan2f(-R.elem[1][2], R.elem[2][2]) * kRad2deg;
408 float z = asinf(R.elem[0][2]) * kRad2deg;
409
410 if (x < 0.0f) {
411 x += 360.0f;
412 }
413
414 if (!addSample(&mTask.sensors[ORIENT],
415 last_sensor_time,
416 x,
417 y,
418 z)) {
419 ret = false;
420 }
421 }
422
423 if (mTask.sensors[GEOMAG].active) {
424 if (!addSample(&mTask.sensors[GEOMAG],
425 last_sensor_time,
426 attitude.x,
427 attitude.y,
428 attitude.z)) {
429 ret = false;
430 }
431 }
432
433 if (mTask.sensors[ROTAT].active) {
434 if (!addSample(&mTask.sensors[ROTAT],
435 last_sensor_time,
436 attitude.x,
437 attitude.y,
438 attitude.z)) {
439 ret = false;
440 }
441 }
442
443 }
444 return ret;
445 }
446
drainSamples()447 static void drainSamples()
448 {
449 struct Vec3 a, w, m;
450 uint64_t a_time, g_time, m_time;
451 size_t i = mTask.sample_indices[ACC];
452 size_t j = 0;
453 size_t k = 0;
454 size_t which;
455 float dT;
456 bool success = true;
457
458 if (mTask.gyro_client_cnt > 0)
459 j = mTask.sample_indices[GYR];
460
461 if (mTask.mag_client_cnt > 0)
462 k = mTask.sample_indices[MAG];
463
464 // Keep draining raw samples and producing fusion samples only if
465 // 1) all raw sensors needed are present (to compare timestamp) and
466 // 2) updateOutput() succeeded (no slab shortage)
467 // Otherwise, wait till next raw sample event.
468 while (mTask.sample_counts[ACC] > 0
469 && (!(mTask.gyro_client_cnt > 0) || mTask.sample_counts[GYR] > 0)
470 && (!(mTask.mag_client_cnt > 0) || mTask.sample_counts[MAG] > 0)
471 && success) {
472 a_time = mTask.samples[ACC][i].time;
473 g_time = mTask.gyro_client_cnt > 0 ? mTask.samples[GYR][j].time
474 : ULONG_LONG_MAX;
475 m_time = mTask.mag_client_cnt > 0 ? mTask.samples[MAG][k].time
476 : ULONG_LONG_MAX;
477
478 // priority with same timestamp: gyro > acc > mag
479 if (g_time <= a_time && g_time <= m_time) {
480 which = GYR;
481 } else if (a_time <= m_time) {
482 which = ACC;
483 } else {
484 which = MAG;
485 }
486
487 dT = floatFromUint64(mTask.ResamplePeriodNs[which]) * 1e-9f;
488 switch (which) {
489 case ACC:
490 initVec3(&a, mTask.samples[ACC][i].x, mTask.samples[ACC][i].y, mTask.samples[ACC][i].z);
491
492 if (mTask.flags & FUSION_FLAG_ENABLED)
493 fusionHandleAcc(&mTask.fusion, &a, dT);
494
495 if (mTask.flags & FUSION_FLAG_GAME_ENABLED)
496 fusionHandleAcc(&mTask.game, &a, dT);
497
498 success = updateOutput(i, mTask.samples[ACC][i].time);
499
500 // Do not remove the accel sample until all active fusion sesnsors
501 // successfully updated the output.
502 // Fusion sensors that have processed this accel sample will bypass
503 // it in addSample().
504 if (success) {
505 --mTask.sample_counts[ACC];
506 if (++i == MAX_NUM_SAMPLES) {
507 i = 0;
508 }
509 }
510 break;
511 case GYR:
512 initVec3(&w, mTask.samples[GYR][j].x, mTask.samples[GYR][j].y, mTask.samples[GYR][j].z);
513
514 if (mTask.flags & FUSION_FLAG_ENABLED)
515 fusionHandleGyro(&mTask.fusion, &w, dT);
516
517 if (mTask.flags & FUSION_FLAG_GAME_ENABLED)
518 fusionHandleGyro(&mTask.game, &w, dT);
519
520 --mTask.sample_counts[GYR];
521 if (++j == MAX_NUM_SAMPLES)
522 j = 0;
523 break;
524 case MAG:
525 initVec3(&m, mTask.samples[MAG][k].x, mTask.samples[MAG][k].y, mTask.samples[MAG][k].z);
526
527 fusionHandleMag(&mTask.fusion, &m, dT);
528
529 --mTask.sample_counts[MAG];
530 if (++k == MAX_NUM_SAMPLES)
531 k = 0;
532 break;
533 }
534 }
535
536 mTask.sample_indices[ACC] = i;
537
538 if (mTask.gyro_client_cnt > 0)
539 mTask.sample_indices[GYR] = j;
540
541 if (mTask.mag_client_cnt > 0)
542 mTask.sample_indices[MAG] = k;
543
544 for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++) {
545 if (mTask.sensors[i].ev != NULL) {
546 osEnqueueEvtOrFree(EVENT_TYPE_BIT_DISCARDABLE | sensorGetMyEventType(mSi[i].sensorType),
547 mTask.sensors[i].ev, dataEvtFree);
548 mTask.sensors[i].ev = NULL;
549 }
550 }
551 }
552
configureFusion()553 static void configureFusion()
554 {
555 if (mTask.sensors[ORIENT].active
556 || mTask.sensors[ROTAT].active
557 || mTask.sensors[GEOMAG].active) {
558 mTask.flags |= FUSION_FLAG_ENABLED;
559 initFusion(&mTask.fusion,
560 (mTask.mag_client_cnt > 0 ? FUSION_USE_MAG : 0) |
561 (mTask.gyro_client_cnt > 0 ? FUSION_USE_GYRO : 0) |
562 ((mTask.flags & FUSION_FLAG_INITIALIZED) ? 0 : FUSION_REINITIALIZE));
563 mTask.flags |= FUSION_FLAG_INITIALIZED;
564 } else {
565 mTask.flags &= ~FUSION_FLAG_ENABLED;
566 mTask.flags &= ~FUSION_FLAG_INITIALIZED;
567 }
568 }
569
configureGame()570 static void configureGame()
571 {
572 if (mTask.sensors[GAME].active || mTask.sensors[GRAVITY].active ||
573 mTask.sensors[LINEAR].active) {
574 mTask.flags |= FUSION_FLAG_GAME_ENABLED;
575 initFusion(&mTask.game, FUSION_USE_GYRO |
576 ((mTask.flags & FUSION_FLAG_INITIALIZED) ? 0 : FUSION_REINITIALIZE));
577 mTask.flags |= FUSION_FLAG_GAME_INITIALIZED;
578 } else {
579 mTask.flags &= ~FUSION_FLAG_GAME_ENABLED;
580 mTask.flags &= ~FUSION_FLAG_GAME_INITIALIZED;
581 }
582 }
583
fusionSetRateAcc(void)584 static void fusionSetRateAcc(void)
585 {
586 int i;
587 if (mTask.accelHandle == 0) {
588 mTask.sample_counts[ACC] = 0;
589 mTask.sample_indices[ACC] = 0;
590 mTask.counters[ACC] = 0;
591 mTask.last_time[ACC] = ULONG_LONG_MAX;
592 for (i = 0; sensorFind(SENS_TYPE_ACCEL, i, &mTask.accelHandle) != NULL; i++) {
593 if (sensorRequest(mTask.tid, mTask.accelHandle, mTask.raw_sensor_rate[ACC],
594 mTask.raw_sensor_latency)) {
595 osEventSubscribe(mTask.tid, EVT_SENSOR_ACC_DATA_RDY);
596 break;
597 }
598 }
599 } else {
600 sensorRequestRateChange(mTask.tid, mTask.accelHandle, mTask.raw_sensor_rate[ACC],
601 mTask.raw_sensor_latency);
602 }
603 }
604
fusionSetRateGyr(void)605 static void fusionSetRateGyr(void)
606 {
607 int i;
608 if (mTask.gyroHandle == 0) {
609 mTask.sample_counts[GYR] = 0;
610 mTask.sample_indices[GYR] = 0;
611 mTask.counters[GYR] = 0;
612 mTask.last_time[GYR] = ULONG_LONG_MAX;
613 for (i = 0; sensorFind(SENS_TYPE_GYRO, i, &mTask.gyroHandle) != NULL; i++) {
614 if (sensorRequest(mTask.tid, mTask.gyroHandle, mTask.raw_sensor_rate[GYR],
615 mTask.raw_sensor_latency)) {
616 osEventSubscribe(mTask.tid, EVT_SENSOR_GYR_DATA_RDY);
617 break;
618 }
619 }
620 } else {
621 sensorRequestRateChange(mTask.tid, mTask.gyroHandle, mTask.raw_sensor_rate[GYR],
622 mTask.raw_sensor_latency);
623 }
624 }
625
fusionSetRateMag(void)626 static void fusionSetRateMag(void)
627 {
628 int i;
629 if (mTask.magHandle == 0) {
630 mTask.sample_counts[MAG] = 0;
631 mTask.sample_indices[MAG] = 0;
632 mTask.counters[MAG] = 0;
633 mTask.last_time[MAG] = ULONG_LONG_MAX;
634 for (i = 0; sensorFind(SENS_TYPE_MAG, i, &mTask.magHandle) != NULL; i++) {
635 if (sensorRequest(mTask.tid, mTask.magHandle, mTask.raw_sensor_rate[MAG],
636 mTask.raw_sensor_latency)) {
637 osEventSubscribe(mTask.tid, EVT_SENSOR_MAG_DATA_RDY);
638 osEventSubscribe(mTask.tid, EVT_SENSOR_MAG_BIAS);
639 break;
640 }
641 }
642 } else {
643 sensorRequestRateChange(mTask.tid, mTask.magHandle, mTask.raw_sensor_rate[MAG],
644 mTask.raw_sensor_latency);
645 }
646 }
647
fusionSetRate(uint32_t rate,uint64_t latency,void * cookie)648 static bool fusionSetRate(uint32_t rate, uint64_t latency, void *cookie)
649 {
650 struct FusionSensor *mSensor = &mTask.sensors[(int)cookie];
651 int i;
652 uint32_t max_rate = 0;
653 uint32_t gyr_rate, mag_rate;
654 uint64_t min_resample_period = ULONG_LONG_MAX;
655
656 mSensor->rate = rate;
657 mSensor->latency = latency;
658
659 for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++) {
660 if (mTask.sensors[i].active) {
661 max_rate = max_rate > mTask.sensors[i].rate ? max_rate : mTask.sensors[i].rate;
662 }
663 }
664
665 if (mTask.accel_client_cnt > 0) {
666 mTask.raw_sensor_rate[ACC] = max_rate;
667 mTask.ResamplePeriodNs[ACC] = sensorTimerLookupCommon(FusionRates, rateTimerVals, max_rate);
668 min_resample_period = mTask.ResamplePeriodNs[ACC] < min_resample_period ?
669 mTask.ResamplePeriodNs[ACC] : min_resample_period;
670 }
671
672 if (mTask.gyro_client_cnt > 0) {
673 gyr_rate = max_rate > MIN_GYRO_RATE_HZ ? max_rate : MIN_GYRO_RATE_HZ;
674 mTask.raw_sensor_rate[GYR] = gyr_rate;
675 mTask.ResamplePeriodNs[GYR] = sensorTimerLookupCommon(FusionRates, rateTimerVals, gyr_rate);
676 min_resample_period = mTask.ResamplePeriodNs[GYR] < min_resample_period ?
677 mTask.ResamplePeriodNs[GYR] : min_resample_period;
678 }
679
680 if (mTask.mag_client_cnt > 0) {
681 mag_rate = max_rate < MAX_MAG_RATE_HZ ? max_rate : MAX_MAG_RATE_HZ;
682 mTask.raw_sensor_rate[MAG] = mag_rate;
683 mTask.ResamplePeriodNs[MAG] = sensorTimerLookupCommon(FusionRates, rateTimerVals, mag_rate);
684 min_resample_period = mTask.ResamplePeriodNs[MAG] < min_resample_period ?
685 mTask.ResamplePeriodNs[MAG] : min_resample_period;
686 }
687
688 // This guarantees that local raw sensor FIFOs won't overflow.
689 mTask.raw_sensor_latency = min_resample_period * (FIFO_DEPTH - 1);
690
691 for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++) {
692 if (mTask.sensors[i].active) {
693 mTask.raw_sensor_latency = mTask.sensors[i].latency < mTask.raw_sensor_latency ?
694 mTask.sensors[i].latency : mTask.raw_sensor_latency;
695 }
696 }
697
698 if (mTask.accel_client_cnt > 0)
699 fusionSetRateAcc();
700 if (mTask.gyro_client_cnt > 0)
701 fusionSetRateGyr();
702 if (mTask.mag_client_cnt > 0)
703 fusionSetRateMag();
704 if (mSensor->rate > 0)
705 sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency);
706
707 return true;
708 }
709
fusionPower(bool on,void * cookie)710 static bool fusionPower(bool on, void *cookie)
711 {
712 struct FusionSensor *mSensor = &mTask.sensors[(int)cookie];
713 int idx;
714
715 mSensor->active = on;
716 if (on == false) {
717 mTask.accel_client_cnt--;
718 if (mSensor->use_gyro_data)
719 mTask.gyro_client_cnt--;
720 if (mSensor->use_mag_data)
721 mTask.mag_client_cnt--;
722
723 // if client_cnt == 0 and Handle == 0, nothing need to be done.
724 // if client_cnt > 0 and Handle == 0, something else is turning it on, all will be done.
725 if (mTask.accel_client_cnt == 0 && mTask.accelHandle != 0) {
726 sensorRelease(mTask.tid, mTask.accelHandle);
727 mTask.accelHandle = 0;
728 osEventUnsubscribe(mTask.tid, EVT_SENSOR_ACC_DATA_RDY);
729 }
730
731 if (mTask.gyro_client_cnt == 0 && mTask.gyroHandle != 0) {
732 sensorRelease(mTask.tid, mTask.gyroHandle);
733 mTask.gyroHandle = 0;
734 osEventUnsubscribe(mTask.tid, EVT_SENSOR_GYR_DATA_RDY);
735 }
736
737 if (mTask.mag_client_cnt == 0 && mTask.magHandle != 0) {
738 sensorRelease(mTask.tid, mTask.magHandle);
739 mTask.magHandle = 0;
740 osEventUnsubscribe(mTask.tid, EVT_SENSOR_MAG_DATA_RDY);
741 }
742
743 idx = mSensor->idx;
744 (void) fusionSetRate(0, ULONG_LONG_MAX, (void *)idx);
745 } else {
746 mTask.accel_client_cnt++;
747 if (mSensor->use_gyro_data)
748 mTask.gyro_client_cnt++;
749 if (mSensor->use_mag_data)
750 mTask.mag_client_cnt++;
751 }
752
753 configureFusion();
754 configureGame();
755 sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, on, 0);
756
757 return true;
758 }
759
fusionFirmwareUpload(void * cookie)760 static bool fusionFirmwareUpload(void *cookie)
761 {
762 struct FusionSensor *mSensor = &mTask.sensors[(int)cookie];
763
764 sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0);
765 return true;
766 }
767
fusionFlush(void * cookie)768 static bool fusionFlush(void *cookie)
769 {
770 struct FusionSensor *mSensor = &mTask.sensors[(int)cookie];
771 uint32_t evtType = sensorGetMyEventType(mSi[mSensor->idx].sensorType);
772
773 osEnqueueEvt(evtType, SENSOR_DATA_EVENT_FLUSH, NULL);
774 return true;
775 }
776
fusionHandleEvent(uint32_t evtType,const void * evtData)777 static void fusionHandleEvent(uint32_t evtType, const void* evtData)
778 {
779 struct TripleAxisDataEvent *ev;
780 int i;
781
782 if (evtData == SENSOR_DATA_EVENT_FLUSH)
783 return;
784
785 switch (evtType) {
786 case EVT_APP_START:
787 // check for gyro and mag
788 osEventUnsubscribe(mTask.tid, EVT_APP_START);
789 if (!sensorFind(SENS_TYPE_GYRO, 0, &mTask.gyroHandle)) {
790 for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++)
791 mTask.sensors[i].use_gyro_data = false;
792 }
793 mTask.gyroHandle = 0;
794 if (!sensorFind(SENS_TYPE_MAG, 0, &mTask.magHandle)) {
795 for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++)
796 mTask.sensors[i].use_mag_data = false;
797 }
798 mTask.magHandle = 0;
799 break;
800 case EVT_SENSOR_ACC_DATA_RDY:
801 ev = (struct TripleAxisDataEvent *)evtData;
802 fillSamples(ev, ACC);
803 drainSamples();
804 break;
805 case EVT_SENSOR_GYR_DATA_RDY:
806 ev = (struct TripleAxisDataEvent *)evtData;
807 fillSamples(ev, GYR);
808 drainSamples();
809 break;
810 case EVT_SENSOR_MAG_BIAS:
811 ev = (struct TripleAxisDataEvent *)evtData;
812 if (ev->samples[0].firstSample.biasPresent && mTask.flags & FUSION_FLAG_ENABLED) {
813 //it is a user initiated mag cal event
814 fusionSetMagTrust(&mTask.fusion, MANUAL_MAG_CAL);
815 }
816 break;
817 case EVT_SENSOR_MAG_DATA_RDY:
818 ev = (struct TripleAxisDataEvent *)evtData;
819 fillSamples(ev, MAG);
820 drainSamples();
821 break;
822 }
823 }
824
825 static const struct SensorOps mSops =
826 {
827 .sensorPower = fusionPower,
828 .sensorFirmwareUpload = fusionFirmwareUpload,
829 .sensorSetRate = fusionSetRate,
830 .sensorFlush = fusionFlush,
831 };
832
fusionStart(uint32_t tid)833 static bool fusionStart(uint32_t tid)
834 {
835 size_t i, slabSize;
836
837 mTask.tid = tid;
838 mTask.flags = 0;
839
840 for (i = 0; i < NUM_OF_RAW_SENSOR; i++) {
841 mTask.sample_counts[i] = 0;
842 mTask.sample_indices[i] = 0;
843 }
844
845 for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++) {
846 mTask.sensors[i].handle = sensorRegister(&mSi[i], &mSops, (void *)i, true);
847 mTask.sensors[i].idx = i;
848 mTask.sensors[i].use_gyro_data = true;
849 mTask.sensors[i].use_mag_data = true;
850 }
851
852 mTask.sensors[GEOMAG].use_gyro_data = false;
853 mTask.sensors[GAME].use_mag_data = false;
854 mTask.sensors[GRAVITY].use_mag_data = false;
855 mTask.sensors[LINEAR].use_mag_data = false;
856
857 mTask.accel_client_cnt = 0;
858 mTask.gyro_client_cnt = 0;
859 mTask.mag_client_cnt = 0;
860
861 slabSize = sizeof(struct TripleAxisDataEvent)
862 + MAX_NUM_COMMS_EVENT_SAMPLES * sizeof(struct TripleAxisDataPoint);
863
864 // worst case 6 output sensors * (N + 1) comms_events
865 mDataSlab = slabAllocatorNew(slabSize, 4, 6 * (NUM_COMMS_EVENTS_IN_FIFO + 1));
866 if (!mDataSlab) {
867 osLog(LOG_ERROR, "ORIENTATION: slabAllocatorNew() FAILED\n");
868 return false;
869 }
870
871 osEventSubscribe(mTask.tid, EVT_APP_START);
872
873 return true;
874 }
875
fusionEnd()876 static void fusionEnd()
877 {
878 mTask.flags &= ~FUSION_FLAG_INITIALIZED;
879 mTask.flags &= ~FUSION_FLAG_GAME_INITIALIZED;
880 slabAllocatorDestroy(mDataSlab);
881 }
882
883 INTERNAL_APP_INIT(
884 APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 4),
885 ORIENTATION_APP_VERSION,
886 fusionStart,
887 fusionEnd,
888 fusionHandleEvent);
889