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 <algos/time_sync.h> 18 #include <atomic.h> 19 #include <cpu/inc/cpuMath.h> 20 #include <gpio.h> 21 #include <heap.h> 22 #include <hostIntf.h> 23 #include <isr.h> 24 #include <nanohub_math.h> 25 #include <nanohubPacket.h> 26 #include <plat/inc/exti.h> 27 #include <plat/inc/gpio.h> 28 #include <plat/inc/syscfg.h> 29 #include <plat/inc/rtc.h> 30 #include <sensors.h> 31 #include <seos.h> 32 #include <slab.h> 33 #include <spi.h> 34 #include <timer.h> 35 #include <variant/inc/sensType.h> 36 #include <variant/inc/variant.h> 37 38 #ifdef MAG_SLAVE_PRESENT 39 #include <algos/mag_cal.h> 40 #endif 41 42 #include <limits.h> 43 #include <stdlib.h> 44 #include <string.h> 45 46 #define INFO_PRINT(fmt, ...) do { \ 47 osLog(LOG_INFO, "%s " fmt, "[BMI160]", ##__VA_ARGS__); \ 48 } while (0); 49 50 #define ERROR_PRINT(fmt, ...) do { \ 51 osLog(LOG_ERROR, "%s " fmt, "[BMI160] ERROR:", ##__VA_ARGS__); \ 52 } while (0); 53 54 #define DEBUG_PRINT(fmt, ...) do { \ 55 if (DBG_ENABLE) { \ 56 INFO_PRINT(fmt, ##__VA_ARGS__); \ 57 } \ 58 } while (0); 59 60 #define DEBUG_PRINT_IF(cond, fmt, ...) do { \ 61 if ((cond) && DBG_ENABLE) { \ 62 INFO_PRINT(fmt, ##__VA_ARGS__); \ 63 } \ 64 } while (0); 65 66 #define DBG_ENABLE 0 67 #define DBG_CHUNKED 0 68 #define DBG_INT 0 69 #define DBG_SHALLOW_PARSE 0 70 #define DBG_STATE 0 71 #define DBG_WM_CALC 0 72 #define TIMESTAMP_DBG 0 73 74 // fixme: to list required definitions for a slave mag 75 #ifdef USE_BMM150 76 #include "bosch_bmm150_slave.h" 77 #elif USE_AK09915 78 #include "akm_ak09915_slave.h" 79 #endif 80 81 #define BMI160_APP_ID APP_ID_MAKE(APP_ID_VENDOR_GOOGLE, 2) 82 83 #define BMI160_SPI_WRITE 0x00 84 #define BMI160_SPI_READ 0x80 85 86 #define BMI160_SPI_BUS_ID 1 87 #define BMI160_SPI_SPEED_HZ 8000000 88 #define BMI160_SPI_MODE 3 89 90 #define BMI160_INT_IRQ EXTI9_5_IRQn 91 #define BMI160_INT1_PIN GPIO_PB(6) 92 #define BMI160_INT2_PIN GPIO_PB(7) 93 94 #define BMI160_ID 0xd1 95 96 #define BMI160_REG_ID 0x00 97 #define BMI160_REG_ERR 0x02 98 #define BMI160_REG_PMU_STATUS 0x03 99 #define BMI160_REG_DATA_0 0x04 100 #define BMI160_REG_DATA_1 0x05 101 #define BMI160_REG_DATA_14 0x12 102 #define BMI160_REG_SENSORTIME_0 0x18 103 #define BMI160_REG_STATUS 0x1b 104 #define BMI160_REG_INT_STATUS_0 0x1c 105 #define BMI160_REG_INT_STATUS_1 0x1d 106 #define BMI160_REG_TEMPERATURE_0 0x20 107 #define BMI160_REG_TEMPERATURE_1 0x21 108 #define BMI160_REG_FIFO_LENGTH_0 0x22 109 #define BMI160_REG_FIFO_DATA 0x24 110 #define BMI160_REG_ACC_CONF 0x40 111 #define BMI160_REG_ACC_RANGE 0x41 112 #define BMI160_REG_GYR_CONF 0x42 113 #define BMI160_REG_GYR_RANGE 0x43 114 #define BMI160_REG_MAG_CONF 0x44 115 #define BMI160_REG_FIFO_DOWNS 0x45 116 #define BMI160_REG_FIFO_CONFIG_0 0x46 117 #define BMI160_REG_FIFO_CONFIG_1 0x47 118 #define BMI160_REG_MAG_IF_0 0x4b 119 #define BMI160_REG_MAG_IF_1 0x4c 120 #define BMI160_REG_MAG_IF_2 0x4d 121 #define BMI160_REG_MAG_IF_3 0x4e 122 #define BMI160_REG_MAG_IF_4 0x4f 123 #define BMI160_REG_INT_EN_0 0x50 124 #define BMI160_REG_INT_EN_1 0x51 125 #define BMI160_REG_INT_EN_2 0x52 126 #define BMI160_REG_INT_OUT_CTRL 0x53 127 #define BMI160_REG_INT_LATCH 0x54 128 #define BMI160_REG_INT_MAP_0 0x55 129 #define BMI160_REG_INT_MAP_1 0x56 130 #define BMI160_REG_INT_MAP_2 0x57 131 #define BMI160_REG_INT_DATA_0 0x58 132 #define BMI160_REG_INT_MOTION_0 0x5f 133 #define BMI160_REG_INT_MOTION_1 0x60 134 #define BMI160_REG_INT_MOTION_2 0x61 135 #define BMI160_REG_INT_MOTION_3 0x62 136 #define BMI160_REG_INT_TAP_0 0x63 137 #define BMI160_REG_INT_TAP_1 0x64 138 #define BMI160_REG_INT_FLAT_0 0x67 139 #define BMI160_REG_INT_FLAT_1 0x68 140 #define BMI160_REG_PMU_TRIGGER 0x6C 141 #define BMI160_REG_FOC_CONF 0x69 142 #define BMI160_REG_CONF 0x6a 143 #define BMI160_REG_IF_CONF 0x6b 144 #define BMI160_REG_SELF_TEST 0x6d 145 #define BMI160_REG_OFFSET_0 0x71 146 #define BMI160_REG_OFFSET_3 0x74 147 #define BMI160_REG_OFFSET_6 0x77 148 #define BMI160_REG_STEP_CNT_0 0x78 149 #define BMI160_REG_STEP_CONF_0 0x7a 150 #define BMI160_REG_STEP_CONF_1 0x7b 151 #define BMI160_REG_CMD 0x7e 152 #define BMI160_REG_MAGIC 0x7f 153 154 #define INT_STEP 0x01 155 #define INT_ANY_MOTION 0x04 156 #define INT_DOUBLE_TAP 0x10 157 #define INT_SINGLE_TAP 0x20 158 #define INT_ORIENT 0x40 159 #define INT_FLAT 0x80 160 #define INT_HIGH_G_Z 0x04 161 #define INT_LOW_G 0x08 162 #define INT_DATA_RDY 0x10 163 #define INT_FIFO_FULL 0x20 164 #define INT_FIFO_WM 0x40 165 #define INT_NO_MOTION 0x80 166 167 #define BMI160_FRAME_HEADER_INVALID 0x80 // mark the end of valid data 168 #define BMI160_FRAME_HEADER_SKIP 0x81 // not defined by hw, used for skip a byte in buffer 169 170 #define WATERMARK_MIN 1 171 #define WATERMARK_MAX 200 // must <= 255 (0xff) 172 173 #define WATERMARK_MAX_SENSOR_RATE 400 // Accel and gyro are 400 Hz max 174 #define WATERMARK_TIME_UNIT_NS (1000000000ULL/(WATERMARK_MAX_SENSOR_RATE)) 175 176 #define gSPI BMI160_SPI_BUS_ID 177 178 #define ACCL_INT_LINE EXTI_LINE_P6 179 #define GYR_INT_LINE EXTI_LINE_P7 180 181 #define SPI_WRITE_0(addr, data) spiQueueWrite(addr, data, 2) 182 #define SPI_WRITE_1(addr, data, delay) spiQueueWrite(addr, data, delay) 183 #define GET_SPI_WRITE_MACRO(_1,_2,_3,NAME,...) NAME 184 #define SPI_WRITE(...) GET_SPI_WRITE_MACRO(__VA_ARGS__, SPI_WRITE_1, SPI_WRITE_0)(__VA_ARGS__) 185 186 #define SPI_READ_0(addr, size, buf) spiQueueRead(addr, size, buf, 0) 187 #define SPI_READ_1(addr, size, buf, delay) spiQueueRead(addr, size, buf, delay) 188 #define GET_SPI_READ_MACRO(_1,_2,_3,_4,NAME,...) NAME 189 #define SPI_READ(...) GET_SPI_READ_MACRO(__VA_ARGS__, SPI_READ_1, SPI_READ_0)(__VA_ARGS__) 190 191 #define EVT_SENSOR_ACC_DATA_RDY sensorGetMyEventType(SENS_TYPE_ACCEL) 192 #define EVT_SENSOR_GYR_DATA_RDY sensorGetMyEventType(SENS_TYPE_GYRO) 193 #define EVT_SENSOR_MAG_DATA_RDY sensorGetMyEventType(SENS_TYPE_MAG) 194 #define EVT_SENSOR_STEP sensorGetMyEventType(SENS_TYPE_STEP_DETECT) 195 #define EVT_SENSOR_NO_MOTION sensorGetMyEventType(SENS_TYPE_NO_MOTION) 196 #define EVT_SENSOR_ANY_MOTION sensorGetMyEventType(SENS_TYPE_ANY_MOTION) 197 #define EVT_SENSOR_FLAT sensorGetMyEventType(SENS_TYPE_FLAT) 198 #define EVT_SENSOR_DOUBLE_TAP sensorGetMyEventType(SENS_TYPE_DOUBLE_TAP) 199 #define EVT_SENSOR_STEP_COUNTER sensorGetMyEventType(SENS_TYPE_STEP_COUNT) 200 201 #define MAX_NUM_COMMS_EVENT_SAMPLES 15 202 203 #define kScale_acc 0.00239501953f // ACC_range * 9.81f / 32768.0f; 204 #define kScale_gyr 0.00106472439f // GYR_range * M_PI / (180.0f * 32768.0f); 205 #define kScale_temp 0.001953125f // temperature in deg C 206 #define kTempInvalid -1000.0f 207 208 #define kTimeSyncPeriodNs 100000000ull // sync sensor and RTC time every 100ms 209 #define kSensorTimerIntervalUs 39ull // bmi160 clock increaments every 39000ns 210 211 #define kMinRTCTimeIncrementNs 1250000ull // forced min rtc time increment, 1.25ms for 400Hz 212 #define kMinSensorTimeIncrement 64 // forced min sensortime increment, 213 // 64 = 2.5 msec for 400Hz 214 215 #define ACC_MIN_RATE 5 216 #define GYR_MIN_RATE 6 217 #define ACC_MAX_RATE 12 218 #define GYR_MAX_RATE 13 219 #define MAG_MAX_RATE 11 220 #define ACC_MAX_OSR 3 221 #define GYR_MAX_OSR 4 222 #define OSR_THRESHOLD 8 223 224 #define MOTION_ODR 7 225 226 #define RETRY_CNT_CALIBRATION 10 227 #define RETRY_CNT_ID 5 228 #define RETRY_CNT_MAG 30 229 230 #define SPI_PACKET_SIZE 30 231 #define FIFO_READ_SIZE (1024+4) 232 #define CHUNKED_READ_SIZE (64) 233 #define BUF_MARGIN 32 // some extra buffer for additional reg RW when a FIFO read happens 234 #define SPI_BUF_SIZE (FIFO_READ_SIZE + CHUNKED_READ_SIZE + BUF_MARGIN) 235 236 enum SensorIndex { 237 ACC = 0, 238 GYR, 239 MAG, 240 STEP, 241 DTAP, 242 FLAT, 243 ANYMO, 244 NOMO, 245 STEPCNT, 246 NUM_OF_SENSOR, 247 }; 248 249 enum SensorEvents { 250 NO_EVT = -1, 251 EVT_SPI_DONE = EVT_APP_START + 1, 252 EVT_SENSOR_INTERRUPT_1, 253 EVT_SENSOR_INTERRUPT_2, 254 EVT_TIME_SYNC, 255 }; 256 257 enum InitState { 258 RESET_BMI160, 259 INIT_BMI160, 260 INIT_MAG, 261 INIT_ON_CHANGE_SENSORS, 262 INIT_DONE, 263 }; 264 265 enum CalibrationState { 266 CALIBRATION_START, 267 CALIBRATION_FOC, 268 CALIBRATION_WAIT_FOC_DONE, 269 CALIBRATION_SET_OFFSET, 270 CALIBRATION_DONE, 271 CALIBRATION_TIMEOUT, 272 }; 273 274 enum SensorState { 275 SENSOR_BOOT, 276 SENSOR_VERIFY_ID, 277 SENSOR_INITIALIZING, 278 SENSOR_IDLE, 279 SENSOR_POWERING_UP, 280 SENSOR_POWERING_DOWN, 281 SENSOR_CONFIG_CHANGING, 282 SENSOR_INT_1_HANDLING, 283 SENSOR_INT_2_HANDLING, 284 SENSOR_CALIBRATING, 285 SENSOR_STEP_CNT, 286 SENSOR_TIME_SYNC, 287 SENSOR_SAVE_CALIBRATION, 288 SENSOR_NUM_OF_STATE 289 }; 290 static const char * getStateName(int32_t s) { 291 #if DBG_STATE 292 static const char* const l[] = {"BOOT", "VERIFY_ID", "INIT", "IDLE", "PWR_UP", 293 "PWR-DN", "CFG_CHANGE", "INT1", "INT2", "CALIB", "STEP_CNT", "SYNC", "SAVE_CALIB"}; 294 if (s >= 0 && s < SENSOR_NUM_OF_STATE) { 295 return l[s]; 296 } 297 #endif 298 return "???"; 299 } 300 301 enum MagConfigState { 302 MAG_SET_START, 303 MAG_SET_IF, 304 305 // BMM150 only 306 MAG_SET_REPXY, 307 MAG_SET_REPZ, 308 MAG_GET_DIG_X, 309 MAG_GET_DIG_Y, 310 MAG_GET_DIG_Z, 311 MAG_SET_SAVE_DIG, 312 313 MAG_SET_FORCE, 314 MAG_SET_ADDR, 315 MAG_SET_DATA, 316 MAG_SET_DONE, 317 318 MAG_INIT_FAILED 319 }; 320 321 struct ConfigStat { 322 uint64_t latency; 323 uint32_t rate; 324 bool enable; 325 }; 326 327 struct CalibrationData { 328 struct HostHubRawPacket header; 329 struct SensorAppEventHeader data_header; 330 int32_t xBias; 331 int32_t yBias; 332 int32_t zBias; 333 } __attribute__((packed)); 334 335 struct BMI160Sensor { 336 struct ConfigStat pConfig; // pending config status request 337 struct TripleAxisDataEvent *data_evt; 338 uint32_t handle; 339 uint32_t rate; 340 uint64_t latency; 341 uint64_t prev_rtc_time; 342 uint32_t offset[3]; 343 bool powered; // activate status 344 bool configed; // configure status 345 bool offset_enable; 346 uint8_t flush; 347 enum SensorIndex idx; 348 }; 349 350 struct BMI160Task { 351 uint32_t tid; 352 struct BMI160Sensor sensors[NUM_OF_SENSOR]; 353 354 // time keeping. 355 uint64_t last_sensortime; 356 uint64_t frame_sensortime; 357 uint64_t prev_frame_time[3]; 358 uint64_t time_delta[3]; 359 uint64_t next_delta[3]; 360 uint64_t tempTime; 361 362 // spi and interrupt 363 spi_cs_t cs; 364 struct SpiMode mode; 365 struct SpiPacket packets[SPI_PACKET_SIZE]; 366 struct SpiDevice *spiDev; 367 struct Gpio *Int1; 368 struct Gpio *Int2; 369 struct ChainedIsr Isr1; 370 struct ChainedIsr Isr2; 371 #ifdef MAG_SLAVE_PRESENT 372 struct MagCal moc; 373 #endif 374 time_sync_t gSensorTime2RTC; 375 376 float tempCelsius; 377 float last_charging_bias_x; 378 uint32_t total_step_cnt; 379 uint32_t last_step_cnt; 380 uint32_t poll_generation; 381 uint32_t active_poll_generation; 382 uint8_t active_oneshot_sensor_cnt; 383 uint8_t interrupt_enable_0; 384 uint8_t interrupt_enable_2; 385 uint8_t acc_downsample; 386 uint8_t gyr_downsample; 387 bool magBiasPosted; 388 bool magBiasCurrent; 389 bool fifo_enabled[3]; 390 391 // for step count 392 uint32_t stepCntSamplingTimerHandle; 393 bool step_cnt_changed; 394 395 // spi buffers 396 int xferCnt; 397 uint8_t *dataBuffer; 398 uint8_t *statusBuffer; 399 uint8_t *sensorTimeBuffer; 400 uint8_t *temperatureBuffer; 401 uint8_t txrxBuffer[SPI_BUF_SIZE]; 402 403 // states 404 volatile uint8_t state; //task state, type enum SensorState, do NOT change this directly 405 enum InitState init_state; 406 enum MagConfigState mag_state; 407 enum CalibrationState calibration_state; 408 409 // pending configs 410 bool pending_int[2]; 411 bool pending_step_cnt; 412 bool pending_config[NUM_OF_SENSOR]; 413 bool pending_calibration_save; 414 bool pending_time_sync; 415 bool pending_delta[3]; 416 bool pending_dispatch; 417 bool frame_sensortime_valid; 418 419 // FIFO setting 420 uint16_t chunkReadSize; 421 uint8_t watermark; 422 423 // spi rw 424 struct SlabAllocator *mDataSlab; 425 uint16_t mWbufCnt; 426 uint8_t mRegCnt; 427 uint8_t mRetryLeft; 428 bool spiInUse; 429 }; 430 431 static uint32_t AccRates[] = { 432 SENSOR_HZ(25.0f/8.0f), 433 SENSOR_HZ(25.0f/4.0f), 434 SENSOR_HZ(25.0f/2.0f), 435 SENSOR_HZ(25.0f), 436 SENSOR_HZ(50.0f), 437 SENSOR_HZ(100.0f), 438 SENSOR_HZ(200.0f), 439 SENSOR_HZ(400.0f), 440 0, 441 }; 442 443 static uint32_t GyrRates[] = { 444 SENSOR_HZ(25.0f/8.0f), 445 SENSOR_HZ(25.0f/4.0f), 446 SENSOR_HZ(25.0f/2.0f), 447 SENSOR_HZ(25.0f), 448 SENSOR_HZ(50.0f), 449 SENSOR_HZ(100.0f), 450 SENSOR_HZ(200.0f), 451 SENSOR_HZ(400.0f), 452 0, 453 }; 454 455 static uint32_t MagRates[] = { 456 SENSOR_HZ(25.0f/8.0f), 457 SENSOR_HZ(25.0f/4.0f), 458 SENSOR_HZ(25.0f/2.0f), 459 SENSOR_HZ(25.0f), 460 SENSOR_HZ(50.0f), 461 SENSOR_HZ(100.0f), 462 0, 463 }; 464 465 static uint32_t StepCntRates[] = { 466 SENSOR_HZ(1.0f/300.0f), 467 SENSOR_HZ(1.0f/240.0f), 468 SENSOR_HZ(1.0f/180.0f), 469 SENSOR_HZ(1.0f/120.0f), 470 SENSOR_HZ(1.0f/90.0f), 471 SENSOR_HZ(1.0f/60.0f), 472 SENSOR_HZ(1.0f/45.0f), 473 SENSOR_HZ(1.0f/30.0f), 474 SENSOR_HZ(1.0f/15.0f), 475 SENSOR_HZ(1.0f/10.0f), 476 SENSOR_HZ(1.0f/5.0f), 477 SENSOR_RATE_ONCHANGE, 478 0 479 }; 480 481 static const uint64_t stepCntRateTimerVals[] = // should match StepCntRates and be the timer length for that rate in nanosecs 482 { 483 300 * 1000000000ULL, 484 240 * 1000000000ULL, 485 180 * 1000000000ULL, 486 120 * 1000000000ULL, 487 90 * 1000000000ULL, 488 60 * 1000000000ULL, 489 45 * 1000000000ULL, 490 30 * 1000000000ULL, 491 15 * 1000000000ULL, 492 10 * 1000000000ULL, 493 5 * 1000000000ULL, 494 }; 495 496 static struct BMI160Task mTask; 497 498 #ifdef MAG_SLAVE_PRESENT 499 static struct MagTask magTask; 500 #endif 501 502 #define MAG_WRITE(addr, data) \ 503 do { \ 504 SPI_WRITE(BMI160_REG_MAG_IF_4, data); \ 505 SPI_WRITE(BMI160_REG_MAG_IF_3, addr); \ 506 } while (0) 507 508 #define MAG_READ(addr, size) \ 509 do { \ 510 SPI_WRITE(BMI160_REG_MAG_IF_2, addr, 5000); \ 511 SPI_READ(BMI160_REG_DATA_0, size, &mTask.dataBuffer); \ 512 } while (0) 513 514 #define DEC_INFO(name, type, axis, inter, samples) \ 515 .sensorName = name, \ 516 .sensorType = type, \ 517 .numAxis = axis, \ 518 .interrupt = inter, \ 519 .minSamples = samples 520 521 #define DEC_INFO_RATE(name, rates, type, axis, inter, samples) \ 522 DEC_INFO(name, type, axis, inter, samples), \ 523 .supportedRates = rates 524 525 #define DEC_INFO_RATE_RAW(name, rates, type, axis, inter, samples, raw, scale) \ 526 DEC_INFO(name, type, axis, inter, samples), \ 527 .supportedRates = rates, \ 528 .flags1 = SENSOR_INFO_FLAGS1_RAW, \ 529 .rawType = raw, \ 530 .rawScale = scale 531 532 #define DEC_INFO_RATE_BIAS(name, rates, type, axis, inter, samples, bias) \ 533 DEC_INFO(name, type, axis, inter, samples), \ 534 .supportedRates = rates, \ 535 .flags1 = SENSOR_INFO_FLAGS1_BIAS, \ 536 .biasType = bias 537 538 typedef struct BMI160Task _Task; 539 #define TASK _Task* const _task 540 541 // To get rid of static variables all task functions should have a task structure pointer input. 542 // This is an intermediate step. 543 #define TDECL() TASK = &mTask; (void)_task 544 545 // Access task variables without explicitly specify the task structure pointer. 546 #define T(v) (_task->v) 547 548 // Atomic get state 549 #define GET_STATE() (atomicReadByte(&(_task->state))) 550 551 // Atomic set state, this set the state to arbitrary value, use with caution 552 #define SET_STATE(s) do{\ 553 DEBUG_PRINT_IF(DBG_STATE, "set state %s\n", getStateName(s));\ 554 atomicWriteByte(&(_task->state), (s));\ 555 }while(0) 556 557 // Atomic switch state from IDLE to desired state. 558 static bool trySwitchState_(TASK, enum SensorState newState) { 559 #if DBG_STATE 560 bool ret = atomicCmpXchgByte(&T(state), SENSOR_IDLE, newState); 561 uint8_t prevState = ret ? SENSOR_IDLE : GET_STATE(); 562 DEBUG_PRINT("switch state %s->%s, %s\n", 563 getStateName(prevState), getStateName(newState), ret ? "ok" : "failed"); 564 return ret; 565 #else 566 return atomicCmpXchgByte(&T(state), SENSOR_IDLE, newState); 567 #endif 568 } 569 // Short-hand 570 #define trySwitchState(s) trySwitchState_(_task, (s)) 571 572 // Chunked FIFO read functions 573 static void chunkedReadInit_(TASK, int index, int size); 574 #define chunkedReadInit(a,b) chunkedReadInit_(_task, (a), (b)) 575 static void chunkedReadSpiCallback(void *cookie, int error); 576 static void initiateFifoRead_(TASK, bool isInterruptContext); 577 #define initiateFifoRead(a) initiateFifoRead_(_task, (a)) 578 static uint8_t* shallowParseFrame(uint8_t * buf, int size); 579 580 // Watermark calculation 581 static uint8_t calcWatermark2_(TASK); 582 #define calcWatermark2() calcWatermark2_(_task) 583 584 static const struct SensorInfo mSensorInfo[NUM_OF_SENSOR] = 585 { 586 { DEC_INFO_RATE_RAW("Accelerometer", AccRates, SENS_TYPE_ACCEL, NUM_AXIS_THREE, 587 NANOHUB_INT_NONWAKEUP, 3000, SENS_TYPE_ACCEL_RAW, 1.0/kScale_acc) }, 588 { DEC_INFO_RATE("Gyroscope", GyrRates, SENS_TYPE_GYRO, NUM_AXIS_THREE, 589 NANOHUB_INT_NONWAKEUP, 20) }, 590 { DEC_INFO_RATE_BIAS("Magnetometer", MagRates, SENS_TYPE_MAG, NUM_AXIS_THREE, 591 NANOHUB_INT_NONWAKEUP, 600, SENS_TYPE_MAG_BIAS) }, 592 { DEC_INFO("Step Detector", SENS_TYPE_STEP_DETECT, NUM_AXIS_EMBEDDED, 593 NANOHUB_INT_NONWAKEUP, 100) }, 594 { DEC_INFO("Double Tap", SENS_TYPE_DOUBLE_TAP, NUM_AXIS_EMBEDDED, 595 NANOHUB_INT_NONWAKEUP, 20) }, 596 { DEC_INFO("Flat", SENS_TYPE_FLAT, NUM_AXIS_EMBEDDED, NANOHUB_INT_NONWAKEUP, 20) }, 597 { DEC_INFO("Any Motion", SENS_TYPE_ANY_MOTION, NUM_AXIS_EMBEDDED, NANOHUB_INT_NONWAKEUP, 20) }, 598 { DEC_INFO("No Motion", SENS_TYPE_NO_MOTION, NUM_AXIS_EMBEDDED, NANOHUB_INT_NONWAKEUP, 20) }, 599 { DEC_INFO_RATE("Step Counter", StepCntRates, SENS_TYPE_STEP_COUNT, NUM_AXIS_EMBEDDED, 600 NANOHUB_INT_NONWAKEUP, 20) }, 601 }; 602 603 static void time_init(void) { 604 time_sync_init(&mTask.gSensorTime2RTC); 605 } 606 607 static bool sensortime_to_rtc_time(uint64_t sensor_time, uint64_t *rtc_time_ns) { 608 // fixme: nsec? 609 return time_sync_estimate_time1( 610 &mTask.gSensorTime2RTC, sensor_time * 39ull, rtc_time_ns); 611 } 612 613 static void map_sensortime_to_rtc_time(uint64_t sensor_time, uint64_t rtc_time_ns) { 614 // fixme: nsec? 615 time_sync_add(&mTask.gSensorTime2RTC, rtc_time_ns, sensor_time * 39ull); 616 } 617 618 static void invalidate_sensortime_to_rtc_time(void) { 619 time_sync_reset(&mTask.gSensorTime2RTC); 620 } 621 622 static void minimize_sensortime_history(void) { 623 // truncate datapoints to the latest two to maintain valid sensortime to rtc 624 // mapping and minimize the inflence of the past mapping 625 time_sync_truncate(&mTask.gSensorTime2RTC, 2); 626 627 // drop the oldest datapoint when a new one arrives for two times to 628 // completely shift out the influence of the past mapping 629 time_sync_hold(&mTask.gSensorTime2RTC, 2); 630 } 631 632 static void dataEvtFree(void *ptr) 633 { 634 TDECL(); 635 struct TripleAxisDataEvent *ev = (struct TripleAxisDataEvent *)ptr; 636 slabAllocatorFree(T(mDataSlab), ev); 637 } 638 639 static void spiQueueWrite(uint8_t addr, uint8_t data, uint32_t delay) 640 { 641 TDECL(); 642 if (T(spiInUse)) { 643 ERROR_PRINT("SPI in use, cannot queue write\n"); 644 return; 645 } 646 T(packets[T(mRegCnt)]).size = 2; 647 T(packets[T(mRegCnt)]).txBuf = &T(txrxBuffer[T(mWbufCnt)]); 648 T(packets[T(mRegCnt)]).rxBuf = &T(txrxBuffer[T(mWbufCnt)]); 649 T(packets[T(mRegCnt)]).delay = delay * 1000; 650 T(txrxBuffer[T(mWbufCnt++)]) = BMI160_SPI_WRITE | addr; 651 T(txrxBuffer[T(mWbufCnt++)]) = data; 652 T(mRegCnt)++; 653 } 654 655 /* 656 * need to be sure size of buf is larger than read size 657 */ 658 static void spiQueueRead(uint8_t addr, size_t size, uint8_t **buf, uint32_t delay) 659 { 660 TDECL(); 661 if (T(spiInUse)) { 662 ERROR_PRINT("SPI in use, cannot queue read %d %d\n", (int)addr, (int)size); 663 return; 664 } 665 666 *buf = &T(txrxBuffer[T(mWbufCnt)]); 667 T(packets[T(mRegCnt)]).size = size + 1; // first byte will not contain valid data 668 T(packets[T(mRegCnt)]).txBuf = &T(txrxBuffer[T(mWbufCnt)]); 669 T(packets[T(mRegCnt)]).rxBuf = *buf; 670 T(packets[T(mRegCnt)]).delay = delay * 1000; 671 T(txrxBuffer[T(mWbufCnt)++]) = BMI160_SPI_READ | addr; 672 T(mWbufCnt) += size; 673 T(mRegCnt)++; 674 } 675 676 static void spiBatchTxRx(struct SpiMode *mode, 677 SpiCbkF callback, void *cookie, const char * src) 678 { 679 TDECL(); 680 if (T(mWbufCnt) > SPI_BUF_SIZE) { 681 ERROR_PRINT("NO enough SPI buffer space, dropping transaction.\n"); 682 return; 683 } 684 if (T(mRegCnt) > SPI_PACKET_SIZE) { 685 ERROR_PRINT("spiBatchTxRx too many packets!\n"); 686 return; 687 } 688 689 T(spiInUse) = true; 690 691 // Reset variables before issuing SPI transaction. 692 // SPI may finish before spiMasterRxTx finish 693 uint8_t regCount = T(mRegCnt); 694 T(mRegCnt) = 0; 695 T(mWbufCnt) = 0; 696 697 if (spiMasterRxTx(T(spiDev), T(cs), T(packets), regCount, mode, callback, cookie) < 0) { 698 ERROR_PRINT("spiMasterRxTx failed!\n"); 699 } 700 } 701 702 703 static bool bmi160Isr1(struct ChainedIsr *isr) 704 { 705 TASK = container_of(isr, struct BMI160Task, Isr1); 706 707 if (!extiIsPendingGpio(T(Int1))) { 708 return false; 709 } 710 DEBUG_PRINT_IF(DBG_INT, "i1\n"); 711 initiateFifoRead(true /*isInterruptContext*/); 712 extiClearPendingGpio(T(Int1)); 713 return true; 714 } 715 716 717 static bool bmi160Isr2(struct ChainedIsr *isr) 718 { 719 TASK = container_of(isr, struct BMI160Task, Isr2); 720 721 if (!extiIsPendingGpio(T(Int2))) 722 return false; 723 724 DEBUG_PRINT_IF(DBG_INT, "i2\n"); 725 osEnqueuePrivateEvt(EVT_SENSOR_INTERRUPT_2, _task, NULL, T(tid)); 726 extiClearPendingGpio(T(Int2)); 727 return true; 728 } 729 730 static void sensorSpiCallback(void *cookie, int err) 731 { 732 mTask.spiInUse = false; 733 osEnqueuePrivateEvt(EVT_SPI_DONE, cookie, NULL, mTask.tid); 734 } 735 736 static void sensorTimerCallback(uint32_t timerId, void *data) 737 { 738 osEnqueuePrivateEvt(EVT_SPI_DONE, data, NULL, mTask.tid); 739 } 740 741 static void timeSyncCallback(uint32_t timerId, void *data) 742 { 743 osEnqueuePrivateEvt(EVT_TIME_SYNC, data, NULL, mTask.tid); 744 } 745 746 static void stepCntSamplingCallback(uint32_t timerId, void *data) 747 { 748 union EmbeddedDataPoint step_cnt; 749 750 if (mTask.sensors[STEPCNT].powered && mTask.step_cnt_changed) { 751 mTask.step_cnt_changed = false; 752 step_cnt.idata = mTask.total_step_cnt; 753 osEnqueueEvt(EVT_SENSOR_STEP_COUNTER, step_cnt.vptr, NULL); 754 } 755 } 756 757 static bool accFirmwareUpload(void *cookie) 758 { 759 sensorSignalInternalEvt(mTask.sensors[ACC].handle, 760 SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0); 761 return true; 762 } 763 764 static bool gyrFirmwareUpload(void *cookie) 765 { 766 sensorSignalInternalEvt(mTask.sensors[GYR].handle, 767 SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0); 768 return true; 769 } 770 771 static bool magFirmwareUpload(void *cookie) 772 { 773 sensorSignalInternalEvt(mTask.sensors[MAG].handle, 774 SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0); 775 return true; 776 } 777 778 static bool stepFirmwareUpload(void *cookie) 779 { 780 sensorSignalInternalEvt(mTask.sensors[STEP].handle, 781 SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0); 782 return true; 783 } 784 785 static bool doubleTapFirmwareUpload(void *cookie) 786 { 787 sensorSignalInternalEvt(mTask.sensors[DTAP].handle, 788 SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0); 789 return true; 790 } 791 792 static bool noMotionFirmwareUpload(void *cookie) 793 { 794 sensorSignalInternalEvt(mTask.sensors[NOMO].handle, 795 SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0); 796 return true; 797 } 798 799 static bool anyMotionFirmwareUpload(void *cookie) 800 { 801 sensorSignalInternalEvt(mTask.sensors[ANYMO].handle, 802 SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0); 803 return true; 804 } 805 806 static bool flatFirmwareUpload(void *cookie) 807 { 808 sensorSignalInternalEvt(mTask.sensors[FLAT].handle, 809 SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0); 810 return true; 811 } 812 813 static bool stepCntFirmwareUpload(void *cookie) 814 { 815 sensorSignalInternalEvt(mTask.sensors[STEPCNT].handle, 816 SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0); 817 return true; 818 } 819 820 static bool enableInterrupt(struct Gpio *pin, struct ChainedIsr *isr) 821 { 822 gpioConfigInput(pin, GPIO_SPEED_LOW, GPIO_PULL_NONE); 823 syscfgSetExtiPort(pin); 824 extiEnableIntGpio(pin, EXTI_TRIGGER_RISING); 825 extiChainIsr(BMI160_INT_IRQ, isr); 826 return true; 827 } 828 829 static bool disableInterrupt(struct Gpio *pin, struct ChainedIsr *isr) 830 { 831 extiUnchainIsr(BMI160_INT_IRQ, isr); 832 extiDisableIntGpio(pin); 833 return true; 834 } 835 836 static void magConfigMagic(void) 837 { 838 // set the MAG power to NORMAL mode 839 SPI_WRITE(BMI160_REG_CMD, 0x19, 10000); 840 841 // Magic register sequence to shift register page table to access hidden 842 // register 843 SPI_WRITE(BMI160_REG_CMD, 0x37); 844 SPI_WRITE(BMI160_REG_CMD, 0x9a); 845 SPI_WRITE(BMI160_REG_CMD, 0xc0); 846 SPI_WRITE(BMI160_REG_MAGIC, 0x90); 847 SPI_READ(BMI160_REG_DATA_1, 1, &mTask.dataBuffer); 848 } 849 850 static void magConfigIf(void) 851 { 852 // Set the on-chip I2C pull-up register settings and shift the register 853 // table back down (magic) 854 SPI_WRITE(BMI160_REG_DATA_1, mTask.dataBuffer[1] | 0x30); 855 SPI_WRITE(BMI160_REG_MAGIC, 0x80); 856 857 // Config the MAG I2C device address 858 #ifdef MAG_SLAVE_PRESENT 859 SPI_WRITE(BMI160_REG_MAG_IF_0, (MAG_I2C_ADDR << 1)); 860 #endif 861 862 // set mag_manual_enable, mag_offset=0, mag_rd_burst='8 bytes' 863 SPI_WRITE(BMI160_REG_MAG_IF_1, 0x83); 864 865 // primary interface: autoconfig, secondary: magnetometer. 866 SPI_WRITE(BMI160_REG_IF_CONF, 0x20); 867 868 // fixme: move to mag-specific function 869 #ifdef USE_BMM150 870 // set mag to SLEEP mode 871 MAG_WRITE(BMM150_REG_CTRL_1, 0x01); 872 #elif USE_AK09915 873 // set "low" Noise Suppression Filter (NSF) settings 874 MAG_WRITE(AKM_AK09915_REG_CNTL1, 0x20); 875 #endif 876 } 877 878 // fixme: break this up to master/slave-specific, so it'll be eventually slave-agnostic, 879 // and slave provides its own stateless config function 880 // fixme: not all async_elem_t is supported 881 static void magConfig(void) 882 { 883 switch (mTask.mag_state) { 884 case MAG_SET_START: 885 magConfigMagic(); 886 mTask.mag_state = MAG_SET_IF; 887 break; 888 case MAG_SET_IF: 889 magConfigIf(); 890 #ifdef USE_AK09915 891 mTask.mag_state = MAG_SET_FORCE; 892 #elif USE_BMM150 893 mTask.mag_state = MAG_SET_REPXY; 894 #endif 895 break; 896 897 #ifdef USE_BMM150 898 case MAG_SET_REPXY: 899 // MAG_SET_REPXY and MAG_SET_REPZ case set: 900 // regular preset, f_max,ODR ~ 102 Hz 901 MAG_WRITE(BMM150_REG_REPXY, 9); 902 mTask.mag_state = MAG_SET_REPZ; 903 break; 904 case MAG_SET_REPZ: 905 MAG_WRITE(BMM150_REG_REPZ, 15); 906 mTask.mag_state = MAG_GET_DIG_X; 907 break; 908 case MAG_GET_DIG_X: 909 // MAG_GET_DIG_X, MAG_GET_DIG_Y and MAG_GET_DIG_Z cases: 910 // save parameters for temperature compensation. 911 MAG_READ(BMM150_REG_DIG_X1, 8); 912 mTask.mag_state = MAG_GET_DIG_Y; 913 break; 914 case MAG_GET_DIG_Y: 915 bmm150SaveDigData(&magTask, &mTask.dataBuffer[1], 0); 916 MAG_READ(BMM150_REG_DIG_X1 + 8, 8); 917 mTask.mag_state = MAG_GET_DIG_Z; 918 break; 919 case MAG_GET_DIG_Z: 920 bmm150SaveDigData(&magTask, &mTask.dataBuffer[1], 8); 921 MAG_READ(BMM150_REG_DIG_X1 + 16, 8); 922 mTask.mag_state = MAG_SET_SAVE_DIG; 923 break; 924 case MAG_SET_SAVE_DIG: 925 bmm150SaveDigData(&magTask, &mTask.dataBuffer[1], 16); 926 // fall through, no break; 927 mTask.mag_state = MAG_SET_FORCE; 928 #endif 929 930 case MAG_SET_FORCE: 931 // set MAG mode to "forced". ready to pull data 932 #ifdef USE_AK09915 933 MAG_WRITE(AKM_AK09915_REG_CNTL2, 0x01); 934 #elif USE_BMM150 935 MAG_WRITE(BMM150_REG_CTRL_2, 0x02); 936 #endif 937 mTask.mag_state = MAG_SET_ADDR; 938 break; 939 case MAG_SET_ADDR: 940 // config MAG read data address to the first data register 941 #ifdef MAG_SLAVE_PRESENT 942 SPI_WRITE(BMI160_REG_MAG_IF_2, MAG_REG_DATA); 943 #endif 944 mTask.mag_state = MAG_SET_DATA; 945 break; 946 case MAG_SET_DATA: 947 // clear mag_manual_en. 948 SPI_WRITE(BMI160_REG_MAG_IF_1, 0x03, 1000); 949 // set the MAG power to SUSPEND mode 950 SPI_WRITE(BMI160_REG_CMD, 0x18, 10000); 951 mTask.mag_state = MAG_SET_DONE; 952 mTask.init_state = INIT_ON_CHANGE_SENSORS; 953 break; 954 default: 955 break; 956 } 957 SPI_READ(BMI160_REG_STATUS, 1, &mTask.statusBuffer, 1000); 958 } 959 960 static inline bool anyFifoEnabled(void) 961 { 962 return (mTask.fifo_enabled[ACC] || mTask.fifo_enabled[GYR] || mTask.fifo_enabled[MAG]); 963 } 964 965 static void configFifo(void) 966 { 967 TDECL(); 968 int i; 969 uint8_t val = 0x12; 970 bool any_fifo_enabled_prev = anyFifoEnabled(); 971 // if ACC is configed, enable ACC bit in fifo_config reg. 972 if (mTask.sensors[ACC].configed && mTask.sensors[ACC].latency != SENSOR_LATENCY_NODATA) { 973 val |= 0x40; 974 mTask.fifo_enabled[ACC] = true; 975 } else { 976 mTask.fifo_enabled[ACC] = false; 977 } 978 979 // if GYR is configed, enable GYR bit in fifo_config reg. 980 if (mTask.sensors[GYR].configed && mTask.sensors[GYR].latency != SENSOR_LATENCY_NODATA) { 981 val |= 0x80; 982 mTask.fifo_enabled[GYR] = true; 983 } else { 984 mTask.fifo_enabled[GYR] = false; 985 } 986 987 // if MAG is configed, enable MAG bit in fifo_config reg. 988 if (mTask.sensors[MAG].configed && mTask.sensors[MAG].latency != SENSOR_LATENCY_NODATA) { 989 val |= 0x20; 990 mTask.fifo_enabled[MAG] = true; 991 } else { 992 mTask.fifo_enabled[MAG] = false; 993 } 994 995 // if this is the first data sensor fifo to enable, start to 996 // sync the sensor time and rtc time 997 if (!any_fifo_enabled_prev && anyFifoEnabled()) { 998 invalidate_sensortime_to_rtc_time(); 999 1000 // start a new poll generation and attach the generation number to event 1001 osEnqueuePrivateEvt(EVT_TIME_SYNC, (void *)mTask.poll_generation, NULL, mTask.tid); 1002 } 1003 1004 // cancel current poll generation 1005 if (any_fifo_enabled_prev && !anyFifoEnabled()) { 1006 ++mTask.poll_generation; 1007 } 1008 1009 // if this is not the first fifo enabled or last fifo disabled, flush all fifo data; 1010 if (any_fifo_enabled_prev && anyFifoEnabled()) { 1011 mTask.pending_dispatch = true; 1012 mTask.xferCnt = FIFO_READ_SIZE; 1013 SPI_READ(BMI160_REG_FIFO_DATA, mTask.xferCnt, &mTask.dataBuffer); 1014 } 1015 1016 // calculate the new watermark level 1017 if (anyFifoEnabled()) { 1018 mTask.watermark = calcWatermark2_(_task); 1019 DEBUG_PRINT("wm=%d", mTask.watermark); 1020 SPI_WRITE(BMI160_REG_FIFO_CONFIG_0, mTask.watermark); 1021 } 1022 1023 // config the fifo register 1024 SPI_WRITE(BMI160_REG_FIFO_CONFIG_1, val); 1025 1026 // if no more fifo enabled, we need to cleanup the fifo and invalidate time 1027 if (!anyFifoEnabled()) { 1028 SPI_WRITE(BMI160_REG_CMD, 0xb0); 1029 mTask.frame_sensortime_valid = false; 1030 for (i = ACC; i <= MAG; i++) { 1031 mTask.pending_delta[i] = false; 1032 mTask.prev_frame_time[i] = ULONG_LONG_MAX; 1033 } 1034 } 1035 } 1036 1037 static bool accPower(bool on, void *cookie) 1038 { 1039 TDECL(); 1040 1041 INFO_PRINT("accPower: on=%d, state=%s\n", on, getStateName(GET_STATE())); 1042 if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) { 1043 if (on) { 1044 // set ACC power mode to NORMAL 1045 SPI_WRITE(BMI160_REG_CMD, 0x11, 50000); 1046 } else { 1047 // set ACC power mode to SUSPEND 1048 mTask.sensors[ACC].configed = false; 1049 configFifo(); 1050 SPI_WRITE(BMI160_REG_CMD, 0x10, 5000); 1051 } 1052 mTask.sensors[ACC].powered = on; 1053 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC], __FUNCTION__); 1054 } else { 1055 mTask.pending_config[ACC] = true; 1056 mTask.sensors[ACC].pConfig.enable = on; 1057 } 1058 return true; 1059 } 1060 1061 static bool gyrPower(bool on, void *cookie) 1062 { 1063 TDECL(); 1064 INFO_PRINT("gyrPower: on=%d, state=%s\n", on, getStateName(GET_STATE())); 1065 1066 if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) { 1067 if (on) { 1068 // set GYR power mode to NORMAL 1069 SPI_WRITE(BMI160_REG_CMD, 0x15, 50000); 1070 } else { 1071 // set GYR power mode to SUSPEND 1072 mTask.sensors[GYR].configed = false; 1073 configFifo(); 1074 SPI_WRITE(BMI160_REG_CMD, 0x14, 5000); 1075 } 1076 1077 if (anyFifoEnabled() && on != mTask.sensors[GYR].powered) { 1078 #if TIMESTAMP_DBG 1079 DEBUG_PRINT("minimize_sensortime_history()\n"); 1080 #endif 1081 minimize_sensortime_history(); 1082 } 1083 1084 mTask.sensors[GYR].powered = on; 1085 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[GYR], __FUNCTION__); 1086 } else { 1087 mTask.pending_config[GYR] = true; 1088 mTask.sensors[GYR].pConfig.enable = on; 1089 } 1090 return true; 1091 } 1092 1093 static bool magPower(bool on, void *cookie) 1094 { 1095 TDECL(); 1096 INFO_PRINT("magPower: on=%d, state=%s\n", on, getStateName(GET_STATE())); 1097 if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) { 1098 if (on) { 1099 // set MAG power mode to NORMAL 1100 SPI_WRITE(BMI160_REG_CMD, 0x19, 10000); 1101 } else { 1102 // set MAG power mode to SUSPEND 1103 mTask.sensors[MAG].configed = false; 1104 configFifo(); 1105 SPI_WRITE(BMI160_REG_CMD, 0x18, 5000); 1106 } 1107 mTask.sensors[MAG].powered = on; 1108 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[MAG], __FUNCTION__); 1109 } else { 1110 mTask.pending_config[MAG] = true; 1111 mTask.sensors[MAG].pConfig.enable = on; 1112 } 1113 return true; 1114 } 1115 1116 static bool stepPower(bool on, void *cookie) 1117 { 1118 TDECL(); 1119 if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) { 1120 // if step counter is powered, no need to change actual config of step 1121 // detector. 1122 // But we choose to perform one SPI_WRITE anyway to go down the code path 1123 // to state SENSOR_POWERING_UP/DOWN to update sensor manager. 1124 if (on) { 1125 mTask.interrupt_enable_2 |= 0x08; 1126 } else { 1127 if (!mTask.sensors[STEPCNT].powered) 1128 mTask.interrupt_enable_2 &= ~0x08; 1129 mTask.sensors[STEP].configed = false; 1130 } 1131 mTask.sensors[STEP].powered = on; 1132 SPI_WRITE(BMI160_REG_INT_EN_2, mTask.interrupt_enable_2, 450); 1133 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[STEP], __FUNCTION__); 1134 } else { 1135 mTask.pending_config[STEP] = true; 1136 mTask.sensors[STEP].pConfig.enable = on; 1137 } 1138 return true; 1139 } 1140 1141 static bool flatPower(bool on, void *cookie) 1142 { 1143 TDECL(); 1144 if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) { 1145 if (on) { 1146 mTask.interrupt_enable_0 |= 0x80; 1147 } else { 1148 mTask.interrupt_enable_0 &= ~0x80; 1149 mTask.sensors[FLAT].configed = false; 1150 } 1151 mTask.sensors[FLAT].powered = on; 1152 SPI_WRITE(BMI160_REG_INT_EN_0, mTask.interrupt_enable_0, 450); 1153 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[FLAT], __FUNCTION__); 1154 } else { 1155 mTask.pending_config[FLAT] = true; 1156 mTask.sensors[FLAT].pConfig.enable = on; 1157 } 1158 return true; 1159 } 1160 1161 static bool doubleTapPower(bool on, void *cookie) 1162 { 1163 TDECL(); 1164 if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) { 1165 if (on) { 1166 mTask.interrupt_enable_0 |= 0x10; 1167 } else { 1168 mTask.interrupt_enable_0 &= ~0x10; 1169 mTask.sensors[DTAP].configed = false; 1170 } 1171 mTask.sensors[DTAP].powered = on; 1172 SPI_WRITE(BMI160_REG_INT_EN_0, mTask.interrupt_enable_0, 450); 1173 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[DTAP], __FUNCTION__); 1174 } else { 1175 mTask.pending_config[DTAP] = true; 1176 mTask.sensors[DTAP].pConfig.enable = on; 1177 } 1178 return true; 1179 } 1180 1181 static bool anyMotionPower(bool on, void *cookie) 1182 { 1183 TDECL(); 1184 DEBUG_PRINT("anyMotionPower: on=%d, oneshot_cnt %d, state=%s\n", 1185 on, mTask.active_oneshot_sensor_cnt, getStateName(GET_STATE())); 1186 1187 if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) { 1188 if (on) { 1189 mTask.interrupt_enable_0 |= 0x07; 1190 } else { 1191 mTask.interrupt_enable_0 &= ~0x07; 1192 mTask.sensors[ANYMO].configed = false; 1193 } 1194 mTask.sensors[ANYMO].powered = on; 1195 SPI_WRITE(BMI160_REG_INT_EN_0, mTask.interrupt_enable_0, 450); 1196 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ANYMO], __FUNCTION__); 1197 } else { 1198 mTask.pending_config[ANYMO] = true; 1199 mTask.sensors[ANYMO].pConfig.enable = on; 1200 } 1201 return true; 1202 } 1203 1204 static bool noMotionPower(bool on, void *cookie) 1205 { 1206 TDECL(); 1207 DEBUG_PRINT("noMotionPower: on=%d, oneshot_cnt %d, state=%s\n", 1208 on, mTask.active_oneshot_sensor_cnt, getStateName(GET_STATE())); 1209 if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) { 1210 if (on) { 1211 mTask.interrupt_enable_2 |= 0x07; 1212 } else { 1213 mTask.interrupt_enable_2 &= ~0x07; 1214 mTask.sensors[NOMO].configed = false; 1215 } 1216 mTask.sensors[NOMO].powered = on; 1217 SPI_WRITE(BMI160_REG_INT_EN_2, mTask.interrupt_enable_2, 450); 1218 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[NOMO], __FUNCTION__); 1219 } else { 1220 mTask.pending_config[NOMO] = true; 1221 mTask.sensors[NOMO].pConfig.enable = on; 1222 } 1223 return true; 1224 } 1225 1226 static bool stepCntPower(bool on, void *cookie) 1227 { 1228 TDECL(); 1229 if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) { 1230 if (on) { 1231 if (!mTask.sensors[STEP].powered) { 1232 mTask.interrupt_enable_2 |= 0x08; 1233 SPI_WRITE(BMI160_REG_INT_EN_2, mTask.interrupt_enable_2, 450); 1234 } 1235 // set step_cnt_en bit 1236 SPI_WRITE(BMI160_REG_STEP_CONF_1, 0x08 | 0x03, 1000); 1237 } else { 1238 if (mTask.stepCntSamplingTimerHandle) { 1239 timTimerCancel(mTask.stepCntSamplingTimerHandle); 1240 mTask.stepCntSamplingTimerHandle = 0; 1241 } 1242 if (!mTask.sensors[STEP].powered) { 1243 mTask.interrupt_enable_2 &= ~0x08; 1244 SPI_WRITE(BMI160_REG_INT_EN_2, mTask.interrupt_enable_2); 1245 } 1246 // unset step_cnt_en bit 1247 SPI_WRITE(BMI160_REG_STEP_CONF_1, 0x03); 1248 mTask.last_step_cnt = 0; 1249 mTask.sensors[STEPCNT].configed = false; 1250 } 1251 mTask.sensors[STEPCNT].powered = on; 1252 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[STEPCNT], __FUNCTION__); 1253 } else { 1254 mTask.pending_config[STEPCNT] = true; 1255 mTask.sensors[STEPCNT].pConfig.enable = on; 1256 } 1257 return true; 1258 } 1259 1260 static void updateTimeDelta(uint8_t idx, uint8_t odr) 1261 { 1262 if (mTask.fifo_enabled[idx]) { 1263 // wait till control frame to update, if not disabled 1264 mTask.next_delta[idx] = 1ull << (16 - odr); 1265 mTask.pending_delta[idx] = true; 1266 } else { 1267 mTask.time_delta[idx] = 1ull << (16 - odr); 1268 } 1269 } 1270 1271 // compute the register value from sensor rate. 1272 static uint8_t computeOdr(uint32_t rate) 1273 { 1274 uint8_t odr = 0x00; 1275 switch (rate) { 1276 // fall through intended to get the correct register value 1277 case SENSOR_HZ(3200): odr ++; 1278 case SENSOR_HZ(1600): odr ++; 1279 case SENSOR_HZ(800): odr ++; 1280 case SENSOR_HZ(400): odr ++; 1281 case SENSOR_HZ(200): odr ++; 1282 case SENSOR_HZ(100): odr ++; 1283 case SENSOR_HZ(50): odr ++; 1284 case SENSOR_HZ(25): odr ++; 1285 case SENSOR_HZ(25.0f/2.0f): odr ++; 1286 case SENSOR_HZ(25.0f/4.0f): odr ++; 1287 case SENSOR_HZ(25.0f/8.0f): odr ++; 1288 case SENSOR_HZ(25.0f/16.0f): odr ++; 1289 case SENSOR_HZ(25.0f/32.0f): odr ++; 1290 default: 1291 return odr; 1292 } 1293 } 1294 1295 static void configMotion(uint8_t odr) { 1296 // motion threshold is element * 15.63mg (for 8g range) 1297 static const uint8_t motion_thresholds[ACC_MAX_RATE+1] = 1298 {5, 5, 5, 5, 5, 5, 5, 5, 4, 3, 2, 2, 2}; 1299 1300 // set any_motion duration to 1 point 1301 // set no_motion duration to (3+1)*1.28sec=5.12sec 1302 SPI_WRITE(BMI160_REG_INT_MOTION_0, 0x03 << 2, 450); 1303 1304 // set any_motion threshold 1305 SPI_WRITE(BMI160_REG_INT_MOTION_1, motion_thresholds[odr], 450); 1306 1307 // set no_motion threshold 1308 SPI_WRITE(BMI160_REG_INT_MOTION_2, motion_thresholds[odr], 450); 1309 } 1310 1311 static bool accSetRate(uint32_t rate, uint64_t latency, void *cookie) 1312 { 1313 TDECL(); 1314 int odr, osr = 0; 1315 1316 INFO_PRINT("accSetRate: rate=%ld, latency=%lld, state=%s\n", rate, latency, 1317 getStateName(GET_STATE())); 1318 1319 if (trySwitchState(SENSOR_CONFIG_CHANGING)) { 1320 odr = computeOdr(rate); 1321 if (!odr) { 1322 ERROR_PRINT("invalid acc rate\n"); 1323 return false; 1324 } 1325 1326 updateTimeDelta(ACC, odr); 1327 1328 // minimum supported rate for ACCEL is 12.5Hz. 1329 // Anything lower than that shall be acheived by downsampling. 1330 if (odr < ACC_MIN_RATE) { 1331 osr = ACC_MIN_RATE - odr; 1332 odr = ACC_MIN_RATE; 1333 } 1334 1335 // for high odrs, oversample to reduce hw latency and downsample 1336 // to get desired odr 1337 if (odr > OSR_THRESHOLD) { 1338 osr = (ACC_MAX_OSR + odr) > ACC_MAX_RATE ? (ACC_MAX_RATE - odr) : ACC_MAX_OSR; 1339 odr += osr; 1340 } 1341 1342 mTask.sensors[ACC].rate = rate; 1343 mTask.sensors[ACC].latency = latency; 1344 mTask.sensors[ACC].configed = true; 1345 mTask.acc_downsample = osr; 1346 1347 // configure ANY_MOTION and NO_MOTION based on odr 1348 configMotion(odr); 1349 1350 // set ACC bandwidth parameter to 2 (bits[4:6]) 1351 // set the rate (bits[0:3]) 1352 SPI_WRITE(BMI160_REG_ACC_CONF, 0x20 | odr); 1353 1354 // configure down sampling ratio, 0x88 is to specify we are using 1355 // filtered samples 1356 SPI_WRITE(BMI160_REG_FIFO_DOWNS, (mTask.acc_downsample << 4) | mTask.gyr_downsample | 0x88); 1357 1358 // flush the data and configure the fifo 1359 configFifo(); 1360 1361 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC], __FUNCTION__); 1362 } else { 1363 mTask.pending_config[ACC] = true; 1364 mTask.sensors[ACC].pConfig.enable = 1; 1365 mTask.sensors[ACC].pConfig.rate = rate; 1366 mTask.sensors[ACC].pConfig.latency = latency; 1367 } 1368 return true; 1369 } 1370 1371 static bool gyrSetRate(uint32_t rate, uint64_t latency, void *cookie) 1372 { 1373 TDECL(); 1374 int odr, osr = 0; 1375 INFO_PRINT("gyrSetRate: rate=%ld, latency=%lld, state=%s\n", rate, latency, 1376 getStateName(GET_STATE())); 1377 1378 if (trySwitchState(SENSOR_CONFIG_CHANGING)) { 1379 odr = computeOdr(rate); 1380 if (!odr) { 1381 ERROR_PRINT("invalid gyr rate\n"); 1382 return false; 1383 } 1384 1385 updateTimeDelta(GYR, odr); 1386 1387 // minimum supported rate for GYRO is 25.0Hz. 1388 // Anything lower than that shall be acheived by downsampling. 1389 if (odr < GYR_MIN_RATE) { 1390 osr = GYR_MIN_RATE - odr; 1391 odr = GYR_MIN_RATE; 1392 } 1393 1394 // for high odrs, oversample to reduce hw latency and downsample 1395 // to get desired odr 1396 if (odr > OSR_THRESHOLD) { 1397 osr = (GYR_MAX_OSR + odr) > GYR_MAX_RATE ? (GYR_MAX_RATE - odr) : GYR_MAX_OSR; 1398 odr += osr; 1399 } 1400 1401 mTask.sensors[GYR].rate = rate; 1402 mTask.sensors[GYR].latency = latency; 1403 mTask.sensors[GYR].configed = true; 1404 mTask.gyr_downsample = osr; 1405 1406 // set GYR bandwidth parameter to 2 (bits[4:6]) 1407 // set the rate (bits[0:3]) 1408 SPI_WRITE(BMI160_REG_GYR_CONF, 0x20 | odr); 1409 1410 // configure down sampling ratio, 0x88 is to specify we are using 1411 // filtered samples 1412 SPI_WRITE(BMI160_REG_FIFO_DOWNS, (mTask.acc_downsample << 4) | mTask.gyr_downsample | 0x88); 1413 1414 // flush the data and configure the fifo 1415 configFifo(); 1416 1417 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[GYR], __FUNCTION__); 1418 } else { 1419 mTask.pending_config[GYR] = true; 1420 mTask.sensors[GYR].pConfig.enable = 1; 1421 mTask.sensors[GYR].pConfig.rate = rate; 1422 mTask.sensors[GYR].pConfig.latency = latency; 1423 } 1424 return true; 1425 } 1426 1427 static bool magSetRate(uint32_t rate, uint64_t latency, void *cookie) 1428 { 1429 TDECL(); 1430 int odr; 1431 1432 if (rate == SENSOR_RATE_ONCHANGE) 1433 rate = SENSOR_HZ(100); 1434 1435 INFO_PRINT("magSetRate: rate=%ld, latency=%lld, state=%s\n", rate, latency, 1436 getStateName(GET_STATE())); 1437 1438 if (trySwitchState(SENSOR_CONFIG_CHANGING)) { 1439 mTask.sensors[MAG].rate = rate; 1440 mTask.sensors[MAG].latency = latency; 1441 mTask.sensors[MAG].configed = true; 1442 1443 odr = computeOdr(rate); 1444 if (!odr) { 1445 ERROR_PRINT("invalid mag rate\n"); 1446 return false; 1447 } 1448 1449 updateTimeDelta(MAG, odr); 1450 1451 odr = odr > MAG_MAX_RATE ? MAG_MAX_RATE : odr; 1452 1453 // set the rate for MAG 1454 SPI_WRITE(BMI160_REG_MAG_CONF, odr); 1455 1456 // flush the data and configure the fifo 1457 configFifo(); 1458 1459 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[MAG], __FUNCTION__); 1460 } else { 1461 mTask.pending_config[MAG] = true; 1462 mTask.sensors[MAG].pConfig.enable = 1; 1463 mTask.sensors[MAG].pConfig.rate = rate; 1464 mTask.sensors[MAG].pConfig.latency = latency; 1465 } 1466 return true; 1467 } 1468 1469 static bool stepSetRate(uint32_t rate, uint64_t latency, void *cookie) 1470 { 1471 mTask.sensors[STEP].rate = rate; 1472 mTask.sensors[STEP].latency = latency; 1473 mTask.sensors[STEP].configed = true; 1474 1475 sensorSignalInternalEvt(mTask.sensors[STEP].handle, 1476 SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency); 1477 return true; 1478 } 1479 1480 static bool flatSetRate(uint32_t rate, uint64_t latency, void *cookie) 1481 { 1482 mTask.sensors[FLAT].rate = rate; 1483 mTask.sensors[FLAT].latency = latency; 1484 mTask.sensors[FLAT].configed = true; 1485 1486 sensorSignalInternalEvt(mTask.sensors[FLAT].handle, 1487 SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency); 1488 return true; 1489 } 1490 1491 static bool doubleTapSetRate(uint32_t rate, uint64_t latency, void *cookie) 1492 { 1493 mTask.sensors[DTAP].rate = rate; 1494 mTask.sensors[DTAP].latency = latency; 1495 mTask.sensors[DTAP].configed = true; 1496 1497 sensorSignalInternalEvt(mTask.sensors[DTAP].handle, 1498 SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency); 1499 return true; 1500 } 1501 1502 static bool anyMotionSetRate(uint32_t rate, uint64_t latency, void *cookie) 1503 { 1504 mTask.sensors[ANYMO].rate = rate; 1505 mTask.sensors[ANYMO].latency = latency; 1506 mTask.sensors[ANYMO].configed = true; 1507 1508 sensorSignalInternalEvt(mTask.sensors[ANYMO].handle, 1509 SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency); 1510 1511 return true; 1512 } 1513 1514 static bool noMotionSetRate(uint32_t rate, uint64_t latency, void *cookie) 1515 { 1516 mTask.sensors[NOMO].rate = rate; 1517 mTask.sensors[NOMO].latency = latency; 1518 mTask.sensors[NOMO].configed = true; 1519 1520 sensorSignalInternalEvt(mTask.sensors[NOMO].handle, 1521 SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency); 1522 return true; 1523 } 1524 1525 static bool stepCntSetRate(uint32_t rate, uint64_t latency, void *cookie) 1526 { 1527 mTask.sensors[STEPCNT].rate = rate; 1528 mTask.sensors[STEPCNT].latency = latency; 1529 mTask.sensors[STEPCNT].configed = true; 1530 1531 if (rate == SENSOR_RATE_ONCHANGE && mTask.stepCntSamplingTimerHandle) { 1532 timTimerCancel(mTask.stepCntSamplingTimerHandle); 1533 mTask.stepCntSamplingTimerHandle = 0; 1534 } else if (rate != SENSOR_RATE_ONCHANGE) { 1535 if (mTask.stepCntSamplingTimerHandle) { 1536 timTimerCancel(mTask.stepCntSamplingTimerHandle); 1537 } 1538 mTask.stepCntSamplingTimerHandle = timTimerSet(sensorTimerLookupCommon(StepCntRates, stepCntRateTimerVals, rate), 1539 0, 50, stepCntSamplingCallback, NULL, false); 1540 } 1541 1542 sensorSignalInternalEvt(mTask.sensors[STEPCNT].handle, 1543 SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency); 1544 return true; 1545 } 1546 1547 static void sendFlushEvt(void) 1548 { 1549 while (mTask.sensors[ACC].flush > 0) { 1550 osEnqueueEvt(EVT_SENSOR_ACC_DATA_RDY, SENSOR_DATA_EVENT_FLUSH, NULL); 1551 mTask.sensors[ACC].flush--; 1552 } 1553 while (mTask.sensors[GYR].flush > 0) { 1554 osEnqueueEvt(EVT_SENSOR_GYR_DATA_RDY, SENSOR_DATA_EVENT_FLUSH, NULL); 1555 mTask.sensors[GYR].flush--; 1556 } 1557 while (mTask.sensors[MAG].flush > 0) { 1558 osEnqueueEvt(EVT_SENSOR_MAG_DATA_RDY, SENSOR_DATA_EVENT_FLUSH, NULL); 1559 mTask.sensors[MAG].flush--; 1560 } 1561 } 1562 1563 static bool accFlush(void *cookie) 1564 { 1565 TDECL(); 1566 mTask.sensors[ACC].flush++; 1567 initiateFifoRead(false /*isInterruptContext*/); 1568 return true; 1569 } 1570 1571 static bool gyrFlush(void *cookie) 1572 { 1573 TDECL(); 1574 mTask.sensors[GYR].flush++; 1575 initiateFifoRead(false /*isInterruptContext*/); 1576 return true; 1577 } 1578 1579 static bool magFlush(void *cookie) 1580 { 1581 TDECL(); 1582 mTask.sensors[MAG].flush++; 1583 initiateFifoRead(false /*isInterruptContext*/); 1584 return true; 1585 } 1586 1587 static bool stepFlush(void *cookie) 1588 { 1589 return osEnqueueEvt(EVT_SENSOR_STEP, SENSOR_DATA_EVENT_FLUSH, NULL); 1590 } 1591 1592 static bool flatFlush(void *cookie) 1593 { 1594 return osEnqueueEvt(EVT_SENSOR_FLAT, SENSOR_DATA_EVENT_FLUSH, NULL); 1595 } 1596 1597 static bool doubleTapFlush(void *cookie) 1598 { 1599 return osEnqueueEvt(EVT_SENSOR_DOUBLE_TAP, SENSOR_DATA_EVENT_FLUSH, NULL); 1600 } 1601 1602 static bool anyMotionFlush(void *cookie) 1603 { 1604 return osEnqueueEvt(EVT_SENSOR_ANY_MOTION, SENSOR_DATA_EVENT_FLUSH, NULL); 1605 } 1606 1607 static bool noMotionFlush(void *cookie) 1608 { 1609 return osEnqueueEvt(EVT_SENSOR_NO_MOTION, SENSOR_DATA_EVENT_FLUSH, NULL); 1610 } 1611 1612 static bool stepCntFlushGetData() 1613 { 1614 TDECL(); 1615 if (trySwitchState(SENSOR_STEP_CNT)) { 1616 SPI_READ(BMI160_REG_STEP_CNT_0, 2, &mTask.dataBuffer); 1617 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[STEPCNT], __FUNCTION__); 1618 return true; 1619 } 1620 return false; 1621 } 1622 1623 static bool stepCntFlush(void *cookie) 1624 { 1625 mTask.sensors[STEPCNT].flush++; 1626 stepCntFlushGetData(); 1627 return true; 1628 } 1629 1630 static void sendStepCnt() 1631 { 1632 union EmbeddedDataPoint step_cnt; 1633 uint32_t cur_step_cnt; 1634 cur_step_cnt = (int)(mTask.dataBuffer[1] | (mTask.dataBuffer[2] << 8)); 1635 1636 if (cur_step_cnt != mTask.last_step_cnt) { 1637 // Check for possible overflow 1638 if (cur_step_cnt < mTask.last_step_cnt) { 1639 mTask.total_step_cnt += cur_step_cnt + (0xFFFF - mTask.last_step_cnt); 1640 } else { 1641 mTask.total_step_cnt += (cur_step_cnt - mTask.last_step_cnt); 1642 } 1643 mTask.last_step_cnt = cur_step_cnt; 1644 1645 // Send the event if the current rate is ONCHANGE or we need to flush; 1646 // otherwise, wait until step count sampling timer expires 1647 if (mTask.sensors[STEPCNT].rate == SENSOR_RATE_ONCHANGE || mTask.sensors[STEPCNT].flush) { 1648 step_cnt.idata = mTask.total_step_cnt; 1649 osEnqueueEvt(EVT_SENSOR_STEP_COUNTER, step_cnt.vptr, NULL); 1650 } else { 1651 mTask.step_cnt_changed = true; 1652 } 1653 } 1654 1655 while (mTask.sensors[STEPCNT].flush) { 1656 osEnqueueEvt(EVT_SENSOR_STEP_COUNTER, SENSOR_DATA_EVENT_FLUSH, NULL); 1657 mTask.sensors[STEPCNT].flush--; 1658 } 1659 } 1660 1661 static bool stepCntSendLastData(void *cookie, uint32_t tid) 1662 { 1663 // If this comes in and we don't have data yet, there's no harm in reporting step_cnt = 0 1664 return osEnqueuePrivateEvt(EVT_SENSOR_STEP_COUNTER, (void *) mTask.total_step_cnt, NULL, tid); 1665 } 1666 1667 static uint64_t parseSensortime(uint32_t sensor_time24) 1668 { 1669 uint32_t prev_time24; 1670 uint32_t kHalf = 1ul << 23; 1671 uint64_t full; 1672 1673 prev_time24 = (uint32_t)mTask.last_sensortime & 0xffffff; 1674 1675 if (mTask.last_sensortime == 0) { 1676 mTask.last_sensortime = (uint64_t)sensor_time24; 1677 return (uint64_t)(sensor_time24); 1678 } 1679 1680 if (sensor_time24 == prev_time24) { 1681 return (uint64_t)(mTask.last_sensortime); 1682 } 1683 1684 full = (mTask.last_sensortime & ~0xffffffull) | sensor_time24; 1685 1686 if (((prev_time24 < sensor_time24) && (sensor_time24 - prev_time24) < kHalf) 1687 || ((prev_time24 > sensor_time24) && (prev_time24 - sensor_time24) > kHalf)) { 1688 if (full < mTask.last_sensortime) { 1689 full += 0x1000000ull; 1690 } 1691 mTask.last_sensortime = full; 1692 return mTask.last_sensortime; 1693 } 1694 1695 if (full < mTask.last_sensortime) { 1696 return full; 1697 } 1698 1699 return (full - 0x1000000ull); 1700 } 1701 1702 static bool flushData(struct BMI160Sensor *sensor, uint32_t eventId) 1703 { 1704 bool success = false; 1705 1706 if (sensor->data_evt) { 1707 success = osEnqueueEvtOrFree(eventId, sensor->data_evt, dataEvtFree); 1708 sensor->data_evt = NULL; 1709 } 1710 1711 return success; 1712 } 1713 1714 static void flushAllData(void) 1715 { 1716 int i; 1717 for (i = ACC; i <= MAG; i++) { 1718 flushData(&mTask.sensors[i], 1719 EVENT_TYPE_BIT_DISCARDABLE | sensorGetMyEventType(mSensorInfo[i].sensorType)); 1720 } 1721 } 1722 1723 static bool allocateDataEvt(struct BMI160Sensor *mSensor, uint64_t rtc_time) 1724 { 1725 TDECL(); 1726 mSensor->data_evt = slabAllocatorAlloc(T(mDataSlab)); 1727 if (mSensor->data_evt == NULL) { 1728 // slab allocation failed 1729 ERROR_PRINT("slabAllocatorAlloc() failed\n"); 1730 return false; 1731 } 1732 1733 // delta time for the first sample is sample count 1734 memset(&mSensor->data_evt->samples[0].firstSample, 0x00, sizeof(struct SensorFirstSample)); 1735 mSensor->data_evt->referenceTime = rtc_time; 1736 mSensor->prev_rtc_time = rtc_time; 1737 1738 return true; 1739 } 1740 1741 static void parseRawData(struct BMI160Sensor *mSensor, uint8_t *buf, float kScale, uint64_t sensorTime) 1742 { 1743 float x, y, z; 1744 int16_t raw_x, raw_y, raw_z; 1745 struct TripleAxisDataPoint *sample; 1746 uint32_t delta_time; 1747 uint64_t rtc_time; 1748 bool newMagBias = false; 1749 1750 if (!sensortime_to_rtc_time(sensorTime, &rtc_time)) { 1751 return; 1752 } 1753 1754 if (rtc_time < mSensor->prev_rtc_time + kMinRTCTimeIncrementNs) { 1755 #if TIMESTAMP_DBG 1756 DEBUG_PRINT("%s prev rtc 0x%08x %08x, curr 0x%08x %08x, delta %d usec\n", 1757 mSensorInfo[mSensor->idx].sensorName, 1758 (unsigned int)((mSensor->prev_rtc_time >> 32) & 0xffffffff), 1759 (unsigned int)(mSensor->prev_rtc_time & 0xffffffff), 1760 (unsigned int)((rtc_time >> 32) & 0xffffffff), 1761 (unsigned int)(rtc_time & 0xffffffff), 1762 (int)(rtc_time - mSensor->prev_rtc_time) / 1000); 1763 #endif 1764 rtc_time = mSensor->prev_rtc_time + kMinRTCTimeIncrementNs; 1765 } 1766 1767 if (mSensor->idx == MAG) { 1768 #ifdef MAG_SLAVE_PRESENT 1769 parseMagData(&magTask, &buf[0], &x, &y, &z); 1770 BMM150_TO_ANDROID_COORDINATE(x, y, z); 1771 1772 float xi, yi, zi; 1773 magCalRemoveSoftiron(&mTask.moc, x, y, z, &xi, &yi, &zi); 1774 1775 newMagBias |= magCalUpdate(&mTask.moc, sensorTime * kSensorTimerIntervalUs, xi, yi, zi); 1776 1777 magCalRemoveBias(&mTask.moc, xi, yi, zi, &x, &y, &z); 1778 #else 1779 return; 1780 #endif 1781 } else { 1782 raw_x = (buf[0] | buf[1] << 8); 1783 raw_y = (buf[2] | buf[3] << 8); 1784 raw_z = (buf[4] | buf[5] << 8); 1785 1786 x = (float)raw_x * kScale; 1787 y = (float)raw_y * kScale; 1788 z = (float)raw_z * kScale; 1789 1790 BMI160_TO_ANDROID_COORDINATE(x, y, z); 1791 } 1792 1793 if (mSensor->data_evt == NULL) { 1794 if (!allocateDataEvt(mSensor, rtc_time)) 1795 return; 1796 } 1797 1798 if (mSensor->data_evt->samples[0].firstSample.numSamples >= MAX_NUM_COMMS_EVENT_SAMPLES) { 1799 ERROR_PRINT("BAD INDEX\n"); 1800 return; 1801 } 1802 1803 if (mSensor->idx == MAG && (newMagBias || !mTask.magBiasPosted)) { 1804 if (mSensor->data_evt->samples[0].firstSample.numSamples > 0) { 1805 // flush existing samples so the bias appears after them 1806 flushData(mSensor, 1807 EVENT_TYPE_BIT_DISCARDABLE | sensorGetMyEventType(mSensorInfo[MAG].sensorType)); 1808 if (!allocateDataEvt(mSensor, rtc_time)) 1809 return; 1810 } 1811 if (newMagBias) 1812 mTask.magBiasCurrent = true; 1813 mSensor->data_evt->samples[0].firstSample.biasCurrent = mTask.magBiasCurrent; 1814 mSensor->data_evt->samples[0].firstSample.biasPresent = 1; 1815 mSensor->data_evt->samples[0].firstSample.biasSample = 1816 mSensor->data_evt->samples[0].firstSample.numSamples; 1817 sample = &mSensor->data_evt->samples[mSensor->data_evt->samples[0].firstSample.numSamples++]; 1818 #ifdef MAG_SLAVE_PRESENT 1819 magCalGetBias(&mTask.moc, &sample->x, &sample->y, &sample->z); 1820 #endif 1821 // bias is non-discardable, if we fail to enqueue, don't clear new_mag_bias 1822 if (flushData(mSensor, sensorGetMyEventType(mSensorInfo[MAG].biasType))) 1823 mTask.magBiasPosted = true; 1824 1825 if (!allocateDataEvt(mSensor, rtc_time)) 1826 return; 1827 } 1828 1829 sample = &mSensor->data_evt->samples[mSensor->data_evt->samples[0].firstSample.numSamples++]; 1830 1831 // the first deltatime is for sample size 1832 if (mSensor->data_evt->samples[0].firstSample.numSamples > 1) { 1833 delta_time = rtc_time - mSensor->prev_rtc_time; 1834 delta_time = delta_time < 0 ? 0 : delta_time; 1835 sample->deltaTime = delta_time; 1836 mSensor->prev_rtc_time = rtc_time; 1837 } 1838 1839 sample->x = x; 1840 sample->y = y; 1841 sample->z = z; 1842 1843 //DEBUG_PRINT("bmi160: x: %d, y: %d, z: %d\n", (int)(1000*x), (int)(1000*y), (int)(1000*z)); 1844 1845 //TODO: This was added to prevent to much data of the same type accumulate in internal buffer. 1846 // It might no longer be necessary and can be removed. 1847 if (mSensor->data_evt->samples[0].firstSample.numSamples == MAX_NUM_COMMS_EVENT_SAMPLES) { 1848 flushAllData(); 1849 } 1850 1851 } 1852 1853 static void dispatchData(void) 1854 { 1855 size_t i = 1, j; 1856 size_t size = mTask.xferCnt; 1857 int fh_mode, fh_param; 1858 uint8_t *buf = mTask.dataBuffer; 1859 1860 uint64_t min_delta = ULONG_LONG_MAX; 1861 uint32_t sensor_time24; 1862 uint64_t full_sensor_time; 1863 uint64_t frame_sensor_time = mTask.frame_sensortime; 1864 bool observed[3] = {false, false, false}; 1865 uint64_t tmp_frame_time, tmp_time[3]; 1866 bool frame_sensor_time_valid = mTask.frame_sensortime_valid; 1867 bool saved_pending_delta[3]; 1868 uint64_t saved_time_delta[3]; 1869 #if TIMESTAMP_DBG 1870 int frame_num = -1; 1871 #endif 1872 1873 if (!mTask.frame_sensortime_valid) { 1874 // This is the first FIFO delivery after any sensor is enabled in 1875 // bmi160. Sensor time reference is not establised until end of this 1876 // FIFO frame. Assume time start from zero and do a dry run to estimate 1877 // the time and then go through this FIFO again. 1878 frame_sensor_time = 0ull; 1879 1880 // Save these states for future recovery by the end of dry run. 1881 for (j = ACC; j <= MAG; j++) { 1882 saved_pending_delta[j] = mTask.pending_delta[j]; 1883 saved_time_delta[j] = mTask.time_delta[j]; 1884 } 1885 } 1886 1887 while (size > 0) { 1888 if (buf[i] == BMI160_FRAME_HEADER_INVALID) { 1889 // reaching invalid header means no more data 1890 break; 1891 } else if (buf[i] == BMI160_FRAME_HEADER_SKIP) { 1892 // manually injected skip header 1893 DEBUG_PRINT_IF(DBG_CHUNKED, "skip nop header"); 1894 i++; 1895 size--; 1896 continue; 1897 } 1898 1899 fh_mode = buf[i] >> 6; 1900 fh_param = (buf[i] >> 2) & 0xf; 1901 1902 i++; 1903 size--; 1904 #if TIMESTAMP_DBG 1905 ++frame_num; 1906 #endif 1907 1908 if (fh_mode == 1) { 1909 // control frame. 1910 if (fh_param == 0) { 1911 // skip frame, we skip it 1912 if (size >= 1) { 1913 i++; 1914 size--; 1915 } else { 1916 size = 0; 1917 } 1918 } else if (fh_param == 1) { 1919 // sensortime frame 1920 if (size >= 3) { 1921 // The active sensor with the highest odr/lowest delta is the one that 1922 // determines the sensor time increments. 1923 for (j = ACC; j <= MAG; j++) { 1924 if (mTask.sensors[j].configed && 1925 mTask.sensors[j].latency != SENSOR_LATENCY_NODATA) { 1926 min_delta = min_delta < mTask.time_delta[j] ? min_delta : 1927 mTask.time_delta[j]; 1928 } 1929 } 1930 sensor_time24 = buf[i + 2] << 16 | buf[i + 1] << 8 | buf[i]; 1931 1932 // clear lower bits that measure time from taking the sample to reading the 1933 // FIFO, something we're not interested in. 1934 sensor_time24 &= ~(min_delta - 1); 1935 1936 full_sensor_time = parseSensortime(sensor_time24); 1937 1938 #if TIMESTAMP_DBG 1939 if (frame_sensor_time == full_sensor_time) { 1940 //DEBUG_PRINT("frame %d FrameTime 0x%08x\n", 1941 // frame_num - 1, 1942 // (unsigned int)frame_sensor_time); 1943 } else if (frame_sensor_time_valid) { 1944 DEBUG_PRINT("frame %d FrameTime 0x%08x != SensorTime 0x%08x, jumped %d msec\n", 1945 frame_num - 1, 1946 (unsigned int)frame_sensor_time, 1947 (unsigned int)full_sensor_time, 1948 (int)(5 * ((int64_t)(full_sensor_time - frame_sensor_time) >> 7))); 1949 } 1950 #endif 1951 1952 1953 if (frame_sensor_time_valid) { 1954 mTask.frame_sensortime = full_sensor_time; 1955 } else { 1956 // Dry run if frame_sensortime_valid == false, 1957 // no sample is added this round. 1958 // So let's time travel back to beginning of frame. 1959 mTask.frame_sensortime_valid = true; 1960 mTask.frame_sensortime = full_sensor_time - frame_sensor_time; 1961 1962 // recover states 1963 for (j = ACC; j <= MAG; j++) { 1964 // reset all prev_frame_time to invalid values 1965 // they should be so anyway at the first FIFO 1966 mTask.prev_frame_time[j] = ULONG_LONG_MAX; 1967 1968 // recover saved time_delta and pending_delta values 1969 mTask.pending_delta[j] = saved_pending_delta[j]; 1970 mTask.time_delta[j] = saved_time_delta[j]; 1971 } 1972 1973 DEBUG_PRINT_IF(TIMESTAMP_DBG, 1974 "sensortime invalid: full, frame, task = %llu, %llu, %llu\n", 1975 full_sensor_time, 1976 frame_sensor_time, 1977 mTask.frame_sensortime); 1978 1979 // Parse again with known valid timing. 1980 // This time the sensor events will be committed into event buffer. 1981 return dispatchData(); 1982 } 1983 1984 // Invalidate sensor timestamp that didn't get corrected by full_sensor_time, 1985 // so it can't be used as a reference at next FIFO read. 1986 // Use (ULONG_LONG_MAX - 1) to indicate this. 1987 for (j = ACC; j <= MAG; j++) { 1988 mTask.prev_frame_time[j] = observed[j] ? full_sensor_time : (ULONG_LONG_MAX - 1); 1989 1990 // sensor can be disabled in the middle of the FIFO, but wait till the FIFO 1991 // end to invalidate prev_frame_time since it's still needed for parsing. 1992 // Also invalidate pending delta just to be safe. 1993 if (!mTask.sensors[j].configed || 1994 mTask.sensors[j].latency == SENSOR_LATENCY_NODATA) { 1995 mTask.prev_frame_time[j] = ULONG_LONG_MAX; 1996 mTask.pending_delta[j] = false; 1997 } 1998 } 1999 i += 3; 2000 size -= 3; 2001 } else { 2002 size = 0; 2003 } 2004 } else if (fh_param == 2) { 2005 // fifo_input config frame 2006 #if TIMESTAMP_DBG 2007 DEBUG_PRINT("frame %d config change 0x%02x\n", frame_num, buf[i]); 2008 #endif 2009 if (size >= 1) { 2010 for (j = ACC; j <= MAG; j++) { 2011 if (buf[i] & (0x01 << (j << 1)) && mTask.pending_delta[j]) { 2012 mTask.pending_delta[j] = false; 2013 mTask.time_delta[j] = mTask.next_delta[j]; 2014 #if TIMESTAMP_DBG 2015 DEBUG_PRINT("%s new delta %u\n", mSensorInfo[j].sensorName, 2016 (unsigned int)mTask.time_delta[j]); 2017 #endif 2018 } 2019 } 2020 i++; 2021 size--; 2022 } else { 2023 size = 0; 2024 } 2025 } else { 2026 size = 0; // drop this batch 2027 ERROR_PRINT("Invalid fh_param in conttrol frame\n"); 2028 } 2029 } else if (fh_mode == 2) { 2030 // Calcutate candidate frame time (tmp_frame_time): 2031 // 1) When sensor is first enabled, reference from other sensors if possible. 2032 // Otherwise, add the smallest increment to the previous data frame time. 2033 // 2) The newly enabled sensor could only underestimate its 2034 // frame time without reference from other sensors. 2035 // 3) The underestimated frame time of a newly enabled sensor will be corrected 2036 // as soon as it shows up in the same frame with another sensor. 2037 // 4) (prev_frame_time == ULONG_LONG_MAX) means the sensor wasn't enabled. 2038 // 5) (prev_frame_time == ULONG_LONG_MAX -1) means the sensor didn't appear in the last 2039 // data frame of the previous fifo read. So it won't be used as a frame time reference. 2040 2041 tmp_frame_time = 0; 2042 for (j = ACC; j <= MAG; j++) { 2043 observed[j] = false; // reset at each data frame 2044 tmp_time[j] = 0; 2045 if ((mTask.prev_frame_time[j] < ULONG_LONG_MAX - 1) && (fh_param & (1 << j))) { 2046 tmp_time[j] = mTask.prev_frame_time[j] + mTask.time_delta[j]; 2047 tmp_frame_time = (tmp_time[j] > tmp_frame_time) ? tmp_time[j] : tmp_frame_time; 2048 } 2049 } 2050 tmp_frame_time = (frame_sensor_time + kMinSensorTimeIncrement > tmp_frame_time) 2051 ? (frame_sensor_time + kMinSensorTimeIncrement) : tmp_frame_time; 2052 2053 // regular frame, dispatch data to each sensor's own fifo 2054 if (fh_param & 4) { // have mag data 2055 if (size >= 8) { 2056 if (frame_sensor_time_valid) { 2057 // scale not used 2058 parseRawData(&mTask.sensors[MAG], &buf[i], 0, tmp_frame_time); 2059 #if TIMESTAMP_DBG 2060 if (mTask.prev_frame_time[MAG] == ULONG_LONG_MAX) { 2061 DEBUG_PRINT("mag enabled: frame %d time 0x%08x\n", 2062 frame_num, (unsigned int)tmp_frame_time); 2063 } else if ((tmp_frame_time != tmp_time[MAG]) && (tmp_time[MAG] != 0)) { 2064 DEBUG_PRINT("frame %d mag time: 0x%08x -> 0x%08x, jumped %d msec\n", 2065 frame_num, 2066 (unsigned int)tmp_time[MAG], 2067 (unsigned int)tmp_frame_time, 2068 (int)(5 * ((int64_t)(tmp_frame_time - tmp_time[MAG]) >> 7))); 2069 } 2070 #endif 2071 } 2072 mTask.prev_frame_time[MAG] = tmp_frame_time; 2073 i += 8; 2074 size -= 8; 2075 observed[MAG] = true; 2076 } else { 2077 size = 0; 2078 } 2079 } 2080 if (fh_param & 2) { // have gyro data 2081 if (size >= 6) { 2082 if (frame_sensor_time_valid) { 2083 parseRawData(&mTask.sensors[GYR], &buf[i], kScale_gyr, tmp_frame_time); 2084 #if TIMESTAMP_DBG 2085 if (mTask.prev_frame_time[GYR] == ULONG_LONG_MAX) { 2086 DEBUG_PRINT("gyr enabled: frame %d time 0x%08x\n", 2087 frame_num, (unsigned int)tmp_frame_time); 2088 } else if ((tmp_frame_time != tmp_time[GYR]) && (tmp_time[GYR] != 0)) { 2089 DEBUG_PRINT("frame %d gyr time: 0x%08x -> 0x%08x, jumped %d msec\n", 2090 frame_num, 2091 (unsigned int)tmp_time[GYR], 2092 (unsigned int)tmp_frame_time, 2093 (int)(5 * ((int64_t)(tmp_frame_time - tmp_time[GYR]) >> 7))); 2094 } 2095 #endif 2096 } 2097 mTask.prev_frame_time[GYR] = tmp_frame_time; 2098 i += 6; 2099 size -= 6; 2100 observed[GYR] = true; 2101 } else { 2102 size = 0; 2103 } 2104 } 2105 if (fh_param & 1) { // have accel data 2106 if (size >= 6) { 2107 if (frame_sensor_time_valid) { 2108 parseRawData(&mTask.sensors[ACC], &buf[i], kScale_acc, tmp_frame_time); 2109 #if TIMESTAMP_DBG 2110 if (mTask.prev_frame_time[ACC] == ULONG_LONG_MAX) { 2111 DEBUG_PRINT("acc enabled: frame %d time 0x%08x\n", 2112 frame_num, (unsigned int)tmp_frame_time); 2113 } else if ((tmp_frame_time != tmp_time[ACC]) && (tmp_time[ACC] != 0)) { 2114 DEBUG_PRINT("frame %d gyr time: 0x%08x -> 0x%08x, jumped %d msec\n", 2115 frame_num, 2116 (unsigned int)tmp_time[ACC], 2117 (unsigned int)tmp_frame_time, 2118 (int)(5 * ((int64_t)(tmp_frame_time - tmp_time[ACC]) >> 7))); 2119 } 2120 #endif 2121 } 2122 mTask.prev_frame_time[ACC] = tmp_frame_time; 2123 i += 6; 2124 size -= 6; 2125 observed[ACC] = true; 2126 } else { 2127 size = 0; 2128 } 2129 } 2130 2131 if (observed[ACC] || observed[GYR] || observed[MAG]) 2132 frame_sensor_time = tmp_frame_time; 2133 } else { 2134 size = 0; // drop this batch 2135 ERROR_PRINT("Invalid fh_mode\n"); 2136 } 2137 } 2138 2139 //flush data events. 2140 flushAllData(); 2141 } 2142 2143 /* 2144 * Read the interrupt type and send corresponding event 2145 * If it's anymo or double tap, also send a single uint32 to indicate which axies 2146 * is this interrupt triggered. 2147 * If it's flat, also send a bit to indicate flat/non-flat position. 2148 * If it's step detector, check if we need to send the total step count. 2149 */ 2150 static void int2Handling(void) 2151 { 2152 TDECL(); 2153 union EmbeddedDataPoint trigger_axies; 2154 uint8_t int_status_0 = mTask.statusBuffer[1]; 2155 uint8_t int_status_1 = mTask.statusBuffer[2]; 2156 if (int_status_0 & INT_STEP) { 2157 if (mTask.sensors[STEP].powered) { 2158 DEBUG_PRINT("Detected step\n"); 2159 osEnqueueEvt(EVT_SENSOR_STEP, NULL, NULL); 2160 } 2161 if (mTask.sensors[STEPCNT].powered) { 2162 T(pending_step_cnt) = true; 2163 } 2164 } 2165 if ((int_status_0 & INT_ANY_MOTION) && mTask.sensors[ANYMO].powered) { 2166 // bit [0:2] of INT_STATUS[2] is set when anymo is triggered by x, y or 2167 // z axies respectively. bit [3] indicates the slope. 2168 trigger_axies.idata = (mTask.statusBuffer[3] & 0x0f); 2169 DEBUG_PRINT("Detected any motion\n"); 2170 osEnqueueEvt(EVT_SENSOR_ANY_MOTION, trigger_axies.vptr, NULL); 2171 } 2172 if ((int_status_0 & INT_DOUBLE_TAP) && mTask.sensors[DTAP].powered) { 2173 // bit [4:6] of INT_STATUS[2] is set when double tap is triggered by 2174 // x, y or z axies respectively. bit [7] indicates the slope. 2175 trigger_axies.idata = ((mTask.statusBuffer[3] & 0xf0) >> 4); 2176 DEBUG_PRINT("Detected double tap\n"); 2177 osEnqueueEvt(EVT_SENSOR_DOUBLE_TAP, trigger_axies.vptr, NULL); 2178 } 2179 if ((int_status_0 & INT_FLAT) && mTask.sensors[FLAT].powered) { 2180 // bit [7] of INT_STATUS[3] indicates flat/non-flat position 2181 trigger_axies.idata = ((mTask.statusBuffer[4] & 0x80) >> 7); 2182 DEBUG_PRINT("Detected flat\n"); 2183 osEnqueueEvt(EVT_SENSOR_FLAT, trigger_axies.vptr, NULL); 2184 } 2185 if ((int_status_1 & INT_NO_MOTION) && mTask.sensors[NOMO].powered) { 2186 DEBUG_PRINT("Detected no motion\n"); 2187 osEnqueueEvt(EVT_SENSOR_NO_MOTION, NULL, NULL); 2188 } 2189 return; 2190 } 2191 2192 static void int2Evt(void) 2193 { 2194 TDECL(); 2195 if (trySwitchState(SENSOR_INT_2_HANDLING)) { 2196 // Read the interrupt reg value to determine what interrupts 2197 SPI_READ(BMI160_REG_INT_STATUS_0, 4, &mTask.statusBuffer); 2198 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask, __FUNCTION__); 2199 } else { 2200 // even if we are still in SENSOR_INT_2_HANDLING, the SPI may already finished and we need 2201 // to issue another SPI read to get the latest status 2202 mTask.pending_int[1] = true; 2203 } 2204 } 2205 2206 // bits[6:7] in OFFSET[6] to enable/disable gyro/accel offset. 2207 // bits[0:5] in OFFSET[6] stores the most significant 2 bits of gyro offset at 2208 // its x, y, z axies. 2209 // Calculate the stored gyro offset and compose it with the intended 2210 // enable/disable mode for gyro/accel offset to determine the value for 2211 // OFFSET[6]. 2212 static uint8_t offset6Mode(void) 2213 { 2214 uint8_t mode = 0; 2215 if (mTask.sensors[GYR].offset_enable) 2216 mode |= 0x01 << 7; 2217 if (mTask.sensors[ACC].offset_enable) 2218 mode |= 0x01 << 6; 2219 mode |= (mTask.sensors[GYR].offset[2] & 0x0300) >> 4; 2220 mode |= (mTask.sensors[GYR].offset[1] & 0x0300) >> 6; 2221 mode |= (mTask.sensors[GYR].offset[0] & 0x0300) >> 8; 2222 DEBUG_PRINT("OFFSET_6_MODE is: %02x\n", mode); 2223 return mode; 2224 } 2225 2226 static bool saveCalibration() 2227 { 2228 TDECL(); 2229 if (trySwitchState(SENSOR_SAVE_CALIBRATION)) { 2230 if (mTask.sensors[ACC].offset_enable) { 2231 SPI_WRITE(BMI160_REG_OFFSET_0, mTask.sensors[ACC].offset[0] & 0xFF, 450); 2232 SPI_WRITE(BMI160_REG_OFFSET_0 + 1, mTask.sensors[ACC].offset[1] & 0xFF, 450); 2233 SPI_WRITE(BMI160_REG_OFFSET_0 + 2, mTask.sensors[ACC].offset[2] & 0xFF, 450); 2234 } 2235 if (mTask.sensors[GYR].offset_enable) { 2236 SPI_WRITE(BMI160_REG_OFFSET_3, mTask.sensors[GYR].offset[0] & 0xFF, 450); 2237 SPI_WRITE(BMI160_REG_OFFSET_3 + 1, mTask.sensors[GYR].offset[1] & 0xFF, 450); 2238 SPI_WRITE(BMI160_REG_OFFSET_3 + 2, mTask.sensors[GYR].offset[2] & 0xFF, 450); 2239 } 2240 SPI_WRITE(BMI160_REG_OFFSET_6, offset6Mode(), 450); 2241 SPI_READ(BMI160_REG_OFFSET_0, 7, &mTask.dataBuffer); 2242 spiBatchTxRx(&mTask.mode, sensorSpiCallback, NULL, __FUNCTION__); 2243 return true; 2244 } else { 2245 DEBUG_PRINT("%s, state != IDLE", __FUNCTION__); 2246 return false; 2247 } 2248 } 2249 2250 static void sendCalibrationResult(uint8_t status, uint8_t sensorType, 2251 int32_t xBias, int32_t yBias, int32_t zBias) { 2252 struct CalibrationData *data = heapAlloc(sizeof(struct CalibrationData)); 2253 if (!data) { 2254 osLog(LOG_WARN, "Couldn't alloc cal result pkt"); 2255 return; 2256 } 2257 2258 data->header.appId = BMI160_APP_ID; 2259 data->header.dataLen = (sizeof(struct CalibrationData) - sizeof(struct HostHubRawPacket)); 2260 data->data_header.msgId = SENSOR_APP_MSG_ID_CAL_RESULT; 2261 data->data_header.sensorType = sensorType; 2262 data->data_header.status = status; 2263 2264 data->xBias = xBias; 2265 data->yBias = yBias; 2266 data->zBias = zBias; 2267 2268 if (!osEnqueueEvtOrFree(EVT_APP_TO_HOST, data, heapFree)) 2269 osLog(LOG_WARN, "Couldn't send cal result evt"); 2270 } 2271 2272 static void accCalibrationHandling(void) 2273 { 2274 TDECL(); 2275 switch (mTask.calibration_state) { 2276 case CALIBRATION_START: 2277 T(mRetryLeft) = RETRY_CNT_CALIBRATION; 2278 2279 // turn ACC to NORMAL mode 2280 SPI_WRITE(BMI160_REG_CMD, 0x11, 50000); 2281 2282 mTask.calibration_state = CALIBRATION_FOC; 2283 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC], __FUNCTION__); 2284 break; 2285 case CALIBRATION_FOC: 2286 2287 // set accel range to +-8g 2288 SPI_WRITE(BMI160_REG_ACC_RANGE, 0x08); 2289 2290 // enable accel fast offset compensation, 2291 // x: 0g, y: 0g, z: 1g 2292 SPI_WRITE(BMI160_REG_FOC_CONF, ACC_FOC_CONFIG); 2293 2294 // start calibration 2295 SPI_WRITE(BMI160_REG_CMD, 0x03, 100000); 2296 2297 // poll the status reg until the calibration finishes. 2298 SPI_READ(BMI160_REG_STATUS, 1, &mTask.statusBuffer, 50000); 2299 2300 mTask.calibration_state = CALIBRATION_WAIT_FOC_DONE; 2301 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC], __FUNCTION__); 2302 break; 2303 case CALIBRATION_WAIT_FOC_DONE: 2304 // if the STATUS REG has bit 3 set, it means calbration is done. 2305 // otherwise, check back in 50ms later. 2306 if (mTask.statusBuffer[1] & 0x08) { 2307 2308 //disable FOC 2309 SPI_WRITE(BMI160_REG_FOC_CONF, 0x00); 2310 2311 //read the offset value for accel 2312 SPI_READ(BMI160_REG_OFFSET_0, 3, &mTask.dataBuffer); 2313 mTask.calibration_state = CALIBRATION_SET_OFFSET; 2314 DEBUG_PRINT("FOC set FINISHED!\n"); 2315 } else { 2316 2317 // calibration hasn't finished yet, go back to wait for 50ms. 2318 SPI_READ(BMI160_REG_STATUS, 1, &mTask.statusBuffer, 50000); 2319 mTask.calibration_state = CALIBRATION_WAIT_FOC_DONE; 2320 T(mRetryLeft)--; 2321 } 2322 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC], __FUNCTION__); 2323 2324 // if calbration hasn't finished after 10 polling on the STATUS reg, 2325 // declare timeout. 2326 if (T(mRetryLeft) == 0) { 2327 mTask.calibration_state = CALIBRATION_TIMEOUT; 2328 } 2329 break; 2330 case CALIBRATION_SET_OFFSET: 2331 mTask.sensors[ACC].offset[0] = mTask.dataBuffer[1]; 2332 mTask.sensors[ACC].offset[1] = mTask.dataBuffer[2]; 2333 mTask.sensors[ACC].offset[2] = mTask.dataBuffer[3]; 2334 // sign extend values 2335 if (mTask.sensors[ACC].offset[0] & 0x80) 2336 mTask.sensors[ACC].offset[0] |= 0xFFFFFF00; 2337 if (mTask.sensors[ACC].offset[1] & 0x80) 2338 mTask.sensors[ACC].offset[1] |= 0xFFFFFF00; 2339 if (mTask.sensors[ACC].offset[2] & 0x80) 2340 mTask.sensors[ACC].offset[2] |= 0xFFFFFF00; 2341 2342 mTask.sensors[ACC].offset_enable = true; 2343 DEBUG_PRINT("ACCELERATION OFFSET is %02x %02x %02x\n", 2344 (unsigned int)mTask.sensors[ACC].offset[0], 2345 (unsigned int)mTask.sensors[ACC].offset[1], 2346 (unsigned int)mTask.sensors[ACC].offset[2]); 2347 2348 sendCalibrationResult(SENSOR_APP_EVT_STATUS_SUCCESS, SENS_TYPE_ACCEL, 2349 mTask.sensors[ACC].offset[0], mTask.sensors[ACC].offset[1], 2350 mTask.sensors[ACC].offset[2]); 2351 2352 // Enable offset compensation for accel 2353 uint8_t mode = offset6Mode(); 2354 SPI_WRITE(BMI160_REG_OFFSET_6, mode); 2355 2356 // turn ACC to SUSPEND mode 2357 SPI_WRITE(BMI160_REG_CMD, 0x10, 5000); 2358 2359 mTask.calibration_state = CALIBRATION_DONE; 2360 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC], __FUNCTION__); 2361 break; 2362 default: 2363 ERROR_PRINT("Invalid calibration state\n"); 2364 break; 2365 } 2366 } 2367 2368 static bool accCalibration(void *cookie) 2369 { 2370 TDECL(); 2371 if (!mTask.sensors[ACC].powered && trySwitchState(SENSOR_CALIBRATING)) { 2372 mTask.calibration_state = CALIBRATION_START; 2373 accCalibrationHandling(); 2374 return true; 2375 } else { 2376 ERROR_PRINT("cannot calibrate accel because sensor is busy\n"); 2377 sendCalibrationResult(SENSOR_APP_EVT_STATUS_BUSY, SENS_TYPE_ACCEL, 0, 0, 0); 2378 return false; 2379 } 2380 } 2381 2382 static bool accCfgData(void *data, void *cookie) 2383 { 2384 int32_t *values = data; 2385 2386 mTask.sensors[ACC].offset[0] = values[0]; 2387 mTask.sensors[ACC].offset[1] = values[1]; 2388 mTask.sensors[ACC].offset[2] = values[2]; 2389 mTask.sensors[ACC].offset_enable = true; 2390 2391 INFO_PRINT("accCfgData: data=%02lx, %02lx, %02lx\n", 2392 values[0] & 0xFF, values[1] & 0xFF, values[2] & 0xFF); 2393 2394 if (!saveCalibration()) { 2395 mTask.pending_calibration_save = true; 2396 } 2397 2398 return true; 2399 } 2400 2401 static void gyrCalibrationHandling(void) 2402 { 2403 TDECL(); 2404 switch (mTask.calibration_state) { 2405 case CALIBRATION_START: 2406 T(mRetryLeft) = RETRY_CNT_CALIBRATION; 2407 2408 // turn GYR to NORMAL mode 2409 SPI_WRITE(BMI160_REG_CMD, 0x15, 50000); 2410 2411 mTask.calibration_state = CALIBRATION_FOC; 2412 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[GYR], __FUNCTION__); 2413 break; 2414 case CALIBRATION_FOC: 2415 2416 // set gyro range to +-2000 deg/sec 2417 SPI_WRITE(BMI160_REG_GYR_RANGE, 0x00); 2418 2419 // enable gyro fast offset compensation 2420 SPI_WRITE(BMI160_REG_FOC_CONF, 0x40); 2421 2422 // start FOC 2423 SPI_WRITE(BMI160_REG_CMD, 0x03, 100000); 2424 2425 // poll the status reg until the calibration finishes. 2426 SPI_READ(BMI160_REG_STATUS, 1, &mTask.statusBuffer, 50000); 2427 2428 mTask.calibration_state = CALIBRATION_WAIT_FOC_DONE; 2429 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[GYR], __FUNCTION__); 2430 break; 2431 case CALIBRATION_WAIT_FOC_DONE: 2432 2433 // if the STATUS REG has bit 3 set, it means calbration is done. 2434 // otherwise, check back in 50ms later. 2435 if (mTask.statusBuffer[1] & 0x08) { 2436 2437 // disable gyro fast offset compensation 2438 SPI_WRITE(BMI160_REG_FOC_CONF, 0x00); 2439 2440 //read the offset value for gyro 2441 SPI_READ(BMI160_REG_OFFSET_3, 4, &mTask.dataBuffer); 2442 mTask.calibration_state = CALIBRATION_SET_OFFSET; 2443 DEBUG_PRINT("FOC set FINISHED!\n"); 2444 } else { 2445 2446 // calibration hasn't finished yet, go back to wait for 50ms. 2447 SPI_READ(BMI160_REG_STATUS, 1, &mTask.statusBuffer, 50000); 2448 mTask.calibration_state = CALIBRATION_WAIT_FOC_DONE; 2449 T(mRetryLeft)--; 2450 } 2451 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[GYR], __FUNCTION__); 2452 2453 // if calbration hasn't finished after 10 polling on the STATUS reg, 2454 // declare timeout. 2455 if (T(mRetryLeft) == 0) { 2456 mTask.calibration_state = CALIBRATION_TIMEOUT; 2457 } 2458 break; 2459 case CALIBRATION_SET_OFFSET: 2460 mTask.sensors[GYR].offset[0] = ((mTask.dataBuffer[4] & 0x03) << 8) | mTask.dataBuffer[1]; 2461 mTask.sensors[GYR].offset[1] = ((mTask.dataBuffer[4] & 0x0C) << 6) | mTask.dataBuffer[2]; 2462 mTask.sensors[GYR].offset[2] = ((mTask.dataBuffer[4] & 0x30) << 4) | mTask.dataBuffer[3]; 2463 // sign extend values 2464 if (mTask.sensors[GYR].offset[0] & 0x200) 2465 mTask.sensors[GYR].offset[0] |= 0xFFFFFC00; 2466 if (mTask.sensors[GYR].offset[1] & 0x200) 2467 mTask.sensors[GYR].offset[1] |= 0xFFFFFC00; 2468 if (mTask.sensors[GYR].offset[2] & 0x200) 2469 mTask.sensors[GYR].offset[2] |= 0xFFFFFC00; 2470 2471 mTask.sensors[GYR].offset_enable = true; 2472 DEBUG_PRINT("GYRO OFFSET is %02x %02x %02x\n", 2473 (unsigned int)mTask.sensors[GYR].offset[0], 2474 (unsigned int)mTask.sensors[GYR].offset[1], 2475 (unsigned int)mTask.sensors[GYR].offset[2]); 2476 2477 sendCalibrationResult(SENSOR_APP_EVT_STATUS_SUCCESS, SENS_TYPE_GYRO, 2478 mTask.sensors[GYR].offset[0], mTask.sensors[GYR].offset[1], 2479 mTask.sensors[GYR].offset[2]); 2480 2481 // Enable offset compensation for gyro 2482 uint8_t mode = offset6Mode(); 2483 SPI_WRITE(BMI160_REG_OFFSET_6, mode); 2484 2485 // turn GYR to SUSPEND mode 2486 SPI_WRITE(BMI160_REG_CMD, 0x14, 1000); 2487 2488 mTask.calibration_state = CALIBRATION_DONE; 2489 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[GYR], __FUNCTION__); 2490 break; 2491 default: 2492 ERROR_PRINT("Invalid calibration state\n"); 2493 break; 2494 } 2495 } 2496 2497 static bool gyrCalibration(void *cookie) 2498 { 2499 TDECL(); 2500 if (!mTask.sensors[GYR].powered && trySwitchState(SENSOR_CALIBRATING)) { 2501 mTask.calibration_state = CALIBRATION_START; 2502 gyrCalibrationHandling(); 2503 return true; 2504 } else { 2505 ERROR_PRINT("cannot calibrate gyro because sensor is busy\n"); 2506 sendCalibrationResult(SENSOR_APP_EVT_STATUS_BUSY, SENS_TYPE_GYRO, 0, 0, 0); 2507 return false; 2508 } 2509 } 2510 2511 static bool gyrCfgData(void *data, void *cookie) 2512 { 2513 int32_t *values = data; 2514 2515 mTask.sensors[GYR].offset[0] = values[0]; 2516 mTask.sensors[GYR].offset[1] = values[1]; 2517 mTask.sensors[GYR].offset[2] = values[2]; 2518 mTask.sensors[GYR].offset_enable = true; 2519 2520 INFO_PRINT("gyrCfgData: data=%02lx, %02lx, %02lx\n", 2521 values[0] & 0xFF, values[1] & 0xFF, values[2] & 0xFF); 2522 2523 if (!saveCalibration()) { 2524 mTask.pending_calibration_save = true; 2525 } 2526 2527 return true; 2528 } 2529 2530 static bool magCfgData(void *data, void *cookie) 2531 { 2532 float *values = data; 2533 2534 INFO_PRINT("magCfgData: %ld, %ld, %ld\n", 2535 (int32_t)(values[0] * 1000), (int32_t)(values[1] * 1000), (int32_t)(values[2] * 1000)); 2536 2537 #ifdef MAG_SLAVE_PRESENT 2538 mTask.moc.x_bias = values[0]; 2539 mTask.moc.y_bias = values[1]; 2540 mTask.moc.z_bias = values[2]; 2541 #endif 2542 2543 mTask.magBiasPosted = false; 2544 2545 return true; 2546 } 2547 2548 #define DEC_OPS(power, firmware, rate, flush) \ 2549 .sensorPower = power, \ 2550 .sensorFirmwareUpload = firmware, \ 2551 .sensorSetRate = rate, \ 2552 .sensorFlush = flush 2553 2554 #define DEC_OPS_SEND(power, firmware, rate, flush, send) \ 2555 DEC_OPS(power, firmware, rate, flush), \ 2556 .sensorSendOneDirectEvt = send 2557 2558 #define DEC_OPS_CAL_CFG(power, firmware, rate, flush, cal, cfg) \ 2559 DEC_OPS(power, firmware, rate, flush), \ 2560 .sensorCalibrate = cal, \ 2561 .sensorCfgData = cfg 2562 2563 #define DEC_OPS_CFG(power, firmware, rate, flush, cfg) \ 2564 DEC_OPS(power, firmware, rate, flush), \ 2565 .sensorCfgData = cfg 2566 2567 static const struct SensorOps mSensorOps[NUM_OF_SENSOR] = 2568 { 2569 { DEC_OPS_CAL_CFG(accPower, accFirmwareUpload, accSetRate, accFlush, accCalibration, 2570 accCfgData) }, 2571 { DEC_OPS_CAL_CFG(gyrPower, gyrFirmwareUpload, gyrSetRate, gyrFlush, gyrCalibration, 2572 gyrCfgData) }, 2573 { DEC_OPS_CFG(magPower, magFirmwareUpload, magSetRate, magFlush, magCfgData) }, 2574 { DEC_OPS(stepPower, stepFirmwareUpload, stepSetRate, stepFlush) }, 2575 { DEC_OPS(doubleTapPower, doubleTapFirmwareUpload, doubleTapSetRate, doubleTapFlush) }, 2576 { DEC_OPS(flatPower, flatFirmwareUpload, flatSetRate, flatFlush) }, 2577 { DEC_OPS(anyMotionPower, anyMotionFirmwareUpload, anyMotionSetRate, anyMotionFlush) }, 2578 { DEC_OPS(noMotionPower, noMotionFirmwareUpload, noMotionSetRate, noMotionFlush) }, 2579 { DEC_OPS_SEND(stepCntPower, stepCntFirmwareUpload, stepCntSetRate, stepCntFlush, 2580 stepCntSendLastData) }, 2581 }; 2582 2583 static void configEvent(struct BMI160Sensor *mSensor, struct ConfigStat *ConfigData) 2584 { 2585 int i; 2586 2587 for (i = 0; &mTask.sensors[i] != mSensor; i++) ; 2588 2589 if (ConfigData->enable == 0 && mSensor->powered) 2590 mSensorOps[i].sensorPower(false, (void *)i); 2591 else if (ConfigData->enable == 1 && !mSensor->powered) 2592 mSensorOps[i].sensorPower(true, (void *)i); 2593 else 2594 mSensorOps[i].sensorSetRate(ConfigData->rate, ConfigData->latency, (void *)i); 2595 } 2596 2597 static void timeSyncEvt(uint32_t evtGeneration, bool evtDataValid) 2598 { 2599 TDECL(); 2600 // not processing pending events 2601 if (evtDataValid) { 2602 // stale event 2603 if (evtGeneration != mTask.poll_generation) 2604 return; 2605 2606 mTask.active_poll_generation = mTask.poll_generation; 2607 } 2608 2609 if (trySwitchState(SENSOR_TIME_SYNC)) { 2610 SPI_READ(BMI160_REG_SENSORTIME_0, 3, &mTask.sensorTimeBuffer); 2611 SPI_READ(BMI160_REG_TEMPERATURE_0, 2, &mTask.temperatureBuffer); 2612 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask, __FUNCTION__); 2613 } else { 2614 mTask.pending_time_sync = true; 2615 } 2616 } 2617 2618 static void processPendingEvt(void) 2619 { 2620 TDECL(); 2621 enum SensorIndex i; 2622 if (mTask.pending_int[0]) { 2623 mTask.pending_int[0] = false; 2624 initiateFifoRead(false /*isInterruptContext*/); 2625 return; 2626 } 2627 if (mTask.pending_int[1]) { 2628 mTask.pending_int[1] = false; 2629 int2Evt(); 2630 return; 2631 } 2632 if (mTask.pending_time_sync) { 2633 mTask.pending_time_sync = false; 2634 timeSyncEvt(0, false); 2635 return; 2636 } 2637 for (i = ACC; i < NUM_OF_SENSOR; i++) { 2638 if (mTask.pending_config[i]) { 2639 mTask.pending_config[i] = false; 2640 configEvent(&mTask.sensors[i], &mTask.sensors[i].pConfig); 2641 return; 2642 } 2643 } 2644 if (mTask.sensors[STEPCNT].flush > 0 || T(pending_step_cnt)) { 2645 T(pending_step_cnt) = T(pending_step_cnt) && !stepCntFlushGetData(); 2646 return; 2647 } 2648 if (mTask.pending_calibration_save) { 2649 mTask.pending_calibration_save = !saveCalibration(); 2650 return; 2651 } 2652 } 2653 2654 static void sensorInit(void) 2655 { 2656 TDECL(); 2657 switch (mTask.init_state) { 2658 case RESET_BMI160: 2659 DEBUG_PRINT("Performing soft reset\n"); 2660 // perform soft reset and wait for 100ms 2661 SPI_WRITE(BMI160_REG_CMD, 0xb6, 100000); 2662 // dummy reads after soft reset, wait 100us 2663 SPI_READ(BMI160_REG_MAGIC, 1, &mTask.dataBuffer, 100); 2664 2665 mTask.init_state = INIT_BMI160; 2666 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask, "sensorInit RESET" ); 2667 break; 2668 2669 case INIT_BMI160: 2670 // Read any pending interrupts to reset them 2671 SPI_READ(BMI160_REG_INT_STATUS_0, 4, &mTask.statusBuffer); 2672 2673 // disable accel, gyro and mag data in FIFO, enable header, enable time. 2674 SPI_WRITE(BMI160_REG_FIFO_CONFIG_1, 0x12, 450); 2675 2676 // set the watermark to 24 byte 2677 SPI_WRITE(BMI160_REG_FIFO_CONFIG_0, 0x06, 450); 2678 2679 // FIFO watermark and fifo_full interrupt enabled 2680 SPI_WRITE(BMI160_REG_INT_EN_0, 0x00, 450); 2681 SPI_WRITE(BMI160_REG_INT_EN_1, 0x60, 450); 2682 SPI_WRITE(BMI160_REG_INT_EN_2, 0x00, 450); 2683 2684 // INT1, INT2 enabled, high-edge (push-pull) triggered. 2685 SPI_WRITE(BMI160_REG_INT_OUT_CTRL, 0xbb, 450); 2686 2687 // INT1, INT2 input disabled, interrupt mode: non-latched 2688 SPI_WRITE(BMI160_REG_INT_LATCH, 0x00, 450); 2689 2690 // Map data interrupts (e.g., FIFO) to INT1 and physical 2691 // interrupts (e.g., any motion) to INT2 2692 SPI_WRITE(BMI160_REG_INT_MAP_0, 0x00, 450); 2693 SPI_WRITE(BMI160_REG_INT_MAP_1, 0xE1, 450); 2694 SPI_WRITE(BMI160_REG_INT_MAP_2, 0xFF, 450); 2695 2696 // Use pre-filtered data for tap interrupt 2697 SPI_WRITE(BMI160_REG_INT_DATA_0, 0x08); 2698 2699 // Disable PMU_TRIGGER 2700 SPI_WRITE(BMI160_REG_PMU_TRIGGER, 0x00, 450); 2701 2702 // tell gyro and accel to NOT use the FOC offset. 2703 mTask.sensors[ACC].offset_enable = false; 2704 mTask.sensors[GYR].offset_enable = false; 2705 SPI_WRITE(BMI160_REG_OFFSET_6, offset6Mode(), 450); 2706 2707 // initial range for accel (+-8g) and gyro (+-2000 degree). 2708 SPI_WRITE(BMI160_REG_ACC_RANGE, 0x08, 450); 2709 SPI_WRITE(BMI160_REG_GYR_RANGE, 0x00, 450); 2710 2711 // Reset step counter 2712 SPI_WRITE(BMI160_REG_CMD, 0xB2, 10000); 2713 // Reset interrupt 2714 SPI_WRITE(BMI160_REG_CMD, 0xB1, 10000); 2715 // Reset fifo 2716 SPI_WRITE(BMI160_REG_CMD, 0xB0, 10000); 2717 2718 #ifdef MAG_SLAVE_PRESENT 2719 mTask.init_state = INIT_MAG; 2720 mTask.mag_state = MAG_SET_START; 2721 #else 2722 // no mag connected to secondary interface 2723 mTask.init_state = INIT_ON_CHANGE_SENSORS; 2724 #endif 2725 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask, "sensorInit INIT"); 2726 break; 2727 2728 case INIT_MAG: 2729 // Don't check statusBuffer if we are just starting mag config 2730 if (mTask.mag_state == MAG_SET_START) { 2731 T(mRetryLeft) = RETRY_CNT_MAG; 2732 magConfig(); 2733 } else if (mTask.mag_state < MAG_SET_DATA && mTask.statusBuffer[1] & 0x04) { 2734 // fixme: poll_until to reduce states 2735 // fixme: check should be done before SPI_READ in MAG_READ 2736 SPI_READ(BMI160_REG_STATUS, 1, &mTask.statusBuffer, 1000); 2737 if (--T(mRetryLeft) == 0) { 2738 ERROR_PRINT("INIT_MAG failed\n"); 2739 // fixme: duplicate suspend mag here 2740 mTask.mag_state = MAG_INIT_FAILED; 2741 mTask.init_state = INIT_ON_CHANGE_SENSORS; 2742 } 2743 } else { 2744 T(mRetryLeft) = RETRY_CNT_MAG; 2745 magConfig(); 2746 } 2747 2748 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask, "sensorInit INIT_MAG"); 2749 break; 2750 2751 case INIT_ON_CHANGE_SENSORS: 2752 // configure any_motion and no_motion for 50Hz accel samples 2753 configMotion(MOTION_ODR); 2754 2755 // select no_motion over slow_motion 2756 // select any_motion over significant motion 2757 SPI_WRITE(BMI160_REG_INT_MOTION_3, 0x15, 450); 2758 2759 // int_tap_quiet=30ms, int_tap_shock=75ms, int_tap_dur=150ms 2760 SPI_WRITE(BMI160_REG_INT_TAP_0, 0x42, 450); 2761 2762 // int_tap_th = 7 * 250 mg (8-g range) 2763 SPI_WRITE(BMI160_REG_INT_TAP_1, TAP_THRESHOLD, 450); 2764 2765 // config step detector 2766 SPI_WRITE(BMI160_REG_STEP_CONF_0, 0x15, 450); 2767 SPI_WRITE(BMI160_REG_STEP_CONF_1, 0x03, 450); 2768 2769 // int_flat_theta = 44.8 deg * (16/64) = 11.2 deg 2770 SPI_WRITE(BMI160_REG_INT_FLAT_0, 0x10, 450); 2771 2772 // int_flat_hold_time = (640 msec) 2773 // int_flat_hy = 44.8 * 4 / 64 = 2.8 deg 2774 SPI_WRITE(BMI160_REG_INT_FLAT_1, 0x14, 450); 2775 2776 mTask.init_state = INIT_DONE; 2777 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask, "sensorInit INIT_ONC"); 2778 break; 2779 2780 default: 2781 INFO_PRINT("Invalid init_state.\n"); 2782 } 2783 } 2784 2785 static void handleSpiDoneEvt(const void* evtData) 2786 { 2787 TDECL(); 2788 struct BMI160Sensor *mSensor; 2789 uint64_t SensorTime; 2790 int16_t temperature16; 2791 int i; 2792 bool returnIdle = false; 2793 2794 switch (GET_STATE()) { 2795 case SENSOR_BOOT: 2796 T(mRetryLeft) = RETRY_CNT_ID; 2797 SET_STATE(SENSOR_VERIFY_ID); 2798 // dummy reads after boot, wait 100us 2799 SPI_READ(BMI160_REG_MAGIC, 1, &mTask.statusBuffer, 100); 2800 // read the device ID for bmi160 2801 SPI_READ(BMI160_REG_ID, 1, &mTask.dataBuffer); 2802 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask, "spiDone SENSOR_BOOT"); 2803 break; 2804 case SENSOR_VERIFY_ID: 2805 if (mTask.dataBuffer[1] != BMI160_ID) { 2806 T(mRetryLeft) --; 2807 ERROR_PRINT("failed id match: %02x\n", mTask.dataBuffer[1]); 2808 if (T(mRetryLeft) == 0) 2809 break; 2810 // For some reason the first ID read will fail to get the 2811 // correct value. need to retry a few times. 2812 SET_STATE(SENSOR_BOOT); 2813 timTimerSet(100000000, 100, 100, sensorTimerCallback, NULL, true); 2814 break; 2815 } else { 2816 SET_STATE(SENSOR_INITIALIZING); 2817 mTask.init_state = RESET_BMI160; 2818 sensorInit(); 2819 break; 2820 } 2821 case SENSOR_INITIALIZING: 2822 if (mTask.init_state == INIT_DONE) { 2823 DEBUG_PRINT("Done initialzing, system IDLE\n"); 2824 for (i=0; i<NUM_OF_SENSOR; i++) 2825 sensorRegisterInitComplete(mTask.sensors[i].handle); 2826 // In case other tasks have already requested us before we finish booting up. 2827 returnIdle = true; 2828 } else { 2829 sensorInit(); 2830 } 2831 break; 2832 case SENSOR_POWERING_UP: 2833 mSensor = (struct BMI160Sensor *)evtData; 2834 if (mSensor->idx > MAG && ++mTask.active_oneshot_sensor_cnt == 1) { 2835 // if this is the first one-shot sensor to enable, we need 2836 // to request the accel at 50Hz. 2837 sensorRequest(mTask.tid, mTask.sensors[ACC].handle, SENSOR_HZ(50), SENSOR_LATENCY_NODATA); 2838 //DEBUG_PRINT("oneshot on\n"); 2839 } 2840 sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, 1, 0); 2841 returnIdle = true; 2842 break; 2843 case SENSOR_POWERING_DOWN: 2844 mSensor = (struct BMI160Sensor *)evtData; 2845 if (mSensor->idx > MAG && --mTask.active_oneshot_sensor_cnt == 0) { 2846 // if this is the last one-shot sensor to disable, we need to 2847 // release the accel. 2848 sensorRelease(mTask.tid, mTask.sensors[ACC].handle); 2849 //DEBUG_PRINT("oneshot off\n"); 2850 } 2851 sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, 0, 0); 2852 2853 if (mTask.pending_dispatch) { 2854 mTask.pending_dispatch = false; 2855 dispatchData(); 2856 } 2857 returnIdle = true; 2858 break; 2859 case SENSOR_INT_1_HANDLING: 2860 dispatchData(); 2861 sendFlushEvt(); 2862 returnIdle = true; 2863 break; 2864 case SENSOR_INT_2_HANDLING: 2865 int2Handling(); 2866 returnIdle = true; 2867 break; 2868 case SENSOR_CONFIG_CHANGING: 2869 mSensor = (struct BMI160Sensor *)evtData; 2870 sensorSignalInternalEvt(mSensor->handle, 2871 SENSOR_INTERNAL_EVT_RATE_CHG, mSensor->rate, mSensor->latency); 2872 2873 if (mTask.pending_dispatch) { 2874 mTask.pending_dispatch = false; 2875 dispatchData(); 2876 } 2877 2878 returnIdle = true; 2879 break; 2880 case SENSOR_CALIBRATING: 2881 mSensor = (struct BMI160Sensor *)evtData; 2882 if (mTask.calibration_state == CALIBRATION_DONE) { 2883 DEBUG_PRINT("DONE calibration\n"); 2884 returnIdle = true; 2885 } else if (mTask.calibration_state == CALIBRATION_TIMEOUT) { 2886 DEBUG_PRINT("Calibration TIMED OUT\n"); 2887 sendCalibrationResult(SENSOR_APP_EVT_STATUS_ERROR, 2888 (mSensor->idx == ACC) ? SENS_TYPE_ACCEL : SENS_TYPE_GYRO, 0, 0, 0); 2889 returnIdle = true; 2890 } else if (mSensor->idx == ACC) { 2891 accCalibrationHandling(); 2892 } else if (mSensor->idx == GYR) { 2893 gyrCalibrationHandling(); 2894 } 2895 break; 2896 case SENSOR_STEP_CNT: 2897 sendStepCnt(); 2898 returnIdle = true; 2899 break; 2900 case SENSOR_TIME_SYNC: 2901 SensorTime = parseSensortime(mTask.sensorTimeBuffer[1] | 2902 (mTask.sensorTimeBuffer[2] << 8) | (mTask.sensorTimeBuffer[3] << 16)); 2903 map_sensortime_to_rtc_time(SensorTime, rtcGetTime()); 2904 2905 temperature16 = (mTask.temperatureBuffer[1] | (mTask.temperatureBuffer[2] << 8)); 2906 if (temperature16 == 0x8000) { 2907 mTask.tempCelsius = kTempInvalid; 2908 } else { 2909 mTask.tempCelsius = 23.0f + temperature16 * kScale_temp; 2910 mTask.tempTime = rtcGetTime(); 2911 } 2912 2913 if (mTask.active_poll_generation == mTask.poll_generation) { 2914 // attach the generation number to event 2915 timTimerSet(kTimeSyncPeriodNs, 100, 100, timeSyncCallback, 2916 (void *)mTask.poll_generation, true); 2917 } 2918 2919 returnIdle = true; 2920 break; 2921 case SENSOR_SAVE_CALIBRATION: 2922 DEBUG_PRINT("SENSOR_SAVE_CALIBRATION: %02x %02x %02x %02x %02x %02x %02x\n", 2923 mTask.dataBuffer[1], mTask.dataBuffer[2], mTask.dataBuffer[3], mTask.dataBuffer[4], 2924 mTask.dataBuffer[5], mTask.dataBuffer[6], mTask.dataBuffer[7]); 2925 returnIdle = true; 2926 break; 2927 default: 2928 break; 2929 } 2930 2931 if (returnIdle) { 2932 SET_STATE(SENSOR_IDLE); 2933 processPendingEvt(); 2934 } 2935 } 2936 2937 static void handleEvent(uint32_t evtType, const void* evtData) 2938 { 2939 TDECL(); 2940 uint64_t currTime; 2941 uint8_t *packet; 2942 float newMagBias; 2943 2944 switch (evtType) { 2945 case EVT_APP_START: 2946 SET_STATE(SENSOR_BOOT); 2947 osEventUnsubscribe(mTask.tid, EVT_APP_START); 2948 2949 // wait 100ms for sensor to boot 2950 currTime = timGetTime(); 2951 if (currTime < 100000000ULL) { 2952 timTimerSet(100000000 - currTime, 100, 100, sensorTimerCallback, NULL, true); 2953 break; 2954 } 2955 /* We have already been powered on long enough - fall through */ 2956 case EVT_SPI_DONE: 2957 handleSpiDoneEvt(evtData); 2958 break; 2959 2960 case EVT_APP_FROM_HOST: 2961 packet = (uint8_t*)evtData; 2962 if (packet[0] == sizeof(float)) { 2963 memcpy(&newMagBias, packet+1, sizeof(float)); 2964 #ifdef MAG_SLAVE_PRESENT 2965 magCalAddBias(&mTask.moc, (mTask.last_charging_bias_x - newMagBias), 0.0, 0.0); 2966 #endif 2967 mTask.last_charging_bias_x = newMagBias; 2968 mTask.magBiasPosted = false; 2969 } 2970 break; 2971 2972 case EVT_SENSOR_INTERRUPT_1: 2973 initiateFifoRead(false /*isInterruptContext*/); 2974 break; 2975 case EVT_SENSOR_INTERRUPT_2: 2976 int2Evt(); 2977 break; 2978 case EVT_TIME_SYNC: 2979 timeSyncEvt((uint32_t)evtData, true); 2980 default: 2981 break; 2982 } 2983 } 2984 2985 static void initSensorStruct(struct BMI160Sensor *sensor, enum SensorIndex idx) 2986 { 2987 sensor->idx = idx; 2988 sensor->powered = false; 2989 sensor->configed = false; 2990 sensor->rate = 0; 2991 sensor->offset[0] = 0; 2992 sensor->offset[1] = 0; 2993 sensor->offset[2] = 0; 2994 sensor->latency = 0; 2995 sensor->data_evt = NULL; 2996 sensor->flush = 0; 2997 sensor->prev_rtc_time = 0; 2998 } 2999 3000 static bool startTask(uint32_t task_id) 3001 { 3002 TDECL(); 3003 DEBUG_PRINT(" IMU: %ld\n", task_id); 3004 3005 enum SensorIndex i; 3006 size_t slabSize; 3007 3008 time_init(); 3009 3010 T(tid) = task_id; 3011 3012 T(Int1) = gpioRequest(BMI160_INT1_PIN); 3013 T(Isr1).func = bmi160Isr1; 3014 T(Int2) = gpioRequest(BMI160_INT2_PIN); 3015 T(Isr2).func = bmi160Isr2; 3016 T(pending_int[0]) = false; 3017 T(pending_int[1]) = false; 3018 T(pending_step_cnt) = false; 3019 T(pending_dispatch) = false; 3020 T(frame_sensortime_valid) = false; 3021 T(poll_generation) = 0; 3022 T(tempCelsius) = kTempInvalid; 3023 T(tempTime) = 0; 3024 3025 T(mode).speed = BMI160_SPI_SPEED_HZ; 3026 T(mode).bitsPerWord = 8; 3027 T(mode).cpol = SPI_CPOL_IDLE_HI; 3028 T(mode).cpha = SPI_CPHA_TRAILING_EDGE; 3029 T(mode).nssChange = true; 3030 T(mode).format = SPI_FORMAT_MSB_FIRST; 3031 T(cs) = GPIO_PB(12); 3032 3033 T(watermark) = 0; 3034 3035 spiMasterRequest(BMI160_SPI_BUS_ID, &T(spiDev)); 3036 3037 for (i = ACC; i < NUM_OF_SENSOR; i++) { 3038 initSensorStruct(&T(sensors[i]), i); 3039 T(sensors[i]).handle = sensorRegister(&mSensorInfo[i], &mSensorOps[i], NULL, false); 3040 T(pending_config[i]) = false; 3041 } 3042 3043 osEventSubscribe(mTask.tid, EVT_APP_START); 3044 3045 #ifdef MAG_SLAVE_PRESENT 3046 initMagCal(&mTask.moc, 3047 0.0f, 0.0f, 0.0f, // bias x, y, z 3048 1.0f, 0.0f, 0.0f, // c00, c01, c02 3049 0.0f, 1.0f, 0.0f, // c10, c11, c12 3050 0.0f, 0.0f, 1.0f); // c20, c21, c22 3051 #endif 3052 3053 slabSize = sizeof(struct TripleAxisDataEvent) + 3054 MAX_NUM_COMMS_EVENT_SAMPLES * sizeof(struct TripleAxisDataPoint); 3055 3056 // each event has 15 samples, with 7 bytes per sample from the fifo. 3057 // the fifo size is 1K. 3058 // 20 slabs because some slabs may only hold 1-2 samples. 3059 // XXX: this consumes too much memeory, need to optimize 3060 T(mDataSlab) = slabAllocatorNew(slabSize, 4, 20); 3061 if (!T(mDataSlab)) { 3062 INFO_PRINT("slabAllocatorNew() failed\n"); 3063 return false; 3064 } 3065 T(mWbufCnt) = 0; 3066 T(mRegCnt) = 0; 3067 T(spiInUse) = false; 3068 3069 T(interrupt_enable_0) = 0x00; 3070 T(interrupt_enable_2) = 0x00; 3071 3072 // initialize the last bmi160 time to be ULONG_MAX, so that we know it's 3073 // not valid yet. 3074 T(last_sensortime) = 0; 3075 T(frame_sensortime) = ULONG_LONG_MAX; 3076 3077 // it's ok to leave interrupt open all the time. 3078 enableInterrupt(T(Int1), &T(Isr1)); 3079 enableInterrupt(T(Int2), &T(Isr2)); 3080 3081 return true; 3082 } 3083 3084 static void endTask(void) 3085 { 3086 TDECL(); 3087 #ifdef MAG_SLAVE_PRESENT 3088 destroy_mag_cal(&mTask.moc); 3089 #endif 3090 slabAllocatorDestroy(T(mDataSlab)); 3091 spiMasterRelease(mTask.spiDev); 3092 3093 // disable and release interrupt. 3094 disableInterrupt(mTask.Int1, &mTask.Isr1); 3095 disableInterrupt(mTask.Int2, &mTask.Isr2); 3096 gpioRelease(mTask.Int1); 3097 gpioRelease(mTask.Int2); 3098 } 3099 3100 /** 3101 * Parse BMI160 FIFO frame without side effect. 3102 * 3103 * The major purpose of this function is to determine if FIFO content is received completely (start 3104 * to see invalid headers). If not, return the pointer to the beginning last incomplete frame so 3105 * additional read can use this pointer as start of read buffer. 3106 * 3107 * @param buf buffer location 3108 * @param size size of data to be parsed 3109 * 3110 * @return NULL if the FIFO is received completely; or pointer to the beginning of last incomplete 3111 * frame for additional read. 3112 */ 3113 static uint8_t* shallowParseFrame(uint8_t * buf, int size) { 3114 int i = 0; 3115 int iLastFrame = 0; // last valid frame header index 3116 3117 DEBUG_PRINT_IF(DBG_SHALLOW_PARSE, "spf start %p: %x %x %x\n", buf, buf[0], buf[1], buf[2]); 3118 while (size > 0) { 3119 int fh_mode, fh_param; 3120 iLastFrame = i; 3121 3122 if (buf[i] == BMI160_FRAME_HEADER_INVALID) { 3123 // no more data 3124 DEBUG_PRINT_IF(DBG_SHALLOW_PARSE, "spf:at%d=0x80\n", iLastFrame); 3125 return NULL; 3126 } else if (buf[i] == BMI160_FRAME_HEADER_SKIP) { 3127 // artifically added nop frame header, skip 3128 DEBUG_PRINT_IF(DBG_SHALLOW_PARSE, "at %d, skip header\n", i); 3129 i++; 3130 size--; 3131 continue; 3132 } 3133 3134 //++frame_num; 3135 3136 fh_mode = buf[i] >> 6; 3137 fh_param = (buf[i] >> 2) & 0xf; 3138 3139 i++; 3140 size--; 3141 3142 if (fh_mode == 1) { 3143 // control frame. 3144 if (fh_param == 0) { 3145 // skip frame, we skip it (1 byte) 3146 i++; 3147 size--; 3148 DEBUG_PRINT_IF(DBG_SHALLOW_PARSE, "at %d, a skip frame\n", iLastFrame); 3149 } else if (fh_param == 1) { 3150 // sensortime frame (3 bytes) 3151 i += 3; 3152 size -= 3; 3153 DEBUG_PRINT_IF(DBG_SHALLOW_PARSE, "at %d, a sensor_time frame\n", iLastFrame); 3154 } else if (fh_param == 2) { 3155 // fifo_input config frame (1byte) 3156 i++; 3157 size--; 3158 DEBUG_PRINT_IF(DBG_SHALLOW_PARSE, "at %d, a fifo cfg frame\n", iLastFrame); 3159 } else { 3160 size = 0; // drop this batch 3161 DEBUG_PRINT_IF(DBG_SHALLOW_PARSE, "Invalid fh_param in control frame!!\n"); 3162 // mark invalid 3163 buf[iLastFrame] = BMI160_FRAME_HEADER_INVALID; 3164 return NULL; 3165 } 3166 } else if (fh_mode == 2) { 3167 // regular frame, dispatch data to each sensor's own fifo 3168 if (fh_param & 4) { // have mag data 3169 i += 8; 3170 size -= 8; 3171 } 3172 if (fh_param & 2) { // have gyro data 3173 i += 6; 3174 size -= 6; 3175 } 3176 if (fh_param & 1) { // have accel data 3177 i += 6; 3178 size -= 6; 3179 } 3180 DEBUG_PRINT_IF(DBG_SHALLOW_PARSE, "at %d, a reg frame acc %d, gyro %d, mag %d\n", 3181 iLastFrame, fh_param &1 ? 1:0, fh_param&2?1:0, fh_param&4?1:0); 3182 } else { 3183 size = 0; // drop this batch 3184 DEBUG_PRINT_IF(DBG_SHALLOW_PARSE, "spf: Invalid fh_mode %d!!\n", fh_mode); 3185 //mark invalid 3186 buf[iLastFrame] = BMI160_FRAME_HEADER_INVALID; 3187 return NULL; 3188 } 3189 } 3190 3191 // there is a partial frame, return where to write next chunck of data 3192 DEBUG_PRINT_IF(DBG_SHALLOW_PARSE, "partial frame ends %p\n", buf + iLastFrame); 3193 return buf + iLastFrame; 3194 } 3195 3196 /** 3197 * Intialize the first read of chunked SPI read sequence. 3198 * 3199 * @param index starting index of the txrxBuffer in which the data will be write into. 3200 */ 3201 static void chunkedReadInit_(TASK, int index, int size) { 3202 3203 if (GET_STATE() != SENSOR_INT_1_HANDLING) { 3204 ERROR_PRINT("chunkedReadInit in wrong mode"); 3205 return; 3206 } 3207 3208 if (T(mRegCnt)) { 3209 //chunked read are always executed as a single command. This should never happen. 3210 ERROR_PRINT("SPI queue not empty at chunkedReadInit, regcnt = %d", T(mRegCnt)); 3211 // In case it did happen, we do not want to write crap to BMI160. 3212 T(mRegCnt) = 0; 3213 } 3214 3215 T(mWbufCnt) = index; 3216 if (T(mWbufCnt) > FIFO_READ_SIZE) { 3217 // drop data to prevent bigger issue 3218 T(mWbufCnt) = 0; 3219 } 3220 T(chunkReadSize) = size > CHUNKED_READ_SIZE ? size : CHUNKED_READ_SIZE; 3221 3222 DEBUG_PRINT_IF(DBG_CHUNKED, "crd %d>>%d\n", T(chunkReadSize), index); 3223 SPI_READ(BMI160_REG_FIFO_DATA, T(chunkReadSize), &T(dataBuffer)); 3224 spiBatchTxRx(&T(mode), chunkedReadSpiCallback, _task, __FUNCTION__); 3225 } 3226 3227 /** 3228 * Chunked SPI read callback. 3229 * 3230 * Handles the chunked read logic: issue additional read if necessary, or calls sensorSpiCallback() 3231 * if the entire FIFO is read. 3232 * 3233 * @param cookie extra data 3234 * @param err error 3235 * 3236 * @see sensorSpiCallback() 3237 */ 3238 static void chunkedReadSpiCallback(void *cookie, int err) { 3239 TASK = (_Task*) cookie; 3240 3241 T(spiInUse) = false; 3242 DEBUG_PRINT_IF(err !=0 || GET_STATE() != SENSOR_INT_1_HANDLING, 3243 "crcb,e:%d,s:%d", err, (int)GET_STATE()); 3244 bool int1 = gpioGet(T(Int1)); 3245 if (err != 0) { 3246 DEBUG_PRINT_IF(DBG_CHUNKED, "crd retry"); 3247 // read full fifo length to be safe 3248 chunkedReadInit(0, FIFO_READ_SIZE); 3249 return; 3250 } 3251 3252 *T(dataBuffer) = BMI160_FRAME_HEADER_SKIP; // fill the 0x00/0xff hole at the first byte 3253 uint8_t* end = shallowParseFrame(T(dataBuffer), T(chunkReadSize)); 3254 3255 if (end == NULL) { 3256 // if interrupt is still set after read for some reason, set the pending interrupt 3257 // to handle it immediately after data is handled. 3258 T(pending_int[0]) = T(pending_int[0]) || int1; 3259 3260 // recover the buffer and valid data size to make it looks like a single read so that 3261 // real frame parse works properly 3262 T(dataBuffer) = T(txrxBuffer); 3263 T(xferCnt) = FIFO_READ_SIZE; 3264 sensorSpiCallback(cookie, err); 3265 } else { 3266 DEBUG_PRINT_IF(DBG_CHUNKED, "crd cont"); 3267 chunkedReadInit(end - T(txrxBuffer), CHUNKED_READ_SIZE); 3268 } 3269 } 3270 3271 /** 3272 * Initiate read of sensor fifo. 3273 * 3274 * If task is in idle state, init chunked FIFO read; otherwise, submit an interrupt message or mark 3275 * the read pending depending if it is called in interrupt context. 3276 * 3277 * @param isInterruptContext true if called from interrupt context; false otherwise. 3278 * 3279 */ 3280 static void initiateFifoRead_(TASK, bool isInterruptContext) { 3281 if (trySwitchState(SENSOR_INT_1_HANDLING)) { 3282 // estimate first read size to be watermark + 1 more sample + some extra 3283 int firstReadSize = T(watermark) * 4 + 32; // 1+6+6+8+1+3 + extra = 25 + extra = 32 3284 if (firstReadSize < CHUNKED_READ_SIZE) { 3285 firstReadSize = CHUNKED_READ_SIZE; 3286 } 3287 chunkedReadInit(0, firstReadSize); 3288 } else { 3289 if (isInterruptContext) { 3290 // called from interrupt context, queue event 3291 osEnqueuePrivateEvt(EVT_SENSOR_INTERRUPT_1, _task, NULL, T(tid)); 3292 } else { 3293 // non-interrupt context, set pending flag, so next time it will be picked up after 3294 // switching back to idle. 3295 // Note: even if we are still in SENSOR_INT_1_HANDLING, the SPI may already finished and 3296 // we need to issue another SPI read to get the latest status. 3297 T(pending_int[0]) = true; 3298 } 3299 } 3300 } 3301 3302 /** 3303 * Calculate fifo size using normalized input. 3304 * 3305 * @param iPeriod normalized period vector 3306 * @param iLatency normalized latency vector 3307 * @param factor vector that contains size factor for each sensor 3308 * @param n size of the vectors 3309 * 3310 * @return max size of FIFO to guarantee latency requirements of all sensors or SIZE_MAX if no 3311 * sensor is active. 3312 */ 3313 static size_t calcFifoSize(const int* iPeriod, const int* iLatency, const int* factor, int n) { 3314 int i; 3315 3316 int minLatency = INT_MAX; 3317 for (i = 0; i < n; i++) { 3318 if (iLatency[i] > 0) { 3319 minLatency = iLatency[i] < minLatency ? iLatency[i] : minLatency; 3320 } 3321 } 3322 DEBUG_PRINT_IF(DBG_WM_CALC, "cfifo: min latency %d unit", minLatency); 3323 3324 bool anyActive = false; 3325 size_t s = 0; 3326 size_t head = 0; 3327 for (i = 0; i < n; i++) { 3328 if (iPeriod[i] > 0) { 3329 anyActive = true; 3330 size_t t = minLatency / iPeriod[i]; 3331 head = t > head ? t : head; 3332 s += t * factor[i]; 3333 DEBUG_PRINT_IF(DBG_WM_CALC, "cfifo: %d, s+= %d*%d, head = %d", i, t, factor[i], head); 3334 } 3335 } 3336 3337 return anyActive ? head + s : SIZE_MAX; 3338 } 3339 3340 /** 3341 * Calculate the watermark setting from sensor registration information 3342 * 3343 * It is assumed that all sensor period share a common denominator (true for BMI160) and the 3344 * latency of sensor will be lower bounded by its sampling period. 3345 * 3346 * @return watermark register setting 3347 */ 3348 static uint8_t calcWatermark2_(TASK) { 3349 int period[] = {-1, -1, -1}; 3350 int latency[] = {-1, -1, -1}; 3351 const int factor[] = {6, 6, 8}; 3352 int i; 3353 3354 for (i = ACC; i <= MAG; ++i) { 3355 if (T(sensors[i]).configed) { 3356 period[i - ACC] = SENSOR_HZ((float)WATERMARK_MAX_SENSOR_RATE) / T(sensors[i]).rate; 3357 latency[i - ACC] = U64_DIV_BY_U64_CONSTANT( 3358 T(sensors[i]).latency + WATERMARK_TIME_UNIT_NS/2, WATERMARK_TIME_UNIT_NS); 3359 DEBUG_PRINT_IF(DBG_WM_CALC, "cwm2: f %dHz, l %dus => T %d unit, L %d unit", 3360 (int) T(sensors[i]).rate/1024, 3361 (int) U64_DIV_BY_U64_CONSTANT(T(sensors[i]).latency, 1000), 3362 period[i-ACC], latency[i-ACC]); 3363 } 3364 } 3365 3366 3367 size_t watermark = calcFifoSize(period, latency, factor, MAG - ACC + 1) / 4; 3368 DEBUG_PRINT_IF(DBG_WM_CALC, "cwm2: wm = %d", watermark); 3369 watermark = watermark < WATERMARK_MIN ? WATERMARK_MIN : watermark; 3370 watermark = watermark > WATERMARK_MAX ? WATERMARK_MAX : watermark; 3371 3372 return watermark; 3373 } 3374 3375 INTERNAL_APP_INIT(BMI160_APP_ID, 1, startTask, endTask, handleEvent); 3376 3377 3378
