Lines Matching refs:mTask
153 static struct FusionTask mTask;
196 if (index == GYR && mTask.gyro_client_cnt == 0) {
199 if (index == MAG && mTask.mag_client_cnt == 0) {
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;
210 if (mTask.last_time[index] == ULONG_LONG_MAX) {
216 curr_sample = &mTask.last_sample[index];
219 curr_time = mTask.last_time[index];
247 mTask.samples[index][w].x = curr_sample->x + weight_next *
249 mTask.samples[index][w].y = curr_sample->y + weight_next *
251 mTask.samples[index][w].z = curr_sample->z + weight_next *
253 mTask.samples[index][w].time = curr_time + counter;
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;
351 if (fusionHasEstimate(&mTask.game)) {
353 if (mTask.sensors[GAME].active) {
354 fusionGetAttitude(&mTask.game, &attitude);
355 if (!addSample(&mTask.sensors[GAME],
364 if (mTask.sensors[GRAVITY].active) {
365 fusionGetRotationMatrix(&mTask.game, &R);
369 if (!addSample(&mTask.sensors[GRAVITY],
379 && mTask.sensors[LINEAR].active) {
381 fusionGetRotationMatrix(&mTask.game, &R);
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);
390 if (!addSample(&mTask.sensors[LINEAR],
391 mTask.samples[0][last_accel_sample_index].time,
400 if (fusionHasEstimate(&mTask.fusion)) {
401 fusionGetAttitude(&mTask.fusion, &attitude);
403 if (mTask.sensors[ORIENT].active) {
404 fusionGetRotationMatrix(&mTask.fusion, &R);
414 if (!addSample(&mTask.sensors[ORIENT],
423 if (mTask.sensors[GEOMAG].active) {
424 if (!addSample(&mTask.sensors[GEOMAG],
433 if (mTask.sensors[ROTAT].active) {
434 if (!addSample(&mTask.sensors[ROTAT],
451 size_t i = mTask.sample_indices[ACC];
458 if (mTask.gyro_client_cnt > 0)
459 j = mTask.sample_indices[GYR];
461 if (mTask.mag_client_cnt > 0)
462 k = mTask.sample_indices[MAG];
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)
472 a_time = mTask.samples[ACC][i].time;
473 g_time = mTask.gyro_client_cnt > 0 ? mTask.samples[GYR][j].time
475 m_time = mTask.mag_client_cnt > 0 ? mTask.samples[MAG][k].time
487 dT = floatFromUint64(mTask.ResamplePeriodNs[which]) * 1e-9f;
490 initVec3(&a, mTask.samples[ACC][i].x, mTask.samples[ACC][i].y, mTask.samples[ACC][i].z);
492 if (mTask.flags & FUSION_FLAG_ENABLED)
493 fusionHandleAcc(&mTask.fusion, &a, dT);
495 if (mTask.flags & FUSION_FLAG_GAME_ENABLED)
496 fusionHandleAcc(&mTask.game, &a, dT);
498 success = updateOutput(i, mTask.samples[ACC][i].time);
505 --mTask.sample_counts[ACC];
512 initVec3(&w, mTask.samples[GYR][j].x, mTask.samples[GYR][j].y, mTask.samples[GYR][j].z);
514 if (mTask
515 fusionHandleGyro(&mTask.fusion, &w, dT);
517 if (mTask.flags & FUSION_FLAG_GAME_ENABLED)
518 fusionHandleGyro(&mTask.game, &w, dT);
520 --mTask.sample_counts[GYR];
525 initVec3(&m, mTask.samples[MAG][k].x, mTask.samples[MAG][k].y, mTask.samples[MAG][k].z);
527 fusionHandleMag(&mTask.fusion, &m, dT);
529 --mTask.sample_counts[MAG];
536 mTask.sample_indices[ACC] = i;
538 if (mTask.gyro_client_cnt > 0)
539 mTask.sample_indices[GYR] = j;
541 if (mTask.mag_client_cnt > 0)
542 mTask.sample_indices[MAG] = k;
545 if (mTask.sensors[i].ev != NULL) {
547 mTask.sensors[i].ev, dataEvtFree);
548 mTask.sensors[i].ev = NULL;
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;
565 mTask.flags &= ~FUSION_FLAG_ENABLED;
566 mTask.flags &= ~FUSION_FLAG_INITIALIZED;
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;
579 mTask.flags &= ~FUSION_FLAG_GAME_ENABLED;
580 mTask.flags &= ~FUSION_FLAG_GAME_INITIALIZED;
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);
600 sensorRequestRateChange(mTask.tid, mTask.accelHandle, mTask.raw_sensor_rate[ACC],
601 mTask.raw_sensor_latency);
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);
621 sensorRequestRateChange(mTask.tid, mTask.gyroHandle, mTask.raw_sensor_rate[GYR],
622 mTask.raw_sensor_latency);
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);
643 sensorRequestRateChange(mTask.tid, mTask.magHandle, mTask.raw_sensor_rate[MAG],
644 mTask.raw_sensor_latency);
650 struct FusionSensor *mSensor = &mTask.sensors[(int)cookie];
660 if (mTask.sensors[i].active) {
661 max_rate = max_rate > mTask.sensors[i].rate ? max_rate : mTask.sensors[i].rate;
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;
672 if (mTask.gyro_client_cnt > 0) {
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;
680 if (mTask.mag_client_cnt > 0) {
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;
689 mTask.raw_sensor_latency = min_resample_period * (FIFO_DEPTH - 1);
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;
698 if (mTask.accel_client_cnt > 0)
700 if (mTask.gyro_client_cnt > 0)
702 if (mTask.mag_client_cnt > 0)
712 struct FusionSensor *mSensor = &mTask.sensors[(int)cookie];
717 mTask.accel_client_cnt--;
719 mTask.gyro_client_cnt--;
721 mTask.mag_client_cnt--;
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);
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);
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);
746 mTask.accel_client_cnt++;
748 mTask.gyro_client_cnt++;
750 mTask.mag_client_cnt++;
762 struct FusionSensor *mSensor = &mTask.sensors[(int)cookie];
770 struct FusionSensor *mSensor = &mTask.sensors[(int)cookie];
788 osEventUnsubscribe(mTask.tid, EVT_APP_START);
789 if (!sensorFind(SENS_TYPE_GYRO, 0, &mTask.gyroHandle)) {
791 mTask.sensors[i].use_gyro_data = false;
793 mTask.gyroHandle = 0;
794 if (!sensorFind(SENS_TYPE_MAG, 0, &mTask.magHandle)) {
796 mTask.sensors[i].use_mag_data = false;
798 mTask.magHandle = 0;
812 if (ev->samples[0].firstSample.biasPresent && mTask.flags & FUSION_FLAG_ENABLED) {
814 fusionSetMagTrust(&mTask.fusion, MANUAL_MAG_CAL);
837 mTask.tid = tid;
838 mTask.flags = 0;
841 mTask.sample_counts[i] = 0;
842 mTask.sample_indices[i] = 0;
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;
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;
857 mTask.accel_client_cnt = 0;
858 mTask.gyro_client_cnt = 0;
859 mTask.mag_client_cnt = 0;
871 osEventSubscribe(mTask.tid, EVT_APP_START);
878 mTask.flags &= ~FUSION_FLAG_INITIALIZED;
879 mTask.flags &= ~FUSION_FLAG_GAME_INITIALIZED;
