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