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