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