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/inc/rtc.h> 22 #include <plat/inc/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/inc/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 286 osLog(LOG_ERROR, "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 static void addSample(struct FusionSensor *mSensor, uint64_t time, float x, float y, float z) 299 { 300 struct TripleAxisDataPoint *sample; 301 302 if (mSensor->ev == NULL) { 303 if (!allocateDataEvt(mSensor, time)) 304 return; 305 } 306 307 if (mSensor->ev->samples[0].firstSample.numSamples >= MAX_NUM_COMMS_EVENT_SAMPLES) { 308 osLog(LOG_ERROR, "ORIENTATION: BAD_INDEX\n"); 309 return; 310 } 311 312 sample = &mSensor->ev->samples[mSensor->ev->samples[0].firstSample.numSamples++]; 313 314 if (mSensor->ev->samples[0].firstSample.numSamples > 1) { 315 sample->deltaTime = time > mSensor->prev_time ? (time - mSensor->prev_time) : 0; 316 mSensor->prev_time = time; 317 } 318 319 sample->x = x; 320 sample->y = y; 321 sample->z = z; 322 323 if (mSensor->ev->samples[0].firstSample.numSamples == MAX_NUM_COMMS_EVENT_SAMPLES) { 324 osEnqueueEvtOrFree( 325 EVENT_TYPE_BIT_DISCARDABLE | sensorGetMyEventType(mSi[mSensor->idx].sensorType), 326 mSensor->ev, dataEvtFree); 327 mSensor->ev = NULL; 328 } 329 } 330 331 static void updateOutput(ssize_t last_accel_sample_index, uint64_t last_sensor_time) 332 { 333 struct Vec4 attitude; 334 struct Vec3 g, a; 335 struct Mat33 R; // direction-cosine/rotation matrix, inertial -> device 336 bool rInited; // indicates if matrix R has been initialzed. for avoiding repeated computation 337 338 if (fusionHasEstimate(&mTask.game)) { 339 rInited = false; 340 if (mTask.sensors[GAME].active) { 341 fusionGetAttitude(&mTask.game, &attitude); 342 addSample(&mTask.sensors[GAME], 343 last_sensor_time, 344 attitude.x, 345 attitude.y, 346 attitude.z); 347 } 348 349 if (mTask.sensors[GRAVITY].active) { 350 fusionGetRotationMatrix(&mTask.game, &R); 351 rInited = true; 352 initVec3(&g, R.elem[0][2], R.elem[1][2], R.elem[2][2]); 353 vec3ScalarMul(&g, kGravityEarth); 354 addSample(&mTask.sensors[GRAVITY], 355 last_sensor_time, 356 g.x, g.y, g.z); 357 } 358 359 if (last_accel_sample_index >= 0 360 && mTask.sensors[LINEAR].active) { 361 if (!rInited) { 362 fusionGetRotationMatrix(&mTask.game, &R); 363 } 364 initVec3(&g, R.elem[0][2], R.elem[1][2], R.elem[2][2]); 365 vec3ScalarMul(&g, kGravityEarth); 366 initVec3(&a, 367 mTask.samples[0][last_accel_sample_index].x, 368 mTask.samples[0][last_accel_sample_index].y, 369 mTask.samples[0][last_accel_sample_index].z); 370 371 addSample(&mTask.sensors[LINEAR], 372 mTask.samples[0][last_accel_sample_index].time, 373 a.x - g.x, 374 a.y - g.y, 375 a.z - g.z); 376 } 377 } 378 379 if (fusionHasEstimate(&mTask.fusion)) { 380 fusionGetAttitude(&mTask.fusion, &attitude); 381 382 if (mTask.sensors[ORIENT].active) { 383 fusionGetRotationMatrix(&mTask.fusion, &R); 384 // x, y, z = yaw, pitch, roll 385 float x = atan2f(-R.elem[0][1], R.elem[0][0]) * kRad2deg; 386 float y = atan2f(-R.elem[1][2], R.elem[2][2]) * kRad2deg; 387 float z = asinf(R.elem[0][2]) * kRad2deg; 388 389 if (x < 0.0f) { 390 x += 360.0f; 391 } 392 393 addSample(&mTask.sensors[ORIENT], 394 last_sensor_time, x, y, z); 395 } 396 397 if (mTask.sensors[GEOMAG].active) { 398 addSample(&mTask.sensors[GEOMAG], 399 last_sensor_time, 400 attitude.x, 401 attitude.y, 402 attitude.z); 403 } 404 405 if (mTask.sensors[ROTAT].active) { 406 addSample(&mTask.sensors[ROTAT], 407 last_sensor_time, 408 attitude.x, 409 attitude.y, 410 attitude.z); 411 } 412 413 } 414 } 415 416 static void drainSamples() 417 { 418 size_t i = mTask.sample_indices[ACC]; 419 size_t j = 0; 420 size_t k = 0; 421 size_t which; 422 struct Vec3 a, w, m; 423 float dT; 424 uint64_t a_time, g_time, m_time; 425 426 if (mTask.gyro_client_cnt > 0) 427 j = mTask.sample_indices[GYR]; 428 429 if (mTask.mag_client_cnt > 0) 430 k = mTask.sample_indices[MAG]; 431 432 while (mTask.sample_counts[ACC] > 0 433 && (!(mTask.gyro_client_cnt > 0) || mTask.sample_counts[GYR] > 0) 434 && (!(mTask.mag_client_cnt > 0) || mTask.sample_counts[MAG] > 0)) { 435 a_time = mTask.samples[ACC][i].time; 436 g_time = mTask.gyro_client_cnt > 0 ? mTask.samples[GYR][j].time 437 : ULONG_LONG_MAX; 438 m_time = mTask.mag_client_cnt > 0 ? mTask.samples[MAG][k].time 439 : ULONG_LONG_MAX; 440 441 // priority with same timestamp: gyro > acc > mag 442 if (g_time <= a_time && g_time <= m_time) { 443 which = GYR; 444 } else if (a_time <= m_time) { 445 which = ACC; 446 } else { 447 which = MAG; 448 } 449 450 dT = floatFromUint64(mTask.ResamplePeriodNs[which]) * 1e-9f; 451 switch (which) { 452 case ACC: 453 initVec3(&a, mTask.samples[ACC][i].x, mTask.samples[ACC][i].y, mTask.samples[ACC][i].z); 454 455 if (mTask.flags & FUSION_FLAG_ENABLED) 456 fusionHandleAcc(&mTask.fusion, &a, dT); 457 458 if (mTask.flags & FUSION_FLAG_GAME_ENABLED) 459 fusionHandleAcc(&mTask.game, &a, dT); 460 461 updateOutput(i, mTask.samples[ACC][i].time); 462 463 --mTask.sample_counts[ACC]; 464 if (++i == MAX_NUM_SAMPLES) 465 i = 0; 466 break; 467 case GYR: 468 initVec3(&w, mTask.samples[GYR][j].x, mTask.samples[GYR][j].y, mTask.samples[GYR][j].z); 469 470 if (mTask.flags & FUSION_FLAG_ENABLED) 471 fusionHandleGyro(&mTask.fusion, &w, dT); 472 473 if (mTask.flags & FUSION_FLAG_GAME_ENABLED) 474 fusionHandleGyro(&mTask.game, &w, dT); 475 476 --mTask.sample_counts[GYR]; 477 if (++j == MAX_NUM_SAMPLES) 478 j = 0; 479 break; 480 case MAG: 481 initVec3(&m, mTask.samples[MAG][k].x, mTask.samples[MAG][k].y, mTask.samples[MAG][k].z); 482 483 fusionHandleMag(&mTask.fusion, &m, dT); 484 485 --mTask.sample_counts[MAG]; 486 if (++k == MAX_NUM_SAMPLES) 487 k = 0; 488 break; 489 } 490 } 491 492 mTask.sample_indices[ACC] = i; 493 494 if (mTask.gyro_client_cnt > 0) 495 mTask.sample_indices[GYR] = j; 496 497 if (mTask.mag_client_cnt > 0) 498 mTask.sample_indices[MAG] = k; 499 500 for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++) { 501 if (mTask.sensors[i].ev != NULL) { 502 osEnqueueEvtOrFree(EVENT_TYPE_BIT_DISCARDABLE | sensorGetMyEventType(mSi[i].sensorType), 503 mTask.sensors[i].ev, dataEvtFree); 504 mTask.sensors[i].ev = NULL; 505 } 506 } 507 } 508 509 static void configureFusion() 510 { 511 if (mTask.sensors[ORIENT].active 512 || mTask.sensors[ROTAT].active 513 || mTask.sensors[GEOMAG].active) { 514 mTask.flags |= FUSION_FLAG_ENABLED; 515 initFusion(&mTask.fusion, 516 (mTask.mag_client_cnt > 0 ? FUSION_USE_MAG : 0) | 517 (mTask.gyro_client_cnt > 0 ? FUSION_USE_GYRO : 0) | 518 ((mTask.flags & FUSION_FLAG_INITIALIZED) ? 0 : FUSION_REINITIALIZE)); 519 mTask.flags |= FUSION_FLAG_INITIALIZED; 520 } else { 521 mTask.flags &= ~FUSION_FLAG_ENABLED; 522 mTask.flags &= ~FUSION_FLAG_INITIALIZED; 523 } 524 } 525 526 static void configureGame() 527 { 528 if (mTask.sensors[GAME].active || mTask.sensors[GRAVITY].active || 529 mTask.sensors[LINEAR].active) { 530 mTask.flags |= FUSION_FLAG_GAME_ENABLED; 531 initFusion(&mTask.game, FUSION_USE_GYRO | 532 ((mTask.flags & FUSION_FLAG_INITIALIZED) ? 0 : FUSION_REINITIALIZE)); 533 mTask.flags |= FUSION_FLAG_GAME_INITIALIZED; 534 } else { 535 mTask.flags &= ~FUSION_FLAG_GAME_ENABLED; 536 mTask.flags &= ~FUSION_FLAG_GAME_INITIALIZED; 537 } 538 } 539 540 static void fusionSetRateAcc(void) 541 { 542 int i; 543 if (mTask.accelHandle == 0) { 544 mTask.sample_counts[ACC] = 0; 545 mTask.sample_indices[ACC] = 0; 546 mTask.counters[ACC] = 0; 547 mTask.last_time[ACC] = ULONG_LONG_MAX; 548 for (i = 0; sensorFind(SENS_TYPE_ACCEL, i, &mTask.accelHandle) != NULL; i++) { 549 if (sensorRequest(mTask.tid, mTask.accelHandle, mTask.raw_sensor_rate[ACC], 550 mTask.raw_sensor_latency)) { 551 osEventSubscribe(mTask.tid, EVT_SENSOR_ACC_DATA_RDY); 552 break; 553 } 554 } 555 } else { 556 sensorRequestRateChange(mTask.tid, mTask.accelHandle, mTask.raw_sensor_rate[ACC], 557 mTask.raw_sensor_latency); 558 } 559 } 560 561 static void fusionSetRateGyr(void) 562 { 563 int i; 564 if (mTask.gyroHandle == 0) { 565 mTask.sample_counts[GYR] = 0; 566 mTask.sample_indices[GYR] = 0; 567 mTask.counters[GYR] = 0; 568 mTask.last_time[GYR] = ULONG_LONG_MAX; 569 for (i = 0; sensorFind(SENS_TYPE_GYRO, i, &mTask.gyroHandle) != NULL; i++) { 570 if (sensorRequest(mTask.tid, mTask.gyroHandle, mTask.raw_sensor_rate[GYR], 571 mTask.raw_sensor_latency)) { 572 osEventSubscribe(mTask.tid, EVT_SENSOR_GYR_DATA_RDY); 573 break; 574 } 575 } 576 } else { 577 sensorRequestRateChange(mTask.tid, mTask.gyroHandle, mTask.raw_sensor_rate[GYR], 578 mTask.raw_sensor_latency); 579 } 580 } 581 582 static void fusionSetRateMag(void) 583 { 584 int i; 585 if (mTask.magHandle == 0) { 586 mTask.sample_counts[MAG] = 0; 587 mTask.sample_indices[MAG] = 0; 588 mTask.counters[MAG] = 0; 589 mTask.last_time[MAG] = ULONG_LONG_MAX; 590 for (i = 0; sensorFind(SENS_TYPE_MAG, i, &mTask.magHandle) != NULL; i++) { 591 if (sensorRequest(mTask.tid, mTask.magHandle, mTask.raw_sensor_rate[MAG], 592 mTask.raw_sensor_latency)) { 593 osEventSubscribe(mTask.tid, EVT_SENSOR_MAG_DATA_RDY); 594 osEventSubscribe(mTask.tid, EVT_SENSOR_MAG_BIAS); 595 break; 596 } 597 } 598 } else { 599 sensorRequestRateChange(mTask.tid, mTask.magHandle, mTask.raw_sensor_rate[MAG], 600 mTask.raw_sensor_latency); 601 } 602 } 603 604 static bool fusionSetRate(uint32_t rate, uint64_t latency, void *cookie) 605 { 606 struct FusionSensor *mSensor = &mTask.sensors[(int)cookie]; 607 int i; 608 uint32_t max_rate = 0; 609 uint32_t gyr_rate, mag_rate; 610 uint64_t min_resample_period = ULONG_LONG_MAX; 611 612 mSensor->rate = rate; 613 mSensor->latency = latency; 614 615 for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++) { 616 if (mTask.sensors[i].active) { 617 max_rate = max_rate > mTask.sensors[i].rate ? max_rate : mTask.sensors[i].rate; 618 } 619 } 620 621 if (mTask.accel_client_cnt > 0) { 622 mTask.raw_sensor_rate[ACC] = max_rate; 623 mTask.ResamplePeriodNs[ACC] = sensorTimerLookupCommon(FusionRates, rateTimerVals, max_rate); 624 min_resample_period = mTask.ResamplePeriodNs[ACC] < min_resample_period ? 625 mTask.ResamplePeriodNs[ACC] : min_resample_period; 626 } 627 628 if (mTask.gyro_client_cnt > 0) { 629 gyr_rate = max_rate > MIN_GYRO_RATE_HZ ? max_rate : MIN_GYRO_RATE_HZ; 630 mTask.raw_sensor_rate[GYR] = gyr_rate; 631 mTask.ResamplePeriodNs[GYR] = sensorTimerLookupCommon(FusionRates, rateTimerVals, gyr_rate); 632 min_resample_period = mTask.ResamplePeriodNs[GYR] < min_resample_period ? 633 mTask.ResamplePeriodNs[GYR] : min_resample_period; 634 } 635 636 if (mTask.mag_client_cnt > 0) { 637 mag_rate = max_rate < MAX_MAG_RATE_HZ ? max_rate : MAX_MAG_RATE_HZ; 638 mTask.raw_sensor_rate[MAG] = mag_rate; 639 mTask.ResamplePeriodNs[MAG] = sensorTimerLookupCommon(FusionRates, rateTimerVals, mag_rate); 640 min_resample_period = mTask.ResamplePeriodNs[MAG] < min_resample_period ? 641 mTask.ResamplePeriodNs[MAG] : min_resample_period; 642 } 643 644 // This guarantees that local raw sensor FIFOs won't overflow. 645 mTask.raw_sensor_latency = min_resample_period * (FIFO_DEPTH - 1); 646 647 for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++) { 648 if (mTask.sensors[i].active) { 649 mTask.raw_sensor_latency = mTask.sensors[i].latency < mTask.raw_sensor_latency ? 650 mTask.sensors[i].latency : mTask.raw_sensor_latency; 651 } 652 } 653 654 if (mTask.accel_client_cnt > 0) 655 fusionSetRateAcc(); 656 if (mTask.gyro_client_cnt > 0) 657 fusionSetRateGyr(); 658 if (mTask.mag_client_cnt > 0) 659 fusionSetRateMag(); 660 if (mSensor->rate > 0) 661 sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency); 662 663 return true; 664 } 665 666 static bool fusionPower(bool on, void *cookie) 667 { 668 struct FusionSensor *mSensor = &mTask.sensors[(int)cookie]; 669 int idx; 670 671 mSensor->active = on; 672 if (on == false) { 673 mTask.accel_client_cnt--; 674 if (mSensor->use_gyro_data) 675 mTask.gyro_client_cnt--; 676 if (mSensor->use_mag_data) 677 mTask.mag_client_cnt--; 678 679 // if client_cnt == 0 and Handle == 0, nothing need to be done. 680 // if client_cnt > 0 and Handle == 0, something else is turning it on, all will be done. 681 if (mTask.accel_client_cnt == 0 && mTask.accelHandle != 0) { 682 sensorRelease(mTask.tid, mTask.accelHandle); 683 mTask.accelHandle = 0; 684 osEventUnsubscribe(mTask.tid, EVT_SENSOR_ACC_DATA_RDY); 685 } 686 687 if (mTask.gyro_client_cnt == 0 && mTask.gyroHandle != 0) { 688 sensorRelease(mTask.tid, mTask.gyroHandle); 689 mTask.gyroHandle = 0; 690 osEventUnsubscribe(mTask.tid, EVT_SENSOR_GYR_DATA_RDY); 691 } 692 693 if (mTask.mag_client_cnt == 0 && mTask.magHandle != 0) { 694 sensorRelease(mTask.tid, mTask.magHandle); 695 mTask.magHandle = 0; 696 osEventUnsubscribe(mTask.tid, EVT_SENSOR_MAG_DATA_RDY); 697 } 698 699 idx = mSensor->idx; 700 (void) fusionSetRate(0, ULONG_LONG_MAX, (void *)idx); 701 } else { 702 mTask.accel_client_cnt++; 703 if (mSensor->use_gyro_data) 704 mTask.gyro_client_cnt++; 705 if (mSensor->use_mag_data) 706 mTask.mag_client_cnt++; 707 } 708 709 configureFusion(); 710 configureGame(); 711 sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, on, 0); 712 713 return true; 714 } 715 716 static bool fusionFirmwareUpload(void *cookie) 717 { 718 struct FusionSensor *mSensor = &mTask.sensors[(int)cookie]; 719 720 sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0); 721 return true; 722 } 723 724 static bool fusionFlush(void *cookie) 725 { 726 struct FusionSensor *mSensor = &mTask.sensors[(int)cookie]; 727 uint32_t evtType = sensorGetMyEventType(mSi[mSensor->idx].sensorType); 728 729 osEnqueueEvt(evtType, SENSOR_DATA_EVENT_FLUSH, NULL); 730 return true; 731 } 732 733 static void fusionHandleEvent(uint32_t evtType, const void* evtData) 734 { 735 struct TripleAxisDataEvent *ev; 736 int i; 737 738 if (evtData == SENSOR_DATA_EVENT_FLUSH) 739 return; 740 741 switch (evtType) { 742 case EVT_APP_START: 743 // check for gyro and mag 744 osEventUnsubscribe(mTask.tid, EVT_APP_START); 745 if (!sensorFind(SENS_TYPE_GYRO, 0, &mTask.gyroHandle)) { 746 for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++) 747 mTask.sensors[i].use_gyro_data = false; 748 } 749 mTask.gyroHandle = 0; 750 if (!sensorFind(SENS_TYPE_MAG, 0, &mTask.magHandle)) { 751 for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++) 752 mTask.sensors[i].use_mag_data = false; 753 } 754 mTask.magHandle = 0; 755 break; 756 case EVT_SENSOR_ACC_DATA_RDY: 757 ev = (struct TripleAxisDataEvent *)evtData; 758 fillSamples(ev, ACC); 759 drainSamples(); 760 break; 761 case EVT_SENSOR_GYR_DATA_RDY: 762 ev = (struct TripleAxisDataEvent *)evtData; 763 fillSamples(ev, GYR); 764 drainSamples(); 765 break; 766 case EVT_SENSOR_MAG_BIAS: 767 ev = (struct TripleAxisDataEvent *)evtData; 768 if (ev->samples[0].firstSample.biasPresent && mTask.flags & FUSION_FLAG_ENABLED) { 769 //it is a user initiated mag cal event 770 fusionSetMagTrust(&mTask.fusion, MANUAL_MAG_CAL); 771 } 772 break; 773 case EVT_SENSOR_MAG_DATA_RDY: 774 ev = (struct TripleAxisDataEvent *)evtData; 775 fillSamples(ev, MAG); 776 drainSamples(); 777 break; 778 } 779 } 780 781 static const struct SensorOps mSops = 782 { 783 .sensorPower = fusionPower, 784 .sensorFirmwareUpload = fusionFirmwareUpload, 785 .sensorSetRate = fusionSetRate, 786 .sensorFlush = fusionFlush, 787 }; 788 789 static bool fusionStart(uint32_t tid) 790 { 791 size_t i, slabSize; 792 793 mTask.tid = tid; 794 mTask.flags = 0; 795 796 for (i = 0; i < NUM_OF_RAW_SENSOR; i++) { 797 mTask.sample_counts[i] = 0; 798 mTask.sample_indices[i] = 0; 799 } 800 801 for (i = ORIENT; i < NUM_OF_FUSION_SENSOR; i++) { 802 mTask.sensors[i].handle = sensorRegister(&mSi[i], &mSops, (void *)i, true); 803 mTask.sensors[i].idx = i; 804 mTask.sensors[i].use_gyro_data = true; 805 mTask.sensors[i].use_mag_data = true; 806 } 807 808 mTask.sensors[GEOMAG].use_gyro_data = false; 809 mTask.sensors[GAME].use_mag_data = false; 810 mTask.sensors[GRAVITY].use_mag_data = false; 811 mTask.sensors[LINEAR].use_mag_data = false; 812 813 mTask.accel_client_cnt = 0; 814 mTask.gyro_client_cnt = 0; 815 mTask.mag_client_cnt = 0; 816 817 slabSize = sizeof(struct TripleAxisDataEvent) 818 + MAX_NUM_COMMS_EVENT_SAMPLES * sizeof(struct TripleAxisDataPoint); 819 820 // worst case 6 output sensors * (N + 1) comms_events 821 mDataSlab = slabAllocatorNew(slabSize, 4, 6 * (NUM_COMMS_EVENTS_IN_FIFO + 1)); 822 if (!mDataSlab) { 823 osLog(LOG_ERROR, "ORIENTATION: slabAllocatorNew() FAILED\n"); 824 return false; 825 } 826 827 osEventSubscribe(mTask.tid, EVT_APP_START); 828 829 return true; 830 } 831 832 static void fusionEnd() 833 { 834 mTask.flags &= ~FUSION_FLAG_INITIALIZED; 835 mTask.flags &= ~FUSION_FLAG_GAME_INITIALIZED; 836 slabAllocatorDestroy(mDataSlab); 837 } 838 839 INTERNAL_APP_INIT( 840 APP_ID_MAKE(APP_ID_VENDOR_GOOGLE, 4), 841 ORIENTATION_APP_VERSION, 842 fusionStart, 843 fusionEnd, 844 fusionHandleEvent); 845
