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"
getStateName(int32_t 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