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 181 static void dataEvtFree(void *ptr) 182 { 183 slabAllocatorFree(mDataSlab, ptr); 184 } 185 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 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 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) 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 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 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 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 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 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 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 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 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 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 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 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 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 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
