Lines Matching refs:mTask
643 static struct BMI160Task mTask;
658 SPI_READ(BMI160_REG_DATA_0, size, &mTask.dataBuffer); \
695 #define TDECL() TASK = &mTask; (void)_task
777 time_sync_init(&mTask.gSensorTime2RTC);
783 &mTask.gSensorTime2RTC, sensor_time * 39ull, rtc_time_ns);
788 time_sync_add(&mTask.gSensorTime2RTC, rtc_time_ns, sensor_time * 39ull);
792 time_sync_reset(&mTask.gSensorTime2RTC);
798 time_sync_truncate(&mTask.gSensorTime2RTC, 2);
802 time_sync_hold(&mTask.gSensorTime2RTC, 2);
916 mTask.spiInUse = false;
918 if (!osEnqueuePrivateEvt(EVT_SPI_DONE, cookie, NULL, mTask.tid))
924 if (!osEnqueuePrivateEvt(EVT_SPI_DONE, data, NULL, mTask.tid))
930 if (!osEnqueuePrivateEvt(EVT_TIME_SYNC, data, NULL, mTask.tid))
938 if (mTask.sensors[STEPCNT].powered && mTask.step_cnt_changed) {
939 mTask.step_cnt_changed = false;
940 step_cnt.idata = mTask.total_step_cnt;
947 sensorSignalInternalEvt(mTask.sensors[ACC].handle,
954 sensorSignalInternalEvt(mTask.sensors[GYR].handle,
962 sensorSignalInternalEvt(mTask.sensors[MAG].handle,
970 sensorSignalInternalEvt(mTask.sensors[STEP].handle,
977 sensorSignalInternalEvt(mTask.sensors[DTAP].handle,
984 sensorSignalInternalEvt(mTask.sensors[NOMO].handle,
991 sensorSignalInternalEvt(mTask.sensors[ANYMO].handle,
998 sensorSignalInternalEvt(mTask.sensors[FLAT].handle,
1005 sensorSignalInternalEvt(mTask.sensors[STEPCNT].handle,
1037 SPI_READ(BMI160_REG_DATA_1, 1, &mTask.dataBuffer);
1044 SPI_WRITE(BMI160_REG_DATA_1, mTask.dataBuffer[1] | 0x30);
1073 switch (mTask.mag_state) {
1076 mTask.mag_state = MAG_SET_IF;
1081 mTask.mag_state = MAG_SET_FORCE;
1083 mTask.mag_state = MAG_SET_REPXY;
1092 mTask.mag_state = MAG_SET_REPZ;
1096 mTask.mag_state = MAG_GET_DIG_X;
1102 mTask.mag_state = MAG_GET_DIG_Y;
1105 bmm150SaveDigData(&magTask, &mTask.dataBuffer[1], 0);
1107 mTask.mag_state = MAG_GET_DIG_Z;
1110 bmm150SaveDigData(&magTask, &mTask.dataBuffer[1], 8);
1112 mTask.mag_state = MAG_SET_SAVE_DIG;
1115 bmm150SaveDigData(&magTask, &mTask
1117 mTask.mag_state = MAG_SET_FORCE;
1127 mTask.mag_state = MAG_SET_ADDR;
1134 mTask.mag_state = MAG_SET_DATA;
1141 mTask.mag_state = MAG_SET_DONE;
1142 mTask.init_state = INIT_ON_CHANGE_SENSORS;
1147 SPI_READ(BMI160_REG_STATUS, 1, &mTask.statusBuffer, 1000);
1166 flushData(&mTask.sensors[i],
1191 bool anyFifoEnabled = mTask.fifo_enabled[ACC] || mTask.fifo_enabled[GYR];
1193 anyFifoEnabled = anyFifoEnabled || mTask.fifo_enabled[MAG];
1213 if (mTask.sensors[ACC].configed && mTask.sensors[ACC].latency != SENSOR_LATENCY_NODATA) {
1215 mTask.fifo_enabled[ACC] = true;
1217 mTask.fifo_enabled[ACC] = false;
1222 accelCalNewBiasAvailable = accelCalUpdateBias(&mTask.acc, &accelCalBiasX, &accelCalBiasY, &accelCalBiasZ);
1224 mSensorAcc = &mTask.sensors[ACC];
1257 if (mTask.sensors[GYR].configed && mTask.sensors[GYR].latency != SENSOR_LATENCY_NODATA) {
1259 mTask.fifo_enabled[GYR] = true;
1261 mTask.fifo_enabled[GYR] = false;
1266 if (mTask.sensors[MAG].configed && mTask.sensors[MAG].latency != SENSOR_LATENCY_NODATA) {
1268 mTask.fifo_enabled[MAG] = true;
1270 mTask.fifo_enabled[MAG] = false;
1280 if (!osEnqueuePrivateEvt(EVT_TIME_SYNC, (void *)mTask.poll_generation, NULL, mTask.tid))
1286 ++mTask.poll_generation;
1291 mTask.pending_dispatch = true;
1292 mTask.xferCnt = FIFO_READ_SIZE;
1293 SPI_READ(BMI160_REG_FIFO_DATA, mTask.xferCnt, &mTask.dataBuffer);
1298 mTask.watermark = calcWatermark2_(_task);
1299 DEBUG_PRINT("wm=%d", mTask.watermark);
1300 SPI_WRITE(BMI160_REG_FIFO_CONFIG_0, mTask.watermark);
1309 mTask.frame_sensortime_valid = false;
1311 mTask.pending_delta[i] = false;
1312 mTask.prev_frame_time[i] = ULONG_LONG_MAX;
1328 mTask.sensors[ACC].configed = false;
1332 mTask.sensors[ACC].powered = on;
1333 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC], __FUNCTION__);
1335 mTask.pending_config[ACC] = true;
1336 mTask.sensors[ACC].pConfig.enable = on;
1352 mTask.sensors[GYR].configed = false;
1357 if (anyFifoEnabled() && on != mTask.sensors[GYR].powered) {
1364 mTask.sensors[GYR].powered = on;
1365 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[GYR], __FUNCTION__);
1367 mTask.pending_config[GYR] = true;
1368 mTask.sensors[GYR].pConfig.enable = on;
1384 mTask.sensors[MAG].configed = false;
1388 mTask.sensors[MAG].powered = on;
1389 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[MAG], __FUNCTION__);
1391 mTask.pending_config[MAG] = true;
1392 mTask.sensors[MAG].pConfig.enable = on;
1407 mTask.interrupt_enable_2 |= 0x08;
1409 if (!mTask.sensors[STEPCNT].powered)
1410 mTask.interrupt_enable_2 &= ~0x08;
1411 mTask.sensors[STEP].configed = false;
1413 mTask.sensors[STEP].powered = on;
1414 SPI_WRITE(BMI160_REG_INT_EN_2, mTask.interrupt_enable_2, 450);
1415 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[STEP], __FUNCTION__);
1417 mTask.pending_config[STEP] = true;
1418 mTask.sensors[STEP].pConfig.enable = on;
1428 mTask.interrupt_enable_0 |= 0x80;
1430 mTask.interrupt_enable_0 &= ~0x80;
1431 mTask.sensors[FLAT].configed = false;
1433 mTask.sensors[FLAT].powered = on;
1434 SPI_WRITE(BMI160_REG_INT_EN_0, mTask.interrupt_enable_0, 450);
1435 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[FLAT], __FUNCTION__);
1437 mTask.pending_config[FLAT] = true;
1438 mTask.sensors[FLAT].pConfig.enable = on;
1448 mTask.interrupt_enable_0 |= 0x10;
1450 mTask.interrupt_enable_0 &= ~0x10;
1451 mTask.sensors[DTAP].configed = false;
1453 mTask.sensors[DTAP].powered = on;
1454 SPI_WRITE(BMI160_REG_INT_EN_0, mTask.interrupt_enable_0, 450);
1455 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[DTAP], __FUNCTION__);
1457 mTask.pending_config[DTAP] = true;
1458 mTask.sensors[DTAP].pConfig.enable = on;
1467 on, mTask.active_oneshot_sensor_cnt, getStateName(GET_STATE()));
1471 mTask.interrupt_enable_0 |= 0x07;
1473 mTask.interrupt_enable_0 &= ~0x07;
1474 mTask.sensors[ANYMO].configed = false;
1476 mTask.sensors[ANYMO].powered = on;
1477 SPI_WRITE(BMI160_REG_INT_EN_0, mTask.interrupt_enable_0, 450);
1478 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ANYMO], __FUNCTION__);
1480 mTask.pending_config[ANYMO] = true;
1481 mTask.sensors[ANYMO].pConfig.enable = on;
1490 on, mTask.active_oneshot_sensor_cnt, getStateName(GET_STATE()));
1493 mTask.interrupt_enable_2 |= 0x07;
1495 mTask.interrupt_enable_2 &= ~0x07;
1496 mTask.sensors[NOMO].configed = false;
1498 mTask.sensors[NOMO].powered = on;
1499 SPI_WRITE(BMI160_REG_INT_EN_2, mTask.interrupt_enable_2, 450);
1500 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[NOMO], __FUNCTION__);
1502 mTask.pending_config[NOMO] = true;
1503 mTask.sensors[NOMO].pConfig.enable = on;
1513 if (!mTask.sensors[STEP].powered) {
1514 mTask.interrupt_enable_2 |= 0x08;
1515 SPI_WRITE(BMI160_REG_INT_EN_2, mTask.interrupt_enable_2, 450);
1520 if (mTask.stepCntSamplingTimerHandle) {
1521 timTimerCancel(mTask.stepCntSamplingTimerHandle);
1522 mTask.stepCntSamplingTimerHandle = 0;
1524 if (!mTask.sensors[STEP].powered) {
1525 mTask.interrupt_enable_2 &= ~0x08;
1526 SPI_WRITE(BMI160_REG_INT_EN_2, mTask.interrupt_enable_2);
1530 mTask.last_step_cnt = 0;
1531 mTask.sensors[STEPCNT].configed = false;
1533 mTask.sensors[STEPCNT].powered = on;
1534 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[STEPCNT], __FUNCTION__);
1536 mTask.pending_config[STEPCNT] = true;
1537 mTask.sensors[STEPCNT].pConfig.enable = on;
1544 if (mTask.fifo_enabled[idx]) {
1546 mTask.next_delta[idx] = 1ull << (16 - odr);
1547 mTask.pending_delta[idx] = true;
1549 mTask.time_delta[idx] = 1ull << (16 - odr);
1639 mTask.sensors[ACC].rate = rate;
1640 mTask.sensors[ACC].latency = latency;
1641 mTask.sensors[ACC].configed = true;
1642 mTask.acc_downsample = osr;
1653 SPI_WRITE(BMI160_REG_FIFO_DOWNS, (mTask.acc_downsample << 4) | mTask.gyr_downsample | 0x88);
1658 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC], __FUNCTION__);
1660 mTask.pending_config[ACC] = true;
1661 mTask.sensors[ACC].pConfig.enable = 1;
1662 mTask.sensors[ACC].pConfig.rate = rate;
1663 mTask.sensors[ACC].pConfig.latency = latency;
1705 mTask.sensors[GYR].rate = rate;
1706 mTask.sensors[GYR].latency = latency;
1707 mTask.sensors[GYR].configed = true;
1708 mTask.gyr_downsample = osr;
1716 SPI_WRITE(BMI160_REG_FIFO_DOWNS, (mTask.acc_downsample << 4) | mTask.gyr_downsample | 0x88);
1721 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[GYR], __FUNCTION__);
1723 mTask.pending_config[GYR] = true;
1724 mTask.sensors[GYR].pConfig.enable = 1;
1725 mTask.sensors[GYR].pConfig.rate = rate;
1726 mTask.sensors[GYR].pConfig.latency = latency;
1744 mTask.sensors[MAG].rate = rate;
1745 mTask.sensors[MAG].latency = latency;
1746 mTask.sensors[MAG].configed = true;
1764 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[MAG], __FUNCTION__);
1766 mTask.pending_config[MAG] = true;
1767 mTask.sensors[MAG].pConfig.enable = 1;
1768 mTask.sensors[MAG].pConfig.rate = rate;
1769 mTask.sensors[MAG].pConfig.latency = latency;
1777 mTask.sensors[STEP].rate = rate;
1778 mTask.sensors[STEP].latency = latency;
1779 mTask.sensors[STEP].configed = true;
1781 sensorSignalInternalEvt(mTask.sensors[STEP].handle,
1788 mTask.sensors[FLAT].rate = rate;
1789 mTask.sensors[FLAT].latency = latency;
1790 mTask.sensors[FLAT].configed = true;
1792 sensorSignalInternalEvt(mTask.sensors[FLAT].handle,
1799 mTask.sensors[DTAP].rate = rate;
1800 mTask.sensors[DTAP].latency = latency;
1801 mTask.sensors[DTAP].configed = true;
1803 sensorSignalInternalEvt(mTask.sensors[DTAP].handle,
1810 mTask.sensors[ANYMO].rate = rate;
1811 mTask.sensors[ANYMO].latency = latency;
1812 mTask.sensors[ANYMO].configed = true;
1814 sensorSignalInternalEvt(mTask.sensors[ANYMO].handle,
1822 mTask.sensors[NOMO].rate = rate;
1823 mTask.sensors[NOMO].latency = latency;
1824 mTask.sensors[NOMO].configed = true;
1826 sensorSignalInternalEvt(mTask.sensors[NOMO].handle,
1833 mTask.sensors[STEPCNT].rate = rate;
1834 mTask.sensors[STEPCNT].latency = latency;
1835 mTask.sensors[STEPCNT].configed = true;
1837 if (rate == SENSOR_RATE_ONCHANGE && mTask.stepCntSamplingTimerHandle) {
1838 timTimerCancel(mTask.stepCntSamplingTimerHandle);
1839 mTask.stepCntSamplingTimerHandle = 0;
1841 if (mTask.stepCntSamplingTimerHandle) {
1842 timTimerCancel(mTask.stepCntSamplingTimerHandle);
1844 mTask.stepCntSamplingTimerHandle = timTimerSet(sensorTimerLookupCommon(StepCntRates, stepCntRateTimerVals, rate),
1846 if (!mTask.stepCntSamplingTimerHandle)
1851 sensorSignalInternalEvt(mTask.sensors[STEPCNT].handle,
1858 while (mTask.sensors[ACC].flush > 0) {
1860 mTask.sensors[ACC].flush--;
1862 while (mTask.sensors[GYR].flush > 0) {
1864 mTask.sensors[GYR].flush--;
1867 while (mTask.sensors[MAG].flush > 0) {
1869 mTask.sensors[MAG].flush--;
1877 mTask.sensors[ACC].flush++;
1885 mTask.sensors[GYR].flush++;
1894 mTask.sensors[MAG].flush++;
1929 SPI_READ(BMI160_REG_STEP_CNT_0, 2, &mTask.dataBuffer);
1930 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[STEPCNT], __FUNCTION__);
1938 mTask.sensors[STEPCNT].flush++;
1947 cur_step_cnt = (int)(mTask.dataBuffer[1] | (mTask.dataBuffer[2] << 8));
1949 if (cur_step_cnt != mTask.last_step_cnt) {
1951 if (cur_step_cnt < mTask.last_step_cnt) {
1952 mTask.total_step_cnt += cur_step_cnt + (0xFFFF - mTask.last_step_cnt);
1954 mTask.total_step_cnt += (cur_step_cnt - mTask.last_step_cnt);
1956 mTask.last_step_cnt = cur_step_cnt;
1960 if (mTask.sensors[STEPCNT].rate == SENSOR_RATE_ONCHANGE || mTask.sensors[STEPCNT].flush) {
1961 step_cnt.idata = mTask.total_step_cnt;
1964 mTask.step_cnt_changed = true;
1968 while (mTask.sensors[STEPCNT].flush) {
1970 mTask.sensors[STEPCNT].flush--;
1977 if (!osEnqueuePrivateEvt(EVT_SENSOR_STEP_COUNTER, (void *) mTask.total_step_cnt, NULL, tid)) {
1991 prev_time24 = (uint32_t)mTask.last_sensortime & 0xffffff;
1993 if (mTask.last_sensortime == 0) {
1994 mTask.last_sensortime = (uint64_t)sensor_time24;
1999 return (uint64_t)(mTask.last_sensortime);
2002 full = (mTask.last_sensortime & ~0xffffffull) | sensor_time24;
2006 if (full < mTask.last_sensortime) {
2009 mTask.last_sensortime = full;
2010 return mTask.last_sensortime;
2013 if (full < mTask.last_sensortime) {
2063 magCalRemoveSoftiron(&mTask.moc, x, y, z, &xi, &yi, &zi);
2065 newMagBias |= magCalUpdate(&mTask.moc, sensorTime * kSensorTimerIntervalUs, xi, yi, zi);
2067 magCalRemoveBias(&mTask.moc, xi, yi, zi, &x, &y, &z);
2071 gyroCalUpdateMag(&mTask.gyro_cal,
2091 accelCalRun(&mTask.acc, rtc_time,
2092 x, y, z, mTask.tempCelsius);
2094 accelCalBiasRemove(&mTask.acc, &x, &y, &z);
2099 gyroCalUpdateAccel(&mTask.gyro_cal,
2106 gyroCalUpdateGyro(&mTask.gyro_cal,
2108 x, y, z, mTask.tempCelsius);
2112 mTask.over_temp_gyro_cal, rtc_time,
2113 mTask.tempCelsius);
2116 overTempCalRemoveOffset(&mTask.over_temp_gyro_cal, rtc_time,
2121 gyroCalRemoveBias(&mTask.gyro_cal,
2134 gyroCalDebugPrint(&mTask.gyro_cal, rtc_time);
2141 overTempCalDebugPrint(&mTask.over_temp_gyro_cal, rtc_time);
2163 if (mSensor->idx == MAG && (newMagBias || !mTask.magBiasPosted)) {
2173 mTask.magBiasCurrent = true;
2175 mSensor->data_evt->samples[0].firstSample.biasCurrent = mTask.magBiasCurrent;
2180 magCalGetBias(&mTask.moc, &sample->x, &sample->y, &sample->z);
2183 mTask.magBiasPosted = true;
2196 bool new_gyrocal_offset_update = gyroCalNewBiasAvailable(&mTask.gyro_cal);
2199 gyroCalGetBias(&mTask.gyro_cal, &gyro_offset[0], &gyro_offset[1],
2204 overTempCalUpdateSensorEstimate(&mTask.over_temp_gyro_cal, rtc_time,
2214 overTempCalNewOffsetAvailable(&mTask.over_temp_gyro_cal);
2215 overTempCalGetOffset(&mTask.over_temp_gyro_cal,
2220 overTempCalNewModelUpdateAvailable(&mTask.over_temp_gyro_cal);
2297 size_t size = mTask.xferCnt;
2299 uint8_t *buf = mTask.dataBuffer;
2304 uint64_t frame_sensor_time = mTask.frame_sensortime;
2307 bool frame_sensor_time_valid = mTask.frame_sensortime_valid;
2317 if (!mTask.frame_sensortime_valid) {
2326 saved_pending_delta[j] = mTask.pending_delta[j];
2327 saved_time_delta[j] = mTask.time_delta[j];
2368 if (mTask.sensors[j].configed &&
2369 mTask.sensors[j].latency != SENSOR_LATENCY_NODATA) {
2370 min_delta = min_delta < mTask.time_delta[j] ? min_delta :
2371 mTask.time_delta[j];
2398 mTask.frame_sensortime = full_sensor_time;
2403 mTask.frame_sensortime_valid = true;
2404 mTask.frame_sensortime = full_sensor_time - frame_sensor_time;
2410 mTask.prev_frame_time[j] = ULONG_LONG_MAX;
2413 mTask.pending_delta[j] = saved_pending_delta[j];
2414 mTask.time_delta[j] = saved_time_delta[j];
2421 mTask.frame_sensortime);
2432 mTask.prev_frame_time[j] = observed[j] ? full_sensor_time : (ULONG_LONG_MAX - 1);
2437 if (!mTask.sensors[j].configed ||
2438 mTask.sensors[j].latency == SENSOR_LATENCY_NODATA) {
2439 mTask.prev_frame_time[j] = ULONG_LONG_MAX;
2440 mTask.pending_delta[j] = false;
2455 if (buf[i] & (0x01 << (j << 1)) && mTask.pending_delta[j]) {
2456 mTask.pending_delta[j] = false;
2457 mTask.time_delta[j] = mTask.next_delta[j];
2460 (unsigned int)mTask.time_delta[j]);
2489 if ((mTask.prev_frame_time[j] < ULONG_LONG_MAX - 1) && (fh_param & (1 << j))) {
2490 tmp_time[j] = mTask.prev_frame_time[j] + mTask.time_delta[j];
2503 parseRawData(&mTask.sensors[MAG], &buf[i], 0, tmp_frame_time);
2505 if (mTask.prev_frame_time[MAG] == ULONG_LONG_MAX) {
2517 mTask.prev_frame_time[MAG] = tmp_frame_time;
2529 parseRawData(&mTask.sensors[GYR], &buf[i], kScale_gyr, tmp_frame_time);
2531 if (mTask.prev_frame_time[GYR] == ULONG_LONG_MAX) {
2543 mTask.prev_frame_time[GYR] = tmp_frame_time;
2554 parseRawData(&mTask.sensors[ACC], &buf[i], kScale_acc, tmp_frame_time);
2556 if (mTask.prev_frame_time[ACC] == ULONG_LONG_MAX) {
2568 mTask.prev_frame_time[ACC] = tmp_frame_time;
2588 int b = ((i + 0x80 > mTask.xferCnt ? mTask.xferCnt : i + 0x80) + 0x0F) & ~0x0F;
2589 dumpBinary(mTask.dataBuffer, a, b - a);
2608 uint8_t int_status_0 = mTask.statusBuffer[1];
2609 uint8_t int_status_1 = mTask.statusBuffer[2];
2611 if (mTask.sensors[STEP].powered) {
2615 if (mTask.sensors[STEPCNT].powered) {
2619 if ((int_status_0 & INT_ANY_MOTION) && mTask.sensors[ANYMO].powered) {
2622 trigger_axies.idata = (mTask.statusBuffer[3] & 0x0f);
2626 if ((int_status_0 & INT_DOUBLE_TAP) && mTask.sensors[DTAP].powered) {
2629 trigger_axies.idata = ((mTask.statusBuffer[3] & 0xf0) >> 4);
2633 if ((int_status_0 & INT_FLAT) && mTask.sensors[FLAT].powered) {
2635 trigger_axies.idata = ((mTask.statusBuffer[4] & 0x80) >> 7);
2639 if ((int_status_1 & INT_NO_MOTION) && mTask.sensors[NOMO].powered) {
2651 SPI_READ(BMI160_REG_INT_STATUS_0, 4, &mTask.statusBuffer);
2652 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask, __FUNCTION__);
2656 mTask.pending_int[1] = true;
2669 if (mTask.sensors[GYR].offset_enable)
2671 if (mTask.sensors[ACC].offset_enable)
2673 mode |= (mTask.sensors[GYR].offset[2] & 0x0300) >> 4;
2674 mode |= (mTask.sensors[GYR].offset[1] & 0x0300) >> 6;
2675 mode |= (mTask.sensors[GYR].offset[0] & 0x0300) >> 8;
2684 if (mTask.sensors[ACC].offset_enable) {
2685 SPI_WRITE(BMI160_REG_OFFSET_0, mTask.sensors[ACC].offset[0] & 0xFF, 450);
2686 SPI_WRITE(BMI160_REG_OFFSET_0 + 1, mTask.sensors[ACC].offset[1] & 0xFF, 450);
2687 SPI_WRITE(BMI160_REG_OFFSET_0 + 2, mTask.sensors[ACC].offset[2] & 0xFF, 450);
2689 if (mTask.sensors[GYR].offset_enable) {
2690 SPI_WRITE(BMI160_REG_OFFSET_3, mTask.sensors[GYR].offset[0] & 0xFF, 450);
2691 SPI_WRITE(BMI160_REG_OFFSET_3 + 1, mTask.sensors[GYR].offset[1] & 0xFF, 450);
2692 SPI_WRITE(BMI160_REG_OFFSET_3 + 2, mTask.sensors[GYR].offset[2] & 0xFF, 450);
2695 SPI_READ(BMI160_REG_OFFSET_0, 7, &mTask.dataBuffer);
2696 spiBatchTxRx(&mTask.mode, sensorSpiCallback, NULL, __FUNCTION__);
2729 switch (mTask.calibration_state) {
2736 mTask.calibration_state = CALIBRATION_FOC;
2737 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC], __FUNCTION__);
2752 SPI_READ(BMI160_REG_STATUS, 1, &mTask.statusBuffer, 50000);
2754 mTask.calibration_state = CALIBRATION_WAIT_FOC_DONE;
2755 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC], __FUNCTION__);
2760 if (mTask.statusBuffer[1] & 0x08) {
2766 SPI_READ(BMI160_REG_OFFSET_0, 3, &mTask.dataBuffer);
2767 mTask.calibration_state = CALIBRATION_SET_OFFSET;
2772 SPI_READ(BMI160_REG_STATUS, 1, &mTask.statusBuffer, 50000);
2773 mTask.calibration_state = CALIBRATION_WAIT_FOC_DONE;
2776 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC], __FUNCTION__);
2781 mTask.calibration_state = CALIBRATION_TIMEOUT;
2785 mTask.sensors[ACC].offset[0] = mTask.dataBuffer[1];
2786 mTask.sensors[ACC].offset[1] = mTask.dataBuffer[2];
2787 mTask.sensors[ACC].offset[2] = mTask.dataBuffer[3];
2789 if (mTask.sensors[ACC].offset[0] & 0x80)
2790 mTask.sensors[ACC].offset[0] |= 0xFFFFFF00;
2791 if (mTask.sensors[ACC].offset[1] & 0x80)
2792 mTask.sensors[ACC].offset[1] |= 0xFFFFFF00;
2793 if (mTask.sensors[ACC].offset[2] & 0x80)
2794 mTask.sensors[ACC].offset[2] |= 0xFFFFFF00;
2796 mTask.sensors[ACC].offset_enable = true;
2798 (unsigned int)mTask.sensors[ACC].offset[0],
2799 (unsigned int)mTask.sensors[ACC].offset[1],
2800 (unsigned int)mTask.sensors[ACC].offset[2]);
2803 mTask.sensors[ACC].offset[0], mTask.sensors[ACC].offset[1],
2804 mTask.sensors[ACC].offset[2]);
2813 mTask.calibration_state = CALIBRATION_DONE;
2814 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC], __FUNCTION__);
2825 if (!mTask.sensors[ACC].powered && trySwitchState(SENSOR_CALIBRATING)) {
2826 mTask.calibration_state = CALIBRATION_START;
2844 mTask.sensors[ACC].offset[0] = values->hw[0];
2845 mTask.sensors[ACC].offset[1] = values->hw[1];
2846 mTask.sensors[ACC].offset[2] = values->hw[2];
2847 mTask.sensors[ACC].offset_enable = true;
2850 accelCalBiasSet(&mTask.acc, values->sw[0], values->sw[1], values->sw[2]);
2857 mTask.pending_calibration_save = true;
2892 switch (mTask.acc_test_state) {
2897 mTask
2898 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC], __FUNCTION__);
2909 SPI_READ(BMI160_REG_DATA_14, 6, &mTask.dataBuffer);
2911 mTask.acc_test_state = ACC_TEST_RUN_0;
2912 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC], __FUNCTION__);
2921 SPI_READ(BMI160_REG_DATA_14, 6, &mTask.dataBuffer);
2923 mTask.acc_test_state = ACC_TEST_RUN_1;
2924 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC], __FUNCTION__);
2929 mTask.accTestX = *(int16_t*)(mTask.dataBuffer+1);
2930 mTask.accTestY = *(int16_t*)(mTask.dataBuffer+3);
2931 mTask.accTestZ = *(int16_t*)(mTask.dataBuffer+5);
2938 SPI_READ(BMI160_REG_DATA_14, 6, &mTask.dataBuffer);
2940 mTask.acc_test_state = ACC_TEST_VERIFY;
2941 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC], __FUNCTION__);
2946 tempTestX = *(int16_t*)(mTask.dataBuffer+1);
2947 tempTestY = *(int16_t*)(mTask.dataBuffer+3);
2948 tempTestZ = *(int16_t*)(mTask.dataBuffer+5);
2951 absDiffX = ABS((int32_t)mTask.accTestX - tempTestX);
2952 absDiffY = ABS((int32_t)mTask.accTestY - tempTestY);
2953 absDiffZ = ABS((int32_t)mTask.accTestZ - tempTestZ);
2967 mTask.acc_test_state = ACC_TEST_DONE;
2968 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC], __FUNCTION__);
2982 if (!mTask.sensors[ACC].powered && trySwitchState(SENSOR_TESTING)) {
2983 mTask.acc_test_state = ACC_TEST_START;
2996 switch (mTask.calibration_state) {
3003 mTask.calibration_state = CALIBRATION_FOC;
3004 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[GYR], __FUNCTION__);
3018 SPI_READ(BMI160_REG_STATUS, 1, &mTask.statusBuffer, 50000);
3020 mTask.calibration_state = CALIBRATION_WAIT_FOC_DONE;
3021 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[GYR], __FUNCTION__);
3027 if (mTask.statusBuffer[1] & 0x08) {
3033 SPI_READ(BMI160_REG_OFFSET_3, 4, &mTask.dataBuffer);
3034 mTask.calibration_state = CALIBRATION_SET_OFFSET;
3039 SPI_READ(BMI160_REG_STATUS, 1, &mTask.statusBuffer, 50000);
3040 mTask.calibration_state = CALIBRATION_WAIT_FOC_DONE;
3043 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[GYR], __FUNCTION__);
3048 mTask.calibration_state = CALIBRATION_TIMEOUT;
3052 mTask.sensors[GYR].offset[0] = ((mTask.dataBuffer[4] & 0x03) << 8) | mTask.dataBuffer[1];
3053 mTask.sensors[GYR].offset[1] = ((mTask.dataBuffer[4] & 0x0C) << 6) | mTask.dataBuffer[2];
3054 mTask.sensors[GYR].offset[2] = ((mTask.dataBuffer[4] & 0x30) << 4) | mTask.dataBuffer[3];
3056 if (mTask.sensors[GYR].offset[0] & 0x200)
3057 mTask.sensors[GYR].offset[0] |= 0xFFFFFC00;
3058 if (mTask.sensors[GYR].offset[1] & 0x200)
3059 mTask.sensors[GYR].offset[1] |= 0xFFFFFC00;
3060 if (mTask.sensors[GYR].offset[2] & 0x200)
3061 mTask.sensors[GYR].offset[2] |= 0xFFFFFC00;
3063 mTask.sensors[GYR].offset_enable = true;
3065 (unsigned int)mTask.sensors[GYR].offset[0],
3066 (unsigned int)mTask.sensors[GYR].offset[1],
3067 (unsigned int)mTask.sensors[GYR].offset[2]);
3070 mTask.sensors[GYR].offset[0], mTask.sensors[GYR].offset[1],
3071 mTask.sensors[GYR].offset[2]);
3080 mTask.calibration_state = CALIBRATION_DONE;
3081 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[GYR], __FUNCTION__);
3092 if (!mTask.sensors[GYR].powered && trySwitchState(SENSOR_CALIBRATING)) {
3093 mTask.calibration_state = CALIBRATION_START;
3109 mTask.sensors[GYR].offset[0] = bias->hardwareBias[0];
3110 mTask.sensors[GYR].offset[1] = bias->hardwareBias[1];
3111 mTask.sensors[GYR].offset[2] = bias->hardwareBias[2];
3112 mTask.sensors[GYR].offset_enable = true;
3139 switch (mTask.gyro_test_state) {
3144 mTask.gyro_test_state = GYRO_TEST_RUN;
3145 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[GYR], __FUNCTION__);
3154 SPI_READ(BMI160_REG_STATUS, 1, &mTask.statusBuffer);
3156 mTask.gyro_test_state = GYRO_TEST_VERIFY;
3157 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[GYR], __FUNCTION__);
3162 if (mTask.statusBuffer[1] & 0x2) {
3171 mTask.gyro_test_state = GYRO_TEST_DONE;
3172 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[GYR], __FUNCTION__);
3186 if (!mTask.sensors[GYR].powered && trySwitchState(SENSOR_TESTING)) {
3187 mTask.gyro_test_state = GYRO_TEST_START;
3208 mTask.moc.x_bias = d->bias[0];
3209 mTask.moc.y_bias = d->bias[1];
3210 mTask.moc.z_bias = d->bias[2];
3211 mTask.magBiasPosted = false;
3221 diversityCheckerLocalFieldUpdate(&mTask.moc.diversity_checker, d->strength);
3273 for (i = 0; &mTask.sensors[i] != mSensor; i++) ;
3289 if (evtGeneration != mTask.poll_generation)
3292 mTask.active_poll_generation = mTask.poll_generation;
3296 SPI_READ(BMI160_REG_SENSORTIME_0, 3, &mTask.sensorTimeBuffer);
3297 SPI_READ(BMI160_REG_TEMPERATURE_0, 2, &mTask.temperatureBuffer);
3302 mTask.timesync_rtc_time = sensorGetTime();
3303 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask, __FUNCTION__);
3305 mTask.pending_time_sync = true;
3313 if (mTask.pending_int[0]) {
3314 mTask.pending_int[0] = false;
3318 if (mTask.pending_int[1]) {
3319 mTask.pending_int[1] = false;
3323 if (mTask.pending_time_sync) {
3324 mTask.pending_time_sync = false;
3329 if (mTask.pending_config[i]) {
3330 mTask.pending_config[i] = false;
3331 configEvent(&mTask.sensors[i], &mTask.sensors[i].pConfig);
3335 if (mTask.sensors[STEPCNT].flush > 0 || T(pending_step_cnt)) {
3339 if (mTask.pending_calibration_save) {
3340 mTask.pending_calibration_save = !saveCalibration();
3355 switch (mTask.init_state) {
3361 SPI_READ(BMI160_REG_MAGIC, 1, &mTask.dataBuffer, 100);
3363 mTask.init_state = INIT_BMI160;
3364 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask, "sensorInit RESET" );
3369 SPI_READ(BMI160_REG_INT_STATUS_0, 4, &mTask.statusBuffer);
3401 mTask.sensors[ACC].offset_enable = false;
3402 mTask.sensors[GYR].offset_enable = false;
3417 mTask.init_state = INIT_MAG;
3418 mTask.mag_state = MAG_SET_START;
3421 mTask.init_state = INIT_ON_CHANGE_SENSORS;
3423 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask, "sensorInit INIT");
3428 if (mTask.mag_state == MAG_SET_START) {
3431 } else if (mTask.mag_state < MAG_SET_DATA && mTask.statusBuffer[1] & 0x04) {
3434 SPI_READ(BMI160_REG_STATUS, 1, &mTask.statusBuffer, 1000);
3438 mTask.mag_state = MAG_INIT_FAILED;
3439 mTask.init_state = INIT_ON_CHANGE_SENSORS;
3446 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask, "sensorInit INIT_MAG");
3479 mTask.init_state = INIT_DONE;
3480 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask, "sensorInit INIT_ONC");
3501 SPI_READ(BMI160_REG_MAGIC, 1, &mTask.statusBuffer, 100);
3503 SPI_READ(BMI160_REG_ID, 1, &mTask.dataBuffer);
3504 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask, "spiDone SENSOR_BOOT");
3507 if (mTask.dataBuffer[1] != BMI160_ID) {
3509 ERROR_PRINT("failed id match: %02x\n", mTask.dataBuffer[1]);
3521 mTask.init_state = RESET_BMI160;
3526 if (mTask.init_state == INIT_DONE) {
3529 sensorRegisterInitComplete(mTask.sensors[i].handle);
3538 if (mSensor->idx >= FIRST_ONESHOT_SENSOR && ++mTask.active_oneshot_sensor_cnt == 1) {
3541 sensorRequest(mTask.tid, mTask.sensors[ACC].handle, SENSOR_HZ(50), SENSOR_LATENCY_NODATA);
3549 if (mSensor->idx >= FIRST_ONESHOT_SENSOR && --mTask.active_oneshot_sensor_cnt == 0) {
3552 sensorRelease(mTask.tid, mTask.sensors[ACC].handle);
3557 if (mTask.pending_dispatch) {
3558 mTask.pending_dispatch = false;
3577 if (mTask.pending_dispatch) {
3578 mTask.pending_dispatch = false;
3586 if (mTask.calibration_state == CALIBRATION_DONE) {
3589 } else if (mTask.calibration_state == CALIBRATION_TIMEOUT) {
3603 if (mTask.acc_test_state == ACC_TEST_DONE) {
3609 if (mTask.gyro_test_state == GYRO_TEST_DONE) {
3621 SensorTime = parseSensortime(mTask.sensorTimeBuffer[1] |
3622 (mTask.sensorTimeBuffer[2] << 8) | (mTask.sensorTimeBuffer[3] << 16));
3623 map_sensortime_to_rtc_time(SensorTime, mTask.timesync_rtc_time);
3625 temperature16 = (mTask.temperatureBuffer[1] | (mTask.temperatureBuffer[2] << 8));
3627 mTask.tempCelsius = kTempInvalid;
3629 mTask.tempCelsius = 23.0f + temperature16 * kScale_temp;
3630 mTask.tempTime = sensorGetTime();
3633 if (mTask.active_poll_generation == mTask.poll_generation) {
3636 (void *)mTask.poll_generation, true) == 0)
3644 mTask.dataBuffer[1], mTask.dataBuffer[2], mTask.dataBuffer[3], mTask.dataBuffer[4],
3645 mTask.dataBuffer[5], mTask.dataBuffer[6], mTask.dataBuffer[7]);
3732 osEventUnsubscribe(mTask.tid, EVT_APP_START);
3751 magCalAddBias(&mTask.moc, (mTask.last_charging_bias_x - newMagBias), 0.0, 0.0);
3753 mTask.last_charging_bias_x = newMagBias;
3754 mTask.magBiasPosted = false;
3833 osEventSubscribe(mTask.tid, EVT_APP_START);
3837 accelCalInit(&mTask.acc,
3852 gyroCalInit(&mTask.gyro_cal,
3875 overTempCalInit(&mTask.over_temp_gyro_cal,
3891 initMagCal(&mTask.moc,
3905 initMagCal(&mTask.moc,
3952 magCalDestroy(&mTask.moc);
3955 accelCalDestroy(&mTask.acc);
3959 spiMasterRelease(mTask.spiDev);
3963 disableInterrupt(mTask.Int1, mTask.Irq1, &mTask.Isr1);
3964 disableInterrupt(mTask.Int2, mTask.Irq2, &mTask.Isr2);
3965 gpioRelease(mTask.Int1);
3966 gpioRelease(mTask.Int2);
