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 float x, y, z; 1902 int16_t raw_x, raw_y, raw_z; 1903 struct TripleAxisDataPoint *sample; 1904 uint32_t delta_time; 1905 uint64_t rtc_time; 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 if (rtc_time < mSensor->prev_rtc_time + kMinRTCTimeIncrementNs) { 1915 #if TIMESTAMP_DBG 1916 DEBUG_PRINT("%s prev rtc 0x%08x %08x, curr 0x%08x %08x, delta %d usec\n", 1917 mSensorInfo[mSensor->idx].sensorName, 1918 (unsigned int)((mSensor->prev_rtc_time >> 32) & 0xffffffff), 1919 (unsigned int)(mSensor->prev_rtc_time & 0xffffffff), 1920 (unsigned int)((rtc_time >> 32) & 0xffffffff), 1921 (unsigned int)(rtc_time & 0xffffffff), 1922 (int)(rtc_time - mSensor->prev_rtc_time) / 1000); 1923 #endif 1924 rtc_time = mSensor->prev_rtc_time + kMinRTCTimeIncrementNs; 1925 } 1926 1927 #ifdef MAG_SLAVE_PRESENT 1928 if (mSensor->idx == MAG) { 1929 parseMagData(&magTask, &buf[0], &x, &y, &z); 1930 BMM150_TO_ANDROID_COORDINATE(x, y, z); 1931 1932 float xi, yi, zi; 1933 magCalRemoveSoftiron(&mTask.moc, x, y, z, &xi, &yi, &zi); 1934 1935 newMagBias |= magCalUpdate(&mTask.moc, sensorTime * kSensorTimerIntervalUs, xi, yi, zi); 1936 1937 magCalRemoveBias(&mTask.moc, xi, yi, zi, &x, &y, &z); 1938 1939 #ifdef GYRO_CAL_ENABLED 1940 // Gyro Cal -- Add magnetometer sample. 1941 gyroCalUpdateMag(&mTask.gyro_cal, 1942 rtc_time, // nsec 1943 x, y, z); 1944 #endif 1945 } else 1946 #endif 1947 { 1948 raw_x = (buf[0] | buf[1] << 8); 1949 raw_y = (buf[2] | buf[3] << 8); 1950 raw_z = (buf[4] | buf[5] << 8); 1951 1952 x = (float)raw_x * kScale; 1953 y = (float)raw_y * kScale; 1954 z = (float)raw_z * kScale; 1955 1956 BMI160_TO_ANDROID_COORDINATE(x, y, z); 1957 1958 if (mSensor->idx == ACC) { 1959 1960 #ifdef ACCEL_CAL_ENABLED 1961 accelCalRun(&mTask.acc, rtc_time, 1962 x, y, z, mTask.tempCelsius); 1963 1964 accelCalBiasRemove(&mTask.acc, &x, &y, &z); 1965 #endif 1966 1967 #ifdef GYRO_CAL_ENABLED 1968 // Gyro Cal -- Add accelerometer sample. 1969 gyroCalUpdateAccel(&mTask.gyro_cal, 1970 rtc_time, // nsec 1971 x, y, z); 1972 #endif 1973 } else if (mSensor->idx == GYR) { 1974 1975 #ifdef GYRO_CAL_ENABLED 1976 // Gyro Cal -- Add gyroscope and temperature sample. 1977 gyroCalUpdateGyro(&mTask.gyro_cal, 1978 rtc_time, // nsec 1979 x, y, z, 1980 mTask.tempCelsius); 1981 1982 // Gyro Cal -- Apply calibration correction. 1983 gyroCalRemoveBias(&mTask.gyro_cal, 1984 x, y, z, /* input values */ 1985 &x, &y, &z /* calibrated output */); 1986 #endif 1987 1988 } 1989 } 1990 1991 if (mSensor->data_evt == NULL) { 1992 if (!allocateDataEvt(mSensor, rtc_time)) { 1993 return; 1994 } 1995 } 1996 1997 if (mSensor->data_evt->samples[0].firstSample.numSamples >= MAX_NUM_COMMS_EVENT_SAMPLES) { 1998 ERROR_PRINT("BAD INDEX\n"); 1999 return; 2000 } 2001 2002 #ifdef MAG_SLAVE_PRESENT 2003 if (mSensor->idx == MAG && (newMagBias || !mTask.magBiasPosted)) { 2004 if (mSensor->data_evt->samples[0].firstSample.numSamples > 0) { 2005 // flush existing samples so the bias appears after them 2006 flushData(mSensor, 2007 EVENT_TYPE_BIT_DISCARDABLE | sensorGetMyEventType(mSensorInfo[MAG].sensorType)); 2008 if (!allocateDataEvt(mSensor, rtc_time)) { 2009 return; 2010 } 2011 } 2012 if (newMagBias) { 2013 mTask.magBiasCurrent = true; 2014 } 2015 mSensor->data_evt->samples[0].firstSample.biasCurrent = mTask.magBiasCurrent; 2016 mSensor->data_evt->samples[0].firstSample.biasPresent = 1; 2017 mSensor->data_evt->samples[0].firstSample.biasSample = 2018 mSensor->data_evt->samples[0].firstSample.numSamples; 2019 sample = &mSensor->data_evt->samples[mSensor->data_evt->samples[0].firstSample.numSamples++]; 2020 magCalGetBias(&mTask.moc, &sample->x, &sample->y, &sample->z); 2021 // bias is non-discardable, if we fail to enqueue, don't clear new_mag_bias 2022 if (flushData(mSensor, sensorGetMyEventType(mSensorInfo[MAG].biasType))) { 2023 mTask.magBiasPosted = true; 2024 } 2025 2026 if (!allocateDataEvt(mSensor, rtc_time)) { 2027 return; 2028 } 2029 } 2030 #endif 2031 #ifdef GYRO_CAL_ENABLED 2032 // Gyro Cal -- Notify HAL about new gyro bias calibration 2033 if (mSensor->idx == GYR && gyroCalNewBiasAvailable(&mTask.gyro_cal)) { 2034 if (mSensor->data_evt->samples[0].firstSample.numSamples > 0) { 2035 // flush existing samples so the bias appears after them 2036 flushData(mSensor, 2037 EVENT_TYPE_BIT_DISCARDABLE | sensorGetMyEventType(mSensorInfo[GYR].sensorType)); 2038 if (!allocateDataEvt(mSensor, rtc_time)) { 2039 return; 2040 } 2041 } 2042 mSensor->data_evt->samples[0].firstSample.biasCurrent = true; 2043 mSensor->data_evt->samples[0].firstSample.biasPresent = 1; 2044 mSensor->data_evt->samples[0].firstSample.biasSample = 2045 mSensor->data_evt->samples[0].firstSample.numSamples; 2046 sample = &mSensor->data_evt->samples[mSensor->data_evt->samples[0].firstSample.numSamples++]; 2047 gyroCalGetBias(&mTask.gyro_cal, &sample->x, &sample->y, &sample->z); 2048 flushData(mSensor, sensorGetMyEventType(mSensorInfo[GYR].biasType)); 2049 2050 if (!allocateDataEvt(mSensor, rtc_time)) { 2051 return; 2052 } 2053 } 2054 #endif 2055 2056 sample = &mSensor->data_evt->samples[mSensor->data_evt->samples[0].firstSample.numSamples++]; 2057 2058 // the first deltatime is for sample size 2059 if (mSensor->data_evt->samples[0].firstSample.numSamples > 1) { 2060 delta_time = rtc_time - mSensor->prev_rtc_time; 2061 delta_time = delta_time < 0 ? 0 : delta_time; 2062 sample->deltaTime = delta_time; 2063 mSensor->prev_rtc_time = rtc_time; 2064 } 2065 2066 sample->x = x; 2067 sample->y = y; 2068 sample->z = z; 2069 2070 //DEBUG_PRINT("bmi160: x: %d, y: %d, z: %d\n", (int)(1000*x), (int)(1000*y), (int)(1000*z)); 2071 2072 //TODO: This was added to prevent to much data of the same type accumulate in internal buffer. 2073 // It might no longer be necessary and can be removed. 2074 if (mSensor->data_evt->samples[0].firstSample.numSamples == MAX_NUM_COMMS_EVENT_SAMPLES) { 2075 flushAllData(); 2076 } 2077 } 2078 2079 static void dispatchData(void) 2080 { 2081 size_t i = 1, j; 2082 size_t size = mTask.xferCnt; 2083 int fh_mode, fh_param; 2084 uint8_t *buf = mTask.dataBuffer; 2085 2086 uint64_t min_delta = ULONG_LONG_MAX; 2087 uint32_t sensor_time24; 2088 uint64_t full_sensor_time; 2089 uint64_t frame_sensor_time = mTask.frame_sensortime; 2090 bool observed[NUM_CONT_SENSOR]; 2091 uint64_t tmp_frame_time, tmp_time[NUM_CONT_SENSOR]; 2092 bool frame_sensor_time_valid = mTask.frame_sensortime_valid; 2093 bool saved_pending_delta[NUM_CONT_SENSOR]; 2094 uint64_t saved_time_delta[NUM_CONT_SENSOR]; 2095 #if TIMESTAMP_DBG 2096 int frame_num = -1; 2097 #endif 2098 2099 for (j = FIRST_CONT_SENSOR; j < NUM_CONT_SENSOR; j++) 2100 observed[j] = false; 2101 2102 if (!mTask.frame_sensortime_valid) { 2103 // This is the first FIFO delivery after any sensor is enabled in 2104 // bmi160. Sensor time reference is not establised until end of this 2105 // FIFO frame. Assume time start from zero and do a dry run to estimate 2106 // the time and then go through this FIFO again. 2107 frame_sensor_time = 0ull; 2108 2109 // Save these states for future recovery by the end of dry run. 2110 for (j = FIRST_CONT_SENSOR; j < NUM_CONT_SENSOR; j++) { 2111 saved_pending_delta[j] = mTask.pending_delta[j]; 2112 saved_time_delta[j] = mTask.time_delta[j]; 2113 } 2114 } 2115 2116 while (size > 0) { 2117 if (buf[i] == BMI160_FRAME_HEADER_INVALID) { 2118 // reaching invalid header means no more data 2119 break; 2120 } else if (buf[i] == BMI160_FRAME_HEADER_SKIP) { 2121 // manually injected skip header 2122 DEBUG_PRINT_IF(DBG_CHUNKED, "skip nop header"); 2123 i++; 2124 size--; 2125 continue; 2126 } 2127 2128 fh_mode = buf[i] >> 6; 2129 fh_param = (buf[i] >> 2) & 0xf; 2130 2131 i++; 2132 size--; 2133 #if TIMESTAMP_DBG 2134 ++frame_num; 2135 #endif 2136 2137 if (fh_mode == 1) { 2138 // control frame. 2139 if (fh_param == 0) { 2140 // skip frame, we skip it 2141 if (size >= 1) { 2142 i++; 2143 size--; 2144 } else { 2145 size = 0; 2146 } 2147 } else if (fh_param == 1) { 2148 // sensortime frame 2149 if (size >= 3) { 2150 // The active sensor with the highest odr/lowest delta is the one that 2151 // determines the sensor time increments. 2152 for (j = FIRST_CONT_SENSOR; j < NUM_CONT_SENSOR; j++) { 2153 if (mTask.sensors[j].configed && 2154 mTask.sensors[j].latency != SENSOR_LATENCY_NODATA) { 2155 min_delta = min_delta < mTask.time_delta[j] ? min_delta : 2156 mTask.time_delta[j]; 2157 } 2158 } 2159 sensor_time24 = buf[i + 2] << 16 | buf[i + 1] << 8 | buf[i]; 2160 2161 // clear lower bits that measure time from taking the sample to reading the 2162 // FIFO, something we're not interested in. 2163 sensor_time24 &= ~(min_delta - 1); 2164 2165 full_sensor_time = parseSensortime(sensor_time24); 2166 2167 #if TIMESTAMP_DBG 2168 if (frame_sensor_time == full_sensor_time) { 2169 //DEBUG_PRINT("frame %d FrameTime 0x%08x\n", 2170 // frame_num - 1, 2171 // (unsigned int)frame_sensor_time); 2172 } else if (frame_sensor_time_valid) { 2173 DEBUG_PRINT("frame %d FrameTime 0x%08x != SensorTime 0x%08x, jumped %d msec\n", 2174 frame_num - 1, 2175 (unsigned int)frame_sensor_time, 2176 (unsigned int)full_sensor_time, 2177 (int)(5 * ((int64_t)(full_sensor_time - frame_sensor_time) >> 7))); 2178 } 2179 #endif 2180 2181 2182 if (frame_sensor_time_valid) { 2183 mTask.frame_sensortime = full_sensor_time; 2184 } else { 2185 // Dry run if frame_sensortime_valid == false, 2186 // no sample is added this round. 2187 // So let's time travel back to beginning of frame. 2188 mTask.frame_sensortime_valid = true; 2189 mTask.frame_sensortime = full_sensor_time - frame_sensor_time; 2190 2191 // recover states 2192 for (j = FIRST_CONT_SENSOR; j < NUM_CONT_SENSOR; j++) { 2193 // reset all prev_frame_time to invalid values 2194 // they should be so anyway at the first FIFO 2195 mTask.prev_frame_time[j] = ULONG_LONG_MAX; 2196 2197 // recover saved time_delta and pending_delta values 2198 mTask.pending_delta[j] = saved_pending_delta[j]; 2199 mTask.time_delta[j] = saved_time_delta[j]; 2200 } 2201 2202 DEBUG_PRINT_IF(TIMESTAMP_DBG, 2203 "sensortime invalid: full, frame, task = %llu, %llu, %llu\n", 2204 full_sensor_time, 2205 frame_sensor_time, 2206 mTask.frame_sensortime); 2207 2208 // Parse again with known valid timing. 2209 // This time the sensor events will be committed into event buffer. 2210 return dispatchData(); 2211 } 2212 2213 // Invalidate sensor timestamp that didn't get corrected by full_sensor_time, 2214 // so it can't be used as a reference at next FIFO read. 2215 // Use (ULONG_LONG_MAX - 1) to indicate this. 2216 for (j = FIRST_CONT_SENSOR; j < NUM_CONT_SENSOR; j++) { 2217 mTask.prev_frame_time[j] = observed[j] ? full_sensor_time : (ULONG_LONG_MAX - 1); 2218 2219 // sensor can be disabled in the middle of the FIFO, but wait till the FIFO 2220 // end to invalidate prev_frame_time since it's still needed for parsing. 2221 // Also invalidate pending delta just to be safe. 2222 if (!mTask.sensors[j].configed || 2223 mTask.sensors[j].latency == SENSOR_LATENCY_NODATA) { 2224 mTask.prev_frame_time[j] = ULONG_LONG_MAX; 2225 mTask.pending_delta[j] = false; 2226 } 2227 } 2228 i += 3; 2229 size -= 3; 2230 } else { 2231 size = 0; 2232 } 2233 } else if (fh_param == 2) { 2234 // fifo_input config frame 2235 #if TIMESTAMP_DBG 2236 DEBUG_PRINT("frame %d config change 0x%02x\n", frame_num, buf[i]); 2237 #endif 2238 if (size >= 1) { 2239 for (j = FIRST_CONT_SENSOR; j < NUM_CONT_SENSOR; j++) { 2240 if (buf[i] & (0x01 << (j << 1)) && mTask.pending_delta[j]) { 2241 mTask.pending_delta[j] = false; 2242 mTask.time_delta[j] = mTask.next_delta[j]; 2243 #if TIMESTAMP_DBG 2244 DEBUG_PRINT("%s new delta %u\n", mSensorInfo[j].sensorName, 2245 (unsigned int)mTask.time_delta[j]); 2246 #endif 2247 } 2248 } 2249 i++; 2250 size--; 2251 } else { 2252 size = 0; 2253 } 2254 } else { 2255 size = 0; // drop this batch 2256 ERROR_PRINT("Invalid fh_param in control frame\n"); 2257 } 2258 } else if (fh_mode == 2) { 2259 // Calcutate candidate frame time (tmp_frame_time): 2260 // 1) When sensor is first enabled, reference from other sensors if possible. 2261 // Otherwise, add the smallest increment to the previous data frame time. 2262 // 2) The newly enabled sensor could only underestimate its 2263 // frame time without reference from other sensors. 2264 // 3) The underestimated frame time of a newly enabled sensor will be corrected 2265 // as soon as it shows up in the same frame with another sensor. 2266 // 4) (prev_frame_time == ULONG_LONG_MAX) means the sensor wasn't enabled. 2267 // 5) (prev_frame_time == ULONG_LONG_MAX -1) means the sensor didn't appear in the last 2268 // data frame of the previous fifo read. So it won't be used as a frame time reference. 2269 2270 tmp_frame_time = 0; 2271 for (j = FIRST_CONT_SENSOR; j < NUM_CONT_SENSOR; j++) { 2272 observed[j] = false; // reset at each data frame 2273 tmp_time[j] = 0; 2274 if ((mTask.prev_frame_time[j] < ULONG_LONG_MAX - 1) && (fh_param & (1 << j))) { 2275 tmp_time[j] = mTask.prev_frame_time[j] + mTask.time_delta[j]; 2276 tmp_frame_time = (tmp_time[j] > tmp_frame_time) ? tmp_time[j] : tmp_frame_time; 2277 } 2278 } 2279 tmp_frame_time = (frame_sensor_time + kMinSensorTimeIncrement > tmp_frame_time) 2280 ? (frame_sensor_time + kMinSensorTimeIncrement) : tmp_frame_time; 2281 2282 // regular frame, dispatch data to each sensor's own fifo 2283 #ifdef MAG_SLAVE_PRESENT 2284 if (fh_param & 4) { // have mag data 2285 if (size >= 8) { 2286 if (frame_sensor_time_valid) { 2287 // scale not used 2288 parseRawData(&mTask.sensors[MAG], &buf[i], 0, tmp_frame_time); 2289 #if TIMESTAMP_DBG 2290 if (mTask.prev_frame_time[MAG] == ULONG_LONG_MAX) { 2291 DEBUG_PRINT("mag enabled: frame %d time 0x%08x\n", 2292 frame_num, (unsigned int)tmp_frame_time); 2293 } else if ((tmp_frame_time != tmp_time[MAG]) && (tmp_time[MAG] != 0)) { 2294 DEBUG_PRINT("frame %d mag time: 0x%08x -> 0x%08x, jumped %d msec\n", 2295 frame_num, 2296 (unsigned int)tmp_time[MAG], 2297 (unsigned int)tmp_frame_time, 2298 (int)(5 * ((int64_t)(tmp_frame_time - tmp_time[MAG]) >> 7))); 2299 } 2300 #endif 2301 } 2302 mTask.prev_frame_time[MAG] = tmp_frame_time; 2303 i += 8; 2304 size -= 8; 2305 observed[MAG] = true; 2306 } else { 2307 size = 0; 2308 } 2309 } 2310 #endif 2311 if (fh_param & 2) { // have gyro data 2312 if (size >= 6) { 2313 if (frame_sensor_time_valid) { 2314 parseRawData(&mTask.sensors[GYR], &buf[i], kScale_gyr, tmp_frame_time); 2315 #if TIMESTAMP_DBG 2316 if (mTask.prev_frame_time[GYR] == ULONG_LONG_MAX) { 2317 DEBUG_PRINT("gyr enabled: frame %d time 0x%08x\n", 2318 frame_num, (unsigned int)tmp_frame_time); 2319 } else if ((tmp_frame_time != tmp_time[GYR]) && (tmp_time[GYR] != 0)) { 2320 DEBUG_PRINT("frame %d gyr time: 0x%08x -> 0x%08x, jumped %d msec\n", 2321 frame_num, 2322 (unsigned int)tmp_time[GYR], 2323 (unsigned int)tmp_frame_time, 2324 (int)(5 * ((int64_t)(tmp_frame_time - tmp_time[GYR]) >> 7))); 2325 } 2326 #endif 2327 } 2328 mTask.prev_frame_time[GYR] = tmp_frame_time; 2329 i += 6; 2330 size -= 6; 2331 observed[GYR] = true; 2332 } else { 2333 size = 0; 2334 } 2335 } 2336 if (fh_param & 1) { // have accel data 2337 if (size >= 6) { 2338 if (frame_sensor_time_valid) { 2339 parseRawData(&mTask.sensors[ACC], &buf[i], kScale_acc, tmp_frame_time); 2340 #if TIMESTAMP_DBG 2341 if (mTask.prev_frame_time[ACC] == ULONG_LONG_MAX) { 2342 DEBUG_PRINT("acc enabled: frame %d time 0x%08x\n", 2343 frame_num, (unsigned int)tmp_frame_time); 2344 } else if ((tmp_frame_time != tmp_time[ACC]) && (tmp_time[ACC] != 0)) { 2345 DEBUG_PRINT("frame %d gyr time: 0x%08x -> 0x%08x, jumped %d msec\n", 2346 frame_num, 2347 (unsigned int)tmp_time[ACC], 2348 (unsigned int)tmp_frame_time, 2349 (int)(5 * ((int64_t)(tmp_frame_time - tmp_time[ACC]) >> 7))); 2350 } 2351 #endif 2352 } 2353 mTask.prev_frame_time[ACC] = tmp_frame_time; 2354 i += 6; 2355 size -= 6; 2356 observed[ACC] = true; 2357 } else { 2358 size = 0; 2359 } 2360 } 2361 2362 if (observed[ACC] || observed[GYR]) 2363 frame_sensor_time = tmp_frame_time; 2364 #ifdef MAG_SLAVE_PRESENT 2365 else if (observed[MAG]) 2366 frame_sensor_time = tmp_frame_time; 2367 #endif 2368 } else { 2369 size = 0; // drop this batch 2370 ERROR_PRINT("Invalid fh_mode %d at 0x%x, data dump:\n", fh_mode, i); 2371 // dump (a) bytes back and (b) bytes forward. 2372 int a = i < 0x80 ? 0 : (i - 0x80) & ~0x0F; 2373 int b = ((i + 0x80 > mTask.xferCnt ? mTask.xferCnt : i + 0x80) + 0x0F) & ~0x0F; 2374 dumpBinary(mTask.dataBuffer, a, b - a); 2375 } 2376 } 2377 2378 //flush data events. 2379 flushAllData(); 2380 } 2381 2382 /* 2383 * Read the interrupt type and send corresponding event 2384 * If it's anymo or double tap, also send a single uint32 to indicate which axies 2385 * is this interrupt triggered. 2386 * If it's flat, also send a bit to indicate flat/non-flat position. 2387 * If it's step detector, check if we need to send the total step count. 2388 */ 2389 static void int2Handling(void) 2390 { 2391 TDECL(); 2392 union EmbeddedDataPoint trigger_axies; 2393 uint8_t int_status_0 = mTask.statusBuffer[1]; 2394 uint8_t int_status_1 = mTask.statusBuffer[2]; 2395 if (int_status_0 & INT_STEP) { 2396 if (mTask.sensors[STEP].powered) { 2397 DEBUG_PRINT("Detected step\n"); 2398 osEnqueueEvt(EVT_SENSOR_STEP, NULL, NULL); 2399 } 2400 if (mTask.sensors[STEPCNT].powered) { 2401 T(pending_step_cnt) = true; 2402 } 2403 } 2404 if ((int_status_0 & INT_ANY_MOTION) && mTask.sensors[ANYMO].powered) { 2405 // bit [0:2] of INT_STATUS[2] is set when anymo is triggered by x, y or 2406 // z axies respectively. bit [3] indicates the slope. 2407 trigger_axies.idata = (mTask.statusBuffer[3] & 0x0f); 2408 DEBUG_PRINT("Detected any motion\n"); 2409 osEnqueueEvt(EVT_SENSOR_ANY_MOTION, trigger_axies.vptr, NULL); 2410 } 2411 if ((int_status_0 & INT_DOUBLE_TAP) && mTask.sensors[DTAP].powered) { 2412 // bit [4:6] of INT_STATUS[2] is set when double tap is triggered by 2413 // x, y or z axies respectively. bit [7] indicates the slope. 2414 trigger_axies.idata = ((mTask.statusBuffer[3] & 0xf0) >> 4); 2415 DEBUG_PRINT("Detected double tap\n"); 2416 osEnqueueEvt(EVT_SENSOR_DOUBLE_TAP, trigger_axies.vptr, NULL); 2417 } 2418 if ((int_status_0 & INT_FLAT) && mTask.sensors[FLAT].powered) { 2419 // bit [7] of INT_STATUS[3] indicates flat/non-flat position 2420 trigger_axies.idata = ((mTask.statusBuffer[4] & 0x80) >> 7); 2421 DEBUG_PRINT("Detected flat\n"); 2422 osEnqueueEvt(EVT_SENSOR_FLAT, trigger_axies.vptr, NULL); 2423 } 2424 if ((int_status_1 & INT_NO_MOTION) && mTask.sensors[NOMO].powered) { 2425 DEBUG_PRINT("Detected no motion\n"); 2426 osEnqueueEvt(EVT_SENSOR_NO_MOTION, NULL, NULL); 2427 } 2428 return; 2429 } 2430 2431 static void int2Evt(void) 2432 { 2433 TDECL(); 2434 if (trySwitchState(SENSOR_INT_2_HANDLING)) { 2435 // Read the interrupt reg value to determine what interrupts 2436 SPI_READ(BMI160_REG_INT_STATUS_0, 4, &mTask.statusBuffer); 2437 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask, __FUNCTION__); 2438 } else { 2439 // even if we are still in SENSOR_INT_2_HANDLING, the SPI may already finished and we need 2440 // to issue another SPI read to get the latest status 2441 mTask.pending_int[1] = true; 2442 } 2443 } 2444 2445 // bits[6:7] in OFFSET[6] to enable/disable gyro/accel offset. 2446 // bits[0:5] in OFFSET[6] stores the most significant 2 bits of gyro offset at 2447 // its x, y, z axies. 2448 // Calculate the stored gyro offset and compose it with the intended 2449 // enable/disable mode for gyro/accel offset to determine the value for 2450 // OFFSET[6]. 2451 static uint8_t offset6Mode(void) 2452 { 2453 uint8_t mode = 0; 2454 if (mTask.sensors[GYR].offset_enable) 2455 mode |= 0x01 << 7; 2456 if (mTask.sensors[ACC].offset_enable) 2457 mode |= 0x01 << 6; 2458 mode |= (mTask.sensors[GYR].offset[2] & 0x0300) >> 4; 2459 mode |= (mTask.sensors[GYR].offset[1] & 0x0300) >> 6; 2460 mode |= (mTask.sensors[GYR].offset[0] & 0x0300) >> 8; 2461 DEBUG_PRINT("OFFSET_6_MODE is: %02x\n", mode); 2462 return mode; 2463 } 2464 2465 static bool saveCalibration() 2466 { 2467 TDECL(); 2468 if (trySwitchState(SENSOR_SAVE_CALIBRATION)) { 2469 if (mTask.sensors[ACC].offset_enable) { 2470 SPI_WRITE(BMI160_REG_OFFSET_0, mTask.sensors[ACC].offset[0] & 0xFF, 450); 2471 SPI_WRITE(BMI160_REG_OFFSET_0 + 1, mTask.sensors[ACC].offset[1] & 0xFF, 450); 2472 SPI_WRITE(BMI160_REG_OFFSET_0 + 2, mTask.sensors[ACC].offset[2] & 0xFF, 450); 2473 } 2474 if (mTask.sensors[GYR].offset_enable) { 2475 SPI_WRITE(BMI160_REG_OFFSET_3, mTask.sensors[GYR].offset[0] & 0xFF, 450); 2476 SPI_WRITE(BMI160_REG_OFFSET_3 + 1, mTask.sensors[GYR].offset[1] & 0xFF, 450); 2477 SPI_WRITE(BMI160_REG_OFFSET_3 + 2, mTask.sensors[GYR].offset[2] & 0xFF, 450); 2478 } 2479 SPI_WRITE(BMI160_REG_OFFSET_6, offset6Mode(), 450); 2480 SPI_READ(BMI160_REG_OFFSET_0, 7, &mTask.dataBuffer); 2481 spiBatchTxRx(&mTask.mode, sensorSpiCallback, NULL, __FUNCTION__); 2482 return true; 2483 } else { 2484 DEBUG_PRINT("%s, state != IDLE", __FUNCTION__); 2485 return false; 2486 } 2487 } 2488 2489 static void sendCalibrationResult(uint8_t status, uint8_t sensorType, 2490 int32_t xBias, int32_t yBias, int32_t zBias) { 2491 struct CalibrationData *data = heapAlloc(sizeof(struct CalibrationData)); 2492 if (!data) { 2493 osLog(LOG_WARN, "Couldn't alloc cal result pkt"); 2494 return; 2495 } 2496 2497 data->header.appId = BMI160_APP_ID; 2498 data->header.dataLen = (sizeof(struct CalibrationData) - sizeof(struct HostHubRawPacket)); 2499 data->data_header.msgId = SENSOR_APP_MSG_ID_CAL_RESULT; 2500 data->data_header.sensorType = sensorType; 2501 data->data_header.status = status; 2502 2503 data->xBias = xBias; 2504 data->yBias = yBias; 2505 data->zBias = zBias; 2506 2507 if (!osEnqueueEvtOrFree(EVT_APP_TO_HOST, data, heapFree)) 2508 osLog(LOG_WARN, "Couldn't send cal result evt"); 2509 } 2510 2511 static void accCalibrationHandling(void) 2512 { 2513 TDECL(); 2514 switch (mTask.calibration_state) { 2515 case CALIBRATION_START: 2516 T(mRetryLeft) = RETRY_CNT_CALIBRATION; 2517 2518 // turn ACC to NORMAL mode 2519 SPI_WRITE(BMI160_REG_CMD, 0x11, 50000); 2520 2521 mTask.calibration_state = CALIBRATION_FOC; 2522 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC], __FUNCTION__); 2523 break; 2524 case CALIBRATION_FOC: 2525 2526 // set accel range to +-8g 2527 SPI_WRITE(BMI160_REG_ACC_RANGE, 0x08); 2528 2529 // enable accel fast offset compensation, 2530 // x: 0g, y: 0g, z: 1g 2531 SPI_WRITE(BMI160_REG_FOC_CONF, ACC_FOC_CONFIG); 2532 2533 // start calibration 2534 SPI_WRITE(BMI160_REG_CMD, 0x03, 100000); 2535 2536 // poll the status reg until the calibration finishes. 2537 SPI_READ(BMI160_REG_STATUS, 1, &mTask.statusBuffer, 50000); 2538 2539 mTask.calibration_state = CALIBRATION_WAIT_FOC_DONE; 2540 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC], __FUNCTION__); 2541 break; 2542 case CALIBRATION_WAIT_FOC_DONE: 2543 // if the STATUS REG has bit 3 set, it means calbration is done. 2544 // otherwise, check back in 50ms later. 2545 if (mTask.statusBuffer[1] & 0x08) { 2546 2547 //disable FOC 2548 SPI_WRITE(BMI160_REG_FOC_CONF, 0x00); 2549 2550 //read the offset value for accel 2551 SPI_READ(BMI160_REG_OFFSET_0, 3, &mTask.dataBuffer); 2552 mTask.calibration_state = CALIBRATION_SET_OFFSET; 2553 DEBUG_PRINT("FOC set FINISHED!\n"); 2554 } else { 2555 2556 // calibration hasn't finished yet, go back to wait for 50ms. 2557 SPI_READ(BMI160_REG_STATUS, 1, &mTask.statusBuffer, 50000); 2558 mTask.calibration_state = CALIBRATION_WAIT_FOC_DONE; 2559 T(mRetryLeft)--; 2560 } 2561 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC], __FUNCTION__); 2562 2563 // if calbration hasn't finished after 10 polling on the STATUS reg, 2564 // declare timeout. 2565 if (T(mRetryLeft) == 0) { 2566 mTask.calibration_state = CALIBRATION_TIMEOUT; 2567 } 2568 break; 2569 case CALIBRATION_SET_OFFSET: 2570 mTask.sensors[ACC].offset[0] = mTask.dataBuffer[1]; 2571 mTask.sensors[ACC].offset[1] = mTask.dataBuffer[2]; 2572 mTask.sensors[ACC].offset[2] = mTask.dataBuffer[3]; 2573 // sign extend values 2574 if (mTask.sensors[ACC].offset[0] & 0x80) 2575 mTask.sensors[ACC].offset[0] |= 0xFFFFFF00; 2576 if (mTask.sensors[ACC].offset[1] & 0x80) 2577 mTask.sensors[ACC].offset[1] |= 0xFFFFFF00; 2578 if (mTask.sensors[ACC].offset[2] & 0x80) 2579 mTask.sensors[ACC].offset[2] |= 0xFFFFFF00; 2580 2581 mTask.sensors[ACC].offset_enable = true; 2582 DEBUG_PRINT("ACCELERATION OFFSET is %02x %02x %02x\n", 2583 (unsigned int)mTask.sensors[ACC].offset[0], 2584 (unsigned int)mTask.sensors[ACC].offset[1], 2585 (unsigned int)mTask.sensors[ACC].offset[2]); 2586 2587 sendCalibrationResult(SENSOR_APP_EVT_STATUS_SUCCESS, SENS_TYPE_ACCEL, 2588 mTask.sensors[ACC].offset[0], mTask.sensors[ACC].offset[1], 2589 mTask.sensors[ACC].offset[2]); 2590 2591 // Enable offset compensation for accel 2592 uint8_t mode = offset6Mode(); 2593 SPI_WRITE(BMI160_REG_OFFSET_6, mode); 2594 2595 // turn ACC to SUSPEND mode 2596 SPI_WRITE(BMI160_REG_CMD, 0x10, 5000); 2597 2598 mTask.calibration_state = CALIBRATION_DONE; 2599 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC], __FUNCTION__); 2600 break; 2601 default: 2602 ERROR_PRINT("Invalid calibration state\n"); 2603 break; 2604 } 2605 } 2606 2607 static bool accCalibration(void *cookie) 2608 { 2609 TDECL(); 2610 if (!mTask.sensors[ACC].powered && trySwitchState(SENSOR_CALIBRATING)) { 2611 mTask.calibration_state = CALIBRATION_START; 2612 accCalibrationHandling(); 2613 return true; 2614 } else { 2615 ERROR_PRINT("cannot calibrate accel because sensor is busy\n"); 2616 sendCalibrationResult(SENSOR_APP_EVT_STATUS_BUSY, SENS_TYPE_ACCEL, 0, 0, 0); 2617 return false; 2618 } 2619 } 2620 2621 static bool accCfgData(void *data, void *cookie) 2622 { 2623 struct CfgData { 2624 int32_t hw[3]; 2625 float sw[3]; 2626 }; 2627 struct CfgData *values = data; 2628 2629 mTask.sensors[ACC].offset[0] = values->hw[0]; 2630 mTask.sensors[ACC].offset[1] = values->hw[1]; 2631 mTask.sensors[ACC].offset[2] = values->hw[2]; 2632 mTask.sensors[ACC].offset_enable = true; 2633 2634 #ifdef ACCEL_CAL_ENABLED 2635 accelCalBiasSet(&mTask.acc, values->sw[0], values->sw[1], values->sw[2]); 2636 #endif 2637 2638 INFO_PRINT("accCfgData: data=%02lx, %02lx, %02lx\n", 2639 values->hw[0] & 0xFF, values->hw[1] & 0xFF, values->hw[2] & 0xFF); 2640 2641 if (!saveCalibration()) { 2642 mTask.pending_calibration_save = true; 2643 } 2644 2645 return true; 2646 } 2647 2648 static void sendTestResult(uint8_t status, uint8_t sensorType) { 2649 struct TestResultData *data = heapAlloc(sizeof(struct TestResultData)); 2650 if (!data) { 2651 osLog(LOG_WARN, "Couldn't alloc test result packet"); 2652 return; 2653 } 2654 2655 data->header.appId = BMI160_APP_ID; 2656 data->header.dataLen = (sizeof(struct TestResultData) - sizeof(struct HostHubRawPacket)); 2657 data->data_header.msgId = SENSOR_APP_MSG_ID_TEST_RESULT; 2658 data->data_header.sensorType = sensorType; 2659 data->data_header.status = status; 2660 2661 if (!osEnqueueEvtOrFree(EVT_APP_TO_HOST, data, heapFree)) 2662 osLog(LOG_WARN, "Couldn't send test result packet"); 2663 } 2664 2665 static void accTestHandling(void) 2666 { 2667 // the minimum absolute differences, according to BMI160 datasheet section 2668 // 2.8.1, are 800 mg for the x and y axes and 400 mg for the z axis 2669 static const int32_t kMinDifferenceXY = (800 * 32767) / 8000; 2670 static const int32_t kMinDifferenceZ = (400 * 32767) / 8000; 2671 2672 int32_t tempTestX, tempTestY, tempTestZ; 2673 int32_t absDiffX, absDiffY, absDiffZ; 2674 2675 TDECL(); 2676 2677 switch (mTask.acc_test_state) { 2678 case ACC_TEST_START: 2679 // turn ACC to NORMAL mode 2680 SPI_WRITE(BMI160_REG_CMD, 0x11, 50000); 2681 2682 mTask.acc_test_state = ACC_TEST_CONFIG; 2683 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC], __FUNCTION__); 2684 break; 2685 2686 case ACC_TEST_CONFIG: 2687 // set accel conf 2688 SPI_WRITE(BMI160_REG_ACC_CONF, 0x2c); 2689 2690 // set accel range to +-8g 2691 SPI_WRITE(BMI160_REG_ACC_RANGE, 0x08); 2692 2693 // read stale accel data 2694 SPI_READ(BMI160_REG_DATA_14, 6, &mTask.dataBuffer); 2695 2696 mTask.acc_test_state = ACC_TEST_RUN_0; 2697 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC], __FUNCTION__); 2698 break; 2699 2700 case ACC_TEST_RUN_0: 2701 // configure acc_self_test_amp=1, acc_self_test_sign=0, acc_self_test_enable=b01 2702 // wait 50ms for data to be available 2703 SPI_WRITE(BMI160_REG_SELF_TEST, 0x09, 50000); 2704 2705 // read accel data 2706 SPI_READ(BMI160_REG_DATA_14, 6, &mTask.dataBuffer); 2707 2708 mTask.acc_test_state = ACC_TEST_RUN_1; 2709 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC], __FUNCTION__); 2710 break; 2711 2712 case ACC_TEST_RUN_1: 2713 // save accel data 2714 mTask.accTestX = *(int16_t*)(mTask.dataBuffer+1); 2715 mTask.accTestY = *(int16_t*)(mTask.dataBuffer+3); 2716 mTask.accTestZ = *(int16_t*)(mTask.dataBuffer+5); 2717 2718 // configure acc_self_test_amp=1, acc_self_test_sign=1, acc_self_test_enable=b01 2719 // wait 50ms for data to be available 2720 SPI_WRITE(BMI160_REG_SELF_TEST, 0x0d, 50000); 2721 2722 // read accel data 2723 SPI_READ(BMI160_REG_DATA_14, 6, &mTask.dataBuffer); 2724 2725 mTask.acc_test_state = ACC_TEST_VERIFY; 2726 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC], __FUNCTION__); 2727 break; 2728 2729 case ACC_TEST_VERIFY: 2730 // save accel data 2731 tempTestX = *(int16_t*)(mTask.dataBuffer+1); 2732 tempTestY = *(int16_t*)(mTask.dataBuffer+3); 2733 tempTestZ = *(int16_t*)(mTask.dataBuffer+5); 2734 2735 // calculate the differences between run 0 and run 1 2736 absDiffX = ABS((int32_t)mTask.accTestX - tempTestX); 2737 absDiffY = ABS((int32_t)mTask.accTestY - tempTestY); 2738 absDiffZ = ABS((int32_t)mTask.accTestZ - tempTestZ); 2739 2740 DEBUG_PRINT("accSelfTest diffs: X %d, Y %d, Z %d\n", (int)absDiffX, (int)absDiffY, (int)absDiffZ); 2741 2742 // verify that the differences between run 0 and run 1 are within spec 2743 if (absDiffX >= kMinDifferenceXY && absDiffY >= kMinDifferenceXY && absDiffZ >= kMinDifferenceZ) { 2744 sendTestResult(SENSOR_APP_EVT_STATUS_SUCCESS, SENS_TYPE_ACCEL); 2745 } else { 2746 sendTestResult(SENSOR_APP_EVT_STATUS_ERROR, SENS_TYPE_ACCEL); 2747 } 2748 2749 // turn ACC to SUSPEND mode 2750 SPI_WRITE(BMI160_REG_CMD, 0x10, 5000); 2751 2752 mTask.acc_test_state = ACC_TEST_DONE; 2753 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC], __FUNCTION__); 2754 break; 2755 2756 default: 2757 ERROR_PRINT("Invalid accel test state\n"); 2758 break; 2759 } 2760 } 2761 2762 static bool accSelfTest(void *cookie) 2763 { 2764 TDECL(); 2765 INFO_PRINT("accSelfTest\n"); 2766 2767 if (!mTask.sensors[ACC].powered && trySwitchState(SENSOR_TESTING)) { 2768 mTask.acc_test_state = ACC_TEST_START; 2769 accTestHandling(); 2770 return true; 2771 } else { 2772 ERROR_PRINT("cannot test accel because sensor is busy\n"); 2773 sendTestResult(SENSOR_APP_EVT_STATUS_BUSY, SENS_TYPE_ACCEL); 2774 return false; 2775 } 2776 } 2777 2778 static void gyrCalibrationHandling(void) 2779 { 2780 TDECL(); 2781 switch (mTask.calibration_state) { 2782 case CALIBRATION_START: 2783 T(mRetryLeft) = RETRY_CNT_CALIBRATION; 2784 2785 // turn GYR to NORMAL mode 2786 SPI_WRITE(BMI160_REG_CMD, 0x15, 50000); 2787 2788 mTask.calibration_state = CALIBRATION_FOC; 2789 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[GYR], __FUNCTION__); 2790 break; 2791 case CALIBRATION_FOC: 2792 2793 // set gyro range to +-1000 deg/sec 2794 SPI_WRITE(BMI160_REG_GYR_RANGE, 0x01); 2795 2796 // enable gyro fast offset compensation 2797 SPI_WRITE(BMI160_REG_FOC_CONF, 0x40); 2798 2799 // start FOC 2800 SPI_WRITE(BMI160_REG_CMD, 0x03, 100000); 2801 2802 // poll the status reg until the calibration finishes. 2803 SPI_READ(BMI160_REG_STATUS, 1, &mTask.statusBuffer, 50000); 2804 2805 mTask.calibration_state = CALIBRATION_WAIT_FOC_DONE; 2806 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[GYR], __FUNCTION__); 2807 break; 2808 case CALIBRATION_WAIT_FOC_DONE: 2809 2810 // if the STATUS REG has bit 3 set, it means calbration is done. 2811 // otherwise, check back in 50ms later. 2812 if (mTask.statusBuffer[1] & 0x08) { 2813 2814 // disable gyro fast offset compensation 2815 SPI_WRITE(BMI160_REG_FOC_CONF, 0x00); 2816 2817 //read the offset value for gyro 2818 SPI_READ(BMI160_REG_OFFSET_3, 4, &mTask.dataBuffer); 2819 mTask.calibration_state = CALIBRATION_SET_OFFSET; 2820 DEBUG_PRINT("FOC set FINISHED!\n"); 2821 } else { 2822 2823 // calibration hasn't finished yet, go back to wait for 50ms. 2824 SPI_READ(BMI160_REG_STATUS, 1, &mTask.statusBuffer, 50000); 2825 mTask.calibration_state = CALIBRATION_WAIT_FOC_DONE; 2826 T(mRetryLeft)--; 2827 } 2828 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[GYR], __FUNCTION__); 2829 2830 // if calbration hasn't finished after 10 polling on the STATUS reg, 2831 // declare timeout. 2832 if (T(mRetryLeft) == 0) { 2833 mTask.calibration_state = CALIBRATION_TIMEOUT; 2834 } 2835 break; 2836 case CALIBRATION_SET_OFFSET: 2837 mTask.sensors[GYR].offset[0] = ((mTask.dataBuffer[4] & 0x03) << 8) | mTask.dataBuffer[1]; 2838 mTask.sensors[GYR].offset[1] = ((mTask.dataBuffer[4] & 0x0C) << 6) | mTask.dataBuffer[2]; 2839 mTask.sensors[GYR].offset[2] = ((mTask.dataBuffer[4] & 0x30) << 4) | mTask.dataBuffer[3]; 2840 // sign extend values 2841 if (mTask.sensors[GYR].offset[0] & 0x200) 2842 mTask.sensors[GYR].offset[0] |= 0xFFFFFC00; 2843 if (mTask.sensors[GYR].offset[1] & 0x200) 2844 mTask.sensors[GYR].offset[1] |= 0xFFFFFC00; 2845 if (mTask.sensors[GYR].offset[2] & 0x200) 2846 mTask.sensors[GYR].offset[2] |= 0xFFFFFC00; 2847 2848 mTask.sensors[GYR].offset_enable = true; 2849 DEBUG_PRINT("GYRO OFFSET is %02x %02x %02x\n", 2850 (unsigned int)mTask.sensors[GYR].offset[0], 2851 (unsigned int)mTask.sensors[GYR].offset[1], 2852 (unsigned int)mTask.sensors[GYR].offset[2]); 2853 2854 sendCalibrationResult(SENSOR_APP_EVT_STATUS_SUCCESS, SENS_TYPE_GYRO, 2855 mTask.sensors[GYR].offset[0], mTask.sensors[GYR].offset[1], 2856 mTask.sensors[GYR].offset[2]); 2857 2858 // Enable offset compensation for gyro 2859 uint8_t mode = offset6Mode(); 2860 SPI_WRITE(BMI160_REG_OFFSET_6, mode); 2861 2862 // turn GYR to SUSPEND mode 2863 SPI_WRITE(BMI160_REG_CMD, 0x14, 1000); 2864 2865 mTask.calibration_state = CALIBRATION_DONE; 2866 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[GYR], __FUNCTION__); 2867 break; 2868 default: 2869 ERROR_PRINT("Invalid calibration state\n"); 2870 break; 2871 } 2872 } 2873 2874 static bool gyrCalibration(void *cookie) 2875 { 2876 TDECL(); 2877 if (!mTask.sensors[GYR].powered && trySwitchState(SENSOR_CALIBRATING)) { 2878 mTask.calibration_state = CALIBRATION_START; 2879 gyrCalibrationHandling(); 2880 return true; 2881 } else { 2882 ERROR_PRINT("cannot calibrate gyro because sensor is busy\n"); 2883 sendCalibrationResult(SENSOR_APP_EVT_STATUS_BUSY, SENS_TYPE_GYRO, 0, 0, 0); 2884 return false; 2885 } 2886 } 2887 2888 static bool gyrCfgData(void *data, void *cookie) 2889 { 2890 struct CfgData { 2891 int32_t hw[3]; 2892 float sw[3]; 2893 }; 2894 struct CfgData *values = data; 2895 2896 mTask.sensors[GYR].offset[0] = values->hw[0]; 2897 mTask.sensors[GYR].offset[1] = values->hw[1]; 2898 mTask.sensors[GYR].offset[2] = values->hw[2]; 2899 mTask.sensors[GYR].offset_enable = true; 2900 2901 #ifdef GYRO_CAL_ENABLED 2902 gyroCalSetBias(&mTask.gyro_cal, values->sw[0], values->sw[1], values->sw[2], sensorGetTime()); 2903 #endif 2904 2905 INFO_PRINT("gyrCfgData: data=%02lx, %02lx, %02lx\n", 2906 values->hw[0] & 0xFF, values->hw[1] & 0xFF, values->hw[2] & 0xFF); 2907 2908 if (!saveCalibration()) { 2909 mTask.pending_calibration_save = true; 2910 } 2911 2912 return true; 2913 } 2914 2915 static void gyroTestHandling(void) 2916 { 2917 TDECL(); 2918 2919 switch (mTask.gyro_test_state) { 2920 case GYRO_TEST_START: 2921 // turn GYR to NORMAL mode 2922 SPI_WRITE(BMI160_REG_CMD, 0x15, 50000); 2923 2924 mTask.gyro_test_state = GYRO_TEST_RUN; 2925 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[GYR], __FUNCTION__); 2926 break; 2927 2928 case GYRO_TEST_RUN: 2929 // set gyr_self_test_enable 2930 // wait 50ms to check test status 2931 SPI_WRITE(BMI160_REG_SELF_TEST, 0x10, 50000); 2932 2933 // check gyro self-test result in status register 2934 SPI_READ(BMI160_REG_STATUS, 1, &mTask.statusBuffer); 2935 2936 mTask.gyro_test_state = GYRO_TEST_VERIFY; 2937 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[GYR], __FUNCTION__); 2938 break; 2939 2940 case GYRO_TEST_VERIFY: 2941 // gyr_self_test_ok is bit 1 2942 if (mTask.statusBuffer[1] & 0x2) { 2943 sendTestResult(SENSOR_APP_EVT_STATUS_SUCCESS, SENS_TYPE_GYRO); 2944 } else { 2945 sendTestResult(SENSOR_APP_EVT_STATUS_ERROR, SENS_TYPE_GYRO); 2946 } 2947 2948 // turn GYR to SUSPEND mode 2949 SPI_WRITE(BMI160_REG_CMD, 0x14, 1000); 2950 2951 mTask.gyro_test_state = GYRO_TEST_DONE; 2952 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[GYR], __FUNCTION__); 2953 break; 2954 2955 default: 2956 ERROR_PRINT("Invalid gyro test state\n"); 2957 break; 2958 } 2959 } 2960 2961 static bool gyrSelfTest(void *cookie) 2962 { 2963 TDECL(); 2964 INFO_PRINT("gyrSelfTest\n"); 2965 2966 if (!mTask.sensors[GYR].powered && trySwitchState(SENSOR_TESTING)) { 2967 mTask.gyro_test_state = GYRO_TEST_START; 2968 gyroTestHandling(); 2969 return true; 2970 } else { 2971 ERROR_PRINT("cannot test gyro because sensor is busy\n"); 2972 sendTestResult(SENSOR_APP_EVT_STATUS_BUSY, SENS_TYPE_GYRO); 2973 return false; 2974 } 2975 } 2976 2977 #ifdef MAG_SLAVE_PRESENT 2978 static bool magCfgData(void *data, void *cookie) 2979 { 2980 float *values = data; 2981 2982 INFO_PRINT("magCfgData: %ld, %ld, %ld\n", 2983 (int32_t)(values[0] * 1000), (int32_t)(values[1] * 1000), (int32_t)(values[2] * 1000)); 2984 2985 mTask.moc.x_bias = values[0]; 2986 mTask.moc.y_bias = values[1]; 2987 mTask.moc.z_bias = values[2]; 2988 2989 mTask.magBiasPosted = false; 2990 2991 return true; 2992 } 2993 #endif 2994 2995 #define DEC_OPS(power, firmware, rate, flush) \ 2996 .sensorPower = power, \ 2997 .sensorFirmwareUpload = firmware, \ 2998 .sensorSetRate = rate, \ 2999 .sensorFlush = flush 3000 3001 #define DEC_OPS_SEND(power, firmware, rate, flush, send) \ 3002 DEC_OPS(power, firmware, rate, flush), \ 3003 .sensorSendOneDirectEvt = send 3004 3005 #define DEC_OPS_CAL_CFG_TEST(power, firmware, rate, flush, cal, cfg, test) \ 3006 DEC_OPS(power, firmware, rate, flush), \ 3007 .sensorCalibrate = cal, \ 3008 .sensorCfgData = cfg, \ 3009 .sensorSelfTest = test, 3010 3011 #define DEC_OPS_CFG(power, firmware, rate, flush, cfg) \ 3012 DEC_OPS(power, firmware, rate, flush), \ 3013 .sensorCfgData = cfg 3014 3015 static const struct SensorOps mSensorOps[NUM_OF_SENSOR] = 3016 { 3017 { DEC_OPS_CAL_CFG_TEST(accPower, accFirmwareUpload, accSetRate, accFlush, accCalibration, 3018 accCfgData, accSelfTest) }, 3019 { DEC_OPS_CAL_CFG_TEST(gyrPower, gyrFirmwareUpload, gyrSetRate, gyrFlush, gyrCalibration, 3020 gyrCfgData, gyrSelfTest) }, 3021 #ifdef MAG_SLAVE_PRESENT 3022 { DEC_OPS_CFG(magPower, magFirmwareUpload, magSetRate, magFlush, magCfgData) }, 3023 #endif 3024 { DEC_OPS(stepPower, stepFirmwareUpload, stepSetRate, stepFlush) }, 3025 { DEC_OPS(doubleTapPower, doubleTapFirmwareUpload, doubleTapSetRate, doubleTapFlush) }, 3026 { DEC_OPS(flatPower, flatFirmwareUpload, flatSetRate, flatFlush) }, 3027 { DEC_OPS(anyMotionPower, anyMotionFirmwareUpload, anyMotionSetRate, anyMotionFlush) }, 3028 { DEC_OPS(noMotionPower, noMotionFirmwareUpload, noMotionSetRate, noMotionFlush) }, 3029 { DEC_OPS_SEND(stepCntPower, stepCntFirmwareUpload, stepCntSetRate, stepCntFlush, 3030 stepCntSendLastData) }, 3031 }; 3032 3033 static void configEvent(struct BMI160Sensor *mSensor, struct ConfigStat *ConfigData) 3034 { 3035 int i; 3036 3037 for (i = 0; &mTask.sensors[i] != mSensor; i++) ; 3038 3039 if (ConfigData->enable == 0 && mSensor->powered) 3040 mSensorOps[i].sensorPower(false, (void *)i); 3041 else if (ConfigData->enable == 1 && !mSensor->powered) 3042 mSensorOps[i].sensorPower(true, (void *)i); 3043 else 3044 mSensorOps[i].sensorSetRate(ConfigData->rate, ConfigData->latency, (void *)i); 3045 } 3046 3047 static void timeSyncEvt(uint32_t evtGeneration, bool evtDataValid) 3048 { 3049 TDECL(); 3050 // not processing pending events 3051 if (evtDataValid) { 3052 // stale event 3053 if (evtGeneration != mTask.poll_generation) 3054 return; 3055 3056 mTask.active_poll_generation = mTask.poll_generation; 3057 } 3058 3059 if (trySwitchState(SENSOR_TIME_SYNC)) { 3060 SPI_READ(BMI160_REG_SENSORTIME_0, 3, &mTask.sensorTimeBuffer); 3061 SPI_READ(BMI160_REG_TEMPERATURE_0, 2, &mTask.temperatureBuffer); 3062 // sensorSpiCallback schedules a private event, which can be delayed 3063 // by other long-running tasks. 3064 // Take the rtc time now so it matches the current sensorTime register 3065 // reading. 3066 mTask.timesync_rtc_time = sensorGetTime(); 3067 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask, __FUNCTION__); 3068 } else { 3069 mTask.pending_time_sync = true; 3070 } 3071 } 3072 3073 static void processPendingEvt(void) 3074 { 3075 TDECL(); 3076 enum SensorIndex i; 3077 if (mTask.pending_int[0]) { 3078 mTask.pending_int[0] = false; 3079 initiateFifoRead(false /*isInterruptContext*/); 3080 return; 3081 } 3082 if (mTask.pending_int[1]) { 3083 mTask.pending_int[1] = false; 3084 int2Evt(); 3085 return; 3086 } 3087 if (mTask.pending_time_sync) { 3088 mTask.pending_time_sync = false; 3089 timeSyncEvt(0, false); 3090 return; 3091 } 3092 for (i = FIRST_CONT_SENSOR; i < NUM_OF_SENSOR; i++) { 3093 if (mTask.pending_config[i]) { 3094 mTask.pending_config[i] = false; 3095 configEvent(&mTask.sensors[i], &mTask.sensors[i].pConfig); 3096 return; 3097 } 3098 } 3099 if (mTask.sensors[STEPCNT].flush > 0 || T(pending_step_cnt)) { 3100 T(pending_step_cnt) = T(pending_step_cnt) && !stepCntFlushGetData(); 3101 return; 3102 } 3103 if (mTask.pending_calibration_save) { 3104 mTask.pending_calibration_save = !saveCalibration(); 3105 return; 3106 } 3107 } 3108 3109 static void sensorInit(void) 3110 { 3111 TDECL(); 3112 switch (mTask.init_state) { 3113 case RESET_BMI160: 3114 DEBUG_PRINT("Performing soft reset\n"); 3115 // perform soft reset and wait for 100ms 3116 SPI_WRITE(BMI160_REG_CMD, 0xb6, 100000); 3117 // dummy reads after soft reset, wait 100us 3118 SPI_READ(BMI160_REG_MAGIC, 1, &mTask.dataBuffer, 100); 3119 3120 mTask.init_state = INIT_BMI160; 3121 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask, "sensorInit RESET" ); 3122 break; 3123 3124 case INIT_BMI160: 3125 // Read any pending interrupts to reset them 3126 SPI_READ(BMI160_REG_INT_STATUS_0, 4, &mTask.statusBuffer); 3127 3128 // disable accel, gyro and mag data in FIFO, enable header, enable time. 3129 SPI_WRITE(BMI160_REG_FIFO_CONFIG_1, 0x12, 450); 3130 3131 // set the watermark to 24 byte 3132 SPI_WRITE(BMI160_REG_FIFO_CONFIG_0, 0x06, 450); 3133 3134 // FIFO watermark and fifo_full interrupt enabled 3135 SPI_WRITE(BMI160_REG_INT_EN_0, 0x00, 450); 3136 SPI_WRITE(BMI160_REG_INT_EN_1, 0x60, 450); 3137 SPI_WRITE(BMI160_REG_INT_EN_2, 0x00, 450); 3138 3139 // INT1, INT2 enabled, high-edge (push-pull) triggered. 3140 SPI_WRITE(BMI160_REG_INT_OUT_CTRL, 0xbb, 450); 3141 3142 // INT1, INT2 input disabled, interrupt mode: non-latched 3143 SPI_WRITE(BMI160_REG_INT_LATCH, 0x00, 450); 3144 3145 // Map data interrupts (e.g., FIFO) to INT1 and physical 3146 // interrupts (e.g., any motion) to INT2 3147 SPI_WRITE(BMI160_REG_INT_MAP_0, 0x00, 450); 3148 SPI_WRITE(BMI160_REG_INT_MAP_1, 0xE1, 450); 3149 SPI_WRITE(BMI160_REG_INT_MAP_2, 0xFF, 450); 3150 3151 // Use pre-filtered data for tap interrupt 3152 SPI_WRITE(BMI160_REG_INT_DATA_0, 0x08); 3153 3154 // Disable PMU_TRIGGER 3155 SPI_WRITE(BMI160_REG_PMU_TRIGGER, 0x00, 450); 3156 3157 // tell gyro and accel to NOT use the FOC offset. 3158 mTask.sensors[ACC].offset_enable = false; 3159 mTask.sensors[GYR].offset_enable = false; 3160 SPI_WRITE(BMI160_REG_OFFSET_6, offset6Mode(), 450); 3161 3162 // initial range for accel (+-8g) and gyro (+-1000 degree). 3163 SPI_WRITE(BMI160_REG_ACC_RANGE, 0x08, 450); 3164 SPI_WRITE(BMI160_REG_GYR_RANGE, 0x01, 450); 3165 3166 // Reset step counter 3167 SPI_WRITE(BMI160_REG_CMD, 0xB2, 10000); 3168 // Reset interrupt 3169 SPI_WRITE(BMI160_REG_CMD, 0xB1, 10000); 3170 // Reset fifo 3171 SPI_WRITE(BMI160_REG_CMD, 0xB0, 10000); 3172 3173 #ifdef MAG_SLAVE_PRESENT 3174 mTask.init_state = INIT_MAG; 3175 mTask.mag_state = MAG_SET_START; 3176 #else 3177 // no mag connected to secondary interface 3178 mTask.init_state = INIT_ON_CHANGE_SENSORS; 3179 #endif 3180 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask, "sensorInit INIT"); 3181 break; 3182 3183 case INIT_MAG: 3184 // Don't check statusBuffer if we are just starting mag config 3185 if (mTask.mag_state == MAG_SET_START) { 3186 T(mRetryLeft) = RETRY_CNT_MAG; 3187 magConfig(); 3188 } else if (mTask.mag_state < MAG_SET_DATA && mTask.statusBuffer[1] & 0x04) { 3189 // fixme: poll_until to reduce states 3190 // fixme: check should be done before SPI_READ in MAG_READ 3191 SPI_READ(BMI160_REG_STATUS, 1, &mTask.statusBuffer, 1000); 3192 if (--T(mRetryLeft) == 0) { 3193 ERROR_PRINT("INIT_MAG failed\n"); 3194 // fixme: duplicate suspend mag here 3195 mTask.mag_state = MAG_INIT_FAILED; 3196 mTask.init_state = INIT_ON_CHANGE_SENSORS; 3197 } 3198 } else { 3199 T(mRetryLeft) = RETRY_CNT_MAG; 3200 magConfig(); 3201 } 3202 3203 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask, "sensorInit INIT_MAG"); 3204 break; 3205 3206 case INIT_ON_CHANGE_SENSORS: 3207 // configure any_motion and no_motion for 50Hz accel samples 3208 configMotion(MOTION_ODR); 3209 3210 // select no_motion over slow_motion 3211 // select any_motion over significant motion 3212 SPI_WRITE(BMI160_REG_INT_MOTION_3, 0x15, 450); 3213 3214 // int_tap_quiet=30ms, int_tap_shock=75ms, int_tap_dur=150ms 3215 SPI_WRITE(BMI160_REG_INT_TAP_0, 0x42, 450); 3216 3217 // int_tap_th = 7 * 250 mg (8-g range) 3218 SPI_WRITE(BMI160_REG_INT_TAP_1, TAP_THRESHOLD, 450); 3219 3220 // config step detector 3221 SPI_WRITE(BMI160_REG_STEP_CONF_0, 0x15, 450); 3222 SPI_WRITE(BMI160_REG_STEP_CONF_1, 0x03, 450); 3223 3224 // int_flat_theta = 44.8 deg * (16/64) = 11.2 deg 3225 SPI_WRITE(BMI160_REG_INT_FLAT_0, 0x10, 450); 3226 3227 // int_flat_hold_time = (640 msec) 3228 // int_flat_hy = 44.8 * 4 / 64 = 2.8 deg 3229 SPI_WRITE(BMI160_REG_INT_FLAT_1, 0x14, 450); 3230 3231 mTask.init_state = INIT_DONE; 3232 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask, "sensorInit INIT_ONC"); 3233 break; 3234 3235 default: 3236 INFO_PRINT("Invalid init_state.\n"); 3237 } 3238 } 3239 3240 static void handleSpiDoneEvt(const void* evtData) 3241 { 3242 TDECL(); 3243 struct BMI160Sensor *mSensor; 3244 uint64_t SensorTime; 3245 int16_t temperature16; 3246 int i; 3247 bool returnIdle = false; 3248 3249 switch (GET_STATE()) { 3250 case SENSOR_BOOT: 3251 SET_STATE(SENSOR_VERIFY_ID); 3252 // dummy reads after boot, wait 100us 3253 SPI_READ(BMI160_REG_MAGIC, 1, &mTask.statusBuffer, 100); 3254 // read the device ID for bmi160 3255 SPI_READ(BMI160_REG_ID, 1, &mTask.dataBuffer); 3256 spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask, "spiDone SENSOR_BOOT"); 3257 break; 3258 case SENSOR_VERIFY_ID: 3259 if (mTask.dataBuffer[1] != BMI160_ID) { 3260 T(mRetryLeft) --; 3261 ERROR_PRINT("failed id match: %02x\n", mTask.dataBuffer[1]); 3262 if (T(mRetryLeft) == 0) 3263 break; 3264 // For some reason the first ID read will fail to get the 3265 // correct value. need to retry a few times. 3266 SET_STATE(SENSOR_BOOT); 3267 if (timTimerSet(100000000, 100, 100, sensorTimerCallback, NULL, true) == 0) 3268 ERROR_PRINT("Couldn't get a timer to verify ID\n"); 3269 break; 3270 } else { 3271 SET_STATE(SENSOR_INITIALIZING); 3272 mTask.init_state = RESET_BMI160; 3273 sensorInit(); 3274 break; 3275 } 3276 case SENSOR_INITIALIZING: 3277 if (mTask.init_state == INIT_DONE) { 3278 DEBUG_PRINT("Done initialzing, system IDLE\n"); 3279 for (i=0; i<NUM_OF_SENSOR; i++) 3280 sensorRegisterInitComplete(mTask.sensors[i].handle); 3281 // In case other tasks have already requested us before we finish booting up. 3282 returnIdle = true; 3283 } else { 3284 sensorInit(); 3285 } 3286 break; 3287 case SENSOR_POWERING_UP: 3288 mSensor = (struct BMI160Sensor *)evtData; 3289 if (mSensor->idx >= FIRST_ONESHOT_SENSOR && ++mTask.active_oneshot_sensor_cnt == 1) { 3290 // if this is the first one-shot sensor to enable, we need 3291 // to request the accel at 50Hz. 3292 sensorRequest(mTask.tid, mTask.sensors[ACC].handle, SENSOR_HZ(50), SENSOR_LATENCY_NODATA); 3293 //DEBUG_PRINT("oneshot on\n"); 3294 } 3295 sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, 1, 0); 3296 returnIdle = true; 3297 break; 3298 case SENSOR_POWERING_DOWN: 3299 mSensor = (struct BMI160Sensor *)evtData; 3300 if (mSensor->idx >= FIRST_ONESHOT_SENSOR && --mTask.active_oneshot_sensor_cnt == 0) { 3301 // if this is the last one-shot sensor to disable, we need to 3302 // release the accel. 3303 sensorRelease(mTask.tid, mTask.sensors[ACC].handle); 3304 //DEBUG_PRINT("oneshot off\n"); 3305 } 3306 sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, 0, 0); 3307 3308 if (mTask.pending_dispatch) { 3309 mTask.pending_dispatch = false; 3310 dispatchData(); 3311 } 3312 returnIdle = true; 3313 break; 3314 case SENSOR_INT_1_HANDLING: 3315 dispatchData(); 3316 sendFlushEvt(); 3317 returnIdle = true; 3318 break; 3319 case SENSOR_INT_2_HANDLING: 3320 int2Handling(); 3321 returnIdle = true; 3322 break; 3323 case SENSOR_CONFIG_CHANGING: 3324 mSensor = (struct BMI160Sensor *)evtData; 3325 sensorSignalInternalEvt(mSensor->handle, 3326 SENSOR_INTERNAL_EVT_RATE_CHG, mSensor->rate, mSensor->latency); 3327 3328 if (mTask.pending_dispatch) { 3329 mTask.pending_dispatch = false; 3330 dispatchData(); 3331 } 3332 3333 returnIdle = true; 3334 break; 3335 case SENSOR_CALIBRATING: 3336 mSensor = (struct BMI160Sensor *)evtData; 3337 if (mTask.calibration_state == CALIBRATION_DONE) { 3338 DEBUG_PRINT("DONE calibration\n"); 3339 returnIdle = true; 3340 } else if (mTask.calibration_state == CALIBRATION_TIMEOUT) { 3341 DEBUG_PRINT("Calibration TIMED OUT\n"); 3342 sendCalibrationResult(SENSOR_APP_EVT_STATUS_ERROR, 3343 (mSensor->idx == ACC) ? SENS_TYPE_ACCEL : SENS_TYPE_GYRO, 0, 0, 0); 3344 returnIdle = true; 3345 } else if (mSensor->idx == ACC) { 3346 accCalibrationHandling(); 3347 } else if (mSensor->idx == GYR) { 3348 gyrCalibrationHandling(); 3349 } 3350 break; 3351 case SENSOR_TESTING: 3352 mSensor = (struct BMI160Sensor *)evtData; 3353 if (mSensor->idx == ACC) { 3354 if (mTask.acc_test_state == ACC_TEST_DONE) { 3355 returnIdle = true; 3356 } else { 3357 accTestHandling(); 3358 } 3359 } else if (mSensor->idx == GYR) { 3360 if (mTask.gyro_test_state == GYRO_TEST_DONE) { 3361 returnIdle = true; 3362 } else { 3363 gyroTestHandling(); 3364 } 3365 } 3366 break; 3367 case SENSOR_STEP_CNT: 3368 sendStepCnt(); 3369 returnIdle = true; 3370 break; 3371 case SENSOR_TIME_SYNC: 3372 SensorTime = parseSensortime(mTask.sensorTimeBuffer[1] | 3373 (mTask.sensorTimeBuffer[2] << 8) | (mTask.sensorTimeBuffer[3] << 16)); 3374 map_sensortime_to_rtc_time(SensorTime, mTask.timesync_rtc_time); 3375 3376 temperature16 = (mTask.temperatureBuffer[1] | (mTask.temperatureBuffer[2] << 8)); 3377 if (temperature16 == 0x8000) { 3378 mTask.tempCelsius = kTempInvalid; 3379 } else { 3380 mTask.tempCelsius = 23.0f + temperature16 * kScale_temp; 3381 mTask.tempTime = sensorGetTime(); 3382 } 3383 3384 if (mTask.active_poll_generation == mTask.poll_generation) { 3385 // attach the generation number to event 3386 if (timTimerSet(kTimeSyncPeriodNs, 100, 100, timeSyncCallback, 3387 (void *)mTask.poll_generation, true) == 0) 3388 ERROR_PRINT("Couldn't get a timer for time sync\n"); 3389 } 3390 3391 returnIdle = true; 3392 break; 3393 case SENSOR_SAVE_CALIBRATION: 3394 DEBUG_PRINT("SENSOR_SAVE_CALIBRATION: %02x %02x %02x %02x %02x %02x %02x\n", 3395 mTask.dataBuffer[1], mTask.dataBuffer[2], mTask.dataBuffer[3], mTask.dataBuffer[4], 3396 mTask.dataBuffer[5], mTask.dataBuffer[6], mTask.dataBuffer[7]); 3397 returnIdle = true; 3398 break; 3399 default: 3400 break; 3401 } 3402 3403 if (returnIdle) { 3404 SET_STATE(SENSOR_IDLE); 3405 processPendingEvt(); 3406 } 3407 } 3408 3409 static void handleEvent(uint32_t evtType, const void* evtData) 3410 { 3411 TDECL(); 3412 uint64_t currTime; 3413 uint8_t *packet; 3414 float newMagBias; 3415 3416 switch (evtType) { 3417 case EVT_APP_START: 3418 SET_STATE(SENSOR_BOOT); 3419 T(mRetryLeft) = RETRY_CNT_ID; 3420 osEventUnsubscribe(mTask.tid, EVT_APP_START); 3421 3422 // wait 100ms for sensor to boot 3423 currTime = timGetTime(); 3424 if (currTime < 100000000ULL) { 3425 if (timTimerSet(100000000 - currTime, 100, 100, sensorTimerCallback, NULL, true) == 0) 3426 ERROR_PRINT("Couldn't get a timer for boot delay\n"); 3427 break; 3428 } 3429 /* We have already been powered on long enough - fall through */ 3430 case EVT_SPI_DONE: 3431 handleSpiDoneEvt(evtData); 3432 break; 3433 3434 case EVT_APP_FROM_HOST: 3435 packet = (uint8_t*)evtData; 3436 if (packet[0] == sizeof(float)) { 3437 memcpy(&newMagBias, packet+1, sizeof(float)); 3438 #ifdef MAG_SLAVE_PRESENT 3439 magCalAddBias(&mTask.moc, (mTask.last_charging_bias_x - newMagBias), 0.0, 0.0); 3440 #endif 3441 mTask.last_charging_bias_x = newMagBias; 3442 mTask.magBiasPosted = false; 3443 } 3444 break; 3445 3446 case EVT_SENSOR_INTERRUPT_1: 3447 initiateFifoRead(false /*isInterruptContext*/); 3448 break; 3449 case EVT_SENSOR_INTERRUPT_2: 3450 int2Evt(); 3451 break; 3452 case EVT_TIME_SYNC: 3453 timeSyncEvt((uint32_t)evtData, true); 3454 default: 3455 break; 3456 } 3457 } 3458 3459 static void initSensorStruct(struct BMI160Sensor *sensor, enum SensorIndex idx) 3460 { 3461 sensor->idx = idx; 3462 sensor->powered = false; 3463 sensor->configed = false; 3464 sensor->rate = 0; 3465 sensor->offset[0] = 0; 3466 sensor->offset[1] = 0; 3467 sensor->offset[2] = 0; 3468 sensor->latency = 0; 3469 sensor->data_evt = NULL; 3470 sensor->flush = 0; 3471 sensor->prev_rtc_time = 0; 3472 } 3473 3474 static bool startTask(uint32_t task_id) 3475 { 3476 TDECL(); 3477 enum SensorIndex i; 3478 size_t slabSize; 3479 3480 time_init(); 3481 3482 T(tid) = task_id; 3483 3484 T(Int1) = gpioRequest(BMI160_INT1_PIN); 3485 T(Isr1).func = bmi160Isr1; 3486 T(Int2) = gpioRequest(BMI160_INT2_PIN); 3487 T(Isr2).func = bmi160Isr2; 3488 T(pending_int[0]) = false; 3489 T(pending_int[1]) = false; 3490 T(pending_step_cnt) = false; 3491 T(pending_dispatch) = false; 3492 T(frame_sensortime_valid) = false; 3493 T(poll_generation) = 0; 3494 T(tempCelsius) = kTempInvalid; 3495 T(tempTime) = 0; 3496 3497 T(mode).speed = BMI160_SPI_SPEED_HZ; 3498 T(mode).bitsPerWord = 8; 3499 T(mode).cpol = SPI_CPOL_IDLE_HI; 3500 T(mode).cpha = SPI_CPHA_TRAILING_EDGE; 3501 T(mode).nssChange = true; 3502 T(mode).format = SPI_FORMAT_MSB_FIRST; 3503 T(cs) = GPIO_PB(12); 3504 3505 T(watermark) = 0; 3506 3507 spiMasterRequest(BMI160_SPI_BUS_ID, &T(spiDev)); 3508 3509 for (i = FIRST_CONT_SENSOR; i < NUM_OF_SENSOR; i++) { 3510 initSensorStruct(&T(sensors[i]), i); 3511 T(sensors[i]).handle = sensorRegister(&mSensorInfo[i], &mSensorOps[i], NULL, false); 3512 T(pending_config[i]) = false; 3513 } 3514 3515 osEventSubscribe(mTask.tid, EVT_APP_START); 3516 3517 #ifdef ACCEL_CAL_ENABLED 3518 // Init Accel Cal 3519 accelCalInit(&mTask.acc, 3520 800000000, /* Stillness Time in ns (0.8s) */ 3521 5, /* Minimum Sample Number */ 3522 0.00025, /* Threshold */ 3523 15, /* nx bucket count */ 3524 15, /* nxb bucket count */ 3525 15, /* ny bucket count */ 3526 15, /* nyb bucket count */ 3527 15, /* nz bucket count */ 3528 15, /* nzb bucket count */ 3529 15); /* nle bucket count */ 3530 #endif 3531 3532 #ifdef GYRO_CAL_ENABLED 3533 // Gyro Cal -- Initialization. 3534 gyroCalInit(&mTask.gyro_cal, 3535 5e9, // min stillness period = 5 seconds 3536 6e9, // max stillness period = 6 seconds 3537 0, 0, 0, // initial bias offset calibration 3538 0, // time stamp of initial bias calibration 3539 1.5e9, // analysis window length = 1.5 seconds 3540 5e-5f, // gyroscope variance threshold [rad/sec]^2 3541 1e-5f, // gyroscope confidence delta [rad/sec]^2 3542 8e-3f, // accelerometer variance threshold [m/sec^2]^2 3543 1.6e-3f, // accelerometer confidence delta [m/sec^2]^2 3544 1.4f, // magnetometer variance threshold [uT]^2 3545 0.25, // magnetometer confidence delta [uT]^2 3546 0.95f, // stillness threshold [0,1] 3547 1); // 1=gyro calibrations will be applied 3548 #endif 3549 3550 #ifdef MAG_SLAVE_PRESENT 3551 initMagCal(&mTask.moc, 3552 0.0f, 0.0f, 0.0f, // bias x, y, z 3553 1.0f, 0.0f, 0.0f, // c00, c01, c02 3554 0.0f, 1.0f, 0.0f, // c10, c11, c12 3555 0.0f, 0.0f, 1.0f); // c20, c21, c22 3556 #endif 3557 3558 slabSize = sizeof(struct TripleAxisDataEvent) + 3559 MAX_NUM_COMMS_EVENT_SAMPLES * sizeof(struct TripleAxisDataPoint); 3560 3561 // each event has 15 samples, with 7 bytes per sample from the fifo. 3562 // the fifo size is 1K. 3563 // 20 slabs because some slabs may only hold 1-2 samples. 3564 // XXX: this consumes too much memeory, need to optimize 3565 T(mDataSlab) = slabAllocatorNew(slabSize, 4, 20); 3566 if (!T(mDataSlab)) { 3567 INFO_PRINT("slabAllocatorNew() failed\n"); 3568 return false; 3569 } 3570 T(mWbufCnt) = 0; 3571 T(mRegCnt) = 0; 3572 T(spiInUse) = false; 3573 3574 T(interrupt_enable_0) = 0x00; 3575 T(interrupt_enable_2) = 0x00; 3576 3577 // initialize the last bmi160 time to be ULONG_MAX, so that we know it's 3578 // not valid yet. 3579 T(last_sensortime) = 0; 3580 T(frame_sensortime) = ULONG_LONG_MAX; 3581 3582 // it's ok to leave interrupt open all the time. 3583 enableInterrupt(T(Int1), &T(Isr1)); 3584 enableInterrupt(T(Int2), &T(Isr2)); 3585 3586 return true; 3587 } 3588 3589 static void endTask(void) 3590 { 3591 TDECL(); 3592 #ifdef MAG_SLAVE_PRESENT 3593 destroy_mag_cal(&mTask.moc); 3594 #endif 3595 #ifdef ACCEL_CAL_ENABLED 3596 accelCalDestroy(&mTask.acc); 3597 #endif 3598 slabAllocatorDestroy(T(mDataSlab)); 3599 spiMasterRelease(mTask.spiDev); 3600 3601 // disable and release interrupt. 3602 disableInterrupt(mTask.Int1, &mTask.Isr1); 3603 disableInterrupt(mTask.Int2, &mTask.Isr2); 3604 gpioRelease(mTask.Int1); 3605 gpioRelease(mTask.Int2); 3606 } 3607 3608 /** 3609 * Parse BMI160 FIFO frame without side effect. 3610 * 3611 * The major purpose of this function is to determine if FIFO content is received completely (start 3612 * to see invalid headers). If not, return the pointer to the beginning last incomplete frame so 3613 * additional read can use this pointer as start of read buffer. 3614 * 3615 * @param buf buffer location 3616 * @param size size of data to be parsed 3617 * 3618 * @return NULL if the FIFO is received completely; or pointer to the beginning of last incomplete 3619 * frame for additional read. 3620 */ 3621 static uint8_t* shallowParseFrame(uint8_t * buf, int size) { 3622 int i = 0; 3623 int iLastFrame = 0; // last valid frame header index 3624 3625 DEBUG_PRINT_IF(DBG_SHALLOW_PARSE, "spf start %p: %x %x %x\n", buf, buf[0], buf[1], buf[2]); 3626 while (size > 0) { 3627 int fh_mode, fh_param; 3628 iLastFrame = i; 3629 3630 if (buf[i] == BMI160_FRAME_HEADER_INVALID) { 3631 // no more data 3632 DEBUG_PRINT_IF(DBG_SHALLOW_PARSE, "spf:at%d=0x80\n", iLastFrame); 3633 return NULL; 3634 } else if (buf[i] == BMI160_FRAME_HEADER_SKIP) { 3635 // artifically added nop frame header, skip 3636 DEBUG_PRINT_IF(DBG_SHALLOW_PARSE, "at %d, skip header\n", i); 3637 i++; 3638 size--; 3639 continue; 3640 } 3641 3642 //++frame_num; 3643 3644 fh_mode = buf[i] >> 6; 3645 fh_param = (buf[i] >> 2) & 0xf; 3646 3647 i++; 3648 size--; 3649 3650 if (fh_mode == 1) { 3651 // control frame. 3652 if (fh_param == 0) { 3653 // skip frame, we skip it (1 byte) 3654 i++; 3655 size--; 3656 DEBUG_PRINT_IF(DBG_SHALLOW_PARSE, "at %d, a skip frame\n", iLastFrame); 3657 } else if (fh_param == 1) { 3658 // sensortime frame (3 bytes) 3659 i += 3; 3660 size -= 3; 3661 DEBUG_PRINT_IF(DBG_SHALLOW_PARSE, "at %d, a sensor_time frame\n", iLastFrame); 3662 } else if (fh_param == 2) { 3663 // fifo_input config frame (1byte) 3664 i++; 3665 size--; 3666 DEBUG_PRINT_IF(DBG_SHALLOW_PARSE, "at %d, a fifo cfg frame\n", iLastFrame); 3667 } else { 3668 size = 0; // drop this batch 3669 DEBUG_PRINT_IF(DBG_SHALLOW_PARSE, "Invalid fh_param in control frame!!\n"); 3670 // mark invalid 3671 buf[iLastFrame] = BMI160_FRAME_HEADER_INVALID; 3672 return NULL; 3673 } 3674 } else if (fh_mode == 2) { 3675 // regular frame, dispatch data to each sensor's own fifo 3676 if (fh_param & 4) { // have mag data 3677 i += 8; 3678 size -= 8; 3679 } 3680 if (fh_param & 2) { // have gyro data 3681 i += 6; 3682 size -= 6; 3683 } 3684 if (fh_param & 1) { // have accel data 3685 i += 6; 3686 size -= 6; 3687 } 3688 DEBUG_PRINT_IF(DBG_SHALLOW_PARSE, "at %d, a reg frame acc %d, gyro %d, mag %d\n", 3689 iLastFrame, fh_param &1 ? 1:0, fh_param&2?1:0, fh_param&4?1:0); 3690 } else { 3691 size = 0; // drop the rest of batch 3692 DEBUG_PRINT_IF(DBG_SHALLOW_PARSE, "spf: Invalid fh_mode %d!!\n", fh_mode); 3693 //mark invalid 3694 buf[iLastFrame] = BMI160_FRAME_HEADER_INVALID; 3695 return NULL; 3696 } 3697 } 3698 3699 // there is a partial frame, return where to write next chunck of data 3700 DEBUG_PRINT_IF(DBG_SHALLOW_PARSE, "partial frame ends %p\n", buf + iLastFrame); 3701 return buf + iLastFrame; 3702 } 3703 3704 /** 3705 * Intialize the first read of chunked SPI read sequence. 3706 * 3707 * @param index starting index of the txrxBuffer in which the data will be write into. 3708 */ 3709 static void chunkedReadInit_(TASK, int index, int size) { 3710 3711 if (GET_STATE() != SENSOR_INT_1_HANDLING) { 3712 ERROR_PRINT("chunkedReadInit in wrong mode"); 3713 return; 3714 } 3715 3716 if (T(mRegCnt)) { 3717 //chunked read are always executed as a single command. This should never happen. 3718 ERROR_PRINT("SPI queue not empty at chunkedReadInit, regcnt = %d", T(mRegCnt)); 3719 // In case it did happen, we do not want to write crap to BMI160. 3720 T(mRegCnt) = 0; 3721 } 3722 3723 T(mWbufCnt) = index; 3724 if (T(mWbufCnt) > FIFO_READ_SIZE) { 3725 // drop data to prevent bigger issue 3726 T(mWbufCnt) = 0; 3727 } 3728 T(chunkReadSize) = size > CHUNKED_READ_SIZE ? size : CHUNKED_READ_SIZE; 3729 3730 DEBUG_PRINT_IF(DBG_CHUNKED, "crd %d>>%d\n", T(chunkReadSize), index); 3731 SPI_READ(BMI160_REG_FIFO_DATA, T(chunkReadSize), &T(dataBuffer)); 3732 spiBatchTxRx(&T(mode), chunkedReadSpiCallback, _task, __FUNCTION__); 3733 } 3734 3735 /** 3736 * Chunked SPI read callback. 3737 * 3738 * Handles the chunked read logic: issue additional read if necessary, or calls sensorSpiCallback() 3739 * if the entire FIFO is read. 3740 * 3741 * @param cookie extra data 3742 * @param err error 3743 * 3744 * @see sensorSpiCallback() 3745 */ 3746 static void chunkedReadSpiCallback(void *cookie, int err) { 3747 TASK = (_Task*) cookie; 3748 3749 T(spiInUse) = false; 3750 DEBUG_PRINT_IF(err !=0 || GET_STATE() != SENSOR_INT_1_HANDLING, 3751 "crcb,e:%d,s:%d", err, (int)GET_STATE()); 3752 bool int1 = gpioGet(T(Int1)); 3753 if (err != 0) { 3754 DEBUG_PRINT_IF(DBG_CHUNKED, "spi err, crd retry"); 3755 // read full fifo length to be safe 3756 chunkedReadInit(0, FIFO_READ_SIZE); 3757 return; 3758 } 3759 3760 *T(dataBuffer) = BMI160_FRAME_HEADER_SKIP; // fill the 0x00/0xff hole at the first byte 3761 uint8_t* end = shallowParseFrame(T(dataBuffer), T(chunkReadSize)); 3762 3763 if (end == NULL) { 3764 // if interrupt is still set after read for some reason, set the pending interrupt 3765 // to handle it immediately after data is handled. 3766 T(pending_int[0]) = T(pending_int[0]) || int1; 3767 3768 // recover the buffer and valid data size to make it looks like a single read so that 3769 // real frame parse works properly 3770 T(dataBuffer) = T(txrxBuffer); 3771 T(xferCnt) = FIFO_READ_SIZE; 3772 sensorSpiCallback(cookie, err); 3773 } else { 3774 DEBUG_PRINT_IF(DBG_CHUNKED, "crd cont"); 3775 chunkedReadInit(end - T(txrxBuffer), CHUNKED_READ_SIZE); 3776 } 3777 } 3778 3779 /** 3780 * Initiate read of sensor fifo. 3781 * 3782 * If task is in idle state, init chunked FIFO read; otherwise, submit an interrupt message or mark 3783 * the read pending depending if it is called in interrupt context. 3784 * 3785 * @param isInterruptContext true if called from interrupt context; false otherwise. 3786 * 3787 */ 3788 static void initiateFifoRead_(TASK, bool isInterruptContext) { 3789 if (trySwitchState(SENSOR_INT_1_HANDLING)) { 3790 // estimate first read size to be watermark + 1 more sample + some extra 3791 int firstReadSize = T(watermark) * 4 + 32; // 1+6+6+8+1+3 + extra = 25 + extra = 32 3792 if (firstReadSize < CHUNKED_READ_SIZE) { 3793 firstReadSize = CHUNKED_READ_SIZE; 3794 } 3795 chunkedReadInit(0, firstReadSize); 3796 } else { 3797 if (isInterruptContext) { 3798 // called from interrupt context, queue event 3799 if (!osEnqueuePrivateEvt(EVT_SENSOR_INTERRUPT_1, _task, NULL, T(tid))) 3800 ERROR_PRINT("initiateFifoRead_: osEnqueuePrivateEvt() failed\n"); 3801 } else { 3802 // non-interrupt context, set pending flag, so next time it will be picked up after 3803 // switching back to idle. 3804 // Note: even if we are still in SENSOR_INT_1_HANDLING, the SPI may already finished and 3805 // we need to issue another SPI read to get the latest status. 3806 T(pending_int[0]) = true; 3807 } 3808 } 3809 } 3810 3811 /** 3812 * Calculate fifo size using normalized input. 3813 * 3814 * @param iPeriod normalized period vector 3815 * @param iLatency normalized latency vector 3816 * @param factor vector that contains size factor for each sensor 3817 * @param n size of the vectors 3818 * 3819 * @return max size of FIFO to guarantee latency requirements of all sensors or SIZE_MAX if no 3820 * sensor is active. 3821 */ 3822 static size_t calcFifoSize(const int* iPeriod, const int* iLatency, const int* factor, int n) { 3823 int i; 3824 3825 int minLatency = INT_MAX; 3826 for (i = 0; i < n; i++) { 3827 if (iLatency[i] > 0) { 3828 minLatency = iLatency[i] < minLatency ? iLatency[i] : minLatency; 3829 } 3830 } 3831 DEBUG_PRINT_IF(DBG_WM_CALC, "cfifo: min latency %d unit", minLatency); 3832 3833 bool anyActive = false; 3834 size_t s = 0; 3835 size_t head = 0; 3836 for (i = 0; i < n; i++) { 3837 if (iPeriod[i] > 0) { 3838 anyActive = true; 3839 size_t t = minLatency / iPeriod[i]; 3840 head = t > head ? t : head; 3841 s += t * factor[i]; 3842 DEBUG_PRINT_IF(DBG_WM_CALC, "cfifo: %d, s+= %d*%d, head = %d", i, t, factor[i], head); 3843 } 3844 } 3845 3846 return anyActive ? head + s : SIZE_MAX; 3847 } 3848 3849 /** 3850 * Calculate the watermark setting from sensor registration information 3851 * 3852 * It is assumed that all sensor period share a common denominator (true for BMI160) and the 3853 * latency of sensor will be lower bounded by its sampling period. 3854 * 3855 * @return watermark register setting 3856 */ 3857 static uint8_t calcWatermark2_(TASK) { 3858 int period[] = {-1, -1, -1}; 3859 int latency[] = {-1, -1, -1}; 3860 const int factor[] = {6, 6, 8}; 3861 int i; 3862 3863 for (i = FIRST_CONT_SENSOR; i < NUM_CONT_SENSOR; ++i) { 3864 if (T(sensors[i]).configed) { 3865 period[i - ACC] = SENSOR_HZ((float)WATERMARK_MAX_SENSOR_RATE) / T(sensors[i]).rate; 3866 latency[i - ACC] = U64_DIV_BY_U64_CONSTANT( 3867 T(sensors[i]).latency + WATERMARK_TIME_UNIT_NS/2, WATERMARK_TIME_UNIT_NS); 3868 DEBUG_PRINT_IF(DBG_WM_CALC, "cwm2: f %dHz, l %dus => T %d unit, L %d unit", 3869 (int) T(sensors[i]).rate/1024, 3870 (int) U64_DIV_BY_U64_CONSTANT(T(sensors[i]).latency, 1000), 3871 period[i-ACC], latency[i-ACC]); 3872 } 3873 } 3874 3875 3876 size_t watermark = calcFifoSize(period, latency, factor, NUM_CONT_SENSOR) / 4; 3877 DEBUG_PRINT_IF(DBG_WM_CALC, "cwm2: wm = %d", watermark); 3878 watermark = watermark < WATERMARK_MIN ? WATERMARK_MIN : watermark; 3879 watermark = watermark > WATERMARK_MAX ? WATERMARK_MAX : watermark; 3880 3881 return watermark; 3882 } 3883 3884 static bool dumpBinaryPutC(void* p, char c) { 3885 *(*(char**)p)++ = c; 3886 return true; 3887 } 3888 3889 static uint32_t cvprintf_ellipsis(printf_write_c writeF, void* writeD, const char* fmtStr, ...) { 3890 va_list vl; 3891 uint32_t ret; 3892 3893 va_start(vl, fmtStr); 3894 ret = cvprintf(writeF, writeD, fmtStr, vl); 3895 va_end(vl); 3896 3897 return ret; 3898 } 3899 3900 static void dumpBinary(void* buf, unsigned int address, size_t size) { 3901 size_t i, j; 3902 char buffer[5+16*3+1+2]; //5: address, 3:each byte+space, 1: middle space, 1: \n and \0 3903 char* p; 3904 3905 for (i = 0; i < size; ) { 3906 p = buffer; 3907 cvprintf_ellipsis(dumpBinaryPutC, &p, "%08x:", address); 3908 for (j = 0; j < 0x10 && i < size; ++i, ++j) { 3909 if (j == 0x8) { 3910 *p++ = ' '; 3911 } 3912 cvprintf_ellipsis(dumpBinaryPutC, &p, " %02x", ((unsigned char *)buf)[i]); 3913 } 3914 *p = '\0'; 3915 3916 osLog(LOG_INFO, "%s\n", buffer); 3917 address += 0x10; 3918 } 3919 } 3920 3921 INTERNAL_APP_INIT(BMI160_APP_ID, BMI160_APP_VERSION, startTask, endTask, handleEvent); 3922
