From d9d8b1ca2bcac3ee531685c41acd563c9155e521 Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Tue, 20 Jun 2023 14:33:52 -0500 Subject: [PATCH 01/36] BMI270 - WIP - failing - incl IFRC_IFLIGHT_F745_AIO_V2 for testing --- .../BoschSensortec/BMI270-Sensor-API/LICENSE | 30 + .../BMI270-Sensor-API/OIS_README.md | 170 + .../BMI270-Sensor-API/README.md | 14 + .../BMI270-Sensor-API/betaflight_info.txt | 9 + .../BoschSensortec/BMI270-Sensor-API/bmi2.c | 10338 ++++++++++++++++ .../BoschSensortec/BMI270-Sensor-API/bmi2.h | 1513 +++ .../BoschSensortec/BMI270-Sensor-API/bmi270.c | 4451 +++++++ .../BoschSensortec/BMI270-Sensor-API/bmi270.h | 422 + .../BMI270-Sensor-API/bmi270_context.c | 3122 +++++ .../BMI270-Sensor-API/bmi270_context.h | 493 + .../BMI270-Sensor-API/bmi270_hc.c | 3165 +++++ .../BMI270-Sensor-API/bmi270_hc.h | 483 + .../BMI270-Sensor-API/bmi270_maximum_fifo.c | 212 + .../BMI270-Sensor-API/bmi270_maximum_fifo.h | 117 + .../BMI270-Sensor-API/bmi270_wh.c | 3431 +++++ .../BMI270-Sensor-API/bmi270_wh.h | 402 + .../BMI270-Sensor-API/bmi2_defs.h | 2475 ++++ .../BMI270-Sensor-API/bmi2_ois.c | 417 + .../BMI270-Sensor-API/bmi2_ois.h | 387 + .../examples/bmi270/accel/Makefile | 18 + .../examples/bmi270/accel/accel.c | 200 + .../examples/bmi270/accel_gyro/Makefile | 18 + .../examples/bmi270/accel_gyro/accel_gyro.c | 268 + .../bmi270/any_motion_interrupt/Makefile | 18 + .../any_motion_interrupt.c | 135 + .../examples/bmi270/common/common.c | 372 + .../examples/bmi270/common/common.h | 122 + .../examples/bmi270/component_retrim/Makefile | 18 + .../component_retrim/component_retrim.c | 52 + .../bmi270/fifo_full_header_mode/Makefile | 18 + .../fifo_full_header_mode.c | 315 + .../bmi270/fifo_full_headerless_mode/Makefile | 18 + .../fifo_full_headerless_mode.c | 304 + .../fifo_watermark_header_mode/Makefile | 18 + .../fifo_watermark_header_mode.c | 335 + .../fifo_watermark_headerless_mode/Makefile | 18 + .../fifo_watermark_headerless_mode.c | 331 + .../examples/bmi270/gyro/Makefile | 18 + .../examples/bmi270/gyro/gyro.c | 195 + .../bmi270/no_motion_interrupt/Makefile | 18 + .../no_motion_interrupt/no_motion_interrupt.c | 134 + .../bmi270/read_aux_data_mode/Makefile | 23 + .../read_aux_data_mode/read_aux_data_mode.c | 327 + .../bmi270/read_aux_manual_mode/Makefile | 23 + .../read_aux_manual_mode.c | 322 + .../examples/bmi270/sig_motion/Makefile | 18 + .../examples/bmi270/sig_motion/sig_motion.c | 93 + .../examples/bmi270/step_activity/Makefile | 18 + .../bmi270/step_activity/step_activity.c | 110 + .../examples/bmi270/step_counter/Makefile | 18 + .../bmi270/step_counter/step_counter.c | 146 + .../examples/bmi270/wrist_gesture/Makefile | 18 + .../bmi270/wrist_gesture/wrist_gesture.c | 147 + .../bmi270/wrist_wear_wakeup/Makefile | 18 + .../wrist_wear_wakeup/wrist_wear_wakeup.c | 131 + .../examples/bmi270_context/accel/Makefile | 18 + .../examples/bmi270_context/accel/accel.c | 200 + .../bmi270_context/accel_gyro/Makefile | 18 + .../bmi270_context/accel_gyro/accel_gyro.c | 268 + .../activity_recognition/Makefile | 18 + .../activity_recognition.c | 154 + .../examples/bmi270_context/common/common.c | 372 + .../examples/bmi270_context/common/common.h | 122 + .../bmi270_context/component_retrim/Makefile | 18 + .../component_retrim/component_retrim.c | 52 + .../fifo_full_header_mode/Makefile | 18 + .../fifo_full_header_mode.c | 315 + .../fifo_full_headerless_mode/Makefile | 18 + .../fifo_full_headerless_mode.c | 304 + .../fifo_watermark_header_mode/Makefile | 18 + .../fifo_watermark_header_mode.c | 335 + .../fifo_watermark_headerless_mode/Makefile | 18 + .../fifo_watermark_headerless_mode.c | 331 + .../examples/bmi270_context/gyro/Makefile | 18 + .../examples/bmi270_context/gyro/gyro.c | 195 + .../bmi270_context/step_counter/Makefile | 18 + .../step_counter/step_counter.c | 146 + .../examples/bmi270_hc/accel/Makefile | 18 + .../examples/bmi270_hc/accel/accel.c | 201 + .../examples/bmi270_hc/accel_gyro/Makefile | 18 + .../bmi270_hc/accel_gyro/accel_gyro.c | 268 + .../bmi270_hc/activity_recognition/Makefile | 18 + .../activity_recognition.c | 154 + .../examples/bmi270_hc/common/common.c | 372 + .../examples/bmi270_hc/common/common.h | 122 + .../bmi270_hc/component_retrim/Makefile | 18 + .../component_retrim/component_retrim.c | 52 + .../bmi270_hc/fifo_full_header_mode/Makefile | 18 + .../fifo_full_header_mode.c | 316 + .../fifo_full_headerless_mode/Makefile | 18 + .../fifo_full_headerless_mode.c | 304 + .../fifo_watermark_header_mode/Makefile | 18 + .../fifo_watermark_header_mode.c | 335 + .../fifo_watermark_headerless_mode/Makefile | 18 + .../fifo_watermark_headerless_mode.c | 331 + .../examples/bmi270_hc/gyro/Makefile | 18 + .../examples/bmi270_hc/gyro/gyro.c | 195 + .../examples/bmi270_hc/step_counter/Makefile | 18 + .../bmi270_hc/step_counter/step_counter.c | 146 + .../bmi270_maximum_fifo/accel/Makefile | 18 + .../bmi270_maximum_fifo/accel/accel.c | 200 + .../bmi270_maximum_fifo/accel_gyro/Makefile | 18 + .../accel_gyro/accel_gyro.c | 268 + .../bmi270_maximum_fifo/common/common.c | 372 + .../bmi270_maximum_fifo/common/common.h | 122 + .../component_retrim/Makefile | 18 + .../component_retrim/component_retrim.c | 52 + .../fifo_watermark_header_mode/Makefile | 18 + .../fifo_watermark_header_mode.c | 335 + .../fifo_watermark_headerless_mode/Makefile | 18 + .../fifo_watermark_headerless_mode.c | 331 + .../bmi270_maximum_fifo/gyro/Makefile | 18 + .../examples/bmi270_maximum_fifo/gyro/gyro.c | 195 + .../examples/bmi270_wh/accel/Makefile | 18 + .../examples/bmi270_wh/accel/accel.c | 201 + .../examples/bmi270_wh/accel_gyro/Makefile | 18 + .../bmi270_wh/accel_gyro/accel_gyro.c | 268 + .../bmi270_wh/any_motion_interrupt/Makefile | 18 + .../any_motion_interrupt.c | 134 + .../examples/bmi270_wh/common/common.c | 372 + .../examples/bmi270_wh/common/common.h | 122 + .../bmi270_wh/fifo_full_header_mode/Makefile | 18 + .../fifo_full_header_mode.c | 316 + .../fifo_full_headerless_mode/Makefile | 18 + .../fifo_full_headerless_mode.c | 304 + .../fifo_watermark_header_mode/Makefile | 18 + .../fifo_watermark_header_mode.c | 335 + .../fifo_watermark_headerless_mode/Makefile | 18 + .../fifo_watermark_headerless_mode.c | 331 + .../examples/bmi270_wh/gyro/Makefile | 18 + .../examples/bmi270_wh/gyro/gyro.c | 195 + .../bmi270_wh/no_motion_interrupt/Makefile | 18 + .../no_motion_interrupt/no_motion_interrupt.c | 135 + .../bmi270_wh/read_aux_data_mode/Makefile | 23 + .../read_aux_data_mode/read_aux_data_mode.c | 327 + .../bmi270_wh/read_aux_manual_mode/Makefile | 23 + .../read_aux_manual_mode.c | 322 + .../examples/bmi270_wh/step_activity/Makefile | 18 + .../bmi270_wh/step_activity/step_activity.c | 110 + .../examples/bmi270_wh/step_counter/Makefile | 18 + .../bmi270_wh/step_counter/step_counter.c | 146 + .../bmi270_wh/wrist_wear_wakeup/Makefile | 18 + .../wrist_wear_wakeup/wrist_wear_wakeup.c | 131 + .../examples/bmi2_ois/common/common.c | 153 + .../examples/bmi2_ois/common/common.h | 92 + .../examples/bmi2_ois/ois_accel_gyro/Makefile | 18 + .../bmi2_ois/ois_accel_gyro/ois_accel_gyro.c | 118 + src/main/drivers/accgyro/accgyro_mpu.c | 14 + src/main/drivers/accgyro/accgyro_spi_bmi270.c | 583 + src/main/drivers/accgyro/accgyro_spi_bmi270.h | 28 + src/main/interface/settings.c | 4 +- src/main/pg/bus_spi.c | 3 + src/main/sensors/acceleration.c | 12 + src/main/sensors/acceleration.h | 1 + src/main/sensors/gyro.c | 15 +- src/main/sensors/gyro.h | 1 + .../target/IFRC_IFLIGHT_F745_AIO_V2/target.c | 44 + .../target/IFRC_IFLIGHT_F745_AIO_V2/target.h | 184 + .../target/IFRC_IFLIGHT_F745_AIO_V2/target.mk | 15 + 159 files changed, 49232 insertions(+), 3 deletions(-) create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/LICENSE create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/OIS_README.md create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/README.md create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/betaflight_info.txt create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/bmi2.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/bmi2.h create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/bmi270.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/bmi270.h create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_context.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_context.h create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_hc.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_hc.h create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_maximum_fifo.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_maximum_fifo.h create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_wh.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_wh.h create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/bmi2_defs.h create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/bmi2_ois.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/bmi2_ois.h create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/accel/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/accel/accel.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/accel_gyro/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/accel_gyro/accel_gyro.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/any_motion_interrupt/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/any_motion_interrupt/any_motion_interrupt.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/common/common.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/common/common.h create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/component_retrim/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/component_retrim/component_retrim.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_full_header_mode/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_full_header_mode/fifo_full_header_mode.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_full_headerless_mode/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_full_headerless_mode/fifo_full_headerless_mode.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_watermark_header_mode/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_watermark_header_mode/fifo_watermark_header_mode.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_watermark_headerless_mode/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/gyro/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/gyro/gyro.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/no_motion_interrupt/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/no_motion_interrupt/no_motion_interrupt.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/read_aux_data_mode/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/read_aux_data_mode/read_aux_data_mode.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/read_aux_manual_mode/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/read_aux_manual_mode/read_aux_manual_mode.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/sig_motion/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/sig_motion/sig_motion.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/step_activity/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/step_activity/step_activity.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/step_counter/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/step_counter/step_counter.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/wrist_gesture/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/wrist_gesture/wrist_gesture.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/wrist_wear_wakeup/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/wrist_wear_wakeup/wrist_wear_wakeup.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/accel/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/accel/accel.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/accel_gyro/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/accel_gyro/accel_gyro.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/activity_recognition/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/activity_recognition/activity_recognition.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/common/common.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/common/common.h create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/component_retrim/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/component_retrim/component_retrim.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_full_header_mode/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_full_header_mode/fifo_full_header_mode.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_full_headerless_mode/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_full_headerless_mode/fifo_full_headerless_mode.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_watermark_header_mode/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_watermark_header_mode/fifo_watermark_header_mode.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_watermark_headerless_mode/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/gyro/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/gyro/gyro.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/step_counter/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/step_counter/step_counter.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/accel/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/accel/accel.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/accel_gyro/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/accel_gyro/accel_gyro.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/activity_recognition/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/activity_recognition/activity_recognition.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/common/common.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/common/common.h create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/component_retrim/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/component_retrim/component_retrim.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_full_header_mode/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_full_header_mode/fifo_full_header_mode.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_full_headerless_mode/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_full_headerless_mode/fifo_full_headerless_mode.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_watermark_header_mode/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_watermark_header_mode/fifo_watermark_header_mode.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_watermark_headerless_mode/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/gyro/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/gyro/gyro.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/step_counter/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/step_counter/step_counter.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/accel/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/accel/accel.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/accel_gyro/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/accel_gyro/accel_gyro.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/common/common.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/common/common.h create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/component_retrim/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/component_retrim/component_retrim.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/fifo_watermark_header_mode/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/fifo_watermark_header_mode/fifo_watermark_header_mode.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/fifo_watermark_headerless_mode/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/gyro/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/gyro/gyro.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/accel/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/accel/accel.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/accel_gyro/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/accel_gyro/accel_gyro.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/any_motion_interrupt/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/any_motion_interrupt/any_motion_interrupt.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/common/common.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/common/common.h create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_full_header_mode/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_full_header_mode/fifo_full_header_mode.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_full_headerless_mode/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_full_headerless_mode/fifo_full_headerless_mode.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_watermark_header_mode/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_watermark_header_mode/fifo_watermark_header_mode.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_watermark_headerless_mode/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/gyro/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/gyro/gyro.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/no_motion_interrupt/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/no_motion_interrupt/no_motion_interrupt.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/read_aux_data_mode/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/read_aux_data_mode/read_aux_data_mode.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/read_aux_manual_mode/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/read_aux_manual_mode/read_aux_manual_mode.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/step_activity/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/step_activity/step_activity.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/step_counter/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/step_counter/step_counter.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/wrist_wear_wakeup/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/wrist_wear_wakeup/wrist_wear_wakeup.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi2_ois/common/common.c create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi2_ois/common/common.h create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi2_ois/ois_accel_gyro/Makefile create mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi2_ois/ois_accel_gyro/ois_accel_gyro.c create mode 100644 src/main/drivers/accgyro/accgyro_spi_bmi270.c create mode 100644 src/main/drivers/accgyro/accgyro_spi_bmi270.h create mode 100644 src/main/target/IFRC_IFLIGHT_F745_AIO_V2/target.c create mode 100644 src/main/target/IFRC_IFLIGHT_F745_AIO_V2/target.h create mode 100644 src/main/target/IFRC_IFLIGHT_F745_AIO_V2/target.mk diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/LICENSE b/lib/main/BoschSensortec/BMI270-Sensor-API/LICENSE new file mode 100644 index 0000000000..315dfa2309 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/LICENSE @@ -0,0 +1,30 @@ +Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + +BSD-3-Clause + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + +3. Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, +STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING +IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +POSSIBILITY OF SUCH DAMAGE. \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/OIS_README.md b/lib/main/BoschSensortec/BMI270-Sensor-API/OIS_README.md new file mode 100644 index 0000000000..2983d0699f --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/OIS_README.md @@ -0,0 +1,170 @@ +# Sensor API for the BMI2's OIS interface + +## Table of Contents + - [Introduction](#Intro) + - [Integration details](#Integration) + - [Driver files information](#file) + - [Sensor interfaces](#interface) + - [Integration Examples](#examples) + +### Introduction + This package contains Bosch Sensortec's BMI2 Sensor API. + +### Integration details +- Integrate _bmi2.c_, _bmi2.h_, _bmi2_ois.c_, _bmi2_ois.h_, _bmi2_defs.h_ and the required variant files in your project. +- User has to include _bmi2_ois.h_ in the code to call OIS related APIs and a _variant header_ for initialization as +well as BMI2 related API calls, as shown below: +``` c +#include "bmi270.h" +#include "bmi2_ois.h" +```` +### Driver files information +- *_bmi2_ois.c_* + * This file has function definitions of OIS related API interfaces. +- *_bmi2_ois.h_* + * This header file has necessary include files, function declarations, required to make OIS related API calls. + +### Sensor interfaces +#### _Host Interface_ +- I2C interface +- SPI interface +_Note: By default, the interface is I2C._ + +#### _OIS Interface_ +- SPI interface + +### Integration examples +#### Configuring SPI/I2C for host interface. +To configure host interface, an instance of the bmi2_dev structure should be +created for initializing BMI2 sensor. "_Refer **README** for initializing BMI2 +sensor._" + +#### Configuring SPI for OIS interface. +To configure OIS interface, an instance of the bmi2_ois_dev structure should be +created. The following parameters are required to be updated in the structure, +by the user. + +Parameters | Details +--------------|----------------------------------- +_intf_ptr_ | device address reference of SPI interface +_ois_read_ | read through SPI interface +_ois_write_ | read through SPI interface +_ois_delay_us_| delay in micro seconds +_acc_en_ | for enabling accelerometer +_gyr_en_ | for enabling gyroscope + +``` c +int8_t rslt = 0; + +struct bmi2_ois_dev ois_dev = { + .intf_ptr = intf_ptr will contain the chip selection info of SPI CS pin, + .ois_read = user_spi_reg_read, + .ois_write = user_spi_reg_write, + .ois_delay_us = user_delay_us +}; +``` +>**_Important Note_**: For initializing and configuring BMI2 sensors, which is +done through host interface, the API's are to be used from bmi2.c file. Rest +of the API's, for OIS configurations and the reading of OIS data, which is done +through OIS interface, are to be used from bmi2_ois.c file. + +##### Get accelerometer and gyroscope data through OIS interface +``` c +int8_t rslt; +/* Array to enable sensor through host interface */ +uint8_t sens_list[2] = {BMI2_ACCEL, BMI2_GYRO}; +/* Array to enable sensor through OIS interface */ +uint8_t sens_sel[2] = {BMI2_OIS_ACCEL, BMI2_OIS_GYRO}; +/* Initialize the configuration structure */ +struct bmi2_sens_config sens_cfg = {0}; + +/* Initialize BMI2 */ +rslt = bmi2_init(&dev); +if (rslt != BMI2_OK) { + printf("Error: %d\n", rslt); + return; +} + +/* Enable accelerometer and gyroscope through host interface */ +rslt = bmi270_sensor_enable(sens_list, 2, &dev); +if (rslt != BMI2_OK) { + printf("Error: %d\n", rslt); + return; +} + +/* Setting of OIS Range is done through host interface */ +/* Select the gyroscope sensor for OIS Range configuration */ +sens_cfg.type = BMI2_GYRO; + +/* Get gyroscope configuration */ +rslt = bmi2_get_sensor_config(&sens_cfg, 1, &dev); +if (rslt != BMI2_OK) { + printf("Error: %d\n", rslt); + return; +} + +/* Set the desired OIS Range */ +sens_cfg.cfg.gyr.ois_range = BMI2_GYR_OIS_2000; + +/* Set gyroscope configuration for default values */ +rslt = bmi2_set_sensor_config(&sens_cfg, 1, &dev); +if (rslt != BMI2_OK) { + printf("Error: %d\n", rslt); + return; +} + +/* Enable OIS through host interface */ +rslt = bmi2_set_ois_interface(BMI2_ENABLE, &dev); +if (rslt != BMI2_OK) { + printf("Error: %d\n", rslt); + return; +} + +/* Disable Advance Power Save Mode through host interface */ +rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &dev); +if (rslt != BMI2_OK) { + printf("Error: %d\n", rslt); + return; +} + +/* Get configurations for OIS through OIS interface for default values */ +rslt = bmi2_ois_get_config(&ois_dev); +if (rslt != BMI2_OK) { + printf("Error: %d\n", rslt); + return; +} + +/* Enable accelerometer and gyroscope for reading OIS data */ +ois_dev.acc_en = BMI2_ENABLE; +ois_dev.gyr_en = BMI2_ENABLE; + +/* Set configurations for OIS through OIS interface */ +rslt = bmi2_ois_set_config(&ois_dev); +if (rslt == BMI2_OK) { + /* Get OIS accelerometer and gyroscope data through OIS interface */ + rslt = bmi2_ois_read_data(sens_sel, 2, &ois_dev); + if (rslt == BMI2_OK) { + /* Print accelerometer data */ + printf("OIS Accel x-axis = %d\t", ois_dev.acc_data.x); + printf("OIS Accel y-axis= %d\t", ois_dev.acc_data.y); + printf("OIS Accel z-axis = %d\r\n", ois_dev.acc_data.z); + + /* Print gyroscope data */ + printf("OIS Gyro x-axis = %d\t", ois_dev.gyr_data.x); + printf("OIS Gyro y-axis= %d\t", ois_dev.gyr_data.y); + printf("OIS Gyro z-axis = %d\r\n", ois_dev.gyr_data.z); + } +} + +if (rslt != BMI2_OK) { + printf("Error code: %d\n", rslt); + return; +} + +/* Enable Advance Power Save Mode through host interface */ +rslt = bmi2_set_adv_power_save(BMI2_ENABLE, &dev); +if (rslt != BMI2_OK) { + printf("Error: %d\n", rslt); + return; +} +``` \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/README.md b/lib/main/BoschSensortec/BMI270-Sensor-API/README.md new file mode 100644 index 0000000000..193b038210 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/README.md @@ -0,0 +1,14 @@ +BMI2xy Sensor API + +> This package contains BMI2xy sensor API + +## Sensor Overview +The BMI2xy is a small, low power, low noise inertial measurement unit designed for use in mobile applications like augmented reality or indoor navigation which require highly accurate, real-time sensor data. + +### Features + +- Indoor navigation, pedestrian dead-reckoning, step-counting + +For more information refer product page [Link](https://www.bosch-sensortec.com/products/motion-sensors/imus/bmi270.html) + +--- \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/betaflight_info.txt b/lib/main/BoschSensortec/BMI270-Sensor-API/betaflight_info.txt new file mode 100644 index 0000000000..9f8947c9d0 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/betaflight_info.txt @@ -0,0 +1,9 @@ +This library is used to support the Bosch BMI270 gyro sensor (see drivers/accgyro/accgyro_spi_bmi270.c). + +Library download location: + https://github.com/BoschSensortec/BMI270-Sensor-API + +Version: + 2.63.1 + +The only file that is compiled as part of Betaflight is bmi270_maximum_fifo.c. This file contains the device microcode that must be uploaded during initialization. diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2.c b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2.c new file mode 100644 index 0000000000..556039a1a4 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2.c @@ -0,0 +1,10338 @@ +/** +* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. +* +* BSD-3-Clause +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* +* 3. Neither the name of the copyright holder nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, +* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING +* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +* @file bmi2.c +* @date 2020-11-04 +* @version v2.63.1 +* +*/ + +/******************************************************************************/ + +/*! @name Header Files */ +/******************************************************************************/ +#include "bmi2.h" + +/***************************************************************************/ + +/*! Local structures + ****************************************************************************/ + +/*! @name Structure to define the difference in accelerometer values */ +struct bmi2_selftest_delta_limit +{ + /*! X data */ + int32_t x; + + /*! Y data */ + int32_t y; + + /*! Z data */ + int32_t z; +}; + +/*! @name Structure to store temporary accelerometer/gyroscope values */ +struct bmi2_foc_temp_value +{ + /*! X data */ + int32_t x; + + /*! Y data */ + int32_t y; + + /*! Z data */ + int32_t z; +}; + +/*! @name Structure to store accelerometer data deviation from ideal value */ +struct bmi2_offset_delta +{ + /*! X axis */ + int16_t x; + + /*! Y axis */ + int16_t y; + + /*! Z axis */ + int16_t z; +}; + +/*! @name Structure to store accelerometer offset values */ +struct bmi2_accel_offset +{ + /*! offset X data */ + uint8_t x; + + /*! offset Y data */ + uint8_t y; + + /*! offset Z data */ + uint8_t z; +}; + +/******************************************************************************/ + +/*! Local Function Prototypes + ******************************************************************************/ + +/*! + * @brief This internal API writes the configuration file. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t write_config_file(struct bmi2_dev *dev); + +/*! + * @brief This internal API enables/disables the loading of the configuration + * file. + * + * @param[in] enable : To enable/disable configuration load. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_config_load(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API loads the configuration file. + * + * @param[in] config_data : Pointer to the configuration file. + * @param[in] index : Variable to define array index. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t upload_file(const uint8_t *config_data, uint16_t index, uint16_t write_len, struct bmi2_dev *dev); + +/*! + * @brief This internal API sets accelerometer configurations like ODR, + * bandwidth, performance mode and g-range. + * + * @param[in,out] config : Structure instance of bmi2_accel_config. + * @param[in,out] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_accel_config(struct bmi2_accel_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API validates bandwidth and performance mode of the + * accelerometer set by the user. + * + * @param[in, out] bandwidth : Pointer to bandwidth value set by the user. + * @param[in, out] perf_mode : Pointer to performance mode set by the user. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @note dev->info contains two warnings: BMI2_I_MIN_VALUE and BMI2_I_MAX_VALUE + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t validate_bw_perf_mode(uint8_t *bandwidth, uint8_t *perf_mode, struct bmi2_dev *dev); + +/*! + * @brief This internal API validates ODR and range of the accelerometer set by + * the user. + * + * @param[in, out] odr : Pointer to ODR value set by the user. + * @param[in, out] range : Pointer to range value set by the user. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @note dev->info contains two warnings: BMI2_I_MIN_VALUE and BMI2_I_MAX_VALUE + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t validate_odr_range(uint8_t *odr, uint8_t *range, struct bmi2_dev *dev); + +/*! + * @brief This internal API sets gyroscope configurations like ODR, bandwidth, + * low power/high performance mode, performance mode and range. + * + * @param[in,out] config : Structure instance of bmi2_gyro_config. + * @param[in,out] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_gyro_config(struct bmi2_gyro_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API validates bandwidth, performance mode, low power/ + * high performance mode, ODR, and range set by the user. + * + * @param[in] config : Structure instance of bmi2_gyro_config. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @note dev->info contains two warnings: BMI2_I_MIN_VALUE and BMI2_I_MAX_VALUE + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t validate_gyro_config(struct bmi2_gyro_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API shows the error status when illegal sensor + * configuration is set. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t cfg_error_status(struct bmi2_dev *dev); + +/*! + * @brief This internal API: + * 1) Enables/Disables auxiliary interface. + * 2) Sets auxiliary interface configurations like I2C address, manual/auto + * mode enable, manual burst read length, AUX burst read length and AUX read + * address. + * 3)It maps/un-maps data interrupts to that of hardware interrupt line. + * + * @param[in] config : Structure instance of bmi2_aux_config. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_aux_config(struct bmi2_aux_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API sets gyroscope user-gain configurations like gain + * update value for x, y and z-axis. + * + * @param[in] config : Structure instance of bmi2_gyro_user_gain_config. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @verbatim + *---------------------------------------------------------------------------- + * bmi2_gyro_user_gain_config| + * Structure parameters | Description + *--------------------------|-------------------------------------------------- + * ratio_x | Gain update value for x-axis + * -------------------------|--------------------------------------------------- + * ratio_y | Gain update value for y-axis + * -------------------------|--------------------------------------------------- + * ratio_z | Gain update value for z-axis + * @endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_gyro_user_gain_config(const struct bmi2_gyro_user_gain_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API enables/disables auxiliary interface. + * + * @param[in] config : Structure instance of bmi2_aux_config. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_aux_interface(const struct bmi2_aux_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API sets auxiliary configurations like manual/auto mode + * FCU write command enable and read burst length for both data and manual mode. + * + * @param[in] config : Structure instance of bmi2_aux_config. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @note Auxiliary sensor should not be busy when configuring aux_i2c_addr, + * man_rd_burst_len, aux_rd_burst_len and aux_rd_addr. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t config_aux_interface(const struct bmi2_aux_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API triggers read out offset and sets ODR of the + * auxiliary sensor. + * + * @param[in] config : Structure instance of bmi2_aux_config. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t config_aux(const struct bmi2_aux_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API validates auxiliary configuration set by the user. + * + * @param[in, out] config : Structure instance of bmi2_aux_config. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @note dev->info contains two warnings: BMI2_I_MIN_VALUE and BMI2_I_MAX_VALUE + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t validate_aux_config(struct bmi2_aux_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets accelerometer configurations like ODR, + * bandwidth, performance mode and g-range. + * + * @param[out] config : Structure instance of bmi2_accel_config. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_accel_config(struct bmi2_accel_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets gyroscope configurations like ODR, bandwidth, + * low power/ high performance mode, performance mode and range. + * + * @param[out] config : Structure instance of bmi2_gyro_config. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_gyro_config(struct bmi2_gyro_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API: + * 1) Gets the status of auxiliary interface enable. + * 2) Gets auxiliary interface configurations like I2C address, manual/auto + * mode enable, manual burst read length, AUX burst read length and AUX read + * address. + * 3) Gets ODR and offset. + * + * @param[out] config : Structure instance of bmi2_aux_config. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_aux_config(struct bmi2_aux_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets gyroscope user-gain configurations like gain + * update value for x, y and z-axis. + * + * @param[out] config : Structure instance of bmi2_gyro_user_gain_config. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @verbatim + *---------------------------------------------------------------------------- + * bmi2_gyro_user_gain_config| + * Structure parameters | Description + *-------------------------|-------------------------------------------------- + * ratio_x | Gain update value for x-axis + * ------------------------|--------------------------------------------------- + * ratio_y | Gain update value for y-axis + * ------------------------|--------------------------------------------------- + * ratio_z | Gain update value for z-axis + * + * @endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_gyro_gain_update_config(struct bmi2_gyro_user_gain_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets the enable status of auxiliary interface. + * + * @param[out] config : Structure instance of bmi2_aux_config. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_aux_interface(struct bmi2_aux_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets auxiliary configurations like manual/auto mode + * FCU write command enable and read burst length for both data and manual mode. + * + * @param[out] config : Structure instance of bmi2_aux_config. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_aux_interface_config(struct bmi2_aux_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets read out offset and ODR of the auxiliary + * sensor. + * + * @param[out] config : Structure instance of bmi2_aux_config. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_aux_cfg(struct bmi2_aux_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets the saturation status for the gyroscope user + * gain update. + * + * @param[out] user_gain_stat : Stores the saturation status of the axes. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_gyro_gain_update_status(struct bmi2_gyr_user_gain_status *user_gain, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to extract the output feature configuration + * details like page and start address from the look-up table. + * + * @param[out] feat_output : Structure that stores output feature + * configurations. + * @param[in] type : Type of feature or sensor. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Returns the feature found flag. + * + * @retval BMI2_FALSE : Feature not found + * BMI2_TRUE : Feature found + */ +static uint8_t extract_output_feat_config(struct bmi2_feature_config *feat_output, + uint8_t type, + const struct bmi2_dev *dev); + +/*! + * @brief This internal API gets the cross sensitivity coefficient between + * gyroscope's X and Z axes. + * + * @param[out] cross_sense : Pointer to the stored cross sensitivity + * coefficient. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_gyro_cross_sense(int16_t *cross_sense, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets the accelerometer data from the register. + * + * @param[out] data : Structure instance of sensor_data. + * @param[in] reg_addr : Register address where data is stored. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_accel_sensor_data(struct bmi2_sens_axes_data *data, uint8_t reg_addr, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets the gyroscope data from the register. + * + * @param[out] data : Structure instance of sensor_data. + * @param[in] reg_addr : Register address where data is stored. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_gyro_sensor_data(struct bmi2_sens_axes_data *data, uint8_t reg_addr, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets the accelerometer/gyroscope data. + * + * @param[out] data : Structure instance of sensor_data. + * @param[in] reg_data : Data stored in the register. + * + * @return None + * + * @retval None + */ +static void get_acc_gyr_data(struct bmi2_sens_axes_data *data, const uint8_t *reg_data); + +/*! + * @brief This internal API gets the re-mapped accelerometer/gyroscope data. + * + * @param[out] data : Structure instance of sensor_data. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return None + * + * @retval None + */ +static void get_remapped_data(struct bmi2_sens_axes_data *data, const struct bmi2_dev *dev); + +/*! + * @brief This internal API reads the user-defined bytes of data from the given + * register address of auxiliary sensor in data mode. + * + * @param[out] aux_data : Pointer to the stored auxiliary data. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t read_aux_data_mode(uint8_t *aux_data, struct bmi2_dev *dev); + +/*! + * @brief This internal API reads the user-defined bytes of data from the given + * register address of auxiliary sensor in manual mode. + * + * @param[in] reg_addr : AUX address from where data is read. + * @param[out] aux_data : Pointer to the stored auxiliary data. + * @param[in] len : Total bytes to be read. + * @param[in] burst_len : Bytes of data to be read in bursts. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t read_aux_data(uint8_t reg_addr, uint8_t *aux_data, uint16_t len, uint8_t burst_len, struct bmi2_dev *dev); + +/*! + * @brief This internal API checks the busy status of auxiliary sensor and sets + * the auxiliary register addresses when not busy. + * + * @param[in] reg_addr : Address in which AUX register address is + * set. + * @param[in] reg_data : Auxiliary register address to be set when AUX is + * not busy. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_if_aux_not_busy(uint8_t reg_addr, uint8_t reg_data, struct bmi2_dev *dev); + +/*! + * @brief his internal API maps the actual burst read length with that of the + * register value set by user. + * + * @param[out] len : Actual burst length. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t map_read_len(uint8_t *len, const struct bmi2_dev *dev); + +/*! + * @brief This internal API writes AUX write address and the user-defined bytes + * of data to the AUX sensor in manual mode. + * + * @note Change of BMI2_AUX_WR_ADDR is only allowed if AUX is not busy. + * + * @param[in] reg_addr : AUX address in which data is to be written. + * @param[in] reg_data : Data to be written + * @param[in] dev : Structure instance of bmi2_dev. + * + * @note Change of BMI2_AUX_WR_ADDR is only allowed if AUX is not busy. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t write_aux_data(uint8_t reg_addr, uint8_t reg_data, struct bmi2_dev *dev); + +/*! + * @brief This internal API maps/unmaps feature interrupts to that of interrupt + * pins. + * + * @param[in] int_pin : Interrupt pin selected. + * @param[in] feat_int : Type of feature interrupt to be mapped. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t map_feat_int(uint8_t *reg_data_array, enum bmi2_hw_int_pin int_pin, uint8_t int_mask); + +/*! + * @brief This internal API computes the number of bytes of accelerometer FIFO + * data which is to be parsed in header-less mode. + * + * @param[out] start_idx : The start index for parsing data. + * @param[out] len : Number of bytes to be parsed. + * @param[in] acc_count : Number of accelerometer frames to be read. + * @param[in] fifo : Structure instance of bmi2_fifo_frame. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t parse_fifo_accel_len(uint16_t *start_idx, + uint16_t *len, + const uint16_t *acc_count, + const struct bmi2_fifo_frame *fifo); + +/*! + * @brief This internal API is used to parse accelerometer data from the FIFO + * data in header mode. + * + * @param[out] acc : Structure instance of bmi2_sens_axes_data where + * the parsed accelerometer data bytes are stored. + * @param[in] accel_length : Number of accelerometer frames (x,y,z data). + * @param[in] fifo : Structure instance of bmi2_fifo_frame. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t extract_accel_header_mode(struct bmi2_sens_axes_data *acc, + uint16_t *accel_length, + struct bmi2_fifo_frame *fifo, + const struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to parse the accelerometer data from the + * FIFO data in both header and header-less mode. It updates the current data + * byte to be parsed. + * + * @param[in,out] acc : Structure instance of bmi2_sens_axes_data where + * where the parsed data bytes are stored. + * @param[in,out] idx : Index value of number of bytes parsed. + * @param[in,out] acc_idx : Index value of accelerometer data (x,y,z axes) + * frame to be parsed. + * @param[in] frame : Either data is enabled by user in header-less + * mode or header frame value in header mode. + * @param[in] fifo : Structure instance of bmi2_fifo_frame. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t unpack_accel_frame(struct bmi2_sens_axes_data *acc, + uint16_t *idx, + uint16_t *acc_idx, + uint8_t frame, + const struct bmi2_fifo_frame *fifo, + const struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to parse accelerometer data from the FIFO + * data. + * + * @param[out] acc : Structure instance of bmi2_sens_axes_data + * where the parsed data bytes are stored. + * @param[in] data_start_index : Index value of the accelerometer data bytes + * which is to be parsed from the FIFO data. + * @param[in] fifo : Structure instance of bmi2_fifo_frame. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return None + * @retval None + */ +static void unpack_accel_data(struct bmi2_sens_axes_data *acc, + uint16_t data_start_index, + const struct bmi2_fifo_frame *fifo, + const struct bmi2_dev *dev); + +/*! + * @brief This internal API computes the number of bytes of gyroscope FIFO data + * which is to be parsed in header-less mode. + * + * @param[out] start_idx : The start index for parsing data. + * @param[out] len : Number of bytes to be parsed. + * @param[in] gyr_count : Number of gyroscope frames to be read. + * @param[in] frame : Either data enabled by user in header-less + * mode or header frame value in header mode. + * @param[in] fifo : Structure instance of bmi2_fifo_frame. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t parse_fifo_gyro_len(uint16_t *start_idx, + uint16_t(*len), + const uint16_t *gyr_count, + const struct bmi2_fifo_frame *fifo); + +/*! + * @brief This internal API is used to parse the gyroscope data from the FIFO + * data in both header and header-less mode It updates the current data byte to + * be parsed. + * + * @param[in,out] gyr : Structure instance of bmi2_sens_axes_data. + * @param[in,out] idx : Index value of number of bytes parsed + * @param[in,out] gyr_idx : Index value of gyroscope data (x,y,z axes) + * frame to be parsed. + * @param[in] frame : Either data is enabled by user in header-less + * mode or header frame value in header mode. + * @param[in] fifo : Structure instance of bmi2_fifo_frame. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t unpack_gyro_frame(struct bmi2_sens_axes_data *gyr, + uint16_t *idx, + uint16_t *gyr_idx, + uint8_t frame, + const struct bmi2_fifo_frame *fifo, + const struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to parse gyroscope data from the FIFO data. + * + * @param[out] gyr : Structure instance of bmi2_sens_axes_data where + * the parsed gyroscope data bytes are stored. + * @param[in] data_start_index : Index value of the gyroscope data bytes + * which is to be parsed from the FIFO data. + * @param[in] fifo : Structure instance of bmi2_fifo_frame. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return None + * @retval None + */ +static void unpack_gyro_data(struct bmi2_sens_axes_data *gyr, + uint16_t data_start_index, + const struct bmi2_fifo_frame *fifo, + const struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to parse the gyroscope data from the + * FIFO data in header mode. + * + * @param[out] gyr : Structure instance of bmi2_sens_axes_data where + * the parsed gyroscope data bytes are stored. + * @param[in] gyro_length : Number of gyroscope frames (x,y,z data). + * @param[in] fifo : Structure instance of bmi2_fifo_frame. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t extract_gyro_header_mode(struct bmi2_sens_axes_data *gyr, + uint16_t *gyro_length, + struct bmi2_fifo_frame *fifo, + const struct bmi2_dev *dev); + +/*! + * @brief This API computes the number of bytes of auxiliary FIFO data + * which is to be parsed in header-less mode. + * + * @param[out] start_idx : The start index for parsing data. + * @param[out] len : Number of bytes to be parsed. + * @param[in] aux_count : Number of accelerometer frames to be read. + * @param[in] fifo : Structure instance of bmi2_fifo_frame. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t parse_fifo_aux_len(uint16_t *start_idx, + uint16_t(*len), + const uint16_t *aux_count, + const struct bmi2_fifo_frame *fifo); + +/*! + * @brief This API is used to parse auxiliary data from the FIFO data. + * + * @param[out] aux : Pointer to buffer where the parsed auxiliary data + * bytes are stored. + * @param[in] aux_length : Number of auxiliary frames (x,y,z data). + * @param[in] fifo : Structure instance of bmi2_fifo_frame. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t extract_aux_header_mode(struct bmi2_aux_fifo_data *aux, + uint16_t *aux_length, + struct bmi2_fifo_frame *fifo, + const struct bmi2_dev *dev); + +/*! + * @brief This API is used to parse the auxiliary data from the FIFO data in + * both header and header-less mode. It updates the current data byte to be + * parsed. + * + * @param[out] aux : Pointer to structure where the parsed auxiliary data + * bytes are stored. + * @param[in,out] idx : Index value of number of bytes parsed + * @param[in,out] aux_idx : Index value of auxiliary data (x,y,z axes) + * frame to be parsed + * @param[in] frame : Either data is enabled by user in header-less + * mode or header frame value in header mode. + * @param[in] fifo : Structure instance of bmi2_fifo_frame. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t unpack_aux_frame(struct bmi2_aux_fifo_data *aux, + uint16_t *idx, + uint16_t *aux_idx, + uint8_t frame, + const struct bmi2_fifo_frame *fifo, + const struct bmi2_dev *dev); + +/*! + * @brief This API is used to parse auxiliary data from the FIFO data. + * + * @param[out] aux : Pointer to structure where the parsed + * auxiliary data bytes are stored. + * @param[in] data_start_index : Index value of the auxiliary data bytes which + * is to be parsed from the FIFO data. + * @param[in] fifo : Structure instance of bmi2_fifo_frame. + * + * @return None + * @retval None + */ +static void unpack_aux_data(struct bmi2_aux_fifo_data *aux, + uint16_t data_start_index, + const struct bmi2_fifo_frame *fifo); + +/*! + * @brief This internal API is used to reset the FIFO related configurations + * in the FIFO frame structure for the next FIFO read. + * + * @param[in, out] fifo : Structure instance of bmi2_fifo_frame. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return None + * @retval None + */ +static void reset_fifo_frame_structure(struct bmi2_fifo_frame *fifo, const struct bmi2_dev *dev); + +/*! + * @brief This internal API checks whether the FIFO data read is an empty frame. + * If empty frame, index is moved to the last byte. + * + * @param[in,out] data_index : The index of the current data to be parsed from + * FIFO data. + * @param[in] fifo : Structure instance of bmi2_fifo_frame. + * + * @return Result of API execution status + * + * @retval BMI2_OK - Success. + * @retval BMI2_W_FIFO_EMPTY - Warning : FIFO is empty + * @retval BMI2_W_PARTIAL_READ - Warning : There are more frames to be read + */ +static int8_t check_empty_fifo(uint16_t *data_index, const struct bmi2_fifo_frame *fifo); + +/*! + * @brief This internal API is used to move the data index ahead of the + * current frame length parameter when unnecessary FIFO data appears while + * extracting the user specified data. + * + * @param[in,out] data_index : Index of the FIFO data which is to be + * moved ahead of the current frame length + * @param[in] current_frame_length : Number of bytes in the current frame. + * @param[in] fifo : Structure instance of bmi2_fifo_frame. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t move_next_frame(uint16_t *data_index, uint8_t current_frame_length, const struct bmi2_fifo_frame *fifo); + +/*! + * @brief This internal API is used to parse and store the sensor time from the + * FIFO data. + * + * @param[in,out] data_index : Index of the FIFO data which has the sensor time. + * @param[in] fifo : Structure instance of bmi2_fifo_frame. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t unpack_sensortime_frame(uint16_t *data_index, struct bmi2_fifo_frame *fifo); + +/*! + * @brief This internal API is used to parse and store the skipped frame count + * from the FIFO data. + * + * @param[in,out] data_index : Index of the FIFO data which contains skipped + * frame count. + * @param[in] fifo : Structure instance of bmi2_fifo_frame. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t unpack_skipped_frame(uint16_t *data_index, struct bmi2_fifo_frame *fifo); + +/*! + * @brief This internal API enables and configures the accelerometer which is + * needed for self-test operation. It also sets the amplitude for the self-test. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t pre_self_test_config(struct bmi2_dev *dev); + +/*! + * @brief This internal API performs the steps needed for self-test operation + * before reading the accelerometer self-test data. + * + * @param[in] sign : Selects sign of self-test excitation + * @param[in] dev : Structure instance of bmi2_dev. + * + * sign | Description + * -------------|--------------- + * BMI2_ENABLE | positive excitation + * BMI2_DISABLE | negative excitation + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t self_test_config(uint8_t sign, struct bmi2_dev *dev); + +/*! + * @brief This internal API enables or disables the accelerometer self-test + * feature in the sensor. + * + * @param[in] enable : Enables/ Disables self-test. + * @param[in] dev : Structure instance of bmi2_dev. + * + * sign | Description + * -------------|--------------- + * BMI2_ENABLE | Enables self-test + * BMI2_DISABLE | Disables self-test + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_accel_self_test_enable(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API selects the sign for accelerometer self-test + * excitation. + * + * @param[in] sign : Selects sign of self-test excitation + * @param[in] dev : Structure instance of bmi2_dev. + * + * sign | Description + * -------------|--------------- + * BMI2_ENABLE | positive excitation + * BMI2_DISABLE | negative excitation + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_acc_self_test_sign(uint8_t sign, struct bmi2_dev *dev); + +/*! + * @brief This internal API sets the amplitude of the accelerometer self-test + * deflection in the sensor. + * + * @param[in] amp : Select amplitude of the self-test deflection. + * @param[in] dev : Structure instance of bmi2_dev. + * + * amp | Description + * -------------|--------------- + * BMI2_ENABLE | self-test amplitude is high + * BMI2_DISABLE | self-test amplitude is low + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_accel_self_test_amp(uint8_t amp, struct bmi2_dev *dev); + +/*! + * @brief This internal API reads the accelerometer data for x,y and z axis from + * the sensor. The data units is in LSB format. + * + * @param[out] accel : Buffer to store the acceleration value. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t read_accel_xyz(struct bmi2_sens_axes_data *accel, struct bmi2_dev *dev); + +/*! + * @brief This internal API converts LSB value of accelerometer axes to form + * 'g' to 'mg' for self-test. + * + * @param[in] acc_data_diff : Stores the acceleration value difference in g. + * @param[out]acc_data_diff_mg : Stores the acceleration value difference in mg. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return None + * @retval None + */ +static void convert_lsb_g(const struct bmi2_selftest_delta_limit *acc_data_diff, + struct bmi2_selftest_delta_limit *acc_data_diff_mg, + const struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to calculate the power of a value. + * + * @param[in] base : base for power calculation. + * @param[in] resolution : exponent for power calculation. + * + * @return the calculated power + * @retval the power value + */ +static int32_t power(int16_t base, uint8_t resolution); + +/*! + * @brief This internal API validates the accelerometer self-test data and + * decides the result of self-test operation. + * + * @param[in] accel_data_diff : Stores the acceleration value difference. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t validate_self_test(const struct bmi2_selftest_delta_limit *accel_data_diff); + +/*! + * @brief This internal API gets the re-mapped x, y and z axes from the sensor. + * + * @param[out] remap : Structure that stores local copy of re-mapped axes. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_remap_axes(struct bmi2_axes_remap *remap, struct bmi2_dev *dev); + +/*! + * @brief This internal API sets the re-mapped x, y and z axes in the sensor. + * + * @param[in] remap : Structure that stores local copy of re-mapped axes. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_remap_axes(const struct bmi2_axes_remap *remap, struct bmi2_dev *dev); + +/*! + * @brief Interface to get max burst length + * + * @param[in] max_burst_len : Pointer to store max burst length + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_maxburst_len(uint8_t *max_burst_len, struct bmi2_dev *dev); + +/*! + * @brief This internal API sets the max burst length. + * + * @param[in] write_len_byte : read & write length + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_maxburst_len(const uint16_t write_len_byte, struct bmi2_dev *dev); + +/*! + * @brief This internal API parses virtual frame header from the FIFO data. + * + * @param[in, out] frame_header : FIFO frame header. + * @param[in, out] data_index : Index value of the FIFO data bytes + * from which sensor frame header is to be parsed + * @param[in] fifo : Structure instance of bmi2_fifo_frame. + * + * @return None + * @retval None + */ +static void parse_if_virtual_header(uint8_t *frame_header, uint16_t *data_index, const struct bmi2_fifo_frame *fifo); + +/*! + * @brief This internal API gets sensor time from the accelerometer and + * gyroscope virtual frames and updates in the data structure. + * + * @param[out] sens : Sensor data structure + * @param[in, out] idx : Index of FIFO from where the data is to retrieved. + * @param[in] fifo : Structure instance of bmi2_fifo_frame. + * + * @return None + * @retval None + */ +static void unpack_virt_sensor_time(struct bmi2_sens_axes_data *sens, uint16_t *idx, + const struct bmi2_fifo_frame *fifo); + +/*! + * @brief This internal API gets sensor time from the auxiliary virtual + * frames and updates in the data structure. + * + * @param[out] aux : Auxiliary sensor data structure + * @param[in, out] idx : Index of FIFO from where the data is to retrieved. + * @param[in] fifo : Structure instance of bmi2_fifo_frame. + * + * @return None + * @retval None + */ +static void unpack_virt_aux_sensor_time(struct bmi2_aux_fifo_data *aux, + uint16_t *idx, + const struct bmi2_fifo_frame *fifo); + +/*! + * @brief This internal API corrects the gyroscope cross-axis sensitivity + * between the z and the x axis. + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[out] gyr_data : Structure instance of gyroscope data + * + * @return Result of API execution status + * + * @return None + * @retval None + */ +static void comp_gyro_cross_axis_sensitivity(struct bmi2_sens_axes_data *gyr_data, const struct bmi2_dev *dev); + +/*! + * @brief This internal API saves the configurations before performing FOC. + * + * @param[out] acc_cfg : Accelerometer configuration value + * @param[out] aps : Advance power mode value + * @param[out] acc_en : Accelerometer enable value + * @param[in] dev : Structure instance of bmi2_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t save_accel_foc_config(struct bmi2_accel_config *acc_cfg, + uint8_t *aps, + uint8_t *acc_en, + struct bmi2_dev *dev); + +/*! + * @brief This internal API performs Fast Offset Compensation for accelerometer. + * + * @param[in] accel_g_value : This parameter selects the accel foc + * axis to be performed + * + * input format is {x, y, z, sign}. '1' to enable. '0' to disable + * + * eg to choose x axis {1, 0, 0, 0} + * eg to choose -x axis {1, 0, 0, 1} + * + * @param[in] acc_cfg : Accelerometer configuration value + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t perform_accel_foc(const struct bmi2_accel_foc_g_value *accel_g_value, + const struct bmi2_accel_config *acc_cfg, + struct bmi2_dev *dev); + +/*! + * @brief This internal sets configurations for performing accelerometer FOC. + * + * @param[in] dev : Structure instance of bmi2_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_accel_foc_config(struct bmi2_dev *dev); + +/*! + * @brief This internal API enables/disables the offset compensation for + * filtered and un-filtered accelerometer data. + * + * @param[in] offset_en : enables/disables offset compensation. + * @param[in] dev : Structure instance of bmi2_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_accel_offset_comp(uint8_t offset_en, struct bmi2_dev *dev); + +/*! + * @brief This internal API converts the range value into accelerometer + * corresponding integer value. + * + * @param[in] range_in : Input range value. + * @param[out] range_out : Stores the integer value of range. + * + * @return None + * @retval None + */ +static void map_accel_range(uint8_t range_in, uint8_t *range_out); + +/*! + * @brief This internal API compensate the accelerometer data against gravity. + * + * @param[in] lsb_per_g : LSB value pre 1g. + * @param[in] g_val : G reference value of all axis. + * @param[in] data : Accelerometer data + * @param[out] comp_data : Stores the data that is compensated by taking the + * difference in accelerometer data and lsb_per_g + * value. + * + * @return None + * @retval None + */ +static void comp_for_gravity(uint16_t lsb_per_g, + const struct bmi2_accel_foc_g_value *g_val, + const struct bmi2_sens_axes_data *data, + struct bmi2_offset_delta *comp_data); + +/*! + * @brief This internal API scales the compensated accelerometer data according + * to the offset register resolution. + * + * @param[in] range : G-range of the accelerometer. + * @param[out] comp_data : Data that is compensated by taking the + * difference in accelerometer data and lsb_per_g + * value. + * @param[out] data : Stores offset data + * + * @return None + * @retval None + */ +static void scale_accel_offset(uint8_t range, const struct bmi2_offset_delta *comp_data, + struct bmi2_accel_offset *data); + +/*! + * @brief This internal API finds the bit position of 3.9mg according to given + * range and resolution. + * + * @param[in] range : G-range of the accelerometer. + * + * @return Result of API execution status + * @retval Bit position of 3.9mg + */ +static int8_t get_bit_pos_3_9mg(uint8_t range); + +/*! + * @brief This internal API inverts the accelerometer offset data. + * + * @param[out] offset_data : Stores the inverted offset data + * + * @return None + * @retval None + */ +static void invert_accel_offset(struct bmi2_accel_offset *offset_data); + +/*! + * @brief This internal API writes the offset data in the offset compensation + * register. + * + * @param[in] offset : offset data + * @param[in] dev : Structure instance of bmi2_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t write_accel_offset(const struct bmi2_accel_offset *offset, struct bmi2_dev *dev); + +/*! + * @brief This internal API restores the configurations saved before performing + * accelerometer FOC. + * + * @param[in] acc_cfg : Accelerometer configuration value + * @param[in] acc_en : Accelerometer enable value + * @param[in] aps : Advance power mode value + * @param[in] dev : Structure instance of bmi2_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t restore_accel_foc_config(struct bmi2_accel_config *acc_cfg, + uint8_t aps, + uint8_t acc_en, + struct bmi2_dev *dev); + +/*! + * @brief This internal API saves the configurations before performing gyroscope + * FOC. + * + * @param[out] gyr_cfg : Gyroscope configuration value + * @param[out] gyr_en : Gyroscope enable value + * @param[out] aps : Advance power mode value + * @param[in] dev : Structure instance of bmi2_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t save_gyro_config(struct bmi2_gyro_config *gyr_cfg, uint8_t *aps, uint8_t *gyr_en, struct bmi2_dev *dev); + +/*! + * @brief This internal sets configurations for performing gyroscope FOC. + * + * @param[in] dev : Structure instance of bmi2_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_gyro_foc_config(struct bmi2_dev *dev); + +/*! + * @brief This internal API inverts the gyroscope offset data. + * + * @param[out] offset_data : Stores the inverted offset data + * + * @return None + * @retval None + */ +static void invert_gyro_offset(struct bmi2_sens_axes_data *offset_data); + +/*! + * @brief This internal API restores the gyroscope configurations saved + * before performing FOC. + * + * @param[in] gyr_cfg : Gyroscope configuration value + * @param[in] gyr_en : Gyroscope enable value + * @param[in] aps : Advance power mode value + * @param[in] dev : Structure instance of bmi2_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t restore_gyro_config(struct bmi2_gyro_config *gyr_cfg, uint8_t aps, uint8_t gyr_en, struct bmi2_dev *dev); + +/*! + * @brief This internal API saturates the gyroscope data value before writing to + * to 10 bit offset register. + * + * @param[in, out] gyr_off : Gyroscope data to be stored in offset register + * + * @return None + * @retval None + */ +static void saturate_gyro_data(struct bmi2_sens_axes_data *gyr_off); + +/*! + * @brief This internal API reads the gyroscope data for x, y and z axis from + * the sensor. + * + * @param[out] gyro : Buffer to store the gyroscope value. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t read_gyro_xyz(struct bmi2_sens_axes_data *gyro, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to check the boundary conditions. + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[in,out] val : Pointer to the value to be validated. + * @param[in] min : minimum value. + * @param[in] max : maximum value. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t check_boundary_val(uint8_t *val, uint8_t min, uint8_t max, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to validate the device pointer for + * null conditions. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t null_ptr_check(const struct bmi2_dev *dev); + +/*! + * @brief This updates the result for CRT or gyro self-test. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t crt_gyro_st_update_result(struct bmi2_dev *dev); + +/*! + * @brief This function is to get the st_status status. + * + * @param[in] *st_status: gets the crt running status + * @param[in] dev : Structure instance of bmi2_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_st_running(uint8_t *st_status, struct bmi2_dev *dev); + +/*! + * @brief This function is to set crt bit to running. + * + * @param[in] enable + * @param[in] dev : Structure instance of bmi2_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_st_running(uint8_t st_status, struct bmi2_dev *dev); + +/*! + * @brief This function is to make the initial changes for CRT. + * Disable the gyro, OIS, aps + * Note: For the purpose of preparing CRT Gyro, OIS and APS are disabled + * + * @param[in] dev : Structure instance of bmi2_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t crt_prepare_setup(struct bmi2_dev *dev); + +static int8_t do_gtrigger_test(uint8_t gyro_st_crt, struct bmi2_dev *dev); + +/*! + * @brief This function is to get the rdy for dl bit status + * this will toggle from 0 to 1 and visevers according to the + * dowload status + * + * @param[in] *rdy_for_dl: gets the rdy_for_dl status + * @param[in] dev : Structure instance of bmi2_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_rdy_for_dl(uint8_t *rdy_for_dl, struct bmi2_dev *dev); + +/*! + * @brief This function is to write the config file in the given location for crt. + * which inter checks the status of the rdy_for_dl bit and also the crt running, and + * wirtes the given size. + * + * @param[in] write_len: length of the words to be written + * @param[in] config_file_size: length of the words to be written + * @param[in] start_index: provide the start index from where config file has to written + * @param[in] dev : Structure instance of bmi2_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t write_crt_config_file(uint16_t write_len, + uint16_t config_file_size, + uint16_t start_index, + struct bmi2_dev *dev); + +/*! + * @brief This function is to check for rdy_for_dl bit to toggle for CRT process + * + * @param[in] retry_complete: wait for given time to toggle + * @param[in] download_ready: get the status for rdy_for_dl + * @param[in] dev : Structure instance of bmi2_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t wait_rdy_for_dl_toggle(uint8_t retry_complete, uint8_t download_ready, struct bmi2_dev *dev); + +/*! + * @brief This function is to wait till the CRT or gyro self-test process is completed + * + * @param[in] retry_complete: wait for given time to complete the crt process + * @param[in] dev : Structure instance of bmi2_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t wait_st_running(uint8_t retry_complete, struct bmi2_dev *dev); + +/*! + * @brief This function is to complete the crt process if max burst length is not zero + * this checks for the crt status and rdy_for_dl bit to toggle + * + * @param[in] last_byte_flag: to provide the last toggled state + * @param[in] dev : Structure instance of bmi2_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t process_crt_download(uint8_t last_byte_flag, struct bmi2_dev *dev); + +/*! + * @brief This api is used to enable the gyro self-test or crt. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t select_self_test(uint8_t gyro_st_crt, struct bmi2_dev *dev); + +/*! + * @brief This api is used to enable/disable abort. + * + * @param[in] abort_enable : variable to enable the abort feature. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t abort_bmi2(uint8_t abort_enable, struct bmi2_dev *dev); + +/*! + * @brief This api is use to wait till gyro self-test is completed and update the status of gyro + * self-test. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t gyro_self_test_completed(struct bmi2_gyro_self_test_status *gyro_st_result, struct bmi2_dev *dev); + +/*! + * @brief This api is used to trigger the preparation for system for NVM programming. + * + * @param[out] nvm_prep : pointer to variable to store the status of nvm_prep_prog. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_nvm_prep_prog(uint8_t nvm_prep, struct bmi2_dev *dev); + +/*! + * @brief This api validates accel foc position as per the range + * + * @param[in] sens_list : Sensor type + * @param[in] accel_g_axis : accel axis to foc. NA for gyro foc + * @param[in] avg_foc_data : average value of sensor sample datas + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t validate_foc_position(uint8_t sens_list, + const struct bmi2_accel_foc_g_value *accel_g_axis, + struct bmi2_sens_axes_data avg_foc_data, + struct bmi2_dev *dev); + +/*! + * @brief This api validates accel foc axis given as input + * + * @param[in] avg_foc_data : average value of sensor sample datas + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t validate_foc_accel_axis(int16_t avg_foc_data, struct bmi2_dev *dev); + +/*! + * @brief This api is used to verify the right position of the sensor before doing accel foc + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[in] sens_list: Sensor type + * @param[in] accel_g_axis: Accel Foc axis and sign input + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t verify_foc_position(uint8_t sens_list, + const struct bmi2_accel_foc_g_value *accel_g_axis, + struct bmi2_dev *dev); + +/*! + * @brief This API reads and provides average for 128 samples of sensor data for foc operation + * gyro. + * + * @param[in] sens_list : Sensor type. + * @param[in] bmi2_dev: Structure instance of bmi2_dev. + * @param[in] temp_foc_data: to store data samples + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_average_of_sensor_data(uint8_t sens_list, + struct bmi2_foc_temp_value *temp_foc_data, + struct bmi2_dev *dev); + +/*! + * @brief This internal api gets major and minor version for config file + * + * @param[out] config_major : Pointer to store the major version + * @param[out] config_minor : Pointer to store the minor version + * @param[in] dev : Structure instance of bmi2_dev + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t extract_config_file(uint8_t *config_major, uint8_t *config_minor, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to map the interrupts to the sensor. + * + * @param[in] map_int : Structure instance of bmi2_map_int. + * @param[in] type : Type of feature or sensor. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return None + * @retval None + */ +static void extract_feat_int_map(struct bmi2_map_int *map_int, uint8_t type, const struct bmi2_dev *dev); + +/*! + * @brief This internal API selects the sensors/features to be enabled or + * disabled. + * + * @param[in] sens_list : Pointer to select the sensor. + * @param[in] n_sens : Number of sensors selected. + * @param[out] sensor_sel : Gets the selected sensor. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t select_sensor(const uint8_t *sens_list, uint8_t n_sens, uint64_t *sensor_sel); + +/*! + * @brief This internal API enables the selected sensor/features. + * + * @param[in] sensor_sel : Selects the desired sensor. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t sensor_enable(uint64_t sensor_sel, struct bmi2_dev *dev); + +/*! + * @brief This internal API disables the selected sensor/features. + * + * @param[in] sensor_sel : Selects the desired sensor. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t sensor_disable(uint64_t sensor_sel, struct bmi2_dev *dev); + +/******************************************************************************/ +/*! @name User Interface Definitions */ +/******************************************************************************/ + +/*! + * @brief This API is the entry point for bmi2 sensor. It selects between + * I2C/SPI interface, based on user selection. It reads and validates the + * chip-id of the sensor. + */ +int8_t bmi2_sec_init(struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to assign chip id */ + uint8_t chip_id = 0; + + /* Structure to define the default values for axes re-mapping */ + struct bmi2_axes_remap axes_remap = { + .x_axis = BMI2_MAP_X_AXIS, .x_axis_sign = BMI2_POS_SIGN, .y_axis = BMI2_MAP_Y_AXIS, + .y_axis_sign = BMI2_POS_SIGN, .z_axis = BMI2_MAP_Z_AXIS, .z_axis_sign = BMI2_POS_SIGN + }; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + /* Perform soft-reset to bring all register values to their + * default values + */ + rslt = bmi2_soft_reset(dev); + + if (rslt == BMI2_OK) + { + /* Read chip-id of the BMI2 sensor */ + rslt = bmi2_get_regs(BMI2_CHIP_ID_ADDR, &chip_id, 1, dev); + + if (rslt == BMI2_OK) + { + /* Validate chip-id */ + if (chip_id == dev->chip_id) + { + /* Assign resolution to the structure */ + dev->resolution = 16; + + /* Set manual enable flag */ + dev->aux_man_en = 1; + + /* Set the default values for axis + * re-mapping in the device structure + */ + dev->remap = axes_remap; + } + else + { + /* Storing the chip-id value read from + * the register to identify the sensor + */ + dev->chip_id = chip_id; + rslt = BMI2_E_DEV_NOT_FOUND; + } + } + } + } + + return rslt; +} + +/*! + * @brief This API reads the data from the given register address of bmi2 + * sensor. + * + * @note For most of the registers auto address increment applies, with the + * exception of a few special registers, which trap the address. For e.g., + * Register address - 0x26, 0x5E. + */ +int8_t bmi2_get_regs(uint8_t reg_addr, uint8_t *data, uint16_t len, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define loop */ + uint16_t index = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (data != NULL)) + { + /* Variable to define temporary length */ + uint16_t temp_len = len + dev->dummy_byte; + + /* Variable to define temporary buffer */ + uint8_t temp_buf[temp_len]; + + /* Configuring reg_addr for SPI Interface */ + if (dev->intf == BMI2_SPI_INTF) + { + reg_addr = (reg_addr | BMI2_SPI_RD_MASK); + } + + dev->intf_rslt = dev->read(reg_addr, temp_buf, temp_len, dev->intf_ptr); + + if (dev->aps_status == BMI2_ENABLE) + { + dev->delay_us(450, dev->intf_ptr); + } + else + { + dev->delay_us(2, dev->intf_ptr); + } + + if (dev->intf_rslt == BMI2_INTF_RET_SUCCESS) + { + /* Read the data from the position next to dummy byte */ + while (index < len) + { + data[index] = temp_buf[index + dev->dummy_byte]; + index++; + } + } + else + { + rslt = BMI2_E_COM_FAIL; + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API writes data to the given register address of bmi2 sensor. + */ +int8_t bmi2_set_regs(uint8_t reg_addr, const uint8_t *data, uint16_t len, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (data != NULL)) + { + /* Configuring reg_addr for SPI Interface */ + if (dev->intf == BMI2_SPI_INTF) + { + reg_addr = (reg_addr & BMI2_SPI_WR_MASK); + } + + dev->intf_rslt = dev->write(reg_addr, data, len, dev->intf_ptr); + + /* Delay for Low power mode of the sensor is 450 us */ + if (dev->aps_status == BMI2_ENABLE) + { + dev->delay_us(450, dev->intf_ptr); + } + /* Delay for Normal mode of the sensor is 2 us */ + else + { + dev->delay_us(2, dev->intf_ptr); + } + + /* updating the advance power saver flag */ + if (reg_addr == BMI2_PWR_CONF_ADDR) + { + if (*data & BMI2_ADV_POW_EN_MASK) + { + dev->aps_status = BMI2_ENABLE; + } + else + { + dev->aps_status = BMI2_DISABLE; + } + } + + if (dev->intf_rslt != BMI2_INTF_RET_SUCCESS) + { + rslt = BMI2_E_COM_FAIL; + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API resets bmi2 sensor. All registers are overwritten with + * their default values. + */ +int8_t bmi2_soft_reset(struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define soft reset value */ + uint8_t data = BMI2_SOFT_RESET_CMD; + + /* Variable to read the dummy byte */ + uint8_t dummy_read = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + /* Reset bmi2 device */ + rslt = bmi2_set_regs(BMI2_CMD_REG_ADDR, &data, 1, dev); + dev->delay_us(2000, dev->intf_ptr); + + /* set APS flag as after soft reset the sensor is on advance power save mode */ + dev->aps_status = BMI2_ENABLE; + + /* Performing a dummy read to bring interface back to SPI from + * I2C after a soft-reset + */ + if ((rslt == BMI2_OK) && (dev->intf == BMI2_SPI_INTF)) + { + rslt = bmi2_get_regs(BMI2_CHIP_ID_ADDR, &dummy_read, 1, dev); + } + + if (rslt == BMI2_OK) + { + /* Write the configuration file */ + rslt = bmi2_write_config_file(dev); + } + + /* Reset the sensor status flag in the device structure */ + if (rslt == BMI2_OK) + { + dev->sens_en_stat = 0; + } + } + + return rslt; +} + +/*! + * @brief This API is used to get the config file major and minor version information. + */ +int8_t bmi2_get_config_file_version(uint8_t *config_major, uint8_t *config_minor, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* NULL pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (config_major != NULL) && (config_minor != NULL)) + { + /* Extract the config file identification from the dmr page and get the major and minor version */ + rslt = extract_config_file(config_major, config_minor, dev); + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API enables/disables the advance power save mode in the sensor. + */ +int8_t bmi2_set_adv_power_save(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store data */ + uint8_t reg_data = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + rslt = bmi2_get_regs(BMI2_PWR_CONF_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_ADV_POW_EN, enable); + rslt = bmi2_set_regs(BMI2_PWR_CONF_ADDR, ®_data, 1, dev); + + if (rslt != BMI2_OK) + { + /* Return error if enable/disable APS fails */ + rslt = BMI2_E_SET_APS_FAIL; + } + + if (rslt == BMI2_OK) + { + dev->aps_status = BMI2_GET_BIT_POS0(reg_data, BMI2_ADV_POW_EN); + } + } + } + + return rslt; +} + +/*! + * @brief This API gets the status of advance power save mode in the sensor. + */ +int8_t bmi2_get_adv_power_save(uint8_t *aps_status, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store data */ + uint8_t reg_data = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (aps_status != NULL)) + { + rslt = bmi2_get_regs(BMI2_PWR_CONF_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + *aps_status = BMI2_GET_BIT_POS0(reg_data, BMI2_ADV_POW_EN); + dev->aps_status = *aps_status; + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API loads the configuration file into the bmi2 sensor. + */ +int8_t bmi2_write_config_file(struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to know the load status */ + uint8_t load_status = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (dev->config_size != 0)) + { + /* Bytes written are multiples of 2 */ + if ((dev->read_write_len % 2) != 0) + { + dev->read_write_len = dev->read_write_len - 1; + } + + if (dev->read_write_len < 2) + { + dev->read_write_len = 2; + } + + /* Write the configuration file */ + rslt = write_config_file(dev); + if (rslt == BMI2_OK) + { + /* Check the configuration load status */ + rslt = bmi2_get_internal_status(&load_status, dev); + + /* Return error if loading not successful */ + if ((rslt == BMI2_OK) && (!(load_status & BMI2_CONFIG_LOAD_SUCCESS))) + { + rslt = BMI2_E_CONFIG_LOAD; + } + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API sets: + * 1) The input output configuration of the selected interrupt pin: + * INT1 or INT2. + * 2) The interrupt mode: permanently latched or non-latched. + */ +int8_t bmi2_set_int_pin_config(const struct bmi2_int_pin_config *int_cfg, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define data array */ + uint8_t data_array[3] = { 0 }; + + /* Variable to store register data */ + uint8_t reg_data = 0; + + /* Variable to define type of interrupt pin */ + uint8_t int_pin = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (int_cfg != NULL)) + { + /* Copy the pin type to a local variable */ + int_pin = int_cfg->pin_type; + if ((int_pin > BMI2_INT_NONE) && (int_pin < BMI2_INT_PIN_MAX)) + { + /* Get the previous configuration data */ + rslt = bmi2_get_regs(BMI2_INT1_IO_CTRL_ADDR, data_array, 3, dev); + if (rslt == BMI2_OK) + { + /* Set interrupt pin 1 configuration */ + if ((int_pin == BMI2_INT1) || (int_pin == BMI2_INT_BOTH)) + { + /* Configure active low or high */ + reg_data = BMI2_SET_BITS(data_array[0], BMI2_INT_LEVEL, int_cfg->pin_cfg[0].lvl); + + /* Configure push-pull or open drain */ + reg_data = BMI2_SET_BITS(reg_data, BMI2_INT_OPEN_DRAIN, int_cfg->pin_cfg[0].od); + + /* Configure output enable */ + reg_data = BMI2_SET_BITS(reg_data, BMI2_INT_OUTPUT_EN, int_cfg->pin_cfg[0].output_en); + + /* Configure input enable */ + reg_data = BMI2_SET_BITS(reg_data, BMI2_INT_INPUT_EN, int_cfg->pin_cfg[0].input_en); + + /* Copy the data to be written in the respective array */ + data_array[0] = reg_data; + } + + /* Set interrupt pin 2 configuration */ + if ((int_pin == BMI2_INT2) || (int_pin == BMI2_INT_BOTH)) + { + /* Configure active low or high */ + reg_data = BMI2_SET_BITS(data_array[1], BMI2_INT_LEVEL, int_cfg->pin_cfg[1].lvl); + + /* Configure push-pull or open drain */ + reg_data = BMI2_SET_BITS(reg_data, BMI2_INT_OPEN_DRAIN, int_cfg->pin_cfg[1].od); + + /* Configure output enable */ + reg_data = BMI2_SET_BITS(reg_data, BMI2_INT_OUTPUT_EN, int_cfg->pin_cfg[1].output_en); + + /* Configure input enable */ + reg_data = BMI2_SET_BITS(reg_data, BMI2_INT_INPUT_EN, int_cfg->pin_cfg[1].input_en); + + /* Copy the data to be written in the respective array */ + data_array[1] = reg_data; + } + + /* Configure the interrupt mode */ + data_array[2] = BMI2_SET_BIT_POS0(data_array[2], BMI2_INT_LATCH, int_cfg->int_latch); + + /* Set the configurations simultaneously as + * INT1_IO_CTRL, INT2_IO_CTRL, and INT_LATCH lie + * in consecutive addresses + */ + rslt = bmi2_set_regs(BMI2_INT1_IO_CTRL_ADDR, data_array, 3, dev); + } + } + else + { + rslt = BMI2_E_INVALID_INT_PIN; + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API gets: + * 1) The input output configuration of the selected interrupt pin: + * INT1 or INT2. + * 2) The interrupt mode: permanently latched or non-latched. + */ +int8_t bmi2_get_int_pin_config(struct bmi2_int_pin_config *int_cfg, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define data array */ + uint8_t data_array[3] = { 0 }; + + /* Variable to define type of interrupt pin */ + uint8_t int_pin = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (int_cfg != NULL)) + { + /* Copy the pin type to a local variable */ + int_pin = int_cfg->pin_type; + + /* Get the previous configuration data */ + rslt = bmi2_get_regs(BMI2_INT1_IO_CTRL_ADDR, data_array, 3, dev); + if (rslt == BMI2_OK) + { + /* Get interrupt pin 1 configuration */ + if ((int_pin == BMI2_INT1) || (int_pin == BMI2_INT_BOTH)) + { + /* Get active low or high */ + int_cfg->pin_cfg[0].lvl = BMI2_GET_BITS(data_array[0], BMI2_INT_LEVEL); + + /* Get push-pull or open drain */ + int_cfg->pin_cfg[0].od = BMI2_GET_BITS(data_array[0], BMI2_INT_OPEN_DRAIN); + + /* Get output enable */ + int_cfg->pin_cfg[0].output_en = BMI2_GET_BITS(data_array[0], BMI2_INT_OUTPUT_EN); + + /* Get input enable */ + int_cfg->pin_cfg[0].input_en = BMI2_GET_BITS(data_array[0], BMI2_INT_INPUT_EN); + } + + /* Get interrupt pin 2 configuration */ + if ((int_pin == BMI2_INT2) || (int_pin == BMI2_INT_BOTH)) + { + /* Get active low or high */ + int_cfg->pin_cfg[1].lvl = BMI2_GET_BITS(data_array[1], BMI2_INT_LEVEL); + + /* Get push-pull or open drain */ + int_cfg->pin_cfg[1].od = BMI2_GET_BITS(data_array[1], BMI2_INT_OPEN_DRAIN); + + /* Get output enable */ + int_cfg->pin_cfg[1].output_en = BMI2_GET_BITS(data_array[1], BMI2_INT_OUTPUT_EN); + + /* Get input enable */ + int_cfg->pin_cfg[1].input_en = BMI2_GET_BITS(data_array[1], BMI2_INT_INPUT_EN); + } + + /* Get interrupt mode */ + int_cfg->int_latch = BMI2_GET_BIT_POS0(data_array[2], BMI2_INT_LATCH); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API gets the interrupt status of both feature and data + * interrupts + */ +int8_t bmi2_get_int_status(uint16_t *int_status, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to store data */ + uint8_t data_array[2] = { 0 }; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (int_status != NULL)) + { + /* Get the interrupt status */ + rslt = bmi2_get_regs(BMI2_INT_STATUS_0_ADDR, data_array, 2, dev); + if (rslt == BMI2_OK) + { + *int_status = (uint16_t) data_array[0] | ((uint16_t) data_array[1] << 8); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API selects the sensors/features to be enabled. + */ +int8_t bmi2_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to select sensor */ + uint64_t sensor_sel = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sens_list != NULL)) + { + /* Get the selected sensors */ + rslt = select_sensor(sens_list, n_sens, &sensor_sel); + if (rslt == BMI2_OK) + { + /* Enable the selected sensors */ + rslt = sensor_enable(sensor_sel, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API selects the sensors/features to be disabled. + */ +int8_t bmi2_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to select sensor */ + uint64_t sensor_sel = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sens_list != NULL)) + { + /* Get the selected sensors */ + rslt = select_sensor(sens_list, n_sens, &sensor_sel); + if (rslt == BMI2_OK) + { + /* Disable the selected sensors */ + rslt = sensor_disable(sensor_sel, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API sets the sensor/feature configuration. + */ +int8_t bmi2_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define loop */ + uint8_t loop; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sens_cfg != NULL)) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + + for (loop = 0; loop < n_sens; loop++) + { + /* Disable Advance power save if enabled for auxiliary + * and feature configurations + */ + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if + * enabled + */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + if (rslt == BMI2_OK) + { + switch (sens_cfg[loop].type) + { + /* Set accelerometer configuration */ + case BMI2_ACCEL: + rslt = set_accel_config(&sens_cfg[loop].cfg.acc, dev); + break; + + /* Set gyroscope configuration */ + case BMI2_GYRO: + rslt = set_gyro_config(&sens_cfg[loop].cfg.gyr, dev); + break; + + /* Set auxiliary configuration */ + case BMI2_AUX: + rslt = set_aux_config(&sens_cfg[loop].cfg.aux, dev); + break; + + /* Set gyroscope user gain configuration */ + case BMI2_GYRO_GAIN_UPDATE: + rslt = set_gyro_user_gain_config(&sens_cfg[loop].cfg.gyro_gain_update, dev); + break; + + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + } + + /* Return error if any of the set configurations fail */ + if (rslt != BMI2_OK) + { + break; + } + } + + /* Enable Advance power save if disabled while configuring and + * not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API gets the sensor/feature configuration. + */ +int8_t bmi2_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define loop */ + uint8_t loop; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sens_cfg != NULL)) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + for (loop = 0; loop < n_sens; loop++) + { + /* Disable Advance power save if enabled for auxiliary + * and feature configurations + */ + if ((sens_cfg[loop].type >= BMI2_MAIN_SENS_MAX_NUM) || (sens_cfg[loop].type == BMI2_AUX)) + { + + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if + * enabled + */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + } + + if (rslt == BMI2_OK) + { + switch (sens_cfg[loop].type) + { + /* Get accelerometer configuration */ + case BMI2_ACCEL: + rslt = get_accel_config(&sens_cfg[loop].cfg.acc, dev); + break; + + /* Get gyroscope configuration */ + case BMI2_GYRO: + rslt = get_gyro_config(&sens_cfg[loop].cfg.gyr, dev); + break; + + /* Get auxiliary configuration */ + case BMI2_AUX: + rslt = get_aux_config(&sens_cfg[loop].cfg.aux, dev); + break; + + /* Get gyroscope user gain configuration */ + case BMI2_GYRO_GAIN_UPDATE: + rslt = get_gyro_gain_update_config(&sens_cfg[loop].cfg.gyro_gain_update, dev); + break; + + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + } + + /* Return error if any of the get configurations fail */ + if (rslt != BMI2_OK) + { + break; + } + } + + /* Enable Advance power save if disabled while configuring and + * not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API gets the sensor/feature data for accelerometer, gyroscope, + * auxiliary sensor, step counter, high-g, gyroscope user-gain update, + * orientation, gyroscope cross sensitivity and error status for NVM and VFRM. + */ +int8_t bmi2_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define loop */ + uint8_t loop; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sensor_data != NULL)) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + for (loop = 0; loop < n_sens; loop++) + { + /* Disable Advance power save if enabled for feature + * configurations + */ + if (sensor_data[loop].type >= BMI2_MAIN_SENS_MAX_NUM) + { + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if + * enabled + */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + } + + if (rslt == BMI2_OK) + { + switch (sensor_data[loop].type) + { + case BMI2_ACCEL: + + /* Get accelerometer data */ + rslt = get_accel_sensor_data(&sensor_data[loop].sens_data.acc, BMI2_ACC_X_LSB_ADDR, dev); + break; + case BMI2_GYRO: + + /* Get gyroscope data */ + rslt = get_gyro_sensor_data(&sensor_data[loop].sens_data.gyr, BMI2_GYR_X_LSB_ADDR, dev); + break; + case BMI2_AUX: + + /* Get auxiliary sensor data in data mode */ + rslt = read_aux_data_mode(sensor_data[loop].sens_data.aux_data, dev); + break; + + case BMI2_GYRO_CROSS_SENSE: + + /* Get Gyroscope cross sense value of z axis */ + rslt = get_gyro_cross_sense(&sensor_data[loop].sens_data.correction_factor_zx, dev); + break; + + case BMI2_GYRO_GAIN_UPDATE: + + /* Get saturation status of gyroscope user gain update */ + rslt = get_gyro_gain_update_status(&sensor_data[loop].sens_data.gyro_user_gain_status, dev); + break; + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + + /* Return error if any of the get sensor data fails */ + if (rslt != BMI2_OK) + { + break; + } + } + + /* Enable Advance power save if disabled while + * configuring and not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API sets the FIFO configuration in the sensor. + */ +int8_t bmi2_set_fifo_config(uint16_t config, uint8_t enable, struct bmi2_dev *dev) +{ + int8_t rslt; + uint8_t data[2] = { 0 }; + uint8_t max_burst_len = 0; + + /* Variable to store data of FIFO configuration register 0 */ + uint8_t fifo_config_0 = (uint8_t)(config & BMI2_FIFO_CONFIG_0_MASK); + + /* Variable to store data of FIFO configuration register 1 */ + uint8_t fifo_config_1 = (uint8_t)((config & BMI2_FIFO_CONFIG_1_MASK) >> 8); + + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + rslt = bmi2_get_regs(BMI2_FIFO_CONFIG_0_ADDR, data, BMI2_FIFO_CONFIG_LENGTH, dev); + if (rslt == BMI2_OK) + { + /* Get data to set FIFO configuration register 0 */ + if (fifo_config_0 > 0) + { + if (enable == BMI2_ENABLE) + { + data[0] = data[0] | fifo_config_0; + } + else + { + data[0] = data[0] & (~fifo_config_0); + } + } + + /* Get data to set FIFO configuration register 1 */ + if (enable == BMI2_ENABLE) + { + data[1] = data[1] | fifo_config_1; + if (dev->variant_feature & BMI2_CRT_RTOSK_ENABLE) + { + + /* Burst length is needed for CRT + * FIFO enable will reset the default values + * So configure the max burst length again. + */ + rslt = get_maxburst_len(&max_burst_len, dev); + if (rslt == BMI2_OK && max_burst_len == 0) + { + rslt = set_maxburst_len(BMI2_CRT_MIN_BURST_WORD_LENGTH, dev); + } + } + } + else + { + data[1] = data[1] & (~fifo_config_1); + } + + /* Set the FIFO configurations */ + if (rslt == BMI2_OK) + { + rslt = bmi2_set_regs(BMI2_FIFO_CONFIG_0_ADDR, data, 2, dev); + } + } + } + + return rslt; +} + +/*! + * @brief This API reads the FIFO configuration from the sensor. + */ +int8_t bmi2_get_fifo_config(uint16_t *fifo_config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to store data */ + uint8_t data[2] = { 0 }; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (fifo_config != NULL)) + { + /* Get the FIFO configuration value */ + rslt = bmi2_get_regs(BMI2_FIFO_CONFIG_0_ADDR, data, BMI2_FIFO_CONFIG_LENGTH, dev); + if (rslt == BMI2_OK) + { + (*fifo_config) = (uint16_t)((uint16_t) data[0] & BMI2_FIFO_CONFIG_0_MASK); + (*fifo_config) |= (uint16_t)(((uint16_t) data[1] << 8) & BMI2_FIFO_CONFIG_1_MASK); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API reads the FIFO data. + */ +int8_t bmi2_read_fifo_data(struct bmi2_fifo_frame *fifo, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to store FIFO configuration data */ + uint8_t config_data[2] = { 0 }; + + /* Variable to define FIFO address */ + uint8_t addr = BMI2_FIFO_DATA_ADDR; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (fifo != NULL)) + { + /* Clear the FIFO data structure */ + reset_fifo_frame_structure(fifo, dev); + + /* Read FIFO data */ + rslt = bmi2_get_regs(addr, fifo->data, fifo->length, dev); + + if (rslt == BMI2_OK) + { + + /* Get the set FIFO frame configurations */ + rslt = bmi2_get_regs(BMI2_FIFO_CONFIG_0_ADDR, config_data, 2, dev); + if (rslt == BMI2_OK) + { + /* Get FIFO header status */ + fifo->header_enable = (uint8_t)((config_data[1]) & (BMI2_FIFO_HEADER_EN >> 8)); + + /* Get sensor enable status, of which the data is to be read */ + fifo->data_enable = + (uint16_t)(((config_data[0]) | ((uint16_t) config_data[1] << 8)) & BMI2_FIFO_ALL_EN); + } + } + else + { + rslt = BMI2_E_COM_FAIL; + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API parses and extracts the accelerometer frames from FIFO data + * read by the "bmi2_read_fifo_data" API and stores it in the "accel_data" + * structure instance. + */ +int8_t bmi2_extract_accel(struct bmi2_sens_axes_data *accel_data, + uint16_t *accel_length, + struct bmi2_fifo_frame *fifo, + const struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to index the bytes */ + uint16_t data_index = 0; + + /* Variable to index accelerometer frames */ + uint16_t accel_index = 0; + + /* Variable to store the number of bytes to be read */ + uint16_t data_read_length = 0; + + /* Variable to define the data enable byte */ + uint8_t data_enable = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (accel_data != NULL) && (accel_length != NULL) && (fifo != NULL)) + { + /* Parsing the FIFO data in header-less mode */ + if (fifo->header_enable == 0) + { + + /* Get the number of accelerometer bytes to be read */ + rslt = parse_fifo_accel_len(&data_index, &data_read_length, accel_length, fifo); + + /* Convert word to byte since all sensor enables are in a byte */ + data_enable = (uint8_t)(fifo->data_enable >> 8); + for (; (data_index < data_read_length) && (rslt != BMI2_W_FIFO_EMPTY);) + { + /* Unpack frame to get the accelerometer data */ + rslt = unpack_accel_frame(accel_data, &data_index, &accel_index, data_enable, fifo, dev); + + if (rslt != BMI2_W_FIFO_EMPTY) + { + /* Check for the availability of next two bytes of FIFO data */ + rslt = check_empty_fifo(&data_index, fifo); + } + } + + /* Update number of accelerometer frames to be read */ + (*accel_length) = accel_index; + + /* Update the accelerometer byte index */ + fifo->acc_byte_start_idx = data_index; + } + else + { + /* Parsing the FIFO data in header mode */ + rslt = extract_accel_header_mode(accel_data, accel_length, fifo, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API parses and extracts the gyroscope frames from FIFO data + * read by the "bmi2_read_fifo_data" API and stores it in the "gyro_data" + * structure instance. + */ +int8_t bmi2_extract_gyro(struct bmi2_sens_axes_data *gyro_data, + uint16_t *gyro_length, + struct bmi2_fifo_frame *fifo, + const struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to index the bytes */ + uint16_t data_index = 0; + + /* Variable to index gyroscope frames */ + uint16_t gyro_index = 0; + + /* Variable to store the number of bytes to be read */ + uint16_t data_read_length = 0; + + /* Variable to define the data enable byte */ + uint8_t data_enable = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (gyro_data != NULL) && (gyro_length != NULL) && (fifo != NULL)) + { + /* Parsing the FIFO data in header-less mode */ + if (fifo->header_enable == 0) + { + /* Get the number of gyro bytes to be read */ + rslt = parse_fifo_gyro_len(&data_index, &data_read_length, gyro_length, fifo); + + /* Convert word to byte since all sensor enables are in a byte */ + data_enable = (uint8_t)(fifo->data_enable >> 8); + for (; (data_index < data_read_length) && (rslt != BMI2_W_FIFO_EMPTY);) + { + /* Unpack frame to get gyroscope data */ + rslt = unpack_gyro_frame(gyro_data, &data_index, &gyro_index, data_enable, fifo, dev); + if (rslt != BMI2_W_FIFO_EMPTY) + { + /* Check for the availability of next two bytes of FIFO data */ + rslt = check_empty_fifo(&data_index, fifo); + } + } + + /* Update number of gyroscope frames to be read */ + (*gyro_length) = gyro_index; + + /* Update the gyroscope byte index */ + fifo->acc_byte_start_idx = data_index; + } + else + { + /* Parsing the FIFO data in header mode */ + rslt = extract_gyro_header_mode(gyro_data, gyro_length, fifo, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API parses and extracts the auxiliary frames from FIFO data + * read by the "bmi2_read_fifo_data" API and stores it in "aux_data" buffer. + */ +int8_t bmi2_extract_aux(struct bmi2_aux_fifo_data *aux, + uint16_t *aux_length, + struct bmi2_fifo_frame *fifo, + const struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to index the bytes */ + uint16_t data_index = 0; + + /* Variable to index auxiliary frames */ + uint16_t aux_index = 0; + + /* Variable to store the number of bytes to be read */ + uint16_t data_read_length = 0; + + /* Variable to define the data enable byte */ + uint8_t data_enable = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (aux != NULL) && (aux_length != NULL) && (fifo != NULL)) + { + /* Parsing the FIFO data in header-less mode */ + if (fifo->header_enable == 0) + { + rslt = parse_fifo_aux_len(&data_index, &data_read_length, aux_length, fifo); + + /* Convert word to byte since all sensor enables are in + * a byte + */ + data_enable = (uint8_t)(fifo->data_enable >> 8); + for (; (data_index < data_read_length) && (rslt != BMI2_W_FIFO_EMPTY);) + { + /* Unpack frame to get auxiliary data */ + rslt = unpack_aux_frame(aux, &data_index, &aux_index, data_enable, fifo, dev); + if (rslt != BMI2_W_FIFO_EMPTY) + { + /* Check for the availability of next + * two bytes of FIFO data + */ + rslt = check_empty_fifo(&data_index, fifo); + } + } + + /* Update number of auxiliary frames to be read */ + *aux_length = aux_index; + + /* Update the auxiliary byte index */ + fifo->aux_byte_start_idx = data_index; + } + else + { + /* Parsing the FIFO data in header mode */ + rslt = extract_aux_header_mode(aux, aux_length, fifo, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API writes the available sensor specific commands to the sensor. + */ +int8_t bmi2_set_command_register(uint8_t command, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + /* Set the command in the command register */ + rslt = bmi2_set_regs(BMI2_CMD_REG_ADDR, &command, 1, dev); + } + + return rslt; +} + +/* + * @brief This API sets the FIFO self wake up functionality in the sensor. + */ +int8_t bmi2_set_fifo_self_wake_up(uint8_t fifo_self_wake_up, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store data */ + uint8_t data = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + /* Set FIFO self wake-up */ + rslt = bmi2_get_regs(BMI2_PWR_CONF_ADDR, &data, 1, dev); + if (rslt == BMI2_OK) + { + data = BMI2_SET_BITS(data, BMI2_FIFO_SELF_WAKE_UP, fifo_self_wake_up); + rslt = bmi2_set_regs(BMI2_PWR_CONF_ADDR, &data, 1, dev); + } + } + + return rslt; +} + +/*! + * @brief This API gets the status of FIFO self wake up functionality from + * the sensor. + */ +int8_t bmi2_get_fifo_self_wake_up(uint8_t *fifo_self_wake_up, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store data */ + uint8_t data = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (fifo_self_wake_up != NULL)) + { + /* Get the status of FIFO self wake-up */ + rslt = bmi2_get_regs(BMI2_PWR_CONF_ADDR, &data, 1, dev); + if (rslt == BMI2_OK) + { + (*fifo_self_wake_up) = BMI2_GET_BITS(data, BMI2_FIFO_SELF_WAKE_UP); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API sets the FIFO water-mark level in the sensor. + */ +int8_t bmi2_set_fifo_wm(uint16_t fifo_wm, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to store data */ + uint8_t data[2] = { 0 }; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + /* Get LSB value of FIFO water-mark */ + data[0] = BMI2_GET_LSB(fifo_wm); + + /* Get MSB value of FIFO water-mark */ + data[1] = BMI2_GET_MSB(fifo_wm); + + /* Set the FIFO water-mark level */ + rslt = bmi2_set_regs(BMI2_FIFO_WTM_0_ADDR, data, 2, dev); + } + + return rslt; +} + +/*! + * @brief This API reads the FIFO water mark level set in the sensor. + */ +int8_t bmi2_get_fifo_wm(uint16_t *fifo_wm, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to to store data */ + uint8_t data[2] = { 0 }; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (fifo_wm != NULL)) + { + /* Read the FIFO water mark level */ + rslt = bmi2_get_regs(BMI2_FIFO_WTM_0_ADDR, data, BMI2_FIFO_WM_LENGTH, dev); + if (rslt == BMI2_OK) + { + (*fifo_wm) = (uint16_t)((uint16_t) data[1] << 8) | (data[0]); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API sets either filtered or un-filtered FIFO accelerometer or + * gyroscope data. + */ +int8_t bmi2_set_fifo_filter_data(uint8_t sens_sel, uint8_t fifo_filter_data, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store data */ + uint8_t data = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + switch (sens_sel) + { + case BMI2_ACCEL: + + /* Validate filter mode */ + if (fifo_filter_data <= BMI2_MAX_VALUE_FIFO_FILTER) + { + /* Set the accelerometer FIFO filter data */ + rslt = bmi2_get_regs(BMI2_FIFO_DOWNS_ADDR, &data, 1, dev); + if (rslt == BMI2_OK) + { + data = BMI2_SET_BITS(data, BMI2_ACC_FIFO_FILT_DATA, fifo_filter_data); + rslt = bmi2_set_regs(BMI2_FIFO_DOWNS_ADDR, &data, 1, dev); + } + } + else + { + rslt = BMI2_E_OUT_OF_RANGE; + } + + break; + case BMI2_GYRO: + + /* Validate filter mode */ + if (fifo_filter_data <= BMI2_MAX_VALUE_FIFO_FILTER) + { + /* Set the gyroscope FIFO filter data */ + rslt = bmi2_get_regs(BMI2_FIFO_DOWNS_ADDR, &data, 1, dev); + if (rslt == BMI2_OK) + { + data = BMI2_SET_BITS(data, BMI2_GYR_FIFO_FILT_DATA, fifo_filter_data); + rslt = bmi2_set_regs(BMI2_FIFO_DOWNS_ADDR, &data, 1, dev); + } + } + else + { + rslt = BMI2_E_OUT_OF_RANGE; + } + + break; + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + } + + return rslt; +} + +/*! + * @brief This API gets the FIFO accelerometer or gyroscope filter data. + */ +int8_t bmi2_get_fifo_filter_data(uint8_t sens_sel, uint8_t *fifo_filter_data, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store FIFO filter mode */ + uint8_t data = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (fifo_filter_data != NULL)) + { + switch (sens_sel) + { + case BMI2_ACCEL: + + /* Read the accelerometer FIFO filter data */ + rslt = bmi2_get_regs(BMI2_FIFO_DOWNS_ADDR, &data, 1, dev); + if (rslt == BMI2_OK) + { + (*fifo_filter_data) = BMI2_GET_BITS(data, BMI2_ACC_FIFO_FILT_DATA); + } + + break; + case BMI2_GYRO: + + /* Read the gyroscope FIFO filter data */ + rslt = bmi2_get_regs(BMI2_FIFO_DOWNS_ADDR, &data, 1, dev); + if (rslt == BMI2_OK) + { + (*fifo_filter_data) = BMI2_GET_BITS(data, BMI2_GYR_FIFO_FILT_DATA); + } + + break; + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API sets the down-sampling rates for accelerometer or gyroscope + * FIFO data. + */ +int8_t bmi2_set_fifo_down_sample(uint8_t sens_sel, uint8_t fifo_down_samp, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store sampling rate */ + uint8_t data = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + switch (sens_sel) + { + case BMI2_ACCEL: + + /* Set the accelerometer FIFO down sampling rate */ + rslt = bmi2_get_regs(BMI2_FIFO_DOWNS_ADDR, &data, 1, dev); + if (rslt == BMI2_OK) + { + data = BMI2_SET_BITS(data, BMI2_ACC_FIFO_DOWNS, fifo_down_samp); + rslt = bmi2_set_regs(BMI2_FIFO_DOWNS_ADDR, &data, 1, dev); + } + + break; + case BMI2_GYRO: + + /* Set the gyroscope FIFO down sampling rate */ + rslt = bmi2_get_regs(BMI2_FIFO_DOWNS_ADDR, &data, 1, dev); + if (rslt == BMI2_OK) + { + data = BMI2_SET_BIT_POS0(data, BMI2_GYR_FIFO_DOWNS, fifo_down_samp); + rslt = bmi2_set_regs(BMI2_FIFO_DOWNS_ADDR, &data, 1, dev); + } + + break; + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + } + + return rslt; +} + +/*! + * @brief This API reads the down sampling rates which is configured for + * accelerometer or gyroscope FIFO data. + */ +int8_t bmi2_get_fifo_down_sample(uint8_t sens_sel, uint8_t *fifo_down_samp, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store sampling rate */ + uint8_t data = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (fifo_down_samp != NULL)) + { + switch (sens_sel) + { + case BMI2_ACCEL: + + /* Read the accelerometer FIFO down data sampling rate */ + rslt = bmi2_get_regs(BMI2_FIFO_DOWNS_ADDR, &data, 1, dev); + if (rslt == BMI2_OK) + { + (*fifo_down_samp) = BMI2_GET_BITS(data, BMI2_ACC_FIFO_DOWNS); + } + + break; + case BMI2_GYRO: + + /* Read the gyroscope FIFO down data sampling rate */ + rslt = bmi2_get_regs(BMI2_FIFO_DOWNS_ADDR, &data, 1, dev); + if (rslt == BMI2_OK) + { + (*fifo_down_samp) = BMI2_GET_BIT_POS0(data, BMI2_GYR_FIFO_DOWNS); + } + + break; + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API gets the length of FIFO data available in the sensor in + * bytes. + */ +int8_t bmi2_get_fifo_length(uint16_t *fifo_length, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define byte index */ + uint8_t index = 0; + + /* Array to store FIFO data length */ + uint8_t data[BMI2_FIFO_DATA_LENGTH] = { 0 }; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (fifo_length != NULL)) + { + /* Read FIFO length */ + rslt = bmi2_get_regs(BMI2_FIFO_LENGTH_0_ADDR, data, BMI2_FIFO_DATA_LENGTH, dev); + if (rslt == BMI2_OK) + { + /* Get the MSB byte index */ + index = BMI2_FIFO_LENGTH_MSB_BYTE; + + /* Get the MSB byte of FIFO length */ + data[index] = BMI2_GET_BIT_POS0(data[index], BMI2_FIFO_BYTE_COUNTER_MSB); + + /* Get total FIFO length */ + (*fifo_length) = ((data[index] << 8) | data[index - 1]); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API reads the user-defined bytes of data from the given register + * address of auxiliary sensor in manual mode. + * + * @note Change of BMI2_AUX_RD_ADDR is only allowed if AUX is not busy. + */ +int8_t bmi2_read_aux_man_mode(uint8_t reg_addr, uint8_t *aux_data, uint16_t len, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store burst length */ + uint8_t burst_len = 0; + + /* Variable to define APS status */ + uint8_t aps_stat = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (aux_data != NULL)) + { + /* Validate if manual mode */ + if (dev->aux_man_en) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + if (aps_stat == BMI2_ENABLE) + { + /* Disable APS if enabled */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + if (rslt == BMI2_OK) + { + /* Map the register value set to that of burst + * length + */ + rslt = map_read_len(&burst_len, dev); + if (rslt == BMI2_OK) + { + /* Read auxiliary data */ + rslt = read_aux_data(reg_addr, aux_data, len, burst_len, dev); + } + } + + /* Enable Advance power save if disabled for reading + * data and not when already disabled + */ + if ((rslt == BMI2_OK) && (aps_stat == BMI2_ENABLE)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + else + { + rslt = BMI2_E_AUX_INVALID_CFG; + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API writes the user-defined bytes of data and the address of + * auxiliary sensor where data is to be written in manual mode. + * + * @note Change of BMI2_AUX_WR_ADDR is only allowed if AUX is not busy. + */ +int8_t bmi2_write_aux_man_mode(uint8_t reg_addr, const uint8_t *aux_data, uint16_t len, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define loop */ + uint8_t loop = 0; + + /* Variable to define APS status */ + uint8_t aps_stat = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (aux_data != NULL)) + { + /* Validate if manual mode */ + if (dev->aux_man_en) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + if (aps_stat == BMI2_ENABLE) + { + /* Disable APS if enabled */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + /* Byte write data in the corresponding address */ + if (rslt == BMI2_OK) + { + for (; ((loop < len) && (rslt == BMI2_OK)); loop++) + { + rslt = write_aux_data((reg_addr + loop), aux_data[loop], dev); + dev->delay_us(1000, dev->intf_ptr); + } + } + + /* Enable Advance power save if disabled for writing + * data and not when already disabled + */ + if ((rslt == BMI2_OK) && (aps_stat == BMI2_ENABLE)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + else + { + rslt = BMI2_E_AUX_INVALID_CFG; + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API writes the user-defined bytes of data and the address of + * auxiliary sensor where data is to be written, from an interleaved input, + * in manual mode. + * + * @note Change of BMI2_AUX_WR_ADDR is only allowed if AUX is not busy. + */ +int8_t bmi2_write_aux_interleaved(uint8_t reg_addr, const uint8_t *aux_data, uint16_t len, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define loop */ + uint8_t loop = 1; + + /* Variable to define APS status */ + uint8_t aps_stat = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (aux_data != NULL)) + { + /* Validate if manual mode */ + if (dev->aux_man_en) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + if (aps_stat == BMI2_ENABLE) + { + /* Disable APS if enabled */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + if (rslt == BMI2_OK) + { + /* Write the start register address extracted + * from the interleaved data + */ + rslt = write_aux_data(reg_addr, aux_data[0], dev); + + /* Extract the remaining address and data from + * the interleaved data and write it in the + * corresponding addresses byte by byte + */ + for (; ((loop < len) && (rslt == BMI2_OK)); loop += 2) + { + rslt = write_aux_data(aux_data[loop], aux_data[loop + 1], dev); + dev->delay_us(1000, dev->intf_ptr); + } + + /* Enable Advance power save if disabled for + * writing data and not when already disabled + */ + if ((rslt == BMI2_OK) && (aps_stat == BMI2_ENABLE)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + } + else + { + rslt = BMI2_E_AUX_INVALID_CFG; + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API gets the data ready status of accelerometer, gyroscope, + * auxiliary, command decoder and busy status of auxiliary. + */ +int8_t bmi2_get_status(uint8_t *status, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (status != NULL)) + { + rslt = bmi2_get_regs(BMI2_STATUS_ADDR, status, 1, dev); + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API enables/disables OIS interface. + */ +int8_t bmi2_set_ois_interface(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store data */ + uint8_t reg_data = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + rslt = bmi2_get_regs(BMI2_IF_CONF_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + /* Enable/Disable OIS interface */ + reg_data = BMI2_SET_BITS(reg_data, BMI2_OIS_IF_EN, enable); + if (enable) + { + /* Disable auxiliary interface if OIS is enabled */ + reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_AUX_IF_EN); + } + + /* Set the OIS interface configurations */ + rslt = bmi2_set_regs(BMI2_IF_CONF_ADDR, ®_data, 1, dev); + } + } + + return rslt; +} + +/*! + * @brief This API can be used to write sync commands like ODR, sync period, + * frequency and phase, resolution ratio, sync time and delay time. + */ +int8_t bmi2_write_sync_commands(const uint8_t *command, uint8_t n_comm, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (command != NULL)) + { + rslt = bmi2_set_regs(BMI2_SYNC_COMMAND_ADDR, command, n_comm, dev); + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API performs self-test to check the proper functionality of the + * accelerometer sensor. + */ +int8_t bmi2_perform_accel_self_test(struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store self-test result */ + int8_t st_rslt = 0; + + /* Structure to define positive accelerometer axes */ + struct bmi2_sens_axes_data positive = { 0, 0, 0, 0 }; + + /* Structure to define negative accelerometer axes */ + struct bmi2_sens_axes_data negative = { 0, 0, 0, 0 }; + + /* Structure for difference of accelerometer values in g */ + struct bmi2_selftest_delta_limit accel_data_diff = { 0, 0, 0 }; + + /* Structure for difference of accelerometer values in mg */ + struct bmi2_selftest_delta_limit accel_data_diff_mg = { 0, 0, 0 }; + + /* Initialize the polarity of self-test as positive */ + int8_t sign = BMI2_ENABLE; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + /* Sets the configuration required before enabling self-test */ + rslt = pre_self_test_config(dev); + + /* Wait for greater than 2 milliseconds */ + dev->delay_us(3000, dev->intf_ptr); + if (rslt == BMI2_OK) + { + do + { + /* Select positive first, then negative polarity + * after enabling self-test + */ + rslt = self_test_config((uint8_t) sign, dev); + if (rslt == BMI2_OK) + { + /* Wait for greater than 50 milli-sec */ + dev->delay_us(51000, dev->intf_ptr); + + /* If polarity is positive */ + if (sign == BMI2_ENABLE) + { + /* Read and store positive acceleration value */ + rslt = read_accel_xyz(&positive, dev); + } + /* If polarity is negative */ + else if (sign == BMI2_DISABLE) + { + /* Read and store negative acceleration value */ + rslt = read_accel_xyz(&negative, dev); + } + } + else + { + /* Break if error */ + break; + } + + /* Break if error */ + if (rslt != BMI2_OK) + { + break; + } + + /* Turn the polarity of self-test negative */ + sign--; + } while (sign >= 0); + if (rslt == BMI2_OK) + { + /* Subtract -ve acceleration values from that of +ve values */ + accel_data_diff.x = (positive.x) - (negative.x); + accel_data_diff.y = (positive.y) - (negative.y); + accel_data_diff.z = (positive.z) - (negative.z); + + /* Convert differences of acceleration values + * from 'g' to 'mg' + */ + convert_lsb_g(&accel_data_diff, &accel_data_diff_mg, dev); + + /* Validate self-test for acceleration values + * in mg and get the self-test result + */ + st_rslt = validate_self_test(&accel_data_diff_mg); + + /* Trigger a soft reset after performing self-test */ + rslt = bmi2_soft_reset(dev); + + /* Return the self-test result */ + if (rslt == BMI2_OK) + { + rslt = st_rslt; + } + } + } + } + + return rslt; +} + +/*! + * @brief This API maps/unmaps feature interrupts to that of interrupt pins. + */ +int8_t bmi2_map_feat_int(uint8_t type, enum bmi2_hw_int_pin hw_int_pin, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define the value of feature interrupts */ + uint8_t feat_int = 0; + + /* Array to store the interrupt mask bits */ + uint8_t data_array[2] = { 0 }; + + /* Structure to define map the interrupts */ + struct bmi2_map_int map_int = { 0 }; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + /* Read interrupt map1 and map2 and register */ + rslt = bmi2_get_regs(BMI2_INT1_MAP_FEAT_ADDR, data_array, 2, dev); + + if (rslt == BMI2_OK) + { + /* Get the value of the feature interrupt to be mapped */ + extract_feat_int_map(&map_int, type, dev); + + feat_int = map_int.sens_map_int; + + /* Map the interrupts */ + rslt = map_feat_int(data_array, hw_int_pin, feat_int); + + /* Map the interrupts to INT1 and INT2 map register */ + if (rslt == BMI2_OK) + { + rslt = bmi2_set_regs(BMI2_INT1_MAP_FEAT_ADDR, &data_array[0], 1, dev); + if (rslt == BMI2_OK) + { + rslt = bmi2_set_regs(BMI2_INT2_MAP_FEAT_ADDR, &data_array[1], 1, dev); + } + } + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API maps/un-maps data interrupts to that of interrupt pins. + */ +int8_t bmi2_map_data_int(uint8_t data_int, enum bmi2_hw_int_pin int_pin, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to mask interrupt pin 1 - lower nibble */ + uint8_t int1_mask = data_int; + + /* Variable to mask interrupt pin 2 - higher nibble */ + uint8_t int2_mask = (uint8_t)(data_int << 4); + + /* Variable to store register data */ + uint8_t reg_data = 0; + + /* Read interrupt map1 and map2 and register */ + rslt = bmi2_get_regs(BMI2_INT_MAP_DATA_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + if (int_pin < BMI2_INT_PIN_MAX) + { + switch (int_pin) + { + case BMI2_INT_NONE: + + /* Un-Map the corresponding data + * interrupt to both interrupt pin 1 and 2 + */ + reg_data &= ~(int1_mask | int2_mask); + break; + case BMI2_INT1: + + /* Map the corresponding data interrupt to + * interrupt pin 1 + */ + reg_data |= int1_mask; + break; + case BMI2_INT2: + + /* Map the corresponding data interrupt to + * interrupt pin 2 + */ + reg_data |= int2_mask; + break; + case BMI2_INT_BOTH: + + /* Map the corresponding data + * interrupt to both interrupt pin 1 and 2 + */ + reg_data |= (int1_mask | int2_mask); + break; + default: + break; + } + + /* Set the interrupts in the map register */ + rslt = bmi2_set_regs(BMI2_INT_MAP_DATA_ADDR, ®_data, 1, dev); + } + else + { + /* Return error if invalid pin selection */ + rslt = BMI2_E_INVALID_INT_PIN; + } + } + + return rslt; +} + +/*! + * @brief This API gets the re-mapped x, y and z axes from the sensor and + * updates the values in the device structure. + */ +int8_t bmi2_get_remap_axes(struct bmi2_remap *remapped_axis, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Initialize the local structure for axis re-mapping */ + struct bmi2_axes_remap remap = { 0, 0, 0, 0, 0, 0 }; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (remapped_axis != NULL)) + { + /* Get the re-mapped axes from the sensor */ + rslt = get_remap_axes(&remap, dev); + if (rslt == BMI2_OK) + { + /* Store the re-mapped x-axis value in device structure + * and its user-value in the interface structure + */ + switch (remap.x_axis) + { + case BMI2_MAP_X_AXIS: + + /* If mapped to x-axis */ + dev->remap.x_axis = BMI2_MAP_X_AXIS; + remapped_axis->x = BMI2_X; + break; + case BMI2_MAP_Y_AXIS: + + /* If mapped to y-axis */ + dev->remap.x_axis = BMI2_MAP_Y_AXIS; + remapped_axis->x = BMI2_Y; + break; + case BMI2_MAP_Z_AXIS: + + /* If mapped to z-axis */ + dev->remap.x_axis = BMI2_MAP_Z_AXIS; + remapped_axis->x = BMI2_Z; + break; + default: + break; + } + + /* Store the re-mapped x-axis sign in device structure + * and its user-value in the interface structure + */ + if (remap.x_axis_sign) + { + /* If x-axis is mapped to -ve sign */ + dev->remap.x_axis_sign = BMI2_NEG_SIGN; + remapped_axis->x |= BMI2_AXIS_SIGN; + } + else + { + dev->remap.x_axis_sign = BMI2_POS_SIGN; + } + + /* Store the re-mapped y-axis value in device structure + * and its user-value in the interface structure + */ + switch (remap.y_axis) + { + case BMI2_MAP_X_AXIS: + + /* If mapped to x-axis */ + dev->remap.y_axis = BMI2_MAP_X_AXIS; + remapped_axis->y = BMI2_X; + break; + case BMI2_MAP_Y_AXIS: + + /* If mapped to y-axis */ + dev->remap.y_axis = BMI2_MAP_Y_AXIS; + remapped_axis->y = BMI2_Y; + break; + case BMI2_MAP_Z_AXIS: + + /* If mapped to z-axis */ + dev->remap.y_axis = BMI2_MAP_Z_AXIS; + remapped_axis->y = BMI2_Z; + break; + default: + break; + } + + /* Store the re-mapped y-axis sign in device structure + * and its user-value in the interface structure + */ + if (remap.y_axis_sign) + { + /* If y-axis is mapped to -ve sign */ + dev->remap.y_axis_sign = BMI2_NEG_SIGN; + remapped_axis->y |= BMI2_AXIS_SIGN; + } + else + { + dev->remap.y_axis_sign = BMI2_POS_SIGN; + } + + /* Store the re-mapped z-axis value in device structure + * and its user-value in the interface structure + */ + switch (remap.z_axis) + { + case BMI2_MAP_X_AXIS: + + /* If mapped to x-axis */ + dev->remap.z_axis = BMI2_MAP_X_AXIS; + remapped_axis->z = BMI2_X; + break; + case BMI2_MAP_Y_AXIS: + + /* If mapped to y-axis */ + dev->remap.z_axis = BMI2_MAP_Y_AXIS; + remapped_axis->z = BMI2_Y; + break; + case BMI2_MAP_Z_AXIS: + + /* If mapped to z-axis */ + dev->remap.z_axis = BMI2_MAP_Z_AXIS; + remapped_axis->z = BMI2_Z; + break; + default: + break; + } + + /* Store the re-mapped z-axis sign in device structure + * and its user-value in the interface structure + */ + if (remap.z_axis_sign) + { + /* If z-axis is mapped to -ve sign */ + dev->remap.z_axis_sign = BMI2_NEG_SIGN; + remapped_axis->z |= BMI2_AXIS_SIGN; + } + else + { + dev->remap.z_axis_sign = BMI2_POS_SIGN; + } + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API sets the re-mapped x, y and z axes to the sensor and + * updates the them in the device structure. + */ +int8_t bmi2_set_remap_axes(const struct bmi2_remap *remapped_axis, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store all the re-mapped axes */ + uint8_t remap_axes = 0; + + /* Variable to store the re-mapped x-axes */ + uint8_t remap_x = 0; + + /* Variable to store the re-mapped y-axes */ + uint8_t remap_y = 0; + + /* Variable to store the re-mapped z-axes */ + uint8_t remap_z = 0; + + /* Initialize the local structure for axis re-mapping */ + struct bmi2_axes_remap remap = { 0, 0, 0, 0, 0, 0 }; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (remapped_axis != NULL)) + { + /* Check whether all the axes are re-mapped */ + remap_axes = remapped_axis->x | remapped_axis->y | remapped_axis->z; + + /* If all the axes are re-mapped */ + if ((remap_axes & BMI2_AXIS_MASK) == BMI2_AXIS_MASK) + { + /* Get the re-mapped value of x, y and z axis */ + remap_x = remapped_axis->x & BMI2_AXIS_MASK; + remap_y = remapped_axis->y & BMI2_AXIS_MASK; + remap_z = remapped_axis->z & BMI2_AXIS_MASK; + + /* Store the value of re-mapped x-axis in both + * device structure and the local structure + */ + switch (remap_x) + { + case BMI2_X: + + /* If mapped to x-axis */ + dev->remap.x_axis = BMI2_MAP_X_AXIS; + remap.x_axis = BMI2_MAP_X_AXIS; + break; + case BMI2_Y: + + /* If mapped to y-axis */ + dev->remap.x_axis = BMI2_MAP_Y_AXIS; + remap.x_axis = BMI2_MAP_Y_AXIS; + break; + case BMI2_Z: + + /* If mapped to z-axis */ + dev->remap.x_axis = BMI2_MAP_Z_AXIS; + remap.x_axis = BMI2_MAP_Z_AXIS; + break; + default: + break; + } + + /* Store the re-mapped x-axis sign in the device + * structure and its value in local structure + */ + if (remapped_axis->x & BMI2_AXIS_SIGN) + { + /* If x-axis is mapped to -ve sign */ + dev->remap.x_axis_sign = BMI2_NEG_SIGN; + remap.x_axis_sign = BMI2_MAP_NEGATIVE; + } + else + { + dev->remap.x_axis_sign = BMI2_POS_SIGN; + remap.x_axis_sign = BMI2_MAP_POSITIVE; + } + + /* Store the value of re-mapped y-axis in both + * device structure and the local structure + */ + switch (remap_y) + { + case BMI2_X: + + /* If mapped to x-axis */ + dev->remap.y_axis = BMI2_MAP_X_AXIS; + remap.y_axis = BMI2_MAP_X_AXIS; + break; + case BMI2_Y: + + /* If mapped to y-axis */ + dev->remap.y_axis = BMI2_MAP_Y_AXIS; + remap.y_axis = BMI2_MAP_Y_AXIS; + break; + case BMI2_Z: + + /* If mapped to z-axis */ + dev->remap.y_axis = BMI2_MAP_Z_AXIS; + remap.y_axis = BMI2_MAP_Z_AXIS; + break; + default: + break; + } + + /* Store the re-mapped y-axis sign in the device + * structure and its value in local structure + */ + if (remapped_axis->y & BMI2_AXIS_SIGN) + { + /* If y-axis is mapped to -ve sign */ + dev->remap.y_axis_sign = BMI2_NEG_SIGN; + remap.y_axis_sign = BMI2_MAP_NEGATIVE; + } + else + { + dev->remap.y_axis_sign = BMI2_POS_SIGN; + remap.y_axis_sign = BMI2_MAP_POSITIVE; + } + + /* Store the value of re-mapped z-axis in both + * device structure and the local structure + */ + switch (remap_z) + { + case BMI2_X: + + /* If mapped to x-axis */ + dev->remap.z_axis = BMI2_MAP_X_AXIS; + remap.z_axis = BMI2_MAP_X_AXIS; + break; + case BMI2_Y: + + /* If mapped to y-axis */ + dev->remap.z_axis = BMI2_MAP_Y_AXIS; + remap.z_axis = BMI2_MAP_Y_AXIS; + break; + case BMI2_Z: + + /* If mapped to z-axis */ + dev->remap.z_axis = BMI2_MAP_Z_AXIS; + remap.z_axis = BMI2_MAP_Z_AXIS; + break; + default: + break; + } + + /* Store the re-mapped z-axis sign in the device + * structure and its value in local structure + */ + if (remapped_axis->z & BMI2_AXIS_SIGN) + { + /* If z-axis is mapped to -ve sign */ + dev->remap.z_axis_sign = BMI2_NEG_SIGN; + remap.z_axis_sign = BMI2_MAP_NEGATIVE; + } + else + { + dev->remap.z_axis_sign = BMI2_POS_SIGN; + remap.z_axis_sign = BMI2_MAP_POSITIVE; + } + + /* Set the re-mapped axes in the sensor */ + rslt = set_remap_axes(&remap, dev); + } + else + { + rslt = BMI2_E_REMAP_ERROR; + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API enables/disables gyroscope offset compensation. It adds the + * offsets defined in the offset register with gyroscope data. + */ +int8_t bmi2_set_gyro_offset_comp(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define register data */ + uint8_t reg_data = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + /* Get the status of gyroscope offset enable */ + rslt = bmi2_get_regs(BMI2_GYR_OFF_COMP_6_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + reg_data = BMI2_SET_BITS(reg_data, BMI2_GYR_OFF_COMP_EN, enable); + + /* Enable/Disable gyroscope offset compensation */ + rslt = bmi2_set_regs(BMI2_GYR_OFF_COMP_6_ADDR, ®_data, 1, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API reads the gyroscope bias values for each axis which is used + * for gyroscope offset compensation. + */ +int8_t bmi2_read_gyro_offset_comp_axes(struct bmi2_sens_axes_data *gyr_off_comp_axes, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define register data */ + uint8_t reg_data[4] = { 0 }; + + /* Variable to store LSB value of offset compensation for x-axis */ + uint8_t gyr_off_lsb_x; + + /* Variable to store LSB value of offset compensation for y-axis */ + uint8_t gyr_off_lsb_y; + + /* Variable to store LSB value of offset compensation for z-axis */ + uint8_t gyr_off_lsb_z; + + /* Variable to store MSB value of offset compensation for x-axis */ + uint8_t gyr_off_msb_x; + + /* Variable to store MSB value of offset compensation for y-axis */ + uint8_t gyr_off_msb_y; + + /* Variable to store MSB value of offset compensation for z-axis */ + uint8_t gyr_off_msb_z; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (gyr_off_comp_axes != NULL)) + { + /* Get the gyroscope compensated offset values */ + rslt = bmi2_get_regs(BMI2_GYR_OFF_COMP_3_ADDR, reg_data, 4, dev); + if (rslt == BMI2_OK) + { + /* Get LSB and MSB values of offset compensation for + * x, y and z axis + */ + gyr_off_lsb_x = reg_data[0]; + gyr_off_lsb_y = reg_data[1]; + gyr_off_lsb_z = reg_data[2]; + gyr_off_msb_x = reg_data[3] & BMI2_GYR_OFF_COMP_MSB_X_MASK; + gyr_off_msb_y = reg_data[3] & BMI2_GYR_OFF_COMP_MSB_Y_MASK; + gyr_off_msb_z = reg_data[3] & BMI2_GYR_OFF_COMP_MSB_Z_MASK; + + /* Gyroscope offset compensation value for x-axis */ + gyr_off_comp_axes->x = (int16_t)(((uint16_t) gyr_off_msb_x << 8) | gyr_off_lsb_x); + + /* Gyroscope offset compensation value for y-axis */ + gyr_off_comp_axes->y = (int16_t)(((uint16_t) gyr_off_msb_y << 6) | gyr_off_lsb_y); + + /* Gyroscope offset compensation value for z-axis */ + gyr_off_comp_axes->z = (int16_t)(((uint16_t) gyr_off_msb_z << 4) | gyr_off_lsb_z); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API writes the gyroscope bias values for each axis which is used + * for gyroscope offset compensation. + */ +int8_t bmi2_write_gyro_offset_comp_axes(const struct bmi2_sens_axes_data *gyr_off_comp_axes, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define register data */ + uint8_t reg_data[4] = { 0 }; + + /* Variable to store MSB value of offset compensation for x-axis */ + uint8_t gyr_off_msb_x; + + /* Variable to store MSB value of offset compensation for y-axis */ + uint8_t gyr_off_msb_y; + + /* Variable to store MSB value of offset compensation for z-axis */ + uint8_t gyr_off_msb_z; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (gyr_off_comp_axes != NULL)) + { + /* Get the MSB values of gyroscope compensated offset values */ + rslt = bmi2_get_regs(BMI2_GYR_OFF_COMP_6_ADDR, ®_data[3], 1, dev); + if (rslt == BMI2_OK) + { + /* Get MSB value of x-axis from user-input */ + gyr_off_msb_x = (uint8_t)((gyr_off_comp_axes->x & BMI2_GYR_OFF_COMP_MSB_MASK) >> 8); + + /* Get MSB value of y-axis from user-input */ + gyr_off_msb_y = (uint8_t)((gyr_off_comp_axes->y & BMI2_GYR_OFF_COMP_MSB_MASK) >> 8); + + /* Get MSB value of z-axis from user-input */ + gyr_off_msb_z = (uint8_t)((gyr_off_comp_axes->z & BMI2_GYR_OFF_COMP_MSB_MASK) >> 8); + + /* Get LSB value of x-axis from user-input */ + reg_data[0] = (uint8_t)(gyr_off_comp_axes->x & BMI2_GYR_OFF_COMP_LSB_MASK); + + /* Get LSB value of y-axis from user-input */ + reg_data[1] = (uint8_t)(gyr_off_comp_axes->y & BMI2_GYR_OFF_COMP_LSB_MASK); + + /* Get LSB value of z-axis from user-input */ + reg_data[2] = (uint8_t)(gyr_off_comp_axes->z & BMI2_GYR_OFF_COMP_LSB_MASK); + + /* Get MSB value of x-axis to be set */ + reg_data[3] = BMI2_SET_BIT_POS0(reg_data[3], BMI2_GYR_OFF_COMP_MSB_X, gyr_off_msb_x); + + /* Get MSB value of y-axis to be set */ + reg_data[3] = BMI2_SET_BITS(reg_data[3], BMI2_GYR_OFF_COMP_MSB_Y, gyr_off_msb_y); + + /* Get MSB value of z-axis to be set */ + reg_data[3] = BMI2_SET_BITS(reg_data[3], BMI2_GYR_OFF_COMP_MSB_Z, gyr_off_msb_z); + + /* Set the offset compensation values of axes */ + rslt = bmi2_set_regs(BMI2_GYR_OFF_COMP_3_ADDR, reg_data, 4, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API updates the cross sensitivity coefficient between gyroscope's + * X and Z axes. + */ +int8_t bmi2_get_gyro_cross_sense(struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + struct bmi2_sensor_data data; + + /* Check if the feature is supported by this variant */ + if (dev->variant_feature & BMI2_GYRO_CROSS_SENS_ENABLE) + { + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + /* Select the feature whose data is to be acquired */ + data.type = BMI2_GYRO_CROSS_SENSE; + + /* Get the respective data */ + rslt = bmi2_get_sensor_data(&data, 1, dev); + if (rslt == BMI2_OK) + { + /* Update the gyroscope cross sense value of z axis + * in the device structure + */ + dev->gyr_cross_sens_zx = data.sens_data.correction_factor_zx; + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + } + + return rslt; +} + +/*! + * @brief This API gets Error bits and message indicating internal status. + */ +int8_t bmi2_get_internal_status(uint8_t *int_stat, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (int_stat != NULL)) + { + /* Delay to read the internal status */ + dev->delay_us(20000, dev->intf_ptr); + + /* Get the error bits and message */ + rslt = bmi2_get_regs(BMI2_INTERNAL_STATUS_ADDR, int_stat, 1, dev); + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! @cond DOXYGEN_SUPRESS */ + +/* Suppressing doxygen warnings triggered for same static function names present across various sensor variant + * directories */ + +/*! + * @brief This API verifies and allows only the correct position to do Fast Offset Compensation for + * accelerometer & gyro. + */ +static int8_t verify_foc_position(uint8_t sens_list, + const struct bmi2_accel_foc_g_value *accel_g_axis, + struct bmi2_dev *dev) +{ + int8_t rslt; + + struct bmi2_sens_axes_data avg_foc_data = { 0 }; + struct bmi2_foc_temp_value temp_foc_data = { 0 }; + + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + /* Enable sensor */ + rslt = bmi2_sensor_enable(&sens_list, 1, dev); + } + + if (rslt == BMI2_OK) + { + + rslt = get_average_of_sensor_data(sens_list, &temp_foc_data, dev); + if (rslt == BMI2_OK) + { + if (sens_list == BMI2_ACCEL) + { + + /* Taking modulus to make negative values as positive */ + if ((accel_g_axis->x == 1) && (accel_g_axis->sign == 1)) + { + temp_foc_data.x = temp_foc_data.x * -1; + } + else if ((accel_g_axis->y == 1) && (accel_g_axis->sign == 1)) + { + temp_foc_data.y = temp_foc_data.y * -1; + } + else if ((accel_g_axis->z == 1) && (accel_g_axis->sign == 1)) + { + temp_foc_data.z = temp_foc_data.z * -1; + } + } + + /* Typecasting into 16bit */ + avg_foc_data.x = (int16_t)(temp_foc_data.x); + avg_foc_data.y = (int16_t)(temp_foc_data.y); + avg_foc_data.z = (int16_t)(temp_foc_data.z); + + rslt = validate_foc_position(sens_list, accel_g_axis, avg_foc_data, dev); + } + } + + return rslt; +} + +/*! @endcond */ + +/*! + * @brief This API performs Fast Offset Compensation for accelerometer. + */ +int8_t bmi2_perform_accel_foc(const struct bmi2_accel_foc_g_value *accel_g_value, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Structure to define the accelerometer configurations */ + struct bmi2_accel_config acc_cfg = { 0, 0, 0, 0 }; + + /* Variable to store status of advance power save */ + uint8_t aps = 0; + + /* Variable to store status of accelerometer enable */ + uint8_t acc_en = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (accel_g_value != NULL)) + { + /* Check for input validity */ + if ((((BMI2_ABS(accel_g_value->x)) + (BMI2_ABS(accel_g_value->y)) + (BMI2_ABS(accel_g_value->z))) == 1) && + ((accel_g_value->sign == 1) || (accel_g_value->sign == 0))) + { + rslt = verify_foc_position(BMI2_ACCEL, accel_g_value, dev); + if (rslt == BMI2_OK) + { + + /* Save accelerometer configurations, accelerometer + * enable status and advance power save status + */ + rslt = save_accel_foc_config(&acc_cfg, &aps, &acc_en, dev); + } + + /* Set configurations for FOC */ + if (rslt == BMI2_OK) + { + rslt = set_accel_foc_config(dev); + } + + /* Perform accelerometer FOC */ + if (rslt == BMI2_OK) + { + rslt = perform_accel_foc(accel_g_value, &acc_cfg, dev); + } + + /* Restore the saved configurations */ + if (rslt == BMI2_OK) + { + rslt = restore_accel_foc_config(&acc_cfg, aps, acc_en, dev); + } + } + else + { + rslt = BMI2_E_INVALID_INPUT; + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API performs Fast Offset Compensation for gyroscope. + */ +int8_t bmi2_perform_gyro_foc(struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Structure to define the gyroscope configurations */ + struct bmi2_gyro_config gyr_cfg = { 0, 0, 0, 0, 0, 0 }; + + /* Variable to store status of advance power save */ + uint8_t aps = 0; + + /* Variable to store status of gyroscope enable */ + uint8_t gyr_en = 0; + + /* Array of structure to store gyroscope data */ + struct bmi2_sens_axes_data gyr_value[128]; + + /* Structure to store gyroscope data temporarily */ + struct bmi2_foc_temp_value temp = { 0, 0, 0 }; + + /* Variable to store status read from the status register */ + uint8_t reg_status = 0; + + /* Variable to define count */ + uint8_t loop = 0; + + /* Structure to store the offset values to be stored in the register */ + struct bmi2_sens_axes_data gyro_offset = { 0, 0, 0, 0 }; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + /* Argument2 is not applicable for gyro */ + rslt = verify_foc_position(BMI2_GYRO, 0, dev); + if (rslt == BMI2_OK) + { + /* Save gyroscope configurations, gyroscope enable + * status and advance power save status + */ + rslt = save_gyro_config(&gyr_cfg, &aps, &gyr_en, dev); + + /* Set configurations for gyroscope FOC */ + if (rslt == BMI2_OK) + { + rslt = set_gyro_foc_config(dev); + } + + /* Perform FOC */ + if (rslt == BMI2_OK) + { + for (loop = 0; loop < 128; loop++) + { + /* Giving a delay of more than 40ms since ODR is configured as 25Hz */ + dev->delay_us(50000, dev->intf_ptr); + + /* Get gyroscope data ready interrupt status */ + rslt = bmi2_get_status(®_status, dev); + + /* Read 128 samples of gyroscope data on data ready interrupt */ + if ((rslt == BMI2_OK) && (reg_status & BMI2_DRDY_GYR)) + { + rslt = read_gyro_xyz(&gyr_value[loop], dev); + if (rslt == BMI2_OK) + { + /* Store the data in a temporary structure */ + temp.x = temp.x + (int32_t)gyr_value[loop].x; + temp.y = temp.y + (int32_t)gyr_value[loop].y; + temp.z = temp.z + (int32_t)gyr_value[loop].z; + } + } + + if (rslt != BMI2_OK) + { + break; + } + else if ((reg_status & BMI2_DRDY_GYR) != BMI2_DRDY_GYR) + { + rslt = BMI2_E_INVALID_STATUS; + break; + } + } + + if (rslt == BMI2_OK) + { + /* Take average of x, y and z data for lesser + * noise. It is same as offset data since lsb/dps + * is same for both data and offset register + */ + gyro_offset.x = (int16_t)(temp.x / 128); + gyro_offset.y = (int16_t)(temp.y / 128); + gyro_offset.z = (int16_t)(temp.z / 128); + + /* Saturate gyroscope data since the offset + * registers are of 10 bit value where as the + * gyroscope data is of 16 bit value + */ + saturate_gyro_data(&gyro_offset); + + /* Invert the gyroscope offset data */ + invert_gyro_offset(&gyro_offset); + + /* Write offset data in the gyroscope offset + * compensation register + */ + rslt = bmi2_write_gyro_offset_comp_axes(&gyro_offset, dev); + } + + /* Enable gyroscope offset compensation */ + if (rslt == BMI2_OK) + { + rslt = bmi2_set_gyro_offset_comp(BMI2_ENABLE, dev); + } + + /* Restore the saved gyroscope configurations */ + if (rslt == BMI2_OK) + { + rslt = restore_gyro_config(&gyr_cfg, aps, gyr_en, dev); + } + } + } + } + + return rslt; +} + +/*! + * @brief This API is used to get the feature configuration from the + * selected page. + */ +int8_t bmi2_get_feat_config(uint8_t sw_page, uint8_t *feat_config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define bytes remaining to read */ + uint8_t bytes_remain = BMI2_FEAT_SIZE_IN_BYTES; + + /* Variable to define the read-write length */ + uint8_t read_write_len = 0; + + /* Variable to define the feature configuration address */ + uint8_t addr = BMI2_FEATURES_REG_ADDR; + + /* Variable to define index */ + uint8_t index = 0; + + if ((feat_config == NULL) || (dev == NULL)) + { + rslt = BMI2_E_NULL_PTR; + } + else + { + /* Check whether the page is valid */ + if (sw_page < dev->page_max) + { + /* Switch page */ + rslt = bmi2_set_regs(BMI2_FEAT_PAGE_ADDR, &sw_page, 1, dev); + + /* If user length is less than feature length */ + if ((rslt == BMI2_OK) && (dev->read_write_len < BMI2_FEAT_SIZE_IN_BYTES)) + { + /* Read-write should be even */ + if ((dev->read_write_len % 2) != 0) + { + dev->read_write_len--; + } + + while (bytes_remain > 0) + { + if (bytes_remain >= dev->read_write_len) + { + /* Read from the page */ + rslt = bmi2_get_regs(addr, &feat_config[index], dev->read_write_len, dev); + + /* Update index */ + index += (uint8_t) dev->read_write_len; + + /* Update address */ + addr += (uint8_t) dev->read_write_len; + + /* Update read-write length */ + read_write_len += (uint8_t) dev->read_write_len; + } + else + { + /* Read from the page */ + rslt = bmi2_get_regs(addr, (uint8_t *) (feat_config + index), (uint16_t) bytes_remain, dev); + + /* Update read-write length */ + read_write_len += bytes_remain; + } + + /* Remaining bytes */ + bytes_remain = BMI2_FEAT_SIZE_IN_BYTES - read_write_len; + + if (rslt != BMI2_OK) + { + break; + } + } + } + else if (rslt == BMI2_OK) + { + /* Get configuration from the page */ + rslt = bmi2_get_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_PAGE; + } + } + + return rslt; +} + +/*! + * @brief This API is used to extract the input feature configuration + * details from the look-up table. + */ +uint8_t bmi2_extract_input_feat_config(struct bmi2_feature_config *feat_config, uint8_t type, + const struct bmi2_dev *dev) +{ + /* Variable to define loop */ + uint8_t loop = 0; + + /* Variable to set flag */ + uint8_t feat_found = BMI2_FALSE; + + /* Search for the input feature from the input configuration array */ + while (loop < dev->input_sens) + { + if (dev->feat_config[loop].type == type) + { + *feat_config = dev->feat_config[loop]; + feat_found = BMI2_TRUE; + break; + } + + loop++; + } + + /* Return flag */ + return feat_found; +} + +/***************************************************************************/ + +/*! Local Function Definitions + ****************************************************************************/ + +/*! @cond DOXYGEN_SUPRESS */ + +/* Suppressing doxygen warnings triggered for same static function names present across various sensor variant + * directories */ + +/*! + * @brief This internal API writes the configuration file. + */ +static int8_t write_config_file(struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to update the configuration file index */ + uint16_t index = 0; + + /* config file size */ + uint16_t config_size = dev->config_size; + + /* Variable to get the remainder */ + uint8_t remain = (uint8_t)(config_size % dev->read_write_len); + + /* Variable to get the balance bytes */ + uint16_t bal_byte = 0; + + /* Variable to define temporary read/write length */ + uint16_t read_write_len = 0; + + /* Disable advanced power save mode */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + /* Disable loading of the configuration */ + rslt = set_config_load(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + if (!remain) + { + /* Write the configuration file */ + for (index = 0; (index < config_size) && (rslt == BMI2_OK); index += dev->read_write_len) + { + rslt = upload_file((dev->config_file_ptr + index), index, dev->read_write_len, dev); + } + } + else + { + /* Get the balance bytes */ + bal_byte = (uint16_t) config_size - (uint16_t) remain; + + /* Write the configuration file for the balancem bytes */ + for (index = 0; (index < bal_byte) && (rslt == BMI2_OK); index += dev->read_write_len) + { + rslt = upload_file((dev->config_file_ptr + index), index, dev->read_write_len, dev); + } + + if (rslt == BMI2_OK) + { + /* Update length in a temporary variable */ + read_write_len = dev->read_write_len; + + /* Write the remaining bytes in 2 bytes length */ + dev->read_write_len = 2; + + /* Write the configuration file for the remaining bytes */ + for (index = bal_byte; + (index < config_size) && (rslt == BMI2_OK); + index += dev->read_write_len) + { + rslt = upload_file((dev->config_file_ptr + index), index, dev->read_write_len, dev); + } + + /* Restore the user set length back from the temporary variable */ + dev->read_write_len = read_write_len; + } + } + + if (rslt == BMI2_OK) + { + /* Enable loading of the configuration */ + rslt = set_config_load(BMI2_ENABLE, dev); + + /* Wait till ASIC is initialized */ + dev->delay_us(150000, dev->intf_ptr); + if (rslt == BMI2_OK) + { + /* Enable advanced power save mode */ + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + } + } + + return rslt; +} + +/*! + * @brief This internal API enables/disables the loading of the configuration + * file. + */ +static int8_t set_config_load(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store data */ + uint8_t reg_data = 0; + + rslt = bmi2_get_regs(BMI2_INIT_CTRL_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_CONF_LOAD_EN, enable); + rslt = bmi2_set_regs(BMI2_INIT_CTRL_ADDR, ®_data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This internal API loads the configuration file. + */ +static int8_t upload_file(const uint8_t *config_data, uint16_t index, uint16_t write_len, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to store address */ + uint8_t addr_array[2] = { 0 }; + + if (config_data != NULL) + { + /* Store 0 to 3 bits of address in first byte */ + addr_array[0] = (uint8_t)((index / 2) & 0x0F); + + /* Store 4 to 11 bits of address in the second byte */ + addr_array[1] = (uint8_t)((index / 2) >> 4); + + /* Write the 2 bytes of address in consecutive locations */ + rslt = bmi2_set_regs(BMI2_INIT_ADDR_0, addr_array, 2, dev); + if (rslt == BMI2_OK) + { + /* Burst write configuration file data corresponding to user set length */ + rslt = bmi2_set_regs(BMI2_INIT_DATA_ADDR, (uint8_t *)config_data, write_len, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This internal API validates bandwidth and performance mode of the + * accelerometer set by the user. + */ +static int8_t validate_bw_perf_mode(uint8_t *bandwidth, uint8_t *perf_mode, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Validate and auto-correct performance mode */ + rslt = check_boundary_val(perf_mode, BMI2_POWER_OPT_MODE, BMI2_PERF_OPT_MODE, dev); + if (rslt == BMI2_OK) + { + /* Validate and auto-correct bandwidth parameter */ + if (*perf_mode == BMI2_PERF_OPT_MODE) + { + /* Validate for continuous filter mode */ + rslt = check_boundary_val(bandwidth, BMI2_ACC_OSR4_AVG1, BMI2_ACC_CIC_AVG8, dev); + } + else + { + /* Validate for CIC averaging mode */ + rslt = check_boundary_val(bandwidth, BMI2_ACC_OSR4_AVG1, BMI2_ACC_RES_AVG128, dev); + } + } + + return rslt; +} + +/*! + * @brief This internal API validates ODR and range of the accelerometer set by + * the user. + */ +static int8_t validate_odr_range(uint8_t *odr, uint8_t *range, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Validate and auto correct ODR */ + rslt = check_boundary_val(odr, BMI2_ACC_ODR_0_78HZ, BMI2_ACC_ODR_1600HZ, dev); + if (rslt == BMI2_OK) + { + /* Validate and auto correct Range */ + rslt = check_boundary_val(range, BMI2_ACC_RANGE_2G, BMI2_ACC_RANGE_16G, dev); + } + + return rslt; +} + +/*! + * @brief This internal API validates bandwidth, performance mode, low power/ + * high performance mode, ODR, and range set by the user. + */ +static int8_t validate_gyro_config(struct bmi2_gyro_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Validate and auto-correct performance mode */ + rslt = check_boundary_val(&config->filter_perf, BMI2_POWER_OPT_MODE, BMI2_PERF_OPT_MODE, dev); + if (rslt == BMI2_OK) + { + /* Validate and auto-correct bandwidth parameter */ + rslt = check_boundary_val(&config->bwp, BMI2_GYR_OSR4_MODE, BMI2_GYR_CIC_MODE, dev); + if (rslt == BMI2_OK) + { + /* Validate and auto-correct low power/high-performance parameter */ + rslt = check_boundary_val(&config->noise_perf, BMI2_POWER_OPT_MODE, BMI2_PERF_OPT_MODE, dev); + if (rslt == BMI2_OK) + { + /* Validate and auto-correct ODR parameter */ + rslt = check_boundary_val(&config->odr, BMI2_GYR_ODR_25HZ, BMI2_GYR_ODR_3200HZ, dev); + if (rslt == BMI2_OK) + { + /* Validate and auto-correct OIS range */ + rslt = check_boundary_val(&config->ois_range, BMI2_GYR_OIS_250, BMI2_GYR_OIS_2000, dev); + if (rslt == BMI2_OK) + { + /* Validate and auto-correct range parameter */ + rslt = check_boundary_val(&config->range, BMI2_GYR_RANGE_2000, BMI2_GYR_RANGE_125, dev); + } + } + } + } + } + + return rslt; +} + +/*! + * @brief This internal API shows the error status when illegal sensor + * configuration is set. + */ +static int8_t cfg_error_status(struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store data */ + uint8_t reg_data; + + /* Get error status of the set sensor configuration */ + rslt = bmi2_get_regs(BMI2_EVENT_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + reg_data = BMI2_GET_BITS(reg_data, BMI2_EVENT_FLAG); + switch (reg_data) + { + case BMI2_NO_ERROR: + rslt = BMI2_OK; + break; + case BMI2_ACC_ERROR: + rslt = BMI2_E_ACC_INVALID_CFG; + break; + case BMI2_GYR_ERROR: + rslt = BMI2_E_GYRO_INVALID_CFG; + break; + case BMI2_ACC_GYR_ERROR: + rslt = BMI2_E_ACC_GYR_INVALID_CFG; + break; + default: + break; + } + } + + return rslt; +} + +/*! + * @brief This internal API: + * 1) Enables/Disables auxiliary interface. + * 2) Sets auxiliary interface configurations like I2C address, manual/auto + * mode enable, manual burst read length, AUX burst read length and AUX read + * address. + * 3)It maps/un-maps data interrupts to that of hardware interrupt line. + */ +static int8_t set_aux_config(struct bmi2_aux_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Validate auxiliary configurations */ + rslt = validate_aux_config(config, dev); + if (rslt == BMI2_OK) + { + /* Enable/Disable auxiliary interface */ + rslt = set_aux_interface(config, dev); + if (rslt == BMI2_OK) + { + /* Set the auxiliary interface configurations */ + rslt = config_aux_interface(config, dev); + if (rslt == BMI2_OK) + { + /* Set read out offset and ODR */ + rslt = config_aux(config, dev); + } + } + } + + return rslt; +} + +/*! + * @brief This internal API sets gyroscope user-gain configurations like gain + * update value for x, y and z-axis. + */ +static int8_t set_gyro_user_gain_config(const struct bmi2_gyro_user_gain_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to define index */ + uint8_t index = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for user-gain */ + struct bmi2_feature_config user_gain_config = { 0, 0, 0 }; + + /* Copy the feature configuration address to a local pointer */ + uint16_t *data_p = (uint16_t *) (void *)feat_config; + + /* Search for user-gain feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&user_gain_config, BMI2_GYRO_GAIN_UPDATE, dev); + if (feat_found) + { + /* Get the configuration from the page where user-gain feature resides */ + rslt = bmi2_get_feat_config(user_gain_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for user-gain select */ + idx = user_gain_config.start_addr; + + /* Get offset in words since all the features are set in words length */ + idx = idx / 2; + + /* Set ratio_x */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_GYR_USER_GAIN_RATIO_X, config->ratio_x); + + /* Increment offset by 1 word to set ratio_y */ + idx++; + + /* Set ratio_y */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_GYR_USER_GAIN_RATIO_Y, config->ratio_y); + + /* Increment offset by 1 word to set ratio_z */ + idx++; + + /* Set ratio_z */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_GYR_USER_GAIN_RATIO_Z, config->ratio_z); + + /* Increment offset by 1 more word to get the total length in words */ + idx++; + + /* Get total length in bytes to copy from local pointer to the array */ + idx = (uint8_t)(idx * 2) - user_gain_config.start_addr; + + /* Copy the bytes to be set back to the array */ + for (index = 0; index < idx; index++) + { + feat_config[user_gain_config.start_addr + + index] = *((uint8_t *) data_p + user_gain_config.start_addr + index); + } + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API enables/disables auxiliary interface. + */ +static int8_t set_aux_interface(const struct bmi2_aux_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store data */ + uint8_t reg_data; + + rslt = bmi2_get_regs(BMI2_IF_CONF_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + reg_data = BMI2_SET_BITS(reg_data, BMI2_AUX_IF_EN, config->aux_en); + + /* Enable/Disable auxiliary interface */ + rslt = bmi2_set_regs(BMI2_IF_CONF_ADDR, ®_data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This internal API sets auxiliary configurations like manual/auto mode + * FCU write command enable and read burst length for both data and manual mode. + * + * @note Auxiliary sensor should not be busy when configuring aux_i2c_addr, + * man_rd_burst_len, aux_rd_burst_len and aux_rd_addr. + */ +static int8_t config_aux_interface(const struct bmi2_aux_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store data */ + uint8_t reg_data[2] = { 0 }; + + /* Variable to store status */ + uint8_t status = 0; + + /* Variable to define count */ + uint8_t count = 0; + + rslt = bmi2_get_regs(BMI2_AUX_DEV_ID_ADDR, reg_data, 2, dev); + if (rslt == BMI2_OK) + { + /* Set I2C address for AUX sensor */ + reg_data[0] = BMI2_SET_BITS(reg_data[0], BMI2_AUX_SET_I2C_ADDR, config->i2c_device_addr); + + /* Set the AUX IF to either manual or auto mode */ + reg_data[1] = BMI2_SET_BITS(reg_data[1], BMI2_AUX_MAN_MODE_EN, config->manual_en); + + /* Enables FCU write command on AUX IF for auxiliary sensors that need a trigger */ + reg_data[1] = BMI2_SET_BITS(reg_data[1], BMI2_AUX_FCU_WR_EN, config->fcu_write_en); + + /* Set the burst read length for manual mode */ + reg_data[1] = BMI2_SET_BITS(reg_data[1], BMI2_AUX_MAN_READ_BURST, config->man_rd_burst); + + /* Set the burst read length for data mode */ + reg_data[1] = BMI2_SET_BIT_POS0(reg_data[1], BMI2_AUX_READ_BURST, config->aux_rd_burst); + for (;;) + { + /* Check if auxiliary sensor is busy */ + rslt = bmi2_get_status(&status, dev); + if ((rslt == BMI2_OK) && (!(status & BMI2_AUX_BUSY))) + { + /* Set the configurations if AUX is not busy */ + rslt = bmi2_set_regs(BMI2_AUX_DEV_ID_ADDR, reg_data, 2, dev); + dev->delay_us(1000, dev->intf_ptr); + if (rslt == BMI2_OK) + { + /* If data mode */ + if (!config->manual_en) + { + /* Disable manual enable flag in device structure */ + dev->aux_man_en = 0; + + /* Set the read address of the AUX sensor */ + rslt = bmi2_set_regs(BMI2_AUX_RD_ADDR, (uint8_t *) &config->read_addr, 1, dev); + dev->delay_us(1000, dev->intf_ptr); + } + else + { + /* Enable manual enable flag in device structure */ + dev->aux_man_en = 1; + + /* Update manual read burst length in device structure */ + dev->aux_man_rd_burst_len = config->man_rd_burst; + } + } + + /* Break after setting the register */ + break; + } + + /* Increment count after every 10 seconds */ + dev->delay_us(10000, dev->intf_ptr); + count++; + + /* Break after 2 seconds if AUX still busy - since slowest ODR is 0.78Hz*/ + if (count > 20) + { + rslt = BMI2_E_AUX_BUSY; + break; + } + } + } + + return rslt; +} + +/*! + * @brief This internal API triggers read out offset and sets ODR of the + * auxiliary sensor. + */ +static int8_t config_aux(const struct bmi2_aux_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store data */ + uint8_t reg_data; + + rslt = bmi2_get_regs(BMI2_AUX_CONF_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + /* Trigger read out offset */ + reg_data = BMI2_SET_BITS(reg_data, BMI2_AUX_OFFSET_READ_OUT, config->offset); + + /* Set ODR */ + reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_AUX_ODR_EN, config->odr); + + /* Set auxiliary configuration register */ + rslt = bmi2_set_regs(BMI2_AUX_CONF_ADDR, ®_data, 1, dev); + dev->delay_us(1000, dev->intf_ptr); + } + + return rslt; +} + +/*! + * @brief This internal API checks the busy status of auxiliary sensor and sets + * the auxiliary register addresses when not busy. + */ +static int8_t set_if_aux_not_busy(uint8_t reg_addr, uint8_t reg_data, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to get status of AUX_BUSY */ + uint8_t status = 0; + + /* Variable to define count for time-out */ + uint8_t count = 0; + + for (;;) + { + /* Check if AUX is busy */ + rslt = bmi2_get_status(&status, dev); + + /* Set the registers if not busy */ + if ((rslt == BMI2_OK) && (!(status & BMI2_AUX_BUSY))) + { + rslt = bmi2_set_regs(reg_addr, ®_data, 1, dev); + dev->delay_us(1000, dev->intf_ptr); + + /* Break after setting the register */ + break; + } + + /* Increment count after every 10 seconds */ + dev->delay_us(10000, dev->intf_ptr); + count++; + + /* Break after 2 seconds if AUX still busy - since slowest ODR is 0.78Hz*/ + if (count > 20) + { + rslt = BMI2_E_AUX_BUSY; + break; + } + } + + return rslt; +} + +/*! + * @brief This internal API validates auxiliary configuration set by the user. + */ +static int8_t validate_aux_config(struct bmi2_aux_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Validate ODR for auxiliary sensor */ + rslt = check_boundary_val(&config->odr, BMI2_AUX_ODR_0_78HZ, BMI2_AUX_ODR_800HZ, dev); + + return rslt; +} + +/*! + * @brief This internal API gets accelerometer configurations like ODR, + * bandwidth, performance mode and g-range. + */ +static int8_t get_accel_config(struct bmi2_accel_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to store data */ + uint8_t data_array[2] = { 0 }; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (config != NULL)) + { + /* Read the sensor configuration details */ + rslt = bmi2_get_regs(BMI2_ACC_CONF_ADDR, data_array, 2, dev); + if (rslt == BMI2_OK) + { + /* Get accelerometer performance mode */ + config->filter_perf = BMI2_GET_BITS(data_array[0], BMI2_ACC_FILTER_PERF_MODE); + + /* Get accelerometer bandwidth */ + config->bwp = BMI2_GET_BITS(data_array[0], BMI2_ACC_BW_PARAM); + + /* Get accelerometer ODR */ + config->odr = BMI2_GET_BIT_POS0(data_array[0], BMI2_ACC_ODR); + + /* Get accelerometer range */ + config->range = BMI2_GET_BIT_POS0(data_array[1], BMI2_ACC_RANGE); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This internal API gets gyroscope configurations like ODR, bandwidth, + * low power/high performance mode, performance mode and range. + */ +static int8_t get_gyro_config(struct bmi2_gyro_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to store data */ + uint8_t data_array[2] = { 0 }; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (config != NULL)) + { + /* Read the sensor configuration details */ + rslt = bmi2_get_regs(BMI2_GYR_CONF_ADDR, data_array, 2, dev); + if (rslt == BMI2_OK) + { + /* Get gyroscope performance mode */ + config->filter_perf = BMI2_GET_BITS(data_array[0], BMI2_GYR_FILTER_PERF_MODE); + + /* Get gyroscope noise performance mode */ + config->noise_perf = BMI2_GET_BITS(data_array[0], BMI2_GYR_NOISE_PERF_MODE); + + /* Get gyroscope bandwidth */ + config->bwp = BMI2_GET_BITS(data_array[0], BMI2_GYR_BW_PARAM); + + /* Get gyroscope ODR */ + config->odr = BMI2_GET_BIT_POS0(data_array[0], BMI2_GYR_ODR); + + /* Get gyroscope OIS range */ + config->ois_range = BMI2_GET_BITS(data_array[1], BMI2_GYR_OIS_RANGE); + + /* Get gyroscope range */ + config->range = BMI2_GET_BIT_POS0(data_array[1], BMI2_GYR_RANGE); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This internal API: + * 1) Gets the status of auxiliary interface enable. + * 2) Gets auxiliary interface configurations like I2C address, manual/auto + * mode enable, manual burst read length, AUX burst read length and AUX read + * address. + * 3) Gets ODR and offset. + */ +static int8_t get_aux_config(struct bmi2_aux_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (config != NULL)) + { + /* Get enable status of auxiliary interface */ + rslt = get_aux_interface(config, dev); + if (rslt == BMI2_OK) + { + /* Get the auxiliary interface configurations */ + rslt = get_aux_interface_config(config, dev); + if (rslt == BMI2_OK) + { + /* Get read out offset and ODR */ + rslt = get_aux_cfg(config, dev); + } + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This internal API gets gyroscope user-gain configurations like gain + * update value for x, y and z-axis. + */ +static int8_t get_gyro_gain_update_config(struct bmi2_gyro_user_gain_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to define LSB */ + uint16_t lsb = 0; + + /* Variable to define MSB */ + uint16_t msb = 0; + + /* Variable to define a word */ + uint16_t lsb_msb = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for user-gain */ + struct bmi2_feature_config user_gain_config = { 0, 0, 0 }; + + /* Search for user-gain feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&user_gain_config, BMI2_GYRO_GAIN_UPDATE, dev); + if (feat_found) + { + /* Get the configuration from the page where user-gain feature resides */ + rslt = bmi2_get_feat_config(user_gain_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for user-gain select */ + idx = user_gain_config.start_addr; + + /* Get word to calculate ratio_x */ + lsb = (uint16_t) feat_config[idx++]; + msb = ((uint16_t) feat_config[idx++] << 8); + lsb_msb = lsb | msb; + + /* Get ratio_x */ + config->ratio_x = lsb_msb & BMI2_GYR_USER_GAIN_RATIO_X_MASK; + + /* Get word to calculate ratio_y */ + lsb = (uint16_t) feat_config[idx++]; + msb = ((uint16_t) feat_config[idx++] << 8); + lsb_msb = lsb | msb; + + /* Get ratio_y */ + config->ratio_y = lsb_msb & BMI2_GYR_USER_GAIN_RATIO_Y_MASK; + + /* Get word to calculate ratio_z */ + lsb = (uint16_t) feat_config[idx++]; + msb = ((uint16_t) feat_config[idx++] << 8); + lsb_msb = lsb | msb; + + /* Get ratio_z */ + config->ratio_z = lsb_msb & BMI2_GYR_USER_GAIN_RATIO_Z_MASK; + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets the enable status of auxiliary interface. + */ +static int8_t get_aux_interface(struct bmi2_aux_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store data */ + uint8_t reg_data; + + /* Get the enable status of auxiliary interface */ + rslt = bmi2_get_regs(BMI2_IF_CONF_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + config->aux_en = BMI2_GET_BITS(reg_data, BMI2_AUX_IF_EN); + } + + return rslt; +} + +/*! + * @brief This internal API gets auxiliary configurations like manual/auto mode + * FCU write command enable and read burst length for both data and manual mode. + */ +static int8_t get_aux_interface_config(struct bmi2_aux_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store data */ + uint8_t reg_data[2] = { 0 }; + + rslt = bmi2_get_regs(BMI2_AUX_DEV_ID_ADDR, reg_data, 2, dev); + if (rslt == BMI2_OK) + { + /* Get I2C address for auxiliary sensor */ + config->i2c_device_addr = BMI2_GET_BITS(reg_data[0], BMI2_AUX_SET_I2C_ADDR); + + /* Get the AUX IF to either manual or auto mode */ + config->manual_en = BMI2_GET_BITS(reg_data[1], BMI2_AUX_MAN_MODE_EN); + + /* Enables FCU write command on AUX IF for auxiliary sensors that need a trigger */ + config->fcu_write_en = BMI2_GET_BITS(reg_data[1], BMI2_AUX_FCU_WR_EN); + + /* Get the burst read length for manual mode */ + config->man_rd_burst = BMI2_GET_BITS(reg_data[1], BMI2_AUX_MAN_READ_BURST); + + /* Get the burst read length for data mode */ + config->aux_rd_burst = BMI2_GET_BIT_POS0(reg_data[1], BMI2_AUX_READ_BURST); + + /* If data mode, get the read address of the auxiliary sensor from where data is to be read */ + if (!config->manual_en) + { + rslt = bmi2_get_regs(BMI2_AUX_RD_ADDR, &config->read_addr, 1, dev); + } + } + + return rslt; +} + +/*! + * @brief This internal API gets read out offset and ODR of the auxiliary + * sensor. + */ +static int8_t get_aux_cfg(struct bmi2_aux_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store data */ + uint8_t reg_data; + + rslt = bmi2_get_regs(BMI2_AUX_CONF_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + /* Get read out offset */ + config->offset = BMI2_GET_BITS(reg_data, BMI2_AUX_OFFSET_READ_OUT); + + /* Get ODR */ + config->odr = BMI2_GET_BIT_POS0(reg_data, BMI2_AUX_ODR_EN); + } + + return rslt; +} + +/*! + * @brief This internal API maps/un-maps feature interrupts to that of interrupt + * pins. + */ +static int8_t map_feat_int(uint8_t *reg_data_array, enum bmi2_hw_int_pin int_pin, uint8_t int_mask) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Check for NULL error */ + if (reg_data_array != NULL) + { + /* Check validity on interrupt pin selection */ + if (int_pin < BMI2_INT_PIN_MAX) + { + switch (int_pin) + { + case BMI2_INT_NONE: + + /* Un-Map the corresponding feature interrupt to interrupt pin 1 and 2 */ + reg_data_array[0] &= ~(int_mask); + reg_data_array[1] &= ~(int_mask); + break; + case BMI2_INT1: + + /* Map the corresponding feature interrupt to interrupt pin 1 */ + reg_data_array[0] |= int_mask; + + /* Un-map the corresponding feature interrupt to interrupt pin 2 */ + reg_data_array[1] &= ~(int_mask); + break; + case BMI2_INT2: + + /* Map the corresponding feature interrupt to interrupt pin 2 */ + reg_data_array[1] |= int_mask; + + /* Un-map the corresponding feature interrupt to interrupt pin 1 */ + reg_data_array[0] &= ~(int_mask); + break; + case BMI2_INT_BOTH: + + /* Map the corresponding feature interrupt to interrupt pin 1 and 2 */ + reg_data_array[0] |= int_mask; + reg_data_array[1] |= int_mask; + break; + default: + break; + } + } + else + { + /* Return error if invalid pin selection */ + rslt = BMI2_E_INVALID_INT_PIN; + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This internal API gets the accelerometer data from the register. + */ +static int8_t get_accel_sensor_data(struct bmi2_sens_axes_data *data, uint8_t reg_addr, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define data stored in register */ + uint8_t reg_data[BMI2_ACC_GYR_NUM_BYTES] = { 0 }; + + /* Read the sensor data */ + rslt = bmi2_get_regs(reg_addr, reg_data, BMI2_ACC_GYR_NUM_BYTES, dev); + if (rslt == BMI2_OK) + { + /* Get accelerometer data from the register */ + get_acc_gyr_data(data, reg_data); + + /* Get the re-mapped accelerometer data */ + get_remapped_data(data, dev); + } + + return rslt; +} + +/*! + * @brief This internal API gets the gyroscope data from the register. + */ +static int8_t get_gyro_sensor_data(struct bmi2_sens_axes_data *data, uint8_t reg_addr, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define data stored in register */ + uint8_t reg_data[BMI2_ACC_GYR_NUM_BYTES] = { 0 }; + + /* Read the sensor data */ + rslt = bmi2_get_regs(reg_addr, reg_data, BMI2_ACC_GYR_NUM_BYTES, dev); + if (rslt == BMI2_OK) + { + /* Get gyroscope data from the register */ + get_acc_gyr_data(data, reg_data); + + /* Get the compensated gyroscope data */ + comp_gyro_cross_axis_sensitivity(data, dev); + + /* Get the re-mapped gyroscope data */ + get_remapped_data(data, dev); + + } + + return rslt; +} + +/*! + * @brief This internal API gets the accelerometer/gyroscope data. + */ +static void get_acc_gyr_data(struct bmi2_sens_axes_data *data, const uint8_t *reg_data) +{ + /* Variables to store msb value */ + uint8_t msb; + + /* Variables to store lsb value */ + uint8_t lsb; + + /* Variables to store both msb and lsb value */ + uint16_t msb_lsb; + + /* Variables to define index */ + uint8_t index = 0; + + /* Read x-axis data */ + lsb = reg_data[index++]; + msb = reg_data[index++]; + msb_lsb = ((uint16_t) msb << 8) | (uint16_t) lsb; + data->x = (int16_t) msb_lsb; + + /* Read y-axis data */ + lsb = reg_data[index++]; + msb = reg_data[index++]; + msb_lsb = ((uint16_t) msb << 8) | (uint16_t) lsb; + data->y = (int16_t) msb_lsb; + + /* Read z-axis data */ + lsb = reg_data[index++]; + msb = reg_data[index++]; + msb_lsb = ((uint16_t) msb << 8) | (uint16_t) lsb; + data->z = (int16_t) msb_lsb; +} + +/*! + * @brief This internal API gets the re-mapped accelerometer/gyroscope data. + */ +static void get_remapped_data(struct bmi2_sens_axes_data *data, const struct bmi2_dev *dev) +{ + /* Array to defined the re-mapped sensor data */ + int16_t remap_data[3] = { 0 }; + int16_t pos_multiplier = INT16_C(1); + int16_t neg_multiplier = INT16_C(-1); + + /* Fill the array with the un-mapped sensor data */ + remap_data[0] = data->x; + remap_data[1] = data->y; + remap_data[2] = data->z; + + /* Get the re-mapped x axis data */ + if (dev->remap.x_axis_sign == BMI2_POS_SIGN) + { + data->x = (int16_t)(remap_data[dev->remap.x_axis] * pos_multiplier); + } + else + { + data->x = (int16_t)(remap_data[dev->remap.x_axis] * neg_multiplier); + } + + /* Get the re-mapped y axis data */ + if (dev->remap.y_axis_sign == BMI2_POS_SIGN) + { + data->y = (int16_t)(remap_data[dev->remap.y_axis] * pos_multiplier); + } + else + { + data->y = (int16_t)(remap_data[dev->remap.y_axis] * neg_multiplier); + } + + /* Get the re-mapped z axis data */ + if (dev->remap.z_axis_sign == BMI2_POS_SIGN) + { + data->z = (int16_t)(remap_data[dev->remap.z_axis] * pos_multiplier); + } + else + { + data->z = (int16_t)(remap_data[dev->remap.z_axis] * neg_multiplier); + } +} + +/*! + * @brief This internal API reads the user-defined bytes of data from the given + * register address of auxiliary sensor in manual mode. + */ +static int8_t read_aux_data(uint8_t reg_addr, uint8_t *aux_data, uint16_t len, uint8_t burst_len, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Array to store the register data */ + uint8_t reg_data[15] = { 0 }; + + /* Variable to define number of bytes to read */ + uint16_t read_length = 0; + + /* Variable to define loop */ + uint8_t loop = 0; + + /* Variable to define counts to get the correct array index */ + uint8_t count = 0; + + /* Variable to define index for the array */ + uint8_t idx = 0; + + while (len > 0) + { + /* Set the read address if AUX is not busy */ + rslt = set_if_aux_not_busy(BMI2_AUX_RD_ADDR, reg_addr, dev); + if (rslt == BMI2_OK) + { + /* Read data from bmi2 data register */ + rslt = bmi2_get_regs(BMI2_AUX_X_LSB_ADDR, reg_data, (uint16_t) burst_len, dev); + dev->delay_us(1000, dev->intf_ptr); + if (rslt == BMI2_OK) + { + /* Get number of bytes to be read */ + if (len < burst_len) + { + read_length = (uint8_t) len; + } + else + { + read_length = burst_len; + } + + /* Update array index and store the data */ + for (loop = 0; loop < read_length; loop++) + { + idx = loop + count; + aux_data[idx] = reg_data[loop]; + } + } + } + + /* Update address for the next read */ + reg_addr += burst_len; + + /* Update count for the array index */ + count += burst_len; + + /* Update no of bytes left to read */ + len -= read_length; + } + + return rslt; +} + +/*! + * @brief This internal API writes AUX write address and the user-defined bytes + * of data to the AUX sensor in manual mode. + * + * @note Change of BMI2_AUX_WR_ADDR is only allowed if AUX is not busy. + */ +static int8_t write_aux_data(uint8_t reg_addr, uint8_t reg_data, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Write data to be written to the AUX sensor in bmi2 register */ + rslt = bmi2_set_regs(BMI2_AUX_WR_DATA_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + /* Write the AUX address where data is to be stored when AUX is not busy */ + rslt = set_if_aux_not_busy(BMI2_AUX_WR_ADDR, reg_addr, dev); + } + + return rslt; +} + +/*! + * @brief This internal API reads the user-defined bytes of data from the given + * register address of auxiliary sensor in data mode. + */ +static int8_t read_aux_data_mode(uint8_t *aux_data, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variables to define loop */ + uint8_t count = 0; + + /* Variables to define index */ + uint8_t index = 0; + + /* Array to define data stored in register */ + uint8_t reg_data[BMI2_AUX_NUM_BYTES] = { 0 }; + + /* Check if data mode */ + if (!dev->aux_man_en) + { + /* Read the auxiliary sensor data */ + rslt = bmi2_get_regs(BMI2_AUX_X_LSB_ADDR, reg_data, BMI2_AUX_NUM_BYTES, dev); + if (rslt == BMI2_OK) + { + /* Get the 8 bytes of auxiliary data */ + do + { + *(aux_data + count++) = *(reg_data + index++); + } while (count < BMI2_AUX_NUM_BYTES); + } + } + else + { + rslt = BMI2_E_AUX_INVALID_CFG; + } + + return rslt; +} + +/*! + * @brief This internal API maps the actual burst read length with that of the + * register value set by user. + */ +static int8_t map_read_len(uint8_t *len, const struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Get the burst read length against the values set by the user */ + switch (dev->aux_man_rd_burst_len) + { + case BMI2_AUX_READ_LEN_0: + *len = 1; + break; + case BMI2_AUX_READ_LEN_1: + *len = 2; + break; + case BMI2_AUX_READ_LEN_2: + *len = 6; + break; + case BMI2_AUX_READ_LEN_3: + *len = 8; + break; + default: + rslt = BMI2_E_AUX_INVALID_CFG; + break; + } + + return rslt; +} + +/*! + * @brief This internal API computes the number of bytes of accelerometer FIFO + * data which is to be parsed in header-less mode. + */ +static int8_t parse_fifo_accel_len(uint16_t *start_idx, + uint16_t *len, + const uint16_t *acc_count, + const struct bmi2_fifo_frame *fifo) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Data start index */ + (*start_idx) = fifo->acc_byte_start_idx; + + /* If only accelerometer is enabled */ + if (fifo->data_enable == BMI2_FIFO_ACC_EN) + { + /* Number of bytes to be read */ + (*len) = (uint16_t)((*acc_count) * BMI2_FIFO_ACC_LENGTH); + } + /* If only accelerometer and auxiliary are enabled */ + else if (fifo->data_enable == (BMI2_FIFO_ACC_EN | BMI2_FIFO_AUX_EN)) + { + /* Number of bytes to be read */ + (*len) = (uint16_t)((*acc_count) * BMI2_FIFO_ACC_AUX_LENGTH); + } + /* If only accelerometer and gyroscope are enabled */ + else if (fifo->data_enable == (BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN)) + { + /* Number of bytes to be read */ + (*len) = (uint16_t)((*acc_count) * BMI2_FIFO_ACC_GYR_LENGTH); + } + /* If only accelerometer, gyroscope and auxiliary are enabled */ + else if (fifo->data_enable == (BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN | BMI2_FIFO_AUX_EN)) + { + /* Number of bytes to be read */ + (*len) = (uint16_t)((*acc_count) * BMI2_FIFO_ALL_LENGTH); + } + else + { + /* Move the data index to the last byte to mark completion when + * no sensors or sensors apart from accelerometer are enabled + */ + (*start_idx) = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + } + + /* If more data is requested than available */ + if ((*len) > fifo->length) + { + (*len) = fifo->length; + } + + return rslt; +} + +/*! + * @brief This internal API is used to parse the accelerometer data from the + * FIFO in header mode. + */ +static int8_t extract_accel_header_mode(struct bmi2_sens_axes_data *acc, + uint16_t *accel_length, + struct bmi2_fifo_frame *fifo, + const struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Variable to define header frame */ + uint8_t frame_header = 0; + + /* Variable to index the data bytes */ + uint16_t data_index; + + /* Variable to index accelerometer frames */ + uint16_t accel_index = 0; + + /* Variable to indicate accelerometer frames read */ + uint16_t frame_to_read = *accel_length; + + for (data_index = fifo->acc_byte_start_idx; data_index < fifo->length;) + { + /* Get frame header byte */ + frame_header = fifo->data[data_index] & BMI2_FIFO_TAG_INTR_MASK; + + /* Parse virtual header if S4S is enabled */ + parse_if_virtual_header(&frame_header, &data_index, fifo); + + /* Index shifted to next byte where data starts */ + data_index++; + switch (frame_header) + { + /* If header defines accelerometer frame */ + case BMI2_FIFO_HEADER_ACC_FRM: + case BMI2_FIFO_HEADER_AUX_ACC_FRM: + case BMI2_FIFO_HEADER_GYR_ACC_FRM: + case BMI2_FIFO_HEADER_ALL_FRM: + + /* Unpack from normal frames */ + rslt = unpack_accel_frame(acc, &data_index, &accel_index, frame_header, fifo, dev); + break; + + /* If header defines only gyroscope frame */ + case BMI2_FIFO_HEADER_GYR_FRM: + rslt = move_next_frame(&data_index, fifo->gyr_frm_len, fifo); + break; + + /* If header defines only auxiliary frame */ + case BMI2_FIFO_HEADER_AUX_FRM: + rslt = move_next_frame(&data_index, fifo->aux_frm_len, fifo); + break; + + /* If header defines only auxiliary and gyroscope frame */ + case BMI2_FIFO_HEADER_AUX_GYR_FRM: + rslt = move_next_frame(&data_index, fifo->aux_gyr_frm_len, fifo); + break; + + /* If header defines sensor time frame */ + case BMI2_FIFO_HEADER_SENS_TIME_FRM: + rslt = unpack_sensortime_frame(&data_index, fifo); + break; + + /* If header defines skip frame */ + case BMI2_FIFO_HEADER_SKIP_FRM: + rslt = unpack_skipped_frame(&data_index, fifo); + break; + + /* If header defines Input configuration frame */ + case BMI2_FIFO_HEADER_INPUT_CFG_FRM: + rslt = move_next_frame(&data_index, BMI2_FIFO_INPUT_CFG_LENGTH, fifo); + break; + + /* If header defines invalid frame or end of valid data */ + case BMI2_FIFO_HEAD_OVER_READ_MSB: + + /* Move the data index to the last byte to mark completion */ + data_index = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + break; + case BMI2_FIFO_VIRT_ACT_RECOG_FRM: + rslt = move_next_frame(&data_index, BMI2_FIFO_VIRT_ACT_DATA_LENGTH, fifo); + break; + default: + + /* Move the data index to the last byte in case of invalid values */ + data_index = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + break; + } + + /* Break if Number of frames to be read is complete or FIFO is mpty */ + if ((frame_to_read == accel_index) || (rslt == BMI2_W_FIFO_EMPTY)) + { + break; + } + } + + /* Update the accelerometer frame index */ + (*accel_length) = accel_index; + + /* Update the accelerometer byte index */ + fifo->acc_byte_start_idx = data_index; + + return rslt; +} + +/*! + * @brief This internal API is used to parse the accelerometer data from the + * FIFO data in both header and header-less mode. It updates the current data + * byte to be parsed. + */ +static int8_t unpack_accel_frame(struct bmi2_sens_axes_data *acc, + uint16_t *idx, + uint16_t *acc_idx, + uint8_t frame, + const struct bmi2_fifo_frame *fifo, + const struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + switch (frame) + { + /* If frame contains only accelerometer data */ + case BMI2_FIFO_HEADER_ACC_FRM: + case BMI2_FIFO_HEAD_LESS_ACC_FRM: + + /* Partially read, then skip the data */ + if (((*idx) + fifo->acc_frm_len) > fifo->length) + { + /* Update the data index as complete*/ + (*idx) = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + break; + } + + /* Get the accelerometer data */ + unpack_accel_data(&acc[(*acc_idx)], *idx, fifo, dev); + + /* Update data index */ + (*idx) = (*idx) + BMI2_FIFO_ACC_LENGTH; + + /* Get virtual sensor time if S4S is enabled */ + if (dev->sens_en_stat & BMI2_EXT_SENS_SEL) + { + unpack_virt_sensor_time(&acc[(*acc_idx)], idx, fifo); + } + + /* Update accelerometer frame index */ + (*acc_idx)++; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + break; + + /* If frame contains accelerometer and gyroscope data */ + case BMI2_FIFO_HEADER_GYR_ACC_FRM: + case BMI2_FIFO_HEAD_LESS_GYR_ACC_FRM: + + /* Partially read, then skip the data */ + if (((*idx) + fifo->acc_gyr_frm_len) > fifo->length) + { + /* Move the data index to the last byte */ + (*idx) = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + break; + } + + /* Get the accelerometer data */ + unpack_accel_data(&acc[(*acc_idx)], ((*idx) + BMI2_FIFO_GYR_LENGTH), fifo, dev); + + /* Update data index */ + (*idx) = (*idx) + BMI2_FIFO_ACC_GYR_LENGTH; + + /* Get virtual sensor time if S4S is enabled */ + if (dev->sens_en_stat & BMI2_EXT_SENS_SEL) + { + unpack_virt_sensor_time(&acc[(*acc_idx)], idx, fifo); + } + + /* Update accelerometer frame index */ + (*acc_idx)++; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + break; + + /* If frame contains accelerometer and auxiliary data */ + case BMI2_FIFO_HEADER_AUX_ACC_FRM: + case BMI2_FIFO_HEAD_LESS_AUX_ACC_FRM: + + /* Partially read, then skip the data */ + if (((*idx) + fifo->acc_aux_frm_len) > fifo->length) + { + /* Move the data index to the last byte */ + (*idx) = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + break; + } + + /* Get the accelerometer data */ + unpack_accel_data(&acc[(*acc_idx)], ((*idx) + BMI2_FIFO_AUX_LENGTH), fifo, dev); + + /* Update data index */ + (*idx) = (*idx) + BMI2_FIFO_ACC_AUX_LENGTH; + + /* Get virtual sensor time if S4S is enabled */ + if (dev->sens_en_stat & BMI2_EXT_SENS_SEL) + { + unpack_virt_sensor_time(&acc[(*acc_idx)], idx, fifo); + } + + /* Update accelerometer frame index */ + (*acc_idx)++; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + break; + + /* If frame contains accelerometer, gyroscope and auxiliary data */ + case BMI2_FIFO_HEADER_ALL_FRM: + case BMI2_FIFO_HEAD_LESS_ALL_FRM: + + /* Partially read, then skip the data*/ + if ((*idx + fifo->all_frm_len) > fifo->length) + { + /* Move the data index to the last byte */ + (*idx) = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + break; + } + + /* Get the accelerometer data */ + unpack_accel_data(&acc[(*acc_idx)], ((*idx) + BMI2_FIFO_GYR_AUX_LENGTH), fifo, dev); + + /* Update data index */ + (*idx) = (*idx) + BMI2_FIFO_ALL_LENGTH; + + /* Get virtual sensor time if S4S is enabled */ + if (dev->sens_en_stat & BMI2_EXT_SENS_SEL) + { + unpack_virt_sensor_time(&acc[(*acc_idx)], idx, fifo); + } + + /* Update accelerometer frame index */ + (*acc_idx)++; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + break; + + /* If frame contains gyroscope and auxiliary data */ + case BMI2_FIFO_HEADER_AUX_GYR_FRM: + case BMI2_FIFO_HEAD_LESS_GYR_AUX_FRM: + + /* Update data index */ + (*idx) = (*idx) + fifo->aux_gyr_frm_len; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + break; + + /* If frame contains only auxiliary data */ + case BMI2_FIFO_HEADER_AUX_FRM: + case BMI2_FIFO_HEAD_LESS_AUX_FRM: + + /* Update data index */ + (*idx) = (*idx) + fifo->aux_frm_len; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + break; + + /* If frame contains only gyroscope data */ + case BMI2_FIFO_HEADER_GYR_FRM: + case BMI2_FIFO_HEAD_LESS_GYR_FRM: + + /* Update data index */ + (*idx) = (*idx) + fifo->gyr_frm_len; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + break; + default: + + /* Move the data index to the last byte in case of invalid values */ + (*idx) = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + break; + } + + return rslt; +} + +/*! + * @brief This internal API is used to parse accelerometer data from the + * FIFO data. + */ +static void unpack_accel_data(struct bmi2_sens_axes_data *acc, + uint16_t data_start_index, + const struct bmi2_fifo_frame *fifo, + const struct bmi2_dev *dev) +{ + /* Variables to store LSB value */ + uint16_t data_lsb; + + /* Variables to store MSB value */ + uint16_t data_msb; + + /* Accelerometer raw x data */ + data_lsb = fifo->data[data_start_index++]; + data_msb = fifo->data[data_start_index++]; + acc->x = (int16_t)((data_msb << 8) | data_lsb); + + /* Accelerometer raw y data */ + data_lsb = fifo->data[data_start_index++]; + data_msb = fifo->data[data_start_index++]; + acc->y = (int16_t)((data_msb << 8) | data_lsb); + + /* Accelerometer raw z data */ + data_lsb = fifo->data[data_start_index++]; + data_msb = fifo->data[data_start_index++]; + acc->z = (int16_t)((data_msb << 8) | data_lsb); + + /* Get the re-mapped accelerometer data */ + get_remapped_data(acc, dev); +} + +/*! + * @brief This internal API computes the number of bytes of gyroscope FIFO data + * which is to be parsed in header-less mode. + */ +static int8_t parse_fifo_gyro_len(uint16_t *start_idx, + uint16_t(*len), + const uint16_t *gyr_count, + const struct bmi2_fifo_frame *fifo) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Data start index */ + (*start_idx) = fifo->gyr_byte_start_idx; + + /* If only gyroscope is enabled */ + if (fifo->data_enable == BMI2_FIFO_GYR_EN) + { + /* Number of bytes to be read */ + (*len) = (uint16_t)((*gyr_count) * BMI2_FIFO_GYR_LENGTH); + } + /* If only gyroscope and auxiliary are enabled */ + else if (fifo->data_enable == (BMI2_FIFO_GYR_EN | BMI2_FIFO_AUX_EN)) + { + /* Number of bytes to be read */ + (*len) = (uint16_t)((*gyr_count) * BMI2_FIFO_GYR_AUX_LENGTH); + } + /* If only accelerometer and gyroscope are enabled */ + else if (fifo->data_enable == (BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN)) + { + /* Number of bytes to be read */ + (*len) = (uint16_t)((*gyr_count) * BMI2_FIFO_ACC_GYR_LENGTH); + } + /* If only accelerometer, gyroscope and auxiliary are enabled */ + else if (fifo->data_enable == (BMI2_FIFO_GYR_EN | BMI2_FIFO_AUX_EN | BMI2_FIFO_ACC_EN)) + { + /* Number of bytes to be read */ + (*len) = (uint16_t)((*gyr_count) * BMI2_FIFO_ALL_LENGTH); + } + else + { + /* Move the data index to the last byte to mark completion when + * no sensors or sensors apart from gyroscope are enabled + */ + (*start_idx) = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + } + + /* If more data is requested than available */ + if (((*len)) > fifo->length) + { + (*len) = fifo->length; + } + + return rslt; +} + +/*! + * @brief This internal API is used to parse the gyroscope data from the + * FIFO data in header mode. + */ +static int8_t extract_gyro_header_mode(struct bmi2_sens_axes_data *gyr, + uint16_t *gyro_length, + struct bmi2_fifo_frame *fifo, + const struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Variable to define header frame */ + uint8_t frame_header = 0; + + /* Variable to index the data bytes */ + uint16_t data_index; + + /* Variable to index gyroscope frames */ + uint16_t gyro_index = 0; + + /* Variable to indicate gyroscope frames read */ + uint16_t frame_to_read = (*gyro_length); + + for (data_index = fifo->gyr_byte_start_idx; data_index < fifo->length;) + { + /* Get frame header byte */ + frame_header = fifo->data[data_index] & BMI2_FIFO_TAG_INTR_MASK; + + /* Parse virtual header if S4S is enabled */ + parse_if_virtual_header(&frame_header, &data_index, fifo); + + /* Index shifted to next byte where data starts */ + data_index++; + switch (frame_header) + { + /* If header defines gyroscope frame */ + case BMI2_FIFO_HEADER_GYR_FRM: + case BMI2_FIFO_HEADER_GYR_ACC_FRM: + case BMI2_FIFO_HEADER_AUX_GYR_FRM: + case BMI2_FIFO_HEADER_ALL_FRM: + + /* Unpack from normal frames */ + rslt = unpack_gyro_frame(gyr, &data_index, &gyro_index, frame_header, fifo, dev); + break; + + /* If header defines only accelerometer frame */ + case BMI2_FIFO_HEADER_ACC_FRM: + rslt = move_next_frame(&data_index, fifo->acc_frm_len, fifo); + break; + + /* If header defines only auxiliary frame */ + case BMI2_FIFO_HEADER_AUX_FRM: + rslt = move_next_frame(&data_index, fifo->aux_frm_len, fifo); + break; + + /* If header defines only auxiliary and accelerometer frame */ + case BMI2_FIFO_HEADER_AUX_ACC_FRM: + rslt = move_next_frame(&data_index, fifo->acc_aux_frm_len, fifo); + break; + + /* If header defines sensor time frame */ + case BMI2_FIFO_HEADER_SENS_TIME_FRM: + rslt = unpack_sensortime_frame(&data_index, fifo); + break; + + /* If header defines skip frame */ + case BMI2_FIFO_HEADER_SKIP_FRM: + rslt = unpack_skipped_frame(&data_index, fifo); + break; + + /* If header defines Input configuration frame */ + case BMI2_FIFO_HEADER_INPUT_CFG_FRM: + rslt = move_next_frame(&data_index, BMI2_FIFO_INPUT_CFG_LENGTH, fifo); + break; + + /* If header defines invalid frame or end of valid data */ + case BMI2_FIFO_HEAD_OVER_READ_MSB: + + /* Move the data index to the last byte */ + data_index = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + break; + case BMI2_FIFO_VIRT_ACT_RECOG_FRM: + rslt = move_next_frame(&data_index, BMI2_FIFO_VIRT_ACT_DATA_LENGTH, fifo); + break; + default: + + /* Move the data index to the last byte in case of invalid values */ + data_index = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + break; + } + + /* Break if number of frames to be read is complete or FIFO is empty */ + if ((frame_to_read == gyro_index) || (rslt == BMI2_W_FIFO_EMPTY)) + { + break; + } + } + + /* Update the gyroscope frame index */ + (*gyro_length) = gyro_index; + + /* Update the gyroscope byte index */ + fifo->gyr_byte_start_idx = data_index; + + return rslt; +} + +/*! + * @brief This internal API is used to parse the gyroscope data from the FIFO + * data in both header and header-less mode. It updates the current data byte to + * be parsed. + */ +static int8_t unpack_gyro_frame(struct bmi2_sens_axes_data *gyr, + uint16_t *idx, + uint16_t *gyr_idx, + uint8_t frame, + const struct bmi2_fifo_frame *fifo, + const struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + switch (frame) + { + /* If frame contains only gyroscope data */ + case BMI2_FIFO_HEADER_GYR_FRM: + case BMI2_FIFO_HEAD_LESS_GYR_FRM: + + /* Partially read, then skip the data */ + if (((*idx) + fifo->gyr_frm_len) > fifo->length) + { + /* Update the data index as complete*/ + (*idx) = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + break; + } + + /* Get the gyroscope data */ + unpack_gyro_data(&gyr[(*gyr_idx)], *idx, fifo, dev); + + /* Update data index */ + (*idx) = (*idx) + BMI2_FIFO_GYR_LENGTH; + + /* Get virtual sensor time if S4S is enabled */ + if (dev->sens_en_stat & BMI2_EXT_SENS_SEL) + { + unpack_virt_sensor_time(&gyr[(*gyr_idx)], idx, fifo); + } + + /* Update gyroscope frame index */ + (*gyr_idx)++; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + break; + + /* If frame contains accelerometer and gyroscope data */ + case BMI2_FIFO_HEADER_GYR_ACC_FRM: + case BMI2_FIFO_HEAD_LESS_GYR_ACC_FRM: + + /* Partially read, then skip the data */ + if (((*idx) + fifo->acc_gyr_frm_len) > fifo->length) + { + /* Move the data index to the last byte */ + (*idx) = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + break; + } + + /* Get the gyroscope data */ + unpack_gyro_data(&gyr[(*gyr_idx)], (*idx), fifo, dev); + + /* Update data index */ + (*idx) = (*idx) + BMI2_FIFO_ACC_GYR_LENGTH; + + /* Get virtual sensor time if S4S is enabled */ + if (dev->sens_en_stat & BMI2_EXT_SENS_SEL) + { + unpack_virt_sensor_time(&gyr[(*gyr_idx)], idx, fifo); + } + + /* Update gyroscope frame index */ + (*gyr_idx)++; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + break; + + /* If frame contains gyroscope and auxiliary data */ + case BMI2_FIFO_HEADER_AUX_GYR_FRM: + case BMI2_FIFO_HEAD_LESS_GYR_AUX_FRM: + + /* Partially read, then skip the data */ + if (((*idx) + fifo->aux_gyr_frm_len) > fifo->length) + { + /* Move the data index to the last byte */ + (*idx) = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + break; + } + + /* Get the gyroscope data */ + unpack_gyro_data(&gyr[(*gyr_idx)], ((*idx) + BMI2_FIFO_AUX_LENGTH), fifo, dev); + + /* Update data index */ + (*idx) = (*idx) + BMI2_FIFO_GYR_AUX_LENGTH; + + /* Get virtual sensor time if S4S is enabled */ + if (dev->sens_en_stat & BMI2_EXT_SENS_SEL) + { + unpack_virt_sensor_time(&gyr[(*gyr_idx)], idx, fifo); + } + + /* Update gyroscope frame index */ + (*gyr_idx)++; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + break; + + /* If frame contains accelerometer, gyroscope and auxiliary data */ + case BMI2_FIFO_HEADER_ALL_FRM: + case BMI2_FIFO_HEAD_LESS_ALL_FRM: + + /* Partially read, then skip the data*/ + if ((*idx + fifo->all_frm_len) > fifo->length) + { + /* Move the data index to the last byte */ + (*idx) = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + break; + } + + /* Get the gyroscope data */ + unpack_gyro_data(&gyr[(*gyr_idx)], ((*idx) + BMI2_FIFO_AUX_LENGTH), fifo, dev); + + /* Update data index */ + (*idx) = (*idx) + BMI2_FIFO_ALL_LENGTH; + + /* Get virtual sensor time if S4S is enabled */ + if (dev->sens_en_stat & BMI2_EXT_SENS_SEL) + { + unpack_virt_sensor_time(&gyr[(*gyr_idx)], idx, fifo); + } + + /* Update gyroscope frame index */ + (*gyr_idx)++; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + break; + + /* If frame contains accelerometer and auxiliary data */ + case BMI2_FIFO_HEADER_AUX_ACC_FRM: + case BMI2_FIFO_HEAD_LESS_AUX_ACC_FRM: + + /* Update data index */ + (*idx) = (*idx) + fifo->acc_aux_frm_len; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + break; + + /* If frame contains only auxiliary data */ + case BMI2_FIFO_HEADER_AUX_FRM: + case BMI2_FIFO_HEAD_LESS_AUX_FRM: + + /* Update data index */ + (*idx) = (*idx) + fifo->aux_frm_len; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + break; + + /* If frame contains only accelerometer data */ + case BMI2_FIFO_HEADER_ACC_FRM: + case BMI2_FIFO_HEAD_LESS_ACC_FRM: + + /* Update data index */ + (*idx) = (*idx) + fifo->acc_frm_len; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + break; + default: + + /* Move the data index to the last byte in case of invalid values */ + (*idx) = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + break; + } + + return rslt; +} + +/*! + * @brief This internal API is used to parse gyroscope data from the FIFO data. + */ +static void unpack_gyro_data(struct bmi2_sens_axes_data *gyr, + uint16_t data_start_index, + const struct bmi2_fifo_frame *fifo, + const struct bmi2_dev *dev) +{ + /* Variables to store LSB value */ + uint16_t data_lsb; + + /* Variables to store MSB value */ + uint16_t data_msb; + + /* Gyroscope raw x data */ + data_lsb = fifo->data[data_start_index++]; + data_msb = fifo->data[data_start_index++]; + gyr->x = (int16_t)((data_msb << 8) | data_lsb); + + /* Gyroscope raw y data */ + data_lsb = fifo->data[data_start_index++]; + data_msb = fifo->data[data_start_index++]; + gyr->y = (int16_t)((data_msb << 8) | data_lsb); + + /* Gyroscope raw z data */ + data_lsb = fifo->data[data_start_index++]; + data_msb = fifo->data[data_start_index++]; + gyr->z = (int16_t)((data_msb << 8) | data_lsb); + + /* Get the compensated gyroscope data */ + comp_gyro_cross_axis_sensitivity(gyr, dev); + + /* Get the re-mapped gyroscope data */ + get_remapped_data(gyr, dev); +} + +/*! + * @brief This API computes the number of bytes of auxiliary FIFO data which is + * to be parsed in header-less mode. + */ +static int8_t parse_fifo_aux_len(uint16_t *start_idx, + uint16_t(*len), + const uint16_t *aux_count, + const struct bmi2_fifo_frame *fifo) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Data start index */ + *start_idx = fifo->aux_byte_start_idx; + + /* If only auxiliary is enabled */ + if (fifo->data_enable == BMI2_FIFO_AUX_EN) + { + /* Number of bytes to be read */ + (*len) = (uint16_t)((*aux_count) * BMI2_FIFO_AUX_LENGTH); + } + /* If only accelerometer and auxiliary are enabled */ + else if (fifo->data_enable == (BMI2_FIFO_AUX_EN | BMI2_FIFO_ACC_EN)) + { + /* Number of bytes to be read */ + (*len) = (uint16_t)((*aux_count) * BMI2_FIFO_ACC_AUX_LENGTH); + } + /* If only accelerometer and gyroscope are enabled */ + else if (fifo->data_enable == (BMI2_FIFO_AUX_EN | BMI2_FIFO_GYR_EN)) + { + /* Number of bytes to be read */ + (*len) = (uint16_t)((*aux_count) * BMI2_FIFO_GYR_AUX_LENGTH); + } + /* If only accelerometer, gyroscope and auxiliary are enabled */ + else if (fifo->data_enable == (BMI2_FIFO_AUX_EN | BMI2_FIFO_GYR_EN | BMI2_FIFO_ACC_EN)) + { + /* Number of bytes to be read */ + (*len) = (uint16_t)((*aux_count) * BMI2_FIFO_ALL_LENGTH); + } + else + { + /* Move the data index to the last byte to mark completion when + * no sensors or sensors apart from gyroscope are enabled + */ + (*start_idx) = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + } + + /* If more data is requested than available */ + if (((*len)) > fifo->length) + { + (*len) = fifo->length; + } + + return rslt; +} + +/*! + * @brief This API is used to parse the auxiliary data from the FIFO data in + * header mode. + */ +static int8_t extract_aux_header_mode(struct bmi2_aux_fifo_data *aux, + uint16_t *aux_len, + struct bmi2_fifo_frame *fifo, + const struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Variable to define header frame */ + uint8_t frame_header = 0; + + /* Variable to index the data bytes */ + uint16_t data_index; + + /* Variable to index gyroscope frames */ + uint16_t aux_index = 0; + + /* Variable to indicate auxiliary frames read */ + uint16_t frame_to_read = *aux_len; + + for (data_index = fifo->aux_byte_start_idx; data_index < fifo->length;) + { + /* Get frame header byte */ + frame_header = fifo->data[data_index] & BMI2_FIFO_TAG_INTR_MASK; + + /* Parse virtual header if S4S is enabled */ + parse_if_virtual_header(&frame_header, &data_index, fifo); + + /* Index shifted to next byte where data starts */ + data_index++; + switch (frame_header) + { + /* If header defines auxiliary frame */ + case BMI2_FIFO_HEADER_AUX_FRM: + case BMI2_FIFO_HEADER_AUX_ACC_FRM: + case BMI2_FIFO_HEADER_AUX_GYR_FRM: + case BMI2_FIFO_HEADER_ALL_FRM: + + /* Unpack from normal frames */ + rslt = unpack_aux_frame(aux, &data_index, &aux_index, frame_header, fifo, dev); + break; + + /* If header defines only accelerometer frame */ + case BMI2_FIFO_HEADER_ACC_FRM: + rslt = move_next_frame(&data_index, fifo->acc_frm_len, fifo); + break; + + /* If header defines only gyroscope frame */ + case BMI2_FIFO_HEADER_GYR_FRM: + rslt = move_next_frame(&data_index, fifo->gyr_frm_len, fifo); + break; + + /* If header defines only gyroscope and accelerometer frame */ + case BMI2_FIFO_HEADER_GYR_ACC_FRM: + rslt = move_next_frame(&data_index, fifo->acc_gyr_frm_len, fifo); + break; + + /* If header defines sensor time frame */ + case BMI2_FIFO_HEADER_SENS_TIME_FRM: + rslt = unpack_sensortime_frame(&data_index, fifo); + break; + + /* If header defines skip frame */ + case BMI2_FIFO_HEADER_SKIP_FRM: + rslt = unpack_skipped_frame(&data_index, fifo); + break; + + /* If header defines Input configuration frame */ + case BMI2_FIFO_HEADER_INPUT_CFG_FRM: + rslt = move_next_frame(&data_index, BMI2_FIFO_INPUT_CFG_LENGTH, fifo); + break; + + /* If header defines invalid frame or end of valid data */ + case BMI2_FIFO_HEAD_OVER_READ_MSB: + + /* Move the data index to the last byte */ + data_index = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + break; + case BMI2_FIFO_VIRT_ACT_RECOG_FRM: + rslt = move_next_frame(&data_index, BMI2_FIFO_VIRT_ACT_DATA_LENGTH, fifo); + break; + default: + + /* Move the data index to the last byte in case + * of invalid values + */ + data_index = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + break; + } + + /* Break if number of frames to be read is complete or FIFO is + * empty + */ + if ((frame_to_read == aux_index) || (rslt == BMI2_W_FIFO_EMPTY)) + { + break; + } + } + + /* Update the gyroscope frame index */ + (*aux_len) = aux_index; + + /* Update the gyroscope byte index */ + fifo->aux_byte_start_idx = data_index; + + return rslt; +} + +/*! + * @brief This API is used to parse the auxiliary frame from the FIFO data in + * both header mode and header-less mode and update the data_index value which + * is used to store the index of the current data byte which is parsed. + */ +static int8_t unpack_aux_frame(struct bmi2_aux_fifo_data *aux, + uint16_t *idx, + uint16_t *aux_idx, + uint8_t frame, + const struct bmi2_fifo_frame *fifo, + const struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + switch (frame) + { + /* If frame contains only auxiliary data */ + case BMI2_FIFO_HEADER_AUX_FRM: + case BMI2_FIFO_HEAD_LESS_AUX_FRM: + + /* Partially read, then skip the data */ + if (((*idx) + fifo->aux_frm_len) > fifo->length) + { + /* Update the data index as complete*/ + (*idx) = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + break; + } + + /* Get the auxiliary data */ + unpack_aux_data(&aux[(*aux_idx)], (*idx), fifo); + + /* Update data index */ + (*idx) = (*idx) + BMI2_FIFO_AUX_LENGTH; + + /* Get virtual sensor time if S4S is enabled */ + if (dev->sens_en_stat & BMI2_EXT_SENS_SEL) + { + unpack_virt_aux_sensor_time(&aux[(*aux_idx)], idx, fifo); + } + + /* Update auxiliary frame index */ + (*aux_idx)++; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + break; + + /* If frame contains accelerometer and auxiliary data */ + case BMI2_FIFO_HEADER_AUX_ACC_FRM: + case BMI2_FIFO_HEAD_LESS_AUX_ACC_FRM: + + /* Partially read, then skip the data */ + if (((*idx) + fifo->acc_aux_frm_len) > fifo->length) + { + /* Move the data index to the last byte */ + (*idx) = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + break; + } + + /* Get the auxiliary data */ + unpack_aux_data(&aux[(*aux_idx)], (*idx), fifo); + + /* Update data index */ + (*idx) = (*idx) + BMI2_FIFO_ACC_AUX_LENGTH; + + /* Get virtual sensor time if S4S is enabled */ + if (dev->sens_en_stat & BMI2_EXT_SENS_SEL) + { + unpack_virt_aux_sensor_time(&aux[(*aux_idx)], idx, fifo); + } + + /* Update auxiliary frame index */ + (*aux_idx)++; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + break; + + /* If frame contains gyroscope and auxiliary data */ + case BMI2_FIFO_HEADER_AUX_GYR_FRM: + case BMI2_FIFO_HEAD_LESS_GYR_AUX_FRM: + + /* Partially read, then skip the data */ + if (((*idx) + fifo->aux_gyr_frm_len) > fifo->length) + { + /* Move the data index to the last byte */ + (*idx) = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + break; + } + + /* Get the auxiliary data */ + unpack_aux_data(&aux[(*aux_idx)], (*idx), fifo); + + /* Update data index */ + (*idx) = (*idx) + BMI2_FIFO_GYR_AUX_LENGTH; + + /* Get virtual sensor time if S4S is enabled */ + if (dev->sens_en_stat & BMI2_EXT_SENS_SEL) + { + unpack_virt_aux_sensor_time(&aux[(*aux_idx)], idx, fifo); + } + + /* Update auxiliary frame index */ + (*aux_idx)++; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + break; + + /* If frame contains accelerometer, gyroscope and auxiliary data */ + case BMI2_FIFO_HEADER_ALL_FRM: + case BMI2_FIFO_HEAD_LESS_ALL_FRM: + + /* Partially read, then skip the data */ + if ((*idx + fifo->all_frm_len) > fifo->length) + { + /* Move the data index to the last byte */ + (*idx) = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + break; + } + + /* Get the auxiliary data */ + unpack_aux_data(&aux[(*aux_idx)], (*idx), fifo); + + /* Update data index */ + (*idx) = (*idx) + BMI2_FIFO_ALL_LENGTH; + + /* Get virtual sensor time if S4S is enabled */ + if (dev->sens_en_stat & BMI2_EXT_SENS_SEL) + { + unpack_virt_aux_sensor_time(&aux[(*aux_idx)], idx, fifo); + } + + /* Update auxiliary frame index */ + (*aux_idx)++; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + break; + + /* If frame contains only accelerometer data */ + case BMI2_FIFO_HEADER_ACC_FRM: + case BMI2_FIFO_HEAD_LESS_ACC_FRM: + + /* Update data index */ + (*idx) = (*idx) + fifo->acc_frm_len; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + break; + + /* If frame contains only gyroscope data */ + case BMI2_FIFO_HEADER_GYR_FRM: + case BMI2_FIFO_HEAD_LESS_GYR_FRM: + + /* Update data index */ + (*idx) = (*idx) + fifo->gyr_frm_len; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + break; + + /* If frame contains accelerometer and gyroscope data */ + case BMI2_FIFO_HEADER_GYR_ACC_FRM: + case BMI2_FIFO_HEAD_LESS_GYR_ACC_FRM: + + /* Update data index */ + (*idx) = (*idx) + fifo->acc_gyr_frm_len; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + break; + default: + + /* Move the data index to the last byte in case of + * invalid values + */ + (*idx) = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + break; + } + + return rslt; +} + +/*! + * @brief This internal API is used to parse auxiliary data from the FIFO data. + */ +static void unpack_aux_data(struct bmi2_aux_fifo_data *aux, + uint16_t data_start_index, + const struct bmi2_fifo_frame *fifo) +{ + /* Variables to store index */ + uint16_t idx = 0; + + /* Get auxiliary data */ + for (; idx < 8; idx++) + { + aux->data[idx] = fifo->data[data_start_index++]; + } +} + +/*! + * @brief This internal API parses virtual frame header from the FIFO data. + */ +static void parse_if_virtual_header(uint8_t *frame_header, uint16_t *data_index, const struct bmi2_fifo_frame *fifo) +{ + /* Variable to extract virtual header byte */ + uint8_t virtual_header_mode; + + /* Extract virtual header mode from the frame header */ + virtual_header_mode = BMI2_GET_BITS(*frame_header, BMI2_FIFO_VIRT_FRM_MODE); + + /* If the extracted header byte is a virtual header */ + if (virtual_header_mode == BMI2_FIFO_VIRT_FRM_MODE) + { + /* If frame header is not activity recognition header */ + if (*frame_header != 0xC8) + { + /* Index shifted to next byte where sensor frame is present */ + (*data_index) = (*data_index) + 1; + + /* Get the sensor frame header */ + *frame_header = fifo->data[*data_index] & BMI2_FIFO_TAG_INTR_MASK; + } + } +} + +/*! + * @brief This internal API gets sensor time from the accelerometer and + * gyroscope virtual frames and updates in the data structure. + */ +static void unpack_virt_sensor_time(struct bmi2_sens_axes_data *sens, uint16_t *idx, const struct bmi2_fifo_frame *fifo) +{ + /* Variables to define 3 bytes of sensor time */ + uint32_t sensor_time_byte3; + uint16_t sensor_time_byte2; + uint8_t sensor_time_byte1; + + /* Get sensor time from the FIFO data */ + sensor_time_byte3 = (uint32_t)(fifo->data[(*idx) + BMI2_SENSOR_TIME_MSB_BYTE] << 16); + sensor_time_byte2 = (uint16_t) fifo->data[(*idx) + BMI2_SENSOR_TIME_XLSB_BYTE] << 8; + sensor_time_byte1 = fifo->data[(*idx)]; + + /* Store sensor time in the sensor data structure */ + sens->virt_sens_time = (uint32_t)(sensor_time_byte3 | sensor_time_byte2 | sensor_time_byte1); + + /* Move the data index by 3 bytes */ + (*idx) = (*idx) + BMI2_SENSOR_TIME_LENGTH; +} + +/*! + * @brief This internal API gets sensor time from the auxiliary virtual + * frames and updates in the data structure. + */ +static void unpack_virt_aux_sensor_time(struct bmi2_aux_fifo_data *aux, + uint16_t *idx, + const struct bmi2_fifo_frame *fifo) +{ + /* Variables to define 3 bytes of sensor time */ + uint32_t sensor_time_byte3; + uint16_t sensor_time_byte2; + uint8_t sensor_time_byte1; + + /* Get sensor time from the FIFO data */ + sensor_time_byte3 = (uint32_t)(fifo->data[(*idx) + BMI2_SENSOR_TIME_MSB_BYTE] << 16); + sensor_time_byte2 = (uint16_t) fifo->data[(*idx) + BMI2_SENSOR_TIME_XLSB_BYTE] << 8; + sensor_time_byte1 = fifo->data[(*idx)]; + + /* Store sensor time in the sensor data structure */ + aux->virt_sens_time = (uint32_t)(sensor_time_byte3 | sensor_time_byte2 | sensor_time_byte1); + + /* Move the data index by 3 bytes */ + (*idx) = (*idx) + BMI2_SENSOR_TIME_LENGTH; +} + +/*! + * @brief This internal API is used to reset the FIFO related configurations in + * the FIFO frame structure for the next FIFO read. + */ +static void reset_fifo_frame_structure(struct bmi2_fifo_frame *fifo, const struct bmi2_dev *dev) +{ + /* Reset FIFO data structure */ + fifo->acc_byte_start_idx = 0; + fifo->aux_byte_start_idx = 0; + fifo->gyr_byte_start_idx = 0; + fifo->sensor_time = 0; + fifo->skipped_frame_count = 0; + fifo->act_recog_byte_start_idx = 0; + + /* If S4S is enabled */ + if ((dev->sens_en_stat & BMI2_EXT_SENS_SEL) == BMI2_EXT_SENS_SEL) + { + fifo->acc_frm_len = BMI2_FIFO_VIRT_ACC_LENGTH; + fifo->gyr_frm_len = BMI2_FIFO_VIRT_GYR_LENGTH; + fifo->aux_frm_len = BMI2_FIFO_VIRT_AUX_LENGTH; + fifo->acc_gyr_frm_len = BMI2_FIFO_VIRT_ACC_GYR_LENGTH; + fifo->acc_aux_frm_len = BMI2_FIFO_VIRT_ACC_AUX_LENGTH; + fifo->aux_gyr_frm_len = BMI2_FIFO_VIRT_GYR_AUX_LENGTH; + fifo->all_frm_len = BMI2_FIFO_VIRT_ALL_LENGTH; + + /* If S4S is not enabled */ + } + else + { + fifo->acc_frm_len = BMI2_FIFO_ACC_LENGTH; + fifo->gyr_frm_len = BMI2_FIFO_GYR_LENGTH; + fifo->aux_frm_len = BMI2_FIFO_AUX_LENGTH; + fifo->acc_gyr_frm_len = BMI2_FIFO_ACC_GYR_LENGTH; + fifo->acc_aux_frm_len = BMI2_FIFO_ACC_AUX_LENGTH; + fifo->aux_gyr_frm_len = BMI2_FIFO_GYR_AUX_LENGTH; + fifo->all_frm_len = BMI2_FIFO_ALL_LENGTH; + } +} + +/*! + * @brief This API internal checks whether the FIFO data read is an empty frame. + * If empty frame, index is moved to the last byte. + */ +static int8_t check_empty_fifo(uint16_t *data_index, const struct bmi2_fifo_frame *fifo) +{ + /* Variables to define error */ + int8_t rslt = BMI2_OK; + + /* Validate data index */ + if (((*data_index) + 6) < fifo->length) + { + /* Check if FIFO is empty */ + if (((fifo->data[(*data_index)] == BMI2_FIFO_MSB_CONFIG_CHECK) && + (fifo->data[(*data_index) + 1] == BMI2_FIFO_LSB_CONFIG_CHECK)) && + ((fifo->data[(*data_index) + 2] == BMI2_FIFO_MSB_CONFIG_CHECK) && + (fifo->data[(*data_index) + 3] == BMI2_FIFO_LSB_CONFIG_CHECK)) && + ((fifo->data[(*data_index) + 4] == BMI2_FIFO_MSB_CONFIG_CHECK) && + (fifo->data[(*data_index) + 5] == BMI2_FIFO_LSB_CONFIG_CHECK))) + { + /* Move the data index to the last byte to mark completion */ + (*data_index) = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + } + else + { + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + } + } + + return rslt; +} + +/*! + * @brief This internal API is used to move the data index ahead of the + * current_frame_length parameter when unnecessary FIFO data appears while + * extracting the user specified data. + */ +static int8_t move_next_frame(uint16_t *data_index, uint8_t current_frame_length, const struct bmi2_fifo_frame *fifo) +{ + /* Variables to define error */ + int8_t rslt = BMI2_OK; + + /* Validate data index */ + if (((*data_index) + current_frame_length) > fifo->length) + { + /* Move the data index to the last byte */ + (*data_index) = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + } + else + { + /* Move the data index to next frame */ + (*data_index) = (*data_index) + current_frame_length; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + } + + return rslt; +} + +/*! + * @brief This internal API is used to parse and store the sensor time from the + * FIFO data. + */ +static int8_t unpack_sensortime_frame(uint16_t *data_index, struct bmi2_fifo_frame *fifo) +{ + /* Variables to define error */ + int8_t rslt = BMI2_OK; + + /* Variables to define 3 bytes of sensor time */ + uint32_t sensor_time_byte3 = 0; + uint16_t sensor_time_byte2 = 0; + uint8_t sensor_time_byte1 = 0; + + /* Validate data index */ + if (((*data_index) + BMI2_SENSOR_TIME_LENGTH) > fifo->length) + { + /* Move the data index to the last byte */ + (*data_index) = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + } + else + { + /* Get sensor time from the FIFO data */ + sensor_time_byte3 = fifo->data[(*data_index) + BMI2_SENSOR_TIME_MSB_BYTE] << 16; + sensor_time_byte2 = fifo->data[(*data_index) + BMI2_SENSOR_TIME_XLSB_BYTE] << 8; + sensor_time_byte1 = fifo->data[(*data_index)]; + + /* Update sensor time in the FIFO structure */ + fifo->sensor_time = (uint32_t)(sensor_time_byte3 | sensor_time_byte2 | sensor_time_byte1); + + /* Move the data index by 3 bytes */ + (*data_index) = (*data_index) + BMI2_SENSOR_TIME_LENGTH; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + } + + return rslt; +} + +/*! + * @brief This internal API is used to parse and store the skipped frame count + * from the FIFO data. + */ +static int8_t unpack_skipped_frame(uint16_t *data_index, struct bmi2_fifo_frame *fifo) +{ + /* Variables to define error */ + int8_t rslt = BMI2_OK; + + /* Validate data index */ + if ((*data_index) >= fifo->length) + { + /* Update the data index to the last byte */ + (*data_index) = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + } + else + { + /* Update skipped frame count in the FIFO structure */ + fifo->skipped_frame_count = fifo->data[(*data_index)]; + + /* Move the data index by 1 byte */ + (*data_index) = (*data_index) + 1; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + } + + return rslt; +} + +/*! + * @brief This internal API enables and configures the accelerometer which is + * needed for self-test operation. It also sets the amplitude for the self-test. + */ +static int8_t pre_self_test_config(struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Structure to define sensor configurations */ + struct bmi2_sens_config sens_cfg; + + /* List the sensors to be selected */ + uint8_t sens_sel = BMI2_ACCEL; + + /* Enable accelerometer */ + rslt = bmi2_sensor_enable(&sens_sel, 1, dev); + dev->delay_us(1000, dev->intf_ptr); + + /* Enable self-test amplitude */ + if (rslt == BMI2_OK) + { + rslt = set_accel_self_test_amp(BMI2_ENABLE, dev); + } + + if (rslt == BMI2_OK) + { + /* Select accelerometer for sensor configurations */ + sens_cfg.type = BMI2_ACCEL; + + /* Get the default values */ + rslt = bmi2_get_sensor_config(&sens_cfg, 1, dev); + if (rslt == BMI2_OK) + { + /* Set the configurations required for self-test */ + sens_cfg.cfg.acc.odr = BMI2_ACC_ODR_1600HZ; + sens_cfg.cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + sens_cfg.cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + sens_cfg.cfg.acc.range = BMI2_ACC_RANGE_16G; + + /* Set accelerometer configurations */ + rslt = bmi2_set_sensor_config(&sens_cfg, 1, dev); + } + } + + return rslt; +} + +/*! + * @brief This internal API performs the steps needed for self-test operation + * before reading the accelerometer self-test data. + */ +static int8_t self_test_config(uint8_t sign, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Enable the accelerometer self-test feature */ + rslt = set_accel_self_test_enable(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + /* Selects the sign of accelerometer self-test excitation */ + rslt = set_acc_self_test_sign(sign, dev); + } + + return rslt; +} + +/*! + * @brief This internal API enables or disables the accelerometer self-test + * feature in the sensor. + */ +static int8_t set_accel_self_test_enable(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define data */ + uint8_t data = 0; + + /* Enable/Disable self-test feature */ + rslt = bmi2_get_regs(BMI2_ACC_SELF_TEST_ADDR, &data, 1, dev); + if (rslt == BMI2_OK) + { + data = BMI2_SET_BIT_POS0(data, BMI2_ACC_SELF_TEST_EN, enable); + rslt = bmi2_set_regs(BMI2_ACC_SELF_TEST_ADDR, &data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This internal API selects the sign for accelerometer self-test + * excitation. + */ +static int8_t set_acc_self_test_sign(uint8_t sign, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define data */ + uint8_t data = 0; + + /* Select the sign for self-test excitation */ + rslt = bmi2_get_regs(BMI2_ACC_SELF_TEST_ADDR, &data, 1, dev); + if (rslt == BMI2_OK) + { + data = BMI2_SET_BITS(data, BMI2_ACC_SELF_TEST_SIGN, sign); + rslt = bmi2_set_regs(BMI2_ACC_SELF_TEST_ADDR, &data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This internal API sets the amplitude of the accelerometer self-test + * deflection in the sensor. + */ +static int8_t set_accel_self_test_amp(uint8_t amp, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define data */ + uint8_t data = 0; + + /* Select amplitude of the self-test deflection */ + rslt = bmi2_get_regs(BMI2_ACC_SELF_TEST_ADDR, &data, 1, dev); + if (rslt == BMI2_OK) + { + data = BMI2_SET_BITS(data, BMI2_ACC_SELF_TEST_AMP, amp); + rslt = bmi2_set_regs(BMI2_ACC_SELF_TEST_ADDR, &data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This internal API reads the accelerometer data for x,y and z axis from + * the sensor. The data units is in LSB format. + */ +static int8_t read_accel_xyz(struct bmi2_sens_axes_data *accel, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define LSB */ + uint16_t lsb = 0; + + /* Variable to define MSB */ + uint16_t msb = 0; + + /* Array to define data buffer */ + uint8_t data[BMI2_ACC_GYR_NUM_BYTES] = { 0 }; + + rslt = bmi2_get_regs(BMI2_ACC_X_LSB_ADDR, data, BMI2_ACC_GYR_NUM_BYTES, dev); + if (rslt == BMI2_OK) + { + /* Accelerometer data x axis */ + msb = data[1]; + lsb = data[0]; + accel->x = (int16_t)((msb << 8) | lsb); + + /* Accelerometer data y axis */ + msb = data[3]; + lsb = data[2]; + accel->y = (int16_t)((msb << 8) | lsb); + + /* Accelerometer data z axis */ + msb = data[5]; + lsb = data[4]; + accel->z = (int16_t)((msb << 8) | lsb); + } + + return rslt; +} + +/*! + * @brief This internal API reads the gyroscope data for x, y and z axis from + * the sensor. The data units is in LSB format. + */ +static int8_t read_gyro_xyz(struct bmi2_sens_axes_data *gyro, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define LSB */ + uint16_t lsb = 0; + + /* Variable to define MSB */ + uint16_t msb = 0; + + /* Array to define data buffer */ + uint8_t data[BMI2_ACC_GYR_NUM_BYTES] = { 0 }; + + rslt = bmi2_get_regs(BMI2_GYR_X_LSB_ADDR, data, BMI2_ACC_GYR_NUM_BYTES, dev); + if (rslt == BMI2_OK) + { + /* Gyroscope data x axis */ + msb = data[1]; + lsb = data[0]; + gyro->x = (int16_t)((msb << 8) | lsb); + + /* Gyroscope data y axis */ + msb = data[3]; + lsb = data[2]; + gyro->y = (int16_t)((msb << 8) | lsb); + + /* Gyroscope data z axis */ + msb = data[5]; + lsb = data[4]; + gyro->z = (int16_t)((msb << 8) | lsb); + } + + return rslt; +} + +/*! + * @brief This internal API converts LSB value of accelerometer axes to form + * 'g' to 'mg' for self-test. + */ +static void convert_lsb_g(const struct bmi2_selftest_delta_limit *acc_data_diff, + struct bmi2_selftest_delta_limit *acc_data_diff_mg, + const struct bmi2_dev *dev) +{ + /* Variable to define LSB/g value of axes */ + uint32_t lsb_per_g; + + /* Range considered for self-test is +/-16g */ + uint8_t range = BMI2_ACC_SELF_TEST_RANGE; + + /* lsb_per_g for the respective resolution and 16g range */ + lsb_per_g = (uint32_t)(power(2, dev->resolution) / (2 * range)); + + /* Accelerometer x value in mg */ + acc_data_diff_mg->x = (acc_data_diff->x / (int32_t) lsb_per_g) * 1000; + + /* Accelerometer y value in mg */ + acc_data_diff_mg->y = (acc_data_diff->y / (int32_t) lsb_per_g) * 1000; + + /* Accelerometer z value in mg */ + acc_data_diff_mg->z = (acc_data_diff->z / (int32_t) lsb_per_g) * 1000; +} + +/*! + * @brief This internal API is used to calculate the power of a value. + */ +static int32_t power(int16_t base, uint8_t resolution) +{ + /* Initialize loop */ + uint8_t loop = 1; + + /* Initialize variable to store the power of 2 value */ + int32_t value = 1; + + for (; loop <= resolution; loop++) + { + value = (int32_t)(value * base); + } + + return value; +} + +/*! + * @brief This internal API validates the accelerometer self-test data and + * decides the result of self-test operation. + */ +static int8_t validate_self_test(const struct bmi2_selftest_delta_limit *accel_data_diff) +{ + /* Variable to define error */ + int8_t rslt; + + /* As per the data sheet, The actually measured signal differences should be significantly + * larger than the minimum differences for each axis in order for the self-test to pass. + */ + if ((accel_data_diff->x > BMI2_ST_ACC_X_SIG_MIN_DIFF) && (accel_data_diff->y < BMI2_ST_ACC_Y_SIG_MIN_DIFF) && + (accel_data_diff->z > BMI2_ST_ACC_Z_SIG_MIN_DIFF)) + { + /* Self-test pass */ + rslt = BMI2_OK; + } + else + { + /* Self-test fail*/ + rslt = BMI2_E_SELF_TEST_FAIL; + } + + return rslt; +} + +/*! + * @brief This internal API gets the re-mapped x, y and z axes from the sensor. + */ +static int8_t get_remap_axes(struct bmi2_axes_remap *remap, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for axis re-mapping */ + struct bmi2_feature_config remap_config = { 0, 0, 0 }; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat; + + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if enabled */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + if (rslt == BMI2_OK) + { + /* Search for axis re-mapping and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&remap_config, BMI2_AXIS_MAP, dev); + if (feat_found) + { + rslt = bmi2_get_feat_config(remap_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for axis re-mapping */ + idx = remap_config.start_addr; + + /* Get the re-mapped x-axis */ + remap->x_axis = BMI2_GET_BIT_POS0(feat_config[idx], BMI2_X_AXIS); + + /* Get the re-mapped x-axis polarity */ + remap->x_axis_sign = BMI2_GET_BITS(feat_config[idx], BMI2_X_AXIS_SIGN); + + /* Get the re-mapped y-axis */ + remap->y_axis = BMI2_GET_BITS(feat_config[idx], BMI2_Y_AXIS); + + /* Get the re-mapped y-axis polarity */ + remap->y_axis_sign = BMI2_GET_BITS(feat_config[idx], BMI2_Y_AXIS_SIGN); + + /* Get the re-mapped z-axis */ + remap->z_axis = BMI2_GET_BITS(feat_config[idx], BMI2_Z_AXIS); + + /* Increment byte to fetch the next data */ + idx++; + + /* Get the re-mapped z-axis polarity */ + remap->z_axis_sign = BMI2_GET_BIT_POS0(feat_config[idx], BMI2_Z_AXIS_SIGN); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + /* Enable Advance power save if disabled while configuring and + * not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + + return rslt; +} + +/*! + * @brief This internal API sets the re-mapped x, y and z axes in the sensor. + */ +static int8_t set_remap_axes(const struct bmi2_axes_remap *remap, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to define the register address */ + uint8_t reg_addr = 0; + + /* Variable to set the re-mapped x-axes in the sensor */ + uint8_t x_axis = 0; + + /* Variable to set the re-mapped y-axes in the sensor */ + uint8_t y_axis = 0; + + /* Variable to set the re-mapped z-axes in the sensor */ + uint8_t z_axis = 0; + + /* Variable to set the re-mapped x-axes sign in the sensor */ + uint8_t x_axis_sign = 0; + + /* Variable to set the re-mapped y-axes sign in the sensor */ + uint8_t y_axis_sign = 0; + + /* Variable to set the re-mapped z-axes sign in the sensor */ + uint8_t z_axis_sign = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for axis re-mapping */ + struct bmi2_feature_config remap_config = { 0, 0, 0 }; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat; + + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if enabled */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + if (rslt == BMI2_OK) + { + /* Search for axis-re-mapping and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&remap_config, BMI2_AXIS_MAP, dev); + if (feat_found) + { + /* Get the configuration from the page where axis re-mapping feature resides */ + rslt = bmi2_get_feat_config(remap_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes */ + idx = remap_config.start_addr; + + /* Set the value of re-mapped x-axis */ + x_axis = remap->x_axis & BMI2_X_AXIS_MASK; + + /* Set the value of re-mapped x-axis sign */ + x_axis_sign = ((remap->x_axis_sign << BMI2_X_AXIS_SIGN_POS) & BMI2_X_AXIS_SIGN_MASK); + + /* Set the value of re-mapped y-axis */ + y_axis = ((remap->y_axis << BMI2_Y_AXIS_POS) & BMI2_Y_AXIS_MASK); + + /* Set the value of re-mapped y-axis sign */ + y_axis_sign = ((remap->y_axis_sign << BMI2_Y_AXIS_SIGN_POS) & BMI2_Y_AXIS_SIGN_MASK); + + /* Set the value of re-mapped z-axis */ + z_axis = ((remap->z_axis << BMI2_Z_AXIS_POS) & BMI2_Z_AXIS_MASK); + + /* Set the value of re-mapped z-axis sign */ + z_axis_sign = remap->z_axis_sign & BMI2_Z_AXIS_SIGN_MASK; + + /* Arrange axes in the first byte */ + feat_config[idx] = x_axis | x_axis_sign | y_axis | y_axis_sign | z_axis; + + /* Increment the index */ + idx++; + + /* Cannot OR in the second byte since it holds + * gyroscope self-offset correction bit + */ + feat_config[idx] = BMI2_SET_BIT_POS0(feat_config[idx], BMI2_Z_AXIS_SIGN, z_axis_sign); + + /* Update the register address */ + reg_addr = BMI2_FEATURES_REG_ADDR + remap_config.start_addr; + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(reg_addr, &feat_config[remap_config.start_addr], 2, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + /* Enable Advance power save if disabled while configuring and + * not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + + return rslt; +} + +/*! + * @brief This internal API corrects the gyroscope cross-axis sensitivity + * between the z and the x axis. + */ +static void comp_gyro_cross_axis_sensitivity(struct bmi2_sens_axes_data *gyr_data, const struct bmi2_dev *dev) +{ + /* Get the compensated gyroscope x-axis */ + gyr_data->x = gyr_data->x - (int16_t)(((int32_t) dev->gyr_cross_sens_zx * (int32_t) gyr_data->z) / 512); +} + +/*! + * @brief This internal API is used to validate the boundary conditions. + */ +static int8_t check_boundary_val(uint8_t *val, uint8_t min, uint8_t max, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + if (val != NULL) + { + /* Check if value is below minimum value */ + if (*val < min) + { + /* Auto correct the invalid value to minimum value */ + *val = min; + dev->info |= BMI2_I_MIN_VALUE; + } + + /* Check if value is above maximum value */ + if (*val > max) + { + /* Auto correct the invalid value to maximum value */ + *val = max; + dev->info |= BMI2_I_MAX_VALUE; + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This internal API saves the configurations before performing FOC. + */ +static int8_t save_accel_foc_config(struct bmi2_accel_config *acc_cfg, + uint8_t *aps, + uint8_t *acc_en, + struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to get the status from PWR_CTRL register */ + uint8_t pwr_ctrl_data = 0; + + /* Get accelerometer configurations to be saved */ + rslt = get_accel_config(acc_cfg, dev); + if (rslt == BMI2_OK) + { + /* Get accelerometer enable status to be saved */ + rslt = bmi2_get_regs(BMI2_PWR_CTRL_ADDR, &pwr_ctrl_data, 1, dev); + *acc_en = BMI2_GET_BITS(pwr_ctrl_data, BMI2_ACC_EN); + + /* Get advance power save mode to be saved */ + if (rslt == BMI2_OK) + { + rslt = bmi2_get_adv_power_save(aps, dev); + } + } + + return rslt; +} + +/*! + * @brief This internal sets configurations for performing accelerometer FOC. + */ +static int8_t set_accel_foc_config(struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to select the sensor */ + uint8_t sens_list = BMI2_ACCEL; + + /* Variable to set the accelerometer configuration value */ + uint8_t acc_conf_data = BMI2_FOC_ACC_CONF_VAL; + + /* Disabling offset compensation */ + rslt = set_accel_offset_comp(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + /* Set accelerometer configurations to 50Hz, continuous mode, CIC mode */ + rslt = bmi2_set_regs(BMI2_ACC_CONF_ADDR, &acc_conf_data, 1, dev); + if (rslt == BMI2_OK) + { + /* Set accelerometer to normal mode by enabling it */ + rslt = bmi2_sensor_enable(&sens_list, 1, dev); + + if (rslt == BMI2_OK) + { + /* Disable advance power save mode */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + } + } + + return rslt; +} + +/*! + * @brief This internal API performs Fast Offset Compensation for accelerometer. + */ +static int8_t perform_accel_foc(const struct bmi2_accel_foc_g_value *accel_g_value, + const struct bmi2_accel_config *acc_cfg, + struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_E_INVALID_STATUS; + + /* Variable to define count */ + uint8_t loop; + + /* Variable to store status read from the status register */ + uint8_t reg_status = 0; + + /* Array of structure to store accelerometer data */ + struct bmi2_sens_axes_data accel_value[128] = { { 0 } }; + + /* Structure to store accelerometer data temporarily */ + struct bmi2_foc_temp_value temp = { 0, 0, 0 }; + + /* Structure to store the average of accelerometer data */ + struct bmi2_sens_axes_data accel_avg = { 0, 0, 0, 0 }; + + /* Variable to define LSB per g value */ + uint16_t lsb_per_g = 0; + + /* Variable to define range */ + uint8_t range = 0; + + /* Structure to store accelerometer data deviation from ideal value */ + struct bmi2_offset_delta delta = { 0, 0, 0 }; + + /* Structure to store accelerometer offset values */ + struct bmi2_accel_offset offset = { 0, 0, 0 }; + + /* Variable tries max 5 times for interrupt then generates timeout */ + uint8_t try_cnt; + + for (loop = 0; loop < 128; loop++) + { + try_cnt = 5; + while (try_cnt && (!(reg_status & BMI2_DRDY_ACC))) + { + /* 20ms delay for 50Hz ODR */ + dev->delay_us(20000, dev->intf_ptr); + rslt = bmi2_get_status(®_status, dev); + try_cnt--; + } + + if ((rslt == BMI2_OK) && (reg_status & BMI2_DRDY_ACC)) + { + rslt = read_accel_xyz(&accel_value[loop], dev); + } + + if (rslt == BMI2_OK) + { + rslt = read_accel_xyz(&accel_value[loop], dev); + } + + if (rslt == BMI2_OK) + { + /* Store the data in a temporary structure */ + temp.x = temp.x + (int32_t)accel_value[loop].x; + temp.y = temp.y + (int32_t)accel_value[loop].y; + temp.z = temp.z + (int32_t)accel_value[loop].z; + } + else + { + break; + } + } + + if (rslt == BMI2_OK) + { + /* Take average of x, y and z data for lesser noise */ + accel_avg.x = (int16_t)(temp.x / 128); + accel_avg.y = (int16_t)(temp.y / 128); + accel_avg.z = (int16_t)(temp.z / 128); + + /* Get the exact range value */ + map_accel_range(acc_cfg->range, &range); + + /* Get the smallest possible measurable acceleration level given the range and + * resolution */ + lsb_per_g = (uint16_t)(power(2, dev->resolution) / (2 * range)); + + /* Compensate acceleration data against gravity */ + comp_for_gravity(lsb_per_g, accel_g_value, &accel_avg, &delta); + + /* Scale according to offset register resolution */ + scale_accel_offset(range, &delta, &offset); + + /* Invert the accelerometer offset data */ + invert_accel_offset(&offset); + + /* Write offset data in the offset compensation register */ + rslt = write_accel_offset(&offset, dev); + + /* Enable offset compensation */ + if (rslt == BMI2_OK) + { + rslt = set_accel_offset_comp(BMI2_ENABLE, dev); + } + } + + return rslt; +} + +/*! + * @brief This internal API enables/disables the offset compensation for + * filtered and un-filtered accelerometer data. + */ +static int8_t set_accel_offset_comp(uint8_t offset_en, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store data */ + uint8_t data = 0; + + /* Enable/Disable offset compensation */ + rslt = bmi2_get_regs(BMI2_NV_CONF_ADDR, &data, 1, dev); + if (rslt == BMI2_OK) + { + data = BMI2_SET_BITS(data, BMI2_NV_ACC_OFFSET, offset_en); + rslt = bmi2_set_regs(BMI2_NV_CONF_ADDR, &data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This internal API converts the accelerometer range value into + * corresponding integer value. + */ +static void map_accel_range(uint8_t range_in, uint8_t *range_out) +{ + switch (range_in) + { + case BMI2_ACC_RANGE_2G: + *range_out = 2; + break; + case BMI2_ACC_RANGE_4G: + *range_out = 4; + break; + case BMI2_ACC_RANGE_8G: + *range_out = 8; + break; + case BMI2_ACC_RANGE_16G: + *range_out = 16; + break; + default: + + /* By default RANGE 8G is set */ + *range_out = 8; + break; + } +} + +/*! + * @brief This internal API compensate the accelerometer data against gravity. + */ +static void comp_for_gravity(uint16_t lsb_per_g, + const struct bmi2_accel_foc_g_value *g_val, + const struct bmi2_sens_axes_data *data, + struct bmi2_offset_delta *comp_data) +{ + /* Array to store the accelerometer values in LSB */ + int16_t accel_value_lsb[3] = { 0 }; + + /* Convert g-value to LSB */ + accel_value_lsb[BMI2_X_AXIS] = (int16_t)(lsb_per_g * g_val->x); + accel_value_lsb[BMI2_Y_AXIS] = (int16_t)(lsb_per_g * g_val->y); + accel_value_lsb[BMI2_Z_AXIS] = (int16_t)(lsb_per_g * g_val->z); + + /* Get the compensated values for X, Y and Z axis */ + comp_data->x = (data->x - accel_value_lsb[BMI2_X_AXIS]); + comp_data->y = (data->y - accel_value_lsb[BMI2_Y_AXIS]); + comp_data->z = (data->z - accel_value_lsb[BMI2_Z_AXIS]); +} + +/*! + * @brief This internal API scales the compensated accelerometer data according + * to the offset register resolution. + * + * @note The bit position is always greater than 0 since accelerometer data is + * 16 bit wide. + */ +static void scale_accel_offset(uint8_t range, const struct bmi2_offset_delta *comp_data, struct bmi2_accel_offset *data) +{ + /* Variable to store the position of bit having 3.9mg resolution */ + int8_t bit_pos_3_9mg; + + /* Variable to store the position previous of bit having 3.9mg resolution */ + int8_t bit_pos_3_9mg_prev_bit; + + /* Variable to store the round-off value */ + uint8_t round_off; + + /* Find the bit position of 3.9mg */ + bit_pos_3_9mg = get_bit_pos_3_9mg(range); + + /* Round off, consider if the next bit is high */ + bit_pos_3_9mg_prev_bit = bit_pos_3_9mg - 1; + round_off = (uint8_t)(power(2, ((uint8_t) bit_pos_3_9mg_prev_bit))); + + /* Scale according to offset register resolution */ + data->x = (uint8_t)((comp_data->x + round_off) / power(2, ((uint8_t) bit_pos_3_9mg))); + data->y = (uint8_t)((comp_data->y + round_off) / power(2, ((uint8_t) bit_pos_3_9mg))); + data->z = (uint8_t)((comp_data->z + round_off) / power(2, ((uint8_t) bit_pos_3_9mg))); +} + +/*! + * @brief This internal API finds the bit position of 3.9mg according to given + * range and resolution. + */ +static int8_t get_bit_pos_3_9mg(uint8_t range) +{ + /* Variable to store the bit position of 3.9mg resolution */ + int8_t bit_pos_3_9mg; + + /* Variable to shift the bits according to the resolution */ + uint32_t divisor = 1; + + /* Scaling factor to get the bit position of 3.9 mg resolution */ + int16_t scale_factor = -1; + + /* Variable to store temporary value */ + uint16_t temp; + + /* Shift left by the times of resolution */ + divisor = divisor << 16; + + /* Get the bit position to be shifted */ + temp = (uint16_t)(divisor / (range * 256)); + + /* Get the scaling factor until bit position is shifted to last bit */ + while (temp != 1) + { + scale_factor++; + temp = temp >> 1; + } + + /* Scaling factor is the bit position of 3.9 mg resolution */ + bit_pos_3_9mg = (int8_t) scale_factor; + + return bit_pos_3_9mg; +} + +/*! + * @brief This internal API inverts the accelerometer offset data. + */ +static void invert_accel_offset(struct bmi2_accel_offset *offset_data) +{ + /* Get the offset data */ + offset_data->x = (uint8_t)((offset_data->x) * (-1)); + offset_data->y = (uint8_t)((offset_data->y) * (-1)); + offset_data->z = (uint8_t)((offset_data->z) * (-1)); +} + +/*! + * @brief This internal API writes the offset data in the offset compensation + * register. + */ +static int8_t write_accel_offset(const struct bmi2_accel_offset *offset, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to store the offset data */ + uint8_t data_array[3] = { 0 }; + + data_array[0] = offset->x; + data_array[1] = offset->y; + data_array[2] = offset->z; + + /* Offset values are written in the offset register */ + rslt = bmi2_set_regs(BMI2_ACC_OFF_COMP_0_ADDR, data_array, 3, dev); + + return rslt; +} + +/*! + * @brief This internal API restores the configurations saved before performing + * accelerometer FOC. + */ +static int8_t restore_accel_foc_config(struct bmi2_accel_config *acc_cfg, + uint8_t aps, + uint8_t acc_en, + struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to get the status from PWR_CTRL register */ + uint8_t pwr_ctrl_data = 0; + + /* Restore the saved accelerometer configurations */ + rslt = set_accel_config(acc_cfg, dev); + if (rslt == BMI2_OK) + { + /* Restore the saved accelerometer enable status */ + rslt = bmi2_get_regs(BMI2_PWR_CTRL_ADDR, &pwr_ctrl_data, 1, dev); + if (rslt == BMI2_OK) + { + pwr_ctrl_data = BMI2_SET_BITS(pwr_ctrl_data, BMI2_ACC_EN, acc_en); + rslt = bmi2_set_regs(BMI2_PWR_CTRL_ADDR, &pwr_ctrl_data, 1, dev); + + /* Restore the saved advance power save */ + if (rslt == BMI2_OK) + { + rslt = bmi2_set_adv_power_save(aps, dev); + } + } + } + + return rslt; +} + +/*! + * @brief This internal API sets accelerometer configurations like ODR, + * bandwidth, performance mode and g-range. + */ +static int8_t set_accel_config(struct bmi2_accel_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store data */ + uint8_t reg_data; + + /* Array to store the default value of accelerometer configuration + * reserved registers + */ + uint8_t data_array[2] = { 0 }; + + /* Validate bandwidth and performance mode */ + rslt = validate_bw_perf_mode(&config->bwp, &config->filter_perf, dev); + if (rslt == BMI2_OK) + { + /* Validate ODR and range */ + rslt = validate_odr_range(&config->odr, &config->range, dev); + if (rslt == BMI2_OK) + { + /* Set accelerometer performance mode */ + reg_data = BMI2_SET_BITS(data_array[0], BMI2_ACC_FILTER_PERF_MODE, config->filter_perf); + + /* Set accelerometer bandwidth */ + reg_data = BMI2_SET_BITS(reg_data, BMI2_ACC_BW_PARAM, config->bwp); + + /* Set accelerometer ODR */ + reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_ACC_ODR, config->odr); + + /* Copy the register data to the array */ + data_array[0] = reg_data; + + /* Set accelerometer range */ + reg_data = BMI2_SET_BIT_POS0(data_array[1], BMI2_ACC_RANGE, config->range); + + /* Copy the register data to the array */ + data_array[1] = reg_data; + + /* Write accelerometer configuration to ACC_CONFand + * ACC_RANGE registers simultaneously as they lie in consecutive places + */ + rslt = bmi2_set_regs(BMI2_ACC_CONF_ADDR, data_array, 2, dev); + + /* Get error status to check for invalid configurations */ + if (rslt == BMI2_OK) + { + rslt = cfg_error_status(dev); + } + } + } + + return rslt; +} + +/*! + * @brief This internal API sets gyroscope configurations like ODR, bandwidth, + * low power/high performance mode, performance mode and range. It also + * maps/un-maps data interrupts to that of hardware interrupt line. + */ +static int8_t set_gyro_config(struct bmi2_gyro_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store data */ + uint8_t reg_data; + + /* Array to store the default value of gyroscope configuration reserved registers */ + uint8_t data_array[2] = { 0 }; + + /* Validate gyroscope configurations */ + rslt = validate_gyro_config(config, dev); + if (rslt == BMI2_OK) + { + /* Set gyroscope performance mode */ + reg_data = BMI2_SET_BITS(data_array[0], BMI2_GYR_FILTER_PERF_MODE, config->filter_perf); + + /* Set gyroscope noise performance mode */ + reg_data = BMI2_SET_BITS(reg_data, BMI2_GYR_NOISE_PERF_MODE, config->noise_perf); + + /* Set gyroscope bandwidth */ + reg_data = BMI2_SET_BITS(reg_data, BMI2_GYR_BW_PARAM, config->bwp); + + /* Set gyroscope ODR */ + reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_GYR_ODR, config->odr); + + /* Copy the register data to the array */ + data_array[0] = reg_data; + + /* Set gyroscope OIS range */ + reg_data = BMI2_SET_BITS(data_array[1], BMI2_GYR_OIS_RANGE, config->ois_range); + + /* Set gyroscope range */ + reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_GYR_RANGE, config->range); + + /* Copy the register data to the array */ + data_array[1] = reg_data; + + /* Write accelerometer configuration to GYR_CONF and GYR_RANGE + * registers simultaneously as they lie in consecutive places + */ + rslt = bmi2_set_regs(BMI2_GYR_CONF_ADDR, data_array, 2, dev); + + /* Get error status to check for invalid configurations */ + if (rslt == BMI2_OK) + { + rslt = cfg_error_status(dev); + } + } + + return rslt; +} + +/*! + * @brief This internal API saves the configurations before performing gyroscope + * FOC. + */ +static int8_t save_gyro_config(struct bmi2_gyro_config *gyr_cfg, uint8_t *aps, uint8_t *gyr_en, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to get the status from PWR_CTRL register */ + uint8_t pwr_ctrl_data = 0; + + /* Get gyroscope configurations to be saved */ + rslt = get_gyro_config(gyr_cfg, dev); + if (rslt == BMI2_OK) + { + /* Get gyroscope enable status to be saved */ + rslt = bmi2_get_regs(BMI2_PWR_CTRL_ADDR, &pwr_ctrl_data, 1, dev); + *gyr_en = BMI2_GET_BITS(pwr_ctrl_data, BMI2_GYR_EN); + + /* Get advance power save mode to be saved */ + if (rslt == BMI2_OK) + { + rslt = bmi2_get_adv_power_save(aps, dev); + } + } + + return rslt; +} + +/*! + * @brief This internal sets configurations for performing gyroscope FOC. + */ +static int8_t set_gyro_foc_config(struct bmi2_dev *dev) +{ + int8_t rslt; + + /* Variable to select the sensor */ + uint8_t sens_list = BMI2_GYRO; + + /* Array to set the gyroscope configuration value (ODR, Performance mode + * and bandwidth) and gyroscope range + */ + uint8_t gyr_conf_data[2] = { BMI2_FOC_GYR_CONF_VAL, BMI2_GYR_RANGE_2000 }; + + /* Disabling gyroscope offset compensation */ + rslt = bmi2_set_gyro_offset_comp(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + /* Set gyroscope configurations to 25Hz, continuous mode, + * CIC mode, and 2000 dps range + */ + rslt = bmi2_set_regs(BMI2_GYR_CONF_ADDR, gyr_conf_data, 2, dev); + if (rslt == BMI2_OK) + { + /* Set gyroscope to normal mode by enabling it */ + rslt = bmi2_sensor_enable(&sens_list, 1, dev); + + if (rslt == BMI2_OK) + { + /* Disable advance power save mode */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + } + } + + return rslt; +} + +/*! + * @brief This internal API inverts the gyroscope offset data. + */ +static void invert_gyro_offset(struct bmi2_sens_axes_data *offset_data) +{ + /* Invert the values */ + offset_data->x = (int16_t)((offset_data->x) * (-1)); + offset_data->y = (int16_t)((offset_data->y) * (-1)); + offset_data->z = (int16_t)((offset_data->z) * (-1)); +} + +/*! + * @brief This internal API restores the gyroscope configurations saved + * before performing FOC. + */ +static int8_t restore_gyro_config(struct bmi2_gyro_config *gyr_cfg, uint8_t aps, uint8_t gyr_en, struct bmi2_dev *dev) +{ + int8_t rslt; + uint8_t pwr_ctrl_data = 0; + + /* Restore the saved gyroscope configurations */ + rslt = set_gyro_config(gyr_cfg, dev); + if (rslt == BMI2_OK) + { + /* Restore the saved gyroscope enable status */ + rslt = bmi2_get_regs(BMI2_PWR_CTRL_ADDR, &pwr_ctrl_data, 1, dev); + if (rslt == BMI2_OK) + { + pwr_ctrl_data = BMI2_SET_BITS(pwr_ctrl_data, BMI2_GYR_EN, gyr_en); + rslt = bmi2_set_regs(BMI2_PWR_CTRL_ADDR, &pwr_ctrl_data, 1, dev); + + /* Restore the saved advance power save */ + if (rslt == BMI2_OK) + { + rslt = bmi2_set_adv_power_save(aps, dev); + } + } + } + + return rslt; +} + +/*! + * @brief This internal API saturates the gyroscope data value before writing to + * to 10 bit offset register. + */ +static void saturate_gyro_data(struct bmi2_sens_axes_data *gyr_off) +{ + if (gyr_off->x > 511) + { + gyr_off->x = 511; + } + + if (gyr_off->x < -512) + { + gyr_off->x = -512; + } + + if (gyr_off->y > 511) + { + gyr_off->y = 511; + } + + if (gyr_off->y < -512) + { + gyr_off->y = -512; + } + + if (gyr_off->z > 511) + { + gyr_off->z = 511; + } + + if (gyr_off->z < -512) + { + gyr_off->z = -512; + } +} + +/*! + * @brief This internal API is used to validate the device structure pointer for + * null conditions. + */ +static int8_t null_ptr_check(const struct bmi2_dev *dev) +{ + int8_t rslt = BMI2_OK; + + if ((dev == NULL) || (dev->read == NULL) || (dev->write == NULL) || (dev->delay_us == NULL)) + { + /* Device structure pointer is not valid */ + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This internal API is to get the status of st_status from gry_crt_conf register + */ +static int8_t get_st_running(uint8_t *st_status, struct bmi2_dev *dev) +{ + int8_t rslt; + uint8_t reg_data = 0; + + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + /* Get the status of crt running */ + rslt = bmi2_get_regs(BMI2_GYR_CRT_CONF_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + (*st_status) = BMI2_GET_BITS(reg_data, BMI2_GYR_CRT_RUNNING); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API enables/disables the CRT running. + */ +static int8_t set_st_running(uint8_t st_status, struct bmi2_dev *dev) +{ + int8_t rslt; + uint8_t reg_data = 0; + + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + rslt = bmi2_get_regs(BMI2_GYR_CRT_CONF_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + reg_data = BMI2_SET_BITS(reg_data, BMI2_GYR_CRT_RUNNING, st_status); + rslt = bmi2_set_regs(BMI2_GYR_CRT_CONF_ADDR, ®_data, 1, dev); + } + } + + return rslt; +} + +/*! + * @brief This API gets the status of rdy for dl bit. + */ +static int8_t get_rdy_for_dl(uint8_t *rdy_for_dl, struct bmi2_dev *dev) +{ + int8_t rslt; + uint8_t reg_data = 0; + + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + /* Get the status of rdy_fo_dl */ + rslt = bmi2_get_regs(BMI2_GYR_CRT_CONF_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + (*rdy_for_dl) = BMI2_GET_BITS(reg_data, BMI2_GYR_RDY_FOR_DL); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API does the crt process if max burst length is not zero. + */ +static int8_t process_crt_download(uint8_t last_byte_flag, struct bmi2_dev *dev) +{ + int8_t rslt; + uint8_t rdy_for_dl = 0; + uint8_t cmd = BMI2_G_TRIGGER_CMD; + + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + rslt = get_rdy_for_dl(&rdy_for_dl, dev); + } + + /* Trigger next CRT command */ + if (rslt == BMI2_OK) + { + rslt = bmi2_set_regs(BMI2_CMD_REG_ADDR, &cmd, 1, dev); + } + + if ((!last_byte_flag) && (rslt == BMI2_OK)) + { + rslt = wait_rdy_for_dl_toggle(BMI2_CRT_READY_FOR_DOWNLOAD_RETRY, rdy_for_dl, dev); + } + + return rslt; +} + +/*! + * @brief This API to write the 2kb size of crt configuration + */ +static int8_t write_crt_config_file(uint16_t write_len, + uint16_t config_file_size, + uint16_t start_index, + struct bmi2_dev *dev) +{ + int8_t rslt = BMI2_OK; + uint16_t index = 0; + uint8_t last_byte_flag = 0; + uint8_t remain = (uint8_t)(config_file_size % write_len); + uint16_t balance_byte = 0; + + if (!remain) + { + + /* Write the configuration file */ + for (index = start_index; + (index < (start_index + config_file_size)) && (rslt == BMI2_OK); + index += write_len) + { + rslt = upload_file((dev->config_file_ptr + index), index, write_len, dev); + if (index >= ((start_index + config_file_size) - (write_len))) + { + last_byte_flag = 1; + } + + if (rslt == BMI2_OK) + { + rslt = process_crt_download(last_byte_flag, dev); + } + } + } + else + { + /* Get the balance bytes */ + balance_byte = (uint16_t)start_index + (uint16_t)config_file_size - (uint16_t)remain; + + /* Write the configuration file for the balance bytes */ + for (index = start_index; (index < balance_byte) && (rslt == BMI2_OK); index += write_len) + { + rslt = upload_file((dev->config_file_ptr + index), index, write_len, dev); + if (rslt == BMI2_OK) + { + rslt = process_crt_download(last_byte_flag, dev); + } + } + + if (rslt == BMI2_OK) + { + /* Write the remaining bytes in 2 bytes length */ + write_len = 2; + rslt = set_maxburst_len(write_len, dev); + + /* Write the configuration file for the remaining bytes */ + for (index = balance_byte; + (index < (start_index + config_file_size)) && (rslt == BMI2_OK); + index += write_len) + { + rslt = upload_file((dev->config_file_ptr + index), index, write_len, dev); + if (index < ((start_index + config_file_size) - write_len)) + { + last_byte_flag = 1; + } + + if (rslt == BMI2_OK) + { + rslt = process_crt_download(last_byte_flag, dev); + } + } + } + } + + return rslt; +} + +/*! + * @brief This API is to wait till the rdy for dl bit toggles after every pack of bytes. + */ +static int8_t wait_rdy_for_dl_toggle(uint8_t retry_complete, uint8_t download_ready, struct bmi2_dev *dev) +{ + int8_t rslt = BMI2_OK; + uint8_t dl_ready = 0; + uint8_t st_status = 0; + + while ((rslt == BMI2_OK) && (retry_complete--)) + { + rslt = get_rdy_for_dl(&dl_ready, dev); + if (download_ready != dl_ready) + { + break; + } + + dev->delay_us(BMI2_CRT_READY_FOR_DOWNLOAD_US, dev->intf_ptr); + } + + if ((rslt == BMI2_OK) && (download_ready == dl_ready)) + { + rslt = BMI2_E_CRT_READY_FOR_DL_FAIL_ABORT; + } + + if (rslt == BMI2_OK) + { + rslt = get_st_running(&st_status, dev); + if ((rslt == BMI2_OK) && (st_status == 0)) + { + rslt = BMI2_E_ST_ALREADY_RUNNING; + } + } + + return rslt; +} + +/*! + * @brief This API is to wait till crt status complete. + */ +static int8_t wait_st_running(uint8_t retry_complete, struct bmi2_dev *dev) +{ + uint8_t st_status = 1; + int8_t rslt = BMI2_OK; + + while (retry_complete--) + { + rslt = get_st_running(&st_status, dev); + if ((rslt == BMI2_OK) && (st_status == 0)) + { + break; + } + + dev->delay_us(BMI2_CRT_WAIT_RUNNING_US, dev->intf_ptr); + } + + if ((rslt == BMI2_OK) && (st_status == 1)) + { + rslt = BMI2_E_ST_ALREADY_RUNNING; + } + + return rslt; +} + +/*! + * @brief This api is used to perform gyroscope self-test. + */ +int8_t bmi2_do_gyro_st(struct bmi2_dev *dev) +{ + int8_t rslt; + + rslt = do_gtrigger_test(BMI2_SELECT_GYRO_SELF_TEST, dev); + + return rslt; +} + +/*! + * @brief This API is to run the CRT process for both max burst length 0 and non zero condition. + */ +int8_t bmi2_do_crt(struct bmi2_dev *dev) +{ + int8_t rslt; + + rslt = do_gtrigger_test(BMI2_SELECT_CRT, dev); + + return rslt; +} + +/*! + * @brief This API is to run the crt process for both max burst length 0 and non zero condition. + */ +static int8_t do_gtrigger_test(uint8_t gyro_st_crt, struct bmi2_dev *dev) +{ + int8_t rslt; + int8_t rslt_crt = BMI2_OK; + uint8_t st_status = 0; + uint8_t max_burst_length = 0; + uint8_t download_ready = 0; + uint8_t cmd = BMI2_G_TRIGGER_CMD; + struct bmi2_gyro_self_test_status gyro_st_result = { 0 }; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat = 0; + + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + /* Check if the variant supports this feature */ + if (dev->variant_feature & BMI2_CRT_RTOSK_ENABLE) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if enabled */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + /* Get max burst length */ + if (rslt == BMI2_OK) + { + rslt = get_maxburst_len(&max_burst_length, dev); + } + + /* Checking for CRT running status */ + if (rslt == BMI2_OK) + { + rslt = get_st_running(&st_status, dev); + } + + /* CRT is not running and Max burst length is zero */ + if (st_status == 0) + { + if (rslt == BMI2_OK) + { + rslt = set_st_running(BMI2_ENABLE, dev); + } + + /* Preparing the setup */ + if (rslt == BMI2_OK) + { + rslt = crt_prepare_setup(dev); + } + + /* Enable the gyro self-test, CRT */ + if (rslt == BMI2_OK) + { + rslt = select_self_test(gyro_st_crt, dev); + } + + /* Check if FIFO is unchanged by checking the max burst length */ + if ((rslt == BMI2_OK) && (max_burst_length == 0)) + { + /* Trigger CRT */ + rslt = bmi2_set_regs(BMI2_CMD_REG_ADDR, &cmd, 1, dev); + if (rslt == BMI2_OK) + { + /* Wait until st_status = 0 or time out is 2 seconds */ + rslt = wait_st_running(BMI2_CRT_WAIT_RUNNING_RETRY_EXECUTION, dev); + + /* CRT Running wait & check is successful */ + if (rslt == BMI2_OK) + { + rslt = crt_gyro_st_update_result(dev); + } + } + } + else + { + /* FIFO may be used */ + if (rslt == BMI2_OK) + { + if (dev->read_write_len < 2) + { + dev->read_write_len = 2; + } + + if (dev->read_write_len > (BMI2_CRT_MAX_BURST_WORD_LENGTH * 2)) + { + dev->read_write_len = BMI2_CRT_MAX_BURST_WORD_LENGTH * 2; + } + + /* Reset the max burst length to default value */ + rslt = set_maxburst_len(dev->read_write_len, dev); + } + + if (rslt == BMI2_OK) + { + rslt = get_rdy_for_dl(&download_ready, dev); + } + + /* Trigger CRT */ + if (rslt == BMI2_OK) + { + rslt = bmi2_set_regs(BMI2_CMD_REG_ADDR, &cmd, 1, dev); + } + + /* Wait till either ready for download toggle or crt running = 0 */ + if (rslt == BMI2_OK) + { + rslt = wait_rdy_for_dl_toggle(BMI2_CRT_READY_FOR_DOWNLOAD_RETRY, download_ready, dev); + if (rslt == BMI2_OK) + { + rslt = write_crt_config_file(dev->read_write_len, BMI2_CRT_CONFIG_FILE_SIZE, 0x1800, dev); + } + + if (rslt == BMI2_OK) + { + rslt = wait_st_running(BMI2_CRT_WAIT_RUNNING_RETRY_EXECUTION, dev); + rslt_crt = crt_gyro_st_update_result(dev); + if (rslt == BMI2_OK) + { + rslt = rslt_crt; + } + } + } + } + } + else + { + rslt = BMI2_E_ST_ALREADY_RUNNING; + } + + if (rslt == BMI2_OK) + { + if (gyro_st_crt == BMI2_SELECT_GYRO_SELF_TEST) + { + rslt = gyro_self_test_completed(&gyro_st_result, dev); + } + } + + /* Enable Advance power save if disabled while configuring and + * not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + } + + return rslt; +} + +/*! + * @brief This API to set up environment for processing the crt. + */ +static int8_t crt_prepare_setup(struct bmi2_dev *dev) +{ + int8_t rslt; + + /* Variable to select the sensor */ + uint8_t sens_list = BMI2_GYRO; + + rslt = null_ptr_check(dev); + + if (rslt == BMI2_OK) + { + /* Disable gyroscope */ + rslt = bmi2_sensor_disable(&sens_list, 1, dev); + } + + /* Disable FIFO for all sensors */ + if (rslt == BMI2_OK) + { + rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, dev); + } + + if (rslt == BMI2_OK) + { + /* Enable accelerometer */ + sens_list = BMI2_ACCEL; + rslt = bmi2_sensor_enable(&sens_list, 1, dev); + } + + if (rslt == BMI2_OK) + { + /* Disable Abort after 1 msec */ + dev->delay_us(1000, dev->intf_ptr); + rslt = abort_bmi2(BMI2_DISABLE, dev); + } + + return rslt; +} + +/*! + * @brief This API is to update the CRT or gyro self-test final result. + */ +static int8_t crt_gyro_st_update_result(struct bmi2_dev *dev) +{ + int8_t rslt; + struct bmi2_gyr_user_gain_status user_gain_stat = { 0, 0, 0, 0 }; + + rslt = null_ptr_check(dev); + + /* CRT status has to be read from the config register map */ + if (rslt == BMI2_OK) + { + rslt = get_gyro_gain_update_status(&user_gain_stat, dev); + } + + if (rslt == BMI2_OK) + { + switch (user_gain_stat.g_trigger_status) + { + case BMI2_G_TRIGGER_NO_ERROR: + + /* CRT is successful - Reset the Max Burst Length */ + rslt = set_maxburst_len(0, dev); + break; + + case BMI2_G_TRIGGER_DL_ERROR: + + /* CRT is Download Error - Keep non zero value for Max Burst Length */ + rslt = set_maxburst_len(dev->read_write_len, dev); + if (rslt == BMI2_OK) + { + rslt = BMI2_E_DL_ERROR; + } + + break; + case BMI2_G_TRIGGER_ABORT_ERROR: + + /* Command is aborted either by host via the block bit or due to motion + * detection. Keep non zero value for Max Burst Length + */ + rslt = set_maxburst_len(dev->read_write_len, dev); + if (rslt == BMI2_OK) + { + rslt = BMI2_E_ABORT_ERROR; + } + + break; + + case BMI2_G_TRIGGER_PRECON_ERROR: + + /* Pre-condition to start the feature was not completed. */ + rslt = BMI2_E_PRECON_ERROR; + break; + + default: + rslt = BMI2_E_INVALID_STATUS; + + break; + } + } + + return rslt; +} + +/*! + * @brief This internal API gets the max burst length. + */ +static int8_t get_maxburst_len(uint8_t *max_burst_len, struct bmi2_dev *dev) +{ + int8_t rslt = BMI2_OK; + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + uint8_t idx = 0; + uint8_t feat_found = 0; + struct bmi2_feature_config maxburst_length_bytes = { 0, 0, 0 }; + uint8_t aps_stat; + + if ((dev->variant_feature & BMI2_CRT_IN_FIFO_NOT_REQ) != 0) + { + *max_burst_len = 0; + + return BMI2_OK; + } + + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if enabled */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + if (rslt == BMI2_OK) + { + /* Search for max burst length */ + feat_found = bmi2_extract_input_feat_config(&maxburst_length_bytes, BMI2_MAX_BURST_LEN, dev); + if (feat_found) + { + rslt = bmi2_get_feat_config(maxburst_length_bytes.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for max burst length */ + idx = maxburst_length_bytes.start_addr; + + /* Get the max burst length */ + *max_burst_len = feat_config[idx]; + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + /* Enable Advance power save if disabled while configuring and + * not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + + return rslt; +} + +/*! + * @brief This internal API sets the max burst length. + */ +static int8_t set_maxburst_len(const uint16_t write_len_byte, struct bmi2_dev *dev) +{ + int8_t rslt = BMI2_OK; + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + uint8_t idx = 0; + uint8_t reg_addr = 0; + uint8_t max_burst_len = 0; + uint8_t feat_found = 0; + struct bmi2_feature_config maxburst_length_bytes = { 0, 0, 0 }; + uint8_t aps_stat; + uint16_t burst_len = write_len_byte / 2; + + /* for variant that support crt outside fifo, do not modify the max burst len */ + if ((dev->variant_feature & BMI2_CRT_IN_FIFO_NOT_REQ) != 0) + { + return BMI2_OK; + } + + /* Max burst length is only 1 byte */ + if (burst_len > BMI2_CRT_MAX_BURST_WORD_LENGTH) + { + max_burst_len = UINT8_C(0xFF); + } + else + { + max_burst_len = (uint8_t)burst_len; + } + + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if enabled */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + if (rslt == BMI2_OK) + { + /* Search for axis-re-mapping and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&maxburst_length_bytes, BMI2_MAX_BURST_LEN, dev); + if (feat_found) + { + /* Get the configuration from the page where axis + * re-mapping feature resides + */ + rslt = bmi2_get_feat_config(maxburst_length_bytes.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes */ + idx = maxburst_length_bytes.start_addr; + + /* update Max burst length */ + feat_config[idx] = max_burst_len; + + /* Update the register address */ + reg_addr = BMI2_FEATURES_REG_ADDR + maxburst_length_bytes.start_addr; + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(reg_addr, &feat_config[maxburst_length_bytes.start_addr], 2, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + /* Enable Advance power save if disabled while configuring and + * not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + + return rslt; +} + +/*! + * @brief This api is used to trigger the preparation for system for NVM programming. + */ +static int8_t set_nvm_prep_prog(uint8_t nvm_prep, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + uint8_t reg_addr = 0; + + /* Initialize feature configuration for nvm preparation*/ + struct bmi2_feature_config nvm_config = { 0, 0, 0 }; + + /* Search for bmi2 gyro self offset correction feature as nvm program preparation feature is + * present in the same Word and extract its configuration details + */ + feat_found = bmi2_extract_input_feat_config(&nvm_config, BMI2_NVM_PROG_PREP, dev); + if (feat_found) + { + /* Get the configuration from the page where nvm preparation feature enable feature + * resides */ + rslt = bmi2_get_feat_config(nvm_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for nvm preparation feature enable */ + idx = nvm_config.start_addr; + + /* update nvm_prog_prep enable bit */ + feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_NVM_PREP_FEATURE_EN, nvm_prep); + + /* Update the register address */ + reg_addr = BMI2_FEATURES_REG_ADDR + nvm_config.start_addr - 1; + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(reg_addr, &feat_config[nvm_config.start_addr - 1], 2, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This api is used to enable the CRT. + */ +static int8_t select_self_test(uint8_t gyro_st_crt, struct bmi2_dev *dev) +{ + int8_t rslt; + + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + uint8_t idx = 0; + + uint8_t feat_found; + uint8_t reg_addr = 0; + + struct bmi2_feature_config gyro_self_test_crt_config = { 0, 0, 0 }; + + /* Search for bmi2 crt gyro self-test feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&gyro_self_test_crt_config, BMI2_CRT_GYRO_SELF_TEST, dev); + if (feat_found) + { + /* Get the configuration from the page where gyro self-test and crt enable feature + * resides */ + rslt = bmi2_get_feat_config(gyro_self_test_crt_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes */ + idx = gyro_self_test_crt_config.start_addr; + + /* update the gyro self-test crt enable bit */ + feat_config[idx] = BMI2_SET_BIT_POS0(feat_config[idx], BMI2_GYRO_SELF_TEST_CRT_EN, gyro_st_crt); + + /* Update the register address */ + reg_addr = BMI2_FEATURES_REG_ADDR + (gyro_self_test_crt_config.start_addr - 1); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(reg_addr, &feat_config[gyro_self_test_crt_config.start_addr - 1], 2, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This api is used to abort ongoing crt or gyro self-test. + */ +int8_t bmi2_abort_crt_gyro_st(struct bmi2_dev *dev) +{ + int8_t rslt = BMI2_OK; + uint8_t aps_stat; + uint8_t st_running = 0; + uint8_t cmd = BMI2_G_TRIGGER_CMD; + + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if enabled */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + /* Checking for ST running status */ + if (rslt == BMI2_OK) + { + rslt = get_st_running(&st_running, dev); + if (rslt == BMI2_OK) + { + /* ST is not running */ + if (st_running == 0) + { + rslt = BMI2_E_ST_NOT_RUNING; + } + } + } + + if (rslt == BMI2_OK) + { + rslt = abort_bmi2(BMI2_ENABLE, dev); + } + + /* send the g trigger command */ + if (rslt == BMI2_OK) + { + rslt = bmi2_set_regs(BMI2_CMD_REG_ADDR, &cmd, 1, dev); + } + + if (rslt == BMI2_OK) + { + /* wait until st_status = 0 or time out is 2 seconds */ + rslt = wait_st_running(BMI2_CRT_WAIT_RUNNING_RETRY_EXECUTION, dev); + } + + /* Check G trigger status for error */ + if (rslt == BMI2_OK) + { + rslt = crt_gyro_st_update_result(dev); + if (rslt == BMI2_E_ABORT_ERROR) + { + rslt = BMI2_OK; + } + else + { + rslt = BMI2_E_ABORT_ERROR; + } + } + + /* Enable Advance power save if disabled while configuring and + * not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + + return rslt; +} + +/*! + * @brief This api is used to enable/disable abort. + */ +static int8_t abort_bmi2(uint8_t abort_enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + uint8_t reg_addr = 0; + + /* Initialize feature configuration for blocking a feature */ + struct bmi2_feature_config block_config = { 0, 0, 0 }; + + /* Search for bmi2 Abort feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&block_config, BMI2_ABORT_CRT_GYRO_SELF_TEST, dev); + if (feat_found) + { + /* Get the configuration from the page where abort(block) feature resides */ + rslt = bmi2_get_feat_config(block_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes */ + idx = block_config.start_addr; + + /* update the gyro self-test crt abort enable bit */ + feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_ABORT_FEATURE_EN, abort_enable); + + /* Update the register address */ + reg_addr = BMI2_FEATURES_REG_ADDR + (block_config.start_addr - 1); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(reg_addr, &feat_config[block_config.start_addr - 1], 2, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This api is use to wait till gyro self-test is completed and update the status of gyro + * self-test. + */ +static int8_t gyro_self_test_completed(struct bmi2_gyro_self_test_status *gyro_st_result, struct bmi2_dev *dev) +{ + int8_t rslt; + uint8_t reg_data; + + rslt = bmi2_get_regs(BMI2_GYR_SELF_TEST_AXES_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + gyro_st_result->gyr_st_axes_done = BMI2_GET_BIT_POS0(reg_data, BMI2_GYR_ST_AXES_DONE); + if (gyro_st_result->gyr_st_axes_done == 0x01) + { + gyro_st_result->gyr_axis_x_ok = BMI2_GET_BITS(reg_data, BMI2_GYR_AXIS_X_OK); + gyro_st_result->gyr_axis_y_ok = BMI2_GET_BITS(reg_data, BMI2_GYR_AXIS_Y_OK); + gyro_st_result->gyr_axis_z_ok = BMI2_GET_BITS(reg_data, BMI2_GYR_AXIS_Z_OK); + } + else + { + rslt = BMI2_E_SELF_TEST_NOT_DONE; + } + } + + return rslt; +} + +/*! + * @brief This api validates accel foc position as per the range + */ +static int8_t validate_foc_position(uint8_t sens_list, + const struct bmi2_accel_foc_g_value *accel_g_axis, + struct bmi2_sens_axes_data avg_foc_data, + struct bmi2_dev *dev) +{ + int8_t rslt = BMI2_E_INVALID_INPUT; + + if (sens_list == BMI2_ACCEL) + { + if (accel_g_axis->x == 1) + { + rslt = validate_foc_accel_axis(avg_foc_data.x, dev); + } + else if (accel_g_axis->y == 1) + { + rslt = validate_foc_accel_axis(avg_foc_data.y, dev); + } + else + { + rslt = validate_foc_accel_axis(avg_foc_data.z, dev); + } + } + else if (sens_list == BMI2_GYRO) + { + if (((avg_foc_data.x >= BMI2_GYRO_FOC_NOISE_LIMIT_NEGATIVE) && + (avg_foc_data.x <= BMI2_GYRO_FOC_NOISE_LIMIT_POSITIVE)) && + ((avg_foc_data.y >= BMI2_GYRO_FOC_NOISE_LIMIT_NEGATIVE) && + (avg_foc_data.y <= BMI2_GYRO_FOC_NOISE_LIMIT_POSITIVE)) && + ((avg_foc_data.z >= BMI2_GYRO_FOC_NOISE_LIMIT_NEGATIVE) && + (avg_foc_data.z <= BMI2_GYRO_FOC_NOISE_LIMIT_POSITIVE))) + { + rslt = BMI2_OK; + } + else + { + rslt = BMI2_E_INVALID_FOC_POSITION; + } + } + + return rslt; +} + +/*! + * @brief This api validates depends on accel foc access input + */ +static int8_t validate_foc_accel_axis(int16_t avg_foc_data, struct bmi2_dev *dev) +{ + struct bmi2_sens_config sens_cfg = { 0 }; + uint8_t range; + int8_t rslt; + + sens_cfg.type = BMI2_ACCEL; + rslt = bmi2_get_sensor_config(&sens_cfg, 1, dev); + range = sens_cfg.cfg.acc.range; + + /* reference LSB value of 16G */ + if ((range == BMI2_ACC_RANGE_2G) && (avg_foc_data > BMI2_ACC_2G_MIN_NOISE_LIMIT) && + (avg_foc_data < BMI2_ACC_2G_MAX_NOISE_LIMIT)) + { + rslt = BMI2_OK; + } + /* reference LSB value of 16G */ + else if ((range == BMI2_ACC_RANGE_4G) && (avg_foc_data > BMI2_ACC_4G_MIN_NOISE_LIMIT) && + (avg_foc_data < BMI2_ACC_4G_MAX_NOISE_LIMIT)) + { + rslt = BMI2_OK; + } + /* reference LSB value of 16G */ + else if ((range == BMI2_ACC_RANGE_8G) && (avg_foc_data > BMI2_ACC_8G_MIN_NOISE_LIMIT) && + (avg_foc_data < BMI2_ACC_8G_MAX_NOISE_LIMIT)) + { + rslt = BMI2_OK; + } + /* reference LSB value of 16G */ + else if ((range == BMI2_ACC_RANGE_16G) && (avg_foc_data > BMI2_ACC_16G_MIN_NOISE_LIMIT) && + (avg_foc_data < BMI2_ACC_16G_MAX_NOISE_LIMIT)) + { + rslt = BMI2_OK; + } + else + { + rslt = BMI2_E_INVALID_FOC_POSITION; + } + + return rslt; +} + +/*! @brief This api is used for programming the non volatile memory(nvm) */ +int8_t bmi2_nvm_prog(struct bmi2_dev *dev) +{ + int8_t rslt = BMI2_OK; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat; + uint8_t status; + uint8_t cmd_rdy; + uint8_t reg_data; + uint8_t write_timeout = 100; + + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if enabled */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + /* Check the Write status and proceed only if there is no ongoing write cycle */ + if (rslt == BMI2_OK) + { + rslt = bmi2_get_status(&status, dev); + + cmd_rdy = BMI2_GET_BITS(status, BMI2_CMD_RDY); + if (cmd_rdy) + { + rslt = set_nvm_prep_prog(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->delay_us(40000, dev->intf_ptr); + + /* Set the NVM_CONF.nvm_prog_en bit in order to enable the NVM + * programming */ + reg_data = BMI2_NVM_UNLOCK_ENABLE; + rslt = bmi2_set_regs(BMI2_NVM_CONF_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + /* Send NVM prog command to command register */ + reg_data = BMI2_NVM_PROG_CMD; + rslt = bmi2_set_regs(BMI2_CMD_REG_ADDR, ®_data, 1, dev); + } + + /* Wait till write operation is completed */ + if (rslt == BMI2_OK) + { + while (write_timeout--) + { + rslt = bmi2_get_status(&status, dev); + if (rslt == BMI2_OK) + { + cmd_rdy = BMI2_GET_BITS(status, BMI2_CMD_RDY); + + /* Nvm is complete once cmd_rdy is 1, break if 1 */ + if (cmd_rdy) + { + break; + } + + /* Wait till cmd_rdy becomes 1 indicating + * nvm process completes */ + dev->delay_us(20000, dev->intf_ptr); + } + } + } + + if ((rslt == BMI2_OK) && (cmd_rdy != BMI2_TRUE)) + { + rslt = BMI2_E_WRITE_CYCLE_ONGOING; + } + } + } + else + { + rslt = BMI2_E_WRITE_CYCLE_ONGOING; + } + } + + if (rslt == BMI2_OK) + { + /* perform soft reset */ + rslt = bmi2_soft_reset(dev); + } + + /* Enable Advance power save if disabled while configuring and not when already disabled */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + + return rslt; +} + +/*! + * @brief This API reads and provides average for 128 samples of sensor data for foc operation + * gyro. + */ +static int8_t get_average_of_sensor_data(uint8_t sens_list, + struct bmi2_foc_temp_value *temp_foc_data, + struct bmi2_dev *dev) +{ + int8_t rslt = 0; + struct bmi2_sensor_data sensor_data = { 0 }; + uint8_t sample_count = 0; + uint8_t datardy_try_cnt; + uint8_t drdy_status = 0; + uint8_t sensor_drdy = 0; + + sensor_data.type = sens_list; + if (sens_list == BMI2_ACCEL) + { + sensor_drdy = BMI2_DRDY_ACC; + } + else + { + sensor_drdy = BMI2_DRDY_GYR; + } + + /* Read sensor values before FOC */ + while (sample_count < BMI2_FOC_SAMPLE_LIMIT) + { + datardy_try_cnt = 5; + do + { + dev->delay_us(20000, dev->intf_ptr); + rslt = bmi2_get_status(&drdy_status, dev); + datardy_try_cnt--; + } while ((rslt == BMI2_OK) && (!(drdy_status & sensor_drdy)) && (datardy_try_cnt)); + + if ((rslt != BMI2_OK) || (datardy_try_cnt == 0)) + { + rslt = BMI2_E_DATA_RDY_INT_FAILED; + break; + } + + rslt = bmi2_get_sensor_data(&sensor_data, 1, dev); + + if (rslt == BMI2_OK) + { + if (sensor_data.type == BMI2_ACCEL) + { + temp_foc_data->x += sensor_data.sens_data.acc.x; + temp_foc_data->y += sensor_data.sens_data.acc.y; + temp_foc_data->z += sensor_data.sens_data.acc.z; + } + else if (sensor_data.type == BMI2_GYRO) + { + temp_foc_data->x += sensor_data.sens_data.gyr.x; + temp_foc_data->y += sensor_data.sens_data.gyr.y; + temp_foc_data->z += sensor_data.sens_data.gyr.z; + } + } + else + { + break; + } + + sample_count++; + } + + if (rslt == BMI2_OK) + { + temp_foc_data->x = (temp_foc_data->x / BMI2_FOC_SAMPLE_LIMIT); + temp_foc_data->y = (temp_foc_data->y / BMI2_FOC_SAMPLE_LIMIT); + temp_foc_data->z = (temp_foc_data->z / BMI2_FOC_SAMPLE_LIMIT); + } + + return rslt; +} + +/*! + * @brief This internal API extract the identification feature from the DMR page + * and retrieve the config file major and minor version. + */ +static int8_t extract_config_file(uint8_t *config_major, uint8_t *config_minor, struct bmi2_dev *dev) +{ + /* Variable to define the result */ + int8_t rslt = BMI2_OK; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to define LSB */ + uint16_t lsb = 0; + + /* Variable to define MSB */ + uint16_t msb = 0; + + /* Variable to define a word */ + uint16_t lsb_msb = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Variable to define advance power save mode status */ + uint8_t aps_stat; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Initialize feature configuration for config file identification */ + struct bmi2_feature_config config_id = { 0, 0, 0 }; + + /* Check the power mode status */ + aps_stat = dev->aps_status; + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if enabled */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + if (rslt == BMI2_OK) + { + /* Search for config file identification feature and extract its configuration + * details */ + feat_found = bmi2_extract_input_feat_config(&config_id, BMI2_CONFIG_ID, dev); + if (feat_found) + { + /* Get the configuration from the page where config file identification + * feature resides */ + rslt = bmi2_get_feat_config(config_id.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for config file identification */ + idx = config_id.start_addr; + + /* Get word to calculate config file identification */ + lsb = (uint16_t) feat_config[idx++]; + msb = ((uint16_t) feat_config[idx++] << 8); + lsb_msb = lsb | msb; + + /* Get major and minor version */ + *config_major = BMI2_GET_BITS(lsb_msb, BMI2_CONFIG_MAJOR); + *config_minor = BMI2_GET_BIT_POS0(lsb, BMI2_CONFIG_MINOR); + } + } + + /* Enable Advance power save if disabled while configuring and + * not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + *@brief This internal API is used to map the interrupts to the sensor. + */ +static void extract_feat_int_map(struct bmi2_map_int *map_int, uint8_t type, const struct bmi2_dev *dev) +{ + /* Variable to define loop */ + uint8_t loop = 0; + + /* Search for the interrupts from the input configuration array */ + while (loop < dev->sens_int_map) + { + if (dev->map_int[loop].type == type) + { + *map_int = dev->map_int[loop]; + break; + } + + loop++; + } +} + +/*! + * @brief This internal API gets the saturation status for the gyroscope user + * gain update. + */ +static int8_t get_gyro_gain_update_status(struct bmi2_gyr_user_gain_status *user_gain_stat, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variables to define index */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature output for gyroscope user gain status */ + struct bmi2_feature_config user_gain_cfg = { 0, 0, 0 }; + + /* Search for gyroscope user gain status output feature and extract its + * configuration details + */ + feat_found = extract_output_feat_config(&user_gain_cfg, BMI2_GYRO_GAIN_UPDATE, dev); + if (feat_found) + { + /* Get the feature output configuration for gyroscope user gain status */ + rslt = bmi2_get_feat_config(user_gain_cfg.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for gyroscope user gain status */ + idx = user_gain_cfg.start_addr; + + /* Get the saturation status for x-axis */ + user_gain_stat->sat_x = BMI2_GET_BIT_POS0(feat_config[idx], BMI2_GYR_USER_GAIN_SAT_STAT_X); + + /* Get the saturation status for y-axis */ + user_gain_stat->sat_y = BMI2_GET_BITS(feat_config[idx], BMI2_GYR_USER_GAIN_SAT_STAT_Y); + + /* Get the saturation status for z-axis */ + user_gain_stat->sat_z = BMI2_GET_BITS(feat_config[idx], BMI2_GYR_USER_GAIN_SAT_STAT_Z); + + /* Get g trigger status */ + user_gain_stat->g_trigger_status = BMI2_GET_BITS(feat_config[idx], BMI2_G_TRIGGER_STAT); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API is used to extract the output feature configuration + * details from the look-up table. + */ +static uint8_t extract_output_feat_config(struct bmi2_feature_config *feat_output, + uint8_t type, + const struct bmi2_dev *dev) +{ + /* Variable to define loop */ + uint8_t loop = 0; + + /* Variable to set flag */ + uint8_t feat_found = BMI2_FALSE; + + /* Search for the output feature from the output configuration array */ + while (loop < dev->out_sens) + { + if (dev->feat_output[loop].type == type) + { + *feat_output = dev->feat_output[loop]; + feat_found = BMI2_TRUE; + break; + } + + loop++; + } + + /* Return flag */ + return feat_found; +} + +/*! + * @brief This internal API gets the cross sensitivity coefficient between + * gyroscope's X and Z axes. + */ +static int8_t get_gyro_cross_sense(int16_t *cross_sense, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define index */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + uint8_t corr_fact_zx; + + /* Initialize feature output for gyroscope cross sensitivity */ + struct bmi2_feature_config cross_sense_out_config = { 0, 0, 0 }; + + if (dev->variant_feature & BMI2_MAXIMUM_FIFO_VARIANT) + { + /* For maximum_fifo variant fetch the correction factor from GPIO0 */ + rslt = bmi2_get_regs(BMI2_GYR_CAS_GPIO0_ADDR, &corr_fact_zx, 1, dev); + if (rslt == BMI2_OK) + { + /* Get the gyroscope cross sensitivity coefficient */ + if (corr_fact_zx & BMI2_GYRO_CROSS_AXES_SENSE_SIGN_BIT_MASK) + { + *cross_sense = (int16_t)(((int16_t)corr_fact_zx) - 128); + } + else + { + *cross_sense = (int16_t)(corr_fact_zx); + } + } + } + else + { + /* Search for gyroscope cross sensitivity feature and extract its configuration details */ + feat_found = extract_output_feat_config(&cross_sense_out_config, BMI2_GYRO_CROSS_SENSE, dev); + if (feat_found) + { + /* Get the feature output configuration for gyroscope cross sensitivity + * feature */ + rslt = bmi2_get_feat_config(cross_sense_out_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for gyroscope cross sensitivity output */ + idx = cross_sense_out_config.start_addr; + + /* discard the MSB as GYR_CAS is of only 7 bit */ + feat_config[idx] = feat_config[idx] & BMI2_GYRO_CROSS_AXES_SENSE_MASK; + + /* Get the gyroscope cross sensitivity coefficient */ + if (feat_config[idx] & BMI2_GYRO_CROSS_AXES_SENSE_SIGN_BIT_MASK) + { + *cross_sense = (int16_t)(((int16_t)feat_config[idx]) - 128); + } + else + { + *cross_sense = (int16_t)(feat_config[idx]); + } + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + } + + return rslt; +} + +/*! + * @brief This internal API selects the sensor/features to be enabled or + * disabled. + */ +static int8_t select_sensor(const uint8_t *sens_list, uint8_t n_sens, uint64_t *sensor_sel) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Variable to define loop */ + uint8_t count; + + for (count = 0; count < n_sens; count++) + { + switch (sens_list[count]) + { + case BMI2_ACCEL: + *sensor_sel |= BMI2_ACCEL_SENS_SEL; + break; + case BMI2_GYRO: + *sensor_sel |= BMI2_GYRO_SENS_SEL; + break; + case BMI2_AUX: + *sensor_sel |= BMI2_AUX_SENS_SEL; + break; + case BMI2_TEMP: + *sensor_sel |= BMI2_TEMP_SENS_SEL; + break; + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + } + + return rslt; +} + +/*! + * @brief This internal API enables the selected sensor/features. + */ +static int8_t sensor_enable(uint64_t sensor_sel, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store register values */ + uint8_t reg_data = 0; + + rslt = bmi2_get_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + /* Enable accelerometer */ + if (sensor_sel & BMI2_ACCEL_SENS_SEL) + { + reg_data = BMI2_SET_BITS(reg_data, BMI2_ACC_EN, BMI2_ENABLE); + } + + /* Enable gyroscope */ + if (sensor_sel & BMI2_GYRO_SENS_SEL) + { + reg_data = BMI2_SET_BITS(reg_data, BMI2_GYR_EN, BMI2_ENABLE); + } + + /* Enable auxiliary sensor */ + if (sensor_sel & BMI2_AUX_SENS_SEL) + { + reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_AUX_EN, BMI2_ENABLE); + } + + /* Enable temperature sensor */ + if (sensor_sel & BMI2_TEMP_SENS_SEL) + { + reg_data = BMI2_SET_BITS(reg_data, BMI2_TEMP_EN, BMI2_ENABLE); + } + + /* Enable the sensors that are set in the power control register */ + if (sensor_sel & BMI2_MAIN_SENSORS) + { + rslt = bmi2_set_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); + } + } + + return rslt; +} + +/*! + * @brief This internal API disables the selected sensors/features. + */ +static int8_t sensor_disable(uint64_t sensor_sel, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store register values */ + uint8_t reg_data = 0; + + rslt = bmi2_get_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + /* Disable accelerometer */ + if (sensor_sel & BMI2_ACCEL_SENS_SEL) + { + reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_ACC_EN); + } + + /* Disable gyroscope */ + if (sensor_sel & BMI2_GYRO_SENS_SEL) + { + reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_GYR_EN); + } + + /* Disable auxiliary sensor */ + if (sensor_sel & BMI2_AUX_SENS_SEL) + { + reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_AUX_EN); + } + + /* Disable temperature sensor */ + if (sensor_sel & BMI2_TEMP_SENS_SEL) + { + reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_TEMP_EN); + } + + /* Enable the sensors that are set in the power control register */ + if (sensor_sel & BMI2_MAIN_SENSORS) + { + rslt = bmi2_set_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); + } + } + + return rslt; +} + +/*! @endcond */ diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2.h b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2.h new file mode 100644 index 0000000000..b1fe627d62 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2.h @@ -0,0 +1,1513 @@ +/** +* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. +* +* BSD-3-Clause +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* +* 3. Neither the name of the copyright holder nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, +* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING +* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +* @file bmi2.h +* @date 2020-11-04 +* @version v2.63.1 +* +*/ + +/*! + * @defgroup bmi2xy BMI2XY + */ + +/** + * \ingroup bmi2xy + * \defgroup bmi2 BMI2 + * @brief Sensor driver for BMI2 sensor + */ + +#ifndef BMI2_H_ +#define BMI2_H_ + +/*! CPP guard */ +#ifdef __cplusplus +extern "C" { +#endif + +/***************************************************************************/ + +/*! Header files + ****************************************************************************/ +#include "bmi2_defs.h" + +/***************************************************************************/ + +/*! BMI2XY User Interface function prototypes + ****************************************************************************/ + +/** + * \ingroup bmi2 + * \defgroup bmi2ApiInit Initialization + * @brief Initialize the sensor and device structure + */ + +/*! + * \ingroup bmi2ApiInit + * \page bmi2_api_bmi2_sec_init bmi2_sec_init + * \code + * int8_t bmi2_sec_init(struct bmi2_dev *dev); + * \endcode + * @details This API is the entry point for bmi2 sensor. It selects between + * I2C/SPI interface, based on user selection. It also reads the chip-id of + * the sensor. + * + * @param[in,out] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_sec_init(struct bmi2_dev *dev); + +/** + * \ingroup bmi2 + * \defgroup bmi2ApiRegs Registers + * @brief Set / Get data from the given register address of the sensor + */ + +/*! + * \ingroup bmi2ApiRegs + * \page bmi2_api_bmi2_get_regs bmi2_get_regs + * \code + * int8_t bmi2_get_regs(uint8_t reg_addr, uint8_t *data, uint16_t len, const struct bmi2_dev *dev); + * \endcode + * @details This API reads the data from the given register address of bmi2 + * sensor. + * + * @param[in] reg_addr : Register address from which data is read. + * @param[out] data : Pointer to data buffer where read data is stored. + * @param[in] len : No. of bytes of data to be read. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @note For most of the registers auto address increment applies, with the + * exception of a few special registers, which trap the address. For e.g., + * Register address - 0x26, 0x5E. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_get_regs(uint8_t reg_addr, uint8_t *data, uint16_t len, struct bmi2_dev *dev); + +/*! + * \ingroup bmi2ApiRegs + * \page bmi2_api_bmi2_set_regs bmi2_set_regs + * \code + * int8_t bmi2_set_regs(uint8_t reg_addr, const uint8_t *data, uint16_t len, struct bmi2_dev *dev); + * \endcode + * @details This API writes data to the given register address of bmi2 sensor. + * + * @param[in] reg_addr : Register address to which the data is written. + * @param[in] data : Pointer to data buffer in which data to be written + * is stored. + * @param[in] len : No. of bytes of data to be written. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_set_regs(uint8_t reg_addr, const uint8_t *data, uint16_t len, struct bmi2_dev *dev); + +/** + * \ingroup bmi2 + * \defgroup bmi2ApiSR Soft reset + * @brief Set / Get data from the given register address of the sensor + */ + +/*! + * \ingroup bmi2ApiSR + * \page bmi2_api_bmi2_soft_reset bmi2_soft_reset + * \code + * int8_t bmi2_soft_reset(struct bmi2_dev *dev); + * \endcode + * @details This API resets bmi2 sensor. All registers are overwritten with + * their default values. + * + * @note If selected interface is SPI, an extra dummy byte is read to bring the + * interface back to SPI from default, after the soft reset command. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_soft_reset(struct bmi2_dev *dev); + +/** + * \ingroup bmi2 + * \defgroup bmi2ApiConfig Configuration + * @brief Functions related to configuration of the sensor + */ + +/*! + * \ingroup bmi2ApiConfig + * \page bmi2_api_bmi2_get_config_file_version bmi2_get_config_file_version + * \code + * int8_t bmi2_get_config_file_version(uint8_t *config_major, uint8_t *config_minor, struct bmi2_dev *dev); + * \endcode + * @details This API is used to get the config file major and minor information. + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[out] config_major : pointer to data buffer to store the config major. + * @param[out] config_minor : pointer to data buffer to store the config minor. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_get_config_file_version(uint8_t *config_major, uint8_t *config_minor, struct bmi2_dev *dev); + +/** + * \ingroup bmi2 + * \defgroup bmi2ApiPowersave Advanced power save mode + * @brief Set / Get Advanced power save mode of the sensor + */ + +/*! + * \ingroup bmi2ApiPowersave + * \page bmi2_api_bmi2_set_adv_power_save bmi2_set_adv_power_save + * \code + * int8_t bmi2_set_adv_power_save(uint8_t enable, struct bmi2_dev *dev); + * \endcode + * @details This API enables/disables the advance power save mode in the sensor. + * + * @param[in] enable : To enable/disable advance power mode. + * @param[in] dev : Structure instance of bmi2_dev. + * + *@verbatim + * Enable | Description + * -------------|--------------- + * BMI2_DISABLE | Disables advance power save. + * BMI2_ENABLE | Enables advance power save. + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_set_adv_power_save(uint8_t enable, struct bmi2_dev *dev); + +/*! + * \ingroup bmi2ApiPowersave + * \page bmi2_api_bmi2_get_adv_power_save bmi2_get_adv_power_save + * \code + * int8_t bmi2_get_adv_power_save(uint8_t *aps_status, struct bmi2_dev *dev); + * \endcode + * @details This API gets the status of advance power save mode in the sensor. + * + * @param[out] aps_status : Pointer to get the status of APS mode. + * @param[in] dev : Structure instance of bmi2_dev. + * + *@verbatim + * aps_status | Description + * -------------|--------------- + * BMI2_DISABLE | Advance power save disabled. + * BMI2_ENABLE | Advance power save enabled. + *@endverbatim + * + * @return Result of API execution status + * + * @retval BMI2_OK - Success. + * @retval BMI2_E_NULL_PTR - Error: Null pointer error + * @retval BMI2_E_COM_FAIL - Error: Communication fail + * @retval BMI2_E_SET_APS_FAIL - Error: Set Advance Power Save Fail + */ +int8_t bmi2_get_adv_power_save(uint8_t *aps_status, struct bmi2_dev *dev); + +/*! + * \ingroup bmi2ApiConfig + * \page bmi2_api_bmi2_write_config_file bmi2_write_config_file + * \code + * int8_t bmi2_write_config_file(struct bmi2_dev *dev); + * \endcode + * @details This API loads the configuration file to the bmi2 sensor. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_write_config_file(struct bmi2_dev *dev); + +/** + * \ingroup bmi2 + * \defgroup bmi2ApiInt Interrupt + * @brief Interrupt operations of the sensor + */ + +/*! + * \ingroup bmi2ApiInt + * \page bmi2_api_bmi2_set_int_pin_config bmi2_set_int_pin_config + * \code + * int8_t bmi2_set_int_pin_config(const struct bmi2_int_pin_config *int_cfg, struct bmi2_dev *dev); + * \endcode + * @details This API sets: + * 1) The input output configuration of the selected interrupt pin: + * INT1 or INT2. + * 2) The interrupt mode: permanently latched or non-latched. + * + * @param[in] int_cfg : Structure instance of bmi2_int_pin_config. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_set_int_pin_config(const struct bmi2_int_pin_config *int_cfg, struct bmi2_dev *dev); + +/*! + * \ingroup bmi2ApiInt + * \page bmi2_api_bmi2_get_int_pin_config bmi2_get_int_pin_config + * \code + * int8_t bmi2_get_int_pin_config(struct bmi2_int_pin_config *int_cfg, const struct bmi2_dev *dev); + * \endcode + * @details This API gets: + * 1) The input output configuration of the selected interrupt pin: + * INT1 or INT2. + * 2) The interrupt mode: permanently latched or non-latched. + * + * @param[in,out] int_cfg : Structure instance of bmi2_int_pin_config. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_get_int_pin_config(struct bmi2_int_pin_config *int_cfg, struct bmi2_dev *dev); + +/*! + * \ingroup bmi2ApiInt + * \page bmi2_api_bmi2_get_int_status bmi2_get_int_status + * \code + * int8_t bmi2_get_int_status(uint16_t *int_status, const struct bmi2_dev *dev); + * \endcode + * @details This API gets the interrupt status of both feature and data + * interrupts. + * + * @param[out] int_status : Pointer to get the status of the interrupts. + * @param[in] dev : Structure instance of bmi2_dev. + * + *@verbatim + * int_status | Status + * -----------|------------ + * 0x00 | BIT0 + * 0x01 | BIT1 + * 0x02 | BIT2 + * 0x03 | BIT3 + * 0x04 | BIT4 + * 0x05 | BIT5 + * 0x06 | BIT6 + * 0x07 | BIT7 + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_get_int_status(uint16_t *int_status, struct bmi2_dev *dev); + +/** + * \ingroup bmi2 + * \defgroup bmi2ApiSensorC Sensor Configuration + * @brief Enable / Disable feature configuration of the sensor + */ + +/*! + * \ingroup bmi2ApiSensorC + * \page bmi2_api_bmi2_set_sensor_config bmi2_set_sensor_config + * \code + * int8_t bmi2_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); + * \endcode + * @details This API sets the sensor/feature configuration. + * + * @param[in] sens_cfg : Structure instance of bmi2_sens_config. + * @param[in] n_sens : Number of sensors selected. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @note Sensors/features that can be configured + * + *@verbatim + * sens_list | Values + * ----------------------------|----------- + * BMI2_ACCEL | 0 + * BMI2_GYRO | 1 + * BMI2_AUX | 2 + * BMI2_GYRO_GAIN_UPDATE | 9 + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); + +/*! + * \ingroup bmi2ApiSensorC + * \page bmi2_api_bmi2_get_sensor_config bmi2_get_sensor_config + * \code + * int8_t bmi2_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); + * \endcode + * @details This API gets the sensor/feature configuration. + * + * @param[in] sens_cfg : Structure instance of bmi2_sens_config. + * @param[in] n_sens : Number of sensors selected. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @note Sensors/features whose configurations can be read. + * + *@verbatim + * sens_list | Values + * -------------------------|----------- + * BMI2_ACCEL | 0 + * BMI2_GYRO | 1 + * BMI2_AUX | 2 + * BMI2_GYRO_GAIN_UPDATE | 9 + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); + +/** + * \ingroup bmi2 + * \defgroup bmi2ApiSensor Feature Set + * @brief Enable / Disable features of the sensor + */ + +/*! + * \ingroup bmi2ApiSensor + * \page bmi2_api_bmi2_sensor_enable bmi2_sensor_enable + * \code + * int8_t bmi2_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); + * \endcode + * @details This API selects the sensors/features to be enabled. + * + * @param[in] sens_list : Pointer to select the sensor/feature. + * @param[in] n_sens : Number of sensors selected. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @note Sensors/features that can be enabled. + * + *@verbatim + * sens_list | Values + * -------------------------|----------- + * BMI2_ACCEL | 0 + * BMI2_GYRO | 1 + * BMI2_AUX | 2 + * BMI2_TEMP | 31 + *@endverbatim + * + * @note : + * example uint8_t sens_list[2] = {BMI2_ACCEL, BMI2_GYRO}; + * uint8_t n_sens = 2; + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); + +/*! + * \ingroup bmi2ApiSensor + * \page bmi2_api_bmi2_sensor_disable bmi2_sensor_disable + * \code + * int8_t bmi2_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); + * \endcode + * @details This API selects the sensors/features to be disabled. + * + * @param[in] sens_list : Pointer to select the sensor/feature. + * @param[in] n_sens : Number of sensors selected. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @note Sensors/features that can be disabled. + * + *@verbatim + * sens_list | Values + * ----------------------------|----------- + * BMI2_ACCEL | 0 + * BMI2_GYRO | 1 + * BMI2_AUX | 2 + * BMI2_TEMP | 31 + *@endverbatim + * + * @note : + * example uint8_t sens_list[2] = {BMI2_ACCEL, BMI2_GYRO}; + * uint8_t n_sens = 2; + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); + +/** + * \ingroup bmi2 + * \defgroup bmi2ApiSensorD Sensor Data + * @brief Get sensor data + */ + +/*! + * \ingroup bmi2ApiSensorD + * \page bmi2_api_bmi2_get_sensor_data bmi2_get_sensor_data + * \code + * int8_t bmi2_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev); + * \endcode + * @details This API gets the sensor/feature data for accelerometer, gyroscope, + * auxiliary sensor, step counter, high-g, gyroscope user-gain update, + * orientation, gyroscope cross sensitivity and error status for NVM and VFRM. + * + * @param[out] sensor_data : Structure instance of bmi2_sensor_data. + * @param[in] n_sens : Number of sensors selected. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @note Sensors/features whose data can be read + * + *@verbatim + * sens_list | Values + * ---------------------|----------- + * BMI2_ACCEL | 0 + * BMI2_GYRO | 1 + * BMI2_AUX | 2 + * BMI2_GYRO_GAIN_UPDATE| 12 + * BMI2_GYRO_CROSS_SENSE| 28 + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev); + +/** + * \ingroup bmi2 + * \defgroup bmi2ApiFIFO FIFO + * @brief FIFO operations of the sensor + */ + +/*! + * \ingroup bmi2ApiFIFO + * \page bmi2_api_bmi2_set_fifo_config bmi2_set_fifo_config + * \code + * int8_t bmi2_set_fifo_config(uint16_t config, uint8_t enable, struct bmi2_dev *dev); + * \endcode + * @details This API sets the FIFO configuration in the sensor. + * + * @param[in] config : FIFO configurations to be enabled/disabled. + * @param[in] enable : Enable/Disable FIFO configurations. + * @param[in] dev : Structure instance of bmi2_dev. + * + *@verbatim + * enable | Description + * -------------|--------------- + * BMI2_DISABLE | Disables FIFO configuration. + * BMI2_ENABLE | Enables FIFO configuration. + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_set_fifo_config(uint16_t config, uint8_t enable, struct bmi2_dev *dev); + +/*! + * \ingroup bmi2ApiFIFO + * \page bmi2_api_bmi2_get_fifo_config bmi2_get_fifo_config + * \code + * int8_t bmi2_get_fifo_config(uint16_t *fifo_config, const struct bmi2_dev *dev); + * \endcode + * @details This API gets the FIFO configuration from the sensor. + * + * @param[out] fifo_config : Pointer variable to get FIFO configuration value. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_get_fifo_config(uint16_t *fifo_config, struct bmi2_dev *dev); + +/*! + * \ingroup bmi2ApiFIFO + * \page bmi2_api_bmi2_read_fifo_data bmi2_read_fifo_data + * \code + * int8_t bmi2_read_fifo_data(struct bmi2_fifo_frame *fifo, const struct bmi2_dev *dev); + * \endcode + * @details This API reads FIFO data. + * + * @param[in, out] fifo : Structure instance of bmi2_fifo_frame. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @note APS has to be disabled before calling this function. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_read_fifo_data(struct bmi2_fifo_frame *fifo, struct bmi2_dev *dev); + +/*! + * \ingroup bmi2ApiFIFO + * \page bmi2_api_bmi2_extract_accel bmi2_extract_accel + * \code + * int8_t bmi2_extract_accel(struct bmi2_sens_axes_data *accel_data, + * uint16_t *accel_length, + * struct bmi2_fifo_frame *fifo, + * const struct bmi2_dev *dev); + * \endcode + * @details This API parses and extracts the accelerometer frames from FIFO data read by + * the "bmi2_read_fifo_data" API and stores it in the "accel_data" structure + * instance. + * + * @param[out] accel_data : Structure instance of bmi2_sens_axes_data + * where the parsed data bytes are stored. + * @param[in,out] accel_length : Number of accelerometer frames. + * @param[in,out] fifo : Structure instance of bmi2_fifo_frame. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_extract_accel(struct bmi2_sens_axes_data *accel_data, + uint16_t *accel_length, + struct bmi2_fifo_frame *fifo, + const struct bmi2_dev *dev); + +/*! + * \ingroup bmi2ApiFIFO + * \page bmi2_api_bmi2_extract_aux bmi2_extract_aux + * \code + * int8_t bmi2_extract_aux(struct bmi2_aux_fifo_data *aux, + * uint16_t *aux_length, + * struct bmi2_fifo_frame *fifo, + * const struct bmi2_dev *dev); + * + * \endcode + * @details This API parses and extracts the auxiliary frames from FIFO data + * read by the "bmi2_read_fifo_data" API and stores it in "aux_data" buffer. + * + * @param[out] aux : Pointer to structure where the parsed auxiliary + * data bytes are stored. + * @param[in,out] aux_length : Number of auxiliary frames. + * @param[in,out] fifo : Structure instance of bmi2_fifo_frame. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_extract_aux(struct bmi2_aux_fifo_data *aux, + uint16_t *aux_length, + struct bmi2_fifo_frame *fifo, + const struct bmi2_dev *dev); + +/*! + * \ingroup bmi2ApiFIFO + * \page bmi2_api_bmi2_extract_gyro bmi2_extract_gyro + * \code + * int8_t bmi2_extract_gyro(struct bmi2_sens_axes_data *gyro_data, + * uint16_t *gyro_length, + * struct bmi2_fifo_frame *fifo, + * const struct bmi2_dev *dev); + * \endcode + * @details This API parses and extracts the gyroscope frames from FIFO data read by the + * "bmi2_read_fifo_data" API and stores it in the "gyro_data" + * structure instance. + * + * @param[out] gyro_data : Structure instance of bmi2_sens_axes_data + * where the parsed data bytes are stored. + * @param[in,out] gyro_length : Number of gyroscope frames. + * @param[in,out] fifo : Structure instance of bmi2_fifo_frame. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_extract_gyro(struct bmi2_sens_axes_data *gyro_data, + uint16_t *gyro_length, + struct bmi2_fifo_frame *fifo, + const struct bmi2_dev *dev); + +/** + * \ingroup bmi2 + * \defgroup bmi2ApiCmd Command Register + * @brief Write commands to the sensor + */ + +/*! + * \ingroup bmi2ApiCmd + * \page bmi2_api_bmi2_set_command_register bmi2_set_command_register + * \code + * int8_t bmi2_set_command_register(uint8_t command, struct bmi2_dev *dev); + * \endcode + * @details This API writes the available sensor specific commands to the sensor. + * + * @param[in] command : Commands to be given to the sensor. + * @param[in] dev : Structure instance of bmi2_dev. + * + *@verbatim + * Commands | Values + * ---------------------|--------------------- + * BMI2_SOFT_RESET_CMD | 0xB6 + * BMI2_FIFO_FLUSH_CMD | 0xB0 + * BMI2_USR_GAIN_CMD | 0x03 + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_set_command_register(uint8_t command, struct bmi2_dev *dev); + +/*! + * \ingroup bmi2ApiFIFO + * \page bmi2_api_bmi2_set_fifo_self_wake_up bmi2_set_fifo_self_wake_up + * \code + * int8_t bmi2_set_fifo_self_wake_up(uint8_t fifo_self_wake_up, struct bmi2_dev *dev); + * \endcode + * @details This API sets the FIFO self wake up functionality in the sensor. + * + * @param[in] fifo_self_wake_up : Variable to set FIFO self wake-up. + * @param[in] dev : Structure instance of bmi2_dev. + * + *@verbatim + * fifo_self_wake_up | Description + * -------------------|--------------- + * BMI2_DISABLE | Disables self wake-up. + * BMI2_ENABLE | Enables self wake-up. + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_set_fifo_self_wake_up(uint8_t fifo_self_wake_up, struct bmi2_dev *dev); + +/*! + * \ingroup bmi2ApiFIFO + * \page bmi2_api_bmi2_get_fifo_self_wake_up bmi2_get_fifo_self_wake_up + * \code + * int8_t bmi2_get_fifo_self_wake_up(uint8_t *fifo_self_wake_up, const struct bmi2_dev *dev); + * \endcode + * @details This API gets the FIFO self wake up functionality from the sensor. + * + * @param[out] fifo_self_wake_up : Pointer variable to get the status of FIFO + * self wake-up. + * @param[in] dev : Structure instance of bmi2_dev. + * + *@verbatim + * fifo_self_wake_up | Description + * -------------------|--------------- + * BMI2_DISABLE | Self wake-up disabled + * BMI2_ENABLE | Self wake-up enabled. + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_get_fifo_self_wake_up(uint8_t *fifo_self_wake_up, struct bmi2_dev *dev); + +/*! + * \ingroup bmi2ApiFIFO + * \page bmi2_api_bmi2_set_fifo_wm bmi2_set_fifo_wm + * \code + * int8_t bmi2_set_fifo_wm(uint16_t fifo_wm, struct bmi2_dev *dev); + * \endcode + * @details This API sets the FIFO water mark level which is set in the sensor. + * + * @param[in] fifo_wm : Variable to set FIFO water-mark level. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_set_fifo_wm(uint16_t fifo_wm, struct bmi2_dev *dev); + +/*! + * \ingroup bmi2ApiFIFO + * \page bmi2_api_bmi2_get_fifo_wm bmi2_get_fifo_wm + * \code + * int8_t bmi2_get_fifo_wm(uint16_t *fifo_wm, const struct bmi2_dev *dev); + * \endcode + * @details This API gets the FIFO water mark level which is set in the sensor. + * + * @param[out] fifo_wm : Pointer variable to store FIFO water-mark level. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_get_fifo_wm(uint16_t *fifo_wm, struct bmi2_dev *dev); + +/*! + * \ingroup bmi2ApiFIFO + * \page bmi2_api_bmi2_set_fifo_filter_data bmi2_set_fifo_filter_data + * \code + * int8_t bmi2_set_fifo_filter_data(uint8_t sens_sel, uint8_t fifo_filter_data, struct bmi2_dev *dev); + * \endcode + * @details This API sets either filtered or un-filtered FIFO accelerometer or + * gyroscope data. + * + * @param[in] sens_sel : Selects either accelerometer or + * gyroscope sensor. + * @param[in] fifo_filter_data : Variable to set the filter data. + * @param[in] dev : Structure instance of bmi2_dev. + * + *@verbatim + * sens_sel | values + * -----------------|---------- + * BMI2_ACCEL | 0x01 + * BMI2_GYRO | 0x02 + *@endverbatim + * + *@verbatim + * Value | fifo_filter_data + * ---------|--------------------- + * 0x00 | Un-filtered data + * 0x01 | Filtered data + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_set_fifo_filter_data(uint8_t sens_sel, uint8_t fifo_filter_data, struct bmi2_dev *dev); + +/*! + * \ingroup bmi2ApiFIFO + * \page bmi2_api_bmi2_get_fifo_filter_data bmi2_get_fifo_filter_data + * \code + * int8_t bmi2_get_fifo_filter_data(uint8_t sens_sel, uint8_t *fifo_filter_data, const struct bmi2_dev *dev); + * \endcode + * @details This API gets the FIFO accelerometer or gyroscope filter data. + * + * @param[in] sens_sel : Selects either accelerometer or + * gyroscope sensor. + * @param[out] fifo_filter_data : Pointer variable to get the filter data. + * @param[in] dev : Structure instance of bmi2_dev. + * + *@verbatim + * sens_sel | values + * -----------------|---------- + * BMI2_ACCEL | 0x01 + * BMI2_GYRO | 0x02 + *@endverbatim + * + *@verbatim + * Value | fifo_filter_data + * ---------|--------------------- + * 0x00 | Un-filtered data + * 0x01 | Filtered data + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_get_fifo_filter_data(uint8_t sens_sel, uint8_t *fifo_filter_data, struct bmi2_dev *dev); + +/*! + * \ingroup bmi2ApiFIFO + * \page bmi2_api_bmi2_set_fifo_down_sample bmi2_set_fifo_down_sample + * \code + * int8_t bmi2_set_fifo_down_sample(uint8_t sens_sel, uint8_t fifo_down_samp, struct bmi2_dev *dev); + * \endcode + * @details This API sets the down sampling rate for FIFO accelerometer or + * gyroscope FIFO data. + * + * @param[in] sens_sel : Selects either either accelerometer or + * gyroscope sensor. + * @param[in] fifo_down_samp : Variable to set the down sampling rate. + * @param[in] dev : Structure instance of bmi2_dev. + * + *@verbatim + * sens_sel | values + * ----------------|---------- + * BMI2_ACCEL | 0x01 + * BMI2_GYRO | 0x02 + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_set_fifo_down_sample(uint8_t sens_sel, uint8_t fifo_down_samp, struct bmi2_dev *dev); + +/*! + * \ingroup bmi2ApiFIFO + * \page bmi2_api_bmi2_get_fifo_down_sample bmi2_get_fifo_down_sample + * \code + * int8_t bmi2_get_fifo_down_sample(uint8_t sens_sel, uint8_t *fifo_down_samp, const struct bmi2_dev *dev); + * \endcode + * @details This API gets the down sampling rate, configured for FIFO + * accelerometer or gyroscope data. + * + * @param[in] sens_sel : Selects either either accelerometer or + * gyroscope sensor. + * @param[out] fifo_down_samp : Pointer variable to store the down sampling rate + * @param[in] dev : Structure instance of bmi2_dev. + * + *@verbatim + * sens_sel | values + * ----------------|---------- + * BMI2_ACCEL | 0x01 + * BMI2_GYRO | 0x02 + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_get_fifo_down_sample(uint8_t sens_sel, uint8_t *fifo_down_samp, struct bmi2_dev *dev); + +/*! + * \ingroup bmi2ApiFIFO + * \page bmi2_api_bmi2_get_fifo_length bmi2_get_fifo_length + * \code + * int8_t bmi2_get_fifo_length(uint16_t *fifo_length, const struct bmi2_dev *dev); + * \endcode + * @details This API gets the length of FIFO data available in the sensor in + * bytes. + * + * @param[out] fifo_length : Pointer variable to store the value of FIFO byte + * counter. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @note The byte counter is updated each time a complete frame is read or + * written. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_get_fifo_length(uint16_t *fifo_length, struct bmi2_dev *dev); + +/** + * \ingroup bmi2 + * \defgroup bmi2ApiOIS OIS + * @brief OIS operations of the sensor + */ + +/*! + * \ingroup bmi2ApiOIS + * \page bmi2_api_bmi2_set_ois_interface bmi2_set_ois_interface + * \code + * int8_t bmi2_set_ois_interface(uint8_t enable, struct bmi2_dev *dev); + * \endcode + * @details This API enables/disables OIS interface. + * + * @param[in] enable : To enable/disable OIS interface. + * @param[in] dev : Structure instance of bmi2_dev. + * + *@verbatim + * Enable | Description + * -------------|--------------- + * BMI2_DISABLE | Disables OIS interface. + * BMI2_ENABLE | Enables OIS interface. + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_set_ois_interface(uint8_t enable, struct bmi2_dev *dev); + +/** + * \ingroup bmi2 + * \defgroup bmi2ApiAux Auxiliary sensor + * @brief Auxiliary sensor operations of the sensor + */ + +/*! + * \ingroup bmi2ApiAux + * \page bmi2_api_bmi2_read_aux_man_mode bmi2_read_aux_man_mode + * \code + * int8_t bmi2_read_aux_man_mode(uint8_t reg_addr, uint8_t *aux_data, uint16_t len, struct bmi2_dev *dev); + * \endcode + * @details This API reads the user-defined bytes of data from the given register + * address of auxiliary sensor in manual mode. + * + * @param[in] reg_addr : Address from where data is read. + * @param[out] aux_data : Pointer to the stored buffer. + * @param[in] len : Total length of data to be read. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @note Change of BMI2_AUX_RD_ADDR is only allowed if AUX is not busy. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_read_aux_man_mode(uint8_t reg_addr, uint8_t *aux_data, uint16_t len, struct bmi2_dev *dev); + +/*! + * \ingroup bmi2ApiAux + * \page bmi2_api_bmi2_write_aux_man_mode bmi2_write_aux_man_mode + * \code + * int8_t bmi2_write_aux_man_mode(uint8_t reg_addr, const uint8_t *aux_data, uint16_t len, struct bmi2_dev *dev); + * \endcode + * @details This API writes the user-defined bytes of data and the address of + * auxiliary sensor where data is to be written in manual mode. + * + * @param[in] reg_addr : AUX address where data is to be written. + * @param[in] aux_data : Pointer to data to be written. + * @param[in] len : Total length of data to be written. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @note Change of BMI2_AUX_WR_ADDR is only allowed if AUX is not busy. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_write_aux_man_mode(uint8_t reg_addr, const uint8_t *aux_data, uint16_t len, struct bmi2_dev *dev); + +/*! + * \ingroup bmi2ApiAux + * \page bmi2_api_bmi2_write_aux_interleaved bmi2_write_aux_interleaved + * \code + * int8_t bmi2_write_aux_interleaved(uint8_t reg_addr, const uint8_t *aux_data, uint16_t len, struct bmi2_dev *dev); + * \endcode + * @details This API writes the user-defined bytes of data and the address of + * auxiliary sensor where data is to be written, from an interleaved input, + * in manual mode. + * + * @param[in] reg_addr : AUX address where data is to be written. + * @param[in] aux_data : Pointer to data to be written. + * @param[in] len : Total length of data to be written. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @note Change of BMI2_AUX_WR_ADDR is only allowed if AUX is not busy. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_write_aux_interleaved(uint8_t reg_addr, const uint8_t *aux_data, uint16_t len, struct bmi2_dev *dev); + +/** + * \ingroup bmi2 + * \defgroup bmi2ApiStatus Sensor Status + * @brief Get sensor status + */ + +/*! + * \ingroup bmi2ApiStatus + * \page bmi2_api_bmi2_get_status bmi2_get_status + * \code + * int8_t bmi2_get_status(uint8_t *status, const struct bmi2_dev *dev); + * \endcode + * @details This API gets the data ready status of accelerometer, gyroscope, + * auxiliary, ready status of command decoder and busy status of auxiliary. + * + * @param[out] status : Pointer variable to the status. + * @param[in] dev : Structure instance of bmi2_dev. + * + *@verbatim + * Value | Status + * ---------|--------------------- + * 0x80 | DRDY_ACC + * 0x40 | DRDY_GYR + * 0x20 | DRDY_AUX + * 0x10 | CMD_RDY + * 0x04 | AUX_BUSY + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_get_status(uint8_t *status, struct bmi2_dev *dev); + +/** + * \ingroup bmi2 + * \defgroup bmi2ApiWSync Sync commands + * @brief Write sync commands + */ + +/*! + * \ingroup bmi2ApiWSync + * \page bmi2_api_bmi2_write_sync_commands bmi2_write_sync_commands + * \code + * int8_t bmi2_write_sync_commands(const uint8_t *command, uint8_t n_comm, struct bmi2_dev *dev); + * \endcode + * @details This API can be used to write sync commands like ODR, sync period, + * frequency and phase, resolution ratio, sync time and delay time. + * + * @param[in] command : Sync command to be written. + * @param[in] n_comm : Length of the command. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_write_sync_commands(const uint8_t *command, uint8_t n_comm, struct bmi2_dev *dev); + +/** + * \ingroup bmi2 + * \defgroup bmi2ApiASelftest Accel self test + * @brief Perform accel self test + */ + +/*! + * \ingroup bmi2ApiASelftest + * \page bmi2_api_bmi2_perform_accel_self_test bmi2_perform_accel_self_test + * \code + * int8_t bmi2_perform_accel_self_test(struct bmi2_dev *dev); + * \endcode + * @details This API performs self-test to check the proper functionality of the + * accelerometer sensor. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_perform_accel_self_test(struct bmi2_dev *dev); + +/*! + * \ingroup bmi2ApiInt + * \page bmi2_api_bmi2_map_feat_int bmi2_map_feat_int + * \code + * int8_t bmi2_map_feat_int(const struct bmi2_sens_int_config *sens_int, struct bmi2_dev *dev); + * \endcode + * @details This API maps/unmaps feature interrupts to that of interrupt pins. + * + * @param[in] sens_int : Structure instance of bmi2_sens_int_config. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_map_feat_int(uint8_t type, enum bmi2_hw_int_pin hw_int_pin, struct bmi2_dev *dev); + +/*! + * \ingroup bmi2ApiInt + * \page bmi2_api_bmi2_map_data_int bmi2_map_data_int + * \code + * int8_t bmi2_map_data_int(uint8_t data_int, enum bmi2_hw_int_pin int_pin, struct bmi2_dev *dev); + * \endcode + * @details This API maps/un-maps data interrupts to that of interrupt pins. + * + * @param[in] int_pin : Interrupt pin selected. + * @param[in] data_int : Type of data interrupt to be mapped. + * @param[in] dev : Structure instance of bmi2_dev. + * + *@verbatim + * data_int | Mask values + * ---------------------|--------------------- + * BMI2_FFULL_INT | 0x01 + * BMI2_FWM_INT | 0x02 + * BMI2_DRDY_INT | 0x04 + * BMI2_ERR_INT | 0x08 + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_map_data_int(uint8_t data_int, enum bmi2_hw_int_pin int_pin, struct bmi2_dev *dev); + +/** + * \ingroup bmi2 + * \defgroup bmi2ApiRemap Remap Axes + * @brief Set / Get remap axes values from the sensor + */ + +/*! + * \ingroup bmi2ApiRemap + * \page bmi2_api_bmi2_get_remap_axes bmi2_get_remap_axes + * \code + * int8_t bmi2_get_remap_axes(struct bmi2_remap *remapped_axis, struct bmi2_dev *dev); + * \endcode + * @details This API gets the re-mapped x, y and z axes from the sensor and + * updates the values in the device structure. + * + * @param[out] remapped_axis : Structure that stores re-mapped axes. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_get_remap_axes(struct bmi2_remap *remapped_axis, struct bmi2_dev *dev); + +/*! + * \ingroup bmi2ApiRemap + * \page bmi2_api_bmi2_set_remap_axes bmi2_set_remap_axes + * \code + * int8_t bmi2_set_remap_axes(const struct bmi2_remap *remapped_axis, struct bmi2_dev *dev); + * \endcode + * @details This API sets the re-mapped x, y and z axes to the sensor and + * updates them in the device structure. + * + * @param[in] remapped_axis : Structure that stores re-mapped axes. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_set_remap_axes(const struct bmi2_remap *remapped_axis, struct bmi2_dev *dev); + +/** + * \ingroup bmi2 + * \defgroup bmi2ApiGyroOC Gyro Offset Compensation + * @brief Gyro Offset Compensation operations of the sensor + */ + +/*! + * \ingroup bmi2ApiGyroOC + * \page bmi2_api_bmi2_set_gyro_offset_comp bmi2_set_gyro_offset_comp + * \code + * int8_t bmi2_set_gyro_offset_comp(uint8_t enable, struct bmi2_dev *dev); + * \endcode + * @details This API enables/disables gyroscope offset compensation. It adds the + * offsets defined in the offset register with gyroscope data. + * + * @param[in] enable : Enables/Disables gyroscope offset compensation. + * @param[in] dev : Structure instance of bmi2_dev. + * + *@verbatim + * enable | Description + * -------------|--------------- + * BMI2_ENABLE | Enables gyroscope offset compensation. + * BMI2_DISABLE | Disables gyroscope offset compensation. + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_set_gyro_offset_comp(uint8_t enable, struct bmi2_dev *dev); + +/*! + * \ingroup bmi2ApiGyroOC + * \page bmi2_api_bmi2_read_gyro_offset_comp_axes bmi2_read_gyro_offset_comp_axes + * \code + * int8_t bmi2_read_gyro_offset_comp_axes(struct bmi2_sens_axes_data *gyr_off_comp_axes, const struct bmi2_dev *dev); + * \endcode + * @details This API reads the gyroscope bias values for each axis which is used + * for gyroscope offset compensation. + * + * @param[out] gyr_off_comp_axes: Structure to store gyroscope offset + * compensated values. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_read_gyro_offset_comp_axes(struct bmi2_sens_axes_data *gyr_off_comp_axes, struct bmi2_dev *dev); + +/*! + * \ingroup bmi2ApiGyroOC + * \page bmi2_api_bmi2_write_gyro_offset_comp_axes bmi2_write_gyro_offset_comp_axes + * \code + * int8_t bmi2_write_gyro_offset_comp_axes(const struct bmi2_sens_axes_data *gyr_off_comp_axes, struct bmi2_dev *dev); + * \endcode + * @details This API writes the gyroscope bias values for each axis which is used + * for gyroscope offset compensation. + * + * @param[in] gyr_off_comp_axes : Structure to store gyroscope offset + * compensated values. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_write_gyro_offset_comp_axes(const struct bmi2_sens_axes_data *gyr_off_comp_axes, struct bmi2_dev *dev); + +/** + * \ingroup bmi2 + * \defgroup bmi2ApiGyroCS Gyro cross sensitivity + * @brief Gyro Cross sensitivity operation + */ + +/*! + * \ingroup bmi2ApiGyroCS + * \page bmi2_api_bmi2_get_gyro_cross_sense bmi2_get_gyro_cross_sense + * \code + * int8_t bmi2_get_gyro_cross_sense(struct bmi2_dev *dev); + * \endcode + * @details This API updates the cross sensitivity coefficient between gyroscope's + * X and Z axes. + * + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_get_gyro_cross_sense(struct bmi2_dev *dev); + +/** + * \ingroup bmi2 + * \defgroup bmi2ApiInts Internal Status + * @brief Get Internal Status of the sensor + */ + +/*! + * \ingroup bmi2ApiInts + * \page bmi2_api_bmi2_get_internal_status bmi2_get_internal_status + * \code + * int8_t bmi2_get_internal_status(uint8_t *int_stat, const struct bmi2_dev *dev); + * \endcode + * @details This API gets error bits and message indicating internal status. + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[out] int_stat : Pointer variable to store error bits and + * message. + * + *@verbatim + * Internal status | *int_stat + * ---------------------|--------------------- + * BMI2_NOT_INIT | 0x00 + * BMI2_INIT_OK | 0x01 + * BMI2_INIT_ERR | 0x02 + * BMI2_DRV_ERR | 0x03 + * BMI2_SNS_STOP | 0x04 + * BMI2_NVM_ERROR | 0x05 + * BMI2_START_UP_ERROR | 0x06 + * BMI2_COMPAT_ERROR | 0x07 + * BMI2_VFM_SKIPPED | 0x10 + * BMI2_AXES_MAP_ERROR | 0x20 + * BMI2_ODR_50_HZ_ERROR | 0x40 + * BMI2_ODR_HIGH_ERROR | 0x80 + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_get_internal_status(uint8_t *int_stat, struct bmi2_dev *dev); + +/** + * \ingroup bmi2 + * \defgroup bmi2ApiFOC FOC + * @brief FOC operations of the sensor + */ + +/*! + * \ingroup bmi2ApiFOC + * \page bmi2_api_bmi2_perform_accel_foc bmi2_perform_accel_foc + * \code + * int8_t bmi2_perform_accel_foc(const struct bmi2_accel_foc_g_value *accel_g_value, struct bmi2_dev *dev); + * \endcode + * @details This API performs Fast Offset Compensation for accelerometer. + * + * @param[in] accel_g_value : This parameter selects the accel foc + * axis to be performed + * + * input format is {x, y, z, sign}. '1' to enable. '0' to disable + * + * eg to choose x axis {1, 0, 0, 0} + * eg to choose -x axis {1, 0, 0, 1} + * + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_perform_accel_foc(const struct bmi2_accel_foc_g_value *accel_g_value, struct bmi2_dev *dev); + +/*! + * \ingroup bmi2ApiFOC + * \page bmi2_api_bmi2_perform_gyro_foc bmi2_perform_gyro_foc + * \code + * int8_t bmi2_perform_gyro_foc(struct bmi2_dev *dev); + * \endcode + * @details This API performs Fast Offset Compensation for gyroscope. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_perform_gyro_foc(struct bmi2_dev *dev); + +/** + * \ingroup bmi2 + * \defgroup bmi2ApiCRT CRT + * @brief CRT operations of the sensor + */ + +/*! + * \ingroup bmi2ApiCRT + * \page bmi2_api_bmi2_do_crt bmi2_do_crt + * \code + * int8_t bmi2_do_crt(struct bmi2_dev *dev); + * \endcode + * @details API performs Component Re-Trim calibration (CRT). + * + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + * + * @note CRT calibration takes approximately 500ms & maximum time out configured as 2 seconds + */ +int8_t bmi2_do_crt(struct bmi2_dev *dev); + +/** + * \ingroup bmi2 + * \defgroup bmi2ApiCRTSt CRT and self test + * @brief Enable / Abort CRT and self test operations of gyroscope + */ + +/*! + * \ingroup bmi2ApiCRTSt + * \page bmi2_api_bmi2_set_gyro_self_test_crt bmi2_set_gyro_self_test_crt + * \code + * int8_t bmi2_set_gyro_self_test_crt + * \endcode + * @details This api is used to enable the gyro self-test and crt. + * *gyro_self_test_crt -> 0 then gyro self test enable + * *gyro_self_test_crt -> 1 then CRT enable + * + * @param[in] gyro_self_test_crt : enable the gyro self test or crt. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_set_gyro_self_test_crt(uint8_t *gyro_self_test_crt, struct bmi2_dev *dev); + +/*! + * \ingroup bmi2ApiCRTSt + * \page bmi2_api_bmi2_abort_crt_gyro_st bmi2_abort_crt_gyro_st + * \code + * int8_t bmi2_abort_crt_gyro_st(struct bmi2_dev *dev); + * \endcode + * @details This api is used to abort ongoing crt or gyro self test. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_abort_crt_gyro_st(struct bmi2_dev *dev); + +/*! + * \ingroup bmi2ApiASelftest + * \page bmi2_api_bmi2_do_gyro_st bmi2_do_gyro_st + * \code + * int8_t bmi2_do_gyro_st + * \endcode + * @details this api is used to perform gyroscope self test. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_do_gyro_st(struct bmi2_dev *dev); + +/** + * \ingroup bmi2 + * \defgroup bmi2ApiNVM NVM + * @brief NVM operations of the sensor + */ + +/*! + * \ingroup bmi2ApiNVM + * \page bmi2_api_bmi2_nvm_prog bmi2_nvm_prog + * \code + * int8_t bmi2_nvm_prog + * \endcode + * @details This api is used for programming the non volatile memory(nvm) + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_nvm_prog(struct bmi2_dev *dev); + +/*! + * @brief This API extracts the input feature configuration + * details like page and start address from the look-up table. + * + * @param[out] feat_config : Structure that stores feature configurations. + * @param[in] type : Type of feature or sensor. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Returns the feature found flag. + * + * @retval BMI2_FALSE : Feature not found + * BMI2_TRUE : Feature found + */ +uint8_t bmi2_extract_input_feat_config(struct bmi2_feature_config *feat_config, uint8_t type, + const struct bmi2_dev *dev); + +/*! + * @brief This API is used to get the feature configuration from the + * selected page. + * + * @param[in] sw_page : Switches to the desired page. + * @param[out] feat_config : Pointer to the feature configuration. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_get_feat_config(uint8_t sw_page, uint8_t *feat_config, struct bmi2_dev *dev); + +#ifdef __cplusplus +} +#endif /* End of CPP guard */ + +#endif /* BMI2_H_ */ diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270.c b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270.c new file mode 100644 index 0000000000..124a984b8b --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270.c @@ -0,0 +1,4451 @@ +/** +* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. +* +* BSD-3-Clause +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* +* 3. Neither the name of the copyright holder nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, +* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING +* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +* @file bmi270.c +* @date 2020-11-04 +* @version v2.63.1 +* +*/ + +/***************************************************************************/ + +/*! Header files + ****************************************************************************/ +#include "bmi270.h" + +/***************************************************************************/ + +/*! Global Variable + ****************************************************************************/ + +/*! @name Global array that stores the configuration file of BMI270 */ +const uint8_t bmi270_config_file[] = { + 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x3d, 0xb1, 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x91, 0x03, 0x80, 0x2e, 0xbc, + 0xb0, 0x80, 0x2e, 0xa3, 0x03, 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x00, 0xb0, 0x50, 0x30, 0x21, 0x2e, 0x59, 0xf5, + 0x10, 0x30, 0x21, 0x2e, 0x6a, 0xf5, 0x80, 0x2e, 0x3b, 0x03, 0x00, 0x00, 0x00, 0x00, 0x08, 0x19, 0x01, 0x00, 0x22, + 0x00, 0x75, 0x00, 0x00, 0x10, 0x00, 0x10, 0xd1, 0x00, 0xb3, 0x43, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0xe0, 0x5f, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x92, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x08, 0x19, 0x00, 0x00, 0x88, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, + 0xe0, 0xaa, 0x38, 0x05, 0xe0, 0x90, 0x30, 0xfa, 0x00, 0x96, 0x00, 0x4b, 0x09, 0x11, 0x00, 0x11, 0x00, 0x02, 0x00, + 0x2d, 0x01, 0xd4, 0x7b, 0x3b, 0x01, 0xdb, 0x7a, 0x04, 0x00, 0x3f, 0x7b, 0xcd, 0x6c, 0xc3, 0x04, 0x85, 0x09, 0xc3, + 0x04, 0xec, 0xe6, 0x0c, 0x46, 0x01, 0x00, 0x27, 0x00, 0x19, 0x00, 0x96, 0x00, 0xa0, 0x00, 0x01, 0x00, 0x0c, 0x00, + 0xf0, 0x3c, 0x00, 0x01, 0x01, 0x00, 0x03, 0x00, 0x01, 0x00, 0x0e, 0x00, 0x00, 0x00, 0x32, 0x00, 0x05, 0x00, 0xee, + 0x06, 0x04, 0x00, 0xc8, 0x00, 0x00, 0x00, 0x04, 0x00, 0xa8, 0x05, 0xee, 0x06, 0x00, 0x04, 0xbc, 0x02, 0xb3, 0x00, + 0x85, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0xb4, 0x00, 0x01, 0x00, 0xb9, 0x00, 0x01, 0x00, 0x98, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x01, 0x00, 0x80, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x2e, 0x00, 0xc1, 0xfd, 0x2d, 0xde, + 0x00, 0xeb, 0x00, 0xda, 0x00, 0x00, 0x0c, 0xff, 0x0f, 0x00, 0x04, 0xc0, 0x00, 0x5b, 0xf5, 0xc9, 0x01, 0x1e, 0xf2, + 0x80, 0x00, 0x3f, 0xff, 0x19, 0xf4, 0x58, 0xf5, 0x66, 0xf5, 0x64, 0xf5, 0xc0, 0xf1, 0xf0, 0x00, 0xe0, 0x00, 0xcd, + 0x01, 0xd3, 0x01, 0xdb, 0x01, 0xff, 0x7f, 0xff, 0x01, 0xe4, 0x00, 0x74, 0xf7, 0xf3, 0x00, 0xfa, 0x00, 0xff, 0x3f, + 0xca, 0x03, 0x6c, 0x38, 0x56, 0xfe, 0x44, 0xfd, 0xbc, 0x02, 0xf9, 0x06, 0x00, 0xfc, 0x12, 0x02, 0xae, 0x01, 0x58, + 0xfa, 0x9a, 0xfd, 0x77, 0x05, 0xbb, 0x02, 0x96, 0x01, 0x95, 0x01, 0x7f, 0x01, 0x82, 0x01, 0x89, 0x01, 0x87, 0x01, + 0x88, 0x01, 0x8a, 0x01, 0x8c, 0x01, 0x8f, 0x01, 0x8d, 0x01, 0x92, 0x01, 0x91, 0x01, 0xdd, 0x00, 0x9f, 0x01, 0x7e, + 0x01, 0xdb, 0x00, 0xb6, 0x01, 0x70, 0x69, 0x26, 0xd3, 0x9c, 0x07, 0x1f, 0x05, 0x9d, 0x00, 0x00, 0x08, 0xbc, 0x05, + 0x37, 0xfa, 0xa2, 0x01, 0xaa, 0x01, 0xa1, 0x01, 0xa8, 0x01, 0xa0, 0x01, 0xa8, 0x05, 0xb4, 0x01, 0xb4, 0x01, 0xce, + 0x00, 0xd0, 0x00, 0xfc, 0x00, 0xc5, 0x01, 0xff, 0xfb, 0xb1, 0x00, 0x00, 0x38, 0x00, 0x30, 0xfd, 0xf5, 0xfc, 0xf5, + 0xcd, 0x01, 0xa0, 0x00, 0x5f, 0xff, 0x00, 0x40, 0xff, 0x00, 0x00, 0x80, 0x6d, 0x0f, 0xeb, 0x00, 0x7f, 0xff, 0xc2, + 0xf5, 0x68, 0xf7, 0xb3, 0xf1, 0x67, 0x0f, 0x5b, 0x0f, 0x61, 0x0f, 0x80, 0x0f, 0x58, 0xf7, 0x5b, 0xf7, 0x83, 0x0f, + 0x86, 0x00, 0x72, 0x0f, 0x85, 0x0f, 0xc6, 0xf1, 0x7f, 0x0f, 0x6c, 0xf7, 0x00, 0xe0, 0x00, 0xff, 0xd1, 0xf5, 0x87, + 0x0f, 0x8a, 0x0f, 0xff, 0x03, 0xf0, 0x3f, 0x8b, 0x00, 0x8e, 0x00, 0x90, 0x00, 0xb9, 0x00, 0x2d, 0xf5, 0xca, 0xf5, + 0xcb, 0x01, 0x20, 0xf2, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x30, 0x50, 0x98, 0x2e, + 0xd7, 0x0e, 0x50, 0x32, 0x98, 0x2e, 0xfa, 0x03, 0x00, 0x30, 0xf0, 0x7f, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0x00, + 0x2e, 0x01, 0x80, 0x08, 0xa2, 0xfb, 0x2f, 0x98, 0x2e, 0xba, 0x03, 0x21, 0x2e, 0x19, 0x00, 0x01, 0x2e, 0xee, 0x00, + 0x00, 0xb2, 0x07, 0x2f, 0x01, 0x2e, 0x19, 0x00, 0x00, 0xb2, 0x03, 0x2f, 0x01, 0x50, 0x03, 0x52, 0x98, 0x2e, 0x07, + 0xcc, 0x01, 0x2e, 0xdd, 0x00, 0x00, 0xb2, 0x27, 0x2f, 0x05, 0x2e, 0x8a, 0x00, 0x05, 0x52, 0x98, 0x2e, 0xc7, 0xc1, + 0x03, 0x2e, 0xe9, 0x00, 0x40, 0xb2, 0xf0, 0x7f, 0x08, 0x2f, 0x01, 0x2e, 0x19, 0x00, 0x00, 0xb2, 0x04, 0x2f, 0x00, + 0x30, 0x21, 0x2e, 0xe9, 0x00, 0x98, 0x2e, 0xb4, 0xb1, 0x01, 0x2e, 0x18, 0x00, 0x00, 0xb2, 0x10, 0x2f, 0x05, 0x50, + 0x98, 0x2e, 0x4d, 0xc3, 0x05, 0x50, 0x98, 0x2e, 0x5a, 0xc7, 0x98, 0x2e, 0xf9, 0xb4, 0x98, 0x2e, 0x54, 0xb2, 0x98, + 0x2e, 0x67, 0xb6, 0x98, 0x2e, 0x17, 0xb2, 0x10, 0x30, 0x21, 0x2e, 0x77, 0x00, 0x01, 0x2e, 0xef, 0x00, 0x00, 0xb2, + 0x04, 0x2f, 0x98, 0x2e, 0x7a, 0xb7, 0x00, 0x30, 0x21, 0x2e, 0xef, 0x00, 0x01, 0x2e, 0xd4, 0x00, 0x04, 0xae, 0x0b, + 0x2f, 0x01, 0x2e, 0xdd, 0x00, 0x00, 0xb2, 0x07, 0x2f, 0x05, 0x52, 0x98, 0x2e, 0x8e, 0x0e, 0x00, 0xb2, 0x02, 0x2f, + 0x10, 0x30, 0x21, 0x2e, 0x7d, 0x00, 0x01, 0x2e, 0x7d, 0x00, 0x00, 0x90, 0x90, 0x2e, 0xf1, 0x02, 0x01, 0x2e, 0xd7, + 0x00, 0x00, 0xb2, 0x04, 0x2f, 0x98, 0x2e, 0x2f, 0x0e, 0x00, 0x30, 0x21, 0x2e, 0x7b, 0x00, 0x01, 0x2e, 0x7b, 0x00, + 0x00, 0xb2, 0x12, 0x2f, 0x01, 0x2e, 0xd4, 0x00, 0x00, 0x90, 0x02, 0x2f, 0x98, 0x2e, 0x1f, 0x0e, 0x09, 0x2d, 0x98, + 0x2e, 0x81, 0x0d, 0x01, 0x2e, 0xd4, 0x00, 0x04, 0x90, 0x02, 0x2f, 0x50, 0x32, 0x98, 0x2e, 0xfa, 0x03, 0x00, 0x30, + 0x21, 0x2e, 0x7b, 0x00, 0x01, 0x2e, 0x7c, 0x00, 0x00, 0xb2, 0x90, 0x2e, 0x09, 0x03, 0x01, 0x2e, 0x7c, 0x00, 0x01, + 0x31, 0x01, 0x08, 0x00, 0xb2, 0x04, 0x2f, 0x98, 0x2e, 0x47, 0xcb, 0x10, 0x30, 0x21, 0x2e, 0x77, 0x00, 0x81, 0x30, + 0x01, 0x2e, 0x7c, 0x00, 0x01, 0x08, 0x00, 0xb2, 0x61, 0x2f, 0x03, 0x2e, 0x89, 0x00, 0x01, 0x2e, 0xd4, 0x00, 0x98, + 0xbc, 0x98, 0xb8, 0x05, 0xb2, 0x0f, 0x58, 0x23, 0x2f, 0x07, 0x90, 0x09, 0x54, 0x00, 0x30, 0x37, 0x2f, 0x15, 0x41, + 0x04, 0x41, 0xdc, 0xbe, 0x44, 0xbe, 0xdc, 0xba, 0x2c, 0x01, 0x61, 0x00, 0x0f, 0x56, 0x4a, 0x0f, 0x0c, 0x2f, 0xd1, + 0x42, 0x94, 0xb8, 0xc1, 0x42, 0x11, 0x30, 0x05, 0x2e, 0x6a, 0xf7, 0x2c, 0xbd, 0x2f, 0xb9, 0x80, 0xb2, 0x08, 0x22, + 0x98, 0x2e, 0xc3, 0xb7, 0x21, 0x2d, 0x61, 0x30, 0x23, 0x2e, 0xd4, 0x00, 0x98, 0x2e, 0xc3, 0xb7, 0x00, 0x30, 0x21, + 0x2e, 0x5a, 0xf5, 0x18, 0x2d, 0xe1, 0x7f, 0x50, 0x30, 0x98, 0x2e, 0xfa, 0x03, 0x0f, 0x52, 0x07, 0x50, 0x50, 0x42, + 0x70, 0x30, 0x0d, 0x54, 0x42, 0x42, 0x7e, 0x82, 0xe2, 0x6f, 0x80, 0xb2, 0x42, 0x42, 0x05, 0x2f, 0x21, 0x2e, 0xd4, + 0x00, 0x10, 0x30, 0x98, 0x2e, 0xc3, 0xb7, 0x03, 0x2d, 0x60, 0x30, 0x21, 0x2e, 0xd4, 0x00, 0x01, 0x2e, 0xd4, 0x00, + 0x06, 0x90, 0x18, 0x2f, 0x01, 0x2e, 0x76, 0x00, 0x0b, 0x54, 0x07, 0x52, 0xe0, 0x7f, 0x98, 0x2e, 0x7a, 0xc1, 0xe1, + 0x6f, 0x08, 0x1a, 0x40, 0x30, 0x08, 0x2f, 0x21, 0x2e, 0xd4, 0x00, 0x20, 0x30, 0x98, 0x2e, 0xaf, 0xb7, 0x50, 0x32, + 0x98, 0x2e, 0xfa, 0x03, 0x05, 0x2d, 0x98, 0x2e, 0x38, 0x0e, 0x00, 0x30, 0x21, 0x2e, 0xd4, 0x00, 0x00, 0x30, 0x21, + 0x2e, 0x7c, 0x00, 0x18, 0x2d, 0x01, 0x2e, 0xd4, 0x00, 0x03, 0xaa, 0x01, 0x2f, 0x98, 0x2e, 0x45, 0x0e, 0x01, 0x2e, + 0xd4, 0x00, 0x3f, 0x80, 0x03, 0xa2, 0x01, 0x2f, 0x00, 0x2e, 0x02, 0x2d, 0x98, 0x2e, 0x5b, 0x0e, 0x30, 0x30, 0x98, + 0x2e, 0xce, 0xb7, 0x00, 0x30, 0x21, 0x2e, 0x7d, 0x00, 0x50, 0x32, 0x98, 0x2e, 0xfa, 0x03, 0x01, 0x2e, 0x77, 0x00, + 0x00, 0xb2, 0x24, 0x2f, 0x98, 0x2e, 0xf5, 0xcb, 0x03, 0x2e, 0xd5, 0x00, 0x11, 0x54, 0x01, 0x0a, 0xbc, 0x84, 0x83, + 0x86, 0x21, 0x2e, 0xc9, 0x01, 0xe0, 0x40, 0x13, 0x52, 0xc4, 0x40, 0x82, 0x40, 0xa8, 0xb9, 0x52, 0x42, 0x43, 0xbe, + 0x53, 0x42, 0x04, 0x0a, 0x50, 0x42, 0xe1, 0x7f, 0xf0, 0x31, 0x41, 0x40, 0xf2, 0x6f, 0x25, 0xbd, 0x08, 0x08, 0x02, + 0x0a, 0xd0, 0x7f, 0x98, 0x2e, 0xa8, 0xcf, 0x06, 0xbc, 0xd1, 0x6f, 0xe2, 0x6f, 0x08, 0x0a, 0x80, 0x42, 0x98, 0x2e, + 0x58, 0xb7, 0x00, 0x30, 0x21, 0x2e, 0xee, 0x00, 0x21, 0x2e, 0x77, 0x00, 0x21, 0x2e, 0xdd, 0x00, 0x80, 0x2e, 0xf4, + 0x01, 0x1a, 0x24, 0x22, 0x00, 0x80, 0x2e, 0xec, 0x01, 0x10, 0x50, 0xfb, 0x7f, 0x98, 0x2e, 0xf3, 0x03, 0x57, 0x50, + 0xfb, 0x6f, 0x01, 0x30, 0x71, 0x54, 0x11, 0x42, 0x42, 0x0e, 0xfc, 0x2f, 0xc0, 0x2e, 0x01, 0x42, 0xf0, 0x5f, 0x80, + 0x2e, 0x00, 0xc1, 0xfd, 0x2d, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x9a, 0x01, + 0x34, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x20, 0x50, 0xe7, 0x7f, 0xf6, 0x7f, 0x06, 0x32, 0x0f, 0x2e, 0x61, 0xf5, 0xfe, 0x09, 0xc0, 0xb3, 0x04, + 0x2f, 0x17, 0x30, 0x2f, 0x2e, 0xef, 0x00, 0x2d, 0x2e, 0x61, 0xf5, 0xf6, 0x6f, 0xe7, 0x6f, 0xe0, 0x5f, 0xc8, 0x2e, + 0x20, 0x50, 0xe7, 0x7f, 0xf6, 0x7f, 0x46, 0x30, 0x0f, 0x2e, 0xa4, 0xf1, 0xbe, 0x09, 0x80, 0xb3, 0x06, 0x2f, 0x0d, + 0x2e, 0xd4, 0x00, 0x84, 0xaf, 0x02, 0x2f, 0x16, 0x30, 0x2d, 0x2e, 0x7b, 0x00, 0x86, 0x30, 0x2d, 0x2e, 0x60, 0xf5, + 0xf6, 0x6f, 0xe7, 0x6f, 0xe0, 0x5f, 0xc8, 0x2e, 0x01, 0x2e, 0x77, 0xf7, 0x09, 0xbc, 0x0f, 0xb8, 0x00, 0xb2, 0x10, + 0x50, 0xfb, 0x7f, 0x10, 0x30, 0x0b, 0x2f, 0x03, 0x2e, 0x8a, 0x00, 0x96, 0xbc, 0x9f, 0xb8, 0x40, 0xb2, 0x05, 0x2f, + 0x03, 0x2e, 0x68, 0xf7, 0x9e, 0xbc, 0x9f, 0xb8, 0x40, 0xb2, 0x07, 0x2f, 0x03, 0x2e, 0x7e, 0x00, 0x41, 0x90, 0x01, + 0x2f, 0x98, 0x2e, 0xdc, 0x03, 0x03, 0x2c, 0x00, 0x30, 0x21, 0x2e, 0x7e, 0x00, 0xfb, 0x6f, 0xf0, 0x5f, 0xb8, 0x2e, + 0x20, 0x50, 0xe0, 0x7f, 0xfb, 0x7f, 0x00, 0x2e, 0x27, 0x50, 0x98, 0x2e, 0x3b, 0xc8, 0x29, 0x50, 0x98, 0x2e, 0xa7, + 0xc8, 0x01, 0x50, 0x98, 0x2e, 0x55, 0xcc, 0xe1, 0x6f, 0x2b, 0x50, 0x98, 0x2e, 0xe0, 0xc9, 0xfb, 0x6f, 0x00, 0x30, + 0xe0, 0x5f, 0x21, 0x2e, 0x7e, 0x00, 0xb8, 0x2e, 0x73, 0x50, 0x01, 0x30, 0x57, 0x54, 0x11, 0x42, 0x42, 0x0e, 0xfc, + 0x2f, 0xb8, 0x2e, 0x21, 0x2e, 0x59, 0xf5, 0x10, 0x30, 0xc0, 0x2e, 0x21, 0x2e, 0x4a, 0xf1, 0x90, 0x50, 0xf7, 0x7f, + 0xe6, 0x7f, 0xd5, 0x7f, 0xc4, 0x7f, 0xb3, 0x7f, 0xa1, 0x7f, 0x90, 0x7f, 0x82, 0x7f, 0x7b, 0x7f, 0x98, 0x2e, 0x35, + 0xb7, 0x00, 0xb2, 0x90, 0x2e, 0x97, 0xb0, 0x03, 0x2e, 0x8f, 0x00, 0x07, 0x2e, 0x91, 0x00, 0x05, 0x2e, 0xb1, 0x00, + 0x3f, 0xba, 0x9f, 0xb8, 0x01, 0x2e, 0xb1, 0x00, 0xa3, 0xbd, 0x4c, 0x0a, 0x05, 0x2e, 0xb1, 0x00, 0x04, 0xbe, 0xbf, + 0xb9, 0xcb, 0x0a, 0x4f, 0xba, 0x22, 0xbd, 0x01, 0x2e, 0xb3, 0x00, 0xdc, 0x0a, 0x2f, 0xb9, 0x03, 0x2e, 0xb8, 0x00, + 0x0a, 0xbe, 0x9a, 0x0a, 0xcf, 0xb9, 0x9b, 0xbc, 0x01, 0x2e, 0x97, 0x00, 0x9f, 0xb8, 0x93, 0x0a, 0x0f, 0xbc, 0x91, + 0x0a, 0x0f, 0xb8, 0x90, 0x0a, 0x25, 0x2e, 0x18, 0x00, 0x05, 0x2e, 0xc1, 0xf5, 0x2e, 0xbd, 0x2e, 0xb9, 0x01, 0x2e, + 0x19, 0x00, 0x31, 0x30, 0x8a, 0x04, 0x00, 0x90, 0x07, 0x2f, 0x01, 0x2e, 0xd4, 0x00, 0x04, 0xa2, 0x03, 0x2f, 0x01, + 0x2e, 0x18, 0x00, 0x00, 0xb2, 0x0c, 0x2f, 0x19, 0x50, 0x05, 0x52, 0x98, 0x2e, 0x4d, 0xb7, 0x05, 0x2e, 0x78, 0x00, + 0x80, 0x90, 0x10, 0x30, 0x01, 0x2f, 0x21, 0x2e, 0x78, 0x00, 0x25, 0x2e, 0xdd, 0x00, 0x98, 0x2e, 0x3e, 0xb7, 0x00, + 0xb2, 0x02, 0x30, 0x01, 0x30, 0x04, 0x2f, 0x01, 0x2e, 0x19, 0x00, 0x00, 0xb2, 0x00, 0x2f, 0x21, 0x30, 0x01, 0x2e, + 0xea, 0x00, 0x08, 0x1a, 0x0e, 0x2f, 0x23, 0x2e, 0xea, 0x00, 0x33, 0x30, 0x1b, 0x50, 0x0b, 0x09, 0x01, 0x40, 0x17, + 0x56, 0x46, 0xbe, 0x4b, 0x08, 0x4c, 0x0a, 0x01, 0x42, 0x0a, 0x80, 0x15, 0x52, 0x01, 0x42, 0x00, 0x2e, 0x01, 0x2e, + 0x18, 0x00, 0x00, 0xb2, 0x1f, 0x2f, 0x03, 0x2e, 0xc0, 0xf5, 0xf0, 0x30, 0x48, 0x08, 0x47, 0xaa, 0x74, 0x30, 0x07, + 0x2e, 0x7a, 0x00, 0x61, 0x22, 0x4b, 0x1a, 0x05, 0x2f, 0x07, 0x2e, 0x66, 0xf5, 0xbf, 0xbd, 0xbf, 0xb9, 0xc0, 0x90, + 0x0b, 0x2f, 0x1d, 0x56, 0x2b, 0x30, 0xd2, 0x42, 0xdb, 0x42, 0x01, 0x04, 0xc2, 0x42, 0x04, 0xbd, 0xfe, 0x80, 0x81, + 0x84, 0x23, 0x2e, 0x7a, 0x00, 0x02, 0x42, 0x02, 0x32, 0x25, 0x2e, 0x62, 0xf5, 0x05, 0x2e, 0xd6, 0x00, 0x81, 0x84, + 0x25, 0x2e, 0xd6, 0x00, 0x02, 0x31, 0x25, 0x2e, 0x60, 0xf5, 0x05, 0x2e, 0x8a, 0x00, 0x0b, 0x50, 0x90, 0x08, 0x80, + 0xb2, 0x0b, 0x2f, 0x05, 0x2e, 0xca, 0xf5, 0xf0, 0x3e, 0x90, 0x08, 0x25, 0x2e, 0xca, 0xf5, 0x05, 0x2e, 0x59, 0xf5, + 0xe0, 0x3f, 0x90, 0x08, 0x25, 0x2e, 0x59, 0xf5, 0x90, 0x6f, 0xa1, 0x6f, 0xb3, 0x6f, 0xc4, 0x6f, 0xd5, 0x6f, 0xe6, + 0x6f, 0xf7, 0x6f, 0x7b, 0x6f, 0x82, 0x6f, 0x70, 0x5f, 0xc8, 0x2e, 0xc0, 0x50, 0x90, 0x7f, 0xe5, 0x7f, 0xd4, 0x7f, + 0xc3, 0x7f, 0xb1, 0x7f, 0xa2, 0x7f, 0x87, 0x7f, 0xf6, 0x7f, 0x7b, 0x7f, 0x00, 0x2e, 0x01, 0x2e, 0x60, 0xf5, 0x60, + 0x7f, 0x98, 0x2e, 0x35, 0xb7, 0x02, 0x30, 0x63, 0x6f, 0x15, 0x52, 0x50, 0x7f, 0x62, 0x7f, 0x5a, 0x2c, 0x02, 0x32, + 0x1a, 0x09, 0x00, 0xb3, 0x14, 0x2f, 0x00, 0xb2, 0x03, 0x2f, 0x09, 0x2e, 0x18, 0x00, 0x00, 0x91, 0x0c, 0x2f, 0x43, + 0x7f, 0x98, 0x2e, 0x97, 0xb7, 0x1f, 0x50, 0x02, 0x8a, 0x02, 0x32, 0x04, 0x30, 0x25, 0x2e, 0x64, 0xf5, 0x15, 0x52, + 0x50, 0x6f, 0x43, 0x6f, 0x44, 0x43, 0x25, 0x2e, 0x60, 0xf5, 0xd9, 0x08, 0xc0, 0xb2, 0x36, 0x2f, 0x98, 0x2e, 0x3e, + 0xb7, 0x00, 0xb2, 0x06, 0x2f, 0x01, 0x2e, 0x19, 0x00, 0x00, 0xb2, 0x02, 0x2f, 0x50, 0x6f, 0x00, 0x90, 0x0a, 0x2f, + 0x01, 0x2e, 0x79, 0x00, 0x00, 0x90, 0x19, 0x2f, 0x10, 0x30, 0x21, 0x2e, 0x79, 0x00, 0x00, 0x30, 0x98, 0x2e, 0xdc, + 0x03, 0x13, 0x2d, 0x01, 0x2e, 0xc3, 0xf5, 0x0c, 0xbc, 0x0f, 0xb8, 0x12, 0x30, 0x10, 0x04, 0x03, 0xb0, 0x26, 0x25, + 0x21, 0x50, 0x03, 0x52, 0x98, 0x2e, 0x4d, 0xb7, 0x10, 0x30, 0x21, 0x2e, 0xee, 0x00, 0x02, 0x30, 0x60, 0x7f, 0x25, + 0x2e, 0x79, 0x00, 0x60, 0x6f, 0x00, 0x90, 0x05, 0x2f, 0x00, 0x30, 0x21, 0x2e, 0xea, 0x00, 0x15, 0x50, 0x21, 0x2e, + 0x64, 0xf5, 0x15, 0x52, 0x23, 0x2e, 0x60, 0xf5, 0x02, 0x32, 0x50, 0x6f, 0x00, 0x90, 0x02, 0x2f, 0x03, 0x30, 0x27, + 0x2e, 0x78, 0x00, 0x07, 0x2e, 0x60, 0xf5, 0x1a, 0x09, 0x00, 0x91, 0xa3, 0x2f, 0x19, 0x09, 0x00, 0x91, 0xa0, 0x2f, + 0x90, 0x6f, 0xa2, 0x6f, 0xb1, 0x6f, 0xc3, 0x6f, 0xd4, 0x6f, 0xe5, 0x6f, 0x7b, 0x6f, 0xf6, 0x6f, 0x87, 0x6f, 0x40, + 0x5f, 0xc8, 0x2e, 0xc0, 0x50, 0xe7, 0x7f, 0xf6, 0x7f, 0x26, 0x30, 0x0f, 0x2e, 0x61, 0xf5, 0x2f, 0x2e, 0x7c, 0x00, + 0x0f, 0x2e, 0x7c, 0x00, 0xbe, 0x09, 0xa2, 0x7f, 0x80, 0x7f, 0x80, 0xb3, 0xd5, 0x7f, 0xc4, 0x7f, 0xb3, 0x7f, 0x91, + 0x7f, 0x7b, 0x7f, 0x0b, 0x2f, 0x23, 0x50, 0x1a, 0x25, 0x12, 0x40, 0x42, 0x7f, 0x74, 0x82, 0x12, 0x40, 0x52, 0x7f, + 0x00, 0x2e, 0x00, 0x40, 0x60, 0x7f, 0x98, 0x2e, 0x6a, 0xd6, 0x81, 0x30, 0x01, 0x2e, 0x7c, 0x00, 0x01, 0x08, 0x00, + 0xb2, 0x42, 0x2f, 0x03, 0x2e, 0x89, 0x00, 0x01, 0x2e, 0x89, 0x00, 0x97, 0xbc, 0x06, 0xbc, 0x9f, 0xb8, 0x0f, 0xb8, + 0x00, 0x90, 0x23, 0x2e, 0xd8, 0x00, 0x10, 0x30, 0x01, 0x30, 0x2a, 0x2f, 0x03, 0x2e, 0xd4, 0x00, 0x44, 0xb2, 0x05, + 0x2f, 0x47, 0xb2, 0x00, 0x30, 0x2d, 0x2f, 0x21, 0x2e, 0x7c, 0x00, 0x2b, 0x2d, 0x03, 0x2e, 0xfd, 0xf5, 0x9e, 0xbc, + 0x9f, 0xb8, 0x40, 0x90, 0x14, 0x2f, 0x03, 0x2e, 0xfc, 0xf5, 0x99, 0xbc, 0x9f, 0xb8, 0x40, 0x90, 0x0e, 0x2f, 0x03, + 0x2e, 0x49, 0xf1, 0x25, 0x54, 0x4a, 0x08, 0x40, 0x90, 0x08, 0x2f, 0x98, 0x2e, 0x35, 0xb7, 0x00, 0xb2, 0x10, 0x30, + 0x03, 0x2f, 0x50, 0x30, 0x21, 0x2e, 0xd4, 0x00, 0x10, 0x2d, 0x98, 0x2e, 0xaf, 0xb7, 0x00, 0x30, 0x21, 0x2e, 0x7c, + 0x00, 0x0a, 0x2d, 0x05, 0x2e, 0x69, 0xf7, 0x2d, 0xbd, 0x2f, 0xb9, 0x80, 0xb2, 0x01, 0x2f, 0x21, 0x2e, 0x7d, 0x00, + 0x23, 0x2e, 0x7c, 0x00, 0xe0, 0x31, 0x21, 0x2e, 0x61, 0xf5, 0xf6, 0x6f, 0xe7, 0x6f, 0x80, 0x6f, 0xa2, 0x6f, 0xb3, + 0x6f, 0xc4, 0x6f, 0xd5, 0x6f, 0x7b, 0x6f, 0x91, 0x6f, 0x40, 0x5f, 0xc8, 0x2e, 0x60, 0x51, 0x0a, 0x25, 0x36, 0x88, + 0xf4, 0x7f, 0xeb, 0x7f, 0x00, 0x32, 0x31, 0x52, 0x32, 0x30, 0x13, 0x30, 0x98, 0x2e, 0x15, 0xcb, 0x0a, 0x25, 0x33, + 0x84, 0xd2, 0x7f, 0x43, 0x30, 0x05, 0x50, 0x2d, 0x52, 0x98, 0x2e, 0x95, 0xc1, 0xd2, 0x6f, 0x27, 0x52, 0x98, 0x2e, + 0xd7, 0xc7, 0x2a, 0x25, 0xb0, 0x86, 0xc0, 0x7f, 0xd3, 0x7f, 0xaf, 0x84, 0x29, 0x50, 0xf1, 0x6f, 0x98, 0x2e, 0x4d, + 0xc8, 0x2a, 0x25, 0xae, 0x8a, 0xaa, 0x88, 0xf2, 0x6e, 0x2b, 0x50, 0xc1, 0x6f, 0xd3, 0x6f, 0xf4, 0x7f, 0x98, 0x2e, + 0xb6, 0xc8, 0xe0, 0x6e, 0x00, 0xb2, 0x32, 0x2f, 0x33, 0x54, 0x83, 0x86, 0xf1, 0x6f, 0xc3, 0x7f, 0x04, 0x30, 0x30, + 0x30, 0xf4, 0x7f, 0xd0, 0x7f, 0xb2, 0x7f, 0xe3, 0x30, 0xc5, 0x6f, 0x56, 0x40, 0x45, 0x41, 0x28, 0x08, 0x03, 0x14, + 0x0e, 0xb4, 0x08, 0xbc, 0x82, 0x40, 0x10, 0x0a, 0x2f, 0x54, 0x26, 0x05, 0x91, 0x7f, 0x44, 0x28, 0xa3, 0x7f, 0x98, + 0x2e, 0xd9, 0xc0, 0x08, 0xb9, 0x33, 0x30, 0x53, 0x09, 0xc1, 0x6f, 0xd3, 0x6f, 0xf4, 0x6f, 0x83, 0x17, 0x47, 0x40, + 0x6c, 0x15, 0xb2, 0x6f, 0xbe, 0x09, 0x75, 0x0b, 0x90, 0x42, 0x45, 0x42, 0x51, 0x0e, 0x32, 0xbc, 0x02, 0x89, 0xa1, + 0x6f, 0x7e, 0x86, 0xf4, 0x7f, 0xd0, 0x7f, 0xb2, 0x7f, 0x04, 0x30, 0x91, 0x6f, 0xd6, 0x2f, 0xeb, 0x6f, 0xa0, 0x5e, + 0xb8, 0x2e, 0x03, 0x2e, 0x97, 0x00, 0x1b, 0xbc, 0x60, 0x50, 0x9f, 0xbc, 0x0c, 0xb8, 0xf0, 0x7f, 0x40, 0xb2, 0xeb, + 0x7f, 0x2b, 0x2f, 0x03, 0x2e, 0x7f, 0x00, 0x41, 0x40, 0x01, 0x2e, 0xc8, 0x00, 0x01, 0x1a, 0x11, 0x2f, 0x37, 0x58, + 0x23, 0x2e, 0xc8, 0x00, 0x10, 0x41, 0xa0, 0x7f, 0x38, 0x81, 0x01, 0x41, 0xd0, 0x7f, 0xb1, 0x7f, 0x98, 0x2e, 0x64, + 0xcf, 0xd0, 0x6f, 0x07, 0x80, 0xa1, 0x6f, 0x11, 0x42, 0x00, 0x2e, 0xb1, 0x6f, 0x01, 0x42, 0x11, 0x30, 0x01, 0x2e, + 0xfc, 0x00, 0x00, 0xa8, 0x03, 0x30, 0xcb, 0x22, 0x4a, 0x25, 0x01, 0x2e, 0x7f, 0x00, 0x3c, 0x89, 0x35, 0x52, 0x05, + 0x54, 0x98, 0x2e, 0xc4, 0xce, 0xc1, 0x6f, 0xf0, 0x6f, 0x98, 0x2e, 0x95, 0xcf, 0x04, 0x2d, 0x01, 0x30, 0xf0, 0x6f, + 0x98, 0x2e, 0x95, 0xcf, 0xeb, 0x6f, 0xa0, 0x5f, 0xb8, 0x2e, 0x03, 0x2e, 0xb3, 0x00, 0x02, 0x32, 0xf0, 0x30, 0x03, + 0x31, 0x30, 0x50, 0x8a, 0x08, 0x08, 0x08, 0xcb, 0x08, 0xe0, 0x7f, 0x80, 0xb2, 0xf3, 0x7f, 0xdb, 0x7f, 0x25, 0x2f, + 0x03, 0x2e, 0xca, 0x00, 0x41, 0x90, 0x04, 0x2f, 0x01, 0x30, 0x23, 0x2e, 0xca, 0x00, 0x98, 0x2e, 0x3f, 0x03, 0xc0, + 0xb2, 0x05, 0x2f, 0x03, 0x2e, 0xda, 0x00, 0x00, 0x30, 0x41, 0x04, 0x23, 0x2e, 0xda, 0x00, 0x98, 0x2e, 0x92, 0xb2, + 0x10, 0x25, 0xf0, 0x6f, 0x00, 0xb2, 0x05, 0x2f, 0x01, 0x2e, 0xda, 0x00, 0x02, 0x30, 0x10, 0x04, 0x21, 0x2e, 0xda, + 0x00, 0x40, 0xb2, 0x01, 0x2f, 0x23, 0x2e, 0xc8, 0x01, 0xdb, 0x6f, 0xe0, 0x6f, 0xd0, 0x5f, 0x80, 0x2e, 0x95, 0xcf, + 0x01, 0x30, 0xe0, 0x6f, 0x98, 0x2e, 0x95, 0xcf, 0x11, 0x30, 0x23, 0x2e, 0xca, 0x00, 0xdb, 0x6f, 0xd0, 0x5f, 0xb8, + 0x2e, 0xd0, 0x50, 0x0a, 0x25, 0x33, 0x84, 0x55, 0x50, 0xd2, 0x7f, 0xe2, 0x7f, 0x03, 0x8c, 0xc0, 0x7f, 0xbb, 0x7f, + 0x00, 0x30, 0x05, 0x5a, 0x39, 0x54, 0x51, 0x41, 0xa5, 0x7f, 0x96, 0x7f, 0x80, 0x7f, 0x98, 0x2e, 0xd9, 0xc0, 0x05, + 0x30, 0xf5, 0x7f, 0x20, 0x25, 0x91, 0x6f, 0x3b, 0x58, 0x3d, 0x5c, 0x3b, 0x56, 0x98, 0x2e, 0x67, 0xcc, 0xc1, 0x6f, + 0xd5, 0x6f, 0x52, 0x40, 0x50, 0x43, 0xc1, 0x7f, 0xd5, 0x7f, 0x10, 0x25, 0x98, 0x2e, 0xfe, 0xc9, 0x10, 0x25, 0x98, + 0x2e, 0x74, 0xc0, 0x86, 0x6f, 0x30, 0x28, 0x92, 0x6f, 0x82, 0x8c, 0xa5, 0x6f, 0x6f, 0x52, 0x69, 0x0e, 0x39, 0x54, + 0xdb, 0x2f, 0x19, 0xa0, 0x15, 0x30, 0x03, 0x2f, 0x00, 0x30, 0x21, 0x2e, 0x81, 0x01, 0x0a, 0x2d, 0x01, 0x2e, 0x81, + 0x01, 0x05, 0x28, 0x42, 0x36, 0x21, 0x2e, 0x81, 0x01, 0x02, 0x0e, 0x01, 0x2f, 0x98, 0x2e, 0xf3, 0x03, 0x57, 0x50, + 0x12, 0x30, 0x01, 0x40, 0x98, 0x2e, 0xfe, 0xc9, 0x51, 0x6f, 0x0b, 0x5c, 0x8e, 0x0e, 0x3b, 0x6f, 0x57, 0x58, 0x02, + 0x30, 0x21, 0x2e, 0x95, 0x01, 0x45, 0x6f, 0x2a, 0x8d, 0xd2, 0x7f, 0xcb, 0x7f, 0x13, 0x2f, 0x02, 0x30, 0x3f, 0x50, + 0xd2, 0x7f, 0xa8, 0x0e, 0x0e, 0x2f, 0xc0, 0x6f, 0x53, 0x54, 0x02, 0x00, 0x51, 0x54, 0x42, 0x0e, 0x10, 0x30, 0x59, + 0x52, 0x02, 0x30, 0x01, 0x2f, 0x00, 0x2e, 0x03, 0x2d, 0x50, 0x42, 0x42, 0x42, 0x12, 0x30, 0xd2, 0x7f, 0x80, 0xb2, + 0x03, 0x2f, 0x00, 0x30, 0x21, 0x2e, 0x80, 0x01, 0x12, 0x2d, 0x01, 0x2e, 0xc9, 0x00, 0x02, 0x80, 0x05, 0x2e, 0x80, + 0x01, 0x11, 0x30, 0x91, 0x28, 0x00, 0x40, 0x25, 0x2e, 0x80, 0x01, 0x10, 0x0e, 0x05, 0x2f, 0x01, 0x2e, 0x7f, 0x01, + 0x01, 0x90, 0x01, 0x2f, 0x98, 0x2e, 0xf3, 0x03, 0x00, 0x2e, 0xa0, 0x41, 0x01, 0x90, 0xa6, 0x7f, 0x90, 0x2e, 0xe3, + 0xb4, 0x01, 0x2e, 0x95, 0x01, 0x00, 0xa8, 0x90, 0x2e, 0xe3, 0xb4, 0x5b, 0x54, 0x95, 0x80, 0x82, 0x40, 0x80, 0xb2, + 0x02, 0x40, 0x2d, 0x8c, 0x3f, 0x52, 0x96, 0x7f, 0x90, 0x2e, 0xc2, 0xb3, 0x29, 0x0e, 0x76, 0x2f, 0x01, 0x2e, 0xc9, + 0x00, 0x00, 0x40, 0x81, 0x28, 0x45, 0x52, 0xb3, 0x30, 0x98, 0x2e, 0x0f, 0xca, 0x5d, 0x54, 0x80, 0x7f, 0x00, 0x2e, + 0xa1, 0x40, 0x72, 0x7f, 0x82, 0x80, 0x82, 0x40, 0x60, 0x7f, 0x98, 0x2e, 0xfe, 0xc9, 0x10, 0x25, 0x98, 0x2e, 0x74, + 0xc0, 0x62, 0x6f, 0x05, 0x30, 0x87, 0x40, 0xc0, 0x91, 0x04, 0x30, 0x05, 0x2f, 0x05, 0x2e, 0x83, 0x01, 0x80, 0xb2, + 0x14, 0x30, 0x00, 0x2f, 0x04, 0x30, 0x05, 0x2e, 0xc9, 0x00, 0x73, 0x6f, 0x81, 0x40, 0xe2, 0x40, 0x69, 0x04, 0x11, + 0x0f, 0xe1, 0x40, 0x16, 0x30, 0xfe, 0x29, 0xcb, 0x40, 0x02, 0x2f, 0x83, 0x6f, 0x83, 0x0f, 0x22, 0x2f, 0x47, 0x56, + 0x13, 0x0f, 0x12, 0x30, 0x77, 0x2f, 0x49, 0x54, 0x42, 0x0e, 0x12, 0x30, 0x73, 0x2f, 0x00, 0x91, 0x0a, 0x2f, 0x01, + 0x2e, 0x8b, 0x01, 0x19, 0xa8, 0x02, 0x30, 0x6c, 0x2f, 0x63, 0x50, 0x00, 0x2e, 0x17, 0x42, 0x05, 0x42, 0x68, 0x2c, + 0x12, 0x30, 0x0b, 0x25, 0x08, 0x0f, 0x50, 0x30, 0x02, 0x2f, 0x21, 0x2e, 0x83, 0x01, 0x03, 0x2d, 0x40, 0x30, 0x21, + 0x2e, 0x83, 0x01, 0x2b, 0x2e, 0x85, 0x01, 0x5a, 0x2c, 0x12, 0x30, 0x00, 0x91, 0x2b, 0x25, 0x04, 0x2f, 0x63, 0x50, + 0x02, 0x30, 0x17, 0x42, 0x17, 0x2c, 0x02, 0x42, 0x98, 0x2e, 0xfe, 0xc9, 0x10, 0x25, 0x98, 0x2e, 0x74, 0xc0, 0x05, + 0x2e, 0xc9, 0x00, 0x81, 0x84, 0x5b, 0x30, 0x82, 0x40, 0x37, 0x2e, 0x83, 0x01, 0x02, 0x0e, 0x07, 0x2f, 0x5f, 0x52, + 0x40, 0x30, 0x62, 0x40, 0x41, 0x40, 0x91, 0x0e, 0x01, 0x2f, 0x21, 0x2e, 0x83, 0x01, 0x05, 0x30, 0x2b, 0x2e, 0x85, + 0x01, 0x12, 0x30, 0x36, 0x2c, 0x16, 0x30, 0x15, 0x25, 0x81, 0x7f, 0x98, 0x2e, 0xfe, 0xc9, 0x10, 0x25, 0x98, 0x2e, + 0x74, 0xc0, 0x19, 0xa2, 0x16, 0x30, 0x15, 0x2f, 0x05, 0x2e, 0x97, 0x01, 0x80, 0x6f, 0x82, 0x0e, 0x05, 0x2f, 0x01, + 0x2e, 0x86, 0x01, 0x06, 0x28, 0x21, 0x2e, 0x86, 0x01, 0x0b, 0x2d, 0x03, 0x2e, 0x87, 0x01, 0x5f, 0x54, 0x4e, 0x28, + 0x91, 0x42, 0x00, 0x2e, 0x82, 0x40, 0x90, 0x0e, 0x01, 0x2f, 0x21, 0x2e, 0x88, 0x01, 0x02, 0x30, 0x13, 0x2c, 0x05, + 0x30, 0xc0, 0x6f, 0x08, 0x1c, 0xa8, 0x0f, 0x16, 0x30, 0x05, 0x30, 0x5b, 0x50, 0x09, 0x2f, 0x02, 0x80, 0x2d, 0x2e, + 0x82, 0x01, 0x05, 0x42, 0x05, 0x80, 0x00, 0x2e, 0x02, 0x42, 0x3e, 0x80, 0x00, 0x2e, 0x06, 0x42, 0x02, 0x30, 0x90, + 0x6f, 0x3e, 0x88, 0x01, 0x40, 0x04, 0x41, 0x4c, 0x28, 0x01, 0x42, 0x07, 0x80, 0x10, 0x25, 0x24, 0x40, 0x00, 0x40, + 0x00, 0xa8, 0xf5, 0x22, 0x23, 0x29, 0x44, 0x42, 0x7a, 0x82, 0x7e, 0x88, 0x43, 0x40, 0x04, 0x41, 0x00, 0xab, 0xf5, + 0x23, 0xdf, 0x28, 0x43, 0x42, 0xd9, 0xa0, 0x14, 0x2f, 0x00, 0x90, 0x02, 0x2f, 0xd2, 0x6f, 0x81, 0xb2, 0x05, 0x2f, + 0x63, 0x54, 0x06, 0x28, 0x90, 0x42, 0x85, 0x42, 0x09, 0x2c, 0x02, 0x30, 0x5b, 0x50, 0x03, 0x80, 0x29, 0x2e, 0x7e, + 0x01, 0x2b, 0x2e, 0x82, 0x01, 0x05, 0x42, 0x12, 0x30, 0x2b, 0x2e, 0x83, 0x01, 0x45, 0x82, 0x00, 0x2e, 0x40, 0x40, + 0x7a, 0x82, 0x02, 0xa0, 0x08, 0x2f, 0x63, 0x50, 0x3b, 0x30, 0x15, 0x42, 0x05, 0x42, 0x37, 0x80, 0x37, 0x2e, 0x7e, + 0x01, 0x05, 0x42, 0x12, 0x30, 0x01, 0x2e, 0xc9, 0x00, 0x02, 0x8c, 0x40, 0x40, 0x84, 0x41, 0x7a, 0x8c, 0x04, 0x0f, + 0x03, 0x2f, 0x01, 0x2e, 0x8b, 0x01, 0x19, 0xa4, 0x04, 0x2f, 0x2b, 0x2e, 0x82, 0x01, 0x98, 0x2e, 0xf3, 0x03, 0x12, + 0x30, 0x81, 0x90, 0x61, 0x52, 0x08, 0x2f, 0x65, 0x42, 0x65, 0x42, 0x43, 0x80, 0x39, 0x84, 0x82, 0x88, 0x05, 0x42, + 0x45, 0x42, 0x85, 0x42, 0x05, 0x43, 0x00, 0x2e, 0x80, 0x41, 0x00, 0x90, 0x90, 0x2e, 0xe1, 0xb4, 0x65, 0x54, 0xc1, + 0x6f, 0x80, 0x40, 0x00, 0xb2, 0x43, 0x58, 0x69, 0x50, 0x44, 0x2f, 0x55, 0x5c, 0xb7, 0x87, 0x8c, 0x0f, 0x0d, 0x2e, + 0x96, 0x01, 0xc4, 0x40, 0x36, 0x2f, 0x41, 0x56, 0x8b, 0x0e, 0x2a, 0x2f, 0x0b, 0x52, 0xa1, 0x0e, 0x0a, 0x2f, 0x05, + 0x2e, 0x8f, 0x01, 0x14, 0x25, 0x98, 0x2e, 0xfe, 0xc9, 0x4b, 0x54, 0x02, 0x0f, 0x69, 0x50, 0x05, 0x30, 0x65, 0x54, + 0x15, 0x2f, 0x03, 0x2e, 0x8e, 0x01, 0x4d, 0x5c, 0x8e, 0x0f, 0x3a, 0x2f, 0x05, 0x2e, 0x8f, 0x01, 0x98, 0x2e, 0xfe, + 0xc9, 0x4f, 0x54, 0x82, 0x0f, 0x05, 0x30, 0x69, 0x50, 0x65, 0x54, 0x30, 0x2f, 0x6d, 0x52, 0x15, 0x30, 0x42, 0x8c, + 0x45, 0x42, 0x04, 0x30, 0x2b, 0x2c, 0x84, 0x43, 0x6b, 0x52, 0x42, 0x8c, 0x00, 0x2e, 0x85, 0x43, 0x15, 0x30, 0x24, + 0x2c, 0x45, 0x42, 0x8e, 0x0f, 0x20, 0x2f, 0x0d, 0x2e, 0x8e, 0x01, 0xb1, 0x0e, 0x1c, 0x2f, 0x23, 0x2e, 0x8e, 0x01, + 0x1a, 0x2d, 0x0e, 0x0e, 0x17, 0x2f, 0xa1, 0x0f, 0x15, 0x2f, 0x23, 0x2e, 0x8d, 0x01, 0x13, 0x2d, 0x98, 0x2e, 0x74, + 0xc0, 0x43, 0x54, 0xc2, 0x0e, 0x0a, 0x2f, 0x65, 0x50, 0x04, 0x80, 0x0b, 0x30, 0x06, 0x82, 0x0b, 0x42, 0x79, 0x80, + 0x41, 0x40, 0x12, 0x30, 0x25, 0x2e, 0x8c, 0x01, 0x01, 0x42, 0x05, 0x30, 0x69, 0x50, 0x65, 0x54, 0x84, 0x82, 0x43, + 0x84, 0xbe, 0x8c, 0x84, 0x40, 0x86, 0x41, 0x26, 0x29, 0x94, 0x42, 0xbe, 0x8e, 0xd5, 0x7f, 0x19, 0xa1, 0x43, 0x40, + 0x0b, 0x2e, 0x8c, 0x01, 0x84, 0x40, 0xc7, 0x41, 0x5d, 0x29, 0x27, 0x29, 0x45, 0x42, 0x84, 0x42, 0xc2, 0x7f, 0x01, + 0x2f, 0xc0, 0xb3, 0x1d, 0x2f, 0x05, 0x2e, 0x94, 0x01, 0x99, 0xa0, 0x01, 0x2f, 0x80, 0xb3, 0x13, 0x2f, 0x80, 0xb3, + 0x18, 0x2f, 0xc0, 0xb3, 0x16, 0x2f, 0x12, 0x40, 0x01, 0x40, 0x92, 0x7f, 0x98, 0x2e, 0x74, 0xc0, 0x92, 0x6f, 0x10, + 0x0f, 0x20, 0x30, 0x03, 0x2f, 0x10, 0x30, 0x21, 0x2e, 0x7e, 0x01, 0x0a, 0x2d, 0x21, 0x2e, 0x7e, 0x01, 0x07, 0x2d, + 0x20, 0x30, 0x21, 0x2e, 0x7e, 0x01, 0x03, 0x2d, 0x10, 0x30, 0x21, 0x2e, 0x7e, 0x01, 0xc2, 0x6f, 0x01, 0x2e, 0xc9, + 0x00, 0xbc, 0x84, 0x02, 0x80, 0x82, 0x40, 0x00, 0x40, 0x90, 0x0e, 0xd5, 0x6f, 0x02, 0x2f, 0x15, 0x30, 0x98, 0x2e, + 0xf3, 0x03, 0x41, 0x91, 0x05, 0x30, 0x07, 0x2f, 0x67, 0x50, 0x3d, 0x80, 0x2b, 0x2e, 0x8f, 0x01, 0x05, 0x42, 0x04, + 0x80, 0x00, 0x2e, 0x05, 0x42, 0x02, 0x2c, 0x00, 0x30, 0x00, 0x30, 0xa2, 0x6f, 0x98, 0x8a, 0x86, 0x40, 0x80, 0xa7, + 0x05, 0x2f, 0x98, 0x2e, 0xf3, 0x03, 0xc0, 0x30, 0x21, 0x2e, 0x95, 0x01, 0x06, 0x25, 0x1a, 0x25, 0xe2, 0x6f, 0x76, + 0x82, 0x96, 0x40, 0x56, 0x43, 0x51, 0x0e, 0xfb, 0x2f, 0xbb, 0x6f, 0x30, 0x5f, 0xb8, 0x2e, 0x01, 0x2e, 0xb8, 0x00, + 0x01, 0x31, 0x41, 0x08, 0x40, 0xb2, 0x20, 0x50, 0xf2, 0x30, 0x02, 0x08, 0xfb, 0x7f, 0x01, 0x30, 0x10, 0x2f, 0x05, + 0x2e, 0xcc, 0x00, 0x81, 0x90, 0xe0, 0x7f, 0x03, 0x2f, 0x23, 0x2e, 0xcc, 0x00, 0x98, 0x2e, 0x55, 0xb6, 0x98, 0x2e, + 0x1d, 0xb5, 0x10, 0x25, 0xfb, 0x6f, 0xe0, 0x6f, 0xe0, 0x5f, 0x80, 0x2e, 0x95, 0xcf, 0x98, 0x2e, 0x95, 0xcf, 0x10, + 0x30, 0x21, 0x2e, 0xcc, 0x00, 0xfb, 0x6f, 0xe0, 0x5f, 0xb8, 0x2e, 0x00, 0x51, 0x05, 0x58, 0xeb, 0x7f, 0x2a, 0x25, + 0x89, 0x52, 0x6f, 0x5a, 0x89, 0x50, 0x13, 0x41, 0x06, 0x40, 0xb3, 0x01, 0x16, 0x42, 0xcb, 0x16, 0x06, 0x40, 0xf3, + 0x02, 0x13, 0x42, 0x65, 0x0e, 0xf5, 0x2f, 0x05, 0x40, 0x14, 0x30, 0x2c, 0x29, 0x04, 0x42, 0x08, 0xa1, 0x00, 0x30, + 0x90, 0x2e, 0x52, 0xb6, 0xb3, 0x88, 0xb0, 0x8a, 0xb6, 0x84, 0xa4, 0x7f, 0xc4, 0x7f, 0xb5, 0x7f, 0xd5, 0x7f, 0x92, + 0x7f, 0x73, 0x30, 0x04, 0x30, 0x55, 0x40, 0x42, 0x40, 0x8a, 0x17, 0xf3, 0x08, 0x6b, 0x01, 0x90, 0x02, 0x53, 0xb8, + 0x4b, 0x82, 0xad, 0xbe, 0x71, 0x7f, 0x45, 0x0a, 0x09, 0x54, 0x84, 0x7f, 0x98, 0x2e, 0xd9, 0xc0, 0xa3, 0x6f, 0x7b, + 0x54, 0xd0, 0x42, 0xa3, 0x7f, 0xf2, 0x7f, 0x60, 0x7f, 0x20, 0x25, 0x71, 0x6f, 0x75, 0x5a, 0x77, 0x58, 0x79, 0x5c, + 0x75, 0x56, 0x98, 0x2e, 0x67, 0xcc, 0xb1, 0x6f, 0x62, 0x6f, 0x50, 0x42, 0xb1, 0x7f, 0xb3, 0x30, 0x10, 0x25, 0x98, + 0x2e, 0x0f, 0xca, 0x84, 0x6f, 0x20, 0x29, 0x71, 0x6f, 0x92, 0x6f, 0xa5, 0x6f, 0x76, 0x82, 0x6a, 0x0e, 0x73, 0x30, + 0x00, 0x30, 0xd0, 0x2f, 0xd2, 0x6f, 0xd1, 0x7f, 0xb4, 0x7f, 0x98, 0x2e, 0x2b, 0xb7, 0x15, 0xbd, 0x0b, 0xb8, 0x02, + 0x0a, 0xc2, 0x6f, 0xc0, 0x7f, 0x98, 0x2e, 0x2b, 0xb7, 0x15, 0xbd, 0x0b, 0xb8, 0x42, 0x0a, 0xc0, 0x6f, 0x08, 0x17, + 0x41, 0x18, 0x89, 0x16, 0xe1, 0x18, 0xd0, 0x18, 0xa1, 0x7f, 0x27, 0x25, 0x16, 0x25, 0x98, 0x2e, 0x79, 0xc0, 0x8b, + 0x54, 0x90, 0x7f, 0xb3, 0x30, 0x82, 0x40, 0x80, 0x90, 0x0d, 0x2f, 0x7d, 0x52, 0x92, 0x6f, 0x98, 0x2e, 0x0f, 0xca, + 0xb2, 0x6f, 0x90, 0x0e, 0x06, 0x2f, 0x8b, 0x50, 0x14, 0x30, 0x42, 0x6f, 0x51, 0x6f, 0x14, 0x42, 0x12, 0x42, 0x01, + 0x42, 0x00, 0x2e, 0x31, 0x6f, 0x98, 0x2e, 0x74, 0xc0, 0x41, 0x6f, 0x80, 0x7f, 0x98, 0x2e, 0x74, 0xc0, 0x82, 0x6f, + 0x10, 0x04, 0x43, 0x52, 0x01, 0x0f, 0x05, 0x2e, 0xcb, 0x00, 0x00, 0x30, 0x04, 0x30, 0x21, 0x2f, 0x51, 0x6f, 0x43, + 0x58, 0x8c, 0x0e, 0x04, 0x30, 0x1c, 0x2f, 0x85, 0x88, 0x41, 0x6f, 0x04, 0x41, 0x8c, 0x0f, 0x04, 0x30, 0x16, 0x2f, + 0x84, 0x88, 0x00, 0x2e, 0x04, 0x41, 0x04, 0x05, 0x8c, 0x0e, 0x04, 0x30, 0x0f, 0x2f, 0x82, 0x88, 0x31, 0x6f, 0x04, + 0x41, 0x04, 0x05, 0x8c, 0x0e, 0x04, 0x30, 0x08, 0x2f, 0x83, 0x88, 0x00, 0x2e, 0x04, 0x41, 0x8c, 0x0f, 0x04, 0x30, + 0x02, 0x2f, 0x21, 0x2e, 0xad, 0x01, 0x14, 0x30, 0x00, 0x91, 0x14, 0x2f, 0x03, 0x2e, 0xa1, 0x01, 0x41, 0x90, 0x0e, + 0x2f, 0x03, 0x2e, 0xad, 0x01, 0x14, 0x30, 0x4c, 0x28, 0x23, 0x2e, 0xad, 0x01, 0x46, 0xa0, 0x06, 0x2f, 0x81, 0x84, + 0x8d, 0x52, 0x48, 0x82, 0x82, 0x40, 0x21, 0x2e, 0xa1, 0x01, 0x42, 0x42, 0x5c, 0x2c, 0x02, 0x30, 0x05, 0x2e, 0xaa, + 0x01, 0x80, 0xb2, 0x02, 0x30, 0x55, 0x2f, 0x03, 0x2e, 0xa9, 0x01, 0x92, 0x6f, 0xb3, 0x30, 0x98, 0x2e, 0x0f, 0xca, + 0xb2, 0x6f, 0x90, 0x0f, 0x00, 0x30, 0x02, 0x30, 0x4a, 0x2f, 0xa2, 0x6f, 0x87, 0x52, 0x91, 0x00, 0x85, 0x52, 0x51, + 0x0e, 0x02, 0x2f, 0x00, 0x2e, 0x43, 0x2c, 0x02, 0x30, 0xc2, 0x6f, 0x7f, 0x52, 0x91, 0x0e, 0x02, 0x30, 0x3c, 0x2f, + 0x51, 0x6f, 0x81, 0x54, 0x98, 0x2e, 0xfe, 0xc9, 0x10, 0x25, 0xb3, 0x30, 0x21, 0x25, 0x98, 0x2e, 0x0f, 0xca, 0x32, + 0x6f, 0xc0, 0x7f, 0xb3, 0x30, 0x12, 0x25, 0x98, 0x2e, 0x0f, 0xca, 0x42, 0x6f, 0xb0, 0x7f, 0xb3, 0x30, 0x12, 0x25, + 0x98, 0x2e, 0x0f, 0xca, 0xb2, 0x6f, 0x90, 0x28, 0x83, 0x52, 0x98, 0x2e, 0xfe, 0xc9, 0xc2, 0x6f, 0x90, 0x0f, 0x00, + 0x30, 0x02, 0x30, 0x1d, 0x2f, 0x05, 0x2e, 0xa1, 0x01, 0x80, 0xb2, 0x12, 0x30, 0x0f, 0x2f, 0x42, 0x6f, 0x03, 0x2e, + 0xab, 0x01, 0x91, 0x0e, 0x02, 0x30, 0x12, 0x2f, 0x52, 0x6f, 0x03, 0x2e, 0xac, 0x01, 0x91, 0x0f, 0x02, 0x30, 0x0c, + 0x2f, 0x21, 0x2e, 0xaa, 0x01, 0x0a, 0x2c, 0x12, 0x30, 0x03, 0x2e, 0xcb, 0x00, 0x8d, 0x58, 0x08, 0x89, 0x41, 0x40, + 0x11, 0x43, 0x00, 0x43, 0x25, 0x2e, 0xa1, 0x01, 0xd4, 0x6f, 0x8f, 0x52, 0x00, 0x43, 0x3a, 0x89, 0x00, 0x2e, 0x10, + 0x43, 0x10, 0x43, 0x61, 0x0e, 0xfb, 0x2f, 0x03, 0x2e, 0xa0, 0x01, 0x11, 0x1a, 0x02, 0x2f, 0x02, 0x25, 0x21, 0x2e, + 0xa0, 0x01, 0xeb, 0x6f, 0x00, 0x5f, 0xb8, 0x2e, 0x91, 0x52, 0x10, 0x30, 0x02, 0x30, 0x95, 0x56, 0x52, 0x42, 0x4b, + 0x0e, 0xfc, 0x2f, 0x8d, 0x54, 0x88, 0x82, 0x93, 0x56, 0x80, 0x42, 0x53, 0x42, 0x40, 0x42, 0x42, 0x86, 0x83, 0x54, + 0xc0, 0x2e, 0xc2, 0x42, 0x00, 0x2e, 0xa3, 0x52, 0x00, 0x51, 0x52, 0x40, 0x47, 0x40, 0x1a, 0x25, 0x01, 0x2e, 0x97, + 0x00, 0x8f, 0xbe, 0x72, 0x86, 0xfb, 0x7f, 0x0b, 0x30, 0x7c, 0xbf, 0xa5, 0x50, 0x10, 0x08, 0xdf, 0xba, 0x70, 0x88, + 0xf8, 0xbf, 0xcb, 0x42, 0xd3, 0x7f, 0x6c, 0xbb, 0xfc, 0xbb, 0xc5, 0x0a, 0x90, 0x7f, 0x1b, 0x7f, 0x0b, 0x43, 0xc0, + 0xb2, 0xe5, 0x7f, 0xb7, 0x7f, 0xa6, 0x7f, 0xc4, 0x7f, 0x90, 0x2e, 0x1c, 0xb7, 0x07, 0x2e, 0xd2, 0x00, 0xc0, 0xb2, + 0x0b, 0x2f, 0x97, 0x52, 0x01, 0x2e, 0xcd, 0x00, 0x82, 0x7f, 0x98, 0x2e, 0xbb, 0xcc, 0x0b, 0x30, 0x37, 0x2e, 0xd2, + 0x00, 0x82, 0x6f, 0x90, 0x6f, 0x1a, 0x25, 0x00, 0xb2, 0x8b, 0x7f, 0x14, 0x2f, 0xa6, 0xbd, 0x25, 0xbd, 0xb6, 0xb9, + 0x2f, 0xb9, 0x80, 0xb2, 0xd4, 0xb0, 0x0c, 0x2f, 0x99, 0x54, 0x9b, 0x56, 0x0b, 0x30, 0x0b, 0x2e, 0xb1, 0x00, 0xa1, + 0x58, 0x9b, 0x42, 0xdb, 0x42, 0x6c, 0x09, 0x2b, 0x2e, 0xb1, 0x00, 0x8b, 0x42, 0xcb, 0x42, 0x86, 0x7f, 0x73, 0x84, + 0xa7, 0x56, 0xc3, 0x08, 0x39, 0x52, 0x05, 0x50, 0x72, 0x7f, 0x63, 0x7f, 0x98, 0x2e, 0xc2, 0xc0, 0xe1, 0x6f, 0x62, + 0x6f, 0xd1, 0x0a, 0x01, 0x2e, 0xcd, 0x00, 0xd5, 0x6f, 0xc4, 0x6f, 0x72, 0x6f, 0x97, 0x52, 0x9d, 0x5c, 0x98, 0x2e, + 0x06, 0xcd, 0x23, 0x6f, 0x90, 0x6f, 0x99, 0x52, 0xc0, 0xb2, 0x04, 0xbd, 0x54, 0x40, 0xaf, 0xb9, 0x45, 0x40, 0xe1, + 0x7f, 0x02, 0x30, 0x06, 0x2f, 0xc0, 0xb2, 0x02, 0x30, 0x03, 0x2f, 0x9b, 0x5c, 0x12, 0x30, 0x94, 0x43, 0x85, 0x43, + 0x03, 0xbf, 0x6f, 0xbb, 0x80, 0xb3, 0x20, 0x2f, 0x06, 0x6f, 0x26, 0x01, 0x16, 0x6f, 0x6e, 0x03, 0x45, 0x42, 0xc0, + 0x90, 0x29, 0x2e, 0xce, 0x00, 0x9b, 0x52, 0x14, 0x2f, 0x9b, 0x5c, 0x00, 0x2e, 0x93, 0x41, 0x86, 0x41, 0xe3, 0x04, + 0xae, 0x07, 0x80, 0xab, 0x04, 0x2f, 0x80, 0x91, 0x0a, 0x2f, 0x86, 0x6f, 0x73, 0x0f, 0x07, 0x2f, 0x83, 0x6f, 0xc0, + 0xb2, 0x04, 0x2f, 0x54, 0x42, 0x45, 0x42, 0x12, 0x30, 0x04, 0x2c, 0x11, 0x30, 0x02, 0x2c, 0x11, 0x30, 0x11, 0x30, + 0x02, 0xbc, 0x0f, 0xb8, 0xd2, 0x7f, 0x00, 0xb2, 0x0a, 0x2f, 0x01, 0x2e, 0xfc, 0x00, 0x05, 0x2e, 0xc7, 0x01, 0x10, + 0x1a, 0x02, 0x2f, 0x21, 0x2e, 0xc7, 0x01, 0x03, 0x2d, 0x02, 0x2c, 0x01, 0x30, 0x01, 0x30, 0xb0, 0x6f, 0x98, 0x2e, + 0x95, 0xcf, 0xd1, 0x6f, 0xa0, 0x6f, 0x98, 0x2e, 0x95, 0xcf, 0xe2, 0x6f, 0x9f, 0x52, 0x01, 0x2e, 0xce, 0x00, 0x82, + 0x40, 0x50, 0x42, 0x0c, 0x2c, 0x42, 0x42, 0x11, 0x30, 0x23, 0x2e, 0xd2, 0x00, 0x01, 0x30, 0xb0, 0x6f, 0x98, 0x2e, + 0x95, 0xcf, 0xa0, 0x6f, 0x01, 0x30, 0x98, 0x2e, 0x95, 0xcf, 0x00, 0x2e, 0xfb, 0x6f, 0x00, 0x5f, 0xb8, 0x2e, 0x83, + 0x86, 0x01, 0x30, 0x00, 0x30, 0x94, 0x40, 0x24, 0x18, 0x06, 0x00, 0x53, 0x0e, 0x4f, 0x02, 0xf9, 0x2f, 0xb8, 0x2e, + 0xa9, 0x52, 0x00, 0x2e, 0x60, 0x40, 0x41, 0x40, 0x0d, 0xbc, 0x98, 0xbc, 0xc0, 0x2e, 0x01, 0x0a, 0x0f, 0xb8, 0xab, + 0x52, 0x53, 0x3c, 0x52, 0x40, 0x40, 0x40, 0x4b, 0x00, 0x82, 0x16, 0x26, 0xb9, 0x01, 0xb8, 0x41, 0x40, 0x10, 0x08, + 0x97, 0xb8, 0x01, 0x08, 0xc0, 0x2e, 0x11, 0x30, 0x01, 0x08, 0x43, 0x86, 0x25, 0x40, 0x04, 0x40, 0xd8, 0xbe, 0x2c, + 0x0b, 0x22, 0x11, 0x54, 0x42, 0x03, 0x80, 0x4b, 0x0e, 0xf6, 0x2f, 0xb8, 0x2e, 0x9f, 0x50, 0x10, 0x50, 0xad, 0x52, + 0x05, 0x2e, 0xd3, 0x00, 0xfb, 0x7f, 0x00, 0x2e, 0x13, 0x40, 0x93, 0x42, 0x41, 0x0e, 0xfb, 0x2f, 0x98, 0x2e, 0xa5, + 0xb7, 0x98, 0x2e, 0x87, 0xcf, 0x01, 0x2e, 0xd9, 0x00, 0x00, 0xb2, 0xfb, 0x6f, 0x0b, 0x2f, 0x01, 0x2e, 0x69, 0xf7, + 0xb1, 0x3f, 0x01, 0x08, 0x01, 0x30, 0xf0, 0x5f, 0x23, 0x2e, 0xd9, 0x00, 0x21, 0x2e, 0x69, 0xf7, 0x80, 0x2e, 0x7a, + 0xb7, 0xf0, 0x5f, 0xb8, 0x2e, 0x01, 0x2e, 0xc0, 0xf8, 0x03, 0x2e, 0xfc, 0xf5, 0x15, 0x54, 0xaf, 0x56, 0x82, 0x08, + 0x0b, 0x2e, 0x69, 0xf7, 0xcb, 0x0a, 0xb1, 0x58, 0x80, 0x90, 0xdd, 0xbe, 0x4c, 0x08, 0x5f, 0xb9, 0x59, 0x22, 0x80, + 0x90, 0x07, 0x2f, 0x03, 0x34, 0xc3, 0x08, 0xf2, 0x3a, 0x0a, 0x08, 0x02, 0x35, 0xc0, 0x90, 0x4a, 0x0a, 0x48, 0x22, + 0xc0, 0x2e, 0x23, 0x2e, 0xfc, 0xf5, 0x10, 0x50, 0xfb, 0x7f, 0x98, 0x2e, 0x56, 0xc7, 0x98, 0x2e, 0x49, 0xc3, 0x10, + 0x30, 0xfb, 0x6f, 0xf0, 0x5f, 0x21, 0x2e, 0xcc, 0x00, 0x21, 0x2e, 0xca, 0x00, 0xb8, 0x2e, 0x03, 0x2e, 0xd3, 0x00, + 0x16, 0xb8, 0x02, 0x34, 0x4a, 0x0c, 0x21, 0x2e, 0x2d, 0xf5, 0xc0, 0x2e, 0x23, 0x2e, 0xd3, 0x00, 0x03, 0xbc, 0x21, + 0x2e, 0xd5, 0x00, 0x03, 0x2e, 0xd5, 0x00, 0x40, 0xb2, 0x10, 0x30, 0x21, 0x2e, 0x77, 0x00, 0x01, 0x30, 0x05, 0x2f, + 0x05, 0x2e, 0xd8, 0x00, 0x80, 0x90, 0x01, 0x2f, 0x23, 0x2e, 0x6f, 0xf5, 0xc0, 0x2e, 0x21, 0x2e, 0xd9, 0x00, 0x11, + 0x30, 0x81, 0x08, 0x01, 0x2e, 0x6a, 0xf7, 0x71, 0x3f, 0x23, 0xbd, 0x01, 0x08, 0x02, 0x0a, 0xc0, 0x2e, 0x21, 0x2e, + 0x6a, 0xf7, 0x30, 0x25, 0x00, 0x30, 0x21, 0x2e, 0x5a, 0xf5, 0x10, 0x50, 0x21, 0x2e, 0x7b, 0x00, 0x21, 0x2e, 0x7c, + 0x00, 0xfb, 0x7f, 0x98, 0x2e, 0xc3, 0xb7, 0x40, 0x30, 0x21, 0x2e, 0xd4, 0x00, 0xfb, 0x6f, 0xf0, 0x5f, 0x03, 0x25, + 0x80, 0x2e, 0xaf, 0xb7, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x01, 0x2e, 0x5d, 0xf7, 0x08, 0xbc, 0x80, 0xac, 0x0e, 0xbb, 0x02, 0x2f, + 0x00, 0x30, 0x41, 0x04, 0x82, 0x06, 0xc0, 0xa4, 0x00, 0x30, 0x11, 0x2f, 0x40, 0xa9, 0x03, 0x2f, 0x40, 0x91, 0x0d, + 0x2f, 0x00, 0xa7, 0x0b, 0x2f, 0x80, 0xb3, 0xb3, 0x58, 0x02, 0x2f, 0x90, 0xa1, 0x26, 0x13, 0x20, 0x23, 0x80, 0x90, + 0x10, 0x30, 0x01, 0x2f, 0xcc, 0x0e, 0x00, 0x2f, 0x00, 0x30, 0xb8, 0x2e, 0xb5, 0x50, 0x18, 0x08, 0x08, 0xbc, 0x88, + 0xb6, 0x0d, 0x17, 0xc6, 0xbd, 0x56, 0xbc, 0xb7, 0x58, 0xda, 0xba, 0x04, 0x01, 0x1d, 0x0a, 0x10, 0x50, 0x05, 0x30, + 0x32, 0x25, 0x45, 0x03, 0xfb, 0x7f, 0xf6, 0x30, 0x21, 0x25, 0x98, 0x2e, 0x37, 0xca, 0x16, 0xb5, 0x9a, 0xbc, 0x06, + 0xb8, 0x80, 0xa8, 0x41, 0x0a, 0x0e, 0x2f, 0x80, 0x90, 0x02, 0x2f, 0x2d, 0x50, 0x48, 0x0f, 0x09, 0x2f, 0xbf, 0xa0, + 0x04, 0x2f, 0xbf, 0x90, 0x06, 0x2f, 0xb7, 0x54, 0xca, 0x0f, 0x03, 0x2f, 0x00, 0x2e, 0x02, 0x2c, 0xb7, 0x52, 0x2d, + 0x52, 0xf2, 0x33, 0x98, 0x2e, 0xd9, 0xc0, 0xfb, 0x6f, 0xf1, 0x37, 0xc0, 0x2e, 0x01, 0x08, 0xf0, 0x5f, 0xbf, 0x56, + 0xb9, 0x54, 0xd0, 0x40, 0xc4, 0x40, 0x0b, 0x2e, 0xfd, 0xf3, 0xbf, 0x52, 0x90, 0x42, 0x94, 0x42, 0x95, 0x42, 0x05, + 0x30, 0xc1, 0x50, 0x0f, 0x88, 0x06, 0x40, 0x04, 0x41, 0x96, 0x42, 0xc5, 0x42, 0x48, 0xbe, 0x73, 0x30, 0x0d, 0x2e, + 0xd8, 0x00, 0x4f, 0xba, 0x84, 0x42, 0x03, 0x42, 0x81, 0xb3, 0x02, 0x2f, 0x2b, 0x2e, 0x6f, 0xf5, 0x06, 0x2d, 0x05, + 0x2e, 0x77, 0xf7, 0xbd, 0x56, 0x93, 0x08, 0x25, 0x2e, 0x77, 0xf7, 0xbb, 0x54, 0x25, 0x2e, 0xc2, 0xf5, 0x07, 0x2e, + 0xfd, 0xf3, 0x42, 0x30, 0xb4, 0x33, 0xda, 0x0a, 0x4c, 0x00, 0x27, 0x2e, 0xfd, 0xf3, 0x43, 0x40, 0xd4, 0x3f, 0xdc, + 0x08, 0x43, 0x42, 0x00, 0x2e, 0x00, 0x2e, 0x43, 0x40, 0x24, 0x30, 0xdc, 0x0a, 0x43, 0x42, 0x04, 0x80, 0x03, 0x2e, + 0xfd, 0xf3, 0x4a, 0x0a, 0x23, 0x2e, 0xfd, 0xf3, 0x61, 0x34, 0xc0, 0x2e, 0x01, 0x42, 0x00, 0x2e, 0x60, 0x50, 0x1a, + 0x25, 0x7a, 0x86, 0xe0, 0x7f, 0xf3, 0x7f, 0x03, 0x25, 0xc3, 0x52, 0x41, 0x84, 0xdb, 0x7f, 0x33, 0x30, 0x98, 0x2e, + 0x16, 0xc2, 0x1a, 0x25, 0x7d, 0x82, 0xf0, 0x6f, 0xe2, 0x6f, 0x32, 0x25, 0x16, 0x40, 0x94, 0x40, 0x26, 0x01, 0x85, + 0x40, 0x8e, 0x17, 0xc4, 0x42, 0x6e, 0x03, 0x95, 0x42, 0x41, 0x0e, 0xf4, 0x2f, 0xdb, 0x6f, 0xa0, 0x5f, 0xb8, 0x2e, + 0xb0, 0x51, 0xfb, 0x7f, 0x98, 0x2e, 0xe8, 0x0d, 0x5a, 0x25, 0x98, 0x2e, 0x0f, 0x0e, 0xcb, 0x58, 0x32, 0x87, 0xc4, + 0x7f, 0x65, 0x89, 0x6b, 0x8d, 0xc5, 0x5a, 0x65, 0x7f, 0xe1, 0x7f, 0x83, 0x7f, 0xa6, 0x7f, 0x74, 0x7f, 0xd0, 0x7f, + 0xb6, 0x7f, 0x94, 0x7f, 0x17, 0x30, 0xc7, 0x52, 0xc9, 0x54, 0x51, 0x7f, 0x00, 0x2e, 0x85, 0x6f, 0x42, 0x7f, 0x00, + 0x2e, 0x51, 0x41, 0x45, 0x81, 0x42, 0x41, 0x13, 0x40, 0x3b, 0x8a, 0x00, 0x40, 0x4b, 0x04, 0xd0, 0x06, 0xc0, 0xac, + 0x85, 0x7f, 0x02, 0x2f, 0x02, 0x30, 0x51, 0x04, 0xd3, 0x06, 0x41, 0x84, 0x05, 0x30, 0x5d, 0x02, 0xc9, 0x16, 0xdf, + 0x08, 0xd3, 0x00, 0x8d, 0x02, 0xaf, 0xbc, 0xb1, 0xb9, 0x59, 0x0a, 0x65, 0x6f, 0x11, 0x43, 0xa1, 0xb4, 0x52, 0x41, + 0x53, 0x41, 0x01, 0x43, 0x34, 0x7f, 0x65, 0x7f, 0x26, 0x31, 0xe5, 0x6f, 0xd4, 0x6f, 0x98, 0x2e, 0x37, 0xca, 0x32, + 0x6f, 0x75, 0x6f, 0x83, 0x40, 0x42, 0x41, 0x23, 0x7f, 0x12, 0x7f, 0xf6, 0x30, 0x40, 0x25, 0x51, 0x25, 0x98, 0x2e, + 0x37, 0xca, 0x14, 0x6f, 0x20, 0x05, 0x70, 0x6f, 0x25, 0x6f, 0x69, 0x07, 0xa2, 0x6f, 0x31, 0x6f, 0x0b, 0x30, 0x04, + 0x42, 0x9b, 0x42, 0x8b, 0x42, 0x55, 0x42, 0x32, 0x7f, 0x40, 0xa9, 0xc3, 0x6f, 0x71, 0x7f, 0x02, 0x30, 0xd0, 0x40, + 0xc3, 0x7f, 0x03, 0x2f, 0x40, 0x91, 0x15, 0x2f, 0x00, 0xa7, 0x13, 0x2f, 0x00, 0xa4, 0x11, 0x2f, 0x84, 0xbd, 0x98, + 0x2e, 0x79, 0xca, 0x55, 0x6f, 0xb7, 0x54, 0x54, 0x41, 0x82, 0x00, 0xf3, 0x3f, 0x45, 0x41, 0xcb, 0x02, 0xf6, 0x30, + 0x98, 0x2e, 0x37, 0xca, 0x35, 0x6f, 0xa4, 0x6f, 0x41, 0x43, 0x03, 0x2c, 0x00, 0x43, 0xa4, 0x6f, 0x35, 0x6f, 0x17, + 0x30, 0x42, 0x6f, 0x51, 0x6f, 0x93, 0x40, 0x42, 0x82, 0x00, 0x41, 0xc3, 0x00, 0x03, 0x43, 0x51, 0x7f, 0x00, 0x2e, + 0x94, 0x40, 0x41, 0x41, 0x4c, 0x02, 0xc4, 0x6f, 0xd1, 0x56, 0x63, 0x0e, 0x74, 0x6f, 0x51, 0x43, 0xa5, 0x7f, 0x8a, + 0x2f, 0x09, 0x2e, 0xd8, 0x00, 0x01, 0xb3, 0x21, 0x2f, 0xcb, 0x58, 0x90, 0x6f, 0x13, 0x41, 0xb6, 0x6f, 0xe4, 0x7f, + 0x00, 0x2e, 0x91, 0x41, 0x14, 0x40, 0x92, 0x41, 0x15, 0x40, 0x17, 0x2e, 0x6f, 0xf5, 0xb6, 0x7f, 0xd0, 0x7f, 0xcb, + 0x7f, 0x98, 0x2e, 0x00, 0x0c, 0x07, 0x15, 0xc2, 0x6f, 0x14, 0x0b, 0x29, 0x2e, 0x6f, 0xf5, 0xc3, 0xa3, 0xc1, 0x8f, + 0xe4, 0x6f, 0xd0, 0x6f, 0xe6, 0x2f, 0x14, 0x30, 0x05, 0x2e, 0x6f, 0xf5, 0x14, 0x0b, 0x29, 0x2e, 0x6f, 0xf5, 0x18, + 0x2d, 0xcd, 0x56, 0x04, 0x32, 0xb5, 0x6f, 0x1c, 0x01, 0x51, 0x41, 0x52, 0x41, 0xc3, 0x40, 0xb5, 0x7f, 0xe4, 0x7f, + 0x98, 0x2e, 0x1f, 0x0c, 0xe4, 0x6f, 0x21, 0x87, 0x00, 0x43, 0x04, 0x32, 0xcf, 0x54, 0x5a, 0x0e, 0xef, 0x2f, 0x15, + 0x54, 0x09, 0x2e, 0x77, 0xf7, 0x22, 0x0b, 0x29, 0x2e, 0x77, 0xf7, 0xfb, 0x6f, 0x50, 0x5e, 0xb8, 0x2e, 0x10, 0x50, + 0x01, 0x2e, 0xd4, 0x00, 0x00, 0xb2, 0xfb, 0x7f, 0x51, 0x2f, 0x01, 0xb2, 0x48, 0x2f, 0x02, 0xb2, 0x42, 0x2f, 0x03, + 0x90, 0x56, 0x2f, 0xd7, 0x52, 0x79, 0x80, 0x42, 0x40, 0x81, 0x84, 0x00, 0x40, 0x42, 0x42, 0x98, 0x2e, 0x93, 0x0c, + 0xd9, 0x54, 0xd7, 0x50, 0xa1, 0x40, 0x98, 0xbd, 0x82, 0x40, 0x3e, 0x82, 0xda, 0x0a, 0x44, 0x40, 0x8b, 0x16, 0xe3, + 0x00, 0x53, 0x42, 0x00, 0x2e, 0x43, 0x40, 0x9a, 0x02, 0x52, 0x42, 0x00, 0x2e, 0x41, 0x40, 0x15, 0x54, 0x4a, 0x0e, + 0x3a, 0x2f, 0x3a, 0x82, 0x00, 0x30, 0x41, 0x40, 0x21, 0x2e, 0x85, 0x0f, 0x40, 0xb2, 0x0a, 0x2f, 0x98, 0x2e, 0xb1, + 0x0c, 0x98, 0x2e, 0x45, 0x0e, 0x98, 0x2e, 0x5b, 0x0e, 0xfb, 0x6f, 0xf0, 0x5f, 0x00, 0x30, 0x80, 0x2e, 0xce, 0xb7, + 0xdd, 0x52, 0xd3, 0x54, 0x42, 0x42, 0x4f, 0x84, 0x73, 0x30, 0xdb, 0x52, 0x83, 0x42, 0x1b, 0x30, 0x6b, 0x42, 0x23, + 0x30, 0x27, 0x2e, 0xd7, 0x00, 0x37, 0x2e, 0xd4, 0x00, 0x21, 0x2e, 0xd6, 0x00, 0x7a, 0x84, 0x17, 0x2c, 0x42, 0x42, + 0x30, 0x30, 0x21, 0x2e, 0xd4, 0x00, 0x12, 0x2d, 0x21, 0x30, 0x00, 0x30, 0x23, 0x2e, 0xd4, 0x00, 0x21, 0x2e, 0x7b, + 0xf7, 0x0b, 0x2d, 0x17, 0x30, 0x98, 0x2e, 0x51, 0x0c, 0xd5, 0x50, 0x0c, 0x82, 0x72, 0x30, 0x2f, 0x2e, 0xd4, 0x00, + 0x25, 0x2e, 0x7b, 0xf7, 0x40, 0x42, 0x00, 0x2e, 0xfb, 0x6f, 0xf0, 0x5f, 0xb8, 0x2e, 0x70, 0x50, 0x0a, 0x25, 0x39, + 0x86, 0xfb, 0x7f, 0xe1, 0x32, 0x62, 0x30, 0x98, 0x2e, 0xc2, 0xc4, 0xb5, 0x56, 0xa5, 0x6f, 0xab, 0x08, 0x91, 0x6f, + 0x4b, 0x08, 0xdf, 0x56, 0xc4, 0x6f, 0x23, 0x09, 0x4d, 0xba, 0x93, 0xbc, 0x8c, 0x0b, 0xd1, 0x6f, 0x0b, 0x09, 0xcb, + 0x52, 0xe1, 0x5e, 0x56, 0x42, 0xaf, 0x09, 0x4d, 0xba, 0x23, 0xbd, 0x94, 0x0a, 0xe5, 0x6f, 0x68, 0xbb, 0xeb, 0x08, + 0xbd, 0xb9, 0x63, 0xbe, 0xfb, 0x6f, 0x52, 0x42, 0xe3, 0x0a, 0xc0, 0x2e, 0x43, 0x42, 0x90, 0x5f, 0xd1, 0x50, 0x03, + 0x2e, 0x25, 0xf3, 0x13, 0x40, 0x00, 0x40, 0x9b, 0xbc, 0x9b, 0xb4, 0x08, 0xbd, 0xb8, 0xb9, 0x98, 0xbc, 0xda, 0x0a, + 0x08, 0xb6, 0x89, 0x16, 0xc0, 0x2e, 0x19, 0x00, 0x62, 0x02, 0x10, 0x50, 0xfb, 0x7f, 0x98, 0x2e, 0x81, 0x0d, 0x01, + 0x2e, 0xd4, 0x00, 0x31, 0x30, 0x08, 0x04, 0xfb, 0x6f, 0x01, 0x30, 0xf0, 0x5f, 0x23, 0x2e, 0xd6, 0x00, 0x21, 0x2e, + 0xd7, 0x00, 0xb8, 0x2e, 0x01, 0x2e, 0xd7, 0x00, 0x03, 0x2e, 0xd6, 0x00, 0x48, 0x0e, 0x01, 0x2f, 0x80, 0x2e, 0x1f, + 0x0e, 0xb8, 0x2e, 0xe3, 0x50, 0x21, 0x34, 0x01, 0x42, 0x82, 0x30, 0xc1, 0x32, 0x25, 0x2e, 0x62, 0xf5, 0x01, 0x00, + 0x22, 0x30, 0x01, 0x40, 0x4a, 0x0a, 0x01, 0x42, 0xb8, 0x2e, 0xe3, 0x54, 0xf0, 0x3b, 0x83, 0x40, 0xd8, 0x08, 0xe5, + 0x52, 0x83, 0x42, 0x00, 0x30, 0x83, 0x30, 0x50, 0x42, 0xc4, 0x32, 0x27, 0x2e, 0x64, 0xf5, 0x94, 0x00, 0x50, 0x42, + 0x40, 0x42, 0xd3, 0x3f, 0x84, 0x40, 0x7d, 0x82, 0xe3, 0x08, 0x40, 0x42, 0x83, 0x42, 0xb8, 0x2e, 0xdd, 0x52, 0x00, + 0x30, 0x40, 0x42, 0x7c, 0x86, 0xb9, 0x52, 0x09, 0x2e, 0x70, 0x0f, 0xbf, 0x54, 0xc4, 0x42, 0xd3, 0x86, 0x54, 0x40, + 0x55, 0x40, 0x94, 0x42, 0x85, 0x42, 0x21, 0x2e, 0xd7, 0x00, 0x42, 0x40, 0x25, 0x2e, 0xfd, 0xf3, 0xc0, 0x42, 0x7e, + 0x82, 0x05, 0x2e, 0x7d, 0x00, 0x80, 0xb2, 0x14, 0x2f, 0x05, 0x2e, 0x89, 0x00, 0x27, 0xbd, 0x2f, 0xb9, 0x80, 0x90, + 0x02, 0x2f, 0x21, 0x2e, 0x6f, 0xf5, 0x0c, 0x2d, 0x07, 0x2e, 0x71, 0x0f, 0x14, 0x30, 0x1c, 0x09, 0x05, 0x2e, 0x77, + 0xf7, 0xbd, 0x56, 0x47, 0xbe, 0x93, 0x08, 0x94, 0x0a, 0x25, 0x2e, 0x77, 0xf7, 0xe7, 0x54, 0x50, 0x42, 0x4a, 0x0e, + 0xfc, 0x2f, 0xb8, 0x2e, 0x50, 0x50, 0x02, 0x30, 0x43, 0x86, 0xe5, 0x50, 0xfb, 0x7f, 0xe3, 0x7f, 0xd2, 0x7f, 0xc0, + 0x7f, 0xb1, 0x7f, 0x00, 0x2e, 0x41, 0x40, 0x00, 0x40, 0x48, 0x04, 0x98, 0x2e, 0x74, 0xc0, 0x1e, 0xaa, 0xd3, 0x6f, + 0x14, 0x30, 0xb1, 0x6f, 0xe3, 0x22, 0xc0, 0x6f, 0x52, 0x40, 0xe4, 0x6f, 0x4c, 0x0e, 0x12, 0x42, 0xd3, 0x7f, 0xeb, + 0x2f, 0x03, 0x2e, 0x86, 0x0f, 0x40, 0x90, 0x11, 0x30, 0x03, 0x2f, 0x23, 0x2e, 0x86, 0x0f, 0x02, 0x2c, 0x00, 0x30, + 0xd0, 0x6f, 0xfb, 0x6f, 0xb0, 0x5f, 0xb8, 0x2e, 0x40, 0x50, 0xf1, 0x7f, 0x0a, 0x25, 0x3c, 0x86, 0xeb, 0x7f, 0x41, + 0x33, 0x22, 0x30, 0x98, 0x2e, 0xc2, 0xc4, 0xd3, 0x6f, 0xf4, 0x30, 0xdc, 0x09, 0x47, 0x58, 0xc2, 0x6f, 0x94, 0x09, + 0xeb, 0x58, 0x6a, 0xbb, 0xdc, 0x08, 0xb4, 0xb9, 0xb1, 0xbd, 0xe9, 0x5a, 0x95, 0x08, 0x21, 0xbd, 0xf6, 0xbf, 0x77, + 0x0b, 0x51, 0xbe, 0xf1, 0x6f, 0xeb, 0x6f, 0x52, 0x42, 0x54, 0x42, 0xc0, 0x2e, 0x43, 0x42, 0xc0, 0x5f, 0x50, 0x50, + 0xf5, 0x50, 0x31, 0x30, 0x11, 0x42, 0xfb, 0x7f, 0x7b, 0x30, 0x0b, 0x42, 0x11, 0x30, 0x02, 0x80, 0x23, 0x33, 0x01, + 0x42, 0x03, 0x00, 0x07, 0x2e, 0x80, 0x03, 0x05, 0x2e, 0xd3, 0x00, 0x23, 0x52, 0xe2, 0x7f, 0xd3, 0x7f, 0xc0, 0x7f, + 0x98, 0x2e, 0xb6, 0x0e, 0xd1, 0x6f, 0x08, 0x0a, 0x1a, 0x25, 0x7b, 0x86, 0xd0, 0x7f, 0x01, 0x33, 0x12, 0x30, 0x98, + 0x2e, 0xc2, 0xc4, 0xd1, 0x6f, 0x08, 0x0a, 0x00, 0xb2, 0x0d, 0x2f, 0xe3, 0x6f, 0x01, 0x2e, 0x80, 0x03, 0x51, 0x30, + 0xc7, 0x86, 0x23, 0x2e, 0x21, 0xf2, 0x08, 0xbc, 0xc0, 0x42, 0x98, 0x2e, 0xa5, 0xb7, 0x00, 0x2e, 0x00, 0x2e, 0xd0, + 0x2e, 0xb0, 0x6f, 0x0b, 0xb8, 0x03, 0x2e, 0x1b, 0x00, 0x08, 0x1a, 0xb0, 0x7f, 0x70, 0x30, 0x04, 0x2f, 0x21, 0x2e, + 0x21, 0xf2, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0x98, 0x2e, 0x6d, 0xc0, 0x98, 0x2e, 0x5d, 0xc0, 0xed, 0x50, 0x98, + 0x2e, 0x44, 0xcb, 0xef, 0x50, 0x98, 0x2e, 0x46, 0xc3, 0xf1, 0x50, 0x98, 0x2e, 0x53, 0xc7, 0x35, 0x50, 0x98, 0x2e, + 0x64, 0xcf, 0x10, 0x30, 0x98, 0x2e, 0xdc, 0x03, 0x20, 0x26, 0xc0, 0x6f, 0x02, 0x31, 0x12, 0x42, 0xab, 0x33, 0x0b, + 0x42, 0x37, 0x80, 0x01, 0x30, 0x01, 0x42, 0xf3, 0x37, 0xf7, 0x52, 0xfb, 0x50, 0x44, 0x40, 0xa2, 0x0a, 0x42, 0x42, + 0x8b, 0x31, 0x09, 0x2e, 0x5e, 0xf7, 0xf9, 0x54, 0xe3, 0x08, 0x83, 0x42, 0x1b, 0x42, 0x23, 0x33, 0x4b, 0x00, 0xbc, + 0x84, 0x0b, 0x40, 0x33, 0x30, 0x83, 0x42, 0x0b, 0x42, 0xe0, 0x7f, 0xd1, 0x7f, 0x98, 0x2e, 0x58, 0xb7, 0xd1, 0x6f, + 0x80, 0x30, 0x40, 0x42, 0x03, 0x30, 0xe0, 0x6f, 0xf3, 0x54, 0x04, 0x30, 0x00, 0x2e, 0x00, 0x2e, 0x01, 0x89, 0x62, + 0x0e, 0xfa, 0x2f, 0x43, 0x42, 0x11, 0x30, 0xfb, 0x6f, 0xc0, 0x2e, 0x01, 0x42, 0xb0, 0x5f, 0xc1, 0x4a, 0x00, 0x00, + 0x6d, 0x57, 0x00, 0x00, 0x77, 0x8e, 0x00, 0x00, 0xe0, 0xff, 0xff, 0xff, 0xd3, 0xff, 0xff, 0xff, 0xe5, 0xff, 0xff, + 0xff, 0xee, 0xe1, 0xff, 0xff, 0x7c, 0x13, 0x00, 0x00, 0x46, 0xe6, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1 +}; + +/*! @name Global array that stores the feature input configuration of BMI270 */ +const struct bmi2_feature_config bmi270_feat_in[BMI270_MAX_FEAT_IN] = { + { .type = BMI2_CONFIG_ID, .page = BMI2_PAGE_1, .start_addr = BMI270_CONFIG_ID_STRT_ADDR }, + { .type = BMI2_MAX_BURST_LEN, .page = BMI2_PAGE_1, .start_addr = BMI270_MAX_BURST_LEN_STRT_ADDR }, + { .type = BMI2_CRT_GYRO_SELF_TEST, .page = BMI2_PAGE_1, .start_addr = BMI270_CRT_GYRO_SELF_TEST_STRT_ADDR }, + { .type = BMI2_ABORT_CRT_GYRO_SELF_TEST, .page = BMI2_PAGE_1, .start_addr = BMI270_ABORT_STRT_ADDR }, + { .type = BMI2_AXIS_MAP, .page = BMI2_PAGE_1, .start_addr = BMI270_AXIS_MAP_STRT_ADDR }, + { .type = BMI2_GYRO_SELF_OFF, .page = BMI2_PAGE_1, .start_addr = BMI270_GYRO_SELF_OFF_STRT_ADDR }, + { .type = BMI2_NVM_PROG_PREP, .page = BMI2_PAGE_1, .start_addr = BMI270_NVM_PROG_PREP_STRT_ADDR }, + { .type = BMI2_GYRO_GAIN_UPDATE, .page = BMI2_PAGE_1, .start_addr = BMI270_GYRO_GAIN_UPDATE_STRT_ADDR }, + { .type = BMI2_ANY_MOTION, .page = BMI2_PAGE_1, .start_addr = BMI270_ANY_MOT_STRT_ADDR }, + { .type = BMI2_NO_MOTION, .page = BMI2_PAGE_2, .start_addr = BMI270_NO_MOT_STRT_ADDR }, + { .type = BMI2_SIG_MOTION, .page = BMI2_PAGE_2, .start_addr = BMI270_SIG_MOT_STRT_ADDR }, + { .type = BMI2_STEP_COUNTER_PARAMS, .page = BMI2_PAGE_3, .start_addr = BMI270_STEP_CNT_1_STRT_ADDR }, + { .type = BMI2_STEP_DETECTOR, .page = BMI2_PAGE_6, .start_addr = BMI270_STEP_CNT_4_STRT_ADDR }, + { .type = BMI2_STEP_COUNTER, .page = BMI2_PAGE_6, .start_addr = BMI270_STEP_CNT_4_STRT_ADDR }, + { .type = BMI2_STEP_ACTIVITY, .page = BMI2_PAGE_6, .start_addr = BMI270_STEP_CNT_4_STRT_ADDR }, + { .type = BMI2_WRIST_GESTURE, .page = BMI2_PAGE_6, .start_addr = BMI270_WRIST_GEST_STRT_ADDR }, + { .type = BMI2_WRIST_WEAR_WAKE_UP, .page = BMI2_PAGE_7, .start_addr = BMI270_WRIST_WEAR_WAKE_UP_STRT_ADDR }, +}; + +/*! @name Global array that stores the feature output configuration */ +const struct bmi2_feature_config bmi270_feat_out[BMI270_MAX_FEAT_OUT] = { + { .type = BMI2_STEP_COUNTER, .page = BMI2_PAGE_0, .start_addr = BMI270_STEP_CNT_OUT_STRT_ADDR }, + { .type = BMI2_STEP_ACTIVITY, .page = BMI2_PAGE_0, .start_addr = BMI270_STEP_ACT_OUT_STRT_ADDR }, + { .type = BMI2_WRIST_GESTURE, .page = BMI2_PAGE_0, .start_addr = BMI270_WRIST_GEST_OUT_STRT_ADDR }, + { .type = BMI2_GYRO_GAIN_UPDATE, .page = BMI2_PAGE_0, .start_addr = BMI270_GYR_USER_GAIN_OUT_STRT_ADDR }, + { .type = BMI2_GYRO_CROSS_SENSE, .page = BMI2_PAGE_0, .start_addr = BMI270_GYRO_CROSS_SENSE_STRT_ADDR }, + { .type = BMI2_NVM_STATUS, .page = BMI2_PAGE_0, .start_addr = BMI270_NVM_VFRM_OUT_STRT_ADDR }, + { .type = BMI2_VFRM_STATUS, .page = BMI2_PAGE_0, .start_addr = BMI270_NVM_VFRM_OUT_STRT_ADDR } +}; + +/*! @name Global array that stores the feature interrupts of BMI270 */ +struct bmi2_map_int bmi270_map_int[BMI270_MAX_INT_MAP] = { + { .type = BMI2_SIG_MOTION, .sens_map_int = BMI270_INT_SIG_MOT_MASK }, + { .type = BMI2_STEP_COUNTER, .sens_map_int = BMI270_INT_STEP_COUNTER_MASK }, + { .type = BMI2_STEP_DETECTOR, .sens_map_int = BMI270_INT_STEP_DETECTOR_MASK }, + { .type = BMI2_STEP_ACTIVITY, .sens_map_int = BMI270_INT_STEP_ACT_MASK }, + { .type = BMI2_WRIST_GESTURE, .sens_map_int = BMI270_INT_WRIST_GEST_MASK }, + { .type = BMI2_WRIST_WEAR_WAKE_UP, .sens_map_int = BMI270_INT_WRIST_WEAR_WAKEUP_MASK }, + { .type = BMI2_ANY_MOTION, .sens_map_int = BMI270_INT_ANY_MOT_MASK }, + { .type = BMI2_NO_MOTION, .sens_map_int = BMI270_INT_NO_MOT_MASK }, +}; + +/******************************************************************************/ + +/*! Local Function Prototypes + ******************************************************************************/ + +/*! + * @brief This internal API is used to validate the device pointer for + * null conditions. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t null_ptr_check(const struct bmi2_dev *dev); + +/*! + * @brief This internal API enables the selected sensor/features. + * + * @param[in] sensor_sel : Selects the desired sensor. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t sensor_enable(uint64_t sensor_sel, struct bmi2_dev *dev); + +/*! + * @brief This internal API disables the selected sensor/features. + * + * @param[in] sensor_sel : Selects the desired sensor. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t sensor_disable(uint64_t sensor_sel, struct bmi2_dev *dev); + +/*! + * @brief This internal API selects the sensors/features to be enabled or + * disabled. + * + * @param[in] sens_list : Pointer to select the sensor. + * @param[in] n_sens : Number of sensors selected. + * @param[out] sensor_sel : Gets the selected sensor. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t select_sensor(const uint8_t *sens_list, uint8_t n_sens, uint64_t *sensor_sel); + +/*! + * @brief This internal API is used to enable/disable any-motion feature. + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[in] enable : Enables/Disables any-motion. + * + * Enable | Description + * -------------|--------------- + * BMI2_DISABLE | Disables any-motion. + * BMI2_ENABLE | Enables any-motion. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_any_motion(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to enable/disable no-motion feature. + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[in] enable : Enables/Disables no-motion. + * + * Enable | Description + * -------------|--------------- + * BMI2_DISABLE | Disables no-motion. + * BMI2_ENABLE | Enables no-motion. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_no_motion(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to enable/disable sig-motion feature. + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[in] enable : Enables/Disables sig-motion. + * + * Enable | Description + * -------------|--------------- + * BMI2_DISABLE | Disables sig-motion. + * BMI2_ENABLE | Enables sig-motion. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_sig_motion(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to enable/disable step detector feature. + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[in] enable : Enables/Disables step-detector. + * + * Enable | Description + * -------------|--------------- + * BMI2_DISABLE | Disables step detector + * BMI2_ENABLE | Enables step detector + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_step_detector(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to enable/disable step counter feature. + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[in] enable : Enables/Disables step counter. + * + * Enable | Description + * -------------|--------------- + * BMI2_DISABLE | Disables step counter + * BMI2_ENABLE | Enables step counter + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_step_counter(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to enable/disable step activity detection. + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[in] enable : Enables/Disables step activity. + * + * Enable | Description + * -------------|--------------- + * BMI2_DISABLE | Disables step activity + * BMI2_ENABLE | Enables step activity + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_step_activity(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API gives an option to enable offset correction + * feature of gyroscope, either internally or by the host. + * + * @param[in] enable : Enables/Disables self-offset correction. + * @param[in] dev : Structure instance of bmi2_dev. + * + * enable | Description + * -------------|--------------- + * BMI2_ENABLE | gyroscope offset correction values are set internally + * BMI2_DISABLE | gyroscope offset correction values has to be set by host + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_gyro_self_offset_corr(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to enable/disable gyroscope user gain + * feature. + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[in] enable : Enables/Disables gyroscope user gain. + * + * Enable | Description + * -------------|--------------- + * BMI2_DISABLE | Disables gyroscope user gain + * BMI2_ENABLE | Enables gyroscope user gain + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_gyro_user_gain(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API enables the wrist gesture feature. + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[in] enable : Enables/Disables wrist gesture. + * + * Enable | Description + * -------------|--------------- + * BMI2_DISABLE | Disables wrist gesture + * BMI2_ENABLE | Enables wrist gesture + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_wrist_gesture(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API enables the wrist wear wake up feature. + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[in] enable : Enables/Disables wrist wear wake up. + * + * Enable | Description + * -------------|--------------- + * BMI2_DISABLE | Disables wrist wear wake up + * BMI2_ENABLE | Enables wrist wear wake up + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_wrist_wear_wake_up(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API sets any-motion configurations like axes select, + * duration, threshold and output-configuration. + * + * @param[in] config : Structure instance of bmi2_any_motion_config. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @verbatim + *---------------------------------------------------------------------------- + * bmi2_any_motion_config | + * Structure parameters | Description + *--------------------------|-------------------------------------------------- + * | Defines the number of consecutive data points for + * | which the threshold condition must be respected, + * | for interrupt assertion. It is expressed in 50 Hz + * duration | samples (20 msec). + * | Range is 0 to 163sec. + * | Default value is 5 = 100ms. + * -------------------------|--------------------------------------------------- + * | Slope threshold value for in 5.11g format. + * threshold | Range is 0 to 1g. + * | Default value is 0xAA = 83mg. + * -------------------------|--------------------------------------------------- + * x_sel, y_sel, z_sel | Selects the feature on a per-axis basis + * -------------------------|--------------------------------------------------- + * @endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_any_motion_config(const struct bmi2_any_motion_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API sets no-motion configurations like axes select, + * duration, threshold and output-configuration. + * + * @param[in] config : Structure instance of bmi2_no_motion_config. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @verbatim + *---------------------------------------------------------------------------- + * bmi2_no_motion_config | + * Structure parameters | Description + *--------------------------|-------------------------------------------------- + * | Defines the number of consecutive data points for + * | which the threshold condition must be respected, + * | for interrupt assertion. It is expressed in 50 Hz + * duration | samples (20 msec). + * | Range is 0 to 163sec. + * | Default value is 5 = 100ms. + * -------------------------|--------------------------------------------------- + * | Slope threshold value for in 5.11g format. + * threshold | Range is 0 to 1g. + * | Default value is 0xAA = 83mg. + * -------------------------|--------------------------------------------------- + * x_sel, y_sel, z_sel | Selects the feature on a per-axis basis + * -------------------------|--------------------------------------------------- + * @endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_no_motion_config(const struct bmi2_no_motion_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API sets sig-motion configurations like block-size, + * output-configuration and other parameters. + * + * @param[in] config : Structure instance of bmi2_sig_motion_config. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + *---------------------------------------------------------------------------- + * bmi2_sig_motion_config | + * Structure parameters | Description + * -------------------------|--------------------------------------------------- + * | Defines the duration after which the significant + * block_size | motion interrupt is triggered. It is expressed in + * | 50 Hz samples (20 ms). Default value is 0xFA=5sec. + *--------------------------|--------------------------------------------------- + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_sig_motion_config(const struct bmi2_sig_motion_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API sets step counter parameter configurations. + * + * @param[in] step_count_params : Array that stores parameters 1 to 25. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_step_count_params_config(const uint16_t *step_count_params, struct bmi2_dev *dev); + +/*! + * @brief This internal API sets step counter/detector/activity configurations. + * + * @param[in] config : Structure instance of bmi2_step_config. + * @param[in] dev : Structure instance of bmi2_dev. + * + *--------------------------------------------------------------------------- + * bmi2_step_config | + * Structure parameters | Description + *--------------------------|-------------------------------------------------- + * | The Step-counter will trigger output every time + * | the number of steps are counted. Holds implicitly + * water-mark level | a 20x factor, so the range is 0 to 10230, + * | with resolution of 20 steps. + * -------------------------|--------------------------------------------------- + * reset counter | Flag to reset the counted steps. + * -------------------------|--------------------------------------------------- + * @endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_step_config(const struct bmi2_step_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API sets wrist gesture configurations like wearable-arm, + * and output-configuration. + * + * @param[in] config : Structure instance of bmi2_wrist_gest_config. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @verbatim + *----------------------------------------------------------------------------- + * bmi2_wrist_gest_config | + * Structure parameters | Description + *--------------------------|-------------------------------------------------- + * | Device in left (0) or right (1) arm. By default, + * wear_arm | the wearable device is assumed to be in left arm + * | i.e. default value is 0. + * -------------------------|--------------------------------------------------- + * @endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_wrist_gest_config(const struct bmi2_wrist_gest_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API sets wrist wear wake-up configurations like + * output-configuration. + * + * @param[in] config : Structure instance of + * bmi2_wrist_wear_wake_up_config. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @verbatim + *----------------------------------------------------------------------------- + * bmi2_wrist_wear_wake_up_config | + * Structure parameters | Description + *----------------------------------|------------------------------------------- + * | To set the wrist wear wake-up parameters like + * | min_angle_focus, min_angle_nonfocus, + * wrist_wear_wakeup_params | angle_landscape_left, angle_landscape_right, + * | angle_potrait_up and down. + * ---------------------------------|------------------------------------------- + * @endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_wrist_wear_wake_up_config(const struct bmi2_wrist_wear_wake_up_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets any-motion configurations like axes select, + * duration, threshold and output-configuration. + * + * @param[out] config : Structure instance of bmi2_any_motion_config. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @verbatim + *---------------------------------------------------------------------------- + * bmi2_any_motion_config | + * Structure parameters | Description + *--------------------------|-------------------------------------------------- + * | Defines the number of consecutive data points for + * | which the threshold condition must be respected, + * | for interrupt assertion. It is expressed in 50 Hz + * duration | samples (20 msec). + * | Range is 0 to 163sec. + * | Default value is 5 = 100ms. + * -------------------------|--------------------------------------------------- + * | Slope threshold value for in 5.11g format. + * threshold | Range is 0 to 1g. + * | Default value is 0xAA = 83mg. + * -------------------------|--------------------------------------------------- + * x_sel, y_sel, z_sel | Selects the feature on a per-axis basis + * -------------------------|--------------------------------------------------- + * @endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_any_motion_config(struct bmi2_any_motion_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets no-motion configurations like axes select, + * duration, threshold and output-configuration. + * + * @param[out] config : Structure instance of bmi2_no_motion_config. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @verbatim + *---------------------------------------------------------------------------- + * bmi2_no_motion_config | + * Structure parameters | Description + *--------------------------|-------------------------------------------------- + * | Defines the number of consecutive data points for + * | which the threshold condition must be respected, + * | for interrupt assertion. It is expressed in 50 Hz + * duration | samples (20 msec). + * | Range is 0 to 163sec. + * | Default value is 5 = 100ms. + * -------------------------|--------------------------------------------------- + * | Slope threshold value for in 5.11g format. + * threshold | Range is 0 to 1g. + * | Default value is 0xAA = 83mg. + * -------------------------|--------------------------------------------------- + * x_sel, y_sel, z_sel | Selects the feature on a per-axis basis + * -------------------------|--------------------------------------------------- + * @endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_no_motion_config(struct bmi2_no_motion_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets sig-motion configurations like block-size, + * output-configuration and other parameters. + * + * @param[out] config : Structure instance of bmi2_sig_motion_config. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + *---------------------------------------------------------------------------- + * bmi2_sig_motion_config | + * Structure parameters | Description + * -------------------------|--------------------------------------------------- + * | Defines the duration after which the significant + * block_size | motion interrupt is triggered. It is expressed in + * | 50 Hz samples (20 ms). Default value is 0xFA=5sec. + *--------------------------|--------------------------------------------------- + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_sig_motion_config(struct bmi2_sig_motion_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets step counter parameter configurations. + * + * @param[in] step_count_params : Array that stores parameters 1 to 25. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_step_count_params_config(uint16_t *step_count_params, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets step counter/detector/activity configurations. + * + * @param[out] config : Structure instance of bmi2_step_config. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @verbatim + *---------------------------------------------------------------------------- + * bmi2_step_config | + * Structure parameters | Description + *--------------------------|-------------------------------------------------- + * | The Step-counter will trigger output every time + * | the number of steps are counted. Holds implicitly + * water-mark level | a 20x factor, so the range is 0 to 10230, + * | with resolution of 20 steps. + * -------------------------|--------------------------------------------------- + * reset counter | Flag to reset the counted steps. + * -------------------------|--------------------------------------------------- + * @endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_step_config(struct bmi2_step_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets wrist gesture configurations like wearable-arm, + * and output-configuration. + * + * @param[out] config : Structure instance of bmi2_wrist_gest_config. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @verbatim + *----------------------------------------------------------------------------- + * bmi2_wrist_gest_config | + * Structure parameters | Description + *--------------------------|-------------------------------------------------- + * | Device in left (0) or right (1) arm. By default, + * wear_arm | the wearable device is assumed to be in left arm + * | i.e. default value is 0. + * -------------------------|--------------------------------------------------- + * @endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_wrist_gest_config(struct bmi2_wrist_gest_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets wrist wear wake-up configurations like + * output-configuration. + * + * @param[out] config : Structure instance of + * bmi2_wrist_wear_wake_up_config. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @verbatim + *------------------------------------|--------------------------------------- + * bmi2_wrist_wear_wake_up_wh_config | + * Structure parameters | Description + *------------------------------------|------------------------------------------- + * | To get the wrist wear wake-up parameters like + * | min_angle_focus, min_angle_nonfocus, + * wrist_wear_wake_params | angle_landscape_left, angle_landscape_right, + * | angle_potrait_up and down. + * -----------------------------------|------------------------------------------- + * @endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_wrist_wear_wake_up_config(struct bmi2_wrist_wear_wake_up_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets the output values of wrist gesture. + * + * @param[out] wrist_gest : Pointer to the stored wrist gesture. + * @param[in] dev : Structure instance of bmi2_dev. + * + * *wrist_gest | Output + * -------------|------------ + * 0x00 | UNKNOWN + * 0x01 | PUSH_ARM_DOWN + * 0x02 | PIVOT_UP + * 0x03 | WRIST_SHAKE_JIGGLE + * 0x04 | FLICK_IN + * 0x05 | FLICK_OUT + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_wrist_gest_status(uint8_t *wrist_gest, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets the output values of step activity. + * + * @param[out] step_act : Pointer to the stored step activity data. + * @param[in] dev : Structure instance of bmi2_dev. + * + * *step_act | Output + * -----------|------------ + * 0x00 | STILL + * 0x01 | WALKING + * 0x02 | RUNNING + * 0x03 | UNKNOWN + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fails + */ +static int8_t get_step_activity_output(uint8_t *step_act, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets the output values of step counter. + * + * @param[out] step_count : Pointer to the stored step counter data. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_step_counter_output(uint32_t *step_count, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets the error status related to NVM. + * + * @param[out] nvm_err_stat : Stores the NVM error status. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_nvm_error_status(struct bmi2_nvm_err_status *nvm_err_stat, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets the error status related to virtual frames. + * + * @param[out] vfrm_err_stat : Stores the VFRM related error status. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_vfrm_error_status(struct bmi2_vfrm_err_status *vfrm_err_stat, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to get enable status of gyroscope user gain + * update. + * + * @param[out] status : Stores status of gyroscope user gain update. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_user_gain_upd_status(uint8_t *status, struct bmi2_dev *dev); + +/*! + * @brief This internal API enables/disables compensation of the gain defined + * in the GAIN register. + * + * @param[in] enable : Enables/Disables gain compensation + * @param[in] dev : Structure instance of bmi2_dev. + * + * enable | Description + * -------------|--------------- + * BMI2_ENABLE | Enable gain compensation. + * BMI2_DISABLE | Disable gain compensation. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t enable_gyro_gain(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to extract the output feature configuration + * details like page and start address from the look-up table. + * + * @param[out] feat_output : Structure that stores output feature + * configurations. + * @param[in] type : Type of feature or sensor. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Returns the feature found flag. + * + * @retval BMI2_FALSE : Feature not found + * BMI2_TRUE : Feature found + */ +static uint8_t extract_output_feat_config(struct bmi2_feature_config *feat_output, + uint8_t type, + const struct bmi2_dev *dev); + +/***************************************************************************/ + +/*! User Interface Definitions + ****************************************************************************/ + +/*! + * @brief This API: + * 1) updates the device structure with address of the configuration file. + * 2) Initializes BMI270 sensor. + * 3) Writes the configuration file. + * 4) Updates the feature offset parameters in the device structure. + * 5) Updates the maximum number of pages, in the device structure. + */ +int8_t bmi270_init(struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + /* Assign chip id of BMI270 */ + dev->chip_id = BMI270_CHIP_ID; + + /* get the size of config array */ + dev->config_size = sizeof(bmi270_config_file); + + /* Enable the variant specific features if any */ + dev->variant_feature = BMI2_GYRO_CROSS_SENS_ENABLE | BMI2_CRT_RTOSK_ENABLE; + + /* An extra dummy byte is read during SPI read */ + if (dev->intf == BMI2_SPI_INTF) + { + dev->dummy_byte = 1; + } + else + { + dev->dummy_byte = 0; + } + + /* If configuration file pointer is not assigned any address */ + if (!dev->config_file_ptr) + { + /* Give the address of the configuration file array to + * the device pointer + */ + dev->config_file_ptr = bmi270_config_file; + } + + /* Initialize BMI2 sensor */ + rslt = bmi2_sec_init(dev); + if (rslt == BMI2_OK) + { + /* Assign the offsets of the feature input + * configuration to the device structure + */ + dev->feat_config = bmi270_feat_in; + + /* Assign the offsets of the feature output to + * the device structure + */ + dev->feat_output = bmi270_feat_out; + + /* Assign the maximum number of pages to the + * device structure + */ + dev->page_max = BMI270_MAX_PAGE_NUM; + + /* Assign maximum number of input sensors/ + * features to device structure + */ + dev->input_sens = BMI270_MAX_FEAT_IN; + + /* Assign maximum number of output sensors/ + * features to device structure + */ + dev->out_sens = BMI270_MAX_FEAT_OUT; + + /* Assign the offsets of the feature interrupt + * to the device structure + */ + dev->map_int = bmi270_map_int; + + /* Assign maximum number of feature interrupts + * to device structure + */ + dev->sens_int_map = BMI270_MAX_INT_MAP; + + /* Get the gyroscope cross axis sensitivity */ + rslt = bmi2_get_gyro_cross_sense(dev); + } + } + + return rslt; +} + +/*! + * @brief This API selects the sensors/features to be enabled. + */ +int8_t bmi270_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to select sensor */ + uint64_t sensor_sel = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sens_list != NULL)) + { + /* Get the selected sensors */ + rslt = select_sensor(sens_list, n_sens, &sensor_sel); + if (rslt == BMI2_OK) + { + /* Enable the selected sensors */ + rslt = sensor_enable(sensor_sel, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API selects the sensors/features to be disabled. + */ +int8_t bmi270_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to select sensor */ + uint64_t sensor_sel = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sens_list != NULL)) + { + /* Get the selected sensors */ + rslt = select_sensor(sens_list, n_sens, &sensor_sel); + if (rslt == BMI2_OK) + { + /* Disable the selected sensors */ + rslt = sensor_disable(sensor_sel, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API sets the sensor/feature configuration. + */ +int8_t bmi270_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define loop */ + uint8_t loop; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sens_cfg != NULL)) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + + for (loop = 0; loop < n_sens; loop++) + { + if ((sens_cfg[loop].type == BMI2_ACCEL) || (sens_cfg[loop].type == BMI2_GYRO) || + (sens_cfg[loop].type == BMI2_AUX) || (sens_cfg[loop].type == BMI2_GYRO_GAIN_UPDATE)) + { + rslt = bmi2_set_sensor_config(&sens_cfg[loop], 1, dev); + } + else + { + /* Disable Advance power save if enabled for auxiliary + * and feature configurations + */ + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if + * enabled + */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + if (rslt == BMI2_OK) + { + switch (sens_cfg[loop].type) + { + /* Set any motion configuration */ + case BMI2_ANY_MOTION: + rslt = set_any_motion_config(&sens_cfg[loop].cfg.any_motion, dev); + break; + + /* Set no motion configuration */ + case BMI2_NO_MOTION: + rslt = set_no_motion_config(&sens_cfg[loop].cfg.no_motion, dev); + break; + + /* Set sig-motion configuration */ + case BMI2_SIG_MOTION: + rslt = set_sig_motion_config(&sens_cfg[loop].cfg.sig_motion, dev); + break; + + /* Set the step counter parameters */ + case BMI2_STEP_COUNTER_PARAMS: + rslt = set_step_count_params_config(sens_cfg[loop].cfg.step_counter_params, dev); + break; + + /* Set step counter/detector/activity configuration */ + case BMI2_STEP_DETECTOR: + case BMI2_STEP_COUNTER: + case BMI2_STEP_ACTIVITY: + rslt = set_step_config(&sens_cfg[loop].cfg.step_counter, dev); + break; + + /* Set the wrist gesture configuration */ + case BMI2_WRIST_GESTURE: + rslt = set_wrist_gest_config(&sens_cfg[loop].cfg.wrist_gest, dev); + break; + + /* Set the wrist wear wake-up configuration */ + case BMI2_WRIST_WEAR_WAKE_UP: + rslt = set_wrist_wear_wake_up_config(&sens_cfg[loop].cfg.wrist_wear_wake_up, dev); + break; + + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + } + + /* Return error if any of the set configurations fail */ + if (rslt != BMI2_OK) + { + break; + } + } + } + + /* Enable Advance power save if disabled while configuring and + * not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API gets the sensor/feature configuration. + */ +int8_t bmi270_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define loop */ + uint8_t loop; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sens_cfg != NULL)) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + for (loop = 0; loop < n_sens; loop++) + { + if ((sens_cfg[loop].type == BMI2_ACCEL) || (sens_cfg[loop].type == BMI2_GYRO) || + (sens_cfg[loop].type == BMI2_AUX) || (sens_cfg[loop].type == BMI2_GYRO_GAIN_UPDATE)) + { + rslt = bmi2_get_sensor_config(&sens_cfg[loop], 1, dev); + } + else + { + /* Disable Advance power save if enabled for auxiliary + * and feature configurations + */ + if ((sens_cfg[loop].type >= BMI2_MAIN_SENS_MAX_NUM) || (sens_cfg[loop].type == BMI2_AUX)) + { + + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if + * enabled + */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + } + + if (rslt == BMI2_OK) + { + switch (sens_cfg[loop].type) + { + /* Get sig-motion configuration */ + case BMI2_SIG_MOTION: + rslt = get_sig_motion_config(&sens_cfg[loop].cfg.sig_motion, dev); + break; + + /* Get any motion configuration */ + case BMI2_ANY_MOTION: + rslt = get_any_motion_config(&sens_cfg[loop].cfg.any_motion, dev); + break; + + /* Get no motion configuration */ + case BMI2_NO_MOTION: + rslt = get_no_motion_config(&sens_cfg[loop].cfg.no_motion, dev); + break; + + /* Set the step counter parameters */ + case BMI2_STEP_COUNTER_PARAMS: + rslt = get_step_count_params_config(sens_cfg[loop].cfg.step_counter_params, dev); + break; + + /* Get step counter/detector/activity configuration */ + case BMI2_STEP_DETECTOR: + case BMI2_STEP_COUNTER: + case BMI2_STEP_ACTIVITY: + rslt = get_step_config(&sens_cfg[loop].cfg.step_counter, dev); + break; + + /* Get the wrist gesture configuration */ + case BMI2_WRIST_GESTURE: + rslt = get_wrist_gest_config(&sens_cfg[loop].cfg.wrist_gest, dev); + break; + + /* Get the wrist wear wake-up configuration */ + case BMI2_WRIST_WEAR_WAKE_UP: + rslt = get_wrist_wear_wake_up_config(&sens_cfg[loop].cfg.wrist_wear_wake_up, dev); + break; + + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + } + + /* Return error if any of the get configurations fail */ + if (rslt != BMI2_OK) + { + break; + } + } + } + + /* Enable Advance power save if disabled while configuring and + * not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API gets the sensor/feature data for accelerometer, gyroscope, + * auxiliary sensor, step counter, high-g, gyroscope user-gain update, + * orientation, gyroscope cross sensitivity and error status for NVM and VFRM. + */ +int8_t bmi270_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define loop */ + uint8_t loop; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sensor_data != NULL)) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + for (loop = 0; loop < n_sens; loop++) + { + if ((sensor_data[loop].type == BMI2_ACCEL) || (sensor_data[loop].type == BMI2_GYRO) || + (sensor_data[loop].type == BMI2_AUX) || (sensor_data[loop].type == BMI2_GYRO_GAIN_UPDATE) || + (sensor_data[loop].type == BMI2_GYRO_CROSS_SENSE)) + { + rslt = bmi2_get_sensor_data(&sensor_data[loop], 1, dev); + } + else + { + /* Disable Advance power save if enabled for feature + * configurations + */ + if (sensor_data[loop].type >= BMI2_MAIN_SENS_MAX_NUM) + { + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if + * enabled + */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + } + + if (rslt == BMI2_OK) + { + switch (sensor_data[loop].type) + { + case BMI2_STEP_COUNTER: + + /* Get step counter output */ + rslt = get_step_counter_output(&sensor_data[loop].sens_data.step_counter_output, dev); + break; + case BMI2_STEP_ACTIVITY: + + /* Get step activity output */ + rslt = get_step_activity_output(&sensor_data[loop].sens_data.activity_output, dev); + break; + case BMI2_NVM_STATUS: + + /* Get NVM error status */ + rslt = get_nvm_error_status(&sensor_data[loop].sens_data.nvm_status, dev); + break; + case BMI2_VFRM_STATUS: + + /* Get VFRM error status */ + rslt = get_vfrm_error_status(&sensor_data[loop].sens_data.vfrm_status, dev); + break; + case BMI2_WRIST_GESTURE: + + /* Get wrist gesture status */ + rslt = get_wrist_gest_status(&sensor_data[loop].sens_data.wrist_gesture_output, dev); + break; + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + + /* Return error if any of the get sensor data fails */ + if (rslt != BMI2_OK) + { + break; + } + } + } + + /* Enable Advance power save if disabled while + * configuring and not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API updates the gyroscope user-gain. + */ +int8_t bmi270_update_gyro_user_gain(const struct bmi2_gyro_user_gain_config *user_gain, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to select sensor */ + uint8_t sens_sel[2] = { BMI2_GYRO, BMI2_GYRO_GAIN_UPDATE }; + + /* Structure to define sensor configurations */ + struct bmi2_sens_config sens_cfg; + + /* Variable to store status of user-gain update module */ + uint8_t status = 0; + + /* Variable to define count */ + uint8_t count = 100; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (user_gain != NULL)) + { + /* Select type of feature */ + sens_cfg.type = BMI2_GYRO_GAIN_UPDATE; + + /* Get the user gain configurations */ + rslt = bmi270_get_sensor_config(&sens_cfg, 1, dev); + if (rslt == BMI2_OK) + { + /* Get the user-defined ratio */ + sens_cfg.cfg.gyro_gain_update = *user_gain; + + /* Set rate ratio for all axes */ + rslt = bmi270_set_sensor_config(&sens_cfg, 1, dev); + } + + /* Disable gyroscope */ + if (rslt == BMI2_OK) + { + rslt = bmi270_sensor_disable(&sens_sel[0], 1, dev); + } + + /* Enable gyroscope user-gain update module */ + if (rslt == BMI2_OK) + { + rslt = bmi270_sensor_enable(&sens_sel[1], 1, dev); + } + + /* Set the command to trigger the computation */ + if (rslt == BMI2_OK) + { + rslt = bmi2_set_command_register(BMI2_USR_GAIN_CMD, dev); + } + + if (rslt == BMI2_OK) + { + /* Poll until enable bit of user-gain update is 0 */ + while (count--) + { + rslt = get_user_gain_upd_status(&status, dev); + if ((rslt == BMI2_OK) && (status == 0)) + { + /* Enable compensation of gain defined + * in the GAIN register + */ + rslt = enable_gyro_gain(BMI2_ENABLE, dev); + + /* Enable gyroscope */ + if (rslt == BMI2_OK) + { + rslt = bmi270_sensor_enable(&sens_sel[0], 1, dev); + } + + break; + } + + dev->delay_us(10000, dev->intf_ptr); + } + + /* Return error if user-gain update is failed */ + if ((rslt == BMI2_OK) && (status != 0)) + { + rslt = BMI2_E_GYR_USER_GAIN_UPD_FAIL; + } + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API reads the compensated gyroscope user-gain values. + */ +int8_t bmi270_read_gyro_user_gain(struct bmi2_gyro_user_gain_data *gyr_usr_gain, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define register data */ + uint8_t reg_data[3] = { 0 }; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (gyr_usr_gain != NULL)) + { + /* Get the gyroscope compensated gain values */ + rslt = bmi2_get_regs(BMI2_GYR_USR_GAIN_0_ADDR, reg_data, 3, dev); + if (rslt == BMI2_OK) + { + /* Gyroscope user gain correction X-axis */ + gyr_usr_gain->x = (int8_t)BMI2_GET_BIT_POS0(reg_data[0], BMI2_GYR_USR_GAIN_X); + + /* Gyroscope user gain correction Y-axis */ + gyr_usr_gain->y = (int8_t)BMI2_GET_BIT_POS0(reg_data[1], BMI2_GYR_USR_GAIN_Y); + + /* Gyroscope user gain correction z-axis */ + gyr_usr_gain->z = (int8_t)BMI2_GET_BIT_POS0(reg_data[2], BMI2_GYR_USR_GAIN_Z); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API maps/unmaps feature interrupts to that of interrupt pins. + */ +int8_t bmi270_map_feat_int(const struct bmi2_sens_int_config *sens_int, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define loop */ + uint8_t loop; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sens_int != NULL)) + { + for (loop = 0; loop < n_sens; loop++) + { + switch (sens_int[loop].type) + { + case BMI2_SIG_MOTION: + case BMI2_WRIST_GESTURE: + case BMI2_ANY_MOTION: + case BMI2_NO_MOTION: + case BMI2_STEP_COUNTER: + case BMI2_STEP_DETECTOR: + case BMI2_STEP_ACTIVITY: + case BMI2_WRIST_WEAR_WAKE_UP: + + rslt = bmi2_map_feat_int(sens_int[loop].type, sens_int[loop].hw_int_pin, dev); + break; + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + + /* Return error if interrupt mapping fails */ + if (rslt != BMI2_OK) + { + break; + } + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/***************************************************************************/ + +/*! Local Function Definitions + ****************************************************************************/ + +/*! + * @brief This internal API is used to validate the device structure pointer for + * null conditions. + */ +static int8_t null_ptr_check(const struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + if ((dev == NULL) || (dev->read == NULL) || (dev->write == NULL) || (dev->delay_us == NULL)) + { + /* Device structure pointer is not valid */ + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This internal API selects the sensor/features to be enabled or + * disabled. + */ +static int8_t select_sensor(const uint8_t *sens_list, uint8_t n_sens, uint64_t *sensor_sel) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Variable to define loop */ + uint8_t count; + + for (count = 0; count < n_sens; count++) + { + switch (sens_list[count]) + { + case BMI2_ACCEL: + *sensor_sel |= BMI2_ACCEL_SENS_SEL; + break; + case BMI2_GYRO: + *sensor_sel |= BMI2_GYRO_SENS_SEL; + break; + case BMI2_AUX: + *sensor_sel |= BMI2_AUX_SENS_SEL; + break; + case BMI2_TEMP: + *sensor_sel |= BMI2_TEMP_SENS_SEL; + break; + case BMI2_SIG_MOTION: + *sensor_sel |= BMI2_SIG_MOTION_SEL; + break; + case BMI2_ANY_MOTION: + *sensor_sel |= BMI2_ANY_MOT_SEL; + break; + case BMI2_NO_MOTION: + *sensor_sel |= BMI2_NO_MOT_SEL; + break; + case BMI2_STEP_DETECTOR: + *sensor_sel |= BMI2_STEP_DETECT_SEL; + break; + case BMI2_STEP_COUNTER: + *sensor_sel |= BMI2_STEP_COUNT_SEL; + break; + case BMI2_STEP_ACTIVITY: + *sensor_sel |= BMI2_STEP_ACT_SEL; + break; + case BMI2_GYRO_GAIN_UPDATE: + *sensor_sel |= BMI2_GYRO_GAIN_UPDATE_SEL; + break; + case BMI2_GYRO_SELF_OFF: + *sensor_sel |= BMI2_GYRO_SELF_OFF_SEL; + break; + case BMI2_WRIST_GESTURE: + *sensor_sel |= BMI2_WRIST_GEST_SEL; + break; + case BMI2_WRIST_WEAR_WAKE_UP: + *sensor_sel |= BMI2_WRIST_WEAR_WAKE_UP_SEL; + break; + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + } + + return rslt; +} + +/*! + * @brief This internal API enables the selected sensor/features. + */ +static int8_t sensor_enable(uint64_t sensor_sel, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store register values */ + uint8_t reg_data = 0; + + /* Variable to define loop */ + uint8_t loop = 1; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat = 0; + + rslt = bmi2_get_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + /* Enable accelerometer */ + if (sensor_sel & BMI2_ACCEL_SENS_SEL) + { + reg_data = BMI2_SET_BITS(reg_data, BMI2_ACC_EN, BMI2_ENABLE); + } + + /* Enable gyroscope */ + if (sensor_sel & BMI2_GYRO_SENS_SEL) + { + reg_data = BMI2_SET_BITS(reg_data, BMI2_GYR_EN, BMI2_ENABLE); + } + + /* Enable auxiliary sensor */ + if (sensor_sel & BMI2_AUX_SENS_SEL) + { + reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_AUX_EN, BMI2_ENABLE); + } + + /* Enable temperature sensor */ + if (sensor_sel & BMI2_TEMP_SENS_SEL) + { + reg_data = BMI2_SET_BITS(reg_data, BMI2_TEMP_EN, BMI2_ENABLE); + } + + /* Enable the sensors that are set in the power control register */ + if (sensor_sel & BMI2_MAIN_SENSORS) + { + rslt = bmi2_set_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); + } + } + + if ((rslt == BMI2_OK) && (sensor_sel & ~(BMI2_MAIN_SENSORS))) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if enabled */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + if (rslt == BMI2_OK) + { + while (loop--) + { + /* Enable sig-motion feature */ + if (sensor_sel & BMI2_SIG_MOTION_SEL) + { + rslt = set_sig_motion(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_SIG_MOTION_SEL; + } + else + { + break; + } + } + + /* Enable any motion feature */ + if (sensor_sel & BMI2_ANY_MOT_SEL) + { + rslt = set_any_motion(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_ANY_MOT_SEL; + } + else + { + break; + } + } + + /* Enable no motion feature */ + if (sensor_sel & BMI2_NO_MOT_SEL) + { + rslt = set_no_motion(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_NO_MOT_SEL; + } + else + { + break; + } + } + + /* Enable step detector feature */ + if (sensor_sel & BMI2_STEP_DETECT_SEL) + { + rslt = set_step_detector(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_STEP_DETECT_SEL; + } + else + { + break; + } + } + + /* Enable step counter feature */ + if (sensor_sel & BMI2_STEP_COUNT_SEL) + { + rslt = set_step_counter(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_STEP_COUNT_SEL; + } + else + { + break; + } + } + + /* Enable step activity feature */ + if (sensor_sel & BMI2_STEP_ACT_SEL) + { + rslt = set_step_activity(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_STEP_ACT_SEL; + } + else + { + break; + } + } + + /* Enable gyroscope user gain */ + if (sensor_sel & BMI2_GYRO_GAIN_UPDATE_SEL) + { + rslt = set_gyro_user_gain(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_GYRO_GAIN_UPDATE_SEL; + } + else + { + break; + } + } + + /* Enable gyroscope self-offset correction feature */ + if (sensor_sel & BMI2_GYRO_SELF_OFF_SEL) + { + rslt = set_gyro_self_offset_corr(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_GYRO_SELF_OFF_SEL; + } + else + { + break; + } + } + + /* Enable wrist gesture feature for wearable variant */ + if (sensor_sel & BMI2_WRIST_GEST_SEL) + { + rslt = set_wrist_gesture(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_WRIST_GEST_SEL; + } + else + { + break; + } + } + + /* Enable wrist wear wake-up feature */ + if (sensor_sel & BMI2_WRIST_WEAR_WAKE_UP_SEL) + { + rslt = set_wrist_wear_wake_up(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_WRIST_WEAR_WAKE_UP_SEL; + } + else + { + break; + } + } + } + + /* Enable Advance power save if disabled while + * configuring and not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + } + + return rslt; +} + +/*! + * @brief This internal API disables the selected sensors/features. + */ +static int8_t sensor_disable(uint64_t sensor_sel, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store register values */ + uint8_t reg_data = 0; + + /* Variable to define loop */ + uint8_t loop = 1; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat = 0; + + rslt = bmi2_get_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + /* Disable accelerometer */ + if (sensor_sel & BMI2_ACCEL_SENS_SEL) + { + reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_ACC_EN); + } + + /* Disable gyroscope */ + if (sensor_sel & BMI2_GYRO_SENS_SEL) + { + reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_GYR_EN); + } + + /* Disable auxiliary sensor */ + if (sensor_sel & BMI2_AUX_SENS_SEL) + { + reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_AUX_EN); + } + + /* Disable temperature sensor */ + if (sensor_sel & BMI2_TEMP_SENS_SEL) + { + reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_TEMP_EN); + } + + /* Disable the sensors that are set in the power control register */ + if (sensor_sel & BMI2_MAIN_SENSORS) + { + rslt = bmi2_set_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); + } + } + + if ((rslt == BMI2_OK) && (sensor_sel & ~(BMI2_MAIN_SENSORS))) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if enabled */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + if (rslt == BMI2_OK) + { + while (loop--) + { + /* Disable sig-motion feature */ + if (sensor_sel & BMI2_SIG_MOTION_SEL) + { + rslt = set_sig_motion(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat &= ~BMI2_SIG_MOTION_SEL; + } + else + { + break; + } + } + + /* Disable any-motion feature */ + if (sensor_sel & BMI2_ANY_MOT_SEL) + { + rslt = set_any_motion(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat &= ~BMI2_ANY_MOT_SEL; + } + else + { + break; + } + } + + /* Disable no-motion feature */ + if (sensor_sel & BMI2_NO_MOT_SEL) + { + rslt = set_no_motion(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat &= ~BMI2_NO_MOT_SEL; + } + else + { + break; + } + } + + /* Disable step detector feature */ + if (sensor_sel & BMI2_STEP_DETECT_SEL) + { + rslt = set_step_detector(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat &= ~BMI2_STEP_DETECT_SEL; + } + else + { + break; + } + } + + /* Disable step counter feature */ + if (sensor_sel & BMI2_STEP_COUNT_SEL) + { + rslt = set_step_counter(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat &= ~BMI2_STEP_COUNT_SEL; + } + else + { + break; + } + } + + /* Disable step activity feature */ + if (sensor_sel & BMI2_STEP_ACT_SEL) + { + rslt = set_step_activity(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat &= ~BMI2_STEP_ACT_SEL; + } + else + { + break; + } + } + + /* Disable gyroscope user gain */ + if (sensor_sel & BMI2_GYRO_GAIN_UPDATE_SEL) + { + rslt = set_gyro_user_gain(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat &= ~BMI2_GYRO_GAIN_UPDATE_SEL; + } + else + { + break; + } + } + + /* Disable gyroscope self-offset correction feature */ + if (sensor_sel & BMI2_GYRO_SELF_OFF_SEL) + { + rslt = set_gyro_self_offset_corr(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat &= ~BMI2_GYRO_SELF_OFF_SEL; + } + else + { + break; + } + } + + /* Disable wrist gesture feature for wearable variant*/ + if (sensor_sel & BMI2_WRIST_GEST_SEL) + { + rslt = set_wrist_gesture(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat &= ~BMI2_WRIST_GEST_SEL; + } + else + { + break; + } + } + + /* Enable wrist wear wake-up feature */ + if (sensor_sel & BMI2_WRIST_WEAR_WAKE_UP_SEL) + { + rslt = set_wrist_wear_wake_up(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat &= ~BMI2_WRIST_WEAR_WAKE_UP_SEL; + } + else + { + break; + } + } + + /* Enable Advance power save if disabled while + * configuring and not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + } + } + + return rslt; +} + +/*! + * @brief This internal API is used to enable/disable any motion feature. + */ +static int8_t set_any_motion(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for any-motion */ + struct bmi2_feature_config any_mot_config = { 0, 0, 0 }; + + /* Search for any-motion feature and extract its configurations details */ + feat_found = bmi2_extract_input_feat_config(&any_mot_config, BMI2_ANY_MOTION, dev); + if (feat_found) + { + /* Get the configuration from the page where any-motion feature resides */ + rslt = bmi2_get_feat_config(any_mot_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of any-motion axes */ + idx = any_mot_config.start_addr + BMI2_ANY_MOT_FEAT_EN_OFFSET; + + /* Set the feature enable bit */ + feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_ANY_NO_MOT_EN, enable); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API is used to enable/disable no-motion feature. + */ +static int8_t set_no_motion(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for no-motion */ + struct bmi2_feature_config no_mot_config = { 0, 0, 0 }; + + /* Search for no-motion feature and extract its configurations details */ + feat_found = bmi2_extract_input_feat_config(&no_mot_config, BMI2_NO_MOTION, dev); + if (feat_found) + { + /* Get the configuration from the page where any/no-motion feature resides */ + rslt = bmi2_get_feat_config(no_mot_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of no-motion axes */ + idx = no_mot_config.start_addr + BMI2_NO_MOT_FEAT_EN_OFFSET; + + /* Set the feature enable bit */ + feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_ANY_NO_MOT_EN, enable); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API is used to enable/disable step detector feature. + */ +static int8_t set_step_detector(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for step detector */ + struct bmi2_feature_config step_det_config = { 0, 0, 0 }; + + /* Search for step detector feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_det_config, BMI2_STEP_DETECTOR, dev); + if (feat_found) + { + /* Get the configuration from the page where step detector feature resides */ + rslt = bmi2_get_feat_config(step_det_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of step detector */ + idx = step_det_config.start_addr + BMI2_STEP_COUNT_FEAT_EN_OFFSET; + + /* Set the feature enable bit */ + feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_STEP_DET_FEAT_EN, enable); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API is used to enable/disable step counter feature. + */ +static int8_t set_step_counter(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for step counter */ + struct bmi2_feature_config step_count_config = { 0, 0, 0 }; + + /* Search for step counter feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_count_config, BMI2_STEP_COUNTER, dev); + if (feat_found) + { + /* Get the configuration from the page where step-counter feature resides */ + rslt = bmi2_get_feat_config(step_count_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of step counter */ + idx = step_count_config.start_addr + BMI2_STEP_COUNT_FEAT_EN_OFFSET; + + /* Set the feature enable bit */ + feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_STEP_COUNT_FEAT_EN, enable); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API is used to enable/disable sig-motion feature. + */ +static int8_t set_sig_motion(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for sig-motion */ + struct bmi2_feature_config sig_mot_config = { 0, 0, 0 }; + + /* Search for sig-motion feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&sig_mot_config, BMI2_SIG_MOTION, dev); + if (feat_found) + { + /* Get the configuration from the page where sig-motion feature resides */ + rslt = bmi2_get_feat_config(sig_mot_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of sig-motion */ + idx = sig_mot_config.start_addr + BMI2_SIG_MOT_FEAT_EN_OFFSET; + + /* Set the feature enable bit */ + feat_config[idx] = BMI2_SET_BIT_POS0(feat_config[idx], BMI2_SIG_MOT_FEAT_EN, enable); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API is used to enable/disable step activity detection. + */ +static int8_t set_step_activity(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for step activity */ + struct bmi2_feature_config step_act_config = { 0, 0, 0 }; + + /* Search for step activity feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_act_config, BMI2_STEP_ACTIVITY, dev); + if (feat_found) + { + /* Get the configuration from the page where step-activity + * feature resides + */ + rslt = bmi2_get_feat_config(step_act_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of step activity */ + idx = step_act_config.start_addr + BMI2_STEP_COUNT_FEAT_EN_OFFSET; + + /* Set the feature enable bit */ + feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_STEP_ACT_FEAT_EN, enable); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gives an option to enable self-offset correction + * feature of gyroscope, either internally or by the host. + */ +static int8_t set_gyro_self_offset_corr(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for self-offset correction */ + struct bmi2_feature_config self_off_corr_cfg = { 0, 0, 0 }; + + /* Search for self-offset correction and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&self_off_corr_cfg, BMI2_GYRO_SELF_OFF, dev); + if (feat_found) + { + /* Get the configuration from the page where self-offset + * correction feature resides + */ + rslt = bmi2_get_feat_config(self_off_corr_cfg.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of self-offset correction */ + idx = self_off_corr_cfg.start_addr; + + /* Set the feature enable bit */ + feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_GYR_SELF_OFF_CORR_FEAT_EN, enable); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API enables the wrist gesture feature. + */ +static int8_t set_wrist_gesture(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for wrist gesture */ + struct bmi2_feature_config wrist_gest_cfg = { 0, 0, 0 }; + + /* Search for wrist gesture and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&wrist_gest_cfg, BMI2_WRIST_GESTURE, dev); + if (feat_found) + { + /* Get the configuration from the page where wrist gesture feature resides */ + rslt = bmi2_get_feat_config(wrist_gest_cfg.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of wrist gesture */ + idx = wrist_gest_cfg.start_addr; + + /* Set the feature enable bit */ + feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_WRIST_GEST_FEAT_EN, enable); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API enables the wrist wear wake up feature. + */ +static int8_t set_wrist_wear_wake_up(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for wrist wear wake up */ + struct bmi2_feature_config wrist_wake_up_cfg = { 0, 0, 0 }; + + /* Search for wrist wear wake up and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&wrist_wake_up_cfg, BMI2_WRIST_WEAR_WAKE_UP, dev); + if (feat_found) + { + /* Get the configuration from the page where wrist wear wake up + * feature resides + */ + rslt = bmi2_get_feat_config(wrist_wake_up_cfg.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of wrist wear wake up */ + idx = wrist_wake_up_cfg.start_addr; + + /* Set the feature enable bit */ + feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_WRIST_WEAR_WAKE_UP_FEAT_EN, enable); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API is used to enable/disable gyroscope user gain + * feature. + */ +static int8_t set_gyro_user_gain(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for gyroscope user gain */ + struct bmi2_feature_config gyr_user_gain_cfg = { 0, 0, 0 }; + + /* Search for user gain feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&gyr_user_gain_cfg, BMI2_GYRO_GAIN_UPDATE, dev); + if (feat_found) + { + /* Get the configuration from the page where user gain feature resides */ + rslt = bmi2_get_feat_config(gyr_user_gain_cfg.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of user gain */ + idx = gyr_user_gain_cfg.start_addr + BMI2_GYR_USER_GAIN_FEAT_EN_OFFSET; + + /* Set the feature enable bit */ + feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_GYR_USER_GAIN_FEAT_EN, enable); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API sets any-motion configurations like axes select, + * duration, threshold and output-configuration. + */ +static int8_t set_any_motion_config(const struct bmi2_any_motion_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to define count */ + uint8_t index = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for any motion */ + struct bmi2_feature_config any_mot_config = { 0, 0, 0 }; + + /* Copy the feature configuration address to a local pointer */ + uint16_t *data_p = (uint16_t *) (void *)feat_config; + + /* Search for any-motion feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&any_mot_config, BMI2_ANY_MOTION, dev); + if (feat_found) + { + /* Get the configuration from the page where any-motion feature resides */ + rslt = bmi2_get_feat_config(any_mot_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for any-motion select */ + idx = any_mot_config.start_addr; + + /* Get offset in words since all the features are set in words length */ + idx = idx / 2; + + /* Set duration */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_ANY_NO_MOT_DUR, config->duration); + + /* Set x-select */ + *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_ANY_NO_MOT_X_SEL, config->select_x); + + /* Set y-select */ + *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_ANY_NO_MOT_Y_SEL, config->select_y); + + /* Set z-select */ + *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_ANY_NO_MOT_Z_SEL, config->select_z); + + /* Increment offset by 1 word to set threshold and output configuration */ + idx++; + + /* Set threshold */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_ANY_NO_MOT_THRES, config->threshold); + + /* Increment offset by 1 more word to get the total length in words */ + idx++; + + /* Get total length in bytes to copy from local pointer to the array */ + idx = (uint8_t)(idx * 2) - any_mot_config.start_addr; + + /* Copy the bytes to be set back to the array */ + for (index = 0; index < idx; index++) + { + feat_config[any_mot_config.start_addr + + index] = *((uint8_t *) data_p + any_mot_config.start_addr + index); + } + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API sets no-motion configurations like axes select, + * duration, threshold and output-configuration. + */ +static int8_t set_no_motion_config(const struct bmi2_no_motion_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to define count */ + uint8_t index = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for no-motion */ + struct bmi2_feature_config no_mot_config = { 0, 0, 0 }; + + /* Copy the feature configuration address to a local pointer */ + uint16_t *data_p = (uint16_t *) (void *)feat_config; + + /* Search for no-motion feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&no_mot_config, BMI2_NO_MOTION, dev); + if (feat_found) + { + /* Get the configuration from the page where no-motion feature resides */ + rslt = bmi2_get_feat_config(no_mot_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for no-motion select */ + idx = no_mot_config.start_addr; + + /* Get offset in words since all the features are set in words length */ + idx = idx / 2; + + /* Set duration */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_ANY_NO_MOT_DUR, config->duration); + + /* Set x-select */ + *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_ANY_NO_MOT_X_SEL, config->select_x); + + /* Set y-select */ + *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_ANY_NO_MOT_Y_SEL, config->select_y); + + /* Set z-select */ + *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_ANY_NO_MOT_Z_SEL, config->select_z); + + /* Increment offset by 1 word to set threshold and output configuration */ + idx++; + + /* Set threshold */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_ANY_NO_MOT_THRES, config->threshold); + + /* Increment offset by 1 more word to get the total length in words */ + idx++; + + /* Get total length in bytes to copy from local pointer to the array */ + idx = (uint8_t)(idx * 2) - no_mot_config.start_addr; + + /* Copy the bytes to be set back to the array */ + for (index = 0; index < idx; index++) + { + feat_config[no_mot_config.start_addr + + index] = *((uint8_t *) data_p + no_mot_config.start_addr + index); + } + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API sets sig-motion configurations like block-size, + * output-configuration and other parameters. + */ +static int8_t set_sig_motion_config(const struct bmi2_sig_motion_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to define index */ + uint8_t index = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for sig-motion */ + struct bmi2_feature_config sig_mot_config = { 0, 0, 0 }; + + /* Copy the feature configuration address to a local pointer */ + uint16_t *data_p = (uint16_t *) (void *)feat_config; + + /* Search for sig-motion feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&sig_mot_config, BMI2_SIG_MOTION, dev); + if (feat_found) + { + /* Get the configuration from the page where sig-motion feature resides */ + rslt = bmi2_get_feat_config(sig_mot_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for sig-motion select */ + idx = sig_mot_config.start_addr; + + /* Get offset in words since all the features are set in words length */ + idx = idx / 2; + + /* Set parameter 1 */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_SIG_MOT_PARAM_1, config->block_size); + + /* Increment offset by 1 word to set parameter 2 */ + idx++; + + /* Set parameter 2 */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_SIG_MOT_PARAM_2, config->param_2); + + /* Increment offset by 1 word to set parameter 3 */ + idx++; + + /* Set parameter 3 */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_SIG_MOT_PARAM_3, config->param_3); + + /* Increment offset by 1 word to set parameter 4 */ + idx++; + + /* Set parameter 4 */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_SIG_MOT_PARAM_4, config->param_4); + + /* Increment offset by 1 word to set parameter 5 */ + idx++; + + /* Set parameter 5 */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_SIG_MOT_PARAM_5, config->param_5); + + /* Increment offset by 1 word to set output- configuration */ + idx++; + + /* Get total length in bytes to copy from local pointer to the array */ + idx = (uint8_t)(idx * 2) - sig_mot_config.start_addr; + + /* Copy the bytes to be set back to the array */ + for (index = 0; index < idx; index++) + { + feat_config[sig_mot_config.start_addr + + index] = *((uint8_t *) data_p + sig_mot_config.start_addr + index); + } + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API sets step counter parameter configurations. + */ +static int8_t set_step_count_params_config(const uint16_t *step_count_params, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define index */ + uint8_t index = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for step counter parameters */ + struct bmi2_feature_config step_params_config = { 0, 0, 0 }; + + /* Variable to index the page number */ + uint8_t page_idx; + + /* Variable to define the start page */ + uint8_t start_page; + + /* Variable to define start address of the parameters */ + uint8_t start_addr; + + /* Variable to define number of bytes */ + uint8_t n_bytes = (BMI2_STEP_CNT_N_PARAMS * 2); + + /* Variable to store number of pages */ + uint8_t n_pages = (n_bytes / 16); + + /* Variable to define the end page */ + uint8_t end_page; + + /* Variable to define the remaining bytes to be read */ + uint8_t remain_len; + + /* Variable to define the maximum words(16 bytes or 8 words) to be read in a page */ + uint8_t max_len = 8; + + /* Variable index bytes in a page */ + uint8_t page_byte_idx; + + /* Variable to index the parameters */ + uint8_t param_idx = 0; + + /* Copy the feature configuration address to a local pointer */ + uint16_t *data_p = (uint16_t *) (void *)feat_config; + + /* Search for step counter parameter feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_params_config, BMI2_STEP_COUNTER_PARAMS, dev); + if (feat_found) + { + /* Get the start page for the step counter parameters */ + start_page = step_params_config.page; + + /* Get the end page for the step counter parameters */ + end_page = start_page + n_pages; + + /* Get the start address for the step counter parameters */ + start_addr = step_params_config.start_addr; + + /* Get the remaining length of bytes to be read */ + remain_len = (uint8_t)((n_bytes - (n_pages * 16)) + start_addr); + for (page_idx = start_page; page_idx <= end_page; page_idx++) + { + /* Get the configuration from the respective page */ + rslt = bmi2_get_feat_config(page_idx, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Start from address 0x00 when switched to next page */ + if (page_idx > start_page) + { + start_addr = 0; + } + + /* Remaining number of words to be read in the page */ + if (page_idx == end_page) + { + max_len = (remain_len / 2); + } + + /* Get offset in words since all the features are set in words length */ + page_byte_idx = start_addr / 2; + for (; page_byte_idx < max_len;) + { + /* Set parameters 1 to 25 */ + *(data_p + page_byte_idx) = BMI2_SET_BIT_POS0(*(data_p + page_byte_idx), + BMI2_STEP_COUNT_PARAMS, + step_count_params[param_idx]); + + /* Increment offset by 1 word to set to the next parameter */ + page_byte_idx++; + + /* Increment to next parameter */ + param_idx++; + } + + /* Get total length in bytes to copy from local pointer to the array */ + page_byte_idx = (uint8_t)(page_byte_idx * 2) - step_params_config.start_addr; + + /* Copy the bytes to be set back to the array */ + for (index = 0; index < page_byte_idx; index++) + { + feat_config[step_params_config.start_addr + + index] = *((uint8_t *) data_p + step_params_config.start_addr + index); + } + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/* @brief This internal API sets step counter configurations like water-mark + * level, reset-counter and output-configuration step detector and activity. + */ +static int8_t set_step_config(const struct bmi2_step_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to define index */ + uint8_t index = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for step counter 4 */ + struct bmi2_feature_config step_count_config = { 0, 0, 0 }; + + /* Copy the feature configuration address to a local pointer */ + uint16_t *data_p = (uint16_t *) (void *)feat_config; + + /* Search for step counter feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_count_config, BMI2_STEP_COUNTER, dev); + if (feat_found) + { + /* Get the configuration from the page where step counter resides */ + rslt = bmi2_get_feat_config(step_count_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes */ + idx = step_count_config.start_addr; + + /* Get offset in words since all the features are set in words length */ + idx = idx / 2; + + /* Set water-mark level */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_STEP_COUNT_WM_LEVEL, config->watermark_level); + + /* Set reset-counter */ + *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_STEP_COUNT_RST_CNT, config->reset_counter); + + /* Increment offset by 1 word to set output + * configuration of step detector and step activity + */ + idx++; + + /* Set step buffer size */ + *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_STEP_BUFFER_SIZE, config->step_buffer_size); + + /* Increment offset by 1 more word to get the total length in words */ + idx++; + + /* Get total length in bytes to copy from local pointer to the array */ + idx = (uint8_t)(idx * 2) - step_count_config.start_addr; + + /* Copy the bytes to be set back to the array */ + for (index = 0; index < idx; index++) + { + feat_config[step_count_config.start_addr + + index] = *((uint8_t *) data_p + step_count_config.start_addr + index); + } + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API sets wrist gesture configurations like wearable-arm, + * and output-configuration. + */ +static int8_t set_wrist_gest_config(const struct bmi2_wrist_gest_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to define index */ + uint8_t index = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for wrist gesture */ + struct bmi2_feature_config wrist_gest_config = { 0, 0, 0 }; + + /* Copy the feature configuration address to a local pointer */ + uint16_t *data_p = (uint16_t *) (void *)feat_config; + + /* Search for wrist gesture feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&wrist_gest_config, BMI2_WRIST_GESTURE, dev); + if (feat_found) + { + /* Get the configuration from the page where wrist gesture feature resides */ + rslt = bmi2_get_feat_config(wrist_gest_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for gesture select */ + idx = wrist_gest_config.start_addr; + + /* Get offset in words since all the features are set in words length */ + idx = idx / 2; + + /* Set wearable arm */ + *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_WRIST_GEST_WEAR_ARM, config->wearable_arm); + + /* Increment offset by 1 more word to set minimum tilt angle (min_flick_peak) */ + idx++; + *(data_p + idx) = config->min_flick_peak; + + /* Increment offset by 1 more word to set min_flick_samples */ + idx++; + *(data_p + idx) = config->min_flick_samples; + + /* Increment offset by 1 more word to set max time within gesture moment has to be completed */ + idx++; + *(data_p + idx) = config->max_duration; + + /* Increment offset by 1 more word to get the total length in words */ + idx++; + + /* Get total length in bytes to copy from local pointer to the array */ + idx = (uint8_t)(idx * 2) - wrist_gest_config.start_addr; + + /* Copy the bytes to be set back to the array */ + for (index = 0; index < idx; index++) + { + feat_config[wrist_gest_config.start_addr + + index] = *((uint8_t *) data_p + wrist_gest_config.start_addr + index); + } + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API sets wrist wear wake-up configurations like + * output-configuration. + */ +static int8_t set_wrist_wear_wake_up_config(const struct bmi2_wrist_wear_wake_up_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to define index */ + uint8_t index = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for wrist wear wake-up */ + struct bmi2_feature_config wrist_wake_up_config = { 0, 0, 0 }; + + /* Copy the feature configuration address to a local pointer */ + uint16_t *data_p = (uint16_t *) (void *)feat_config; + + /* Search for wrist wear wake-up feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&wrist_wake_up_config, BMI2_WRIST_WEAR_WAKE_UP, dev); + if (feat_found) + { + /* Get the configuration from the page where wrist wear wake-up feature resides */ + rslt = bmi2_get_feat_config(wrist_wake_up_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for wrist wear wake-up select */ + idx = wrist_wake_up_config.start_addr; + + /* Get offset in words since all the features are set in words length */ + idx = idx / 2; + + *(data_p + idx) = config->min_angle_focus; + + /* Increment offset by 1 more word to set min_angle_nonfocus */ + idx++; + *(data_p + idx) = config->min_angle_nonfocus; + + /* Increment offset by 1 more word to set max_tilt_lr */ + idx++; + *(data_p + idx) = config->max_tilt_lr; + + /* Increment offset by 1 more word to set max_tilt_ll */ + idx++; + *(data_p + idx) = config->max_tilt_ll; + + /* Increment offset by 1 more word to set max_tilt_pd */ + idx++; + *(data_p + idx) = config->max_tilt_pd; + + /* Increment offset by 1 more word to set max_tilt_pu */ + idx++; + *(data_p + idx) = config->max_tilt_pu; + + /* Increment offset by 1 more word to get the total length in words */ + idx++; + + /* Get total length in bytes to copy from local pointer to the array */ + idx = (uint8_t)(idx * 2) - wrist_wake_up_config.start_addr; + + /* Copy the bytes to be set back to the array */ + for (index = 0; index < idx; index++) + { + feat_config[wrist_wake_up_config.start_addr + + index] = *((uint8_t *) data_p + wrist_wake_up_config.start_addr + index); + } + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets any-motion configurations like axes select, + * duration, threshold and output-configuration. + */ +static int8_t get_any_motion_config(struct bmi2_any_motion_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to define LSB */ + uint16_t lsb; + + /* Variable to define MSB */ + uint16_t msb; + + /* Variable to define a word */ + uint16_t lsb_msb; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for any-motion */ + struct bmi2_feature_config any_mot_config = { 0, 0, 0 }; + + /* Search for any-motion feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&any_mot_config, BMI2_ANY_MOTION, dev); + if (feat_found) + { + /* Get the configuration from the page where any-motion feature resides */ + rslt = bmi2_get_feat_config(any_mot_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for feature enable for any-motion */ + idx = any_mot_config.start_addr; + + /* Get word to calculate duration, x, y and z select */ + lsb = (uint16_t) feat_config[idx++]; + msb = ((uint16_t) feat_config[idx++] << 8); + lsb_msb = lsb | msb; + + /* Get duration */ + config->duration = lsb_msb & BMI2_ANY_NO_MOT_DUR_MASK; + + /* Get x-select */ + config->select_x = (lsb_msb & BMI2_ANY_NO_MOT_X_SEL_MASK) >> BMI2_ANY_NO_MOT_X_SEL_POS; + + /* Get y-select */ + config->select_y = (lsb_msb & BMI2_ANY_NO_MOT_Y_SEL_MASK) >> BMI2_ANY_NO_MOT_Y_SEL_POS; + + /* Get z-select */ + config->select_z = (lsb_msb & BMI2_ANY_NO_MOT_Z_SEL_MASK) >> BMI2_ANY_NO_MOT_Z_SEL_POS; + + /* Get word to calculate threshold, output configuration from the same word */ + lsb = (uint16_t) feat_config[idx++]; + msb = ((uint16_t) feat_config[idx++] << 8); + lsb_msb = lsb | msb; + + /* Get threshold */ + config->threshold = lsb_msb & BMI2_ANY_NO_MOT_THRES_MASK; + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets no-motion configurations like axes select, + * duration, threshold and output-configuration. + */ +static int8_t get_no_motion_config(struct bmi2_no_motion_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to define LSB */ + uint16_t lsb = 0; + + /* Variable to define MSB */ + uint16_t msb = 0; + + /* Variable to define a word */ + uint16_t lsb_msb = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for no-motion */ + struct bmi2_feature_config no_mot_config = { 0, 0, 0 }; + + /* Search for no-motion feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&no_mot_config, BMI2_NO_MOTION, dev); + if (feat_found) + { + /* Get the configuration from the page where no-motion feature resides */ + rslt = bmi2_get_feat_config(no_mot_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for feature enable for no-motion */ + idx = no_mot_config.start_addr; + + /* Get word to calculate duration, x, y and z select */ + lsb = (uint16_t) feat_config[idx++]; + msb = ((uint16_t) feat_config[idx++] << 8); + lsb_msb = lsb | msb; + + /* Get duration */ + config->duration = lsb_msb & BMI2_ANY_NO_MOT_DUR_MASK; + + /* Get x-select */ + config->select_x = (lsb_msb & BMI2_ANY_NO_MOT_X_SEL_MASK) >> BMI2_ANY_NO_MOT_X_SEL_POS; + + /* Get y-select */ + config->select_y = (lsb_msb & BMI2_ANY_NO_MOT_Y_SEL_MASK) >> BMI2_ANY_NO_MOT_Y_SEL_POS; + + /* Get z-select */ + config->select_z = (lsb_msb & BMI2_ANY_NO_MOT_Z_SEL_MASK) >> BMI2_ANY_NO_MOT_Z_SEL_POS; + + /* Get word to calculate threshold, output configuration from the same word */ + lsb = (uint16_t) feat_config[idx++]; + msb = ((uint16_t) feat_config[idx++] << 8); + lsb_msb = lsb | msb; + + /* Get threshold */ + config->threshold = lsb_msb & BMI2_ANY_NO_MOT_THRES_MASK; + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets sig-motion configurations like block-size, + * output-configuration and other parameters. + */ +static int8_t get_sig_motion_config(struct bmi2_sig_motion_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to define LSB */ + uint16_t lsb = 0; + + /* Variable to define MSB */ + uint16_t msb = 0; + + /* Variable to define a word */ + uint16_t lsb_msb = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration sig-motion */ + struct bmi2_feature_config sig_mot_config = { 0, 0, 0 }; + + /* Search for sig-motion feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&sig_mot_config, BMI2_SIG_MOTION, dev); + if (feat_found) + { + /* Get the configuration from the page where sig-motion feature resides */ + rslt = bmi2_get_feat_config(sig_mot_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for feature enable for sig-motion */ + idx = sig_mot_config.start_addr; + + /* Get word to calculate parameter 1 */ + lsb = (uint16_t) feat_config[idx++]; + msb = ((uint16_t) feat_config[idx++] << 8); + lsb_msb = lsb | msb; + + /* Get parameter 1 */ + config->block_size = lsb_msb & BMI2_SIG_MOT_PARAM_1_MASK; + + /* Get word to calculate parameter 2 */ + lsb = (uint16_t) feat_config[idx++]; + msb = ((uint16_t) feat_config[idx++] << 8); + lsb_msb = lsb | msb; + + /* Get parameter 2 */ + config->param_2 = lsb_msb & BMI2_SIG_MOT_PARAM_2_MASK; + + /* Get word to calculate parameter 3 */ + lsb = (uint16_t) feat_config[idx++]; + msb = ((uint16_t) feat_config[idx++] << 8); + lsb_msb = lsb | msb; + + /* Get parameter 3 */ + config->param_3 = lsb_msb & BMI2_SIG_MOT_PARAM_3_MASK; + + /* Get word to calculate parameter 4 */ + lsb = (uint16_t) feat_config[idx++]; + msb = ((uint16_t) feat_config[idx++] << 8); + lsb_msb = lsb | msb; + + /* Get parameter 4 */ + config->param_4 = lsb_msb & BMI2_SIG_MOT_PARAM_4_MASK; + + /* Get word to calculate parameter 5 */ + lsb = (uint16_t) feat_config[idx++]; + msb = ((uint16_t) feat_config[idx++] << 8); + lsb_msb = lsb | msb; + + /* Get parameter 5 */ + config->param_5 = lsb_msb & BMI2_SIG_MOT_PARAM_5_MASK; + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets step counter parameter configurations. + */ +static int8_t get_step_count_params_config(uint16_t *step_count_params, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Variable to define LSB */ + uint16_t lsb = 0; + + /* Variable to define MSB */ + uint16_t msb = 0; + + /* Variable to define a word */ + uint16_t lsb_msb = 0; + + /* Initialize feature configuration for step counter 1 */ + struct bmi2_feature_config step_params_config = { 0, 0, 0 }; + + /* Variable to index the page number */ + uint8_t page_idx; + + /* Variable to define the start page */ + uint8_t start_page; + + /* Variable to define start address of the parameters */ + uint8_t start_addr; + + /* Variable to define number of bytes */ + uint8_t n_bytes = (BMI2_STEP_CNT_N_PARAMS * 2); + + /* Variable to store number of pages */ + uint8_t n_pages = (n_bytes / 16); + + /* Variable to define the end page */ + uint8_t end_page; + + /* Variable to define the remaining bytes to be read */ + uint8_t remain_len; + + /* Variable to define the maximum words to be read in a page */ + uint8_t max_len = BMI2_FEAT_SIZE_IN_BYTES; + + /* Variable index bytes in a page */ + uint8_t page_byte_idx; + + /* Variable to index the parameters */ + uint8_t param_idx = 0; + + /* Search for step counter parameter feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_params_config, BMI2_STEP_COUNTER_PARAMS, dev); + if (feat_found) + { + /* Get the start page for the step counter parameters */ + start_page = step_params_config.page; + + /* Get the end page for the step counter parameters */ + end_page = start_page + n_pages; + + /* Get the start address for the step counter parameters */ + start_addr = step_params_config.start_addr; + + /* Get the remaining length of bytes to be read */ + remain_len = (uint8_t)((n_bytes - (n_pages * 16)) + start_addr); + for (page_idx = start_page; page_idx <= end_page; page_idx++) + { + /* Get the configuration from the respective page */ + rslt = bmi2_get_feat_config(page_idx, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Start from address 0x00 when switched to next page */ + if (page_idx > start_page) + { + start_addr = 0; + } + + /* Remaining number of bytes to be read in the page */ + if (page_idx == end_page) + { + max_len = remain_len; + } + + /* Get the offset */ + page_byte_idx = start_addr; + while (page_byte_idx < max_len) + { + /* Get word to calculate the parameter*/ + lsb = (uint16_t) feat_config[page_byte_idx++]; + if (page_byte_idx < max_len) + { + msb = ((uint16_t) feat_config[page_byte_idx++] << 8); + } + + lsb_msb = lsb | msb; + + /* Get parameters 1 to 25 */ + step_count_params[param_idx] = lsb_msb & BMI2_STEP_COUNT_PARAMS_MASK; + + /* Increment to next parameter */ + param_idx++; + } + } + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets step counter/detector/activity configurations. + */ +static int8_t get_step_config(struct bmi2_step_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to define LSB */ + uint16_t lsb = 0; + + /* Variable to define MSB */ + uint16_t msb = 0; + + /* Variable to define a word */ + uint16_t lsb_msb = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for step counter */ + struct bmi2_feature_config step_count_config = { 0, 0, 0 }; + + /* Search for step counter 4 feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_count_config, BMI2_STEP_COUNTER, dev); + if (feat_found) + { + /* Get the configuration from the page where step counter 4 parameter resides */ + rslt = bmi2_get_feat_config(step_count_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for feature enable for step counter/detector/activity */ + idx = step_count_config.start_addr; + + /* Get word to calculate water-mark level and reset counter */ + lsb = (uint16_t) feat_config[idx++]; + msb = ((uint16_t) feat_config[idx++] << 8); + lsb_msb = lsb | msb; + + /* Get water-mark level */ + config->watermark_level = lsb_msb & BMI2_STEP_COUNT_WM_LEVEL_MASK; + + /* Get reset counter */ + config->reset_counter = (lsb_msb & BMI2_STEP_COUNT_RST_CNT_MASK) >> BMI2_STEP_COUNT_RST_CNT_POS; + + /* Get word to calculate output configuration of step detector and activity */ + lsb = (uint16_t) feat_config[idx++]; + msb = ((uint16_t) feat_config[idx++] << 8); + lsb_msb = lsb | msb; + + config->step_buffer_size = (lsb_msb & BMI2_STEP_BUFFER_SIZE_MASK) >> BMI2_STEP_BUFFER_SIZE_POS; + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets wrist gesture configurations like wearable-arm, + * and output-configuration. + */ +static int8_t get_wrist_gest_config(struct bmi2_wrist_gest_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for wrist gesture */ + struct bmi2_feature_config wrist_gest_config = { 0, 0, 0 }; + + /* Copy the feature configuration address to a local pointer */ + uint16_t *data_p = (uint16_t *) (void *)feat_config; + + /* Search for wrist gesture feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&wrist_gest_config, BMI2_WRIST_GESTURE, dev); + if (feat_found) + { + /* Get the configuration from the page where wrist gesture feature resides */ + rslt = bmi2_get_feat_config(wrist_gest_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for wrist gesture select */ + idx = wrist_gest_config.start_addr; + + /* Get offset in words since all the features are set in words length */ + idx = idx / 2; + + /* Get wearable arm */ + config->wearable_arm = (*(data_p + idx) & BMI2_WRIST_GEST_WEAR_ARM_MASK) >> BMI2_WRIST_GEST_WEAR_ARM_POS; + + /* Increment the offset by 1 word to get min_flick_peak */ + idx++; + config->min_flick_peak = *(data_p + idx); + + /* Increment the offset by 1 word to get min_flick_samples */ + idx++; + config->min_flick_samples = *(data_p + idx); + + /* Increment the offset by 1 word to get max_duration */ + idx++; + config->max_duration = *(data_p + idx); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets wrist wear wake-up configurations like + * output-configuration. + */ +static int8_t get_wrist_wear_wake_up_config(struct bmi2_wrist_wear_wake_up_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for wrist wear wake-up */ + struct bmi2_feature_config wrist_wake_up_config = { 0, 0, 0 }; + + /* Copy the feature configuration address to a local pointer */ + uint16_t *data_p = (uint16_t *) (void *)feat_config; + + /* Search for wrist wear wake-up feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&wrist_wake_up_config, BMI2_WRIST_WEAR_WAKE_UP, dev); + if (feat_found) + { + /* Get the configuration from the page where wrist wear wake-up feature resides */ + rslt = bmi2_get_feat_config(wrist_wake_up_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for wrist wear wake-up select */ + idx = wrist_wake_up_config.start_addr; + + /* Get offset in words since all the features are set in words length */ + idx = idx / 2; + + config->min_angle_focus = *(data_p + idx); + + /* Increment the offset value by 1 word to get min_angle_nonfocus */ + idx++; + config->min_angle_nonfocus = *(data_p + idx); + + /* Increment the offset value by 1 word to get max_tilt_lr */ + idx++; + config->max_tilt_lr = *(data_p + idx); + + /* Increment the offset value by 1 word to get max_tilt_ll */ + idx++; + config->max_tilt_ll = *(data_p + idx); + + /* Increment the offset value by 1 word to get max_tilt_pd */ + idx++; + config->max_tilt_pd = *(data_p + idx); + + /* Increment the offset value by 1 word to get max_tilt_pu */ + idx++; + config->max_tilt_pu = *(data_p + idx); + + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets the output values of the wrist gesture. + */ +static int8_t get_wrist_gest_status(uint8_t *wrist_gest, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variables to define index */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature output for wrist gesture */ + struct bmi2_feature_config wrist_gest_out_config = { 0, 0, 0 }; + + /* Search for wrist gesture feature and extract its configuration details */ + feat_found = extract_output_feat_config(&wrist_gest_out_config, BMI2_WRIST_GESTURE, dev); + if (feat_found) + { + /* Get the feature output configuration for wrist gesture */ + rslt = bmi2_get_feat_config(wrist_gest_out_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for wrist gesture output */ + idx = wrist_gest_out_config.start_addr; + + /* Get the wrist gesture output */ + *wrist_gest = feat_config[idx]; + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets the output values of step counter. + */ +static int8_t get_step_counter_output(uint32_t *step_count, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variables to define index */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature output for step counter */ + struct bmi2_feature_config step_cnt_out_config = { 0, 0, 0 }; + + /* Search for step counter output feature and extract its configuration details */ + feat_found = extract_output_feat_config(&step_cnt_out_config, BMI2_STEP_COUNTER, dev); + if (feat_found) + { + /* Get the feature output configuration for step-counter */ + rslt = bmi2_get_feat_config(step_cnt_out_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for step counter output */ + idx = step_cnt_out_config.start_addr; + + /* Get the step counter output in 4 bytes */ + *step_count = (uint32_t) feat_config[idx++]; + *step_count |= ((uint32_t) feat_config[idx++] << 8); + *step_count |= ((uint32_t) feat_config[idx++] << 16); + *step_count |= ((uint32_t) feat_config[idx++] << 24); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets the error status related to NVM. + */ +static int8_t get_nvm_error_status(struct bmi2_nvm_err_status *nvm_err_stat, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variables to define index */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature output for NVM error status */ + struct bmi2_feature_config nvm_err_cfg = { 0, 0, 0 }; + + /* Search for NVM error status feature and extract its configuration details */ + feat_found = extract_output_feat_config(&nvm_err_cfg, BMI2_NVM_STATUS, dev); + if (feat_found) + { + /* Get the feature output configuration for NVM error status */ + rslt = bmi2_get_feat_config(nvm_err_cfg.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for NVM error status */ + idx = nvm_err_cfg.start_addr; + + /* Increment index to get the error status */ + idx++; + + /* Error when NVM load action fails */ + nvm_err_stat->load_error = BMI2_GET_BIT_POS0(feat_config[idx], BMI2_NVM_LOAD_ERR_STATUS); + + /* Error when NVM program action fails */ + nvm_err_stat->prog_error = BMI2_GET_BITS(feat_config[idx], BMI2_NVM_PROG_ERR_STATUS); + + /* Error when NVM erase action fails */ + nvm_err_stat->erase_error = BMI2_GET_BITS(feat_config[idx], BMI2_NVM_ERASE_ERR_STATUS); + + /* Error when NVM program limit is exceeded */ + nvm_err_stat->exceed_error = BMI2_GET_BITS(feat_config[idx], BMI2_NVM_END_EXCEED_STATUS); + + /* Error when NVM privilege mode is not acquired */ + nvm_err_stat->privil_error = BMI2_GET_BITS(feat_config[idx], BMI2_NVM_PRIV_ERR_STATUS); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API is used to get enable status of gyroscope user gain + * update. + */ +static int8_t get_user_gain_upd_status(uint8_t *status, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Variable to check APS status */ + uint8_t aps_stat = 0; + + /* Initialize feature configuration for gyroscope user gain */ + struct bmi2_feature_config gyr_user_gain_cfg = { 0, 0, 0 }; + + /* Search for user gain feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&gyr_user_gain_cfg, BMI2_GYRO_GAIN_UPDATE, dev); + if (feat_found) + { + /* Disable advance power save */ + aps_stat = dev->aps_status; + if (aps_stat == BMI2_ENABLE) + { + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + if (rslt == BMI2_OK) + { + /* Get the configuration from the page where user gain feature resides */ + rslt = bmi2_get_feat_config(gyr_user_gain_cfg.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of user gain */ + idx = gyr_user_gain_cfg.start_addr + BMI2_GYR_USER_GAIN_FEAT_EN_OFFSET; + + /* Set the feature enable status */ + *status = BMI2_GET_BITS(feat_config[idx], BMI2_GYR_USER_GAIN_FEAT_EN); + } + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + /* Enable Advance power save if disabled while configuring and not when already disabled */ + if ((rslt == BMI2_OK) && (aps_stat == BMI2_ENABLE)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + + return rslt; +} + +/*! + * @brief This internal API gets the output values of step activity. + */ +static int8_t get_step_activity_output(uint8_t *step_act, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variables to define index */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature output for step activity */ + struct bmi2_feature_config step_act_out_config = { 0, 0, 0 }; + + /* Search for step activity output feature and extract its configuration details */ + feat_found = extract_output_feat_config(&step_act_out_config, BMI2_STEP_ACTIVITY, dev); + if (feat_found) + { + /* Get the feature output configuration for step-activity */ + rslt = bmi2_get_feat_config(step_act_out_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for step activity output */ + idx = step_act_out_config.start_addr; + + /* Get the step activity output */ + *step_act = feat_config[idx]; + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets the error status related to virtual frames. + */ +static int8_t get_vfrm_error_status(struct bmi2_vfrm_err_status *vfrm_err_stat, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variables to define index */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature output for VFRM error status */ + struct bmi2_feature_config vfrm_err_cfg = { 0, 0, 0 }; + + /* Search for VFRM error status feature and extract its configuration details */ + feat_found = extract_output_feat_config(&vfrm_err_cfg, BMI2_VFRM_STATUS, dev); + if (feat_found) + { + /* Get the feature output configuration for VFRM error status */ + rslt = bmi2_get_feat_config(vfrm_err_cfg.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for VFRM error status */ + idx = vfrm_err_cfg.start_addr; + + /* Increment index to get the error status */ + idx++; + + /* Internal error while acquiring lock for FIFO */ + vfrm_err_stat->lock_error = BMI2_GET_BITS(feat_config[idx], BMI2_VFRM_LOCK_ERR_STATUS); + + /* Internal error while writing byte into FIFO */ + vfrm_err_stat->write_error = BMI2_GET_BITS(feat_config[idx], BMI2_VFRM_WRITE_ERR_STATUS); + + /* Internal error while writing into FIFO */ + vfrm_err_stat->fatal_error = BMI2_GET_BITS(feat_config[idx], BMI2_VFRM_FATAL_ERR_STATUS); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API enables/disables compensation of the gain defined + * in the GAIN register. + */ +static int8_t enable_gyro_gain(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define register data */ + uint8_t reg_data = 0; + + rslt = bmi2_get_regs(BMI2_GYR_OFF_COMP_6_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + reg_data = BMI2_SET_BITS(reg_data, BMI2_GYR_GAIN_EN, enable); + rslt = bmi2_set_regs(BMI2_GYR_OFF_COMP_6_ADDR, ®_data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This internal API is used to extract the output feature configuration + * details from the look-up table. + */ +static uint8_t extract_output_feat_config(struct bmi2_feature_config *feat_output, + uint8_t type, + const struct bmi2_dev *dev) +{ + /* Variable to define loop */ + uint8_t loop = 0; + + /* Variable to set flag */ + uint8_t feat_found = BMI2_FALSE; + + /* Search for the output feature from the output configuration array */ + while (loop < dev->out_sens) + { + if (dev->feat_output[loop].type == type) + { + *feat_output = dev->feat_output[loop]; + feat_found = BMI2_TRUE; + break; + } + + loop++; + } + + /* Return flag */ + return feat_found; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270.h b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270.h new file mode 100644 index 0000000000..f42676d65e --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270.h @@ -0,0 +1,422 @@ +/** +* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. +* +* BSD-3-Clause +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* +* 3. Neither the name of the copyright holder nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, +* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING +* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +* @file bmi270.h +* @date 2020-11-04 +* @version v2.63.1 +* +*/ + +/** + * \ingroup bmi2xy + * \defgroup bmi270 BMI270 + * @brief Sensor driver for BMI270 sensor + */ + +#ifndef BMI270_H_ +#define BMI270_H_ + +/*! CPP guard */ +#ifdef __cplusplus +extern "C" { +#endif + +/***************************************************************************/ + +/*! Header files + ****************************************************************************/ +#include "bmi2.h" + +/***************************************************************************/ + +/*! Macro definitions + ****************************************************************************/ + +/*! @name BMI270 Chip identifier */ +#define BMI270_CHIP_ID UINT8_C(0x24) + +/*! @name BMI270 feature input start addresses */ +#define BMI270_CONFIG_ID_STRT_ADDR UINT8_C(0x00) +#define BMI270_MAX_BURST_LEN_STRT_ADDR UINT8_C(0x02) +#define BMI270_CRT_GYRO_SELF_TEST_STRT_ADDR UINT8_C(0x03) +#define BMI270_ABORT_STRT_ADDR UINT8_C(0x03) +#define BMI270_AXIS_MAP_STRT_ADDR UINT8_C(0x04) +#define BMI270_GYRO_SELF_OFF_STRT_ADDR UINT8_C(0x05) +#define BMI270_NVM_PROG_PREP_STRT_ADDR UINT8_C(0x05) +#define BMI270_GYRO_GAIN_UPDATE_STRT_ADDR UINT8_C(0x06) +#define BMI270_ANY_MOT_STRT_ADDR UINT8_C(0x0C) +#define BMI270_NO_MOT_STRT_ADDR UINT8_C(0x00) +#define BMI270_SIG_MOT_STRT_ADDR UINT8_C(0x04) +#define BMI270_STEP_CNT_1_STRT_ADDR UINT8_C(0x00) +#define BMI270_STEP_CNT_4_STRT_ADDR UINT8_C(0x02) +#define BMI270_WRIST_GEST_STRT_ADDR UINT8_C(0x06) +#define BMI270_WRIST_WEAR_WAKE_UP_STRT_ADDR UINT8_C(0x00) + +/*! @name BMI270 feature output start addresses */ +#define BMI270_STEP_CNT_OUT_STRT_ADDR UINT8_C(0x00) +#define BMI270_STEP_ACT_OUT_STRT_ADDR UINT8_C(0x04) +#define BMI270_WRIST_GEST_OUT_STRT_ADDR UINT8_C(0x06) +#define BMI270_GYR_USER_GAIN_OUT_STRT_ADDR UINT8_C(0x08) +#define BMI270_GYRO_CROSS_SENSE_STRT_ADDR UINT8_C(0x0C) +#define BMI270_NVM_VFRM_OUT_STRT_ADDR UINT8_C(0x0E) + +/*! @name Defines maximum number of pages */ +#define BMI270_MAX_PAGE_NUM UINT8_C(8) + +/*! @name Defines maximum number of feature input configurations */ +#define BMI270_MAX_FEAT_IN UINT8_C(17) + +/*! @name Defines maximum number of feature outputs */ +#define BMI270_MAX_FEAT_OUT UINT8_C(7) + +/*! @name Mask definitions for feature interrupt status bits */ +#define BMI270_SIG_MOT_STATUS_MASK UINT8_C(0x01) +#define BMI270_STEP_CNT_STATUS_MASK UINT8_C(0x02) +#define BMI270_STEP_ACT_STATUS_MASK UINT8_C(0x04) +#define BMI270_WRIST_WAKE_UP_STATUS_MASK UINT8_C(0x08) +#define BMI270_WRIST_GEST_STATUS_MASK UINT8_C(0x10) +#define BMI270_NO_MOT_STATUS_MASK UINT8_C(0x20) +#define BMI270_ANY_MOT_STATUS_MASK UINT8_C(0x40) + +/*! @name Mask definitions for feature interrupt mapping bits */ +#define BMI270_INT_SIG_MOT_MASK UINT8_C(0x01) +#define BMI270_INT_STEP_COUNTER_MASK UINT8_C(0x02) +#define BMI270_INT_STEP_DETECTOR_MASK UINT8_C(0x02) +#define BMI270_INT_STEP_ACT_MASK UINT8_C(0x04) +#define BMI270_INT_WRIST_WEAR_WAKEUP_MASK UINT8_C(0x08) +#define BMI270_INT_WRIST_GEST_MASK UINT8_C(0x10) +#define BMI270_INT_NO_MOT_MASK UINT8_C(0x20) +#define BMI270_INT_ANY_MOT_MASK UINT8_C(0x40) + +/*! @name Defines maximum number of feature interrupts */ +#define BMI270_MAX_INT_MAP UINT8_C(8) + +/***************************************************************************/ + +/*! BMI270 User Interface function prototypes + ****************************************************************************/ + +/** + * \ingroup bmi270 + * \defgroup bmi270ApiInit Initialization + * @brief Initialize the sensor and device structure + */ + +/*! + * \ingroup bmi270ApiInit + * \page bmi270_api_bmi270_init bmi270_init + * \code + * int8_t bmi270_init(struct bmi2_dev *dev); + * \endcode + * @details This API: + * 1) updates the device structure with address of the configuration file. + * 2) Initializes BMI270 sensor. + * 3) Writes the configuration file. + * 4) Updates the feature offset parameters in the device structure. + * 5) Updates the maximum number of pages, in the device structure. + * + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_init(struct bmi2_dev *dev); + +/** + * \ingroup bmi270 + * \defgroup bmi270ApiSensor Feature Set + * @brief Enable / Disable features of the sensor + */ + +/*! + * \ingroup bmi270ApiSensor + * \page bmi270_api_bmi270_sensor_enable bmi270_sensor_enable + * \code + * int8_t bmi270_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); + * \endcode + * @details This API selects the sensors/features to be enabled. + * + * @param[in] sens_list : Pointer to select the sensor/feature. + * @param[in] n_sens : Number of sensors selected. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @note Sensors/features that can be enabled. + * + *@verbatim + * sens_list | Values + * ----------------------------|----------- + * BMI2_ACCEL | 0 + * BMI2_GYRO | 1 + * BMI2_AUX | 2 + * BMI2_SIG_MOTION | 3 + * BMI2_ANY_MOTION | 4 + * BMI2_NO_MOTION | 5 + * BMI2_STEP_DETECTOR | 6 + * BMI2_STEP_COUNTER | 7 + * BMI2_STEP_ACTIVITY | 8 + * BMI2_GYRO_GAIN_UPDATE | 9 + * BMI2_WRIST_GESTURE | 19 + * BMI2_WRIST_WEAR_WAKE_UP | 20 + * BMI2_GYRO_SELF_OFF | 33 + *@endverbatim + * + * @note : + * example uint8_t sens_list[2] = {BMI2_ACCEL, BMI2_GYRO}; + * uint8_t n_sens = 2; + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); + +/*! + * \ingroup bmi270ApiSensor + * \page bmi270_api_bmi270_sensor_disable bmi270_sensor_disable + * \code + * int8_t bmi270_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); + * \endcode + * @details This API selects the sensors/features to be disabled. + * + * @param[in] sens_list : Pointer to select the sensor/feature. + * @param[in] n_sens : Number of sensors selected. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @note Sensors/features that can be disabled. + * + *@verbatim + * sens_list | Values + * ----------------------------|----------- + * BMI2_ACCEL | 0 + * BMI2_GYRO | 1 + * BMI2_AUX | 2 + * BMI2_SIG_MOTION | 3 + * BMI2_ANY_MOTION | 4 + * BMI2_NO_MOTION | 5 + * BMI2_STEP_DETECTOR | 6 + * BMI2_STEP_COUNTER | 7 + * BMI2_STEP_ACTIVITY | 8 + * BMI2_GYRO_GAIN_UPDATE | 9 + * BMI2_WRIST_GESTURE | 19 + * BMI2_WRIST_WEAR_WAKE_UP | 20 + * BMI2_GYRO_SELF_OFF | 33 + *@endverbatim + * + * @note : + * example uint8_t sens_list[2] = {BMI2_ACCEL, BMI2_GYRO}; + * uint8_t n_sens = 2; + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); + +/** + * \ingroup bmi270 + * \defgroup bmi270ApiSensorC Sensor Configuration + * @brief Enable / Disable feature configuration of the sensor + */ + +/*! + * \ingroup bmi270ApiSensorC + * \page bmi270_api_bmi270_set_sensor_config bmi270_set_sensor_config + * \code + * int8_t bmi270_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); + * \endcode + * @details This API sets the sensor/feature configuration. + * + * @param[in] sens_cfg : Structure instance of bmi2_sens_config. + * @param[in] n_sens : Number of sensors selected. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @note Sensors/features that can be configured + * + *@verbatim + * sens_list | Values + * ----------------------------|----------- + * BMI2_SIG_MOTION | 3 + * BMI2_ANY_MOTION | 4 + * BMI2_NO_MOTION | 5 + * BMI2_STEP_DETECTOR | 6 + * BMI2_STEP_COUNTER | 7 + * BMI2_STEP_ACTIVITY | 8 + * BMI2_WRIST_GESTURE | 19 + * BMI2_WRIST_WEAR_WAKE_UP | 20 + * BMI2_STEP_COUNTER_PARAMS | 28 + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); + +/*! + * \ingroup bmi270ApiSensorC + * \page bmi270_api_bmi270_get_sensor_config bmi270_get_sensor_config + * \code + * int8_t bmi270_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); + * \endcode + * @details This API gets the sensor/feature configuration. + * + * @param[in] sens_cfg : Structure instance of bmi2_sens_config. + * @param[in] n_sens : Number of sensors selected. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @note Sensors/features whose configurations can be read. + * + *@verbatim + * sens_list | Values + * ----------------------------|----------- + * BMI2_SIG_MOTION | 3 + * BMI2_ANY_MOTION | 4 + * BMI2_NO_MOTION | 5 + * BMI2_STEP_DETECTOR | 6 + * BMI2_STEP_COUNTER | 7 + * BMI2_STEP_ACTIVITY | 8 + * BMI2_WRIST_GESTURE | 19 + * BMI2_WRIST_WEAR_WAKE_UP | 20 + * BMI2_STEP_COUNTER_PARAMS | 28 + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); + +/** + * \ingroup bmi270 + * \defgroup bmi270ApiSensorD Sensor Data + * @brief Get sensor data + */ + +/*! + * \ingroup bmi270ApiSensorD + * \page bmi270_api_bmi270_get_sensor_data bmi270_get_sensor_data + * \code + * int8_t bmi270_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev); + * \endcode + * @details This API gets the sensor/feature data for accelerometer, gyroscope, + * auxiliary sensor, step counter, high-g, gyroscope user-gain update, + * orientation, gyroscope cross sensitivity and error status for NVM and VFRM. + * + * @param[out] sensor_data : Structure instance of bmi2_sensor_data. + * @param[in] n_sens : Number of sensors selected. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @note Sensors/features whose data can be read + * + *@verbatim + * sens_list | Values + * ----------------------------|----------- + * BMI2_STEP_COUNTER | 7 + * BMI2_STEP_ACTIVITY | 8 + * BMI2_WRIST_GESTURE | 19 + * BMI2_NVM_STATUS | 38 + * BMI2_VFRM_STATUS | 39 + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev); + +/** + * \ingroup bmi270 + * \defgroup bmi270ApiGyroUG Gyro User Gain + * @brief Set / Get Gyro User Gain of the sensor + */ + +/*! + * \ingroup bmi270ApiGyroUG + * \page bmi270_api_bmi270_update_gyro_user_gain bmi270_update_gyro_user_gain + * \code + * int8_t bmi270_update_gyro_user_gain(const struct bmi2_gyro_user_gain_config *user_gain, struct bmi2_dev *dev); + * \endcode + * @details This API updates the gyroscope user-gain. + * + * @param[in] user_gain : Structure that stores user-gain configurations. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_update_gyro_user_gain(const struct bmi2_gyro_user_gain_config *user_gain, struct bmi2_dev *dev); + +/*! + * \ingroup bmi270ApiGyroUG + * \page bmi270_api_bmi270_read_gyro_user_gain bmi270_read_gyro_user_gain + * \code + * int8_t bmi270_read_gyro_user_gain(struct bmi2_gyro_user_gain_data *gyr_usr_gain, const struct bmi2_dev *dev); + * \endcode + * @details This API reads the compensated gyroscope user-gain values. + * + * @param[out] gyr_usr_gain : Structure that stores gain values. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_read_gyro_user_gain(struct bmi2_gyro_user_gain_data *gyr_usr_gain, struct bmi2_dev *dev); + +/*! + * \ingroup bmi270ApiInt + * \page bmi270_api_bmi270_map_feat_int bmi270_map_feat_int + * \code + * int8_t bmi270_map_feat_int(const struct bmi2_sens_int_config *sens_int, uint8_t n_sens, struct bmi2_dev *dev) + * \endcode + * @details This API maps/unmaps feature interrupts to that of interrupt pins. + * + * @param[in] sens_int : Structure instance of bmi2_sens_int_config. + * @param[in] n_sens : Number of interrupts to be mapped. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_map_feat_int(const struct bmi2_sens_int_config *sens_int, uint8_t n_sens, struct bmi2_dev *dev); + +/******************************************************************************/ +/*! @name C++ Guard Macros */ +/******************************************************************************/ +#ifdef __cplusplus +} +#endif /* End of CPP guard */ + +#endif /* BMI270_H_ */ diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_context.c b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_context.c new file mode 100644 index 0000000000..4f6e300351 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_context.c @@ -0,0 +1,3122 @@ +/** +* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. +* +* BSD-3-Clause +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* +* 3. Neither the name of the copyright holder nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, +* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING +* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +* @file bmi270_context.c +* @date 2020-11-04 +* @version v2.63.1 +* +*/ + +/***************************************************************************/ + +/*! Header files + ****************************************************************************/ +#include "bmi270_context.h" + +/***************************************************************************/ + +/*! Global Variable + ****************************************************************************/ + +/*! @name Global array that stores the configuration file of BMI270_CONTEXT */ +const uint8_t bmi270_context_config_file[] = { + 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x00, 0xb0, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0xc9, + 0x01, 0x80, 0x2e, 0xe2, 0x00, 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x77, 0xb0, 0x50, 0x30, 0x21, 0x2e, 0x59, 0xf5, + 0x10, 0x30, 0x21, 0x2e, 0x6a, 0xf5, 0x80, 0x2e, 0xaf, 0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0x09, 0x01, 0x00, 0x22, + 0x00, 0x76, 0x00, 0x00, 0x10, 0x00, 0x10, 0xd1, 0x00, 0xcb, 0xa7, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0xfd, 0x2d, 0x2c, 0x56, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x02, 0x08, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x04, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1a, 0x24, 0x22, 0x00, 0x80, 0x2e, 0x48, 0x02, 0x01, 0x2e, 0x49, + 0xf1, 0x0b, 0xbc, 0x10, 0x50, 0x0f, 0xb8, 0x00, 0x90, 0xfb, 0x7f, 0x07, 0x2f, 0x03, 0x2e, 0x21, 0xf2, 0x02, 0x31, + 0x4a, 0x0a, 0x23, 0x2e, 0x21, 0xf2, 0x09, 0x2c, 0x00, 0x30, 0x98, 0x2e, 0x0e, 0xc7, 0x03, 0x2e, 0x21, 0xf2, 0xf2, + 0x3e, 0x4a, 0x08, 0x23, 0x2e, 0x21, 0xf2, 0xfb, 0x6f, 0xf0, 0x5f, 0xb8, 0x2e, 0x13, 0x52, 0x00, 0x2e, 0x60, 0x40, + 0x41, 0x40, 0x0d, 0xbc, 0x98, 0xbc, 0xc0, 0x2e, 0x01, 0x0a, 0x0f, 0xb8, 0x43, 0x86, 0x25, 0x40, 0x04, 0x40, 0xd8, + 0xbe, 0x2c, 0x0b, 0x22, 0x11, 0x54, 0x42, 0x03, 0x80, 0x4b, 0x0e, 0xf6, 0x2f, 0xb8, 0x2e, 0x20, 0x50, 0xe7, 0x7f, + 0xf6, 0x7f, 0x46, 0x30, 0x0f, 0x2e, 0xa4, 0xf1, 0xbe, 0x09, 0x80, 0xb3, 0x06, 0x2f, 0x0d, 0x2e, 0x84, 0x00, 0x84, + 0xaf, 0x02, 0x2f, 0x16, 0x30, 0x2d, 0x2e, 0x7b, 0x00, 0x86, 0x30, 0x2d, 0x2e, 0x60, 0xf5, 0xf6, 0x6f, 0xe7, 0x6f, + 0xe0, 0x5f, 0xc8, 0x2e, 0x80, 0x2e, 0xfb, 0x00, 0x00, 0x30, 0xc0, 0x2e, 0x21, 0x2e, 0x8d, 0x00, 0x44, 0x47, 0x99, + 0x00, 0xff, 0x3f, 0x00, 0x0c, 0xff, 0x0f, 0x00, 0x04, 0xc0, 0x00, 0x5b, 0xf5, 0x90, 0x00, 0x1e, 0xf2, 0xfd, 0xf5, + 0x8e, 0x00, 0x96, 0x00, 0x96, 0x00, 0xe0, 0x00, 0x19, 0xf4, 0x66, 0xf5, 0x00, 0x18, 0x64, 0xf5, 0x9d, 0x00, 0x7f, + 0x00, 0x81, 0x00, 0xae, 0x00, 0xff, 0xfb, 0x21, 0x02, 0x00, 0x10, 0x00, 0x40, 0xff, 0x00, 0x00, 0x80, 0xff, 0x7f, + 0x54, 0x0f, 0xeb, 0x00, 0x7f, 0xff, 0xc2, 0xf5, 0x68, 0xf7, 0xb3, 0xf1, 0x4e, 0x0f, 0x42, 0x0f, 0x48, 0x0f, 0x80, + 0x00, 0x67, 0x0f, 0x58, 0xf7, 0x5b, 0xf7, 0x6a, 0x0f, 0x86, 0x00, 0x59, 0x0f, 0x6c, 0x0f, 0xc6, 0xf1, 0x66, 0x0f, + 0x6c, 0xf7, 0x00, 0xe0, 0x00, 0xff, 0xd1, 0xf5, 0x6e, 0x0f, 0x71, 0x0f, 0xff, 0x03, 0x00, 0xfc, 0xf0, 0x3f, 0xb9, + 0x00, 0x2d, 0xf5, 0xca, 0xf5, 0x8a, 0x00, 0x00, 0x08, 0x71, 0x7d, 0xfe, 0xc0, 0x03, 0x3f, 0x05, 0x3e, 0x49, 0x01, + 0x92, 0x02, 0xf5, 0xd6, 0xe8, 0x63, 0xd3, 0xf8, 0x2e, 0x07, 0x5c, 0xce, 0xa5, 0x67, 0x28, 0x02, 0x4e, 0x01, 0x00, + 0xf0, 0x33, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x15, 0x50, 0x10, + 0x50, 0x17, 0x52, 0x05, 0x2e, 0x7d, 0x00, 0xfb, 0x7f, 0x00, 0x2e, 0x13, 0x40, 0x93, 0x42, 0x41, 0x0e, 0xfb, 0x2f, + 0x98, 0x2e, 0x91, 0x03, 0x98, 0x2e, 0x87, 0xcf, 0x01, 0x2e, 0x89, 0x00, 0x00, 0xb2, 0x08, 0x2f, 0x01, 0x2e, 0x69, + 0xf7, 0xb1, 0x3f, 0x01, 0x08, 0x01, 0x30, 0x23, 0x2e, 0x89, 0x00, 0x21, 0x2e, 0x69, 0xf7, 0xfb, 0x6f, 0xf0, 0x5f, + 0xb8, 0x2e, 0xa0, 0x50, 0x80, 0x7f, 0xe7, 0x7f, 0xd5, 0x7f, 0xc4, 0x7f, 0xb3, 0x7f, 0xa2, 0x7f, 0x91, 0x7f, 0xf6, + 0x7f, 0x7b, 0x7f, 0x00, 0x2e, 0x01, 0x2e, 0x60, 0xf5, 0x60, 0x7f, 0x98, 0x2e, 0xce, 0x00, 0x62, 0x6f, 0x01, 0x32, + 0x91, 0x08, 0x80, 0xb2, 0x11, 0x2f, 0x00, 0xb2, 0x03, 0x2f, 0x05, 0x2e, 0x18, 0x00, 0x80, 0x90, 0x09, 0x2f, 0x60, + 0x7f, 0x98, 0x2e, 0xf9, 0x00, 0x23, 0x50, 0x01, 0x32, 0x01, 0x42, 0x02, 0x86, 0x60, 0x6f, 0x02, 0x30, 0xc2, 0x42, + 0x23, 0x2e, 0x60, 0xf5, 0x00, 0x90, 0x00, 0x30, 0x01, 0x2f, 0x21, 0x2e, 0x7a, 0x00, 0xf6, 0x6f, 0x91, 0x6f, 0xa2, + 0x6f, 0xb3, 0x6f, 0xc4, 0x6f, 0xd5, 0x6f, 0xe7, 0x6f, 0x7b, 0x6f, 0x80, 0x6f, 0x60, 0x5f, 0xc8, 0x2e, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x2d, 0x01, 0xd4, 0x7b, 0x3b, + 0x01, 0xdb, 0x7a, 0x04, 0x00, 0x3f, 0x7b, 0xcd, 0x6c, 0xc3, 0x04, 0x85, 0x09, 0xc3, 0x04, 0xec, 0xe6, 0x0c, 0x46, + 0x01, 0x00, 0x27, 0x00, 0x19, 0x00, 0x96, 0x00, 0xa0, 0x00, 0x01, 0x00, 0x0c, 0x00, 0xf0, 0x3c, 0x00, 0x01, 0x01, + 0x00, 0x03, 0x00, 0x01, 0x00, 0x0e, 0x00, 0x00, 0x00, 0x01, 0x00, 0x07, 0x09, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x01, 0x00, 0xe1, 0x06, 0x66, 0x0a, 0x0a, 0x00, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x50, 0x98, 0x2e, 0xd7, 0x0e, 0x50, 0x32, 0x98, 0x2e, + 0x48, 0x03, 0x10, 0x30, 0x21, 0x2e, 0x21, 0xf2, 0x00, 0x30, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0x00, 0x2e, 0x01, + 0x80, 0x06, 0xa2, 0xfb, 0x2f, 0x01, 0x2e, 0x9c, 0x00, 0x00, 0xb2, 0x10, 0x2f, 0x01, 0x2e, 0x18, 0x00, 0x00, 0xb2, + 0x0c, 0x2f, 0x01, 0x54, 0x03, 0x52, 0x01, 0x50, 0x98, 0x2e, 0xc2, 0xc0, 0x98, 0x2e, 0xf5, 0xb0, 0x01, 0x50, 0x98, + 0x2e, 0xd5, 0xb6, 0x10, 0x30, 0x21, 0x2e, 0x19, 0x00, 0x01, 0x2e, 0x84, 0x00, 0x04, 0xae, 0x0b, 0x2f, 0x01, 0x2e, + 0x9c, 0x00, 0x00, 0xb2, 0x07, 0x2f, 0x01, 0x52, 0x98, 0x2e, 0x8e, 0x0e, 0x00, 0xb2, 0x02, 0x2f, 0x10, 0x30, 0x21, + 0x2e, 0x79, 0x00, 0x01, 0x2e, 0x79, 0x00, 0x00, 0x90, 0x90, 0x2e, 0x14, 0x03, 0x01, 0x2e, 0x87, 0x00, 0x00, 0xb2, + 0x04, 0x2f, 0x98, 0x2e, 0x2f, 0x0e, 0x00, 0x30, 0x21, 0x2e, 0x7b, 0x00, 0x01, 0x2e, 0x7b, 0x00, 0x00, 0xb2, 0x12, + 0x2f, 0x01, 0x2e, 0x84, 0x00, 0x00, 0x90, 0x02, 0x2f, 0x98, 0x2e, 0x1f, 0x0e, 0x09, 0x2d, 0x98, 0x2e, 0x81, 0x0d, + 0x01, 0x2e, 0x84, 0x00, 0x04, 0x90, 0x02, 0x2f, 0x50, 0x32, 0x98, 0x2e, 0x48, 0x03, 0x00, 0x30, 0x21, 0x2e, 0x7b, + 0x00, 0x01, 0x2e, 0x78, 0x00, 0x00, 0xb2, 0x90, 0x2e, 0x2c, 0x03, 0x01, 0x2e, 0x78, 0x00, 0x81, 0x30, 0x01, 0x08, + 0x00, 0xb2, 0x61, 0x2f, 0x03, 0x2e, 0x24, 0x02, 0x01, 0x2e, 0x84, 0x00, 0x98, 0xbc, 0x98, 0xb8, 0x05, 0xb2, 0x0d, + 0x58, 0x23, 0x2f, 0x07, 0x90, 0x07, 0x54, 0x00, 0x30, 0x37, 0x2f, 0x15, 0x41, 0x04, 0x41, 0xdc, 0xbe, 0x44, 0xbe, + 0xdc, 0xba, 0x2c, 0x01, 0x61, 0x00, 0x0d, 0x56, 0x4a, 0x0f, 0x0c, 0x2f, 0xd1, 0x42, 0x94, 0xb8, 0xc1, 0x42, 0x11, + 0x30, 0x05, 0x2e, 0x6a, 0xf7, 0x2c, 0xbd, 0x2f, 0xb9, 0x80, 0xb2, 0x08, 0x22, 0x98, 0x2e, 0xaf, 0x03, 0x21, 0x2d, + 0x61, 0x30, 0x23, 0x2e, 0x84, 0x00, 0x98, 0x2e, 0xaf, 0x03, 0x00, 0x30, 0x21, 0x2e, 0x5a, 0xf5, 0x18, 0x2d, 0xf1, + 0x7f, 0x50, 0x30, 0x98, 0x2e, 0x48, 0x03, 0x0d, 0x52, 0x05, 0x50, 0x50, 0x42, 0x70, 0x30, 0x0b, 0x54, 0x42, 0x42, + 0x7e, 0x82, 0xf2, 0x6f, 0x80, 0xb2, 0x42, 0x42, 0x05, 0x2f, 0x21, 0x2e, 0x84, 0x00, 0x10, 0x30, 0x98, 0x2e, 0xaf, + 0x03, 0x03, 0x2d, 0x60, 0x30, 0x21, 0x2e, 0x84, 0x00, 0x01, 0x2e, 0x84, 0x00, 0x06, 0x90, 0x18, 0x2f, 0x01, 0x2e, + 0x77, 0x00, 0x09, 0x54, 0x05, 0x52, 0xf0, 0x7f, 0x98, 0x2e, 0x7a, 0xc1, 0xf1, 0x6f, 0x08, 0x1a, 0x40, 0x30, 0x08, + 0x2f, 0x21, 0x2e, 0x84, 0x00, 0x20, 0x30, 0x98, 0x2e, 0x9b, 0x03, 0x50, 0x32, 0x98, 0x2e, 0x48, 0x03, 0x05, 0x2d, + 0x98, 0x2e, 0x38, 0x0e, 0x00, 0x30, 0x21, 0x2e, 0x84, 0x00, 0x00, 0x30, 0x21, 0x2e, 0x78, 0x00, 0x18, 0x2d, 0x01, + 0x2e, 0x84, 0x00, 0x03, 0xaa, 0x01, 0x2f, 0x98, 0x2e, 0x45, 0x0e, 0x01, 0x2e, 0x84, 0x00, 0x3f, 0x80, 0x03, 0xa2, + 0x01, 0x2f, 0x00, 0x2e, 0x02, 0x2d, 0x98, 0x2e, 0x5b, 0x0e, 0x30, 0x30, 0x98, 0x2e, 0xba, 0x03, 0x00, 0x30, 0x21, + 0x2e, 0x79, 0x00, 0x50, 0x32, 0x98, 0x2e, 0x48, 0x03, 0x01, 0x2e, 0x19, 0x00, 0x00, 0xb2, 0x10, 0x2f, 0x01, 0x2e, + 0x85, 0x00, 0x21, 0x2e, 0x90, 0x00, 0x0f, 0x52, 0x7e, 0x82, 0x11, 0x50, 0x41, 0x40, 0x18, 0xb9, 0x11, 0x42, 0x02, + 0x42, 0x02, 0x80, 0x00, 0x2e, 0x01, 0x40, 0x01, 0x42, 0x98, 0x2e, 0xaa, 0x01, 0x00, 0x30, 0x21, 0x2e, 0x19, 0x00, + 0x21, 0x2e, 0x9c, 0x00, 0x80, 0x2e, 0x52, 0x02, 0x21, 0x2e, 0x59, 0xf5, 0x10, 0x30, 0xc0, 0x2e, 0x21, 0x2e, 0x4a, + 0xf1, 0x80, 0x2e, 0x00, 0xc1, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x9a, 0x01, + 0x34, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x03, 0x2e, 0x7d, 0x00, 0x16, 0xb8, 0x02, 0x34, 0x4a, 0x0c, 0x21, 0x2e, 0x2d, 0xf5, 0xc0, 0x2e, 0x23, + 0x2e, 0x7d, 0x00, 0x03, 0xbc, 0x21, 0x2e, 0x85, 0x00, 0x03, 0x2e, 0x85, 0x00, 0x40, 0xb2, 0x10, 0x30, 0x21, 0x2e, + 0x19, 0x00, 0x01, 0x30, 0x05, 0x2f, 0x05, 0x2e, 0x88, 0x00, 0x80, 0x90, 0x01, 0x2f, 0x23, 0x2e, 0x6f, 0xf5, 0xc0, + 0x2e, 0x21, 0x2e, 0x89, 0x00, 0x11, 0x30, 0x81, 0x08, 0x01, 0x2e, 0x6a, 0xf7, 0x71, 0x3f, 0x23, 0xbd, 0x01, 0x08, + 0x02, 0x0a, 0xc0, 0x2e, 0x21, 0x2e, 0x6a, 0xf7, 0x30, 0x25, 0x00, 0x30, 0x21, 0x2e, 0x5a, 0xf5, 0x10, 0x50, 0x21, + 0x2e, 0x7b, 0x00, 0x21, 0x2e, 0x78, 0x00, 0xfb, 0x7f, 0x98, 0x2e, 0xaf, 0x03, 0x40, 0x30, 0x21, 0x2e, 0x84, 0x00, + 0xfb, 0x6f, 0xf0, 0x5f, 0x03, 0x25, 0x80, 0x2e, 0x9b, 0x03, 0x0b, 0x00, 0x94, 0x02, 0x14, 0x24, 0x80, 0x00, 0x04, + 0x00, 0x04, 0x30, 0x08, 0xb8, 0x94, 0x02, 0xc0, 0x2e, 0x28, 0xbd, 0x02, 0x0a, 0x0d, 0x82, 0x02, 0x30, 0x12, 0x42, + 0x41, 0x0e, 0xfc, 0x2f, 0xb8, 0x2e, 0x95, 0x50, 0xc0, 0x2e, 0x21, 0x2e, 0xa9, 0x01, 0x02, 0x30, 0x02, 0x2c, 0x41, + 0x00, 0x12, 0x42, 0x41, 0x0e, 0xfc, 0x2f, 0xb8, 0x2e, 0x13, 0x82, 0x02, 0x30, 0x12, 0x42, 0x41, 0x0e, 0xfc, 0x2f, + 0x3f, 0x80, 0xa1, 0x30, 0xc0, 0x2e, 0x01, 0x42, 0x00, 0x2e, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0xc0, 0x50, 0xe7, 0x7f, + 0xf6, 0x7f, 0x26, 0x30, 0x0f, 0x2e, 0x61, 0xf5, 0x2f, 0x2e, 0x78, 0x00, 0x0f, 0x2e, 0x78, 0x00, 0xbe, 0x09, 0xa2, + 0x7f, 0x80, 0x7f, 0x80, 0xb3, 0xd5, 0x7f, 0xc4, 0x7f, 0xb3, 0x7f, 0x91, 0x7f, 0x7b, 0x7f, 0x0b, 0x2f, 0x19, 0x50, + 0x1a, 0x25, 0x12, 0x40, 0x42, 0x7f, 0x74, 0x82, 0x12, 0x40, 0x52, 0x7f, 0x00, 0x2e, 0x00, 0x40, 0x60, 0x7f, 0x98, + 0x2e, 0x6a, 0xd6, 0x81, 0x30, 0x01, 0x2e, 0x78, 0x00, 0x01, 0x08, 0x00, 0xb2, 0x42, 0x2f, 0x03, 0x2e, 0x24, 0x02, + 0x01, 0x2e, 0x24, 0x02, 0x97, 0xbc, 0x06, 0xbc, 0x9f, 0xb8, 0x0f, 0xb8, 0x00, 0x90, 0x23, 0x2e, 0x88, 0x00, 0x10, + 0x30, 0x01, 0x30, 0x2a, 0x2f, 0x03, 0x2e, 0x84, 0x00, 0x44, 0xb2, 0x05, 0x2f, 0x47, 0xb2, 0x00, 0x30, 0x2d, 0x2f, + 0x21, 0x2e, 0x78, 0x00, 0x2b, 0x2d, 0x03, 0x2e, 0xfd, 0xf5, 0x9e, 0xbc, 0x9f, 0xb8, 0x40, 0x90, 0x14, 0x2f, 0x03, + 0x2e, 0xfc, 0xf5, 0x99, 0xbc, 0x9f, 0xb8, 0x40, 0x90, 0x0e, 0x2f, 0x03, 0x2e, 0x49, 0xf1, 0x1b, 0x54, 0x4a, 0x08, + 0x40, 0x90, 0x08, 0x2f, 0x98, 0x2e, 0xce, 0x00, 0x00, 0xb2, 0x10, 0x30, 0x03, 0x2f, 0x50, 0x30, 0x21, 0x2e, 0x84, + 0x00, 0x10, 0x2d, 0x98, 0x2e, 0x9b, 0x03, 0x00, 0x30, 0x21, 0x2e, 0x78, 0x00, 0x0a, 0x2d, 0x05, 0x2e, 0x69, 0xf7, + 0x2d, 0xbd, 0x2f, 0xb9, 0x80, 0xb2, 0x01, 0x2f, 0x21, 0x2e, 0x79, 0x00, 0x23, 0x2e, 0x78, 0x00, 0xe0, 0x31, 0x21, + 0x2e, 0x61, 0xf5, 0xf6, 0x6f, 0xe7, 0x6f, 0x80, 0x6f, 0xa2, 0x6f, 0xb3, 0x6f, 0xc4, 0x6f, 0xd5, 0x6f, 0x7b, 0x6f, + 0x91, 0x6f, 0x40, 0x5f, 0xc8, 0x2e, 0x90, 0x50, 0xf7, 0x7f, 0xe6, 0x7f, 0xd5, 0x7f, 0xc4, 0x7f, 0xb3, 0x7f, 0xa1, + 0x7f, 0x90, 0x7f, 0x82, 0x7f, 0x7b, 0x7f, 0x98, 0x2e, 0xce, 0x00, 0x00, 0xb2, 0x10, 0x30, 0x49, 0x2f, 0x05, 0x2e, + 0x21, 0x02, 0x03, 0x2e, 0x2d, 0x02, 0x21, 0x56, 0x08, 0x08, 0x93, 0x08, 0x90, 0x0a, 0x25, 0x2e, 0x18, 0x00, 0x05, + 0x2e, 0xc1, 0xf5, 0x2e, 0xbc, 0x05, 0x2e, 0x84, 0x00, 0x84, 0xa2, 0x0e, 0xb8, 0x31, 0x30, 0x88, 0x04, 0x03, 0x2f, + 0x01, 0x2e, 0x18, 0x00, 0x00, 0xb2, 0x0c, 0x2f, 0x1d, 0x50, 0x01, 0x52, 0x98, 0x2e, 0xd7, 0x00, 0x05, 0x2e, 0x7a, + 0x00, 0x80, 0x90, 0x02, 0x2f, 0x10, 0x30, 0x21, 0x2e, 0x7a, 0x00, 0x25, 0x2e, 0x9c, 0x00, 0x05, 0x2e, 0x18, 0x00, + 0x80, 0xb2, 0x20, 0x2f, 0x01, 0x2e, 0xc0, 0xf5, 0xf2, 0x30, 0x02, 0x08, 0x07, 0xaa, 0x73, 0x30, 0x03, 0x2e, 0x7c, + 0x00, 0x18, 0x22, 0x41, 0x1a, 0x05, 0x2f, 0x03, 0x2e, 0x66, 0xf5, 0x9f, 0xbc, 0x9f, 0xb8, 0x40, 0x90, 0x0c, 0x2f, + 0x1f, 0x52, 0x03, 0x30, 0x53, 0x42, 0x2b, 0x30, 0x90, 0x04, 0x5b, 0x42, 0x21, 0x2e, 0x7c, 0x00, 0x24, 0xbd, 0x7e, + 0x80, 0x81, 0x84, 0x43, 0x42, 0x02, 0x42, 0x02, 0x32, 0x25, 0x2e, 0x62, 0xf5, 0x05, 0x2e, 0x86, 0x00, 0x81, 0x84, + 0x25, 0x2e, 0x86, 0x00, 0x02, 0x31, 0x25, 0x2e, 0x60, 0xf5, 0x05, 0x2e, 0x25, 0x02, 0x10, 0x30, 0x90, 0x08, 0x80, + 0xb2, 0x0b, 0x2f, 0x05, 0x2e, 0xca, 0xf5, 0xf0, 0x3e, 0x90, 0x08, 0x25, 0x2e, 0xca, 0xf5, 0x05, 0x2e, 0x59, 0xf5, + 0xe0, 0x3f, 0x90, 0x08, 0x25, 0x2e, 0x59, 0xf5, 0x90, 0x6f, 0xa1, 0x6f, 0xb3, 0x6f, 0xc4, 0x6f, 0xd5, 0x6f, 0xe6, + 0x6f, 0xf7, 0x6f, 0x7b, 0x6f, 0x82, 0x6f, 0x70, 0x5f, 0xc8, 0x2e, 0x2f, 0x52, 0x90, 0x50, 0x53, 0x40, 0x4a, 0x25, + 0x40, 0x40, 0x39, 0x8b, 0xfb, 0x7f, 0x0c, 0xbc, 0x21, 0x52, 0x37, 0x89, 0x0b, 0x30, 0x59, 0x08, 0x0c, 0xb8, 0xe0, + 0x7f, 0x8b, 0x7f, 0x4b, 0x43, 0x0b, 0x43, 0x40, 0xb2, 0xd1, 0x7f, 0x6e, 0x2f, 0x01, 0x2e, 0x83, 0x00, 0x00, 0xb2, + 0x0e, 0x2f, 0x25, 0x52, 0x01, 0x2e, 0x7e, 0x00, 0xc3, 0x7f, 0xb4, 0x7f, 0xa5, 0x7f, 0x98, 0x2e, 0xbb, 0xcc, 0x05, + 0x30, 0x2b, 0x2e, 0x83, 0x00, 0xc3, 0x6f, 0xd1, 0x6f, 0xb4, 0x6f, 0xa5, 0x6f, 0x36, 0xbc, 0x06, 0xb9, 0x35, 0xbc, + 0x0f, 0xb8, 0x94, 0xb0, 0xc6, 0x7f, 0x00, 0xb2, 0x0c, 0x2f, 0x27, 0x50, 0x29, 0x56, 0x0b, 0x30, 0x05, 0x2e, 0x21, + 0x02, 0x2d, 0x5c, 0x1b, 0x42, 0xdb, 0x42, 0x96, 0x08, 0x25, 0x2e, 0x21, 0x02, 0x0b, 0x42, 0xcb, 0x42, 0x00, 0x2e, + 0x31, 0x56, 0xcb, 0x08, 0x25, 0x52, 0x01, 0x2e, 0x7e, 0x00, 0x01, 0x54, 0x2b, 0x5c, 0x98, 0x2e, 0x06, 0xcd, 0xd2, + 0x6f, 0x27, 0x5a, 0x94, 0x6f, 0xa4, 0xbc, 0x53, 0x41, 0x00, 0xb3, 0x1f, 0xb8, 0x44, 0x41, 0x01, 0x30, 0xd5, 0x7f, + 0x05, 0x2f, 0x00, 0xb2, 0x03, 0x2f, 0x29, 0x5c, 0x11, 0x30, 0x93, 0x43, 0x84, 0x43, 0x23, 0xbd, 0x2f, 0xb9, 0x80, + 0xb2, 0x1c, 0x2f, 0x72, 0x6f, 0xda, 0x00, 0x82, 0x6f, 0x22, 0x03, 0x44, 0x43, 0x00, 0x90, 0x27, 0x2e, 0x7f, 0x00, + 0x29, 0x5a, 0x12, 0x2f, 0x29, 0x54, 0x00, 0x2e, 0x90, 0x40, 0x82, 0x40, 0x18, 0x04, 0xa2, 0x06, 0x80, 0xaa, 0x04, + 0x2f, 0x80, 0x90, 0x08, 0x2f, 0xc2, 0x6f, 0x50, 0x0f, 0x05, 0x2f, 0xc0, 0x6f, 0x00, 0xb2, 0x02, 0x2f, 0x53, 0x43, + 0x44, 0x43, 0x11, 0x30, 0xe0, 0x6f, 0x98, 0x2e, 0x95, 0xcf, 0xd1, 0x6f, 0x15, 0x5a, 0x09, 0x2e, 0x7f, 0x00, 0x41, + 0x40, 0x54, 0x43, 0x08, 0x2c, 0x41, 0x43, 0x15, 0x30, 0x2b, 0x2e, 0x83, 0x00, 0x01, 0x30, 0xe0, 0x6f, 0x98, 0x2e, + 0x95, 0xcf, 0x00, 0x2e, 0xfb, 0x6f, 0x70, 0x5f, 0xb8, 0x2e, 0x50, 0x86, 0xcd, 0x88, 0x34, 0x85, 0xc5, 0x40, 0x91, + 0x40, 0x8c, 0x80, 0x06, 0x41, 0x13, 0x40, 0x50, 0x50, 0x6e, 0x01, 0x82, 0x40, 0x04, 0x40, 0x34, 0x8c, 0xfb, 0x7f, + 0x98, 0x2e, 0xce, 0x03, 0xe0, 0x7f, 0x00, 0x2e, 0x91, 0x41, 0x8c, 0x81, 0x82, 0x41, 0x13, 0x40, 0x04, 0x40, 0x34, + 0x8e, 0x98, 0x2e, 0xce, 0x03, 0xc0, 0x7f, 0xd5, 0x7f, 0x13, 0x24, 0xff, 0x00, 0xd6, 0x41, 0xcc, 0x83, 0xc2, 0x41, + 0x57, 0x40, 0x74, 0x80, 0x44, 0x40, 0x11, 0x40, 0x0c, 0x8a, 0xf7, 0x01, 0x94, 0x03, 0x12, 0x24, 0x80, 0x00, 0x3a, + 0x01, 0x02, 0x30, 0xb2, 0x03, 0xce, 0x17, 0xfb, 0x08, 0x23, 0x01, 0xb2, 0x02, 0x48, 0xbb, 0x28, 0xbd, 0xf2, 0x0b, + 0x53, 0x41, 0x02, 0x40, 0x44, 0x41, 0x74, 0x8d, 0xb7, 0x7f, 0x98, 0x2e, 0xce, 0x03, 0x50, 0x25, 0x91, 0x41, 0x8c, + 0x81, 0x82, 0x41, 0x13, 0x40, 0x04, 0x40, 0x34, 0x8e, 0x98, 0x2e, 0xce, 0x03, 0x60, 0x25, 0xd1, 0x41, 0xcc, 0x81, + 0xc2, 0x41, 0x13, 0x40, 0x04, 0x40, 0x98, 0x2e, 0xce, 0x03, 0x11, 0x24, 0xb3, 0x00, 0x71, 0x0e, 0xd3, 0x6f, 0xe1, + 0x6f, 0x33, 0x2f, 0x12, 0x24, 0xdd, 0x00, 0xda, 0x0f, 0x2b, 0x2f, 0x12, 0x24, 0x8c, 0x00, 0x5a, 0x0e, 0x09, 0x2f, + 0x10, 0x24, 0x83, 0x05, 0x48, 0x0e, 0x11, 0x24, 0x7f, 0x22, 0x10, 0x24, 0x18, 0x32, 0x08, 0x22, 0x80, 0x2e, 0xd7, + 0xb4, 0x13, 0x24, 0xf4, 0x00, 0x73, 0x0e, 0x0f, 0x2f, 0x10, 0x24, 0x11, 0x10, 0x68, 0x0e, 0x10, 0x24, 0xa2, 0x30, + 0x13, 0x24, 0x97, 0x23, 0x03, 0x22, 0x13, 0x24, 0x3b, 0x04, 0x4b, 0x0e, 0x11, 0x24, 0x0f, 0x30, 0x01, 0x22, 0x80, + 0x2e, 0xd7, 0xb4, 0x11, 0x24, 0x53, 0x02, 0x41, 0x0e, 0x11, 0x24, 0xe7, 0x31, 0x10, 0x24, 0xfc, 0x25, 0x08, 0x22, + 0x80, 0x2e, 0xd7, 0xb4, 0x10, 0x24, 0xe8, 0x40, 0x80, 0x2e, 0xd7, 0xb4, 0xf2, 0x37, 0x5a, 0x0e, 0x90, 0x2e, 0x50, + 0xb3, 0x12, 0x24, 0xea, 0x00, 0x4a, 0x0e, 0x90, 0x2e, 0xc7, 0xb2, 0xc2, 0x6f, 0x14, 0x24, 0x4c, 0x0b, 0x54, 0x0e, + 0x90, 0x2e, 0xab, 0xb2, 0x14, 0x24, 0x9b, 0x00, 0x5c, 0x0e, 0x90, 0x2e, 0xa1, 0xb2, 0x14, 0x24, 0x22, 0x01, 0x4c, + 0x0e, 0x70, 0x2f, 0x82, 0xa3, 0x5e, 0x2f, 0x11, 0x24, 0xba, 0x0b, 0x51, 0x0e, 0x35, 0x2f, 0x11, 0x24, 0x03, 0x08, + 0x69, 0x0e, 0x2d, 0x2f, 0xb1, 0x6f, 0x14, 0x24, 0x90, 0x00, 0x0c, 0x0e, 0x24, 0x2f, 0x11, 0x24, 0x31, 0x08, 0x69, + 0x0e, 0x16, 0x2f, 0x11, 0x24, 0x7d, 0x01, 0x59, 0x0e, 0x0e, 0x2f, 0x11, 0x24, 0xd7, 0x0c, 0x51, 0x0e, 0x11, 0x24, + 0x9f, 0x44, 0x13, 0x24, 0x41, 0x57, 0x4b, 0x22, 0x93, 0x35, 0x43, 0x0e, 0x10, 0x24, 0xbd, 0x42, 0x08, 0x22, 0x80, + 0x2e, 0xd7, 0xb4, 0x10, 0x24, 0x1c, 0x42, 0x80, 0x2e, 0xd7, 0xb4, 0x11, 0x24, 0x47, 0x01, 0x59, 0x0e, 0x11, 0x24, + 0xa2, 0x45, 0x10, 0x24, 0x31, 0x51, 0x08, 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0x10, 0x24, 0x80, 0x41, 0x80, 0x2e, 0xd7, + 0xb4, 0x10, 0x24, 0x67, 0x54, 0x80, 0x2e, 0xd7, 0xb4, 0x11, 0x24, 0x8c, 0x08, 0xe9, 0x0f, 0x10, 0x24, 0x0a, 0x48, + 0x90, 0x2e, 0xd7, 0xb4, 0xb1, 0x6f, 0x13, 0x24, 0xe8, 0x03, 0x8b, 0x0f, 0x10, 0x24, 0xcd, 0x57, 0x90, 0x2e, 0xd7, + 0xb4, 0x73, 0x35, 0x8b, 0x0f, 0x10, 0x24, 0x6f, 0x42, 0x90, 0x2e, 0xd7, 0xb4, 0x10, 0x24, 0xa0, 0xfe, 0x08, 0x0e, + 0x10, 0x24, 0x38, 0x54, 0x13, 0x24, 0xa3, 0x46, 0x03, 0x22, 0x13, 0x24, 0x45, 0xfd, 0x0b, 0x0e, 0x11, 0x24, 0x04, + 0x43, 0x08, 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0xb1, 0x6f, 0x00, 0x3a, 0x08, 0x0e, 0x11, 0x24, 0x3d, 0x45, 0x10, 0x24, + 0x52, 0x54, 0x48, 0x22, 0x10, 0x24, 0x8f, 0x01, 0x58, 0x0e, 0x10, 0x24, 0x48, 0x44, 0x01, 0x22, 0x80, 0x2e, 0xd7, + 0xb4, 0xb1, 0x6f, 0x13, 0x24, 0xfa, 0x03, 0x0b, 0x0e, 0x11, 0x24, 0x85, 0x43, 0x13, 0x24, 0x35, 0x55, 0x4b, 0x22, + 0x11, 0xa2, 0x10, 0x24, 0xf6, 0x57, 0x08, 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0x11, 0x24, 0xa4, 0x0a, 0x69, 0x0e, 0x11, + 0x24, 0x7b, 0x5a, 0x10, 0x24, 0x5e, 0x20, 0x08, 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0x11, 0x24, 0x0f, 0x01, 0x59, 0x0e, + 0x0d, 0x2f, 0x18, 0xa2, 0x11, 0x24, 0x2b, 0x47, 0x10, 0x24, 0xf4, 0x55, 0x48, 0x22, 0x10, 0x24, 0x16, 0x0b, 0x50, + 0x0e, 0x10, 0x24, 0xc7, 0x51, 0x01, 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0x11, 0x24, 0x72, 0x0a, 0x51, 0x0e, 0x11, 0x24, + 0x85, 0x55, 0x10, 0x24, 0xb2, 0x47, 0x08, 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0x10, 0x24, 0x83, 0x00, 0x48, 0x0e, 0x53, + 0x2f, 0x11, 0x24, 0xe1, 0x07, 0x69, 0x0e, 0x2d, 0x2f, 0x95, 0xaf, 0x27, 0x2f, 0x82, 0xaf, 0x21, 0x2f, 0x11, 0x24, + 0xd7, 0x00, 0x59, 0x0e, 0x19, 0x2f, 0xb1, 0x6f, 0x10, 0x24, 0xcc, 0x03, 0x88, 0x0f, 0x10, 0x2f, 0x10, 0x24, 0xe8, + 0xfe, 0x08, 0x0e, 0x11, 0x24, 0x7e, 0x56, 0x10, 0x24, 0x94, 0x45, 0x48, 0x22, 0xc0, 0x6f, 0x13, 0x24, 0x06, 0x0b, + 0x43, 0x0e, 0x10, 0x24, 0x2f, 0x51, 0x01, 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0x10, 0x24, 0xde, 0x51, 0x80, 0x2e, 0xd7, + 0xb4, 0x10, 0x24, 0xe8, 0x54, 0x80, 0x2e, 0xd7, 0xb4, 0x10, 0x24, 0xa4, 0x52, 0x80, 0x2e, 0xd7, 0xb4, 0x10, 0x24, + 0xd0, 0x44, 0x80, 0x2e, 0xd7, 0xb4, 0x11, 0x24, 0xb8, 0x00, 0xd9, 0x0f, 0x19, 0x2f, 0xc1, 0x6f, 0x10, 0x24, 0xe7, + 0x0c, 0xc8, 0x0f, 0x10, 0x2f, 0x11, 0x24, 0xc7, 0x07, 0x69, 0x0e, 0x11, 0x24, 0xf6, 0x52, 0x10, 0x24, 0x7a, 0x12, + 0x48, 0x22, 0xb0, 0x6f, 0x13, 0x24, 0x5d, 0x02, 0x03, 0x0e, 0x10, 0x24, 0x7c, 0x54, 0x01, 0x22, 0x80, 0x2e, 0xd7, + 0xb4, 0x10, 0x24, 0x8d, 0x51, 0x80, 0x2e, 0xd7, 0xb4, 0x10, 0x24, 0x28, 0x52, 0x80, 0x2e, 0xd7, 0xb4, 0x10, 0x24, + 0xd2, 0x07, 0xe8, 0x0f, 0x28, 0x2f, 0x10, 0x24, 0xb0, 0x00, 0xd8, 0x0f, 0x20, 0x2f, 0x10, 0x24, 0xc6, 0x07, 0x68, + 0x0e, 0x18, 0x2f, 0x50, 0x35, 0x48, 0x0e, 0x11, 0x2f, 0xb1, 0x6f, 0x10, 0x24, 0xf4, 0x01, 0x08, 0x0e, 0x11, 0x24, + 0x35, 0x51, 0x10, 0x24, 0x22, 0x12, 0x48, 0x22, 0xc0, 0x6f, 0x13, 0x24, 0xe0, 0x0c, 0x43, 0x0e, 0x10, 0x24, 0x7b, + 0x50, 0x08, 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0x10, 0x24, 0x81, 0x52, 0x80, 0x2e, 0xd7, 0xb4, 0x10, 0x24, 0x3b, 0x53, + 0x80, 0x2e, 0xd7, 0xb4, 0x10, 0x24, 0x63, 0x51, 0x80, 0x2e, 0xd7, 0xb4, 0x10, 0x24, 0x27, 0x51, 0x80, 0x2e, 0xd7, + 0xb4, 0x18, 0xa2, 0x90, 0x2e, 0xdb, 0xb3, 0x12, 0x24, 0x08, 0x02, 0x4a, 0x0e, 0x37, 0x2f, 0x12, 0x24, 0x2a, 0x09, + 0x6a, 0x0e, 0x1d, 0x2f, 0x13, 0x24, 0x8e, 0x00, 0x73, 0x0e, 0x09, 0x2f, 0x11, 0x24, 0xa5, 0x01, 0x41, 0x0e, 0x11, + 0x24, 0x76, 0x32, 0x10, 0x24, 0x12, 0x25, 0x08, 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0x10, 0x24, 0xa9, 0x0d, 0x68, 0x0e, + 0x10, 0x24, 0x04, 0x27, 0x13, 0x24, 0x73, 0x20, 0x03, 0x22, 0x13, 0x24, 0x14, 0x04, 0x4b, 0x0e, 0x11, 0x24, 0x15, + 0x2c, 0x01, 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0x11, 0x24, 0xae, 0x08, 0x69, 0x0e, 0x08, 0x2f, 0xa1, 0x35, 0x71, 0x0e, + 0x11, 0x24, 0x8b, 0x2b, 0x10, 0x24, 0x07, 0x35, 0x08, 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0x91, 0x34, 0x59, 0x0e, 0x11, + 0x24, 0x7b, 0x19, 0x10, 0x24, 0x50, 0x59, 0x08, 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0x62, 0x32, 0x42, 0x0e, 0x22, 0x2f, + 0xa2, 0x32, 0x5a, 0x0e, 0x1b, 0x2f, 0x12, 0x24, 0x0b, 0x08, 0x6a, 0x0e, 0x0e, 0x2f, 0xa3, 0x34, 0x43, 0x0e, 0x10, + 0x24, 0x28, 0x2b, 0x13, 0x24, 0x20, 0x23, 0x03, 0x22, 0x13, 0x24, 0x8d, 0x01, 0x4b, 0x0e, 0x11, 0x24, 0x5c, 0x21, + 0x08, 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0x31, 0x36, 0x59, 0x0e, 0x11, 0x24, 0x43, 0x25, 0x10, 0x24, 0xfa, 0x49, 0x08, + 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0x10, 0x24, 0xc7, 0x2a, 0x80, 0x2e, 0xd7, 0xb4, 0x40, 0x36, 0x58, 0x0e, 0x09, 0x2f, + 0x11, 0x24, 0x9e, 0x08, 0x69, 0x0e, 0x11, 0x24, 0xe3, 0x54, 0x10, 0x24, 0x73, 0x22, 0x08, 0x22, 0x80, 0x2e, 0xd7, + 0xb4, 0x10, 0x24, 0x38, 0x01, 0xc8, 0x0f, 0x10, 0x2f, 0x11, 0x24, 0x11, 0x08, 0x69, 0x0e, 0x11, 0x24, 0x6e, 0x48, + 0x10, 0x24, 0x2b, 0x28, 0x48, 0x22, 0xc0, 0x6f, 0x13, 0x24, 0xc1, 0x0a, 0x43, 0x0e, 0x10, 0x24, 0x0f, 0x23, 0x08, + 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0x10, 0x24, 0xd0, 0x1a, 0x80, 0x2e, 0xd7, 0xb4, 0xe2, 0x33, 0x5a, 0x0e, 0x77, 0x2f, + 0x12, 0x24, 0x0c, 0x08, 0x6a, 0x0e, 0x2a, 0x2f, 0x12, 0x24, 0xc5, 0x00, 0x4a, 0x0e, 0x08, 0x2f, 0x11, 0x36, 0x59, + 0x0e, 0x11, 0x24, 0xfd, 0x18, 0x10, 0x24, 0x75, 0x58, 0x08, 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0xc2, 0x34, 0x5a, 0x0e, + 0x0d, 0x2f, 0x11, 0x24, 0x36, 0x08, 0x69, 0x0e, 0x11, 0x24, 0x08, 0x58, 0x13, 0x24, 0x3b, 0x54, 0x4b, 0x22, 0x01, + 0xa2, 0x10, 0x24, 0xc6, 0x52, 0x08, 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0xb3, 0x36, 0x4b, 0x0e, 0x11, 0x24, 0x0e, 0x24, + 0x13, 0x24, 0x7b, 0x50, 0x59, 0x22, 0x0e, 0xa2, 0x10, 0x24, 0xf7, 0x56, 0x01, 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0xc2, + 0x35, 0x5a, 0x0e, 0x12, 0x2f, 0x01, 0xa2, 0x0c, 0x2f, 0x84, 0xa3, 0x10, 0x24, 0xd4, 0x58, 0x13, 0x24, 0x76, 0x56, + 0x03, 0x22, 0x73, 0x36, 0x4b, 0x0e, 0x11, 0x24, 0xeb, 0x52, 0x08, 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0x10, 0x24, 0x87, + 0x16, 0x80, 0x2e, 0xd7, 0xb4, 0xb0, 0x6f, 0x13, 0x24, 0x02, 0xfd, 0x03, 0x0e, 0x29, 0x2f, 0x84, 0xa3, 0xc0, 0x6f, + 0x09, 0x2f, 0x11, 0x24, 0xe4, 0x0a, 0x41, 0x0e, 0x11, 0x24, 0x5d, 0x44, 0x10, 0x24, 0x2f, 0x5a, 0x08, 0x22, 0x80, + 0x2e, 0xd7, 0xb4, 0x13, 0x24, 0x96, 0x0c, 0x43, 0x0e, 0x0e, 0x2f, 0x40, 0x33, 0x48, 0x0e, 0x10, 0x24, 0xf2, 0x18, + 0x13, 0x24, 0x31, 0x49, 0x03, 0x22, 0x13, 0x24, 0x99, 0x00, 0x4b, 0x0e, 0x11, 0x24, 0xab, 0x18, 0x01, 0x22, 0x80, + 0x2e, 0xd7, 0xb4, 0x11, 0x24, 0xc6, 0x07, 0x69, 0x0e, 0x11, 0x24, 0xb0, 0x49, 0x10, 0x24, 0xbf, 0x17, 0x08, 0x22, + 0x80, 0x2e, 0xd7, 0xb4, 0x10, 0x24, 0x03, 0x15, 0x80, 0x2e, 0xd7, 0xb4, 0xb0, 0x32, 0x48, 0x0e, 0x57, 0x2f, 0xa0, + 0x37, 0x48, 0x0e, 0x13, 0x2f, 0x83, 0xa3, 0x08, 0x2f, 0x10, 0x24, 0xe0, 0x00, 0x48, 0x0e, 0x11, 0x24, 0xf6, 0x25, + 0x10, 0x24, 0x75, 0x17, 0x71, 0x2c, 0x08, 0x22, 0x10, 0x24, 0xa0, 0x00, 0x48, 0x0e, 0x11, 0x24, 0x7f, 0x18, 0x10, + 0x24, 0xa6, 0x13, 0x68, 0x2c, 0x08, 0x22, 0x11, 0x24, 0xf9, 0x07, 0x69, 0x0e, 0x0d, 0x2f, 0x11, 0x24, 0x10, 0x08, + 0x69, 0x0e, 0x11, 0x24, 0xb1, 0x14, 0x10, 0x24, 0x8e, 0x58, 0x48, 0x22, 0x90, 0x32, 0x58, 0x0e, 0x10, 0x24, 0x6d, + 0x14, 0x56, 0x2c, 0x01, 0x22, 0xc1, 0x6f, 0x10, 0x24, 0x68, 0x0c, 0x48, 0x0e, 0xb1, 0x6f, 0x0c, 0x2f, 0xcd, 0xa2, + 0x10, 0x24, 0x23, 0x14, 0x13, 0x24, 0x8d, 0x42, 0x03, 0x22, 0x13, 0x24, 0x2a, 0xfd, 0x0b, 0x0e, 0x11, 0x24, 0x53, + 0x12, 0x43, 0x2c, 0x08, 0x22, 0x10, 0x24, 0xcc, 0x07, 0x68, 0x0e, 0x0e, 0x2f, 0x10, 0x24, 0x08, 0xfd, 0x08, 0x0e, + 0x10, 0x24, 0x08, 0x16, 0x13, 0x24, 0x83, 0x45, 0x03, 0x22, 0x13, 0x24, 0xa1, 0xfd, 0x0b, 0x0e, 0x11, 0x24, 0xa6, + 0x14, 0x30, 0x2c, 0x01, 0x22, 0x10, 0x24, 0x5b, 0x01, 0x08, 0x0e, 0x11, 0x24, 0x2f, 0x12, 0x10, 0x24, 0xdd, 0x44, + 0x27, 0x2c, 0x08, 0x22, 0xdb, 0xa2, 0x0f, 0x2f, 0xc1, 0x6f, 0x10, 0x24, 0xb2, 0x0b, 0x48, 0x0e, 0x11, 0x24, 0x21, + 0x55, 0x10, 0x24, 0xc8, 0x14, 0x48, 0x22, 0x10, 0x24, 0x4c, 0x08, 0x68, 0x0e, 0x10, 0x24, 0xe4, 0x57, 0x15, 0x2c, + 0x01, 0x22, 0x44, 0xa2, 0x0f, 0x2f, 0xc1, 0x6f, 0x10, 0x24, 0xcb, 0x0b, 0x48, 0x0e, 0x11, 0x24, 0x09, 0x58, 0x10, + 0x24, 0xe4, 0x10, 0x48, 0x22, 0x10, 0x24, 0x4d, 0x08, 0x68, 0x0e, 0x10, 0x24, 0x1a, 0x12, 0x03, 0x2c, 0x01, 0x22, + 0x10, 0x24, 0x0c, 0x10, 0xfb, 0x6f, 0xb0, 0x5f, 0xb8, 0x2e, 0xa3, 0x32, 0xc3, 0x00, 0x60, 0x51, 0xc2, 0x40, 0x81, + 0x84, 0xd3, 0x7f, 0xd2, 0x42, 0xe0, 0x7f, 0x00, 0x30, 0xc4, 0x40, 0x20, 0x02, 0xc3, 0x7f, 0xd0, 0x42, 0x42, 0x3d, + 0xc0, 0x40, 0x01, 0x80, 0xc0, 0x42, 0xda, 0x00, 0x93, 0x7f, 0xb1, 0x7f, 0xab, 0x7f, 0x98, 0x2e, 0xb3, 0xc0, 0x91, + 0x6f, 0xf3, 0x32, 0x40, 0x42, 0x00, 0xac, 0x8b, 0x00, 0x02, 0x2f, 0xe1, 0x6f, 0x39, 0x56, 0x43, 0x42, 0xa1, 0x82, + 0x91, 0x7f, 0x33, 0x33, 0x4b, 0x00, 0x81, 0x7f, 0x13, 0x3c, 0x4b, 0x00, 0x80, 0x40, 0x53, 0x34, 0xb5, 0x6f, 0x8b, + 0x00, 0x0d, 0xb0, 0x43, 0x87, 0x76, 0x7f, 0xb2, 0x7f, 0x63, 0x7f, 0x65, 0x25, 0xb5, 0x6f, 0x92, 0x41, 0x63, 0x41, + 0x64, 0x41, 0x44, 0x81, 0x56, 0x7f, 0x41, 0x7f, 0x00, 0x2e, 0x26, 0x40, 0x27, 0x40, 0x45, 0x41, 0xf7, 0x7f, 0xb0, + 0x7f, 0x98, 0x2e, 0x67, 0xcc, 0x81, 0x6f, 0x0f, 0xa4, 0x43, 0x40, 0x72, 0x6f, 0x94, 0x6f, 0x05, 0x30, 0x01, 0x2f, + 0xc0, 0xa0, 0x03, 0x2f, 0x31, 0xac, 0x07, 0x2f, 0xc0, 0xa4, 0x05, 0x2f, 0xa2, 0x00, 0xeb, 0x04, 0x80, 0x40, 0x01, + 0x80, 0x43, 0x42, 0x80, 0x42, 0x41, 0x86, 0x56, 0x6f, 0x62, 0x6f, 0x41, 0x6f, 0x42, 0x82, 0x72, 0x0e, 0x83, 0x7f, + 0xd5, 0x2f, 0x53, 0x32, 0x8b, 0x00, 0xa1, 0x86, 0x56, 0x25, 0xf0, 0x82, 0x82, 0x40, 0x8d, 0xb0, 0x52, 0x40, 0xde, + 0x00, 0x91, 0x7f, 0xb3, 0x7f, 0x85, 0x7f, 0xb3, 0x30, 0x7b, 0x52, 0x98, 0x2e, 0x5a, 0xca, 0x1a, 0x25, 0x83, 0x6f, + 0x6d, 0x82, 0xfd, 0x88, 0x50, 0x7f, 0x71, 0x7f, 0x81, 0x7f, 0x05, 0x30, 0x83, 0x30, 0x00, 0x30, 0x11, 0x41, 0x52, + 0x6f, 0x25, 0x7f, 0x30, 0x7f, 0x44, 0x7f, 0x98, 0x2e, 0x0f, 0xca, 0x73, 0x6f, 0x20, 0x25, 0x90, 0x6f, 0x7d, 0x52, + 0xd2, 0x42, 0x73, 0x7f, 0x12, 0x7f, 0x98, 0x2e, 0x86, 0xb7, 0x93, 0x6f, 0x11, 0x6f, 0xd2, 0x40, 0x0a, 0x18, 0x31, + 0x6f, 0x0e, 0x00, 0x93, 0x7f, 0x83, 0x30, 0x44, 0x6f, 0x21, 0x6f, 0x62, 0x6f, 0x62, 0x0e, 0x4f, 0x03, 0xe1, 0x2f, + 0x33, 0x52, 0x01, 0x00, 0x01, 0x30, 0x69, 0x03, 0x3a, 0x25, 0xea, 0x82, 0x92, 0x6f, 0xf0, 0x86, 0xd1, 0xbe, 0x0f, + 0xb8, 0xbd, 0x84, 0x94, 0x7f, 0x05, 0x0a, 0x23, 0x7f, 0x52, 0x7f, 0x40, 0x7f, 0x31, 0x7f, 0x71, 0x7f, 0xd3, 0x30, + 0x84, 0x6f, 0x55, 0x6f, 0x10, 0x41, 0x52, 0x41, 0x41, 0x6f, 0x55, 0x7f, 0x10, 0x7f, 0x04, 0x7f, 0x98, 0x2e, 0x0f, + 0xca, 0x11, 0x6f, 0x20, 0x25, 0x98, 0x2e, 0xfe, 0xc9, 0x31, 0x6f, 0x04, 0x6f, 0x50, 0x42, 0x31, 0x7f, 0xd3, 0x30, + 0x21, 0x6f, 0x61, 0x0e, 0xea, 0x2f, 0xb1, 0x6f, 0x41, 0x84, 0x32, 0x25, 0x90, 0x40, 0x84, 0x40, 0x71, 0x6f, 0xb4, + 0x7f, 0x72, 0x7f, 0x40, 0x7f, 0x33, 0x7f, 0x98, 0x2e, 0xb3, 0xc0, 0x53, 0x6f, 0xb1, 0x32, 0x99, 0x00, 0x83, 0xb9, + 0x41, 0x6f, 0x4b, 0x00, 0xb0, 0x6f, 0x03, 0x30, 0xc3, 0x02, 0x84, 0x40, 0xb2, 0x7f, 0xa1, 0x84, 0x0d, 0xb1, 0x52, + 0x7f, 0x56, 0x01, 0x74, 0x6f, 0x30, 0x6f, 0x92, 0x6f, 0x43, 0x8b, 0x03, 0x43, 0x01, 0x42, 0x95, 0x7f, 0xbd, 0x86, + 0x51, 0x41, 0x41, 0x7f, 0x75, 0x7f, 0x00, 0x2e, 0xd1, 0x40, 0x42, 0x41, 0x32, 0x7f, 0x23, 0x7f, 0x98, 0x2e, 0x74, + 0xc0, 0x41, 0x6f, 0xc8, 0x00, 0x90, 0x6f, 0x01, 0x30, 0x75, 0x6f, 0x32, 0x6f, 0x03, 0x42, 0x91, 0x02, 0x23, 0x6f, + 0x61, 0x6f, 0x59, 0x0e, 0x62, 0x43, 0x95, 0x7f, 0xe7, 0x2f, 0xb2, 0x6f, 0x51, 0x6f, 0x82, 0x40, 0x8d, 0xb0, 0x8e, + 0x00, 0xfd, 0x8a, 0xb2, 0x7f, 0x02, 0x30, 0x79, 0x52, 0x05, 0x25, 0x03, 0x30, 0x54, 0x40, 0xec, 0x01, 0x16, 0x40, + 0x43, 0x89, 0xc7, 0x41, 0x37, 0x18, 0x3d, 0x8b, 0x96, 0x00, 0x44, 0x0e, 0xdf, 0x02, 0xf4, 0x2f, 0x09, 0x52, 0x51, + 0x00, 0x02, 0x30, 0x9a, 0x02, 0xb5, 0x6f, 0x45, 0x87, 0x1b, 0xba, 0x25, 0xbc, 0x51, 0x6f, 0x4d, 0x8b, 0x7a, 0x82, + 0xc6, 0x40, 0x20, 0x0a, 0x30, 0x00, 0xd0, 0x42, 0x2b, 0xb5, 0xc0, 0x40, 0x82, 0x02, 0x40, 0x34, 0x08, 0x00, 0xd2, + 0x42, 0xb0, 0x7f, 0x75, 0x7f, 0x93, 0x7f, 0x00, 0x2e, 0xb5, 0x6f, 0xe2, 0x6f, 0x63, 0x41, 0x64, 0x41, 0x44, 0x8f, + 0x82, 0x40, 0xe6, 0x41, 0xc0, 0x41, 0xc4, 0x8f, 0x45, 0x41, 0xf0, 0x7f, 0xb7, 0x7f, 0x61, 0x7f, 0x98, 0x2e, 0x67, + 0xcc, 0x00, 0x18, 0x09, 0x52, 0x71, 0x00, 0x03, 0x30, 0xbb, 0x02, 0x1b, 0xba, 0x93, 0x6f, 0x25, 0xbc, 0x61, 0x6f, + 0xc5, 0x40, 0x42, 0x82, 0x20, 0x0a, 0x28, 0x00, 0xd0, 0x42, 0x2b, 0xb9, 0xc0, 0x40, 0x82, 0x02, 0xd2, 0x42, 0x93, + 0x7f, 0x00, 0x2e, 0x72, 0x6f, 0x5a, 0x0e, 0xd9, 0x2f, 0xb1, 0x6f, 0xf3, 0x3c, 0xcb, 0x00, 0xda, 0x82, 0xc3, 0x40, + 0x41, 0x40, 0x59, 0x0e, 0x50, 0x2f, 0xe1, 0x6f, 0xe3, 0x32, 0xcb, 0x00, 0xb3, 0x7f, 0x22, 0x30, 0xc0, 0x40, 0x01, + 0x80, 0xc0, 0x42, 0x02, 0xa2, 0x30, 0x2f, 0xc2, 0x42, 0x98, 0x2e, 0x83, 0xb1, 0xe1, 0x6f, 0xb3, 0x35, 0xcb, 0x00, + 0x24, 0x3d, 0xc2, 0x40, 0xdc, 0x00, 0x84, 0x40, 0x00, 0x91, 0x93, 0x7f, 0x02, 0x2f, 0x00, 0x2e, 0x06, 0x2c, 0x0c, + 0xb8, 0x30, 0x25, 0x00, 0x33, 0x48, 0x00, 0x98, 0x2e, 0xf6, 0xb6, 0x91, 0x6f, 0x90, 0x7f, 0x00, 0x2e, 0x44, 0x40, + 0x20, 0x1a, 0x15, 0x2f, 0xd3, 0x6f, 0xc1, 0x6f, 0xc3, 0x40, 0x35, 0x5a, 0x42, 0x40, 0xd3, 0x7e, 0x08, 0xbc, 0x25, + 0x09, 0xe2, 0x7e, 0xc4, 0x0a, 0x42, 0x82, 0xf3, 0x7e, 0xd1, 0x7f, 0x34, 0x30, 0x83, 0x6f, 0x82, 0x30, 0x31, 0x30, + 0x98, 0x2e, 0xb3, 0x00, 0xd1, 0x6f, 0x93, 0x6f, 0x43, 0x42, 0xf0, 0x32, 0xb1, 0x6f, 0x41, 0x82, 0xe2, 0x6f, 0x43, + 0x40, 0xc1, 0x86, 0xc2, 0xa2, 0x43, 0x42, 0x03, 0x30, 0x02, 0x2f, 0x90, 0x00, 0x00, 0x2e, 0x83, 0x42, 0x61, 0x88, + 0x42, 0x40, 0x8d, 0xb0, 0x26, 0x00, 0x98, 0x2e, 0xd9, 0x03, 0x1c, 0x83, 0x00, 0x2e, 0x43, 0x42, 0x00, 0x2e, 0xab, + 0x6f, 0xa0, 0x5e, 0xb8, 0x2e, 0xb1, 0x35, 0x40, 0x51, 0x41, 0x01, 0x02, 0x30, 0x1a, 0x25, 0x13, 0x30, 0x40, 0x25, + 0x12, 0x42, 0x45, 0x0e, 0xfc, 0x2f, 0x65, 0x34, 0x28, 0x80, 0x25, 0x01, 0x13, 0x42, 0x44, 0x0e, 0xfc, 0x2f, 0x27, + 0x80, 0x65, 0x56, 0x03, 0x42, 0x15, 0x80, 0xa3, 0x30, 0x03, 0x42, 0x04, 0x80, 0x4d, 0x56, 0x7f, 0x58, 0x13, 0x42, + 0xd4, 0x7e, 0xc2, 0x7e, 0xf2, 0x7e, 0x6c, 0x8c, 0x81, 0x56, 0x83, 0x58, 0xe3, 0x7e, 0x04, 0x7f, 0x71, 0x8a, 0x97, + 0x41, 0x17, 0x42, 0x75, 0x0e, 0xfb, 0x2f, 0x85, 0x5c, 0x87, 0x5e, 0x16, 0x7f, 0x36, 0x7f, 0x27, 0x7f, 0x00, 0x2e, + 0x89, 0x5c, 0x8b, 0x5e, 0x46, 0x7f, 0x57, 0x7f, 0x76, 0x8c, 0x57, 0x41, 0x17, 0x42, 0x6e, 0x0e, 0xfb, 0x2f, 0x8d, + 0x5a, 0x8f, 0x5e, 0x65, 0x7f, 0x87, 0x7f, 0x72, 0x7f, 0x00, 0x2e, 0x91, 0x5a, 0x93, 0x5e, 0x95, 0x7f, 0xa7, 0x7f, + 0x7b, 0x8a, 0x97, 0x41, 0x17, 0x42, 0x75, 0x0e, 0xfb, 0x2f, 0x7f, 0x5c, 0xb2, 0x7f, 0xc6, 0x7f, 0xd3, 0x7f, 0xe2, + 0x7f, 0xf4, 0x7f, 0x40, 0x82, 0x52, 0x41, 0x12, 0x42, 0x69, 0x0e, 0xfb, 0x2f, 0xc0, 0x5e, 0xb8, 0x2e, 0x03, 0x2e, + 0x2d, 0x02, 0x9f, 0xbc, 0x9f, 0xb8, 0x20, 0x50, 0x40, 0xb2, 0x14, 0x2f, 0x10, 0x25, 0x01, 0x2e, 0x8d, 0x00, 0x00, + 0x90, 0x0b, 0x2f, 0x97, 0x50, 0xf1, 0x7f, 0xeb, 0x7f, 0x98, 0x2e, 0x83, 0xb6, 0x01, 0x2e, 0x8d, 0x00, 0x01, 0x80, + 0x21, 0x2e, 0x8d, 0x00, 0xf1, 0x6f, 0xeb, 0x6f, 0xe0, 0x5f, 0x97, 0x50, 0x80, 0x2e, 0xda, 0xb4, 0x00, 0x30, 0x21, + 0x2e, 0x8d, 0x00, 0xe0, 0x5f, 0xb8, 0x2e, 0x41, 0x25, 0x42, 0x8a, 0x50, 0x50, 0x99, 0x52, 0x81, 0x80, 0x99, 0x09, + 0xf5, 0x7f, 0x52, 0x25, 0x07, 0x52, 0x03, 0x8e, 0xd9, 0x08, 0x02, 0x40, 0x03, 0x81, 0x44, 0x83, 0x6c, 0xbb, 0xda, + 0x0e, 0xe7, 0x7f, 0xdb, 0x7f, 0x20, 0x2f, 0x02, 0x41, 0x32, 0x1a, 0x1d, 0x2f, 0x42, 0x85, 0x00, 0x2e, 0x82, 0x40, + 0xda, 0x0e, 0x03, 0x30, 0x05, 0x2f, 0xf1, 0x6f, 0x06, 0x30, 0x42, 0x40, 0x81, 0x84, 0x18, 0x2c, 0x42, 0x42, 0xbf, + 0x85, 0x82, 0x00, 0x41, 0x40, 0x86, 0x40, 0x81, 0x8d, 0x86, 0x42, 0x20, 0x25, 0x13, 0x30, 0x06, 0x30, 0x97, 0x40, + 0x81, 0x8d, 0xf9, 0x0f, 0x09, 0x2f, 0x85, 0xa3, 0xf9, 0x2f, 0x03, 0x30, 0x06, 0x2c, 0x06, 0x30, 0x9b, 0x52, 0xd9, + 0x0e, 0x13, 0x30, 0x01, 0x30, 0xd9, 0x22, 0xc0, 0xb2, 0x12, 0x83, 0xc1, 0x7f, 0x03, 0x30, 0xb4, 0x7f, 0x06, 0x2f, + 0x51, 0x30, 0x70, 0x25, 0x98, 0x2e, 0xe3, 0x03, 0xff, 0x81, 0x00, 0x2e, 0x03, 0x42, 0x43, 0x8b, 0xe0, 0x6f, 0xf1, + 0x6f, 0x00, 0x40, 0x41, 0x40, 0xc8, 0x0f, 0x37, 0x2f, 0x00, 0x41, 0x80, 0xa7, 0x3c, 0x2f, 0x01, 0x83, 0x47, 0x8e, + 0x42, 0x40, 0xfa, 0x01, 0x81, 0x84, 0x08, 0x89, 0x45, 0x41, 0x55, 0x0e, 0xc6, 0x43, 0x42, 0x42, 0xf4, 0x7f, 0x00, + 0x2f, 0x43, 0x42, 0x51, 0x82, 0x70, 0x1a, 0x41, 0x40, 0x06, 0x2f, 0xc3, 0x6f, 0x41, 0x82, 0xc1, 0x42, 0xcd, 0x0e, + 0x26, 0x2f, 0xc5, 0x42, 0x25, 0x2d, 0x7f, 0x82, 0x51, 0xbb, 0xa5, 0x00, 0xce, 0x0f, 0x12, 0x2f, 0x14, 0x30, 0x05, + 0x30, 0xf7, 0x6f, 0x06, 0x30, 0x05, 0x2c, 0xe0, 0x7f, 0xd0, 0x41, 0x05, 0x1a, 0x23, 0x22, 0xb0, 0x01, 0x7a, 0x0e, + 0xf9, 0x2f, 0x71, 0x0f, 0xe0, 0x6f, 0x28, 0x22, 0x41, 0x8b, 0x71, 0x22, 0x45, 0xa7, 0xee, 0x2f, 0xb3, 0x6f, 0xc2, + 0x6f, 0xc0, 0x42, 0x81, 0x42, 0x08, 0x2d, 0x04, 0x25, 0xc4, 0x6f, 0x98, 0x2e, 0xea, 0x03, 0x00, 0x2e, 0x40, 0x41, + 0x00, 0x43, 0x00, 0x30, 0xdb, 0x6f, 0xb0, 0x5f, 0xb8, 0x2e, 0x10, 0x50, 0x03, 0x40, 0x19, 0x18, 0x37, 0x56, 0x19, + 0x05, 0x36, 0x25, 0xf7, 0x7f, 0x4a, 0x17, 0x54, 0x18, 0xec, 0x18, 0x09, 0x17, 0x01, 0x30, 0x0c, 0x07, 0xe2, 0x18, + 0xde, 0x00, 0xf2, 0x6f, 0x97, 0x02, 0x33, 0x58, 0xdc, 0x00, 0x91, 0x02, 0xbf, 0xb8, 0x21, 0xbd, 0x8a, 0x0a, 0xc0, + 0x2e, 0x02, 0x42, 0xf0, 0x5f, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x01, 0x2e, 0x5d, 0xf7, 0x08, 0xbc, 0x80, 0xac, 0x0e, 0xbb, 0x02, 0x2f, + 0x00, 0x30, 0x41, 0x04, 0x82, 0x06, 0xc0, 0xa4, 0x00, 0x30, 0x11, 0x2f, 0x40, 0xa9, 0x03, 0x2f, 0x40, 0x91, 0x0d, + 0x2f, 0x00, 0xa7, 0x0b, 0x2f, 0x80, 0xb3, 0x33, 0x58, 0x02, 0x2f, 0x90, 0xa1, 0x26, 0x13, 0x20, 0x23, 0x80, 0x90, + 0x10, 0x30, 0x01, 0x2f, 0xcc, 0x0e, 0x00, 0x2f, 0x00, 0x30, 0xb8, 0x2e, 0x35, 0x50, 0x18, 0x08, 0x08, 0xbc, 0x88, + 0xb6, 0x0d, 0x17, 0xc6, 0xbd, 0x56, 0xbc, 0x37, 0x58, 0xda, 0xba, 0x04, 0x01, 0x1d, 0x0a, 0x10, 0x50, 0x05, 0x30, + 0x32, 0x25, 0x45, 0x03, 0xfb, 0x7f, 0xf6, 0x30, 0x21, 0x25, 0x98, 0x2e, 0x37, 0xca, 0x16, 0xb5, 0x9a, 0xbc, 0x06, + 0xb8, 0x80, 0xa8, 0x41, 0x0a, 0x0e, 0x2f, 0x80, 0x90, 0x02, 0x2f, 0x39, 0x50, 0x48, 0x0f, 0x09, 0x2f, 0xbf, 0xa0, + 0x04, 0x2f, 0xbf, 0x90, 0x06, 0x2f, 0x37, 0x54, 0xca, 0x0f, 0x03, 0x2f, 0x00, 0x2e, 0x02, 0x2c, 0x37, 0x52, 0x39, + 0x52, 0xf2, 0x33, 0x98, 0x2e, 0xd9, 0xc0, 0xfb, 0x6f, 0xf1, 0x37, 0xc0, 0x2e, 0x01, 0x08, 0xf0, 0x5f, 0x41, 0x56, + 0x3b, 0x54, 0xd0, 0x40, 0xc4, 0x40, 0x0b, 0x2e, 0xfd, 0xf3, 0x41, 0x52, 0x90, 0x42, 0x94, 0x42, 0x95, 0x42, 0x05, + 0x30, 0x43, 0x50, 0x0f, 0x88, 0x06, 0x40, 0x04, 0x41, 0x96, 0x42, 0xc5, 0x42, 0x48, 0xbe, 0x73, 0x30, 0x0d, 0x2e, + 0x88, 0x00, 0x4f, 0xba, 0x84, 0x42, 0x03, 0x42, 0x81, 0xb3, 0x02, 0x2f, 0x2b, 0x2e, 0x6f, 0xf5, 0x06, 0x2d, 0x05, + 0x2e, 0x77, 0xf7, 0x3f, 0x56, 0x93, 0x08, 0x25, 0x2e, 0x77, 0xf7, 0x3d, 0x54, 0x25, 0x2e, 0xc2, 0xf5, 0x07, 0x2e, + 0xfd, 0xf3, 0x42, 0x30, 0xb4, 0x33, 0xda, 0x0a, 0x4c, 0x00, 0x27, 0x2e, 0xfd, 0xf3, 0x43, 0x40, 0xd4, 0x3f, 0xdc, + 0x08, 0x43, 0x42, 0x00, 0x2e, 0x00, 0x2e, 0x43, 0x40, 0x24, 0x30, 0xdc, 0x0a, 0x43, 0x42, 0x04, 0x80, 0x03, 0x2e, + 0xfd, 0xf3, 0x4a, 0x0a, 0x23, 0x2e, 0xfd, 0xf3, 0x61, 0x34, 0xc0, 0x2e, 0x01, 0x42, 0x00, 0x2e, 0x60, 0x50, 0x1a, + 0x25, 0x7a, 0x86, 0xe0, 0x7f, 0xf3, 0x7f, 0x03, 0x25, 0x45, 0x52, 0x41, 0x84, 0xdb, 0x7f, 0x33, 0x30, 0x98, 0x2e, + 0x16, 0xc2, 0x1a, 0x25, 0x7d, 0x82, 0xf0, 0x6f, 0xe2, 0x6f, 0x32, 0x25, 0x16, 0x40, 0x94, 0x40, 0x26, 0x01, 0x85, + 0x40, 0x8e, 0x17, 0xc4, 0x42, 0x6e, 0x03, 0x95, 0x42, 0x41, 0x0e, 0xf4, 0x2f, 0xdb, 0x6f, 0xa0, 0x5f, 0xb8, 0x2e, + 0xb0, 0x51, 0xfb, 0x7f, 0x98, 0x2e, 0xe8, 0x0d, 0x5a, 0x25, 0x98, 0x2e, 0x0f, 0x0e, 0x4f, 0x58, 0x32, 0x87, 0xc4, + 0x7f, 0x65, 0x89, 0x6b, 0x8d, 0x47, 0x5a, 0x65, 0x7f, 0xe1, 0x7f, 0x83, 0x7f, 0xa6, 0x7f, 0x74, 0x7f, 0xd0, 0x7f, + 0xb6, 0x7f, 0x94, 0x7f, 0x17, 0x30, 0x49, 0x52, 0x4b, 0x54, 0x51, 0x7f, 0x00, 0x2e, 0x85, 0x6f, 0x42, 0x7f, 0x00, + 0x2e, 0x51, 0x41, 0x45, 0x81, 0x42, 0x41, 0x13, 0x40, 0x3b, 0x8a, 0x00, 0x40, 0x4b, 0x04, 0xd0, 0x06, 0xc0, 0xac, + 0x85, 0x7f, 0x02, 0x2f, 0x02, 0x30, 0x51, 0x04, 0xd3, 0x06, 0x41, 0x84, 0x05, 0x30, 0x5d, 0x02, 0xc9, 0x16, 0xdf, + 0x08, 0xd3, 0x00, 0x8d, 0x02, 0xaf, 0xbc, 0xb1, 0xb9, 0x59, 0x0a, 0x65, 0x6f, 0x11, 0x43, 0xa1, 0xb4, 0x52, 0x41, + 0x53, 0x41, 0x01, 0x43, 0x34, 0x7f, 0x65, 0x7f, 0x26, 0x31, 0xe5, 0x6f, 0xd4, 0x6f, 0x98, 0x2e, 0x37, 0xca, 0x32, + 0x6f, 0x75, 0x6f, 0x83, 0x40, 0x42, 0x41, 0x23, 0x7f, 0x12, 0x7f, 0xf6, 0x30, 0x40, 0x25, 0x51, 0x25, 0x98, 0x2e, + 0x37, 0xca, 0x14, 0x6f, 0x20, 0x05, 0x70, 0x6f, 0x25, 0x6f, 0x69, 0x07, 0xa2, 0x6f, 0x31, 0x6f, 0x0b, 0x30, 0x04, + 0x42, 0x9b, 0x42, 0x8b, 0x42, 0x55, 0x42, 0x32, 0x7f, 0x40, 0xa9, 0xc3, 0x6f, 0x71, 0x7f, 0x02, 0x30, 0xd0, 0x40, + 0xc3, 0x7f, 0x03, 0x2f, 0x40, 0x91, 0x15, 0x2f, 0x00, 0xa7, 0x13, 0x2f, 0x00, 0xa4, 0x11, 0x2f, 0x84, 0xbd, 0x98, + 0x2e, 0x79, 0xca, 0x55, 0x6f, 0x37, 0x54, 0x54, 0x41, 0x82, 0x00, 0xf3, 0x3f, 0x45, 0x41, 0xcb, 0x02, 0xf6, 0x30, + 0x98, 0x2e, 0x37, 0xca, 0x35, 0x6f, 0xa4, 0x6f, 0x41, 0x43, 0x03, 0x2c, 0x00, 0x43, 0xa4, 0x6f, 0x35, 0x6f, 0x17, + 0x30, 0x42, 0x6f, 0x51, 0x6f, 0x93, 0x40, 0x42, 0x82, 0x00, 0x41, 0xc3, 0x00, 0x03, 0x43, 0x51, 0x7f, 0x00, 0x2e, + 0x94, 0x40, 0x41, 0x41, 0x4c, 0x02, 0xc4, 0x6f, 0x55, 0x56, 0x63, 0x0e, 0x74, 0x6f, 0x51, 0x43, 0xa5, 0x7f, 0x8a, + 0x2f, 0x09, 0x2e, 0x88, 0x00, 0x01, 0xb3, 0x21, 0x2f, 0x4f, 0x58, 0x90, 0x6f, 0x13, 0x41, 0xb6, 0x6f, 0xe4, 0x7f, + 0x00, 0x2e, 0x91, 0x41, 0x14, 0x40, 0x92, 0x41, 0x15, 0x40, 0x17, 0x2e, 0x6f, 0xf5, 0xb6, 0x7f, 0xd0, 0x7f, 0xcb, + 0x7f, 0x98, 0x2e, 0x00, 0x0c, 0x07, 0x15, 0xc2, 0x6f, 0x14, 0x0b, 0x29, 0x2e, 0x6f, 0xf5, 0xc3, 0xa3, 0xc1, 0x8f, + 0xe4, 0x6f, 0xd0, 0x6f, 0xe6, 0x2f, 0x14, 0x30, 0x05, 0x2e, 0x6f, 0xf5, 0x14, 0x0b, 0x29, 0x2e, 0x6f, 0xf5, 0x18, + 0x2d, 0x51, 0x56, 0x04, 0x32, 0xb5, 0x6f, 0x1c, 0x01, 0x51, 0x41, 0x52, 0x41, 0xc3, 0x40, 0xb5, 0x7f, 0xe4, 0x7f, + 0x98, 0x2e, 0x1f, 0x0c, 0xe4, 0x6f, 0x21, 0x87, 0x00, 0x43, 0x04, 0x32, 0x53, 0x54, 0x5a, 0x0e, 0xef, 0x2f, 0x4d, + 0x54, 0x09, 0x2e, 0x77, 0xf7, 0x22, 0x0b, 0x29, 0x2e, 0x77, 0xf7, 0xfb, 0x6f, 0x50, 0x5e, 0xb8, 0x2e, 0x10, 0x50, + 0x01, 0x2e, 0x84, 0x00, 0x00, 0xb2, 0xfb, 0x7f, 0x51, 0x2f, 0x01, 0xb2, 0x48, 0x2f, 0x02, 0xb2, 0x42, 0x2f, 0x03, + 0x90, 0x56, 0x2f, 0x5b, 0x52, 0x79, 0x80, 0x42, 0x40, 0x81, 0x84, 0x00, 0x40, 0x42, 0x42, 0x98, 0x2e, 0x93, 0x0c, + 0x5d, 0x54, 0x5b, 0x50, 0xa1, 0x40, 0x98, 0xbd, 0x82, 0x40, 0x3e, 0x82, 0xda, 0x0a, 0x44, 0x40, 0x8b, 0x16, 0xe3, + 0x00, 0x53, 0x42, 0x00, 0x2e, 0x43, 0x40, 0x9a, 0x02, 0x52, 0x42, 0x00, 0x2e, 0x41, 0x40, 0x4d, 0x54, 0x4a, 0x0e, + 0x3a, 0x2f, 0x3a, 0x82, 0x00, 0x30, 0x41, 0x40, 0x21, 0x2e, 0x6c, 0x0f, 0x40, 0xb2, 0x0a, 0x2f, 0x98, 0x2e, 0xb1, + 0x0c, 0x98, 0x2e, 0x45, 0x0e, 0x98, 0x2e, 0x5b, 0x0e, 0xfb, 0x6f, 0xf0, 0x5f, 0x00, 0x30, 0x80, 0x2e, 0xba, 0x03, + 0x61, 0x52, 0x57, 0x54, 0x42, 0x42, 0x4f, 0x84, 0x73, 0x30, 0x5f, 0x52, 0x83, 0x42, 0x1b, 0x30, 0x6b, 0x42, 0x23, + 0x30, 0x27, 0x2e, 0x87, 0x00, 0x37, 0x2e, 0x84, 0x00, 0x21, 0x2e, 0x86, 0x00, 0x7a, 0x84, 0x17, 0x2c, 0x42, 0x42, + 0x30, 0x30, 0x21, 0x2e, 0x84, 0x00, 0x12, 0x2d, 0x21, 0x30, 0x00, 0x30, 0x23, 0x2e, 0x84, 0x00, 0x21, 0x2e, 0x7b, + 0xf7, 0x0b, 0x2d, 0x17, 0x30, 0x98, 0x2e, 0x51, 0x0c, 0x59, 0x50, 0x0c, 0x82, 0x72, 0x30, 0x2f, 0x2e, 0x84, 0x00, + 0x25, 0x2e, 0x7b, 0xf7, 0x40, 0x42, 0x00, 0x2e, 0xfb, 0x6f, 0xf0, 0x5f, 0xb8, 0x2e, 0x70, 0x50, 0x0a, 0x25, 0x39, + 0x86, 0xfb, 0x7f, 0xe1, 0x32, 0x62, 0x30, 0x98, 0x2e, 0xc2, 0xc4, 0x35, 0x56, 0xa5, 0x6f, 0xab, 0x08, 0x91, 0x6f, + 0x4b, 0x08, 0x63, 0x56, 0xc4, 0x6f, 0x23, 0x09, 0x4d, 0xba, 0x93, 0xbc, 0x8c, 0x0b, 0xd1, 0x6f, 0x0b, 0x09, 0x4f, + 0x52, 0x65, 0x5e, 0x56, 0x42, 0xaf, 0x09, 0x4d, 0xba, 0x23, 0xbd, 0x94, 0x0a, 0xe5, 0x6f, 0x68, 0xbb, 0xeb, 0x08, + 0xbd, 0xb9, 0x63, 0xbe, 0xfb, 0x6f, 0x52, 0x42, 0xe3, 0x0a, 0xc0, 0x2e, 0x43, 0x42, 0x90, 0x5f, 0x55, 0x50, 0x03, + 0x2e, 0x25, 0xf3, 0x13, 0x40, 0x00, 0x40, 0x9b, 0xbc, 0x9b, 0xb4, 0x08, 0xbd, 0xb8, 0xb9, 0x98, 0xbc, 0xda, 0x0a, + 0x08, 0xb6, 0x89, 0x16, 0xc0, 0x2e, 0x19, 0x00, 0x62, 0x02, 0x10, 0x50, 0xfb, 0x7f, 0x98, 0x2e, 0x81, 0x0d, 0x01, + 0x2e, 0x84, 0x00, 0x31, 0x30, 0x08, 0x04, 0xfb, 0x6f, 0x01, 0x30, 0xf0, 0x5f, 0x23, 0x2e, 0x86, 0x00, 0x21, 0x2e, + 0x87, 0x00, 0xb8, 0x2e, 0x01, 0x2e, 0x87, 0x00, 0x03, 0x2e, 0x86, 0x00, 0x48, 0x0e, 0x01, 0x2f, 0x80, 0x2e, 0x1f, + 0x0e, 0xb8, 0x2e, 0x67, 0x50, 0x21, 0x34, 0x01, 0x42, 0x82, 0x30, 0xc1, 0x32, 0x25, 0x2e, 0x62, 0xf5, 0x01, 0x00, + 0x22, 0x30, 0x01, 0x40, 0x4a, 0x0a, 0x01, 0x42, 0xb8, 0x2e, 0x67, 0x54, 0xf0, 0x3b, 0x83, 0x40, 0xd8, 0x08, 0x69, + 0x52, 0x83, 0x42, 0x00, 0x30, 0x83, 0x30, 0x50, 0x42, 0xc4, 0x32, 0x27, 0x2e, 0x64, 0xf5, 0x94, 0x00, 0x50, 0x42, + 0x40, 0x42, 0xd3, 0x3f, 0x84, 0x40, 0x7d, 0x82, 0xe3, 0x08, 0x40, 0x42, 0x83, 0x42, 0xb8, 0x2e, 0x61, 0x52, 0x00, + 0x30, 0x40, 0x42, 0x7c, 0x86, 0x3b, 0x52, 0x09, 0x2e, 0x57, 0x0f, 0x41, 0x54, 0xc4, 0x42, 0xd3, 0x86, 0x54, 0x40, + 0x55, 0x40, 0x94, 0x42, 0x85, 0x42, 0x21, 0x2e, 0x87, 0x00, 0x42, 0x40, 0x25, 0x2e, 0xfd, 0xf3, 0xc0, 0x42, 0x7e, + 0x82, 0x05, 0x2e, 0x79, 0x00, 0x80, 0xb2, 0x14, 0x2f, 0x05, 0x2e, 0x24, 0x02, 0x27, 0xbd, 0x2f, 0xb9, 0x80, 0x90, + 0x02, 0x2f, 0x21, 0x2e, 0x6f, 0xf5, 0x0c, 0x2d, 0x07, 0x2e, 0x58, 0x0f, 0x14, 0x30, 0x1c, 0x09, 0x05, 0x2e, 0x77, + 0xf7, 0x3f, 0x56, 0x47, 0xbe, 0x93, 0x08, 0x94, 0x0a, 0x25, 0x2e, 0x77, 0xf7, 0x6b, 0x54, 0x50, 0x42, 0x4a, 0x0e, + 0xfc, 0x2f, 0xb8, 0x2e, 0x50, 0x50, 0x02, 0x30, 0x43, 0x86, 0x69, 0x50, 0xfb, 0x7f, 0xe3, 0x7f, 0xd2, 0x7f, 0xc0, + 0x7f, 0xb1, 0x7f, 0x00, 0x2e, 0x41, 0x40, 0x00, 0x40, 0x48, 0x04, 0x98, 0x2e, 0x74, 0xc0, 0x1e, 0xaa, 0xd3, 0x6f, + 0x14, 0x30, 0xb1, 0x6f, 0xe3, 0x22, 0xc0, 0x6f, 0x52, 0x40, 0xe4, 0x6f, 0x4c, 0x0e, 0x12, 0x42, 0xd3, 0x7f, 0xeb, + 0x2f, 0x03, 0x2e, 0x6d, 0x0f, 0x40, 0x90, 0x11, 0x30, 0x03, 0x2f, 0x23, 0x2e, 0x6d, 0x0f, 0x02, 0x2c, 0x00, 0x30, + 0xd0, 0x6f, 0xfb, 0x6f, 0xb0, 0x5f, 0xb8, 0x2e, 0x40, 0x50, 0xf1, 0x7f, 0x0a, 0x25, 0x3c, 0x86, 0xeb, 0x7f, 0x41, + 0x33, 0x22, 0x30, 0x98, 0x2e, 0xc2, 0xc4, 0xd3, 0x6f, 0xf4, 0x30, 0xdc, 0x09, 0x6f, 0x58, 0xc2, 0x6f, 0x94, 0x09, + 0x71, 0x58, 0x6a, 0xbb, 0xdc, 0x08, 0xb4, 0xb9, 0xb1, 0xbd, 0x6d, 0x5a, 0x95, 0x08, 0x21, 0xbd, 0xf6, 0xbf, 0x77, + 0x0b, 0x51, 0xbe, 0xf1, 0x6f, 0xeb, 0x6f, 0x52, 0x42, 0x54, 0x42, 0xc0, 0x2e, 0x43, 0x42, 0xc0, 0x5f, 0x50, 0x50, + 0x75, 0x52, 0x93, 0x30, 0x53, 0x42, 0xfb, 0x7f, 0x7b, 0x30, 0x4b, 0x42, 0x13, 0x30, 0x42, 0x82, 0x20, 0x33, 0x43, + 0x42, 0xc8, 0x00, 0x01, 0x2e, 0x80, 0x03, 0x05, 0x2e, 0x7d, 0x00, 0x19, 0x52, 0xe2, 0x7f, 0xd0, 0x7f, 0xc3, 0x7f, + 0x98, 0x2e, 0xb6, 0x0e, 0xd1, 0x6f, 0x48, 0x0a, 0xd1, 0x7f, 0x3a, 0x25, 0xfb, 0x86, 0x01, 0x33, 0x12, 0x30, 0x98, + 0x2e, 0xc2, 0xc4, 0xd1, 0x6f, 0x48, 0x0a, 0x40, 0xb2, 0x0d, 0x2f, 0xe0, 0x6f, 0x03, 0x2e, 0x80, 0x03, 0x53, 0x30, + 0x07, 0x80, 0x27, 0x2e, 0x21, 0xf2, 0x98, 0xbc, 0x01, 0x42, 0x98, 0x2e, 0x91, 0x03, 0x00, 0x2e, 0x00, 0x2e, 0xd0, + 0x2e, 0xb1, 0x6f, 0x9b, 0xb8, 0x07, 0x2e, 0x1b, 0x00, 0x19, 0x1a, 0xb1, 0x7f, 0x71, 0x30, 0x04, 0x2f, 0x23, 0x2e, + 0x21, 0xf2, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0x98, 0x2e, 0x6d, 0xc0, 0x98, 0x2e, 0x5d, 0xc0, 0x98, 0x2e, 0xdf, + 0x03, 0x20, 0x26, 0xc1, 0x6f, 0x02, 0x31, 0x52, 0x42, 0xab, 0x30, 0x4b, 0x42, 0x20, 0x33, 0x77, 0x56, 0xf1, 0x37, + 0xc4, 0x40, 0xa2, 0x0a, 0xc2, 0x42, 0xd8, 0x00, 0x01, 0x2e, 0x5e, 0xf7, 0x41, 0x08, 0x23, 0x2e, 0x94, 0x00, 0xe3, + 0x7f, 0x98, 0x2e, 0xaa, 0x01, 0xe1, 0x6f, 0x83, 0x30, 0x43, 0x42, 0x03, 0x30, 0xfb, 0x6f, 0x73, 0x50, 0x02, 0x30, + 0x00, 0x2e, 0x00, 0x2e, 0x81, 0x84, 0x50, 0x0e, 0xfa, 0x2f, 0x43, 0x42, 0x11, 0x30, 0xb0, 0x5f, 0x23, 0x2e, 0x21, + 0xf2, 0xb8, 0x2e, 0xc1, 0x4a, 0x00, 0x00, 0x6d, 0x57, 0x00, 0x00, 0x77, 0x8e, 0x00, 0x00, 0xe0, 0xff, 0xff, 0xff, + 0xd3, 0xff, 0xff, 0xff, 0xe5, 0xff, 0xff, 0xff, 0xee, 0xe1, 0xff, 0xff, 0x7c, 0x13, 0x00, 0x00, 0x46, 0xe6, 0xff, + 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0xfd, 0x2d +}; + +/*! @name Global array that stores the feature input configuration of + * BMI270_CONTEXT + */ +const struct bmi2_feature_config bmi270_context_feat_in[BMI270_CONTEXT_MAX_FEAT_IN] = { + { .type = BMI2_CONFIG_ID, .page = BMI2_PAGE_4, .start_addr = BMI270_CONTEXT_CONFIG_ID_STRT_ADDR }, + { .type = BMI2_STEP_COUNTER_PARAMS, .page = BMI2_PAGE_1, .start_addr = BMI270_CONTEXT_STEP_CNT_1_STRT_ADDR }, + { .type = BMI2_STEP_DETECTOR, .page = BMI2_PAGE_4, .start_addr = BMI270_CONTEXT_STEP_CNT_4_STRT_ADDR }, + { .type = BMI2_STEP_COUNTER, .page = BMI2_PAGE_4, .start_addr = BMI270_CONTEXT_STEP_CNT_4_STRT_ADDR }, + { .type = BMI2_NVM_PROG_PREP, .page = BMI2_PAGE_4, .start_addr = BMI270_CONTEXT_NVM_PROG_PREP_STRT_ADDR }, + { .type = BMI2_MAX_BURST_LEN, .page = BMI2_PAGE_4, .start_addr = BMI270_CONTEXT_MAX_BURST_LEN_STRT_ADDR }, + { .type = BMI2_CRT_GYRO_SELF_TEST, .page = BMI2_PAGE_4, .start_addr = BMI270_CONTEXT_CRT_GYRO_SELF_TEST_STRT_ADDR }, + { .type = BMI2_ABORT_CRT_GYRO_SELF_TEST, .page = BMI2_PAGE_4, .start_addr = BMI270_CONTEXT_ABORT_STRT_ADDR }, + { .type = BMI2_ACTIVITY_RECOGNITION_SETTINGS, .page = BMI2_PAGE_5, + .start_addr = BMI270_CONTEXT_ACT_RGN_SETT_STRT_ADDR }, + { .type = BMI2_ACTIVITY_RECOGNITION, .page = BMI2_PAGE_5, .start_addr = BMI270_CONTEXT_ACT_RGN_STRT_ADDR }, +}; + +/*! @name Global array that stores the feature output configuration */ +const struct bmi2_feature_config bmi270_context_feat_out[BMI270_CONTEXT_MAX_FEAT_OUT] = { + { .type = BMI2_STEP_COUNTER, .page = BMI2_PAGE_0, .start_addr = BMI270_CONTEXT_STEP_CNT_OUT_STRT_ADDR }, + { .type = BMI2_GYRO_CROSS_SENSE, .page = BMI2_PAGE_0, .start_addr = BMI270_CONTEXT_GYRO_CROSS_SENSE_STRT_ADDR }, + { .type = BMI2_GYRO_GAIN_UPDATE, .page = BMI2_PAGE_0, .start_addr = BMI270_CONTEXT_GYR_USER_GAIN_OUT_STRT_ADDR }, + { .type = BMI2_NVM_STATUS, .page = BMI2_PAGE_0, .start_addr = BMI270_CONTEXT_NVM_VFRM_OUT_STRT_ADDR }, + { .type = BMI2_VFRM_STATUS, .page = BMI2_PAGE_0, .start_addr = BMI270_CONTEXT_NVM_VFRM_OUT_STRT_ADDR } +}; + +/*! @name Global array that stores the feature interrupts of BMI270_CONTEXT */ +struct bmi2_map_int bmi270_c_map_int[BMI270_C_MAX_INT_MAP] = { + { .type = BMI2_STEP_COUNTER, .sens_map_int = BMI270_C_INT_STEP_COUNTER_MASK }, + { .type = BMI2_STEP_DETECTOR, .sens_map_int = BMI270_C_INT_STEP_DETECTOR_MASK }, +}; + +/******************************************************************************/ + +/*! Local Function Prototypes + ******************************************************************************/ + +/*! + * @brief This internal API is used to validate the device pointer for + * null conditions. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t null_ptr_check(const struct bmi2_dev *dev); + +/*! + * @brief This internal API enables the selected sensor/features. + * + * @param[in] sensor_sel : Selects the desired sensor. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t sensor_enable(uint64_t sensor_sel, struct bmi2_dev *dev); + +/*! + * @brief This internal API disables the selected sensor/features. + * + * @param[in] sensor_sel : Selects the desired sensor. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t sensor_disable(uint64_t sensor_sel, struct bmi2_dev *dev); + +/*! + * @brief This internal API selects the sensors/features to be enabled or + * disabled. + * + * @param[in] sens_list : Pointer to select the sensor. + * @param[in] n_sens : Number of sensors selected. + * @param[out] sensor_sel : Gets the selected sensor. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t select_sensor(const uint8_t *sens_list, uint8_t n_sens, uint64_t *sensor_sel); + +/*! + * @brief This internal API is used to enable/disable step detector feature. + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[in] enable : Enables/Disables step-detector. + * + * Enable | Description + * -------------|--------------- + * BMI2_DISABLE | Disables step detector + * BMI2_ENABLE | Enables step detector + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_step_detector(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to enable/disable step counter feature. + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[in] enable : Enables/Disables step counter. + * + * Enable | Description + * -------------|--------------- + * BMI2_DISABLE | Disables step counter + * BMI2_ENABLE | Enables step counter + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_step_counter(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to enable/disable gyroscope user gain + * feature. + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[in] enable : Enables/Disables gyroscope user gain. + * + * Enable | Description + * -------------|--------------- + * BMI2_DISABLE | Disables gyroscope user gain + * BMI2_ENABLE | Enables gyroscope user gain + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_gyro_user_gain(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API enables/disables the activity recognition feature. + * + * @param[in] enable : Enables/Disables activity recognition. + * @param[in] dev : Structure instance of bmi2_dev. + * + * enable | Description + * -------------|--------------- + * BMI2_ENABLE | Enables activity recognition. + * BMI2_DISABLE | Disables activity recognition. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_act_recog(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API sets step counter parameter configurations. + * + * @param[in] step_count_params : Array that stores parameters 1 to 25. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_step_count_params_config(const uint16_t *step_count_params, struct bmi2_dev *dev); + +/*! + * @brief This internal API sets step counter/detector/activity configurations. + * + * @param[in] config : Structure instance of bmi2_step_config. + * @param[in] dev : Structure instance of bmi2_dev. + * + *--------------------------------------------------------------------------- + * bmi2_step_config | + * Structure parameters | Description + *--------------------------|-------------------------------------------------- + * | The Step-counter will trigger output every time + * | the number of steps are counted. Holds implicitly + * water-mark level | a 20x factor, so the range is 0 to 10230, + * | with resolution of 20 steps. + * -------------------------|--------------------------------------------------- + * reset counter | Flag to reset the counted steps. + * -------------------------|--------------------------------------------------- + * @endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_step_config(const struct bmi2_step_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets step counter parameter configurations. + * + * @param[in] step_count_params : Array that stores parameters 1 to 25. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_step_count_params_config(uint16_t *step_count_params, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets step counter/detector/activity configurations. + * + * @param[out] config : Structure instance of bmi2_step_config. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @verbatim + *---------------------------------------------------------------------------- + * bmi2_step_config | + * Structure parameters | Description + *--------------------------|-------------------------------------------------- + * | The Step-counter will trigger output every time + * | the number of steps are counted. Holds implicitly + * water-mark level | a 20x factor, so the range is 0 to 10230, + * | with resolution of 20 steps. + * -------------------------|--------------------------------------------------- + * reset counter | Flag to reset the counted steps. + * -------------------------|--------------------------------------------------- + * @endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_step_config(struct bmi2_step_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to parse and store the activity recognition + * output from the FIFO data. + * + * @param[out] act_recog : Structure to retrieve output of activity + * recognition frame. + * @param[in] data_index : Index of the FIFO data which contains + * activity recognition frame. + * @param[out] fifo : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t unpack_act_recog_output(struct bmi2_act_recog_output *act_recog, + uint16_t *data_index, + const struct bmi2_fifo_frame *fifo); + +/*! + * @brief This internal API gets the output values of step counter. + * + * @param[out] step_count : Pointer to the stored step counter data. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_step_counter_output(uint32_t *step_count, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets the error status related to NVM. + * + * @param[out] nvm_err_stat : Stores the NVM error status. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_nvm_error_status(struct bmi2_nvm_err_status *nvm_err_stat, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets the error status related to virtual frames. + * + * @param[out] vfrm_err_stat : Stores the VFRM related error status. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_vfrm_error_status(struct bmi2_vfrm_err_status *vfrm_err_stat, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to get enable status of gyroscope user gain + * update. + * + * @param[out] status : Stores status of gyroscope user gain update. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_user_gain_upd_status(uint8_t *status, struct bmi2_dev *dev); + +/*! + * @brief This internal API skips S4S frame in the FIFO data while getting + * activity recognition output. + * + * @param[in, out] frame_header : FIFO frame header. + * @param[in, out] data_index : Index value of the FIFO data bytes + * from which S4S frame header is to be + * skipped. + * @param[in] fifo : Structure instance of bmi2_fifo_frame. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t move_if_s4s_frame(const uint8_t *frame_header, uint16_t *data_index, const struct bmi2_fifo_frame *fifo); + +/*! + * @brief This internal API enables/disables compensation of the gain defined + * in the GAIN register. + * + * @param[in] enable : Enables/Disables gain compensation + * @param[in] dev : Structure instance of bmi2_dev. + * + * enable | Description + * -------------|--------------- + * BMI2_ENABLE | Enable gain compensation. + * BMI2_DISABLE | Disable gain compensation. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t enable_gyro_gain(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to extract the output feature configuration + * details like page and start address from the look-up table. + * + * @param[out] feat_output : Structure that stores output feature + * configurations. + * @param[in] type : Type of feature or sensor. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Returns the feature found flag. + * + * @retval BMI2_FALSE : Feature not found + * BMI2_TRUE : Feature found + */ +static uint8_t extract_output_feat_config(struct bmi2_feature_config *feat_output, + uint8_t type, + const struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to move the data index ahead of the + * current frame length parameter when unnecessary FIFO data appears while + * extracting the user specified data. + * + * @param[in,out] data_index : Index of the FIFO data which is to be + * moved ahead of the current frame length + * @param[in] current_frame_length : Number of bytes in the current frame. + * @param[in] fifo : Structure instance of bmi2_fifo_frame. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t move_next_frame(uint16_t *data_index, uint8_t current_frame_length, const struct bmi2_fifo_frame *fifo); + +/***************************************************************************/ + +/*! User Interface Definitions + ****************************************************************************/ + +/*! + * @brief This API: + * 1) Updates the device structure with address of the configuration file. + * 2) Initializes BMI270_CONTEXT sensor. + * 3) Writes the configuration file. + * 4) Updates the feature offset parameters in the device structure. + * 5) Updates the maximum number of pages, in the device structure. + */ +int8_t bmi270_context_init(struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + /* Assign chip id of BMI270_CONTEXT */ + dev->chip_id = BMI270_CONTEXT_CHIP_ID; + + /* Get the size of config array */ + dev->config_size = sizeof(bmi270_context_config_file); + + /* Enable the variant specific features if any */ + dev->variant_feature = BMI2_CRT_RTOSK_ENABLE | BMI2_GYRO_CROSS_SENS_ENABLE; + + /* An extra dummy byte is read during SPI read */ + if (dev->intf == BMI2_SPI_INTF) + { + dev->dummy_byte = 1; + } + else + { + dev->dummy_byte = 0; + } + + /* If configuration file pointer is not assigned any address */ + if (!dev->config_file_ptr) + { + /* Give the address of the configuration file array to + * the device pointer + */ + dev->config_file_ptr = bmi270_context_config_file; + } + + /* Initialize BMI2 sensor */ + rslt = bmi2_sec_init(dev); + if (rslt == BMI2_OK) + { + /* Assign the offsets of the feature input + * configuration to the device structure + */ + dev->feat_config = bmi270_context_feat_in; + + /* Assign the offsets of the feature output to + * the device structure + */ + dev->feat_output = bmi270_context_feat_out; + + /* Assign the maximum number of pages to the + * device structure + */ + dev->page_max = BMI270_CONTEXT_MAX_PAGE_NUM; + + /* Assign maximum number of input sensors/ + * features to device structure + */ + dev->input_sens = BMI270_CONTEXT_MAX_FEAT_IN; + + /* Assign maximum number of output sensors/ + * features to device structure + */ + dev->out_sens = BMI270_CONTEXT_MAX_FEAT_OUT; + + /* Assign the offsets of the feature interrupt + * to the device structure + */ + dev->map_int = bmi270_c_map_int; + + /* Assign maximum number of feature interrupts + * to device structure + */ + dev->sens_int_map = BMI270_C_MAX_INT_MAP; + } + } + + return rslt; +} + +/*! + * @brief This API selects the sensors/features to be enabled. + */ +int8_t bmi270_context_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to select sensor */ + uint64_t sensor_sel = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sens_list != NULL)) + { + /* Get the selected sensors */ + rslt = select_sensor(sens_list, n_sens, &sensor_sel); + if (rslt == BMI2_OK) + { + /* Enable the selected sensors */ + rslt = sensor_enable(sensor_sel, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API selects the sensors/features to be disabled. + */ +int8_t bmi270_context_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to select sensor */ + uint64_t sensor_sel = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sens_list != NULL)) + { + /* Get the selected sensors */ + rslt = select_sensor(sens_list, n_sens, &sensor_sel); + if (rslt == BMI2_OK) + { + /* Disable the selected sensors */ + rslt = sensor_disable(sensor_sel, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API sets the sensor/feature configuration. + */ +int8_t bmi270_context_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define loop */ + uint8_t loop; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sens_cfg != NULL)) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + + for (loop = 0; loop < n_sens; loop++) + { + if ((sens_cfg[loop].type == BMI2_ACCEL) || (sens_cfg[loop].type == BMI2_GYRO) || + (sens_cfg[loop].type == BMI2_AUX) || (sens_cfg[loop].type == BMI2_GYRO_GAIN_UPDATE)) + { + rslt = bmi2_set_sensor_config(&sens_cfg[loop], 1, dev); + } + else + { + /* Disable Advance power save if enabled for auxiliary + * and feature configurations + */ + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if + * enabled + */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + if (rslt == BMI2_OK) + { + switch (sens_cfg[loop].type) + { + /* Set the step counter parameters */ + case BMI2_STEP_COUNTER_PARAMS: + rslt = set_step_count_params_config(sens_cfg[loop].cfg.step_counter_params, dev); + break; + + /* Set step counter/detector/activity configuration */ + case BMI2_STEP_DETECTOR: + case BMI2_STEP_COUNTER: + rslt = set_step_config(&sens_cfg[loop].cfg.step_counter, dev); + break; + + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + } + + /* Return error if any of the set configurations fail */ + if (rslt != BMI2_OK) + { + break; + } + } + } + + /* Enable Advance power save if disabled while configuring and + * not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API gets the sensor/feature configuration. + */ +int8_t bmi270_context_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define loop */ + uint8_t loop; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sens_cfg != NULL)) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + for (loop = 0; loop < n_sens; loop++) + { + if ((sens_cfg[loop].type == BMI2_ACCEL) || (sens_cfg[loop].type == BMI2_GYRO) || + (sens_cfg[loop].type == BMI2_AUX) || (sens_cfg[loop].type == BMI2_GYRO_GAIN_UPDATE)) + { + rslt = bmi2_get_sensor_config(&sens_cfg[loop], 1, dev); + } + else + { + /* Disable Advance power save if enabled for auxiliary + * and feature configurations + */ + if ((sens_cfg[loop].type >= BMI2_MAIN_SENS_MAX_NUM) || (sens_cfg[loop].type == BMI2_AUX)) + { + + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if + * enabled + */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + } + + if (rslt == BMI2_OK) + { + switch (sens_cfg[loop].type) + { + /* Set the step counter parameters */ + case BMI2_STEP_COUNTER_PARAMS: + rslt = get_step_count_params_config(sens_cfg[loop].cfg.step_counter_params, dev); + break; + + /* Get step counter/detector/activity configuration */ + case BMI2_STEP_DETECTOR: + case BMI2_STEP_COUNTER: + rslt = get_step_config(&sens_cfg[loop].cfg.step_counter, dev); + break; + + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + } + + /* Return error if any of the get configurations fail */ + if (rslt != BMI2_OK) + { + break; + } + } + } + + /* Enable Advance power save if disabled while configuring and + * not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API gets the sensor/feature data for accelerometer, gyroscope, + * auxiliary sensor, step counter, high-g, gyroscope user-gain update, + * orientation, gyroscope cross sensitivity and error status for NVM and VFRM. + */ +int8_t bmi270_context_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define loop */ + uint8_t loop; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sensor_data != NULL)) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + for (loop = 0; loop < n_sens; loop++) + { + if ((sensor_data[loop].type == BMI2_ACCEL) || (sensor_data[loop].type == BMI2_GYRO) || + (sensor_data[loop].type == BMI2_AUX) || (sensor_data[loop].type == BMI2_GYRO_GAIN_UPDATE) || + (sensor_data[loop].type == BMI2_GYRO_CROSS_SENSE)) + { + rslt = bmi2_get_sensor_data(&sensor_data[loop], 1, dev); + } + else + { + /* Disable Advance power save if enabled for feature + * configurations + */ + if (sensor_data[loop].type >= BMI2_MAIN_SENS_MAX_NUM) + { + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if + * enabled + */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + } + + if (rslt == BMI2_OK) + { + switch (sensor_data[loop].type) + { + case BMI2_STEP_COUNTER: + + /* Get step counter output */ + rslt = get_step_counter_output(&sensor_data[loop].sens_data.step_counter_output, dev); + break; + case BMI2_NVM_STATUS: + + /* Get NVM error status */ + rslt = get_nvm_error_status(&sensor_data[loop].sens_data.nvm_status, dev); + break; + case BMI2_VFRM_STATUS: + + /* Get VFRM error status */ + rslt = get_vfrm_error_status(&sensor_data[loop].sens_data.vfrm_status, dev); + break; + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + + /* Return error if any of the get sensor data fails */ + if (rslt != BMI2_OK) + { + break; + } + } + } + + /* Enable Advance power save if disabled while + * configuring and not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This api is used for retrieving the activity recognition settings currently set. + */ +int8_t bmi270_context_get_act_recg_sett(struct bmi2_act_recg_sett *sett, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat; + + /* Variable to set flag */ + uint8_t feat_found; + uint16_t msb_lsb; + uint8_t lsb; + uint8_t msb; + + /* Initialize feature configuration for activity recognition */ + struct bmi2_feature_config bmi2_act_recg_sett = { 0, 0, 0 }; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + + /* Search for bmi2 Abort feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&bmi2_act_recg_sett, BMI2_ACTIVITY_RECOGNITION_SETTINGS, dev); + if (feat_found) + { + aps_stat = dev->aps_status; + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if enabled */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + /* Get the configuration from the page where activity recognition setting feature resides */ + if (rslt == BMI2_OK) + { + rslt = bmi2_get_feat_config(bmi2_act_recg_sett.page, feat_config, dev); + } + + if (rslt == BMI2_OK) + { + /* Define the offset in bytes */ + idx = bmi2_act_recg_sett.start_addr; + + /* get the status of enable/disable post processing */ + sett->act_rec_1 = BMI2_GET_BIT_POS0(feat_config[idx], BMI2_ACT_RECG_POST_PROS_EN_DIS); + + /* increment idx by 2 to point min gdi thres addres */ + idx = idx + 2; + lsb = feat_config[idx]; + idx++; + msb = feat_config[idx]; + msb_lsb = (uint16_t)(lsb | msb << 8); + sett->act_rec_2 = msb_lsb; + + /* increment idx by 1 to point max gdi thres addres */ + idx++; + lsb = feat_config[idx]; + idx++; + msb = feat_config[idx]; + msb_lsb = (uint16_t)(lsb | msb << 8); + sett->act_rec_3 = msb_lsb; + + /* increment idx by 1 to point buffer size */ + idx++; + sett->act_rec_4 = BMI2_GET_BIT_POS0(feat_config[idx], BMI2_ACT_RECG_BUFF_SIZE); + + /* increment idx by 2 to to point to min segment confidence */ + idx = idx + 2; + sett->act_rec_5 = BMI2_GET_BIT_POS0(feat_config[idx], BMI2_ACT_RECG_MIN_SEG_CONF); + } + + /* Enable Advance power save if disabled while + * configuring and not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + } + + return rslt; +} + +/*! + * @brief This api is used for setting the activity recognition settings. + */ +int8_t bmi270_context_set_act_recg_sett(const struct bmi2_act_recg_sett *sett, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for activity recognition */ + struct bmi2_feature_config bmi2_act_recg_sett = { 0, 0, 0 }; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + + /* Search for bmi2 Abort feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&bmi2_act_recg_sett, BMI2_ACTIVITY_RECOGNITION_SETTINGS, dev); + if (feat_found) + { + aps_stat = dev->aps_status; + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if enabled */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + /* Get the configuration from the page where activity recognition setting feature resides */ + if (rslt == BMI2_OK) + { + rslt = bmi2_get_feat_config(bmi2_act_recg_sett.page, feat_config, dev); + } + + if (rslt == BMI2_OK) + { + /* Define the offset in bytes */ + idx = bmi2_act_recg_sett.start_addr; + if ((sett->act_rec_4 > 10) || (sett->act_rec_5 > 10)) + { + rslt = BMI2_E_INVALID_INPUT; + } + + if (rslt == BMI2_OK) + { + feat_config[idx] = BMI2_SET_BIT_POS0(feat_config[idx], + BMI2_ACT_RECG_POST_PROS_EN_DIS, + sett->act_rec_1); + + /* Increment idx by 2 to point min gdi thres addres */ + idx = idx + 2; + feat_config[idx] = BMI2_GET_LSB(sett->act_rec_2); + idx++; + feat_config[idx] = BMI2_GET_MSB(sett->act_rec_2); + + /* Increment idx by 1 to point max gdi thres addres */ + idx++; + feat_config[idx] = BMI2_GET_LSB(sett->act_rec_3); + idx++; + feat_config[idx] = BMI2_GET_MSB(sett->act_rec_3); + + /* Increment idx by 1 to point buffer size */ + idx++; + feat_config[idx] = BMI2_SET_BIT_POS0(feat_config[idx], BMI2_ACT_RECG_BUFF_SIZE, sett->act_rec_4); + + /* Increment idx by 2 to to point to min segment confidence */ + idx = idx + 2; + feat_config[idx] = BMI2_SET_BIT_POS0(feat_config[idx], BMI2_ACT_RECG_MIN_SEG_CONF, sett->act_rec_5); + + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + + /* Enable Advance power save if disabled while + * configuring and not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + } + + return rslt; +} + +/*! + * @brief This internal API is used to parse the activity output from the + * FIFO in header mode. + */ +int8_t bmi270_context_get_act_recog_output(struct bmi2_act_recog_output *act_recog, + uint16_t *act_frm_len, + struct bmi2_fifo_frame *fifo, + const struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define header frame */ + uint8_t frame_header = 0; + + /* Variable to index the data bytes */ + uint16_t data_index; + + /* Variable to index activity frames */ + uint16_t act_idx = 0; + + /* Variable to indicate activity frames read */ + uint16_t frame_to_read = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (act_recog != NULL) && (act_frm_len != NULL) && (fifo != NULL)) + { + + /* Store the number of frames to be read */ + frame_to_read = *act_frm_len; + for (data_index = fifo->act_recog_byte_start_idx; data_index < fifo->length;) + { + /* Get frame header byte */ + frame_header = fifo->data[data_index] & BMI2_FIFO_TAG_INTR_MASK; + + /* Skip S4S frames if S4S is enabled */ + rslt = move_if_s4s_frame(&frame_header, &data_index, fifo); + + /* Break if FIFO is empty */ + if (rslt == BMI2_W_FIFO_EMPTY) + { + break; + } + + /* Index shifted to next byte where data starts */ + data_index++; + switch (frame_header) + { + /* If header defines accelerometer frame */ + case BMI2_FIFO_HEADER_ACC_FRM: + rslt = move_next_frame(&data_index, fifo->acc_frm_len, fifo); + break; + + /* If header defines accelerometer and auxiliary frames */ + case BMI2_FIFO_HEADER_AUX_ACC_FRM: + rslt = move_next_frame(&data_index, fifo->acc_aux_frm_len, fifo); + break; + + /* If header defines accelerometer and gyroscope frames */ + case BMI2_FIFO_HEADER_GYR_ACC_FRM: + rslt = move_next_frame(&data_index, fifo->acc_gyr_frm_len, fifo); + break; + + /* If header defines accelerometer, auxiliary and gyroscope frames */ + case BMI2_FIFO_HEADER_ALL_FRM: + rslt = move_next_frame(&data_index, fifo->all_frm_len, fifo); + break; + + /* If header defines only gyroscope frame */ + case BMI2_FIFO_HEADER_GYR_FRM: + rslt = move_next_frame(&data_index, fifo->gyr_frm_len, fifo); + break; + + /* If header defines only auxiliary frame */ + case BMI2_FIFO_HEADER_AUX_FRM: + rslt = move_next_frame(&data_index, fifo->aux_frm_len, fifo); + break; + + /* If header defines auxiliary and gyroscope frame */ + case BMI2_FIFO_HEADER_AUX_GYR_FRM: + rslt = move_next_frame(&data_index, fifo->aux_gyr_frm_len, fifo); + break; + + /* If header defines sensor time frame */ + case BMI2_FIFO_HEADER_SENS_TIME_FRM: + rslt = move_next_frame(&data_index, BMI2_SENSOR_TIME_LENGTH, fifo); + break; + + /* If header defines skip frame */ + case BMI2_FIFO_HEADER_SKIP_FRM: + rslt = move_next_frame(&data_index, BMI2_FIFO_SKIP_FRM_LENGTH, fifo); + break; + + /* If header defines Input configuration frame */ + case BMI2_FIFO_HEADER_INPUT_CFG_FRM: + rslt = move_next_frame(&data_index, BMI2_FIFO_INPUT_CFG_LENGTH, fifo); + break; + + /* If header defines invalid frame or end of valid data */ + case BMI2_FIFO_HEAD_OVER_READ_MSB: + + /* Move the data index to the last byte to mark completion */ + data_index = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + break; + + /* If header defines activity recognition frame */ + case BMI2_FIFO_VIRT_ACT_RECOG_FRM: + + /* Get the activity output */ + rslt = unpack_act_recog_output(&act_recog[(act_idx)], &data_index, fifo); + + /* Update activity frame index */ + (act_idx)++; + break; + default: + + /* Move the data index to the last byte in case of invalid values */ + data_index = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + break; + } + + /* Number of frames to be read is complete or FIFO is empty */ + if ((frame_to_read == act_idx) || (rslt == BMI2_W_FIFO_EMPTY)) + { + break; + } + } + + /* Update the activity frame index */ + (*act_frm_len) = act_idx; + + /* Update the activity byte index */ + fifo->act_recog_byte_start_idx = data_index; + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API updates the gyroscope user-gain. + */ +int8_t bmi270_context_update_gyro_user_gain(const struct bmi2_gyro_user_gain_config *user_gain, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to select sensor */ + uint8_t sens_sel[2] = { BMI2_GYRO, BMI2_GYRO_GAIN_UPDATE }; + + /* Structure to define sensor configurations */ + struct bmi2_sens_config sens_cfg; + + /* Variable to store status of user-gain update module */ + uint8_t status = 0; + + /* Variable to define count */ + uint8_t count = 100; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (user_gain != NULL)) + { + /* Select type of feature */ + sens_cfg.type = BMI2_GYRO_GAIN_UPDATE; + + /* Get the user gain configurations */ + rslt = bmi270_context_get_sensor_config(&sens_cfg, 1, dev); + if (rslt == BMI2_OK) + { + /* Get the user-defined ratio */ + sens_cfg.cfg.gyro_gain_update = *user_gain; + + /* Set rate ratio for all axes */ + rslt = bmi270_context_set_sensor_config(&sens_cfg, 1, dev); + } + + /* Disable gyroscope */ + if (rslt == BMI2_OK) + { + rslt = bmi270_context_sensor_disable(&sens_sel[0], 1, dev); + } + + /* Enable gyroscope user-gain update module */ + if (rslt == BMI2_OK) + { + rslt = bmi270_context_sensor_enable(&sens_sel[1], 1, dev); + } + + /* Set the command to trigger the computation */ + if (rslt == BMI2_OK) + { + rslt = bmi2_set_command_register(BMI2_USR_GAIN_CMD, dev); + } + + if (rslt == BMI2_OK) + { + /* Poll until enable bit of user-gain update is 0 */ + while (count--) + { + rslt = get_user_gain_upd_status(&status, dev); + if ((rslt == BMI2_OK) && (status == 0)) + { + /* Enable compensation of gain defined + * in the GAIN register + */ + rslt = enable_gyro_gain(BMI2_ENABLE, dev); + + /* Enable gyroscope */ + if (rslt == BMI2_OK) + { + rslt = bmi270_context_sensor_enable(&sens_sel[0], 1, dev); + } + + break; + } + + dev->delay_us(10000, dev->intf_ptr); + } + + /* Return error if user-gain update is failed */ + if ((rslt == BMI2_OK) && (status != 0)) + { + rslt = BMI2_E_GYR_USER_GAIN_UPD_FAIL; + } + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API reads the compensated gyroscope user-gain values. + */ +int8_t bmi270_context_read_gyro_user_gain(struct bmi2_gyro_user_gain_data *gyr_usr_gain, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define register data */ + uint8_t reg_data[3] = { 0 }; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (gyr_usr_gain != NULL)) + { + /* Get the gyroscope compensated gain values */ + rslt = bmi2_get_regs(BMI2_GYR_USR_GAIN_0_ADDR, reg_data, 3, dev); + if (rslt == BMI2_OK) + { + /* Gyroscope user gain correction X-axis */ + gyr_usr_gain->x = (int8_t)BMI2_GET_BIT_POS0(reg_data[0], BMI2_GYR_USR_GAIN_X); + + /* Gyroscope user gain correction Y-axis */ + gyr_usr_gain->y = (int8_t)BMI2_GET_BIT_POS0(reg_data[1], BMI2_GYR_USR_GAIN_Y); + + /* Gyroscope user gain correction z-axis */ + gyr_usr_gain->z = (int8_t)BMI2_GET_BIT_POS0(reg_data[2], BMI2_GYR_USR_GAIN_Z); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API maps/unmaps feature interrupts to that of interrupt pins. + */ +int8_t bmi270_context_map_feat_int(const struct bmi2_sens_int_config *sens_int, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define loop */ + uint8_t loop; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sens_int != NULL)) + { + for (loop = 0; loop < n_sens; loop++) + { + switch (sens_int[loop].type) + { + case BMI2_STEP_COUNTER: + case BMI2_STEP_DETECTOR: + + rslt = bmi2_map_feat_int(sens_int[loop].type, sens_int[loop].hw_int_pin, dev); + break; + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + + /* Return error if interrupt mapping fails */ + if (rslt != BMI2_OK) + { + break; + } + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/***************************************************************************/ + +/*! Local Function Definitions + ****************************************************************************/ + +/*! + * @brief This internal API is used to validate the device structure pointer for + * null conditions. + */ +static int8_t null_ptr_check(const struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + if ((dev == NULL) || (dev->read == NULL) || (dev->write == NULL) || (dev->delay_us == NULL)) + { + /* Device structure pointer is not valid */ + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This internal API selects the sensor/features to be enabled or + * disabled. + */ +static int8_t select_sensor(const uint8_t *sens_list, uint8_t n_sens, uint64_t *sensor_sel) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Variable to define loop */ + uint8_t count; + + for (count = 0; count < n_sens; count++) + { + switch (sens_list[count]) + { + case BMI2_ACCEL: + *sensor_sel |= BMI2_ACCEL_SENS_SEL; + break; + case BMI2_GYRO: + *sensor_sel |= BMI2_GYRO_SENS_SEL; + break; + case BMI2_AUX: + *sensor_sel |= BMI2_AUX_SENS_SEL; + break; + case BMI2_TEMP: + *sensor_sel |= BMI2_TEMP_SENS_SEL; + break; + case BMI2_STEP_DETECTOR: + *sensor_sel |= BMI2_STEP_DETECT_SEL; + break; + case BMI2_STEP_COUNTER: + *sensor_sel |= BMI2_STEP_COUNT_SEL; + break; + case BMI2_GYRO_GAIN_UPDATE: + *sensor_sel |= BMI2_GYRO_GAIN_UPDATE_SEL; + break; + case BMI2_ACTIVITY_RECOGNITION: + *sensor_sel |= BMI2_ACTIVITY_RECOGNITION_SEL; + break; + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + } + + return rslt; +} + +/*! + * @brief This internal API enables the selected sensor/features. + */ +static int8_t sensor_enable(uint64_t sensor_sel, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store register values */ + uint8_t reg_data = 0; + + /* Variable to define loop */ + uint8_t loop = 1; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat = 0; + + rslt = bmi2_get_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + /* Enable accelerometer */ + if (sensor_sel & BMI2_ACCEL_SENS_SEL) + { + reg_data = BMI2_SET_BITS(reg_data, BMI2_ACC_EN, BMI2_ENABLE); + } + + /* Enable gyroscope */ + if (sensor_sel & BMI2_GYRO_SENS_SEL) + { + reg_data = BMI2_SET_BITS(reg_data, BMI2_GYR_EN, BMI2_ENABLE); + } + + /* Enable auxiliary sensor */ + if (sensor_sel & BMI2_AUX_SENS_SEL) + { + reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_AUX_EN, BMI2_ENABLE); + } + + /* Enable temperature sensor */ + if (sensor_sel & BMI2_TEMP_SENS_SEL) + { + reg_data = BMI2_SET_BITS(reg_data, BMI2_TEMP_EN, BMI2_ENABLE); + } + + /* Enable the sensors that are set in the power control register */ + if (sensor_sel & BMI2_MAIN_SENSORS) + { + rslt = bmi2_set_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); + } + } + + if ((rslt == BMI2_OK) && (sensor_sel & ~(BMI2_MAIN_SENSORS))) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if enabled */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + if (rslt == BMI2_OK) + { + while (loop--) + { + /* Enable step detector feature */ + if (sensor_sel & BMI2_STEP_DETECT_SEL) + { + rslt = set_step_detector(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_STEP_DETECT_SEL; + } + else + { + break; + } + } + + /* Enable step counter feature */ + if (sensor_sel & BMI2_STEP_COUNT_SEL) + { + rslt = set_step_counter(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_STEP_COUNT_SEL; + } + else + { + break; + } + } + + /* Enable gyroscope user gain */ + if (sensor_sel & BMI2_GYRO_GAIN_UPDATE_SEL) + { + rslt = set_gyro_user_gain(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_GYRO_GAIN_UPDATE_SEL; + } + else + { + break; + } + } + + /* Enable activity recognition feature */ + if (sensor_sel & BMI2_ACTIVITY_RECOGNITION_SEL) + { + rslt = set_act_recog(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_ACTIVITY_RECOGNITION_SEL; + } + else + { + break; + } + } + } + + /* Enable Advance power save if disabled while + * configuring and not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + } + + return rslt; +} + +/*! + * @brief This internal API disables the selected sensors/features. + */ +static int8_t sensor_disable(uint64_t sensor_sel, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store register values */ + uint8_t reg_data = 0; + + /* Variable to define loop */ + uint8_t loop = 1; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat = 0; + + rslt = bmi2_get_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + /* Disable accelerometer */ + if (sensor_sel & BMI2_ACCEL_SENS_SEL) + { + reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_ACC_EN); + } + + /* Disable gyroscope */ + if (sensor_sel & BMI2_GYRO_SENS_SEL) + { + reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_GYR_EN); + } + + /* Disable auxiliary sensor */ + if (sensor_sel & BMI2_AUX_SENS_SEL) + { + reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_AUX_EN); + } + + /* Disable temperature sensor */ + if (sensor_sel & BMI2_TEMP_SENS_SEL) + { + reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_TEMP_EN); + } + + /* Disable the sensors that are set in the power control register */ + if (sensor_sel & BMI2_MAIN_SENSORS) + { + rslt = bmi2_set_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); + } + } + + if ((rslt == BMI2_OK) && (sensor_sel & ~(BMI2_MAIN_SENSORS))) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if enabled */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + if (rslt == BMI2_OK) + { + while (loop--) + { + /* Disable step detector feature */ + if (sensor_sel & BMI2_STEP_DETECT_SEL) + { + rslt = set_step_detector(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat &= ~BMI2_STEP_DETECT_SEL; + } + else + { + break; + } + } + + /* Disable step counter feature */ + if (sensor_sel & BMI2_STEP_COUNT_SEL) + { + rslt = set_step_counter(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat &= ~BMI2_STEP_COUNT_SEL; + } + else + { + break; + } + } + + /* Disable gyroscope user gain */ + if (sensor_sel & BMI2_GYRO_GAIN_UPDATE_SEL) + { + rslt = set_gyro_user_gain(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat &= ~BMI2_GYRO_GAIN_UPDATE_SEL; + } + else + { + break; + } + } + + if (sensor_sel & BMI2_ACTIVITY_RECOGNITION_SEL) + { + rslt = set_act_recog(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_ACTIVITY_RECOGNITION_SEL; + } + else + { + break; + } + } + + /* Enable Advance power save if disabled while + * configuring and not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + } + } + + return rslt; +} + +/*! + * @brief This internal API is used to enable/disable step detector feature. + */ +static int8_t set_step_detector(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for step detector */ + struct bmi2_feature_config step_det_config = { 0, 0, 0 }; + + /* Search for step detector feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_det_config, BMI2_STEP_DETECTOR, dev); + if (feat_found) + { + /* Get the configuration from the page where step detector feature resides */ + rslt = bmi2_get_feat_config(step_det_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of step detector */ + idx = step_det_config.start_addr + BMI2_STEP_COUNT_FEAT_EN_OFFSET; + + /* Set the feature enable bit */ + feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_STEP_DET_FEAT_EN, enable); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API is used to enable/disable step counter feature. + */ +static int8_t set_step_counter(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for step counter */ + struct bmi2_feature_config step_count_config = { 0, 0, 0 }; + + /* Search for step counter feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_count_config, BMI2_STEP_COUNTER, dev); + if (feat_found) + { + /* Get the configuration from the page where step-counter feature resides */ + rslt = bmi2_get_feat_config(step_count_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of step counter */ + idx = step_count_config.start_addr + BMI2_STEP_COUNT_FEAT_EN_OFFSET; + + /* Set the feature enable bit */ + feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_STEP_COUNT_FEAT_EN, enable); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API is used to enable/disable gyroscope user gain + * feature. + */ +static int8_t set_gyro_user_gain(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for gyroscope user gain */ + struct bmi2_feature_config gyr_user_gain_cfg = { 0, 0, 0 }; + + /* Search for user gain feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&gyr_user_gain_cfg, BMI2_GYRO_GAIN_UPDATE, dev); + if (feat_found) + { + /* Get the configuration from the page where user gain feature resides */ + rslt = bmi2_get_feat_config(gyr_user_gain_cfg.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of user gain */ + idx = gyr_user_gain_cfg.start_addr + BMI2_GYR_USER_GAIN_FEAT_EN_OFFSET; + + /* Set the feature enable bit */ + feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_GYR_USER_GAIN_FEAT_EN, enable); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API enables/disables the activity recognition feature. + */ +static int8_t set_act_recog(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for activity recognition */ + struct bmi2_feature_config act_recog_cfg = { 0, 0, 0 }; + + /* Search for activity recognition and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&act_recog_cfg, BMI2_ACTIVITY_RECOGNITION, dev); + if (feat_found) + { + /* Get the configuration from the page where activity + * recognition feature resides + */ + rslt = bmi2_get_feat_config(act_recog_cfg.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of activity recognition */ + idx = act_recog_cfg.start_addr; + + /* Set the feature enable bit */ + feat_config[idx] = BMI2_SET_BIT_POS0(feat_config[idx], BMI2_ACTIVITY_RECOG_EN, enable); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API sets step counter parameter configurations. + */ +static int8_t set_step_count_params_config(const uint16_t *step_count_params, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define index */ + uint8_t index = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for step counter parameters */ + struct bmi2_feature_config step_params_config = { 0, 0, 0 }; + + /* Variable to index the page number */ + uint8_t page_idx; + + /* Variable to define the start page */ + uint8_t start_page; + + /* Variable to define start address of the parameters */ + uint8_t start_addr; + + /* Variable to define number of bytes */ + uint8_t n_bytes = (BMI2_STEP_CNT_N_PARAMS * 2); + + /* Variable to store number of pages */ + uint8_t n_pages = (n_bytes / 16); + + /* Variable to define the end page */ + uint8_t end_page; + + /* Variable to define the remaining bytes to be read */ + uint8_t remain_len; + + /* Variable to define the maximum words(16 bytes or 8 words) to be read in a page */ + uint8_t max_len = 8; + + /* Variable index bytes in a page */ + uint8_t page_byte_idx; + + /* Variable to index the parameters */ + uint8_t param_idx = 0; + + /* Copy the feature configuration address to a local pointer */ + uint16_t *data_p = (uint16_t *) (void *)feat_config; + + /* Search for step counter parameter feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_params_config, BMI2_STEP_COUNTER_PARAMS, dev); + if (feat_found) + { + /* Get the start page for the step counter parameters */ + start_page = step_params_config.page; + + /* Get the end page for the step counter parameters */ + end_page = start_page + n_pages; + + /* Get the start address for the step counter parameters */ + start_addr = step_params_config.start_addr; + + /* Get the remaining length of bytes to be read */ + remain_len = (uint8_t)((n_bytes - (n_pages * 16)) + start_addr); + for (page_idx = start_page; page_idx <= end_page; page_idx++) + { + /* Get the configuration from the respective page */ + rslt = bmi2_get_feat_config(page_idx, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Start from address 0x00 when switched to next page */ + if (page_idx > start_page) + { + start_addr = 0; + } + + /* Remaining number of words to be read in the page */ + if (page_idx == end_page) + { + max_len = (remain_len / 2); + } + + /* Get offset in words since all the features are set in words length */ + page_byte_idx = start_addr / 2; + for (; page_byte_idx < max_len;) + { + /* Set parameters 1 to 25 */ + *(data_p + page_byte_idx) = BMI2_SET_BIT_POS0(*(data_p + page_byte_idx), + BMI2_STEP_COUNT_PARAMS, + step_count_params[param_idx]); + + /* Increment offset by 1 word to set to the next parameter */ + page_byte_idx++; + + /* Increment to next parameter */ + param_idx++; + } + + /* Get total length in bytes to copy from local pointer to the array */ + page_byte_idx = (uint8_t)(page_byte_idx * 2) - step_params_config.start_addr; + + /* Copy the bytes to be set back to the array */ + for (index = 0; index < page_byte_idx; index++) + { + feat_config[step_params_config.start_addr + + index] = *((uint8_t *) data_p + step_params_config.start_addr + index); + } + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/* @brief This internal API sets step counter configurations like water-mark + * level, reset-counter and output-configuration step detector and activity. + */ +static int8_t set_step_config(const struct bmi2_step_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to define index */ + uint8_t index = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for step counter 4 */ + struct bmi2_feature_config step_count_config = { 0, 0, 0 }; + + /* Copy the feature configuration address to a local pointer */ + uint16_t *data_p = (uint16_t *) (void *)feat_config; + + /* Search for step counter feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_count_config, BMI2_STEP_COUNTER, dev); + if (feat_found) + { + /* Get the configuration from the page where step counter resides */ + rslt = bmi2_get_feat_config(step_count_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes */ + idx = step_count_config.start_addr; + + /* Get offset in words since all the features are set in words length */ + idx = idx / 2; + + /* Set water-mark level */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_STEP_COUNT_WM_LEVEL, config->watermark_level); + + /* Set reset-counter */ + *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_STEP_COUNT_RST_CNT, config->reset_counter); + + /* Increment offset by 1 word to set output + * configuration of step detector and step activity + */ + idx++; + + /* Set step buffer size */ + *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_STEP_BUFFER_SIZE, config->step_buffer_size); + + /* Increment offset by 1 more word to get the total length in words */ + idx++; + + /* Get total length in bytes to copy from local pointer to the array */ + idx = (uint8_t)(idx * 2) - step_count_config.start_addr; + + /* Copy the bytes to be set back to the array */ + for (index = 0; index < idx; index++) + { + feat_config[step_count_config.start_addr + + index] = *((uint8_t *) data_p + step_count_config.start_addr + index); + } + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets step counter parameter configurations. + */ +static int8_t get_step_count_params_config(uint16_t *step_count_params, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Variable to define LSB */ + uint16_t lsb = 0; + + /* Variable to define MSB */ + uint16_t msb = 0; + + /* Variable to define a word */ + uint16_t lsb_msb = 0; + + /* Initialize feature configuration for step counter 1 */ + struct bmi2_feature_config step_params_config = { 0, 0, 0 }; + + /* Variable to index the page number */ + uint8_t page_idx; + + /* Variable to define the start page */ + uint8_t start_page; + + /* Variable to define start address of the parameters */ + uint8_t start_addr; + + /* Variable to define number of bytes */ + uint8_t n_bytes = (BMI2_STEP_CNT_N_PARAMS * 2); + + /* Variable to store number of pages */ + uint8_t n_pages = (n_bytes / 16); + + /* Variable to define the end page */ + uint8_t end_page; + + /* Variable to define the remaining bytes to be read */ + uint8_t remain_len; + + /* Variable to define the maximum words to be read in a page */ + uint8_t max_len = BMI2_FEAT_SIZE_IN_BYTES; + + /* Variable index bytes in a page */ + uint8_t page_byte_idx; + + /* Variable to index the parameters */ + uint8_t param_idx = 0; + + /* Search for step counter parameter feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_params_config, BMI2_STEP_COUNTER_PARAMS, dev); + if (feat_found) + { + /* Get the start page for the step counter parameters */ + start_page = step_params_config.page; + + /* Get the end page for the step counter parameters */ + end_page = start_page + n_pages; + + /* Get the start address for the step counter parameters */ + start_addr = step_params_config.start_addr; + + /* Get the remaining length of bytes to be read */ + remain_len = (uint8_t)((n_bytes - (n_pages * 16)) + start_addr); + for (page_idx = start_page; page_idx <= end_page; page_idx++) + { + /* Get the configuration from the respective page */ + rslt = bmi2_get_feat_config(page_idx, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Start from address 0x00 when switched to next page */ + if (page_idx > start_page) + { + start_addr = 0; + } + + /* Remaining number of bytes to be read in the page */ + if (page_idx == end_page) + { + max_len = remain_len; + } + + /* Get the offset */ + page_byte_idx = start_addr; + while (page_byte_idx < max_len) + { + /* Get word to calculate the parameter*/ + lsb = (uint16_t) feat_config[page_byte_idx++]; + if (page_byte_idx < max_len) + { + msb = ((uint16_t) feat_config[page_byte_idx++] << 8); + } + + lsb_msb = lsb | msb; + + /* Get parameters 1 to 25 */ + step_count_params[param_idx] = lsb_msb & BMI2_STEP_COUNT_PARAMS_MASK; + + /* Increment to next parameter */ + param_idx++; + } + } + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets step counter/detector/activity configurations. + */ +static int8_t get_step_config(struct bmi2_step_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to define LSB */ + uint16_t lsb = 0; + + /* Variable to define MSB */ + uint16_t msb = 0; + + /* Variable to define a word */ + uint16_t lsb_msb = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for step counter */ + struct bmi2_feature_config step_count_config = { 0, 0, 0 }; + + /* Search for step counter 4 feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_count_config, BMI2_STEP_COUNTER, dev); + if (feat_found) + { + /* Get the configuration from the page where step counter 4 parameter resides */ + rslt = bmi2_get_feat_config(step_count_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for feature enable for step counter/detector/activity */ + idx = step_count_config.start_addr; + + /* Get word to calculate water-mark level and reset counter */ + lsb = (uint16_t) feat_config[idx++]; + msb = ((uint16_t) feat_config[idx++] << 8); + lsb_msb = lsb | msb; + + /* Get water-mark level */ + config->watermark_level = lsb_msb & BMI2_STEP_COUNT_WM_LEVEL_MASK; + + /* Get reset counter */ + config->reset_counter = (lsb_msb & BMI2_STEP_COUNT_RST_CNT_MASK) >> BMI2_STEP_COUNT_RST_CNT_POS; + + /* Get word to calculate output configuration of step detector and activity */ + lsb = (uint16_t) feat_config[idx++]; + msb = ((uint16_t) feat_config[idx++] << 8); + lsb_msb = lsb | msb; + + config->step_buffer_size = (lsb_msb & BMI2_STEP_BUFFER_SIZE_MASK) >> BMI2_STEP_BUFFER_SIZE_POS; + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets the output values of step counter. + */ +static int8_t get_step_counter_output(uint32_t *step_count, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variables to define index */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature output for step counter */ + struct bmi2_feature_config step_cnt_out_config = { 0, 0, 0 }; + + /* Search for step counter output feature and extract its configuration details */ + feat_found = extract_output_feat_config(&step_cnt_out_config, BMI2_STEP_COUNTER, dev); + if (feat_found) + { + /* Get the feature output configuration for step-counter */ + rslt = bmi2_get_feat_config(step_cnt_out_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for step counter output */ + idx = step_cnt_out_config.start_addr; + + /* Get the step counter output in 4 bytes */ + *step_count = (uint32_t) feat_config[idx++]; + *step_count |= ((uint32_t) feat_config[idx++] << 8); + *step_count |= ((uint32_t) feat_config[idx++] << 16); + *step_count |= ((uint32_t) feat_config[idx++] << 24); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets the error status related to NVM. + */ +static int8_t get_nvm_error_status(struct bmi2_nvm_err_status *nvm_err_stat, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variables to define index */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature output for NVM error status */ + struct bmi2_feature_config nvm_err_cfg = { 0, 0, 0 }; + + /* Search for NVM error status feature and extract its configuration details */ + feat_found = extract_output_feat_config(&nvm_err_cfg, BMI2_NVM_STATUS, dev); + if (feat_found) + { + /* Get the feature output configuration for NVM error status */ + rslt = bmi2_get_feat_config(nvm_err_cfg.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for NVM error status */ + idx = nvm_err_cfg.start_addr; + + /* Increment index to get the error status */ + idx++; + + /* Error when NVM load action fails */ + nvm_err_stat->load_error = BMI2_GET_BIT_POS0(feat_config[idx], BMI2_NVM_LOAD_ERR_STATUS); + + /* Error when NVM program action fails */ + nvm_err_stat->prog_error = BMI2_GET_BITS(feat_config[idx], BMI2_NVM_PROG_ERR_STATUS); + + /* Error when NVM erase action fails */ + nvm_err_stat->erase_error = BMI2_GET_BITS(feat_config[idx], BMI2_NVM_ERASE_ERR_STATUS); + + /* Error when NVM program limit is exceeded */ + nvm_err_stat->exceed_error = BMI2_GET_BITS(feat_config[idx], BMI2_NVM_END_EXCEED_STATUS); + + /* Error when NVM privilege mode is not acquired */ + nvm_err_stat->privil_error = BMI2_GET_BITS(feat_config[idx], BMI2_NVM_PRIV_ERR_STATUS); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API is used to get enable status of gyroscope user gain + * update. + */ +static int8_t get_user_gain_upd_status(uint8_t *status, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Variable to check APS status */ + uint8_t aps_stat = 0; + + /* Initialize feature configuration for gyroscope user gain */ + struct bmi2_feature_config gyr_user_gain_cfg = { 0, 0, 0 }; + + /* Search for user gain feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&gyr_user_gain_cfg, BMI2_GYRO_GAIN_UPDATE, dev); + if (feat_found) + { + /* Disable advance power save */ + aps_stat = dev->aps_status; + if (aps_stat == BMI2_ENABLE) + { + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + if (rslt == BMI2_OK) + { + /* Get the configuration from the page where user gain feature resides */ + rslt = bmi2_get_feat_config(gyr_user_gain_cfg.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of user gain */ + idx = gyr_user_gain_cfg.start_addr + BMI2_GYR_USER_GAIN_FEAT_EN_OFFSET; + + /* Set the feature enable status */ + *status = BMI2_GET_BITS(feat_config[idx], BMI2_GYR_USER_GAIN_FEAT_EN); + } + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + /* Enable Advance power save if disabled while configuring and not when already disabled */ + if ((rslt == BMI2_OK) && (aps_stat == BMI2_ENABLE)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + + return rslt; +} + +/*! + * @brief This internal API is used to parse and store the activity recognition + * output from the FIFO data. + */ +static int8_t unpack_act_recog_output(struct bmi2_act_recog_output *act_recog, + uint16_t *data_index, + const struct bmi2_fifo_frame *fifo) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Variables to define 4 bytes of sensor time */ + uint32_t time_stamp_byte4 = 0; + uint32_t time_stamp_byte3 = 0; + uint32_t time_stamp_byte2 = 0; + uint32_t time_stamp_byte1 = 0; + + /* Validate data index */ + if ((*data_index + BMI2_FIFO_VIRT_ACT_DATA_LENGTH) >= fifo->length) + { + /* Update the data index to the last byte */ + (*data_index) = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + } + else + { + /* Get time-stamp from the activity recognition frame */ + time_stamp_byte4 = ((uint32_t)(fifo->data[(*data_index) + 3]) << 24); + time_stamp_byte3 = ((uint32_t)(fifo->data[(*data_index) + 2]) << 16); + time_stamp_byte2 = fifo->data[(*data_index) + 1] << 8; + time_stamp_byte1 = fifo->data[(*data_index)]; + + /* Update time-stamp from the virtual frame */ + act_recog->time_stamp = (time_stamp_byte4 | time_stamp_byte3 | time_stamp_byte2 | time_stamp_byte1); + + /* Move the data index by 4 bytes */ + (*data_index) = (*data_index) + BMI2_FIFO_VIRT_ACT_TIME_LENGTH; + + /* Update the previous activity from the virtual frame */ + act_recog->prev_act = fifo->data[(*data_index)]; + + /* Move the data index by 1 byte */ + (*data_index) = (*data_index) + BMI2_FIFO_VIRT_ACT_TYPE_LENGTH; + + /* Update the current activity from the virtual frame */ + act_recog->curr_act = fifo->data[(*data_index)]; + + /* Move the data index by 1 byte */ + (*data_index) = (*data_index) + BMI2_FIFO_VIRT_ACT_STAT_LENGTH; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + } + + return rslt; +} + +/*! + * @brief This internal API gets the error status related to virtual frames. + */ +static int8_t get_vfrm_error_status(struct bmi2_vfrm_err_status *vfrm_err_stat, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variables to define index */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature output for VFRM error status */ + struct bmi2_feature_config vfrm_err_cfg = { 0, 0, 0 }; + + /* Search for VFRM error status feature and extract its configuration details */ + feat_found = extract_output_feat_config(&vfrm_err_cfg, BMI2_VFRM_STATUS, dev); + if (feat_found) + { + /* Get the feature output configuration for VFRM error status */ + rslt = bmi2_get_feat_config(vfrm_err_cfg.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for VFRM error status */ + idx = vfrm_err_cfg.start_addr; + + /* Increment index to get the error status */ + idx++; + + /* Internal error while acquiring lock for FIFO */ + vfrm_err_stat->lock_error = BMI2_GET_BITS(feat_config[idx], BMI2_VFRM_LOCK_ERR_STATUS); + + /* Internal error while writing byte into FIFO */ + vfrm_err_stat->write_error = BMI2_GET_BITS(feat_config[idx], BMI2_VFRM_WRITE_ERR_STATUS); + + /* Internal error while writing into FIFO */ + vfrm_err_stat->fatal_error = BMI2_GET_BITS(feat_config[idx], BMI2_VFRM_FATAL_ERR_STATUS); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API skips S4S frame in the FIFO data while getting + * step activity output. + */ +static int8_t move_if_s4s_frame(const uint8_t *frame_header, uint16_t *data_index, const struct bmi2_fifo_frame *fifo) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Variable to extract virtual header byte */ + uint8_t virtual_header_mode; + + /* Variable to define pay-load in words */ + uint8_t payload_word = 0; + + /* Variable to define pay-load in bytes */ + uint8_t payload_bytes = 0; + + /* Extract virtual header mode from the frame header */ + virtual_header_mode = BMI2_GET_BITS(*frame_header, BMI2_FIFO_VIRT_FRM_MODE); + + /* If the extracted header byte is a virtual header */ + if (virtual_header_mode == BMI2_FIFO_VIRT_FRM_MODE) + { + /* If frame header is not activity recognition header */ + if (*frame_header != 0xC8) + { + /* Extract pay-load in words from the header byte */ + payload_word = BMI2_GET_BITS(*frame_header, BMI2_FIFO_VIRT_PAYLOAD) + 1; + + /* Convert to bytes */ + payload_bytes = (uint8_t)(payload_word * 2); + + /* Move the data index by those pay-load bytes */ + rslt = move_next_frame(data_index, payload_bytes, fifo); + } + } + + return rslt; +} + +/*! + * @brief This internal API enables/disables compensation of the gain defined + * in the GAIN register. + */ +static int8_t enable_gyro_gain(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define register data */ + uint8_t reg_data = 0; + + rslt = bmi2_get_regs(BMI2_GYR_OFF_COMP_6_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + reg_data = BMI2_SET_BITS(reg_data, BMI2_GYR_GAIN_EN, enable); + rslt = bmi2_set_regs(BMI2_GYR_OFF_COMP_6_ADDR, ®_data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This internal API is used to extract the output feature configuration + * details from the look-up table. + */ +static uint8_t extract_output_feat_config(struct bmi2_feature_config *feat_output, + uint8_t type, + const struct bmi2_dev *dev) +{ + /* Variable to define loop */ + uint8_t loop = 0; + + /* Variable to set flag */ + uint8_t feat_found = BMI2_FALSE; + + /* Search for the output feature from the output configuration array */ + while (loop < dev->out_sens) + { + if (dev->feat_output[loop].type == type) + { + *feat_output = dev->feat_output[loop]; + feat_found = BMI2_TRUE; + break; + } + + loop++; + } + + /* Return flag */ + return feat_found; +} + +/*! + * @brief This internal API is used to move the data index ahead of the + * current_frame_length parameter when unnecessary FIFO data appears while + * extracting the user specified data. + */ +static int8_t move_next_frame(uint16_t *data_index, uint8_t current_frame_length, const struct bmi2_fifo_frame *fifo) +{ + /* Variables to define error */ + int8_t rslt = BMI2_OK; + + /* Validate data index */ + if (((*data_index) + current_frame_length) > fifo->length) + { + /* Move the data index to the last byte */ + (*data_index) = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + } + else + { + /* Move the data index to next frame */ + (*data_index) = (*data_index) + current_frame_length; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_context.h b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_context.h new file mode 100644 index 0000000000..4708dc7db3 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_context.h @@ -0,0 +1,493 @@ +/** +* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. +* +* BSD-3-Clause +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* +* 3. Neither the name of the copyright holder nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, +* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING +* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +* @file bmi270_context.h +* @date 2020-11-04 +* @version v2.63.1 +* +*/ + +/** + * \ingroup bmi2xy + * \defgroup bmi270_context BMI270_CONTEXT + * @brief Sensor driver for BMI270_CONTEXT sensor + */ + +#ifndef BMI270_CONTEXT_H_ +#define BMI270_CONTEXT_H_ + +/*! CPP guard */ +#ifdef __cplusplus +extern "C" { +#endif + +/***************************************************************************/ + +/*! Header files + ****************************************************************************/ +#include "bmi2.h" + +/***************************************************************************/ + +/*! Macro definitions + ****************************************************************************/ + +/*! @name BMI270_CONTEXT Chip identifier */ +#define BMI270_CONTEXT_CHIP_ID UINT8_C(0x24) + +/*! @name BMI270_CONTEXT feature input start addresses */ +#define BMI270_CONTEXT_CONFIG_ID_STRT_ADDR UINT8_C(0x06) +#define BMI270_CONTEXT_STEP_CNT_1_STRT_ADDR UINT8_C(0x00) +#define BMI270_CONTEXT_STEP_CNT_4_STRT_ADDR UINT8_C(0x02) +#define BMI270_CONTEXT_MAX_BURST_LEN_STRT_ADDR UINT8_C(0x08) +#define BMI270_CONTEXT_CRT_GYRO_SELF_TEST_STRT_ADDR UINT8_C(0x09) +#define BMI270_CONTEXT_ABORT_STRT_ADDR UINT8_C(0x09) +#define BMI270_CONTEXT_NVM_PROG_PREP_STRT_ADDR UINT8_C(0x0A) +#define BMI270_CONTEXT_ACT_RGN_SETT_STRT_ADDR UINT8_C(0x00) +#define BMI270_CONTEXT_ACT_RGN_STRT_ADDR UINT8_C(0x0A) + +/*! @name BMI270_CONTEXT feature output start addresses */ +#define BMI270_CONTEXT_STEP_CNT_OUT_STRT_ADDR UINT8_C(0x00) +#define BMI270_CONTEXT_GYR_USER_GAIN_OUT_STRT_ADDR UINT8_C(0x04) +#define BMI270_CONTEXT_GYRO_CROSS_SENSE_STRT_ADDR UINT8_C(0x0C) +#define BMI270_CONTEXT_NVM_VFRM_OUT_STRT_ADDR UINT8_C(0x0E) + +/*! @name Defines maximum number of pages */ +#define BMI270_CONTEXT_MAX_PAGE_NUM UINT8_C(8) + +/*! @name Defines maximum number of feature input configurations */ +#define BMI270_CONTEXT_MAX_FEAT_IN UINT8_C(10) + +/*! @name Defines maximum number of feature outputs */ +#define BMI270_CONTEXT_MAX_FEAT_OUT UINT8_C(5) + +/*! @name Mask definitions for feature interrupt status bits */ +#define BMI270_CONTEXT_STEP_CNT_STATUS_MASK UINT8_C(0x01) + +/*! @name Mask definitions for feature interrupt mapping bits */ +#define BMI270_C_INT_STEP_COUNTER_MASK UINT8_C(0x01) +#define BMI270_C_INT_STEP_DETECTOR_MASK UINT8_C(0x01) + +/*! @name Defines maximum number of feature interrupts */ +#define BMI270_C_MAX_INT_MAP UINT8_C(2) + +/***************************************************************************/ + +/*! BMI270_CONTEXT User Interface function prototypes + ****************************************************************************/ + +/** + * \ingroup bmi270_context + * \defgroup bmi270_contextApiInit Initialization + * @brief Initialize the sensor and device structure + */ + +/*! + * \ingroup bmi270_contextApiInit + * \page bmi270_context_api_bmi270_context_init bmi270_context_init + * \code + * int8_t bmi270_context_init(struct bmi2_dev *dev); + * \endcode + * @details This API: + * 1) updates the device structure with address of the configuration file. + * 2) Initializes BMI270_CONTEXT sensor. + * 3) Writes the configuration file. + * 4) Updates the feature offset parameters in the device structure. + * 5) Updates the maximum number of pages, in the device structure. + * + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_context_init(struct bmi2_dev *dev); + +/** + * \ingroup bmi270_context + * \defgroup bmi270_contextApiSensor Feature Set + * @brief Enable / Disable features of the sensor + */ + +/*! + * \ingroup bmi270_contextApiSensor + * \page bmi270_context_api_bmi270_context_sensor_enable bmi270_context_sensor_enable + * \code + * int8_t bmi270_context_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); + * \endcode + * @details This API selects the sensors/features to be enabled. + * + * @param[in] sens_list : Pointer to select the sensor/feature. + * @param[in] n_sens : Number of sensors selected. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @note Sensors/features that can be enabled. + * + *@verbatim + * sens_list | Values + * ----------------------------|----------- + * BMI2_ACCEL | 0 + * BMI2_GYRO | 1 + * BMI2_AUX | 2 + * BMI2_STEP_DETECTOR | 6 + * BMI2_STEP_COUNTER | 7 + * BMI2_GYRO_GAIN_UPDATE | 9 + * BMI2_ACTIVITY_RECOGNITION | 34 + *@endverbatim + * + * @note : + * example uint8_t sens_list[2] = {BMI2_ACCEL, BMI2_GYRO}; + * uint8_t n_sens = 2; + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_context_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); + +/*! + * \ingroup bmi270_contextApiSensor + * \page bmi270_context_api_bmi270_context_sensor_disable bmi270_context_sensor_disable + * \code + * int8_t bmi270_context_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); + * \endcode + * @details This API selects the sensors/features to be disabled. + * + * @param[in] sens_list : Pointer to select the sensor/feature. + * @param[in] n_sens : Number of sensors selected. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @note Sensors/features that can be disabled. + * + *@verbatim + * sens_list | Values + * ----------------------------|----------- + * BMI2_ACCEL | 0 + * BMI2_GYRO | 1 + * BMI2_AUX | 2 + * BMI2_STEP_DETECTOR | 6 + * BMI2_STEP_COUNTER | 7 + * BMI2_GYRO_GAIN_UPDATE | 9 + * BMI2_ACTIVITY_RECOGNITION | 34 + *@endverbatim + * + * @note : + * example uint8_t sens_list[2] = {BMI2_ACCEL, BMI2_GYRO}; + * uint8_t n_sens = 2; + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_context_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); + +/** + * \ingroup bmi270_context + * \defgroup bmi270_contextApiSensorC Sensor Configuration + * @brief Enable / Disable feature configuration of the sensor + */ + +/*! + * \ingroup bmi270_contextApiSensorC + * \page bmi270_context_api_bmi270_context_set_sensor_config bmi270_context_set_sensor_config + * \code + * int8_t bmi270_context_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); + * \endcode + * @details This API sets the sensor/feature configuration. + * + * @param[in] sens_cfg : Structure instance of bmi2_sens_config. + * @param[in] n_sens : Number of sensors selected. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @note Sensors/features that can be configured + * + *@verbatim + * sens_list | Values + * ----------------------------|----------- + * BMI2_STEP_DETECTOR | 6 + * BMI2_STEP_COUNTER | 7 + * BMI2_STEP_COUNTER_PARAMS | 28 + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_context_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); + +/*! + * \ingroup bmi270_contextApiSensorC + * \page bmi270_context_api_bmi270_context_get_sensor_config bmi270_context_get_sensor_config + * \code + * int8_t bmi270_context_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); + * \endcode + * @details This API gets the sensor/feature configuration. + * + * @param[in] sens_cfg : Structure instance of bmi2_sens_config. + * @param[in] n_sens : Number of sensors selected. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @note Sensors/features whose configurations can be read. + * + *@verbatim + * sens_list | Values + * ----------------------------|----------- + * BMI2_STEP_DETECTOR | 6 + * BMI2_STEP_COUNTER | 7 + * BMI2_STEP_COUNTER_PARAMS | 28 + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_context_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); + +/** + * \ingroup bmi270_context + * \defgroup bmi270_contextApiSensorD Sensor Data + * @brief Get sensor data + */ + +/*! + * \ingroup bmi270_contextApiSensorD + * \page bmi270_context_api_bmi270_context_get_sensor_data bmi270_context_get_sensor_data + * \code + * int8_t bmi270_context_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev); + * \endcode + * @details This API gets the sensor/feature data for accelerometer, gyroscope, + * auxiliary sensor, step counter, high-g, gyroscope user-gain update, + * orientation, gyroscope cross sensitivity and error status for NVM and VFRM. + * + * @param[out] sensor_data : Structure instance of bmi2_sensor_data. + * @param[in] n_sens : Number of sensors selected. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @note Sensors/features whose data can be read + * + *@verbatim + * sens_list | Values + * ---------------------|----------- + * BMI2_STEP_COUNTER | 7 + * BMI2_NVM_STATUS | 38 + * BMI2_VFRM_STATUS | 39 + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_context_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev); + +/** + * \ingroup bmi270_context + * \defgroup bmi270_contextApiARecog Activity recognition settings + * @brief Set / Get activity recognition settings of the sensor + */ + +/*! + * \ingroup bmi270_contextApiARecog + * \page bmi270_context_api_bmi270_context_get_act_recg_sett bmi270_context_get_act_recg_sett + * \code + * int8_t bmi270_context_get_act_recg_sett(struct bmi2_act_recg_sett *sett, struct bmi2_dev *dev); + * \endcode + * @details This api is used for retrieving the following activity recognition settings currently set. + * enable/disable post processing(0/1) by default -> 1(enable), + * Setting the min & max Gini's diversity index (GDI) threshold. min_GDI_tres(0-0XFFFF) by default ->(0x06e1) + * max_GDI_tres(0-0xFFFF) by default ->(0x0A66) + * buffer size for post processing. range (1-0x0A) default -> (0x0A) + * min segment confidence. range (1-0x0A) default -> (0x0A) + * + * @param[in] sett : Structure instance of bmi2_act_recg_sett. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_context_get_act_recg_sett(struct bmi2_act_recg_sett *sett, struct bmi2_dev *dev); + +/*! + * \ingroup bmi270_contextApiARecog + * \page bmi270_context_api_bmi270_context_set_act_recg_sett bmi270_context_set_act_recg_sett + * \code + * int8_t bmi270_context_set_act_recg_sett(const struct bmi2_act_recg_sett *sett, struct bmi2_dev *dev); + * \endcode + * @details This api is used for setting the following activity recognition settings + * enable/disable post processing(0/1) by default -> 1(enable), + * Setting the min & max Gini's diversity index (GDI) threshold. min_GDI_tres(0-0XFFFF) by default ->(0x06e1) + * max_GDI_tres(0-0xFFFF) by default ->(0x0A66) + * buffer size for post processing. range (1-0x0A) default -> (0x0A) + * min segment confidence. range (1-0x0A) default -> (0x0A) + * + * @param[in] sett : Structure instance of bmi2_act_recg_sett. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_context_set_act_recg_sett(const struct bmi2_act_recg_sett *sett, struct bmi2_dev *dev); + +/** + * \ingroup bmi270_contex + * \defgroup bmi270_contextApiactOut Activity Output + * @brief Activity output operations of the sensor + */ + +/*! + * \ingroup bmi270_contextApiactOut + * \page bmi270_context_api_bmi270_context_get_act_recog_output bmi270_context_get_act_recog_output + * \code + * int8_t bmi270_context_get_act_recog_output(struct bmi2_act_recog_output *act_recog, + * uint16_t *act_frm_len, + * struct bmi2_fifo_frame *fifo, + * const struct bmi2_dev *dev); + * + * \endcode + * @details This internal API is used to parse the activity output from the + * FIFO in header mode. + * + * @param[out] act_recog : Pointer to buffer where the parsed activity data + * bytes are stored. + * @param[in] act_frm_len : Number of activity frames parsed. + * @param[in] fifo : Structure instance of bmi2_fifo_frame. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @verbatim + * ---------------------------------------------------------------------------- + * bmi2_act_rec_output | + * Structure parameters | Description + *--------------------------|-------------------------------------------------- + * time_stamp | time-stamp (expressed in 50Hz ticks) + * -------------------------|--------------------------------------------------- + * type | Type of activity + * -------------------------|--------------------------------------------------- + * stat | Activity status + * -------------------------|--------------------------------------------------- + * @endverbatim + * + *@verbatim + * type | Activities + *----------|--------------------- + * 0 | UNKNOWN + * 1 | STILL + * 2 | WALK + * 3 | RUN + * 4 | BIKE + * 5 | VEHICLE + * 6 | TILTED + *@endverbatim + * + * + *@verbatim + * stat | Activity status + *----------|--------------------- + * 1 | START + * 2 | END + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_context_get_act_recog_output(struct bmi2_act_recog_output *act_recog, + uint16_t *act_frm_len, + struct bmi2_fifo_frame *fifo, + const struct bmi2_dev *dev); + +/** + * \ingroup bmi270_contex + * \defgroup bmi270_contextApiGyroUG Gyro User Gain + * @brief Set / Get Gyro User Gain of the sensor + */ + +/*! + * \ingroup bmi270_contextApiGyroUG + * \page bmi270_context_api_bmi270_context_update_gyro_user_gain bmi270_context_update_gyro_user_gain + * \code + * int8_t bmi270_context_update_gyro_user_gain(const struct bmi2_gyro_user_gain_config *user_gain, struct bmi2_dev *dev); + * \endcode + * @details This API updates the gyroscope user-gain. + * + * @param[in] user_gain : Structure that stores user-gain configurations. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_context_update_gyro_user_gain(const struct bmi2_gyro_user_gain_config *user_gain, struct bmi2_dev *dev); + +/*! + * \ingroup bmi270_contextApiGyroUG + * \page bmi270_context_api_bmi270_context_read_gyro_user_gain bmi270_context_read_gyro_user_gain + * \code + * int8_t bmi270_context_read_gyro_user_gain(struct bmi2_gyro_user_gain_data *gyr_usr_gain, const struct bmi2_dev *dev); + * \endcode + * @details This API reads the compensated gyroscope user-gain values. + * + * @param[out] gyr_usr_gain : Structure that stores gain values. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_context_read_gyro_user_gain(struct bmi2_gyro_user_gain_data *gyr_usr_gain, struct bmi2_dev *dev); + +/*! + * \ingroup bmi270_contextApiInt + * \page bmi270_context_api_bmi270_context_map_feat_int bmi270_context_map_feat_int + * \code + * int8_t bmi270_context_map_feat_int(const struct bmi2_sens_int_config *sens_int, uint8_t n_sens, struct bmi2_dev *dev) + * \endcode + * @details This API maps/unmaps feature interrupts to that of interrupt pins. + * + * @param[in] sens_int : Structure instance of bmi2_sens_int_config. + * @param[in] n_sens : Number of interrupts to be mapped. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_context_map_feat_int(const struct bmi2_sens_int_config *sens_int, uint8_t n_sens, struct bmi2_dev *dev); + +/******************************************************************************/ +/*! @name C++ Guard Macros */ +/******************************************************************************/ +#ifdef __cplusplus +} +#endif /* End of CPP guard */ + +#endif /* BMI270_CONTEXT_H_ */ diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_hc.c b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_hc.c new file mode 100644 index 0000000000..a7c9172dd4 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_hc.c @@ -0,0 +1,3165 @@ +/** +* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. +* +* BSD-3-Clause +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* +* 3. Neither the name of the copyright holder nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, +* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING +* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +* @file bmi270_hc.c +* @date 2020-11-04 +* @version v2.63.1 +* +*/ + +/***************************************************************************/ + +/*! Header files + ****************************************************************************/ +#include "bmi270_hc.h" + +/***************************************************************************/ + +/*! Global Variable + ****************************************************************************/ + +/*! @name Global array that stores the configuration file of BMI270_huawei_context */ +const uint8_t bmi270_hc_config_file[] = { + 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x00, 0xb0, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0xa8, + 0x03, 0x80, 0x2e, 0x91, 0x03, 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x7d, 0xb0, 0x50, 0x30, 0x21, 0x2e, 0x59, 0xf5, + 0x10, 0x30, 0x21, 0x2e, 0x6a, 0xf5, 0x80, 0x2e, 0xae, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x39, 0x01, 0x00, 0x22, + 0x00, 0x76, 0x00, 0x00, 0x10, 0x00, 0x10, 0xd1, 0x00, 0x72, 0xb3, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0xfd, 0x2d, 0xd0, 0x86, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x03, 0x00, 0x08, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x01, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1a, 0x24, 0x22, 0x00, 0x80, 0x2e, 0x48, 0x02, 0x01, 0x2e, 0x49, 0xf1, 0x0b, + 0xbc, 0x10, 0x50, 0x0f, 0xb8, 0x00, 0x90, 0xfb, 0x7f, 0x07, 0x2f, 0x03, 0x2e, 0x21, 0xf2, 0x02, 0x31, 0x4a, 0x0a, + 0x23, 0x2e, 0x21, 0xf2, 0x09, 0x2c, 0x00, 0x30, 0x98, 0x2e, 0x0e, 0xc7, 0x03, 0x2e, 0x21, 0xf2, 0xf2, 0x3e, 0x4a, + 0x08, 0x23, 0x2e, 0x21, 0xf2, 0xfb, 0x6f, 0xf0, 0x5f, 0xb8, 0x2e, 0x13, 0x52, 0x00, 0x2e, 0x60, 0x40, 0x41, 0x40, + 0x0d, 0xbc, 0x98, 0xbc, 0xc0, 0x2e, 0x01, 0x0a, 0x0f, 0xb8, 0x43, 0x86, 0x25, 0x40, 0x04, 0x40, 0xd8, 0xbe, 0x2c, + 0x0b, 0x22, 0x11, 0x54, 0x42, 0x03, 0x80, 0x4b, 0x0e, 0xf6, 0x2f, 0xb8, 0x2e, 0x15, 0x50, 0x10, 0x50, 0x17, 0x52, + 0x05, 0x2e, 0x7e, 0x00, 0xfb, 0x7f, 0x00, 0x2e, 0x13, 0x40, 0x93, 0x42, 0x41, 0x0e, 0xfb, 0x2f, 0x98, 0x2e, 0xe0, + 0x03, 0x98, 0x2e, 0x87, 0xcf, 0x01, 0x2e, 0x8b, 0x00, 0x00, 0xb2, 0x08, 0x2f, 0x01, 0x2e, 0x69, 0xf7, 0xb1, 0x3f, + 0x01, 0x08, 0x01, 0x30, 0x23, 0x2e, 0x8b, 0x00, 0x21, 0x2e, 0x69, 0xf7, 0xfb, 0x6f, 0xf0, 0x5f, 0xb8, 0x2e, 0x98, + 0x00, 0xff, 0x3f, 0x00, 0x0c, 0xff, 0x0f, 0x00, 0x04, 0xc0, 0x00, 0x5b, 0xf5, 0x8f, 0x00, 0x1e, 0xf2, 0xfd, 0xf5, + 0x8d, 0x00, 0x95, 0x00, 0x95, 0x00, 0xe0, 0x00, 0x19, 0xf4, 0x66, 0xf5, 0x00, 0x18, 0xff, 0x00, 0x64, 0xf5, 0x9c, + 0x00, 0x81, 0x00, 0x83, 0x00, 0x7f, 0x00, 0xff, 0xfb, 0x21, 0x02, 0x00, 0x10, 0x00, 0x40, 0x3a, 0x0f, 0xeb, 0x00, + 0x7f, 0xff, 0xc2, 0xf5, 0x68, 0xf7, 0x34, 0x0f, 0x28, 0x0f, 0x2e, 0x0f, 0x80, 0x00, 0x4d, 0x0f, 0x58, 0xf7, 0x5b, + 0xf7, 0x50, 0x0f, 0x00, 0x80, 0xff, 0x7f, 0x86, 0x00, 0x3f, 0x0f, 0x52, 0x0f, 0xb3, 0xf1, 0x4c, 0x0f, 0x6c, 0xf7, + 0xb9, 0xf1, 0xc6, 0xf1, 0x00, 0xe0, 0x00, 0xff, 0xd1, 0xf5, 0x54, 0x0f, 0x57, 0x0f, 0xff, 0x03, 0x00, 0xfc, 0xf0, + 0x3f, 0xb9, 0x00, 0x2d, 0xf5, 0xca, 0xf5, 0x6d, 0x03, 0xf0, 0x07, 0xc3, 0x11, 0x1d, 0x08, 0x12, 0x08, 0xb7, 0x15, + 0x9c, 0x01, 0xed, 0x14, 0xc2, 0x53, 0xca, 0x10, 0xa2, 0x00, 0x74, 0x01, 0xba, 0x17, 0xb4, 0x56, 0xe8, 0x16, 0x9f, + 0x00, 0xd7, 0x13, 0x2f, 0x52, 0x8a, 0x51, 0x64, 0x01, 0xc6, 0x53, 0xf4, 0x0b, 0x06, 0x08, 0xfc, 0x57, 0xe3, 0x44, + 0xad, 0x55, 0x4c, 0x01, 0x62, 0x01, 0x0b, 0x08, 0xa0, 0x18, 0x39, 0x59, 0x56, 0x2b, 0x14, 0x01, 0x2a, 0x58, 0x5e, + 0x0c, 0xc1, 0x47, 0x16, 0x5a, 0xd2, 0x07, 0x2b, 0x21, 0x7f, 0x53, 0x03, 0x08, 0xca, 0x59, 0x1a, 0x0b, 0x5f, 0x57, + 0x36, 0x47, 0xa6, 0x17, 0x02, 0x01, 0x33, 0x49, 0xdd, 0x58, 0x71, 0x0c, 0x32, 0x08, 0xc6, 0x59, 0xd4, 0x42, 0x65, + 0x54, 0x15, 0x59, 0x64, 0x08, 0x0c, 0x08, 0x8c, 0x01, 0x98, 0x00, 0x0f, 0x48, 0x02, 0x58, 0x96, 0x0c, 0x1c, 0x43, + 0x77, 0x48, 0xe9, 0x00, 0x1e, 0x0c, 0xf5, 0x41, 0xf6, 0x46, 0x64, 0x51, 0x98, 0x41, 0xcc, 0x01, 0x66, 0x58, 0x70, + 0x45, 0x28, 0x21, 0xe7, 0x59, 0x22, 0x30, 0x00, 0x08, 0x71, 0x7d, 0x49, 0x01, 0x92, 0x02, 0xf5, 0xd6, 0xe8, 0x63, + 0x7a, 0xfa, 0x87, 0x05, 0x0f, 0xcb, 0xa3, 0x72, 0x37, 0xfc, 0xca, 0x03, 0x95, 0xc7, 0x2e, 0x6d, 0x39, 0xc4, 0xc8, + 0x3b, 0x91, 0x37, 0x29, 0x02, 0x9e, 0x01, 0x00, 0xf0, 0x33, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x2e, 0xd5, 0xb5, 0x80, 0x2e, 0x00, 0xc1, 0xfd, 0x2d, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x2d, 0x01, 0xd4, 0x7b, 0x3b, + 0x01, 0xdb, 0x7a, 0x04, 0x00, 0x3f, 0x7b, 0xcd, 0x6c, 0xc3, 0x04, 0x85, 0x09, 0xc3, 0x04, 0xec, 0xe6, 0x0c, 0x46, + 0x01, 0x00, 0x27, 0x00, 0x19, 0x00, 0x96, 0x00, 0xa0, 0x00, 0x01, 0x00, 0x0c, 0x00, 0xf0, 0x3c, 0x00, 0x01, 0x01, + 0x00, 0x03, 0x00, 0x01, 0x00, 0x0e, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x39, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0xe1, 0x06, 0x66, 0x0a, 0x0a, 0x00, 0x0a, 0x00, 0x5a, 0x8d, 0x33, + 0x73, 0xcd, 0x8c, 0x9a, 0x99, 0x71, 0x7d, 0xcd, 0x8c, 0xcd, 0x6c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x50, 0x98, 0x2e, 0xbd, 0x0e, 0x50, 0x32, 0x98, 0x2e, + 0x48, 0x03, 0x10, 0x30, 0x21, 0x2e, 0x21, 0xf2, 0x00, 0x30, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0x00, 0x2e, 0x01, + 0x80, 0x06, 0xa2, 0xfb, 0x2f, 0x01, 0x2e, 0x9b, 0x00, 0x00, 0xb2, 0x10, 0x2f, 0x01, 0x2e, 0x18, 0x00, 0x00, 0xb2, + 0x0c, 0x2f, 0x01, 0x54, 0x03, 0x52, 0x01, 0x50, 0x98, 0x2e, 0xc2, 0xc0, 0x98, 0x2e, 0x0e, 0xb1, 0x01, 0x50, 0x98, + 0x2e, 0xdd, 0xb5, 0x10, 0x30, 0x21, 0x2e, 0x19, 0x00, 0x01, 0x2e, 0x86, 0x00, 0x04, 0xae, 0x0b, 0x2f, 0x01, 0x2e, + 0x9b, 0x00, 0x00, 0xb2, 0x07, 0x2f, 0x01, 0x52, 0x98, 0x2e, 0x74, 0x0e, 0x00, 0xb2, 0x02, 0x2f, 0x10, 0x30, 0x21, + 0x2e, 0x79, 0x00, 0x01, 0x2e, 0x79, 0x00, 0x00, 0x90, 0x90, 0x2e, 0x14, 0x03, 0x01, 0x2e, 0x89, 0x00, 0x00, 0xb2, + 0x04, 0x2f, 0x98, 0x2e, 0x15, 0x0e, 0x00, 0x30, 0x21, 0x2e, 0x7c, 0x00, 0x01, 0x2e, 0x7c, 0x00, 0x00, 0xb2, 0x12, + 0x2f, 0x01, 0x2e, 0x86, 0x00, 0x00, 0x90, 0x02, 0x2f, 0x98, 0x2e, 0x05, 0x0e, 0x09, 0x2d, 0x98, 0x2e, 0x5d, 0x0d, + 0x01, 0x2e, 0x86, 0x00, 0x04, 0x90, 0x02, 0x2f, 0x50, 0x32, 0x98, 0x2e, 0x48, 0x03, 0x00, 0x30, 0x21, 0x2e, 0x7c, + 0x00, 0x01, 0x2e, 0x78, 0x00, 0x00, 0xb2, 0x90, 0x2e, 0x2c, 0x03, 0x01, 0x2e, 0x78, 0x00, 0x81, 0x30, 0x01, 0x08, + 0x00, 0xb2, 0x61, 0x2f, 0x03, 0x2e, 0x24, 0x02, 0x01, 0x2e, 0x86, 0x00, 0x98, 0xbc, 0x98, 0xb8, 0x05, 0xb2, 0x0d, + 0x58, 0x23, 0x2f, 0x07, 0x90, 0x07, 0x54, 0x00, 0x30, 0x37, 0x2f, 0x15, 0x41, 0x04, 0x41, 0xdc, 0xbe, 0x44, 0xbe, + 0xdc, 0xba, 0x2c, 0x01, 0x61, 0x00, 0x0d, 0x56, 0x4a, 0x0f, 0x0c, 0x2f, 0xd1, 0x42, 0x94, 0xb8, 0xc1, 0x42, 0x11, + 0x30, 0x05, 0x2e, 0x6a, 0xf7, 0x2c, 0xbd, 0x2f, 0xb9, 0x80, 0xb2, 0x08, 0x22, 0x98, 0x2e, 0xa4, 0xb1, 0x21, 0x2d, + 0x61, 0x30, 0x23, 0x2e, 0x86, 0x00, 0x98, 0x2e, 0xa4, 0xb1, 0x00, 0x30, 0x21, 0x2e, 0x5a, 0xf5, 0x18, 0x2d, 0xf1, + 0x7f, 0x50, 0x30, 0x98, 0x2e, 0x48, 0x03, 0x0d, 0x52, 0x05, 0x50, 0x50, 0x42, 0x70, 0x30, 0x0b, 0x54, 0x42, 0x42, + 0x7e, 0x82, 0xf2, 0x6f, 0x80, 0xb2, 0x42, 0x42, 0x05, 0x2f, 0x21, 0x2e, 0x86, 0x00, 0x10, 0x30, 0x98, 0x2e, 0xa4, + 0xb1, 0x03, 0x2d, 0x60, 0x30, 0x21, 0x2e, 0x86, 0x00, 0x01, 0x2e, 0x86, 0x00, 0x06, 0x90, 0x18, 0x2f, 0x01, 0x2e, + 0x77, 0x00, 0x09, 0x54, 0x05, 0x52, 0xf0, 0x7f, 0x98, 0x2e, 0x7a, 0xc1, 0xf1, 0x6f, 0x08, 0x1a, 0x40, 0x30, 0x08, + 0x2f, 0x21, 0x2e, 0x86, 0x00, 0x20, 0x30, 0x98, 0x2e, 0xea, 0x03, 0x50, 0x32, 0x98, 0x2e, 0x48, 0x03, 0x05, 0x2d, + 0x98, 0x2e, 0x1e, 0x0e, 0x00, 0x30, 0x21, 0x2e, 0x86, 0x00, 0x00, 0x30, 0x21, 0x2e, 0x78, 0x00, 0x18, 0x2d, 0x01, + 0x2e, 0x86, 0x00, 0x03, 0xaa, 0x01, 0x2f, 0x98, 0x2e, 0x2b, 0x0e, 0x01, 0x2e, 0x86, 0x00, 0x3f, 0x80, 0x03, 0xa2, + 0x01, 0x2f, 0x00, 0x2e, 0x02, 0x2d, 0x98, 0x2e, 0x41, 0x0e, 0x30, 0x30, 0x98, 0x2e, 0xaf, 0xb1, 0x00, 0x30, 0x21, + 0x2e, 0x79, 0x00, 0x50, 0x32, 0x98, 0x2e, 0x48, 0x03, 0x01, 0x2e, 0x19, 0x00, 0x00, 0xb2, 0x10, 0x2f, 0x01, 0x2e, + 0x87, 0x00, 0x21, 0x2e, 0x8f, 0x00, 0x0f, 0x52, 0x7e, 0x82, 0x11, 0x50, 0x41, 0x40, 0x18, 0xb9, 0x11, 0x42, 0x02, + 0x42, 0x02, 0x80, 0x00, 0x2e, 0x01, 0x40, 0x01, 0x42, 0x98, 0x2e, 0xe1, 0x00, 0x00, 0x30, 0x21, 0x2e, 0x19, 0x00, + 0x21, 0x2e, 0x9b, 0x00, 0x80, 0x2e, 0x52, 0x02, 0x21, 0x2e, 0x59, 0xf5, 0x10, 0x30, 0xc0, 0x2e, 0x21, 0x2e, 0x4a, + 0xf1, 0x80, 0x2e, 0x00, 0xc1, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x9a, 0x01, + 0x34, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x20, 0x50, 0xe7, 0x7f, 0xf6, 0x7f, 0x46, 0x30, 0x0f, 0x2e, 0xa4, 0xf1, 0xbe, 0x09, 0x80, 0xb3, 0x06, + 0x2f, 0x0d, 0x2e, 0x86, 0x00, 0x84, 0xaf, 0x02, 0x2f, 0x16, 0x30, 0x2d, 0x2e, 0x7c, 0x00, 0x86, 0x30, 0x2d, 0x2e, + 0x60, 0xf5, 0xf6, 0x6f, 0xe7, 0x6f, 0xe0, 0x5f, 0xc8, 0x2e, 0xa0, 0x50, 0x80, 0x7f, 0xe7, 0x7f, 0xd5, 0x7f, 0xc4, + 0x7f, 0xb3, 0x7f, 0xa2, 0x7f, 0x91, 0x7f, 0xf6, 0x7f, 0x7b, 0x7f, 0x00, 0x2e, 0x01, 0x2e, 0x60, 0xf5, 0x60, 0x7f, + 0x98, 0x2e, 0xcd, 0x00, 0x62, 0x6f, 0x01, 0x32, 0x91, 0x08, 0x80, 0xb2, 0x0d, 0x2f, 0x00, 0xb2, 0x03, 0x2f, 0x05, + 0x2e, 0x18, 0x00, 0x80, 0x90, 0x05, 0x2f, 0x25, 0x56, 0x02, 0x30, 0xc1, 0x42, 0xc2, 0x86, 0x00, 0x2e, 0xc2, 0x42, + 0x23, 0x2e, 0x60, 0xf5, 0x00, 0x90, 0x00, 0x30, 0x06, 0x2f, 0x21, 0x2e, 0x7a, 0x00, 0x23, 0x50, 0x21, 0x2e, 0x5a, + 0xf2, 0x98, 0x2e, 0xfb, 0x01, 0xf6, 0x6f, 0x80, 0x6f, 0x91, 0x6f, 0xa2, 0x6f, 0xb3, 0x6f, 0xc4, 0x6f, 0xd5, 0x6f, + 0xe7, 0x6f, 0x7b, 0x6f, 0x60, 0x5f, 0xc8, 0x2e, 0x03, 0x2e, 0x7e, 0x00, 0x16, 0xb8, 0x02, 0x34, 0x4a, 0x0c, 0x21, + 0x2e, 0x2d, 0xf5, 0xc0, 0x2e, 0x23, 0x2e, 0x7e, 0x00, 0x03, 0xbc, 0x21, 0x2e, 0x87, 0x00, 0x03, 0x2e, 0x87, 0x00, + 0x40, 0xb2, 0x10, 0x30, 0x21, 0x2e, 0x19, 0x00, 0x01, 0x30, 0x05, 0x2f, 0x05, 0x2e, 0x8a, 0x00, 0x80, 0x90, 0x01, + 0x2f, 0x23, 0x2e, 0x6f, 0xf5, 0xc0, 0x2e, 0x21, 0x2e, 0x8b, 0x00, 0x80, 0x2e, 0x00, 0xc1, 0xc0, 0x50, 0xe7, 0x7f, + 0xf6, 0x7f, 0x26, 0x30, 0x0f, 0x2e, 0x61, 0xf5, 0x2f, 0x2e, 0x78, 0x00, 0x0f, 0x2e, 0x78, 0x00, 0xbe, 0x09, 0xa2, + 0x7f, 0x91, 0x7f, 0x80, 0x7f, 0x80, 0xb3, 0xd5, 0x7f, 0xc4, 0x7f, 0xb3, 0x7f, 0x7b, 0x7f, 0x11, 0x2f, 0x19, 0x50, + 0x1a, 0x25, 0x12, 0x40, 0x42, 0x7f, 0x74, 0x82, 0x12, 0x40, 0x52, 0x7f, 0x00, 0x2e, 0x00, 0x40, 0x60, 0x7f, 0x98, + 0x2e, 0x6a, 0xd6, 0x01, 0x2e, 0x61, 0xf7, 0x01, 0x31, 0x01, 0x0a, 0x21, 0x2e, 0x61, 0xf7, 0x80, 0x30, 0x03, 0x2e, + 0x78, 0x00, 0x08, 0x08, 0x00, 0xb2, 0x42, 0x2f, 0x03, 0x2e, 0x24, 0x02, 0x01, 0x2e, 0x24, 0x02, 0x97, 0xbc, 0x06, + 0xbc, 0x9f, 0xb8, 0x0f, 0xb8, 0x00, 0x90, 0x23, 0x2e, 0x8a, 0x00, 0x10, 0x30, 0x01, 0x30, 0x2a, 0x2f, 0x03, 0x2e, + 0x86, 0x00, 0x44, 0xb2, 0x05, 0x2f, 0x47, 0xb2, 0x00, 0x30, 0x2d, 0x2f, 0x21, 0x2e, 0x78, 0x00, 0x2b, 0x2d, 0x03, + 0x2e, 0xfd, 0xf5, 0x9e, 0xbc, 0x9f, 0xb8, 0x40, 0x90, 0x14, 0x2f, 0x03, 0x2e, 0xfc, 0xf5, 0x99, 0xbc, 0x9f, 0xb8, + 0x40, 0x90, 0x0e, 0x2f, 0x03, 0x2e, 0x49, 0xf1, 0x1b, 0x54, 0x4a, 0x08, 0x40, 0x90, 0x08, 0x2f, 0x98, 0x2e, 0xcd, + 0x00, 0x00, 0xb2, 0x10, 0x30, 0x03, 0x2f, 0x50, 0x30, 0x21, 0x2e, 0x86, 0x00, 0x10, 0x2d, 0x98, 0x2e, 0xea, 0x03, + 0x00, 0x30, 0x21, 0x2e, 0x78, 0x00, 0x0a, 0x2d, 0x05, 0x2e, 0x69, 0xf7, 0x2d, 0xbd, 0x2f, 0xb9, 0x80, 0xb2, 0x01, + 0x2f, 0x21, 0x2e, 0x79, 0x00, 0x23, 0x2e, 0x78, 0x00, 0xe0, 0x31, 0x21, 0x2e, 0x61, 0xf5, 0xf6, 0x6f, 0xe7, 0x6f, + 0x80, 0x6f, 0xa2, 0x6f, 0xb3, 0x6f, 0xc4, 0x6f, 0xd5, 0x6f, 0x7b, 0x6f, 0x91, 0x6f, 0x40, 0x5f, 0xc8, 0x2e, 0x90, + 0x50, 0xf7, 0x7f, 0xe6, 0x7f, 0xd5, 0x7f, 0xc4, 0x7f, 0xb3, 0x7f, 0xa1, 0x7f, 0x90, 0x7f, 0x82, 0x7f, 0x7b, 0x7f, + 0x98, 0x2e, 0xcd, 0x00, 0x00, 0xb2, 0x12, 0x30, 0x5c, 0x2f, 0x01, 0x2e, 0x21, 0x02, 0x03, 0x2e, 0x28, 0x02, 0x9f, + 0xbc, 0x9f, 0xb8, 0x21, 0x56, 0x8a, 0x08, 0x03, 0x08, 0x82, 0x0a, 0x25, 0x2e, 0x18, 0x00, 0x05, 0x2e, 0xc1, 0xf5, + 0x2e, 0xbc, 0x05, 0x2e, 0x86, 0x00, 0x84, 0xa2, 0x0e, 0xb8, 0x31, 0x30, 0x88, 0x04, 0x07, 0x2f, 0x01, 0x2e, 0x18, + 0x00, 0x00, 0x90, 0x03, 0x2f, 0x01, 0x2e, 0x7b, 0x00, 0x00, 0xb2, 0x19, 0x2f, 0x1d, 0x50, 0x01, 0x52, 0x98, 0x2e, + 0xd6, 0x00, 0x05, 0x2e, 0x7a, 0x00, 0x25, 0x2e, 0x9b, 0x00, 0x05, 0x2e, 0x7a, 0x00, 0x80, 0x90, 0x02, 0x2f, 0x12, + 0x30, 0x25, 0x2e, 0x7a, 0x00, 0x01, 0x2e, 0x7b, 0x00, 0x00, 0xb2, 0x05, 0x2e, 0x18, 0x00, 0x02, 0x2f, 0x10, 0x30, + 0x21, 0x2e, 0x18, 0x00, 0x25, 0x2e, 0x7b, 0x00, 0x05, 0x2e, 0x18, 0x00, 0x80, 0xb2, 0x20, 0x2f, 0x01, 0x2e, 0xc0, + 0xf5, 0xf2, 0x30, 0x02, 0x08, 0x07, 0xaa, 0x73, 0x30, 0x03, 0x2e, 0x7d, 0x00, 0x18, 0x22, 0x41, 0x1a, 0x05, 0x2f, + 0x03, 0x2e, 0x66, 0xf5, 0x9f, 0xbc, 0x9f, 0xb8, 0x40, 0x90, 0x0c, 0x2f, 0x1f, 0x52, 0x03, 0x30, 0x53, 0x42, 0x2b, + 0x30, 0x90, 0x04, 0x5b, 0x42, 0x21, 0x2e, 0x7d, 0x00, 0x24, 0xbd, 0x7e, 0x80, 0x81, 0x84, 0x43, 0x42, 0x02, 0x42, + 0x02, 0x32, 0x25, 0x2e, 0x62, 0xf5, 0x05, 0x2e, 0x88, 0x00, 0x81, 0x84, 0x25, 0x2e, 0x88, 0x00, 0x02, 0x31, 0x25, + 0x2e, 0x60, 0xf5, 0x05, 0x2e, 0x25, 0x02, 0x10, 0x30, 0x90, 0x08, 0x80, 0xb2, 0x0b, 0x2f, 0x05, 0x2e, 0xca, 0xf5, + 0xf0, 0x3e, 0x90, 0x08, 0x25, 0x2e, 0xca, 0xf5, 0x05, 0x2e, 0x59, 0xf5, 0xe0, 0x3f, 0x90, 0x08, 0x25, 0x2e, 0x59, + 0xf5, 0x90, 0x6f, 0xa1, 0x6f, 0xb3, 0x6f, 0xc4, 0x6f, 0xd5, 0x6f, 0xe6, 0x6f, 0xf7, 0x6f, 0x7b, 0x6f, 0x82, 0x6f, + 0x70, 0x5f, 0xc8, 0x2e, 0x31, 0x50, 0xe0, 0x50, 0x12, 0x40, 0x06, 0x40, 0x1a, 0x25, 0x77, 0x88, 0x75, 0x8a, 0x21, + 0x56, 0x6c, 0xbf, 0x00, 0x30, 0xd3, 0x08, 0x6c, 0xbb, 0x60, 0x7f, 0x00, 0x43, 0x40, 0x43, 0xc0, 0xb2, 0xd6, 0x7f, + 0xe5, 0x7f, 0xf4, 0x7f, 0xc3, 0x7f, 0xbb, 0x7f, 0x74, 0x2f, 0x01, 0x2e, 0x85, 0x00, 0x00, 0xb2, 0x0b, 0x2f, 0x27, + 0x52, 0x01, 0x2e, 0x80, 0x00, 0xa2, 0x7f, 0x98, 0x2e, 0xbb, 0xcc, 0x01, 0x30, 0x23, 0x2e, 0x85, 0x00, 0xa2, 0x6f, + 0xc3, 0x6f, 0x1a, 0x25, 0x26, 0xbc, 0x86, 0xba, 0x25, 0xbc, 0x0f, 0xb8, 0x54, 0xb1, 0x00, 0xb2, 0xa6, 0x7f, 0x0c, + 0x2f, 0x29, 0x50, 0x2b, 0x54, 0x0b, 0x30, 0x0b, 0x2e, 0x21, 0x02, 0x2f, 0x58, 0x1b, 0x42, 0x9b, 0x42, 0x6c, 0x09, + 0x0b, 0x42, 0x2b, 0x2e, 0x21, 0x02, 0x8b, 0x42, 0x72, 0x84, 0x33, 0x50, 0x58, 0x09, 0x03, 0x52, 0x01, 0x50, 0x92, + 0x7f, 0x85, 0x7f, 0x98, 0x2e, 0xc2, 0xc0, 0x01, 0x2e, 0x80, 0x00, 0xf5, 0x6f, 0xe4, 0x6f, 0x83, 0x6f, 0x92, 0x6f, + 0x27, 0x52, 0x2d, 0x5c, 0x98, 0x2e, 0x06, 0xcd, 0xc3, 0x6f, 0x29, 0x50, 0x72, 0x6f, 0xb4, 0xbc, 0x14, 0x40, 0x80, + 0xb2, 0x9f, 0xba, 0x02, 0x40, 0x01, 0x30, 0xf0, 0x7f, 0x05, 0x2f, 0x40, 0xb3, 0x03, 0x2f, 0x2b, 0x5c, 0x11, 0x30, + 0x94, 0x43, 0x82, 0x43, 0xb3, 0xbd, 0xbf, 0xb9, 0xc0, 0xb2, 0x1c, 0x2f, 0x53, 0x6f, 0x23, 0x01, 0x63, 0x6f, 0x93, + 0x02, 0x02, 0x42, 0x40, 0x91, 0x29, 0x2e, 0x81, 0x00, 0x2b, 0x50, 0x12, 0x2f, 0x2b, 0x56, 0x00, 0x2e, 0xd5, 0x40, + 0xc3, 0x40, 0x65, 0x05, 0xd3, 0x06, 0xc0, 0xaa, 0x04, 0x2f, 0xc0, 0x90, 0x08, 0x2f, 0xa3, 0x6f, 0x5d, 0x0f, 0x05, + 0x2f, 0xa5, 0x6f, 0x40, 0xb3, 0x02, 0x2f, 0x14, 0x42, 0x02, 0x42, 0x11, 0x30, 0xd0, 0x6f, 0x98, 0x2e, 0x95, 0xcf, + 0xf2, 0x6f, 0x15, 0x52, 0x01, 0x2e, 0x81, 0x00, 0x82, 0x40, 0x50, 0x42, 0x08, 0x2c, 0x42, 0x42, 0x11, 0x30, 0x23, + 0x2e, 0x85, 0x00, 0x01, 0x30, 0xd0, 0x6f, 0x98, 0x2e, 0x95, 0xcf, 0x00, 0x2e, 0xbb, 0x6f, 0x20, 0x5f, 0xb8, 0x2e, + 0x11, 0x30, 0x81, 0x08, 0x01, 0x2e, 0x6a, 0xf7, 0x71, 0x3f, 0x23, 0xbd, 0x01, 0x08, 0x02, 0x0a, 0xc0, 0x2e, 0x21, + 0x2e, 0x6a, 0xf7, 0x30, 0x25, 0x00, 0x30, 0x21, 0x2e, 0x5a, 0xf5, 0x10, 0x50, 0x21, 0x2e, 0x7c, 0x00, 0x21, 0x2e, + 0x78, 0x00, 0xfb, 0x7f, 0x98, 0x2e, 0xa4, 0xb1, 0x40, 0x30, 0x21, 0x2e, 0x86, 0x00, 0xfb, 0x6f, 0xf0, 0x5f, 0x03, + 0x25, 0x80, 0x2e, 0xea, 0x03, 0x51, 0x56, 0x05, 0x40, 0x69, 0x18, 0x0d, 0x17, 0xe1, 0x18, 0x19, 0x05, 0x16, 0x25, + 0x37, 0x25, 0x4a, 0x17, 0x54, 0x18, 0xec, 0x18, 0x04, 0x30, 0x64, 0x07, 0xea, 0x18, 0x8e, 0x00, 0x5f, 0x02, 0x35, + 0x56, 0x93, 0x00, 0x4c, 0x02, 0x2f, 0xb9, 0x91, 0xbc, 0x91, 0x0a, 0x02, 0x42, 0xb8, 0x2e, 0x0b, 0x00, 0x40, 0xb3, + 0x94, 0x02, 0x0b, 0x2f, 0x50, 0xa1, 0x03, 0x2f, 0x70, 0x8b, 0x85, 0x14, 0x07, 0x2c, 0x00, 0x30, 0x04, 0x31, 0x25, + 0x05, 0x04, 0x13, 0x95, 0x14, 0x05, 0x14, 0x94, 0x0a, 0x47, 0x5a, 0x45, 0x01, 0x04, 0x30, 0x94, 0x02, 0xd8, 0xba, + 0xc0, 0x2e, 0x28, 0xbd, 0x2a, 0x0a, 0x4a, 0x18, 0x00, 0x30, 0xc1, 0x18, 0x6f, 0xb8, 0xc0, 0x2e, 0xf1, 0xbc, 0x01, + 0x0a, 0x4e, 0x86, 0x80, 0x40, 0x70, 0x50, 0xd1, 0x40, 0x86, 0x84, 0xf0, 0x7f, 0x84, 0x30, 0xce, 0x8a, 0xe2, 0x7f, + 0x20, 0x04, 0x54, 0x41, 0xc2, 0x40, 0x4c, 0x04, 0x43, 0x41, 0x93, 0x06, 0x49, 0x18, 0x03, 0x31, 0xd8, 0x04, 0x30, + 0x88, 0xd1, 0x18, 0xd4, 0x7f, 0x00, 0xb2, 0x72, 0x8b, 0xd1, 0x18, 0xc3, 0x7f, 0xbb, 0x7f, 0x0a, 0x2f, 0x10, 0xa0, + 0x03, 0x2f, 0xd1, 0x6f, 0xb9, 0x13, 0x06, 0x2c, 0x07, 0x30, 0xc1, 0x6f, 0x79, 0x14, 0xb0, 0x12, 0xf8, 0x13, 0x91, + 0x0b, 0x51, 0x41, 0x4e, 0x85, 0x00, 0xb2, 0x94, 0x40, 0xb2, 0x86, 0x82, 0x40, 0x26, 0x01, 0xa3, 0x7f, 0x97, 0x02, + 0x43, 0x41, 0x4c, 0x00, 0x9a, 0x02, 0x0a, 0x2f, 0x10, 0xa0, 0x03, 0x2f, 0xd1, 0x6f, 0x51, 0x12, 0x06, 0x2c, 0x02, + 0x30, 0xc3, 0x6f, 0xd3, 0x14, 0x48, 0x12, 0x90, 0x12, 0x4b, 0x0a, 0x98, 0x2e, 0x79, 0xc0, 0xa1, 0x6f, 0x4f, 0x8a, + 0x72, 0x8d, 0x43, 0x41, 0x42, 0x40, 0xf5, 0x6f, 0xab, 0xbc, 0x35, 0xba, 0x25, 0xb9, 0xbb, 0xbd, 0xf0, 0x7f, 0x75, + 0x25, 0x98, 0x2e, 0xdb, 0xb1, 0xd0, 0x7f, 0x57, 0x25, 0x91, 0x41, 0x8e, 0x81, 0x82, 0x41, 0x13, 0x40, 0x04, 0x40, + 0x32, 0x8c, 0x98, 0x2e, 0xdb, 0xb1, 0xc0, 0x7f, 0x57, 0x25, 0x91, 0x41, 0x8e, 0x81, 0x82, 0x41, 0x13, 0x40, 0x04, + 0x40, 0x32, 0x8c, 0x98, 0x2e, 0xdb, 0xb1, 0xa0, 0x7f, 0x57, 0x25, 0x91, 0x41, 0x8e, 0x81, 0x82, 0x41, 0x13, 0x40, + 0x04, 0x40, 0x32, 0x8c, 0x98, 0x2e, 0xdb, 0xb1, 0x90, 0x7f, 0x57, 0x25, 0x91, 0x41, 0x8e, 0x81, 0x82, 0x41, 0x13, + 0x40, 0x04, 0x40, 0x32, 0x8c, 0x98, 0x2e, 0xdb, 0xb1, 0x57, 0x25, 0x91, 0x41, 0x8e, 0x89, 0xe7, 0x6f, 0x13, 0x41, + 0x82, 0x41, 0x04, 0x41, 0xe0, 0x7f, 0x98, 0x2e, 0xdb, 0xb1, 0x57, 0x25, 0xf1, 0x6f, 0x52, 0x41, 0xf0, 0x7f, 0x98, + 0x2e, 0xf3, 0xb1, 0xd1, 0x6f, 0x52, 0x41, 0x30, 0x25, 0x98, 0x2e, 0xf3, 0xb1, 0xc1, 0x6f, 0x52, 0x41, 0xd0, 0x7f, + 0x98, 0x2e, 0xf3, 0xb1, 0xa1, 0x6f, 0x52, 0x41, 0xc0, 0x7f, 0x98, 0x2e, 0xf3, 0xb1, 0x91, 0x6f, 0x52, 0x41, 0xa0, + 0x7f, 0x98, 0x2e, 0xf3, 0xb1, 0xe1, 0x6f, 0x52, 0x41, 0x40, 0x25, 0x98, 0x2e, 0xf3, 0xb1, 0x70, 0x25, 0x42, 0x41, + 0xf1, 0x6f, 0x57, 0x25, 0x98, 0x2e, 0xf3, 0xb1, 0x7b, 0x52, 0xd9, 0x0f, 0x90, 0x2e, 0x70, 0xb3, 0x81, 0x32, 0x69, + 0x0e, 0x06, 0x2f, 0x01, 0x32, 0x41, 0x0e, 0x0d, 0x53, 0x0f, 0x55, 0x0a, 0x22, 0x80, 0x2e, 0x71, 0xb3, 0x51, 0x34, + 0x59, 0x0e, 0xd1, 0x6f, 0xc2, 0x6f, 0x90, 0x2e, 0x4f, 0xb3, 0x99, 0x5e, 0x57, 0x0e, 0x7c, 0x2f, 0xaf, 0x5e, 0x4f, + 0x0e, 0x4d, 0x2f, 0x02, 0xa2, 0x2f, 0x2f, 0xe9, 0x50, 0x60, 0x0e, 0x06, 0x2f, 0x07, 0x55, 0x4a, 0x0e, 0x09, 0x53, + 0x0b, 0x55, 0x0a, 0x22, 0x80, 0x2e, 0x71, 0xb3, 0xeb, 0x50, 0x60, 0x0e, 0x10, 0x2f, 0xfb, 0x52, 0xd1, 0x0f, 0x0a, + 0x2f, 0x45, 0xa3, 0x01, 0x53, 0x03, 0x55, 0x4a, 0x22, 0xa2, 0x6f, 0xfd, 0x50, 0x50, 0x0e, 0xff, 0x54, 0x11, 0x22, + 0x80, 0x2e, 0x71, 0xb3, 0x05, 0x51, 0x80, 0x2e, 0x71, 0xb3, 0xed, 0x54, 0x4a, 0x0e, 0x07, 0x2f, 0xa1, 0x6f, 0xf5, + 0x54, 0x4a, 0x0e, 0xf7, 0x52, 0xf9, 0x54, 0x0a, 0x22, 0x80, 0x2e, 0x71, 0xb3, 0xef, 0x52, 0x59, 0x0e, 0xf1, 0x52, + 0xf3, 0x54, 0x0a, 0x22, 0x80, 0x2e, 0x71, 0xb3, 0xcb, 0x52, 0x61, 0x0e, 0x0e, 0x2f, 0xa1, 0x6f, 0xdd, 0x54, 0xca, + 0x0f, 0x08, 0x2f, 0x01, 0xa2, 0xe1, 0x52, 0xe3, 0x54, 0x4a, 0x22, 0xdf, 0x54, 0x62, 0x0e, 0xe5, 0x54, 0x77, 0x2c, + 0x0a, 0x22, 0x75, 0x2c, 0xe7, 0x50, 0xd7, 0x52, 0x51, 0x0e, 0xd9, 0x52, 0xdb, 0x54, 0x4a, 0x22, 0x72, 0x35, 0x5a, + 0x0e, 0xd5, 0x54, 0x6b, 0x2c, 0x11, 0x22, 0xb1, 0x56, 0x53, 0x0e, 0x0a, 0x2f, 0xa1, 0x6f, 0xcf, 0x54, 0x4a, 0x0e, + 0xd1, 0x52, 0xd3, 0x54, 0x4a, 0x22, 0xcb, 0x54, 0x62, 0x0e, 0xcd, 0x54, 0x5d, 0x2c, 0x11, 0x22, 0x0c, 0xa2, 0x05, + 0x2f, 0xc5, 0x52, 0x61, 0x0e, 0xc7, 0x52, 0xc9, 0x54, 0x55, 0x2c, 0x0a, 0x22, 0xb3, 0x54, 0x62, 0x0e, 0x0a, 0x2f, + 0xa2, 0x6f, 0xbf, 0x50, 0x50, 0x0e, 0xc1, 0x54, 0xc3, 0x50, 0x90, 0x22, 0xbb, 0x50, 0x48, 0x0e, 0xbd, 0x52, 0x47, + 0x2c, 0x0a, 0x22, 0x43, 0xa3, 0xb5, 0x52, 0xb7, 0x54, 0x4a, 0x22, 0x54, 0xa3, 0xb9, 0x54, 0x3f, 0x2c, 0x0a, 0x22, + 0x41, 0xa3, 0x14, 0x2f, 0xa0, 0x37, 0x50, 0x0e, 0x0f, 0x2f, 0xa1, 0x54, 0x4a, 0x0e, 0x0a, 0x2f, 0xa7, 0x52, 0x61, + 0x0e, 0xa9, 0x52, 0xab, 0x54, 0x4a, 0x22, 0xa2, 0x6f, 0xa5, 0x50, 0x50, 0x0e, 0xad, 0x54, 0x2c, 0x2c, 0x0a, 0x22, + 0x2a, 0x2c, 0xa3, 0x50, 0x28, 0x2c, 0x9f, 0x50, 0x01, 0xa2, 0x9b, 0x52, 0x9d, 0x54, 0x23, 0x2c, 0x0a, 0x22, 0x20, + 0x33, 0x58, 0x0e, 0x09, 0x2f, 0x91, 0x50, 0x48, 0x0e, 0x93, 0x52, 0x95, 0x50, 0x48, 0x22, 0x8f, 0x50, 0x50, 0x0e, + 0x97, 0x54, 0x16, 0x2c, 0x0a, 0x22, 0x7d, 0x54, 0x62, 0x0e, 0x0e, 0x2f, 0x81, 0x54, 0xe2, 0x0f, 0x09, 0x2f, 0x87, + 0x54, 0x4a, 0x0e, 0x89, 0x52, 0x8b, 0x54, 0x4a, 0x22, 0x83, 0x54, 0x62, 0x0e, 0x85, 0x54, 0x06, 0x2c, 0x11, 0x22, + 0x04, 0x2c, 0x8d, 0x50, 0x02, 0x2c, 0x7f, 0x50, 0x11, 0x51, 0xbb, 0x6f, 0x90, 0x5f, 0xb8, 0x2e, 0x32, 0x25, 0xb0, + 0x51, 0xc2, 0x32, 0x82, 0x00, 0xe0, 0x7f, 0xd2, 0x7f, 0x04, 0x30, 0x80, 0x40, 0x01, 0x80, 0x90, 0x42, 0xc2, 0x7f, + 0x10, 0x30, 0x85, 0x40, 0x2c, 0x03, 0x94, 0x42, 0x4a, 0x25, 0x85, 0x40, 0x28, 0x28, 0x80, 0x42, 0x25, 0x3d, 0xc3, + 0x80, 0x2b, 0x89, 0x95, 0x00, 0x81, 0x7f, 0xb2, 0x7f, 0xa4, 0x7f, 0x90, 0x7f, 0x7b, 0x7f, 0x00, 0x2e, 0xd1, 0x40, + 0x03, 0x54, 0x53, 0x7f, 0x64, 0x7f, 0x98, 0x2e, 0xd9, 0xc0, 0x64, 0x6f, 0x53, 0x6f, 0x91, 0x6f, 0x10, 0x43, 0x59, + 0x0e, 0xf3, 0x2f, 0xa1, 0x6f, 0x98, 0x2e, 0xb3, 0xc0, 0xb1, 0x6f, 0x13, 0x33, 0x40, 0x42, 0x00, 0xac, 0xcb, 0x00, + 0x02, 0x2f, 0xe1, 0x6f, 0x53, 0x54, 0x42, 0x42, 0xd1, 0x3d, 0x59, 0x00, 0xc3, 0x40, 0xcf, 0xb0, 0xce, 0x00, 0x72, + 0x80, 0xc2, 0x40, 0x11, 0x40, 0x91, 0x00, 0xd2, 0x42, 0x89, 0x16, 0xc4, 0x40, 0x22, 0x03, 0x02, 0x40, 0x05, 0x33, + 0x05, 0x00, 0xd4, 0x42, 0xb3, 0x7f, 0x90, 0x7f, 0x98, 0x2e, 0xfe, 0xc9, 0x91, 0x6f, 0xd2, 0x3d, 0xb3, 0x6f, 0x00, + 0x18, 0x8a, 0x00, 0xc0, 0x40, 0xb2, 0x88, 0x06, 0x00, 0xd0, 0x42, 0xa0, 0x35, 0xc5, 0x40, 0x6f, 0x03, 0x46, 0x40, + 0x8f, 0xb1, 0x96, 0x00, 0x20, 0x00, 0xc5, 0x42, 0x92, 0x7f, 0xb0, 0x7f, 0x02, 0x32, 0x01, 0x41, 0x33, 0x30, 0x98, + 0x2e, 0x0f, 0xca, 0xb5, 0x6f, 0x11, 0x3b, 0x63, 0x41, 0x64, 0x41, 0x44, 0x85, 0x45, 0x41, 0xa6, 0x40, 0x87, 0x40, + 0x51, 0x00, 0xf7, 0x7f, 0xb1, 0x7f, 0x20, 0x25, 0x98, 0x2e, 0x67, 0xcc, 0xb1, 0x6f, 0xb3, 0x33, 0xcb, 0x00, 0xea, + 0x82, 0xc2, 0x40, 0x1a, 0xa4, 0xe5, 0x6f, 0x04, 0x30, 0x13, 0x30, 0x01, 0x2f, 0x80, 0xa0, 0x03, 0x2f, 0x2e, 0xac, + 0x0a, 0x2f, 0x80, 0xa4, 0x08, 0x2f, 0x90, 0x6f, 0x04, 0x80, 0x77, 0x34, 0x06, 0x40, 0x6f, 0x01, 0xa2, 0x04, 0xf3, + 0x28, 0x42, 0x43, 0x03, 0x42, 0xd3, 0x3d, 0x42, 0x40, 0xcb, 0x00, 0xf2, 0x82, 0x8f, 0xb0, 0xde, 0x00, 0x43, 0x80, + 0x42, 0x40, 0xb3, 0x7f, 0x90, 0x7f, 0xb3, 0x30, 0x13, 0x53, 0x98, 0x2e, 0x5a, 0xca, 0x3a, 0x25, 0xe8, 0x82, 0xee, + 0x86, 0xe2, 0x6f, 0x82, 0x84, 0x43, 0x7f, 0x20, 0x7f, 0x51, 0x7f, 0x61, 0x7f, 0x32, 0x7f, 0x05, 0x30, 0xa4, 0x6f, + 0x83, 0x30, 0x00, 0x30, 0x11, 0x41, 0x22, 0x6f, 0x14, 0x7f, 0x00, 0x7f, 0xf5, 0x7e, 0x98, 0x2e, 0x0f, 0xca, 0x33, + 0x6f, 0x51, 0x6f, 0xc3, 0x40, 0x50, 0x42, 0x51, 0x7f, 0xe0, 0x7e, 0xc0, 0x90, 0x02, 0x2f, 0x91, 0x6f, 0x00, 0x2e, + 0x40, 0x42, 0x00, 0x2e, 0xe2, 0x6e, 0x90, 0x6f, 0x15, 0x53, 0x98, 0x2e, 0xc3, 0xb1, 0x93, 0x6f, 0xe1, 0x6e, 0xd2, + 0x40, 0x0a, 0x18, 0x01, 0x6f, 0x0e, 0x00, 0x93, 0x7f, 0x83, 0x30, 0x14, 0x6f, 0xf1, 0x6e, 0x42, 0x6f, 0x62, 0x0e, + 0x4f, 0x03, 0xd9, 0x2f, 0x35, 0x52, 0xc1, 0x00, 0x01, 0x30, 0xa9, 0x02, 0x91, 0x6f, 0x7c, 0x82, 0x21, 0xbd, 0xbf, + 0xb9, 0x1b, 0x30, 0x0a, 0x25, 0xda, 0x0a, 0x5b, 0x42, 0x25, 0x80, 0x33, 0x7f, 0x51, 0x7f, 0x20, 0x7f, 0x90, 0x7f, + 0xd3, 0x30, 0x64, 0x6f, 0x55, 0x6f, 0x10, 0x41, 0x52, 0x41, 0x31, 0x6f, 0x55, 0x7f, 0x10, 0x7f, 0x04, 0x7f, 0x98, + 0x2e, 0x0f, 0xca, 0x11, 0x6f, 0x20, 0x25, 0x98, 0x2e, 0xfe, 0xc9, 0x21, 0x6f, 0x04, 0x6f, 0x50, 0x42, 0x21, 0x7f, + 0xd3, 0x30, 0xa1, 0x6f, 0x61, 0x0e, 0xea, 0x2f, 0xb1, 0x6f, 0x45, 0x84, 0x32, 0x25, 0x90, 0x40, 0x84, 0x40, 0x91, + 0x6f, 0xb4, 0x7f, 0x92, 0x7f, 0x30, 0x7f, 0x23, 0x7f, 0x98, 0x2e, 0xb3, 0xc0, 0x53, 0x6f, 0xb1, 0x32, 0x19, 0x01, + 0x83, 0xb9, 0x31, 0x6f, 0x4b, 0x00, 0x02, 0x41, 0xb0, 0x6f, 0x03, 0x30, 0xc3, 0x02, 0x8f, 0xb0, 0xb4, 0x7f, 0xd5, + 0x3d, 0x25, 0x01, 0xa2, 0x6f, 0xa4, 0x7f, 0x26, 0x01, 0x27, 0x6f, 0x90, 0x6f, 0x07, 0x89, 0xc1, 0x43, 0x94, 0x7f, + 0x03, 0x42, 0x00, 0x2e, 0x11, 0x41, 0x31, 0x7f, 0x54, 0x7f, 0x00, 0x2e, 0x91, 0x40, 0x03, 0x41, 0x23, 0x7f, 0x12, + 0x7f, 0x98, 0x2e, 0x74, 0xc0, 0x31, 0x6f, 0xc8, 0x00, 0x90, 0x6f, 0x01, 0x30, 0x54, 0x6f, 0x22, 0x6f, 0x03, 0x42, + 0xd1, 0x02, 0x41, 0x6f, 0x12, 0x6f, 0x23, 0x43, 0x94, 0x7f, 0x51, 0x0e, 0xe7, 0x2f, 0xb1, 0x6f, 0xa3, 0x6f, 0x42, + 0x40, 0x8f, 0xb0, 0xf8, 0x82, 0xde, 0x00, 0xc9, 0x86, 0x52, 0x34, 0xb3, 0x7f, 0x8a, 0x00, 0xc6, 0x86, 0xa2, 0x7f, + 0x93, 0x7f, 0x00, 0x2e, 0xa5, 0x6f, 0xe2, 0x6f, 0x63, 0x41, 0x64, 0x41, 0x44, 0x8f, 0x82, 0x40, 0xe6, 0x41, 0xc0, + 0x41, 0xc4, 0x8f, 0x45, 0x41, 0xf0, 0x7f, 0xa7, 0x7f, 0x51, 0x7f, 0x98, 0x2e, 0x67, 0xcc, 0x00, 0x18, 0x09, 0x52, + 0x71, 0x00, 0x03, 0x30, 0xbb, 0x02, 0x1b, 0xba, 0xb3, 0x6f, 0x25, 0xbc, 0x51, 0x6f, 0xc5, 0x40, 0x42, 0x82, 0x20, + 0x0a, 0x28, 0x00, 0xd0, 0x42, 0x2b, 0xb9, 0xc0, 0x40, 0x82, 0x02, 0xd2, 0x42, 0xb3, 0x7f, 0x00, 0x2e, 0x92, 0x6f, + 0x5a, 0x0e, 0xd9, 0x2f, 0xa1, 0x6f, 0x43, 0x3d, 0x8b, 0x00, 0x9a, 0x82, 0x83, 0x6f, 0x41, 0x40, 0xc3, 0x40, 0xe0, + 0x6f, 0x14, 0x33, 0x04, 0x00, 0x82, 0x40, 0x4b, 0x12, 0x51, 0x0e, 0xb0, 0x7f, 0x90, 0x2e, 0x7a, 0xb5, 0x02, 0x40, + 0x8f, 0xb0, 0xd1, 0x3d, 0x41, 0x00, 0x72, 0x30, 0xd3, 0x04, 0x73, 0x80, 0x4e, 0x00, 0x02, 0x31, 0xd3, 0x05, 0xa0, + 0x7f, 0xf0, 0x8c, 0x04, 0x40, 0x52, 0x40, 0xc0, 0xb2, 0x50, 0x40, 0x4c, 0x17, 0x96, 0x7f, 0x57, 0x7f, 0x0a, 0x2f, + 0xd0, 0xa0, 0x03, 0x2f, 0x95, 0x6f, 0x65, 0x15, 0x06, 0x2c, 0x04, 0x30, 0x56, 0x6f, 0xa6, 0x13, 0x6b, 0x15, 0x23, + 0x15, 0x6e, 0x0b, 0x14, 0x05, 0x64, 0x18, 0x45, 0x07, 0xec, 0x18, 0xc0, 0xb2, 0xec, 0x18, 0x0a, 0x2f, 0xd0, 0xa0, + 0x03, 0x2f, 0x94, 0x6f, 0xbc, 0x13, 0x06, 0x2c, 0x07, 0x30, 0x54, 0x6f, 0x3c, 0x15, 0x73, 0x13, 0xfb, 0x13, 0xac, + 0x0b, 0x44, 0x40, 0x26, 0x05, 0x54, 0x42, 0xc0, 0xb2, 0x44, 0x40, 0x27, 0x07, 0x44, 0x42, 0x08, 0x2f, 0xd0, 0xa0, + 0x02, 0x2f, 0x91, 0x6f, 0x05, 0x2c, 0x81, 0x10, 0x51, 0x6f, 0x41, 0x14, 0xd3, 0x12, 0x99, 0x0a, 0xa1, 0x6f, 0xf3, + 0x32, 0x42, 0x42, 0x4b, 0x00, 0x13, 0x30, 0x42, 0x40, 0xd3, 0x28, 0x53, 0x42, 0xa1, 0x7f, 0xc2, 0xa2, 0x30, 0x2f, + 0x82, 0x6f, 0xe1, 0x6f, 0x98, 0x2e, 0xfa, 0xb1, 0x81, 0x6f, 0x41, 0x84, 0x00, 0x2e, 0x81, 0x40, 0x40, 0x90, 0x02, + 0x2f, 0x00, 0x2e, 0x07, 0x2c, 0x0c, 0xb8, 0x30, 0x25, 0xe1, 0x6f, 0x20, 0x33, 0x48, 0x00, 0x98, 0x2e, 0x07, 0xb6, + 0xe1, 0x6f, 0xf3, 0x32, 0x4b, 0x00, 0xe0, 0x7f, 0x00, 0x2e, 0x44, 0x40, 0x20, 0x1a, 0x15, 0x2f, 0xd3, 0x6f, 0xc1, + 0x6f, 0xc3, 0x40, 0x23, 0x5a, 0x42, 0x40, 0x83, 0x7e, 0x08, 0xbc, 0x25, 0x09, 0x92, 0x7e, 0xc4, 0x0a, 0x42, 0x82, + 0xa3, 0x7e, 0xd1, 0x7f, 0x34, 0x30, 0x63, 0x6f, 0x82, 0x30, 0x31, 0x30, 0x98, 0x2e, 0xb2, 0x00, 0xd1, 0x6f, 0xe3, + 0x6f, 0x43, 0x42, 0x00, 0x2e, 0xa1, 0x6f, 0xb2, 0x6f, 0x43, 0x40, 0xc1, 0x86, 0xc2, 0xa2, 0x43, 0x42, 0x03, 0x30, + 0x00, 0x2f, 0x83, 0x42, 0xd2, 0x3d, 0x40, 0x40, 0x0f, 0xb0, 0x0a, 0x01, 0x26, 0x00, 0x05, 0x32, 0x98, 0x2e, 0x7e, + 0xb5, 0x65, 0x00, 0x00, 0x2e, 0x43, 0x42, 0x00, 0x2e, 0x7b, 0x6f, 0x50, 0x5e, 0xb8, 0x2e, 0x0f, 0x82, 0x02, 0x30, + 0x12, 0x42, 0x41, 0x0e, 0xfc, 0x2f, 0xb8, 0x2e, 0xc1, 0x35, 0x40, 0x51, 0x01, 0x01, 0x02, 0x30, 0x1a, 0x25, 0x13, + 0x30, 0x12, 0x42, 0x44, 0x0e, 0xfc, 0x2f, 0x54, 0x3a, 0x04, 0x01, 0x1d, 0x5b, 0x05, 0x7f, 0x75, 0x34, 0x13, 0x5f, + 0x07, 0x43, 0x25, 0x01, 0x47, 0x5a, 0x05, 0x43, 0x27, 0x89, 0x67, 0x5a, 0x05, 0x43, 0x18, 0x8b, 0x19, 0x59, 0x17, + 0x51, 0xd4, 0x7e, 0x43, 0x43, 0xc0, 0x7e, 0xe0, 0x7e, 0x6c, 0x88, 0x1b, 0x5d, 0xf6, 0x7e, 0x71, 0x86, 0x42, 0x81, + 0x15, 0x41, 0x15, 0x42, 0x63, 0x0e, 0xfb, 0x2f, 0x21, 0x59, 0x1f, 0x5d, 0x25, 0x5f, 0x23, 0x5b, 0x34, 0x7f, 0x16, + 0x7f, 0x45, 0x7f, 0x57, 0x7f, 0x22, 0x7f, 0x76, 0x88, 0xd5, 0x40, 0x15, 0x42, 0x5c, 0x0e, 0xfb, 0x2f, 0x29, 0x57, + 0x27, 0x5d, 0x2d, 0x5f, 0x2b, 0x5b, 0x83, 0x7f, 0x66, 0x7f, 0x95, 0x7f, 0xa7, 0x7f, 0x72, 0x7f, 0x7b, 0x86, 0x15, + 0x41, 0x15, 0x42, 0x63, 0x0e, 0xfb, 0x2f, 0x31, 0x59, 0x33, 0x5d, 0x2f, 0x5b, 0xc5, 0x7f, 0xd4, 0x7f, 0xf6, 0x7f, + 0xb2, 0x7f, 0xe2, 0x7f, 0x40, 0x82, 0xd2, 0x40, 0x12, 0x42, 0x59, 0x0e, 0xfb, 0x2f, 0xc0, 0x5e, 0xb8, 0x2e, 0x00, + 0x30, 0xc0, 0x2e, 0x21, 0x2e, 0x8c, 0x00, 0x35, 0x51, 0xc0, 0x2e, 0x21, 0x2e, 0xad, 0x00, 0x05, 0x2e, 0x28, 0x02, + 0x2f, 0xbd, 0x2f, 0xb9, 0x20, 0x50, 0x80, 0xb2, 0x16, 0x2f, 0x20, 0x25, 0x01, 0x2e, 0x8c, 0x00, 0x00, 0x90, 0x0b, + 0x2f, 0x37, 0x51, 0xf2, 0x7f, 0xeb, 0x7f, 0x98, 0x2e, 0x84, 0xb5, 0x01, 0x2e, 0x8c, 0x00, 0x01, 0x80, 0x21, 0x2e, + 0x8c, 0x00, 0xf2, 0x6f, 0xeb, 0x6f, 0xe0, 0x5f, 0x03, 0x2e, 0xad, 0x00, 0x37, 0x51, 0x80, 0x2e, 0x74, 0xb3, 0x00, + 0x30, 0x21, 0x2e, 0x8c, 0x00, 0xe0, 0x5f, 0xb8, 0x2e, 0x02, 0x30, 0x02, 0x2c, 0x41, 0x00, 0x12, 0x42, 0x41, 0x0e, + 0xfc, 0x2f, 0xb8, 0x2e, 0x81, 0x8e, 0x39, 0x51, 0x07, 0x5c, 0x58, 0x09, 0x9e, 0x09, 0xc3, 0x41, 0x50, 0x50, 0xc3, + 0x89, 0xf3, 0x0e, 0x41, 0x80, 0xf0, 0x7f, 0xdc, 0xb9, 0xe4, 0x7f, 0x44, 0x80, 0x84, 0x8e, 0x43, 0x88, 0xd1, 0x7f, + 0x05, 0x30, 0xc2, 0x7f, 0x21, 0x2f, 0xf1, 0x6f, 0x00, 0x2e, 0x41, 0x40, 0x19, 0x1a, 0x1c, 0x2f, 0x82, 0x84, 0x00, + 0x2e, 0x82, 0x40, 0xf2, 0x0e, 0x05, 0x2f, 0x02, 0x41, 0x81, 0x84, 0x01, 0x30, 0x03, 0x30, 0x22, 0x2c, 0x02, 0x43, + 0xff, 0x84, 0x42, 0x00, 0x60, 0x25, 0x43, 0x40, 0xc1, 0x86, 0x43, 0x42, 0x11, 0x30, 0xc2, 0x41, 0x03, 0x30, 0x97, + 0x41, 0xc1, 0x86, 0xfa, 0x0f, 0x13, 0x2f, 0xc5, 0xa2, 0xf9, 0x2f, 0x03, 0x30, 0x10, 0x2c, 0x01, 0x30, 0xd2, 0x6f, + 0x00, 0x2e, 0x82, 0x40, 0x80, 0x90, 0x04, 0x2f, 0xd2, 0x6f, 0xf1, 0x6f, 0x17, 0x30, 0x87, 0x42, 0x43, 0x42, 0x00, + 0x2e, 0x3b, 0x55, 0xf2, 0x0e, 0x12, 0x30, 0x55, 0x22, 0x40, 0xb2, 0xbb, 0x7f, 0x06, 0x2f, 0x51, 0x30, 0x60, 0x25, + 0x98, 0x2e, 0x00, 0xb6, 0xbf, 0x81, 0x00, 0x2e, 0x05, 0x42, 0x00, 0x2e, 0xe2, 0x6f, 0x01, 0x41, 0x82, 0x40, 0xd0, + 0x6f, 0x13, 0x88, 0xca, 0x0f, 0x49, 0x2f, 0xf2, 0x6f, 0xc0, 0xa6, 0x87, 0x40, 0x43, 0x2f, 0xc5, 0x6f, 0x02, 0x82, + 0x43, 0x8d, 0x42, 0x40, 0x81, 0x8a, 0x86, 0x41, 0x6e, 0x0e, 0x45, 0x42, 0x47, 0x8a, 0xaa, 0x00, 0x1b, 0x30, 0x83, + 0x42, 0x09, 0x84, 0xf2, 0x7f, 0x02, 0x30, 0x03, 0x2f, 0x0b, 0x43, 0x2f, 0x89, 0x00, 0x2e, 0x02, 0x43, 0x51, 0x88, + 0x41, 0x40, 0x15, 0x41, 0x40, 0xb3, 0x4e, 0x22, 0x5f, 0x1a, 0x14, 0x80, 0x03, 0x41, 0x06, 0x2f, 0xc1, 0x84, 0xd6, + 0x0e, 0x02, 0x42, 0x22, 0x2f, 0x00, 0x2e, 0x21, 0x2c, 0x06, 0x42, 0xff, 0x80, 0x41, 0x0f, 0x91, 0xb9, 0x10, 0x22, + 0xf4, 0x6f, 0x61, 0x00, 0xc3, 0x0f, 0x12, 0x2f, 0x13, 0x30, 0x04, 0x30, 0xf6, 0x6f, 0x05, 0x30, 0x05, 0x2c, 0xe7, + 0x7f, 0x97, 0x41, 0x3c, 0x1a, 0xda, 0x23, 0x6f, 0x01, 0x71, 0x0e, 0xf9, 0x2f, 0x68, 0x0f, 0xe6, 0x6f, 0xe6, 0x23, + 0x01, 0x89, 0x28, 0x22, 0x05, 0xa7, 0xee, 0x2f, 0xf2, 0x6f, 0xb8, 0x84, 0x93, 0x82, 0x87, 0x42, 0x40, 0x42, 0x08, + 0x2c, 0x07, 0x25, 0x13, 0x30, 0x50, 0x25, 0x98, 0x2e, 0xb3, 0xb6, 0x00, 0x30, 0x43, 0x43, 0x03, 0x43, 0x00, 0x2e, + 0xbb, 0x6f, 0xb0, 0x5f, 0xb8, 0x2e, 0x15, 0x82, 0x02, 0x30, 0x12, 0x42, 0x41, 0x0e, 0xfc, 0x2f, 0xb8, 0x2e, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0xfd, 0x2d, 0x01, 0x2e, 0x5d, 0xf7, 0x08, 0xbc, 0x80, 0xac, 0x0e, 0xbb, 0x02, 0x2f, + 0x00, 0x30, 0x41, 0x04, 0x82, 0x06, 0xc0, 0xa4, 0x00, 0x30, 0x11, 0x2f, 0x40, 0xa9, 0x03, 0x2f, 0x40, 0x91, 0x0d, + 0x2f, 0x00, 0xa7, 0x0b, 0x2f, 0x80, 0xb3, 0x35, 0x58, 0x02, 0x2f, 0x90, 0xa1, 0x26, 0x13, 0x20, 0x23, 0x80, 0x90, + 0x10, 0x30, 0x01, 0x2f, 0xcc, 0x0e, 0x00, 0x2f, 0x00, 0x30, 0xb8, 0x2e, 0x3d, 0x56, 0x37, 0x54, 0xd0, 0x40, 0xc4, + 0x40, 0x0b, 0x2e, 0xfd, 0xf3, 0x3d, 0x52, 0x90, 0x42, 0x94, 0x42, 0x95, 0x42, 0x05, 0x30, 0x3f, 0x50, 0x0f, 0x88, + 0x06, 0x40, 0x04, 0x41, 0x96, 0x42, 0xc5, 0x42, 0x48, 0xbe, 0x73, 0x30, 0x0d, 0x2e, 0x8a, 0x00, 0x4f, 0xba, 0x84, + 0x42, 0x03, 0x42, 0x81, 0xb3, 0x02, 0x2f, 0x2b, 0x2e, 0x6f, 0xf5, 0x06, 0x2d, 0x05, 0x2e, 0x77, 0xf7, 0x3b, 0x56, + 0x93, 0x08, 0x25, 0x2e, 0x77, 0xf7, 0x39, 0x54, 0x25, 0x2e, 0xc2, 0xf5, 0x07, 0x2e, 0xfd, 0xf3, 0x42, 0x30, 0xb4, + 0x33, 0xda, 0x0a, 0x4c, 0x00, 0x27, 0x2e, 0xfd, 0xf3, 0x43, 0x40, 0xd4, 0x3f, 0xdc, 0x08, 0x43, 0x42, 0x00, 0x2e, + 0x00, 0x2e, 0x43, 0x40, 0x24, 0x30, 0xdc, 0x0a, 0x43, 0x42, 0x04, 0x80, 0x03, 0x2e, 0xfd, 0xf3, 0x4a, 0x0a, 0x23, + 0x2e, 0xfd, 0xf3, 0x61, 0x34, 0xc0, 0x2e, 0x01, 0x42, 0x00, 0x2e, 0xd0, 0x51, 0xfb, 0x7f, 0x98, 0x2e, 0xcf, 0x0d, + 0x5a, 0x25, 0x98, 0x2e, 0xf6, 0x0d, 0x6b, 0x87, 0x49, 0x54, 0xe1, 0x7f, 0xa3, 0x7f, 0xb3, 0x7f, 0xb2, 0x88, 0x41, + 0x52, 0xc2, 0x7f, 0x65, 0x8b, 0x43, 0x56, 0x84, 0x7f, 0x61, 0x7f, 0x75, 0x7f, 0xd0, 0x7f, 0x95, 0x7f, 0x53, 0x7f, + 0x14, 0x30, 0x45, 0x54, 0x81, 0x6f, 0x42, 0x7f, 0x00, 0x2e, 0x53, 0x40, 0x45, 0x8c, 0x42, 0x40, 0x90, 0x41, 0xbb, + 0x83, 0x86, 0x41, 0xd8, 0x04, 0x16, 0x06, 0x00, 0xac, 0x81, 0x7f, 0x02, 0x2f, 0x02, 0x30, 0xd3, 0x04, 0x10, 0x06, + 0xc1, 0x84, 0x01, 0x30, 0xc1, 0x02, 0x0b, 0x16, 0x04, 0x09, 0x14, 0x01, 0x99, 0x02, 0xc1, 0xb9, 0xaf, 0xbc, 0x59, + 0x0a, 0x64, 0x6f, 0x51, 0x43, 0xa1, 0xb4, 0x12, 0x41, 0x13, 0x41, 0x41, 0x43, 0x35, 0x7f, 0x64, 0x7f, 0x26, 0x31, + 0xe5, 0x6f, 0xd4, 0x6f, 0x98, 0x2e, 0x37, 0xca, 0x32, 0x6f, 0x75, 0x6f, 0x83, 0x40, 0x42, 0x41, 0x23, 0x7f, 0x12, + 0x7f, 0xf6, 0x30, 0x40, 0x25, 0x51, 0x25, 0x98, 0x2e, 0x37, 0xca, 0x14, 0x6f, 0x20, 0x05, 0x70, 0x6f, 0x25, 0x6f, + 0x69, 0x07, 0xa2, 0x6f, 0x31, 0x6f, 0x0b, 0x30, 0x04, 0x42, 0x9b, 0x42, 0x8b, 0x42, 0x55, 0x42, 0x32, 0x7f, 0x40, + 0xa9, 0xc3, 0x6f, 0x71, 0x7f, 0x02, 0x30, 0xd0, 0x40, 0xc3, 0x7f, 0x03, 0x2f, 0x40, 0x91, 0x15, 0x2f, 0x00, 0xa7, + 0x13, 0x2f, 0x00, 0xa4, 0x11, 0x2f, 0x84, 0xbd, 0x98, 0x2e, 0x79, 0xca, 0x55, 0x6f, 0x51, 0x54, 0x54, 0x41, 0x82, + 0x00, 0xf3, 0x3f, 0x45, 0x41, 0xcb, 0x02, 0xf6, 0x30, 0x98, 0x2e, 0x37, 0xca, 0x33, 0x6f, 0xa4, 0x6f, 0xc1, 0x42, + 0x03, 0x2c, 0x00, 0x43, 0xa4, 0x6f, 0x33, 0x6f, 0x00, 0x2e, 0x42, 0x6f, 0x55, 0x6f, 0x91, 0x40, 0x42, 0x8b, 0x00, + 0x41, 0x41, 0x00, 0x01, 0x43, 0x55, 0x7f, 0x14, 0x30, 0xc1, 0x40, 0x95, 0x40, 0x4d, 0x02, 0xc5, 0x6f, 0x4f, 0x50, + 0x68, 0x0e, 0x75, 0x6f, 0xd1, 0x42, 0xa3, 0x7f, 0x8a, 0x2f, 0x09, 0x2e, 0x8a, 0x00, 0x01, 0xb3, 0x22, 0x2f, 0x49, + 0x58, 0x90, 0x6f, 0x17, 0x30, 0x13, 0x41, 0xb6, 0x6f, 0xe4, 0x7f, 0x00, 0x2e, 0x91, 0x41, 0x14, 0x40, 0x92, 0x41, + 0x15, 0x40, 0x17, 0x2e, 0x6f, 0xf5, 0xb6, 0x7f, 0xd0, 0x7f, 0xcb, 0x7f, 0x98, 0x2e, 0x00, 0x0c, 0x07, 0x15, 0xc2, + 0x6f, 0x14, 0x0b, 0x29, 0x2e, 0x6f, 0xf5, 0xc3, 0xa3, 0xc1, 0x8f, 0xe4, 0x6f, 0xd0, 0x6f, 0xe6, 0x2f, 0x14, 0x30, + 0x05, 0x2e, 0x6f, 0xf5, 0x14, 0x0b, 0x29, 0x2e, 0x6f, 0xf5, 0x44, 0x2d, 0x4b, 0x54, 0x01, 0x32, 0x51, 0x58, 0x05, + 0x30, 0x23, 0x50, 0x83, 0x40, 0xd8, 0x08, 0x91, 0x01, 0xb8, 0xbd, 0x38, 0xb5, 0xe6, 0x7f, 0x0a, 0x16, 0xb1, 0x6f, + 0x2a, 0xbb, 0xa6, 0xbd, 0x1c, 0x01, 0x06, 0xbc, 0x52, 0x40, 0x06, 0x0a, 0x53, 0x40, 0x45, 0x03, 0xb1, 0x7f, 0xf6, + 0x30, 0x98, 0x2e, 0x37, 0xca, 0x1a, 0xbd, 0x16, 0xb6, 0x86, 0xba, 0x00, 0xa9, 0xaa, 0x0a, 0x53, 0x52, 0x0f, 0x2f, + 0x00, 0x91, 0x53, 0x52, 0x03, 0x2f, 0x53, 0x5a, 0x55, 0x0f, 0x53, 0x52, 0x08, 0x2f, 0x3f, 0xa1, 0x04, 0x2f, 0x3f, + 0x91, 0x03, 0x2f, 0x51, 0x58, 0xd4, 0x0f, 0x00, 0x2f, 0x51, 0x54, 0x12, 0x25, 0xf2, 0x33, 0x98, 0x2e, 0xd9, 0xc0, + 0xe4, 0x6f, 0xf5, 0x37, 0x45, 0x09, 0x21, 0x85, 0x05, 0x43, 0x05, 0x30, 0x4d, 0x52, 0x51, 0x0e, 0x01, 0x32, 0x51, + 0x58, 0xc5, 0x2f, 0x47, 0x54, 0x09, 0x2e, 0x77, 0xf7, 0x22, 0x0b, 0x29, 0x2e, 0x77, 0xf7, 0xfb, 0x6f, 0x30, 0x5e, + 0xb8, 0x2e, 0x10, 0x50, 0x01, 0x2e, 0x86, 0x00, 0x00, 0xb2, 0xfb, 0x7f, 0x5d, 0x2f, 0x01, 0xb2, 0x54, 0x2f, 0x02, + 0xb2, 0x4e, 0x2f, 0x03, 0x90, 0x63, 0x2f, 0x59, 0x50, 0x39, 0x82, 0x02, 0x40, 0x81, 0x88, 0x5b, 0x54, 0x41, 0x40, + 0x61, 0x56, 0x04, 0x42, 0x00, 0x2e, 0x94, 0x40, 0x95, 0x40, 0xd8, 0xbe, 0x2c, 0x0b, 0x45, 0x40, 0x6c, 0x01, 0x55, + 0x42, 0x0c, 0x17, 0x45, 0x40, 0x2c, 0x03, 0x54, 0x42, 0x53, 0x0e, 0xf2, 0x2f, 0x63, 0x56, 0x3e, 0x82, 0xe2, 0x40, + 0xc3, 0x40, 0x28, 0xbd, 0x93, 0x0a, 0x43, 0x40, 0xda, 0x00, 0x53, 0x42, 0x8a, 0x16, 0x43, 0x40, 0x9a, 0x02, 0x52, + 0x42, 0x00, 0x2e, 0x41, 0x40, 0x47, 0x54, 0x4a, 0x0e, 0x3b, 0x2f, 0x3a, 0x82, 0x00, 0x30, 0x41, 0x40, 0x21, 0x2e, + 0x52, 0x0f, 0x40, 0xb2, 0x0a, 0x2f, 0x98, 0x2e, 0x61, 0x0c, 0x98, 0x2e, 0x2b, 0x0e, 0x98, 0x2e, 0x41, 0x0e, 0xfb, + 0x6f, 0xf0, 0x5f, 0x00, 0x30, 0x80, 0x2e, 0xaf, 0xb1, 0x5f, 0x54, 0x55, 0x56, 0x83, 0x42, 0x8f, 0x86, 0x74, 0x30, + 0x5d, 0x54, 0xc4, 0x42, 0x11, 0x30, 0x23, 0x2e, 0x86, 0x00, 0xa1, 0x42, 0x23, 0x30, 0x27, 0x2e, 0x89, 0x00, 0x21, + 0x2e, 0x88, 0x00, 0xba, 0x82, 0x18, 0x2c, 0x81, 0x42, 0x30, 0x30, 0x21, 0x2e, 0x86, 0x00, 0x13, 0x2d, 0x21, 0x30, + 0x00, 0x30, 0x23, 0x2e, 0x86, 0x00, 0x21, 0x2e, 0x7b, 0xf7, 0x0c, 0x2d, 0x77, 0x30, 0x98, 0x2e, 0x1f, 0x0c, 0x57, + 0x50, 0x0c, 0x82, 0x12, 0x30, 0x40, 0x42, 0x25, 0x2e, 0x86, 0x00, 0x2f, 0x2e, 0x7b, 0xf7, 0xfb, 0x6f, 0xf0, 0x5f, + 0xb8, 0x2e, 0x70, 0x50, 0x0a, 0x25, 0x39, 0x86, 0xfb, 0x7f, 0xe1, 0x32, 0x62, 0x30, 0x98, 0x2e, 0xc2, 0xc4, 0x23, + 0x56, 0xa5, 0x6f, 0xab, 0x08, 0x91, 0x6f, 0x4b, 0x08, 0x65, 0x56, 0xc4, 0x6f, 0x23, 0x09, 0x4d, 0xba, 0x93, 0xbc, + 0x8c, 0x0b, 0xd1, 0x6f, 0x0b, 0x09, 0x49, 0x52, 0x67, 0x5e, 0x56, 0x42, 0xaf, 0x09, 0x4d, 0xba, 0x23, 0xbd, 0x94, + 0x0a, 0xe5, 0x6f, 0x68, 0xbb, 0xeb, 0x08, 0xbd, 0xb9, 0x63, 0xbe, 0xfb, 0x6f, 0x52, 0x42, 0xe3, 0x0a, 0xc0, 0x2e, + 0x43, 0x42, 0x90, 0x5f, 0x4f, 0x50, 0x03, 0x2e, 0x25, 0xf3, 0x12, 0x40, 0x00, 0x40, 0x28, 0xba, 0x9b, 0xbc, 0x88, + 0xbd, 0x93, 0xb4, 0xe3, 0x0a, 0x89, 0x16, 0x08, 0xb6, 0xc0, 0x2e, 0x19, 0x00, 0x62, 0x02, 0x10, 0x50, 0xfb, 0x7f, + 0x98, 0x2e, 0x5d, 0x0d, 0x01, 0x2e, 0x86, 0x00, 0x31, 0x30, 0x08, 0x04, 0xfb, 0x6f, 0x01, 0x30, 0xf0, 0x5f, 0x23, + 0x2e, 0x88, 0x00, 0x21, 0x2e, 0x89, 0x00, 0xb8, 0x2e, 0x01, 0x2e, 0x89, 0x00, 0x03, 0x2e, 0x88, 0x00, 0x48, 0x0e, + 0x01, 0x2f, 0x80, 0x2e, 0x05, 0x0e, 0xb8, 0x2e, 0x69, 0x50, 0x21, 0x34, 0x01, 0x42, 0x82, 0x30, 0xc1, 0x32, 0x25, + 0x2e, 0x62, 0xf5, 0x01, 0x00, 0x22, 0x30, 0x01, 0x40, 0x4a, 0x0a, 0x01, 0x42, 0xb8, 0x2e, 0x69, 0x54, 0xf0, 0x3b, + 0x83, 0x40, 0xd8, 0x08, 0x6b, 0x52, 0x83, 0x42, 0x00, 0x30, 0x83, 0x30, 0x50, 0x42, 0xc4, 0x32, 0x27, 0x2e, 0x64, + 0xf5, 0x94, 0x00, 0x50, 0x42, 0x40, 0x42, 0xd3, 0x3f, 0x84, 0x40, 0x7d, 0x82, 0xe3, 0x08, 0x40, 0x42, 0x83, 0x42, + 0xb8, 0x2e, 0x5f, 0x52, 0x00, 0x30, 0x40, 0x42, 0x7c, 0x86, 0x37, 0x52, 0x09, 0x2e, 0x3d, 0x0f, 0x3d, 0x54, 0xc4, + 0x42, 0xd3, 0x86, 0x54, 0x40, 0x55, 0x40, 0x94, 0x42, 0x85, 0x42, 0x21, 0x2e, 0x89, 0x00, 0x42, 0x40, 0x25, 0x2e, + 0xfd, 0xf3, 0xc0, 0x42, 0x7e, 0x82, 0x05, 0x2e, 0x79, 0x00, 0x80, 0xb2, 0x14, 0x2f, 0x05, 0x2e, 0x24, 0x02, 0x27, + 0xbd, 0x2f, 0xb9, 0x80, 0x90, 0x02, 0x2f, 0x21, 0x2e, 0x6f, 0xf5, 0x0c, 0x2d, 0x07, 0x2e, 0x3e, 0x0f, 0x14, 0x30, + 0x1c, 0x09, 0x05, 0x2e, 0x77, 0xf7, 0x3b, 0x56, 0x47, 0xbe, 0x93, 0x08, 0x94, 0x0a, 0x25, 0x2e, 0x77, 0xf7, 0x6d, + 0x54, 0x50, 0x42, 0x4a, 0x0e, 0xfc, 0x2f, 0xb8, 0x2e, 0x50, 0x50, 0x02, 0x30, 0x43, 0x86, 0x6b, 0x50, 0xfb, 0x7f, + 0xe3, 0x7f, 0xd2, 0x7f, 0xc0, 0x7f, 0xb1, 0x7f, 0x00, 0x2e, 0x41, 0x40, 0x00, 0x40, 0x48, 0x04, 0x98, 0x2e, 0x74, + 0xc0, 0x1e, 0xaa, 0xd3, 0x6f, 0x14, 0x30, 0xb1, 0x6f, 0xe3, 0x22, 0xc0, 0x6f, 0x52, 0x40, 0xe4, 0x6f, 0x4c, 0x0e, + 0x12, 0x42, 0xd3, 0x7f, 0xeb, 0x2f, 0x03, 0x2e, 0x53, 0x0f, 0x40, 0x90, 0x11, 0x30, 0x03, 0x2f, 0x23, 0x2e, 0x53, + 0x0f, 0x02, 0x2c, 0x00, 0x30, 0xd0, 0x6f, 0xfb, 0x6f, 0xb0, 0x5f, 0xb8, 0x2e, 0x40, 0x50, 0xf1, 0x7f, 0x0a, 0x25, + 0x3c, 0x86, 0xeb, 0x7f, 0x41, 0x33, 0x22, 0x30, 0x98, 0x2e, 0xc2, 0xc4, 0xd3, 0x6f, 0xf4, 0x30, 0xdc, 0x09, 0x71, + 0x58, 0xc2, 0x6f, 0x94, 0x09, 0x73, 0x58, 0x6a, 0xbb, 0xdc, 0x08, 0xb4, 0xb9, 0xb1, 0xbd, 0x6f, 0x5a, 0x95, 0x08, + 0x21, 0xbd, 0xf6, 0xbf, 0x77, 0x0b, 0x51, 0xbe, 0xf1, 0x6f, 0xeb, 0x6f, 0x52, 0x42, 0x54, 0x42, 0xc0, 0x2e, 0x43, + 0x42, 0xc0, 0x5f, 0x50, 0x50, 0x77, 0x52, 0x93, 0x30, 0x53, 0x42, 0xfb, 0x7f, 0x7b, 0x30, 0x4b, 0x42, 0x13, 0x30, + 0x42, 0x82, 0x20, 0x33, 0x43, 0x42, 0xc8, 0x00, 0x01, 0x2e, 0x80, 0x03, 0x05, 0x2e, 0x7e, 0x00, 0x19, 0x52, 0xe2, + 0x7f, 0xd0, 0x7f, 0xc3, 0x7f, 0x98, 0x2e, 0x9c, 0x0e, 0xd1, 0x6f, 0x48, 0x0a, 0xd1, 0x7f, 0x3a, 0x25, 0xfb, 0x86, + 0x01, 0x33, 0x12, 0x30, 0x98, 0x2e, 0xc2, 0xc4, 0xd1, 0x6f, 0x48, 0x0a, 0x40, 0xb2, 0x0d, 0x2f, 0xe0, 0x6f, 0x03, + 0x2e, 0x80, 0x03, 0x53, 0x30, 0x07, 0x80, 0x27, 0x2e, 0x21, 0xf2, 0x98, 0xbc, 0x01, 0x42, 0x98, 0x2e, 0xe0, 0x03, + 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0xb1, 0x6f, 0x9b, 0xb8, 0x07, 0x2e, 0x1b, 0x00, 0x19, 0x1a, 0xb1, 0x7f, 0x71, + 0x30, 0x04, 0x2f, 0x23, 0x2e, 0x21, 0xf2, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0x98, 0x2e, 0x6d, 0xc0, 0x98, 0x2e, + 0x5d, 0xc0, 0x98, 0x2e, 0xd9, 0xb5, 0x20, 0x26, 0xc1, 0x6f, 0x02, 0x31, 0x52, 0x42, 0xab, 0x30, 0x4b, 0x42, 0x20, + 0x33, 0x79, 0x56, 0xf1, 0x37, 0xc4, 0x40, 0xa2, 0x0a, 0xc2, 0x42, 0xd8, 0x00, 0x01, 0x2e, 0x5e, 0xf7, 0x41, 0x08, + 0x23, 0x2e, 0x93, 0x00, 0xe3, 0x7f, 0x98, 0x2e, 0xe1, 0x00, 0xe1, 0x6f, 0x83, 0x30, 0x43, 0x42, 0x03, 0x30, 0xfb, + 0x6f, 0x75, 0x50, 0x02, 0x30, 0x00, 0x2e, 0x00, 0x2e, 0x81, 0x84, 0x50, 0x0e, 0xfa, 0x2f, 0x43, 0x42, 0x11, 0x30, + 0xb0, 0x5f, 0x23, 0x2e, 0x21, 0xf2, 0xb8, 0x2e, 0xc1, 0x4a, 0x00, 0x00, 0x6d, 0x57, 0x00, 0x00, 0x77, 0x8e, 0x00, + 0x00, 0xe0, 0xff, 0xff, 0xff, 0xd3, 0xff, 0xff, 0xff, 0xe5, 0xff, 0xff, 0xff, 0xee, 0xe1, 0xff, 0xff, 0x7c, 0x13, + 0x00, 0x00, 0x46, 0xe6, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0xfd, 0x2d +}; + +/*! @name Global array that stores the feature input configuration of + * BMI270_HUAWEI_CONTEXT + */ +const struct bmi2_feature_config bmi270_hc_feat_in[BMI270_HC_MAX_FEAT_IN] = { + { .type = BMI2_CONFIG_ID, .page = BMI2_PAGE_4, .start_addr = BMI270_HC_CONFIG_ID_STRT_ADDR }, + { .type = BMI2_STEP_COUNTER_PARAMS, .page = BMI2_PAGE_1, .start_addr = BMI270_HC_STEP_CNT_1_STRT_ADDR }, + { .type = BMI2_STEP_DETECTOR, .page = BMI2_PAGE_4, .start_addr = BMI270_HC_STEP_CNT_4_STRT_ADDR }, + { .type = BMI2_STEP_COUNTER, .page = BMI2_PAGE_4, .start_addr = BMI270_HC_STEP_CNT_4_STRT_ADDR }, + { .type = BMI2_NVM_PROG_PREP, .page = BMI2_PAGE_4, .start_addr = BMI270_HC_NVM_PROG_PREP_STRT_ADDR }, + { .type = BMI2_MAX_BURST_LEN, .page = BMI2_PAGE_4, .start_addr = BMI270_HC_MAX_BURST_LEN_STRT_ADDR }, + { .type = BMI2_CRT_GYRO_SELF_TEST, .page = BMI2_PAGE_4, .start_addr = BMI270_HC_CRT_GYRO_SELF_TEST_STRT_ADDR }, + { .type = BMI2_ABORT_CRT_GYRO_SELF_TEST, .page = BMI2_PAGE_4, .start_addr = BMI270_HC_ABORT_STRT_ADDR }, + { .type = BMI2_ACTIVITY_RECOGNITION_SETTINGS, .page = BMI2_PAGE_5, .start_addr = BMI270_HC_ACT_RGN_SETT_STRT_ADDR }, + { .type = BMI2_ACTIVITY_RECOGNITION, .page = BMI2_PAGE_5, .start_addr = BMI270_HC_ACT_RGN_STRT_ADDR }, +}; + +/*! @name Global array that stores the feature output configuration */ +const struct bmi2_feature_config bmi270_hc_feat_out[BMI270_HC_MAX_FEAT_OUT] = { + { .type = BMI2_STEP_COUNTER, .page = BMI2_PAGE_0, .start_addr = BMI270_HC_STEP_CNT_OUT_STRT_ADDR }, + { .type = BMI2_GYRO_CROSS_SENSE, .page = BMI2_PAGE_0, .start_addr = BMI270_HC_GYRO_CROSS_SENSE_STRT_ADDR }, + { .type = BMI2_GYRO_GAIN_UPDATE, .page = BMI2_PAGE_0, .start_addr = BMI270_HC_GYR_USER_GAIN_OUT_STRT_ADDR }, + { .type = BMI2_NVM_STATUS, .page = BMI2_PAGE_0, .start_addr = BMI270_HC_NVM_VFRM_OUT_STRT_ADDR }, + { .type = BMI2_VFRM_STATUS, .page = BMI2_PAGE_0, .start_addr = BMI270_HC_NVM_VFRM_OUT_STRT_ADDR } +}; + +/*! @name Global array that stores the feature interrupts of BMI270_HUAWEI_CONTEXT */ +struct bmi2_map_int bmi270_hc_map_int[BMI270_HC_MAX_INT_MAP] = { + { .type = BMI2_STEP_COUNTER, .sens_map_int = BMI270_HC_INT_STEP_COUNTER_MASK }, + { .type = BMI2_STEP_DETECTOR, .sens_map_int = BMI270_HC_INT_STEP_DETECTOR_MASK }, +}; + +/******************************************************************************/ + +/*! Local Function Prototypes + ******************************************************************************/ + +/*! + * @brief This internal API is used to validate the device pointer for + * null conditions. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t null_ptr_check(const struct bmi2_dev *dev); + +/*! + * @brief This internal API enables the selected sensor/features. + * + * @param[in] sensor_sel : Selects the desired sensor. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t sensor_enable(uint64_t sensor_sel, struct bmi2_dev *dev); + +/*! + * @brief This internal API disables the selected sensor/features. + * + * @param[in] sensor_sel : Selects the desired sensor. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t sensor_disable(uint64_t sensor_sel, struct bmi2_dev *dev); + +/*! + * @brief This internal API selects the sensors/features to be enabled or + * disabled. + * + * @param[in] sens_list : Pointer to select the sensor. + * @param[in] n_sens : Number of sensors selected. + * @param[out] sensor_sel : Gets the selected sensor. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t select_sensor(const uint8_t *sens_list, uint8_t n_sens, uint64_t *sensor_sel); + +/*! + * @brief This internal API is used to enable/disable step detector feature. + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[in] enable : Enables/Disables step-detector. + * + * Enable | Description + * -------------|--------------- + * BMI2_DISABLE | Disables step detector + * BMI2_ENABLE | Enables step detector + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_step_detector(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to enable/disable step counter feature. + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[in] enable : Enables/Disables step counter. + * + * Enable | Description + * -------------|--------------- + * BMI2_DISABLE | Disables step counter + * BMI2_ENABLE | Enables step counter + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_step_counter(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API enables/disables the activity recognition feature. + * + * @param[in] enable : Enables/Disables activity recognition. + * @param[in] dev : Structure instance of bmi2_dev. + * + * enable | Description + * -------------|--------------- + * BMI2_ENABLE | Enables activity recognition. + * BMI2_DISABLE | Disables activity recognition. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_act_recog(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to enable/disable gyroscope user gain + * feature. + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[in] enable : Enables/Disables gyroscope user gain. + * + * Enable | Description + * -------------|--------------- + * BMI2_DISABLE | Disables gyroscope user gain + * BMI2_ENABLE | Enables gyroscope user gain + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_gyro_user_gain(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API sets step counter parameter configurations. + * + * @param[in] step_count_params : Array that stores parameters 1 to 25. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_step_count_params_config(const uint16_t *step_count_params, struct bmi2_dev *dev); + +/*! + * @brief This internal API sets step counter/detector/activity configurations. + * + * @param[in] config : Structure instance of bmi2_step_config. + * @param[in] dev : Structure instance of bmi2_dev. + * + *--------------------------------------------------------------------------- + * bmi2_step_config | + * Structure parameters | Description + *--------------------------|-------------------------------------------------- + * | The Step-counter will trigger output every time + * | the number of steps are counted. Holds implicitly + * water-mark level | a 20x factor, so the range is 0 to 10230, + * | with resolution of 20 steps. + * -------------------------|--------------------------------------------------- + * reset counter | Flag to reset the counted steps. + * -------------------------|--------------------------------------------------- + * @endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_step_config(const struct bmi2_step_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets step counter parameter configurations. + * + * @param[in] step_count_params : Array that stores parameters 1 to 25. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_step_count_params_config(uint16_t *step_count_params, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets step counter/detector/activity configurations. + * + * @param[out] config : Structure instance of bmi2_step_config. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @verbatim + *---------------------------------------------------------------------------- + * bmi2_step_config | + * Structure parameters | Description + *--------------------------|-------------------------------------------------- + * | The Step-counter will trigger output every time + * | the number of steps are counted. Holds implicitly + * water-mark level | a 20x factor, so the range is 0 to 10230, + * | with resolution of 20 steps. + * -------------------------|--------------------------------------------------- + * reset counter | Flag to reset the counted steps. + * -------------------------|--------------------------------------------------- + * @endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_step_config(struct bmi2_step_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to parse and store the activity recognition + * output from the FIFO data. + * + * @param[out] act_recog : Structure to retrieve output of activity + * recognition frame. + * @param[in] data_index : Index of the FIFO data which contains + * activity recognition frame. + * @param[out] fifo : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t unpack_act_recog_output(struct bmi2_act_recog_output *act_recog, + uint16_t *data_index, + const struct bmi2_fifo_frame *fifo); + +/*! + * @brief This internal API gets the output values of step counter. + * + * @param[out] step_count : Pointer to the stored step counter data. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_step_counter_output(uint32_t *step_count, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets the error status related to NVM. + * + * @param[out] nvm_err_stat : Stores the NVM error status. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_nvm_error_status(struct bmi2_nvm_err_status *nvm_err_stat, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets the error status related to virtual frames. + * + * @param[out] vfrm_err_stat : Stores the VFRM related error status. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_vfrm_error_status(struct bmi2_vfrm_err_status *vfrm_err_stat, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to get enable status of gyroscope user gain + * update. + * + * @param[out] status : Stores status of gyroscope user gain update. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_user_gain_upd_status(uint8_t *status, struct bmi2_dev *dev); + +/*! + * @brief This internal API skips S4S frame in the FIFO data while getting + * activity recognition output. + * + * @param[in, out] frame_header : FIFO frame header. + * @param[in, out] data_index : Index value of the FIFO data bytes + * from which S4S frame header is to be + * skipped. + * @param[in] fifo : Structure instance of bmi2_fifo_frame. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t move_if_s4s_frame(const uint8_t *frame_header, uint16_t *data_index, const struct bmi2_fifo_frame *fifo); + +/*! + * @brief This internal API enables/disables compensation of the gain defined + * in the GAIN register. + * + * @param[in] enable : Enables/Disables gain compensation + * @param[in] dev : Structure instance of bmi2_dev. + * + * enable | Description + * -------------|--------------- + * BMI2_ENABLE | Enable gain compensation. + * BMI2_DISABLE | Disable gain compensation. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t enable_gyro_gain(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to extract the output feature configuration + * details like page and start address from the look-up table. + * + * @param[out] feat_output : Structure that stores output feature + * configurations. + * @param[in] type : Type of feature or sensor. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Returns the feature found flag. + * + * @retval BMI2_FALSE : Feature not found + * BMI2_TRUE : Feature found + */ +static uint8_t extract_output_feat_config(struct bmi2_feature_config *feat_output, + uint8_t type, + const struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to move the data index ahead of the + * current frame length parameter when unnecessary FIFO data appears while + * extracting the user specified data. + * + * @param[in,out] data_index : Index of the FIFO data which is to be + * moved ahead of the current frame length + * @param[in] current_frame_length : Number of bytes in the current frame. + * @param[in] fifo : Structure instance of bmi2_fifo_frame. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t move_next_frame(uint16_t *data_index, uint8_t current_frame_length, const struct bmi2_fifo_frame *fifo); + +/***************************************************************************/ + +/*! User Interface Definitions + ****************************************************************************/ + +/*! + * @brief This API: + * 1) Updates the device structure with address of the configuration file. + * 2) Initializes BMI270_HUAWEI_CONTEXT sensor. + * 3) Writes the configuration file. + * 4) Updates the feature offset parameters in the device structure. + * 5) Updates the maximum number of pages, in the device structure. + */ +int8_t bmi270_hc_init(struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + /* Assign chip id of BMI270_HUAWEI_CONTEXT */ + dev->chip_id = BMI270_HC_CHIP_ID; + + /* Get the size of config array */ + dev->config_size = sizeof(bmi270_hc_config_file); + + /* Enable the variant specific features if any */ + dev->variant_feature = BMI2_CRT_RTOSK_ENABLE | BMI2_GYRO_CROSS_SENS_ENABLE; + + /* An extra dummy byte is read during SPI read */ + if (dev->intf == BMI2_SPI_INTF) + { + dev->dummy_byte = 1; + } + else + { + dev->dummy_byte = 0; + } + + /* If configuration file pointer is not assigned any address */ + if (!dev->config_file_ptr) + { + /* Give the address of the configuration file array to + * the device pointer + */ + dev->config_file_ptr = bmi270_hc_config_file; + } + + /* Initialize BMI2 sensor */ + rslt = bmi2_sec_init(dev); + if (rslt == BMI2_OK) + { + /* Assign the offsets of the feature input + * configuration to the device structure + */ + dev->feat_config = bmi270_hc_feat_in; + + /* Assign the offsets of the feature output to + * the device structure + */ + dev->feat_output = bmi270_hc_feat_out; + + /* Assign the maximum number of pages to the + * device structure + */ + dev->page_max = BMI270_HC_MAX_PAGE_NUM; + + /* Assign maximum number of input sensors/ + * features to device structure + */ + dev->input_sens = BMI270_HC_MAX_FEAT_IN; + + /* Assign maximum number of output sensors/ + * features to device structure + */ + dev->out_sens = BMI270_HC_MAX_FEAT_OUT; + + /* Assign the offsets of the feature interrupt + * to the device structure + */ + dev->map_int = bmi270_hc_map_int; + + /* Assign maximum number of feature interrupts + * to device structure + */ + dev->sens_int_map = BMI270_HC_MAX_INT_MAP; + } + } + + return rslt; +} + +/*! + * @brief This API selects the sensors/features to be enabled. + */ +int8_t bmi270_hc_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to select sensor */ + uint64_t sensor_sel = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sens_list != NULL)) + { + /* Get the selected sensors */ + rslt = select_sensor(sens_list, n_sens, &sensor_sel); + if (rslt == BMI2_OK) + { + /* Enable the selected sensors */ + rslt = sensor_enable(sensor_sel, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API selects the sensors/features to be disabled. + */ +int8_t bmi270_hc_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to select sensor */ + uint64_t sensor_sel = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sens_list != NULL)) + { + /* Get the selected sensors */ + rslt = select_sensor(sens_list, n_sens, &sensor_sel); + if (rslt == BMI2_OK) + { + /* Disable the selected sensors */ + rslt = sensor_disable(sensor_sel, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API sets the sensor/feature configuration. + */ +int8_t bmi270_hc_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define loop */ + uint8_t loop; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sens_cfg != NULL)) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + + for (loop = 0; loop < n_sens; loop++) + { + if ((sens_cfg[loop].type == BMI2_ACCEL) || (sens_cfg[loop].type == BMI2_GYRO) || + (sens_cfg[loop].type == BMI2_AUX) || (sens_cfg[loop].type == BMI2_GYRO_GAIN_UPDATE)) + { + rslt = bmi2_set_sensor_config(&sens_cfg[loop], 1, dev); + } + else + { + /* Disable Advance power save if enabled for auxiliary + * and feature configurations + */ + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if + * enabled + */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + if (rslt == BMI2_OK) + { + switch (sens_cfg[loop].type) + { + /* Set the step counter parameters */ + case BMI2_STEP_COUNTER_PARAMS: + rslt = set_step_count_params_config(sens_cfg[loop].cfg.step_counter_params, dev); + break; + + /* Set step counter/detector/activity configuration */ + case BMI2_STEP_DETECTOR: + case BMI2_STEP_COUNTER: + rslt = set_step_config(&sens_cfg[loop].cfg.step_counter, dev); + break; + + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + } + + /* Return error if any of the set configurations fail */ + if (rslt != BMI2_OK) + { + break; + } + } + } + + /* Enable Advance power save if disabled while configuring and + * not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API gets the sensor/feature configuration. + */ +int8_t bmi270_hc_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define loop */ + uint8_t loop; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sens_cfg != NULL)) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + for (loop = 0; loop < n_sens; loop++) + { + if ((sens_cfg[loop].type == BMI2_ACCEL) || (sens_cfg[loop].type == BMI2_GYRO) || + (sens_cfg[loop].type == BMI2_AUX) || (sens_cfg[loop].type == BMI2_GYRO_GAIN_UPDATE)) + { + rslt = bmi2_get_sensor_config(&sens_cfg[loop], 1, dev); + } + else + { + /* Disable Advance power save if enabled for auxiliary + * and feature configurations + */ + if ((sens_cfg[loop].type >= BMI2_MAIN_SENS_MAX_NUM) || (sens_cfg[loop].type == BMI2_AUX)) + { + + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if + * enabled + */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + } + + if (rslt == BMI2_OK) + { + switch (sens_cfg[loop].type) + { + /* Set the step counter parameters */ + case BMI2_STEP_COUNTER_PARAMS: + rslt = get_step_count_params_config(sens_cfg[loop].cfg.step_counter_params, dev); + break; + + /* Get step counter/detector/activity configuration */ + case BMI2_STEP_DETECTOR: + case BMI2_STEP_COUNTER: + rslt = get_step_config(&sens_cfg[loop].cfg.step_counter, dev); + break; + + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + } + + /* Return error if any of the get configurations fail */ + if (rslt != BMI2_OK) + { + break; + } + } + } + + /* Enable Advance power save if disabled while configuring and + * not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API gets the sensor/feature data for accelerometer, gyroscope, + * auxiliary sensor, step counter, high-g, gyroscope user-gain update, + * orientation, gyroscope cross sensitivity and error status for NVM and VFRM. + */ +int8_t bmi270_hc_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define loop */ + uint8_t loop; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sensor_data != NULL)) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + for (loop = 0; loop < n_sens; loop++) + { + if ((sensor_data[loop].type == BMI2_ACCEL) || (sensor_data[loop].type == BMI2_GYRO) || + (sensor_data[loop].type == BMI2_AUX) || (sensor_data[loop].type == BMI2_GYRO_GAIN_UPDATE) || + (sensor_data[loop].type == BMI2_GYRO_CROSS_SENSE)) + { + rslt = bmi2_get_sensor_data(&sensor_data[loop], 1, dev); + } + else + { + /* Disable Advance power save if enabled for feature + * configurations + */ + if (sensor_data[loop].type >= BMI2_MAIN_SENS_MAX_NUM) + { + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if + * enabled + */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + } + + if (rslt == BMI2_OK) + { + switch (sensor_data[loop].type) + { + case BMI2_STEP_COUNTER: + + /* Get step counter output */ + rslt = get_step_counter_output(&sensor_data[loop].sens_data.step_counter_output, dev); + break; + case BMI2_NVM_STATUS: + + /* Get NVM error status */ + rslt = get_nvm_error_status(&sensor_data[loop].sens_data.nvm_status, dev); + break; + case BMI2_VFRM_STATUS: + + /* Get VFRM error status */ + rslt = get_vfrm_error_status(&sensor_data[loop].sens_data.vfrm_status, dev); + break; + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + + /* Return error if any of the get sensor data fails */ + if (rslt != BMI2_OK) + { + break; + } + } + } + + /* Enable Advance power save if disabled while + * configuring and not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This api is used for retrieving the activity recognition settings currently set for bmi270hc. + */ +int8_t bmi270_hc_get_act_recg_sett(struct bmi2_hc_act_recg_sett *sett, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat; + + /* Variable to set flag */ + uint8_t feat_found; + uint16_t msb_lsb; + uint8_t lsb; + uint8_t msb; + + /* Initialize feature configuration for activity recognition */ + struct bmi2_feature_config bmi2_act_recg_sett = { 0, 0, 0 }; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + /* Search for activity recognition feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&bmi2_act_recg_sett, BMI2_ACTIVITY_RECOGNITION_SETTINGS, dev); + if (feat_found) + { + aps_stat = dev->aps_status; + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if enabled */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + /* Get the configuration from the page where activity recognition setting feature resides */ + if (rslt == BMI2_OK) + { + rslt = bmi2_get_feat_config(bmi2_act_recg_sett.page, feat_config, dev); + } + + if (rslt == BMI2_OK) + { + /* Define the offset in bytes */ + idx = bmi2_act_recg_sett.start_addr; + + lsb = feat_config[idx++]; + msb = feat_config[idx++]; + msb_lsb = ((uint16_t)msb << 8) | lsb; + + sett->segment_size = msb_lsb & BMI2_HC_ACT_RECG_SEGMENT_SIZE_MASK; + + /* Increment idx by 2 to point post processing enable/disable address */ + idx = 4; + lsb = feat_config[idx++]; + msb = feat_config[idx++]; + msb_lsb = ((uint16_t)msb << 8) | lsb; + sett->pp_en = msb_lsb & BMI2_HC_ACT_RECG_PP_EN_MASK; + + /* Increment idx by 2 to point mix gdi thres addres */ + idx = 6; + lsb = feat_config[idx++]; + msb = feat_config[idx++]; + msb_lsb = ((uint16_t)msb << 8) | lsb; + sett->min_gdi_thres = msb_lsb & BMI2_HC_ACT_RECG_MIN_GDI_THRES_MASK; + + /* Increment idx by 2 to point max gdi thres addres */ + idx = 8; + lsb = feat_config[idx++]; + msb = feat_config[idx++]; + msb_lsb = ((uint16_t)msb << 8) | lsb; + sett->max_gdi_thres = msb_lsb & BMI2_HC_ACT_RECG_MAX_GDI_THRES_MASK; + + /* Increment idx by 2 to point to buffer size */ + idx = 10; + lsb = feat_config[idx++]; + msb = feat_config[idx++]; + msb_lsb = ((uint16_t)msb << 8) | lsb; + sett->buf_size = msb_lsb & BMI2_HC_ACT_RECG_BUF_SIZE_MASK; + + /* Increment idx by 2 to to point to min segment confidence */ + idx = 12; + lsb = feat_config[idx++]; + msb = feat_config[idx++]; + msb_lsb = ((uint16_t)msb << 8) | lsb; + sett->min_seg_conf = msb_lsb & BMI2_HC_ACT_RECG_MIN_SEG_CONF_MASK; + } + + /* Enable Advance power save if disabled while + * configuring and not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + } + + return rslt; +} + +/*! + * @brief This api is used for setting the activity recognition settings for bmi270hc. + */ +int8_t bmi270_hc_set_act_recg_sett(const struct bmi2_hc_act_recg_sett *sett, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Variable to define index */ + uint8_t index = 0; + + /* Initialize feature configuration for activity recognition */ + struct bmi2_feature_config bmi2_act_recg_sett = { 0, 0, 0 }; + + /* Copy the feature configuration address to a local pointer */ + uint16_t *data_p = (uint16_t *) (void *)feat_config; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + /* Search for activity recognition feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&bmi2_act_recg_sett, BMI2_ACTIVITY_RECOGNITION_SETTINGS, dev); + if (feat_found) + { + aps_stat = dev->aps_status; + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if enabled */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + if (rslt == BMI2_OK) + { + /* Get the configuration from the page where activity recognition setting feature resides */ + rslt = bmi2_get_feat_config(bmi2_act_recg_sett.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes */ + idx = bmi2_act_recg_sett.start_addr; + + /* Get offset in words since all the features are set in words length */ + idx = idx / 2; + + /* Set segment size */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), + BMI2_HC_ACT_RECG_SEGMENT_SIZE, + sett->segment_size); + + /* Starting address of post processing represented in word length */ + idx = 2; + + /* Set post processing */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_HC_ACT_RECG_PP_EN, sett->pp_en); + + /* Starting address of min_gdi_thres represented in word length */ + idx = 3; + + /* Set minimum gdi threshold */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), + BMI2_HC_ACT_RECG_MIN_GDI_THRES, + sett->min_gdi_thres); + + /* Starting address of max_gdi_thres represented in word length */ + idx = 4; + + /* Set maximum gdi threshold */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), + BMI2_HC_ACT_RECG_MAX_GDI_THRES, + sett->max_gdi_thres); + + /* Starting address of buffer size represented in word length */ + idx = 5; + + /* Set buffer size */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_HC_ACT_RECG_BUF_SIZE, sett->buf_size); + + /* Starting address of min_seg_conf represented in word length */ + idx = 6; + + /* Set min segment confidence */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), + BMI2_HC_ACT_RECG_MIN_SEG_CONF, + sett->min_seg_conf); + + /* Increment offset by 1 more word to get the total length in words */ + idx++; + + /* Get total length in bytes to copy from local pointer to the array */ + idx = (uint8_t)(idx * 2) - bmi2_act_recg_sett.start_addr; + + /* Copy the bytes to be set back to the array */ + for (index = 0; index <= idx; index++) + { + feat_config[bmi2_act_recg_sett.start_addr + + index] = *((uint8_t *) data_p + bmi2_act_recg_sett.start_addr + index); + } + + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + + /* Enable Advance power save if disabled while + * configuring and not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + } + + return rslt; +} + +/*! + * @brief This internal API is used to parse the activity output from the + * FIFO in header mode. + */ +int8_t bmi270_hc_get_act_recog_output(struct bmi2_act_recog_output *act_recog, + uint16_t *act_frm_len, + struct bmi2_fifo_frame *fifo, + const struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define header frame */ + uint8_t frame_header = 0; + + /* Variable to index the data bytes */ + uint16_t data_index; + + /* Variable to index activity frames */ + uint16_t act_idx = 0; + + /* Variable to indicate activity frames read */ + uint16_t frame_to_read = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (act_recog != NULL) && (act_frm_len != NULL) && (fifo != NULL)) + { + + /* Store the number of frames to be read */ + frame_to_read = *act_frm_len; + for (data_index = fifo->act_recog_byte_start_idx; data_index < fifo->length;) + { + /* Get frame header byte */ + frame_header = fifo->data[data_index] & BMI2_FIFO_TAG_INTR_MASK; + + /* Skip S4S frames if S4S is enabled */ + rslt = move_if_s4s_frame(&frame_header, &data_index, fifo); + + /* Break if FIFO is empty */ + if (rslt == BMI2_W_FIFO_EMPTY) + { + break; + } + + /* Index shifted to next byte where data starts */ + data_index++; + switch (frame_header) + { + /* If header defines accelerometer frame */ + case BMI2_FIFO_HEADER_ACC_FRM: + rslt = move_next_frame(&data_index, fifo->acc_frm_len, fifo); + break; + + /* If header defines accelerometer and auxiliary frames */ + case BMI2_FIFO_HEADER_AUX_ACC_FRM: + rslt = move_next_frame(&data_index, fifo->acc_aux_frm_len, fifo); + break; + + /* If header defines accelerometer and gyroscope frames */ + case BMI2_FIFO_HEADER_GYR_ACC_FRM: + rslt = move_next_frame(&data_index, fifo->acc_gyr_frm_len, fifo); + break; + + /* If header defines accelerometer, auxiliary and gyroscope frames */ + case BMI2_FIFO_HEADER_ALL_FRM: + rslt = move_next_frame(&data_index, fifo->all_frm_len, fifo); + break; + + /* If header defines only gyroscope frame */ + case BMI2_FIFO_HEADER_GYR_FRM: + rslt = move_next_frame(&data_index, fifo->gyr_frm_len, fifo); + break; + + /* If header defines only auxiliary frame */ + case BMI2_FIFO_HEADER_AUX_FRM: + rslt = move_next_frame(&data_index, fifo->aux_frm_len, fifo); + break; + + /* If header defines auxiliary and gyroscope frame */ + case BMI2_FIFO_HEADER_AUX_GYR_FRM: + rslt = move_next_frame(&data_index, fifo->aux_gyr_frm_len, fifo); + break; + + /* If header defines sensor time frame */ + case BMI2_FIFO_HEADER_SENS_TIME_FRM: + rslt = move_next_frame(&data_index, BMI2_SENSOR_TIME_LENGTH, fifo); + break; + + /* If header defines skip frame */ + case BMI2_FIFO_HEADER_SKIP_FRM: + rslt = move_next_frame(&data_index, BMI2_FIFO_SKIP_FRM_LENGTH, fifo); + break; + + /* If header defines Input configuration frame */ + case BMI2_FIFO_HEADER_INPUT_CFG_FRM: + rslt = move_next_frame(&data_index, BMI2_FIFO_INPUT_CFG_LENGTH, fifo); + break; + + /* If header defines invalid frame or end of valid data */ + case BMI2_FIFO_HEAD_OVER_READ_MSB: + + /* Move the data index to the last byte to mark completion */ + data_index = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + break; + + /* If header defines activity recognition frame */ + case BMI2_FIFO_VIRT_ACT_RECOG_FRM: + + /* Get the activity output */ + rslt = unpack_act_recog_output(&act_recog[(act_idx)], &data_index, fifo); + + /* Update activity frame index */ + (act_idx)++; + break; + default: + + /* Move the data index to the last byte in case of invalid values */ + data_index = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + break; + } + + /* Number of frames to be read is complete or FIFO is empty */ + if ((frame_to_read == act_idx) || (rslt == BMI2_W_FIFO_EMPTY)) + { + break; + } + } + + /* Update the activity frame index */ + (*act_frm_len) = act_idx; + + /* Update the activity byte index */ + fifo->act_recog_byte_start_idx = data_index; + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API updates the gyroscope user-gain. + */ +int8_t bmi270_hc_update_gyro_user_gain(const struct bmi2_gyro_user_gain_config *user_gain, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to select sensor */ + uint8_t sens_sel[2] = { BMI2_GYRO, BMI2_GYRO_GAIN_UPDATE }; + + /* Structure to define sensor configurations */ + struct bmi2_sens_config sens_cfg; + + /* Variable to store status of user-gain update module */ + uint8_t status = 0; + + /* Variable to define count */ + uint8_t count = 100; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (user_gain != NULL)) + { + /* Select type of feature */ + sens_cfg.type = BMI2_GYRO_GAIN_UPDATE; + + /* Get the user gain configurations */ + rslt = bmi270_hc_get_sensor_config(&sens_cfg, 1, dev); + if (rslt == BMI2_OK) + { + /* Get the user-defined ratio */ + sens_cfg.cfg.gyro_gain_update = *user_gain; + + /* Set rate ratio for all axes */ + rslt = bmi270_hc_set_sensor_config(&sens_cfg, 1, dev); + } + + /* Disable gyroscope */ + if (rslt == BMI2_OK) + { + rslt = bmi270_hc_sensor_disable(&sens_sel[0], 1, dev); + } + + /* Enable gyroscope user-gain update module */ + if (rslt == BMI2_OK) + { + rslt = bmi270_hc_sensor_enable(&sens_sel[1], 1, dev); + } + + /* Set the command to trigger the computation */ + if (rslt == BMI2_OK) + { + rslt = bmi2_set_command_register(BMI2_USR_GAIN_CMD, dev); + } + + if (rslt == BMI2_OK) + { + /* Poll until enable bit of user-gain update is 0 */ + while (count--) + { + rslt = get_user_gain_upd_status(&status, dev); + if ((rslt == BMI2_OK) && (status == 0)) + { + /* Enable compensation of gain defined + * in the GAIN register + */ + rslt = enable_gyro_gain(BMI2_ENABLE, dev); + + /* Enable gyroscope */ + if (rslt == BMI2_OK) + { + rslt = bmi270_hc_sensor_enable(&sens_sel[0], 1, dev); + } + + break; + } + + dev->delay_us(10000, dev->intf_ptr); + } + + /* Return error if user-gain update is failed */ + if ((rslt == BMI2_OK) && (status != 0)) + { + rslt = BMI2_E_GYR_USER_GAIN_UPD_FAIL; + } + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API reads the compensated gyroscope user-gain values. + */ +int8_t bmi270_hc_read_gyro_user_gain(struct bmi2_gyro_user_gain_data *gyr_usr_gain, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define register data */ + uint8_t reg_data[3] = { 0 }; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (gyr_usr_gain != NULL)) + { + /* Get the gyroscope compensated gain values */ + rslt = bmi2_get_regs(BMI2_GYR_USR_GAIN_0_ADDR, reg_data, 3, dev); + if (rslt == BMI2_OK) + { + /* Gyroscope user gain correction X-axis */ + gyr_usr_gain->x = (int8_t)BMI2_GET_BIT_POS0(reg_data[0], BMI2_GYR_USR_GAIN_X); + + /* Gyroscope user gain correction Y-axis */ + gyr_usr_gain->y = (int8_t)BMI2_GET_BIT_POS0(reg_data[1], BMI2_GYR_USR_GAIN_Y); + + /* Gyroscope user gain correction z-axis */ + gyr_usr_gain->z = (int8_t)BMI2_GET_BIT_POS0(reg_data[2], BMI2_GYR_USR_GAIN_Z); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API maps/unmaps feature interrupts to that of interrupt pins. + */ +int8_t bmi270_hc_map_feat_int(const struct bmi2_sens_int_config *sens_int, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define loop */ + uint8_t loop; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sens_int != NULL)) + { + for (loop = 0; loop < n_sens; loop++) + { + switch (sens_int[loop].type) + { + case BMI2_STEP_COUNTER: + case BMI2_STEP_DETECTOR: + + rslt = bmi2_map_feat_int(sens_int[loop].type, sens_int[loop].hw_int_pin, dev); + break; + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + + /* Return error if interrupt mapping fails */ + if (rslt != BMI2_OK) + { + break; + } + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/***************************************************************************/ + +/*! Local Function Definitions + ****************************************************************************/ + +/*! + * @brief This internal API is used to validate the device structure pointer for + * null conditions. + */ +static int8_t null_ptr_check(const struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + if ((dev == NULL) || (dev->read == NULL) || (dev->write == NULL) || (dev->delay_us == NULL)) + { + /* Device structure pointer is not valid */ + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This internal API selects the sensor/features to be enabled or + * disabled. + */ +static int8_t select_sensor(const uint8_t *sens_list, uint8_t n_sens, uint64_t *sensor_sel) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Variable to define loop */ + uint8_t count; + + for (count = 0; count < n_sens; count++) + { + switch (sens_list[count]) + { + case BMI2_ACCEL: + *sensor_sel |= BMI2_ACCEL_SENS_SEL; + break; + case BMI2_GYRO: + *sensor_sel |= BMI2_GYRO_SENS_SEL; + break; + case BMI2_AUX: + *sensor_sel |= BMI2_AUX_SENS_SEL; + break; + case BMI2_TEMP: + *sensor_sel |= BMI2_TEMP_SENS_SEL; + break; + case BMI2_STEP_DETECTOR: + *sensor_sel |= BMI2_STEP_DETECT_SEL; + break; + case BMI2_STEP_COUNTER: + *sensor_sel |= BMI2_STEP_COUNT_SEL; + break; + case BMI2_GYRO_GAIN_UPDATE: + *sensor_sel |= BMI2_GYRO_GAIN_UPDATE_SEL; + break; + case BMI2_ACTIVITY_RECOGNITION: + *sensor_sel |= BMI2_ACTIVITY_RECOGNITION_SEL; + break; + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + } + + return rslt; +} + +/*! + * @brief This internal API enables the selected sensor/features. + */ +static int8_t sensor_enable(uint64_t sensor_sel, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store register values */ + uint8_t reg_data = 0; + + /* Variable to define loop */ + uint8_t loop = 1; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat = 0; + + rslt = bmi2_get_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + /* Enable accelerometer */ + if (sensor_sel & BMI2_ACCEL_SENS_SEL) + { + reg_data = BMI2_SET_BITS(reg_data, BMI2_ACC_EN, BMI2_ENABLE); + } + + /* Enable gyroscope */ + if (sensor_sel & BMI2_GYRO_SENS_SEL) + { + reg_data = BMI2_SET_BITS(reg_data, BMI2_GYR_EN, BMI2_ENABLE); + } + + /* Enable auxiliary sensor */ + if (sensor_sel & BMI2_AUX_SENS_SEL) + { + reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_AUX_EN, BMI2_ENABLE); + } + + /* Enable temperature sensor */ + if (sensor_sel & BMI2_TEMP_SENS_SEL) + { + reg_data = BMI2_SET_BITS(reg_data, BMI2_TEMP_EN, BMI2_ENABLE); + } + + /* Enable the sensors that are set in the power control register */ + if (sensor_sel & BMI2_MAIN_SENSORS) + { + rslt = bmi2_set_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); + } + } + + if ((rslt == BMI2_OK) && (sensor_sel & ~(BMI2_MAIN_SENSORS))) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if enabled */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + if (rslt == BMI2_OK) + { + while (loop--) + { + /* Enable step detector feature */ + if (sensor_sel & BMI2_STEP_DETECT_SEL) + { + rslt = set_step_detector(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_STEP_DETECT_SEL; + } + else + { + break; + } + } + + /* Enable step counter feature */ + if (sensor_sel & BMI2_STEP_COUNT_SEL) + { + rslt = set_step_counter(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_STEP_COUNT_SEL; + } + else + { + break; + } + } + + /* Enable activity recognition feature */ + if (sensor_sel & BMI2_ACTIVITY_RECOGNITION_SEL) + { + rslt = set_act_recog(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_ACTIVITY_RECOGNITION_SEL; + } + else + { + break; + } + } + + /* Enable gyroscope user gain */ + if (sensor_sel & BMI2_GYRO_GAIN_UPDATE_SEL) + { + rslt = set_gyro_user_gain(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_GYRO_GAIN_UPDATE_SEL; + } + else + { + break; + } + } + } + + /* Enable Advance power save if disabled while + * configuring and not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + } + + return rslt; +} + +/*! + * @brief This internal API disables the selected sensors/features. + */ +static int8_t sensor_disable(uint64_t sensor_sel, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store register values */ + uint8_t reg_data = 0; + + /* Variable to define loop */ + uint8_t loop = 1; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat = 0; + + rslt = bmi2_get_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + /* Disable accelerometer */ + if (sensor_sel & BMI2_ACCEL_SENS_SEL) + { + reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_ACC_EN); + } + + /* Disable gyroscope */ + if (sensor_sel & BMI2_GYRO_SENS_SEL) + { + reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_GYR_EN); + } + + /* Disable auxiliary sensor */ + if (sensor_sel & BMI2_AUX_SENS_SEL) + { + reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_AUX_EN); + } + + /* Disable temperature sensor */ + if (sensor_sel & BMI2_TEMP_SENS_SEL) + { + reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_TEMP_EN); + } + + /* Disable the sensors that are set in the power control register */ + if (sensor_sel & BMI2_MAIN_SENSORS) + { + rslt = bmi2_set_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); + } + } + + if ((rslt == BMI2_OK) && (sensor_sel & ~(BMI2_MAIN_SENSORS))) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if enabled */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + if (rslt == BMI2_OK) + { + while (loop--) + { + /* Disable step detector feature */ + if (sensor_sel & BMI2_STEP_DETECT_SEL) + { + rslt = set_step_detector(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat &= ~BMI2_STEP_DETECT_SEL; + } + else + { + break; + } + } + + /* Disable step counter feature */ + if (sensor_sel & BMI2_STEP_COUNT_SEL) + { + rslt = set_step_counter(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat &= ~BMI2_STEP_COUNT_SEL; + } + else + { + break; + } + } + + /* Disable activity recognition feature */ + if (sensor_sel & BMI2_ACTIVITY_RECOGNITION_SEL) + { + rslt = set_act_recog(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_ACTIVITY_RECOGNITION_SEL; + } + else + { + break; + } + } + + /* Disable gyroscope user gain */ + if (sensor_sel & BMI2_GYRO_GAIN_UPDATE_SEL) + { + rslt = set_gyro_user_gain(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat &= ~BMI2_GYRO_GAIN_UPDATE_SEL; + } + else + { + break; + } + } + + /* Enable Advance power save if disabled while + * configuring and not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + } + } + + return rslt; +} + +/*! + * @brief This internal API is used to enable/disable step detector feature. + */ +static int8_t set_step_detector(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for step detector */ + struct bmi2_feature_config step_det_config = { 0, 0, 0 }; + + /* Search for step detector feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_det_config, BMI2_STEP_DETECTOR, dev); + if (feat_found) + { + /* Get the configuration from the page where step detector feature resides */ + rslt = bmi2_get_feat_config(step_det_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of step detector */ + idx = step_det_config.start_addr + BMI2_STEP_COUNT_FEAT_EN_OFFSET; + + /* Set the feature enable bit */ + feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_STEP_DET_FEAT_EN, enable); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API is used to enable/disable step counter feature. + */ +static int8_t set_step_counter(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for step counter */ + struct bmi2_feature_config step_count_config = { 0, 0, 0 }; + + /* Search for step counter feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_count_config, BMI2_STEP_COUNTER, dev); + if (feat_found) + { + /* Get the configuration from the page where step-counter feature resides */ + rslt = bmi2_get_feat_config(step_count_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of step counter */ + idx = step_count_config.start_addr + BMI2_STEP_COUNT_FEAT_EN_OFFSET; + + /* Set the feature enable bit */ + feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_STEP_COUNT_FEAT_EN, enable); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! +* @brief This internal API enables/disables the activity recognition feature. +*/ +static int8_t set_act_recog(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for activity recognition */ + struct bmi2_feature_config act_recog_cfg = { 0, 0, 0 }; + + /* Search for activity recognition and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&act_recog_cfg, BMI2_ACTIVITY_RECOGNITION, dev); + if (feat_found) + { + /* Get the configuration from the page where activity + * recognition feature resides + */ + rslt = bmi2_get_feat_config(act_recog_cfg.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of activity recognition */ + idx = act_recog_cfg.start_addr; + + /* Set the feature enable bit */ + feat_config[idx] = BMI2_SET_BIT_POS0(feat_config[idx], BMI2_ACTIVITY_RECOG_EN, enable); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API is used to enable/disable gyroscope user gain + * feature. + */ +static int8_t set_gyro_user_gain(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for gyroscope user gain */ + struct bmi2_feature_config gyr_user_gain_cfg = { 0, 0, 0 }; + + /* Search for user gain feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&gyr_user_gain_cfg, BMI2_GYRO_GAIN_UPDATE, dev); + if (feat_found) + { + /* Get the configuration from the page where user gain feature resides */ + rslt = bmi2_get_feat_config(gyr_user_gain_cfg.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of user gain */ + idx = gyr_user_gain_cfg.start_addr + BMI2_GYR_USER_GAIN_FEAT_EN_OFFSET; + + /* Set the feature enable bit */ + feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_GYR_USER_GAIN_FEAT_EN, enable); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API sets step counter parameter configurations. + */ +static int8_t set_step_count_params_config(const uint16_t *step_count_params, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define index */ + uint8_t index = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for step counter parameters */ + struct bmi2_feature_config step_params_config = { 0, 0, 0 }; + + /* Variable to index the page number */ + uint8_t page_idx; + + /* Variable to define the start page */ + uint8_t start_page; + + /* Variable to define start address of the parameters */ + uint8_t start_addr; + + /* Variable to define number of bytes */ + uint8_t n_bytes = (BMI2_STEP_CNT_N_PARAMS * 2); + + /* Variable to store number of pages */ + uint8_t n_pages = (n_bytes / 16); + + /* Variable to define the end page */ + uint8_t end_page; + + /* Variable to define the remaining bytes to be read */ + uint8_t remain_len; + + /* Variable to define the maximum words(16 bytes or 8 words) to be read in a page */ + uint8_t max_len = 8; + + /* Variable index bytes in a page */ + uint8_t page_byte_idx; + + /* Variable to index the parameters */ + uint8_t param_idx = 0; + + /* Copy the feature configuration address to a local pointer */ + uint16_t *data_p = (uint16_t *) (void *)feat_config; + + /* Search for step counter parameter feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_params_config, BMI2_STEP_COUNTER_PARAMS, dev); + if (feat_found) + { + /* Get the start page for the step counter parameters */ + start_page = step_params_config.page; + + /* Get the end page for the step counter parameters */ + end_page = start_page + n_pages; + + /* Get the start address for the step counter parameters */ + start_addr = step_params_config.start_addr; + + /* Get the remaining length of bytes to be read */ + remain_len = (uint8_t)((n_bytes - (n_pages * 16)) + start_addr); + for (page_idx = start_page; page_idx <= end_page; page_idx++) + { + /* Get the configuration from the respective page */ + rslt = bmi2_get_feat_config(page_idx, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Start from address 0x00 when switched to next page */ + if (page_idx > start_page) + { + start_addr = 0; + } + + /* Remaining number of words to be read in the page */ + if (page_idx == end_page) + { + max_len = (remain_len / 2); + } + + /* Get offset in words since all the features are set in words length */ + page_byte_idx = start_addr / 2; + for (; page_byte_idx < max_len;) + { + /* Set parameters 1 to 25 */ + *(data_p + page_byte_idx) = BMI2_SET_BIT_POS0(*(data_p + page_byte_idx), + BMI2_STEP_COUNT_PARAMS, + step_count_params[param_idx]); + + /* Increment offset by 1 word to set to the next parameter */ + page_byte_idx++; + + /* Increment to next parameter */ + param_idx++; + } + + /* Get total length in bytes to copy from local pointer to the array */ + page_byte_idx = (uint8_t)(page_byte_idx * 2) - step_params_config.start_addr; + + /* Copy the bytes to be set back to the array */ + for (index = 0; index < page_byte_idx; index++) + { + feat_config[step_params_config.start_addr + + index] = *((uint8_t *) data_p + step_params_config.start_addr + index); + } + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/* @brief This internal API sets step counter configurations like water-mark + * level, reset-counter and output-configuration step detector and activity. + */ +static int8_t set_step_config(const struct bmi2_step_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to define index */ + uint8_t index = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for step counter 4 */ + struct bmi2_feature_config step_count_config = { 0, 0, 0 }; + + /* Copy the feature configuration address to a local pointer */ + uint16_t *data_p = (uint16_t *) (void *)feat_config; + + /* Search for step counter feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_count_config, BMI2_STEP_COUNTER, dev); + if (feat_found) + { + /* Get the configuration from the page where step counter resides */ + rslt = bmi2_get_feat_config(step_count_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes */ + idx = step_count_config.start_addr; + + /* Get offset in words since all the features are set in words length */ + idx = idx / 2; + + /* Set water-mark level */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_STEP_COUNT_WM_LEVEL, config->watermark_level); + + /* Set reset-counter */ + *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_STEP_COUNT_RST_CNT, config->reset_counter); + + /* Increment offset by 1 word to set output + * configuration of step detector and step activity + */ + idx++; + + /* Set step buffer size */ + *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_STEP_BUFFER_SIZE, config->step_buffer_size); + + /* Increment offset by 1 more word to get the total length in words */ + idx++; + + /* Get total length in bytes to copy from local pointer to the array */ + idx = (uint8_t)(idx * 2) - step_count_config.start_addr; + + /* Copy the bytes to be set back to the array */ + for (index = 0; index < idx; index++) + { + feat_config[step_count_config.start_addr + + index] = *((uint8_t *) data_p + step_count_config.start_addr + index); + } + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets step counter parameter configurations. + */ +static int8_t get_step_count_params_config(uint16_t *step_count_params, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Variable to define LSB */ + uint16_t lsb = 0; + + /* Variable to define MSB */ + uint16_t msb = 0; + + /* Variable to define a word */ + uint16_t lsb_msb = 0; + + /* Initialize feature configuration for step counter 1 */ + struct bmi2_feature_config step_params_config = { 0, 0, 0 }; + + /* Variable to index the page number */ + uint8_t page_idx; + + /* Variable to define the start page */ + uint8_t start_page; + + /* Variable to define start address of the parameters */ + uint8_t start_addr; + + /* Variable to define number of bytes */ + uint8_t n_bytes = (BMI2_STEP_CNT_N_PARAMS * 2); + + /* Variable to store number of pages */ + uint8_t n_pages = (n_bytes / 16); + + /* Variable to define the end page */ + uint8_t end_page; + + /* Variable to define the remaining bytes to be read */ + uint8_t remain_len; + + /* Variable to define the maximum words to be read in a page */ + uint8_t max_len = BMI2_FEAT_SIZE_IN_BYTES; + + /* Variable index bytes in a page */ + uint8_t page_byte_idx; + + /* Variable to index the parameters */ + uint8_t param_idx = 0; + + /* Search for step counter parameter feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_params_config, BMI2_STEP_COUNTER_PARAMS, dev); + if (feat_found) + { + /* Get the start page for the step counter parameters */ + start_page = step_params_config.page; + + /* Get the end page for the step counter parameters */ + end_page = start_page + n_pages; + + /* Get the start address for the step counter parameters */ + start_addr = step_params_config.start_addr; + + /* Get the remaining length of bytes to be read */ + remain_len = (uint8_t)((n_bytes - (n_pages * 16)) + start_addr); + for (page_idx = start_page; page_idx <= end_page; page_idx++) + { + /* Get the configuration from the respective page */ + rslt = bmi2_get_feat_config(page_idx, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Start from address 0x00 when switched to next page */ + if (page_idx > start_page) + { + start_addr = 0; + } + + /* Remaining number of bytes to be read in the page */ + if (page_idx == end_page) + { + max_len = remain_len; + } + + /* Get the offset */ + page_byte_idx = start_addr; + while (page_byte_idx < max_len) + { + /* Get word to calculate the parameter*/ + lsb = (uint16_t) feat_config[page_byte_idx++]; + if (page_byte_idx < max_len) + { + msb = ((uint16_t) feat_config[page_byte_idx++] << 8); + } + + lsb_msb = lsb | msb; + + /* Get parameters 1 to 25 */ + step_count_params[param_idx] = lsb_msb & BMI2_STEP_COUNT_PARAMS_MASK; + + /* Increment to next parameter */ + param_idx++; + } + } + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets step counter/detector/activity configurations. + */ +static int8_t get_step_config(struct bmi2_step_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to define LSB */ + uint16_t lsb = 0; + + /* Variable to define MSB */ + uint16_t msb = 0; + + /* Variable to define a word */ + uint16_t lsb_msb = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for step counter */ + struct bmi2_feature_config step_count_config = { 0, 0, 0 }; + + /* Search for step counter 4 feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_count_config, BMI2_STEP_COUNTER, dev); + if (feat_found) + { + /* Get the configuration from the page where step counter 4 parameter resides */ + rslt = bmi2_get_feat_config(step_count_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for feature enable for step counter/detector/activity */ + idx = step_count_config.start_addr; + + /* Get word to calculate water-mark level and reset counter */ + lsb = (uint16_t) feat_config[idx++]; + msb = ((uint16_t) feat_config[idx++] << 8); + lsb_msb = lsb | msb; + + /* Get water-mark level */ + config->watermark_level = lsb_msb & BMI2_STEP_COUNT_WM_LEVEL_MASK; + + /* Get reset counter */ + config->reset_counter = (lsb_msb & BMI2_STEP_COUNT_RST_CNT_MASK) >> BMI2_STEP_COUNT_RST_CNT_POS; + + /* Get word to calculate output configuration of step detector and activity */ + lsb = (uint16_t) feat_config[idx++]; + msb = ((uint16_t) feat_config[idx++] << 8); + lsb_msb = lsb | msb; + + config->step_buffer_size = (lsb_msb & BMI2_STEP_BUFFER_SIZE_MASK) >> BMI2_STEP_BUFFER_SIZE_POS; + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets the output values of step counter. + */ +static int8_t get_step_counter_output(uint32_t *step_count, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variables to define index */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature output for step counter */ + struct bmi2_feature_config step_cnt_out_config = { 0, 0, 0 }; + + /* Search for step counter output feature and extract its configuration details */ + feat_found = extract_output_feat_config(&step_cnt_out_config, BMI2_STEP_COUNTER, dev); + if (feat_found) + { + /* Get the feature output configuration for step-counter */ + rslt = bmi2_get_feat_config(step_cnt_out_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for step counter output */ + idx = step_cnt_out_config.start_addr; + + /* Get the step counter output in 4 bytes */ + *step_count = (uint32_t) feat_config[idx++]; + *step_count |= ((uint32_t) feat_config[idx++] << 8); + *step_count |= ((uint32_t) feat_config[idx++] << 16); + *step_count |= ((uint32_t) feat_config[idx++] << 24); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets the error status related to NVM. + */ +static int8_t get_nvm_error_status(struct bmi2_nvm_err_status *nvm_err_stat, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variables to define index */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature output for NVM error status */ + struct bmi2_feature_config nvm_err_cfg = { 0, 0, 0 }; + + /* Search for NVM error status feature and extract its configuration details */ + feat_found = extract_output_feat_config(&nvm_err_cfg, BMI2_NVM_STATUS, dev); + if (feat_found) + { + /* Get the feature output configuration for NVM error status */ + rslt = bmi2_get_feat_config(nvm_err_cfg.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for NVM error status */ + idx = nvm_err_cfg.start_addr; + + /* Increment index to get the error status */ + idx++; + + /* Error when NVM load action fails */ + nvm_err_stat->load_error = BMI2_GET_BIT_POS0(feat_config[idx], BMI2_NVM_LOAD_ERR_STATUS); + + /* Error when NVM program action fails */ + nvm_err_stat->prog_error = BMI2_GET_BITS(feat_config[idx], BMI2_NVM_PROG_ERR_STATUS); + + /* Error when NVM erase action fails */ + nvm_err_stat->erase_error = BMI2_GET_BITS(feat_config[idx], BMI2_NVM_ERASE_ERR_STATUS); + + /* Error when NVM program limit is exceeded */ + nvm_err_stat->exceed_error = BMI2_GET_BITS(feat_config[idx], BMI2_NVM_END_EXCEED_STATUS); + + /* Error when NVM privilege mode is not acquired */ + nvm_err_stat->privil_error = BMI2_GET_BITS(feat_config[idx], BMI2_NVM_PRIV_ERR_STATUS); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API is used to get enable status of gyroscope user gain + * update. + */ +static int8_t get_user_gain_upd_status(uint8_t *status, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Variable to check APS status */ + uint8_t aps_stat = 0; + + /* Initialize feature configuration for gyroscope user gain */ + struct bmi2_feature_config gyr_user_gain_cfg = { 0, 0, 0 }; + + /* Search for user gain feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&gyr_user_gain_cfg, BMI2_GYRO_GAIN_UPDATE, dev); + if (feat_found) + { + /* Disable advance power save */ + aps_stat = dev->aps_status; + if (aps_stat == BMI2_ENABLE) + { + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + if (rslt == BMI2_OK) + { + /* Get the configuration from the page where user gain feature resides */ + rslt = bmi2_get_feat_config(gyr_user_gain_cfg.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of user gain */ + idx = gyr_user_gain_cfg.start_addr + BMI2_GYR_USER_GAIN_FEAT_EN_OFFSET; + + /* Set the feature enable status */ + *status = BMI2_GET_BITS(feat_config[idx], BMI2_GYR_USER_GAIN_FEAT_EN); + } + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + /* Enable Advance power save if disabled while configuring and not when already disabled */ + if ((rslt == BMI2_OK) && (aps_stat == BMI2_ENABLE)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + + return rslt; +} + +/*! + * @brief This internal API is used to parse and store the activity recognition + * output from the FIFO data. + */ +static int8_t unpack_act_recog_output(struct bmi2_act_recog_output *act_recog, + uint16_t *data_index, + const struct bmi2_fifo_frame *fifo) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Variables to define 4 bytes of sensor time */ + uint32_t time_stamp_byte4 = 0; + uint32_t time_stamp_byte3 = 0; + uint32_t time_stamp_byte2 = 0; + uint32_t time_stamp_byte1 = 0; + + /* Validate data index */ + if ((*data_index + BMI2_FIFO_VIRT_ACT_DATA_LENGTH) >= fifo->length) + { + /* Update the data index to the last byte */ + (*data_index) = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + } + else + { + /* Get time-stamp from the activity recognition frame */ + time_stamp_byte4 = ((uint32_t)(fifo->data[(*data_index) + 3]) << 24); + time_stamp_byte3 = ((uint32_t)(fifo->data[(*data_index) + 2]) << 16); + time_stamp_byte2 = fifo->data[(*data_index) + 1] << 8; + time_stamp_byte1 = fifo->data[(*data_index)]; + + /* Update time-stamp from the virtual frame */ + act_recog->time_stamp = (time_stamp_byte4 | time_stamp_byte3 | time_stamp_byte2 | time_stamp_byte1); + + /* Move the data index by 4 bytes */ + (*data_index) = (*data_index) + BMI2_FIFO_VIRT_ACT_TIME_LENGTH; + + /* Update the previous activity from the virtual frame */ + act_recog->prev_act = fifo->data[(*data_index)]; + + /* Move the data index by 1 byte */ + (*data_index) = (*data_index) + BMI2_FIFO_VIRT_ACT_TYPE_LENGTH; + + /* Update the current activity from the virtual frame */ + act_recog->curr_act = fifo->data[(*data_index)]; + + /* Move the data index by 1 byte */ + (*data_index) = (*data_index) + BMI2_FIFO_VIRT_ACT_STAT_LENGTH; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + } + + return rslt; +} + +/*! + * @brief This internal API gets the error status related to virtual frames. + */ +static int8_t get_vfrm_error_status(struct bmi2_vfrm_err_status *vfrm_err_stat, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variables to define index */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature output for VFRM error status */ + struct bmi2_feature_config vfrm_err_cfg = { 0, 0, 0 }; + + /* Search for VFRM error status feature and extract its configuration details */ + feat_found = extract_output_feat_config(&vfrm_err_cfg, BMI2_VFRM_STATUS, dev); + if (feat_found) + { + /* Get the feature output configuration for VFRM error status */ + rslt = bmi2_get_feat_config(vfrm_err_cfg.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for VFRM error status */ + idx = vfrm_err_cfg.start_addr; + + /* Increment index to get the error status */ + idx++; + + /* Internal error while acquiring lock for FIFO */ + vfrm_err_stat->lock_error = BMI2_GET_BITS(feat_config[idx], BMI2_VFRM_LOCK_ERR_STATUS); + + /* Internal error while writing byte into FIFO */ + vfrm_err_stat->write_error = BMI2_GET_BITS(feat_config[idx], BMI2_VFRM_WRITE_ERR_STATUS); + + /* Internal error while writing into FIFO */ + vfrm_err_stat->fatal_error = BMI2_GET_BITS(feat_config[idx], BMI2_VFRM_FATAL_ERR_STATUS); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API skips S4S frame in the FIFO data while getting + * step activity output. + */ +static int8_t move_if_s4s_frame(const uint8_t *frame_header, uint16_t *data_index, const struct bmi2_fifo_frame *fifo) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Variable to extract virtual header byte */ + uint8_t virtual_header_mode; + + /* Variable to define pay-load in words */ + uint8_t payload_word = 0; + + /* Variable to define pay-load in bytes */ + uint8_t payload_bytes = 0; + + /* Extract virtual header mode from the frame header */ + virtual_header_mode = BMI2_GET_BITS(*frame_header, BMI2_FIFO_VIRT_FRM_MODE); + + /* If the extracted header byte is a virtual header */ + if (virtual_header_mode == BMI2_FIFO_VIRT_FRM_MODE) + { + /* If frame header is not activity recognition header */ + if (*frame_header != 0xC8) + { + /* Extract pay-load in words from the header byte */ + payload_word = BMI2_GET_BITS(*frame_header, BMI2_FIFO_VIRT_PAYLOAD) + 1; + + /* Convert to bytes */ + payload_bytes = (uint8_t)(payload_word * 2); + + /* Move the data index by those pay-load bytes */ + rslt = move_next_frame(data_index, payload_bytes, fifo); + } + } + + return rslt; +} + +/*! + * @brief This internal API enables/disables compensation of the gain defined + * in the GAIN register. + */ +static int8_t enable_gyro_gain(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define register data */ + uint8_t reg_data = 0; + + rslt = bmi2_get_regs(BMI2_GYR_OFF_COMP_6_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + reg_data = BMI2_SET_BITS(reg_data, BMI2_GYR_GAIN_EN, enable); + rslt = bmi2_set_regs(BMI2_GYR_OFF_COMP_6_ADDR, ®_data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This internal API is used to extract the output feature configuration + * details from the look-up table. + */ +static uint8_t extract_output_feat_config(struct bmi2_feature_config *feat_output, + uint8_t type, + const struct bmi2_dev *dev) +{ + /* Variable to define loop */ + uint8_t loop = 0; + + /* Variable to set flag */ + uint8_t feat_found = BMI2_FALSE; + + /* Search for the output feature from the output configuration array */ + while (loop < dev->out_sens) + { + if (dev->feat_output[loop].type == type) + { + *feat_output = dev->feat_output[loop]; + feat_found = BMI2_TRUE; + break; + } + + loop++; + } + + /* Return flag */ + return feat_found; +} + +/*! + * @brief This internal API is used to move the data index ahead of the + * current_frame_length parameter when unnecessary FIFO data appears while + * extracting the user specified data. + */ +static int8_t move_next_frame(uint16_t *data_index, uint8_t current_frame_length, const struct bmi2_fifo_frame *fifo) +{ + /* Variables to define error */ + int8_t rslt = BMI2_OK; + + /* Validate data index */ + if (((*data_index) + current_frame_length) > fifo->length) + { + /* Move the data index to the last byte */ + (*data_index) = fifo->length; + + /* FIFO is empty */ + rslt = BMI2_W_FIFO_EMPTY; + } + else + { + /* Move the data index to next frame */ + (*data_index) = (*data_index) + current_frame_length; + + /* More frames could be read */ + rslt = BMI2_W_PARTIAL_READ; + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_hc.h b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_hc.h new file mode 100644 index 0000000000..2a1b5bc07b --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_hc.h @@ -0,0 +1,483 @@ +/** +* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. +* +* BSD-3-Clause +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* +* 3. Neither the name of the copyright holder nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, +* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING +* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +* @file bmi270_hc.h +* @date 2020-11-04 +* @version v2.63.1 +* +*/ + +/** + * \ingroup bmi2xy + * \defgroup bmi270_hc BMI270_HC + * @brief Sensor driver for BMI270_HC sensor + */ + +#ifndef BMI270_HC_H_ +#define BMI270_HC_H_ + +/*! CPP guard */ +#ifdef __cplusplus +extern "C" { +#endif + +/***************************************************************************/ + +/*! Header files + ****************************************************************************/ +#include "bmi2.h" + +/***************************************************************************/ + +/*! Macro definitions + ****************************************************************************/ + +/*! @name BMI270_HC Chip identifier */ +#define BMI270_HC_CHIP_ID UINT8_C(0x24) + +/*! @name BMI270_HC feature input start addresses */ +#define BMI270_HC_CONFIG_ID_STRT_ADDR UINT8_C(0x06) +#define BMI270_HC_STEP_CNT_1_STRT_ADDR UINT8_C(0x00) +#define BMI270_HC_STEP_CNT_4_STRT_ADDR UINT8_C(0x02) +#define BMI270_HC_MAX_BURST_LEN_STRT_ADDR UINT8_C(0x08) +#define BMI270_HC_CRT_GYRO_SELF_TEST_STRT_ADDR UINT8_C(0x09) +#define BMI270_HC_ABORT_STRT_ADDR UINT8_C(0x09) +#define BMI270_HC_NVM_PROG_PREP_STRT_ADDR UINT8_C(0x0A) +#define BMI270_HC_ACT_RGN_SETT_STRT_ADDR UINT8_C(0x02) +#define BMI270_HC_ACT_RGN_STRT_ADDR UINT8_C(0x00) + +/*! @name BMI270_HC feature output start addresses */ +#define BMI270_HC_STEP_CNT_OUT_STRT_ADDR UINT8_C(0x00) +#define BMI270_HC_GYR_USER_GAIN_OUT_STRT_ADDR UINT8_C(0x04) +#define BMI270_HC_GYRO_CROSS_SENSE_STRT_ADDR UINT8_C(0x0C) +#define BMI270_HC_NVM_VFRM_OUT_STRT_ADDR UINT8_C(0x0E) + +/*! @name Defines maximum number of pages */ +#define BMI270_HC_MAX_PAGE_NUM UINT8_C(8) + +/*! @name Defines maximum number of feature input configurations */ +#define BMI270_HC_MAX_FEAT_IN UINT8_C(10) + +/*! @name Defines maximum number of feature outputs */ +#define BMI270_HC_MAX_FEAT_OUT UINT8_C(5) + +/*! @name Mask definitions for feature interrupt status bits */ +#define BMI270_HC_STEP_CNT_STATUS_MASK UINT8_C(0x01) + +/*! @name Mask definitions for feature interrupt mapping bits */ +#define BMI270_HC_INT_STEP_COUNTER_MASK UINT8_C(0x01) +#define BMI270_HC_INT_STEP_DETECTOR_MASK UINT8_C(0x01) + +/*! @name Defines maximum number of feature interrupts */ +#define BMI270_HC_MAX_INT_MAP UINT8_C(2) + +/***************************************************************************/ + +/*! BMI270_HC User Interface function prototypes + ****************************************************************************/ + +/** + * \ingroup bmi270_hc + * \defgroup bmi270_hcApiInit Initialization + * @brief Initialize the sensor and device structure + */ + +/*! + * \ingroup bmi270_hcApiInit + * \page bmi270_hc_api_bmi270_hc_init bmi270_hc_init + * \code + * int8_t bmi270_hc_init(struct bmi2_dev *dev); + * \endcode + * @details This API: + * 1) updates the device structure with address of the configuration file. + * 2) Initializes BMI270_HC sensor. + * 3) Writes the configuration file. + * 4) Updates the feature offset parameters in the device structure. + * 5) Updates the maximum number of pages, in the device structure. + * + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_hc_init(struct bmi2_dev *dev); + +/** + * \ingroup bmi270_hc + * \defgroup bmi270_hcApiSensor Feature Set + * @brief Enable / Disable features of the sensor + */ + +/*! + * \ingroup bmi270_hcApiSensor + * \page bmi270_hc_api_bmi270_hc_sensor_enable bmi270_hc_sensor_enable + * \code + * int8_t bmi270_hc_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); + * \endcode + * @details This API selects the sensors/features to be enabled. + * + * @param[in] sens_list : Pointer to select the sensor/feature. + * @param[in] n_sens : Number of sensors selected. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @note Sensors/features that can be enabled. + * + *@verbatim + * sens_list | Values + * ----------------------------|----------- + * BMI2_ACCEL | 0 + * BMI2_GYRO | 1 + * BMI2_AUX | 2 + * BMI2_STEP_DETECTOR | 6 + * BMI2_STEP_COUNTER | 7 + * BMI2_GYRO_GAIN_UPDATE | 9 + * BMI2_ACTIVITY_RECOGNITION | 34 + *@endverbatim + * + * @note : + * example uint8_t sens_list[2] = {BMI2_ACCEL, BMI2_GYRO}; + * uint8_t n_sens = 2; + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_hc_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); + +/*! + * \ingroup bmi270_hcApiSensor + * \page bmi270_hc_api_bmi270_hc_sensor_disable bmi270_hc_sensor_disable + * \code + * int8_t bmi270_hc_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); + * \endcode + * @details This API selects the sensors/features to be disabled. + * + * @param[in] sens_list : Pointer to select the sensor/feature. + * @param[in] n_sens : Number of sensors selected. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @note Sensors/features that can be disabled. + * + *@verbatim + * sens_list | Values + * ----------------------------|----------- + * BMI2_ACCEL | 0 + * BMI2_GYRO | 1 + * BMI2_AUX | 2 + * BMI2_STEP_DETECTOR | 6 + * BMI2_STEP_COUNTER | 7 + * BMI2_GYRO_GAIN_UPDATE | 9 + * BMI2_ACTIVITY_RECOGNITION | 34 + *@endverbatim + * + * @note : + * example uint8_t sens_list[2] = {BMI2_ACCEL, BMI2_GYRO}; + * uint8_t n_sens = 2; + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_hc_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); + +/** + * \ingroup bmi270_hc + * \defgroup bmi270_hcApiSensorC Sensor Configuration + * @brief Enable / Disable feature configuration of the sensor + */ + +/*! + * \ingroup bmi270_hcApiSensorC + * \page bmi270_hc_api_bmi270_hc_set_sensor_config bmi270_hc_set_sensor_config + * \code + * int8_t bmi270_hc_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); + * \endcode + * @details This API sets the sensor/feature configuration. + * + * @param[in] sens_cfg : Structure instance of bmi2_sens_config. + * @param[in] n_sens : Number of sensors selected. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @note Sensors/features that can be configured + * + *@verbatim + * sens_list | Values + * ----------------------------|----------- + * BMI2_STEP_DETECTOR | 6 + * BMI2_STEP_COUNTER | 7 + * BMI2_STEP_COUNTER_PARAMS | 28 + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_hc_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); + +/*! + * \ingroup bmi270_hcApiSensorC + * \page bmi270_hc_api_bmi270_hc_get_sensor_config bmi270_hc_get_sensor_config + * \code + * int8_t bmi270_hc_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); + * \endcode + * @details This API gets the sensor/feature configuration. + * + * @param[in] sens_cfg : Structure instance of bmi2_sens_config. + * @param[in] n_sens : Number of sensors selected. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @note Sensors/features whose configurations can be read. + * + *@verbatim + * sens_list | Values + * ----------------------------|----------- + * BMI2_STEP_DETECTOR | 6 + * BMI2_STEP_COUNTER | 7 + * BMI2_STEP_COUNTER_PARAMS | 28 + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_hc_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); + +/** + * \ingroup bmi270_hc + * \defgroup bmi270_hcApiSensorD Sensor Data + * @brief Get sensor data + */ + +/*! + * \ingroup bmi270_hcApiSensorD + * \page bmi270_hc_api_bmi270_hc_get_sensor_data bmi270_hc_get_sensor_data + * \code + * int8_t bmi270_hc_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev); + * \endcode + * @details This API gets the sensor/feature data for accelerometer, gyroscope, + * auxiliary sensor, step counter, high-g, gyroscope user-gain update, + * orientation, gyroscope cross sensitivity and error status for NVM and VFRM. + * + * @param[out] sensor_data : Structure instance of bmi2_sensor_data. + * @param[in] n_sens : Number of sensors selected. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @note Sensors/features whose data can be read + * + *@verbatim + * sens_list | Values + * ---------------------|----------- + * BMI2_STEP_COUNTER | 7 + * BMI2_NVM_STATUS | 38 + * BMI2_VFRM_STATUS | 39 + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_hc_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev); + +/** + * \ingroup bmi270_hc + * \defgroup bmi270_hcApiARecoghc Activity recognition settings + * @brief Set / Get activity recognition settings of bmi270hc sensor + */ + +/*! + * \ingroup bmi270_hcApiARecoghc + * \page bmi270_hc_api_bmi270_hc_get_act_recg_sett bmi270_hc_get_act_recg_sett + * \code + * int8_t bmi270_hc_get_act_recg_sett(struct bmi2_hc_act_recg_sett *sett, struct bmi2_dev *dev); + * \endcode + * @details This api is used for retrieving the following activity recognition settings currently set. + * + * @param[in] sett : Structure instance of bmi2_hc_act_recg_sett. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_hc_get_act_recg_sett(struct bmi2_hc_act_recg_sett *sett, struct bmi2_dev *dev); + +/*! + * \ingroup bmi270_hcApiARecoghc + * \page bmi270_hc_api_bmi270_hc_set_act_recg_sett bmi270_hc_set_act_recg_sett + * \code + * int8_t bmi270_hc_set_act_recg_sett(const struct bmi2_hc_act_recg_sett *sett, struct bmi2_dev *dev); + * \endcode + * @details This api is used for setting the following activity recognition settings + * + * @param[in] sett : Structure instance of bmi2_hc_act_recg_sett. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_hc_set_act_recg_sett(const struct bmi2_hc_act_recg_sett *sett, struct bmi2_dev *dev); + +/** + * \ingroup bmi270_hc + * \defgroup bmi270_hcApiactOut Activity Output + * @brief Activity output operations of the sensor + */ + +/*! + * \ingroup bmi270_hcApiactOut + * \page bmi270_hc_api_bmi270_hc_get_act_recog_output bmi270_hc_get_act_recog_output + * \code + * int8_t bmi270_hc_get_act_recog_output(struct bmi2_act_recog_output *act_recog, + * uint16_t *act_frm_len, + * struct bmi2_fifo_frame *fifo, + * const struct bmi2_dev *dev); + * + * \endcode + * @details This internal API is used to parse the activity output from the + * FIFO in header mode. + * + * @param[out] act_recog : Pointer to buffer where the parsed activity data + * bytes are stored. + * @param[in] act_frm_len : Number of activity frames parsed. + * @param[in] fifo : Structure instance of bmi2_fifo_frame. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @verbatim + * ---------------------------------------------------------------------------- + * bmi2_act_rec_output | + * Structure parameters | Description + *--------------------------|-------------------------------------------------- + * time_stamp | time-stamp (expressed in 50Hz ticks) + * -------------------------|--------------------------------------------------- + * type | Type of activity + * -------------------------|--------------------------------------------------- + * stat | Activity status + * -------------------------|--------------------------------------------------- + * @endverbatim + * + *@verbatim + * type | Activities + *----------|--------------------- + * 0 | UNKNOWN + * 1 | STILL + * 2 | WALK + * 3 | RUN + * 4 | BIKE + * 5 | VEHICLE + * 6 | TILTED + *@endverbatim + * + * + *@verbatim + * stat | Activity status + *----------|--------------------- + * 1 | START + * 2 | END + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_hc_get_act_recog_output(struct bmi2_act_recog_output *act_recog, + uint16_t *act_frm_len, + struct bmi2_fifo_frame *fifo, + const struct bmi2_dev *dev); + +/** + * \ingroup bmi270_hc + * \defgroup bmi270_hcApiGyroUG Gyro User Gain + * @brief Set / Get Gyro User Gain of the sensor + */ + +/*! + * \ingroup bmi270_hcApiGyroUG + * \page bmi270_hc_api_bmi270_hc_update_gyro_user_gain bmi270_hc_update_gyro_user_gain + * \code + * int8_t bmi270_hc_update_gyro_user_gain(const struct bmi2_gyro_user_gain_config *user_gain, struct bmi2_dev *dev); + * \endcode + * @details This API updates the gyroscope user-gain. + * + * @param[in] user_gain : Structure that stores user-gain configurations. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_hc_update_gyro_user_gain(const struct bmi2_gyro_user_gain_config *user_gain, struct bmi2_dev *dev); + +/*! + * \ingroup bmi270_hcApiGyroUG + * \page bmi270_hc_api_bmi270_hc_read_gyro_user_gain bmi270_hc_read_gyro_user_gain + * \code + * int8_t bmi270_hc_read_gyro_user_gain(struct bmi2_gyro_user_gain_data *gyr_usr_gain, const struct bmi2_dev *dev); + * \endcode + * @details This API reads the compensated gyroscope user-gain values. + * + * @param[out] gyr_usr_gain : Structure that stores gain values. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_hc_read_gyro_user_gain(struct bmi2_gyro_user_gain_data *gyr_usr_gain, struct bmi2_dev *dev); + +/*! + * \ingroup bmi270_hcApiInt + * \page bmi270_hc_api_bmi270_hc_map_feat_int bmi270_hc_map_feat_int + * \code + * int8_t bmi270_hc_map_feat_int(const struct bmi2_sens_int_config *sens_int, uint8_t n_sens, struct bmi2_dev *dev) + * \endcode + * @details This API maps/unmaps feature interrupts to that of interrupt pins. + * + * @param[in] sens_int : Structure instance of bmi2_sens_int_config. + * @param[in] n_sens : Number of interrupts to be mapped. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_hc_map_feat_int(const struct bmi2_sens_int_config *sens_int, uint8_t n_sens, struct bmi2_dev *dev); + +/******************************************************************************/ +/*! @name C++ Guard Macros */ +/******************************************************************************/ +#ifdef __cplusplus +} +#endif /* End of CPP guard */ + +#endif /* BMI270_HC_H_ */ diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_maximum_fifo.c b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_maximum_fifo.c new file mode 100644 index 0000000000..c97e4bdd8b --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_maximum_fifo.c @@ -0,0 +1,212 @@ +/** +* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. +* +* BSD-3-Clause +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* +* 3. Neither the name of the copyright holder nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, +* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING +* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +* @file bmi270_maximum_fifo.c +* @date 2020-11-04 +* @version v2.63.1 +* +*/ + +/***************************************************************************/ + +/*! Header files + ****************************************************************************/ +#include "bmi270_maximum_fifo.h" + +/***************************************************************************/ + +/*! Global Variable + ****************************************************************************/ + +/*! @name Global array that stores the configuration file of BMI270 */ +const uint8_t bmi270_maximum_fifo_config_file[] = { + 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x1a, 0x00, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, + 0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0x90, 0x32, 0x21, 0x2e, 0x59, 0xf5, + 0x10, 0x30, 0x21, 0x2e, 0x6a, 0xf5, 0x1a, 0x24, 0x22, 0x00, 0x80, 0x2e, 0x3b, 0x00, 0xc8, 0x2e, 0x44, 0x47, 0x22, + 0x00, 0x37, 0x00, 0xa4, 0x00, 0xff, 0x0f, 0xd1, 0x00, 0x07, 0xad, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x11, 0x24, 0xfc, 0xf5, 0x80, 0x30, 0x40, 0x42, 0x50, 0x50, 0x00, 0x30, 0x12, 0x24, 0xeb, + 0x00, 0x03, 0x30, 0x00, 0x2e, 0xc1, 0x86, 0x5a, 0x0e, 0xfb, 0x2f, 0x21, 0x2e, 0xfc, 0xf5, 0x13, 0x24, 0x63, 0xf5, + 0xe0, 0x3c, 0x48, 0x00, 0x22, 0x30, 0xf7, 0x80, 0xc2, 0x42, 0xe1, 0x7f, 0x3a, 0x25, 0xfc, 0x86, 0xf0, 0x7f, 0x41, + 0x33, 0x98, 0x2e, 0xc2, 0xc4, 0xd6, 0x6f, 0xf1, 0x30, 0xf1, 0x08, 0xc4, 0x6f, 0x11, 0x24, 0xff, 0x03, 0x12, 0x24, + 0x00, 0xfc, 0x61, 0x09, 0xa2, 0x08, 0x36, 0xbe, 0x2a, 0xb9, 0x13, 0x24, 0x38, 0x00, 0x64, 0xbb, 0xd1, 0xbe, 0x94, + 0x0a, 0x71, 0x08, 0xd5, 0x42, 0x21, 0xbd, 0x91, 0xbc, 0xd2, 0x42, 0xc1, 0x42, 0x00, 0xb2, 0xfe, 0x82, 0x05, 0x2f, + 0x50, 0x30, 0x21, 0x2e, 0x21, 0xf2, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0xf0, 0x6f, 0x02, 0x30, 0x02, 0x42, 0x20, + 0x26, 0xe0, 0x6f, 0x02, 0x31, 0x03, 0x40, 0x9a, 0x0a, 0x02, 0x42, 0xf0, 0x37, 0x05, 0x2e, 0x5e, 0xf7, 0x10, 0x08, + 0x12, 0x24, 0x1e, 0xf2, 0x80, 0x42, 0x83, 0x84, 0xf1, 0x7f, 0x0a, 0x25, 0x13, 0x30, 0x83, 0x42, 0x3b, 0x82, 0xf0, + 0x6f, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0x12, 0x40, 0x52, 0x42, 0x00, 0x2e, 0x12, 0x40, 0x52, 0x42, 0x3e, 0x84, + 0x00, 0x40, 0x40, 0x42, 0x7e, 0x82, 0xe1, 0x7f, 0xf2, 0x7f, 0x98, 0x2e, 0x6a, 0xd6, 0x21, 0x30, 0x23, 0x2e, 0x61, + 0xf5, 0xeb, 0x2c, 0xe1, 0x6f +}; + +// The rest of this is not needed, avoid compiler errors due to pedantic settings +#if false + +/*! @name Global array that stores the feature input configuration of BMI270 */ +const struct bmi2_feature_config bmi270_maximum_fifo_feat_in[BMI270_MAXIMUM_FIFO_MAX_FEAT_IN] = { + +}; + +/*! @name Global array that stores the feature output configuration */ +const struct bmi2_feature_config bmi270_maximum_fifo_feat_out[BMI270_MAXIMUM_FIFO_MAX_FEAT_OUT] = { + +}; + +/******************************************************************************/ + +/*! Local Function Prototypes + ******************************************************************************/ + +/*! + * @brief This internal API is used to validate the device pointer for + * null conditions. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t null_ptr_check(const struct bmi2_dev *dev); + +/***************************************************************************/ + +/*! User Interface Definitions + ****************************************************************************/ + +/*! + * @brief This API: + * 1) updates the device structure with address of the configuration file. + * 2) Initializes BMI270 sensor. + * 3) Writes the configuration file. + * 4) Updates the feature offset parameters in the device structure. + * 5) Updates the maximum number of pages, in the device structure. + */ +int8_t bmi270_maximum_fifo_init(struct bmi2_dev *dev) +{ + /* Variable to define result */ + int8_t rslt; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + + if (rslt == BMI2_OK) + { + /* Assign chip id of BMI270 */ + dev->chip_id = BMI270_MAXIMUM_FIFO_CHIP_ID; + + dev->config_size = sizeof(bmi270_maximum_fifo_config_file); + + /* Enable the variant specific features if any */ + dev->variant_feature = BMI2_GYRO_CROSS_SENS_ENABLE | BMI2_MAXIMUM_FIFO_VARIANT; + + /* An extra dummy byte is read during SPI read */ + if (dev->intf == BMI2_SPI_INTF) + { + dev->dummy_byte = 1; + } + else + { + dev->dummy_byte = 0; + } + + /* If configuration file pointer is not assigned any address */ + if (!dev->config_file_ptr) + { + /* Give the address of the configuration file array to + * the device pointer + */ + dev->config_file_ptr = bmi270_maximum_fifo_config_file; + } + + /* Initialize BMI2 sensor */ + rslt = bmi2_sec_init(dev); + + if (rslt == BMI2_OK) + { + /* Assign the offsets of the feature input + * configuration to the device structure + */ + dev->feat_config = bmi270_maximum_fifo_feat_in; + + /* Assign the offsets of the feature output to + * the device structure + */ + dev->feat_output = bmi270_maximum_fifo_feat_out; + + /* Assign the maximum number of pages to the + * device structure + */ + dev->page_max = BMI270_MAXIMUM_FIFO_MAX_PAGE_NUM; + + /* Assign maximum number of input sensors/ + * features to device structure + */ + dev->input_sens = BMI270_MAXIMUM_FIFO_MAX_FEAT_IN; + + /* Assign maximum number of output sensors/ + * features to device structure + */ + dev->out_sens = BMI270_MAXIMUM_FIFO_MAX_FEAT_OUT; + + /* Get the gyroscope cross axis sensitivity */ + rslt = bmi2_get_gyro_cross_sense(dev); + } + } + + return rslt; +} + +/***************************************************************************/ + +/*! Local Function Definitions + ****************************************************************************/ + +/*! + * @brief This internal API is used to validate the device structure pointer for + * null conditions. + */ +static int8_t null_ptr_check(const struct bmi2_dev *dev) +{ + /* Variable to define result */ + int8_t rslt = BMI2_OK; + + if ((dev == NULL) || (dev->read == NULL) || (dev->write == NULL) || (dev->delay_us == NULL)) + { + /* Device structure pointer is not valid */ + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} +#endif diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_maximum_fifo.h b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_maximum_fifo.h new file mode 100644 index 0000000000..e912a96a63 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_maximum_fifo.h @@ -0,0 +1,117 @@ +/** +* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. +* +* BSD-3-Clause +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* +* 3. Neither the name of the copyright holder nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, +* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING +* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +* @file bmi270_maximum_fifo.h +* @date 2020-11-04 +* @version v2.63.1 +* +*/ + +/** + * \ingroup bmi2xy + * \defgroup bmi270_maximum_fifo BMI270_MAXIMUM_FIFO + * @brief Sensor driver for BMI270_MAXIMUM_FIFO sensor + */ + +#ifndef BMI270_MAXIMUM_FIFO_H_ +#define BMI270_MAXIMUM_FIFO_H_ + +/*! CPP guard */ +#ifdef __cplusplus +extern "C" { +#endif + +/***************************************************************************/ + +/*! Header files + ****************************************************************************/ +#include "bmi2.h" + +/***************************************************************************/ + +/*! Macro definitions + ****************************************************************************/ + +/*! @name BMI270 Chip identifier */ +#define BMI270_MAXIMUM_FIFO_CHIP_ID UINT8_C(0x24) + +/*! @name Defines maximum number of pages */ +#define BMI270_MAXIMUM_FIFO_MAX_PAGE_NUM UINT8_C(0) + +/*! @name Defines maximum number of feature input configurations */ +#define BMI270_MAXIMUM_FIFO_MAX_FEAT_IN UINT8_C(0) + +/*! @name Defines maximum number of feature outputs */ +#define BMI270_MAXIMUM_FIFO_MAX_FEAT_OUT UINT8_C(0) + +/*! @name Mask definitions for feature interrupt status bits */ + +/***************************************************************************/ + +/*! BMI270 User Interface function prototypes + ****************************************************************************/ + +/** + * \ingroup bmi270_maximum_fifo + * \defgroup bmi270_maximum_fifoApiInit Initialization + * @brief Initialize the sensor and device structure + */ + +/*! + * \ingroup bmi270_maximum_fifoApiInit + * \page bmi270_maximum_fifo_api_bmi270_maximum_fifo_init bmi270_maximum_fifo_init + * \code + * int8_t bmi270_maximum_fifo_init(struct bmi2_dev *dev); + * \endcode + * @details This API: + * 1) updates the device structure with address of the configuration file. + * 2) Initializes BMI270 sensor. + * 3) Writes the configuration file. + * 4) Updates the feature offset parameters in the device structure. + * 5) Updates the maximum number of pages, in the device structure. + * + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_maximum_fifo_init(struct bmi2_dev *dev); + +/******************************************************************************/ +/*! @name C++ Guard Macros */ +/******************************************************************************/ +#ifdef __cplusplus +} +#endif /* End of CPP guard */ + +#endif /* BMI270_MAXIMUM_FIFO_H_ */ diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_wh.c b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_wh.c new file mode 100644 index 0000000000..eda23cc022 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_wh.c @@ -0,0 +1,3431 @@ +/** +* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. +* +* BSD-3-Clause +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* +* 3. Neither the name of the copyright holder nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, +* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING +* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +* @file bmi270_wh.c +* @date 2020-11-04 +* @version v2.63.1 +* +*/ + +/***************************************************************************/ + +/*! Header files + ****************************************************************************/ +#include "bmi270_wh.h" + +/***************************************************************************/ + +/*! Global Variable + ****************************************************************************/ + +/*! @name Global array that stores the configuration file of BMI270_WH */ +const uint8_t bmi270_wh_config_file[] = { + 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0xf8, 0x02, 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0xe1, 0x00, 0x80, 0x2e, 0xc0, + 0x02, 0x80, 0x2e, 0xe2, 0x00, 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x2c, 0x02, 0x80, 0x2e, 0xe4, 0x07, 0x59, 0xf5, + 0x10, 0x30, 0x21, 0x2e, 0x6a, 0xf5, 0x80, 0x2e, 0xdd, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3e, 0x31, 0x01, 0x00, 0x22, + 0x00, 0x80, 0x00, 0xfa, 0x07, 0xff, 0x0f, 0xd1, 0x00, 0x65, 0x9d, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, + 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, + 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0xfd, 0x2d, 0x7b, 0xaf, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x03, 0x00, 0x18, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x01, 0x12, + 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x1a, 0x24, 0x22, 0x00, 0x80, 0x2e, 0x8e, 0x01, 0xc8, 0x2e, 0xc8, 0x2e, 0xbb, 0x52, + 0x00, 0x2e, 0x60, 0x40, 0x41, 0x40, 0x0d, 0xbc, 0x98, 0xbc, 0xc0, 0x2e, 0x01, 0x0a, 0x0f, 0xb8, 0x43, 0x86, 0x25, + 0x40, 0x04, 0x40, 0xd8, 0xbe, 0x2c, 0x0b, 0x22, 0x11, 0x54, 0x42, 0x03, 0x80, 0x4b, 0x0e, 0xf6, 0x2f, 0xb8, 0x2e, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0xfd, 0x2d, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3e, 0x31, 0x00, 0x00, + 0x88, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0xe0, 0xaa, 0x38, 0x05, 0xe0, 0x90, 0x30, 0x04, 0x00, 0xee, + 0x06, 0xf2, 0x05, 0x80, 0x80, 0x16, 0xf1, 0x02, 0x02, 0x2d, 0x01, 0xd4, 0x7b, 0x3b, 0x01, 0xdb, 0x7a, 0x04, 0x00, + 0x3f, 0x7b, 0xcd, 0x6c, 0xc3, 0x04, 0x85, 0x09, 0xc3, 0x04, 0xec, 0xe6, 0x0c, 0x46, 0x01, 0x00, 0x27, 0x00, 0x19, + 0x00, 0x96, 0x00, 0xa0, 0x00, 0x01, 0x00, 0x0c, 0x00, 0xf0, 0x3c, 0x00, 0x01, 0x01, 0x00, 0x03, 0x00, 0x01, 0x00, + 0x0e, 0x00, 0x00, 0x00, 0x32, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x92, 0x00, 0x98, 0xf1, 0xaf, 0x00, 0xae, 0x00, 0x1e, + 0xf2, 0x19, 0xf4, 0x66, 0xf5, 0x00, 0x04, 0xff, 0x00, 0x64, 0xf5, 0x96, 0x00, 0xaa, 0x00, 0x86, 0x00, 0x99, 0x00, + 0x88, 0x00, 0x8a, 0x00, 0xff, 0x3f, 0xff, 0xfb, 0x31, 0x01, 0x00, 0x38, 0x00, 0x30, 0xfd, 0xf5, 0xb2, 0x00, 0xff, + 0x03, 0x00, 0xfc, 0xf0, 0x3f, 0x0b, 0x01, 0x0e, 0x01, 0x10, 0x01, 0xb9, 0x00, 0x2d, 0xf5, 0xca, 0xf5, 0xb0, 0x00, + 0x20, 0xf2, 0xb2, 0x00, 0xff, 0x1f, 0x00, 0x40, 0x80, 0x2e, 0xc0, 0x08, 0x1d, 0x6c, 0xad, 0x00, 0x59, 0x01, 0x31, + 0xd1, 0xff, 0x7f, 0x00, 0x08, 0xee, 0x7a, 0xe4, 0x0c, 0x12, 0x02, 0xb3, 0x00, 0xb4, 0x04, 0x62, 0x03, 0xc0, 0x02, + 0x1f, 0xf8, 0xe1, 0x07, 0xd3, 0x07, 0xd7, 0x00, 0xd3, 0x00, 0xb9, 0x00, 0xc3, 0x00, 0xc5, 0x00, 0xbd, 0x00, 0xbc, + 0x00, 0xbf, 0x00, 0xdd, 0x00, 0x95, 0x00, 0xc2, 0x00, 0xd6, 0x00, 0xf0, 0x00, 0x00, 0xff, 0x00, 0x80, 0x30, 0x50, + 0x98, 0x2e, 0xc3, 0xb0, 0x02, 0x30, 0x00, 0x30, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0x01, 0x30, 0x00, 0x2e, 0x41, + 0x82, 0x48, 0xa2, 0xfb, 0x2f, 0x03, 0x2e, 0x95, 0x00, 0x40, 0xb2, 0x1a, 0x2f, 0x05, 0x2e, 0x0a, 0x01, 0x91, 0x52, + 0x98, 0x2e, 0xc7, 0xc1, 0x05, 0x2e, 0x18, 0x00, 0x80, 0xb2, 0x02, 0x30, 0x10, 0x2f, 0xf0, 0x7f, 0x00, 0x2e, 0x91, + 0x50, 0x98, 0x2e, 0x4d, 0xc3, 0x91, 0x50, 0x98, 0x2e, 0x5a, 0xc7, 0x98, 0x2e, 0xbc, 0x03, 0x98, 0x2e, 0x00, 0xb0, + 0x10, 0x30, 0x21, 0x2e, 0x19, 0x00, 0xf0, 0x6f, 0x02, 0x30, 0x03, 0x2e, 0x85, 0x00, 0x40, 0xb2, 0x40, 0x2f, 0x03, + 0x2e, 0x85, 0x00, 0x03, 0x31, 0x4b, 0x08, 0x40, 0xb2, 0x07, 0x2f, 0xf0, 0x7f, 0x98, 0x2e, 0x47, 0xcb, 0x10, 0x30, + 0x21, 0x2e, 0x19, 0x00, 0xf0, 0x6f, 0x02, 0x30, 0x03, 0x2e, 0x85, 0x00, 0x43, 0x30, 0x4b, 0x08, 0x40, 0xb2, 0x2a, + 0x2f, 0x03, 0x2e, 0x90, 0x00, 0xf0, 0x7f, 0x40, 0xb2, 0x0e, 0x2f, 0x41, 0x90, 0x20, 0x2f, 0x05, 0x2e, 0x3f, 0x01, + 0x07, 0x2e, 0x91, 0x00, 0xa1, 0x32, 0x95, 0x58, 0x98, 0x2e, 0x97, 0xb3, 0x02, 0x30, 0x25, 0x2e, 0x90, 0x00, 0x15, + 0x2c, 0xf0, 0x6f, 0x93, 0x58, 0xa3, 0x32, 0x10, 0x41, 0x11, 0x41, 0x18, 0xb9, 0x04, 0x41, 0x98, 0xbc, 0x41, 0x0a, + 0x94, 0x0a, 0x98, 0x2e, 0xf1, 0x03, 0x12, 0x30, 0x21, 0x2e, 0x91, 0x00, 0x21, 0x2e, 0xaf, 0x00, 0x25, 0x2e, 0x90, + 0x00, 0xf0, 0x6f, 0x02, 0x30, 0x11, 0x30, 0x23, 0x2e, 0x19, 0x00, 0x25, 0x2e, 0x85, 0x00, 0x03, 0x2e, 0x19, 0x00, + 0x40, 0xb2, 0x22, 0x2f, 0xf0, 0x7f, 0x98, 0x2e, 0xf5, 0xcb, 0x97, 0x54, 0xbc, 0x84, 0x21, 0x2e, 0xae, 0x00, 0x81, + 0x40, 0x82, 0x86, 0x99, 0x54, 0x18, 0xb8, 0xc3, 0x40, 0x91, 0x42, 0x90, 0x42, 0x33, 0xbc, 0x90, 0x42, 0xe2, 0x7f, + 0xf0, 0x31, 0x82, 0x40, 0x10, 0x08, 0xf2, 0x6f, 0x25, 0xbd, 0x02, 0x0a, 0xd0, 0x7f, 0x98, 0x2e, 0xa8, 0xcf, 0x06, + 0xbc, 0xd2, 0x6f, 0xe1, 0x6f, 0x10, 0x0a, 0x40, 0x42, 0x98, 0x2e, 0x2c, 0x03, 0xf0, 0x6f, 0x02, 0x30, 0x25, 0x2e, + 0x19, 0x00, 0x25, 0x2e, 0x95, 0x00, 0x80, 0x2e, 0x93, 0x01, 0x90, 0x50, 0xf7, 0x7f, 0xe6, 0x7f, 0xd5, 0x7f, 0xc4, + 0x7f, 0xb3, 0x7f, 0xa1, 0x7f, 0x90, 0x7f, 0x82, 0x7f, 0x7b, 0x7f, 0x98, 0x2e, 0xe3, 0x00, 0x00, 0xb2, 0x65, 0x2f, + 0x05, 0x2e, 0x31, 0x01, 0x01, 0x2e, 0x11, 0x01, 0x03, 0x2e, 0x0f, 0x01, 0x8f, 0xb9, 0x23, 0xbe, 0x9f, 0xb8, 0xcb, + 0x0a, 0x24, 0xbc, 0x4f, 0xba, 0x03, 0x2e, 0x12, 0x01, 0x0f, 0xb8, 0x22, 0xbd, 0xdc, 0x0a, 0x2f, 0xb9, 0x9b, 0xbc, + 0x18, 0x0a, 0x9f, 0xb8, 0x82, 0x0a, 0x91, 0x0a, 0x25, 0x2e, 0x18, 0x00, 0x05, 0x2e, 0xc1, 0xf5, 0x2e, 0xbd, 0x2e, + 0xb9, 0x01, 0x2e, 0x18, 0x00, 0x31, 0x30, 0x8a, 0x04, 0x00, 0x90, 0x03, 0x2f, 0x01, 0x2e, 0x83, 0x00, 0x00, 0xb2, + 0x19, 0x2f, 0x9b, 0x50, 0x91, 0x52, 0x98, 0x2e, 0xec, 0x00, 0x05, 0x2e, 0x82, 0x00, 0x25, 0x2e, 0x95, 0x00, 0x05, + 0x2e, 0x82, 0x00, 0x80, 0x90, 0x02, 0x2f, 0x12, 0x30, 0x25, 0x2e, 0x82, 0x00, 0x01, 0x2e, 0x83, 0x00, 0x00, 0xb2, + 0x10, 0x30, 0x05, 0x2e, 0x18, 0x00, 0x01, 0x2f, 0x21, 0x2e, 0x18, 0x00, 0x25, 0x2e, 0x83, 0x00, 0x05, 0x2e, 0x18, + 0x00, 0x80, 0xb2, 0x20, 0x2f, 0x01, 0x2e, 0xc0, 0xf5, 0xf2, 0x30, 0x02, 0x08, 0x07, 0xaa, 0x73, 0x30, 0x03, 0x2e, + 0x84, 0x00, 0x18, 0x22, 0x41, 0x1a, 0x05, 0x2f, 0x03, 0x2e, 0x66, 0xf5, 0x9f, 0xbc, 0x9f, 0xb8, 0x40, 0x90, 0x0c, + 0x2f, 0x9d, 0x52, 0x03, 0x30, 0x53, 0x42, 0x2b, 0x30, 0x90, 0x04, 0x5b, 0x42, 0x21, 0x2e, 0x84, 0x00, 0x24, 0xbd, + 0x7e, 0x80, 0x81, 0x84, 0x43, 0x42, 0x02, 0x42, 0x02, 0x32, 0x25, 0x2e, 0x62, 0xf5, 0x9f, 0x52, 0x05, 0x2e, 0x0a, + 0x01, 0x91, 0x08, 0x00, 0x31, 0x21, 0x2e, 0x60, 0xf5, 0x80, 0xb2, 0x0b, 0x2f, 0xf2, 0x3e, 0x01, 0x2e, 0xca, 0xf5, + 0x82, 0x08, 0x25, 0x2e, 0xca, 0xf5, 0x05, 0x2e, 0x59, 0xf5, 0xe0, 0x3f, 0x90, 0x08, 0x25, 0x2e, 0x59, 0xf5, 0xa1, + 0x6f, 0xb3, 0x6f, 0xc4, 0x6f, 0xd5, 0x6f, 0xe6, 0x6f, 0xf7, 0x6f, 0x7b, 0x6f, 0x82, 0x6f, 0x90, 0x6f, 0x70, 0x5f, + 0xc8, 0x2e, 0xa0, 0x50, 0x80, 0x7f, 0xe7, 0x7f, 0xd5, 0x7f, 0xc4, 0x7f, 0xb3, 0x7f, 0xa2, 0x7f, 0x91, 0x7f, 0xf6, + 0x7f, 0x7b, 0x7f, 0x00, 0x2e, 0x01, 0x2e, 0x60, 0xf5, 0x60, 0x7f, 0x98, 0x2e, 0xe3, 0x00, 0x62, 0x6f, 0x01, 0x32, + 0x91, 0x08, 0x80, 0xb2, 0x0d, 0x2f, 0x00, 0xb2, 0x03, 0x2f, 0x05, 0x2e, 0x18, 0x00, 0x80, 0x90, 0x05, 0x2f, 0xa3, + 0x56, 0x02, 0x30, 0xc1, 0x42, 0xc2, 0x86, 0x00, 0x2e, 0xc2, 0x42, 0x23, 0x2e, 0x60, 0xf5, 0x00, 0x90, 0x00, 0x30, + 0x06, 0x2f, 0x21, 0x2e, 0x82, 0x00, 0xa1, 0x50, 0x21, 0x2e, 0x5a, 0xf2, 0x98, 0x2e, 0x3d, 0x03, 0xf6, 0x6f, 0x80, + 0x6f, 0x91, 0x6f, 0xa2, 0x6f, 0xb3, 0x6f, 0xc4, 0x6f, 0xd5, 0x6f, 0xe7, 0x6f, 0x7b, 0x6f, 0x60, 0x5f, 0xc8, 0x2e, + 0xc0, 0x50, 0xe7, 0x7f, 0xf6, 0x7f, 0x26, 0x30, 0x0f, 0x2e, 0x61, 0xf5, 0x2f, 0x2e, 0x85, 0x00, 0x0f, 0x2e, 0x85, + 0x00, 0xbe, 0x09, 0xd5, 0x7f, 0xc4, 0x7f, 0xb3, 0x7f, 0xa2, 0x7f, 0x90, 0x7f, 0x7b, 0x7f, 0x80, 0xb3, 0x81, 0x7f, + 0x11, 0x2f, 0xa5, 0x50, 0x1a, 0x25, 0x12, 0x40, 0x42, 0x7f, 0x74, 0x82, 0x12, 0x40, 0x52, 0x7f, 0x00, 0x2e, 0x00, + 0x40, 0x60, 0x7f, 0x98, 0x2e, 0x6a, 0xd6, 0x03, 0x2e, 0x61, 0xf7, 0x00, 0x31, 0x48, 0x0a, 0x23, 0x2e, 0x61, 0xf7, + 0xe1, 0x31, 0x23, 0x2e, 0x61, 0xf5, 0xf6, 0x6f, 0xe7, 0x6f, 0x81, 0x6f, 0x90, 0x6f, 0xa2, 0x6f, 0xb3, 0x6f, 0xc4, + 0x6f, 0xd5, 0x6f, 0x7b, 0x6f, 0x40, 0x5f, 0xc8, 0x2e, 0xa7, 0x50, 0x10, 0x50, 0xbd, 0x52, 0x05, 0x2e, 0x8d, 0x00, + 0xfb, 0x7f, 0x00, 0x2e, 0x13, 0x40, 0x93, 0x42, 0x41, 0x0e, 0xfb, 0x2f, 0x98, 0x2e, 0x91, 0x03, 0xfb, 0x6f, 0xf0, + 0x5f, 0x80, 0x2e, 0x87, 0xcf, 0x10, 0x50, 0xfb, 0x7f, 0x98, 0x2e, 0x56, 0xc7, 0x98, 0x2e, 0x49, 0xc3, 0x11, 0x30, + 0x30, 0x30, 0xfb, 0x6f, 0xf0, 0x5f, 0x23, 0x2e, 0x8f, 0x00, 0x21, 0x2e, 0x86, 0x00, 0x23, 0x2e, 0x19, 0x00, 0x21, + 0x2e, 0xac, 0x00, 0xb8, 0x2e, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x9a, 0x01, + 0x34, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x03, 0x2e, 0x8d, 0x00, 0x16, 0xb8, 0x02, 0x34, 0x4a, 0x0c, 0x21, 0x2e, 0x2d, 0xf5, 0xc0, 0x2e, 0x23, + 0x2e, 0x8d, 0x00, 0x40, 0x50, 0xf1, 0x7f, 0x0a, 0x25, 0x3c, 0x86, 0xeb, 0x7f, 0x41, 0x33, 0x22, 0x30, 0x98, 0x2e, + 0xc2, 0xc4, 0xd3, 0x6f, 0xf4, 0x30, 0xdc, 0x09, 0xc1, 0x58, 0xc2, 0x6f, 0x94, 0x09, 0xc3, 0x58, 0x6a, 0xbb, 0xdc, + 0x08, 0xb4, 0xb9, 0xb1, 0xbd, 0xbf, 0x5a, 0x95, 0x08, 0x21, 0xbd, 0xf6, 0xbf, 0x77, 0x0b, 0x51, 0xbe, 0xf1, 0x6f, + 0xeb, 0x6f, 0x52, 0x42, 0x54, 0x42, 0xc0, 0x2e, 0x43, 0x42, 0xc0, 0x5f, 0x03, 0x2e, 0x12, 0x01, 0x00, 0x31, 0x08, + 0x08, 0xf2, 0x30, 0x20, 0x50, 0x4a, 0x08, 0xe1, 0x7f, 0x00, 0xb2, 0xfb, 0x7f, 0x01, 0x30, 0x15, 0x2f, 0x01, 0x2e, + 0x8f, 0x00, 0x01, 0x90, 0x03, 0x2f, 0x23, 0x2e, 0x8f, 0x00, 0x98, 0x2e, 0xe7, 0x03, 0x98, 0x2e, 0xa8, 0xcf, 0x11, + 0x30, 0x41, 0x08, 0x23, 0x2e, 0xd5, 0x00, 0x98, 0x2e, 0x41, 0xb1, 0x10, 0x25, 0xfb, 0x6f, 0xe0, 0x6f, 0xe0, 0x5f, + 0x80, 0x2e, 0x95, 0xcf, 0xe0, 0x6f, 0x98, 0x2e, 0x95, 0xcf, 0x11, 0x30, 0x23, 0x2e, 0x8f, 0x00, 0xfb, 0x6f, 0xe0, + 0x5f, 0xb8, 0x2e, 0xd5, 0x50, 0x01, 0x30, 0x0f, 0x55, 0x11, 0x42, 0x42, 0x0e, 0xfc, 0x2f, 0x10, 0x30, 0xc0, 0x2e, + 0x21, 0x2e, 0xbc, 0x00, 0x40, 0x50, 0x0a, 0x25, 0x3c, 0x80, 0xfb, 0x7f, 0x01, 0x42, 0xd2, 0x7f, 0xe3, 0x7f, 0x32, + 0x30, 0x10, 0x25, 0x98, 0x2e, 0x7a, 0xc1, 0xfb, 0x6f, 0xc0, 0x5f, 0xb8, 0x2e, 0x44, 0x47, 0xb5, 0x50, 0xf0, 0x50, + 0x12, 0x40, 0x06, 0x40, 0x1a, 0x25, 0x6c, 0xbe, 0x76, 0x8a, 0x74, 0x80, 0xfb, 0x7f, 0x68, 0xbf, 0xb7, 0x56, 0x0b, + 0x30, 0xd3, 0x08, 0x4c, 0xba, 0x6c, 0xbb, 0x5b, 0x7f, 0x4b, 0x43, 0x0b, 0x42, 0xc0, 0xb2, 0xc6, 0x7f, 0xb4, 0x7f, + 0xd0, 0x7f, 0xe5, 0x7f, 0xa3, 0x7f, 0x90, 0x2e, 0xaf, 0xb0, 0x01, 0x2e, 0x8c, 0x00, 0x00, 0xb2, 0x0b, 0x2f, 0xab, + 0x52, 0x01, 0x2e, 0x87, 0x00, 0x92, 0x7f, 0x98, 0x2e, 0xbb, 0xcc, 0x01, 0x30, 0x23, 0x2e, 0x8c, 0x00, 0x92, 0x6f, + 0xa3, 0x6f, 0x1a, 0x25, 0x26, 0xbc, 0x86, 0xba, 0x25, 0xbc, 0x0f, 0xb8, 0x54, 0xb1, 0x00, 0xb2, 0x96, 0x7f, 0x0c, + 0x2f, 0xad, 0x50, 0xaf, 0x54, 0x0b, 0x30, 0x0b, 0x2e, 0x31, 0x01, 0xb3, 0x58, 0x1b, 0x42, 0x9b, 0x42, 0x6c, 0x09, + 0x0b, 0x42, 0x2b, 0x2e, 0x31, 0x01, 0x8b, 0x42, 0x71, 0x84, 0xb9, 0x50, 0x58, 0x09, 0xb1, 0x52, 0x91, 0x50, 0x82, + 0x7f, 0x75, 0x7f, 0x98, 0x2e, 0xc2, 0xc0, 0x01, 0x2e, 0x87, 0x00, 0xe5, 0x6f, 0xd4, 0x6f, 0x73, 0x6f, 0x82, 0x6f, + 0xab, 0x52, 0xa9, 0x5c, 0x98, 0x2e, 0x06, 0xcd, 0x65, 0x6f, 0xa0, 0x6f, 0xad, 0x52, 0x40, 0xb3, 0x04, 0xbd, 0x53, + 0x40, 0xaf, 0xba, 0x44, 0x40, 0xe1, 0x7f, 0x02, 0x30, 0x06, 0x2f, 0x40, 0xb3, 0x02, 0x30, 0x03, 0x2f, 0xaf, 0x5c, + 0x12, 0x30, 0x93, 0x43, 0x84, 0x43, 0x03, 0xbf, 0x6f, 0xbb, 0x80, 0xb3, 0x20, 0x2f, 0x46, 0x6f, 0xde, 0x00, 0x56, + 0x6f, 0x26, 0x03, 0x44, 0x42, 0x40, 0x91, 0x27, 0x2e, 0x88, 0x00, 0xaf, 0x52, 0x14, 0x2f, 0xaf, 0x5c, 0x00, 0x2e, + 0x95, 0x41, 0x86, 0x41, 0x5d, 0x05, 0xa6, 0x07, 0x80, 0xab, 0x04, 0x2f, 0x80, 0x91, 0x0a, 0x2f, 0x96, 0x6f, 0x75, + 0x0f, 0x07, 0x2f, 0x95, 0x6f, 0x40, 0xb3, 0x04, 0x2f, 0x53, 0x42, 0x44, 0x42, 0x12, 0x30, 0x04, 0x2c, 0x11, 0x30, + 0x02, 0x2c, 0x11, 0x30, 0x11, 0x30, 0x02, 0xbc, 0x0f, 0xb8, 0xd2, 0x7f, 0x00, 0x90, 0x06, 0x2f, 0x31, 0x30, 0x23, + 0x2e, 0xac, 0x00, 0x23, 0x2e, 0x86, 0x00, 0x0b, 0x2c, 0x01, 0x30, 0x01, 0x2e, 0x86, 0x00, 0x05, 0x2e, 0xac, 0x00, + 0x10, 0x1a, 0x02, 0x2f, 0x21, 0x2e, 0xac, 0x00, 0x01, 0x2d, 0x01, 0x30, 0xc0, 0x6f, 0x98, 0x2e, 0x95, 0xcf, 0xd1, + 0x6f, 0xb0, 0x6f, 0x98, 0x2e, 0x95, 0xcf, 0xe2, 0x6f, 0xa7, 0x52, 0x01, 0x2e, 0x88, 0x00, 0x82, 0x40, 0x50, 0x42, + 0x11, 0x2c, 0x42, 0x42, 0x10, 0x30, 0x31, 0x30, 0x21, 0x2e, 0x8c, 0x00, 0x23, 0x2e, 0xac, 0x00, 0x23, 0x2e, 0x86, + 0x00, 0xc0, 0x6f, 0x01, 0x30, 0x98, 0x2e, 0x95, 0xcf, 0xb0, 0x6f, 0x01, 0x30, 0x98, 0x2e, 0x95, 0xcf, 0x00, 0x2e, + 0xfb, 0x6f, 0x10, 0x5f, 0xb8, 0x2e, 0x50, 0x50, 0xcd, 0x50, 0x51, 0x30, 0x11, 0x42, 0xfb, 0x7f, 0x7b, 0x30, 0x0b, + 0x42, 0x11, 0x30, 0x02, 0x80, 0x23, 0x33, 0x01, 0x42, 0x03, 0x00, 0x07, 0x2e, 0x80, 0x03, 0x05, 0x2e, 0x8d, 0x00, + 0xa5, 0x52, 0xe2, 0x7f, 0xd3, 0x7f, 0xc0, 0x7f, 0x98, 0x2e, 0x9b, 0x03, 0xd1, 0x6f, 0x08, 0x0a, 0x1a, 0x25, 0x7b, + 0x86, 0xd0, 0x7f, 0x01, 0x33, 0x12, 0x30, 0x98, 0x2e, 0xc2, 0xc4, 0xd1, 0x6f, 0x08, 0x0a, 0x00, 0xb2, 0x0d, 0x2f, + 0xe3, 0x6f, 0x01, 0x2e, 0x80, 0x03, 0x51, 0x30, 0xc7, 0x86, 0x23, 0x2e, 0x21, 0xf2, 0x08, 0xbc, 0xc0, 0x42, 0x98, + 0x2e, 0x91, 0x03, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0xb0, 0x6f, 0x0b, 0xb8, 0x03, 0x2e, 0x1b, 0x00, 0x08, 0x1a, + 0xb0, 0x7f, 0x70, 0x30, 0x04, 0x2f, 0x21, 0x2e, 0x21, 0xf2, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0x98, 0x2e, 0x6d, + 0xc0, 0x98, 0x2e, 0x5d, 0xc0, 0xc5, 0x50, 0x98, 0x2e, 0x44, 0xcb, 0xc7, 0x50, 0x98, 0x2e, 0x46, 0xc3, 0xc9, 0x50, + 0x98, 0x2e, 0x53, 0xc7, 0x20, 0x26, 0xc0, 0x6f, 0x02, 0x31, 0x12, 0x42, 0x6b, 0x31, 0x0b, 0x42, 0x37, 0x80, 0x0b, + 0x30, 0x0b, 0x42, 0xf3, 0x37, 0xcf, 0x52, 0xd3, 0x50, 0x44, 0x40, 0xa2, 0x0a, 0x42, 0x42, 0x8b, 0x31, 0x09, 0x2e, + 0x5e, 0xf7, 0xd1, 0x54, 0xe3, 0x08, 0x83, 0x42, 0x1b, 0x42, 0x23, 0x33, 0x4b, 0x00, 0xbc, 0x84, 0x0b, 0x40, 0x33, + 0x30, 0x83, 0x42, 0x0b, 0x42, 0xe0, 0x7f, 0xd1, 0x7f, 0x98, 0x2e, 0x2c, 0x03, 0xd1, 0x6f, 0x80, 0x30, 0x40, 0x42, + 0x03, 0x30, 0xe0, 0x6f, 0xcb, 0x54, 0x04, 0x30, 0x00, 0x2e, 0x00, 0x2e, 0x01, 0x89, 0x62, 0x0e, 0xfa, 0x2f, 0x43, + 0x42, 0x11, 0x30, 0xfb, 0x6f, 0xc0, 0x2e, 0x01, 0x42, 0xb0, 0x5f, 0x50, 0x51, 0x91, 0x54, 0xeb, 0x7f, 0x11, 0x30, + 0x0a, 0x25, 0xff, 0x56, 0x11, 0x5d, 0x95, 0x40, 0xc4, 0x40, 0x25, 0x01, 0xd4, 0x42, 0x4d, 0x17, 0xc4, 0x40, 0x65, + 0x03, 0xd5, 0x42, 0x56, 0x0e, 0xf5, 0x2f, 0x15, 0x57, 0x02, 0x30, 0xc6, 0x40, 0xb1, 0x29, 0xe6, 0x42, 0x00, 0x2e, + 0xc3, 0x40, 0xc0, 0xb2, 0x0a, 0x23, 0x4c, 0x14, 0x71, 0x0e, 0xd4, 0x7f, 0x90, 0x2e, 0x93, 0xb3, 0x03, 0x31, 0xe1, + 0x52, 0xe3, 0x54, 0x5c, 0x05, 0x8a, 0x28, 0x2b, 0x82, 0x03, 0x57, 0xdd, 0x5e, 0x2e, 0x80, 0xde, 0x8c, 0x30, 0x89, + 0xff, 0x29, 0x96, 0x7f, 0x60, 0x7f, 0xc5, 0x7f, 0xb3, 0x7f, 0xa4, 0x7f, 0x82, 0x7f, 0x77, 0x7f, 0x51, 0x7f, 0x00, + 0x2e, 0x90, 0x41, 0x92, 0x41, 0xd3, 0x6f, 0xc0, 0xb2, 0x46, 0x7f, 0x31, 0x7f, 0x08, 0x2f, 0xd0, 0xa0, 0x02, 0x2f, + 0xa0, 0x6f, 0x05, 0x2c, 0x10, 0x10, 0xc6, 0x6f, 0x96, 0x14, 0x03, 0x12, 0x02, 0x0a, 0x05, 0x2e, 0xd3, 0x00, 0x80, + 0x90, 0xd7, 0x54, 0x40, 0x42, 0x4a, 0x2f, 0x41, 0x40, 0x98, 0x2e, 0xd9, 0xc0, 0xdb, 0x54, 0xd9, 0x52, 0x20, 0x7f, + 0x98, 0x2e, 0xfe, 0xc9, 0x72, 0x6f, 0x10, 0x25, 0x98, 0x2e, 0xfe, 0xc9, 0x22, 0x6f, 0xe3, 0x30, 0x10, 0x25, 0x98, + 0x2e, 0x0f, 0xca, 0x91, 0x6f, 0x76, 0x86, 0xd9, 0x52, 0xdd, 0x54, 0xd0, 0x42, 0x93, 0x7f, 0x98, 0x2e, 0xfe, 0xc9, + 0x22, 0x6f, 0xe3, 0x30, 0x10, 0x25, 0x98, 0x2e, 0x0f, 0xca, 0x91, 0x6f, 0xdf, 0x54, 0x40, 0x42, 0x79, 0x80, 0xd9, + 0x52, 0x90, 0x7f, 0x98, 0x2e, 0xfe, 0xc9, 0x82, 0x6f, 0x10, 0x25, 0x98, 0x2e, 0xfe, 0xc9, 0x22, 0x6f, 0xe3, 0x30, + 0x10, 0x25, 0x98, 0x2e, 0x0f, 0xca, 0x93, 0x6f, 0xe1, 0x54, 0xd9, 0x52, 0xd0, 0x42, 0x13, 0x7f, 0x98, 0x2e, 0xfe, + 0xc9, 0x22, 0x6f, 0xe3, 0x30, 0x10, 0x25, 0x98, 0x2e, 0x0f, 0xca, 0x13, 0x6f, 0xe5, 0x5e, 0xc0, 0x42, 0xf7, 0x7f, + 0x00, 0x2e, 0x22, 0x6f, 0x91, 0x6f, 0xe1, 0x5a, 0xe3, 0x58, 0xdf, 0x5c, 0xe1, 0x56, 0x98, 0x2e, 0x67, 0xcc, 0xb1, + 0x6f, 0x02, 0x2c, 0x40, 0x42, 0xb1, 0x6f, 0x41, 0x80, 0x32, 0x6f, 0x81, 0x82, 0x62, 0x6f, 0x46, 0x6f, 0x96, 0x7f, + 0x4a, 0x0e, 0xb0, 0x7f, 0x94, 0x2f, 0xf1, 0x3d, 0x01, 0x55, 0x10, 0x30, 0x53, 0x6f, 0x91, 0x00, 0x21, 0x2e, 0xd3, + 0x00, 0x66, 0x6f, 0xd1, 0x40, 0x80, 0x40, 0x01, 0x00, 0x90, 0x42, 0x09, 0x16, 0x85, 0x40, 0x28, 0x02, 0x80, 0x42, + 0x9a, 0x80, 0xd7, 0x54, 0xd6, 0x7f, 0xc3, 0x7f, 0xb0, 0x7f, 0x98, 0x2e, 0xd9, 0xc0, 0x05, 0x30, 0xf5, 0x7f, 0x20, + 0x25, 0xb1, 0x6f, 0xdd, 0x58, 0xdb, 0x5c, 0xdd, 0x56, 0x98, 0x2e, 0x67, 0xcc, 0xd6, 0x6f, 0xc3, 0x6f, 0xb2, 0x6f, + 0x61, 0x6f, 0xa7, 0x84, 0x90, 0x43, 0x59, 0x0e, 0xdf, 0x2f, 0x10, 0x30, 0x81, 0x40, 0x08, 0x28, 0x90, 0x42, 0xd2, + 0x7f, 0x02, 0xa0, 0x27, 0x2f, 0xd5, 0x54, 0x03, 0x53, 0x03, 0x30, 0x96, 0x40, 0x80, 0x40, 0x08, 0x17, 0x15, 0x30, + 0x65, 0x09, 0xb5, 0x01, 0xc3, 0x02, 0x61, 0xb8, 0x94, 0x8c, 0xb6, 0x7f, 0xbf, 0xbd, 0xd7, 0x54, 0xc1, 0x7f, 0x43, + 0x0a, 0x98, 0x2e, 0xd9, 0xc0, 0xe5, 0x54, 0xf2, 0x7f, 0x20, 0x25, 0xb1, 0x6f, 0xe1, 0x5a, 0xe3, 0x58, 0xdf, 0x5c, + 0xe1, 0x56, 0x98, 0x2e, 0x67, 0xcc, 0xb2, 0x6f, 0x03, 0x30, 0xc1, 0x6f, 0xab, 0x84, 0x0b, 0x5d, 0x93, 0x42, 0x50, + 0x42, 0x4e, 0x0e, 0x93, 0x42, 0xdb, 0x2f, 0x83, 0x42, 0x00, 0x2e, 0xd1, 0x6f, 0x98, 0x2e, 0xb3, 0xc0, 0x61, 0x6f, + 0xc0, 0x7f, 0x98, 0x2e, 0xb3, 0xc0, 0x51, 0x6f, 0xb0, 0x7f, 0x98, 0x2e, 0xb3, 0xc0, 0x00, 0xa0, 0xe7, 0x52, 0x08, + 0x22, 0xb2, 0x6f, 0xc1, 0x6f, 0xa0, 0x7f, 0xb3, 0x30, 0x98, 0x2e, 0x0f, 0xca, 0xb2, 0x6f, 0xb0, 0x7f, 0xb3, 0x30, + 0xe9, 0x52, 0x98, 0x2e, 0x5a, 0xca, 0xd6, 0x6f, 0x02, 0x30, 0x61, 0x6f, 0xd2, 0x7f, 0x90, 0x7f, 0x81, 0x7f, 0xb3, + 0x30, 0x42, 0x40, 0x91, 0x41, 0x76, 0x7f, 0x98, 0x2e, 0x0f, 0xca, 0xd2, 0x6f, 0x81, 0x6f, 0x10, 0x28, 0x41, 0x40, + 0x92, 0x6f, 0xd0, 0x7f, 0xb3, 0x30, 0x98, 0x2e, 0x0f, 0xca, 0x81, 0x6f, 0x76, 0x6f, 0x0b, 0x55, 0x50, 0x42, 0x72, + 0x0e, 0xe9, 0x2f, 0xc2, 0x6f, 0xa1, 0x6f, 0x98, 0x2e, 0xfe, 0xc9, 0x10, 0x25, 0xb3, 0x30, 0x21, 0x25, 0x98, 0x2e, + 0x0f, 0xca, 0x20, 0x25, 0x13, 0x51, 0xeb, 0x52, 0x98, 0x2e, 0xcb, 0xb3, 0xc5, 0x6f, 0x13, 0x51, 0xed, 0x5c, 0x12, + 0x40, 0x75, 0x05, 0x31, 0x30, 0x11, 0x86, 0x05, 0x5d, 0xc1, 0x7f, 0x15, 0x0f, 0x00, 0x40, 0x08, 0x2f, 0x3f, 0x80, + 0x00, 0xa8, 0x21, 0x2e, 0xc3, 0x00, 0x00, 0x30, 0x0a, 0x2f, 0x90, 0x43, 0x09, 0x2c, 0x80, 0x43, 0x01, 0x80, 0x03, + 0xa0, 0x21, 0x2e, 0xc3, 0x00, 0x10, 0x30, 0x01, 0x2f, 0x91, 0x43, 0x80, 0x43, 0x00, 0x2e, 0xef, 0x54, 0x00, 0x6f, + 0x82, 0x0f, 0x03, 0x2f, 0xf0, 0x6e, 0xf1, 0x54, 0x02, 0x0f, 0x14, 0x2f, 0xe1, 0x6e, 0xc3, 0x7f, 0x98, 0x2e, 0x74, + 0xc0, 0xc3, 0x6f, 0xf3, 0x54, 0x01, 0x30, 0xc1, 0x7f, 0xc2, 0x0f, 0x0a, 0x2f, 0xf0, 0x6e, 0xf5, 0x52, 0x81, 0x0e, + 0x11, 0x30, 0x04, 0x2f, 0x00, 0x6f, 0x00, 0xa4, 0x11, 0x30, 0x00, 0x2f, 0x21, 0x30, 0xc1, 0x7f, 0xf1, 0x80, 0xc2, + 0x40, 0x80, 0x90, 0x07, 0x2f, 0x07, 0x55, 0xb8, 0x86, 0x12, 0x30, 0xc1, 0x42, 0xd7, 0x86, 0x23, 0x2e, 0xc5, 0x00, + 0xc2, 0x42, 0x38, 0x8a, 0x02, 0x40, 0x0a, 0x1a, 0x07, 0x55, 0x00, 0x30, 0x02, 0x2f, 0x91, 0x42, 0x0e, 0x2c, 0x80, + 0x42, 0x03, 0x2e, 0xc6, 0x00, 0x12, 0x30, 0x4a, 0x28, 0x23, 0x2e, 0xc6, 0x00, 0x50, 0xa0, 0x04, 0x2f, 0x09, 0x55, + 0x89, 0x82, 0xc3, 0x6f, 0x83, 0x42, 0x40, 0x42, 0x00, 0x2e, 0x05, 0x2e, 0x8e, 0x00, 0x84, 0x8c, 0x47, 0x41, 0xa1, + 0x41, 0xa1, 0x56, 0xc0, 0xb3, 0x0b, 0x09, 0xc4, 0x05, 0x44, 0x89, 0x85, 0x41, 0xf3, 0xbf, 0x82, 0x8d, 0xa7, 0x7f, + 0x94, 0x7f, 0x0a, 0x2f, 0x09, 0x2e, 0xc4, 0x00, 0x00, 0x91, 0x06, 0x2f, 0x81, 0x80, 0xf9, 0x58, 0x0b, 0x40, 0xfb, + 0x5a, 0x84, 0x7f, 0x0c, 0x2c, 0xfb, 0x50, 0x2b, 0x09, 0x98, 0xb9, 0x44, 0x04, 0xd8, 0xba, 0x82, 0x84, 0x53, 0xbc, + 0xf7, 0x5e, 0xb3, 0xbe, 0x8b, 0x40, 0x13, 0xbe, 0x87, 0x7f, 0xb3, 0x30, 0x86, 0x41, 0xfd, 0x52, 0xb2, 0x6f, 0x76, + 0x7f, 0x60, 0x7f, 0x55, 0x7f, 0x4b, 0x7f, 0x34, 0x7f, 0x98, 0x2e, 0x0f, 0xca, 0xd1, 0x6f, 0x88, 0x0e, 0x03, 0x2f, + 0x01, 0x2e, 0xbc, 0x00, 0x00, 0xb2, 0x03, 0x2f, 0x00, 0x30, 0x21, 0x2e, 0xbe, 0x00, 0x10, 0x2d, 0x03, 0x2e, 0xbe, + 0x00, 0x10, 0x30, 0x48, 0x28, 0x72, 0x6f, 0xa8, 0xb9, 0x23, 0x2e, 0xbe, 0x00, 0x0b, 0x0e, 0xc1, 0x6f, 0x0b, 0x55, + 0x03, 0x2f, 0x90, 0x42, 0x91, 0x42, 0x00, 0x30, 0x80, 0x42, 0xb3, 0x30, 0xb2, 0x6f, 0x41, 0x6f, 0x98, 0x2e, 0x0f, + 0xca, 0xd1, 0x6f, 0x08, 0x0f, 0x03, 0x2f, 0x01, 0x2e, 0xbc, 0x00, 0x00, 0x90, 0x03, 0x2f, 0x00, 0x30, 0x21, 0x2e, + 0xc0, 0x00, 0x12, 0x2d, 0x01, 0x2e, 0xc0, 0x00, 0x71, 0x6f, 0xa1, 0x54, 0x01, 0x80, 0x4a, 0x08, 0x21, 0x2e, 0xc0, + 0x00, 0x01, 0x0e, 0x07, 0x2f, 0x0d, 0x51, 0x02, 0x80, 0x01, 0x30, 0x21, 0x42, 0x12, 0x30, 0x01, 0x42, 0x25, 0x2e, + 0xbf, 0x00, 0x91, 0x6f, 0x7e, 0x84, 0x43, 0x40, 0x82, 0x40, 0x7c, 0x80, 0x9a, 0x29, 0x03, 0x40, 0x46, 0x42, 0xc3, + 0xb2, 0x06, 0x30, 0x00, 0x30, 0x2b, 0x2f, 0xc0, 0x6f, 0x00, 0xb2, 0x00, 0x30, 0x27, 0x2f, 0x01, 0x2e, 0xc4, 0x00, + 0x00, 0x90, 0x00, 0x30, 0x05, 0x2f, 0xc2, 0x90, 0x03, 0x2f, 0xc0, 0x6f, 0x03, 0x90, 0x00, 0x30, 0x1c, 0x2f, 0x00, + 0x6f, 0x83, 0x6f, 0x83, 0x0e, 0x00, 0x30, 0x17, 0x2f, 0xf3, 0x6e, 0x50, 0x6f, 0x98, 0x0f, 0x00, 0x30, 0x12, 0x2f, + 0xa0, 0x6f, 0x98, 0x0e, 0x00, 0x30, 0x0e, 0x2f, 0xe3, 0x6e, 0x60, 0x6f, 0x98, 0x0f, 0x00, 0x30, 0x09, 0x2f, 0x30, + 0x6f, 0x98, 0x0e, 0x00, 0x30, 0x05, 0x2f, 0x80, 0xb2, 0x00, 0x30, 0x02, 0x2f, 0x2d, 0x2e, 0xbc, 0x00, 0x10, 0x30, + 0x42, 0x40, 0x82, 0xac, 0x56, 0x82, 0x01, 0x2f, 0x00, 0xb2, 0x05, 0x2f, 0x0d, 0x55, 0x82, 0x84, 0x2d, 0x2e, 0xbf, + 0x00, 0x86, 0x42, 0x00, 0x2e, 0x0f, 0x55, 0x56, 0x42, 0x56, 0x42, 0x4a, 0x0e, 0xfb, 0x2f, 0x2d, 0x2e, 0xd6, 0x00, + 0x01, 0x2d, 0x00, 0x30, 0xeb, 0x6f, 0xb0, 0x5e, 0xb8, 0x2e, 0x70, 0x50, 0x0a, 0x25, 0x39, 0x80, 0xf3, 0x7f, 0x03, + 0x42, 0xa1, 0x7f, 0xc2, 0x7f, 0xd1, 0x7f, 0x03, 0x30, 0x03, 0x43, 0xe4, 0x7f, 0xbb, 0x7f, 0x22, 0x30, 0x10, 0x25, + 0x98, 0x2e, 0x7a, 0xc1, 0xd2, 0x6f, 0x02, 0x17, 0x04, 0x08, 0xc1, 0x6f, 0x0c, 0x09, 0x04, 0x1a, 0x10, 0x30, 0x04, + 0x30, 0x20, 0x22, 0x01, 0xb2, 0x14, 0x2f, 0x17, 0x59, 0x14, 0x09, 0xf3, 0x30, 0x93, 0x08, 0x24, 0xbd, 0x44, 0xba, + 0x94, 0x0a, 0x02, 0x17, 0xf3, 0x6f, 0x4c, 0x08, 0x9a, 0x08, 0x8a, 0x0a, 0x19, 0x53, 0x51, 0x08, 0xa1, 0x58, 0x94, + 0x08, 0x28, 0xbd, 0x98, 0xb8, 0xe4, 0x6f, 0x51, 0x0a, 0x01, 0x43, 0x00, 0x2e, 0xbb, 0x6f, 0x90, 0x5f, 0xb8, 0x2e, + 0x1b, 0x57, 0x05, 0x40, 0x69, 0x18, 0x0d, 0x17, 0xe1, 0x18, 0x19, 0x05, 0x16, 0x25, 0x37, 0x25, 0x4a, 0x17, 0x54, + 0x18, 0xec, 0x18, 0x04, 0x30, 0x64, 0x07, 0xea, 0x18, 0x8e, 0x00, 0x5f, 0x02, 0xd9, 0x56, 0x93, 0x00, 0x4c, 0x02, + 0x2f, 0xb9, 0x91, 0xbc, 0x91, 0x0a, 0x02, 0x42, 0xb8, 0x2e, 0x00, 0x2e, 0x10, 0x24, 0xfa, 0x07, 0x11, 0x24, 0x00, + 0x10, 0x12, 0x24, 0x80, 0x2e, 0x13, 0x24, 0x00, 0xc1, 0x12, 0x42, 0x13, 0x42, 0x41, 0x1a, 0xfb, 0x2f, 0x10, 0x24, + 0x50, 0x32, 0x11, 0x24, 0x21, 0x2e, 0x21, 0x2e, 0x10, 0x00, 0x23, 0x2e, 0x11, 0x00, 0x80, 0x2e, 0x10, 0x00 +}; + +/*! @name Global array that stores the feature input configuration of BMI270_WH */ +const struct bmi2_feature_config bmi270_wh_feat_in[BMI270_WH_MAX_FEAT_IN] = { + { .type = BMI2_CONFIG_ID, .page = BMI2_PAGE_1, .start_addr = BMI270_WH_CONFIG_ID_STRT_ADDR }, + { .type = BMI2_MAX_BURST_LEN, .page = BMI2_PAGE_1, .start_addr = BMI270_WH_MAX_BURST_LEN_STRT_ADDR }, + { .type = BMI2_AXIS_MAP, .page = BMI2_PAGE_1, .start_addr = BMI270_WH_AXIS_MAP_STRT_ADDR }, + { .type = BMI2_NVM_PROG_PREP, .page = BMI2_PAGE_1, .start_addr = BMI270_WH_NVM_PROG_PREP_STRT_ADDR }, + { .type = BMI2_GYRO_GAIN_UPDATE, .page = BMI2_PAGE_1, .start_addr = BMI270_WH_GYRO_GAIN_UPDATE_STRT_ADDR }, + { .type = BMI2_ANY_MOTION, .page = BMI2_PAGE_1, .start_addr = BMI270_WH_ANY_MOT_STRT_ADDR }, + { .type = BMI2_NO_MOTION, .page = BMI2_PAGE_2, .start_addr = BMI270_WH_NO_MOT_STRT_ADDR }, + { .type = BMI2_STEP_COUNTER_PARAMS, .page = BMI2_PAGE_3, .start_addr = BMI270_WH_STEP_CNT_1_STRT_ADDR }, + { .type = BMI2_STEP_DETECTOR, .page = BMI2_PAGE_6, .start_addr = BMI270_WH_STEP_CNT_4_STRT_ADDR }, + { .type = BMI2_STEP_COUNTER, .page = BMI2_PAGE_6, .start_addr = BMI270_WH_STEP_CNT_4_STRT_ADDR }, + { .type = BMI2_STEP_ACTIVITY, .page = BMI2_PAGE_6, .start_addr = BMI270_WH_STEP_CNT_4_STRT_ADDR }, + { .type = BMI2_WRIST_WEAR_WAKE_UP_WH, .page = BMI2_PAGE_2, .start_addr = BMI270_WH_WRIST_WEAR_WAKE_UP_STRT_ADDR }, +}; + +/*! @name Global array that stores the feature output configuration */ +const struct bmi2_feature_config bmi270_wh_feat_out[BMI270_WH_MAX_FEAT_OUT] = { + { .type = BMI2_STEP_COUNTER, .page = BMI2_PAGE_0, .start_addr = BMI270_WH_STEP_CNT_OUT_STRT_ADDR }, + { .type = BMI2_STEP_ACTIVITY, .page = BMI2_PAGE_0, .start_addr = BMI270_WH_STEP_ACT_OUT_STRT_ADDR }, + { .type = BMI2_GYRO_GAIN_UPDATE, .page = BMI2_PAGE_0, .start_addr = BMI270_WH_GYR_USER_GAIN_OUT_STRT_ADDR }, + { .type = BMI2_GYRO_CROSS_SENSE, .page = BMI2_PAGE_0, .start_addr = BMI270_WH_GYRO_CROSS_SENSE_STRT_ADDR }, + { .type = BMI2_NVM_STATUS, .page = BMI2_PAGE_0, .start_addr = BMI270_WH_NVM_VFRM_OUT_STRT_ADDR }, + { .type = BMI2_VFRM_STATUS, .page = BMI2_PAGE_0, .start_addr = BMI270_WH_NVM_VFRM_OUT_STRT_ADDR } +}; + +/*! @name Global array that stores the feature interrupts of BMI270_WH */ +struct bmi2_map_int bmi270wh_map_int[BMI270_WH_MAX_INT_MAP] = { + { .type = BMI2_STEP_COUNTER, .sens_map_int = BMI270_WH_INT_STEP_COUNTER_MASK }, + { .type = BMI2_STEP_DETECTOR, .sens_map_int = BMI270_WH_INT_STEP_DETECTOR_MASK }, + { .type = BMI2_STEP_ACTIVITY, .sens_map_int = BMI270_WH_INT_STEP_ACT_MASK }, + { .type = BMI2_WRIST_WEAR_WAKE_UP_WH, .sens_map_int = BMI270_WH_INT_WRIST_WEAR_WAKEUP_WH_MASK }, + { .type = BMI2_ANY_MOTION, .sens_map_int = BMI270_WH_INT_ANY_MOT_MASK }, + { .type = BMI2_NO_MOTION, .sens_map_int = BMI270_WH_INT_NO_MOT_MASK }, +}; + +/******************************************************************************/ + +/*! Local Function Prototypes + ******************************************************************************/ + +/*! + * @brief This internal API is used to validate the device pointer for + * null conditions. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t null_ptr_check(const struct bmi2_dev *dev); + +/*! + * @brief This internal API enables the selected sensor/features. + * + * @param[in] sensor_sel : Selects the desired sensor. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t sensor_enable(uint64_t sensor_sel, struct bmi2_dev *dev); + +/*! + * @brief This internal API disables the selected sensor/features. + * + * @param[in] sensor_sel : Selects the desired sensor. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t sensor_disable(uint64_t sensor_sel, struct bmi2_dev *dev); + +/*! + * @brief This internal API selects the sensors/features to be enabled or + * disabled. + * + * @param[in] sens_list : Pointer to select the sensor. + * @param[in] n_sens : Number of sensors selected. + * @param[out] sensor_sel : Gets the selected sensor. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t select_sensor(const uint8_t *sens_list, uint8_t n_sens, uint64_t *sensor_sel); + +/*! + * @brief This internal API is used to enable/disable any-motion feature. + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[in] enable : Enables/Disables any-motion. + * + * Enable | Description + * -------------|--------------- + * BMI2_DISABLE | Disables any-motion. + * BMI2_ENABLE | Enables any-motion. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_any_motion(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to enable/disable no-motion feature. + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[in] enable : Enables/Disables no-motion. + * + * Enable | Description + * -------------|--------------- + * BMI2_DISABLE | Disables no-motion. + * BMI2_ENABLE | Enables no-motion. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_no_motion(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to enable/disable step detector feature. + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[in] enable : Enables/Disables step-detector. + * + * Enable | Description + * -------------|--------------- + * BMI2_DISABLE | Disables step detector + * BMI2_ENABLE | Enables step detector + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_step_detector(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to enable/disable step counter feature. + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[in] enable : Enables/Disables step counter. + * + * Enable | Description + * -------------|--------------- + * BMI2_DISABLE | Disables step counter + * BMI2_ENABLE | Enables step counter + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_step_counter(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to enable/disable step activity detection. + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[in] enable : Enables/Disables step activity. + * + * Enable | Description + * -------------|--------------- + * BMI2_DISABLE | Disables step activity + * BMI2_ENABLE | Enables step activity + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_step_activity(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to enable/disable gyroscope user gain + * feature. + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[in] enable : Enables/Disables gyroscope user gain. + * + * Enable | Description + * -------------|--------------- + * BMI2_DISABLE | Disables gyroscope user gain + * BMI2_ENABLE | Enables gyroscope user gain + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_gyro_user_gain(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API enables the wrist wear wake up feature for wearable variant. + * + * @param[in] dev : Structure instance of bmi2_dev. + * @param[in] enable : Enables/Disables wrist wear wake up. + * + * Enable | Description + * -------------|--------------- + * BMI2_DISABLE | Disables wrist wear wake up + * BMI2_ENABLE | Enables wrist wear wake up + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_wrist_wear_wake_up_wh(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API sets any-motion configurations like axes select, + * duration, threshold and output-configuration. + * + * @param[in] config : Structure instance of bmi2_any_motion_config. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @verbatim + *---------------------------------------------------------------------------- + * bmi2_any_motion_config | + * Structure parameters | Description + *--------------------------|-------------------------------------------------- + * | Defines the number of consecutive data points for + * | which the threshold condition must be respected, + * | for interrupt assertion. It is expressed in 50 Hz + * duration | samples (20 msec). + * | Range is 0 to 163sec. + * | Default value is 5 = 100ms. + * -------------------------|--------------------------------------------------- + * | Slope threshold value for in 5.11g format. + * threshold | Range is 0 to 1g. + * | Default value is 0xAA = 83mg. + * -------------------------|--------------------------------------------------- + * x_sel, y_sel, z_sel | Selects the feature on a per-axis basis + * -------------------------|--------------------------------------------------- + * @endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_any_motion_config(const struct bmi2_any_motion_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API sets no-motion configurations like axes select, + * duration, threshold and output-configuration. + * + * @param[in] config : Structure instance of bmi2_no_motion_config. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @verbatim + *---------------------------------------------------------------------------- + * bmi2_no_motion_config | + * Structure parameters | Description + *--------------------------|-------------------------------------------------- + * | Defines the number of consecutive data points for + * | which the threshold condition must be respected, + * | for interrupt assertion. It is expressed in 50 Hz + * duration | samples (20 msec). + * | Range is 0 to 163sec. + * | Default value is 5 = 100ms. + * -------------------------|--------------------------------------------------- + * | Slope threshold value for in 5.11g format. + * threshold | Range is 0 to 1g. + * | Default value is 0xAA = 83mg. + * -------------------------|--------------------------------------------------- + * x_sel, y_sel, z_sel | Selects the feature on a per-axis basis + * -------------------------|--------------------------------------------------- + * @endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_no_motion_config(const struct bmi2_no_motion_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API sets step counter parameter configurations. + * + * @param[in] step_count_params : Array that stores parameters 1 to 25. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_step_count_params_config(const uint16_t *step_count_params, struct bmi2_dev *dev); + +/*! + * @brief This internal API sets step counter/detector/activity configurations. + * + * @param[in] config : Structure instance of bmi2_step_config. + * @param[in] dev : Structure instance of bmi2_dev. + * + *--------------------------------------------------------------------------- + * bmi2_step_config | + * Structure parameters | Description + *--------------------------|-------------------------------------------------- + * | The Step-counter will trigger output every time + * | the number of steps are counted. Holds implicitly + * water-mark level | a 20x factor, so the range is 0 to 10230, + * | with resolution of 20 steps. + * -------------------------|--------------------------------------------------- + * reset counter | Flag to reset the counted steps. + * -------------------------|--------------------------------------------------- + * @endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_step_config(const struct bmi2_step_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API sets wrist wear wake-up configurations like + * output-configuration for wearable variant. + * + * @param[in] config : Structure instance of + * bmi2_wrist_wear_wake_up_config. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @verbatim + *-------------------------------------|---------------------------------------- + * bmi2_wrist_wear_wake_up_wh_config | + * Structure parameters | Description + *-------------------------------------|------------------------------------------- + * | To set the wrist wear wake-up parameters like + * | min_angle_focus, min_angle_nonfocus, + * wrist_wear_wake_wh_params | angle_landscape_left, angle_landscape_right, + * | angle_potrait_up and down. + * ------------------------------------|------------------------------------------- + * @endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t set_wrist_wear_wake_up_wh_config(const struct bmi2_wrist_wear_wake_up_wh_config *config, + struct bmi2_dev *dev); + +/*! + * @brief This internal API gets any-motion configurations like axes select, + * duration, threshold and output-configuration. + * + * @param[out] config : Structure instance of bmi2_any_motion_config. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @verbatim + *---------------------------------------------------------------------------- + * bmi2_any_motion_config | + * Structure parameters | Description + *--------------------------|-------------------------------------------------- + * | Defines the number of consecutive data points for + * | which the threshold condition must be respected, + * | for interrupt assertion. It is expressed in 50 Hz + * duration | samples (20 msec). + * | Range is 0 to 163sec. + * | Default value is 5 = 100ms. + * -------------------------|--------------------------------------------------- + * | Slope threshold value for in 5.11g format. + * threshold | Range is 0 to 1g. + * | Default value is 0xAA = 83mg. + * -------------------------|--------------------------------------------------- + * x_sel, y_sel, z_sel | Selects the feature on a per-axis basis + * -------------------------|--------------------------------------------------- + * @endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_any_motion_config(struct bmi2_any_motion_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets no-motion configurations like axes select, + * duration, threshold and output-configuration. + * + * @param[out] config : Structure instance of bmi2_no_motion_config. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @verbatim + *---------------------------------------------------------------------------- + * bmi2_no_motion_config | + * Structure parameters | Description + *--------------------------|-------------------------------------------------- + * | Defines the number of consecutive data points for + * | which the threshold condition must be respected, + * | for interrupt assertion. It is expressed in 50 Hz + * duration | samples (20 msec). + * | Range is 0 to 163sec. + * | Default value is 5 = 100ms. + * -------------------------|--------------------------------------------------- + * | Slope threshold value for in 5.11g format. + * threshold | Range is 0 to 1g. + * | Default value is 0xAA = 83mg. + * -------------------------|--------------------------------------------------- + * x_sel, y_sel, z_sel | Selects the feature on a per-axis basis + * -------------------------|--------------------------------------------------- + * @endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_no_motion_config(struct bmi2_no_motion_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets step counter parameter configurations. + * + * @param[in] step_count_params : Array that stores parameters 1 to 25. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_step_count_params_config(uint16_t *step_count_params, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets step counter/detector/activity configurations. + * + * @param[out] config : Structure instance of bmi2_step_config. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @verbatim + *---------------------------------------------------------------------------- + * bmi2_step_config | + * Structure parameters | Description + *--------------------------|-------------------------------------------------- + * | The Step-counter will trigger output every time + * | the number of steps are counted. Holds implicitly + * water-mark level | a 20x factor, so the range is 0 to 10230, + * | with resolution of 20 steps. + * -------------------------|--------------------------------------------------- + * reset counter | Flag to reset the counted steps. + * -------------------------|--------------------------------------------------- + * @endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_step_config(struct bmi2_step_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets wrist wear wake-up configurations like + * output-configuration for wearable variant. + * + * @param[out] config : Structure instance of + * bmi2_wrist_wear_wake_up_config. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @verbatim + *----------------------------------------------------------------------------- + * bmi2_wrist_wear_wake_up_config | + * Structure parameters | Description + *----------------------------------|------------------------------------------- + * | To get the wrist wear wake-up parameters like + * | min_angle_focus, min_angle_nonfocus, + * wrist_wear_wake_params | angle_landscape_left, angle_landscape_right, + * | angle_potrait_up and down. + * ---------------------------------|------------------------------------------- + * @endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_wrist_wear_wake_up_wh_config(struct bmi2_wrist_wear_wake_up_wh_config *config, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets the output values of step activity. + * + * @param[out] step_act : Pointer to the stored step activity data. + * @param[in] dev : Structure instance of bmi2_dev. + * + * *step_act | Output + * -----------|------------ + * 0x00 | STILL + * 0x01 | WALKING + * 0x02 | RUNNING + * 0x03 | UNKNOWN + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_step_activity_output(uint8_t *step_act, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets the output values of step counter. + * + * @param[out] step_count : Pointer to the stored step counter data. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_step_counter_output(uint32_t *step_count, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets the error status related to NVM. + * + * @param[out] nvm_err_stat : Stores the NVM error status. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_nvm_error_status(struct bmi2_nvm_err_status *nvm_err_stat, struct bmi2_dev *dev); + +/*! + * @brief This internal API gets the error status related to virtual frames. + * + * @param[out] vfrm_err_stat : Stores the VFRM related error status. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_vfrm_error_status(struct bmi2_vfrm_err_status *vfrm_err_stat, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to get enable status of gyroscope user gain + * update. + * + * @param[out] status : Stores status of gyroscope user gain update. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_user_gain_upd_status(uint8_t *status, struct bmi2_dev *dev); + +/*! + * @brief This internal API enables/disables compensation of the gain defined + * in the GAIN register. + * + * @param[in] enable : Enables/Disables gain compensation + * @param[in] dev : Structure instance of bmi2_dev. + * + * enable | Description + * -------------|--------------- + * BMI2_ENABLE | Enable gain compensation. + * BMI2_DISABLE | Disable gain compensation. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t enable_gyro_gain(uint8_t enable, struct bmi2_dev *dev); + +/*! + * @brief This internal API is used to extract the output feature configuration + * details like page and start address from the look-up table. + * + * @param[out] feat_output : Structure that stores output feature + * configurations. + * @param[in] type : Type of feature or sensor. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Returns the feature found flag. + * + * @retval BMI2_FALSE : Feature not found + * BMI2_TRUE : Feature found + */ +static uint8_t extract_output_feat_config(struct bmi2_feature_config *feat_output, + uint8_t type, + const struct bmi2_dev *dev); + +/***************************************************************************/ + +/*! User Interface Definitions + ****************************************************************************/ + +/*! + * @brief This API: + * 1) updates the device structure with address of the configuration file. + * 2) Initializes BMI270_WH sensor. + * 3) Writes the configuration file. + * 4) Updates the feature offset parameters in the device structure. + * 5) Updates the maximum number of pages, in the device structure. + */ +int8_t bmi270_wh_init(struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if (rslt == BMI2_OK) + { + /* Assign chip id of BMI270_WH */ + dev->chip_id = BMI270_WH_CHIP_ID; + + /* get the size of config array */ + dev->config_size = sizeof(bmi270_wh_config_file); + + /* Enable the variant specific features if any */ + dev->variant_feature = BMI2_GYRO_CROSS_SENS_ENABLE | BMI2_CRT_RTOSK_ENABLE; + + /* An extra dummy byte is read during SPI read */ + if (dev->intf == BMI2_SPI_INTF) + { + dev->dummy_byte = 1; + } + else + { + dev->dummy_byte = 0; + } + + /* If configuration file pointer is not assigned any address */ + if (!dev->config_file_ptr) + { + /* Give the address of the configuration file array to + * the device pointer + */ + dev->config_file_ptr = bmi270_wh_config_file; + } + + /* Initialize BMI2 sensor */ + rslt = bmi2_sec_init(dev); + if (rslt == BMI2_OK) + { + /* Assign the offsets of the feature input + * configuration to the device structure + */ + dev->feat_config = bmi270_wh_feat_in; + + /* Assign the offsets of the feature output to + * the device structure + */ + dev->feat_output = bmi270_wh_feat_out; + + /* Assign the maximum number of pages to the + * device structure + */ + dev->page_max = BMI270_WH_MAX_PAGE_NUM; + + /* Assign maximum number of input sensors/ + * features to device structure + */ + dev->input_sens = BMI270_WH_MAX_FEAT_IN; + + /* Assign maximum number of output sensors/ + * features to device structure + */ + dev->out_sens = BMI270_WH_MAX_FEAT_OUT; + + /* Assign the offsets of the feature interrupt + * to the device structure + */ + dev->map_int = bmi270wh_map_int; + + /* Assign maximum number of feature interrupts + * to device structure + */ + dev->sens_int_map = BMI270_WH_MAX_INT_MAP; + + /* Get the gyroscope cross axis sensitivity */ + rslt = bmi2_get_gyro_cross_sense(dev); + } + } + + return rslt; +} + +/*! + * @brief This API selects the sensors/features to be enabled. + */ +int8_t bmi270_wh_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to select sensor */ + uint64_t sensor_sel = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sens_list != NULL)) + { + /* Get the selected sensors */ + rslt = select_sensor(sens_list, n_sens, &sensor_sel); + if (rslt == BMI2_OK) + { + /* Enable the selected sensors */ + rslt = sensor_enable(sensor_sel, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API selects the sensors/features to be disabled. + */ +int8_t bmi270_wh_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to select sensor */ + uint64_t sensor_sel = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sens_list != NULL)) + { + /* Get the selected sensors */ + rslt = select_sensor(sens_list, n_sens, &sensor_sel); + if (rslt == BMI2_OK) + { + /* Disable the selected sensors */ + rslt = sensor_disable(sensor_sel, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API sets the sensor/feature configuration. + */ +int8_t bmi270_wh_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define loop */ + uint8_t loop; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sens_cfg != NULL)) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + + for (loop = 0; loop < n_sens; loop++) + { + if ((sens_cfg[loop].type == BMI2_ACCEL) || (sens_cfg[loop].type == BMI2_GYRO) || + (sens_cfg[loop].type == BMI2_AUX) || (sens_cfg[loop].type == BMI2_GYRO_GAIN_UPDATE)) + { + rslt = bmi2_set_sensor_config(&sens_cfg[loop], 1, dev); + } + else + { + /* Disable Advance power save if enabled for auxiliary + * and feature configurations + */ + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if + * enabled + */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + if (rslt == BMI2_OK) + { + switch (sens_cfg[loop].type) + { + /* Set any motion configuration */ + case BMI2_ANY_MOTION: + rslt = set_any_motion_config(&sens_cfg[loop].cfg.any_motion, dev); + break; + + /* Set no motion configuration */ + case BMI2_NO_MOTION: + rslt = set_no_motion_config(&sens_cfg[loop].cfg.no_motion, dev); + break; + + /* Set the step counter parameters */ + case BMI2_STEP_COUNTER_PARAMS: + rslt = set_step_count_params_config(sens_cfg[loop].cfg.step_counter_params, dev); + break; + + /* Set step counter/detector/activity configuration */ + case BMI2_STEP_DETECTOR: + case BMI2_STEP_COUNTER: + case BMI2_STEP_ACTIVITY: + rslt = set_step_config(&sens_cfg[loop].cfg.step_counter, dev); + break; + + /* Set the wrist wear wake-up configuration for wearable variant */ + case BMI2_WRIST_WEAR_WAKE_UP_WH: + rslt = set_wrist_wear_wake_up_wh_config(&sens_cfg[loop].cfg.wrist_wear_wake_up_wh, dev); + break; + + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + } + + /* Return error if any of the set configurations fail */ + if (rslt != BMI2_OK) + { + break; + } + } + } + + /* Enable Advance power save if disabled while configuring and + * not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API gets the sensor/feature configuration. + */ +int8_t bmi270_wh_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define loop */ + uint8_t loop; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sens_cfg != NULL)) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + for (loop = 0; loop < n_sens; loop++) + { + if ((sens_cfg[loop].type == BMI2_ACCEL) || (sens_cfg[loop].type == BMI2_GYRO) || + (sens_cfg[loop].type == BMI2_AUX) || (sens_cfg[loop].type == BMI2_GYRO_GAIN_UPDATE)) + { + rslt = bmi2_get_sensor_config(&sens_cfg[loop], 1, dev); + } + else + { + /* Disable Advance power save if enabled for auxiliary + * and feature configurations + */ + if ((sens_cfg[loop].type >= BMI2_MAIN_SENS_MAX_NUM) || (sens_cfg[loop].type == BMI2_AUX)) + { + + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if + * enabled + */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + } + + if (rslt == BMI2_OK) + { + switch (sens_cfg[loop].type) + { + /* Get any motion configuration */ + case BMI2_ANY_MOTION: + rslt = get_any_motion_config(&sens_cfg[loop].cfg.any_motion, dev); + break; + + /* Get no motion configuration */ + case BMI2_NO_MOTION: + rslt = get_no_motion_config(&sens_cfg[loop].cfg.no_motion, dev); + break; + + /* Set the step counter parameters */ + case BMI2_STEP_COUNTER_PARAMS: + rslt = get_step_count_params_config(sens_cfg[loop].cfg.step_counter_params, dev); + break; + + /* Get step counter/detector/activity configuration */ + case BMI2_STEP_DETECTOR: + case BMI2_STEP_COUNTER: + case BMI2_STEP_ACTIVITY: + rslt = get_step_config(&sens_cfg[loop].cfg.step_counter, dev); + break; + + /* Get the wrist wear wake-up configuration for wearable variant */ + case BMI2_WRIST_WEAR_WAKE_UP_WH: + rslt = get_wrist_wear_wake_up_wh_config(&sens_cfg[loop].cfg.wrist_wear_wake_up_wh, dev); + break; + + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + } + + /* Return error if any of the get configurations fail */ + if (rslt != BMI2_OK) + { + break; + } + } + } + + /* Enable Advance power save if disabled while configuring and + * not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API gets the sensor/feature data for accelerometer, gyroscope, + * auxiliary sensor, step counter, high-g, gyroscope user-gain update, + * orientation, gyroscope cross sensitivity and error status for NVM and VFRM. + */ +int8_t bmi270_wh_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define loop */ + uint8_t loop; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sensor_data != NULL)) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + for (loop = 0; loop < n_sens; loop++) + { + if ((sensor_data[loop].type == BMI2_ACCEL) || (sensor_data[loop].type == BMI2_GYRO) || + (sensor_data[loop].type == BMI2_AUX) || (sensor_data[loop].type == BMI2_GYRO_GAIN_UPDATE) || + (sensor_data[loop].type == BMI2_GYRO_CROSS_SENSE)) + { + rslt = bmi2_get_sensor_data(&sensor_data[loop], 1, dev); + } + else + { + /* Disable Advance power save if enabled for feature + * configurations + */ + if (sensor_data[loop].type >= BMI2_MAIN_SENS_MAX_NUM) + { + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if + * enabled + */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + } + + if (rslt == BMI2_OK) + { + switch (sensor_data[loop].type) + { + case BMI2_STEP_COUNTER: + + /* Get step counter output */ + rslt = get_step_counter_output(&sensor_data[loop].sens_data.step_counter_output, dev); + break; + case BMI2_STEP_ACTIVITY: + + /* Get step activity output */ + rslt = get_step_activity_output(&sensor_data[loop].sens_data.activity_output, dev); + break; + case BMI2_NVM_STATUS: + + /* Get NVM error status */ + rslt = get_nvm_error_status(&sensor_data[loop].sens_data.nvm_status, dev); + break; + case BMI2_VFRM_STATUS: + + /* Get VFRM error status */ + rslt = get_vfrm_error_status(&sensor_data[loop].sens_data.vfrm_status, dev); + break; + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + + /* Return error if any of the get sensor data fails */ + if (rslt != BMI2_OK) + { + break; + } + } + } + + /* Enable Advance power save if disabled while + * configuring and not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API updates the gyroscope user-gain. + */ +int8_t bmi270_wh_update_gyro_user_gain(const struct bmi2_gyro_user_gain_config *user_gain, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to select sensor */ + uint8_t sens_sel[2] = { BMI2_GYRO, BMI2_GYRO_GAIN_UPDATE }; + + /* Structure to define sensor configurations */ + struct bmi2_sens_config sens_cfg; + + /* Variable to store status of user-gain update module */ + uint8_t status = 0; + + /* Variable to define count */ + uint8_t count = 100; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (user_gain != NULL)) + { + /* Select type of feature */ + sens_cfg.type = BMI2_GYRO_GAIN_UPDATE; + + /* Get the user gain configurations */ + rslt = bmi270_wh_get_sensor_config(&sens_cfg, 1, dev); + if (rslt == BMI2_OK) + { + /* Get the user-defined ratio */ + sens_cfg.cfg.gyro_gain_update = *user_gain; + + /* Set rate ratio for all axes */ + rslt = bmi270_wh_set_sensor_config(&sens_cfg, 1, dev); + } + + /* Disable gyroscope */ + if (rslt == BMI2_OK) + { + rslt = bmi270_wh_sensor_disable(&sens_sel[0], 1, dev); + } + + /* Enable gyroscope user-gain update module */ + if (rslt == BMI2_OK) + { + rslt = bmi270_wh_sensor_enable(&sens_sel[1], 1, dev); + } + + /* Set the command to trigger the computation */ + if (rslt == BMI2_OK) + { + rslt = bmi2_set_command_register(BMI2_USR_GAIN_CMD, dev); + } + + if (rslt == BMI2_OK) + { + /* Poll until enable bit of user-gain update is 0 */ + while (count--) + { + rslt = get_user_gain_upd_status(&status, dev); + if ((rslt == BMI2_OK) && (status == 0)) + { + /* Enable compensation of gain defined + * in the GAIN register + */ + rslt = enable_gyro_gain(BMI2_ENABLE, dev); + + /* Enable gyroscope */ + if (rslt == BMI2_OK) + { + rslt = bmi270_wh_sensor_enable(&sens_sel[0], 1, dev); + } + + break; + } + + dev->delay_us(10000, dev->intf_ptr); + } + + /* Return error if user-gain update is failed */ + if ((rslt == BMI2_OK) && (status != 0)) + { + rslt = BMI2_E_GYR_USER_GAIN_UPD_FAIL; + } + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API reads the compensated gyroscope user-gain values. + */ +int8_t bmi270_wh_read_gyro_user_gain(struct bmi2_gyro_user_gain_data *gyr_usr_gain, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define register data */ + uint8_t reg_data[3] = { 0 }; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (gyr_usr_gain != NULL)) + { + /* Get the gyroscope compensated gain values */ + rslt = bmi2_get_regs(BMI2_GYR_USR_GAIN_0_ADDR, reg_data, 3, dev); + if (rslt == BMI2_OK) + { + /* Gyroscope user gain correction X-axis */ + gyr_usr_gain->x = (int8_t)BMI2_GET_BIT_POS0(reg_data[0], BMI2_GYR_USR_GAIN_X); + + /* Gyroscope user gain correction Y-axis */ + gyr_usr_gain->y = (int8_t)BMI2_GET_BIT_POS0(reg_data[1], BMI2_GYR_USR_GAIN_Y); + + /* Gyroscope user gain correction z-axis */ + gyr_usr_gain->z = (int8_t)BMI2_GET_BIT_POS0(reg_data[2], BMI2_GYR_USR_GAIN_Z); + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API maps/unmaps feature interrupts to that of interrupt pins. + */ +int8_t bmi270_wh_map_feat_int(const struct bmi2_sens_int_config *sens_int, uint8_t n_sens, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define loop */ + uint8_t loop; + + /* Null-pointer check */ + rslt = null_ptr_check(dev); + if ((rslt == BMI2_OK) && (sens_int != NULL)) + { + for (loop = 0; loop < n_sens; loop++) + { + switch (sens_int[loop].type) + { + case BMI2_ANY_MOTION: + case BMI2_NO_MOTION: + case BMI2_STEP_COUNTER: + case BMI2_STEP_DETECTOR: + case BMI2_STEP_ACTIVITY: + case BMI2_WRIST_WEAR_WAKE_UP_WH: + + rslt = bmi2_map_feat_int(sens_int[loop].type, sens_int[loop].hw_int_pin, dev); + break; + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + + /* Return error if interrupt mapping fails */ + if (rslt != BMI2_OK) + { + break; + } + } + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/***************************************************************************/ + +/*! Local Function Definitions + ****************************************************************************/ + +/*! + * @brief This internal API is used to validate the device structure pointer for + * null conditions. + */ +static int8_t null_ptr_check(const struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + if ((dev == NULL) || (dev->read == NULL) || (dev->write == NULL) || (dev->delay_us == NULL)) + { + /* Device structure pointer is not valid */ + rslt = BMI2_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This internal API selects the sensor/features to be enabled or + * disabled. + */ +static int8_t select_sensor(const uint8_t *sens_list, uint8_t n_sens, uint64_t *sensor_sel) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Variable to define loop */ + uint8_t count; + + for (count = 0; count < n_sens; count++) + { + switch (sens_list[count]) + { + case BMI2_ACCEL: + *sensor_sel |= BMI2_ACCEL_SENS_SEL; + break; + case BMI2_GYRO: + *sensor_sel |= BMI2_GYRO_SENS_SEL; + break; + case BMI2_AUX: + *sensor_sel |= BMI2_AUX_SENS_SEL; + break; + case BMI2_TEMP: + *sensor_sel |= BMI2_TEMP_SENS_SEL; + break; + case BMI2_ANY_MOTION: + *sensor_sel |= BMI2_ANY_MOT_SEL; + break; + case BMI2_NO_MOTION: + *sensor_sel |= BMI2_NO_MOT_SEL; + break; + case BMI2_STEP_DETECTOR: + *sensor_sel |= BMI2_STEP_DETECT_SEL; + break; + case BMI2_STEP_COUNTER: + *sensor_sel |= BMI2_STEP_COUNT_SEL; + break; + case BMI2_STEP_ACTIVITY: + *sensor_sel |= BMI2_STEP_ACT_SEL; + break; + case BMI2_GYRO_GAIN_UPDATE: + *sensor_sel |= BMI2_GYRO_GAIN_UPDATE_SEL; + break; + case BMI2_WRIST_WEAR_WAKE_UP_WH: + *sensor_sel |= BMI2_WRIST_WEAR_WAKE_UP_WH_SEL; + break; + default: + rslt = BMI2_E_INVALID_SENSOR; + break; + } + } + + return rslt; +} + +/*! + * @brief This internal API enables the selected sensor/features. + */ +static int8_t sensor_enable(uint64_t sensor_sel, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store register values */ + uint8_t reg_data = 0; + + /* Variable to define loop */ + uint8_t loop = 1; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat = 0; + + rslt = bmi2_get_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + /* Enable accelerometer */ + if (sensor_sel & BMI2_ACCEL_SENS_SEL) + { + reg_data = BMI2_SET_BITS(reg_data, BMI2_ACC_EN, BMI2_ENABLE); + } + + /* Enable gyroscope */ + if (sensor_sel & BMI2_GYRO_SENS_SEL) + { + reg_data = BMI2_SET_BITS(reg_data, BMI2_GYR_EN, BMI2_ENABLE); + } + + /* Enable auxiliary sensor */ + if (sensor_sel & BMI2_AUX_SENS_SEL) + { + reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_AUX_EN, BMI2_ENABLE); + } + + /* Enable temperature sensor */ + if (sensor_sel & BMI2_TEMP_SENS_SEL) + { + reg_data = BMI2_SET_BITS(reg_data, BMI2_TEMP_EN, BMI2_ENABLE); + } + + /* Enable the sensors that are set in the power control register */ + if (sensor_sel & BMI2_MAIN_SENSORS) + { + rslt = bmi2_set_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); + } + } + + if ((rslt == BMI2_OK) && (sensor_sel & ~(BMI2_MAIN_SENSORS))) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if enabled */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + if (rslt == BMI2_OK) + { + while (loop--) + { + /* Enable any motion feature */ + if (sensor_sel & BMI2_ANY_MOT_SEL) + { + rslt = set_any_motion(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_ANY_MOT_SEL; + } + else + { + break; + } + } + + /* Enable no motion feature */ + if (sensor_sel & BMI2_NO_MOT_SEL) + { + rslt = set_no_motion(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_NO_MOT_SEL; + } + else + { + break; + } + } + + /* Enable step detector feature */ + if (sensor_sel & BMI2_STEP_DETECT_SEL) + { + rslt = set_step_detector(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_STEP_DETECT_SEL; + } + else + { + break; + } + } + + /* Enable step counter feature */ + if (sensor_sel & BMI2_STEP_COUNT_SEL) + { + rslt = set_step_counter(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_STEP_COUNT_SEL; + } + else + { + break; + } + } + + /* Enable step activity feature */ + if (sensor_sel & BMI2_STEP_ACT_SEL) + { + rslt = set_step_activity(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_STEP_ACT_SEL; + } + else + { + break; + } + } + + /* Enable gyroscope user gain */ + if (sensor_sel & BMI2_GYRO_GAIN_UPDATE_SEL) + { + rslt = set_gyro_user_gain(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_GYRO_GAIN_UPDATE_SEL; + } + else + { + break; + } + } + + /* Enable wrist wear wake-up feature for wearable variant */ + if (sensor_sel & BMI2_WRIST_WEAR_WAKE_UP_WH_SEL) + { + rslt = set_wrist_wear_wake_up_wh(BMI2_ENABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat |= BMI2_WRIST_WEAR_WAKE_UP_WH_SEL; + } + else + { + break; + } + } + } + + /* Enable Advance power save if disabled while + * configuring and not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + } + + return rslt; +} + +/*! + * @brief This internal API disables the selected sensors/features. + */ +static int8_t sensor_disable(uint64_t sensor_sel, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store register values */ + uint8_t reg_data = 0; + + /* Variable to define loop */ + uint8_t loop = 1; + + /* Variable to get the status of advance power save */ + uint8_t aps_stat = 0; + + rslt = bmi2_get_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + /* Disable accelerometer */ + if (sensor_sel & BMI2_ACCEL_SENS_SEL) + { + reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_ACC_EN); + } + + /* Disable gyroscope */ + if (sensor_sel & BMI2_GYRO_SENS_SEL) + { + reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_GYR_EN); + } + + /* Disable auxiliary sensor */ + if (sensor_sel & BMI2_AUX_SENS_SEL) + { + reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_AUX_EN); + } + + /* Disable temperature sensor */ + if (sensor_sel & BMI2_TEMP_SENS_SEL) + { + reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_TEMP_EN); + } + + /* Disable the sensors that are set in the power control register */ + if (sensor_sel & BMI2_MAIN_SENSORS) + { + rslt = bmi2_set_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); + } + } + + if ((rslt == BMI2_OK) && (sensor_sel & ~(BMI2_MAIN_SENSORS))) + { + /* Get status of advance power save mode */ + aps_stat = dev->aps_status; + if (aps_stat == BMI2_ENABLE) + { + /* Disable advance power save if enabled */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + if (rslt == BMI2_OK) + { + while (loop--) + { + /* Disable any-motion feature */ + if (sensor_sel & BMI2_ANY_MOT_SEL) + { + rslt = set_any_motion(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat &= ~BMI2_ANY_MOT_SEL; + } + else + { + break; + } + } + + /* Disable no-motion feature */ + if (sensor_sel & BMI2_NO_MOT_SEL) + { + rslt = set_no_motion(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat &= ~BMI2_NO_MOT_SEL; + } + else + { + break; + } + } + + /* Disable step detector feature */ + if (sensor_sel & BMI2_STEP_DETECT_SEL) + { + rslt = set_step_detector(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat &= ~BMI2_STEP_DETECT_SEL; + } + else + { + break; + } + } + + /* Disable step counter feature */ + if (sensor_sel & BMI2_STEP_COUNT_SEL) + { + rslt = set_step_counter(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat &= ~BMI2_STEP_COUNT_SEL; + } + else + { + break; + } + } + + /* Disable step activity feature */ + if (sensor_sel & BMI2_STEP_ACT_SEL) + { + rslt = set_step_activity(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat &= ~BMI2_STEP_ACT_SEL; + } + else + { + break; + } + } + + /* Disable gyroscope user gain */ + if (sensor_sel & BMI2_GYRO_GAIN_UPDATE_SEL) + { + rslt = set_gyro_user_gain(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat &= ~BMI2_GYRO_GAIN_UPDATE_SEL; + } + else + { + break; + } + } + + /* Disable wrist wear wake-up feature for wearable variant */ + if (sensor_sel & BMI2_WRIST_WEAR_WAKE_UP_WH_SEL) + { + rslt = set_wrist_wear_wake_up_wh(BMI2_DISABLE, dev); + if (rslt == BMI2_OK) + { + dev->sens_en_stat &= ~BMI2_WRIST_WEAR_WAKE_UP_WH_SEL; + } + else + { + break; + } + } + + /* Enable Advance power save if disabled while + * configuring and not when already disabled + */ + if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + } + } + } + + return rslt; +} + +/*! + * @brief This internal API is used to enable/disable any motion feature. + */ +static int8_t set_any_motion(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for any-motion */ + struct bmi2_feature_config any_mot_config = { 0, 0, 0 }; + + /* Search for any-motion feature and extract its configurations details */ + feat_found = bmi2_extract_input_feat_config(&any_mot_config, BMI2_ANY_MOTION, dev); + if (feat_found) + { + /* Get the configuration from the page where any-motion feature resides */ + rslt = bmi2_get_feat_config(any_mot_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of any-motion axes */ + idx = any_mot_config.start_addr + BMI2_ANY_MOT_FEAT_EN_OFFSET; + + /* Set the feature enable bit */ + feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_ANY_NO_MOT_EN, enable); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API is used to enable/disable no-motion feature. + */ +static int8_t set_no_motion(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for no-motion */ + struct bmi2_feature_config no_mot_config = { 0, 0, 0 }; + + /* Search for no-motion feature and extract its configurations details */ + feat_found = bmi2_extract_input_feat_config(&no_mot_config, BMI2_NO_MOTION, dev); + if (feat_found) + { + /* Get the configuration from the page where any/no-motion feature resides */ + rslt = bmi2_get_feat_config(no_mot_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of no-motion axes */ + idx = no_mot_config.start_addr + BMI2_NO_MOT_FEAT_EN_OFFSET; + + /* Set the feature enable bit */ + feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_ANY_NO_MOT_EN, enable); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API is used to enable/disable step detector feature. + */ +static int8_t set_step_detector(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for step detector */ + struct bmi2_feature_config step_det_config = { 0, 0, 0 }; + + /* Search for step detector feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_det_config, BMI2_STEP_DETECTOR, dev); + if (feat_found) + { + /* Get the configuration from the page where step detector feature resides */ + rslt = bmi2_get_feat_config(step_det_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of step detector */ + idx = step_det_config.start_addr + BMI2_STEP_COUNT_FEAT_EN_OFFSET; + + /* Set the feature enable bit */ + feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_STEP_DET_FEAT_EN, enable); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API is used to enable/disable step counter feature. + */ +static int8_t set_step_counter(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for step counter */ + struct bmi2_feature_config step_count_config = { 0, 0, 0 }; + + /* Search for step counter feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_count_config, BMI2_STEP_COUNTER, dev); + if (feat_found) + { + /* Get the configuration from the page where step-counter feature resides */ + rslt = bmi2_get_feat_config(step_count_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of step counter */ + idx = step_count_config.start_addr + BMI2_STEP_COUNT_FEAT_EN_OFFSET; + + /* Set the feature enable bit */ + feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_STEP_COUNT_FEAT_EN, enable); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API is used to enable/disable step activity detection. + */ +static int8_t set_step_activity(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for step activity */ + struct bmi2_feature_config step_act_config = { 0, 0, 0 }; + + /* Search for step activity feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_act_config, BMI2_STEP_ACTIVITY, dev); + if (feat_found) + { + /* Get the configuration from the page where step-activity + * feature resides + */ + rslt = bmi2_get_feat_config(step_act_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of step activity */ + idx = step_act_config.start_addr + BMI2_STEP_COUNT_FEAT_EN_OFFSET; + + /* Set the feature enable bit */ + feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_STEP_ACT_FEAT_EN, enable); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API enables the wrist wear wake up feature. + */ +static int8_t set_wrist_wear_wake_up_wh(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for wrist wear wake up */ + struct bmi2_feature_config wrist_wake_up_cfg = { 0, 0, 0 }; + + /* Search for wrist wear wake up and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&wrist_wake_up_cfg, BMI2_WRIST_WEAR_WAKE_UP_WH, dev); + if (feat_found) + { + /* Get the configuration from the page where wrist wear wake up + * feature resides + */ + rslt = bmi2_get_feat_config(wrist_wake_up_cfg.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of wrist wear wake up */ + idx = wrist_wake_up_cfg.start_addr; + + /* Set the feature enable bit */ + feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_WRIST_WEAR_WAKE_UP_FEAT_EN, enable); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API is used to enable/disable gyroscope user gain + * feature. + */ +static int8_t set_gyro_user_gain(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for gyroscope user gain */ + struct bmi2_feature_config gyr_user_gain_cfg = { 0, 0, 0 }; + + /* Search for user gain feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&gyr_user_gain_cfg, BMI2_GYRO_GAIN_UPDATE, dev); + if (feat_found) + { + /* Get the configuration from the page where user gain feature resides */ + rslt = bmi2_get_feat_config(gyr_user_gain_cfg.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of user gain */ + idx = gyr_user_gain_cfg.start_addr + BMI2_GYR_USER_GAIN_FEAT_EN_OFFSET; + + /* Set the feature enable bit */ + feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_GYR_USER_GAIN_FEAT_EN, enable); + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API sets any-motion configurations like axes select, + * duration, threshold and output-configuration. + */ +static int8_t set_any_motion_config(const struct bmi2_any_motion_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to define count */ + uint8_t index = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for any motion */ + struct bmi2_feature_config any_mot_config = { 0, 0, 0 }; + + /* Copy the feature configuration address to a local pointer */ + uint16_t *data_p = (uint16_t *) (void *)feat_config; + + /* Search for any-motion feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&any_mot_config, BMI2_ANY_MOTION, dev); + if (feat_found) + { + /* Get the configuration from the page where any-motion feature resides */ + rslt = bmi2_get_feat_config(any_mot_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for any-motion select */ + idx = any_mot_config.start_addr; + + /* Get offset in words since all the features are set in words length */ + idx = idx / 2; + + /* Set duration */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_ANY_NO_MOT_DUR, config->duration); + + /* Set x-select */ + *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_ANY_NO_MOT_X_SEL, config->select_x); + + /* Set y-select */ + *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_ANY_NO_MOT_Y_SEL, config->select_y); + + /* Set z-select */ + *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_ANY_NO_MOT_Z_SEL, config->select_z); + + /* Increment offset by 1 word to set threshold and output configuration */ + idx++; + + /* Set threshold */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_ANY_NO_MOT_THRES, config->threshold); + + /* Increment offset by 1 more word to get the total length in words */ + idx++; + + /* Get total length in bytes to copy from local pointer to the array */ + idx = (uint8_t)(idx * 2) - any_mot_config.start_addr; + + /* Copy the bytes to be set back to the array */ + for (index = 0; index < idx; index++) + { + feat_config[any_mot_config.start_addr + + index] = *((uint8_t *) data_p + any_mot_config.start_addr + index); + } + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API sets no-motion configurations like axes select, + * duration, threshold and output-configuration. + */ +static int8_t set_no_motion_config(const struct bmi2_no_motion_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to define count */ + uint8_t index = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for no-motion */ + struct bmi2_feature_config no_mot_config = { 0, 0, 0 }; + + /* Copy the feature configuration address to a local pointer */ + uint16_t *data_p = (uint16_t *) (void *)feat_config; + + /* Search for no-motion feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&no_mot_config, BMI2_NO_MOTION, dev); + if (feat_found) + { + /* Get the configuration from the page where no-motion feature resides */ + rslt = bmi2_get_feat_config(no_mot_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for no-motion select */ + idx = no_mot_config.start_addr; + + /* Get offset in words since all the features are set in words length */ + idx = idx / 2; + + /* Set duration */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_ANY_NO_MOT_DUR, config->duration); + + /* Set x-select */ + *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_ANY_NO_MOT_X_SEL, config->select_x); + + /* Set y-select */ + *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_ANY_NO_MOT_Y_SEL, config->select_y); + + /* Set z-select */ + *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_ANY_NO_MOT_Z_SEL, config->select_z); + + /* Increment offset by 1 word to set threshold and output configuration */ + idx++; + + /* Set threshold */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_ANY_NO_MOT_THRES, config->threshold); + + /* Increment offset by 1 more word to get the total length in words */ + idx++; + + /* Get total length in bytes to copy from local pointer to the array */ + idx = (uint8_t)(idx * 2) - no_mot_config.start_addr; + + /* Copy the bytes to be set back to the array */ + for (index = 0; index < idx; index++) + { + feat_config[no_mot_config.start_addr + + index] = *((uint8_t *) data_p + no_mot_config.start_addr + index); + } + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API sets step counter parameter configurations. + */ +static int8_t set_step_count_params_config(const uint16_t *step_count_params, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define index */ + uint8_t index = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for step counter parameters */ + struct bmi2_feature_config step_params_config = { 0, 0, 0 }; + + /* Variable to index the page number */ + uint8_t page_idx; + + /* Variable to define the start page */ + uint8_t start_page; + + /* Variable to define start address of the parameters */ + uint8_t start_addr; + + /* Variable to define number of bytes */ + uint8_t n_bytes = (BMI2_STEP_CNT_N_PARAMS * 2); + + /* Variable to store number of pages */ + uint8_t n_pages = (n_bytes / 16); + + /* Variable to define the end page */ + uint8_t end_page; + + /* Variable to define the remaining bytes to be read */ + uint8_t remain_len; + + /* Variable to define the maximum words(16 bytes or 8 words) to be read in a page */ + uint8_t max_len = 8; + + /* Variable index bytes in a page */ + uint8_t page_byte_idx; + + /* Variable to index the parameters */ + uint8_t param_idx = 0; + + /* Copy the feature configuration address to a local pointer */ + uint16_t *data_p = (uint16_t *) (void *)feat_config; + + /* Search for step counter parameter feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_params_config, BMI2_STEP_COUNTER_PARAMS, dev); + if (feat_found) + { + /* Get the start page for the step counter parameters */ + start_page = step_params_config.page; + + /* Get the end page for the step counter parameters */ + end_page = start_page + n_pages; + + /* Get the start address for the step counter parameters */ + start_addr = step_params_config.start_addr; + + /* Get the remaining length of bytes to be read */ + remain_len = (uint8_t)((n_bytes - (n_pages * 16)) + start_addr); + for (page_idx = start_page; page_idx <= end_page; page_idx++) + { + /* Get the configuration from the respective page */ + rslt = bmi2_get_feat_config(page_idx, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Start from address 0x00 when switched to next page */ + if (page_idx > start_page) + { + start_addr = 0; + } + + /* Remaining number of words to be read in the page */ + if (page_idx == end_page) + { + max_len = (remain_len / 2); + } + + /* Get offset in words since all the features are set in words length */ + page_byte_idx = start_addr / 2; + for (; page_byte_idx < max_len;) + { + /* Set parameters 1 to 25 */ + *(data_p + page_byte_idx) = BMI2_SET_BIT_POS0(*(data_p + page_byte_idx), + BMI2_STEP_COUNT_PARAMS, + step_count_params[param_idx]); + + /* Increment offset by 1 word to set to the next parameter */ + page_byte_idx++; + + /* Increment to next parameter */ + param_idx++; + } + + /* Get total length in bytes to copy from local pointer to the array */ + page_byte_idx = (uint8_t)(page_byte_idx * 2) - step_params_config.start_addr; + + /* Copy the bytes to be set back to the array */ + for (index = 0; index < page_byte_idx; index++) + { + feat_config[step_params_config.start_addr + + index] = *((uint8_t *) data_p + step_params_config.start_addr + index); + } + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/* @brief This internal API sets step counter configurations like water-mark + * level, reset-counter and output-configuration step detector and activity. + */ +static int8_t set_step_config(const struct bmi2_step_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to define index */ + uint8_t index = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for step counter 4 */ + struct bmi2_feature_config step_count_config = { 0, 0, 0 }; + + /* Copy the feature configuration address to a local pointer */ + uint16_t *data_p = (uint16_t *) (void *)feat_config; + + /* Search for step counter feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_count_config, BMI2_STEP_COUNTER, dev); + if (feat_found) + { + /* Get the configuration from the page where step counter resides */ + rslt = bmi2_get_feat_config(step_count_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes */ + idx = step_count_config.start_addr; + + /* Get offset in words since all the features are set in words length */ + idx = idx / 2; + + /* Set water-mark level */ + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_STEP_COUNT_WM_LEVEL, config->watermark_level); + + /* Set reset-counter */ + *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_STEP_COUNT_RST_CNT, config->reset_counter); + + /* Increment offset by 1 word to set output + * configuration of step detector and step activity + */ + idx++; + + /* Set step buffer size */ + *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_STEP_BUFFER_SIZE, config->step_buffer_size); + + /* Increment offset by 1 more word to get the total length in words */ + idx++; + + /* Get total length in bytes to copy from local pointer to the array */ + idx = (uint8_t)(idx * 2) - step_count_config.start_addr; + + /* Copy the bytes to be set back to the array */ + for (index = 0; index < idx; index++) + { + feat_config[step_count_config.start_addr + + index] = *((uint8_t *) data_p + step_count_config.start_addr + index); + } + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API sets wrist wear wake-up configurations like + * output-configuration for wearable variant. + */ +static int8_t set_wrist_wear_wake_up_wh_config(const struct bmi2_wrist_wear_wake_up_wh_config *config, + struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to define index */ + uint8_t index = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for wrist wear wake-up */ + struct bmi2_feature_config wrist_wake_up_config = { 0, 0, 0 }; + + /* Copy the feature configuration address to a local pointer */ + uint16_t *data_p = (uint16_t *) (void *)feat_config; + + /* Search for wrist wear wake-up feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&wrist_wake_up_config, BMI2_WRIST_WEAR_WAKE_UP_WH, dev); + if (feat_found) + { + /* Get the configuration from the page where wrist wear wake-up feature resides */ + rslt = bmi2_get_feat_config(wrist_wake_up_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for wrist wear wake-up select */ + idx = wrist_wake_up_config.start_addr; + + /* Get offset in words since all the features are set in words length */ + idx = idx / 2; + + *(data_p + idx) = config->min_angle_focus; + + /* Increment offset by 1 more word to set min_angle_nonfocus */ + idx++; + *(data_p + idx) = config->min_angle_nonfocus; + + /* Increment offset by 1 more word to set angle landscape right and angle landscape left */ + idx++; + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_WRIST_WAKE_UP_ANGLE_LR, config->angle_lr); + *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_WRIST_WAKE_UP_ANGLE_LL, config->angle_ll); + + /* Increment offset by 1 more word to set angle portrait down and angle portrait left */ + idx++; + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_WRIST_WAKE_UP_ANGLE_PD, config->angle_pd); + *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_WRIST_WAKE_UP_ANGLE_PU, config->angle_pu); + + /* Increment offset by 1 more word to set min duration moved and min duration quite */ + idx++; + *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_WRIST_WAKE_UP_MIN_DUR_MOVED, config->min_dur_mov); + *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_WRIST_WAKE_UP_MIN_DUR_QUITE, config->min_dur_quite); + + /* Increment offset by 1 more word to get the total length in words */ + idx++; + + /* Get total length in bytes to copy from local pointer to the array */ + idx = (uint8_t)(idx * 2) - wrist_wake_up_config.start_addr; + + /* Copy the bytes to be set back to the array */ + for (index = 0; index < idx; index++) + { + feat_config[wrist_wake_up_config.start_addr + + index] = *((uint8_t *) data_p + wrist_wake_up_config.start_addr + index); + } + + /* Set the configuration back to the page */ + rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets any-motion configurations like axes select, + * duration, threshold and output-configuration. + */ +static int8_t get_any_motion_config(struct bmi2_any_motion_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to define LSB */ + uint16_t lsb; + + /* Variable to define MSB */ + uint16_t msb; + + /* Variable to define a word */ + uint16_t lsb_msb; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for any-motion */ + struct bmi2_feature_config any_mot_config = { 0, 0, 0 }; + + /* Search for any-motion feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&any_mot_config, BMI2_ANY_MOTION, dev); + if (feat_found) + { + /* Get the configuration from the page where any-motion feature resides */ + rslt = bmi2_get_feat_config(any_mot_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for feature enable for any-motion */ + idx = any_mot_config.start_addr; + + /* Get word to calculate duration, x, y and z select */ + lsb = (uint16_t) feat_config[idx++]; + msb = ((uint16_t) feat_config[idx++] << 8); + lsb_msb = lsb | msb; + + /* Get duration */ + config->duration = lsb_msb & BMI2_ANY_NO_MOT_DUR_MASK; + + /* Get x-select */ + config->select_x = (lsb_msb & BMI2_ANY_NO_MOT_X_SEL_MASK) >> BMI2_ANY_NO_MOT_X_SEL_POS; + + /* Get y-select */ + config->select_y = (lsb_msb & BMI2_ANY_NO_MOT_Y_SEL_MASK) >> BMI2_ANY_NO_MOT_Y_SEL_POS; + + /* Get z-select */ + config->select_z = (lsb_msb & BMI2_ANY_NO_MOT_Z_SEL_MASK) >> BMI2_ANY_NO_MOT_Z_SEL_POS; + + /* Get word to calculate threshold, output configuration from the same word */ + lsb = (uint16_t) feat_config[idx++]; + msb = ((uint16_t) feat_config[idx++] << 8); + lsb_msb = lsb | msb; + + /* Get threshold */ + config->threshold = lsb_msb & BMI2_ANY_NO_MOT_THRES_MASK; + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets no-motion configurations like axes select, + * duration, threshold and output-configuration. + */ +static int8_t get_no_motion_config(struct bmi2_no_motion_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to define LSB */ + uint16_t lsb = 0; + + /* Variable to define MSB */ + uint16_t msb = 0; + + /* Variable to define a word */ + uint16_t lsb_msb = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for no-motion */ + struct bmi2_feature_config no_mot_config = { 0, 0, 0 }; + + /* Search for no-motion feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&no_mot_config, BMI2_NO_MOTION, dev); + if (feat_found) + { + /* Get the configuration from the page where no-motion feature resides */ + rslt = bmi2_get_feat_config(no_mot_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for feature enable for no-motion */ + idx = no_mot_config.start_addr; + + /* Get word to calculate duration, x, y and z select */ + lsb = (uint16_t) feat_config[idx++]; + msb = ((uint16_t) feat_config[idx++] << 8); + lsb_msb = lsb | msb; + + /* Get duration */ + config->duration = lsb_msb & BMI2_ANY_NO_MOT_DUR_MASK; + + /* Get x-select */ + config->select_x = (lsb_msb & BMI2_ANY_NO_MOT_X_SEL_MASK) >> BMI2_ANY_NO_MOT_X_SEL_POS; + + /* Get y-select */ + config->select_y = (lsb_msb & BMI2_ANY_NO_MOT_Y_SEL_MASK) >> BMI2_ANY_NO_MOT_Y_SEL_POS; + + /* Get z-select */ + config->select_z = (lsb_msb & BMI2_ANY_NO_MOT_Z_SEL_MASK) >> BMI2_ANY_NO_MOT_Z_SEL_POS; + + /* Get word to calculate threshold, output configuration from the same word */ + lsb = (uint16_t) feat_config[idx++]; + msb = ((uint16_t) feat_config[idx++] << 8); + lsb_msb = lsb | msb; + + /* Get threshold */ + config->threshold = lsb_msb & BMI2_ANY_NO_MOT_THRES_MASK; + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets step counter parameter configurations. + */ +static int8_t get_step_count_params_config(uint16_t *step_count_params, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Variable to define LSB */ + uint16_t lsb = 0; + + /* Variable to define MSB */ + uint16_t msb = 0; + + /* Variable to define a word */ + uint16_t lsb_msb = 0; + + /* Initialize feature configuration for step counter 1 */ + struct bmi2_feature_config step_params_config = { 0, 0, 0 }; + + /* Variable to index the page number */ + uint8_t page_idx; + + /* Variable to define the start page */ + uint8_t start_page; + + /* Variable to define start address of the parameters */ + uint8_t start_addr; + + /* Variable to define number of bytes */ + uint8_t n_bytes = (BMI2_STEP_CNT_N_PARAMS * 2); + + /* Variable to store number of pages */ + uint8_t n_pages = (n_bytes / 16); + + /* Variable to define the end page */ + uint8_t end_page; + + /* Variable to define the remaining bytes to be read */ + uint8_t remain_len; + + /* Variable to define the maximum words to be read in a page */ + uint8_t max_len = BMI2_FEAT_SIZE_IN_BYTES; + + /* Variable index bytes in a page */ + uint8_t page_byte_idx; + + /* Variable to index the parameters */ + uint8_t param_idx = 0; + + /* Search for step counter parameter feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_params_config, BMI2_STEP_COUNTER_PARAMS, dev); + if (feat_found) + { + /* Get the start page for the step counter parameters */ + start_page = step_params_config.page; + + /* Get the end page for the step counter parameters */ + end_page = start_page + n_pages; + + /* Get the start address for the step counter parameters */ + start_addr = step_params_config.start_addr; + + /* Get the remaining length of bytes to be read */ + remain_len = (uint8_t)((n_bytes - (n_pages * 16)) + start_addr); + for (page_idx = start_page; page_idx <= end_page; page_idx++) + { + /* Get the configuration from the respective page */ + rslt = bmi2_get_feat_config(page_idx, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Start from address 0x00 when switched to next page */ + if (page_idx > start_page) + { + start_addr = 0; + } + + /* Remaining number of bytes to be read in the page */ + if (page_idx == end_page) + { + max_len = remain_len; + } + + /* Get the offset */ + page_byte_idx = start_addr; + while (page_byte_idx < max_len) + { + /* Get word to calculate the parameter*/ + lsb = (uint16_t) feat_config[page_byte_idx++]; + if (page_byte_idx < max_len) + { + msb = ((uint16_t) feat_config[page_byte_idx++] << 8); + } + + lsb_msb = lsb | msb; + + /* Get parameters 1 to 25 */ + step_count_params[param_idx] = lsb_msb & BMI2_STEP_COUNT_PARAMS_MASK; + + /* Increment to next parameter */ + param_idx++; + } + } + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets step counter/detector/activity configurations. + */ +static int8_t get_step_config(struct bmi2_step_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to define LSB */ + uint16_t lsb = 0; + + /* Variable to define MSB */ + uint16_t msb = 0; + + /* Variable to define a word */ + uint16_t lsb_msb = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for step counter */ + struct bmi2_feature_config step_count_config = { 0, 0, 0 }; + + /* Search for step counter 4 feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&step_count_config, BMI2_STEP_COUNTER, dev); + if (feat_found) + { + /* Get the configuration from the page where step counter 4 parameter resides */ + rslt = bmi2_get_feat_config(step_count_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for feature enable for step counter/detector/activity */ + idx = step_count_config.start_addr; + + /* Get word to calculate water-mark level and reset counter */ + lsb = (uint16_t) feat_config[idx++]; + msb = ((uint16_t) feat_config[idx++] << 8); + lsb_msb = lsb | msb; + + /* Get water-mark level */ + config->watermark_level = lsb_msb & BMI2_STEP_COUNT_WM_LEVEL_MASK; + + /* Get reset counter */ + config->reset_counter = (lsb_msb & BMI2_STEP_COUNT_RST_CNT_MASK) >> BMI2_STEP_COUNT_RST_CNT_POS; + + /* Get word to calculate output configuration of step detector and activity */ + lsb = (uint16_t) feat_config[idx++]; + msb = ((uint16_t) feat_config[idx++] << 8); + lsb_msb = lsb | msb; + + config->step_buffer_size = (lsb_msb & BMI2_STEP_BUFFER_SIZE_MASK) >> BMI2_STEP_BUFFER_SIZE_POS; + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets wrist wear wake-up configurations like min_angle_focus, + * min_angle_nonfocus for wearable variant. + */ +static int8_t get_wrist_wear_wake_up_wh_config(struct bmi2_wrist_wear_wake_up_wh_config *config, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature configuration for wrist wear wake-up */ + struct bmi2_feature_config wrist_wake_up_config = { 0, 0, 0 }; + + /* Copy the feature configuration address to a local pointer */ + uint16_t *data_p = (uint16_t *) (void *)feat_config; + + /* Search for wrist wear wake-up feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&wrist_wake_up_config, BMI2_WRIST_WEAR_WAKE_UP_WH, dev); + if (feat_found) + { + /* Get the configuration from the page where wrist wear wake-up feature resides */ + rslt = bmi2_get_feat_config(wrist_wake_up_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for wrist wear wake-up select */ + idx = wrist_wake_up_config.start_addr; + + /* Get offset in words since all the features are set in words length */ + idx = idx / 2; + + config->min_angle_focus = *(data_p + idx); + + /* Increment the offset value by 1 word to get min_angle_nonfocus */ + idx++; + config->min_angle_nonfocus = *(data_p + idx); + + /* Increment the offset value by 1 word to get angle landscape right and angle landscape left */ + idx++; + config->angle_lr = BMI2_GET_BIT_POS0(*(data_p + idx), BMI2_WRIST_WAKE_UP_ANGLE_LR); + config->angle_ll = BMI2_GET_BITS(*(data_p + idx), BMI2_WRIST_WAKE_UP_ANGLE_LL); + + /* Increment the offset value by 1 word to get angle portrait down and angle portrait up */ + idx++; + config->angle_pd = BMI2_GET_BIT_POS0(*(data_p + idx), BMI2_WRIST_WAKE_UP_ANGLE_PD); + config->angle_pu = BMI2_GET_BITS(*(data_p + idx), BMI2_WRIST_WAKE_UP_ANGLE_PU); + + /* Increment the offset value by 1 word to get min duration quite and min duration moved */ + idx++; + config->min_dur_mov = BMI2_GET_BIT_POS0(*(data_p + idx), BMI2_WRIST_WAKE_UP_MIN_DUR_MOVED); + config->min_dur_quite = BMI2_GET_BITS(*(data_p + idx), BMI2_WRIST_WAKE_UP_MIN_DUR_QUITE); + + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets the output values of step counter. + */ +static int8_t get_step_counter_output(uint32_t *step_count, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variables to define index */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature output for step counter */ + struct bmi2_feature_config step_cnt_out_config = { 0, 0, 0 }; + + /* Search for step counter output feature and extract its configuration details */ + feat_found = extract_output_feat_config(&step_cnt_out_config, BMI2_STEP_COUNTER, dev); + if (feat_found) + { + /* Get the feature output configuration for step-counter */ + rslt = bmi2_get_feat_config(step_cnt_out_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for step counter output */ + idx = step_cnt_out_config.start_addr; + + /* Get the step counter output in 4 bytes */ + *step_count = (uint32_t) feat_config[idx++]; + *step_count |= ((uint32_t) feat_config[idx++] << 8); + *step_count |= ((uint32_t) feat_config[idx++] << 16); + *step_count |= ((uint32_t) feat_config[idx++] << 24); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets the error status related to NVM. + */ +static int8_t get_nvm_error_status(struct bmi2_nvm_err_status *nvm_err_stat, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variables to define index */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature output for NVM error status */ + struct bmi2_feature_config nvm_err_cfg = { 0, 0, 0 }; + + /* Search for NVM error status feature and extract its configuration details */ + feat_found = extract_output_feat_config(&nvm_err_cfg, BMI2_NVM_STATUS, dev); + if (feat_found) + { + /* Get the feature output configuration for NVM error status */ + rslt = bmi2_get_feat_config(nvm_err_cfg.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for NVM error status */ + idx = nvm_err_cfg.start_addr; + + /* Increment index to get the error status */ + idx++; + + /* Error when NVM load action fails */ + nvm_err_stat->load_error = BMI2_GET_BIT_POS0(feat_config[idx], BMI2_NVM_LOAD_ERR_STATUS); + + /* Error when NVM program action fails */ + nvm_err_stat->prog_error = BMI2_GET_BITS(feat_config[idx], BMI2_NVM_PROG_ERR_STATUS); + + /* Error when NVM erase action fails */ + nvm_err_stat->erase_error = BMI2_GET_BITS(feat_config[idx], BMI2_NVM_ERASE_ERR_STATUS); + + /* Error when NVM program limit is exceeded */ + nvm_err_stat->exceed_error = BMI2_GET_BITS(feat_config[idx], BMI2_NVM_END_EXCEED_STATUS); + + /* Error when NVM privilege mode is not acquired */ + nvm_err_stat->privil_error = BMI2_GET_BITS(feat_config[idx], BMI2_NVM_PRIV_ERR_STATUS); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API is used to get enable status of gyroscope user gain + * update. + */ +static int8_t get_user_gain_upd_status(uint8_t *status, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OK; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variable to define the array offset */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Variable to check APS status */ + uint8_t aps_stat = 0; + + /* Initialize feature configuration for gyroscope user gain */ + struct bmi2_feature_config gyr_user_gain_cfg = { 0, 0, 0 }; + + /* Search for user gain feature and extract its configuration details */ + feat_found = bmi2_extract_input_feat_config(&gyr_user_gain_cfg, BMI2_GYRO_GAIN_UPDATE, dev); + if (feat_found) + { + /* Disable advance power save */ + aps_stat = dev->aps_status; + if (aps_stat == BMI2_ENABLE) + { + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); + } + + if (rslt == BMI2_OK) + { + /* Get the configuration from the page where user gain feature resides */ + rslt = bmi2_get_feat_config(gyr_user_gain_cfg.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset for enable/disable of user gain */ + idx = gyr_user_gain_cfg.start_addr + BMI2_GYR_USER_GAIN_FEAT_EN_OFFSET; + + /* Set the feature enable status */ + *status = BMI2_GET_BITS(feat_config[idx], BMI2_GYR_USER_GAIN_FEAT_EN); + } + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + /* Enable Advance power save if disabled while configuring and not when already disabled */ + if ((rslt == BMI2_OK) && (aps_stat == BMI2_ENABLE)) + { + rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); + } + + return rslt; +} + +/*! + * @brief This internal API gets the output values of step activity. + */ +static int8_t get_step_activity_output(uint8_t *step_act, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variables to define index */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature output for step activity */ + struct bmi2_feature_config step_act_out_config = { 0, 0, 0 }; + + /* Search for step activity output feature and extract its configuration details */ + feat_found = extract_output_feat_config(&step_act_out_config, BMI2_STEP_ACTIVITY, dev); + if (feat_found) + { + /* Get the feature output configuration for step-activity */ + rslt = bmi2_get_feat_config(step_act_out_config.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for step activity output */ + idx = step_act_out_config.start_addr; + + /* Get the step activity output */ + *step_act = feat_config[idx]; + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API gets the error status related to virtual frames. + */ +static int8_t get_vfrm_error_status(struct bmi2_vfrm_err_status *vfrm_err_stat, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Array to define the feature configuration */ + uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; + + /* Variables to define index */ + uint8_t idx = 0; + + /* Variable to set flag */ + uint8_t feat_found; + + /* Initialize feature output for VFRM error status */ + struct bmi2_feature_config vfrm_err_cfg = { 0, 0, 0 }; + + /* Search for VFRM error status feature and extract its configuration details */ + feat_found = extract_output_feat_config(&vfrm_err_cfg, BMI2_VFRM_STATUS, dev); + if (feat_found) + { + /* Get the feature output configuration for VFRM error status */ + rslt = bmi2_get_feat_config(vfrm_err_cfg.page, feat_config, dev); + if (rslt == BMI2_OK) + { + /* Define the offset in bytes for VFRM error status */ + idx = vfrm_err_cfg.start_addr; + + /* Increment index to get the error status */ + idx++; + + /* Internal error while acquiring lock for FIFO */ + vfrm_err_stat->lock_error = BMI2_GET_BITS(feat_config[idx], BMI2_VFRM_LOCK_ERR_STATUS); + + /* Internal error while writing byte into FIFO */ + vfrm_err_stat->write_error = BMI2_GET_BITS(feat_config[idx], BMI2_VFRM_WRITE_ERR_STATUS); + + /* Internal error while writing into FIFO */ + vfrm_err_stat->fatal_error = BMI2_GET_BITS(feat_config[idx], BMI2_VFRM_FATAL_ERR_STATUS); + } + } + else + { + rslt = BMI2_E_INVALID_SENSOR; + } + + return rslt; +} + +/*! + * @brief This internal API enables/disables compensation of the gain defined + * in the GAIN register. + */ +static int8_t enable_gyro_gain(uint8_t enable, struct bmi2_dev *dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define register data */ + uint8_t reg_data = 0; + + rslt = bmi2_get_regs(BMI2_GYR_OFF_COMP_6_ADDR, ®_data, 1, dev); + if (rslt == BMI2_OK) + { + reg_data = BMI2_SET_BITS(reg_data, BMI2_GYR_GAIN_EN, enable); + rslt = bmi2_set_regs(BMI2_GYR_OFF_COMP_6_ADDR, ®_data, 1, dev); + } + + return rslt; +} + +/*! + * @brief This internal API is used to extract the output feature configuration + * details from the look-up table. + */ +static uint8_t extract_output_feat_config(struct bmi2_feature_config *feat_output, + uint8_t type, + const struct bmi2_dev *dev) +{ + /* Variable to define loop */ + uint8_t loop = 0; + + /* Variable to set flag */ + uint8_t feat_found = BMI2_FALSE; + + /* Search for the output feature from the output configuration array */ + while (loop < dev->out_sens) + { + if (dev->feat_output[loop].type == type) + { + *feat_output = dev->feat_output[loop]; + feat_found = BMI2_TRUE; + break; + } + + loop++; + } + + /* Return flag */ + return feat_found; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_wh.h b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_wh.h new file mode 100644 index 0000000000..d46251bbd1 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_wh.h @@ -0,0 +1,402 @@ +/** +* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. +* +* BSD-3-Clause +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* +* 3. Neither the name of the copyright holder nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, +* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING +* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +* @file bmi270_wh.h +* @date 2020-11-04 +* @version v2.63.1 +* +*/ + +/** + * \ingroup bmi2xy + * \defgroup bmi270_wh BMI270_WH + * @brief Sensor driver for BMI270_WH sensor + */ + +#ifndef BMI270_WH_H_ +#define BMI270_WH_H_ + +/*! CPP guard */ +#ifdef __cplusplus +extern "C" { +#endif + +/***************************************************************************/ + +/*! Header files + ****************************************************************************/ +#include "bmi2.h" + +/***************************************************************************/ + +/*! Macro definitions + ****************************************************************************/ + +/*! @name BMI270_WH Chip identifier */ +#define BMI270_WH_CHIP_ID UINT8_C(0x24) + +/*! @name BMI270_WH feature input start addresses */ +#define BMI270_WH_CONFIG_ID_STRT_ADDR UINT8_C(0x00) +#define BMI270_WH_MAX_BURST_LEN_STRT_ADDR UINT8_C(0x02) +#define BMI270_WH_AXIS_MAP_STRT_ADDR UINT8_C(0x04) +#define BMI270_WH_NVM_PROG_PREP_STRT_ADDR UINT8_C(0x05) +#define BMI270_WH_GYRO_GAIN_UPDATE_STRT_ADDR UINT8_C(0x06) +#define BMI270_WH_ANY_MOT_STRT_ADDR UINT8_C(0x0C) +#define BMI270_WH_NO_MOT_STRT_ADDR UINT8_C(0x00) +#define BMI270_WH_STEP_CNT_1_STRT_ADDR UINT8_C(0x00) +#define BMI270_WH_STEP_CNT_4_STRT_ADDR UINT8_C(0x02) +#define BMI270_WH_WRIST_WEAR_WAKE_UP_STRT_ADDR UINT8_C(0x04) + +/*! @name BMI270_WH feature output start addresses */ +#define BMI270_WH_STEP_CNT_OUT_STRT_ADDR UINT8_C(0x00) +#define BMI270_WH_STEP_ACT_OUT_STRT_ADDR UINT8_C(0x04) +#define BMI270_WH_WRIST_GEST_OUT_STRT_ADDR UINT8_C(0x06) +#define BMI270_WH_GYR_USER_GAIN_OUT_STRT_ADDR UINT8_C(0x08) +#define BMI270_WH_GYRO_CROSS_SENSE_STRT_ADDR UINT8_C(0x0C) +#define BMI270_WH_NVM_VFRM_OUT_STRT_ADDR UINT8_C(0x0E) + +/*! @name Defines maximum number of pages */ +#define BMI270_WH_MAX_PAGE_NUM UINT8_C(8) + +/*! @name Defines maximum number of feature input configurations */ +#define BMI270_WH_MAX_FEAT_IN UINT8_C(12) + +/*! @name Defines maximum number of feature outputs */ +#define BMI270_WH_MAX_FEAT_OUT UINT8_C(6) + +/*! @name Mask definitions for feature interrupt status bits */ +#define BMI270_WH_STEP_CNT_STATUS_MASK UINT8_C(0x02) +#define BMI270_WH_STEP_ACT_STATUS_MASK UINT8_C(0x04) +#define BMI270_WH_WRIST_WEAR_WAKEUP_WH_STATUS_MASK UINT8_C(0x08) +#define BMI270_WH_NO_MOT_STATUS_MASK UINT8_C(0x20) +#define BMI270_WH_ANY_MOT_STATUS_MASK UINT8_C(0x40) + +/*! @name Mask definitions for feature interrupt mapping bits */ +#define BMI270_WH_INT_STEP_COUNTER_MASK UINT8_C(0x02) +#define BMI270_WH_INT_STEP_DETECTOR_MASK UINT8_C(0x02) +#define BMI270_WH_INT_STEP_ACT_MASK UINT8_C(0x04) +#define BMI270_WH_INT_WRIST_WEAR_WAKEUP_WH_MASK UINT8_C(0x08) +#define BMI270_WH_INT_NO_MOT_MASK UINT8_C(0x20) +#define BMI270_WH_INT_ANY_MOT_MASK UINT8_C(0x40) + +/*! @name Defines maximum number of feature interrupts */ +#define BMI270_WH_MAX_INT_MAP UINT8_C(6) + +/***************************************************************************/ + +/*! BMI270_WH User Interface function prototypes + ****************************************************************************/ + +/** + * \ingroup bmi270_wh + * \defgroup bmi270_whApiInit Initialization + * @brief Initialize the sensor and device structure + */ + +/*! + * \ingroup bmi270_whApiInit + * \page bmi270_wh_api_bmi270_wh_init bmi270_wh_init + * \code + * int8_t bmi270_wh_init(struct bmi2_dev *dev); + * \endcode + * @details This API: + * 1) updates the device structure with address of the configuration file. + * 2) Initializes BMI270_WH sensor. + * 3) Writes the configuration file. + * 4) Updates the feature offset parameters in the device structure. + * 5) Updates the maximum number of pages, in the device structure. + * + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_wh_init(struct bmi2_dev *dev); + +/** + * \ingroup bmi270_wh + * \defgroup bmi270_whApiSensor Feature Set + * @brief Enable / Disable features of the sensor + */ + +/*! + * \ingroup bmi270_whApiSensor + * \page bmi270_wh_api_bmi270_wh_sensor_enable bmi270_wh_sensor_enable + * \code + * int8_t bmi270_wh_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); + * \endcode + * @details This API selects the sensors/features to be enabled. + * + * @param[in] sens_list : Pointer to select the sensor/feature. + * @param[in] n_sens : Number of sensors selected. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @note Sensors/features that can be enabled. + * + *@verbatim + * sens_list | Values + * ----------------------------|----------- + * BMI2_ACCEL | 0 + * BMI2_GYRO | 1 + * BMI2_AUX | 2 + * BMI2_ANY_MOTION | 4 + * BMI2_NO_MOTION | 5 + * BMI2_STEP_DETECTOR | 6 + * BMI2_STEP_COUNTER | 7 + * BMI2_STEP_ACTIVITY | 8 + * BMI2_GYRO_GAIN_UPDATE | 9 + * BMI2_WRIST_WEAR_WAKE_UP_WH | 21 + *@endverbatim + * + * @note : + * example uint8_t sens_list[2] = {BMI2_ACCEL, BMI2_GYRO}; + * uint8_t n_sens = 2; + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_wh_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); + +/*! + * \ingroup bmi270_whApiSensor + * \page bmi270_wh_api_bmi270_wh_sensor_disable bmi270_wh_sensor_disable + * \code + * int8_t bmi270_wh_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); + * \endcode + * @details This API selects the sensors/features to be disabled. + * + * @param[in] sens_list : Pointer to select the sensor/feature. + * @param[in] n_sens : Number of sensors selected. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @note Sensors/features that can be disabled. + * + *@verbatim + * sens_list | Values + * ----------------------------|----------- + * BMI2_ACCEL | 0 + * BMI2_GYRO | 1 + * BMI2_AUX | 2 + * BMI2_ANY_MOTION | 4 + * BMI2_NO_MOTION | 5 + * BMI2_STEP_DETECTOR | 6 + * BMI2_STEP_COUNTER | 7 + * BMI2_STEP_ACTIVITY | 8 + * BMI2_GYRO_GAIN_UPDATE | 9 + * BMI2_WRIST_WEAR_WAKE_UP_WH | 21 + *@endverbatim + * + * @note : + * example uint8_t sens_list[2] = {BMI2_ACCEL, BMI2_GYRO}; + * uint8_t n_sens = 2; + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_wh_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); + +/** + * \ingroup bmi270_wh + * \defgroup bmi270_whApiSensorC Sensor Configuration + * @brief Enable / Disable feature configuration of the sensor + */ + +/*! + * \ingroup bmi270_whApiSensorC + * \page bmi270_wh_api_bmi270_wh_set_sensor_config bmi270_wh_set_sensor_config + * \code + * int8_t bmi270_wh_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); + * \endcode + * @details This API sets the sensor/feature configuration. + * + * @param[in] sens_cfg : Structure instance of bmi2_sens_config. + * @param[in] n_sens : Number of sensors selected. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @note Sensors/features that can be configured + * + *@verbatim + * sens_list | Values + * ----------------------------|----------- + * BMI2_ANY_MOTION | 4 + * BMI2_NO_MOTION | 5 + * BMI2_STEP_DETECTOR | 6 + * BMI2_STEP_COUNTER | 7 + * BMI2_STEP_ACTIVITY | 8 + * BMI2_WRIST_WEAR_WAKE_UP_WH | 21 + * BMI2_STEP_COUNTER_PARAMS | 28 + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_wh_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); + +/*! + * \ingroup bmi270_whApiSensorC + * \page bmi270_wh_api_bmi270_wh_get_sensor_config bmi270_wh_get_sensor_config + * \code + * int8_t bmi270_wh_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); + * \endcode + * @details This API gets the sensor/feature configuration. + * + * @param[in] sens_cfg : Structure instance of bmi2_sens_config. + * @param[in] n_sens : Number of sensors selected. + * @param[in, out] dev : Structure instance of bmi2_dev. + * + * @note Sensors/features whose configurations can be read. + * + *@verbatim + * sens_list | Values + * ----------------------------|----------- + * BMI2_ANY_MOTION | 4 + * BMI2_NO_MOTION | 5 + * BMI2_STEP_DETECTOR | 6 + * BMI2_STEP_COUNTER | 7 + * BMI2_STEP_ACTIVITY | 8 + * BMI2_WRIST_WEAR_WAKE_UP_WH | 21 + * BMI2_STEP_COUNTER_PARAMS | 28 + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_wh_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); + +/** + * \ingroup bmi270_wh + * \defgroup bmi270_whApiSensorD Sensor Data + * @brief Get sensor data + */ + +/*! + * \ingroup bmi270_whApiSensorD + * \page bmi270_wh_api_bmi270_wh_get_sensor_data bmi270_wh_get_sensor_data + * \code + * int8_t bmi270_wh_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev); + * \endcode + * @details This API gets the sensor/feature data for accelerometer, gyroscope, + * auxiliary sensor, step counter, high-g, gyroscope user-gain update, + * orientation, gyroscope cross sensitivity and error status for NVM and VFRM. + * + * @param[out] sensor_data : Structure instance of bmi2_sensor_data. + * @param[in] n_sens : Number of sensors selected. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @note Sensors/features whose data can be read + * + *@verbatim + * sens_list | Values + * ---------------------|----------- + * BMI2_STEP_COUNTER | 7 + * BMI2_STEP_ACTIVITY | 8 + * BMI2_NVM_STATUS | 38 + * BMI2_VFRM_STATUS | 39 + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_wh_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev); + +/** + * \ingroup bmi270_wh + * \defgroup bmi270_whApiGyroUG Gyro User Gain + * @brief Set / Get Gyro User Gain of the sensor + */ + +/*! + * \ingroup bmi270_whApiGyroUG + * \page bmi270_wh_api_bmi270_wh_update_gyro_user_gain bmi270_wh_update_gyro_user_gain + * \code + * int8_t bmi270_wh_update_gyro_user_gain(const struct bmi2_gyro_user_gain_config *user_gain, struct bmi2_dev *dev); + * \endcode + * @details This API updates the gyroscope user-gain. + * + * @param[in] user_gain : Structure that stores user-gain configurations. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_wh_update_gyro_user_gain(const struct bmi2_gyro_user_gain_config *user_gain, struct bmi2_dev *dev); + +/*! + * \ingroup bmi270_whApiGyroUG + * \page bmi270_wh_api_bmi270_wh_read_gyro_user_gain bmi270_wh_read_gyro_user_gain + * \code + * int8_t bmi270_wh_read_gyro_user_gain(struct bmi2_gyro_user_gain_data *gyr_usr_gain, const struct bmi2_dev *dev); + * \endcode + * @details This API reads the compensated gyroscope user-gain values. + * + * @param[out] gyr_usr_gain : Structure that stores gain values. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_wh_read_gyro_user_gain(struct bmi2_gyro_user_gain_data *gyr_usr_gain, struct bmi2_dev *dev); + +/*! + * \ingroup bmi270_whApiInt + * \page bmi270_wh_api_bmi270_wh_map_feat_int bmi270_wh_map_feat_int + * \code + * int8_t bmi270_wh_map_feat_int(const struct bmi2_sens_int_config *sens_int, uint8_t n_sens, struct bmi2_dev *dev) + * \endcode + * @details This API maps/unmaps feature interrupts to that of interrupt pins. + * + * @param[in] sens_int : Structure instance of bmi2_sens_int_config. + * @param[in] n_sens : Number of interrupts to be mapped. + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi270_wh_map_feat_int(const struct bmi2_sens_int_config *sens_int, uint8_t n_sens, struct bmi2_dev *dev); + +/******************************************************************************/ +/*! @name C++ Guard Macros */ +/******************************************************************************/ +#ifdef __cplusplus +} +#endif /* End of CPP guard */ + +#endif /* BMI270_WH_H_ */ diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2_defs.h b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2_defs.h new file mode 100644 index 0000000000..889bf82564 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2_defs.h @@ -0,0 +1,2475 @@ +/** +* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. +* +* BSD-3-Clause +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in the +* documentation and/or other materials provided with the distribution. +* +* 3. Neither the name of the copyright holder nor the names of its +* contributors may be used to endorse or promote products derived from +* this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, +* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING +* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +* @file bmi2_defs.h +* @date 2020-11-04 +* @version v2.63.1 +* +*/ + +#ifndef BMI2_DEFS_H_ +#define BMI2_DEFS_H_ + +/******************************************************************************/ +/*! @name Header includes */ +/******************************************************************************/ +#ifdef __KERNEL__ +#include +#include +#else +#include +#include +#endif + +/******************************************************************************/ +/*! @name Common macros */ +/******************************************************************************/ +#ifdef __KERNEL__ +#if !defined(UINT8_C) && !defined(INT8_C) +#define INT8_C(x) S8_C(x) +#define UINT8_C(x) U8_C(x) +#endif + +#if !defined(UINT16_C) && !defined(INT16_C) +#define INT16_C(x) S16_C(x) +#define UINT16_C(x) U16_C(x) +#endif + +#if !defined(INT32_C) && !defined(UINT32_C) +#define INT32_C(x) S32_C(x) +#define UINT32_C(x) U32_C(x) +#endif + +#if !defined(INT64_C) && !defined(UINT64_C) +#define INT64_C(x) S64_C(x) +#define UINT64_C(x) U64_C(x) +#endif +#endif + +/*! @name C standard macros */ +#ifndef NULL +#ifdef __cplusplus +#define NULL 0 +#else +#define NULL ((void *) 0) +#endif +#endif + +/******************************************************************************/ +/*! @name General Macro Definitions */ +/******************************************************************************/ +/*! @name Utility macros */ +#define BMI2_SET_BITS(reg_data, bitname, data) \ + ((reg_data & ~(bitname##_MASK)) | \ + ((data << bitname##_POS) & bitname##_MASK)) + +#define BMI2_GET_BITS(reg_data, bitname) \ + ((reg_data & (bitname##_MASK)) >> \ + (bitname##_POS)) + +#define BMI2_SET_BIT_POS0(reg_data, bitname, data) \ + ((reg_data & ~(bitname##_MASK)) | \ + (data & bitname##_MASK)) + +#define BMI2_GET_BIT_POS0(reg_data, bitname) (reg_data & (bitname##_MASK)) +#define BMI2_SET_BIT_VAL0(reg_data, bitname) (reg_data & ~(bitname##_MASK)) + +/*! @name For getting LSB and MSB */ +#define BMI2_GET_LSB(var) (uint8_t)(var & BMI2_SET_LOW_BYTE) +#define BMI2_GET_MSB(var) (uint8_t)((var & BMI2_SET_HIGH_BYTE) >> 8) + +#ifndef BMI2_INTF_RETURN_TYPE +#define BMI2_INTF_RETURN_TYPE int8_t +#endif + +/*! @name For defining absolute values */ +#define BMI2_ABS(a) ((a) > 0 ? (a) : -(a)) + +/*! @name LSB and MSB mask definitions */ +#define BMI2_SET_LOW_BYTE UINT16_C(0x00FF) +#define BMI2_SET_HIGH_BYTE UINT16_C(0xFF00) +#define BMI2_SET_LOW_NIBBLE UINT8_C(0x0F) + +/*! @name For enable and disable */ +#define BMI2_ENABLE UINT8_C(1) +#define BMI2_DISABLE UINT8_C(0) + +/*! @name To define TRUE or FALSE */ +#define BMI2_TRUE UINT8_C(1) +#define BMI2_FALSE UINT8_C(0) + +/*! @name To define sensor interface success code */ +#define BMI2_INTF_RET_SUCCESS INT8_C(0) + +/*! @name To define success code */ +#define BMI2_OK INT8_C(0) + +/*! @name To define error codes */ +#define BMI2_E_NULL_PTR INT8_C(-1) +#define BMI2_E_COM_FAIL INT8_C(-2) +#define BMI2_E_DEV_NOT_FOUND INT8_C(-3) +#define BMI2_E_OUT_OF_RANGE INT8_C(-4) +#define BMI2_E_ACC_INVALID_CFG INT8_C(-5) +#define BMI2_E_GYRO_INVALID_CFG INT8_C(-6) +#define BMI2_E_ACC_GYR_INVALID_CFG INT8_C(-7) +#define BMI2_E_INVALID_SENSOR INT8_C(-8) +#define BMI2_E_CONFIG_LOAD INT8_C(-9) +#define BMI2_E_INVALID_PAGE INT8_C(-10) +#define BMI2_E_INVALID_FEAT_BIT INT8_C(-11) +#define BMI2_E_INVALID_INT_PIN INT8_C(-12) +#define BMI2_E_SET_APS_FAIL INT8_C(-13) +#define BMI2_E_AUX_INVALID_CFG INT8_C(-14) +#define BMI2_E_AUX_BUSY INT8_C(-15) +#define BMI2_E_SELF_TEST_FAIL INT8_C(-16) +#define BMI2_E_REMAP_ERROR INT8_C(-17) +#define BMI2_E_GYR_USER_GAIN_UPD_FAIL INT8_C(-18) +#define BMI2_E_SELF_TEST_NOT_DONE INT8_C(-19) +#define BMI2_E_INVALID_INPUT INT8_C(-20) +#define BMI2_E_INVALID_STATUS INT8_C(-21) +#define BMI2_E_CRT_ERROR INT8_C(-22) +#define BMI2_E_ST_ALREADY_RUNNING INT8_C(-23) +#define BMI2_E_CRT_READY_FOR_DL_FAIL_ABORT INT8_C(-24) +#define BMI2_E_DL_ERROR INT8_C(-25) +#define BMI2_E_PRECON_ERROR INT8_C(-26) +#define BMI2_E_ABORT_ERROR INT8_C(-27) +#define BMI2_E_GYRO_SELF_TEST_ERROR INT8_C(-28) +#define BMI2_E_GYRO_SELF_TEST_TIMEOUT INT8_C(-29) +#define BMI2_E_WRITE_CYCLE_ONGOING INT8_C(-30) +#define BMI2_E_WRITE_CYCLE_TIMEOUT INT8_C(-31) +#define BMI2_E_ST_NOT_RUNING INT8_C(-32) +#define BMI2_E_DATA_RDY_INT_FAILED INT8_C(-33) +#define BMI2_E_INVALID_FOC_POSITION INT8_C(-34) + +/*! @name To define warnings for FIFO activity */ +#define BMI2_W_FIFO_EMPTY INT8_C(1) +#define BMI2_W_PARTIAL_READ INT8_C(2) + +/*! @name Bit wise to define information */ +#define BMI2_I_MIN_VALUE UINT8_C(1) +#define BMI2_I_MAX_VALUE UINT8_C(2) + +/*! @name BMI2 register addresses */ +#define BMI2_CHIP_ID_ADDR UINT8_C(0x00) +#define BMI2_STATUS_ADDR UINT8_C(0x03) +#define BMI2_AUX_X_LSB_ADDR UINT8_C(0x04) +#define BMI2_ACC_X_LSB_ADDR UINT8_C(0x0C) +#define BMI2_GYR_X_LSB_ADDR UINT8_C(0x12) +#define BMI2_EVENT_ADDR UINT8_C(0x1B) +#define BMI2_INT_STATUS_0_ADDR UINT8_C(0x1C) +#define BMI2_INT_STATUS_1_ADDR UINT8_C(0x1D) +#define BMI2_SC_OUT_0_ADDR UINT8_C(0x1E) +#define BMI2_SYNC_COMMAND_ADDR UINT8_C(0x1E) +#define BMI2_GYR_CAS_GPIO0_ADDR UINT8_C(0x1E) +#define BMI2_INTERNAL_STATUS_ADDR UINT8_C(0x21) +#define BMI2_FIFO_LENGTH_0_ADDR UINT8_C(0X24) +#define BMI2_FIFO_DATA_ADDR UINT8_C(0X26) +#define BMI2_FEAT_PAGE_ADDR UINT8_C(0x2F) +#define BMI2_FEATURES_REG_ADDR UINT8_C(0x30) +#define BMI2_ACC_CONF_ADDR UINT8_C(0x40) +#define BMI2_GYR_CONF_ADDR UINT8_C(0x42) +#define BMI2_AUX_CONF_ADDR UINT8_C(0x44) +#define BMI2_FIFO_DOWNS_ADDR UINT8_C(0X45) +#define BMI2_FIFO_WTM_0_ADDR UINT8_C(0X46) +#define BMI2_FIFO_WTM_1_ADDR UINT8_C(0X47) +#define BMI2_FIFO_CONFIG_0_ADDR UINT8_C(0X48) +#define BMI2_FIFO_CONFIG_1_ADDR UINT8_C(0X49) +#define BMI2_AUX_DEV_ID_ADDR UINT8_C(0x4B) +#define BMI2_AUX_IF_CONF_ADDR UINT8_C(0x4C) +#define BMI2_AUX_RD_ADDR UINT8_C(0x4D) +#define BMI2_AUX_WR_ADDR UINT8_C(0x4E) +#define BMI2_AUX_WR_DATA_ADDR UINT8_C(0x4F) +#define BMI2_INT1_IO_CTRL_ADDR UINT8_C(0x53) +#define BMI2_INT2_IO_CTRL_ADDR UINT8_C(0x54) +#define BMI2_INT_LATCH_ADDR UINT8_C(0x55) +#define BMI2_INT1_MAP_FEAT_ADDR UINT8_C(0x56) +#define BMI2_INT2_MAP_FEAT_ADDR UINT8_C(0x57) +#define BMI2_INT_MAP_DATA_ADDR UINT8_C(0x58) +#define BMI2_INIT_CTRL_ADDR UINT8_C(0x59) +#define BMI2_INIT_ADDR_0 UINT8_C(0x5B) +#define BMI2_INIT_ADDR_1 UINT8_C(0x5C) +#define BMI2_INIT_DATA_ADDR UINT8_C(0x5E) +#define BMI2_AUX_IF_TRIM UINT8_C(0x68) +#define BMI2_GYR_CRT_CONF_ADDR UINT8_C(0X69) +#define BMI2_NVM_CONF_ADDR UINT8_C(0x6A) +#define BMI2_IF_CONF_ADDR UINT8_C(0X6B) +#define BMI2_ACC_SELF_TEST_ADDR UINT8_C(0X6D) +#define BMI2_GYR_SELF_TEST_AXES_ADDR UINT8_C(0x6E) +#define BMI2_SELF_TEST_MEMS_ADDR UINT8_C(0X6F) +#define BMI2_NV_CONF_ADDR UINT8_C(0x70) +#define BMI2_ACC_OFF_COMP_0_ADDR UINT8_C(0X71) +#define BMI2_GYR_OFF_COMP_3_ADDR UINT8_C(0X74) +#define BMI2_GYR_OFF_COMP_6_ADDR UINT8_C(0X77) +#define BMI2_GYR_USR_GAIN_0_ADDR UINT8_C(0X78) +#define BMI2_PWR_CONF_ADDR UINT8_C(0x7C) +#define BMI2_PWR_CTRL_ADDR UINT8_C(0x7D) +#define BMI2_CMD_REG_ADDR UINT8_C(0x7E) + +/*! @name BMI2 I2C address */ +#define BMI2_I2C_PRIM_ADDR UINT8_C(0x68) +#define BMI2_I2C_SEC_ADDR UINT8_C(0x69) + +/*! @name BMI2 Commands */ +#define BMI2_G_TRIGGER_CMD UINT8_C(0x02) +#define BMI2_USR_GAIN_CMD UINT8_C(0x03) +#define BMI2_NVM_PROG_CMD UINT8_C(0xA0) +#define BMI2_SOFT_RESET_CMD UINT8_C(0xB6) +#define BMI2_FIFO_FLUSH_CMD UINT8_C(0xB0) + +/*! @name BMI2 sensor data bytes */ + +#define BMI2_ACC_GYR_NUM_BYTES UINT8_C(6) +#define BMI2_AUX_NUM_BYTES UINT8_C(8) +#define BMI2_CRT_CONFIG_FILE_SIZE UINT16_C(2048) +#define BMI2_FEAT_SIZE_IN_BYTES UINT8_C(16) +#define BMI2_ACC_CONFIG_LENGTH UINT8_C(2) + +/*! @name BMI2 configuration load status */ +#define BMI2_CONFIG_LOAD_SUCCESS UINT8_C(1) + +/*! @name To define BMI2 pages */ +#define BMI2_PAGE_0 UINT8_C(0) +#define BMI2_PAGE_1 UINT8_C(1) +#define BMI2_PAGE_2 UINT8_C(2) +#define BMI2_PAGE_3 UINT8_C(3) +#define BMI2_PAGE_4 UINT8_C(4) +#define BMI2_PAGE_5 UINT8_C(5) +#define BMI2_PAGE_6 UINT8_C(6) +#define BMI2_PAGE_7 UINT8_C(7) + +/*! @name Array Parameter DefinItions */ +#define BMI2_SENSOR_TIME_LSB_BYTE UINT8_C(0) +#define BMI2_SENSOR_TIME_XLSB_BYTE UINT8_C(1) +#define BMI2_SENSOR_TIME_MSB_BYTE UINT8_C(2) + +/*! @name Mask definitions for Gyro CRT */ +#define BMI2_GYR_RDY_FOR_DL_MASK UINT8_C(0x08) +#define BMI2_GYR_CRT_RUNNING_MASK UINT8_C(0x04) + +/*! @name mask definition for status register */ +#define BMI2_AUX_BUSY_MASK UINT8_C(0x04) +#define BMI2_CMD_RDY_MASK UINT8_C(0x10) +#define BMI2_DRDY_AUX_MASK UINT8_C(0x20) +#define BMI2_DRDY_GYR_MASK UINT8_C(0x40) +#define BMI2_DRDY_ACC_MASK UINT8_C(0x80) + +/*! @name Mask definitions for SPI read/write address */ +#define BMI2_SPI_RD_MASK UINT8_C(0x80) +#define BMI2_SPI_WR_MASK UINT8_C(0x7F) + +/*! @name Mask definitions for power configuration register */ +#define BMI2_ADV_POW_EN_MASK UINT8_C(0x01) + +/*! @name Mask definitions for initialization control register */ +#define BMI2_CONF_LOAD_EN_MASK UINT8_C(0x01) + +/*! @name Mask definitions for power control register */ +#define BMI2_AUX_EN_MASK UINT8_C(0x01) +#define BMI2_GYR_EN_MASK UINT8_C(0x02) +#define BMI2_ACC_EN_MASK UINT8_C(0x04) +#define BMI2_TEMP_EN_MASK UINT8_C(0x08) + +/*! @name Mask definitions for sensor event flags */ +#define BMI2_EVENT_FLAG_MASK UINT8_C(0x1C) + +/*! @name Mask definitions to switch page */ +#define BMI2_SWITCH_PAGE_EN_MASK UINT8_C(0x07) + +/*! @name Mask definitions of NVM register */ +#define BMI2_NV_ACC_OFFSET_MASK UINT8_C(0x08) + +/*! @name Mask definition for config version */ +#define BMI2_CONFIG_MAJOR_MASK UINT16_C(0x3C0) +#define BMI2_CONFIG_MINOR_MASK UINT8_C(0x3F) + +/*! @name mask and bit position for activity recognition settings */ +#define BMI2_ACT_RECG_POST_PROS_EN_DIS_MASK UINT8_C(0x01) +#define BMI2_ACT_RECG_BUFF_SIZE_MASK UINT8_C(0x0F) +#define BMI2_ACT_RECG_MIN_SEG_CONF_MASK UINT8_C(0x0F) + +/*! @name mask and bit position for activity recognition hc settings */ +#define BMI2_HC_ACT_RECG_SEGMENT_SIZE_MASK UINT8_C(0x03) +#define BMI2_HC_ACT_RECG_PP_EN_MASK UINT8_C(0x01) +#define BMI2_HC_ACT_RECG_MIN_GDI_THRES_MASK UINT16_C(0xFFFF) +#define BMI2_HC_ACT_RECG_MAX_GDI_THRES_MASK UINT16_C(0xFFFF) +#define BMI2_HC_ACT_RECG_BUF_SIZE_MASK UINT16_C(0xFFFF) +#define BMI2_HC_ACT_RECG_MIN_SEG_CONF_MASK UINT16_C(0xFFFF) + +#define BMI2_GYRO_CROSS_AXES_SENSE_MASK UINT8_C(0x7F) +#define BMI2_GYRO_CROSS_AXES_SENSE_SIGN_BIT_MASK UINT8_C(0x40) + +/*! @name Bit position definitions for Gyro CRT */ +#define BMI2_GYR_RDY_FOR_DL_POS UINT8_C(0x03) +#define BMI2_GYR_CRT_RUNNING_POS UINT8_C(0x02) + +/*! @name Bit position for status register*/ +#define BMI2_AUX_BUSY_POS UINT8_C(0x02) +#define BMI2_CMD_RDY_POS UINT8_C(0x04) +#define BMI2_DRDY_AUX_POS UINT8_C(0x05) +#define BMI2_DRDY_GYR_POS UINT8_C(0x06) +#define BMI2_DRDY_ACC_POS UINT8_C(0x07) + +/*! @name Bit position definitions for power control register */ +#define BMI2_GYR_EN_POS UINT8_C(0x01) +#define BMI2_ACC_EN_POS UINT8_C(0x02) +#define BMI2_TEMP_EN_POS UINT8_C(0x03) + +/*! @name Bit position definitions for sensor event flags */ +#define BMI2_EVENT_FLAG_POS UINT8_C(0x02) + +/*! @name Bit position definitions of NVM register */ +#define BMI2_NV_ACC_OFFSET_POS UINT8_C(0x03) + +/*! @name Bit position for major version from config */ +#define BMI2_CONFIG_MAJOR_POS UINT8_C(0x06) + +/*! @name Accelerometer and Gyroscope Filter/Noise performance modes */ +/* Power optimized mode */ +#define BMI2_POWER_OPT_MODE UINT8_C(0) + +/* Performance optimized */ +#define BMI2_PERF_OPT_MODE UINT8_C(1) + +/*! @name index for config major minor information */ +#define BMI2_CONFIG_INFO_LOWER UINT8_C(52) +#define BMI2_CONFIG_INFO_HIGHER UINT8_C(53) + +/*! @name Sensor status */ +#define BMI2_DRDY_ACC UINT8_C(0x80) +#define BMI2_DRDY_GYR UINT8_C(0x40) +#define BMI2_DRDY_AUX UINT8_C(0x20) +#define BMI2_CMD_RDY UINT8_C(0x10) +#define BMI2_AUX_BUSY UINT8_C(0x04) + +/*! @name Macro to define accelerometer configuration value for FOC */ +#define BMI2_FOC_ACC_CONF_VAL UINT8_C(0xB7) + +/*! @name Macro to define gyroscope configuration value for FOC */ +#define BMI2_FOC_GYR_CONF_VAL UINT8_C(0xB6) + +/*! @name Macro to define X Y and Z axis for an array */ +#define BMI2_X_AXIS UINT8_C(0) +#define BMI2_Y_AXIS UINT8_C(1) +#define BMI2_Z_AXIS UINT8_C(2) + +/******************************************************************************/ +/*! @name Sensor Macro Definitions */ +/******************************************************************************/ +/*! @name Macros to define BMI2 sensor/feature types */ +#define BMI2_ACCEL UINT8_C(0) +#define BMI2_GYRO UINT8_C(1) +#define BMI2_AUX UINT8_C(2) +#define BMI2_SIG_MOTION UINT8_C(3) +#define BMI2_ANY_MOTION UINT8_C(4) +#define BMI2_NO_MOTION UINT8_C(5) +#define BMI2_STEP_DETECTOR UINT8_C(6) +#define BMI2_STEP_COUNTER UINT8_C(7) +#define BMI2_STEP_ACTIVITY UINT8_C(8) +#define BMI2_GYRO_GAIN_UPDATE UINT8_C(9) +#define BMI2_TILT UINT8_C(10) +#define BMI2_UP_HOLD_TO_WAKE UINT8_C(11) +#define BMI2_GLANCE_DETECTOR UINT8_C(12) +#define BMI2_WAKE_UP UINT8_C(13) +#define BMI2_ORIENTATION UINT8_C(14) +#define BMI2_HIGH_G UINT8_C(15) +#define BMI2_LOW_G UINT8_C(16) +#define BMI2_FLAT UINT8_C(17) +#define BMI2_EXT_SENS_SYNC UINT8_C(18) +#define BMI2_WRIST_GESTURE UINT8_C(19) +#define BMI2_WRIST_WEAR_WAKE_UP UINT8_C(20) +#define BMI2_WRIST_WEAR_WAKE_UP_WH UINT8_C(21) +#define BMI2_WRIST_GESTURE_WH UINT8_C(22) +#define BMI2_PRIMARY_OIS UINT8_C(23) +#define BMI2_FREE_FALL_DET UINT8_C(24) +#define BMI2_SINGLE_TAP UINT8_C(25) +#define BMI2_DOUBLE_TAP UINT8_C(26) +#define BMI2_TRIPLE_TAP UINT8_C(27) +#define BMI2_TAP UINT8_C(28) + +/* Non virtual sensor features */ +#define BMI2_STEP_COUNTER_PARAMS UINT8_C(29) +#define BMI2_TAP_DETECTOR_1 UINT8_C(30) +#define BMI2_TAP_DETECTOR_2 UINT8_C(31) +#define BMI2_TEMP UINT8_C(32) +#define BMI2_ACCEL_SELF_TEST UINT8_C(33) +#define BMI2_GYRO_SELF_OFF UINT8_C(34) +#define BMI2_ACTIVITY_RECOGNITION UINT8_C(35) +#define BMI2_MAX_BURST_LEN UINT8_C(36) +#define BMI2_SENS_MAX_NUM UINT8_C(37) +#define BMI2_AXIS_MAP UINT8_C(38) +#define BMI2_NVM_STATUS UINT8_C(39) +#define BMI2_VFRM_STATUS UINT8_C(40) +#define BMI2_GYRO_CROSS_SENSE UINT8_C(41) +#define BMI2_CRT_GYRO_SELF_TEST UINT8_C(42) +#define BMI2_ABORT_CRT_GYRO_SELF_TEST UINT8_C(43) +#define BMI2_NVM_PROG_PREP UINT8_C(44) +#define BMI2_ACTIVITY_RECOGNITION_SETTINGS UINT8_C(45) +#define BMI2_OIS_OUTPUT UINT8_C(46) +#define BMI2_CONFIG_ID UINT8_C(47) + +/*! @name Bit wise for selecting BMI2 sensors/features */ +#define BMI2_ACCEL_SENS_SEL (1) +#define BMI2_GYRO_SENS_SEL (1 << BMI2_GYRO) +#define BMI2_AUX_SENS_SEL (1 << BMI2_AUX) +#define BMI2_TEMP_SENS_SEL ((uint64_t)1 << BMI2_TEMP) +#define BMI2_ANY_MOT_SEL (1 << BMI2_ANY_MOTION) +#define BMI2_NO_MOT_SEL (1 << BMI2_NO_MOTION) +#define BMI2_TILT_SEL (1 << BMI2_TILT) +#define BMI2_ORIENT_SEL (1 << BMI2_ORIENTATION) +#define BMI2_SIG_MOTION_SEL (1 << BMI2_SIG_MOTION) +#define BMI2_STEP_DETECT_SEL (1 << BMI2_STEP_DETECTOR) +#define BMI2_STEP_COUNT_SEL (1 << BMI2_STEP_COUNTER) +#define BMI2_STEP_ACT_SEL (1 << BMI2_STEP_ACTIVITY) +#define BMI2_GYRO_GAIN_UPDATE_SEL (1 << BMI2_GYRO_GAIN_UPDATE) +#define BMI2_UP_HOLD_TO_WAKE_SEL (1 << BMI2_UP_HOLD_TO_WAKE) +#define BMI2_GLANCE_DET_SEL (1 << BMI2_GLANCE_DETECTOR) +#define BMI2_WAKE_UP_SEL (1 << BMI2_WAKE_UP) +#define BMI2_TAP_SEL (1 << BMI2_TAP) +#define BMI2_HIGH_G_SEL (1 << BMI2_HIGH_G) +#define BMI2_LOW_G_SEL (1 << BMI2_LOW_G) +#define BMI2_FLAT_SEL (1 << BMI2_FLAT) +#define BMI2_EXT_SENS_SEL (1 << BMI2_EXT_SENS_SYNC) +#define BMI2_SINGLE_TAP_SEL (1 << BMI2_SINGLE_TAP) +#define BMI2_DOUBLE_TAP_SEL (1 << BMI2_DOUBLE_TAP) +#define BMI2_TRIPLE_TAP_SEL (1 << BMI2_TRIPLE_TAP) +#define BMI2_GYRO_SELF_OFF_SEL ((uint64_t)1 << BMI2_GYRO_SELF_OFF) +#define BMI2_WRIST_GEST_SEL (1 << BMI2_WRIST_GESTURE) +#define BMI2_WRIST_WEAR_WAKE_UP_SEL (1 << BMI2_WRIST_WEAR_WAKE_UP) +#define BMI2_ACTIVITY_RECOGNITION_SEL ((uint64_t)1 << BMI2_ACTIVITY_RECOGNITION) +#define BMI2_ACCEL_SELF_TEST_SEL ((uint64_t)1 << BMI2_ACCEL_SELF_TEST) +#define BMI2_WRIST_GEST_W_SEL (1 << BMI2_WRIST_GESTURE_WH) +#define BMI2_WRIST_WEAR_WAKE_UP_WH_SEL (1 << BMI2_WRIST_WEAR_WAKE_UP_WH) +#define BMI2_PRIMARY_OIS_SEL (1 << BMI2_PRIMARY_OIS) +#define BMI2_FREE_FALL_DET_SEL (1 << BMI2_FREE_FALL_DET) + +/*! @name Mask definitions for BMI2 wake-up feature configuration for bmi260 */ +#define BMI2_WAKEUP_SENSITIVITY_MASK UINT8_C(0x0E) +#define BMI2_WAKEUP_SINGLE_TAP_EN_MASK UINT8_C(0x01) +#define BMI2_WAKEUP_DOUBLE_TAP_EN_MASK UINT8_C(0x02) +#define BMI2_WAKEUP_TRIPLE_TAP_EN_MASK UINT8_C(0x04) +#define BMI2_WAKEUP_DATA_REG_EN_MASK UINT8_C(0x08) +#define BMI2_WAKEUP_AXIS_SEL_MASK UINT8_C(0x03) + +/*! @name Bit position definitions for BMI2 wake-up feature configuration for bmi260 */ +#define BMI2_WAKEUP_SENSITIVITY_POS UINT8_C(0x01) +#define BMI2_WAKEUP_DOUBLE_TAP_EN_POS UINT8_C(0x01) +#define BMI2_WAKEUP_TRIPLE_TAP_EN_POS UINT8_C(0x02) +#define BMI2_WAKEUP_DATA_REG_EN_POS UINT8_C(0x03) + +/*! @name Mask definitions for BMI2 tap feature configuration for bmi260t */ +#define BMI2_TAP_SENSITIVITY_MASK UINT8_C(0x0E) +#define BMI2_TAP_SINGLE_TAP_EN_MASK UINT8_C(0x01) +#define BMI2_TAP_DOUBLE_TAP_EN_MASK UINT8_C(0x02) +#define BMI2_TAP_TRIPLE_TAP_EN_MASK UINT8_C(0x04) +#define BMI2_TAP_DATA_REG_EN_MASK UINT8_C(0x08) +#define BMI2_TAP_AXIS_SEL_MASK UINT8_C(0x03) + +/*! @name Bit position definitions for BMI2 tap feature configuration for bmi260t */ +#define BMI2_TAP_SENSITIVITY_POS UINT8_C(0x01) +#define BMI2_TAP_DOUBLE_TAP_EN_POS UINT8_C(0x01) +#define BMI2_TAP_TRIPLE_TAP_EN_POS UINT8_C(0x02) +#define BMI2_TAP_DATA_REG_EN_POS UINT8_C(0x03) + +/*! @name Mask definitions for BMI2 wake-up feature configuration for other than bmi261 */ +#define BMI2_WAKE_UP_SENSITIVITY_MASK UINT16_C(0x000E) +#define BMI2_WAKE_UP_SINGLE_TAP_EN_MASK UINT16_C(0x0010) + +/*! @name Bit position definitions for BMI2 wake-up feature configuration for other than bmi261 */ +#define BMI2_WAKE_UP_SENSITIVITY_POS UINT8_C(0x01) +#define BMI2_WAKE_UP_SINGLE_TAP_EN_POS UINT8_C(0x04) + +/*! @name Offsets from feature start address for BMI2 feature enable/disable */ +#define BMI2_ANY_MOT_FEAT_EN_OFFSET UINT8_C(0x03) +#define BMI2_NO_MOT_FEAT_EN_OFFSET UINT8_C(0x03) +#define BMI2_SIG_MOT_FEAT_EN_OFFSET UINT8_C(0x0A) +#define BMI2_STEP_COUNT_FEAT_EN_OFFSET UINT8_C(0x01) +#define BMI2_GYR_USER_GAIN_FEAT_EN_OFFSET UINT8_C(0x05) +#define BMI2_HIGH_G_FEAT_EN_OFFSET UINT8_C(0x03) +#define BMI2_LOW_G_FEAT_EN_OFFSET UINT8_C(0x03) +#define BMI2_TILT_FEAT_EN_OFFSET UINT8_C(0x00) + +/*! @name Mask definitions for BMI2 feature enable/disable */ +#define BMI2_ANY_NO_MOT_EN_MASK UINT8_C(0x80) +#define BMI2_TILT_FEAT_EN_MASK UINT8_C(0x01) +#define BMI2_ORIENT_FEAT_EN_MASK UINT8_C(0x01) +#define BMI2_SIG_MOT_FEAT_EN_MASK UINT8_C(0x01) +#define BMI2_STEP_DET_FEAT_EN_MASK UINT8_C(0x08) +#define BMI2_STEP_COUNT_FEAT_EN_MASK UINT8_C(0x10) +#define BMI2_STEP_ACT_FEAT_EN_MASK UINT8_C(0x20) +#define BMI2_GYR_USER_GAIN_FEAT_EN_MASK UINT8_C(0x08) +#define BMI2_UP_HOLD_TO_WAKE_FEAT_EN_MASK UINT8_C(0x01) +#define BMI2_GLANCE_FEAT_EN_MASK UINT8_C(0x01) +#define BMI2_WAKE_UP_FEAT_EN_MASK UINT8_C(0x01) +#define BMI2_TAP_FEAT_EN_MASK UINT8_C(0x01) +#define BMI2_HIGH_G_FEAT_EN_MASK UINT8_C(0x80) +#define BMI2_LOW_G_FEAT_EN_MASK UINT8_C(0x10) +#define BMI2_FLAT_FEAT_EN_MASK UINT8_C(0x01) +#define BMI2_EXT_SENS_SYNC_FEAT_EN_MASK UINT8_C(0x01) +#define BMI2_GYR_SELF_OFF_CORR_FEAT_EN_MASK UINT8_C(0x02) +#define BMI2_WRIST_GEST_FEAT_EN_MASK UINT8_C(0x20) +#define BMI2_WRIST_WEAR_WAKE_UP_FEAT_EN_MASK UINT8_C(0x10) +#define BMI2_ACTIVITY_RECOG_EN_MASK UINT8_C(0x01) +#define BMI2_ACC_SELF_TEST_FEAT_EN_MASK UINT8_C(0x02) +#define BMI2_GYRO_SELF_TEST_CRT_EN_MASK UINT8_C(0x01) +#define BMI2_ABORT_FEATURE_EN_MASK UINT8_C(0x02) +#define BMI2_NVM_PREP_FEATURE_EN_MASK UINT8_C(0x04) +#define BMI2_FREE_FALL_DET_FEAT_EN_MASK UINT8_C(0x01) +#define BMI2_WRIST_GEST_WH_FEAT_EN_MASK UINT8_C(0x02) + +/*! @name Bit position definitions for BMI2 feature enable/disable */ +#define BMI2_ANY_NO_MOT_EN_POS UINT8_C(0x07) +#define BMI2_STEP_DET_FEAT_EN_POS UINT8_C(0x03) +#define BMI2_STEP_COUNT_FEAT_EN_POS UINT8_C(0x04) +#define BMI2_STEP_ACT_FEAT_EN_POS UINT8_C(0x05) +#define BMI2_GYR_USER_GAIN_FEAT_EN_POS UINT8_C(0x03) +#define BMI2_HIGH_G_FEAT_EN_POS UINT8_C(0x07) +#define BMI2_LOW_G_FEAT_EN_POS UINT8_C(0x04) +#define BMI2_GYR_SELF_OFF_CORR_FEAT_EN_POS UINT8_C(0x01) +#define BMI2_WRIST_GEST_FEAT_EN_POS UINT8_C(0x05) +#define BMI2_WRIST_WEAR_WAKE_UP_FEAT_EN_POS UINT8_C(0x04) +#define BMI2_ACC_SELF_TEST_FEAT_EN_POS UINT8_C(0x01) +#define BMI2_ABORT_FEATURE_EN_POS UINT8_C(0x1) +#define BMI2_NVM_PREP_FEATURE_EN_POS UINT8_C(0x02) +#define BMI2_WRIST_GEST_WH_FEAT_EN_POS UINT8_C(0x01) + +/*! Primary OIS low pass filter configuration position and mask */ +#define BMI2_LP_FILTER_EN_MASK UINT8_C(0x01) + +#define BMI2_LP_FILTER_CONFIG_POS UINT8_C(0x01) +#define BMI2_LP_FILTER_CONFIG_MASK UINT8_C(0x06) + +#define BMI2_PRIMARY_OIS_GYR_EN_POS UINT8_C(0x06) +#define BMI2_PRIMARY_OIS_GYR_EN_MASK UINT8_C(0x40) + +#define BMI2_PRIMARY_OIS_ACC_EN_POS UINT8_C(0x07) +#define BMI2_PRIMARY_OIS_ACC_EN_MASK UINT8_C(0x80) + +/*! @name Mask definitions for BMI2 any and no-motion feature configuration */ +#define BMI2_ANY_NO_MOT_DUR_MASK UINT16_C(0x1FFF) +#define BMI2_ANY_NO_MOT_X_SEL_MASK UINT16_C(0x2000) +#define BMI2_ANY_NO_MOT_Y_SEL_MASK UINT16_C(0x4000) +#define BMI2_ANY_NO_MOT_Z_SEL_MASK UINT16_C(0x8000) +#define BMI2_ANY_NO_MOT_THRES_MASK UINT16_C(0x07FF) +#define BMI2_ANY_MOT_INT_MASK UINT8_C(0x40) + +/*! @name Mask definitions for BMI2 no-motion interrupt mapping */ +#define BMI2_NO_MOT_INT_MASK UINT8_C(0x20) + +/*! @name Bit position definitions for BMI2 any and no-motion feature + * configuration + */ +#define BMI2_ANY_NO_MOT_X_SEL_POS UINT8_C(0x0D) +#define BMI2_ANY_NO_MOT_Y_SEL_POS UINT8_C(0x0E) +#define BMI2_ANY_NO_MOT_Z_SEL_POS UINT8_C(0x0F) + +/*! @name Mask definitions for BMI2 orientation feature configuration */ +#define BMI2_ORIENT_UP_DOWN_MASK UINT16_C(0x0002) +#define BMI2_ORIENT_SYMM_MODE_MASK UINT16_C(0x000C) +#define BMI2_ORIENT_BLOCK_MODE_MASK UINT16_C(0x0030) +#define BMI2_ORIENT_THETA_MASK UINT16_C(0x0FC0) +#define BMI2_ORIENT_HYST_MASK UINT16_C(0x07FF) + +/*! @name Bit position definitions for BMI2 orientation feature configuration */ +#define BMI2_ORIENT_UP_DOWN_POS UINT8_C(0x01) +#define BMI2_ORIENT_SYMM_MODE_POS UINT8_C(0x02) +#define BMI2_ORIENT_BLOCK_MODE_POS UINT8_C(0x04) +#define BMI2_ORIENT_THETA_POS UINT8_C(0x06) + +/*! @name Mask definitions for BMI2 sig-motion feature configuration */ +#define BMI2_SIG_MOT_PARAM_1_MASK UINT16_C(0xFFFF) +#define BMI2_SIG_MOT_PARAM_2_MASK UINT16_C(0xFFFF) +#define BMI2_SIG_MOT_PARAM_3_MASK UINT16_C(0xFFFF) +#define BMI2_SIG_MOT_PARAM_4_MASK UINT16_C(0xFFFF) +#define BMI2_SIG_MOT_PARAM_5_MASK UINT16_C(0xFFFF) + +/*! @name Mask definitions for BMI2 parameter configurations */ +#define BMI2_STEP_COUNT_PARAMS_MASK UINT16_C(0xFFFF) + +/*! @name Mask definitions for BMI2 step-counter/detector feature configuration */ +#define BMI2_STEP_COUNT_WM_LEVEL_MASK UINT16_C(0x03FF) +#define BMI2_STEP_COUNT_RST_CNT_MASK UINT16_C(0x0400) +#define BMI2_STEP_BUFFER_SIZE_MASK UINT16_C(0XFF00) +#define BMI2_STEP_COUNT_INT_MASK UINT8_C(0x02) +#define BMI2_STEP_ACT_INT_MASK UINT8_C(0x04) + +/*! @name Bit position definitions for BMI2 step-counter/detector feature + * configuration + */ +#define BMI2_STEP_COUNT_RST_CNT_POS UINT8_C(0x0A) +#define BMI2_STEP_BUFFER_SIZE_POS UINT8_C(0X08) + +/*! @name Mask definitions for BMI2 gyroscope user gain feature + * configuration + */ +#define BMI2_GYR_USER_GAIN_RATIO_X_MASK UINT16_C(0x07FF) +#define BMI2_GYR_USER_GAIN_RATIO_Y_MASK UINT16_C(0x07FF) +#define BMI2_GYR_USER_GAIN_RATIO_Z_MASK UINT16_C(0x07FF) + +/*! @name Mask definitions for BMI2 gyroscope user gain saturation status */ +#define BMI2_GYR_USER_GAIN_SAT_STAT_X_MASK UINT8_C(0x01) +#define BMI2_GYR_USER_GAIN_SAT_STAT_Y_MASK UINT8_C(0x02) +#define BMI2_GYR_USER_GAIN_SAT_STAT_Z_MASK UINT8_C(0x04) +#define BMI2_G_TRIGGER_STAT_MASK UINT8_C(0x38) + +/*! @name Bit position definitions for BMI2 gyroscope user gain saturation status */ +#define BMI2_GYR_USER_GAIN_SAT_STAT_Y_POS UINT8_C(0x01) +#define BMI2_GYR_USER_GAIN_SAT_STAT_Z_POS UINT8_C(0x02) +#define BMI2_G_TRIGGER_STAT_POS UINT8_C(0x03) + +/*! @name Mask definitions for MSB values of BMI2 gyroscope compensation */ +#define BMI2_GYR_OFF_COMP_MSB_X_MASK UINT8_C(0x03) +#define BMI2_GYR_OFF_COMP_MSB_Y_MASK UINT8_C(0x0C) +#define BMI2_GYR_OFF_COMP_MSB_Z_MASK UINT8_C(0x30) + +/*! @name Bit positions for MSB values of BMI2 gyroscope compensation */ +#define BMI2_GYR_OFF_COMP_MSB_Y_POS UINT8_C(0x02) +#define BMI2_GYR_OFF_COMP_MSB_Z_POS UINT8_C(0x04) + +/*! @name Mask definitions for MSB values of BMI2 gyroscope compensation from user input */ +#define BMI2_GYR_OFF_COMP_MSB_MASK UINT16_C(0x0300) +#define BMI2_GYR_OFF_COMP_LSB_MASK UINT16_C(0x00FF) + +/*! @name Mask definitions for BMI2 orientation status */ +#define BMI2_ORIENT_DETECT_MASK UINT8_C(0x03) +#define BMI2_ORIENT_FACE_UP_DWN_MASK UINT8_C(0x04) + +/*! @name Bit position definitions for BMI2 orientation status */ +#define BMI2_ORIENT_FACE_UP_DWN_POS UINT8_C(0x02) + +/*! @name Mask definitions for NVM-VFRM error status */ +#define BMI2_NVM_LOAD_ERR_STATUS_MASK UINT8_C(0x01) +#define BMI2_NVM_PROG_ERR_STATUS_MASK UINT8_C(0x02) +#define BMI2_NVM_ERASE_ERR_STATUS_MASK UINT8_C(0x04) +#define BMI2_NVM_END_EXCEED_STATUS_MASK UINT8_C(0x08) +#define BMI2_NVM_PRIV_ERR_STATUS_MASK UINT8_C(0x10) +#define BMI2_VFRM_LOCK_ERR_STATUS_MASK UINT8_C(0x20) +#define BMI2_VFRM_WRITE_ERR_STATUS_MASK UINT8_C(0x40) +#define BMI2_VFRM_FATAL_ERR_STATUS_MASK UINT8_C(0x80) + +/*! @name Bit positions for NVM-VFRM error status */ +#define BMI2_NVM_PROG_ERR_STATUS_POS UINT8_C(0x01) +#define BMI2_NVM_ERASE_ERR_STATUS_POS UINT8_C(0x02) +#define BMI2_NVM_END_EXCEED_STATUS_POS UINT8_C(0x03) +#define BMI2_NVM_PRIV_ERR_STATUS_POS UINT8_C(0x04) +#define BMI2_VFRM_LOCK_ERR_STATUS_POS UINT8_C(0x05) +#define BMI2_VFRM_WRITE_ERR_STATUS_POS UINT8_C(0x06) +#define BMI2_VFRM_FATAL_ERR_STATUS_POS UINT8_C(0x07) + +/*! @name Mask definitions for accelerometer self-test status */ +#define BMI2_ACC_SELF_TEST_DONE_MASK UINT8_C(0x01) +#define BMI2_ACC_X_OK_MASK UINT8_C(0x02) +#define BMI2_ACC_Y_OK_MASK UINT8_C(0x04) +#define BMI2_ACC_Z_OK_MASK UINT8_C(0x08) + +/*! @name Bit Positions for accelerometer self-test status */ +#define BMI2_ACC_X_OK_POS UINT8_C(0x01) +#define BMI2_ACC_Y_OK_POS UINT8_C(0x02) +#define BMI2_ACC_Z_OK_POS UINT8_C(0x03) + +/*! @name Mask definitions for BMI2 high-g feature configuration */ +#define BMI2_HIGH_G_THRES_MASK UINT16_C(0x7FFF) +#define BMI2_HIGH_G_HYST_MASK UINT16_C(0x0FFF) +#define BMI2_HIGH_G_X_SEL_MASK UINT16_C(0x1000) +#define BMI2_HIGH_G_Y_SEL_MASK UINT16_C(0x2000) +#define BMI2_HIGH_G_Z_SEL_MASK UINT16_C(0x4000) +#define BMI2_HIGH_G_DUR_MASK UINT16_C(0x0FFF) + +/*! @name Bit position definitions for BMI2 high-g feature configuration */ +#define BMI2_HIGH_G_X_SEL_POS UINT8_C(0x0C) +#define BMI2_HIGH_G_Y_SEL_POS UINT8_C(0x0D) +#define BMI2_HIGH_G_Z_SEL_POS UINT8_C(0x0E) + +/*! @name Mask definitions for BMI2 low-g feature configuration */ +#define BMI2_LOW_G_THRES_MASK UINT16_C(0x7FFF) +#define BMI2_LOW_G_HYST_MASK UINT16_C(0x0FFF) +#define BMI2_LOW_G_DUR_MASK UINT16_C(0x0FFF) + +/*! @name Mask definitions for BMI2 free-fall detection feature configuration */ +#define BMI2_FREE_FALL_ACCEL_SETT_MASK UINT16_C(0xFFFF) + +/*! @name Mask definitions for BMI2 flat feature configuration */ +#define BMI2_FLAT_THETA_MASK UINT16_C(0x007E) +#define BMI2_FLAT_BLOCK_MASK UINT16_C(0x0180) +#define BMI2_FLAT_HYST_MASK UINT16_C(0x003F) +#define BMI2_FLAT_HOLD_TIME_MASK UINT16_C(0x3FC0) + +/*! @name Bit position definitions for BMI2 flat feature configuration */ +#define BMI2_FLAT_THETA_POS UINT8_C(0x01) +#define BMI2_FLAT_BLOCK_POS UINT8_C(0x07) +#define BMI2_FLAT_HOLD_TIME_POS UINT8_C(0x06) + +/*! @name Mask definitions for BMI2 wrist gesture configuration */ +#define BMI2_WRIST_GEST_WEAR_ARM_MASK UINT16_C(0x0010) + +/*! @name Bit position definitions for wrist gesture configuration */ +#define BMI2_WRIST_GEST_WEAR_ARM_POS UINT8_C(0x04) + +/*! @name Mask definitions for BMI2 wrist gesture wh configuration */ +#define BMI2_WRIST_GEST_WH_DEVICE_POS_MASK UINT16_C(0x0001) +#define BMI2_WRIST_GEST_WH_INT UINT8_C(0x10) +#define BMI2_WRIST_GEST_WH_START_ADD UINT8_C(0x08) + +/*! @name Mask definitions for BMI2 wrist wear wake-up configuration */ +#define BMI2_WRIST_WAKE_UP_WH_INT_MASK UINT8_C(0x08) + +/*! @name Mask definition for BMI2 wrist wear wake-up configuration for wearable variant */ +#define BMI2_WRIST_WAKE_UP_ANGLE_LR_MASK UINT16_C(0x00FF) +#define BMI2_WRIST_WAKE_UP_ANGLE_LL_MASK UINT16_C(0xFF00) +#define BMI2_WRIST_WAKE_UP_ANGLE_PD_MASK UINT16_C(0x00FF) +#define BMI2_WRIST_WAKE_UP_ANGLE_PU_MASK UINT16_C(0xFF00) +#define BMI2_WRIST_WAKE_UP_MIN_DUR_MOVED_MASK UINT16_C(0x00FF) +#define BMI2_WRIST_WAKE_UP_MIN_DUR_QUITE_MASK UINT16_C(0xFF00) + +/*! @name Bit position definition for BMI2 wrist wear wake-up configuration for wearable variant */ +#define BMI2_WRIST_WAKE_UP_ANGLE_LL_POS UINT16_C(0x0008) +#define BMI2_WRIST_WAKE_UP_ANGLE_PU_POS UINT16_C(0x0008) +#define BMI2_WRIST_WAKE_UP_MIN_DUR_QUITE_POS UINT16_C(0x0008) + +/*! @name Macros to define values of BMI2 axis and its sign for re-map + * settings + */ +#define BMI2_MAP_X_AXIS UINT8_C(0x00) +#define BMI2_MAP_Y_AXIS UINT8_C(0x01) +#define BMI2_MAP_Z_AXIS UINT8_C(0x02) +#define BMI2_MAP_POSITIVE UINT8_C(0x00) +#define BMI2_MAP_NEGATIVE UINT8_C(0x01) + +/*! @name Mask definitions of BMI2 axis re-mapping */ +#define BMI2_X_AXIS_MASK UINT8_C(0x03) +#define BMI2_X_AXIS_SIGN_MASK UINT8_C(0x04) +#define BMI2_Y_AXIS_MASK UINT8_C(0x18) +#define BMI2_Y_AXIS_SIGN_MASK UINT8_C(0x20) +#define BMI2_Z_AXIS_MASK UINT8_C(0xC0) +#define BMI2_Z_AXIS_SIGN_MASK UINT8_C(0x01) + +/*! @name Bit position definitions of BMI2 axis re-mapping */ +#define BMI2_X_AXIS_SIGN_POS UINT8_C(0x02) +#define BMI2_Y_AXIS_POS UINT8_C(0x03) +#define BMI2_Y_AXIS_SIGN_POS UINT8_C(0x05) +#define BMI2_Z_AXIS_POS UINT8_C(0x06) + +/*! @name Macros to define polarity */ +#define BMI2_NEG_SIGN UINT8_C(1) +#define BMI2_POS_SIGN UINT8_C(0) + +/*! @name Macro to define related to CRT */ +#define BMI2_CRT_READY_FOR_DOWNLOAD_US UINT16_C(2000) +#define BMI2_CRT_READY_FOR_DOWNLOAD_RETRY UINT8_C(100) + +#define BMI2_CRT_WAIT_RUNNING_US UINT16_C(10000) +#define BMI2_CRT_WAIT_RUNNING_RETRY_EXECUTION UINT8_C(200) + +#define BMI2_CRT_MIN_BURST_WORD_LENGTH UINT8_C(2) +#define BMI2_CRT_MAX_BURST_WORD_LENGTH UINT16_C(255) + +#define BMI2_ACC_FOC_2G_REF UINT16_C(16384) +#define BMI2_ACC_FOC_4G_REF UINT16_C(8192) +#define BMI2_ACC_FOC_8G_REF UINT16_C(4096) +#define BMI2_ACC_FOC_16G_REF UINT16_C(2048) + +#define BMI2_GYRO_FOC_NOISE_LIMIT_NEGATIVE INT8_C(-20) +#define BMI2_GYRO_FOC_NOISE_LIMIT_POSITIVE INT8_C(20) + +/* reference value with positive and negative noise range in lsb */ +#define BMI2_ACC_2G_MAX_NOISE_LIMIT (BMI2_ACC_FOC_2G_REF + UINT16_C(255)) +#define BMI2_ACC_2G_MIN_NOISE_LIMIT (BMI2_ACC_FOC_2G_REF - UINT16_C(255)) +#define BMI2_ACC_4G_MAX_NOISE_LIMIT (BMI2_ACC_FOC_4G_REF + UINT16_C(255)) +#define BMI2_ACC_4G_MIN_NOISE_LIMIT (BMI2_ACC_FOC_4G_REF - UINT16_C(255)) +#define BMI2_ACC_8G_MAX_NOISE_LIMIT (BMI2_ACC_FOC_8G_REF + UINT16_C(255)) +#define BMI2_ACC_8G_MIN_NOISE_LIMIT (BMI2_ACC_FOC_8G_REF - UINT16_C(255)) +#define BMI2_ACC_16G_MAX_NOISE_LIMIT (BMI2_ACC_FOC_16G_REF + UINT16_C(255)) +#define BMI2_ACC_16G_MIN_NOISE_LIMIT (BMI2_ACC_FOC_16G_REF - UINT16_C(255)) + +#define BMI2_FOC_SAMPLE_LIMIT UINT8_C(128) + +/*! @name Bit wise selection of BMI2 sensors */ +#define BMI2_MAIN_SENSORS \ + (BMI2_ACCEL_SENS_SEL | BMI2_GYRO_SENS_SEL \ + | BMI2_AUX_SENS_SEL | BMI2_TEMP_SENS_SEL) + +/*! @name Maximum number of BMI2 main sensors */ +#define BMI2_MAIN_SENS_MAX_NUM UINT8_C(4) + +/*! @name Macro to specify the number of step counter parameters */ +#define BMI2_STEP_CNT_N_PARAMS UINT8_C(25) + +/*! @name Macro to specify the number of free-fall accel setting parameters */ +#define BMI2_FREE_FALL_ACCEL_SET_PARAMS UINT8_C(7) + +#define BMI2_SELECT_GYRO_SELF_TEST UINT8_C(0) +#define BMI2_SELECT_CRT UINT8_C(1) + +/*! @name Macro for NVM enable */ +#define BMI2_NVM_UNLOCK_ENABLE UINT8_C(0x02) +#define BMI2_NVM_UNLOCK_DISABLE UINT8_C(0x00) + +/*! @name macro to select between gyro self test and CRT */ +#define BMI2_GYRO_SELF_TEST_SEL UINT8_C(0) +#define BMI2_CRT_SEL UINT8_C(1) + +/******************************************************************************/ +/*! @name Accelerometer Macro Definitions */ +/******************************************************************************/ +/*! @name Accelerometer Bandwidth parameters */ +#define BMI2_ACC_OSR4_AVG1 UINT8_C(0x00) +#define BMI2_ACC_OSR2_AVG2 UINT8_C(0x01) +#define BMI2_ACC_NORMAL_AVG4 UINT8_C(0x02) +#define BMI2_ACC_CIC_AVG8 UINT8_C(0x03) +#define BMI2_ACC_RES_AVG16 UINT8_C(0x04) +#define BMI2_ACC_RES_AVG32 UINT8_C(0x05) +#define BMI2_ACC_RES_AVG64 UINT8_C(0x06) +#define BMI2_ACC_RES_AVG128 UINT8_C(0x07) + +/*! @name Accelerometer Output Data Rate */ +#define BMI2_ACC_ODR_0_78HZ UINT8_C(0x01) +#define BMI2_ACC_ODR_1_56HZ UINT8_C(0x02) +#define BMI2_ACC_ODR_3_12HZ UINT8_C(0x03) +#define BMI2_ACC_ODR_6_25HZ UINT8_C(0x04) +#define BMI2_ACC_ODR_12_5HZ UINT8_C(0x05) +#define BMI2_ACC_ODR_25HZ UINT8_C(0x06) +#define BMI2_ACC_ODR_50HZ UINT8_C(0x07) +#define BMI2_ACC_ODR_100HZ UINT8_C(0x08) +#define BMI2_ACC_ODR_200HZ UINT8_C(0x09) +#define BMI2_ACC_ODR_400HZ UINT8_C(0x0A) +#define BMI2_ACC_ODR_800HZ UINT8_C(0x0B) +#define BMI2_ACC_ODR_1600HZ UINT8_C(0x0C) + +/*! @name Accelerometer G Range */ +#define BMI2_ACC_RANGE_2G UINT8_C(0x00) +#define BMI2_ACC_RANGE_4G UINT8_C(0x01) +#define BMI2_ACC_RANGE_8G UINT8_C(0x02) +#define BMI2_ACC_RANGE_16G UINT8_C(0x03) + +/*! @name Mask definitions for accelerometer configuration register */ +#define BMI2_ACC_RANGE_MASK UINT8_C(0x03) +#define BMI2_ACC_ODR_MASK UINT8_C(0x0F) +#define BMI2_ACC_BW_PARAM_MASK UINT8_C(0x70) +#define BMI2_ACC_FILTER_PERF_MODE_MASK UINT8_C(0x80) + +/*! @name Bit position definitions for accelerometer configuration register */ +#define BMI2_ACC_BW_PARAM_POS UINT8_C(0x04) +#define BMI2_ACC_FILTER_PERF_MODE_POS UINT8_C(0x07) + +/*! @name Self test macro to define range */ +#define BMI2_ACC_SELF_TEST_RANGE UINT8_C(16) + +/*! @name Self test macro to show resulting minimum and maximum difference + * signal of the axes in mg + */ +#define BMI2_ST_ACC_X_SIG_MIN_DIFF INT16_C(16000) +#define BMI2_ST_ACC_Y_SIG_MIN_DIFF INT16_C(-15000) +#define BMI2_ST_ACC_Z_SIG_MIN_DIFF INT16_C(10000) + +/*! @name Mask definitions for accelerometer self-test */ +#define BMI2_ACC_SELF_TEST_EN_MASK UINT8_C(0x01) +#define BMI2_ACC_SELF_TEST_SIGN_MASK UINT8_C(0x04) +#define BMI2_ACC_SELF_TEST_AMP_MASK UINT8_C(0x08) + +/*! @name Bit Positions for accelerometer self-test */ +#define BMI2_ACC_SELF_TEST_SIGN_POS UINT8_C(0x02) +#define BMI2_ACC_SELF_TEST_AMP_POS UINT8_C(0x03) + +/*! @name MASK definition for gyro self test status */ +#define BMI2_GYR_ST_AXES_DONE_MASK UINT8_C(0X01) +#define BMI2_GYR_AXIS_X_OK_MASK UINT8_C(0x02) +#define BMI2_GYR_AXIS_Y_OK_MASK UINT8_C(0x04) +#define BMI2_GYR_AXIS_Z_OK_MASK UINT8_C(0x08) + +/*! @name Bit position for gyro self test status */ +#define BMI2_GYR_AXIS_X_OK_POS UINT8_C(0x01) +#define BMI2_GYR_AXIS_Y_OK_POS UINT8_C(0x02) +#define BMI2_GYR_AXIS_Z_OK_POS UINT8_C(0x03) + +/******************************************************************************/ +/*! @name Gyroscope Macro Definitions */ +/******************************************************************************/ +/*! @name Gyroscope Bandwidth parameters */ +#define BMI2_GYR_OSR4_MODE UINT8_C(0x00) +#define BMI2_GYR_OSR2_MODE UINT8_C(0x01) +#define BMI2_GYR_NORMAL_MODE UINT8_C(0x02) +#define BMI2_GYR_CIC_MODE UINT8_C(0x03) + +/*! @name Gyroscope Output Data Rate */ +#define BMI2_GYR_ODR_25HZ UINT8_C(0x06) +#define BMI2_GYR_ODR_50HZ UINT8_C(0x07) +#define BMI2_GYR_ODR_100HZ UINT8_C(0x08) +#define BMI2_GYR_ODR_200HZ UINT8_C(0x09) +#define BMI2_GYR_ODR_400HZ UINT8_C(0x0A) +#define BMI2_GYR_ODR_800HZ UINT8_C(0x0B) +#define BMI2_GYR_ODR_1600HZ UINT8_C(0x0C) +#define BMI2_GYR_ODR_3200HZ UINT8_C(0x0D) + +/*! @name Gyroscope OIS Range */ +#define BMI2_GYR_OIS_250 UINT8_C(0x00) +#define BMI2_GYR_OIS_2000 UINT8_C(0x01) + +/*! @name Gyroscope Angular Rate Measurement Range */ +#define BMI2_GYR_RANGE_2000 UINT8_C(0x00) +#define BMI2_GYR_RANGE_1000 UINT8_C(0x01) +#define BMI2_GYR_RANGE_500 UINT8_C(0x02) +#define BMI2_GYR_RANGE_250 UINT8_C(0x03) +#define BMI2_GYR_RANGE_125 UINT8_C(0x04) + +/*! @name Mask definitions for gyroscope configuration register */ +#define BMI2_GYR_RANGE_MASK UINT8_C(0x07) +#define BMI2_GYR_OIS_RANGE_MASK UINT8_C(0x08) +#define BMI2_GYR_ODR_MASK UINT8_C(0x0F) +#define BMI2_GYR_BW_PARAM_MASK UINT8_C(0x30) +#define BMI2_GYR_NOISE_PERF_MODE_MASK UINT8_C(0x40) +#define BMI2_GYR_FILTER_PERF_MODE_MASK UINT8_C(0x80) + +/*! @name Bit position definitions for gyroscope configuration register */ +#define BMI2_GYR_OIS_RANGE_POS UINT8_C(0x03) +#define BMI2_GYR_BW_PARAM_POS UINT8_C(0x04) +#define BMI2_GYR_NOISE_PERF_MODE_POS UINT8_C(0x06) +#define BMI2_GYR_FILTER_PERF_MODE_POS UINT8_C(0x07) + +/******************************************************************************/ +/*! @name Auxiliary Macro Definitions */ +/******************************************************************************/ +/*! @name Auxiliary Output Data Rate */ +#define BMI2_AUX_ODR_RESERVED UINT8_C(0x00) +#define BMI2_AUX_ODR_0_78HZ UINT8_C(0x01) +#define BMI2_AUX_ODR_1_56HZ UINT8_C(0x02) +#define BMI2_AUX_ODR_3_12HZ UINT8_C(0x03) +#define BMI2_AUX_ODR_6_25HZ UINT8_C(0x04) +#define BMI2_AUX_ODR_12_5HZ UINT8_C(0x05) +#define BMI2_AUX_ODR_25HZ UINT8_C(0x06) +#define BMI2_AUX_ODR_50HZ UINT8_C(0x07) +#define BMI2_AUX_ODR_100HZ UINT8_C(0x08) +#define BMI2_AUX_ODR_200HZ UINT8_C(0x09) +#define BMI2_AUX_ODR_400HZ UINT8_C(0x0A) +#define BMI2_AUX_ODR_800HZ UINT8_C(0x0B) + +/*! @name Macro to define burst read lengths for both manual and auto modes */ +#define BMI2_AUX_READ_LEN_0 UINT8_C(0x00) +#define BMI2_AUX_READ_LEN_1 UINT8_C(0x01) +#define BMI2_AUX_READ_LEN_2 UINT8_C(0x02) +#define BMI2_AUX_READ_LEN_3 UINT8_C(0x03) + +/*! @name Mask definitions for auxiliary interface configuration register */ +#define BMI2_AUX_SET_I2C_ADDR_MASK UINT8_C(0xFE) +#define BMI2_AUX_MAN_MODE_EN_MASK UINT8_C(0x80) +#define BMI2_AUX_FCU_WR_EN_MASK UINT8_C(0x40) +#define BMI2_AUX_MAN_READ_BURST_MASK UINT8_C(0x0C) +#define BMI2_AUX_READ_BURST_MASK UINT8_C(0x03) +#define BMI2_AUX_ODR_EN_MASK UINT8_C(0x0F) +#define BMI2_AUX_OFFSET_READ_OUT_MASK UINT8_C(0xF0) + +/*! @name Bit positions for auxiliary interface configuration register */ +#define BMI2_AUX_SET_I2C_ADDR_POS UINT8_C(0x01) +#define BMI2_AUX_MAN_MODE_EN_POS UINT8_C(0x07) +#define BMI2_AUX_FCU_WR_EN_POS UINT8_C(0x06) +#define BMI2_AUX_MAN_READ_BURST_POS UINT8_C(0x02) +#define BMI2_AUX_OFFSET_READ_OUT_POS UINT8_C(0x04) + +/******************************************************************************/ +/*! @name FIFO Macro Definitions */ +/******************************************************************************/ +/*! @name Macros to define virtual FIFO frame mode */ +#define BMI2_FIFO_VIRT_FRM_MODE UINT8_C(0x03) + +/*! @name FIFO Header Mask definitions */ +#define BMI2_FIFO_HEADER_ACC_FRM UINT8_C(0x84) +#define BMI2_FIFO_HEADER_AUX_FRM UINT8_C(0x90) +#define BMI2_FIFO_HEADER_GYR_FRM UINT8_C(0x88) +#define BMI2_FIFO_HEADER_GYR_ACC_FRM UINT8_C(0x8C) +#define BMI2_FIFO_HEADER_AUX_ACC_FRM UINT8_C(0x94) +#define BMI2_FIFO_HEADER_AUX_GYR_FRM UINT8_C(0x98) +#define BMI2_FIFO_HEADER_ALL_FRM UINT8_C(0x9C) +#define BMI2_FIFO_HEADER_SENS_TIME_FRM UINT8_C(0x44) +#define BMI2_FIFO_HEADER_SKIP_FRM UINT8_C(0x40) +#define BMI2_FIFO_HEADER_INPUT_CFG_FRM UINT8_C(0x48) +#define BMI2_FIFO_HEAD_OVER_READ_MSB UINT8_C(0x80) +#define BMI2_FIFO_VIRT_ACT_RECOG_FRM UINT8_C(0xC8) + +/*! @name BMI2 sensor selection for header-less frames */ +#define BMI2_FIFO_HEAD_LESS_ACC_FRM UINT8_C(0x40) +#define BMI2_FIFO_HEAD_LESS_AUX_FRM UINT8_C(0x20) +#define BMI2_FIFO_HEAD_LESS_GYR_FRM UINT8_C(0x80) +#define BMI2_FIFO_HEAD_LESS_GYR_AUX_FRM UINT8_C(0xA0) +#define BMI2_FIFO_HEAD_LESS_GYR_ACC_FRM UINT8_C(0xC0) +#define BMI2_FIFO_HEAD_LESS_AUX_ACC_FRM UINT8_C(0x60) +#define BMI2_FIFO_HEAD_LESS_ALL_FRM UINT8_C(0xE0) + +/*! @name Mask definitions for FIFO frame content configuration */ +#define BMI2_FIFO_STOP_ON_FULL UINT16_C(0x0001) +#define BMI2_FIFO_TIME_EN UINT16_C(0x0002) +#define BMI2_FIFO_TAG_INT1 UINT16_C(0x0300) +#define BMI2_FIFO_TAG_INT2 UINT16_C(0x0C00) +#define BMI2_FIFO_HEADER_EN UINT16_C(0x1000) +#define BMI2_FIFO_AUX_EN UINT16_C(0x2000) +#define BMI2_FIFO_ACC_EN UINT16_C(0x4000) +#define BMI2_FIFO_GYR_EN UINT16_C(0x8000) +#define BMI2_FIFO_ALL_EN UINT16_C(0xE000) + +/*! @name FIFO sensor data lengths */ +#define BMI2_FIFO_ACC_LENGTH UINT8_C(6) +#define BMI2_FIFO_GYR_LENGTH UINT8_C(6) +#define BMI2_FIFO_AUX_LENGTH UINT8_C(8) +#define BMI2_FIFO_ACC_AUX_LENGTH UINT8_C(14) +#define BMI2_FIFO_GYR_AUX_LENGTH UINT8_C(14) +#define BMI2_FIFO_ACC_GYR_LENGTH UINT8_C(12) +#define BMI2_FIFO_ALL_LENGTH UINT8_C(20) +#define BMI2_SENSOR_TIME_LENGTH UINT8_C(3) +#define BMI2_FIFO_CONFIG_LENGTH UINT8_C(2) +#define BMI2_FIFO_WM_LENGTH UINT8_C(2) +#define BMI2_MAX_VALUE_FIFO_FILTER UINT8_C(1) +#define BMI2_FIFO_DATA_LENGTH UINT8_C(2) +#define BMI2_FIFO_LENGTH_MSB_BYTE UINT8_C(1) +#define BMI2_FIFO_INPUT_CFG_LENGTH UINT8_C(4) +#define BMI2_FIFO_SKIP_FRM_LENGTH UINT8_C(1) + +/*! @name FIFO sensor virtual data lengths: sensor data plus sensor time */ +#define BMI2_FIFO_VIRT_ACC_LENGTH UINT8_C(9) +#define BMI2_FIFO_VIRT_GYR_LENGTH UINT8_C(9) +#define BMI2_FIFO_VIRT_AUX_LENGTH UINT8_C(11) +#define BMI2_FIFO_VIRT_ACC_AUX_LENGTH UINT8_C(17) +#define BMI2_FIFO_VIRT_GYR_AUX_LENGTH UINT8_C(17) +#define BMI2_FIFO_VIRT_ACC_GYR_LENGTH UINT8_C(15) +#define BMI2_FIFO_VIRT_ALL_LENGTH UINT8_C(23) + +/*! @name FIFO sensor virtual data lengths: activity recognition */ +#define BMI2_FIFO_VIRT_ACT_DATA_LENGTH UINT8_C(6) +#define BMI2_FIFO_VIRT_ACT_TIME_LENGTH UINT8_C(4) +#define BMI2_FIFO_VIRT_ACT_TYPE_LENGTH UINT8_C(1) +#define BMI2_FIFO_VIRT_ACT_STAT_LENGTH UINT8_C(1) + +/*! @name BMI2 FIFO data filter modes */ +#define BMI2_FIFO_UNFILTERED_DATA UINT8_C(0) +#define BMI2_FIFO_FILTERED_DATA UINT8_C(1) + +/*! @name FIFO frame masks */ +#define BMI2_FIFO_LSB_CONFIG_CHECK UINT8_C(0x00) +#define BMI2_FIFO_MSB_CONFIG_CHECK UINT8_C(0x80) +#define BMI2_FIFO_TAG_INTR_MASK UINT8_C(0xFF) + +/*! @name BMI2 Mask definitions of FIFO configuration registers */ +#define BMI2_FIFO_CONFIG_0_MASK UINT16_C(0x0003) +#define BMI2_FIFO_CONFIG_1_MASK UINT16_C(0xFF00) + +/*! @name FIFO self wake-up mask definition */ +#define BMI2_FIFO_SELF_WAKE_UP_MASK UINT8_C(0x02) + +/*! @name FIFO down sampling mask definition */ +#define BMI2_ACC_FIFO_DOWNS_MASK UINT8_C(0x70) +#define BMI2_GYR_FIFO_DOWNS_MASK UINT8_C(0x07) + +/*! @name FIFO down sampling bit positions */ +#define BMI2_ACC_FIFO_DOWNS_POS UINT8_C(0x04) + +/*! @name FIFO filter mask definition */ +#define BMI2_ACC_FIFO_FILT_DATA_MASK UINT8_C(0x80) +#define BMI2_GYR_FIFO_FILT_DATA_MASK UINT8_C(0x08) + +/*! @name FIFO filter bit positions */ +#define BMI2_ACC_FIFO_FILT_DATA_POS UINT8_C(0x07) +#define BMI2_GYR_FIFO_FILT_DATA_POS UINT8_C(0x03) + +/*! @name FIFO byte counter mask definition */ +#define BMI2_FIFO_BYTE_COUNTER_MSB_MASK UINT8_C(0x3F) + +/*! @name FIFO self wake-up bit positions */ +#define BMI2_FIFO_SELF_WAKE_UP_POS UINT8_C(0x01) + +/*! @name Mask Definitions for Virtual FIFO frames */ +#define BMI2_FIFO_VIRT_FRM_MODE_MASK UINT8_C(0xC0) +#define BMI2_FIFO_VIRT_PAYLOAD_MASK UINT8_C(0x3C) + +/*! @name Bit Positions for Virtual FIFO frames */ +#define BMI2_FIFO_VIRT_FRM_MODE_POS UINT8_C(0x06) +#define BMI2_FIFO_VIRT_PAYLOAD_POS UINT8_C(0x02) + +/******************************************************************************/ +/*! @name Interrupt Macro Definitions */ +/******************************************************************************/ +/*! @name BMI2 Interrupt Modes */ +/* Non latched */ +#define BMI2_INT_NON_LATCH UINT8_C(0) + +/* Permanently latched */ +#define BMI2_INT_LATCH UINT8_C(1) + +/*! @name BMI2 Interrupt Pin Behavior */ +#define BMI2_INT_PUSH_PULL UINT8_C(0) +#define BMI2_INT_OPEN_DRAIN UINT8_C(1) + +/*! @name BMI2 Interrupt Pin Level */ +#define BMI2_INT_ACTIVE_LOW UINT8_C(0) +#define BMI2_INT_ACTIVE_HIGH UINT8_C(1) + +/*! @name BMI2 Interrupt Output Enable */ +#define BMI2_INT_OUTPUT_DISABLE UINT8_C(0) +#define BMI2_INT_OUTPUT_ENABLE UINT8_C(1) + +/*! @name BMI2 Interrupt Input Enable */ +#define BMI2_INT_INPUT_DISABLE UINT8_C(0) +#define BMI2_INT_INPUT_ENABLE UINT8_C(1) + +/*! @name Mask definitions for interrupt pin configuration */ +#define BMI2_INT_LATCH_MASK UINT8_C(0x01) +#define BMI2_INT_LEVEL_MASK UINT8_C(0x02) +#define BMI2_INT_OPEN_DRAIN_MASK UINT8_C(0x04) +#define BMI2_INT_OUTPUT_EN_MASK UINT8_C(0x08) +#define BMI2_INT_INPUT_EN_MASK UINT8_C(0x10) + +/*! @name Bit position definitions for interrupt pin configuration */ +#define BMI2_INT_LEVEL_POS UINT8_C(0x01) +#define BMI2_INT_OPEN_DRAIN_POS UINT8_C(0x02) +#define BMI2_INT_OUTPUT_EN_POS UINT8_C(0x03) +#define BMI2_INT_INPUT_EN_POS UINT8_C(0x04) + +/*! @name Mask definitions for data interrupt mapping */ +#define BMI2_FFULL_INT UINT8_C(0x01) +#define BMI2_FWM_INT UINT8_C(0x02) +#define BMI2_DRDY_INT UINT8_C(0x04) +#define BMI2_ERR_INT UINT8_C(0x08) + +/*! @name Mask definitions for data interrupt status bits */ +#define BMI2_FFULL_INT_STATUS_MASK UINT16_C(0x0100) +#define BMI2_FWM_INT_STATUS_MASK UINT16_C(0x0200) +#define BMI2_ERR_INT_STATUS_MASK UINT16_C(0x0400) +#define BMI2_AUX_DRDY_INT_MASK UINT16_C(0x2000) +#define BMI2_GYR_DRDY_INT_MASK UINT16_C(0x4000) +#define BMI2_ACC_DRDY_INT_MASK UINT16_C(0x8000) + +/*! @name Maximum number of interrupt pins */ +#define BMI2_INT_PIN_MAX_NUM UINT8_C(2) + +/*! @name Macro for mapping feature interrupts */ +#define BMI2_FEAT_BIT_DISABLE UINT8_C(0) +#define BMI2_FEAT_BIT0 UINT8_C(1) +#define BMI2_FEAT_BIT1 UINT8_C(2) +#define BMI2_FEAT_BIT2 UINT8_C(3) +#define BMI2_FEAT_BIT3 UINT8_C(4) +#define BMI2_FEAT_BIT4 UINT8_C(5) +#define BMI2_FEAT_BIT5 UINT8_C(6) +#define BMI2_FEAT_BIT6 UINT8_C(7) +#define BMI2_FEAT_BIT7 UINT8_C(8) +#define BMI2_FEAT_BIT_MAX UINT8_C(9) + +/******************************************************************************/ +/*! @name OIS Interface Macro Definitions */ +/******************************************************************************/ +/*! @name Mask definitions for interface configuration register */ +#define BMI2_OIS_IF_EN_MASK UINT8_C(0x10) +#define BMI2_AUX_IF_EN_MASK UINT8_C(0x20) + +/*! @name Bit positions for OIS interface enable */ +#define BMI2_OIS_IF_EN_POS UINT8_C(0x04) +#define BMI2_AUX_IF_EN_POS UINT8_C(0x05) + +/******************************************************************************/ +/*! @name Macro Definitions for Axes re-mapping */ +/******************************************************************************/ +/*! @name Macros for the user-defined values of axes and their polarities */ +#define BMI2_X UINT8_C(0x01) +#define BMI2_NEG_X UINT8_C(0x09) +#define BMI2_Y UINT8_C(0x02) +#define BMI2_NEG_Y UINT8_C(0x0A) +#define BMI2_Z UINT8_C(0x04) +#define BMI2_NEG_Z UINT8_C(0x0C) +#define BMI2_AXIS_MASK UINT8_C(0x07) +#define BMI2_AXIS_SIGN UINT8_C(0x08) + +/******************************************************************************/ +/*! @name Macro Definitions for offset and gain compensation */ +/******************************************************************************/ +/*! @name Mask definitions of gyroscope offset compensation registers */ +#define BMI2_GYR_GAIN_EN_MASK UINT8_C(0x80) +#define BMI2_GYR_OFF_COMP_EN_MASK UINT8_C(0x40) + +/*! @name Bit positions of gyroscope offset compensation registers */ +#define BMI2_GYR_OFF_COMP_EN_POS UINT8_C(0x06) + +/*! @name Mask definitions of gyroscope user-gain registers */ +#define BMI2_GYR_USR_GAIN_X_MASK UINT8_C(0x7F) +#define BMI2_GYR_USR_GAIN_Y_MASK UINT8_C(0x7F) +#define BMI2_GYR_USR_GAIN_Z_MASK UINT8_C(0x7F) + +/*! @name Bit positions of gyroscope offset compensation registers */ +#define BMI2_GYR_GAIN_EN_POS UINT8_C(0x07) + +/******************************************************************************/ +/*! @name Macro Definitions for internal status */ +/******************************************************************************/ +#define BMI2_NOT_INIT UINT8_C(0x00) +#define BMI2_INIT_OK UINT8_C(0x01) +#define BMI2_INIT_ERR UINT8_C(0x02) +#define BMI2_DRV_ERR UINT8_C(0x03) +#define BMI2_SNS_STOP UINT8_C(0x04) +#define BMI2_NVM_ERROR UINT8_C(0x05) +#define BMI2_START_UP_ERROR UINT8_C(0x06) +#define BMI2_COMPAT_ERROR UINT8_C(0x07) +#define BMI2_VFM_SKIPPED UINT8_C(0x10) +#define BMI2_AXES_MAP_ERROR UINT8_C(0x20) +#define BMI2_ODR_50_HZ_ERROR UINT8_C(0x40) +#define BMI2_ODR_HIGH_ERROR UINT8_C(0x80) + +/******************************************************************************/ +/*! @name error status form gyro gain update status. */ +/******************************************************************************/ +#define BMI2_G_TRIGGER_NO_ERROR UINT8_C(0x00) + +#define BMI2_G_TRIGGER_PRECON_ERROR UINT8_C(0x01) +#define BMI2_G_TRIGGER_DL_ERROR UINT8_C(0x02) +#define BMI2_G_TRIGGER_ABORT_ERROR UINT8_C(0x03) + +/******************************************************************************/ +/*! @name Variant specific features selection macros */ +/******************************************************************************/ +#define BMI2_CRT_RTOSK_ENABLE UINT8_C(0x01) +#define BMI2_GYRO_CROSS_SENS_ENABLE UINT8_C(0x02) +#define BMI2_GYRO_USER_GAIN_ENABLE UINT8_C(0x08) +#define BMI2_NO_FEATURE_ENABLE UINT8_C(0x00) +#define BMI2_CRT_IN_FIFO_NOT_REQ UINT8_C(0x10) +#define BMI2_MAXIMUM_FIFO_VARIANT UINT8_C(0x20) + +/*! Pull-up configuration for ASDA */ +#define BMI2_ASDA_PUPSEL_OFF UINT8_C(0x00) +#define BMI2_ASDA_PUPSEL_40K UINT8_C(0x01) +#define BMI2_ASDA_PUPSEL_10K UINT8_C(0x02) +#define BMI2_ASDA_PUPSEL_2K UINT8_C(0x03) + +/******************************************************************************/ +/*! @name Function Pointers */ +/******************************************************************************/ + +/*! + * @brief Bus communication function pointer which should be mapped to + * the platform specific read functions of the user + * + * @param[in] reg_addr : Register address from which data is read. + * @param[out] reg_data : Pointer to data buffer where read data is stored. + * @param[in] len : Number of bytes of data to be read. + * @param[in, out] intf_ptr : Void pointer that can enable the linking of descriptors + * for interface related call backs. + * + * retval = BMA4_INTF_RET_SUCCESS -> Success + * retval != BMA4_INTF_RET_SUCCESS -> Failure + * + */ +typedef BMI2_INTF_RETURN_TYPE (*bmi2_read_fptr_t)(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr); + +/*! + * @brief Bus communication function pointer which should be mapped to + * the platform specific write functions of the user + * + * @param[in] reg_addr : Register address to which the data is written. + * @param[in] reg_data : Pointer to data buffer in which data to be written + * is stored. + * @param[in] len : Number of bytes of data to be written. + * @param[in, out] intf_ptr : Void pointer that can enable the linking of descriptors + * for interface related call backs + * + * retval = BMA4_INTF_RET_SUCCESS -> Success + * retval != BMA4_INTF_RET_SUCCESS -> Failure + * + */ +typedef BMI2_INTF_RETURN_TYPE (*bmi2_write_fptr_t)(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, + void *intf_ptr); + +/*! + * @brief Delay function pointer which should be mapped to + * delay function of the user + * + * @param[in] period : Delay in microseconds. + * @param[in, out] intf_ptr : Void pointer that can enable the linking of descriptors + * for interface related call backs + * + */ +typedef void (*bmi2_delay_fptr_t)(uint32_t period, void *intf_ptr); + +/*! + * @brief To get the configurations for wake_up feature, since wakeup feature is different for bmi260 and bmi261. + * + * @param[out] wake_up : Void pointer to store bmi2_wake_up_config structure. + * @param[in, out] bmi2_dev : Void pointer to store bmi2_dev structure. + * + * @return Result of API execution status + * + * @retval BMI2_OK - Success. + * @retval BMI2_E_COM_FAIL - Error: Communication fail + * @retval BMI2_E_NULL_PTR - Error: Null pointer error + * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page + * + */ +typedef int8_t (*bmi2_wake_up_fptr_t)(void *wake_up, void *bmi2_dev); + +/*! + * @brief To get the configurations for tap feature. + * + * @param[out] tap : Void pointer to store bmi2_tap_config structure. + * @param[in, out] bmi2_dev : Void pointer to store bmi2_dev structure. + * + * @return Result of API execution status + * + * @retval BMI2_OK - Success. + * @retval BMI2_E_COM_FAIL - Error: Communication fail + * @retval BMI2_E_NULL_PTR - Error: Null pointer error + * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page + * + */ +typedef int8_t (*bmi2_tap_fptr_t)(void *tap, void *bmi2_dev); + +/******************************************************************************/ +/*! @name Enum Declarations */ +/******************************************************************************/ +/*! @name Enum to define BMI2 sensor interfaces */ +enum bmi2_intf { + BMI2_SPI_INTF = 0, + BMI2_I2C_INTF, + BMI2_I3C_INTF +}; + +/*! @name Enum to define BMI2 sensor configuration errors for accelerometer + * and gyroscope + */ +enum bmi2_sensor_config_error { + BMI2_NO_ERROR, + BMI2_ACC_ERROR, + BMI2_GYR_ERROR, + BMI2_ACC_GYR_ERROR +}; + +/*! @name Enum to define interrupt lines */ +enum bmi2_hw_int_pin { + BMI2_INT_NONE, + BMI2_INT1, + BMI2_INT2, + BMI2_INT_BOTH, + BMI2_INT_PIN_MAX +}; + +/*! @name Enum for the position of the wearable device */ +enum bmi2_wear_arm_pos { + BMI2_ARM_LEFT, + BMI2_ARM_RIGHT +}; + +/*! @name Enum to display type of activity recognition */ +enum bmi2_act_recog_type { + BMI2_ACT_UNKNOWN, + BMI2_ACT_STILL, + BMI2_ACT_WALK, + BMI2_ACT_RUN, + BMI2_ACT_BIKE, + BMI2_ACT_VEHICLE, + BMI2_ACT_TILTED +}; + +/*! @name Enum to display activity recognition status */ +enum bmi2_act_recog_stat { + BMI2_ACT_START = 1, + BMI2_ACT_END +}; + +/******************************************************************************/ +/*! @name Structure Declarations */ +/******************************************************************************/ +/*! @name Structure to store the compensated user-gain data of gyroscope */ +struct bmi2_gyro_user_gain_data +{ + /*! x-axis */ + int8_t x; + + /*! y-axis */ + int8_t y; + + /*! z-axis */ + int8_t z; +}; + +/*! @name Structure to store the re-mapped axis */ +struct bmi2_remap +{ + /*! Re-mapped x-axis */ + uint8_t x; + + /*! Re-mapped y-axis */ + uint8_t y; + + /*! Re-mapped z-axis */ + uint8_t z; +}; + +/*! @name Structure to store the value of re-mapped axis and its sign */ +struct bmi2_axes_remap +{ + /*! Re-mapped x-axis */ + uint8_t x_axis; + + /*! Re-mapped y-axis */ + uint8_t y_axis; + + /*! Re-mapped z-axis */ + uint8_t z_axis; + + /*! Re-mapped x-axis sign */ + uint8_t x_axis_sign; + + /*! Re-mapped y-axis sign */ + uint8_t y_axis_sign; + + /*! Re-mapped z-axis sign */ + uint8_t z_axis_sign; +}; + +/*! @name Structure to define the type of sensor and its interrupt pin */ +struct bmi2_sens_int_config +{ + /*! Defines the type of sensor */ + uint8_t type; + + /*! Type of interrupt pin */ + enum bmi2_hw_int_pin hw_int_pin; +}; + +/*! @name Structure to define output for activity recognition */ +struct bmi2_act_recog_output +{ + /*! Time stamp */ + uint32_t time_stamp; + + /*! current activity */ + uint8_t curr_act; + + /*! previous activity */ + uint8_t prev_act; +}; + +/*! @name Structure to define FIFO frame configuration */ +struct bmi2_fifo_frame +{ + /*! Pointer to FIFO data */ + uint8_t *data; + + /*! Number of user defined bytes of FIFO to be read */ + uint16_t length; + + /*! Defines header/header-less mode */ + uint8_t header_enable; + + /*! Enables type of data to be streamed - accelerometer, auxiliary or + * gyroscope + */ + uint16_t data_enable; + + /*! To index accelerometer bytes */ + uint16_t acc_byte_start_idx; + + /*! To index activity output bytes */ + uint16_t act_recog_byte_start_idx; + + /*! To index auxiliary bytes */ + uint16_t aux_byte_start_idx; + + /*! To index gyroscope bytes */ + uint16_t gyr_byte_start_idx; + + /*! FIFO sensor time */ + uint32_t sensor_time; + + /*! Skipped frame count */ + uint8_t skipped_frame_count; + + /*! Type of data interrupt to be mapped */ + uint8_t data_int_map; + + /*! Water-mark level for water-mark interrupt */ + uint16_t wm_lvl; + + /*! Accelerometer frame length */ + uint8_t acc_frm_len; + + /*! Gyroscope frame length */ + uint8_t gyr_frm_len; + + /*! Auxiliary frame length */ + uint8_t aux_frm_len; + + /*! Accelerometer and gyroscope frame length */ + uint8_t acc_gyr_frm_len; + + /*! Accelerometer and auxiliary frame length */ + uint8_t acc_aux_frm_len; + + /*! Gyroscope and auxiliary frame length */ + uint8_t aux_gyr_frm_len; + + /*! Accelerometer, Gyroscope and auxiliary frame length */ + uint8_t all_frm_len; +}; + +/*! @name Structure to define Interrupt pin configuration */ +struct bmi2_int_pin_cfg +{ + /*! Configure level of interrupt pin */ + uint8_t lvl; + + /*! Configure behavior of interrupt pin */ + uint8_t od; + + /*! Output enable for interrupt pin */ + uint8_t output_en; + + /*! Input enable for interrupt pin */ + uint8_t input_en; +}; + +/*! @name Structure to define interrupt pin type, mode and configurations */ +struct bmi2_int_pin_config +{ + /*! Interrupt pin type: INT1 or INT2 or BOTH */ + uint8_t pin_type; + + /*! Latched or non-latched mode*/ + uint8_t int_latch; + + /*! Structure to define Interrupt pin configuration */ + struct bmi2_int_pin_cfg pin_cfg[BMI2_INT_PIN_MAX_NUM]; +}; + +/*! @name Structure to define an array of 8 auxiliary data bytes */ +struct bmi2_aux_fifo_data +{ + /*! Auxiliary data */ + uint8_t data[8]; + + /*! Sensor time for virtual frames */ + uint32_t virt_sens_time; +}; + +/*! @name Structure to define accelerometer and gyroscope sensor axes and + * sensor time for virtual frames + */ +struct bmi2_sens_axes_data +{ + /*! Data in x-axis */ + int16_t x; + + /*! Data in y-axis */ + int16_t y; + + /*! Data in z-axis */ + int16_t z; + + /*! Sensor time for virtual frames */ + uint32_t virt_sens_time; +}; + +/*! @name Structure to define gyroscope saturation status of user gain */ +struct bmi2_gyr_user_gain_status +{ + /*! Status in x-axis */ + uint8_t sat_x; + + /*! Status in y-axis */ + uint8_t sat_y; + + /*! Status in z-axis */ + uint8_t sat_z; + + /*! G trigger status */ + uint8_t g_trigger_status; +}; + +/*! @name Structure to store the status of gyro self test result */ +struct bmi2_gyro_self_test_status +{ + /*! gyro self test axes done */ + uint8_t gyr_st_axes_done : 1; + + /*! status of gyro X-axis self test */ + uint8_t gyr_axis_x_ok : 1; + + /*! status of gyro Y-axis self test */ + uint8_t gyr_axis_y_ok : 1; + + /*! status of gyro Z-axis self test */ + uint8_t gyr_axis_z_ok : 1; +}; + +/*! @name Structure to define NVM error status */ +struct bmi2_nvm_err_status +{ + /*! NVM load action error */ + uint8_t load_error; + + /*! NVM program action error */ + uint8_t prog_error; + + /*! NVM erase action error */ + uint8_t erase_error; + + /*! NVM program limit exceeded */ + uint8_t exceed_error; + + /*! NVM privilege error */ + uint8_t privil_error; +}; + +/*! @name Structure to define VFRM error status */ +struct bmi2_vfrm_err_status +{ + /*! VFRM lock acquire error */ + uint8_t lock_error; + + /*! VFRM write error */ + uint8_t write_error; + + /*! VFRM fatal err */ + uint8_t fatal_error; +}; + +/*! @name Structure to define accelerometer self test feature status */ +struct bmi2_acc_self_test_status +{ + /*! Accelerometer test completed */ + uint8_t acc_self_test_done; + + /*! Bit is set to 1 when accelerometer X-axis test passed */ + uint8_t acc_x_ok; + + /*! Bit is set to 1 when accelerometer y-axis test passed */ + uint8_t acc_y_ok; + + /*! Bit is set to 1 when accelerometer z-axis test passed */ + uint8_t acc_z_ok; +}; + +/*! @name Structure to define orientation output */ +struct bmi2_orientation_output +{ + /*! Orientation portrait landscape */ + uint8_t portrait_landscape; + + /*! Orientation face-up down */ + uint8_t faceup_down; +}; + +/*! @name Structure to define OIS output */ +struct bmi2_ois_output +{ + /*! OIS accel x axis */ + int16_t ois_acc_x; + + /*! OIS accel y axis */ + int16_t ois_acc_y; + + /*! OIS accel z axis */ + int16_t ois_acc_z; + + /*! ois gyro x axis */ + int16_t ois_gyro_x; + + /*! OIS gyro y axis */ + int16_t ois_gyro_y; + + /*! OIS gyro z axis */ + int16_t ois_gyro_z; +}; + +/*! @name Union to define BMI2 sensor data */ +union bmi2_sens_data +{ + /*! Accelerometer axes data */ + struct bmi2_sens_axes_data acc; + + /*! Gyroscope axes data */ + struct bmi2_sens_axes_data gyr; + + /*! Auxiliary sensor data */ + uint8_t aux_data[BMI2_AUX_NUM_BYTES]; + + /*! Step counter output */ + uint32_t step_counter_output; + + /*! Step activity output */ + uint8_t activity_output; + + /*! Orientation output */ + struct bmi2_orientation_output orient_output; + + /*! High-g output */ + uint8_t high_g_output; + + /*! Gyroscope user gain saturation status */ + struct bmi2_gyr_user_gain_status gyro_user_gain_status; + + /*! NVM error status */ + struct bmi2_nvm_err_status nvm_status; + + /*! Virtual frame error status */ + struct bmi2_vfrm_err_status vfrm_status; + + /*! Wrist gesture output */ + uint8_t wrist_gesture_output; + + /*! Gyroscope cross sense value of z axis */ + int16_t correction_factor_zx; + + /*! Accelerometer self test feature status */ + struct bmi2_acc_self_test_status accel_self_test_output; + + /*! OIS output */ + struct bmi2_ois_output ois_output; +}; + +/*! @name Structure to define type of sensor and their respective data */ +struct bmi2_sensor_data +{ + /*! Defines the type of sensor */ + uint8_t type; + + /*! Defines various sensor data */ + union bmi2_sens_data sens_data; +}; + +/*! @name Structure to define accelerometer configuration */ +struct bmi2_accel_config +{ + /*! Output data rate in Hz */ + uint8_t odr; + + /*! Bandwidth parameter */ + uint8_t bwp; + + /*! Filter performance mode */ + uint8_t filter_perf; + + /*! g-range */ + uint8_t range; +}; + +/*! @name Structure to define gyroscope configuration */ +struct bmi2_gyro_config +{ + /*! Output data rate in Hz */ + uint8_t odr; + + /*! Bandwidth parameter */ + uint8_t bwp; + + /*! Filter performance mode */ + uint8_t filter_perf; + + /*! OIS Range */ + uint8_t ois_range; + + /*! Gyroscope Range */ + uint8_t range; + + /*! Selects noise performance */ + uint8_t noise_perf; +}; + +/*! @name Structure to define auxiliary sensor configuration */ +struct bmi2_aux_config +{ + /*! Enable/Disable auxiliary interface */ + uint8_t aux_en; + + /*! Manual or Auto mode*/ + uint8_t manual_en; + + /*! Enables FCU write command on auxiliary interface */ + uint8_t fcu_write_en; + + /*! Read burst length for manual mode */ + uint8_t man_rd_burst; + + /*! Read burst length for data mode */ + uint8_t aux_rd_burst; + + /*! Output data rate */ + uint8_t odr; + + /*! Read-out offset */ + uint8_t offset; + + /*! I2c address of auxiliary sensor */ + uint8_t i2c_device_addr; + + /*! Read address of auxiliary sensor */ + uint8_t read_addr; +}; + +/*! @name Structure to define any-motion configuration */ +struct bmi2_any_motion_config +{ + /*! Duration in 50Hz samples(20msec) */ + uint16_t duration; + + /*! Acceleration slope threshold */ + uint16_t threshold; + + /*! To select per x-axis */ + uint16_t select_x; + + /*! To select per y-axis */ + uint16_t select_y; + + /*! To select per z-axis */ + uint16_t select_z; +}; + +/*! @name Structure to define no-motion configuration */ +struct bmi2_no_motion_config +{ + /*! Duration in 50Hz samples(20msec) */ + uint16_t duration; + + /*! Acceleration slope threshold */ + uint16_t threshold; + + /*! To select per x-axis */ + uint16_t select_x; + + /*! To select per y-axis */ + uint16_t select_y; + + /*! To select per z-axis */ + uint16_t select_z; +}; + +/*! @name Structure to define sig-motion configuration */ +struct bmi2_sig_motion_config +{ + /*! Block size */ + uint16_t block_size; + + /*! Parameter 2 */ + uint16_t param_2; + + /*! Parameter 3 */ + uint16_t param_3; + + /*! Parameter 4 */ + uint16_t param_4; + + /*! Parameter 5 */ + uint16_t param_5; +}; + +/*! @name Structure to define step counter/detector/activity configuration */ +struct bmi2_step_config +{ + /*! Water-mark level */ + uint16_t watermark_level; + + /*! Reset counter */ + uint16_t reset_counter; + + /*! Step buffer size */ + uint8_t step_buffer_size; +}; + +/*! @name Structure to define gyroscope user gain configuration */ +struct bmi2_gyro_user_gain_config +{ + /*! Gain update value for x-axis */ + uint16_t ratio_x; + + /*! Gain update value for y-axis */ + uint16_t ratio_y; + + /*! Gain update value for z-axis */ + uint16_t ratio_z; +}; + +/*! @name Structure to define wake-up configuration */ +struct bmi2_wake_up_config +{ + /*! Wake-up sensitivity for bmi261 */ + uint16_t sensitivity; + + /*! Tap feature for BMI261 + * For Single tap, single_tap_en = 1 + * For Double tap, single_tap_en = 0 + */ + uint16_t single_tap_en; + + /*! Enable -> Filtered tap data, Disable -> Unfiltered data */ + uint16_t data_reg_en; + + /*! Scaling factor of threshold */ + uint16_t tap_sens_thres; + + /*! Maximum duration between each taps */ + uint16_t max_gest_dur; + + /*! Minimum quite time between the two gesture detection */ + uint16_t quite_time_after_gest; + + /*! Wait time */ + uint16_t wait_for_timeout; + + /*! Axis selection */ + uint16_t axis_sel; +}; + +/*! @name Structure to define tap configuration */ +struct bmi2_tap_config +{ + /*! Tap sensitivity */ + uint16_t sensitivity; + + /*! Tap feature. + * For Single tap, single_tap_en = 1 + * For Double tap, single_tap_en = 0 + */ + uint16_t single_tap_en; + + /*! Enable -> Filtered tap data, Disable -> Unfiltered data */ + uint16_t data_reg_en; + + /*! Scaling factor of threshold */ + uint16_t tap_sens_thres; + + /*! Maximum duration between each taps */ + uint16_t max_gest_dur; + + /*! Minimum quite time between the two gesture detection */ + uint16_t quite_time_after_gest; + + /*! Wait time */ + uint16_t wait_for_timeout; + + /*! Axis selection */ + uint16_t axis_sel; +}; + +/*! @name Structure to define orientation configuration */ +struct bmi2_orient_config +{ + /*! Upside/down detection */ + uint16_t ud_en; + + /*! Symmetrical, high or low Symmetrical */ + uint16_t mode; + + /*! Blocking mode */ + uint16_t blocking; + + /*! Threshold angle */ + uint16_t theta; + + /*! Acceleration hysteresis for orientation detection */ + uint16_t hysteresis; +}; + +/*! @name Structure to define high-g configuration */ +struct bmi2_high_g_config +{ + /*! Acceleration threshold */ + uint16_t threshold; + + /*! Hysteresis */ + uint16_t hysteresis; + + /*! To select per x-axis */ + uint16_t select_x; + + /*! To select per y-axis */ + uint16_t select_y; + + /*! To select per z-axis */ + uint16_t select_z; + + /*! Duration interval */ + uint16_t duration; +}; + +/*! @name Structure to define low-g configuration */ +struct bmi2_low_g_config +{ + /*! Acceleration threshold */ + uint16_t threshold; + + /*! Hysteresis */ + uint16_t hysteresis; + + /*! Duration interval */ + uint16_t duration; +}; + +/*! @name Structure to define flat configuration */ +struct bmi2_flat_config +{ + /*! Theta angle for flat detection */ + uint16_t theta; + + /*! Blocking mode */ + uint16_t blocking; + + /*! Hysteresis for theta flat detection */ + uint16_t hysteresis; + + /*! Holds the duration in 50Hz samples(20msec) */ + uint16_t hold_time; +}; + +/*! @name Structure to define wrist gesture configuration */ +struct bmi2_wrist_gest_config +{ + /*! Wearable arm (left or right) */ + uint16_t wearable_arm; + + /*! Sine of the minimum tilt angle in portrait down direction of the device when wrist is rolled + * away from user. The configuration parameter is scaled by 2048 i.e. 2048 * sin(angle). + * Range is 1448 to 1774. Default value is 1774. */ + uint16_t min_flick_peak; + + /*! Value of minimum time difference between wrist roll-out and roll-in movement during flick gesture. + * Range is 3 to 5 samples at 50Hz. Default value is 4 (i.e. 0.08 seconds). */ + uint16_t min_flick_samples; + + /*! Maximum time within which gesture movement has to be completed. Range is 150 to 250 samples at 50Hz. + * Default value is 200 (i.e. 4 seconds). */ + uint16_t max_duration; +}; + +/*! @name Structure to define wrist wear wake-up configuration */ +struct bmi2_wrist_wear_wake_up_config +{ + /*! Cosine of min expected attitude change of the device within 1 second time window when + * moving within focus position. + * The parameter is scaled by 2048 i.e. 2048 * cos(angle). Range is 1024 to 1774. + * Default is 1448. */ + uint16_t min_angle_focus; + + /*! Cosine of min expected attitude change of the device within 1 second time window when + * moving from non-focus to focus position. + * The parameter is scaled by 2048 i.e. 2048 * cos(angle). Range is 1448 to 1856. + * Default value is 1774. */ + uint16_t min_angle_nonfocus; + + /*! Sine of the max allowed downward tilt angle in landscape right direction of the device, + * when it is in focus position + * (i.e. user is able to comfortably look at the dial of wear device). + * The configuration parameter is scaled by 2048 i.e. 2048 * sin(angle). Range is 700 to 1024. + * Default value is 1024. */ + uint16_t max_tilt_lr; + + /*! Sine of the max allowed downward tilt angle in landscape left direction of the device, + * when it is in focus position + * (i.e. user is able to comfortably look at the dial of wear device). + * The configuration parameter is scaled by 2048 i.e. 2048 * sin(angle). Range is 700 to + * 1024. Default value is 700. */ + uint16_t max_tilt_ll; + + /*! Sine of the max allowed backward tilt angle in portrait down direction of the device, + * when it is in focus position + * (i.e. user is able to comfortably look at the dial of wear device). + * The configuration parameter is scaled by 2048 i.e. 2048 * sin(angle). Range is 0 to179. + * Default value is 179. */ + uint16_t max_tilt_pd; + + /*! Sine of the maximum allowed forward tilt angle in portrait up direction of the + * device, when it is in focus position + * (i.e. user is able to comfortably look at the dial of wear device). + * The configuration parameter is scaled by 2048 i.e. 2048 * sin(angle). Range is 1774 to 1978. + * Default value is 1925. */ + uint16_t max_tilt_pu; +}; + +/*! @name Structure to define wrist gesture configuration for wearable variant */ +struct bmi2_wrist_gest_w_config +{ + /*! Wearable arm (left or right) */ + uint8_t device_position; + + /*! Minimum threshold for flick peak on y-axis */ + uint16_t min_flick_peak_y_threshold; + + /*! Minimum threshold for flick peak on z-axis */ + uint16_t min_flick_peak_z_threshold; + + /*! Maximum expected value of positive gravitational acceleration on x-axis + * when arm is in focus pose */ + uint16_t gravity_bounds_x_pos; + + /*! Maximum expected value of negative gravitational acceleration on x-axis + * when arm is in focus pose */ + uint16_t gravity_bounds_x_neg; + + /*! Maximum expected value of negative gravitational acceleration on y-axis + * when arm is in focus pose */ + uint16_t gravity_bounds_y_neg; + + /*! Maximum expected value of negative gravitational acceleration on z-axis + * when arm is in focus pose */ + uint16_t gravity_bounds_z_neg; + + /*! Exponential smoothing coefficient for adaptive peak threshold decay */ + uint16_t flick_peak_decay_coeff; + + /*! Exponential smoothing coefficient for acceleration mean estimation */ + uint16_t lp_mean_filter_coeff; + + /*! Maximum duration between 2 peaks of jiggle in samples @50Hz */ + uint16_t max_duration_jiggle_peaks; +}; + +/*! @name Structure to define wrist wear wake-up configuration for wearable configuration */ +struct bmi2_wrist_wear_wake_up_wh_config +{ + /*! Cosine of min expected attitude change of the device within 1 second time window when + * moving within focus position. + * The parameter is scaled by 2048 i.e. 2048 * cos(angle). Range is 1024 to 1774. + * Default is 1448. */ + uint16_t min_angle_focus; + + /*! Cosine of min expected attitude change of the device within 1 second time window when + * moving from non-focus to focus position. + * The parameter is scaled by 2048 i.e. 2048 * cos(angle). Range is 1448 to 1856. + * Default value is 1774. */ + uint16_t min_angle_nonfocus; + + /*! Sine of the max allowed downward tilt angle in landscape right direction of the device, + * when it is in focus position (i.e. user is able to comfortably look at the dial of wear device). + * The configuration parameter is scaled by 256 i.e. 256 * sin(angle). Range is 88 to 128. + * Default value is 128. */ + uint8_t angle_lr; + + /*! Sine of the max allowed downward tilt angle in landscape left direction of the device, + * when it is in focus position (i.e. user is able to comfortably look at the dial of wear device). + * The configuration parameter is scaled by 256 i.e. 256 * sin(angle). Range is 88 to 128. + * Default value is 128. */ + uint8_t angle_ll; + + /*! Sine of the max allowed backward tilt angle in portrait down direction of the device, + * when it is in focus position (i.e. user is able to comfortably look at the dial of wear device). + * The configuration parameter is scaled by 256 i.e. 256 * sin(angle). Range is 0 to 179. + * Default value is 22. */ + uint8_t angle_pd; + + /*! Sine of the maximum allowed forward tilt angle in portrait up direction of the device, + * when it is in focus position (i.e. user is able to comfortably look at the dial of wear device). + * The configuration parameter is scaled by 256 i.e. 256 * sin(angle). Range is 222 to 247. + * Default value is 241. */ + uint8_t angle_pu; + + /*! Minimum duration the arm should be moved while performing gesture. Range: 1 to 10, + * resolution = 20 ms. + * Default 2(40 ms)*/ + uint8_t min_dur_mov; + + /*! Minimum duration the arm should be static between two consecutive gestures. Range: 1 to + * 10, resolution = 20 ms + * Default 2(40 ms)*/ + uint8_t min_dur_quite; +}; + +/*! @name Structure to define primary OIS configuration */ +struct bmi2_primary_ois_config +{ + uint8_t lp_filter_enable; + + uint8_t lp_filter_config; + + uint8_t primary_ois_reserved; + + uint8_t primary_ois_gyro_en; + + uint8_t primary_ois_accel_en; +}; + +/*! @name Structure to configure free-fall detection settings */ +struct bmi2_free_fall_det_config +{ + /*! free-fall accel settings */ + uint16_t freefall_accel_settings[BMI2_FREE_FALL_ACCEL_SET_PARAMS]; +}; + +/*! @name Union to define the sensor configurations */ +union bmi2_sens_config_types +{ + /*! Accelerometer configuration */ + struct bmi2_accel_config acc; + + /*! Gyroscope configuration */ + struct bmi2_gyro_config gyr; + + /*! Auxiliary configuration */ + struct bmi2_aux_config aux; + + /*! Any-motion configuration */ + struct bmi2_any_motion_config any_motion; + + /*! No-motion configuration */ + struct bmi2_no_motion_config no_motion; + + /*! Sig_motion configuration */ + struct bmi2_sig_motion_config sig_motion; + + /*! Step counter parameter configuration */ + uint16_t step_counter_params[BMI2_STEP_CNT_N_PARAMS]; + + /*! Step counter/detector/activity configuration */ + struct bmi2_step_config step_counter; + + /*! Gyroscope user gain configuration */ + struct bmi2_gyro_user_gain_config gyro_gain_update; + + /*! Wake-up configuration */ + struct bmi2_wake_up_config tap; + + /*! Tap configuration */ + struct bmi2_tap_config tap_conf; + + /*! Orientation configuration */ + struct bmi2_orient_config orientation; + + /*! High-g configuration */ + struct bmi2_high_g_config high_g; + + /*! Low-g configuration */ + struct bmi2_low_g_config low_g; + + /*! Flat configuration */ + struct bmi2_flat_config flat; + + /*! Wrist gesture configuration */ + struct bmi2_wrist_gest_config wrist_gest; + + /*! Wrist wear wake-up configuration */ + struct bmi2_wrist_wear_wake_up_config wrist_wear_wake_up; + + /*! Wrist gesture configuration for wearable variant */ + struct bmi2_wrist_gest_w_config wrist_gest_w; + + /*! Wrist wear wake-up configuration for wearable variant */ + struct bmi2_wrist_wear_wake_up_wh_config wrist_wear_wake_up_wh; + + /*! Primary OIS configuration */ + struct bmi2_primary_ois_config primary_ois; + + /* Free-fall detection configurations */ + struct bmi2_free_fall_det_config free_fall_det; +}; + +/*! @name Structure to define the type of the sensor and its configurations */ +struct bmi2_sens_config +{ + /*! Defines the type of sensor */ + uint8_t type; + + /*! Defines various sensor configurations */ + union bmi2_sens_config_types cfg; +}; + +/*! @name Structure to define the feature configuration */ +struct bmi2_feature_config +{ + /*! Defines the type of sensor */ + uint8_t type; + + /*! Page to where the feature is mapped */ + uint8_t page; + + /*! Address of the feature */ + uint8_t start_addr; +}; + +/*! @name Structure to define the feature interrupt configurations */ +struct bmi2_map_int +{ + /*! Defines the type of sensor */ + uint8_t type; + + /*! Defines the feature interrupt */ + uint8_t sens_map_int; +}; + +/*! @name Structure to define BMI2 sensor configurations */ +struct bmi2_dev +{ + /*! Chip id of BMI2 */ + uint8_t chip_id; + + /*! The interface pointer is used to enable the user + * to link their interface descriptors for reference during the + * implementation of the read and write interfaces to the + * hardware. + */ + void *intf_ptr; + + /*! To store warnings */ + uint8_t info; + + /*! Type of Interface */ + enum bmi2_intf intf; + + /*! To store interface pointer error */ + BMI2_INTF_RETURN_TYPE intf_rslt; + + /*! For switching from I2C to SPI */ + uint8_t dummy_byte; + + /*! Resolution for FOC */ + uint8_t resolution; + + /*! User set read/write length */ + uint16_t read_write_len; + + /*! Pointer to the configuration data buffer address */ + const uint8_t *config_file_ptr; + + /*! To define maximum page number */ + uint8_t page_max; + + /*! To define maximum number of input sensors/features */ + uint8_t input_sens; + + /*! To define maximum number of output sensors/features */ + uint8_t out_sens; + + /*! Indicate manual enable for auxiliary communication */ + uint8_t aux_man_en; + + /*! Defines manual read burst length for auxiliary communication */ + uint8_t aux_man_rd_burst_len; + + /*! Array of feature input configuration structure */ + const struct bmi2_feature_config *feat_config; + + /*! Array of feature output configuration structure */ + const struct bmi2_feature_config *feat_output; + + /*! Structure to maintain a copy of the re-mapped axis */ + struct bmi2_axes_remap remap; + + /*! Flag to hold enable status of sensors */ + uint64_t sens_en_stat; + + /*! Read function pointer */ + bmi2_read_fptr_t read; + + /*! Write function pointer */ + bmi2_write_fptr_t write; + + /*! Delay function pointer */ + bmi2_delay_fptr_t delay_us; + + /*! To store the gyroscope cross sensitivity value */ + int16_t gyr_cross_sens_zx; + + /* gyro enable status, used as a flag in CRT enabling and aborting */ + uint8_t gyro_en : 1; + + /* advance power saving mode status, used as a flag in CRT enabling and aborting */ + uint8_t aps_status; + + /* used as a flag to enable variant specific features like crt */ + uint16_t variant_feature; + + /* To store hold the size of config file */ + uint16_t config_size; + + /*! Function pointer to get wakeup configurations */ + bmi2_wake_up_fptr_t get_wakeup_config; + + /*! Function pointer to set wakeup configurations */ + bmi2_wake_up_fptr_t set_wakeup_config; + + /*! Function pointer to get tap configurations */ + bmi2_tap_fptr_t get_tap_config; + + /*! Function pointer to set tap configurations */ + bmi2_tap_fptr_t set_tap_config; + + /*! Array of feature interrupts configuration structure */ + struct bmi2_map_int *map_int; + + /*! To define maximum number of interrupts */ + uint8_t sens_int_map; +}; + +/*! @name Structure to enable an accel axis for foc */ +struct bmi2_accel_foc_g_value +{ + /*! '0' to disable x axis and '1' to enable x axis */ + uint8_t x; + + /*! '0' to disable y axis and '1' to enable y axis */ + uint8_t y; + + /*! '0' to disable z axis and '1' to enable z axis */ + uint8_t z; + + /*! '0' for positive input and '1' for negative input */ + uint8_t sign; +}; + +/*! @name Structure to configure activity recognition settings */ +struct bmi2_act_recg_sett +{ + /*! Activity recognition register 1 */ + uint8_t act_rec_1 : 1; + + /*! Activity recognition register 2 */ + uint16_t act_rec_2; + + /*! Activity recognition register 3 */ + uint16_t act_rec_3; + + /*! Activity recognition register 4 */ + uint8_t act_rec_4 : 4; + + /*! Activity recognition register 5 */ + uint8_t act_rec_5 : 4; +}; + +/*! @name Structure to configure activity recognition settings for bmi270hc */ +struct bmi2_hc_act_recg_sett +{ + /*! Static segment size for activity classification. */ + uint8_t segment_size; + + /*! Enable/Disable post processing of the activity detected */ + uint8_t pp_en; + + /*! Minimum threshold of the Gini's diversity index (GDI) */ + uint16_t min_gdi_thres; + + /*! Maximum threshold of the Gini's diversity index (GDI) */ + uint16_t max_gdi_thres; + + /*! Buffer size for post processing of the activity detected */ + uint16_t buf_size; + + /*! Minimum segments belonging to a certain activity type */ + uint16_t min_seg_conf; +}; + +#endif /* BMI2_DEFS_H_ */ diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2_ois.c b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2_ois.c new file mode 100644 index 0000000000..04350db08d --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2_ois.c @@ -0,0 +1,417 @@ +/** + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * BSD-3-Clause + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING + * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * @file bmi2_ois.c + * @date 2020-05-05 + * @version v2.23.2 + * + */ + +/******************************************************************************/ + +/*! @name Header Files */ +/******************************************************************************/ +#include "bmi2_ois.h" + +/******************************************************************************/ + +/*! Local Function Prototypes + ******************************************************************************/ + +/*! + * @brief This internal API gets the OIS accelerometer and the gyroscope data. + * + * @param[out] ois_data : Structure instance of bmi2_sens_axes_data. + * @param[in] reg_addr : Register address where data is stored. + * @param[in] ois_dev : Structure instance of bmi2_ois_dev. + * @param[in] ois_gyr_cross_sens_zx :"gyroscope cross sensitivity value which was calculated during + * bmi2xy_init(), refer the example ois_accel_gyro.c for more info" + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t get_ois_acc_gyr_data(struct bmi2_ois_sens_axes_data *ois_data, + uint8_t reg_addr, + struct bmi2_ois_dev *ois_dev, + int16_t ois_gyr_cross_sens_zx); + +/*! + * @brief This internal API is used to validate the OIS device pointer for null + * conditions. + * + * @param[in] ois_dev : Structure instance of bmi2_ois_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +static int8_t null_ptr_check(const struct bmi2_ois_dev *ois_dev); + +/*! + * @brief This internal API corrects the gyroscope cross-axis sensitivity + * between the z and the x axis. + * + * @param[in] ois_dev : Structure instance of bmi2_ois_dev. + * @param[in] ois_gyr_cross_sens_zx : "gyroscope cross sensitivity value which was calculated during bmi2xy_init(), + * refer the example ois_accel_gyro.c for more info" + * + */ +static void comp_gyro_cross_axis_sensitivity(struct bmi2_ois_sens_axes_data *ois_data, int16_t ois_gyr_cross_sens_zx); + +/******************************************************************************/ +/*! @name User Interface Definitions */ +/******************************************************************************/ + +/*! + * @brief This API reads the data from the given OIS register address of bmi2 + * sensor. + */ +int8_t bmi2_ois_get_regs(uint8_t ois_reg_addr, uint8_t *ois_reg_data, uint16_t data_len, struct bmi2_ois_dev *ois_dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define dummy byte for SPI read */ + uint8_t dummy_byte = 1; + + /* Variable to define temporary length */ + uint16_t temp_len = data_len + dummy_byte; + + /* Variable to define temporary buffer */ + uint8_t temp_buf[temp_len]; + + /* Variable to index bytes read */ + uint16_t index = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(ois_dev); + if ((rslt == BMI2_OIS_OK) && (ois_reg_data != NULL)) + { + /* Configuring reg_addr for SPI Interface */ + ois_reg_addr = (ois_reg_addr | BMI2_OIS_SPI_RD_MASK); + + /* Read from OIS register through OIS interface */ + ois_dev->intf_rslt = ois_dev->ois_read(ois_reg_addr, temp_buf, temp_len, ois_dev->intf_ptr); + if (ois_dev->intf_rslt == BMI2_INTF_RET_SUCCESS) + { + /* Read the data from the position next to dummy byte */ + while (index < data_len) + { + ois_reg_data[index] = temp_buf[index + dummy_byte]; + index++; + } + } + else + { + rslt = BMI2_OIS_E_COM_FAIL; + } + } + else + { + rslt = BMI2_OIS_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API writes data to the given OIS register address of bmi2 sensor. + */ +int8_t bmi2_ois_set_regs(uint8_t ois_reg_addr, + const uint8_t *ois_reg_data, + uint16_t data_len, + struct bmi2_ois_dev *ois_dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Null-pointer check */ + rslt = null_ptr_check(ois_dev); + if ((rslt == BMI2_OIS_OK) && (ois_reg_data != NULL)) + { + /* Configuring reg_addr for SPI Interface */ + ois_reg_addr = (ois_reg_addr & BMI2_OIS_SPI_WR_MASK); + + /* Burst write to OIS register through OIS interface */ + ois_dev->intf_rslt = ois_dev->ois_write(ois_reg_addr, ois_reg_data, data_len, ois_dev->intf_ptr); + if (ois_dev->intf_rslt != BMI2_INTF_RET_SUCCESS) + { + rslt = BMI2_OIS_E_COM_FAIL; + } + } + else + { + rslt = BMI2_OIS_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This API enables/disables accelerometer/gyroscope data read through + * OIS interface. + */ +int8_t bmi2_ois_set_config(struct bmi2_ois_dev *ois_dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store data */ + uint8_t reg_data = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(ois_dev); + if (rslt == BMI2_OIS_OK) + { + rslt = bmi2_ois_get_regs(BMI2_OIS_CONFIG_ADDR, ®_data, 1, ois_dev); + if (rslt == BMI2_OIS_OK) + { + /* Enable/Disable Low pass filter */ + reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_OIS_LP_FILTER_EN, ois_dev->lp_filter_en); + + /* Configures Low pass filter cut-off frequency */ + reg_data = BMI2_OIS_SET_BITS(reg_data, BMI2_OIS_LP_FILTER_CONFIG, ois_dev->lp_filter_config); + + /* Low pass filter - mute on secondary interface */ + reg_data = BMI2_OIS_SET_BITS(reg_data, BMI2_OIS_LP_FILTER_MUTE, ois_dev->lp_filter_mute); + + /* Enable/Disable ois on accelerometer */ + reg_data = BMI2_OIS_SET_BITS(reg_data, BMI2_OIS_ACC_EN, ois_dev->acc_en); + + /* Enable/Disable ois on gyroscope */ + reg_data = BMI2_OIS_SET_BITS(reg_data, BMI2_OIS_GYR_EN, ois_dev->gyr_en); + + /* Set the OIS configurations */ + rslt = bmi2_ois_set_regs(BMI2_OIS_CONFIG_ADDR, ®_data, 1, ois_dev); + } + } + + return rslt; +} + +/*! + * @brief This API gets the status of accelerometer/gyroscope enable for data + * read through OIS interface. + */ +int8_t bmi2_ois_get_config(struct bmi2_ois_dev *ois_dev) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to store data */ + uint8_t reg_data = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(ois_dev); + if (rslt == BMI2_OIS_OK) + { + rslt = bmi2_ois_get_regs(BMI2_OIS_CONFIG_ADDR, ®_data, 1, ois_dev); + if (rslt == BMI2_OIS_OK) + { + /* Get the status of Low pass filter enable */ + ois_dev->lp_filter_en = BMI2_GET_BIT_POS0(reg_data, BMI2_OIS_LP_FILTER_EN); + + /* Get the status of Low pass filter cut-off frequency */ + ois_dev->lp_filter_config = BMI2_OIS_GET_BITS(reg_data, BMI2_OIS_LP_FILTER_CONFIG); + + /* Get the status of Low pass filter mute */ + ois_dev->lp_filter_mute = BMI2_OIS_GET_BITS(reg_data, BMI2_OIS_LP_FILTER_MUTE); + + /* Get the status of accelerometer enable */ + ois_dev->acc_en = BMI2_OIS_GET_BITS(reg_data, BMI2_OIS_ACC_EN); + + /* Get the status of gyroscope enable */ + ois_dev->gyr_en = BMI2_OIS_GET_BITS(reg_data, BMI2_OIS_GYR_EN); + } + } + + return rslt; +} + +/*! + * @brief This API reads accelerometer/gyroscope data through OIS interface. + */ +int8_t bmi2_ois_read_data(const uint8_t *sens_sel, + uint8_t n_sens, + struct bmi2_ois_dev *ois_dev, + int16_t gyr_cross_sens_zx) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variable to define loop */ + uint8_t loop = 0; + + /* Variable to update register address */ + uint8_t reg_addr = 0; + + /* Null-pointer check */ + rslt = null_ptr_check(ois_dev); + if ((rslt == BMI2_OIS_OK) && (sens_sel != NULL)) + { + + for (loop = 0; loop < n_sens; loop++) + { + switch (sens_sel[loop]) + { + case BMI2_OIS_ACCEL: + + /* Update OIS accelerometer address */ + reg_addr = BMI2_OIS_ACC_X_LSB_ADDR; + + /* Get OIS accelerometer data */ + rslt = get_ois_acc_gyr_data(&ois_dev->acc_data, reg_addr, ois_dev, 0); + break; + case BMI2_OIS_GYRO: + + /* Update OIS gyroscope address */ + reg_addr = BMI2_OIS_GYR_X_LSB_ADDR; + + /* Get OIS gyroscope data */ + rslt = get_ois_acc_gyr_data(&ois_dev->gyr_data, reg_addr, ois_dev, gyr_cross_sens_zx); + break; + default: + rslt = BMI2_OIS_E_INVALID_SENSOR; + break; + } + + /* Return error if any of the get sensor data fails */ + if (rslt != BMI2_OIS_OK) + { + break; + } + } + } + else + { + rslt = BMI2_OIS_E_NULL_PTR; + } + + return rslt; +} + +/***************************************************************************/ + +/*! Local Function Definitions + ****************************************************************************/ + +/*! @cond DOXYGEN_SUPRESS */ + +/* Suppressing doxygen warnings triggered for same static function names present across various sensor variant + * directories */ + +/*! + * @brief This internal API gets the accelerometer and the gyroscope data. + */ +static int8_t get_ois_acc_gyr_data(struct bmi2_ois_sens_axes_data *ois_data, + uint8_t reg_addr, + struct bmi2_ois_dev *ois_dev, + int16_t ois_gyr_cross_sens_zx) +{ + /* Variable to define error */ + int8_t rslt; + + /* Variables to store MSB value */ + uint8_t msb; + + /* Variables to store LSB value */ + uint8_t lsb; + + /* Variables to store both MSB and LSB value */ + uint16_t msb_lsb; + + /* Variables to define index */ + uint8_t index = 0; + + /* Array to define data stored in register */ + uint8_t reg_data[BMI2_OIS_ACC_GYR_NUM_BYTES] = { 0 }; + + /* Read the sensor data */ + rslt = bmi2_ois_get_regs(reg_addr, reg_data, BMI2_OIS_ACC_GYR_NUM_BYTES, ois_dev); + if (rslt == BMI2_OIS_OK) + { + /* Read x-axis data */ + lsb = reg_data[index++]; + msb = reg_data[index++]; + msb_lsb = ((uint16_t)msb << 8) | (uint16_t)lsb; + ois_data->x = (int16_t)msb_lsb; + + /* Read y-axis data */ + lsb = reg_data[index++]; + msb = reg_data[index++]; + msb_lsb = ((uint16_t)msb << 8) | (uint16_t)lsb; + ois_data->y = (int16_t)msb_lsb; + + /* Read z-axis data */ + lsb = reg_data[index++]; + msb = reg_data[index++]; + msb_lsb = ((uint16_t)msb << 8) | (uint16_t)lsb; + ois_data->z = (int16_t)msb_lsb; + + comp_gyro_cross_axis_sensitivity(ois_data, ois_gyr_cross_sens_zx); + } + + return rslt; +} + +/*! + * @brief This internal API is used to validate the device structure pointer for + * null conditions. + */ +static int8_t null_ptr_check(const struct bmi2_ois_dev *ois_dev) +{ + /* Variable to define error */ + int8_t rslt = BMI2_OIS_OK; + + if ((ois_dev == NULL) || (ois_dev->ois_read == NULL) || (ois_dev->ois_write == NULL) || + (ois_dev->ois_delay_us == NULL)) + { + /* Device structure pointer is NULL */ + rslt = BMI2_OIS_E_NULL_PTR; + } + + return rslt; +} + +/*! + * @brief This internal API corrects the gyroscope cross-axis sensitivity + * between the z and the x axis. + */ +static void comp_gyro_cross_axis_sensitivity(struct bmi2_ois_sens_axes_data *ois_data, int16_t ois_gyr_cross_sens_zx) +{ + + /* Get the compensated gyroscope x-axis */ + ois_data->x = ois_data->x - (int16_t)(((int32_t)ois_gyr_cross_sens_zx * (int32_t)ois_data->z) / 512); +} + +/*! @endcond */ diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2_ois.h b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2_ois.h new file mode 100644 index 0000000000..9b700cd955 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2_ois.h @@ -0,0 +1,387 @@ +/** + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * BSD-3-Clause + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING + * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * @file bmi2_ois.h + * @date 2020-05-05 + * @version v2.23.2 + * + */ + +/** + * \ingroup bmi2xy + * \defgroup bmi2_ois BMI2_OIS + * @brief Sensor driver for BMI2_OIS sensor + */ +#ifndef _BMI2_OIS_H +#define _BMI2_OIS_H + +/*! CPP guard */ +#ifdef __cplusplus +extern "C" { +#endif + +/***************************************************************************/ + +/*! Header files + ****************************************************************************/ +#ifdef __KERNEL__ +#include +#include +#else +#include +#include +#endif + +/******************************************************************************/ +/*! @name Macros */ +/******************************************************************************/ + +#ifndef BMI2_INTF_RETURN_TYPE +#define BMI2_INTF_RETURN_TYPE int8_t +#endif + +/*! @name Utility macros */ +#define BMI2_OIS_SET_BITS(reg_data, bitname, data) \ + ((reg_data & ~(bitname##_MASK)) | \ + ((data << bitname##_POS) & bitname##_MASK)) + +#define BMI2_OIS_GET_BITS(reg_data, bitname) \ + ((reg_data & (bitname##_MASK)) >> \ + (bitname##_POS)) + +#define BMI2_SET_BIT_POS0(reg_data, bitname, data) \ + ((reg_data & ~(bitname##_MASK)) | \ + (data & bitname##_MASK)) + +#define BMI2_GET_BIT_POS0(reg_data, bitname) (reg_data & (bitname##_MASK)) + +/*! @name For enable and disable */ +#define BMI2_OIS_ENABLE UINT8_C(1) +#define BMI2_OIS_DISABLE UINT8_C(0) + +/*! @name To define sensor interface success code */ +#define BMI2_INTF_RET_SUCCESS INT8_C(0) + +/*! @name To define success code */ +#define BMI2_OIS_OK UINT8_C(0) + +/*! @name To define error codes */ +#define BMI2_OIS_E_NULL_PTR INT8_C(-1) +#define BMI2_OIS_E_COM_FAIL INT8_C(-2) +#define BMI2_OIS_E_INVALID_SENSOR INT8_C(-8) + +/*! @name Mask definitions for SPI read/write address for OIS */ +#define BMI2_OIS_SPI_RD_MASK UINT8_C(0x80) +#define BMI2_OIS_SPI_WR_MASK UINT8_C(0x7F) + +/*! @name BMI2 OIS data bytes */ +#define BMI2_OIS_ACC_GYR_NUM_BYTES UINT8_C(6) + +/*! @name Macros to select sensor for OIS data read */ +#define BMI2_OIS_ACCEL UINT8_C(0x01) +#define BMI2_OIS_GYRO UINT8_C(0x02) + +/*! @name Macros to define OIS register addresses */ +#define BMI2_OIS_CONFIG_ADDR UINT8_C(0x40) +#define BMI2_OIS_ACC_X_LSB_ADDR UINT8_C(0x0C) +#define BMI2_OIS_GYR_X_LSB_ADDR UINT8_C(0x12) + +/*! @name Mask definitions for OIS configurations */ +#define BMI2_OIS_GYR_EN_MASK UINT8_C(0x40) +#define BMI2_OIS_ACC_EN_MASK UINT8_C(0x80) + +/*! @name Bit Positions for OIS configurations */ +#define BMI2_OIS_GYR_EN_POS UINT8_C(0x06) +#define BMI2_OIS_ACC_EN_POS UINT8_C(0x07) + +/*! Low pass filter configuration position and mask */ +#define BMI2_OIS_LP_FILTER_EN_POS UINT8_C(0x00) +#define BMI2_OIS_LP_FILTER_EN_MASK UINT8_C(0x01) + +#define BMI2_OIS_LP_FILTER_CONFIG_POS UINT8_C(0x01) +#define BMI2_OIS_LP_FILTER_CONFIG_MASK UINT8_C(0x06) + +#define BMI2_OIS_LP_FILTER_MUTE_POS UINT8_C(0x05) +#define BMI2_OIS_LP_FILTER_MUTE_MASK UINT8_C(0x20) + +/******************************************************************************/ +/*! @name Function Pointers */ +/******************************************************************************/ + +/*! + * @brief Bus communication function pointer which should be mapped to + * the platform specific read functions of the user + * + * @param[in] reg_addr : Register address from which data is read. + * @param[out] reg_data : Pointer to data buffer where read data is stored. + * @param[in] len : Number of bytes of data to be read. + * @param[in, out] intf_ptr : Void pointer that can enable the linking of descriptors + * for interface related call backs. + * + * retval = BMI2_INTF_RET_SUCCESS -> Success + * retval != BMI2_INTF_RET_SUCCESS -> Failure + * + */ +typedef BMI2_INTF_RETURN_TYPE (*bmi2_ois_read_fptr_t)(uint8_t reg_addr, uint8_t *data, uint32_t len, void *intf_ptr); + +/*! + * @brief Bus communication function pointer which should be mapped to + * the platform specific write functions of the user + * + * @param[in] reg_addr : Register address to which the data is written. + * @param[in] reg_data : Pointer to data buffer in which data to be written + * is stored. + * @param[in] len : Number of bytes of data to be written. + * @param[in, out] intf_ptr : Void pointer that can enable the linking of descriptors + * for interface related call backs + * + * retval = BMI2_INTF_RET_SUCCESS -> Success + * retval != BMI2_INTF_RET_SUCCESS -> Failure + * + */ +typedef BMI2_INTF_RETURN_TYPE (*bmi2_ois_write_fptr_t)(uint8_t reg_addr, const uint8_t *data, uint32_t len, + void *intf_ptr); + +/*! + * @brief Delay function pointer which should be mapped to + * delay function of the user + * + * @param[in] period : Delay in microseconds. + * @param[in, out] intf_ptr : Void pointer that can enable the linking of descriptors + * for interface related call backs + * + */ +typedef void (*bmi2_ois_delay_fptr_t)(uint32_t period, void *intf_ptr); + +/******************************************************************************/ +/*! @name Structure Declarations */ +/******************************************************************************/ +/*! @name Structure to define accelerometer and gyroscope sensor axes for OIS */ +struct bmi2_ois_sens_axes_data +{ + /*! Data in x-axis */ + int16_t x; + + /*! Data in y-axis */ + int16_t y; + + /*! Data in z-axis */ + int16_t z; + +}; + +/*! @name Structure to define bmi2 OIS sensor configurations */ +struct bmi2_ois_dev +{ + /*! Read function pointer */ + bmi2_ois_read_fptr_t ois_read; + + /*! Write function pointer */ + bmi2_ois_write_fptr_t ois_write; + + /*! Delay function pointer */ + bmi2_ois_delay_fptr_t ois_delay_us; + + /*! Low pass filter en/dis */ + uint8_t lp_filter_en; + + /*! Void interface pointer */ + void *intf_ptr; + + /*! To store interface pointer error */ + int8_t intf_rslt; + + /*! Low pass filter cut-off frequency */ + uint8_t lp_filter_config; + + /*! Low pass filter mute */ + uint8_t lp_filter_mute; + + /*! Accelerometer enable for OIS */ + uint8_t acc_en; + + /*! Gyroscope enable for OIS */ + uint8_t gyr_en; + + /*! Accelerometer data axes */ + struct bmi2_ois_sens_axes_data acc_data; + + /*! Gyroscope data axes */ + struct bmi2_ois_sens_axes_data gyr_data; +}; + +/***************************************************************************/ + +/*! BMI2 OIS User Interface function prototypes + ****************************************************************************/ + +/** + * \ingroup bmi2_ois + * \defgroup bmi2_oisApiRegs Registers + * @brief Read data from the given OIS register address of bmi2 + */ + +/*! + * \ingroup bmi2_oisApiRegs + * \page bmi2_ois_api_bmi2_ois_get_regs bmi2_ois_get_regs + * \code + * int8_t bmi2_ois_get_regs(uint8_t ois_reg_addr, + * uint8_t *ois_reg_data, + * uint16_t data_len, + * const struct bmi2_ois_dev *ois_dev); + * \endcode + * @details This API reads the data from the given OIS register address of bmi2 + * sensor. + * + * @param[in] ois_reg_addr : OIS register address from which data is read. + * @param[out] ois_reg_data : Pointer to data buffer where read data is stored. + * @param[in] data_len : No. of bytes of data to be read. + * @param[in] ois_dev : Structure instance of bmi2_ois_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_ois_get_regs(uint8_t ois_reg_addr, uint8_t *ois_reg_data, uint16_t data_len, struct bmi2_ois_dev *ois_dev); + +/*! + * \ingroup bmi2_oisApiRegs + * \page bmi2_ois_api_bmi2_ois_set_regs bmi2_ois_set_regs + * \code + * int8_t bmi2_ois_set_regs(uint8_t ois_reg_addr, + * uint8_t *ois_reg_data, + * uint16_t data_len, + * const struct bmi2_ois_dev *ois_dev); + * \endcode + * @details This API writes data to the given OIS register address of bmi2 sensor. + * + * @param[in] ois_reg_addr : OIS register address to which the data is written. + * @param[in] ois_reg_data : Pointer to data buffer in which data to be written + * is stored. + * @param[in] data_len : No. of bytes of data to be written. + * @param[in] ois_dev : Structure instance of bmi2_ois_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_ois_set_regs(uint8_t ois_reg_addr, + const uint8_t *ois_reg_data, + uint16_t data_len, + struct bmi2_ois_dev *ois_dev); + +/** + * \ingroup bmi2_ois + * \defgroup bmi2_oisApiConfig Status update + * @brief Get / Set the status of Enable / Disable accelerometer / gyroscope data read through OIS interface + */ + +/*! + * \ingroup bmi2_oisApiConfig + * \page bmi2_ois_api_bmi2_ois_set_config bmi2_ois_set_config + * \code + * int8_t bmi2_ois_set_config(const struct bmi2_ois_dev *ois_dev); + * \endcode + * @details This API sets the status of enable/disable accelerometer/gyroscope data read through + * OIS interface. + * + * @param[in] ois_dev : Structure instance of bmi2_ois_dev. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_ois_set_config(struct bmi2_ois_dev *ois_dev); + +/*! + * \ingroup bmi2_oisApiConfig + * \page bmi2_ois_api_bmi2_ois_get_config bmi2_ois_get_config + * \code + * int8_t bmi2_ois_get_config(struct bmi2_ois_dev *ois_dev); + * \endcode + * @details This API gets the status of accelerometer/gyroscope enable for data + * read through OIS interface. + * + * @param[in, out] ois_dev : Structure instance of bmi2_ois_dev. + * + * @note Enabling and disabling is done during OIS structure initialization. + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_ois_get_config(struct bmi2_ois_dev *ois_dev); + +/** + * \ingroup bmi2_ois + * \defgroup bmi2_oisApiRead Data read + * @brief Read accelerometer / gyroscope data through OIS interface + */ + +/*! + * \ingroup bmi2_oisApiRead + * \page bmi2_ois_api_bmi2_ois_read_data bmi2_ois_read_data + * \code + * int8_t bmi2_ois_read_data(const uint8_t *sens_sel, + * uint8_t n_sens, + * struct bmi2_ois_dev *ois_dev, + * int16_t gyr_cross_sens_zx); + * \endcode + * @details This API reads accelerometer/gyroscope data through OIS interface. + * + * @param[in] sens_sel : Select sensor whose data is to be read. + * @param[in] n_sens : Number of sensors selected. + * @param[in, out] ois_dev : Structure instance of bmi2_ois_dev. + * @param[in] gyr_cross_sens_zx : Store the gyroscope cross sensitivity values taken from the bmi2xy + * (refer bmi2_ois example). + * + *@verbatim + * sens_sel | Value + * ---------------|--------------- + * BMI2_OIS_ACCEL | 0x01 + * BMI2_OIS_GYRO | 0x02 + *@endverbatim + * + * @return Result of API execution status + * @retval 0 -> Success + * @retval < 0 -> Fail + */ +int8_t bmi2_ois_read_data(const uint8_t *sens_sel, + uint8_t n_sens, + struct bmi2_ois_dev *ois_dev, + int16_t gyr_cross_sens_zx); + +#ifdef __cplusplus +} +#endif /* End of CPP guard */ + +#endif /* End of _BMI2_OIS_H */ diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/accel/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/accel/Makefile new file mode 100644 index 0000000000..c63d602ce5 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/accel/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= accel.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/accel/accel.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/accel/accel.c new file mode 100644 index 0000000000..ea385d7441 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/accel/accel.c @@ -0,0 +1,200 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270.h" +#include "common.h" + +/******************************************************************************/ +/*! Macro definition */ + +/*! Earth's gravity in m/s^2 */ +#define GRAVITY_EARTH (9.80665f) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t set_accel_config(struct bmi2_dev *bmi2_dev); + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + * + * @param[in] val : LSB from each axis. + * @param[in] g_range : Gravity range. + * @param[in] bit_width : Resolution for accel. + * + * @return Gravity. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to define limit to print accel data. */ + uint8_t limit = 20; + + /* Assign accel sensor to variable. */ + uint8_t sensor_list = BMI2_ACCEL; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Create an instance of sensor data structure. */ + struct bmi2_sensor_data sensor_data = { 0 }; + + /* Initialize the interrupt status of accel. */ + uint16_t int_status = 0; + + uint8_t indx = 0; + float x = 0, y = 0, z = 0; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270. */ + rslt = bmi270_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the accel sensor. */ + rslt = bmi270_sensor_enable(&sensor_list, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Accel configuration settings. */ + rslt = set_accel_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Assign accel sensor. */ + sensor_data.type = BMI2_ACCEL; + printf("Accel and m/s2 data \n"); + printf("Accel data collected at 2G Range with 16-bit resolution\n"); + + /* Loop to print the accel data when interrupt occurs. */ + while (indx <= limit) + { + /* To get the status of accel data ready interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the accel data ready interrupt status and print the status for 10 samples. */ + if (int_status & BMI2_ACC_DRDY_INT_MASK) + { + /* Get accelerometer data for x, y and z axis. */ + rslt = bmi270_get_sensor_data(&sensor_data, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + printf("\nAcc_X = %d\t", sensor_data.sens_data.acc.x); + printf("Acc_Y = %d\t", sensor_data.sens_data.acc.y); + printf("Acc_Z = %d\r\n", sensor_data.sens_data.acc.z); + + /* Converting lsb to meter per second squared for 16 bit accelerometer at 2G range. */ + x = lsb_to_mps2(sensor_data.sens_data.acc.x, 2, bmi2_dev.resolution); + y = lsb_to_mps2(sensor_data.sens_data.acc.y, 2, bmi2_dev.resolution); + z = lsb_to_mps2(sensor_data.sens_data.acc.z, 2, bmi2_dev.resolution); + + /* Print the data in m/s2. */ + printf("\nAcc_ms2_X = %4.2f, Acc_ms2_Y = %4.2f, Acc_ms2_Z = %4.2f\n", x, y, z); + + indx++; + } + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel. + */ +static int8_t set_accel_config(struct bmi2_dev *bmi2_dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accelerometer configuration. */ + struct bmi2_sens_config config; + + /* Configure the type of feature. */ + config.type = BMI2_ACCEL; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_get_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* Set Output Data Rate */ + config.cfg.acc.odr = BMI2_ACC_ODR_200HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config.cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples. + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config.cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config.cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set the accel configurations. */ + rslt = bmi270_set_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Map data ready interrupt to interrupt pin. */ + rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (GRAVITY_EARTH * val * g_range) / half_scale; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/accel_gyro/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/accel_gyro/Makefile new file mode 100644 index 0000000000..c0617d02e2 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/accel_gyro/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= accel_gyro.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/accel_gyro/accel_gyro.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/accel_gyro/accel_gyro.c new file mode 100644 index 0000000000..1e7c9dfc06 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/accel_gyro/accel_gyro.c @@ -0,0 +1,268 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270.h" +#include "common.h" + +/******************************************************************************/ +/*! Macro definition */ + +/*! Earth's gravity in m/s^2 */ +#define GRAVITY_EARTH (9.80665f) + +/*! Macros to select the sensors */ +#define ACCEL UINT8_C(0x00) +#define GYRO UINT8_C(0x01) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev); + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + * + * @param[in] val : LSB from each axis. + * @param[in] g_range : Gravity range. + * @param[in] bit_width : Resolution for accel. + * + * @return Gravity. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width); + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + * + * @param[in] val : LSB from each axis. + * @param[in] dps : Degree per second. + * @param[in] bit_width : Resolution for gyro. + * + * @return Degree per second. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to define limit to print accel data. */ + uint8_t limit = 10; + + /* Assign accel and gyro sensor to variable. */ + uint8_t sensor_list[2] = { BMI2_ACCEL, BMI2_GYRO }; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Create an instance of sensor data structure. */ + struct bmi2_sensor_data sensor_data[2] = { { 0 } }; + + /* Initialize the interrupt status of accel and gyro. */ + uint16_t int_status = 0; + + uint8_t indx = 1; + + float x = 0, y = 0, z = 0; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_SPI_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270. */ + rslt = bmi270_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the accel and gyro sensor. */ + rslt = bmi270_sensor_enable(sensor_list, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Accel and gyro configuration settings. */ + rslt = set_accel_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Assign accel and gyro sensor. */ + sensor_data[ACCEL].type = BMI2_ACCEL; + sensor_data[GYRO].type = BMI2_GYRO; + + /* Loop to print accel and gyro data when interrupt occurs. */ + while (indx <= limit) + { + /* To get the data ready interrupt status of accel and gyro. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the data ready interrupt status and print the status for 10 samples. */ + if ((int_status & BMI2_ACC_DRDY_INT_MASK) && (int_status & BMI2_GYR_DRDY_INT_MASK)) + { + /* Get accel and gyro data for x, y and z axis. */ + rslt = bmi270_get_sensor_data(sensor_data, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("\n******* Accel(Raw and m/s2) Gyro(Raw and dps) data : %d *******\n", indx); + + printf("\nAcc_x = %d\t", sensor_data[ACCEL].sens_data.acc.x); + printf("Acc_y = %d\t", sensor_data[ACCEL].sens_data.acc.y); + printf("Acc_z = %d", sensor_data[ACCEL].sens_data.acc.z); + + /* Converting lsb to meter per second squared for 16 bit accelerometer at 2G range. */ + x = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.x, 2, bmi2_dev.resolution); + y = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.y, 2, bmi2_dev.resolution); + z = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.z, 2, bmi2_dev.resolution); + + /* Print the data in m/s2. */ + printf("\nAcc_ms2_X = %4.2f, Acc_ms2_Y = %4.2f, Acc_ms2_Z = %4.2f\n", x, y, z); + + printf("\nGyr_X = %d\t", sensor_data[GYRO].sens_data.gyr.x); + printf("Gyr_Y = %d\t", sensor_data[GYRO].sens_data.gyr.y); + printf("Gyr_Z= %d\n", sensor_data[GYRO].sens_data.gyr.z); + + /* Converting lsb to degree per second for 16 bit gyro at 2000dps range. */ + x = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.x, 2000, bmi2_dev.resolution); + y = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.y, 2000, bmi2_dev.resolution); + z = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.z, 2000, bmi2_dev.resolution); + + /* Print the data in dps. */ + printf("Gyro_DPS_X = %4.2f, Gyro_DPS_Y = %4.2f, Gyro_DPS_Z = %4.2f\n", x, y, z); + + indx++; + } + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accelerometer and gyro configuration. */ + struct bmi2_sens_config config[2]; + + /* Configure the type of feature. */ + config[ACCEL].type = BMI2_ACCEL; + config[GYRO].type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_get_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Map data ready interrupt to interrupt pin. */ + rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* Set Output Data Rate */ + config[ACCEL].cfg.acc.odr = BMI2_ACC_ODR_200HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config[ACCEL].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples. + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config[ACCEL].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config[ACCEL].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* The user can change the following configuration parameters according to their requirement. */ + /* Set Output Data Rate */ + config[GYRO].cfg.gyr.odr = BMI2_GYR_ODR_200HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config[GYRO].cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config[GYRO].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config[GYRO].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config[GYRO].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set the accel and gyro configurations. */ + rslt = bmi270_set_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (GRAVITY_EARTH * val * g_range) / half_scale; +} + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (dps / ((half_scale) + BMI2_GYR_RANGE_2000)) * (val); +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/any_motion_interrupt/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/any_motion_interrupt/Makefile new file mode 100644 index 0000000000..9158f05773 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/any_motion_interrupt/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= any_motion_interrupt.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/any_motion_interrupt/any_motion_interrupt.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/any_motion_interrupt/any_motion_interrupt.c new file mode 100644 index 0000000000..062b29eb31 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/any_motion_interrupt/any_motion_interrupt.c @@ -0,0 +1,135 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270.h" +#include "common.h" + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for any-motion. + * + * @param[in] bmi2_dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t set_feature_config(struct bmi2_dev *bmi2_dev); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Accel sensor and any-motion feature are listed in array. */ + uint8_t sens_list[2] = { BMI2_ACCEL, BMI2_ANY_MOTION }; + + /* Variable to get any-motion interrupt status. */ + uint16_t int_status = 0; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Select features and their pins to be mapped to. */ + struct bmi2_sens_int_config sens_int = { .type = BMI2_ANY_MOTION, .hw_int_pin = BMI2_INT1 }; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270. */ + rslt = bmi270_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the selected sensors. */ + rslt = bmi270_sensor_enable(sens_list, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Set feature configurations for any-motion. */ + rslt = set_feature_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Map the feature interrupt for any-motion. */ + rslt = bmi270_map_feat_int(&sens_int, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + printf("Move the board\n"); + + /* Loop to get any-motion interrupt. */ + do + { + /* Clear buffer. */ + int_status = 0; + + /* To get the interrupt status of any-motion. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the interrupt status of any-motion. */ + if (int_status & BMI270_ANY_MOT_STATUS_MASK) + { + printf("Any-motion interrupt is generated\n"); + break; + } + } while (rslt == BMI2_OK); + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for any-motion. + */ +static int8_t set_feature_config(struct bmi2_dev *bmi2_dev) +{ + + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define the type of sensor and its configurations. */ + struct bmi2_sens_config config; + + /* Configure the type of feature. */ + config.type = BMI2_ANY_MOTION; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_get_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* 1LSB equals 20ms. Default is 100ms, setting to 80ms. */ + config.cfg.any_motion.duration = 0x04; + + /* 1LSB equals to 0.48mg. Default is 83mg, setting to 50mg. */ + config.cfg.any_motion.threshold = 0x68; + + /* Set new configurations. */ + rslt = bmi270_set_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/common/common.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/common/common.c new file mode 100644 index 0000000000..afff37bc84 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/common/common.c @@ -0,0 +1,372 @@ +/** + * Copyright (C) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + */ + +#include +#include +#include + +#include "coines.h" +#include "bmi2_defs.h" + +/******************************************************************************/ +/*! Macro definitions */ +#define BMI2XY_SHUTTLE_ID UINT16_C(0x1B8) + +/*! Macro that defines read write length */ +#define READ_WRITE_LEN UINT8_C(46) + +/******************************************************************************/ +/*! Static variable definition */ + +/*! Variable that holds the I2C device address or SPI chip selection */ +static uint8_t dev_addr; + +/******************************************************************************/ +/*! User interface functions */ + +/*! + * I2C read function map to COINES platform + */ +BMI2_INTF_RETURN_TYPE bmi2_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr) +{ + uint8_t dev_addr = *(uint8_t*)intf_ptr; + + return coines_read_i2c(dev_addr, reg_addr, reg_data, (uint16_t)len); +} + +/*! + * I2C write function map to COINES platform + */ +BMI2_INTF_RETURN_TYPE bmi2_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr) +{ + uint8_t dev_addr = *(uint8_t*)intf_ptr; + + return coines_write_i2c(dev_addr, reg_addr, (uint8_t *)reg_data, (uint16_t)len); +} + +/*! + * SPI read function map to COINES platform + */ +BMI2_INTF_RETURN_TYPE bmi2_spi_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr) +{ + uint8_t dev_addr = *(uint8_t*)intf_ptr; + + return coines_read_spi(dev_addr, reg_addr, reg_data, (uint16_t)len); +} + +/*! + * SPI write function map to COINES platform + */ +BMI2_INTF_RETURN_TYPE bmi2_spi_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr) +{ + uint8_t dev_addr = *(uint8_t*)intf_ptr; + + return coines_write_spi(dev_addr, reg_addr, (uint8_t *)reg_data, (uint16_t)len); +} + +/*! + * Delay function map to COINES platform + */ +void bmi2_delay_us(uint32_t period, void *intf_ptr) +{ + coines_delay_usec(period); +} + +/*! + * @brief Function to select the interface between SPI and I2C. + * Also to initialize coines platform + */ +int8_t bmi2_interface_init(struct bmi2_dev *bmi, uint8_t intf) +{ + int8_t rslt = BMI2_OK; + struct coines_board_info board_info; + + if (bmi != NULL) + { + int16_t result = coines_open_comm_intf(COINES_COMM_INTF_USB); + if (result < COINES_SUCCESS) + { + printf( + "\n Unable to connect with Application Board ! \n" " 1. Check if the board is connected and powered on. \n" " 2. Check if Application Board USB driver is installed. \n" + " 3. Check if board is in use by another application. (Insufficient permissions to access USB) \n"); + exit(result); + } + + result = coines_get_board_info(&board_info); + +#if defined(PC) + setbuf(stdout, NULL); +#endif + + if (result == COINES_SUCCESS) + { + if ((board_info.shuttle_id != BMI2XY_SHUTTLE_ID)) + { + printf("! Warning invalid sensor shuttle \n ," "This application will not support this sensor \n"); + exit(COINES_E_FAILURE); + } + } + + coines_set_shuttleboard_vdd_vddio_config(0, 0); + coines_delay_msec(100); + + /* Bus configuration : I2C */ + if (intf == BMI2_I2C_INTF) + { + printf("I2C Interface \n"); + + /* To initialize the user I2C function */ + dev_addr = BMI2_I2C_PRIM_ADDR; + bmi->intf = BMI2_I2C_INTF; + bmi->read = bmi2_i2c_read; + bmi->write = bmi2_i2c_write; + coines_config_i2c_bus(COINES_I2C_BUS_0, COINES_I2C_FAST_MODE); + } + /* Bus configuration : SPI */ + else if (intf == BMI2_SPI_INTF) + { + printf("SPI Interface \n"); + + /* To initialize the user SPI function */ + dev_addr = COINES_SHUTTLE_PIN_7; + bmi->intf = BMI2_SPI_INTF; + bmi->read = bmi2_spi_read; + bmi->write = bmi2_spi_write; + coines_config_spi_bus(COINES_SPI_BUS_0, COINES_SPI_SPEED_5_MHZ, COINES_SPI_MODE3); + coines_set_pin_config(COINES_SHUTTLE_PIN_7, COINES_PIN_DIRECTION_OUT, COINES_PIN_VALUE_HIGH); + } + + /* Assign device address to interface pointer */ + bmi->intf_ptr = &dev_addr; + + /* Configure delay in microseconds */ + bmi->delay_us = bmi2_delay_us; + + /* Configure max read/write length (in bytes) ( Supported length depends on target machine) */ + bmi->read_write_len = READ_WRITE_LEN; + + /* Assign to NULL to load the default config file. */ + bmi->config_file_ptr = NULL; + + coines_delay_usec(10000); + + coines_set_shuttleboard_vdd_vddio_config(3300, 3300); + + coines_delay_usec(10000); + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; + +} + +/*! + * @brief Prints the execution status of the APIs. + */ +void bmi2_error_codes_print_result(int8_t rslt) +{ + switch (rslt) + { + case BMI2_OK: + + /* Do nothing */ + break; + + case BMI2_W_FIFO_EMPTY: + printf("Warning [%d] : FIFO empty\r\n", rslt); + break; + case BMI2_W_PARTIAL_READ: + printf("Warning [%d] : FIFO partial read\r\n", rslt); + break; + case BMI2_E_NULL_PTR: + printf( + "Error [%d] : Null pointer error. It occurs when the user tries to assign value (not address) to a pointer," " which has been initialized to NULL.\r\n", + rslt); + break; + + case BMI2_E_COM_FAIL: + printf( + "Error [%d] : Communication failure error. It occurs due to read/write operation failure and also due " "to power failure during communication\r\n", + rslt); + break; + + case BMI2_E_DEV_NOT_FOUND: + printf("Error [%d] : Device not found error. It occurs when the device chip id is incorrectly read\r\n", + rslt); + break; + + case BMI2_E_INVALID_SENSOR: + printf( + "Error [%d] : Invalid sensor error. It occurs when there is a mismatch in the requested feature with the " "available one\r\n", + rslt); + break; + + case BMI2_E_SELF_TEST_FAIL: + printf( + "Error [%d] : Self-test failed error. It occurs when the validation of accel self-test data is " "not satisfied\r\n", + rslt); + break; + + case BMI2_E_INVALID_INT_PIN: + printf( + "Error [%d] : Invalid interrupt pin error. It occurs when the user tries to configure interrupt pins " "apart from INT1 and INT2\r\n", + rslt); + break; + + case BMI2_E_OUT_OF_RANGE: + printf( + "Error [%d] : Out of range error. It occurs when the data exceeds from filtered or unfiltered data from " "fifo and also when the range exceeds the maximum range for accel and gyro while performing FOC\r\n", + rslt); + break; + + case BMI2_E_ACC_INVALID_CFG: + printf( + "Error [%d] : Invalid Accel configuration error. It occurs when there is an error in accel configuration" " register which could be one among range, BW or filter performance in reg address 0x40\r\n", + rslt); + break; + + case BMI2_E_GYRO_INVALID_CFG: + printf( + "Error [%d] : Invalid Gyro configuration error. It occurs when there is a error in gyro configuration" "register which could be one among range, BW or filter performance in reg address 0x42\r\n", + rslt); + break; + + case BMI2_E_ACC_GYR_INVALID_CFG: + printf( + "Error [%d] : Invalid Accel-Gyro configuration error. It occurs when there is a error in accel and gyro" " configuration registers which could be one among range, BW or filter performance in reg address 0x40 " "and 0x42\r\n", + rslt); + break; + + case BMI2_E_CONFIG_LOAD: + printf( + "Error [%d] : Configuration load error. It occurs when failure observed while loading the configuration " "into the sensor\r\n", + rslt); + break; + + case BMI2_E_INVALID_PAGE: + printf( + "Error [%d] : Invalid page error. It occurs due to failure in writing the correct feature configuration " "from selected page\r\n", + rslt); + break; + + case BMI2_E_SET_APS_FAIL: + printf( + "Error [%d] : APS failure error. It occurs due to failure in write of advance power mode configuration " "register\r\n", + rslt); + break; + + case BMI2_E_AUX_INVALID_CFG: + printf( + "Error [%d] : Invalid AUX configuration error. It occurs when the auxiliary interface settings are not " "enabled properly\r\n", + rslt); + break; + + case BMI2_E_AUX_BUSY: + printf( + "Error [%d] : AUX busy error. It occurs when the auxiliary interface buses are engaged while configuring" " the AUX\r\n", + rslt); + break; + + case BMI2_E_REMAP_ERROR: + printf( + "Error [%d] : Remap error. It occurs due to failure in assigning the remap axes data for all the axes " "after change in axis position\r\n", + rslt); + break; + + case BMI2_E_GYR_USER_GAIN_UPD_FAIL: + printf( + "Error [%d] : Gyro user gain update fail error. It occurs when the reading of user gain update status " "fails\r\n", + rslt); + break; + + case BMI2_E_SELF_TEST_NOT_DONE: + printf( + "Error [%d] : Self-test not done error. It occurs when the self-test process is ongoing or not " "completed\r\n", + rslt); + break; + + case BMI2_E_INVALID_INPUT: + printf("Error [%d] : Invalid input error. It occurs when the sensor input validity fails\r\n", rslt); + break; + + case BMI2_E_INVALID_STATUS: + printf("Error [%d] : Invalid status error. It occurs when the feature/sensor validity fails\r\n", rslt); + break; + + case BMI2_E_CRT_ERROR: + printf("Error [%d] : CRT error. It occurs when the CRT test has failed\r\n", rslt); + break; + + case BMI2_E_ST_ALREADY_RUNNING: + printf( + "Error [%d] : Self-test already running error. It occurs when the self-test is already running and " "another has been initiated\r\n", + rslt); + break; + + case BMI2_E_CRT_READY_FOR_DL_FAIL_ABORT: + printf( + "Error [%d] : CRT ready for download fail abort error. It occurs when download in CRT fails due to wrong " "address location\r\n", + rslt); + break; + + case BMI2_E_DL_ERROR: + printf( + "Error [%d] : Download error. It occurs when write length exceeds that of the maximum burst length\r\n", + rslt); + break; + + case BMI2_E_PRECON_ERROR: + printf( + "Error [%d] : Pre-conditional error. It occurs when precondition to start the feature was not " "completed\r\n", + rslt); + break; + + case BMI2_E_ABORT_ERROR: + printf("Error [%d] : Abort error. It occurs when the device was shaken during CRT test\r\n", rslt); + break; + + case BMI2_E_WRITE_CYCLE_ONGOING: + printf( + "Error [%d] : Write cycle ongoing error. It occurs when the write cycle is already running and another " "has been initiated\r\n", + rslt); + break; + + case BMI2_E_ST_NOT_RUNING: + printf( + "Error [%d] : Self-test is not running error. It occurs when self-test running is disabled while it's " "running\r\n", + rslt); + break; + + case BMI2_E_DATA_RDY_INT_FAILED: + printf( + "Error [%d] : Data ready interrupt error. It occurs when the sample count exceeds the FOC sample limit " "and data ready status is not updated\r\n", + rslt); + break; + + case BMI2_E_INVALID_FOC_POSITION: + printf( + "Error [%d] : Invalid FOC position error. It occurs when average FOC data is obtained for the wrong" " axes\r\n", + rslt); + break; + + default: + printf("Error [%d] : Unknown error code\r\n", rslt); + break; + } +} + +/*! + * @brief Deinitializes coines platform + * + * @return void. + */ +void bmi2_coines_deinit(void) +{ + coines_close_comm_intf(COINES_COMM_INTF_USB); +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/common/common.h b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/common/common.h new file mode 100644 index 0000000000..763983da4a --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/common/common.h @@ -0,0 +1,122 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +#ifndef _COMMON_H +#define _COMMON_H + +/*! CPP guard */ +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include "bmi2.h" + +/*! + * @brief Function for reading the sensor's registers through I2C bus. + * + * @param[in] reg_addr : Register address. + * @param[out] reg_data : Pointer to the data buffer to store the read data. + * @param[in] length : No of bytes to read. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval = BMI2_INTF_RET_SUCCESS -> Success + * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info + * + */ +BMI2_INTF_RETURN_TYPE bmi2_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr); + +/*! + * @brief Function for writing the sensor's registers through I2C bus. + * + * @param[in] reg_addr : Register address. + * @param[in] reg_data : Pointer to the data buffer whose value is to be written. + * @param[in] length : No of bytes to write. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval = BMI2_INTF_RET_SUCCESS -> Success + * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info + * + */ +BMI2_INTF_RETURN_TYPE bmi2_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr); + +/*! + * @brief Function for reading the sensor's registers through SPI bus. + * + * @param[in] reg_addr : Register address. + * @param[out] reg_data : Pointer to the data buffer to store the read data. + * @param[in] length : No of bytes to read. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval = BMI2_INTF_RET_SUCCESS -> Success + * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info + * + */ +BMI2_INTF_RETURN_TYPE bmi2_spi_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr); + +/*! + * @brief Function for writing the sensor's registers through SPI bus. + * + * @param[in] reg_addr : Register address. + * @param[in] reg_data : Pointer to the data buffer whose data has to be written. + * @param[in] length : No of bytes to write. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval = BMI2_INTF_RET_SUCCESS -> Success + * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info + * + */ +BMI2_INTF_RETURN_TYPE bmi2_spi_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr); + +/*! + * @brief This function provides the delay for required time (Microsecond) as per the input provided in some of the + * APIs. + * + * @param[in] period_us : The required wait time in microsecond. + * @param[in] intf_ptr : Interface pointer + * + * @return void. + * + */ +void bmi2_delay_us(uint32_t period, void *intf_ptr); + +/*! + * @brief Function to select the interface between SPI and I2C. + * + * @param[in] bma : Structure instance of bmi2_dev + * @param[in] intf : Interface selection parameter + * + * @return Status of execution + * @retval 0 -> Success + * @retval < 0 -> Failure Info + */ +int8_t bmi2_interface_init(struct bmi2_dev *bma, uint8_t intf); + +/*! + * @brief Prints the execution status of the APIs. + * + * @param[in] rslt : Error code returned by the API whose execution status has to be printed. + * + * @return void. + */ +void bmi2_error_codes_print_result(int8_t rslt); + +/*! + * @brief Deinitializes coines platform + * + * @return void. + */ +void bmi2_coines_deinit(void); + +#ifdef __cplusplus +} +#endif /* End of CPP guard */ + +#endif /* _COMMON_H */ diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/component_retrim/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/component_retrim/Makefile new file mode 100644 index 0000000000..aad6a0b877 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/component_retrim/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= component_retrim.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/component_retrim/component_retrim.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/component_retrim/component_retrim.c new file mode 100644 index 0000000000..d85c148d2a --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/component_retrim/component_retrim.c @@ -0,0 +1,52 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270.h" +#include "common.h" + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270. */ + rslt = bmi270_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* This API runs the CRT process. */ + rslt = bmi2_do_crt(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Do not move the board while doing CRT. If so, it will throw an abort error. */ + if (rslt == BMI2_OK) + { + printf("CRT successfully completed."); + } + } + + bmi2_coines_deinit(); + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_full_header_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_full_header_mode/Makefile new file mode 100644 index 0000000000..83a8f49ff9 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_full_header_mode/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= fifo_full_header_mode.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_full_header_mode/fifo_full_header_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_full_header_mode/fifo_full_header_mode.c new file mode 100644 index 0000000000..2118c241df --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_full_header_mode/fifo_full_header_mode.c @@ -0,0 +1,315 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270.h" +#include "common.h" + +/******************************************************************************/ +/*! Macros */ + +/*! Buffer size allocated to store raw FIFO data. */ +#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) + +/*! Length of data to be read from FIFO. */ +#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) + +/*! Number of accel frames to be extracted from FIFO. */ + +/*! Calculation for frame count: Total frame count = Fifo buffer size(2048)/ Total frames(6 Accel, 6 Gyro and 1 header, + * totaling to 13) which equals to 157. + */ +#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(157) + +/*! Number of gyro frames to be extracted from FIFO. */ +#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(157) + +/*! Macro to read sensortime byte in FIFO. */ +#define SENSORTIME_OVERHEAD_BYTE UINT8_C(3) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + * @param[in] dev : Structure instance of bmi2_dev. + * @return Status of execution. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev); + +/******************************************************************************/ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + uint16_t index = 0; + uint16_t fifo_length = 0; + uint16_t config = 0; + + /* To read sensortime, extra 3 bytes are added to fifo buffer. */ + uint16_t fifo_buffer_size = BMI2_FIFO_RAW_DATA_BUFFER_SIZE + SENSORTIME_OVERHEAD_BYTE; + + /* Variable to get fifo full interrupt status. */ + uint16_t int_status = 0; + + uint16_t accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; + + uint16_t gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; + + /* Variable to store the available frame length count in FIFO */ + uint8_t frame_count; + + int8_t try = 1; + + /* Number of bytes of FIFO data. */ + uint8_t fifo_data[fifo_buffer_size]; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Array of accelerometer frames -> Total bytes = + * 157 * (6 axes + 1 header bytes) = 1099 bytes */ + struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; + + /* Array of gyro frames -> Total bytes = + * 157 * (6 axes + 1 header bytes) = 1099 bytes */ + struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; + + /* Initialize FIFO frame structure. */ + struct bmi2_fifo_frame fifoframe = { 0 }; + + /* Accel and gyro sensor are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270. */ + rslt = bmi270_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Enable accel and gyro sensor. */ + rslt = bmi270_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Configuration settings for accel and gyro. */ + rslt = set_accel_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Before setting FIFO, disable the advance power save mode. */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initially disable all configurations in fifo. */ + rslt = bmi2_get_fifo_config(&config, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initially disable all configurations in fifo. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Update FIFO structure. */ + /* Mapping the buffer to store the fifo data. */ + fifoframe.data = fifo_data; + + /* Length of FIFO frame. */ + /* To read sensortime, extra 3 bytes are added to fifo user length. */ + fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH + SENSORTIME_OVERHEAD_BYTE; + + /* Set FIFO configuration by enabling accel, gyro and timestamp. + * NOTE 1: The header mode is enabled by default. + * NOTE 2: By default the FIFO operating mode is in FIFO mode. + * NOTE 3: Sensortime is enabled by default */ + printf("FIFO is configured in header mode\n"); + rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Map FIFO full interrupt. */ + fifoframe.data_int_map = BMI2_FFULL_INT; + rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + while (try <= 10) + { + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if ((rslt == BMI2_OK) && (int_status & BMI2_FFULL_INT_STATUS_MASK)) + { + printf("\nIteration : %d\n", try); + + accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; + + gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; + + rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("\nFIFO data bytes available : %d \n", fifo_length); + printf("\nFIFO data bytes requested : %d \n", fifoframe.length); + + /* Read FIFO data. */ + rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + printf("\nFIFO accel frames requested : %d \n", accel_frame_length); + + /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ + rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); + + printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); + + /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ + rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); + + /* Calculating the frame count from the available bytes in FIFO + * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len + header_byte)) */ + frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH + 1)); + printf("\nAvailable frame count from available bytes: %d\n", frame_count); + + printf("\nExtracted accel frames\n"); + + if (accel_frame_length > frame_count) + { + accel_frame_length = frame_count; + } + + /* Print the parsed accelerometer data from the FIFO buffer. */ + for (index = 0; index < accel_frame_length; index++) + { + printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, + fifo_accel_data[index].y, fifo_accel_data[index].z); + } + + printf("\nExtracted gyro frames\n"); + + if (gyro_frame_length > frame_count) + { + gyro_frame_length = frame_count; + } + + /* Print the parsed gyro data from the FIFO buffer. */ + for (index = 0; index < gyro_frame_length; index++) + { + printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", + index, + fifo_gyro_data[index].x, + fifo_gyro_data[index].y, + fifo_gyro_data[index].z); + } + + /* Print control frames like sensor time and skipped frame count. */ + printf("\nSkipped frame count = %d\n", fifoframe.skipped_frame_count); + + printf("Sensor time = %lu\r\n", fifoframe.sensor_time); + + try++; + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accel and gyro configurations. */ + struct bmi2_sens_config config[2]; + + /* Configure the type of feature. */ + config[0].type = BMI2_ACCEL; + config[1].type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_get_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* Accel configuration settings. */ + /* Set Output Data Rate */ + config[0].cfg.acc.odr = BMI2_ACC_ODR_25HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* Gyro configuration settings. */ + /* Set Output Data Rate */ + config[1].cfg.gyr.odr = BMI2_GYR_ODR_25HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set new configurations. */ + rslt = bmi270_set_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_full_headerless_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_full_headerless_mode/Makefile new file mode 100644 index 0000000000..ae9a413fe5 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_full_headerless_mode/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= fifo_full_headerless_mode.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_full_headerless_mode/fifo_full_headerless_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_full_headerless_mode/fifo_full_headerless_mode.c new file mode 100644 index 0000000000..53dddc4ba6 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_full_headerless_mode/fifo_full_headerless_mode.c @@ -0,0 +1,304 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270.h" +#include "common.h" + +/******************************************************************************/ +/*! Macros */ + +/*! Buffer size allocated to store raw FIFO data */ +#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) + +/*! Length of data to be read from FIFO */ +#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) + +/*! Number of accel frames to be extracted from FIFO */ + +/*! Calculation for frame count: Total frame count = Fifo buffer size(2048)/ Total frames(6 Accel, 6 Gyro totaling to + * 12) which equals to 170. + */ +#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(169) + +/*! Number of gyro frames to be extracted from FIFO */ +#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(169) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + * @param[in] dev : Structure instance of bmi2_dev. + * @return Status of execution. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev); + +/******************************************************************************/ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + uint16_t index = 0; + + uint16_t fifo_length = 0; + + uint8_t try = 1; + + /* Variable to get fifo full interrupt status. */ + uint16_t int_status = 0; + + uint16_t accel_frame_length; + + uint16_t gyro_frame_length; + + /* Variable to store the available frame length count in FIFO */ + uint8_t frame_count; + + /* Number of bytes of FIFO data. */ + uint8_t fifo_data[BMI2_FIFO_RAW_DATA_BUFFER_SIZE] = { 0 }; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Array of accelerometer frames -> Total bytes = + * 170 * (6 axes bytes) = 1020 bytes */ + struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; + + /* Array of gyro frames -> Total bytes = + * 170 * (6 axes bytes) = 1020 bytes */ + struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; + + /* Initialize FIFO frame structure. */ + struct bmi2_fifo_frame fifoframe = { 0 }; + + /* Accel and gyro sensor are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270. */ + rslt = bmi270_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Enable accel and gyro sensor. */ + rslt = bmi270_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Configuration settings for accel and gyro. */ + rslt = set_accel_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Before setting FIFO, disable the advance power save mode. */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initially disable all configurations in fifo. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Update FIFO structure. */ + /* Mapping the buffer to store the fifo data. */ + fifoframe.data = fifo_data; + + /* Length of FIFO frame. */ + fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH; + + /* Set FIFO configuration by enabling accel, gyro. + * NOTE 1: The header mode is enabled by default. + * NOTE 2: By default the FIFO operating mode is FIFO mode. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("FIFO is configured in headerless mode\n"); + + /* To enable headerless mode, disable the header. */ + if (rslt == BMI2_OK) + { + rslt = bmi2_set_fifo_config(BMI2_FIFO_HEADER_EN, BMI2_DISABLE, &bmi2_dev); + } + + /* Map FIFO full interrupt. */ + fifoframe.data_int_map = BMI2_FFULL_INT; + rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + while (try <= 10) + { + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if ((rslt == BMI2_OK) && (int_status & BMI2_FFULL_INT_STATUS_MASK)) + { + printf("\nIteration : %d\n", try); + + rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; + gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; + + printf("\nFIFO data bytes available : %d \n", fifo_length); + printf("\nFIFO data bytes requested : %d \n", fifoframe.length); + + /* Read FIFO data. */ + rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + printf("\nFIFO accel frames requested : %d \n", accel_frame_length); + + /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ + rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); + + printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); + + /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ + rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); + + /* Calculating the frame count from the available bytes in FIFO + * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len)) */ + frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH)); + printf("\nAvailable frame count from available bytes: %d\n", frame_count); + + printf("\nExtracted accel frames\n"); + + if (accel_frame_length > frame_count) + { + accel_frame_length = frame_count; + } + + /* Print the parsed accelerometer data from the FIFO buffer. */ + for (index = 0; index < accel_frame_length; index++) + { + printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, + fifo_accel_data[index].y, fifo_accel_data[index].z); + } + + printf("\nExtracted gyro frames\n"); + + if (gyro_frame_length > frame_count) + { + gyro_frame_length = frame_count; + } + + /* Print the parsed gyro data from the FIFO buffer. */ + for (index = 0; index < gyro_frame_length; index++) + { + printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", + index, + fifo_gyro_data[index].x, + fifo_gyro_data[index].y, + fifo_gyro_data[index].z); + } + } + + try++; + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accel and gyro configurations. */ + struct bmi2_sens_config config[2]; + + /* Configure the type of feature. */ + config[0].type = BMI2_ACCEL; + config[1].type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_get_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* Accel configuration settings. */ + /* Set Output Data Rate */ + config[0].cfg.acc.odr = BMI2_ACC_ODR_100HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* Gyro configuration settings. */ + /* Set Output Data Rate */ + config[1].cfg.gyr.odr = BMI2_GYR_ODR_100HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set new configurations. */ + rslt = bmi270_set_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_watermark_header_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_watermark_header_mode/Makefile new file mode 100644 index 0000000000..08158205b5 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_watermark_header_mode/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= fifo_watermark_header_mode.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_watermark_header_mode/fifo_watermark_header_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_watermark_header_mode/fifo_watermark_header_mode.c new file mode 100644 index 0000000000..faaeb307ea --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_watermark_header_mode/fifo_watermark_header_mode.c @@ -0,0 +1,335 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270.h" +#include "common.h" + +/******************************************************************************/ +/*! Macros */ + +/*! Buffer size allocated to store raw FIFO data */ +#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) + +/*! Length of data to be read from FIFO */ +#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) + +/*! Number of accel frames to be extracted from FIFO */ + +/*! + * Calculation: + * fifo_watermark_level = 650, accel_frame_len = 6, gyro_frame_len = 6 header_byte = 1. + * fifo_accel_frame_count = (650 / (6 + 6 + 1 )) = 50 frames + * NOTE: Extra frames are read in order to get sensor time + */ +#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(55) + +/*! Number of gyro frames to be extracted from FIFO */ +#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(55) + +/*! Setting a watermark level in FIFO */ +#define BMI270_FIFO_WATERMARK_LEVEL UINT16_C(650) + +/*! Macro to read sensortime byte in FIFO */ +#define SENSORTIME_OVERHEAD_BYTE UINT8_C(3) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + * @param[in] dev : Structure instance of bmi2_dev. + * @return Status of execution. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to store the available frame length count in FIFO */ + uint8_t frame_count; + + /* Variable to index bytes. */ + uint16_t index = 0; + + uint16_t fifo_length = 0; + + uint16_t accel_frame_length; + + uint16_t gyro_frame_length; + + uint8_t try = 1; + + /* To read sensortime, extra 3 bytes are added to fifo buffer */ + uint16_t fifo_buffer_size = BMI2_FIFO_RAW_DATA_BUFFER_SIZE + SENSORTIME_OVERHEAD_BYTE; + + /* Number of bytes of FIFO data. */ + uint8_t fifo_data[fifo_buffer_size]; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Array of accelerometer frames -> Total bytes = + * 55 * (6 axes bytes) = 330 bytes */ + struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; + + /* Array of gyro frames -> Total bytes = + * 55 * (6 axes bytes) = 330 bytes */ + struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; + + /* Initialize FIFO frame structure. */ + struct bmi2_fifo_frame fifoframe = { 0 }; + + /* Accel and gyro sensor are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; + + /* Variable to get fifo water-mark interrupt status. */ + uint16_t int_status = 0; + + uint16_t watermark = 0; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270. */ + rslt = bmi270_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Enable accel and gyro sensor. */ + rslt = bmi270_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Configuration settings for accel and gyro. */ + rslt = set_accel_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Before setting FIFO, disable the advance power save mode. */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initially disable all configurations in fifo. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Update FIFO structure. */ + /* Mapping the buffer to store the fifo data. */ + fifoframe.data = fifo_data; + + /* Length of FIFO frame. */ + /* To read sensortime, extra 3 bytes are added to fifo user length. */ + fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH + SENSORTIME_OVERHEAD_BYTE; + + /* Set FIFO configuration by enabling accel, gyro and timestamp. + * NOTE 1: The header mode is enabled by default. + * NOTE 2: By default the FIFO operating mode is FIFO mode. + * NOTE 3: Sensortime is enabled by default */ + printf("FIFO is configured in header mode\n"); + rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* FIFO water-mark interrupt is enabled. */ + fifoframe.data_int_map = BMI2_FWM_INT; + + /* Map water-mark interrupt to the required interrupt pin. */ + rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Set water-mark level. */ + fifoframe.wm_lvl = BMI270_FIFO_WATERMARK_LEVEL; + + /* Set the water-mark level if water-mark interrupt is mapped. */ + rslt = bmi2_set_fifo_wm(fifoframe.wm_lvl, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + rslt = bmi2_get_fifo_wm(&watermark, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + printf("\nFIFO watermark level is %d\n", watermark); + + while (try <= 10) + { + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the status of FIFO watermark interrupt. */ + if ((rslt == BMI2_OK) && (int_status & BMI2_FWM_INT_STATUS_MASK)) + { + printf("\nIteration : %d\n", try); + + printf("\nWatermark interrupt occurred\n"); + + rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; + gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; + + printf("\nFIFO data bytes available : %d \n", fifo_length); + printf("\nFIFO data bytes requested : %d \n", fifoframe.length); + + /* Read FIFO data. */ + rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + printf("\nFIFO accel frames requested : %d \n", accel_frame_length); + + /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ + rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); + + printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); + + /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ + rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); + + /* Calculating the frame count from the available bytes in FIFO + * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len + header_byte)) */ + frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH + 1)); + printf("\nAvailable frame count from available bytes: %d\n", frame_count); + + printf("\nExracted accel frames\n"); + + if (accel_frame_length > frame_count) + { + accel_frame_length = frame_count; + } + + /* Print the parsed accelerometer data from the FIFO buffer. */ + for (index = 0; index < accel_frame_length; index++) + { + printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, + fifo_accel_data[index].y, fifo_accel_data[index].z); + } + + printf("\nExtracted gyro frames\n"); + + if (gyro_frame_length > frame_count) + { + gyro_frame_length = frame_count; + } + + /* Print the parsed gyro data from the FIFO buffer. */ + for (index = 0; index < gyro_frame_length; index++) + { + printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", + index, + fifo_gyro_data[index].x, + fifo_gyro_data[index].y, + fifo_gyro_data[index].z); + } + + /* Print control frames like sensor time and skipped frame count. */ + printf("Skipped frame count = %d\n", fifoframe.skipped_frame_count); + printf("Sensor time = %lu\r\n", fifoframe.sensor_time); + + try++; + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accel and gyro configurations. */ + struct bmi2_sens_config config[2]; + + /* Configure the type of feature. */ + config[0].type = BMI2_ACCEL; + config[1].type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_get_sensor_config(config, 2, dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* Accel configuration settings. */ + /* Set Output Data Rate */ + config[0].cfg.acc.odr = BMI2_ACC_ODR_25HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* Gyro configuration settings. */ + /* Set Output Data Rate */ + config[1].cfg.gyr.odr = BMI2_GYR_ODR_25HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set new configurations. */ + rslt = bmi270_set_sensor_config(config, 2, dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_watermark_headerless_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_watermark_headerless_mode/Makefile new file mode 100644 index 0000000000..527a18ab8e --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_watermark_headerless_mode/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= fifo_watermark_headerless_mode.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c new file mode 100644 index 0000000000..b3adf89659 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c @@ -0,0 +1,331 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270.h" +#include "common.h" + +/******************************************************************************/ +/*! Macros */ + +/*! Buffer size allocated to store raw FIFO data */ +#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) + +/*! Length of data to be read from FIFO */ +#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) + +/*! Number of accel frames to be extracted from FIFO */ + +/*! + * Calculation: + * fifo_watermark_level = 650, accel_frame_len = 6, gyro_frame_len = 6. + * fifo_accel_frame_count = (650 / (6 + 6 )) = 54 frames + */ +#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(55) + +/*! Number of gyro frames to be extracted from FIFO */ +#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(55) + +/*! Setting a watermark level in FIFO */ +#define BMI270_FIFO_WATERMARK_LEVEL UINT16_C(650) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + * @param[in] dev : Structure instance of bmi2_dev. + * @return Status of execution. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to index bytes. */ + uint16_t index = 0; + + /* Variable to store the available frame length count in FIFO */ + uint8_t frame_count; + + uint16_t fifo_length = 0; + + uint16_t accel_frame_length; + + uint16_t gyro_frame_length; + + uint8_t try = 1; + + /* Number of bytes of FIFO data. */ + uint8_t fifo_data[BMI2_FIFO_RAW_DATA_BUFFER_SIZE] = { 0 }; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Array of accelerometer frames -> Total bytes = + * 50 * (6 axes bytes) = 300 bytes */ + struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; + + /* Array of gyro frames -> Total bytes = + * 50 * (6 axes bytes) = 300 bytes */ + struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; + + /* Initialize FIFO frame structure. */ + struct bmi2_fifo_frame fifoframe = { 0 }; + + /* Accel and gyro sensor are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; + + /* Variable to get fifo water-mark interrupt status. */ + uint16_t int_status = 0; + + uint16_t watermark = 0; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270. */ + rslt = bmi270_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Enable accel and gyro sensor. */ + rslt = bmi270_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Configuration settings for accel and gyro. */ + rslt = set_accel_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Before setting FIFO, disable the advance power save mode. */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initially disable all configurations in fifo. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Update FIFO structure. */ + /* Mapping the buffer to store the fifo data. */ + fifoframe.data = fifo_data; + + /* Length of FIFO frame. */ + fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH; + + /* Set FIFO configuration by enabling accel, gyro. + * NOTE 1: The header mode is enabled by default. + * NOTE 2: By default the FIFO operating mode is FIFO mode. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("FIFO is configured in headerless mode\n"); + + /* To enable headerless mode, disable the header. */ + if (rslt == BMI2_OK) + { + rslt = bmi2_set_fifo_config(BMI2_FIFO_HEADER_EN, BMI2_DISABLE, &bmi2_dev); + } + + /* FIFO water-mark interrupt is enabled. */ + fifoframe.data_int_map = BMI2_FWM_INT; + + /* Map water-mark interrupt to the required interrupt pin. */ + rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Set water-mark level. */ + fifoframe.wm_lvl = BMI270_FIFO_WATERMARK_LEVEL; + + fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH; + + /* Set the water-mark level if water-mark interrupt is mapped. */ + rslt = bmi2_set_fifo_wm(fifoframe.wm_lvl, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + rslt = bmi2_get_fifo_wm(&watermark, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + printf("\nFIFO watermark level is %d\n", watermark); + + while (try <= 10) + { + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the status of FIFO watermark interrupt. */ + if ((rslt == BMI2_OK) && (int_status & BMI2_FWM_INT_STATUS_MASK)) + { + printf("\nIteration : %d\n", try); + + printf("\nWatermark interrupt occurred\n"); + + rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; + gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; + + printf("\nFIFO data bytes available : %d \n", fifo_length); + printf("\nFIFO data bytes requested : %d \n", fifoframe.length); + + /* Read FIFO data. */ + rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + printf("\nFIFO accel frames requested : %d \n", accel_frame_length); + + /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ + rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); + + printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); + + /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ + rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); + + /* Calculating the frame count from the available bytes in FIFO + * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len)) */ + frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH)); + printf("\nAvailable frame count from available bytes: %d\n", frame_count); + + printf("\nExracted accel frames\n"); + + if (accel_frame_length > frame_count) + { + accel_frame_length = frame_count; + } + + /* Print the parsed accelerometer data from the FIFO buffer. */ + for (index = 0; index < accel_frame_length; index++) + { + printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, + fifo_accel_data[index].y, fifo_accel_data[index].z); + } + + printf("\nExtracted gyro frames\n"); + + if (gyro_frame_length > frame_count) + { + gyro_frame_length = frame_count; + } + + /* Print the parsed gyro data from the FIFO buffer. */ + for (index = 0; index < gyro_frame_length; index++) + { + printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", + index, + fifo_gyro_data[index].x, + fifo_gyro_data[index].y, + fifo_gyro_data[index].z); + } + } + + try++; + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accel and gyro configurations. */ + struct bmi2_sens_config config[2]; + + /* Configure the type of feature. */ + config[0].type = BMI2_ACCEL; + config[1].type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_get_sensor_config(config, 2, dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameter according to their requirement. */ + /* Accel configuration settings. */ + /* Set Output Data Rate */ + config[0].cfg.acc.odr = BMI2_ACC_ODR_100HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* Gyro configuration settings. */ + /* Set Output Data Rate */ + config[1].cfg.gyr.odr = BMI2_GYR_ODR_100HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set new configurations. */ + rslt = bmi270_set_sensor_config(config, 2, dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/gyro/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/gyro/Makefile new file mode 100644 index 0000000000..e1f03b8bd2 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/gyro/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= gyro.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/gyro/gyro.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/gyro/gyro.c new file mode 100644 index 0000000000..1bb0ca471b --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/gyro/gyro.c @@ -0,0 +1,195 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270.h" +#include "common.h" + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for gyro. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t set_gyro_config(struct bmi2_dev *dev); + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + * + * @param[in] val : LSB from each axis. + * @param[in] dps : Degree per second. + * @param[in] bit_width : Resolution for gyro. + * + * @return Degree per second. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to define limit to print gyro data. */ + uint8_t limit = 10; + + uint8_t indx = 0; + + float x = 0, y = 0, z = 0; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Create an instance of sensor data structure. */ + struct bmi2_sensor_data sensor_data = { 0 }; + + /* Assign gyro sensor to variable. */ + uint8_t sens_list = BMI2_GYRO; + + /* Initialize the interrupt status of gyro. */ + uint16_t int_status = 0; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270. */ + rslt = bmi270_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the selected sensors. */ + rslt = bmi270_sensor_enable(&sens_list, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Gyro configuration settings. */ + rslt = set_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Select gyro sensor. */ + sensor_data.type = BMI2_GYRO; + printf("Gyro and DPS data\n"); + printf("Gyro data collected at Range 2000 dps with 16-bit resolution\n"); + + /* Loop to print gyro data when interrupt occurs. */ + while (indx <= limit) + { + /* To get the data ready interrupt status of gyro. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the data ready interrupt status and print the status for 10 samples. */ + if (int_status & BMI2_GYR_DRDY_INT_MASK) + { + /* Get gyro data for x, y and z axis. */ + rslt = bmi270_get_sensor_data(&sensor_data, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("\nGyr_X = %d\t", sensor_data.sens_data.gyr.x); + printf("Gyr_Y = %d\t", sensor_data.sens_data.gyr.y); + printf("Gyr_Z = %d\r\n", sensor_data.sens_data.gyr.z); + + /* Converting lsb to degree per second for 16 bit gyro at 2000dps range. */ + x = lsb_to_dps(sensor_data.sens_data.gyr.x, 2000, bmi2_dev.resolution); + y = lsb_to_dps(sensor_data.sens_data.gyr.y, 2000, bmi2_dev.resolution); + z = lsb_to_dps(sensor_data.sens_data.gyr.z, 2000, bmi2_dev.resolution); + + /* Print the data in dps. */ + printf("Gyr_DPS_X = %4.2f, Gyr_DPS_Y = %4.2f, Gyr_DPS_Z = %4.2f\n", x, y, z); + + indx++; + } + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for gyro. + */ +static int8_t set_gyro_config(struct bmi2_dev *dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define the type of sensor and its configurations. */ + struct bmi2_sens_config config; + + /* Configure the type of feature. */ + config.type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_get_sensor_config(&config, 1, dev); + bmi2_error_codes_print_result(rslt); + + /* Map data ready interrupt to interrupt pin. */ + rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT2, dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* The user can change the following configuration parameters according to their requirement. */ + /* Set Output Data Rate */ + config.cfg.gyr.odr = BMI2_GYR_ODR_100HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config.cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config.cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config.cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config.cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set the gyro configurations. */ + rslt = bmi270_set_sensor_config(&config, 1, dev); + } + + return rslt; +} + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (dps / ((half_scale) + BMI2_GYR_RANGE_2000)) * (val); +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/no_motion_interrupt/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/no_motion_interrupt/Makefile new file mode 100644 index 0000000000..c5df6dfb3c --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/no_motion_interrupt/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= no_motion_interrupt.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/no_motion_interrupt/no_motion_interrupt.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/no_motion_interrupt/no_motion_interrupt.c new file mode 100644 index 0000000000..5c46306602 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/no_motion_interrupt/no_motion_interrupt.c @@ -0,0 +1,134 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270.h" +#include "common.h" + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for no-motion. + * + * @param[in] bmi2_dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t set_feature_config(struct bmi2_dev *bmi2_dev); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Accel sensor and no-motion feature are listed in array. */ + uint8_t sens_list[2] = { BMI2_ACCEL, BMI2_NO_MOTION }; + + /* Variable to get no-motion interrupt status. */ + uint16_t int_status = 0; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Select features and their pins to be mapped to. */ + struct bmi2_sens_int_config sens_int = { .type = BMI2_NO_MOTION, .hw_int_pin = BMI2_INT1 }; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270. */ + rslt = bmi270_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the selected sensors. */ + rslt = bmi270_sensor_enable(sens_list, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Set feature configurations for no-motion. */ + rslt = set_feature_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Map the feature interrupt for no-motion. */ + rslt = bmi270_map_feat_int(&sens_int, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + printf("Do not move the board\n"); + + /* Loop to get no-motion interrupt. */ + do + { + /* Clear buffer. */ + int_status = 0; + + /* To get the interrupt status of no-motion. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the interrupt status of no-motion. */ + if (int_status & BMI270_NO_MOT_STATUS_MASK) + { + printf("No-motion interrupt is generated\n"); + break; + } + } while (rslt == BMI2_OK); + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for no-motion. + */ +static int8_t set_feature_config(struct bmi2_dev *bmi2_dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define the type of sensor and its configurations. */ + struct bmi2_sens_config config; + + /* Configure the type of feature. */ + config.type = BMI2_NO_MOTION; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_get_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* 1LSB equals 20ms. Default is 100ms, setting to 80ms. */ + config.cfg.no_motion.duration = 0x04; + + /* 1LSB equals to 0.48mg. Default is 83mg, setting to 50mg. */ + config.cfg.no_motion.threshold = 0x68; + + /* Set new configurations. */ + rslt = bmi270_set_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/read_aux_data_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/read_aux_data_mode/Makefile new file mode 100644 index 0000000000..db669c09dc --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/read_aux_data_mode/Makefile @@ -0,0 +1,23 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= read_aux_data_mode.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +BMM150_SOURCE ?= ../../../bmm150 + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270.c \ +$(BMM150_SOURCE)/bmm150.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(BMM150_SOURCE) \ +$(COMMON_LOCATION)/common + + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/read_aux_data_mode/read_aux_data_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/read_aux_data_mode/read_aux_data_mode.c new file mode 100644 index 0000000000..a542f97c61 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/read_aux_data_mode/read_aux_data_mode.c @@ -0,0 +1,327 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270.h" +#include "bmm150.h" +#include "coines.h" +#include "common.h" + +/******************************************************************************/ +/*! Macro definition */ + +/*! Macros to select the sensors */ +#define ACCEL UINT8_C(0x00) +#define GYRO UINT8_C(0x01) +#define AUX UINT8_C(0x02) + +/*! Earth's gravity in m/s^2 */ +#define GRAVITY_EARTH (9.80665f) + +/*****************************************************************************/ +/*! Structure declaration */ + +/* Sensor initialization configuration. */ +struct bmi2_dev bmi2_dev; + +/*******************************************************************************/ +/*! Functions */ + +/** + * user_aux_read - Reads data from auxiliary sensor in manual mode. + * + * @param[in] reg_addr : Register address. + * @param[out] aux_data : Aux data pointer to store the read data. + * @param[in] length : No of bytes to read. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval 0 -> Success + * @retval < 0 -> Failure Info + */ +static int8_t user_aux_read(uint8_t reg_addr, uint8_t *aux_data, uint32_t len, void *intf_ptr); + +/** + * user_aux_write - Writes data to the auxiliary sensor in manual mode. + * + * @param[in] reg_addr : Register address. + * @param[out] aux_data : Aux data pointer to store the data being written. + * @param[in] length : No of bytes to read. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval 0 -> Success + * @retval < 0 -> Failure Info + */ +static int8_t user_aux_write(uint8_t reg_addr, const uint8_t *aux_data, uint32_t len, void *intf_ptr); + +/*! + * @brief This function provides the delay for required time (Microsecond) as per the input provided in some of the + * APIs. + * + * @param[in] period_us : The required wait time in microsecond. + * @param[in] intf_ptr : Interface pointer + * + * @return void. + */ +static void user_delay_us(uint32_t period, void *intf_ptr); + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + * + * @param[in] val : LSB from each axis. + * @param[in] g_range : Gravity range. + * @param[in] bit_width : Resolution for accel. + * + * @return Gravity. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width); + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + * + * @param[in] val : LSB from each axis. + * @param[in] dps : Degree per second. + * @param[in] bit_width : Resolution for gyro. + * + * @return Degree per second. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width); + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to select the pull-up resistor which is set to trim register */ + uint8_t regdata; + + /* Variable to define limit to print aux data. */ + uint8_t limit = 20; + + /* Accel, Gyro and Aux sensors are listed in array. */ + uint8_t sensor_list[3] = { BMI2_ACCEL, BMI2_GYRO, BMI2_AUX }; + + /* Structure to define the type of the sensor and its configurations. */ + struct bmi2_sens_config config[3]; + + /* Sensor initialization configuration. */ + struct bmm150_dev bmm150_dev; + + /* bmm150 settings configuration */ + struct bmm150_settings settings; + + /* bmm150 magnetometer data */ + struct bmm150_mag_data mag_data; + + /* Structure to define type of sensor and their respective data. */ + struct bmi2_sensor_data sensor_data[3]; + + /* Variables to define read the accel and gyro data in float */ + float x = 0, y = 0, z = 0; + + config[ACCEL].type = BMI2_ACCEL; + config[GYRO].type = BMI2_GYRO; + config[AUX].type = BMI2_AUX; + + /* To enable the i2c interface settings for bmm150. */ + uint8_t bmm150_dev_addr = BMM150_DEFAULT_I2C_ADDRESS; + bmm150_dev.intf_ptr = &bmm150_dev_addr; + bmm150_dev.read = user_aux_read; + bmm150_dev.write = user_aux_write; + bmm150_dev.delay_us = user_delay_us; + + /* As per datasheet, aux interface with bmi270 will support only for I2C */ + bmm150_dev.intf = BMM150_I2C_INTF; + + sensor_data[ACCEL].type = BMI2_ACCEL; + sensor_data[GYRO].type = BMI2_GYRO; + sensor_data[AUX].type = BMI2_AUX; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270. */ + rslt = bmi270_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Pull-up resistor 2k is set to the trim regiter */ + regdata = BMI2_ASDA_PUPSEL_2K; + rslt = bmi2_set_regs(BMI2_AUX_IF_TRIM, ®data, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Enable the accel, gyro and aux sensor. */ + rslt = bmi270_sensor_enable(sensor_list, 3, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_get_sensor_config(config, 3, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Configurations for accel. */ + config[ACCEL].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + config[ACCEL].cfg.acc.bwp = BMI2_ACC_OSR2_AVG2; + config[ACCEL].cfg.acc.odr = BMI2_ACC_ODR_100HZ; + config[ACCEL].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* Configurations for gyro. */ + config[GYRO].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + config[GYRO].cfg.gyr.noise_perf = BMI2_GYR_RANGE_2000; + config[GYRO].cfg.gyr.bwp = BMI2_GYR_OSR2_MODE; + config[GYRO].cfg.gyr.odr = BMI2_GYR_ODR_100HZ; + config[GYRO].cfg.gyr.range = BMI2_GYR_RANGE_2000; + config[GYRO].cfg.gyr.ois_range = BMI2_GYR_OIS_2000; + + /* Configurations for aux. */ + config[AUX].cfg.aux.odr = BMI2_AUX_ODR_100HZ; + config[AUX].cfg.aux.aux_en = BMI2_ENABLE; + config[AUX].cfg.aux.i2c_device_addr = BMM150_DEFAULT_I2C_ADDRESS; + config[AUX].cfg.aux.manual_en = BMI2_ENABLE; + config[AUX].cfg.aux.fcu_write_en = BMI2_ENABLE; + config[AUX].cfg.aux.man_rd_burst = BMI2_AUX_READ_LEN_3; + config[AUX].cfg.aux.read_addr = BMM150_REG_DATA_X_LSB; + + /* Set new configurations for accel, gyro and aux. */ + rslt = bmi270_set_sensor_config(config, 3, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmm150. */ + rslt = bmm150_init(&bmm150_dev); + bmi2_error_codes_print_result(rslt); + + /* Set the power mode to normal mode. */ + settings.pwr_mode = BMM150_POWERMODE_NORMAL; + rslt = bmm150_set_op_mode(&settings, &bmm150_dev); + bmi2_error_codes_print_result(rslt); + + rslt = bmi270_get_sensor_config(config, 3, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Disable manual mode so that the data mode is enabled. */ + config[AUX].cfg.aux.manual_en = BMI2_DISABLE; + + /* Set the aux configurations. */ + rslt = bmi270_set_sensor_config(config, 3, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("MAGNETOMETER, ACCEL, AND GYRO DATA IN DATA MODE\n"); + + if (bmm150_dev.chip_id == BMM150_CHIP_ID) + { + while (limit--) + { + /* Delay has been added to get aux, accel and gyro data at the interval of every 0.05 second. */ + bmi2_dev.delay_us(50000, bmi2_dev.intf_ptr); + + rslt = bmi270_get_sensor_data(sensor_data, 3, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + printf("\nAcc_x = %d\t", sensor_data[ACCEL].sens_data.acc.x); + printf("Acc_y = %d\t", sensor_data[ACCEL].sens_data.acc.y); + printf("Acc_z = %d", sensor_data[ACCEL].sens_data.acc.z); + + /* Converting lsb to meter per second squared for 16 bit accelerometer at 2G range. */ + x = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.x, 2, bmi2_dev.resolution); + y = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.y, 2, bmi2_dev.resolution); + z = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.z, 2, bmi2_dev.resolution); + + /* Print the data in m/s2. */ + printf("\nAcc_ms2_X = %4.2f, Acc_ms2_Y = %4.2f, Acc_ms2_Z = %4.2f\n", x, y, z); + + printf("\nGyr_X = %d\t", sensor_data[GYRO].sens_data.gyr.x); + printf("Gyr_Y = %d\t", sensor_data[GYRO].sens_data.gyr.y); + printf("Gyr_Z= %d", sensor_data[GYRO].sens_data.gyr.z); + + /* Converting lsb to degree per second for 16 bit gyro at 2000dps range. */ + x = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.x, 2000, bmi2_dev.resolution); + y = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.y, 2000, bmi2_dev.resolution); + z = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.z, 2000, bmi2_dev.resolution); + + /* Print the data in dps. */ + printf("\nGyro_DPS_X = %4.2f, Gyro_DPS_Y = %4.2f, Gyro_DPS_Z = %4.2f\n", x, y, z); + + /* Compensating the raw auxiliary data available from the BMM150 API. */ + rslt = bmm150_aux_mag_data(sensor_data[AUX].sens_data.aux_data, &mag_data, &bmm150_dev); + bmi2_error_codes_print_result(rslt); + printf("\nMag_x_axis = %d \t Mag_y_axis = %d \t Mag_z_axis = %d \t\n", + mag_data.x, + mag_data.y, + mag_data.z); + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This function reads the data from auxiliary sensor in manual mode. + */ +static int8_t user_aux_read(uint8_t reg_addr, uint8_t *aux_data, uint32_t len, void *intf_ptr) +{ + int8_t rslt; + + /* Discarding the parameter id as it is redundant */ + rslt = bmi2_read_aux_man_mode(reg_addr, aux_data, len, &bmi2_dev); + + return rslt; +} + +/*! + * @brief This function writes the data to auxiliary sensor in manual mode. + */ +static int8_t user_aux_write(uint8_t reg_addr, const uint8_t *aux_data, uint32_t len, void *intf_ptr) +{ + int8_t rslt; + + /* Discarding the parameter id as it is redundant */ + rslt = bmi2_write_aux_man_mode(reg_addr, aux_data, len, &bmi2_dev); + + return rslt; +} + +/*! + * Delay function map to COINES platform + */ +static void user_delay_us(uint32_t period, void *intf_ptr) +{ + coines_delay_usec(period); +} + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (GRAVITY_EARTH * val * g_range) / half_scale; +} + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (dps / ((half_scale) + BMI2_GYR_RANGE_2000)) * (val); +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/read_aux_manual_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/read_aux_manual_mode/Makefile new file mode 100644 index 0000000000..cc9e6e4e74 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/read_aux_manual_mode/Makefile @@ -0,0 +1,23 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= read_aux_manual_mode.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +BMM150_SOURCE ?= ../../../bmm150 + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270.c \ +$(BMM150_SOURCE)/bmm150.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(BMM150_SOURCE) \ +$(COMMON_LOCATION)/common + + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/read_aux_manual_mode/read_aux_manual_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/read_aux_manual_mode/read_aux_manual_mode.c new file mode 100644 index 0000000000..ced8f59c7a --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/read_aux_manual_mode/read_aux_manual_mode.c @@ -0,0 +1,322 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270.h" +#include "bmm150.h" +#include "coines.h" +#include "common.h" + +/******************************************************************************/ +/*! Macro definition */ + +/*! Macros to select the sensors */ +#define ACCEL UINT8_C(0x00) +#define GYRO UINT8_C(0x01) +#define AUX UINT8_C(0x02) + +/*! Earth's gravity in m/s^2 */ +#define GRAVITY_EARTH (9.80665f) + +/*****************************************************************************/ +/*! Structure declaration */ + +/* Sensor initialization configuration. */ +struct bmi2_dev bmi2_dev; + +/******************************************************************************/ +/*! Functions */ + +/** + * user_aux_read - Reads data from auxiliary sensor in manual mode. + * + * @param[in] reg_addr : Register address. + * @param[out] aux_data : Aux data pointer to store the read data. + * @param[in] length : No of bytes to read. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval 0 -> Success + * @retval < 0 -> Failure Info + */ +static int8_t user_aux_read(uint8_t reg_addr, uint8_t *aux_data, uint32_t len, void *intf_ptr); + +/** + * user_aux_write - Writes data to the auxiliary sensor in manual mode. + * + * @param[in] reg_addr : Register address. + * @param[out] aux_data : Aux data pointer to store the data being written. + * @param[in] length : No of bytes to read. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval 0 -> Success + * @retval < 0 -> Failure Info + */ +static int8_t user_aux_write(uint8_t reg_addr, const uint8_t *aux_data, uint32_t len, void *intf_ptr); + +/*! + * @brief This function provides the delay for required time (Microsecond) as per the input provided in some of the + * APIs. + * + * @param[in] period_us : The required wait time in microsecond. + * @param[in] intf_ptr : Interface pointer + * + * @return void. + */ +static void user_delay_us(uint32_t period, void *intf_ptr); + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + * + * @param[in] val : LSB from each axis. + * @param[in] g_range : Gravity range. + * @param[in] bit_width : Resolution for accel. + * + * @return Gravity. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width); + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + * + * @param[in] val : LSB from each axis. + * @param[in] dps : Degree per second. + * @param[in] bit_width : Resolution for gyro. + * + * @return Degree per second. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width); + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to select the pull-up resistor which is set to trim register */ + uint8_t regdata; + + /* Variable to define limit to print aux data. */ + uint8_t limit = 20; + + /* Accel, Gyro and Aux sensors are listed in array. */ + uint8_t sensor_list[3] = { BMI2_ACCEL, BMI2_GYRO, BMI2_AUX }; + + /* Sensor initialization configuration. */ + struct bmm150_dev bmm150_dev; + + /* bmm150 settings configuration */ + struct bmm150_settings settings; + + /* bmm150 magnetometer data */ + struct bmm150_mag_data mag_data; + + /* Structure to define the type of the sensor and its configurations. */ + struct bmi2_sens_config config[3]; + + /* Structure to define type of sensor and their respective data. */ + struct bmi2_sensor_data sensor_data[2]; + + /* Variables to define read the accel and gyro data in float */ + float x = 0, y = 0, z = 0; + + config[ACCEL].type = BMI2_ACCEL; + config[GYRO].type = BMI2_GYRO; + config[AUX].type = BMI2_AUX; + + sensor_data[ACCEL].type = BMI2_ACCEL; + sensor_data[GYRO].type = BMI2_GYRO; + + /* Array of eight bytes to store x, y, z and r axis aux data. */ + uint8_t aux_data[8] = { 0 }; + + /* To enable the i2c interface settings for bmm150. */ + uint8_t bmm150_dev_addr = BMM150_DEFAULT_I2C_ADDRESS; + bmm150_dev.intf_ptr = &bmm150_dev_addr; + bmm150_dev.read = user_aux_read; + bmm150_dev.write = user_aux_write; + bmm150_dev.delay_us = user_delay_us; + + /* As per datasheet, aux interface with bmi270 will support only for I2C */ + bmm150_dev.intf = BMM150_I2C_INTF; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270. */ + rslt = bmi270_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Pull-up resistor 2k is set to the trim register */ + regdata = BMI2_ASDA_PUPSEL_2K; + rslt = bmi2_set_regs(BMI2_AUX_IF_TRIM, ®data, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Enable the accel, gyro and aux sensor. */ + rslt = bmi270_sensor_enable(sensor_list, 3, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_get_sensor_config(config, 3, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Configurations for accel. */ + config[ACCEL].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + config[ACCEL].cfg.acc.bwp = BMI2_ACC_OSR2_AVG2; + config[ACCEL].cfg.acc.odr = BMI2_ACC_ODR_100HZ; + config[ACCEL].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* Configurations for gyro. */ + config[GYRO].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + config[GYRO].cfg.gyr.noise_perf = BMI2_GYR_RANGE_2000; + config[GYRO].cfg.gyr.bwp = BMI2_GYR_OSR2_MODE; + config[GYRO].cfg.gyr.odr = BMI2_GYR_ODR_100HZ; + config[GYRO].cfg.gyr.range = BMI2_GYR_RANGE_2000; + config[GYRO].cfg.gyr.ois_range = BMI2_GYR_OIS_2000; + + /* Configurations for aux. */ + config[AUX].cfg.aux.odr = BMI2_AUX_ODR_100HZ; + config[AUX].cfg.aux.aux_en = BMI2_ENABLE; + config[AUX].cfg.aux.i2c_device_addr = BMM150_DEFAULT_I2C_ADDRESS; + config[AUX].cfg.aux.manual_en = BMI2_ENABLE; + config[AUX].cfg.aux.fcu_write_en = BMI2_ENABLE; + config[AUX].cfg.aux.man_rd_burst = BMI2_AUX_READ_LEN_3; + + /* Set new configurations for accel, gyro and aux. */ + rslt = bmi270_set_sensor_config(config, 3, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmm150. */ + rslt = bmm150_init(&bmm150_dev); + bmi2_error_codes_print_result(rslt); + + /* Set the power mode to normal mode. */ + settings.pwr_mode = BMM150_POWERMODE_NORMAL; + rslt = bmm150_set_op_mode(&settings, &bmm150_dev); + bmi2_error_codes_print_result(rslt); + + printf("MAGNETOMETER, ACCEL AND GYRO DATA IN MANUAL MODE\n"); + + if (bmm150_dev.chip_id == BMM150_CHIP_ID) + { + while (limit--) + { + /* Delay has been added to get aux, accel and gyro data at the interval of every 0.05 second. */ + bmi2_dev.delay_us(50000, bmi2_dev.intf_ptr); + + rslt = bmi270_get_sensor_data(sensor_data, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + printf("\nAcc_x = %d\t", sensor_data[ACCEL].sens_data.acc.x); + printf("Acc_y = %d\t", sensor_data[ACCEL].sens_data.acc.y); + printf("Acc_z = %d", sensor_data[ACCEL].sens_data.acc.z); + + /* Converting lsb to meter per second squared for 16 bit accelerometer at 2G range. */ + x = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.x, 2, bmi2_dev.resolution); + y = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.y, 2, bmi2_dev.resolution); + z = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.z, 2, bmi2_dev.resolution); + + /* Print the data in m/s2. */ + printf("\nAcc_ms2_X = %4.2f, Acc_ms2_Y = %4.2f, Acc_ms2_Z = %4.2f\n", x, y, z); + + printf("\nGyr_X = %d\t", sensor_data[GYRO].sens_data.gyr.x); + printf("Gyr_Y = %d\t", sensor_data[GYRO].sens_data.gyr.y); + printf("Gyr_Z = %d", sensor_data[GYRO].sens_data.gyr.z); + + /* Converting lsb to degree per second for 16 bit gyro at 2000dps range. */ + x = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.x, 2000, bmi2_dev.resolution); + y = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.y, 2000, bmi2_dev.resolution); + z = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.z, 2000, bmi2_dev.resolution); + + /* Print the data in dps. */ + printf("\nGyro_DPS_X = %4.2f, Gyro_DPS_Y = %4.2f, Gyro_DPS_Z = %4.2f\n", x, y, z); + } + + /* Read aux data from the bmm150 data registers. */ + rslt = bmi2_read_aux_man_mode(BMM150_REG_DATA_X_LSB, aux_data, 8, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + if (rslt == BMI2_OK) + { + /* Compensating the raw auxiliary data available from the BMM150 API. */ + rslt = bmm150_aux_mag_data(aux_data, &mag_data, &bmm150_dev); + bmi2_error_codes_print_result(rslt); + printf("Mag_x_axis = %d \t Mag_y_axis = %d \t Mag_z_axis = %d \t\n", mag_data.x, mag_data.y, + mag_data.z); + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This function reads the data from auxiliary sensor in manual mode. + */ +int8_t user_aux_read(uint8_t reg_addr, uint8_t *aux_data, uint32_t len, void *intf_ptr) +{ + int8_t rslt; + + /* Discarding the parameter id as it is redundant */ + rslt = bmi2_read_aux_man_mode(reg_addr, aux_data, len, &bmi2_dev); + + return rslt; +} + +/*! + * @brief This function writes the data to auxiliary sensor in manual mode. + */ +int8_t user_aux_write(uint8_t reg_addr, const uint8_t *aux_data, uint32_t len, void *intf_ptr) +{ + int8_t rslt; + + /* Discarding the parameter id as it is redundant */ + rslt = bmi2_write_aux_man_mode(reg_addr, aux_data, len, &bmi2_dev); + + return rslt; +} + +/*! + * Delay function map to COINES platform + */ +static void user_delay_us(uint32_t period, void *intf_ptr) +{ + coines_delay_usec(period); +} + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (GRAVITY_EARTH * val * g_range) / half_scale; +} + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (dps / ((half_scale) + BMI2_GYR_RANGE_2000)) * (val); +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/sig_motion/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/sig_motion/Makefile new file mode 100644 index 0000000000..97edcc2d16 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/sig_motion/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= sig_motion.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/sig_motion/sig_motion.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/sig_motion/sig_motion.c new file mode 100644 index 0000000000..47e1c2864d --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/sig_motion/sig_motion.c @@ -0,0 +1,93 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270.h" +#include "common.h" + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define the type of sensor and its configurations. */ + struct bmi2_sens_config config; + + /* Accel sensor and significant-motion feature are listed in array. */ + uint8_t sens_list[2] = { BMI2_ACCEL, BMI2_SIG_MOTION }; + + /* Variable to get significant-motion interrupt status. */ + uint16_t int_status = 0; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Select features and their pins to be mapped to. */ + struct bmi2_sens_int_config sens_int = { .type = BMI2_SIG_MOTION, .hw_int_pin = BMI2_INT1 }; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270. */ + rslt = bmi270_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the selected sensors. */ + rslt = bmi270_sensor_enable(sens_list, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Configure the type of feature. */ + config.type = BMI2_SIG_MOTION; + + if (rslt == BMI2_OK) + { + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_get_sensor_config(&config, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Map the feature interrupt for significant-motion. */ + rslt = bmi270_map_feat_int(&sens_int, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* By default the significant-motion interrupt occurs when the sensor is in motion for about 5 seconds. + * */ + printf("Move the board for 5 secs in any direction\n"); + + do + { + /* To get the interrupt status of significant-motion. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the interrupt status of significant-motion. */ + if (int_status & BMI270_SIG_MOT_STATUS_MASK) + { + printf("Significant motion interrupt is generated\n"); + break; + } + } while (rslt == BMI2_OK); + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/step_activity/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/step_activity/Makefile new file mode 100644 index 0000000000..91962d5026 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/step_activity/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= step_activity.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/step_activity/step_activity.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/step_activity/step_activity.c new file mode 100644 index 0000000000..87775b706b --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/step_activity/step_activity.c @@ -0,0 +1,110 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270.h" +#include "common.h" + +/******************************************************************************/ +/*! Functions */ +/* This function starts the execution of program. */ +int main(void) +{ + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Structure to define the type of sensor and their respective data. */ + struct bmi2_sensor_data sensor_data; + + /* Structure to define the type of sensor and its configurations. */ + struct bmi2_sens_config config; + + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Accel sensor and step activity feature are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_STEP_ACTIVITY }; + + /* Type of sensor to get step activity data. */ + sensor_data.type = BMI2_STEP_ACTIVITY; + + /* Variable to get step activity interrupt status. */ + uint16_t int_status = 0; + + uint16_t loop = 10; + + /* Select features and their pins to be mapped to. */ + struct bmi2_sens_int_config sens_int = { .type = BMI2_STEP_ACTIVITY, .hw_int_pin = BMI2_INT2 }; + + /* The step activities are listed in array. */ + const char *activity_output[4] = { "BMI2_STILL", "BMI2_WALK", "BMI2_RUN", "BMI2_UNKNOWN" }; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270. */ + rslt = bmi270_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the selected sensor. */ + rslt = bmi270_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Configure the type of sensor. */ + config.type = BMI2_STEP_ACTIVITY; + + if (rslt == BMI2_OK) + { + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_get_sensor_config(&config, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Map the feature interrupt for step activity. */ + rslt = bmi270_map_feat_int(&sens_int, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("\nMove the board in steps to perform step activity:\n"); + + /* Loop to get step activity. */ + while (loop) + { + /* To get the interrupt status of step detector. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the interrupt status of step detector. */ + if (int_status & BMI270_STEP_ACT_STATUS_MASK) + { + printf("Step detected\n"); + + /* Get step activity output. */ + rslt = bmi270_get_sensor_data(&sensor_data, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Print the step activity output. */ + printf("Step activity = %s\n", activity_output[sensor_data.sens_data.activity_output]); + + loop--; + } + } + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/step_counter/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/step_counter/Makefile new file mode 100644 index 0000000000..b47fb0e9d1 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/step_counter/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= step_counter.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/step_counter/step_counter.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/step_counter/step_counter.c new file mode 100644 index 0000000000..b08f958a22 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/step_counter/step_counter.c @@ -0,0 +1,146 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270.h" +#include "common.h" + +/******************************************************************************/ +/*! Static function declaration */ + +/*! + * @brief This internal API is used to set configurations for step counter. + * + * @param[in] bmi2_dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t set_feature_config(struct bmi2_dev *bmi2_dev); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Structure to define type of sensor and their respective data. */ + struct bmi2_sensor_data sensor_data; + + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Accel sensor and step counter feature are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_STEP_COUNTER }; + + /* Variable to get step counter interrupt status. */ + uint16_t int_status = 0; + + /* Select features and their pins to be mapped to. */ + struct bmi2_sens_int_config sens_int = { .type = BMI2_STEP_COUNTER, .hw_int_pin = BMI2_INT2 }; + + /* Type of sensor to get step counter data. */ + sensor_data.type = BMI2_STEP_COUNTER; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270. */ + rslt = bmi270_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the selected sensor. */ + rslt = bmi270_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Set the feature configuration for step counter. */ + rslt = set_feature_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("Step counter watermark level set to 1 (20 steps)\n"); + + if (rslt == BMI2_OK) + { + /* Map the step counter feature interrupt. */ + rslt = bmi270_map_feat_int(&sens_int, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Move the board in steps for 20 times to get step counter interrupt. */ + printf("Move the board in steps\n"); + + /* Loop to get number of steps counted. */ + do + { + /* To get the interrupt status of the step counter. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the interrupt status of the step counter. */ + if (int_status & BMI270_STEP_CNT_STATUS_MASK) + { + printf("Step counter interrupt occurred when watermark level (20 steps) is reached\n"); + + /* Get step counter output. */ + rslt = bmi270_get_sensor_data(&sensor_data, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Print the step counter output. */ + printf("No of steps counted = %ld", sensor_data.sens_data.step_counter_output); + break; + } + } while (rslt == BMI2_OK); + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for step counter. + */ +static int8_t set_feature_config(struct bmi2_dev *bmi2_dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define the type of sensor and its configurations. */ + struct bmi2_sens_config config; + + /* Configure the type of sensor. */ + config.type = BMI2_STEP_COUNTER; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_get_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Setting water-mark level to 1 for step counter to get interrupt after 20 step counts. Every 20 steps once + * output triggers. */ + config.cfg.step_counter.watermark_level = 1; + + /* Set new configuration. */ + rslt = bmi270_set_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/wrist_gesture/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/wrist_gesture/Makefile new file mode 100644 index 0000000000..0f959907ba --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/wrist_gesture/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= wrist_gesture.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/wrist_gesture/wrist_gesture.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/wrist_gesture/wrist_gesture.c new file mode 100644 index 0000000000..be494b8be9 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/wrist_gesture/wrist_gesture.c @@ -0,0 +1,147 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270.h" +#include "common.h" + +/******************************************************************************/ +/*! Static function declaration */ + +/*! + * @brief This internal API is used to set the sensor configuration. + * + * @param[in] bmi2_dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t bmi2_set_config(struct bmi2_dev *bmi2_dev); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program */ +int main(void) +{ + /* Variable to define result */ + int8_t rslt; + + /* Initialize status of wrist gesture interrupt */ + uint16_t int_status = 0; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Select features and their pins to be mapped to */ + struct bmi2_sens_int_config sens_int = { .type = BMI2_WRIST_GESTURE, .hw_int_pin = BMI2_INT1 }; + + /* Sensor data structure */ + struct bmi2_sensor_data sens_data = { 0 }; + + /* The gesture movements are listed in array */ + const char *gesture_output[6] = + { "unknown_gesture", "push_arm_down", "pivot_up", "wrist_shake_jiggle", "flick_in", "flick_out" }; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270. */ + rslt = bmi270_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + sens_data.type = BMI2_WRIST_GESTURE; + + /* Set the sensor configuration */ + rslt = bmi2_set_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Map the feature interrupt */ + rslt = bmi270_map_feat_int(&sens_int, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + printf("Flip the board in portrait/landscape mode:\n"); + + /* Loop to print the wrist gesture data when interrupt occurs */ + while (1) + { + /* Get the interrupt status of the wrist gesture */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if ((rslt == BMI2_OK) && (int_status & BMI270_WRIST_GEST_STATUS_MASK)) + { + printf("Wrist gesture detected\n"); + + /* Get wrist gesture output */ + rslt = bmi270_get_sensor_data(&sens_data, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("Wrist gesture = %d\r\n", sens_data.sens_data.wrist_gesture_output); + + printf("Gesture output = %s\n", gesture_output[sens_data.sens_data.wrist_gesture_output]); + break; + } + } + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API sets the sensor configuration + */ +static int8_t bmi2_set_config(struct bmi2_dev *bmi2_dev) +{ + /* Variable to define result */ + int8_t rslt; + + /* List the sensors which are required to enable */ + uint8_t sens_list[2] = { BMI2_ACCEL, BMI2_WRIST_GESTURE }; + + /* Structure to define the type of the sensor and its configurations */ + struct bmi2_sens_config config; + + /* Configure type of feature */ + config.type = BMI2_WRIST_GESTURE; + + /* Enable the selected sensors */ + rslt = bmi270_sensor_enable(sens_list, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Get default configurations for the type of feature selected */ + rslt = bmi270_get_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + config.cfg.wrist_gest.wearable_arm = BMI2_ARM_LEFT; + + /* Set the new configuration along with interrupt mapping */ + rslt = bmi270_set_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/wrist_wear_wakeup/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/wrist_wear_wakeup/Makefile new file mode 100644 index 0000000000..1e20da2ea8 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/wrist_wear_wakeup/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= wrist_wear_wakeup.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/wrist_wear_wakeup/wrist_wear_wakeup.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/wrist_wear_wakeup/wrist_wear_wakeup.c new file mode 100644 index 0000000000..3547b2d309 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/wrist_wear_wakeup/wrist_wear_wakeup.c @@ -0,0 +1,131 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270.h" +#include "common.h" + +/******************************************************************************/ +/*! Static function declaration */ + +/*! + * @brief This internal API is used to set the sensor configuration. + * + * @param[in] bmi2_dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t bmi2_set_config(struct bmi2_dev *bmi2_dev); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program */ +int main(void) +{ + /* Variable to define result */ + int8_t rslt; + + /* Initialize status of wrist wear wakeup interrupt */ + uint16_t int_status = 0; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Select features and their pins to be mapped to */ + struct bmi2_sens_int_config sens_int = { .type = BMI2_WRIST_WEAR_WAKE_UP, .hw_int_pin = BMI2_INT1 }; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270. */ + rslt = bmi270_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Set the sensor configuration */ + rslt = bmi2_set_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Map the feature interrupt */ + rslt = bmi270_map_feat_int(&sens_int, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + printf("Tilt the board in landscape position to trigger wrist wear wakeup\n"); + + /* Loop to print the wrist wear wakeup data when interrupt occurs */ + while (1) + { + int_status = 0; + + /* To get the interrupt status of the wrist wear wakeup */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the interrupt status of the wrist gesture */ + if ((rslt == BMI2_OK) && (int_status & BMI270_WRIST_WAKE_UP_STATUS_MASK)) + { + printf("Wrist wear wakeup detected\n"); + break; + } + } + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API sets the sensor configuration + */ +static int8_t bmi2_set_config(struct bmi2_dev *bmi2_dev) +{ + /* Variable to define result */ + int8_t rslt; + + /* List the sensors which are required to enable */ + uint8_t sens_list[2] = { BMI2_ACCEL, BMI2_WRIST_WEAR_WAKE_UP }; + + /* Structure to define the type of the sensor and its configurations */ + struct bmi2_sens_config config; + + /* Configure type of feature */ + config.type = BMI2_WRIST_WEAR_WAKE_UP; + + /* Enable the selected sensors */ + rslt = bmi270_sensor_enable(sens_list, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Get default configurations for the type of feature selected */ + rslt = bmi270_get_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Set the new configuration */ + rslt = bmi270_set_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/accel/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/accel/Makefile new file mode 100644 index 0000000000..d215172455 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/accel/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= accel.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_context.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/accel/accel.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/accel/accel.c new file mode 100644 index 0000000000..5b36fffe42 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/accel/accel.c @@ -0,0 +1,200 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_context.h" +#include "common.h" + +/******************************************************************************/ +/*! Macro definition */ + +/*! Earth's gravity in m/s^2 */ +#define GRAVITY_EARTH (9.80665f) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t set_accel_config(struct bmi2_dev *bmi2_dev); + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + * + * @param[in] val : LSB from each axis. + * @param[in] g_range : Gravity range. + * @param[in] bit_width : Resolution for accel. + * + * @return Gravity. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to define limit to print accel data. */ + uint8_t limit = 20; + + /* Assign accel sensor to variable. */ + uint8_t sensor_list = BMI2_ACCEL; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Create an instance of sensor data structure. */ + struct bmi2_sensor_data sensor_data = { 0 }; + + /* Initialize the interrupt status of accel. */ + uint16_t int_status = 0; + + uint8_t indx = 0; + float x = 0, y = 0, z = 0; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_context. */ + rslt = bmi270_context_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the accel sensor. */ + rslt = bmi270_context_sensor_enable(&sensor_list, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Accel configuration settings. */ + rslt = set_accel_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Assign accel sensor. */ + sensor_data.type = BMI2_ACCEL; + printf("Accel and m/s2 data \n"); + printf("Accel data collected at 2G Range with 16-bit resolution\n"); + + /* Loop to print the accel data when interrupt occurs. */ + while (indx <= limit) + { + /* To get the status of accel data ready interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the accel data ready interrupt status and print the status for 10 samples. */ + if (int_status & BMI2_ACC_DRDY_INT_MASK) + { + /* Get accelerometer data for x, y and z axis. */ + rslt = bmi270_context_get_sensor_data(&sensor_data, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + printf("\nAcc_X = %d\t", sensor_data.sens_data.acc.x); + printf("Acc_Y = %d\t", sensor_data.sens_data.acc.y); + printf("Acc_Z = %d\r\n", sensor_data.sens_data.acc.z); + + /* Converting lsb to meter per second squared for 16 bit accelerometer at 2G range. */ + x = lsb_to_mps2(sensor_data.sens_data.acc.x, 2, bmi2_dev.resolution); + y = lsb_to_mps2(sensor_data.sens_data.acc.y, 2, bmi2_dev.resolution); + z = lsb_to_mps2(sensor_data.sens_data.acc.z, 2, bmi2_dev.resolution); + + /* Print the data in m/s2. */ + printf("\nAcc_ms2_X = %4.2f, Acc_ms2_Y = %4.2f, Acc_ms2_Z = %4.2f\n", x, y, z); + + indx++; + } + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel. + */ +static int8_t set_accel_config(struct bmi2_dev *bmi2_dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accelerometer configuration. */ + struct bmi2_sens_config config; + + /* Configure the type of feature. */ + config.type = BMI2_ACCEL; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_context_get_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* Set Output Data Rate */ + config.cfg.acc.odr = BMI2_ACC_ODR_200HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config.cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples. + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config.cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config.cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set the accel configurations. */ + rslt = bmi270_context_set_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Map data ready interrupt to interrupt pin. */ + rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (GRAVITY_EARTH * val * g_range) / half_scale; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/accel_gyro/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/accel_gyro/Makefile new file mode 100644 index 0000000000..2eed195611 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/accel_gyro/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= accel_gyro.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_context.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/accel_gyro/accel_gyro.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/accel_gyro/accel_gyro.c new file mode 100644 index 0000000000..7458969efb --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/accel_gyro/accel_gyro.c @@ -0,0 +1,268 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_context.h" +#include "common.h" + +/******************************************************************************/ +/*! Macro definition */ + +/*! Earth's gravity in m/s^2 */ +#define GRAVITY_EARTH (9.80665f) + +/*! Macros to select the sensors */ +#define ACCEL UINT8_C(0x00) +#define GYRO UINT8_C(0x01) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev); + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + * + * @param[in] val : LSB from each axis. + * @param[in] g_range : Gravity range. + * @param[in] bit_width : Resolution for accel. + * + * @return Gravity. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width); + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + * + * @param[in] val : LSB from each axis. + * @param[in] dps : Degree per second. + * @param[in] bit_width : Resolution for gyro. + * + * @return Degree per second. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to define limit to print accel data. */ + uint8_t limit = 10; + + /* Assign accel and gyro sensor to variable. */ + uint8_t sensor_list[2] = { BMI2_ACCEL, BMI2_GYRO }; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Create an instance of sensor data structure. */ + struct bmi2_sensor_data sensor_data[2] = { { 0 } }; + + /* Initialize the interrupt status of accel and gyro. */ + uint16_t int_status = 0; + + uint8_t indx = 1; + + float x = 0, y = 0, z = 0; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_context. */ + rslt = bmi270_context_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the accel and gyro sensor. */ + rslt = bmi270_context_sensor_enable(sensor_list, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Accel and gyro configuration settings. */ + rslt = set_accel_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Assign accel and gyro sensor. */ + sensor_data[ACCEL].type = BMI2_ACCEL; + sensor_data[GYRO].type = BMI2_GYRO; + + /* Loop to print accel and gyro data when interrupt occurs. */ + while (indx <= limit) + { + /* To get the data ready interrupt status of accel and gyro. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the data ready interrupt status and print the status for 10 samples. */ + if ((int_status & BMI2_ACC_DRDY_INT_MASK) && (int_status & BMI2_GYR_DRDY_INT_MASK)) + { + /* Get accel and gyro data for x, y and z axis. */ + rslt = bmi270_context_get_sensor_data(sensor_data, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("\n******* Accel(Raw and m/s2) Gyro(Raw and dps) data : %d *******\n", indx); + + printf("\nAcc_x = %d\t", sensor_data[ACCEL].sens_data.acc.x); + printf("Acc_y = %d\t", sensor_data[ACCEL].sens_data.acc.y); + printf("Acc_z = %d", sensor_data[ACCEL].sens_data.acc.z); + + /* Converting lsb to meter per second squared for 16 bit accelerometer at 2G range. */ + x = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.x, 2, bmi2_dev.resolution); + y = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.y, 2, bmi2_dev.resolution); + z = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.z, 2, bmi2_dev.resolution); + + /* Print the data in m/s2. */ + printf("\nAcc_ms2_X = %4.2f, Acc_ms2_Y = %4.2f, Acc_ms2_Z = %4.2f\n", x, y, z); + + printf("\nGyr_X = %d\t", sensor_data[GYRO].sens_data.gyr.x); + printf("Gyr_Y = %d\t", sensor_data[GYRO].sens_data.gyr.y); + printf("Gyr_Z = %d\n", sensor_data[GYRO].sens_data.gyr.z); + + /* Converting lsb to degree per second for 16 bit gyro at 2000dps range. */ + x = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.x, 2000, bmi2_dev.resolution); + y = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.y, 2000, bmi2_dev.resolution); + z = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.z, 2000, bmi2_dev.resolution); + + /* Print the data in dps. */ + printf("Gyro_DPS_X = %4.2f, Gyro_DPS_Y = %4.2f, Gyro_DPS_Z = %4.2f\n", x, y, z); + + indx++; + } + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accelerometer and gyro configuration. */ + struct bmi2_sens_config config[2]; + + /* Configure the type of feature. */ + config[ACCEL].type = BMI2_ACCEL; + config[GYRO].type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_context_get_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Map data ready interrupt to interrupt pin. */ + rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* Set Output Data Rate */ + config[ACCEL].cfg.acc.odr = BMI2_ACC_ODR_200HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config[ACCEL].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples. + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config[ACCEL].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config[ACCEL].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* The user can change the following configuration parameters according to their requirement. */ + /* Set Output Data Rate */ + config[GYRO].cfg.gyr.odr = BMI2_GYR_ODR_200HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config[GYRO].cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config[GYRO].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config[GYRO].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config[GYRO].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set the accel and gyro configurations. */ + rslt = bmi270_context_set_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (GRAVITY_EARTH * val * g_range) / half_scale; +} + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (dps / ((half_scale) + BMI2_GYR_RANGE_2000)) * (val); +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/activity_recognition/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/activity_recognition/Makefile new file mode 100644 index 0000000000..28ed939cd6 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/activity_recognition/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= activity_recognition.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_context.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/activity_recognition/activity_recognition.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/activity_recognition/activity_recognition.c new file mode 100644 index 0000000000..9f0714d384 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/activity_recognition/activity_recognition.c @@ -0,0 +1,154 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_context.h" +#include "common.h" + +/******************************************************************************/ +/*! Static function declaration */ + +/*! + * @brief This internal API is used to set configurations for step counter. + * + * @param[in] bmi2_dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t set_config(struct bmi2_dev *bmi2_dev); + +/******************************************************************************/ +/*! Functions */ +/* This function starts the execution of program. */ +int main(void) +{ + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to define index for FIFO data */ + uint8_t count; + + /* Various Activity recognitions are listed in array. */ + const char *activity_reg_output[6] = { "OTHERS", "STILL", "WALKING", "RUNNING", "ON_BICYCLE", "IN_VEHICLE" }; + + /* Accel sensor and step activity feature are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_ACTIVITY_RECOGNITION }; + + /* Array to define FIFO data */ + uint8_t fifo_data[516] = { 0 }; + + /* Variable to store number of activity frames */ + uint16_t act_frames; + + /* Structure to define a FIFO */ + struct bmi2_fifo_frame fifoframe = { 0 }; + + /* Structure to define output for activity recognition */ + struct bmi2_act_recog_output act_recog_data[5] = { { 0 } }; + + uint16_t fifo_length; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_context. */ + rslt = bmi270_context_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the selected sensor. */ + rslt = bmi270_context_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + rslt = set_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Update FIFO structure */ + fifoframe.data = fifo_data; + fifoframe.length = 516; + + rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); + printf("Fifo length = %d \n", fifo_length); + + /* Read FIFO data */ + rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + while (rslt != BMI2_W_FIFO_EMPTY) + { + /* Provide the number of frames to be read */ + act_frames = 5; + + printf("Requested FIFO data frames : %d\n", act_frames); + + /* Get the activity output */ + rslt = bmi270_context_get_act_recog_output(act_recog_data, &act_frames, &fifoframe, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("Parsed FIFO data frames : %d\r\n", act_frames); + + for (count = 0; count < act_frames; count++) + { + printf( + "Activity Recognition output[%d]:Sensor time: %lu\t Previous activity: %s\t current: %s\n", + count, + act_recog_data[count].time_stamp, + activity_reg_output[act_recog_data[count].prev_act], + activity_reg_output[act_recog_data[count].curr_act]); + } + } + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +static int8_t set_config(struct bmi2_dev *bmi2_dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + uint8_t temp_data[2]; + + /* Disable advanced power save */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Reset FIFO */ + rslt = bmi2_set_command_register(BMI2_FIFO_FLUSH_CMD, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("Move the board for activity recognition for 30 sec :\n"); + bmi2_dev->delay_us(30000000, bmi2_dev->intf_ptr); + + if (rslt == BMI2_OK) + { + rslt = bmi2_get_regs(BMI2_FIFO_CONFIG_0_ADDR, temp_data, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/common/common.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/common/common.c new file mode 100644 index 0000000000..afff37bc84 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/common/common.c @@ -0,0 +1,372 @@ +/** + * Copyright (C) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + */ + +#include +#include +#include + +#include "coines.h" +#include "bmi2_defs.h" + +/******************************************************************************/ +/*! Macro definitions */ +#define BMI2XY_SHUTTLE_ID UINT16_C(0x1B8) + +/*! Macro that defines read write length */ +#define READ_WRITE_LEN UINT8_C(46) + +/******************************************************************************/ +/*! Static variable definition */ + +/*! Variable that holds the I2C device address or SPI chip selection */ +static uint8_t dev_addr; + +/******************************************************************************/ +/*! User interface functions */ + +/*! + * I2C read function map to COINES platform + */ +BMI2_INTF_RETURN_TYPE bmi2_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr) +{ + uint8_t dev_addr = *(uint8_t*)intf_ptr; + + return coines_read_i2c(dev_addr, reg_addr, reg_data, (uint16_t)len); +} + +/*! + * I2C write function map to COINES platform + */ +BMI2_INTF_RETURN_TYPE bmi2_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr) +{ + uint8_t dev_addr = *(uint8_t*)intf_ptr; + + return coines_write_i2c(dev_addr, reg_addr, (uint8_t *)reg_data, (uint16_t)len); +} + +/*! + * SPI read function map to COINES platform + */ +BMI2_INTF_RETURN_TYPE bmi2_spi_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr) +{ + uint8_t dev_addr = *(uint8_t*)intf_ptr; + + return coines_read_spi(dev_addr, reg_addr, reg_data, (uint16_t)len); +} + +/*! + * SPI write function map to COINES platform + */ +BMI2_INTF_RETURN_TYPE bmi2_spi_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr) +{ + uint8_t dev_addr = *(uint8_t*)intf_ptr; + + return coines_write_spi(dev_addr, reg_addr, (uint8_t *)reg_data, (uint16_t)len); +} + +/*! + * Delay function map to COINES platform + */ +void bmi2_delay_us(uint32_t period, void *intf_ptr) +{ + coines_delay_usec(period); +} + +/*! + * @brief Function to select the interface between SPI and I2C. + * Also to initialize coines platform + */ +int8_t bmi2_interface_init(struct bmi2_dev *bmi, uint8_t intf) +{ + int8_t rslt = BMI2_OK; + struct coines_board_info board_info; + + if (bmi != NULL) + { + int16_t result = coines_open_comm_intf(COINES_COMM_INTF_USB); + if (result < COINES_SUCCESS) + { + printf( + "\n Unable to connect with Application Board ! \n" " 1. Check if the board is connected and powered on. \n" " 2. Check if Application Board USB driver is installed. \n" + " 3. Check if board is in use by another application. (Insufficient permissions to access USB) \n"); + exit(result); + } + + result = coines_get_board_info(&board_info); + +#if defined(PC) + setbuf(stdout, NULL); +#endif + + if (result == COINES_SUCCESS) + { + if ((board_info.shuttle_id != BMI2XY_SHUTTLE_ID)) + { + printf("! Warning invalid sensor shuttle \n ," "This application will not support this sensor \n"); + exit(COINES_E_FAILURE); + } + } + + coines_set_shuttleboard_vdd_vddio_config(0, 0); + coines_delay_msec(100); + + /* Bus configuration : I2C */ + if (intf == BMI2_I2C_INTF) + { + printf("I2C Interface \n"); + + /* To initialize the user I2C function */ + dev_addr = BMI2_I2C_PRIM_ADDR; + bmi->intf = BMI2_I2C_INTF; + bmi->read = bmi2_i2c_read; + bmi->write = bmi2_i2c_write; + coines_config_i2c_bus(COINES_I2C_BUS_0, COINES_I2C_FAST_MODE); + } + /* Bus configuration : SPI */ + else if (intf == BMI2_SPI_INTF) + { + printf("SPI Interface \n"); + + /* To initialize the user SPI function */ + dev_addr = COINES_SHUTTLE_PIN_7; + bmi->intf = BMI2_SPI_INTF; + bmi->read = bmi2_spi_read; + bmi->write = bmi2_spi_write; + coines_config_spi_bus(COINES_SPI_BUS_0, COINES_SPI_SPEED_5_MHZ, COINES_SPI_MODE3); + coines_set_pin_config(COINES_SHUTTLE_PIN_7, COINES_PIN_DIRECTION_OUT, COINES_PIN_VALUE_HIGH); + } + + /* Assign device address to interface pointer */ + bmi->intf_ptr = &dev_addr; + + /* Configure delay in microseconds */ + bmi->delay_us = bmi2_delay_us; + + /* Configure max read/write length (in bytes) ( Supported length depends on target machine) */ + bmi->read_write_len = READ_WRITE_LEN; + + /* Assign to NULL to load the default config file. */ + bmi->config_file_ptr = NULL; + + coines_delay_usec(10000); + + coines_set_shuttleboard_vdd_vddio_config(3300, 3300); + + coines_delay_usec(10000); + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; + +} + +/*! + * @brief Prints the execution status of the APIs. + */ +void bmi2_error_codes_print_result(int8_t rslt) +{ + switch (rslt) + { + case BMI2_OK: + + /* Do nothing */ + break; + + case BMI2_W_FIFO_EMPTY: + printf("Warning [%d] : FIFO empty\r\n", rslt); + break; + case BMI2_W_PARTIAL_READ: + printf("Warning [%d] : FIFO partial read\r\n", rslt); + break; + case BMI2_E_NULL_PTR: + printf( + "Error [%d] : Null pointer error. It occurs when the user tries to assign value (not address) to a pointer," " which has been initialized to NULL.\r\n", + rslt); + break; + + case BMI2_E_COM_FAIL: + printf( + "Error [%d] : Communication failure error. It occurs due to read/write operation failure and also due " "to power failure during communication\r\n", + rslt); + break; + + case BMI2_E_DEV_NOT_FOUND: + printf("Error [%d] : Device not found error. It occurs when the device chip id is incorrectly read\r\n", + rslt); + break; + + case BMI2_E_INVALID_SENSOR: + printf( + "Error [%d] : Invalid sensor error. It occurs when there is a mismatch in the requested feature with the " "available one\r\n", + rslt); + break; + + case BMI2_E_SELF_TEST_FAIL: + printf( + "Error [%d] : Self-test failed error. It occurs when the validation of accel self-test data is " "not satisfied\r\n", + rslt); + break; + + case BMI2_E_INVALID_INT_PIN: + printf( + "Error [%d] : Invalid interrupt pin error. It occurs when the user tries to configure interrupt pins " "apart from INT1 and INT2\r\n", + rslt); + break; + + case BMI2_E_OUT_OF_RANGE: + printf( + "Error [%d] : Out of range error. It occurs when the data exceeds from filtered or unfiltered data from " "fifo and also when the range exceeds the maximum range for accel and gyro while performing FOC\r\n", + rslt); + break; + + case BMI2_E_ACC_INVALID_CFG: + printf( + "Error [%d] : Invalid Accel configuration error. It occurs when there is an error in accel configuration" " register which could be one among range, BW or filter performance in reg address 0x40\r\n", + rslt); + break; + + case BMI2_E_GYRO_INVALID_CFG: + printf( + "Error [%d] : Invalid Gyro configuration error. It occurs when there is a error in gyro configuration" "register which could be one among range, BW or filter performance in reg address 0x42\r\n", + rslt); + break; + + case BMI2_E_ACC_GYR_INVALID_CFG: + printf( + "Error [%d] : Invalid Accel-Gyro configuration error. It occurs when there is a error in accel and gyro" " configuration registers which could be one among range, BW or filter performance in reg address 0x40 " "and 0x42\r\n", + rslt); + break; + + case BMI2_E_CONFIG_LOAD: + printf( + "Error [%d] : Configuration load error. It occurs when failure observed while loading the configuration " "into the sensor\r\n", + rslt); + break; + + case BMI2_E_INVALID_PAGE: + printf( + "Error [%d] : Invalid page error. It occurs due to failure in writing the correct feature configuration " "from selected page\r\n", + rslt); + break; + + case BMI2_E_SET_APS_FAIL: + printf( + "Error [%d] : APS failure error. It occurs due to failure in write of advance power mode configuration " "register\r\n", + rslt); + break; + + case BMI2_E_AUX_INVALID_CFG: + printf( + "Error [%d] : Invalid AUX configuration error. It occurs when the auxiliary interface settings are not " "enabled properly\r\n", + rslt); + break; + + case BMI2_E_AUX_BUSY: + printf( + "Error [%d] : AUX busy error. It occurs when the auxiliary interface buses are engaged while configuring" " the AUX\r\n", + rslt); + break; + + case BMI2_E_REMAP_ERROR: + printf( + "Error [%d] : Remap error. It occurs due to failure in assigning the remap axes data for all the axes " "after change in axis position\r\n", + rslt); + break; + + case BMI2_E_GYR_USER_GAIN_UPD_FAIL: + printf( + "Error [%d] : Gyro user gain update fail error. It occurs when the reading of user gain update status " "fails\r\n", + rslt); + break; + + case BMI2_E_SELF_TEST_NOT_DONE: + printf( + "Error [%d] : Self-test not done error. It occurs when the self-test process is ongoing or not " "completed\r\n", + rslt); + break; + + case BMI2_E_INVALID_INPUT: + printf("Error [%d] : Invalid input error. It occurs when the sensor input validity fails\r\n", rslt); + break; + + case BMI2_E_INVALID_STATUS: + printf("Error [%d] : Invalid status error. It occurs when the feature/sensor validity fails\r\n", rslt); + break; + + case BMI2_E_CRT_ERROR: + printf("Error [%d] : CRT error. It occurs when the CRT test has failed\r\n", rslt); + break; + + case BMI2_E_ST_ALREADY_RUNNING: + printf( + "Error [%d] : Self-test already running error. It occurs when the self-test is already running and " "another has been initiated\r\n", + rslt); + break; + + case BMI2_E_CRT_READY_FOR_DL_FAIL_ABORT: + printf( + "Error [%d] : CRT ready for download fail abort error. It occurs when download in CRT fails due to wrong " "address location\r\n", + rslt); + break; + + case BMI2_E_DL_ERROR: + printf( + "Error [%d] : Download error. It occurs when write length exceeds that of the maximum burst length\r\n", + rslt); + break; + + case BMI2_E_PRECON_ERROR: + printf( + "Error [%d] : Pre-conditional error. It occurs when precondition to start the feature was not " "completed\r\n", + rslt); + break; + + case BMI2_E_ABORT_ERROR: + printf("Error [%d] : Abort error. It occurs when the device was shaken during CRT test\r\n", rslt); + break; + + case BMI2_E_WRITE_CYCLE_ONGOING: + printf( + "Error [%d] : Write cycle ongoing error. It occurs when the write cycle is already running and another " "has been initiated\r\n", + rslt); + break; + + case BMI2_E_ST_NOT_RUNING: + printf( + "Error [%d] : Self-test is not running error. It occurs when self-test running is disabled while it's " "running\r\n", + rslt); + break; + + case BMI2_E_DATA_RDY_INT_FAILED: + printf( + "Error [%d] : Data ready interrupt error. It occurs when the sample count exceeds the FOC sample limit " "and data ready status is not updated\r\n", + rslt); + break; + + case BMI2_E_INVALID_FOC_POSITION: + printf( + "Error [%d] : Invalid FOC position error. It occurs when average FOC data is obtained for the wrong" " axes\r\n", + rslt); + break; + + default: + printf("Error [%d] : Unknown error code\r\n", rslt); + break; + } +} + +/*! + * @brief Deinitializes coines platform + * + * @return void. + */ +void bmi2_coines_deinit(void) +{ + coines_close_comm_intf(COINES_COMM_INTF_USB); +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/common/common.h b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/common/common.h new file mode 100644 index 0000000000..763983da4a --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/common/common.h @@ -0,0 +1,122 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +#ifndef _COMMON_H +#define _COMMON_H + +/*! CPP guard */ +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include "bmi2.h" + +/*! + * @brief Function for reading the sensor's registers through I2C bus. + * + * @param[in] reg_addr : Register address. + * @param[out] reg_data : Pointer to the data buffer to store the read data. + * @param[in] length : No of bytes to read. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval = BMI2_INTF_RET_SUCCESS -> Success + * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info + * + */ +BMI2_INTF_RETURN_TYPE bmi2_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr); + +/*! + * @brief Function for writing the sensor's registers through I2C bus. + * + * @param[in] reg_addr : Register address. + * @param[in] reg_data : Pointer to the data buffer whose value is to be written. + * @param[in] length : No of bytes to write. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval = BMI2_INTF_RET_SUCCESS -> Success + * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info + * + */ +BMI2_INTF_RETURN_TYPE bmi2_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr); + +/*! + * @brief Function for reading the sensor's registers through SPI bus. + * + * @param[in] reg_addr : Register address. + * @param[out] reg_data : Pointer to the data buffer to store the read data. + * @param[in] length : No of bytes to read. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval = BMI2_INTF_RET_SUCCESS -> Success + * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info + * + */ +BMI2_INTF_RETURN_TYPE bmi2_spi_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr); + +/*! + * @brief Function for writing the sensor's registers through SPI bus. + * + * @param[in] reg_addr : Register address. + * @param[in] reg_data : Pointer to the data buffer whose data has to be written. + * @param[in] length : No of bytes to write. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval = BMI2_INTF_RET_SUCCESS -> Success + * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info + * + */ +BMI2_INTF_RETURN_TYPE bmi2_spi_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr); + +/*! + * @brief This function provides the delay for required time (Microsecond) as per the input provided in some of the + * APIs. + * + * @param[in] period_us : The required wait time in microsecond. + * @param[in] intf_ptr : Interface pointer + * + * @return void. + * + */ +void bmi2_delay_us(uint32_t period, void *intf_ptr); + +/*! + * @brief Function to select the interface between SPI and I2C. + * + * @param[in] bma : Structure instance of bmi2_dev + * @param[in] intf : Interface selection parameter + * + * @return Status of execution + * @retval 0 -> Success + * @retval < 0 -> Failure Info + */ +int8_t bmi2_interface_init(struct bmi2_dev *bma, uint8_t intf); + +/*! + * @brief Prints the execution status of the APIs. + * + * @param[in] rslt : Error code returned by the API whose execution status has to be printed. + * + * @return void. + */ +void bmi2_error_codes_print_result(int8_t rslt); + +/*! + * @brief Deinitializes coines platform + * + * @return void. + */ +void bmi2_coines_deinit(void); + +#ifdef __cplusplus +} +#endif /* End of CPP guard */ + +#endif /* _COMMON_H */ diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/component_retrim/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/component_retrim/Makefile new file mode 100644 index 0000000000..61db2e1351 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/component_retrim/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= component_retrim.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_context.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/component_retrim/component_retrim.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/component_retrim/component_retrim.c new file mode 100644 index 0000000000..94ed043772 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/component_retrim/component_retrim.c @@ -0,0 +1,52 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_context.h" +#include "common.h" + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_context. */ + rslt = bmi270_context_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* This API runs the CRT process. */ + rslt = bmi2_do_crt(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Do not move the board while doing CRT. If so, it will throw an abort error. */ + if (rslt == BMI2_OK) + { + printf("CRT successfully completed."); + } + } + + bmi2_coines_deinit(); + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_full_header_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_full_header_mode/Makefile new file mode 100644 index 0000000000..2d328d5f8a --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_full_header_mode/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= fifo_full_header_mode.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_context.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_full_header_mode/fifo_full_header_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_full_header_mode/fifo_full_header_mode.c new file mode 100644 index 0000000000..3a5a810ec7 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_full_header_mode/fifo_full_header_mode.c @@ -0,0 +1,315 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_context.h" +#include "common.h" + +/******************************************************************************/ +/*! Macros */ + +/*! Buffer size allocated to store raw FIFO data. */ +#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) + +/*! Length of data to be read from FIFO. */ +#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) + +/*! Number of accel frames to be extracted from FIFO. */ + +/*! Calculation for frame count: Total frame count = Fifo buffer size(2048)/ Total frames(6 Accel, 6 Gyro and 1 header, + * totaling to 13) which equals to 157. + */ +#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(157) + +/*! Number of gyro frames to be extracted from FIFO. */ +#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(157) + +/*! Macro to read sensortime byte in FIFO. */ +#define SENSORTIME_OVERHEAD_BYTE UINT8_C(3) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + * @param[in] dev : Structure instance of bmi2_dev. + * @return Status of execution. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev); + +/******************************************************************************/ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + uint16_t index = 0; + uint16_t fifo_length = 0; + uint16_t config = 0; + + /* To read sensortime, extra 3 bytes are added to fifo buffer. */ + uint16_t fifo_buffer_size = BMI2_FIFO_RAW_DATA_BUFFER_SIZE + SENSORTIME_OVERHEAD_BYTE; + + /* Variable to get fifo full interrupt status. */ + uint16_t int_status = 0; + + uint16_t accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; + + uint16_t gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; + + /* Variable to store the available frame length count in FIFO */ + uint8_t frame_count; + + int8_t try = 1; + + /* Number of bytes of FIFO data. */ + uint8_t fifo_data[fifo_buffer_size]; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Array of accelerometer frames -> Total bytes = + * 157 * (6 axes + 1 header bytes) = 1099 bytes */ + struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; + + /* Array of gyro frames -> Total bytes = + * 157 * (6 axes + 1 header bytes) = 1099 bytes */ + struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; + + /* Initialize FIFO frame structure. */ + struct bmi2_fifo_frame fifoframe = { 0 }; + + /* Accel and gyro sensor are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_context. */ + rslt = bmi270_context_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Enable accel and gyro sensor. */ + rslt = bmi270_context_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Configuration settings for accel and gyro. */ + rslt = set_accel_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Before setting FIFO, disable the advance power save mode. */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initially disable all configurations in fifo. */ + rslt = bmi2_get_fifo_config(&config, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initially disable all configurations in fifo. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Update FIFO structure. */ + /* Mapping the buffer to store the fifo data. */ + fifoframe.data = fifo_data; + + /* Length of FIFO frame. */ + /* To read sensortime, extra 3 bytes are added to fifo user length. */ + fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH + SENSORTIME_OVERHEAD_BYTE; + + /* Set FIFO configuration by enabling accel, gyro and timestamp. + * NOTE 1: The header mode is enabled by default. + * NOTE 2: By default the FIFO operating mode is in FIFO mode. + * NOTE 3: Sensortime is enabled by default */ + printf("FIFO is configured in header mode\n"); + rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Map FIFO full interrupt. */ + fifoframe.data_int_map = BMI2_FFULL_INT; + rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + while (try <= 10) + { + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if ((rslt == BMI2_OK) && (int_status & BMI2_FFULL_INT_STATUS_MASK)) + { + printf("\nIteration : %d\n", try); + + accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; + + gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; + + rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("\nFIFO data bytes available : %d \n", fifo_length); + printf("\nFIFO data bytes requested : %d \n", fifoframe.length); + + /* Read FIFO data. */ + rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + printf("\nFIFO accel frames requested : %d \n", accel_frame_length); + + /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ + rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); + + printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); + + /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ + rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); + + /* Calculating the frame count from the available bytes in FIFO + * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len + header_byte)) */ + frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH + 1)); + printf("\nAvailable frame count from available bytes: %d\n", frame_count); + + printf("\nExtracted accel frames\n"); + + if (accel_frame_length > frame_count) + { + accel_frame_length = frame_count; + } + + /* Print the parsed accelerometer data from the FIFO buffer. */ + for (index = 0; index < accel_frame_length; index++) + { + printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, + fifo_accel_data[index].y, fifo_accel_data[index].z); + } + + printf("\nExtracted gyro frames\n"); + + if (gyro_frame_length > frame_count) + { + gyro_frame_length = frame_count; + } + + /* Print the parsed gyro data from the FIFO buffer. */ + for (index = 0; index < gyro_frame_length; index++) + { + printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", + index, + fifo_gyro_data[index].x, + fifo_gyro_data[index].y, + fifo_gyro_data[index].z); + } + + /* Print control frames like sensor time and skipped frame count. */ + printf("\nSkipped frame count = %d\n", fifoframe.skipped_frame_count); + + printf("Sensor time = %lu\r\n", fifoframe.sensor_time); + + try++; + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accel and gyro configurations. */ + struct bmi2_sens_config config[2]; + + /* Configure the type of feature. */ + config[0].type = BMI2_ACCEL; + config[1].type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_context_get_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* Accel configuration settings. */ + /* Set Output Data Rate */ + config[0].cfg.acc.odr = BMI2_ACC_ODR_25HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* Gyro configuration settings. */ + /* Set Output Data Rate */ + config[1].cfg.gyr.odr = BMI2_GYR_ODR_50HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set new configurations. */ + rslt = bmi270_context_set_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_full_headerless_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_full_headerless_mode/Makefile new file mode 100644 index 0000000000..8f1f9609dd --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_full_headerless_mode/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= fifo_full_headerless_mode.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_context.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_full_headerless_mode/fifo_full_headerless_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_full_headerless_mode/fifo_full_headerless_mode.c new file mode 100644 index 0000000000..bfc11455a8 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_full_headerless_mode/fifo_full_headerless_mode.c @@ -0,0 +1,304 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_context.h" +#include "common.h" + +/******************************************************************************/ +/*! Macros */ + +/*! Buffer size allocated to store raw FIFO data */ +#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) + +/*! Length of data to be read from FIFO */ +#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) + +/*! Number of accel frames to be extracted from FIFO */ + +/*! Calculation for frame count: Total frame count = Fifo buffer size(2048)/ Total frames(6 Accel, 6 Gyro totaling to + * 12) which equals to 170. + */ +#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(169) + +/*! Number of gyro frames to be extracted from FIFO */ +#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(169) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + * @param[in] dev : Structure instance of bmi2_dev. + * @return Status of execution. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev); + +/******************************************************************************/ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + uint16_t index = 0; + + uint16_t fifo_length = 0; + + uint8_t try = 1; + + /* Variable to get fifo full interrupt status. */ + uint16_t int_status = 0; + + uint16_t accel_frame_length; + + uint16_t gyro_frame_length; + + /* Variable to store the available frame length count in FIFO */ + uint8_t frame_count; + + /* Number of bytes of FIFO data. */ + uint8_t fifo_data[BMI2_FIFO_RAW_DATA_BUFFER_SIZE] = { 0 }; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Array of accelerometer frames -> Total bytes = + * 170 * (6 axes bytes) = 1020 bytes */ + struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; + + /* Array of gyro frames -> Total bytes = + * 170 * (6 axes bytes) = 1020 bytes */ + struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; + + /* Initialize FIFO frame structure. */ + struct bmi2_fifo_frame fifoframe = { 0 }; + + /* Accel and gyro sensor are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_context. */ + rslt = bmi270_context_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Enable accel and gyro sensor. */ + rslt = bmi270_context_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Configuration settings for accel and gyro. */ + rslt = set_accel_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Before setting FIFO, disable the advance power save mode. */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initially disable all configurations in fifo. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Update FIFO structure. */ + /* Mapping the buffer to store the fifo data. */ + fifoframe.data = fifo_data; + + /* Length of FIFO frame. */ + fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH; + + /* Set FIFO configuration by enabling accel, gyro. + * NOTE 1: The header mode is enabled by default. + * NOTE 2: By default the FIFO operating mode is FIFO mode. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("FIFO is configured in headerless mode\n"); + + /* To enable headerless mode, disable the header. */ + if (rslt == BMI2_OK) + { + rslt = bmi2_set_fifo_config(BMI2_FIFO_HEADER_EN, BMI2_DISABLE, &bmi2_dev); + } + + /* Map FIFO full interrupt. */ + fifoframe.data_int_map = BMI2_FFULL_INT; + rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + while (try <= 10) + { + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if ((rslt == BMI2_OK) && (int_status & BMI2_FFULL_INT_STATUS_MASK)) + { + printf("\nIteration : %d\n", try); + + rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; + gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; + + printf("\nFIFO data bytes available : %d \n", fifo_length); + printf("\nFIFO data bytes requested : %d \n", fifoframe.length); + + /* Read FIFO data. */ + rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + printf("\nFIFO accel frames requested : %d \n", accel_frame_length); + + /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ + rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); + + printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); + + /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ + rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); + + /* Calculating the frame count from the available bytes in FIFO + * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len)) */ + frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH)); + printf("\nAvailable frame count from available bytes: %d\n", frame_count); + + printf("\nExtracted accel frames\n"); + + if (accel_frame_length > frame_count) + { + accel_frame_length = frame_count; + } + + /* Print the parsed accelerometer data from the FIFO buffer. */ + for (index = 0; index < accel_frame_length; index++) + { + printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, + fifo_accel_data[index].y, fifo_accel_data[index].z); + } + + printf("\nExtracted gyro frames\n"); + + if (gyro_frame_length > frame_count) + { + gyro_frame_length = frame_count; + } + + /* Print the parsed gyro data from the FIFO buffer. */ + for (index = 0; index < gyro_frame_length; index++) + { + printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", + index, + fifo_gyro_data[index].x, + fifo_gyro_data[index].y, + fifo_gyro_data[index].z); + } + } + + try++; + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accel and gyro configurations. */ + struct bmi2_sens_config config[2]; + + /* Configure the type of feature. */ + config[0].type = BMI2_ACCEL; + config[1].type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_context_get_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* Accel configuration settings. */ + /* Set Output Data Rate */ + config[0].cfg.acc.odr = BMI2_ACC_ODR_100HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* Gyro configuration settings. */ + /* Set Output Data Rate */ + config[1].cfg.gyr.odr = BMI2_GYR_ODR_100HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set new configurations. */ + rslt = bmi270_context_set_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_watermark_header_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_watermark_header_mode/Makefile new file mode 100644 index 0000000000..38c5ce6a59 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_watermark_header_mode/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= fifo_watermark_header_mode.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_context.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_watermark_header_mode/fifo_watermark_header_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_watermark_header_mode/fifo_watermark_header_mode.c new file mode 100644 index 0000000000..59ff8f758a --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_watermark_header_mode/fifo_watermark_header_mode.c @@ -0,0 +1,335 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_context.h" +#include "common.h" + +/******************************************************************************/ +/*! Macros */ + +/*! Buffer size allocated to store raw FIFO data */ +#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) + +/*! Length of data to be read from FIFO */ +#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) + +/*! Number of accel frames to be extracted from FIFO */ + +/*! + * Calculation: + * fifo_watermark_level = 650, accel_frame_len = 6, gyro_frame_len = 6 header_byte = 1. + * fifo_accel_frame_count = (650 / (6 + 6 + 1 )) = 50 frames + * NOTE: Extra frames are read in order to get sensor time + */ +#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(55) + +/*! Number of gyro frames to be extracted from FIFO */ +#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(55) + +/*! Setting a watermark level in FIFO */ +#define BMI270_FIFO_WATERMARK_LEVEL UINT16_C(650) + +/*! Macro to read sensortime byte in FIFO */ +#define SENSORTIME_OVERHEAD_BYTE UINT8_C(3) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + * @param[in] dev : Structure instance of bmi2_dev. + * @return Status of execution. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to store the available frame length count in FIFO */ + uint8_t frame_count; + + /* Variable to index bytes. */ + uint16_t index = 0; + + uint16_t fifo_length = 0; + + uint16_t accel_frame_length; + + uint16_t gyro_frame_length; + + uint8_t try = 1; + + /* To read sensortime, extra 3 bytes are added to fifo buffer */ + uint16_t fifo_buffer_size = BMI2_FIFO_RAW_DATA_BUFFER_SIZE + SENSORTIME_OVERHEAD_BYTE; + + /* Number of bytes of FIFO data. */ + uint8_t fifo_data[fifo_buffer_size]; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Array of accelerometer frames -> Total bytes = + * 55 * (6 axes bytes) = 330 bytes */ + struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; + + /* Array of gyro frames -> Total bytes = + * 55 * (6 axes bytes) = 330 bytes */ + struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; + + /* Initialize FIFO frame structure. */ + struct bmi2_fifo_frame fifoframe = { 0 }; + + /* Accel and gyro sensor are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; + + /* Variable to get fifo water-mark interrupt status. */ + uint16_t int_status = 0; + + uint16_t watermark = 0; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_context. */ + rslt = bmi270_context_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Enable accel and gyro sensor. */ + rslt = bmi270_context_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Configuration settings for accel and gyro. */ + rslt = set_accel_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Before setting FIFO, disable the advance power save mode. */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initially disable all configurations in fifo. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Update FIFO structure. */ + /* Mapping the buffer to store the fifo data. */ + fifoframe.data = fifo_data; + + /* Length of FIFO frame. */ + /* To read sensortime, extra 3 bytes are added to fifo user length. */ + fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH + SENSORTIME_OVERHEAD_BYTE; + + /* Set FIFO configuration by enabling accel, gyro and timestamp. + * NOTE 1: The header mode is enabled by default. + * NOTE 2: By default the FIFO operating mode is FIFO mode. + * NOTE 3: Sensortime is enabled by default */ + printf("FIFO is configured in header mode\n"); + rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* FIFO water-mark interrupt is enabled. */ + fifoframe.data_int_map = BMI2_FWM_INT; + + /* Map water-mark interrupt to the required interrupt pin. */ + rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Set water-mark level. */ + fifoframe.wm_lvl = BMI270_FIFO_WATERMARK_LEVEL; + + /* Set the water-mark level if water-mark interrupt is mapped. */ + rslt = bmi2_set_fifo_wm(fifoframe.wm_lvl, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + rslt = bmi2_get_fifo_wm(&watermark, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + printf("\nFIFO watermark level is %d\n", watermark); + + while (try <= 10) + { + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the status of FIFO watermark interrupt. */ + if ((rslt == BMI2_OK) && (int_status & BMI2_FWM_INT_STATUS_MASK)) + { + printf("\nIteration : %d\n", try); + + printf("\nWatermark interrupt occurred\n"); + + rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; + gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; + + printf("\nFIFO data bytes available : %d \n", fifo_length); + printf("\nFIFO data bytes requested : %d \n", fifoframe.length); + + /* Read FIFO data. */ + rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + printf("\nFIFO accel frames requested : %d \n", accel_frame_length); + + /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ + rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); + + printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); + + /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ + rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); + + /* Calculating the frame count from the available bytes in FIFO + * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len + header_byte)) */ + frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH + 1)); + printf("\nAvailable frame count from available bytes: %d\n", frame_count); + + printf("\nExracted accel frames\n"); + + if (accel_frame_length > frame_count) + { + accel_frame_length = frame_count; + } + + /* Print the parsed accelerometer data from the FIFO buffer. */ + for (index = 0; index < accel_frame_length; index++) + { + printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, + fifo_accel_data[index].y, fifo_accel_data[index].z); + } + + printf("\nExtracted gyro frames\n"); + + if (gyro_frame_length > frame_count) + { + gyro_frame_length = frame_count; + } + + /* Print the parsed gyro data from the FIFO buffer. */ + for (index = 0; index < gyro_frame_length; index++) + { + printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", + index, + fifo_gyro_data[index].x, + fifo_gyro_data[index].y, + fifo_gyro_data[index].z); + } + + /* Print control frames like sensor time and skipped frame count. */ + printf("Skipped frame count = %d\n", fifoframe.skipped_frame_count); + printf("Sensor time = %lu\r\n", fifoframe.sensor_time); + + try++; + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accel and gyro configurations. */ + struct bmi2_sens_config config[2]; + + /* Configure the type of feature. */ + config[0].type = BMI2_ACCEL; + config[1].type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_context_get_sensor_config(config, 2, dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* Accel configuration settings. */ + /* Set Output Data Rate */ + config[0].cfg.acc.odr = BMI2_ACC_ODR_25HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* Gyro configuration settings. */ + /* Set Output Data Rate */ + config[1].cfg.gyr.odr = BMI2_GYR_ODR_25HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set new configurations. */ + rslt = bmi270_context_set_sensor_config(config, 2, dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_watermark_headerless_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_watermark_headerless_mode/Makefile new file mode 100644 index 0000000000..9d5e86e564 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_watermark_headerless_mode/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= fifo_watermark_headerless_mode.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_context.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c new file mode 100644 index 0000000000..878fe43bb0 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c @@ -0,0 +1,331 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_context.h" +#include "common.h" + +/******************************************************************************/ +/*! Macros */ + +/*! Buffer size allocated to store raw FIFO data */ +#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) + +/*! Length of data to be read from FIFO */ +#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) + +/*! Number of accel frames to be extracted from FIFO */ + +/*! + * Calculation: + * fifo_watermark_level = 650, accel_frame_len = 6, gyro_frame_len = 6. + * fifo_accel_frame_count = (650 / (6 + 6 )) = 54 frames + */ +#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(55) + +/*! Number of gyro frames to be extracted from FIFO */ +#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(55) + +/*! Setting a watermark level in FIFO */ +#define BMI270_FIFO_WATERMARK_LEVEL UINT16_C(650) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + * @param[in] dev : Structure instance of bmi2_dev. + * @return Status of execution. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to index bytes. */ + uint16_t index = 0; + + /* Variable to store the available frame length count in FIFO */ + uint8_t frame_count; + + uint16_t fifo_length = 0; + + uint16_t accel_frame_length; + + uint16_t gyro_frame_length; + + uint8_t try = 1; + + /* Number of bytes of FIFO data. */ + uint8_t fifo_data[BMI2_FIFO_RAW_DATA_BUFFER_SIZE] = { 0 }; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Array of accelerometer frames -> Total bytes = + * 50 * (6 axes bytes) = 300 bytes */ + struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; + + /* Array of gyro frames -> Total bytes = + * 50 * (6 axes bytes) = 300 bytes */ + struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; + + /* Initialize FIFO frame structure. */ + struct bmi2_fifo_frame fifoframe = { 0 }; + + /* Accel and gyro sensor are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; + + /* Variable to get fifo water-mark interrupt status. */ + uint16_t int_status = 0; + + uint16_t watermark = 0; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_context. */ + rslt = bmi270_context_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Enable accel and gyro sensor. */ + rslt = bmi270_context_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Configuration settings for accel and gyro. */ + rslt = set_accel_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Before setting FIFO, disable the advance power save mode. */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initially disable all configurations in fifo. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Update FIFO structure. */ + /* Mapping the buffer to store the fifo data. */ + fifoframe.data = fifo_data; + + /* Length of FIFO frame. */ + fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH; + + /* Set FIFO configuration by enabling accel, gyro. + * NOTE 1: The header mode is enabled by default. + * NOTE 2: By default the FIFO operating mode is FIFO mode. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("FIFO is configured in headerless mode\n"); + + /* To enable headerless mode, disable the header. */ + if (rslt == BMI2_OK) + { + rslt = bmi2_set_fifo_config(BMI2_FIFO_HEADER_EN, BMI2_DISABLE, &bmi2_dev); + } + + /* FIFO water-mark interrupt is enabled. */ + fifoframe.data_int_map = BMI2_FWM_INT; + + /* Map water-mark interrupt to the required interrupt pin. */ + rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Set water-mark level. */ + fifoframe.wm_lvl = BMI270_FIFO_WATERMARK_LEVEL; + + fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH; + + /* Set the water-mark level if water-mark interrupt is mapped. */ + rslt = bmi2_set_fifo_wm(fifoframe.wm_lvl, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + rslt = bmi2_get_fifo_wm(&watermark, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + printf("\nFIFO watermark level is %d\n", watermark); + + while (try <= 10) + { + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the status of FIFO watermark interrupt. */ + if ((rslt == BMI2_OK) && (int_status & BMI2_FWM_INT_STATUS_MASK)) + { + printf("\nIteration : %d\n", try); + + printf("\nWatermark interrupt occurred\n"); + + rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; + gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; + + printf("\nFIFO data bytes available : %d \n", fifo_length); + printf("\nFIFO data bytes requested : %d \n", fifoframe.length); + + /* Read FIFO data. */ + rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + printf("\nFIFO accel frames requested : %d \n", accel_frame_length); + + /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ + rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); + + printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); + + /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ + rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); + + /* Calculating the frame count from the available bytes in FIFO + * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len)) */ + frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH)); + printf("\nAvailable frame count from available bytes: %d\n", frame_count); + + printf("\nExracted accel frames\n"); + + if (accel_frame_length > frame_count) + { + accel_frame_length = frame_count; + } + + /* Print the parsed accelerometer data from the FIFO buffer. */ + for (index = 0; index < accel_frame_length; index++) + { + printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, + fifo_accel_data[index].y, fifo_accel_data[index].z); + } + + printf("\nExtracted gyro frames\n"); + + if (gyro_frame_length > frame_count) + { + gyro_frame_length = frame_count; + } + + /* Print the parsed gyro data from the FIFO buffer. */ + for (index = 0; index < gyro_frame_length; index++) + { + printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", + index, + fifo_gyro_data[index].x, + fifo_gyro_data[index].y, + fifo_gyro_data[index].z); + } + } + + try++; + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accel and gyro configurations. */ + struct bmi2_sens_config config[2]; + + /* Configure the type of feature. */ + config[0].type = BMI2_ACCEL; + config[1].type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_context_get_sensor_config(config, 2, dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameter according to their requirement. */ + /* Accel configuration settings. */ + /* Set Output Data Rate */ + config[0].cfg.acc.odr = BMI2_ACC_ODR_100HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* Gyro configuration settings. */ + /* Set Output Data Rate */ + config[1].cfg.gyr.odr = BMI2_GYR_ODR_100HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set new configurations. */ + rslt = bmi270_context_set_sensor_config(config, 2, dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/gyro/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/gyro/Makefile new file mode 100644 index 0000000000..135fe5ba3f --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/gyro/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= gyro.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_context.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/gyro/gyro.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/gyro/gyro.c new file mode 100644 index 0000000000..0a057e4647 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/gyro/gyro.c @@ -0,0 +1,195 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_context.h" +#include "common.h" + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for gyro. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t set_gyro_config(struct bmi2_dev *dev); + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + * + * @param[in] val : LSB from each axis. + * @param[in] dps : Degree per second. + * @param[in] bit_width : Resolution for gyro. + * + * @return Degree per second. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to define limit to print gyro data. */ + uint8_t limit = 10; + + uint8_t indx = 0; + + float x = 0, y = 0, z = 0; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Create an instance of sensor data structure. */ + struct bmi2_sensor_data sensor_data = { 0 }; + + /* Assign gyro sensor to variable. */ + uint8_t sens_list = BMI2_GYRO; + + /* Initialize the interrupt status of gyro. */ + uint16_t int_status = 0; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_context. */ + rslt = bmi270_context_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the selected sensors. */ + rslt = bmi270_context_sensor_enable(&sens_list, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Gyro configuration settings. */ + rslt = set_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Select gyro sensor. */ + sensor_data.type = BMI2_GYRO; + printf("Gyro and DPS data\n"); + printf("Gyro data collected at Range 2000 dps with 16-bit resolution\n"); + + /* Loop to print gyro data when interrupt occurs. */ + while (indx <= limit) + { + /* To get the data ready interrupt status of gyro. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the data ready interrupt status and print the status for 10 samples. */ + if (int_status & BMI2_GYR_DRDY_INT_MASK) + { + /* Get gyro data for x, y and z axis. */ + rslt = bmi270_context_get_sensor_data(&sensor_data, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("\nGyr_X = %d\t", sensor_data.sens_data.gyr.x); + printf("Gyr_Y = %d\t", sensor_data.sens_data.gyr.y); + printf("Gyr_Z = %d\r\n", sensor_data.sens_data.gyr.z); + + /* Converting lsb to degree per second for 16 bit gyro at 2000dps range. */ + x = lsb_to_dps(sensor_data.sens_data.gyr.x, 2000, bmi2_dev.resolution); + y = lsb_to_dps(sensor_data.sens_data.gyr.y, 2000, bmi2_dev.resolution); + z = lsb_to_dps(sensor_data.sens_data.gyr.z, 2000, bmi2_dev.resolution); + + /* Print the data in dps. */ + printf("Gyr_DPS_X = %4.2f, Gyr_DPS_Y = %4.2f, Gyr_DPS_Z = %4.2f\n", x, y, z); + + indx++; + } + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for gyro. + */ +static int8_t set_gyro_config(struct bmi2_dev *dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define the type of sensor and its configurations. */ + struct bmi2_sens_config config; + + /* Configure the type of feature. */ + config.type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_context_get_sensor_config(&config, 1, dev); + bmi2_error_codes_print_result(rslt); + + /* Map data ready interrupt to interrupt pin. */ + rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT2, dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* The user can change the following configuration parameters according to their requirement. */ + /* Set Output Data Rate */ + config.cfg.gyr.odr = BMI2_GYR_ODR_200HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config.cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config.cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config.cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config.cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set the gyro configurations. */ + rslt = bmi270_context_set_sensor_config(&config, 1, dev); + } + + return rslt; +} + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (dps / ((half_scale) + BMI2_GYR_RANGE_2000)) * (val); +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/step_counter/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/step_counter/Makefile new file mode 100644 index 0000000000..1e8be48cdc --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/step_counter/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= step_counter.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_context.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/step_counter/step_counter.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/step_counter/step_counter.c new file mode 100644 index 0000000000..2513b028ac --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/step_counter/step_counter.c @@ -0,0 +1,146 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_context.h" +#include "common.h" + +/******************************************************************************/ +/*! Static function declaration */ + +/*! + * @brief This internal API is used to set configurations for step counter. + * + * @param[in] bmi2_dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t set_feature_config(struct bmi2_dev *bmi2_dev); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Structure to define type of sensor and their respective data. */ + struct bmi2_sensor_data sensor_data; + + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Accel sensor and step counter feature are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_STEP_COUNTER }; + + /* Variable to get step counter interrupt status. */ + uint16_t int_status = 0; + + /* Select features and their pins to be mapped to. */ + struct bmi2_sens_int_config sens_int = { .type = BMI2_STEP_COUNTER, .hw_int_pin = BMI2_INT2 }; + + /* Type of sensor to get step counter data. */ + sensor_data.type = BMI2_STEP_COUNTER; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_context. */ + rslt = bmi270_context_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the selected sensor. */ + rslt = bmi270_context_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Set the feature configuration for step counter. */ + rslt = set_feature_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("Step counter watermark level set to 1 (20 steps)\n"); + + if (rslt == BMI2_OK) + { + /* Map the step counter feature interrupt. */ + rslt = bmi270_context_map_feat_int(&sens_int, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Move the board in steps for 20 times to get step counter interrupt. */ + printf("Move the board in steps\n"); + + /* Loop to get number of steps counted. */ + do + { + /* To get the interrupt status of the step counter. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the interrupt status of the step counter. */ + if (int_status & BMI270_CONTEXT_STEP_CNT_STATUS_MASK) + { + printf("Step counter interrupt occurred when watermark level (20 steps) is reached\n"); + + /* Get step counter output. */ + rslt = bmi270_context_get_sensor_data(&sensor_data, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Print the step counter output. */ + printf("No of steps counted = %ld", sensor_data.sens_data.step_counter_output); + break; + } + } while (rslt == BMI2_OK); + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for step counter. + */ +static int8_t set_feature_config(struct bmi2_dev *bmi2_dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define the type of sensor and its configurations. */ + struct bmi2_sens_config config; + + /* Configure the type of sensor. */ + config.type = BMI2_STEP_COUNTER; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_context_get_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Setting water-mark level to 1 for step counter to get interrupt after 20 step counts. Every 20 steps once + * output triggers. */ + config.cfg.step_counter.watermark_level = 1; + + /* Set new configuration. */ + rslt = bmi270_context_set_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/accel/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/accel/Makefile new file mode 100644 index 0000000000..d4cc64caf4 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/accel/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= accel.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_hc.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/accel/accel.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/accel/accel.c new file mode 100644 index 0000000000..553db37724 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/accel/accel.c @@ -0,0 +1,201 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_hc.h" +#include "common.h" + +/******************************************************************************/ +/*! Macro definition */ + +/*! Earth's gravity in m/s^2 */ +#define GRAVITY_EARTH (9.80665f) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t set_accel_config(struct bmi2_dev *bmi2_dev); + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + * + * @param[in] val : LSB from each axis. + * @param[in] g_range : Gravity range. + * @param[in] bit_width : Resolution for accel. + * + * @return Gravity. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to define limit to print accel data. */ + uint8_t limit = 20; + + /* Assign accel sensor to variable. */ + uint8_t sensor_list = BMI2_ACCEL; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Create an instance of sensor data structure. */ + struct bmi2_sensor_data sensor_data = { 0 }; + + /* Initialize the interrupt status of accel. */ + uint16_t int_status = 0; + + uint8_t indx = 0; + float x = 0, y = 0, z = 0; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_hc. */ + rslt = bmi270_hc_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the accel sensor. */ + rslt = bmi270_hc_sensor_enable(&sensor_list, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Accel configuration settings. */ + rslt = set_accel_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Assign accel sensor. */ + sensor_data.type = BMI2_ACCEL; + printf("Accel and m/s2 data \n"); + printf("Accel data collected at 2G Range with 16-bit resolution\n"); + + /* Loop to print the accel data when interrupt occurs. */ + while (indx <= limit) + { + /* To get the status of accel data ready interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the accel data ready interrupt status and print the status for 10 samples. */ + if (int_status & BMI2_ACC_DRDY_INT_MASK) + { + /* Get accelerometer data for x, y and z axis. */ + rslt = bmi270_hc_get_sensor_data(&sensor_data, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + printf("\nAcc_X = %d\t", sensor_data.sens_data.acc.x); + printf("Acc_Y = %d\t", sensor_data.sens_data.acc.y); + printf("Acc_Z = %d\r\n", sensor_data.sens_data.acc.z); + + /* Converting lsb to meter per second squared for 16 bit accelerometer at 2G range. */ + x = lsb_to_mps2(sensor_data.sens_data.acc.x, 2, bmi2_dev.resolution); + y = lsb_to_mps2(sensor_data.sens_data.acc.y, 2, bmi2_dev.resolution); + z = lsb_to_mps2(sensor_data.sens_data.acc.z, 2, bmi2_dev.resolution); + + /* Print the data in m/s2. */ + printf("\nAcc_ms2_X = %4.2f, Acc_ms2_Y = %4.2f, Acc_ms2_Z = %4.2f\n", x, y, z); + + indx++; + } + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel. + */ +static int8_t set_accel_config(struct bmi2_dev *bmi2_dev) +{ + + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accelerometer configuration. */ + struct bmi2_sens_config config; + + /* Configure the type of feature. */ + config.type = BMI2_ACCEL; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_hc_get_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* Set Output Data Rate */ + config.cfg.acc.odr = BMI2_ACC_ODR_200HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config.cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples. + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config.cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config.cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set the accel configurations. */ + rslt = bmi270_hc_set_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Map data ready interrupt to interrupt pin. */ + rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (GRAVITY_EARTH * val * g_range) / half_scale; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/accel_gyro/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/accel_gyro/Makefile new file mode 100644 index 0000000000..19eb609eae --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/accel_gyro/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= accel_gyro.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_hc.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/accel_gyro/accel_gyro.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/accel_gyro/accel_gyro.c new file mode 100644 index 0000000000..c92a1b82d5 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/accel_gyro/accel_gyro.c @@ -0,0 +1,268 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_hc.h" +#include "common.h" + +/******************************************************************************/ +/*! Macro definition */ + +/*! Earth's gravity in m/s^2 */ +#define GRAVITY_EARTH (9.80665f) + +/*! Macros to select the sensors */ +#define ACCEL UINT8_C(0x00) +#define GYRO UINT8_C(0x01) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev); + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + * + * @param[in] val : LSB from each axis. + * @param[in] g_range : Gravity range. + * @param[in] bit_width : Resolution for accel. + * + * @return Gravity. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width); + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + * + * @param[in] val : LSB from each axis. + * @param[in] dps : Degree per second. + * @param[in] bit_width : Resolution for gyro. + * + * @return Degree per second. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to define limit to print accel data. */ + uint8_t limit = 10; + + /* Assign accel and gyro sensor to variable. */ + uint8_t sensor_list[2] = { BMI2_ACCEL, BMI2_GYRO }; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Create an instance of sensor data structure. */ + struct bmi2_sensor_data sensor_data[2] = { { 0 } }; + + /* Initialize the interrupt status of accel and gyro. */ + uint16_t int_status = 0; + + uint8_t indx = 1; + + float x = 0, y = 0, z = 0; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_hc. */ + rslt = bmi270_hc_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the accel and gyro sensor. */ + rslt = bmi270_hc_sensor_enable(sensor_list, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Accel and gyro configuration settings. */ + rslt = set_accel_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Assign accel and gyro sensor. */ + sensor_data[ACCEL].type = BMI2_ACCEL; + sensor_data[GYRO].type = BMI2_GYRO; + + /* Loop to print accel and gyro data when interrupt occurs. */ + while (indx <= limit) + { + /* To get the data ready interrupt status of accel and gyro. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the data ready interrupt status and print the status for 10 samples. */ + if ((int_status & BMI2_ACC_DRDY_INT_MASK) && (int_status & BMI2_GYR_DRDY_INT_MASK)) + { + /* Get accel and gyro data for x, y and z axis. */ + rslt = bmi270_hc_get_sensor_data(sensor_data, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("\n******* Accel(Raw and m/s2) Gyro(Raw and dps) data : %d *******\n", indx); + + printf("\nAcc_x = %d\t", sensor_data[ACCEL].sens_data.acc.x); + printf("Acc_y = %d\t", sensor_data[ACCEL].sens_data.acc.y); + printf("Acc_z = %d", sensor_data[ACCEL].sens_data.acc.z); + + /* Converting lsb to meter per second squared for 16 bit accelerometer at 2G range. */ + x = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.x, 2, bmi2_dev.resolution); + y = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.y, 2, bmi2_dev.resolution); + z = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.z, 2, bmi2_dev.resolution); + + /* Print the data in m/s2. */ + printf("\nAcc_ms2_X = %4.2f, Acc_ms2_Y = %4.2f, Acc_ms2_Z = %4.2f\n", x, y, z); + + printf("\nGyr_X = %d\t", sensor_data[GYRO].sens_data.gyr.x); + printf("Gyr_Y = %d\t", sensor_data[GYRO].sens_data.gyr.y); + printf("Gyr_Z = %d\n", sensor_data[GYRO].sens_data.gyr.z); + + /* Converting lsb to degree per second for 16 bit gyro at 2000dps range. */ + x = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.x, 2000, bmi2_dev.resolution); + y = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.y, 2000, bmi2_dev.resolution); + z = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.z, 2000, bmi2_dev.resolution); + + /* Print the data in dps. */ + printf("Gyro_DPS_X = %4.2f, Gyro_DPS_Y = %4.2f, Gyro_DPS_Z = %4.2f\n", x, y, z); + + indx++; + } + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accelerometer and gyro configuration. */ + struct bmi2_sens_config config[2]; + + /* Configure the type of feature. */ + config[ACCEL].type = BMI2_ACCEL; + config[GYRO].type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_hc_get_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Map data ready interrupt to interrupt pin. */ + rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* Set Output Data Rate */ + config[ACCEL].cfg.acc.odr = BMI2_ACC_ODR_200HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config[ACCEL].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples. + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config[ACCEL].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config[ACCEL].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* The user can change the following configuration parameters according to their requirement. */ + /* Set Output Data Rate */ + config[GYRO].cfg.gyr.odr = BMI2_GYR_ODR_200HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config[GYRO].cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config[GYRO].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config[GYRO].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config[GYRO].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set the accel and gyro configurations. */ + rslt = bmi270_hc_set_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (GRAVITY_EARTH * val * g_range) / half_scale; +} + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (dps / ((half_scale) + BMI2_GYR_RANGE_2000)) * (val); +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/activity_recognition/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/activity_recognition/Makefile new file mode 100644 index 0000000000..ae036b47c1 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/activity_recognition/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= activity_recognition.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_hc.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/activity_recognition/activity_recognition.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/activity_recognition/activity_recognition.c new file mode 100644 index 0000000000..11750ca538 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/activity_recognition/activity_recognition.c @@ -0,0 +1,154 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_hc.h" +#include "common.h" + +/******************************************************************************/ +/*! Static function declaration */ + +/*! + * @brief This internal API is used to set configurations for step counter. + * + * @param[in] bmi2_dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t set_config(struct bmi2_dev *bmi2_dev); + +/******************************************************************************/ +/*! Functions */ +/* This function starts the execution of program. */ +int main(void) +{ + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to define index for FIFO data */ + uint8_t count; + + /* Various Activity recognitions are listed in array. */ + const char *activity_reg_output[6] = { "OTHERS", "STILL", "WALKING", "RUNNING", "ON_BICYCLE", "IN_VEHICLE" }; + + /* Accel sensor and step activity feature are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_ACTIVITY_RECOGNITION }; + + /* Array to define FIFO data */ + uint8_t fifo_data[516] = { 0 }; + + /* Variable to store number of activity frames */ + uint16_t act_frames; + + /* Structure to define a FIFO */ + struct bmi2_fifo_frame fifoframe = { 0 }; + + /* Structure to define output for activity recognition */ + struct bmi2_act_recog_output act_recog_data[5] = { { 0 } }; + + uint16_t fifo_length; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_hc. */ + rslt = bmi270_hc_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the selected sensor. */ + rslt = bmi270_hc_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + rslt = set_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Update FIFO structure */ + fifoframe.data = fifo_data; + fifoframe.length = 516; + + rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); + printf("Fifo length = %d \n", fifo_length); + + /* Read FIFO data */ + rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + while (rslt != BMI2_W_FIFO_EMPTY) + { + /* Provide the number of frames to be read */ + act_frames = 5; + + printf("Requested FIFO data frames : %d\n", act_frames); + + /* Get the activity output */ + rslt = bmi270_hc_get_act_recog_output(act_recog_data, &act_frames, &fifoframe, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("Parsed FIFO data frames : %d\r\n", act_frames); + + for (count = 0; count < act_frames; count++) + { + printf( + "Activity Recognition output[%d]:Sensor time: %lu\t Previous activity: %s\t current: %s\n", + count, + act_recog_data[count].time_stamp, + activity_reg_output[act_recog_data[count].prev_act], + activity_reg_output[act_recog_data[count].curr_act]); + } + } + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +static int8_t set_config(struct bmi2_dev *bmi2_dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + uint8_t temp_data[2]; + + /* Disable advanced power save */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Reset FIFO */ + rslt = bmi2_set_command_register(BMI2_FIFO_FLUSH_CMD, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("Move the board for activity recognition for 30 sec :\n"); + bmi2_dev->delay_us(30000000, bmi2_dev->intf_ptr); + + if (rslt == BMI2_OK) + { + rslt = bmi2_get_regs(BMI2_FIFO_CONFIG_0_ADDR, temp_data, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/common/common.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/common/common.c new file mode 100644 index 0000000000..afff37bc84 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/common/common.c @@ -0,0 +1,372 @@ +/** + * Copyright (C) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + */ + +#include +#include +#include + +#include "coines.h" +#include "bmi2_defs.h" + +/******************************************************************************/ +/*! Macro definitions */ +#define BMI2XY_SHUTTLE_ID UINT16_C(0x1B8) + +/*! Macro that defines read write length */ +#define READ_WRITE_LEN UINT8_C(46) + +/******************************************************************************/ +/*! Static variable definition */ + +/*! Variable that holds the I2C device address or SPI chip selection */ +static uint8_t dev_addr; + +/******************************************************************************/ +/*! User interface functions */ + +/*! + * I2C read function map to COINES platform + */ +BMI2_INTF_RETURN_TYPE bmi2_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr) +{ + uint8_t dev_addr = *(uint8_t*)intf_ptr; + + return coines_read_i2c(dev_addr, reg_addr, reg_data, (uint16_t)len); +} + +/*! + * I2C write function map to COINES platform + */ +BMI2_INTF_RETURN_TYPE bmi2_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr) +{ + uint8_t dev_addr = *(uint8_t*)intf_ptr; + + return coines_write_i2c(dev_addr, reg_addr, (uint8_t *)reg_data, (uint16_t)len); +} + +/*! + * SPI read function map to COINES platform + */ +BMI2_INTF_RETURN_TYPE bmi2_spi_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr) +{ + uint8_t dev_addr = *(uint8_t*)intf_ptr; + + return coines_read_spi(dev_addr, reg_addr, reg_data, (uint16_t)len); +} + +/*! + * SPI write function map to COINES platform + */ +BMI2_INTF_RETURN_TYPE bmi2_spi_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr) +{ + uint8_t dev_addr = *(uint8_t*)intf_ptr; + + return coines_write_spi(dev_addr, reg_addr, (uint8_t *)reg_data, (uint16_t)len); +} + +/*! + * Delay function map to COINES platform + */ +void bmi2_delay_us(uint32_t period, void *intf_ptr) +{ + coines_delay_usec(period); +} + +/*! + * @brief Function to select the interface between SPI and I2C. + * Also to initialize coines platform + */ +int8_t bmi2_interface_init(struct bmi2_dev *bmi, uint8_t intf) +{ + int8_t rslt = BMI2_OK; + struct coines_board_info board_info; + + if (bmi != NULL) + { + int16_t result = coines_open_comm_intf(COINES_COMM_INTF_USB); + if (result < COINES_SUCCESS) + { + printf( + "\n Unable to connect with Application Board ! \n" " 1. Check if the board is connected and powered on. \n" " 2. Check if Application Board USB driver is installed. \n" + " 3. Check if board is in use by another application. (Insufficient permissions to access USB) \n"); + exit(result); + } + + result = coines_get_board_info(&board_info); + +#if defined(PC) + setbuf(stdout, NULL); +#endif + + if (result == COINES_SUCCESS) + { + if ((board_info.shuttle_id != BMI2XY_SHUTTLE_ID)) + { + printf("! Warning invalid sensor shuttle \n ," "This application will not support this sensor \n"); + exit(COINES_E_FAILURE); + } + } + + coines_set_shuttleboard_vdd_vddio_config(0, 0); + coines_delay_msec(100); + + /* Bus configuration : I2C */ + if (intf == BMI2_I2C_INTF) + { + printf("I2C Interface \n"); + + /* To initialize the user I2C function */ + dev_addr = BMI2_I2C_PRIM_ADDR; + bmi->intf = BMI2_I2C_INTF; + bmi->read = bmi2_i2c_read; + bmi->write = bmi2_i2c_write; + coines_config_i2c_bus(COINES_I2C_BUS_0, COINES_I2C_FAST_MODE); + } + /* Bus configuration : SPI */ + else if (intf == BMI2_SPI_INTF) + { + printf("SPI Interface \n"); + + /* To initialize the user SPI function */ + dev_addr = COINES_SHUTTLE_PIN_7; + bmi->intf = BMI2_SPI_INTF; + bmi->read = bmi2_spi_read; + bmi->write = bmi2_spi_write; + coines_config_spi_bus(COINES_SPI_BUS_0, COINES_SPI_SPEED_5_MHZ, COINES_SPI_MODE3); + coines_set_pin_config(COINES_SHUTTLE_PIN_7, COINES_PIN_DIRECTION_OUT, COINES_PIN_VALUE_HIGH); + } + + /* Assign device address to interface pointer */ + bmi->intf_ptr = &dev_addr; + + /* Configure delay in microseconds */ + bmi->delay_us = bmi2_delay_us; + + /* Configure max read/write length (in bytes) ( Supported length depends on target machine) */ + bmi->read_write_len = READ_WRITE_LEN; + + /* Assign to NULL to load the default config file. */ + bmi->config_file_ptr = NULL; + + coines_delay_usec(10000); + + coines_set_shuttleboard_vdd_vddio_config(3300, 3300); + + coines_delay_usec(10000); + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; + +} + +/*! + * @brief Prints the execution status of the APIs. + */ +void bmi2_error_codes_print_result(int8_t rslt) +{ + switch (rslt) + { + case BMI2_OK: + + /* Do nothing */ + break; + + case BMI2_W_FIFO_EMPTY: + printf("Warning [%d] : FIFO empty\r\n", rslt); + break; + case BMI2_W_PARTIAL_READ: + printf("Warning [%d] : FIFO partial read\r\n", rslt); + break; + case BMI2_E_NULL_PTR: + printf( + "Error [%d] : Null pointer error. It occurs when the user tries to assign value (not address) to a pointer," " which has been initialized to NULL.\r\n", + rslt); + break; + + case BMI2_E_COM_FAIL: + printf( + "Error [%d] : Communication failure error. It occurs due to read/write operation failure and also due " "to power failure during communication\r\n", + rslt); + break; + + case BMI2_E_DEV_NOT_FOUND: + printf("Error [%d] : Device not found error. It occurs when the device chip id is incorrectly read\r\n", + rslt); + break; + + case BMI2_E_INVALID_SENSOR: + printf( + "Error [%d] : Invalid sensor error. It occurs when there is a mismatch in the requested feature with the " "available one\r\n", + rslt); + break; + + case BMI2_E_SELF_TEST_FAIL: + printf( + "Error [%d] : Self-test failed error. It occurs when the validation of accel self-test data is " "not satisfied\r\n", + rslt); + break; + + case BMI2_E_INVALID_INT_PIN: + printf( + "Error [%d] : Invalid interrupt pin error. It occurs when the user tries to configure interrupt pins " "apart from INT1 and INT2\r\n", + rslt); + break; + + case BMI2_E_OUT_OF_RANGE: + printf( + "Error [%d] : Out of range error. It occurs when the data exceeds from filtered or unfiltered data from " "fifo and also when the range exceeds the maximum range for accel and gyro while performing FOC\r\n", + rslt); + break; + + case BMI2_E_ACC_INVALID_CFG: + printf( + "Error [%d] : Invalid Accel configuration error. It occurs when there is an error in accel configuration" " register which could be one among range, BW or filter performance in reg address 0x40\r\n", + rslt); + break; + + case BMI2_E_GYRO_INVALID_CFG: + printf( + "Error [%d] : Invalid Gyro configuration error. It occurs when there is a error in gyro configuration" "register which could be one among range, BW or filter performance in reg address 0x42\r\n", + rslt); + break; + + case BMI2_E_ACC_GYR_INVALID_CFG: + printf( + "Error [%d] : Invalid Accel-Gyro configuration error. It occurs when there is a error in accel and gyro" " configuration registers which could be one among range, BW or filter performance in reg address 0x40 " "and 0x42\r\n", + rslt); + break; + + case BMI2_E_CONFIG_LOAD: + printf( + "Error [%d] : Configuration load error. It occurs when failure observed while loading the configuration " "into the sensor\r\n", + rslt); + break; + + case BMI2_E_INVALID_PAGE: + printf( + "Error [%d] : Invalid page error. It occurs due to failure in writing the correct feature configuration " "from selected page\r\n", + rslt); + break; + + case BMI2_E_SET_APS_FAIL: + printf( + "Error [%d] : APS failure error. It occurs due to failure in write of advance power mode configuration " "register\r\n", + rslt); + break; + + case BMI2_E_AUX_INVALID_CFG: + printf( + "Error [%d] : Invalid AUX configuration error. It occurs when the auxiliary interface settings are not " "enabled properly\r\n", + rslt); + break; + + case BMI2_E_AUX_BUSY: + printf( + "Error [%d] : AUX busy error. It occurs when the auxiliary interface buses are engaged while configuring" " the AUX\r\n", + rslt); + break; + + case BMI2_E_REMAP_ERROR: + printf( + "Error [%d] : Remap error. It occurs due to failure in assigning the remap axes data for all the axes " "after change in axis position\r\n", + rslt); + break; + + case BMI2_E_GYR_USER_GAIN_UPD_FAIL: + printf( + "Error [%d] : Gyro user gain update fail error. It occurs when the reading of user gain update status " "fails\r\n", + rslt); + break; + + case BMI2_E_SELF_TEST_NOT_DONE: + printf( + "Error [%d] : Self-test not done error. It occurs when the self-test process is ongoing or not " "completed\r\n", + rslt); + break; + + case BMI2_E_INVALID_INPUT: + printf("Error [%d] : Invalid input error. It occurs when the sensor input validity fails\r\n", rslt); + break; + + case BMI2_E_INVALID_STATUS: + printf("Error [%d] : Invalid status error. It occurs when the feature/sensor validity fails\r\n", rslt); + break; + + case BMI2_E_CRT_ERROR: + printf("Error [%d] : CRT error. It occurs when the CRT test has failed\r\n", rslt); + break; + + case BMI2_E_ST_ALREADY_RUNNING: + printf( + "Error [%d] : Self-test already running error. It occurs when the self-test is already running and " "another has been initiated\r\n", + rslt); + break; + + case BMI2_E_CRT_READY_FOR_DL_FAIL_ABORT: + printf( + "Error [%d] : CRT ready for download fail abort error. It occurs when download in CRT fails due to wrong " "address location\r\n", + rslt); + break; + + case BMI2_E_DL_ERROR: + printf( + "Error [%d] : Download error. It occurs when write length exceeds that of the maximum burst length\r\n", + rslt); + break; + + case BMI2_E_PRECON_ERROR: + printf( + "Error [%d] : Pre-conditional error. It occurs when precondition to start the feature was not " "completed\r\n", + rslt); + break; + + case BMI2_E_ABORT_ERROR: + printf("Error [%d] : Abort error. It occurs when the device was shaken during CRT test\r\n", rslt); + break; + + case BMI2_E_WRITE_CYCLE_ONGOING: + printf( + "Error [%d] : Write cycle ongoing error. It occurs when the write cycle is already running and another " "has been initiated\r\n", + rslt); + break; + + case BMI2_E_ST_NOT_RUNING: + printf( + "Error [%d] : Self-test is not running error. It occurs when self-test running is disabled while it's " "running\r\n", + rslt); + break; + + case BMI2_E_DATA_RDY_INT_FAILED: + printf( + "Error [%d] : Data ready interrupt error. It occurs when the sample count exceeds the FOC sample limit " "and data ready status is not updated\r\n", + rslt); + break; + + case BMI2_E_INVALID_FOC_POSITION: + printf( + "Error [%d] : Invalid FOC position error. It occurs when average FOC data is obtained for the wrong" " axes\r\n", + rslt); + break; + + default: + printf("Error [%d] : Unknown error code\r\n", rslt); + break; + } +} + +/*! + * @brief Deinitializes coines platform + * + * @return void. + */ +void bmi2_coines_deinit(void) +{ + coines_close_comm_intf(COINES_COMM_INTF_USB); +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/common/common.h b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/common/common.h new file mode 100644 index 0000000000..763983da4a --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/common/common.h @@ -0,0 +1,122 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +#ifndef _COMMON_H +#define _COMMON_H + +/*! CPP guard */ +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include "bmi2.h" + +/*! + * @brief Function for reading the sensor's registers through I2C bus. + * + * @param[in] reg_addr : Register address. + * @param[out] reg_data : Pointer to the data buffer to store the read data. + * @param[in] length : No of bytes to read. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval = BMI2_INTF_RET_SUCCESS -> Success + * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info + * + */ +BMI2_INTF_RETURN_TYPE bmi2_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr); + +/*! + * @brief Function for writing the sensor's registers through I2C bus. + * + * @param[in] reg_addr : Register address. + * @param[in] reg_data : Pointer to the data buffer whose value is to be written. + * @param[in] length : No of bytes to write. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval = BMI2_INTF_RET_SUCCESS -> Success + * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info + * + */ +BMI2_INTF_RETURN_TYPE bmi2_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr); + +/*! + * @brief Function for reading the sensor's registers through SPI bus. + * + * @param[in] reg_addr : Register address. + * @param[out] reg_data : Pointer to the data buffer to store the read data. + * @param[in] length : No of bytes to read. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval = BMI2_INTF_RET_SUCCESS -> Success + * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info + * + */ +BMI2_INTF_RETURN_TYPE bmi2_spi_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr); + +/*! + * @brief Function for writing the sensor's registers through SPI bus. + * + * @param[in] reg_addr : Register address. + * @param[in] reg_data : Pointer to the data buffer whose data has to be written. + * @param[in] length : No of bytes to write. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval = BMI2_INTF_RET_SUCCESS -> Success + * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info + * + */ +BMI2_INTF_RETURN_TYPE bmi2_spi_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr); + +/*! + * @brief This function provides the delay for required time (Microsecond) as per the input provided in some of the + * APIs. + * + * @param[in] period_us : The required wait time in microsecond. + * @param[in] intf_ptr : Interface pointer + * + * @return void. + * + */ +void bmi2_delay_us(uint32_t period, void *intf_ptr); + +/*! + * @brief Function to select the interface between SPI and I2C. + * + * @param[in] bma : Structure instance of bmi2_dev + * @param[in] intf : Interface selection parameter + * + * @return Status of execution + * @retval 0 -> Success + * @retval < 0 -> Failure Info + */ +int8_t bmi2_interface_init(struct bmi2_dev *bma, uint8_t intf); + +/*! + * @brief Prints the execution status of the APIs. + * + * @param[in] rslt : Error code returned by the API whose execution status has to be printed. + * + * @return void. + */ +void bmi2_error_codes_print_result(int8_t rslt); + +/*! + * @brief Deinitializes coines platform + * + * @return void. + */ +void bmi2_coines_deinit(void); + +#ifdef __cplusplus +} +#endif /* End of CPP guard */ + +#endif /* _COMMON_H */ diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/component_retrim/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/component_retrim/Makefile new file mode 100644 index 0000000000..633a817299 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/component_retrim/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= component_retrim.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_hc.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/component_retrim/component_retrim.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/component_retrim/component_retrim.c new file mode 100644 index 0000000000..6084574f4a --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/component_retrim/component_retrim.c @@ -0,0 +1,52 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_hc.h" +#include "common.h" + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_hc. */ + rslt = bmi270_hc_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* This API runs the CRT process. */ + rslt = bmi2_do_crt(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Do not move the board while doing CRT. If so, it will throw an abort error. */ + if (rslt == BMI2_OK) + { + printf("CRT successfully completed."); + } + } + + bmi2_coines_deinit(); + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_full_header_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_full_header_mode/Makefile new file mode 100644 index 0000000000..00794a37a0 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_full_header_mode/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= fifo_full_header_mode.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_hc.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_full_header_mode/fifo_full_header_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_full_header_mode/fifo_full_header_mode.c new file mode 100644 index 0000000000..6f11642843 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_full_header_mode/fifo_full_header_mode.c @@ -0,0 +1,316 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_hc.h" +#include "common.h" + +/******************************************************************************/ +/*! Macros */ + +/*! Buffer size allocated to store raw FIFO data. */ +#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) + +/*! Length of data to be read from FIFO. */ +#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) + +/*! Number of accel frames to be extracted from FIFO. */ + +/*! Calculation for frame count: Total frame count = Fifo buffer size(2048)/ Total frames(6 Accel, 6 Gyro and 1 header, + * totaling to 13) which equals to 157. + */ +#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(157) + +/*! Number of gyro frames to be extracted from FIFO. */ +#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(157) + +/*! Macro to read sensortime byte in FIFO. */ +#define SENSORTIME_OVERHEAD_BYTE UINT8_C(3) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + * @param[in] dev : Structure instance of bmi2_dev. + * @return Status of execution. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev); + +/******************************************************************************/ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + uint16_t index = 0; + uint16_t fifo_length = 0; + uint16_t config = 0; + + /* To read sensortime, extra 3 bytes are added to fifo buffer. */ + uint16_t fifo_buffer_size = BMI2_FIFO_RAW_DATA_BUFFER_SIZE + SENSORTIME_OVERHEAD_BYTE; + + /* Variable to get fifo full interrupt status. */ + uint16_t int_status = 0; + + uint16_t accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; + + uint16_t gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; + + /* Variable to store the available frame length count in FIFO */ + uint8_t frame_count; + + int8_t try = 1; + + /* Number of bytes of FIFO data. */ + uint8_t fifo_data[fifo_buffer_size]; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Array of accelerometer frames -> Total bytes = + * 157 * (6 axes + 1 header bytes) = 1099 bytes */ + struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; + + /* Array of gyro frames -> Total bytes = + * 157 * (6 axes + 1 header bytes) = 1099 bytes */ + struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; + + /* Initialize FIFO frame structure. */ + struct bmi2_fifo_frame fifoframe = { 0 }; + + /* Accel and gyro sensor are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_hc. */ + rslt = bmi270_hc_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Enable accel and gyro sensor. */ + rslt = bmi270_hc_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Configuration settings for accel and gyro. */ + rslt = set_accel_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Before setting FIFO, disable the advance power save mode. */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initially disable all configurations in fifo. */ + rslt = bmi2_get_fifo_config(&config, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initially disable all configurations in fifo. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Update FIFO structure. */ + /* Mapping the buffer to store the fifo data. */ + fifoframe.data = fifo_data; + + /* Length of FIFO frame. */ + /* To read sensortime, extra 3 bytes are added to fifo user length. */ + fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH + SENSORTIME_OVERHEAD_BYTE; + + /* Set FIFO configuration by enabling accel, gyro and timestamp. + * NOTE 1: The header mode is enabled by default. + * NOTE 2: By default the FIFO operating mode is in FIFO mode. + * NOTE 3: Sensortime is enabled by default */ + printf("FIFO is configured in header mode\n"); + rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Map FIFO full interrupt. */ + fifoframe.data_int_map = BMI2_FFULL_INT; + rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + while (try <= 10) + { + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if ((rslt == BMI2_OK) && (int_status & BMI2_FFULL_INT_STATUS_MASK)) + { + printf("\nIteration : %d\n", try); + + accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; + + gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; + + rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("\nFIFO data bytes available : %d \n", fifo_length); + printf("\nFIFO data bytes requested : %d \n", fifoframe.length); + + /* Read FIFO data. */ + rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + printf("\nFIFO accel frames requested : %d \n", accel_frame_length); + + /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ + rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); + + printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); + + /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ + rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); + + /* Calculating the frame count from the available bytes in FIFO + * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len + header_byte)) */ + frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH + 1)); + printf("\nAvailable frame count from available bytes: %d\n", frame_count); + + printf("\nExtracted accel frames\n"); + + if (accel_frame_length > frame_count) + { + accel_frame_length = frame_count; + } + + /* Print the parsed accelerometer data from the FIFO buffer. */ + for (index = 0; index < accel_frame_length; index++) + { + printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, + fifo_accel_data[index].y, fifo_accel_data[index].z); + } + + printf("\nExtracted gyro frames\n"); + + if (gyro_frame_length > frame_count) + { + gyro_frame_length = frame_count; + } + + /* Print the parsed gyro data from the FIFO buffer. */ + for (index = 0; index < gyro_frame_length; index++) + { + printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", + index, + fifo_gyro_data[index].x, + fifo_gyro_data[index].y, + fifo_gyro_data[index].z); + } + + /* Print control frames like sensor time and skipped frame count. */ + printf("\nSkipped frame count = %d\n", fifoframe.skipped_frame_count); + + printf("Sensor time = %lu\r\n", fifoframe.sensor_time); + + try++; + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accel and gyro configurations. */ + struct bmi2_sens_config config[2]; + + /* Configure the type of feature. */ + config[0].type = BMI2_ACCEL; + config[1].type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_hc_get_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* Accel configuration settings. */ + /* Set Output Data Rate */ + config[0].cfg.acc.odr = BMI2_ACC_ODR_25HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* Gyro configuration settings. */ + /* Set Output Data Rate */ + config[1].cfg.gyr.odr = BMI2_GYR_ODR_25HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set new configurations. */ + rslt = bmi270_hc_set_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_full_headerless_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_full_headerless_mode/Makefile new file mode 100644 index 0000000000..5eefc71ab8 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_full_headerless_mode/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= fifo_full_headerless_mode.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_hc.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_full_headerless_mode/fifo_full_headerless_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_full_headerless_mode/fifo_full_headerless_mode.c new file mode 100644 index 0000000000..1bce488be0 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_full_headerless_mode/fifo_full_headerless_mode.c @@ -0,0 +1,304 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_hc.h" +#include "common.h" + +/******************************************************************************/ +/*! Macros */ + +/*! Buffer size allocated to store raw FIFO data */ +#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) + +/*! Length of data to be read from FIFO */ +#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) + +/*! Number of accel frames to be extracted from FIFO */ + +/*! Calculation for frame count: Total frame count = Fifo buffer size(2048)/ Total frames(6 Accel, 6 Gyro totaling to + * 12) which equals to 170. + */ +#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(169) + +/*! Number of gyro frames to be extracted from FIFO */ +#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(169) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + * @param[in] dev : Structure instance of bmi2_dev. + * @return Status of execution. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev); + +/******************************************************************************/ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + uint16_t index = 0; + + uint16_t fifo_length = 0; + + uint8_t try = 1; + + /* Variable to get fifo full interrupt status. */ + uint16_t int_status = 0; + + uint16_t accel_frame_length; + + uint16_t gyro_frame_length; + + /* Variable to store the available frame length count in FIFO */ + uint8_t frame_count; + + /* Number of bytes of FIFO data. */ + uint8_t fifo_data[BMI2_FIFO_RAW_DATA_BUFFER_SIZE] = { 0 }; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Array of accelerometer frames -> Total bytes = + * 170 * (6 axes bytes) = 1020 bytes */ + struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; + + /* Array of gyro frames -> Total bytes = + * 170 * (6 axes bytes) = 1020 bytes */ + struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; + + /* Initialize FIFO frame structure. */ + struct bmi2_fifo_frame fifoframe = { 0 }; + + /* Accel and gyro sensor are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_hc. */ + rslt = bmi270_hc_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Enable accel and gyro sensor. */ + rslt = bmi270_hc_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Configuration settings for accel and gyro. */ + rslt = set_accel_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Before setting FIFO, disable the advance power save mode. */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initially disable all configurations in fifo. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Update FIFO structure. */ + /* Mapping the buffer to store the fifo data. */ + fifoframe.data = fifo_data; + + /* Length of FIFO frame. */ + fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH; + + /* Set FIFO configuration by enabling accel, gyro. + * NOTE 1: The header mode is enabled by default. + * NOTE 2: By default the FIFO operating mode is FIFO mode. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("FIFO is configured in headerless mode\n"); + + /* To enable headerless mode, disable the header. */ + if (rslt == BMI2_OK) + { + rslt = bmi2_set_fifo_config(BMI2_FIFO_HEADER_EN, BMI2_DISABLE, &bmi2_dev); + } + + /* Map FIFO full interrupt. */ + fifoframe.data_int_map = BMI2_FFULL_INT; + rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + while (try <= 10) + { + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if ((rslt == BMI2_OK) && (int_status & BMI2_FFULL_INT_STATUS_MASK)) + { + printf("\nIteration : %d\n", try); + + rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; + gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; + + printf("\nFIFO data bytes available : %d \n", fifo_length); + printf("\nFIFO data bytes requested : %d \n", fifoframe.length); + + /* Read FIFO data. */ + rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + printf("\nFIFO accel frames requested : %d \n", accel_frame_length); + + /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ + rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); + + printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); + + /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ + rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); + + /* Calculating the frame count from the available bytes in FIFO + * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len)) */ + frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH)); + printf("\nAvailable frame count from available bytes: %d\n", frame_count); + + printf("\nExtracted accel frames\n"); + + if (accel_frame_length > frame_count) + { + accel_frame_length = frame_count; + } + + /* Print the parsed accelerometer data from the FIFO buffer. */ + for (index = 0; index < accel_frame_length; index++) + { + printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, + fifo_accel_data[index].y, fifo_accel_data[index].z); + } + + printf("\nExtracted gyro frames\n"); + + if (gyro_frame_length > frame_count) + { + gyro_frame_length = frame_count; + } + + /* Print the parsed gyro data from the FIFO buffer. */ + for (index = 0; index < gyro_frame_length; index++) + { + printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", + index, + fifo_gyro_data[index].x, + fifo_gyro_data[index].y, + fifo_gyro_data[index].z); + } + } + + try++; + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accel and gyro configurations. */ + struct bmi2_sens_config config[2]; + + /* Configure the type of feature. */ + config[0].type = BMI2_ACCEL; + config[1].type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_hc_get_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* Accel configuration settings. */ + /* Set Output Data Rate */ + config[0].cfg.acc.odr = BMI2_ACC_ODR_100HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* Gyro configuration settings. */ + /* Set Output Data Rate */ + config[1].cfg.gyr.odr = BMI2_GYR_ODR_100HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set new configurations. */ + rslt = bmi270_hc_set_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_watermark_header_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_watermark_header_mode/Makefile new file mode 100644 index 0000000000..8dea084da5 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_watermark_header_mode/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= fifo_watermark_header_mode.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_hc.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_watermark_header_mode/fifo_watermark_header_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_watermark_header_mode/fifo_watermark_header_mode.c new file mode 100644 index 0000000000..46599f21cd --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_watermark_header_mode/fifo_watermark_header_mode.c @@ -0,0 +1,335 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_hc.h" +#include "common.h" + +/******************************************************************************/ +/*! Macros */ + +/*! Buffer size allocated to store raw FIFO data */ +#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) + +/*! Length of data to be read from FIFO */ +#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) + +/*! Number of accel frames to be extracted from FIFO */ + +/*! + * Calculation: + * fifo_watermark_level = 650, accel_frame_len = 6, gyro_frame_len = 6 header_byte = 1. + * fifo_accel_frame_count = (650 / (6 + 6 + 1 )) = 50 frames + * NOTE: Extra frames are read in order to get sensor time + */ +#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(55) + +/*! Number of gyro frames to be extracted from FIFO */ +#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(55) + +/*! Setting a watermark level in FIFO */ +#define BMI270_FIFO_WATERMARK_LEVEL UINT16_C(650) + +/*! Macro to read sensortime byte in FIFO */ +#define SENSORTIME_OVERHEAD_BYTE UINT8_C(3) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + * @param[in] dev : Structure instance of bmi2_dev. + * @return Status of execution. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to store the available frame length count in FIFO */ + uint8_t frame_count; + + /* Variable to index bytes. */ + uint16_t index = 0; + + uint16_t fifo_length = 0; + + uint16_t accel_frame_length; + + uint16_t gyro_frame_length; + + uint8_t try = 1; + + /* To read sensortime, extra 3 bytes are added to fifo buffer */ + uint16_t fifo_buffer_size = BMI2_FIFO_RAW_DATA_BUFFER_SIZE + SENSORTIME_OVERHEAD_BYTE; + + /* Number of bytes of FIFO data. */ + uint8_t fifo_data[fifo_buffer_size]; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Array of accelerometer frames -> Total bytes = + * 55 * (6 axes bytes) = 330 bytes */ + struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; + + /* Array of gyro frames -> Total bytes = + * 55 * (6 axes bytes) = 330 bytes */ + struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; + + /* Initialize FIFO frame structure. */ + struct bmi2_fifo_frame fifoframe = { 0 }; + + /* Accel and gyro sensor are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; + + /* Variable to get fifo water-mark interrupt status. */ + uint16_t int_status = 0; + + uint16_t watermark = 0; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_hc. */ + rslt = bmi270_hc_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Enable accel and gyro sensor. */ + rslt = bmi270_hc_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Configuration settings for accel and gyro. */ + rslt = set_accel_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Before setting FIFO, disable the advance power save mode. */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initially disable all configurations in fifo. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Update FIFO structure. */ + /* Mapping the buffer to store the fifo data. */ + fifoframe.data = fifo_data; + + /* Length of FIFO frame. */ + /* To read sensortime, extra 3 bytes are added to fifo user length. */ + fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH + SENSORTIME_OVERHEAD_BYTE; + + /* Set FIFO configuration by enabling accel, gyro and timestamp. + * NOTE 1: The header mode is enabled by default. + * NOTE 2: By default the FIFO operating mode is FIFO mode. + * NOTE 3: Sensortime is enabled by default */ + printf("FIFO is configured in header mode\n"); + rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* FIFO water-mark interrupt is enabled. */ + fifoframe.data_int_map = BMI2_FWM_INT; + + /* Map water-mark interrupt to the required interrupt pin. */ + rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Set water-mark level. */ + fifoframe.wm_lvl = BMI270_FIFO_WATERMARK_LEVEL; + + /* Set the water-mark level if water-mark interrupt is mapped. */ + rslt = bmi2_set_fifo_wm(fifoframe.wm_lvl, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + rslt = bmi2_get_fifo_wm(&watermark, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + printf("\nFIFO watermark level is %d\n", watermark); + + while (try <= 10) + { + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the status of FIFO watermark interrupt. */ + if ((rslt == BMI2_OK) && (int_status & BMI2_FWM_INT_STATUS_MASK)) + { + printf("\nIteration : %d\n", try); + + printf("\nWatermark interrupt occurred\n"); + + rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; + gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; + + printf("\nFIFO data bytes available : %d \n", fifo_length); + printf("\nFIFO data bytes requested : %d \n", fifoframe.length); + + /* Read FIFO data. */ + rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + printf("\nFIFO accel frames requested : %d \n", accel_frame_length); + + /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ + rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); + + printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); + + /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ + rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); + + /* Calculating the frame count from the available bytes in FIFO + * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len + header_byte)) */ + frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH + 1)); + printf("\nAvailable frame count from available bytes: %d\n", frame_count); + + printf("\nExracted accel frames\n"); + + if (accel_frame_length > frame_count) + { + accel_frame_length = frame_count; + } + + /* Print the parsed accelerometer data from the FIFO buffer. */ + for (index = 0; index < accel_frame_length; index++) + { + printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, + fifo_accel_data[index].y, fifo_accel_data[index].z); + } + + printf("\nExtracted gyro frames\n"); + + if (gyro_frame_length > frame_count) + { + gyro_frame_length = frame_count; + } + + /* Print the parsed gyro data from the FIFO buffer. */ + for (index = 0; index < gyro_frame_length; index++) + { + printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", + index, + fifo_gyro_data[index].x, + fifo_gyro_data[index].y, + fifo_gyro_data[index].z); + } + + /* Print control frames like sensor time and skipped frame count. */ + printf("Skipped frame count = %d\n", fifoframe.skipped_frame_count); + printf("Sensor time = %lu\r\n", fifoframe.sensor_time); + + try++; + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accel and gyro configurations. */ + struct bmi2_sens_config config[2]; + + /* Configure the type of feature. */ + config[0].type = BMI2_ACCEL; + config[1].type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_hc_get_sensor_config(config, 2, dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* Accel configuration settings. */ + /* Set Output Data Rate */ + config[0].cfg.acc.odr = BMI2_ACC_ODR_25HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* Gyro configuration settings. */ + /* Set Output Data Rate */ + config[1].cfg.gyr.odr = BMI2_GYR_ODR_25HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set new configurations. */ + rslt = bmi270_hc_set_sensor_config(config, 2, dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_watermark_headerless_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_watermark_headerless_mode/Makefile new file mode 100644 index 0000000000..bd842a8170 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_watermark_headerless_mode/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= fifo_watermark_headerless_mode.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_hc.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c new file mode 100644 index 0000000000..bb0bfd1118 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c @@ -0,0 +1,331 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_hc.h" +#include "common.h" + +/******************************************************************************/ +/*! Macros */ + +/*! Buffer size allocated to store raw FIFO data */ +#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) + +/*! Length of data to be read from FIFO */ +#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) + +/*! Number of accel frames to be extracted from FIFO */ + +/*! + * Calculation: + * fifo_watermark_level = 650, accel_frame_len = 6, gyro_frame_len = 6. + * fifo_accel_frame_count = (650 / (6 + 6 )) = 54 frames + */ +#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(55) + +/*! Number of gyro frames to be extracted from FIFO */ +#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(55) + +/*! Setting a watermark level in FIFO */ +#define BMI270_FIFO_WATERMARK_LEVEL UINT16_C(650) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + * @param[in] dev : Structure instance of bmi2_dev. + * @return Status of execution. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to index bytes. */ + uint16_t index = 0; + + /* Variable to store the available frame length count in FIFO */ + uint8_t frame_count; + + uint16_t fifo_length = 0; + + uint16_t accel_frame_length; + + uint16_t gyro_frame_length; + + uint8_t try = 1; + + /* Number of bytes of FIFO data. */ + uint8_t fifo_data[BMI2_FIFO_RAW_DATA_BUFFER_SIZE] = { 0 }; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Array of accelerometer frames -> Total bytes = + * 50 * (6 axes bytes) = 300 bytes */ + struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; + + /* Array of gyro frames -> Total bytes = + * 50 * (6 axes bytes) = 300 bytes */ + struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; + + /* Initialize FIFO frame structure. */ + struct bmi2_fifo_frame fifoframe = { 0 }; + + /* Accel and gyro sensor are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; + + /* Variable to get fifo water-mark interrupt status. */ + uint16_t int_status = 0; + + uint16_t watermark = 0; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_hc. */ + rslt = bmi270_hc_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Enable accel and gyro sensor. */ + rslt = bmi270_hc_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Configuration settings for accel and gyro. */ + rslt = set_accel_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Before setting FIFO, disable the advance power save mode. */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initially disable all configurations in fifo. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Update FIFO structure. */ + /* Mapping the buffer to store the fifo data. */ + fifoframe.data = fifo_data; + + /* Length of FIFO frame. */ + fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH; + + /* Set FIFO configuration by enabling accel, gyro. + * NOTE 1: The header mode is enabled by default. + * NOTE 2: By default the FIFO operating mode is FIFO mode. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("FIFO is configured in headerless mode\n"); + + /* To enable headerless mode, disable the header. */ + if (rslt == BMI2_OK) + { + rslt = bmi2_set_fifo_config(BMI2_FIFO_HEADER_EN, BMI2_DISABLE, &bmi2_dev); + } + + /* FIFO water-mark interrupt is enabled. */ + fifoframe.data_int_map = BMI2_FWM_INT; + + /* Map water-mark interrupt to the required interrupt pin. */ + rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Set water-mark level. */ + fifoframe.wm_lvl = BMI270_FIFO_WATERMARK_LEVEL; + + fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH; + + /* Set the water-mark level if water-mark interrupt is mapped. */ + rslt = bmi2_set_fifo_wm(fifoframe.wm_lvl, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + rslt = bmi2_get_fifo_wm(&watermark, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + printf("\nFIFO watermark level is %d\n", watermark); + + while (try <= 10) + { + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the status of FIFO watermark interrupt. */ + if ((rslt == BMI2_OK) && (int_status & BMI2_FWM_INT_STATUS_MASK)) + { + printf("\nIteration : %d\n", try); + + printf("\nWatermark interrupt occurred\n"); + + rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; + gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; + + printf("\nFIFO data bytes available : %d \n", fifo_length); + printf("\nFIFO data bytes requested : %d \n", fifoframe.length); + + /* Read FIFO data. */ + rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + printf("\nFIFO accel frames requested : %d \n", accel_frame_length); + + /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ + rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); + + printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); + + /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ + rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); + + /* Calculating the frame count from the available bytes in FIFO + * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len)) */ + frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH)); + printf("\nAvailable frame count from available bytes: %d\n", frame_count); + + printf("\nExracted accel frames\n"); + + if (accel_frame_length > frame_count) + { + accel_frame_length = frame_count; + } + + /* Print the parsed accelerometer data from the FIFO buffer. */ + for (index = 0; index < accel_frame_length; index++) + { + printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, + fifo_accel_data[index].y, fifo_accel_data[index].z); + } + + printf("\nExtracted gyro frames\n"); + + if (gyro_frame_length > frame_count) + { + gyro_frame_length = frame_count; + } + + /* Print the parsed gyro data from the FIFO buffer. */ + for (index = 0; index < gyro_frame_length; index++) + { + printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", + index, + fifo_gyro_data[index].x, + fifo_gyro_data[index].y, + fifo_gyro_data[index].z); + } + } + + try++; + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accel and gyro configurations. */ + struct bmi2_sens_config config[2]; + + /* Configure the type of feature. */ + config[0].type = BMI2_ACCEL; + config[1].type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_hc_get_sensor_config(config, 2, dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameter according to their requirement. */ + /* Accel configuration settings. */ + /* Set Output Data Rate */ + config[0].cfg.acc.odr = BMI2_ACC_ODR_100HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* Gyro configuration settings. */ + /* Set Output Data Rate */ + config[1].cfg.gyr.odr = BMI2_GYR_ODR_100HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set new configurations. */ + rslt = bmi270_hc_set_sensor_config(config, 2, dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/gyro/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/gyro/Makefile new file mode 100644 index 0000000000..cb6bfaa3e1 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/gyro/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= gyro.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_hc.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/gyro/gyro.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/gyro/gyro.c new file mode 100644 index 0000000000..c7c50277fd --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/gyro/gyro.c @@ -0,0 +1,195 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_hc.h" +#include "common.h" + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for gyro. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t set_gyro_config(struct bmi2_dev *dev); + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + * + * @param[in] val : LSB from each axis. + * @param[in] dps : Degree per second. + * @param[in] bit_width : Resolution for gyro. + * + * @return Degree per second. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to define limit to print gyro data. */ + uint8_t limit = 10; + + uint8_t indx = 0; + + float x = 0, y = 0, z = 0; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Create an instance of sensor data structure. */ + struct bmi2_sensor_data sensor_data = { 0 }; + + /* Assign gyro sensor to variable. */ + uint8_t sens_list = BMI2_GYRO; + + /* Initialize the interrupt status of gyro. */ + uint16_t int_status = 0; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_hc. */ + rslt = bmi270_hc_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the selected sensors. */ + rslt = bmi270_hc_sensor_enable(&sens_list, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Gyro configuration settings. */ + rslt = set_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Select gyro sensor. */ + sensor_data.type = BMI2_GYRO; + printf("Gyro and DPS data\n"); + printf("Gyro data collected at Range 2000 dps with 16-bit resolution\n"); + + /* Loop to print gyro data when interrupt occurs. */ + while (indx <= limit) + { + /* To get the data ready interrupt status of gyro. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the data ready interrupt status and print the status for 10 samples. */ + if (int_status & BMI2_GYR_DRDY_INT_MASK) + { + /* Get gyro data for x, y and z axis. */ + rslt = bmi270_hc_get_sensor_data(&sensor_data, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("\nGyr_X = %d\t", sensor_data.sens_data.gyr.x); + printf("Gyr_Y = %d\t", sensor_data.sens_data.gyr.y); + printf("Gyr_Z = %d\r\n", sensor_data.sens_data.gyr.z); + + /* Converting lsb to degree per second for 16 bit gyro at 2000dps range. */ + x = lsb_to_dps(sensor_data.sens_data.gyr.x, 2000, bmi2_dev.resolution); + y = lsb_to_dps(sensor_data.sens_data.gyr.y, 2000, bmi2_dev.resolution); + z = lsb_to_dps(sensor_data.sens_data.gyr.z, 2000, bmi2_dev.resolution); + + /* Print the data in dps. */ + printf("Gyr_DPS_X = %4.2f, Gyr_DPS_Y = %4.2f, Gyr_DPS_Z = %4.2f\n", x, y, z); + + indx++; + } + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for gyro. + */ +static int8_t set_gyro_config(struct bmi2_dev *dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define the type of sensor and its configurations. */ + struct bmi2_sens_config config; + + /* Configure the type of feature. */ + config.type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_hc_get_sensor_config(&config, 1, dev); + bmi2_error_codes_print_result(rslt); + + /* Map data ready interrupt to interrupt pin. */ + rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT2, dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* The user can change the following configuration parameters according to their requirement. */ + /* Set Output Data Rate */ + config.cfg.gyr.odr = BMI2_GYR_ODR_200HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config.cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config.cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config.cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config.cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set the gyro configurations. */ + rslt = bmi270_hc_set_sensor_config(&config, 1, dev); + } + + return rslt; +} + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (dps / ((half_scale) + BMI2_GYR_RANGE_2000)) * (val); +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/step_counter/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/step_counter/Makefile new file mode 100644 index 0000000000..fc47b9bb1d --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/step_counter/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= step_counter.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_hc.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/step_counter/step_counter.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/step_counter/step_counter.c new file mode 100644 index 0000000000..785dc28ffc --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/step_counter/step_counter.c @@ -0,0 +1,146 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_hc.h" +#include "common.h" + +/******************************************************************************/ +/*! Static function declaration */ + +/*! + * @brief This internal API is used to set configurations for step counter. + * + * @param[in] bmi2_dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t set_feature_config(struct bmi2_dev *bmi2_dev); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Structure to define type of sensor and their respective data. */ + struct bmi2_sensor_data sensor_data; + + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Accel sensor and step counter feature are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_STEP_COUNTER }; + + /* Variable to get step counter interrupt status. */ + uint16_t int_status = 0; + + /* Select features and their pins to be mapped to. */ + struct bmi2_sens_int_config sens_int = { .type = BMI2_STEP_COUNTER, .hw_int_pin = BMI2_INT2 }; + + /* Type of sensor to get step counter data. */ + sensor_data.type = BMI2_STEP_COUNTER; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_hc. */ + rslt = bmi270_hc_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the selected sensor. */ + rslt = bmi270_hc_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Set the feature configuration for step counter. */ + rslt = set_feature_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("Step counter watermark level set to 1 (20 steps)\n"); + + if (rslt == BMI2_OK) + { + /* Map the step counter feature interrupt. */ + rslt = bmi270_hc_map_feat_int(&sens_int, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Move the board in steps for 20 times to get step counter interrupt. */ + printf("Move the board in steps\n"); + + /* Loop to get number of steps counted. */ + do + { + /* To get the interrupt status of the step counter. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the interrupt status of the step counter. */ + if (int_status & BMI270_HC_STEP_CNT_STATUS_MASK) + { + printf("Step counter interrupt occurred when watermark level (20 steps) is reached\n"); + + /* Get step counter output. */ + rslt = bmi270_hc_get_sensor_data(&sensor_data, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Print the step counter output. */ + printf("No of steps counted = %ld", sensor_data.sens_data.step_counter_output); + break; + } + } while (rslt == BMI2_OK); + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for step counter. + */ +static int8_t set_feature_config(struct bmi2_dev *bmi2_dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define the type of sensor and its configurations. */ + struct bmi2_sens_config config; + + /* Configure the type of sensor. */ + config.type = BMI2_STEP_COUNTER; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_hc_get_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Setting water-mark level to 1 for step counter to get interrupt after 20 step counts. Every 20 steps once + * output triggers. */ + config.cfg.step_counter.watermark_level = 1; + + /* Set new configuration. */ + rslt = bmi270_hc_set_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/accel/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/accel/Makefile new file mode 100644 index 0000000000..e4400a45ba --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/accel/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= accel.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_maximum_fifo.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/accel/accel.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/accel/accel.c new file mode 100644 index 0000000000..c32a43a41f --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/accel/accel.c @@ -0,0 +1,200 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_maximum_fifo.h" +#include "common.h" + +/******************************************************************************/ +/*! Macro definition */ + +/*! Earth's gravity in m/s^2 */ +#define GRAVITY_EARTH (9.80665f) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t set_accel_config(struct bmi2_dev *bmi2_dev); + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + * + * @param[in] val : LSB from each axis. + * @param[in] g_range : Gravity range. + * @param[in] bit_width : Resolution for accel. + * + * @return Gravity. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to define limit to print accel data. */ + uint8_t limit = 20; + + /* Assign accel sensor to variable. */ + uint8_t sensor_list = BMI2_ACCEL; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Create an instance of sensor data structure. */ + struct bmi2_sensor_data sensor_data = { 0 }; + + /* Initialize the interrupt status of accel. */ + uint16_t int_status = 0; + + uint8_t indx = 0; + float x = 0, y = 0, z = 0; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_maximum_fifo. */ + rslt = bmi270_maximum_fifo_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the accel sensor. */ + rslt = bmi2_sensor_enable(&sensor_list, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Accel configuration settings. */ + rslt = set_accel_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Assign accel sensor. */ + sensor_data.type = BMI2_ACCEL; + printf("Accel and m/s2 data \n"); + printf("Accel data collected at 2G Range with 16-bit resolution\n"); + + /* Loop to print the accel data when interrupt occurs. */ + while (indx <= limit) + { + /* To get the status of accel data ready interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the accel data ready interrupt status and print the status for 10 samples. */ + if (int_status & BMI2_ACC_DRDY_INT_MASK) + { + /* Get accelerometer data for x, y and z axis. */ + rslt = bmi2_get_sensor_data(&sensor_data, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + printf("\nAcc_X = %d\t", sensor_data.sens_data.acc.x); + printf("Acc_Y = %d\t", sensor_data.sens_data.acc.y); + printf("Acc_Z = %d\r\n", sensor_data.sens_data.acc.z); + + /* Converting lsb to meter per second squared for 16 bit accelerometer at 2G range. */ + x = lsb_to_mps2(sensor_data.sens_data.acc.x, 2, bmi2_dev.resolution); + y = lsb_to_mps2(sensor_data.sens_data.acc.y, 2, bmi2_dev.resolution); + z = lsb_to_mps2(sensor_data.sens_data.acc.z, 2, bmi2_dev.resolution); + + /* Print the data in m/s2. */ + printf("\nAcc_ms2_X = %4.2f, Acc_ms2_Y = %4.2f, Acc_ms2_Z = %4.2f\n", x, y, z); + + indx++; + } + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel. + */ +static int8_t set_accel_config(struct bmi2_dev *bmi2_dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accelerometer configuration. */ + struct bmi2_sens_config config; + + /* Configure the type of feature. */ + config.type = BMI2_ACCEL; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi2_get_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* Set Output Data Rate */ + config.cfg.acc.odr = BMI2_ACC_ODR_200HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config.cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples. + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config.cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config.cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set the accel configurations. */ + rslt = bmi2_set_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Map data ready interrupt to interrupt pin. */ + rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (GRAVITY_EARTH * val * g_range) / half_scale; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/accel_gyro/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/accel_gyro/Makefile new file mode 100644 index 0000000000..a33f36617a --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/accel_gyro/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= accel_gyro.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_maximum_fifo.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/accel_gyro/accel_gyro.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/accel_gyro/accel_gyro.c new file mode 100644 index 0000000000..b77ea1d810 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/accel_gyro/accel_gyro.c @@ -0,0 +1,268 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_maximum_fifo.h" +#include "common.h" + +/******************************************************************************/ +/*! Macro definition */ + +/*! Earth's gravity in m/s^2 */ +#define GRAVITY_EARTH (9.80665f) + +/*! Macros to select the sensors */ +#define ACCEL UINT8_C(0x00) +#define GYRO UINT8_C(0x01) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev); + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + * + * @param[in] val : LSB from each axis. + * @param[in] g_range : Gravity range. + * @param[in] bit_width : Resolution for accel. + * + * @return Gravity. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width); + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + * + * @param[in] val : LSB from each axis. + * @param[in] dps : Degree per second. + * @param[in] bit_width : Resolution for gyro. + * + * @return Degree per second. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to define limit to print accel data. */ + uint8_t limit = 10; + + /* Assign accel and gyro sensor to variable. */ + uint8_t sensor_list[2] = { BMI2_ACCEL, BMI2_GYRO }; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Create an instance of sensor data structure. */ + struct bmi2_sensor_data sensor_data[2] = { { 0 } }; + + /* Initialize the interrupt status of accel and gyro. */ + uint16_t int_status = 0; + + uint8_t indx = 1; + + float x = 0, y = 0, z = 0; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_maximum_fifo. */ + rslt = bmi270_maximum_fifo_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the accel and gyro sensor. */ + rslt = bmi2_sensor_enable(sensor_list, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Accel and gyro configuration settings. */ + rslt = set_accel_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Assign accel and gyro sensor. */ + sensor_data[ACCEL].type = BMI2_ACCEL; + sensor_data[GYRO].type = BMI2_GYRO; + + /* Loop to print accel and gyro data when interrupt occurs. */ + while (indx <= limit) + { + /* To get the data ready interrupt status of accel and gyro. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the data ready interrupt status and print the status for 10 samples. */ + if ((int_status & BMI2_ACC_DRDY_INT_MASK) && (int_status & BMI2_GYR_DRDY_INT_MASK)) + { + /* Get accel and gyro data for x, y and z axis. */ + rslt = bmi2_get_sensor_data(sensor_data, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("\n******* Accel(Raw and m/s2) Gyro(Raw and dps) data : %d *******\n", indx); + + printf("\nAcc_x = %d\t", sensor_data[ACCEL].sens_data.acc.x); + printf("Acc_y = %d\t", sensor_data[ACCEL].sens_data.acc.y); + printf("Acc_z = %d", sensor_data[ACCEL].sens_data.acc.z); + + /* Converting lsb to meter per second squared for 16 bit accelerometer at 2G range. */ + x = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.x, 2, bmi2_dev.resolution); + y = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.y, 2, bmi2_dev.resolution); + z = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.z, 2, bmi2_dev.resolution); + + /* Print the data in m/s2. */ + printf("\nAcc_ms2_X = %4.2f, Acc_ms2_Y = %4.2f, Acc_ms2_Z = %4.2f\n", x, y, z); + + printf("\nGyr_X = %d\t", sensor_data[GYRO].sens_data.gyr.x); + printf("Gyr_Y = %d\t", sensor_data[GYRO].sens_data.gyr.y); + printf("Gyr_Z= %d\n", sensor_data[GYRO].sens_data.gyr.z); + + /* Converting lsb to degree per second for 16 bit gyro at 2000dps range. */ + x = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.x, 2000, bmi2_dev.resolution); + y = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.y, 2000, bmi2_dev.resolution); + z = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.z, 2000, bmi2_dev.resolution); + + /* Print the data in dps. */ + printf("Gyro_DPS_X = %4.2f, Gyro_DPS_Y = %4.2f, Gyro_DPS_Z = %4.2f\n", x, y, z); + + indx++; + } + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accelerometer and gyro configuration. */ + struct bmi2_sens_config config[2]; + + /* Configure the type of feature. */ + config[ACCEL].type = BMI2_ACCEL; + config[GYRO].type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi2_get_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Map data ready interrupt to interrupt pin. */ + rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* Set Output Data Rate */ + config[ACCEL].cfg.acc.odr = BMI2_ACC_ODR_200HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config[ACCEL].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples. + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config[ACCEL].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config[ACCEL].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* The user can change the following configuration parameters according to their requirement. */ + /* Set Output Data Rate */ + config[GYRO].cfg.gyr.odr = BMI2_GYR_ODR_200HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config[GYRO].cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config[GYRO].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config[GYRO].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config[GYRO].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set the accel and gyro configurations. */ + rslt = bmi2_set_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (GRAVITY_EARTH * val * g_range) / half_scale; +} + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (dps / ((half_scale) + BMI2_GYR_RANGE_2000)) * (val); +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/common/common.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/common/common.c new file mode 100644 index 0000000000..afff37bc84 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/common/common.c @@ -0,0 +1,372 @@ +/** + * Copyright (C) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + */ + +#include +#include +#include + +#include "coines.h" +#include "bmi2_defs.h" + +/******************************************************************************/ +/*! Macro definitions */ +#define BMI2XY_SHUTTLE_ID UINT16_C(0x1B8) + +/*! Macro that defines read write length */ +#define READ_WRITE_LEN UINT8_C(46) + +/******************************************************************************/ +/*! Static variable definition */ + +/*! Variable that holds the I2C device address or SPI chip selection */ +static uint8_t dev_addr; + +/******************************************************************************/ +/*! User interface functions */ + +/*! + * I2C read function map to COINES platform + */ +BMI2_INTF_RETURN_TYPE bmi2_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr) +{ + uint8_t dev_addr = *(uint8_t*)intf_ptr; + + return coines_read_i2c(dev_addr, reg_addr, reg_data, (uint16_t)len); +} + +/*! + * I2C write function map to COINES platform + */ +BMI2_INTF_RETURN_TYPE bmi2_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr) +{ + uint8_t dev_addr = *(uint8_t*)intf_ptr; + + return coines_write_i2c(dev_addr, reg_addr, (uint8_t *)reg_data, (uint16_t)len); +} + +/*! + * SPI read function map to COINES platform + */ +BMI2_INTF_RETURN_TYPE bmi2_spi_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr) +{ + uint8_t dev_addr = *(uint8_t*)intf_ptr; + + return coines_read_spi(dev_addr, reg_addr, reg_data, (uint16_t)len); +} + +/*! + * SPI write function map to COINES platform + */ +BMI2_INTF_RETURN_TYPE bmi2_spi_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr) +{ + uint8_t dev_addr = *(uint8_t*)intf_ptr; + + return coines_write_spi(dev_addr, reg_addr, (uint8_t *)reg_data, (uint16_t)len); +} + +/*! + * Delay function map to COINES platform + */ +void bmi2_delay_us(uint32_t period, void *intf_ptr) +{ + coines_delay_usec(period); +} + +/*! + * @brief Function to select the interface between SPI and I2C. + * Also to initialize coines platform + */ +int8_t bmi2_interface_init(struct bmi2_dev *bmi, uint8_t intf) +{ + int8_t rslt = BMI2_OK; + struct coines_board_info board_info; + + if (bmi != NULL) + { + int16_t result = coines_open_comm_intf(COINES_COMM_INTF_USB); + if (result < COINES_SUCCESS) + { + printf( + "\n Unable to connect with Application Board ! \n" " 1. Check if the board is connected and powered on. \n" " 2. Check if Application Board USB driver is installed. \n" + " 3. Check if board is in use by another application. (Insufficient permissions to access USB) \n"); + exit(result); + } + + result = coines_get_board_info(&board_info); + +#if defined(PC) + setbuf(stdout, NULL); +#endif + + if (result == COINES_SUCCESS) + { + if ((board_info.shuttle_id != BMI2XY_SHUTTLE_ID)) + { + printf("! Warning invalid sensor shuttle \n ," "This application will not support this sensor \n"); + exit(COINES_E_FAILURE); + } + } + + coines_set_shuttleboard_vdd_vddio_config(0, 0); + coines_delay_msec(100); + + /* Bus configuration : I2C */ + if (intf == BMI2_I2C_INTF) + { + printf("I2C Interface \n"); + + /* To initialize the user I2C function */ + dev_addr = BMI2_I2C_PRIM_ADDR; + bmi->intf = BMI2_I2C_INTF; + bmi->read = bmi2_i2c_read; + bmi->write = bmi2_i2c_write; + coines_config_i2c_bus(COINES_I2C_BUS_0, COINES_I2C_FAST_MODE); + } + /* Bus configuration : SPI */ + else if (intf == BMI2_SPI_INTF) + { + printf("SPI Interface \n"); + + /* To initialize the user SPI function */ + dev_addr = COINES_SHUTTLE_PIN_7; + bmi->intf = BMI2_SPI_INTF; + bmi->read = bmi2_spi_read; + bmi->write = bmi2_spi_write; + coines_config_spi_bus(COINES_SPI_BUS_0, COINES_SPI_SPEED_5_MHZ, COINES_SPI_MODE3); + coines_set_pin_config(COINES_SHUTTLE_PIN_7, COINES_PIN_DIRECTION_OUT, COINES_PIN_VALUE_HIGH); + } + + /* Assign device address to interface pointer */ + bmi->intf_ptr = &dev_addr; + + /* Configure delay in microseconds */ + bmi->delay_us = bmi2_delay_us; + + /* Configure max read/write length (in bytes) ( Supported length depends on target machine) */ + bmi->read_write_len = READ_WRITE_LEN; + + /* Assign to NULL to load the default config file. */ + bmi->config_file_ptr = NULL; + + coines_delay_usec(10000); + + coines_set_shuttleboard_vdd_vddio_config(3300, 3300); + + coines_delay_usec(10000); + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; + +} + +/*! + * @brief Prints the execution status of the APIs. + */ +void bmi2_error_codes_print_result(int8_t rslt) +{ + switch (rslt) + { + case BMI2_OK: + + /* Do nothing */ + break; + + case BMI2_W_FIFO_EMPTY: + printf("Warning [%d] : FIFO empty\r\n", rslt); + break; + case BMI2_W_PARTIAL_READ: + printf("Warning [%d] : FIFO partial read\r\n", rslt); + break; + case BMI2_E_NULL_PTR: + printf( + "Error [%d] : Null pointer error. It occurs when the user tries to assign value (not address) to a pointer," " which has been initialized to NULL.\r\n", + rslt); + break; + + case BMI2_E_COM_FAIL: + printf( + "Error [%d] : Communication failure error. It occurs due to read/write operation failure and also due " "to power failure during communication\r\n", + rslt); + break; + + case BMI2_E_DEV_NOT_FOUND: + printf("Error [%d] : Device not found error. It occurs when the device chip id is incorrectly read\r\n", + rslt); + break; + + case BMI2_E_INVALID_SENSOR: + printf( + "Error [%d] : Invalid sensor error. It occurs when there is a mismatch in the requested feature with the " "available one\r\n", + rslt); + break; + + case BMI2_E_SELF_TEST_FAIL: + printf( + "Error [%d] : Self-test failed error. It occurs when the validation of accel self-test data is " "not satisfied\r\n", + rslt); + break; + + case BMI2_E_INVALID_INT_PIN: + printf( + "Error [%d] : Invalid interrupt pin error. It occurs when the user tries to configure interrupt pins " "apart from INT1 and INT2\r\n", + rslt); + break; + + case BMI2_E_OUT_OF_RANGE: + printf( + "Error [%d] : Out of range error. It occurs when the data exceeds from filtered or unfiltered data from " "fifo and also when the range exceeds the maximum range for accel and gyro while performing FOC\r\n", + rslt); + break; + + case BMI2_E_ACC_INVALID_CFG: + printf( + "Error [%d] : Invalid Accel configuration error. It occurs when there is an error in accel configuration" " register which could be one among range, BW or filter performance in reg address 0x40\r\n", + rslt); + break; + + case BMI2_E_GYRO_INVALID_CFG: + printf( + "Error [%d] : Invalid Gyro configuration error. It occurs when there is a error in gyro configuration" "register which could be one among range, BW or filter performance in reg address 0x42\r\n", + rslt); + break; + + case BMI2_E_ACC_GYR_INVALID_CFG: + printf( + "Error [%d] : Invalid Accel-Gyro configuration error. It occurs when there is a error in accel and gyro" " configuration registers which could be one among range, BW or filter performance in reg address 0x40 " "and 0x42\r\n", + rslt); + break; + + case BMI2_E_CONFIG_LOAD: + printf( + "Error [%d] : Configuration load error. It occurs when failure observed while loading the configuration " "into the sensor\r\n", + rslt); + break; + + case BMI2_E_INVALID_PAGE: + printf( + "Error [%d] : Invalid page error. It occurs due to failure in writing the correct feature configuration " "from selected page\r\n", + rslt); + break; + + case BMI2_E_SET_APS_FAIL: + printf( + "Error [%d] : APS failure error. It occurs due to failure in write of advance power mode configuration " "register\r\n", + rslt); + break; + + case BMI2_E_AUX_INVALID_CFG: + printf( + "Error [%d] : Invalid AUX configuration error. It occurs when the auxiliary interface settings are not " "enabled properly\r\n", + rslt); + break; + + case BMI2_E_AUX_BUSY: + printf( + "Error [%d] : AUX busy error. It occurs when the auxiliary interface buses are engaged while configuring" " the AUX\r\n", + rslt); + break; + + case BMI2_E_REMAP_ERROR: + printf( + "Error [%d] : Remap error. It occurs due to failure in assigning the remap axes data for all the axes " "after change in axis position\r\n", + rslt); + break; + + case BMI2_E_GYR_USER_GAIN_UPD_FAIL: + printf( + "Error [%d] : Gyro user gain update fail error. It occurs when the reading of user gain update status " "fails\r\n", + rslt); + break; + + case BMI2_E_SELF_TEST_NOT_DONE: + printf( + "Error [%d] : Self-test not done error. It occurs when the self-test process is ongoing or not " "completed\r\n", + rslt); + break; + + case BMI2_E_INVALID_INPUT: + printf("Error [%d] : Invalid input error. It occurs when the sensor input validity fails\r\n", rslt); + break; + + case BMI2_E_INVALID_STATUS: + printf("Error [%d] : Invalid status error. It occurs when the feature/sensor validity fails\r\n", rslt); + break; + + case BMI2_E_CRT_ERROR: + printf("Error [%d] : CRT error. It occurs when the CRT test has failed\r\n", rslt); + break; + + case BMI2_E_ST_ALREADY_RUNNING: + printf( + "Error [%d] : Self-test already running error. It occurs when the self-test is already running and " "another has been initiated\r\n", + rslt); + break; + + case BMI2_E_CRT_READY_FOR_DL_FAIL_ABORT: + printf( + "Error [%d] : CRT ready for download fail abort error. It occurs when download in CRT fails due to wrong " "address location\r\n", + rslt); + break; + + case BMI2_E_DL_ERROR: + printf( + "Error [%d] : Download error. It occurs when write length exceeds that of the maximum burst length\r\n", + rslt); + break; + + case BMI2_E_PRECON_ERROR: + printf( + "Error [%d] : Pre-conditional error. It occurs when precondition to start the feature was not " "completed\r\n", + rslt); + break; + + case BMI2_E_ABORT_ERROR: + printf("Error [%d] : Abort error. It occurs when the device was shaken during CRT test\r\n", rslt); + break; + + case BMI2_E_WRITE_CYCLE_ONGOING: + printf( + "Error [%d] : Write cycle ongoing error. It occurs when the write cycle is already running and another " "has been initiated\r\n", + rslt); + break; + + case BMI2_E_ST_NOT_RUNING: + printf( + "Error [%d] : Self-test is not running error. It occurs when self-test running is disabled while it's " "running\r\n", + rslt); + break; + + case BMI2_E_DATA_RDY_INT_FAILED: + printf( + "Error [%d] : Data ready interrupt error. It occurs when the sample count exceeds the FOC sample limit " "and data ready status is not updated\r\n", + rslt); + break; + + case BMI2_E_INVALID_FOC_POSITION: + printf( + "Error [%d] : Invalid FOC position error. It occurs when average FOC data is obtained for the wrong" " axes\r\n", + rslt); + break; + + default: + printf("Error [%d] : Unknown error code\r\n", rslt); + break; + } +} + +/*! + * @brief Deinitializes coines platform + * + * @return void. + */ +void bmi2_coines_deinit(void) +{ + coines_close_comm_intf(COINES_COMM_INTF_USB); +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/common/common.h b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/common/common.h new file mode 100644 index 0000000000..763983da4a --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/common/common.h @@ -0,0 +1,122 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +#ifndef _COMMON_H +#define _COMMON_H + +/*! CPP guard */ +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include "bmi2.h" + +/*! + * @brief Function for reading the sensor's registers through I2C bus. + * + * @param[in] reg_addr : Register address. + * @param[out] reg_data : Pointer to the data buffer to store the read data. + * @param[in] length : No of bytes to read. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval = BMI2_INTF_RET_SUCCESS -> Success + * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info + * + */ +BMI2_INTF_RETURN_TYPE bmi2_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr); + +/*! + * @brief Function for writing the sensor's registers through I2C bus. + * + * @param[in] reg_addr : Register address. + * @param[in] reg_data : Pointer to the data buffer whose value is to be written. + * @param[in] length : No of bytes to write. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval = BMI2_INTF_RET_SUCCESS -> Success + * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info + * + */ +BMI2_INTF_RETURN_TYPE bmi2_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr); + +/*! + * @brief Function for reading the sensor's registers through SPI bus. + * + * @param[in] reg_addr : Register address. + * @param[out] reg_data : Pointer to the data buffer to store the read data. + * @param[in] length : No of bytes to read. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval = BMI2_INTF_RET_SUCCESS -> Success + * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info + * + */ +BMI2_INTF_RETURN_TYPE bmi2_spi_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr); + +/*! + * @brief Function for writing the sensor's registers through SPI bus. + * + * @param[in] reg_addr : Register address. + * @param[in] reg_data : Pointer to the data buffer whose data has to be written. + * @param[in] length : No of bytes to write. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval = BMI2_INTF_RET_SUCCESS -> Success + * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info + * + */ +BMI2_INTF_RETURN_TYPE bmi2_spi_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr); + +/*! + * @brief This function provides the delay for required time (Microsecond) as per the input provided in some of the + * APIs. + * + * @param[in] period_us : The required wait time in microsecond. + * @param[in] intf_ptr : Interface pointer + * + * @return void. + * + */ +void bmi2_delay_us(uint32_t period, void *intf_ptr); + +/*! + * @brief Function to select the interface between SPI and I2C. + * + * @param[in] bma : Structure instance of bmi2_dev + * @param[in] intf : Interface selection parameter + * + * @return Status of execution + * @retval 0 -> Success + * @retval < 0 -> Failure Info + */ +int8_t bmi2_interface_init(struct bmi2_dev *bma, uint8_t intf); + +/*! + * @brief Prints the execution status of the APIs. + * + * @param[in] rslt : Error code returned by the API whose execution status has to be printed. + * + * @return void. + */ +void bmi2_error_codes_print_result(int8_t rslt); + +/*! + * @brief Deinitializes coines platform + * + * @return void. + */ +void bmi2_coines_deinit(void); + +#ifdef __cplusplus +} +#endif /* End of CPP guard */ + +#endif /* _COMMON_H */ diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/component_retrim/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/component_retrim/Makefile new file mode 100644 index 0000000000..0729972a20 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/component_retrim/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= component_retrim.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_maximum_fifo.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/component_retrim/component_retrim.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/component_retrim/component_retrim.c new file mode 100644 index 0000000000..ea24ab2e9f --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/component_retrim/component_retrim.c @@ -0,0 +1,52 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_maximum_fifo.h" +#include "common.h" + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_maximum_fifo. */ + rslt = bmi270_maximum_fifo_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* This API runs the CRT process. */ + rslt = bmi2_do_crt(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Do not move the board while doing CRT. If so, it will throw an abort error. */ + if (rslt == BMI2_OK) + { + printf("CRT successfully completed."); + } + } + + bmi2_coines_deinit(); + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/fifo_watermark_header_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/fifo_watermark_header_mode/Makefile new file mode 100644 index 0000000000..131538f30c --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/fifo_watermark_header_mode/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= fifo_watermark_header_mode.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_maximum_fifo.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/fifo_watermark_header_mode/fifo_watermark_header_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/fifo_watermark_header_mode/fifo_watermark_header_mode.c new file mode 100644 index 0000000000..f031379f78 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/fifo_watermark_header_mode/fifo_watermark_header_mode.c @@ -0,0 +1,335 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_maximum_fifo.h" +#include "common.h" + +/******************************************************************************/ +/*! Macros */ + +/*! Buffer size allocated to store raw FIFO data */ +#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) + +/*! Length of data to be read from FIFO */ +#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) + +/*! Number of accel frames to be extracted from FIFO */ + +/*! + * Calculation: + * fifo_watermark_level = 650, accel_frame_len = 6, gyro_frame_len = 6 header_byte = 1. + * fifo_accel_frame_count = (650 / (6 + 6 + 1 )) = 50 frames + * NOTE: Extra frames are read in order to get sensor time + */ +#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(55) + +/*! Number of gyro frames to be extracted from FIFO */ +#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(55) + +/*! Setting a watermark level in FIFO */ +#define BMI270_FIFO_WATERMARK_LEVEL UINT16_C(650) + +/*! Macro to read sensortime byte in FIFO */ +#define SENSORTIME_OVERHEAD_BYTE UINT8_C(3) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + * @param[in] dev : Structure instance of bmi2_dev. + * @return Status of execution. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to store the available frame length count in FIFO */ + uint8_t frame_count; + + /* Variable to index bytes. */ + uint16_t index = 0; + + uint16_t fifo_length = 0; + + uint16_t accel_frame_length; + + uint16_t gyro_frame_length; + + uint8_t try = 1; + + /* To read sensortime, extra 3 bytes are added to fifo buffer */ + uint16_t fifo_buffer_size = BMI2_FIFO_RAW_DATA_BUFFER_SIZE + SENSORTIME_OVERHEAD_BYTE; + + /* Number of bytes of FIFO data. */ + uint8_t fifo_data[fifo_buffer_size]; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Array of accelerometer frames -> Total bytes = + * 55 * (6 axes bytes) = 330 bytes */ + struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; + + /* Array of gyro frames -> Total bytes = + * 55 * (6 axes bytes) = 330 bytes */ + struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; + + /* Initialize FIFO frame structure. */ + struct bmi2_fifo_frame fifoframe = { 0 }; + + /* Accel and gyro sensor are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; + + /* Variable to get fifo water-mark interrupt status. */ + uint16_t int_status = 0; + + uint16_t watermark = 0; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_maximum_fifo. */ + rslt = bmi270_maximum_fifo_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Enable accel and gyro sensor. */ + rslt = bmi2_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Configuration settings for accel and gyro. */ + rslt = set_accel_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Before setting FIFO, disable the advance power save mode. */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initially disable all configurations in fifo. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Update FIFO structure. */ + /* Mapping the buffer to store the fifo data. */ + fifoframe.data = fifo_data; + + /* Length of FIFO frame. */ + /* To read sensortime, extra 3 bytes are added to fifo user length. */ + fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH + SENSORTIME_OVERHEAD_BYTE; + + /* Set FIFO configuration by enabling accel, gyro and timestamp. + * NOTE 1: The header mode is enabled by default. + * NOTE 2: By default the FIFO operating mode is FIFO mode. + * NOTE 3: Sensortime is enabled by default */ + printf("FIFO is configured in header mode\n"); + rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* FIFO water-mark interrupt is enabled. */ + fifoframe.data_int_map = BMI2_FWM_INT; + + /* Map water-mark interrupt to the required interrupt pin. */ + rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Set water-mark level. */ + fifoframe.wm_lvl = BMI270_FIFO_WATERMARK_LEVEL; + + /* Set the water-mark level if water-mark interrupt is mapped. */ + rslt = bmi2_set_fifo_wm(fifoframe.wm_lvl, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + rslt = bmi2_get_fifo_wm(&watermark, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + printf("\nFIFO watermark level is %d\n", watermark); + + while (try <= 10) + { + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the status of FIFO watermark interrupt. */ + if ((rslt == BMI2_OK) && (int_status & BMI2_FWM_INT_STATUS_MASK)) + { + printf("\nIteration : %d\n", try); + + printf("\nWatermark interrupt occurred\n"); + + rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; + gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; + + printf("\nFIFO data bytes available : %d \n", fifo_length); + printf("\nFIFO data bytes requested : %d \n", fifoframe.length); + + /* Read FIFO data. */ + rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + printf("\nFIFO accel frames requested : %d \n", accel_frame_length); + + /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ + rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); + + printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); + + /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ + rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); + + /* Calculating the frame count from the available bytes in FIFO + * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len + header_byte)) */ + frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH + 1)); + printf("\nAvailable frame count from available bytes: %d\n", frame_count); + + printf("\nExracted accel frames\n"); + + if (accel_frame_length > frame_count) + { + accel_frame_length = frame_count; + } + + /* Print the parsed accelerometer data from the FIFO buffer. */ + for (index = 0; index < accel_frame_length; index++) + { + printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, + fifo_accel_data[index].y, fifo_accel_data[index].z); + } + + printf("\nExtracted gyro frames\n"); + + if (gyro_frame_length > frame_count) + { + gyro_frame_length = frame_count; + } + + /* Print the parsed gyro data from the FIFO buffer. */ + for (index = 0; index < gyro_frame_length; index++) + { + printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", + index, + fifo_gyro_data[index].x, + fifo_gyro_data[index].y, + fifo_gyro_data[index].z); + } + + /* Print control frames like sensor time and skipped frame count. */ + printf("Skipped frame count = %d\n", fifoframe.skipped_frame_count); + printf("Sensor time = %lu\r\n", fifoframe.sensor_time); + + try++; + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accel and gyro configurations. */ + struct bmi2_sens_config config[2]; + + /* Configure the type of feature. */ + config[0].type = BMI2_ACCEL; + config[1].type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi2_get_sensor_config(config, 2, dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* Accel configuration settings. */ + /* Set Output Data Rate */ + config[0].cfg.acc.odr = BMI2_ACC_ODR_25HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* Gyro configuration settings. */ + /* Set Output Data Rate */ + config[1].cfg.gyr.odr = BMI2_GYR_ODR_25HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set new configurations. */ + rslt = bmi2_set_sensor_config(config, 2, dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/fifo_watermark_headerless_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/fifo_watermark_headerless_mode/Makefile new file mode 100644 index 0000000000..9790a92d4b --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/fifo_watermark_headerless_mode/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= fifo_watermark_headerless_mode.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_maximum_fifo.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c new file mode 100644 index 0000000000..541ff11163 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c @@ -0,0 +1,331 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_maximum_fifo.h" +#include "common.h" + +/******************************************************************************/ +/*! Macros */ + +/*! Buffer size allocated to store raw FIFO data */ +#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) + +/*! Length of data to be read from FIFO */ +#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) + +/*! Number of accel frames to be extracted from FIFO */ + +/*! + * Calculation: + * fifo_watermark_level = 650, accel_frame_len = 6, gyro_frame_len = 6. + * fifo_accel_frame_count = (650 / (6 + 6 )) = 54 frames + */ +#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(55) + +/*! Number of gyro frames to be extracted from FIFO */ +#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(55) + +/*! Setting a watermark level in FIFO */ +#define BMI270_FIFO_WATERMARK_LEVEL UINT16_C(650) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + * @param[in] dev : Structure instance of bmi2_dev. + * @return Status of execution. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to index bytes. */ + uint16_t index = 0; + + /* Variable to store the available frame length count in FIFO */ + uint8_t frame_count; + + uint16_t fifo_length = 0; + + uint16_t accel_frame_length; + + uint16_t gyro_frame_length; + + uint8_t try = 1; + + /* Number of bytes of FIFO data. */ + uint8_t fifo_data[BMI2_FIFO_RAW_DATA_BUFFER_SIZE] = { 0 }; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Array of accelerometer frames -> Total bytes = + * 50 * (6 axes bytes) = 300 bytes */ + struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; + + /* Array of gyro frames -> Total bytes = + * 50 * (6 axes bytes) = 300 bytes */ + struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; + + /* Initialize FIFO frame structure. */ + struct bmi2_fifo_frame fifoframe = { 0 }; + + /* Accel and gyro sensor are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; + + /* Variable to get fifo water-mark interrupt status. */ + uint16_t int_status = 0; + + uint16_t watermark = 0; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270. */ + rslt = bmi270_maximum_fifo_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Enable accel and gyro sensor. */ + rslt = bmi2_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Configuration settings for accel and gyro. */ + rslt = set_accel_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Before setting FIFO, disable the advance power save mode. */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initially disable all configurations in fifo. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Update FIFO structure. */ + /* Mapping the buffer to store the fifo data. */ + fifoframe.data = fifo_data; + + /* Length of FIFO frame. */ + fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH; + + /* Set FIFO configuration by enabling accel, gyro. + * NOTE 1: The header mode is enabled by default. + * NOTE 2: By default the FIFO operating mode is FIFO mode. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("FIFO is configured in headerless mode\n"); + + /* To enable headerless mode, disable the header. */ + if (rslt == BMI2_OK) + { + rslt = bmi2_set_fifo_config(BMI2_FIFO_HEADER_EN, BMI2_DISABLE, &bmi2_dev); + } + + /* FIFO water-mark interrupt is enabled. */ + fifoframe.data_int_map = BMI2_FWM_INT; + + /* Map water-mark interrupt to the required interrupt pin. */ + rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Set water-mark level. */ + fifoframe.wm_lvl = BMI270_FIFO_WATERMARK_LEVEL; + + fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH; + + /* Set the water-mark level if water-mark interrupt is mapped. */ + rslt = bmi2_set_fifo_wm(fifoframe.wm_lvl, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + rslt = bmi2_get_fifo_wm(&watermark, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + printf("\nFIFO watermark level is %d\n", watermark); + + while (try <= 10) + { + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the status of FIFO watermark interrupt. */ + if ((rslt == BMI2_OK) && (int_status & BMI2_FWM_INT_STATUS_MASK)) + { + printf("\nIteration : %d\n", try); + + printf("\nWatermark interrupt occurred\n"); + + rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; + gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; + + printf("\nFIFO data bytes available : %d \n", fifo_length); + printf("\nFIFO data bytes requested : %d \n", fifoframe.length); + + /* Read FIFO data. */ + rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + printf("\nFIFO accel frames requested : %d \n", accel_frame_length); + + /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ + rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); + + printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); + + /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ + rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); + + /* Calculating the frame count from the available bytes in FIFO + * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len)) */ + frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH)); + printf("\nAvailable frame count from available bytes: %d\n", frame_count); + + printf("\nExracted accel frames\n"); + + if (accel_frame_length > frame_count) + { + accel_frame_length = frame_count; + } + + /* Print the parsed accelerometer data from the FIFO buffer. */ + for (index = 0; index < accel_frame_length; index++) + { + printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, + fifo_accel_data[index].y, fifo_accel_data[index].z); + } + + printf("\nExtracted gyro frames\n"); + + if (gyro_frame_length > frame_count) + { + gyro_frame_length = frame_count; + } + + /* Print the parsed gyro data from the FIFO buffer. */ + for (index = 0; index < gyro_frame_length; index++) + { + printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", + index, + fifo_gyro_data[index].x, + fifo_gyro_data[index].y, + fifo_gyro_data[index].z); + } + } + + try++; + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accel and gyro configurations. */ + struct bmi2_sens_config config[2]; + + /* Configure the type of feature. */ + config[0].type = BMI2_ACCEL; + config[1].type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi2_get_sensor_config(config, 2, dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameter according to their requirement. */ + /* Accel configuration settings. */ + /* Set Output Data Rate */ + config[0].cfg.acc.odr = BMI2_ACC_ODR_100HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* Gyro configuration settings. */ + /* Set Output Data Rate */ + config[1].cfg.gyr.odr = BMI2_GYR_ODR_100HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set new configurations. */ + rslt = bmi2_set_sensor_config(config, 2, dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/gyro/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/gyro/Makefile new file mode 100644 index 0000000000..5ab6e7fd76 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/gyro/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= gyro.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_maximum_fifo.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/gyro/gyro.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/gyro/gyro.c new file mode 100644 index 0000000000..8b56f08429 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/gyro/gyro.c @@ -0,0 +1,195 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_maximum_fifo.h" +#include "common.h" + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for gyro. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t set_gyro_config(struct bmi2_dev *dev); + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + * + * @param[in] val : LSB from each axis. + * @param[in] dps : Degree per second. + * @param[in] bit_width : Resolution for gyro. + * + * @return Degree per second. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to define limit to print gyro data. */ + uint8_t limit = 10; + + uint8_t indx = 0; + + float x = 0, y = 0, z = 0; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Create an instance of sensor data structure. */ + struct bmi2_sensor_data sensor_data = { 0 }; + + /* Assign gyro sensor to variable. */ + uint8_t sens_list = BMI2_GYRO; + + /* Initialize the interrupt status of gyro. */ + uint16_t int_status = 0; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_maximum_fifo_init */ + rslt = bmi270_maximum_fifo_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the selected sensors. */ + rslt = bmi2_sensor_enable(&sens_list, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Gyro configuration settings. */ + rslt = set_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Select gyro sensor. */ + sensor_data.type = BMI2_GYRO; + printf("Gyro and DPS data\n"); + printf("Gyro data collected at Range 2000 dps with 16-bit resolution\n"); + + /* Loop to print gyro data when interrupt occurs. */ + while (indx <= limit) + { + /* To get the data ready interrupt status of gyro. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the data ready interrupt status and print the status for 10 samples. */ + if (int_status & BMI2_GYR_DRDY_INT_MASK) + { + /* Get gyro data for x, y and z axis. */ + rslt = bmi2_get_sensor_data(&sensor_data, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("\nGyr_X = %d\t", sensor_data.sens_data.gyr.x); + printf("Gyr_Y = %d\t", sensor_data.sens_data.gyr.y); + printf("Gyr_Z = %d\r\n", sensor_data.sens_data.gyr.z); + + /* Converting lsb to degree per second for 16 bit gyro at 2000dps range. */ + x = lsb_to_dps(sensor_data.sens_data.gyr.x, 2000, bmi2_dev.resolution); + y = lsb_to_dps(sensor_data.sens_data.gyr.y, 2000, bmi2_dev.resolution); + z = lsb_to_dps(sensor_data.sens_data.gyr.z, 2000, bmi2_dev.resolution); + + /* Print the data in dps. */ + printf("Gyr_DPS_X = %4.2f, Gyr_DPS_Y = %4.2f, Gyr_DPS_Z = %4.2f\n", x, y, z); + + indx++; + } + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for gyro. + */ +static int8_t set_gyro_config(struct bmi2_dev *dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define the type of sensor and its configurations. */ + struct bmi2_sens_config config; + + /* Configure the type of feature. */ + config.type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi2_get_sensor_config(&config, 1, dev); + bmi2_error_codes_print_result(rslt); + + /* Map data ready interrupt to interrupt pin. */ + rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT2, dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* The user can change the following configuration parameters according to their requirement. */ + /* Set Output Data Rate */ + config.cfg.gyr.odr = BMI2_GYR_ODR_200HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config.cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config.cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config.cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config.cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set the gyro configurations. */ + rslt = bmi2_set_sensor_config(&config, 1, dev); + } + + return rslt; +} + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (dps / ((half_scale) + BMI2_GYR_RANGE_2000)) * (val); +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/accel/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/accel/Makefile new file mode 100644 index 0000000000..3ab55f00e4 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/accel/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= accel.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_wh.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/accel/accel.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/accel/accel.c new file mode 100644 index 0000000000..cf6825229e --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/accel/accel.c @@ -0,0 +1,201 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_wh.h" +#include "common.h" + +/******************************************************************************/ +/*! Macro definition */ + +/*! Earth's gravity in m/s^2 */ +#define GRAVITY_EARTH (9.80665f) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t set_accel_config(struct bmi2_dev *bmi2_dev); + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + * + * @param[in] val : LSB from each axis. + * @param[in] g_range : Gravity range. + * @param[in] bit_width : Resolution for accel. + * + * @return Gravity. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to define limit to print accel data. */ + uint8_t limit = 20; + + /* Assign accel sensor to variable. */ + uint8_t sensor_list = BMI2_ACCEL; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Create an instance of sensor data structure. */ + struct bmi2_sensor_data sensor_data = { 0 }; + + /* Initialize the interrupt status of accel. */ + uint16_t int_status = 0; + + uint8_t indx = 0; + float x = 0, y = 0, z = 0; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_wh. */ + rslt = bmi270_wh_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the accel sensor. */ + rslt = bmi270_wh_sensor_enable(&sensor_list, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Accel configuration settings. */ + rslt = set_accel_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Assign accel sensor. */ + sensor_data.type = BMI2_ACCEL; + printf("Accel and m/s2 data \n"); + printf("Accel data collected at 2G Range with 16-bit resolution\n"); + + /* Loop to print the accel data when interrupt occurs. */ + while (indx <= limit) + { + /* To get the status of accel data ready interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the accel data ready interrupt status and print the status for 10 samples. */ + if (int_status & BMI2_ACC_DRDY_INT_MASK) + { + /* Get accelerometer data for x, y and z axis. */ + rslt = bmi270_wh_get_sensor_data(&sensor_data, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + printf("\nAcc_X = %d\t", sensor_data.sens_data.acc.x); + printf("Acc_Y = %d\t", sensor_data.sens_data.acc.y); + printf("Acc_Z = %d\r\n", sensor_data.sens_data.acc.z); + + /* Converting lsb to meter per second squared for 16 bit accelerometer at 2G range. */ + x = lsb_to_mps2(sensor_data.sens_data.acc.x, 2, bmi2_dev.resolution); + y = lsb_to_mps2(sensor_data.sens_data.acc.y, 2, bmi2_dev.resolution); + z = lsb_to_mps2(sensor_data.sens_data.acc.z, 2, bmi2_dev.resolution); + + /* Print the data in m/s2. */ + printf("\nAcc_ms2_X = %4.2f, Acc_ms2_Y = %4.2f, Acc_ms2_Z = %4.2f\n", x, y, z); + + indx++; + } + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel. + */ +static int8_t set_accel_config(struct bmi2_dev *bmi2_dev) +{ + + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accelerometer configuration. */ + struct bmi2_sens_config config; + + /* Configure the type of feature. */ + config.type = BMI2_ACCEL; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_wh_get_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* Set Output Data Rate */ + config.cfg.acc.odr = BMI2_ACC_ODR_200HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config.cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples. + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config.cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config.cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set the accel configurations. */ + rslt = bmi270_wh_set_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Map data ready interrupt to interrupt pin. */ + rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (GRAVITY_EARTH * val * g_range) / half_scale; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/accel_gyro/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/accel_gyro/Makefile new file mode 100644 index 0000000000..1d3ea28c5f --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/accel_gyro/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= accel_gyro.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_wh.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/accel_gyro/accel_gyro.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/accel_gyro/accel_gyro.c new file mode 100644 index 0000000000..e543cca7cf --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/accel_gyro/accel_gyro.c @@ -0,0 +1,268 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_wh.h" +#include "common.h" + +/******************************************************************************/ +/*! Macro definition */ + +/*! Earth's gravity in m/s^2 */ +#define GRAVITY_EARTH (9.80665f) + +/*! Macros to select the sensors */ +#define ACCEL UINT8_C(0x00) +#define GYRO UINT8_C(0x01) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev); + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + * + * @param[in] val : LSB from each axis. + * @param[in] g_range : Gravity range. + * @param[in] bit_width : Resolution for accel. + * + * @return Gravity. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width); + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + * + * @param[in] val : LSB from each axis. + * @param[in] dps : Degree per second. + * @param[in] bit_width : Resolution for gyro. + * + * @return Degree per second. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to define limit to print accel data. */ + uint8_t limit = 10; + + /* Assign accel and gyro sensor to variable. */ + uint8_t sensor_list[2] = { BMI2_ACCEL, BMI2_GYRO }; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Create an instance of sensor data structure. */ + struct bmi2_sensor_data sensor_data[2] = { { 0 } }; + + /* Initialize the interrupt status of accel and gyro. */ + uint16_t int_status = 0; + + uint8_t indx = 1; + + float x = 0, y = 0, z = 0; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270. */ + rslt = bmi270_wh_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the accel and gyro sensor. */ + rslt = bmi270_wh_sensor_enable(sensor_list, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Accel and gyro configuration settings. */ + rslt = set_accel_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Assign accel and gyro sensor. */ + sensor_data[ACCEL].type = BMI2_ACCEL; + sensor_data[GYRO].type = BMI2_GYRO; + + /* Loop to print accel and gyro data when interrupt occurs. */ + while (indx <= limit) + { + /* To get the data ready interrupt status of accel and gyro. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the data ready interrupt status and print the status for 10 samples. */ + if ((int_status & BMI2_ACC_DRDY_INT_MASK) && (int_status & BMI2_GYR_DRDY_INT_MASK)) + { + /* Get accel and gyro data for x, y and z axis. */ + rslt = bmi270_wh_get_sensor_data(sensor_data, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("\n******* Accel(Raw and m/s2) Gyro(Raw and dps) data : %d *******\n", indx); + + printf("\nAcc_x = %d\t", sensor_data[ACCEL].sens_data.acc.x); + printf("Acc_y = %d\t", sensor_data[ACCEL].sens_data.acc.y); + printf("Acc_z = %d", sensor_data[ACCEL].sens_data.acc.z); + + /* Converting lsb to meter per second squared for 16 bit accelerometer at 2G range. */ + x = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.x, 2, bmi2_dev.resolution); + y = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.y, 2, bmi2_dev.resolution); + z = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.z, 2, bmi2_dev.resolution); + + /* Print the data in m/s2. */ + printf("\nAcc_ms2_X = %4.2f, Acc_ms2_Y = %4.2f, Acc_ms2_Z = %4.2f\n", x, y, z); + + printf("\nGyr_X = %d\t", sensor_data[GYRO].sens_data.gyr.x); + printf("Gyr_Y = %d\t", sensor_data[GYRO].sens_data.gyr.y); + printf("Gyr_Z= %d\n", sensor_data[GYRO].sens_data.gyr.z); + + /* Converting lsb to degree per second for 16 bit gyro at 2000dps range. */ + x = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.x, 2000, bmi2_dev.resolution); + y = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.y, 2000, bmi2_dev.resolution); + z = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.z, 2000, bmi2_dev.resolution); + + /* Print the data in dps. */ + printf("Gyro_DPS_X = %4.2f, Gyro_DPS_Y = %4.2f, Gyro_DPS_Z = %4.2f\n", x, y, z); + + indx++; + } + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accelerometer and gyro configuration. */ + struct bmi2_sens_config config[2]; + + /* Configure the type of feature. */ + config[ACCEL].type = BMI2_ACCEL; + config[GYRO].type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_wh_get_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Map data ready interrupt to interrupt pin. */ + rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* Set Output Data Rate */ + config[ACCEL].cfg.acc.odr = BMI2_ACC_ODR_200HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config[ACCEL].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples. + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config[ACCEL].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config[ACCEL].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* The user can change the following configuration parameters according to their requirement. */ + /* Set Output Data Rate */ + config[GYRO].cfg.gyr.odr = BMI2_GYR_ODR_200HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config[GYRO].cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config[GYRO].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config[GYRO].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config[GYRO].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set the accel and gyro configurations. */ + rslt = bmi270_wh_set_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (GRAVITY_EARTH * val * g_range) / half_scale; +} + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (dps / ((half_scale) + BMI2_GYR_RANGE_2000)) * (val); +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/any_motion_interrupt/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/any_motion_interrupt/Makefile new file mode 100644 index 0000000000..1598c62cbf --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/any_motion_interrupt/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= any_motion_interrupt.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_wh.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/any_motion_interrupt/any_motion_interrupt.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/any_motion_interrupt/any_motion_interrupt.c new file mode 100644 index 0000000000..d1953e322c --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/any_motion_interrupt/any_motion_interrupt.c @@ -0,0 +1,134 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_wh.h" +#include "common.h" + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for any-motion. + * + * @param[in] bmi2_dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t set_feature_config(struct bmi2_dev *bmi2_dev); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Accel sensor and any-motion feature are listed in array. */ + uint8_t sens_list[2] = { BMI2_ACCEL, BMI2_ANY_MOTION }; + + /* Variable to get any-motion interrupt status. */ + uint16_t int_status = 0; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Select features and their pins to be mapped to. */ + struct bmi2_sens_int_config sens_int = { .type = BMI2_ANY_MOTION, .hw_int_pin = BMI2_INT1 }; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_wh. */ + rslt = bmi270_wh_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the selected sensors. */ + rslt = bmi270_wh_sensor_enable(sens_list, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Set feature configurations for any-motion. */ + rslt = set_feature_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Map the feature interrupt for any-motion. */ + rslt = bmi270_wh_map_feat_int(&sens_int, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + printf("Move the board\n"); + + /* Loop to get any-motion interrupt. */ + do + { + /* Clear buffer. */ + int_status = 0; + + /* To get the interrupt status of any-motion. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the interrupt status of any-motion. */ + if (int_status & BMI270_WH_ANY_MOT_STATUS_MASK) + { + printf("Any-motion interrupt is generated\n"); + break; + } + } while (rslt == BMI2_OK); + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for any-motion. + */ +static int8_t set_feature_config(struct bmi2_dev *bmi2_dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define the type of sensor and its configurations. */ + struct bmi2_sens_config config; + + /* Configure the type of feature. */ + config.type = BMI2_ANY_MOTION; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_wh_get_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* 1LSB equals 20ms. Default is 100ms, setting to 80ms. */ + config.cfg.any_motion.duration = 0x04; + + /* 1LSB equals to 0.48mg. Default is 83mg, setting to 50mg. */ + config.cfg.any_motion.threshold = 0x68; + + /* Set new configurations. */ + rslt = bmi270_wh_set_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/common/common.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/common/common.c new file mode 100644 index 0000000000..afff37bc84 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/common/common.c @@ -0,0 +1,372 @@ +/** + * Copyright (C) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + */ + +#include +#include +#include + +#include "coines.h" +#include "bmi2_defs.h" + +/******************************************************************************/ +/*! Macro definitions */ +#define BMI2XY_SHUTTLE_ID UINT16_C(0x1B8) + +/*! Macro that defines read write length */ +#define READ_WRITE_LEN UINT8_C(46) + +/******************************************************************************/ +/*! Static variable definition */ + +/*! Variable that holds the I2C device address or SPI chip selection */ +static uint8_t dev_addr; + +/******************************************************************************/ +/*! User interface functions */ + +/*! + * I2C read function map to COINES platform + */ +BMI2_INTF_RETURN_TYPE bmi2_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr) +{ + uint8_t dev_addr = *(uint8_t*)intf_ptr; + + return coines_read_i2c(dev_addr, reg_addr, reg_data, (uint16_t)len); +} + +/*! + * I2C write function map to COINES platform + */ +BMI2_INTF_RETURN_TYPE bmi2_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr) +{ + uint8_t dev_addr = *(uint8_t*)intf_ptr; + + return coines_write_i2c(dev_addr, reg_addr, (uint8_t *)reg_data, (uint16_t)len); +} + +/*! + * SPI read function map to COINES platform + */ +BMI2_INTF_RETURN_TYPE bmi2_spi_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr) +{ + uint8_t dev_addr = *(uint8_t*)intf_ptr; + + return coines_read_spi(dev_addr, reg_addr, reg_data, (uint16_t)len); +} + +/*! + * SPI write function map to COINES platform + */ +BMI2_INTF_RETURN_TYPE bmi2_spi_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr) +{ + uint8_t dev_addr = *(uint8_t*)intf_ptr; + + return coines_write_spi(dev_addr, reg_addr, (uint8_t *)reg_data, (uint16_t)len); +} + +/*! + * Delay function map to COINES platform + */ +void bmi2_delay_us(uint32_t period, void *intf_ptr) +{ + coines_delay_usec(period); +} + +/*! + * @brief Function to select the interface between SPI and I2C. + * Also to initialize coines platform + */ +int8_t bmi2_interface_init(struct bmi2_dev *bmi, uint8_t intf) +{ + int8_t rslt = BMI2_OK; + struct coines_board_info board_info; + + if (bmi != NULL) + { + int16_t result = coines_open_comm_intf(COINES_COMM_INTF_USB); + if (result < COINES_SUCCESS) + { + printf( + "\n Unable to connect with Application Board ! \n" " 1. Check if the board is connected and powered on. \n" " 2. Check if Application Board USB driver is installed. \n" + " 3. Check if board is in use by another application. (Insufficient permissions to access USB) \n"); + exit(result); + } + + result = coines_get_board_info(&board_info); + +#if defined(PC) + setbuf(stdout, NULL); +#endif + + if (result == COINES_SUCCESS) + { + if ((board_info.shuttle_id != BMI2XY_SHUTTLE_ID)) + { + printf("! Warning invalid sensor shuttle \n ," "This application will not support this sensor \n"); + exit(COINES_E_FAILURE); + } + } + + coines_set_shuttleboard_vdd_vddio_config(0, 0); + coines_delay_msec(100); + + /* Bus configuration : I2C */ + if (intf == BMI2_I2C_INTF) + { + printf("I2C Interface \n"); + + /* To initialize the user I2C function */ + dev_addr = BMI2_I2C_PRIM_ADDR; + bmi->intf = BMI2_I2C_INTF; + bmi->read = bmi2_i2c_read; + bmi->write = bmi2_i2c_write; + coines_config_i2c_bus(COINES_I2C_BUS_0, COINES_I2C_FAST_MODE); + } + /* Bus configuration : SPI */ + else if (intf == BMI2_SPI_INTF) + { + printf("SPI Interface \n"); + + /* To initialize the user SPI function */ + dev_addr = COINES_SHUTTLE_PIN_7; + bmi->intf = BMI2_SPI_INTF; + bmi->read = bmi2_spi_read; + bmi->write = bmi2_spi_write; + coines_config_spi_bus(COINES_SPI_BUS_0, COINES_SPI_SPEED_5_MHZ, COINES_SPI_MODE3); + coines_set_pin_config(COINES_SHUTTLE_PIN_7, COINES_PIN_DIRECTION_OUT, COINES_PIN_VALUE_HIGH); + } + + /* Assign device address to interface pointer */ + bmi->intf_ptr = &dev_addr; + + /* Configure delay in microseconds */ + bmi->delay_us = bmi2_delay_us; + + /* Configure max read/write length (in bytes) ( Supported length depends on target machine) */ + bmi->read_write_len = READ_WRITE_LEN; + + /* Assign to NULL to load the default config file. */ + bmi->config_file_ptr = NULL; + + coines_delay_usec(10000); + + coines_set_shuttleboard_vdd_vddio_config(3300, 3300); + + coines_delay_usec(10000); + } + else + { + rslt = BMI2_E_NULL_PTR; + } + + return rslt; + +} + +/*! + * @brief Prints the execution status of the APIs. + */ +void bmi2_error_codes_print_result(int8_t rslt) +{ + switch (rslt) + { + case BMI2_OK: + + /* Do nothing */ + break; + + case BMI2_W_FIFO_EMPTY: + printf("Warning [%d] : FIFO empty\r\n", rslt); + break; + case BMI2_W_PARTIAL_READ: + printf("Warning [%d] : FIFO partial read\r\n", rslt); + break; + case BMI2_E_NULL_PTR: + printf( + "Error [%d] : Null pointer error. It occurs when the user tries to assign value (not address) to a pointer," " which has been initialized to NULL.\r\n", + rslt); + break; + + case BMI2_E_COM_FAIL: + printf( + "Error [%d] : Communication failure error. It occurs due to read/write operation failure and also due " "to power failure during communication\r\n", + rslt); + break; + + case BMI2_E_DEV_NOT_FOUND: + printf("Error [%d] : Device not found error. It occurs when the device chip id is incorrectly read\r\n", + rslt); + break; + + case BMI2_E_INVALID_SENSOR: + printf( + "Error [%d] : Invalid sensor error. It occurs when there is a mismatch in the requested feature with the " "available one\r\n", + rslt); + break; + + case BMI2_E_SELF_TEST_FAIL: + printf( + "Error [%d] : Self-test failed error. It occurs when the validation of accel self-test data is " "not satisfied\r\n", + rslt); + break; + + case BMI2_E_INVALID_INT_PIN: + printf( + "Error [%d] : Invalid interrupt pin error. It occurs when the user tries to configure interrupt pins " "apart from INT1 and INT2\r\n", + rslt); + break; + + case BMI2_E_OUT_OF_RANGE: + printf( + "Error [%d] : Out of range error. It occurs when the data exceeds from filtered or unfiltered data from " "fifo and also when the range exceeds the maximum range for accel and gyro while performing FOC\r\n", + rslt); + break; + + case BMI2_E_ACC_INVALID_CFG: + printf( + "Error [%d] : Invalid Accel configuration error. It occurs when there is an error in accel configuration" " register which could be one among range, BW or filter performance in reg address 0x40\r\n", + rslt); + break; + + case BMI2_E_GYRO_INVALID_CFG: + printf( + "Error [%d] : Invalid Gyro configuration error. It occurs when there is a error in gyro configuration" "register which could be one among range, BW or filter performance in reg address 0x42\r\n", + rslt); + break; + + case BMI2_E_ACC_GYR_INVALID_CFG: + printf( + "Error [%d] : Invalid Accel-Gyro configuration error. It occurs when there is a error in accel and gyro" " configuration registers which could be one among range, BW or filter performance in reg address 0x40 " "and 0x42\r\n", + rslt); + break; + + case BMI2_E_CONFIG_LOAD: + printf( + "Error [%d] : Configuration load error. It occurs when failure observed while loading the configuration " "into the sensor\r\n", + rslt); + break; + + case BMI2_E_INVALID_PAGE: + printf( + "Error [%d] : Invalid page error. It occurs due to failure in writing the correct feature configuration " "from selected page\r\n", + rslt); + break; + + case BMI2_E_SET_APS_FAIL: + printf( + "Error [%d] : APS failure error. It occurs due to failure in write of advance power mode configuration " "register\r\n", + rslt); + break; + + case BMI2_E_AUX_INVALID_CFG: + printf( + "Error [%d] : Invalid AUX configuration error. It occurs when the auxiliary interface settings are not " "enabled properly\r\n", + rslt); + break; + + case BMI2_E_AUX_BUSY: + printf( + "Error [%d] : AUX busy error. It occurs when the auxiliary interface buses are engaged while configuring" " the AUX\r\n", + rslt); + break; + + case BMI2_E_REMAP_ERROR: + printf( + "Error [%d] : Remap error. It occurs due to failure in assigning the remap axes data for all the axes " "after change in axis position\r\n", + rslt); + break; + + case BMI2_E_GYR_USER_GAIN_UPD_FAIL: + printf( + "Error [%d] : Gyro user gain update fail error. It occurs when the reading of user gain update status " "fails\r\n", + rslt); + break; + + case BMI2_E_SELF_TEST_NOT_DONE: + printf( + "Error [%d] : Self-test not done error. It occurs when the self-test process is ongoing or not " "completed\r\n", + rslt); + break; + + case BMI2_E_INVALID_INPUT: + printf("Error [%d] : Invalid input error. It occurs when the sensor input validity fails\r\n", rslt); + break; + + case BMI2_E_INVALID_STATUS: + printf("Error [%d] : Invalid status error. It occurs when the feature/sensor validity fails\r\n", rslt); + break; + + case BMI2_E_CRT_ERROR: + printf("Error [%d] : CRT error. It occurs when the CRT test has failed\r\n", rslt); + break; + + case BMI2_E_ST_ALREADY_RUNNING: + printf( + "Error [%d] : Self-test already running error. It occurs when the self-test is already running and " "another has been initiated\r\n", + rslt); + break; + + case BMI2_E_CRT_READY_FOR_DL_FAIL_ABORT: + printf( + "Error [%d] : CRT ready for download fail abort error. It occurs when download in CRT fails due to wrong " "address location\r\n", + rslt); + break; + + case BMI2_E_DL_ERROR: + printf( + "Error [%d] : Download error. It occurs when write length exceeds that of the maximum burst length\r\n", + rslt); + break; + + case BMI2_E_PRECON_ERROR: + printf( + "Error [%d] : Pre-conditional error. It occurs when precondition to start the feature was not " "completed\r\n", + rslt); + break; + + case BMI2_E_ABORT_ERROR: + printf("Error [%d] : Abort error. It occurs when the device was shaken during CRT test\r\n", rslt); + break; + + case BMI2_E_WRITE_CYCLE_ONGOING: + printf( + "Error [%d] : Write cycle ongoing error. It occurs when the write cycle is already running and another " "has been initiated\r\n", + rslt); + break; + + case BMI2_E_ST_NOT_RUNING: + printf( + "Error [%d] : Self-test is not running error. It occurs when self-test running is disabled while it's " "running\r\n", + rslt); + break; + + case BMI2_E_DATA_RDY_INT_FAILED: + printf( + "Error [%d] : Data ready interrupt error. It occurs when the sample count exceeds the FOC sample limit " "and data ready status is not updated\r\n", + rslt); + break; + + case BMI2_E_INVALID_FOC_POSITION: + printf( + "Error [%d] : Invalid FOC position error. It occurs when average FOC data is obtained for the wrong" " axes\r\n", + rslt); + break; + + default: + printf("Error [%d] : Unknown error code\r\n", rslt); + break; + } +} + +/*! + * @brief Deinitializes coines platform + * + * @return void. + */ +void bmi2_coines_deinit(void) +{ + coines_close_comm_intf(COINES_COMM_INTF_USB); +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/common/common.h b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/common/common.h new file mode 100644 index 0000000000..763983da4a --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/common/common.h @@ -0,0 +1,122 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +#ifndef _COMMON_H +#define _COMMON_H + +/*! CPP guard */ +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include "bmi2.h" + +/*! + * @brief Function for reading the sensor's registers through I2C bus. + * + * @param[in] reg_addr : Register address. + * @param[out] reg_data : Pointer to the data buffer to store the read data. + * @param[in] length : No of bytes to read. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval = BMI2_INTF_RET_SUCCESS -> Success + * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info + * + */ +BMI2_INTF_RETURN_TYPE bmi2_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr); + +/*! + * @brief Function for writing the sensor's registers through I2C bus. + * + * @param[in] reg_addr : Register address. + * @param[in] reg_data : Pointer to the data buffer whose value is to be written. + * @param[in] length : No of bytes to write. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval = BMI2_INTF_RET_SUCCESS -> Success + * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info + * + */ +BMI2_INTF_RETURN_TYPE bmi2_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr); + +/*! + * @brief Function for reading the sensor's registers through SPI bus. + * + * @param[in] reg_addr : Register address. + * @param[out] reg_data : Pointer to the data buffer to store the read data. + * @param[in] length : No of bytes to read. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval = BMI2_INTF_RET_SUCCESS -> Success + * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info + * + */ +BMI2_INTF_RETURN_TYPE bmi2_spi_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr); + +/*! + * @brief Function for writing the sensor's registers through SPI bus. + * + * @param[in] reg_addr : Register address. + * @param[in] reg_data : Pointer to the data buffer whose data has to be written. + * @param[in] length : No of bytes to write. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval = BMI2_INTF_RET_SUCCESS -> Success + * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info + * + */ +BMI2_INTF_RETURN_TYPE bmi2_spi_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr); + +/*! + * @brief This function provides the delay for required time (Microsecond) as per the input provided in some of the + * APIs. + * + * @param[in] period_us : The required wait time in microsecond. + * @param[in] intf_ptr : Interface pointer + * + * @return void. + * + */ +void bmi2_delay_us(uint32_t period, void *intf_ptr); + +/*! + * @brief Function to select the interface between SPI and I2C. + * + * @param[in] bma : Structure instance of bmi2_dev + * @param[in] intf : Interface selection parameter + * + * @return Status of execution + * @retval 0 -> Success + * @retval < 0 -> Failure Info + */ +int8_t bmi2_interface_init(struct bmi2_dev *bma, uint8_t intf); + +/*! + * @brief Prints the execution status of the APIs. + * + * @param[in] rslt : Error code returned by the API whose execution status has to be printed. + * + * @return void. + */ +void bmi2_error_codes_print_result(int8_t rslt); + +/*! + * @brief Deinitializes coines platform + * + * @return void. + */ +void bmi2_coines_deinit(void); + +#ifdef __cplusplus +} +#endif /* End of CPP guard */ + +#endif /* _COMMON_H */ diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_full_header_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_full_header_mode/Makefile new file mode 100644 index 0000000000..bde7c19627 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_full_header_mode/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= fifo_full_header_mode.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_wh.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_full_header_mode/fifo_full_header_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_full_header_mode/fifo_full_header_mode.c new file mode 100644 index 0000000000..ab331eaeed --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_full_header_mode/fifo_full_header_mode.c @@ -0,0 +1,316 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_wh.h" +#include "common.h" + +/******************************************************************************/ +/*! Macros */ + +/*! Buffer size allocated to store raw FIFO data. */ +#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) + +/*! Length of data to be read from FIFO. */ +#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) + +/*! Number of accel frames to be extracted from FIFO. */ + +/*! Calculation for frame count: Total frame count = Fifo buffer size(2048)/ Total frames(6 Accel, 6 Gyro and 1 header, + * totaling to 13) which equals to 157. + */ +#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(157) + +/*! Number of gyro frames to be extracted from FIFO. */ +#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(157) + +/*! Macro to read sensortime byte in FIFO. */ +#define SENSORTIME_OVERHEAD_BYTE UINT8_C(3) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + * @param[in] dev : Structure instance of bmi2_dev. + * @return Status of execution. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev); + +/******************************************************************************/ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + uint16_t index = 0; + uint16_t fifo_length = 0; + uint16_t config = 0; + + /* To read sensortime, extra 3 bytes are added to fifo buffer. */ + uint16_t fifo_buffer_size = BMI2_FIFO_RAW_DATA_BUFFER_SIZE + SENSORTIME_OVERHEAD_BYTE; + + /* Variable to get fifo full interrupt status. */ + uint16_t int_status = 0; + + uint16_t accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; + + uint16_t gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; + + /* Variable to store the available frame length count in FIFO */ + uint8_t frame_count; + + int8_t try = 1; + + /* Number of bytes of FIFO data. */ + uint8_t fifo_data[fifo_buffer_size]; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Array of accelerometer frames -> Total bytes = + * 157 * (6 axes + 1 header bytes) = 1099 bytes */ + struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; + + /* Array of gyro frames -> Total bytes = + * 157 * (6 axes + 1 header bytes) = 1099 bytes */ + struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; + + /* Initialize FIFO frame structure. */ + struct bmi2_fifo_frame fifoframe = { 0 }; + + /* Accel and gyro sensor are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_wh. */ + rslt = bmi270_wh_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Enable accel and gyro sensor. */ + rslt = bmi270_wh_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Configuration settings for accel and gyro. */ + rslt = set_accel_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Before setting FIFO, disable the advance power save mode. */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initially disable all configurations in fifo. */ + rslt = bmi2_get_fifo_config(&config, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initially disable all configurations in fifo. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Update FIFO structure. */ + /* Mapping the buffer to store the fifo data. */ + fifoframe.data = fifo_data; + + /* Length of FIFO frame. */ + /* To read sensortime, extra 3 bytes are added to fifo user length. */ + fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH + SENSORTIME_OVERHEAD_BYTE; + + /* Set FIFO configuration by enabling accel, gyro and timestamp. + * NOTE 1: The header mode is enabled by default. + * NOTE 2: By default the FIFO operating mode is in FIFO mode. + * NOTE 3: Sensortime is enabled by default */ + printf("FIFO is configured in header mode\n"); + rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Map FIFO full interrupt. */ + fifoframe.data_int_map = BMI2_FFULL_INT; + rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + while (try <= 10) + { + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if ((rslt == BMI2_OK) && (int_status & BMI2_FFULL_INT_STATUS_MASK)) + { + printf("\nIteration : %d\n", try); + + accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; + + gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; + + rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("\nFIFO data bytes available : %d \n", fifo_length); + printf("\nFIFO data bytes requested : %d \n", fifoframe.length); + + /* Read FIFO data. */ + rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + printf("\nFIFO accel frames requested : %d \n", accel_frame_length); + + /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ + rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); + + printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); + + /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ + rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); + + /* Calculating the frame count from the available bytes in FIFO + * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len + header_byte)) */ + frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH + 1)); + printf("\nAvailable frame count from available bytes: %d\n", frame_count); + + printf("\nExtracted accel frames\n"); + + if (accel_frame_length > frame_count) + { + accel_frame_length = frame_count; + } + + /* Print the parsed accelerometer data from the FIFO buffer. */ + for (index = 0; index < accel_frame_length; index++) + { + printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, + fifo_accel_data[index].y, fifo_accel_data[index].z); + } + + printf("\nExtracted gyro frames\n"); + + if (gyro_frame_length > frame_count) + { + gyro_frame_length = frame_count; + } + + /* Print the parsed gyro data from the FIFO buffer. */ + for (index = 0; index < gyro_frame_length; index++) + { + printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", + index, + fifo_gyro_data[index].x, + fifo_gyro_data[index].y, + fifo_gyro_data[index].z); + } + + /* Print control frames like sensor time and skipped frame count. */ + printf("\nSkipped frame count = %d\n", fifoframe.skipped_frame_count); + + printf("Sensor time = %lu\r\n", fifoframe.sensor_time); + + try++; + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accel and gyro configurations. */ + struct bmi2_sens_config config[2]; + + /* Configure the type of feature. */ + config[0].type = BMI2_ACCEL; + config[1].type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_wh_get_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* Accel configuration settings. */ + /* Set Output Data Rate */ + config[0].cfg.acc.odr = BMI2_ACC_ODR_25HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* Gyro configuration settings. */ + /* Set Output Data Rate */ + config[1].cfg.gyr.odr = BMI2_GYR_ODR_25HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set new configurations. */ + rslt = bmi270_wh_set_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_full_headerless_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_full_headerless_mode/Makefile new file mode 100644 index 0000000000..5c407e9094 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_full_headerless_mode/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= fifo_full_headerless_mode.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_wh.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_full_headerless_mode/fifo_full_headerless_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_full_headerless_mode/fifo_full_headerless_mode.c new file mode 100644 index 0000000000..14b0b22b3d --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_full_headerless_mode/fifo_full_headerless_mode.c @@ -0,0 +1,304 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_wh.h" +#include "common.h" + +/******************************************************************************/ +/*! Macros */ + +/*! Buffer size allocated to store raw FIFO data */ +#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) + +/*! Length of data to be read from FIFO */ +#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) + +/*! Number of accel frames to be extracted from FIFO */ + +/*! Calculation for frame count: Total frame count = Fifo buffer size(2048)/ Total frames(6 Accel, 6 Gyro totaling to + * 12) which equals to 170. + */ +#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(169) + +/*! Number of gyro frames to be extracted from FIFO */ +#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(169) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + * @param[in] dev : Structure instance of bmi2_dev. + * @return Status of execution. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev); + +/******************************************************************************/ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + uint16_t index = 0; + + uint16_t fifo_length = 0; + + uint8_t try = 1; + + /* Variable to get fifo full interrupt status. */ + uint16_t int_status = 0; + + uint16_t accel_frame_length; + + uint16_t gyro_frame_length; + + /* Variable to store the available frame length count in FIFO */ + uint8_t frame_count; + + /* Number of bytes of FIFO data. */ + uint8_t fifo_data[BMI2_FIFO_RAW_DATA_BUFFER_SIZE] = { 0 }; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Array of accelerometer frames -> Total bytes = + * 170 * (6 axes bytes) = 1020 bytes */ + struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; + + /* Array of gyro frames -> Total bytes = + * 170 * (6 axes bytes) = 1020 bytes */ + struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; + + /* Initialize FIFO frame structure. */ + struct bmi2_fifo_frame fifoframe = { 0 }; + + /* Accel and gyro sensor are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_wh. */ + rslt = bmi270_wh_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Enable accel and gyro sensor. */ + rslt = bmi270_wh_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Configuration settings for accel and gyro. */ + rslt = set_accel_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Before setting FIFO, disable the advance power save mode. */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initially disable all configurations in fifo. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Update FIFO structure. */ + /* Mapping the buffer to store the fifo data. */ + fifoframe.data = fifo_data; + + /* Length of FIFO frame. */ + fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH; + + /* Set FIFO configuration by enabling accel, gyro. + * NOTE 1: The header mode is enabled by default. + * NOTE 2: By default the FIFO operating mode is FIFO mode. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("FIFO is configured in headerless mode\n"); + + /* To enable headerless mode, disable the header. */ + if (rslt == BMI2_OK) + { + rslt = bmi2_set_fifo_config(BMI2_FIFO_HEADER_EN, BMI2_DISABLE, &bmi2_dev); + } + + /* Map FIFO full interrupt. */ + fifoframe.data_int_map = BMI2_FFULL_INT; + rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + while (try <= 10) + { + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if ((rslt == BMI2_OK) && (int_status & BMI2_FFULL_INT_STATUS_MASK)) + { + printf("\nIteration : %d\n", try); + + rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; + gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; + + printf("\nFIFO data bytes available : %d \n", fifo_length); + printf("\nFIFO data bytes requested : %d \n", fifoframe.length); + + /* Read FIFO data. */ + rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + printf("\nFIFO accel frames requested : %d \n", accel_frame_length); + + /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ + rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); + + printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); + + /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ + rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); + + /* Calculating the frame count from the available bytes in FIFO + * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len)) */ + frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH)); + printf("\nAvailable frame count from available bytes: %d\n", frame_count); + + printf("\nExtracted accel frames\n"); + + if (accel_frame_length > frame_count) + { + accel_frame_length = frame_count; + } + + /* Print the parsed accelerometer data from the FIFO buffer. */ + for (index = 0; index < accel_frame_length; index++) + { + printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, + fifo_accel_data[index].y, fifo_accel_data[index].z); + } + + printf("\nExtracted gyro frames\n"); + + if (gyro_frame_length > frame_count) + { + gyro_frame_length = frame_count; + } + + /* Print the parsed gyro data from the FIFO buffer. */ + for (index = 0; index < gyro_frame_length; index++) + { + printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", + index, + fifo_gyro_data[index].x, + fifo_gyro_data[index].y, + fifo_gyro_data[index].z); + } + } + + try++; + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accel and gyro configurations. */ + struct bmi2_sens_config config[2]; + + /* Configure the type of feature. */ + config[0].type = BMI2_ACCEL; + config[1].type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_wh_get_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* Accel configuration settings. */ + /* Set Output Data Rate */ + config[0].cfg.acc.odr = BMI2_ACC_ODR_100HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* Gyro configuration settings. */ + /* Set Output Data Rate */ + config[1].cfg.gyr.odr = BMI2_GYR_ODR_100HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set new configurations. */ + rslt = bmi270_wh_set_sensor_config(config, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_watermark_header_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_watermark_header_mode/Makefile new file mode 100644 index 0000000000..e0a42e369a --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_watermark_header_mode/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= fifo_watermark_header_mode.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_wh.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_watermark_header_mode/fifo_watermark_header_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_watermark_header_mode/fifo_watermark_header_mode.c new file mode 100644 index 0000000000..f33bb5ebff --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_watermark_header_mode/fifo_watermark_header_mode.c @@ -0,0 +1,335 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_wh.h" +#include "common.h" + +/******************************************************************************/ +/*! Macros */ + +/*! Buffer size allocated to store raw FIFO data */ +#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) + +/*! Length of data to be read from FIFO */ +#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) + +/*! Number of accel frames to be extracted from FIFO */ + +/*! + * Calculation: + * fifo_watermark_level = 650, accel_frame_len = 6, gyro_frame_len = 6 header_byte = 1. + * fifo_accel_frame_count = (650 / (6 + 6 + 1 )) = 50 frames + * NOTE: Extra frames are read in order to get sensor time + */ +#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(55) + +/*! Number of gyro frames to be extracted from FIFO */ +#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(55) + +/*! Setting a watermark level in FIFO */ +#define BMI270_FIFO_WATERMARK_LEVEL UINT16_C(650) + +/*! Macro to read sensortime byte in FIFO */ +#define SENSORTIME_OVERHEAD_BYTE UINT8_C(3) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + * @param[in] dev : Structure instance of bmi2_dev. + * @return Status of execution. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to store the available frame length count in FIFO */ + uint8_t frame_count; + + /* Variable to index bytes. */ + uint16_t index = 0; + + uint16_t fifo_length = 0; + + uint16_t accel_frame_length; + + uint16_t gyro_frame_length; + + uint8_t try = 1; + + /* To read sensortime, extra 3 bytes are added to fifo buffer */ + uint16_t fifo_buffer_size = BMI2_FIFO_RAW_DATA_BUFFER_SIZE + SENSORTIME_OVERHEAD_BYTE; + + /* Number of bytes of FIFO data. */ + uint8_t fifo_data[fifo_buffer_size]; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Array of accelerometer frames -> Total bytes = + * 55 * (6 axes bytes) = 330 bytes */ + struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; + + /* Array of gyro frames -> Total bytes = + * 55 * (6 axes bytes) = 330 bytes */ + struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; + + /* Initialize FIFO frame structure. */ + struct bmi2_fifo_frame fifoframe = { 0 }; + + /* Accel and gyro sensor are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; + + /* Variable to get fifo water-mark interrupt status. */ + uint16_t int_status = 0; + + uint16_t watermark = 0; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_wh. */ + rslt = bmi270_wh_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Enable accel and gyro sensor. */ + rslt = bmi270_wh_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Configuration settings for accel and gyro. */ + rslt = set_accel_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Before setting FIFO, disable the advance power save mode. */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initially disable all configurations in fifo. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Update FIFO structure. */ + /* Mapping the buffer to store the fifo data. */ + fifoframe.data = fifo_data; + + /* Length of FIFO frame. */ + /* To read sensortime, extra 3 bytes are added to fifo user length. */ + fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH + SENSORTIME_OVERHEAD_BYTE; + + /* Set FIFO configuration by enabling accel, gyro and timestamp. + * NOTE 1: The header mode is enabled by default. + * NOTE 2: By default the FIFO operating mode is FIFO mode. + * NOTE 3: Sensortime is enabled by default */ + printf("FIFO is configured in header mode\n"); + rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* FIFO water-mark interrupt is enabled. */ + fifoframe.data_int_map = BMI2_FWM_INT; + + /* Map water-mark interrupt to the required interrupt pin. */ + rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Set water-mark level. */ + fifoframe.wm_lvl = BMI270_FIFO_WATERMARK_LEVEL; + + /* Set the water-mark level if water-mark interrupt is mapped. */ + rslt = bmi2_set_fifo_wm(fifoframe.wm_lvl, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + rslt = bmi2_get_fifo_wm(&watermark, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + printf("\nFIFO watermark level is %d\n", watermark); + + while (try <= 10) + { + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the status of FIFO watermark interrupt. */ + if ((rslt == BMI2_OK) && (int_status & BMI2_FWM_INT_STATUS_MASK)) + { + printf("\nIteration : %d\n", try); + + printf("\nWatermark interrupt occurred\n"); + + rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; + gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; + + printf("\nFIFO data bytes available : %d \n", fifo_length); + printf("\nFIFO data bytes requested : %d \n", fifoframe.length); + + /* Read FIFO data. */ + rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + printf("\nFIFO accel frames requested : %d \n", accel_frame_length); + + /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ + rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); + + printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); + + /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ + rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); + + /* Calculating the frame count from the available bytes in FIFO + * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len + header_byte)) */ + frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH + 1)); + printf("\nAvailable frame count from available bytes: %d\n", frame_count); + + printf("\nExracted accel frames\n"); + + if (accel_frame_length > frame_count) + { + accel_frame_length = frame_count; + } + + /* Print the parsed accelerometer data from the FIFO buffer. */ + for (index = 0; index < accel_frame_length; index++) + { + printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, + fifo_accel_data[index].y, fifo_accel_data[index].z); + } + + printf("\nExtracted gyro frames\n"); + + if (gyro_frame_length > frame_count) + { + gyro_frame_length = frame_count; + } + + /* Print the parsed gyro data from the FIFO buffer. */ + for (index = 0; index < gyro_frame_length; index++) + { + printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", + index, + fifo_gyro_data[index].x, + fifo_gyro_data[index].y, + fifo_gyro_data[index].z); + } + + /* Print control frames like sensor time and skipped frame count. */ + printf("Skipped frame count = %d\n", fifoframe.skipped_frame_count); + printf("Sensor time = %lu\r\n", fifoframe.sensor_time); + + try++; + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accel and gyro configurations. */ + struct bmi2_sens_config config[2]; + + /* Configure the type of feature. */ + config[0].type = BMI2_ACCEL; + config[1].type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_wh_get_sensor_config(config, 2, dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* Accel configuration settings. */ + /* Set Output Data Rate */ + config[0].cfg.acc.odr = BMI2_ACC_ODR_25HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* Gyro configuration settings. */ + /* Set Output Data Rate */ + config[1].cfg.gyr.odr = BMI2_GYR_ODR_25HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set new configurations. */ + rslt = bmi270_wh_set_sensor_config(config, 2, dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_watermark_headerless_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_watermark_headerless_mode/Makefile new file mode 100644 index 0000000000..ce97737eab --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_watermark_headerless_mode/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= fifo_watermark_headerless_mode.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_wh.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c new file mode 100644 index 0000000000..160c01de5b --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c @@ -0,0 +1,331 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_wh.h" +#include "common.h" + +/******************************************************************************/ +/*! Macros */ + +/*! Buffer size allocated to store raw FIFO data */ +#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) + +/*! Length of data to be read from FIFO */ +#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) + +/*! Number of accel frames to be extracted from FIFO */ + +/*! + * Calculation: + * fifo_watermark_level = 650, accel_frame_len = 6, gyro_frame_len = 6. + * fifo_accel_frame_count = (650 / (6 + 6 )) = 54 frames + */ +#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(55) + +/*! Number of gyro frames to be extracted from FIFO */ +#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(55) + +/*! Setting a watermark level in FIFO */ +#define BMI270_FIFO_WATERMARK_LEVEL UINT16_C(650) + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for accel and gyro. + * @param[in] dev : Structure instance of bmi2_dev. + * @return Status of execution. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to index bytes. */ + uint16_t index = 0; + + /* Variable to store the available frame length count in FIFO */ + uint8_t frame_count; + + uint16_t fifo_length = 0; + + uint16_t accel_frame_length; + + uint16_t gyro_frame_length; + + uint8_t try = 1; + + /* Number of bytes of FIFO data. */ + uint8_t fifo_data[BMI2_FIFO_RAW_DATA_BUFFER_SIZE] = { 0 }; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Array of accelerometer frames -> Total bytes = + * 50 * (6 axes bytes) = 300 bytes */ + struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; + + /* Array of gyro frames -> Total bytes = + * 50 * (6 axes bytes) = 300 bytes */ + struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; + + /* Initialize FIFO frame structure. */ + struct bmi2_fifo_frame fifoframe = { 0 }; + + /* Accel and gyro sensor are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; + + /* Variable to get fifo water-mark interrupt status. */ + uint16_t int_status = 0; + + uint16_t watermark = 0; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_wh. */ + rslt = bmi270_wh_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Enable accel and gyro sensor. */ + rslt = bmi270_wh_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Configuration settings for accel and gyro. */ + rslt = set_accel_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Before setting FIFO, disable the advance power save mode. */ + rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initially disable all configurations in fifo. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Update FIFO structure. */ + /* Mapping the buffer to store the fifo data. */ + fifoframe.data = fifo_data; + + /* Length of FIFO frame. */ + fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH; + + /* Set FIFO configuration by enabling accel, gyro. + * NOTE 1: The header mode is enabled by default. + * NOTE 2: By default the FIFO operating mode is FIFO mode. */ + rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("FIFO is configured in headerless mode\n"); + + /* To enable headerless mode, disable the header. */ + if (rslt == BMI2_OK) + { + rslt = bmi2_set_fifo_config(BMI2_FIFO_HEADER_EN, BMI2_DISABLE, &bmi2_dev); + } + + /* FIFO water-mark interrupt is enabled. */ + fifoframe.data_int_map = BMI2_FWM_INT; + + /* Map water-mark interrupt to the required interrupt pin. */ + rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Set water-mark level. */ + fifoframe.wm_lvl = BMI270_FIFO_WATERMARK_LEVEL; + + fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH; + + /* Set the water-mark level if water-mark interrupt is mapped. */ + rslt = bmi2_set_fifo_wm(fifoframe.wm_lvl, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + rslt = bmi2_get_fifo_wm(&watermark, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + printf("\nFIFO watermark level is %d\n", watermark); + + while (try <= 10) + { + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the status of FIFO watermark interrupt. */ + if ((rslt == BMI2_OK) && (int_status & BMI2_FWM_INT_STATUS_MASK)) + { + printf("\nIteration : %d\n", try); + + printf("\nWatermark interrupt occurred\n"); + + rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; + gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; + + printf("\nFIFO data bytes available : %d \n", fifo_length); + printf("\nFIFO data bytes requested : %d \n", fifoframe.length); + + /* Read FIFO data. */ + rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Read FIFO data on interrupt. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + printf("\nFIFO accel frames requested : %d \n", accel_frame_length); + + /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ + rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); + + printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); + + /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ + rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); + printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); + + /* Calculating the frame count from the available bytes in FIFO + * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len)) */ + frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH)); + printf("\nAvailable frame count from available bytes: %d\n", frame_count); + + printf("\nExracted accel frames\n"); + + if (accel_frame_length > frame_count) + { + accel_frame_length = frame_count; + } + + /* Print the parsed accelerometer data from the FIFO buffer. */ + for (index = 0; index < accel_frame_length; index++) + { + printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, + fifo_accel_data[index].y, fifo_accel_data[index].z); + } + + printf("\nExtracted gyro frames\n"); + + if (gyro_frame_length > frame_count) + { + gyro_frame_length = frame_count; + } + + /* Print the parsed gyro data from the FIFO buffer. */ + for (index = 0; index < gyro_frame_length; index++) + { + printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", + index, + fifo_gyro_data[index].x, + fifo_gyro_data[index].y, + fifo_gyro_data[index].z); + } + } + + try++; + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for accel. + */ +static int8_t set_accel_gyro_config(struct bmi2_dev *dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define accel and gyro configurations. */ + struct bmi2_sens_config config[2]; + + /* Configure the type of feature. */ + config[0].type = BMI2_ACCEL; + config[1].type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_wh_get_sensor_config(config, 2, dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* NOTE: The user can change the following configuration parameter according to their requirement. */ + /* Accel configuration settings. */ + /* Set Output Data Rate */ + config[0].cfg.acc.odr = BMI2_ACC_ODR_100HZ; + + /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ + config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* The bandwidth parameter is used to configure the number of sensor samples that are averaged + * if it is set to 2, then 2^(bandwidth parameter) samples + * are averaged, resulting in 4 averaged samples + * Note1 : For more information, refer the datasheet. + * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but + * this has an adverse effect on the power consumed. + */ + config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; + + /* Enable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + * For more info refer datasheet. + */ + config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + + /* Gyro configuration settings. */ + /* Set Output Data Rate */ + config[1].cfg.gyr.odr = BMI2_GYR_ODR_100HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set new configurations. */ + rslt = bmi270_wh_set_sensor_config(config, 2, dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/gyro/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/gyro/Makefile new file mode 100644 index 0000000000..9192877d58 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/gyro/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= gyro.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_wh.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/gyro/gyro.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/gyro/gyro.c new file mode 100644 index 0000000000..d1ce9fe16f --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/gyro/gyro.c @@ -0,0 +1,195 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_wh.h" +#include "common.h" + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for gyro. + * + * @param[in] dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t set_gyro_config(struct bmi2_dev *dev); + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + * + * @param[in] val : LSB from each axis. + * @param[in] dps : Degree per second. + * @param[in] bit_width : Resolution for gyro. + * + * @return Degree per second. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to define limit to print gyro data. */ + uint8_t limit = 10; + + uint8_t indx = 0; + + float x = 0, y = 0, z = 0; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Create an instance of sensor data structure. */ + struct bmi2_sensor_data sensor_data = { 0 }; + + /* Assign gyro sensor to variable. */ + uint8_t sens_list = BMI2_GYRO; + + /* Initialize the interrupt status of gyro. */ + uint16_t int_status = 0; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_wh. */ + rslt = bmi270_wh_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the selected sensors. */ + rslt = bmi270_wh_sensor_enable(&sens_list, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Gyro configuration settings. */ + rslt = set_gyro_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Select gyro sensor. */ + sensor_data.type = BMI2_GYRO; + printf("Gyro and DPS data\n"); + printf("Gyro data collected at Range 2000 dps with 16-bit resolution\n"); + + /* Loop to print gyro data when interrupt occurs. */ + while (indx <= limit) + { + /* To get the data ready interrupt status of gyro. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the data ready interrupt status and print the status for 10 samples. */ + if (int_status & BMI2_GYR_DRDY_INT_MASK) + { + /* Get gyro data for x, y and z axis. */ + rslt = bmi270_wh_get_sensor_data(&sensor_data, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("\nGyr_X = %d\t", sensor_data.sens_data.gyr.x); + printf("Gyr_Y = %d\t", sensor_data.sens_data.gyr.y); + printf("Gyr_Z = %d\r\n", sensor_data.sens_data.gyr.z); + + /* Converting lsb to degree per second for 16 bit gyro at 2000dps range. */ + x = lsb_to_dps(sensor_data.sens_data.gyr.x, 2000, bmi2_dev.resolution); + y = lsb_to_dps(sensor_data.sens_data.gyr.y, 2000, bmi2_dev.resolution); + z = lsb_to_dps(sensor_data.sens_data.gyr.z, 2000, bmi2_dev.resolution); + + /* Print the data in dps. */ + printf("Gyr_DPS_X = %4.2f, Gyr_DPS_Y = %4.2f, Gyr_DPS_Z = %4.2f\n", x, y, z); + + indx++; + } + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for gyro. + */ +static int8_t set_gyro_config(struct bmi2_dev *dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define the type of sensor and its configurations. */ + struct bmi2_sens_config config; + + /* Configure the type of feature. */ + config.type = BMI2_GYRO; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_wh_get_sensor_config(&config, 1, dev); + bmi2_error_codes_print_result(rslt); + + /* Map data ready interrupt to interrupt pin. */ + rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT2, dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* The user can change the following configuration parameters according to their requirement. */ + /* Set Output Data Rate */ + config.cfg.gyr.odr = BMI2_GYR_ODR_200HZ; + + /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ + config.cfg.gyr.range = BMI2_GYR_RANGE_2000; + + /* Gyroscope bandwidth parameters. By default the gyro bandwidth is in normal mode. */ + config.cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; + + /* Enable/Disable the noise performance mode for precision yaw rate sensing + * There are two modes + * 0 -> Ultra low power mode(Default) + * 1 -> High performance mode + */ + config.cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; + + /* Enable/Disable the filter performance mode where averaging of samples + * will be done based on above set bandwidth and ODR. + * There are two modes + * 0 -> Ultra low power mode + * 1 -> High performance mode(Default) + */ + config.cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + + /* Set the gyro configurations. */ + rslt = bmi270_wh_set_sensor_config(&config, 1, dev); + } + + return rslt; +} + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (dps / ((half_scale) + BMI2_GYR_RANGE_2000)) * (val); +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/no_motion_interrupt/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/no_motion_interrupt/Makefile new file mode 100644 index 0000000000..61b7016bb2 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/no_motion_interrupt/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= no_motion_interrupt.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_wh.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/no_motion_interrupt/no_motion_interrupt.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/no_motion_interrupt/no_motion_interrupt.c new file mode 100644 index 0000000000..5f584de28c --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/no_motion_interrupt/no_motion_interrupt.c @@ -0,0 +1,135 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_wh.h" +#include "common.h" + +/******************************************************************************/ +/*! Static Function Declaration */ + +/*! + * @brief This internal API is used to set configurations for no-motion. + * + * @param[in] bmi2_dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t set_feature_config(struct bmi2_dev *bmi2_dev); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Accel sensor and no-motion feature are listed in array. */ + uint8_t sens_list[2] = { BMI2_ACCEL, BMI2_NO_MOTION }; + + /* Variable to get no-motion interrupt status. */ + uint16_t int_status = 0; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Select features and their pins to be mapped to. */ + struct bmi2_sens_int_config sens_int = { .type = BMI2_NO_MOTION, .hw_int_pin = BMI2_INT1 }; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_wh. */ + rslt = bmi270_wh_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the selected sensors. */ + rslt = bmi270_wh_sensor_enable(sens_list, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Set feature configurations for no-motion. */ + rslt = set_feature_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Map the feature interrupt for no-motion. */ + rslt = bmi270_wh_map_feat_int(&sens_int, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + printf("Do not move the board\n"); + + /* Loop to get no-motion interrupt. */ + do + { + /* Clear buffer. */ + int_status = 0; + + /* To get the interrupt status of no-motion. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the interrupt status of no-motion. */ + if (int_status & BMI270_WH_NO_MOT_STATUS_MASK) + { + printf("No-motion interrupt is generated\n"); + break; + } + } while (rslt == BMI2_OK); + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for no-motion. + */ +static int8_t set_feature_config(struct bmi2_dev *bmi2_dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define the type of sensor and its configurations. */ + struct bmi2_sens_config config; + + /* Configure the type of feature. */ + config.type = BMI2_NO_MOTION; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_wh_get_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + if (rslt == BMI2_OK) + { + + /* NOTE: The user can change the following configuration parameters according to their requirement. */ + /* 1LSB equals 20ms. Default is 100ms, setting to 80ms. */ + config.cfg.no_motion.duration = 0x04; + + /* 1LSB equals to 0.48mg. Default is 83mg, setting to 50mg. */ + config.cfg.no_motion.threshold = 0x68; + + /* Set new configurations. */ + rslt = bmi270_wh_set_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/read_aux_data_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/read_aux_data_mode/Makefile new file mode 100644 index 0000000000..b995b42dd9 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/read_aux_data_mode/Makefile @@ -0,0 +1,23 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= read_aux_data_mode.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +BMM150_SOURCE ?= ../../../bmm150 + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_wh.c \ +$(BMM150_SOURCE)/bmm150.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(BMM150_SOURCE) \ +$(COMMON_LOCATION)/common + + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/read_aux_data_mode/read_aux_data_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/read_aux_data_mode/read_aux_data_mode.c new file mode 100644 index 0000000000..6d8cd67f35 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/read_aux_data_mode/read_aux_data_mode.c @@ -0,0 +1,327 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_wh.h" +#include "bmm150.h" +#include "coines.h" +#include "common.h" + +/******************************************************************************/ +/*! Macro definition */ + +/*! Macros to select the sensors */ +#define ACCEL UINT8_C(0x00) +#define GYRO UINT8_C(0x01) +#define AUX UINT8_C(0x02) + +/*! Earth's gravity in m/s^2 */ +#define GRAVITY_EARTH (9.80665f) + +/*****************************************************************************/ +/*! Structure declaration */ + +/* Sensor initialization configuration. */ +struct bmi2_dev bmi2_dev; + +/*******************************************************************************/ +/*! Functions */ + +/** + * user_aux_read - Reads data from auxiliary sensor in manual mode. + * + * @param[in] reg_addr : Register address. + * @param[out] aux_data : Aux data pointer to store the read data. + * @param[in] length : No of bytes to read. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval 0 -> Success + * @retval < 0 -> Failure Info + */ +static int8_t user_aux_read(uint8_t reg_addr, uint8_t *aux_data, uint32_t len, void *intf_ptr); + +/** + * user_aux_write - Writes data to the auxiliary sensor in manual mode. + * + * @param[in] reg_addr : Register address. + * @param[out] aux_data : Aux data pointer to store the data being written. + * @param[in] length : No of bytes to read. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval 0 -> Success + * @retval < 0 -> Failure Info + */ +static int8_t user_aux_write(uint8_t reg_addr, const uint8_t *aux_data, uint32_t len, void *intf_ptr); + +/*! + * @brief This function provides the delay for required time (Microsecond) as per the input provided in some of the + * APIs. + * + * @param[in] period_us : The required wait time in microsecond. + * @param[in] intf_ptr : Interface pointer + * + * @return void. + */ +static void user_delay_us(uint32_t period, void *intf_ptr); + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + * + * @param[in] val : LSB from each axis. + * @param[in] g_range : Gravity range. + * @param[in] bit_width : Resolution for accel. + * + * @return Gravity. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width); + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + * + * @param[in] val : LSB from each axis. + * @param[in] dps : Degree per second. + * @param[in] bit_width : Resolution for gyro. + * + * @return Degree per second. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width); + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to select the pull-up resistor which is set to trim register */ + uint8_t regdata; + + /* Variable to define limit to print aux data. */ + uint8_t limit = 20; + + /* Accel, Gyro and Aux sensors are listed in array. */ + uint8_t sensor_list[3] = { BMI2_ACCEL, BMI2_GYRO, BMI2_AUX }; + + /* Structure to define the type of the sensor and its configurations. */ + struct bmi2_sens_config config[3]; + + /* Sensor initialization configuration. */ + struct bmm150_dev bmm150_dev; + + /* bmm150 settings configuration */ + struct bmm150_settings settings; + + /* bmm150 magnetometer data */ + struct bmm150_mag_data mag_data; + + /* Structure to define type of sensor and their respective data. */ + struct bmi2_sensor_data sensor_data[3]; + + /* Variables to define read the accel and gyro data in float */ + float x = 0, y = 0, z = 0; + + config[ACCEL].type = BMI2_ACCEL; + config[GYRO].type = BMI2_GYRO; + config[AUX].type = BMI2_AUX; + + /* To enable the i2c interface settings for bmm150. */ + uint8_t bmm150_dev_addr = BMM150_DEFAULT_I2C_ADDRESS; + bmm150_dev.intf_ptr = &bmm150_dev_addr; + bmm150_dev.read = user_aux_read; + bmm150_dev.write = user_aux_write; + bmm150_dev.delay_us = user_delay_us; + + /* As per datasheet, aux interface with bmi270_wh will support only for I2C */ + bmm150_dev.intf = BMM150_I2C_INTF; + + sensor_data[ACCEL].type = BMI2_ACCEL; + sensor_data[GYRO].type = BMI2_GYRO; + sensor_data[AUX].type = BMI2_AUX; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_wh. */ + rslt = bmi270_wh_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Pull-up resistor 2k is set to the trim regiter */ + regdata = BMI2_ASDA_PUPSEL_2K; + rslt = bmi2_set_regs(BMI2_AUX_IF_TRIM, ®data, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Enable the accel, gyro and aux sensor. */ + rslt = bmi270_wh_sensor_enable(sensor_list, 3, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_wh_get_sensor_config(config, 3, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Configurations for accel. */ + config[ACCEL].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + config[ACCEL].cfg.acc.bwp = BMI2_ACC_OSR2_AVG2; + config[ACCEL].cfg.acc.odr = BMI2_ACC_ODR_100HZ; + config[ACCEL].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* Configurations for gyro. */ + config[GYRO].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + config[GYRO].cfg.gyr.noise_perf = BMI2_GYR_RANGE_2000; + config[GYRO].cfg.gyr.bwp = BMI2_GYR_OSR2_MODE; + config[GYRO].cfg.gyr.odr = BMI2_GYR_ODR_100HZ; + config[GYRO].cfg.gyr.range = BMI2_GYR_RANGE_2000; + config[GYRO].cfg.gyr.ois_range = BMI2_GYR_OIS_2000; + + /* Configurations for aux. */ + config[AUX].cfg.aux.odr = BMI2_AUX_ODR_100HZ; + config[AUX].cfg.aux.aux_en = BMI2_ENABLE; + config[AUX].cfg.aux.i2c_device_addr = BMM150_DEFAULT_I2C_ADDRESS; + config[AUX].cfg.aux.manual_en = BMI2_ENABLE; + config[AUX].cfg.aux.fcu_write_en = BMI2_ENABLE; + config[AUX].cfg.aux.man_rd_burst = BMI2_AUX_READ_LEN_3; + config[AUX].cfg.aux.read_addr = BMM150_REG_DATA_X_LSB; + + /* Set new configurations for accel, gyro and aux. */ + rslt = bmi270_wh_set_sensor_config(config, 3, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmm150. */ + rslt = bmm150_init(&bmm150_dev); + bmi2_error_codes_print_result(rslt); + + /* Set the power mode to normal mode. */ + settings.pwr_mode = BMM150_POWERMODE_NORMAL; + rslt = bmm150_set_op_mode(&settings, &bmm150_dev); + bmi2_error_codes_print_result(rslt); + + rslt = bmi270_wh_get_sensor_config(config, 3, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Disable manual mode so that the data mode is enabled. */ + config[AUX].cfg.aux.manual_en = BMI2_DISABLE; + + /* Set the aux configurations. */ + rslt = bmi270_wh_set_sensor_config(config, 3, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("MAGNETOMETER, ACCEL, AND GYRO DATA IN DATA MODE\n"); + + if (bmm150_dev.chip_id == BMM150_CHIP_ID) + { + while (limit--) + { + /* Delay has been added to get aux, accel and gyro data at the interval of every 0.05 second. */ + bmi2_dev.delay_us(50000, bmi2_dev.intf_ptr); + + rslt = bmi270_wh_get_sensor_data(sensor_data, 3, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + printf("\nAcc_x = %d\t", sensor_data[ACCEL].sens_data.acc.x); + printf("Acc_y = %d\t", sensor_data[ACCEL].sens_data.acc.y); + printf("Acc_z = %d", sensor_data[ACCEL].sens_data.acc.z); + + /* Converting lsb to meter per second squared for 16 bit accelerometer at 2G range. */ + x = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.x, 2, bmi2_dev.resolution); + y = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.y, 2, bmi2_dev.resolution); + z = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.z, 2, bmi2_dev.resolution); + + /* Print the data in m/s2. */ + printf("\nAcc_ms2_X = %4.2f, Acc_ms2_Y = %4.2f, Acc_ms2_Z = %4.2f\n", x, y, z); + + printf("\nGyr_X = %d\t", sensor_data[GYRO].sens_data.gyr.x); + printf("Gyr_Y = %d\t", sensor_data[GYRO].sens_data.gyr.y); + printf("Gyr_Z= %d", sensor_data[GYRO].sens_data.gyr.z); + + /* Converting lsb to degree per second for 16 bit gyro at 2000dps range. */ + x = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.x, 2000, bmi2_dev.resolution); + y = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.y, 2000, bmi2_dev.resolution); + z = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.z, 2000, bmi2_dev.resolution); + + /* Print the data in dps. */ + printf("\nGyro_DPS_X = %4.2f, Gyro_DPS_Y = %4.2f, Gyro_DPS_Z = %4.2f\n", x, y, z); + + /* Compensating the raw auxiliary data available from the BMM150 API. */ + rslt = bmm150_aux_mag_data(sensor_data[AUX].sens_data.aux_data, &mag_data, &bmm150_dev); + bmi2_error_codes_print_result(rslt); + printf("\nMag_x_axis = %d \t Mag_y_axis = %d \t Mag_z_axis = %d \t\n", + mag_data.x, + mag_data.y, + mag_data.z); + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This function reads the data from auxiliary sensor in manual mode. + */ +static int8_t user_aux_read(uint8_t reg_addr, uint8_t *aux_data, uint32_t len, void *intf_ptr) +{ + int8_t rslt; + + /* Discarding the parameter id as it is redundant */ + rslt = bmi2_read_aux_man_mode(reg_addr, aux_data, len, &bmi2_dev); + + return rslt; +} + +/*! + * @brief This function writes the data to auxiliary sensor in manual mode. + */ +static int8_t user_aux_write(uint8_t reg_addr, const uint8_t *aux_data, uint32_t len, void *intf_ptr) +{ + int8_t rslt; + + /* Discarding the parameter id as it is redundant */ + rslt = bmi2_write_aux_man_mode(reg_addr, aux_data, len, &bmi2_dev); + + return rslt; +} + +/*! + * Delay function map to COINES platform + */ +static void user_delay_us(uint32_t period, void *intf_ptr) +{ + coines_delay_usec(period); +} + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (GRAVITY_EARTH * val * g_range) / half_scale; +} + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (dps / ((half_scale) + BMI2_GYR_RANGE_2000)) * (val); +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/read_aux_manual_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/read_aux_manual_mode/Makefile new file mode 100644 index 0000000000..843a2ce8c7 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/read_aux_manual_mode/Makefile @@ -0,0 +1,23 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= read_aux_manual_mode.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +BMM150_SOURCE ?= ../../../bmm150 + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_wh.c \ +$(BMM150_SOURCE)/bmm150.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(BMM150_SOURCE) \ +$(COMMON_LOCATION)/common + + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/read_aux_manual_mode/read_aux_manual_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/read_aux_manual_mode/read_aux_manual_mode.c new file mode 100644 index 0000000000..524eb8d2ff --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/read_aux_manual_mode/read_aux_manual_mode.c @@ -0,0 +1,322 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_wh.h" +#include "bmm150.h" +#include "coines.h" +#include "common.h" + +/******************************************************************************/ +/*! Macro definition */ + +/*! Macros to select the sensors */ +#define ACCEL UINT8_C(0x00) +#define GYRO UINT8_C(0x01) +#define AUX UINT8_C(0x02) + +/*! Earth's gravity in m/s^2 */ +#define GRAVITY_EARTH (9.80665f) + +/*****************************************************************************/ +/*! Structure declaration */ + +/* Sensor initialization configuration. */ +struct bmi2_dev bmi2_dev; + +/******************************************************************************/ +/*! Functions */ + +/** + * user_aux_read - Reads data from auxiliary sensor in manual mode. + * + * @param[in] reg_addr : Register address. + * @param[out] aux_data : Aux data pointer to store the read data. + * @param[in] length : No of bytes to read. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval 0 -> Success + * @retval < 0 -> Failure Info + */ +static int8_t user_aux_read(uint8_t reg_addr, uint8_t *aux_data, uint32_t len, void *intf_ptr); + +/** + * user_aux_write - Writes data to the auxiliary sensor in manual mode. + * + * @param[in] reg_addr : Register address. + * @param[out] aux_data : Aux data pointer to store the data being written. + * @param[in] length : No of bytes to read. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval 0 -> Success + * @retval < 0 -> Failure Info + */ +static int8_t user_aux_write(uint8_t reg_addr, const uint8_t *aux_data, uint32_t len, void *intf_ptr); + +/*! + * @brief This function provides the delay for required time (Microsecond) as per the input provided in some of the + * APIs. + * + * @param[in] period_us : The required wait time in microsecond. + * @param[in] intf_ptr : Interface pointer + * + * @return void. + */ +static void user_delay_us(uint32_t period, void *intf_ptr); + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + * + * @param[in] val : LSB from each axis. + * @param[in] g_range : Gravity range. + * @param[in] bit_width : Resolution for accel. + * + * @return Gravity. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width); + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + * + * @param[in] val : LSB from each axis. + * @param[in] dps : Degree per second. + * @param[in] bit_width : Resolution for gyro. + * + * @return Degree per second. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width); + +/* This function starts the execution of program. */ +int main(void) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Variable to select the pull-up resistor which is set to trim register */ + uint8_t regdata; + + /* Variable to define limit to print aux data. */ + uint8_t limit = 20; + + /* Accel, Gyro and Aux sensors are listed in array. */ + uint8_t sensor_list[3] = { BMI2_ACCEL, BMI2_GYRO, BMI2_AUX }; + + /* Sensor initialization configuration. */ + struct bmm150_dev bmm150_dev; + + /* bmm150 settings configuration */ + struct bmm150_settings settings; + + /* bmm150 magnetometer data */ + struct bmm150_mag_data mag_data; + + /* Structure to define the type of the sensor and its configurations. */ + struct bmi2_sens_config config[3]; + + /* Structure to define type of sensor and their respective data. */ + struct bmi2_sensor_data sensor_data[2]; + + /* Variables to define read the accel and gyro data in float */ + float x = 0, y = 0, z = 0; + + config[ACCEL].type = BMI2_ACCEL; + config[GYRO].type = BMI2_GYRO; + config[AUX].type = BMI2_AUX; + + sensor_data[ACCEL].type = BMI2_ACCEL; + sensor_data[GYRO].type = BMI2_GYRO; + + /* Array of eight bytes to store x, y, z and r axis aux data. */ + uint8_t aux_data[8] = { 0 }; + + /* To enable the i2c interface settings for bmm150. */ + uint8_t bmm150_dev_addr = BMM150_DEFAULT_I2C_ADDRESS; + bmm150_dev.intf_ptr = &bmm150_dev_addr; + bmm150_dev.read = user_aux_read; + bmm150_dev.write = user_aux_write; + bmm150_dev.delay_us = user_delay_us; + + /* As per datasheet, aux interface with bmi270_wh will support only for I2C */ + bmm150_dev.intf = BMM150_I2C_INTF; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_SPI_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_wh. */ + rslt = bmi270_wh_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Pull-up resistor 2k is set to the trim register */ + regdata = BMI2_ASDA_PUPSEL_2K; + rslt = bmi2_set_regs(BMI2_AUX_IF_TRIM, ®data, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Enable the accel, gyro and aux sensor. */ + rslt = bmi270_wh_sensor_enable(sensor_list, 3, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_wh_get_sensor_config(config, 3, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Configurations for accel. */ + config[ACCEL].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; + config[ACCEL].cfg.acc.bwp = BMI2_ACC_OSR2_AVG2; + config[ACCEL].cfg.acc.odr = BMI2_ACC_ODR_100HZ; + config[ACCEL].cfg.acc.range = BMI2_ACC_RANGE_2G; + + /* Configurations for gyro. */ + config[GYRO].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; + config[GYRO].cfg.gyr.noise_perf = BMI2_GYR_RANGE_2000; + config[GYRO].cfg.gyr.bwp = BMI2_GYR_OSR2_MODE; + config[GYRO].cfg.gyr.odr = BMI2_GYR_ODR_100HZ; + config[GYRO].cfg.gyr.range = BMI2_GYR_RANGE_2000; + config[GYRO].cfg.gyr.ois_range = BMI2_GYR_OIS_2000; + + /* Configurations for aux. */ + config[AUX].cfg.aux.odr = BMI2_AUX_ODR_100HZ; + config[AUX].cfg.aux.aux_en = BMI2_ENABLE; + config[AUX].cfg.aux.i2c_device_addr = BMM150_DEFAULT_I2C_ADDRESS; + config[AUX].cfg.aux.manual_en = BMI2_ENABLE; + config[AUX].cfg.aux.fcu_write_en = BMI2_ENABLE; + config[AUX].cfg.aux.man_rd_burst = BMI2_AUX_READ_LEN_3; + + /* Set new configurations for accel, gyro and aux. */ + rslt = bmi270_wh_set_sensor_config(config, 3, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmm150. */ + rslt = bmm150_init(&bmm150_dev); + bmi2_error_codes_print_result(rslt); + + /* Set the power mode to normal mode. */ + settings.pwr_mode = BMM150_POWERMODE_NORMAL; + rslt = bmm150_set_op_mode(&settings, &bmm150_dev); + bmi2_error_codes_print_result(rslt); + + printf("MAGNETOMETER, ACCEL AND GYRO DATA IN MANUAL MODE\n"); + + if (bmm150_dev.chip_id == BMM150_CHIP_ID) + { + while (limit--) + { + /* Delay has been added to get aux, accel and gyro data at the interval of every 0.05 second. */ + bmi2_dev.delay_us(50000, bmi2_dev.intf_ptr); + + rslt = bmi270_wh_get_sensor_data(sensor_data, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + printf("\nAcc_x = %d\t", sensor_data[ACCEL].sens_data.acc.x); + printf("Acc_y = %d\t", sensor_data[ACCEL].sens_data.acc.y); + printf("Acc_z = %d", sensor_data[ACCEL].sens_data.acc.z); + + /* Converting lsb to meter per second squared for 16 bit accelerometer at 2G range. */ + x = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.x, 2, bmi2_dev.resolution); + y = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.y, 2, bmi2_dev.resolution); + z = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.z, 2, bmi2_dev.resolution); + + /* Print the data in m/s2. */ + printf("\nAcc_ms2_X = %4.2f, Acc_ms2_Y = %4.2f, Acc_ms2_Z = %4.2f\n", x, y, z); + + printf("\nGyr_X = %d\t", sensor_data[GYRO].sens_data.gyr.x); + printf("Gyr_Y = %d\t", sensor_data[GYRO].sens_data.gyr.y); + printf("Gyr_Z = %d", sensor_data[GYRO].sens_data.gyr.z); + + /* Converting lsb to degree per second for 16 bit gyro at 2000dps range. */ + x = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.x, 2000, bmi2_dev.resolution); + y = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.y, 2000, bmi2_dev.resolution); + z = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.z, 2000, bmi2_dev.resolution); + + /* Print the data in dps. */ + printf("\nGyro_DPS_X = %4.2f, Gyro_DPS_Y = %4.2f, Gyro_DPS_Z = %4.2f\n", x, y, z); + } + + /* Read aux data from the bmm150 data registers. */ + rslt = bmi2_read_aux_man_mode(BMM150_REG_DATA_X_LSB, aux_data, 8, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + if (rslt == BMI2_OK) + { + /* Compensating the raw auxiliary data available from the BMM150 API. */ + rslt = bmm150_aux_mag_data(aux_data, &mag_data, &bmm150_dev); + bmi2_error_codes_print_result(rslt); + printf("Mag_x_axis = %d \t Mag_y_axis = %d \t Mag_z_axis = %d \t\n", mag_data.x, mag_data.y, + mag_data.z); + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This function reads the data from auxiliary sensor in manual mode. + */ +int8_t user_aux_read(uint8_t reg_addr, uint8_t *aux_data, uint32_t len, void *intf_ptr) +{ + int8_t rslt; + + /* Discarding the parameter id as it is redundant */ + rslt = bmi2_read_aux_man_mode(reg_addr, aux_data, len, &bmi2_dev); + + return rslt; +} + +/*! + * @brief This function writes the data to auxiliary sensor in manual mode. + */ +int8_t user_aux_write(uint8_t reg_addr, const uint8_t *aux_data, uint32_t len, void *intf_ptr) +{ + int8_t rslt; + + /* Discarding the parameter id as it is redundant */ + rslt = bmi2_write_aux_man_mode(reg_addr, aux_data, len, &bmi2_dev); + + return rslt; +} + +/*! + * Delay function map to COINES platform + */ +static void user_delay_us(uint32_t period, void *intf_ptr) +{ + coines_delay_usec(period); +} + +/*! + * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at + * range 2G, 4G, 8G or 16G. + */ +static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (GRAVITY_EARTH * val * g_range) / half_scale; +} + +/*! + * @brief This function converts lsb to degree per second for 16 bit gyro at + * range 125, 250, 500, 1000 or 2000dps. + */ +static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width) +{ + float half_scale = ((float)(1 << bit_width) / 2.0f); + + return (dps / ((half_scale) + BMI2_GYR_RANGE_2000)) * (val); +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/step_activity/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/step_activity/Makefile new file mode 100644 index 0000000000..a4fcf100cd --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/step_activity/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= step_activity.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_wh.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/step_activity/step_activity.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/step_activity/step_activity.c new file mode 100644 index 0000000000..0c3e9c7558 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/step_activity/step_activity.c @@ -0,0 +1,110 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_wh.h" +#include "common.h" + +/******************************************************************************/ +/*! Functions */ +/* This function starts the execution of program. */ +int main(void) +{ + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Structure to define the type of sensor and their respective data. */ + struct bmi2_sensor_data sensor_data; + + /* Structure to define the type of sensor and its configurations. */ + struct bmi2_sens_config config; + + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Accel sensor and step activity feature are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_STEP_ACTIVITY }; + + /* Type of sensor to get step activity data. */ + sensor_data.type = BMI2_STEP_ACTIVITY; + + /* Variable to get step activity interrupt status. */ + uint16_t int_status = 0; + + uint16_t loop = 10; + + /* Select features and their pins to be mapped to. */ + struct bmi2_sens_int_config sens_int = { .type = BMI2_STEP_ACTIVITY, .hw_int_pin = BMI2_INT2 }; + + /* The step activities are listed in array. */ + const char *activity_output[4] = { "BMI2_STILL", "BMI2_WALK", "BMI2_RUN", "BMI2_UNKNOWN" }; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270. */ + rslt = bmi270_wh_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the selected sensor. */ + rslt = bmi270_wh_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Configure the type of sensor. */ + config.type = BMI2_STEP_ACTIVITY; + + if (rslt == BMI2_OK) + { + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_wh_get_sensor_config(&config, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Map the feature interrupt for step activity. */ + rslt = bmi270_wh_map_feat_int(&sens_int, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("\nMove the board in steps to perform step activity:\n"); + + /* Loop to get step activity. */ + while (loop) + { + /* To get the interrupt status of step detector. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the interrupt status of step detector. */ + if (int_status & BMI270_WH_STEP_ACT_STATUS_MASK) + { + printf("Step detected\n"); + + /* Get step activity output. */ + rslt = bmi270_wh_get_sensor_data(&sensor_data, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Print the step activity output. */ + printf("Step activity = %s\n", activity_output[sensor_data.sens_data.activity_output]); + + loop--; + } + } + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/step_counter/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/step_counter/Makefile new file mode 100644 index 0000000000..1c16cc62a2 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/step_counter/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= step_counter.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_wh.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/step_counter/step_counter.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/step_counter/step_counter.c new file mode 100644 index 0000000000..8d5f968fb7 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/step_counter/step_counter.c @@ -0,0 +1,146 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_wh.h" +#include "common.h" + +/******************************************************************************/ +/*! Static function declaration */ + +/*! + * @brief This internal API is used to set configurations for step counter. + * + * @param[in] bmi2_dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t set_feature_config(struct bmi2_dev *bmi2_dev); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program. */ +int main(void) +{ + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Structure to define type of sensor and their respective data. */ + struct bmi2_sensor_data sensor_data; + + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Accel sensor and step counter feature are listed in array. */ + uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_STEP_COUNTER }; + + /* Variable to get step counter interrupt status. */ + uint16_t int_status = 0; + + /* Select features and their pins to be mapped to. */ + struct bmi2_sens_int_config sens_int = { .type = BMI2_STEP_COUNTER, .hw_int_pin = BMI2_INT2 }; + + /* Type of sensor to get step counter data. */ + sensor_data.type = BMI2_STEP_COUNTER; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_wh. */ + rslt = bmi270_wh_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Enable the selected sensor. */ + rslt = bmi270_wh_sensor_enable(sensor_sel, 2, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Set the feature configuration for step counter. */ + rslt = set_feature_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + printf("Step counter watermark level set to 1 (20 steps)\n"); + + if (rslt == BMI2_OK) + { + /* Map the step counter feature interrupt. */ + rslt = bmi270_wh_map_feat_int(&sens_int, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Move the board in steps for 20 times to get step counter interrupt. */ + printf("Move the board in steps\n"); + + /* Loop to get number of steps counted. */ + do + { + /* To get the interrupt status of the step counter. */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the interrupt status of the step counter. */ + if (int_status & BMI270_WH_STEP_CNT_STATUS_MASK) + { + printf("Step counter interrupt occurred when watermark level (20 steps) is reached\n"); + + /* Get step counter output. */ + rslt = bmi270_wh_get_sensor_data(&sensor_data, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* Print the step counter output. */ + printf("No of steps counted = %ld", sensor_data.sens_data.step_counter_output); + break; + } + } while (rslt == BMI2_OK); + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API is used to set configurations for step counter. + */ +static int8_t set_feature_config(struct bmi2_dev *bmi2_dev) +{ + /* Status of api are returned to this variable. */ + int8_t rslt; + + /* Structure to define the type of sensor and its configurations. */ + struct bmi2_sens_config config; + + /* Configure the type of sensor. */ + config.type = BMI2_STEP_COUNTER; + + /* Get default configurations for the type of feature selected. */ + rslt = bmi270_wh_get_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Setting water-mark level to 1 for step counter to get interrupt after 20 step counts. Every 20 steps once + * output triggers. */ + config.cfg.step_counter.watermark_level = 1; + + /* Set new configuration. */ + rslt = bmi270_wh_set_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/wrist_wear_wakeup/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/wrist_wear_wakeup/Makefile new file mode 100644 index 0000000000..65d3561d83 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/wrist_wear_wakeup/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= wrist_wear_wakeup.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi270_wh.c \ +$(COMMON_LOCATION)/common/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/wrist_wear_wakeup/wrist_wear_wakeup.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/wrist_wear_wakeup/wrist_wear_wakeup.c new file mode 100644 index 0000000000..bee1191d94 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/wrist_wear_wakeup/wrist_wear_wakeup.c @@ -0,0 +1,131 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +/*! Header Files */ +#include +#include "bmi270_wh.h" +#include "common.h" + +/******************************************************************************/ +/*! Static function declaration */ + +/*! + * @brief This internal API is used to set the sensor configuration. + * + * @param[in] bmi2_dev : Structure instance of bmi2_dev. + * + * @return Status of execution. + */ +static int8_t bmi2_set_config(struct bmi2_dev *bmi2_dev); + +/******************************************************************************/ +/*! Functions */ + +/* This function starts the execution of program */ +int main(void) +{ + /* Variable to define result */ + int8_t rslt; + + /* Initialize status of wrist wear wakeup interrupt */ + uint16_t int_status = 0; + + /* Sensor initialization configuration. */ + struct bmi2_dev bmi2_dev; + + /* Select features and their pins to be mapped to */ + struct bmi2_sens_int_config sens_int = { .type = BMI2_WRIST_WEAR_WAKE_UP_WH, .hw_int_pin = BMI2_INT1 }; + + /* Interface reference is given as a parameter + * For I2C : BMI2_I2C_INTF + * For SPI : BMI2_SPI_INTF + */ + rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); + bmi2_error_codes_print_result(rslt); + + /* Initialize bmi270_wh. */ + rslt = bmi270_wh_init(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Set the sensor configuration */ + rslt = bmi2_set_config(&bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Map the feature interrupt */ + rslt = bmi270_wh_map_feat_int(&sens_int, 1, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + printf("Tilt the board in landscape position to trigger wrist wear wakeup\n"); + + /* Loop to print the wrist wear wakeup data when interrupt occurs */ + while (1) + { + int_status = 0; + + /* To get the interrupt status of the wrist wear wakeup */ + rslt = bmi2_get_int_status(&int_status, &bmi2_dev); + bmi2_error_codes_print_result(rslt); + + /* To check the interrupt status of the wrist gesture */ + if ((rslt == BMI2_OK) && (int_status & BMI270_WH_WRIST_WEAR_WAKEUP_WH_STATUS_MASK)) + { + printf("Wrist wear wakeup detected\n"); + break; + } + } + } + } + } + + bmi2_coines_deinit(); + + return rslt; +} + +/*! + * @brief This internal API sets the sensor configuration + */ +static int8_t bmi2_set_config(struct bmi2_dev *bmi2_dev) +{ + /* Variable to define result */ + int8_t rslt; + + /* List the sensors which are required to enable */ + uint8_t sens_list[2] = { BMI2_ACCEL, BMI2_WRIST_WEAR_WAKE_UP_WH }; + + /* Structure to define the type of the sensor and its configurations */ + struct bmi2_sens_config config; + + /* Configure type of feature */ + config.type = BMI2_WRIST_WEAR_WAKE_UP_WH; + + /* Enable the selected sensors */ + rslt = bmi270_wh_sensor_enable(sens_list, 2, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Get default configurations for the type of feature selected */ + rslt = bmi270_wh_get_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + + if (rslt == BMI2_OK) + { + /* Set the new configuration */ + rslt = bmi270_wh_set_sensor_config(&config, 1, bmi2_dev); + bmi2_error_codes_print_result(rslt); + } + } + + return rslt; +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi2_ois/common/common.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi2_ois/common/common.c new file mode 100644 index 0000000000..1a43947a99 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi2_ois/common/common.c @@ -0,0 +1,153 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +#include +#include +#include + +#include "coines.h" +#include "common.h" + +/******************************************************************************/ +/*! Global declaration */ + +/*! Device address for primary and secondary interface */ +static uint8_t ois_dev_addr; + +/******************************************************************************/ +/*! Functions */ + +/*! + * SPI read function map to COINES platform + */ +int8_t bmi2_ois_spi_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr) +{ + uint8_t dev_addr = *(uint8_t*)intf_ptr; + + return coines_read_spi(dev_addr, reg_addr, reg_data, (uint16_t)len); +} + +/*! + * SPI write function map to COINES platform + */ +int8_t bmi2_ois_spi_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr) +{ + uint8_t dev_addr = *(uint8_t*)intf_ptr; + + return coines_write_spi(dev_addr, reg_addr, (uint8_t *)reg_data, (uint16_t)len); +} + +/*! + * Delay function map to COINES platform + */ +void bmi2_ois_delay_us(uint32_t period, void *intf_ptr) +{ + coines_delay_usec(period); +} + +/******************************************************************************/ +int8_t bmi2_ois_init(struct bmi2_ois_dev *ois_dev) +{ + int8_t rslt; + + if (ois_dev != NULL) + { + int16_t result = coines_open_comm_intf(COINES_COMM_INTF_USB); + if (result < COINES_SUCCESS) + { + printf( + "\n Unable to connect with Application Board ! \n" " 1. Check if the board is connected and powered on. \n" " 2. Check if Application Board USB driver is installed. \n" + " 3. Check if board is in use by another application. (Insufficient permissions to access USB) \n"); + exit(result); + } + + coines_set_shuttleboard_vdd_vddio_config(0, 0); + coines_delay_msec(100); + + printf("SPI Interface \n"); + + /* To initialize the user SPI function */ + ois_dev_addr = COINES_SHUTTLE_PIN_8; /*COINES_SHUTTLE_PIN_7 */ + ois_dev->ois_read = bmi2_ois_spi_read; + ois_dev->ois_write = bmi2_ois_spi_write; + + /* Assign device address to interface pointer */ + ois_dev->intf_ptr = &ois_dev_addr; + + /* Configure delay in microseconds */ + ois_dev->ois_delay_us = bmi2_ois_delay_us; + + /* SDO pin is made low for selecting I2C address 0x76*/ + coines_set_pin_config(COINES_SHUTTLE_PIN_8, COINES_PIN_DIRECTION_OUT, COINES_PIN_VALUE_LOW); + + /* coines_config_spi_bus(COINES_SPI_BUS_0, COINES_SPI_SPEED_5_MHZ, COINES_SPI_MODE3); */ + coines_config_spi_bus(COINES_SPI_BUS_0, COINES_SPI_SPEED_7_5_MHZ, COINES_SPI_MODE0); + coines_delay_msec(10); + + /* PS pin is made high for selecting I2C protocol (gyroscope)*/ + coines_set_pin_config(COINES_SHUTTLE_PIN_9, COINES_PIN_DIRECTION_OUT, COINES_PIN_VALUE_HIGH); + + coines_delay_usec(10000); + + coines_set_shuttleboard_vdd_vddio_config(3300, 3300); + + coines_delay_usec(10000); + + } + else + { + rslt = BMI2_OIS_E_NULL_PTR; + } + + return rslt; +} + +/******************************************************************************/ + +/*! + * @brief This internal API prints the execution status + */ +void bmi2_ois_error_codes_print_result(const char api_name[], int8_t rslt) +{ + if (rslt != BMI2_OIS_OK) + { + printf("%s\t", api_name); + + if (rslt == BMI2_OIS_E_NULL_PTR) + { + printf("Error [%d] : Null pointer error.\r\n", rslt); + printf( + "It occurs when the user tries to assign value (not address) to a pointer, which has been initialized to NULL.\r\n"); + } + else if (rslt == BMI2_OIS_E_COM_FAIL) + { + printf("Error [%d] : Communication failure error.\r\n", rslt); + printf( + "It occurs due to read/write operation failure and also due to power failure during communication\r\n"); + } + else if (rslt == BMI2_OIS_E_INVALID_SENSOR) + { + printf( + "Error [%d] : Invalid sensor error. It occurs when there is a mismatch in the requested feature with the available one\r\n", + rslt); + } + else + { + printf("Error [%d] : Unknown error code\r\n", rslt); + } + } +} + +/*! + * @brief Deinitializes coines platform + * + * @return void. + */ +void bmi2_ois_coines_deinit(void) +{ + coines_close_comm_intf(COINES_COMM_INTF_USB); +} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi2_ois/common/common.h b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi2_ois/common/common.h new file mode 100644 index 0000000000..82a843bbf5 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi2_ois/common/common.h @@ -0,0 +1,92 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +#ifndef _COMMON_H +#define _COMMON_H + +/*! CPP guard */ +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include "bmi2_ois.h" + +/*! + * @brief Function for reading the sensor's registers through SPI bus. + * + * @param[in] reg_addr : Register address. + * @param[out] reg_data : Pointer to the data buffer to store the read data. + * @param[in] length : No of bytes to read. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval = BMI2_INTF_RET_SUCCESS -> Success + * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info + * + */ +int8_t bmi2_ois_spi_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr); + +/*! + * @brief Function for writing the sensor's registers through SPI bus. + * + * @param[in] reg_addr : Register address. + * @param[in] reg_data : Pointer to the data buffer whose data has to be written. + * @param[in] length : No of bytes to write. + * @param[in] intf_ptr : Interface pointer + * + * @return Status of execution + * @retval = BMI2_INTF_RET_SUCCESS -> Success + * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info + * + */ +int8_t bmi2_ois_spi_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr); + +/*! + * @brief This function provides the delay for required time (Microsecond) as per the input provided in some of the + * APIs. + * + * @param[in] period_us : The required wait time in microsecond. + * @param[in] intf_ptr : Interface pointer + * + * @return void. + * + */ +void bmi2_ois_delay_us(uint32_t period, void *intf_ptr); + +/*! + * @brief Function to initialize device structure and coines platform + * + * @param[in] bma : Structure instance of bmi2_dev + * + * @return Status of execution + * @retval 0 -> Success + * @retval < 0 -> Failure Info + */ +int8_t bmi2_ois_init(struct bmi2_ois_dev *ois_dev); + +/*! + * @brief Prints the execution status of the APIs. + * + * @param[in] api_name : API name with return status + * @param[in] rslt : Error code returned by the API whose execution status has to be printed. + * + * @return void. + */ +void bmi2_ois_error_codes_print_result(const char api_name[], int8_t rslt); + +/*! + * @brief Deinitializes coines platform + * + * @return void. + */ +void bmi2_ois_coines_deinit(void); + +#ifdef __cplusplus +} +#endif /* End of CPP guard */ + +#endif /* End _COMMON_H */ diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi2_ois/ois_accel_gyro/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi2_ois/ois_accel_gyro/Makefile new file mode 100644 index 0000000000..65cd946442 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi2_ois/ois_accel_gyro/Makefile @@ -0,0 +1,18 @@ +COINES_INSTALL_PATH ?= ../../../.. + +EXAMPLE_FILE ?= ois_accel_gyro.c + +API_LOCATION ?= ../.. + +COMMON_LOCATION ?= .. + +C_SRCS += \ +$(API_LOCATION)/bmi2.c \ +$(API_LOCATION)/bmi2_ois.c \ +$(COMMON_LOCATION)/common.c + +INCLUDEPATHS += \ +$(API_LOCATION) \ +$(COMMON_LOCATION)/common + +include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi2_ois/ois_accel_gyro/ois_accel_gyro.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi2_ois/ois_accel_gyro/ois_accel_gyro.c new file mode 100644 index 0000000000..bdb9d8f936 --- /dev/null +++ b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi2_ois/ois_accel_gyro/ois_accel_gyro.c @@ -0,0 +1,118 @@ +/**\ + * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + **/ + +/******************************************************************************/ +#include + +#include "bmi2_ois.h" +#include "bmi2_ois_common.h" + +/******************************************************************************/ +/*! Macro definition */ + +/*! Macro that holds the total number of accel x,y and z axes sample counts to be printed */ +#define ACCEL_GYRO_SAMPLE_COUNT UINT8_C(50) + +/******************************************************************************/ +/*! Functions */ + +int main(void) +{ + int8_t rslt; + + struct bmi2_ois_dev ois_dev; + + int8_t idx; + + /* To store the gyroscope cross sensitivity value */ + int16_t ois_gyr_cross_sens_zx = 0; + + /* Array to enable sensor through OIS interface */ + uint8_t sens_sel[2] = { BMI2_OIS_ACCEL, BMI2_OIS_GYRO }; + + /* Variable that holds the accel and gyro sample count */ + uint8_t n_data = ACCEL_GYRO_SAMPLE_COUNT; + + /* Initialize the device structure */ + rslt = bmi2_ois_init(&ois_dev); + bmi2_ois_error_codes_print_result("bmi2_ois_init", rslt); + + /* Get configurations for OIS */ + rslt = bmi2_ois_get_config(&ois_dev); + bmi2_ois_error_codes_print_result("bmi2_ois_get_config", rslt); + + /* OIS configurations */ + ois_dev.acc_en = BMI2_OIS_ENABLE; + ois_dev.gyr_en = BMI2_OIS_ENABLE; + ois_dev.lp_filter_en = BMI2_OIS_ENABLE; + + /* Set configurations for OIS */ + rslt = bmi2_ois_set_config(&ois_dev); + bmi2_ois_error_codes_print_result("bmi2_ois_set_config", rslt); + + /* Get configurations for OIS */ + rslt = bmi2_ois_get_config(&ois_dev); + bmi2_ois_error_codes_print_result("bmi2_ois_get_config", rslt); + + if (rslt == BMI2_OIS_OK) + { + for (idx = 0; idx < n_data; idx++) + { + ois_dev.ois_delay_us(156, ois_dev.intf_ptr); + + /* Accel ODR is 1600hz and gyro ODR is 6400hz.Delay required for + * accel 156us and 625us for gyro. + * taken modules from accel and gyro ODR which results every 4th sample accel and gyro + * read happens, rest of three samples gyro data alone will be read */ + if (idx % 4 == 0) + { + /* Get OIS accelerometer and gyro data through OIS interface + * @note for sensor which support gyro cross axes sensitivity pass the + * gyr_cross_sens_zx from the bmi2_dev structure */ + rslt = bmi2_ois_read_data(sens_sel, 2, &ois_dev, ois_gyr_cross_sens_zx); + bmi2_ois_error_codes_print_result("bmi2_ois_read_data", rslt); + + if (rslt == BMI2_OIS_OK) + { + printf("\n"); + + /* Print accelerometer data */ + printf("OIS Accel x-axis = %d ", ois_dev.acc_data.x); + printf("OIS Accel y-axis = %d ", ois_dev.acc_data.y); + printf("OIS Accel z-axis = %d \n", ois_dev.acc_data.z); + + /* Print gyro data */ + printf("OIS Gyro x-axis = %d ", ois_dev.gyr_data.x); + printf("OIS Gyro y-axis = %d ", ois_dev.gyr_data.y); + printf("OIS Gyro z-axis = %d \n", ois_dev.gyr_data.z); + + printf("\n"); + } + } + else + { + /* Get OIS gyro data through OIS interface + * @note for sensor which support gyro cross axes sensitivity pass the + * gyr_cross_sens_zx from the bmi2_dev structure */ + rslt = bmi2_ois_read_data(&sens_sel[1], 1, &ois_dev, ois_gyr_cross_sens_zx); + bmi2_ois_error_codes_print_result("bmi2_ois_read_data", rslt); + if (rslt == BMI2_OIS_OK) + { + /* Print gyro data */ + printf("OIS Gyro x-axis = %d ", ois_dev.gyr_data.x); + printf("OIS Gyro y-axis = %d ", ois_dev.gyr_data.y); + printf("OIS Gyro z-axis = %d ", ois_dev.gyr_data.z); + + printf("\n"); + } + } + } + } + + bmi2_ois_coines_deinit(); + + return rslt; +} diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/src/main/drivers/accgyro/accgyro_mpu.c index 5265e27342..f4b913736e 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.c +++ b/src/main/drivers/accgyro/accgyro_mpu.c @@ -53,6 +53,7 @@ #include "drivers/accgyro/accgyro_mpu6050.h" #include "drivers/accgyro/accgyro_mpu6500.h" #include "drivers/accgyro/accgyro_spi_bmi160.h" +#include "drivers/accgyro/accgyro_spi_bmi270.h" #include "drivers/accgyro/accgyro_spi_icm20649.h" #include "drivers/accgyro/accgyro_spi_icm20689.h" #include "drivers/accgyro/accgyro_spi_icm426xx.h" @@ -404,6 +405,19 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) { gyro->mpuDetectionResult.sensor = sensor; return true; } +#endif +#ifdef USE_ACCGYRO_BMI270 +#ifndef USE_DUAL_GYRO + spiBusSetInstance(&gyro->bus, BMI270_SPI_INSTANCE); +#endif +#ifdef BMI270_CS_PIN + gyro->bus.busdev_u.spi.csnPin = gyro->bus.busdev_u.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(BMI270_CS_PIN)) : gyro->bus.busdev_u.spi.csnPin; +#endif + sensor = bmi270Detect(&gyro->bus); + if (sensor != MPU_NONE) { + gyro->mpuDetectionResult.sensor = sensor; + return true; + } #endif return false; } diff --git a/src/main/drivers/accgyro/accgyro_spi_bmi270.c b/src/main/drivers/accgyro/accgyro_spi_bmi270.c new file mode 100644 index 0000000000..0e97a1b46e --- /dev/null +++ b/src/main/drivers/accgyro/accgyro_spi_bmi270.c @@ -0,0 +1,583 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include +#include +#include +#include + +#include "platform.h" + +#ifdef USE_ACCGYRO_BMI270 + +#include "drivers/accgyro/accgyro.h" +#include "drivers/accgyro/accgyro_spi_bmi270.h" +#include "drivers/bus_spi.h" +#include "drivers/exti.h" +#include "drivers/io.h" +#include "drivers/io_impl.h" +#include "drivers/nvic.h" +#include "drivers/sensor.h" +#include "drivers/system.h" +#include "drivers/time.h" + +#include "sensors/gyro.h" + +// 10 MHz max SPI frequency +#define BMI270_MAX_SPI_CLK_HZ 10000000 + +#define BMI270_FIFO_FRAME_SIZE 6 + +#define BMI270_CONFIG_SIZE 328 + +// Declaration for the device config (microcode) that must be uploaded to the sensor +extern const uint8_t bmi270_maximum_fifo_config_file[BMI270_CONFIG_SIZE]; + +#define BMI270_CHIP_ID 0x24 + +// BMI270 registers (not the complete list) +typedef enum { + BMI270_REG_CHIP_ID = 0x00, + BMI270_REG_ERR_REG = 0x02, + BMI270_REG_STATUS = 0x03, + BMI270_REG_ACC_DATA_X_LSB = 0x0C, + BMI270_REG_GYR_DATA_X_LSB = 0x12, + BMI270_REG_SENSORTIME_0 = 0x18, + BMI270_REG_SENSORTIME_1 = 0x19, + BMI270_REG_SENSORTIME_2 = 0x1A, + BMI270_REG_EVENT = 0x1B, + BMI270_REG_INT_STATUS_0 = 0x1C, + BMI270_REG_INT_STATUS_1 = 0x1D, + BMI270_REG_INTERNAL_STATUS = 0x21, + BMI270_REG_TEMPERATURE_LSB = 0x22, + BMI270_REG_TEMPERATURE_MSB = 0x23, + BMI270_REG_FIFO_LENGTH_LSB = 0x24, + BMI270_REG_FIFO_LENGTH_MSB = 0x25, + BMI270_REG_FIFO_DATA = 0x26, + BMI270_REG_ACC_CONF = 0x40, + BMI270_REG_ACC_RANGE = 0x41, + BMI270_REG_GYRO_CONF = 0x42, + BMI270_REG_GYRO_RANGE = 0x43, + BMI270_REG_AUX_CONF = 0x44, + BMI270_REG_FIFO_DOWNS = 0x45, + BMI270_REG_FIFO_WTM_0 = 0x46, + BMI270_REG_FIFO_WTM_1 = 0x47, + BMI270_REG_FIFO_CONFIG_0 = 0x48, + BMI270_REG_FIFO_CONFIG_1 = 0x49, + BMI270_REG_SATURATION = 0x4A, + BMI270_REG_INT1_IO_CTRL = 0x53, + BMI270_REG_INT2_IO_CTRL = 0x54, + BMI270_REG_INT_LATCH = 0x55, + BMI270_REG_INT1_MAP_FEAT = 0x56, + BMI270_REG_INT2_MAP_FEAT = 0x57, + BMI270_REG_INT_MAP_DATA = 0x58, + BMI270_REG_INIT_CTRL = 0x59, + BMI270_REG_INIT_DATA = 0x5E, + BMI270_REG_ACC_SELF_TEST = 0x6D, + BMI270_REG_GYR_SELF_TEST_AXES = 0x6E, + BMI270_REG_PWR_CONF = 0x7C, + BMI270_REG_PWR_CTRL = 0x7D, + BMI270_REG_CMD = 0x7E, +} bmi270Register_e; + +// BMI270 register configuration values +typedef enum { + BMI270_VAL_CMD_SOFTRESET = 0xB6, + BMI270_VAL_CMD_FIFOFLUSH = 0xB0, + BMI270_VAL_PWR_CTRL = 0x0E, // enable gyro, acc and temp sensors + BMI270_VAL_PWR_CONF = 0x02, // disable advanced power save, enable FIFO self-wake + BMI270_VAL_ACC_CONF_ODR800 = 0x0B, // set acc sample rate to 800hz + BMI270_VAL_ACC_CONF_ODR1600 = 0x0C, // set acc sample rate to 1600hz + BMI270_VAL_ACC_CONF_BWP = 0x02, // set acc filter in normal mode + BMI270_VAL_ACC_CONF_HP = 0x01, // set acc in high performance mode + BMI270_VAL_ACC_RANGE_8G = 0x02, // set acc to 8G full scale + BMI270_VAL_ACC_RANGE_16G = 0x03, // set acc to 16G full scale + BMI270_VAL_GYRO_CONF_ODR3200 = 0x0D, // set gyro sample rate to 3200hz + BMI270_VAL_GYRO_CONF_BWP_OSR4 = 0x00, // set gyro filter in OSR4 mode + BMI270_VAL_GYRO_CONF_BWP_OSR2 = 0x01, // set gyro filter in OSR2 mode + BMI270_VAL_GYRO_CONF_BWP_NORM = 0x02, // set gyro filter in normal mode + BMI270_VAL_GYRO_CONF_NOISE_PERF = 0x01, // set gyro in high performance noise mode + BMI270_VAL_GYRO_CONF_FILTER_PERF = 0x01, // set gyro in high performance filter mode + + BMI270_VAL_GYRO_RANGE_2000DPS = 0x08, // set gyro to 2000dps full scale + // for some reason you have to enable the ois_range bit (bit 3) for 2000dps as well + // or else the gyro scale will be 250dps when in prefiltered FIFO mode (not documented in datasheet!) + + BMI270_VAL_INT_MAP_DATA_DRDY_INT1 = 0x04,// enable the data ready interrupt pin 1 + BMI270_VAL_INT_MAP_FIFO_WM_INT1 = 0x02, // enable the FIFO watermark interrupt pin 1 + BMI270_VAL_INT1_IO_CTRL_PINMODE = 0x0A, // active high, push-pull, output enabled, input disabled + BMI270_VAL_FIFO_CONFIG_0 = 0x00, // don't stop when full, disable sensortime frame + BMI270_VAL_FIFO_CONFIG_1 = 0x80, // only gyro data in FIFO, use headerless mode + BMI270_VAL_FIFO_DOWNS = 0x00, // select unfiltered gyro data with no downsampling (6.4KHz samples) + BMI270_VAL_FIFO_WTM_0 = 0x06, // set the FIFO watermark level to 1 gyro sample (6 bytes) + BMI270_VAL_FIFO_WTM_1 = 0x00, // FIFO watermark MSB +} bmi270ConfigValues_e; + +// Need to see at least this many interrupts during initialisation to confirm EXTI connectivity +#define GYRO_EXTI_DETECT_THRESHOLD 1000 + +// BMI270 register reads are 16bits with the first byte a "dummy" value 0 +// that must be ignored. The result is in the second byte. +static uint8_t bmi270RegisterRead(const extDevice_t *dev, bmi270Register_e registerId) +{ + uint8_t data[2] = { 0, 0 }; + + if (spiReadRegMskBufRB(dev, registerId, data, 2)) { + return data[1]; + } else { + return 0; + } +} + +static void bmi270RegisterWrite(const extDevice_t *dev, bmi270Register_e registerId, uint8_t value, unsigned delayMs) +{ + spiWriteReg(dev, registerId, value); + if (delayMs) { + delay(delayMs); + } +} + +// Toggle the CS to switch the device into SPI mode. +// Device switches initializes as I2C and switches to SPI on a low to high CS transition +static void bmi270EnableSPI(const extDevice_t *dev) +{ + IOLo(dev->busType_u.spi.csnPin); + delay(1); + IOHi(dev->busType_u.spi.csnPin); + delay(10); +} + +uint8_t bmi270Detect(const extDevice_t *dev) +{ + spiSetClkDivisor(dev, spiCalculateDivider(BMI270_MAX_SPI_CLK_HZ)); + bmi270EnableSPI(dev); + + if (bmi270RegisterRead(dev, BMI270_REG_CHIP_ID) == BMI270_CHIP_ID) { + return BMI_270_SPI; + } + + return MPU_NONE; +} + +static void bmi270UploadConfig(const extDevice_t *dev) +{ + bmi270RegisterWrite(dev, BMI270_REG_PWR_CONF, 0, 1); + bmi270RegisterWrite(dev, BMI270_REG_INIT_CTRL, 0, 1); + + // Transfer the config file + spiWriteRegBuf(dev, BMI270_REG_INIT_DATA, (uint8_t *)bmi270_maximum_fifo_config_file, sizeof(bmi270_maximum_fifo_config_file)); + + delay(10); + bmi270RegisterWrite(dev, BMI270_REG_INIT_CTRL, 1, 1); +} + +static uint8_t getBmiOsrMode() +{ + switch(gyroConfig()->gyro_hardware_lpf) { + case GYRO_HARDWARE_LPF_NORMAL: + return BMI270_VAL_GYRO_CONF_BWP_OSR4; + case GYRO_HARDWARE_LPF_OPTION_1: + return BMI270_VAL_GYRO_CONF_BWP_OSR2; + case GYRO_HARDWARE_LPF_OPTION_2: + return BMI270_VAL_GYRO_CONF_BWP_NORM; + case GYRO_HARDWARE_LPF_EXPERIMENTAL: + return BMI270_VAL_GYRO_CONF_BWP_NORM; + } + return 0; +} + +static void bmi270Config(gyroDev_t *gyro) +{ + extDevice_t *dev = &gyro->dev; + + // If running in hardware_lpf experimental mode then switch to FIFO-based, + // 6.4KHz sampling, unfiltered data vs. the default 3.2KHz with hardware filtering +#ifdef USE_GYRO_DLPF_EXPERIMENTAL + const bool fifoMode = (gyro->hardware_lpf == GYRO_HARDWARE_LPF_EXPERIMENTAL); +#else + const bool fifoMode = false; +#endif + + // Perform a soft reset to set all configuration to default + // Delay 100ms before continuing configuration + bmi270RegisterWrite(dev, BMI270_REG_CMD, BMI270_VAL_CMD_SOFTRESET, 100); + + // Toggle the chip into SPI mode + bmi270EnableSPI(dev); + + bmi270UploadConfig(dev); + + // Configure the FIFO + if (fifoMode) { + bmi270RegisterWrite(dev, BMI270_REG_FIFO_CONFIG_0, BMI270_VAL_FIFO_CONFIG_0, 1); + bmi270RegisterWrite(dev, BMI270_REG_FIFO_CONFIG_1, BMI270_VAL_FIFO_CONFIG_1, 1); + bmi270RegisterWrite(dev, BMI270_REG_FIFO_DOWNS, BMI270_VAL_FIFO_DOWNS, 1); + bmi270RegisterWrite(dev, BMI270_REG_FIFO_WTM_0, BMI270_VAL_FIFO_WTM_0, 1); + bmi270RegisterWrite(dev, BMI270_REG_FIFO_WTM_1, BMI270_VAL_FIFO_WTM_1, 1); + } + + // Configure the accelerometer + bmi270RegisterWrite(dev, BMI270_REG_ACC_CONF, (BMI270_VAL_ACC_CONF_HP << 7) | (BMI270_VAL_ACC_CONF_BWP << 4) | BMI270_VAL_ACC_CONF_ODR800, 1); + + // Configure the accelerometer full-scale range + bmi270RegisterWrite(dev, BMI270_REG_ACC_RANGE, BMI270_VAL_ACC_RANGE_16G, 1); + + // Configure the gyro + bmi270RegisterWrite(dev, BMI270_REG_GYRO_CONF, (BMI270_VAL_GYRO_CONF_FILTER_PERF << 7) | (BMI270_VAL_GYRO_CONF_NOISE_PERF << 6) | (getBmiOsrMode() << 4) | BMI270_VAL_GYRO_CONF_ODR3200, 1); + + // Configure the gyro full-range scale + bmi270RegisterWrite(dev, BMI270_REG_GYRO_RANGE, BMI270_VAL_GYRO_RANGE_2000DPS, 1); + + // Configure the gyro data ready interrupt + if (fifoMode) { + // Interrupt driven by FIFO watermark level + bmi270RegisterWrite(dev, BMI270_REG_INT_MAP_DATA, BMI270_VAL_INT_MAP_FIFO_WM_INT1, 1); + } else { + // Interrupt driven by data ready + bmi270RegisterWrite(dev, BMI270_REG_INT_MAP_DATA, BMI270_VAL_INT_MAP_DATA_DRDY_INT1, 1); + } + + // Configure the behavior of the INT1 pin + bmi270RegisterWrite(dev, BMI270_REG_INT1_IO_CTRL, BMI270_VAL_INT1_IO_CTRL_PINMODE, 1); + + // Configure the device for performance mode + bmi270RegisterWrite(dev, BMI270_REG_PWR_CONF, BMI270_VAL_PWR_CONF, 1); + + // Enable the gyro, accelerometer and temperature sensor - disable aux interface + bmi270RegisterWrite(dev, BMI270_REG_PWR_CTRL, BMI270_VAL_PWR_CTRL, 1); + + // Flush the FIFO + if (fifoMode) { + bmi270RegisterWrite(dev, BMI270_REG_CMD, BMI270_VAL_CMD_FIFOFLUSH, 1); + } +} + +extiCallbackRec_t bmi270IntCallbackRec; + +/* + * Gyro interrupt service routine + */ +#ifdef USE_GYRO_EXTI +// Called in ISR context +// Gyro read has just completed +busStatus_e bmi270Intcallback(uint32_t arg) +{ + gyroDev_t *gyro = (gyroDev_t *)arg; + int32_t gyroDmaDuration = cmpTimeCycles(getCycleCounter(), gyro->gyroLastEXTI); + + if (gyroDmaDuration > gyro->gyroDmaMaxDuration) { + gyro->gyroDmaMaxDuration = gyroDmaDuration; + } + + gyro->dataReady = true; + + return BUS_READY; +} + +void bmi270ExtiHandler(extiCallbackRec_t *cb) +{ + gyroDev_t *gyro = container_of(cb, gyroDev_t, exti); + // Ideally we'd use a timer to capture such information, but unfortunately the port used for EXTI interrupt does + // not have an associated timer + uint32_t nowCycles = getCycleCounter(); + gyro->gyroSyncEXTI = gyro->gyroLastEXTI + gyro->gyroDmaMaxDuration; + gyro->gyroLastEXTI = nowCycles; + + if (gyro->gyroModeSPI == GYRO_EXTI_INT_DMA) { + spiSequence(&gyro->dev, gyro->segments); + } + + gyro->detectedEXTI++; + +} + +static void bmi270IntExtiInit(gyroDev_t *gyro) +{ + if (gyro->mpuIntExtiTag == IO_TAG_NONE) { + return; + } + + IO_t mpuIntIO = IOGetByTag(gyro->mpuIntExtiTag); + + IOInit(mpuIntIO, OWNER_GYRO_EXTI, 0); + EXTIHandlerInit(&gyro->exti, bmi270ExtiHandler); + EXTIConfig(mpuIntIO, &gyro->exti, NVIC_PRIO_MPU_INT_EXTI, IOCFG_IN_FLOATING, BETAFLIGHT_EXTI_TRIGGER_RISING); + EXTIEnable(mpuIntIO); +} +#else +void bmi270ExtiHandler(extiCallbackRec_t *cb) +{ + gyroDev_t *gyro = container_of(cb, gyroDev_t, exti); + gyro->dataReady = true; +} +#endif + +static bool bmi270AccRead(accDev_t *acc) +{ + switch (acc->gyro->gyroModeSPI) { + case GYRO_EXTI_INT: + case GYRO_EXTI_NO_INT: + { + acc->gyro->dev.txBuf[0] = BMI270_REG_ACC_DATA_X_LSB | 0x80; + + busSegment_t segments[] = { + {.u.buffers = {NULL, NULL}, 8, true, NULL}, + {.u.link = {NULL, NULL}, 0, true, NULL}, + }; + segments[0].u.buffers.txData = acc->gyro->dev.txBuf; + segments[0].u.buffers.rxData = acc->gyro->dev.rxBuf; + + spiSequence(&acc->gyro->dev, &segments[0]); + + // Wait for completion + spiWait(&acc->gyro->dev); + + // Fall through + FALLTHROUGH; + } + + case GYRO_EXTI_INT_DMA: + { + // If read was triggered in interrupt don't bother waiting. The worst that could happen is that we pick + // up an old value. + + // This data was read from the gyro, which is the same SPI device as the acc + uint16_t *accData = (uint16_t *)acc->gyro->dev.rxBuf; + acc->ADCRaw[X] = accData[1]; + acc->ADCRaw[Y] = accData[2]; + acc->ADCRaw[Z] = accData[3]; + break; + } + + case GYRO_EXTI_INIT: + default: + break; + } + + return true; +} + +static bool bmi270GyroReadRegister(gyroDev_t *gyro) +{ + uint16_t *gyroData = (uint16_t *)gyro->dev.rxBuf; + switch (gyro->gyroModeSPI) { + case GYRO_EXTI_INIT: + { + // Initialise the tx buffer to all 0x00 + memset(gyro->dev.txBuf, 0x00, 14); +#ifdef USE_GYRO_EXTI + // Check that minimum number of interrupts have been detected + + // We need some offset from the gyro interrupts to ensure sampling after the interrupt + gyro->gyroDmaMaxDuration = 5; + // Using DMA for gyro access upsets the scheduler on the F4 + if (gyro->detectedEXTI > GYRO_EXTI_DETECT_THRESHOLD) { + if (spiUseDMA(&gyro->dev)) { + gyro->dev.callbackArg = (uint32_t)gyro; + gyro->dev.txBuf[0] = BMI270_REG_ACC_DATA_X_LSB | 0x80; + gyro->segments[0].len = 14; + gyro->segments[0].callback = bmi270Intcallback; + gyro->segments[0].u.buffers.txData = gyro->dev.txBuf; + gyro->segments[0].u.buffers.rxData = gyro->dev.rxBuf; + gyro->segments[0].negateCS = true; + gyro->gyroModeSPI = GYRO_EXTI_INT_DMA; + } else { + // Interrupts are present, but no DMA + gyro->gyroModeSPI = GYRO_EXTI_INT; + } + } else +#endif + { + gyro->gyroModeSPI = GYRO_EXTI_NO_INT; + } + break; + } + + case GYRO_EXTI_INT: + case GYRO_EXTI_NO_INT: + { + gyro->dev.txBuf[0] = BMI270_REG_GYR_DATA_X_LSB | 0x80; + + busSegment_t segments[] = { + {.u.buffers = {NULL, NULL}, 8, true, NULL}, + {.u.link = {NULL, NULL}, 0, true, NULL}, + }; + segments[0].u.buffers.txData = gyro->dev.txBuf; + segments[0].u.buffers.rxData = gyro->dev.rxBuf; + + spiSequence(&gyro->dev, &segments[0]); + + // Wait for completion + spiWait(&gyro->dev); + + gyro->gyroADCRaw[X] = gyroData[1]; + gyro->gyroADCRaw[Y] = gyroData[2]; + gyro->gyroADCRaw[Z] = gyroData[3]; + + break; + } + + case GYRO_EXTI_INT_DMA: + { + // If read was triggered in interrupt don't bother waiting. The worst that could happen is that we pick + // up an old value. + gyro->gyroADCRaw[X] = gyroData[4]; + gyro->gyroADCRaw[Y] = gyroData[5]; + gyro->gyroADCRaw[Z] = gyroData[6]; + break; + } + + default: + break; + } + + return true; +} + +#ifdef USE_GYRO_DLPF_EXPERIMENTAL +static bool bmi270GyroReadFifo(gyroDev_t *gyro) +{ + enum { + IDX_REG = 0, + IDX_SKIP, + IDX_FIFO_LENGTH_L, + IDX_FIFO_LENGTH_H, + IDX_GYRO_XOUT_L, + IDX_GYRO_XOUT_H, + IDX_GYRO_YOUT_L, + IDX_GYRO_YOUT_H, + IDX_GYRO_ZOUT_L, + IDX_GYRO_ZOUT_H, + BUFFER_SIZE, + }; + + bool dataRead = false; + STATIC_DMA_DATA_AUTO uint8_t bmi270_tx_buf[BUFFER_SIZE] = {BMI270_REG_FIFO_LENGTH_LSB | 0x80, 0, 0, 0, 0, 0, 0, 0, 0, 0}; + STATIC_DMA_DATA_AUTO uint8_t bmi270_rx_buf[BUFFER_SIZE]; + + // Burst read the FIFO length followed by the next 6 bytes containing the gyro axis data for + // the first sample in the queue. It's possible for the FIFO to be empty so we need to check the + // length before using the sample. + spiReadWriteBuf(&gyro->dev, (uint8_t *)bmi270_tx_buf, bmi270_rx_buf, BUFFER_SIZE); // receive response + + int fifoLength = (uint16_t)((bmi270_rx_buf[IDX_FIFO_LENGTH_H] << 8) | bmi270_rx_buf[IDX_FIFO_LENGTH_L]); + + if (fifoLength >= BMI270_FIFO_FRAME_SIZE) { + + const int16_t gyroX = (int16_t)((bmi270_rx_buf[IDX_GYRO_XOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_XOUT_L]); + const int16_t gyroY = (int16_t)((bmi270_rx_buf[IDX_GYRO_YOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_YOUT_L]); + const int16_t gyroZ = (int16_t)((bmi270_rx_buf[IDX_GYRO_ZOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_ZOUT_L]); + + // If the FIFO data is invalid then the returned values will be 0x8000 (-32768) (pg. 43 of datasheet). + // This shouldn't happen since we're only using the data if the FIFO length indicates + // that data is available, but this safeguard is needed to prevent bad things in + // case it does happen. + if ((gyroX != INT16_MIN) || (gyroY != INT16_MIN) || (gyroZ != INT16_MIN)) { + gyro->gyroADCRaw[X] = gyroX; + gyro->gyroADCRaw[Y] = gyroY; + gyro->gyroADCRaw[Z] = gyroZ; + dataRead = true; + } + fifoLength -= BMI270_FIFO_FRAME_SIZE; + } + + // If there are additional samples in the FIFO then we don't use those for now and simply + // flush the FIFO. Under normal circumstances we only expect one sample in the FIFO since + // the gyro loop is running at the native sample rate of 6.4KHz. + // However the way the FIFO works in the sensor is that if a frame is partially read then + // it remains in the queue instead of bein removed. So if we ever got into a state where there + // was a partial frame or other unexpected data in the FIFO is may never get cleared and we + // would end up in a lock state of always re-reading the same partial or invalid sample. + if (fifoLength > 0) { + // Partial or additional frames left - flush the FIFO + bmi270RegisterWrite(&gyro->dev, BMI270_REG_CMD, BMI270_VAL_CMD_FIFOFLUSH, 0); + } + + return dataRead; +} +#endif + +static bool bmi270GyroRead(gyroDev_t *gyro) +{ +#ifdef USE_GYRO_DLPF_EXPERIMENTAL + if (gyro->hardware_lpf == GYRO_HARDWARE_LPF_EXPERIMENTAL) { + // running in 6.4KHz FIFO mode + return bmi270GyroReadFifo(gyro); + } else +#endif + { + // running in 3.2KHz register mode + return bmi270GyroReadRegister(gyro); + } +} + +static void bmi270SpiGyroInit(gyroDev_t *gyro) +{ + bmi270Config(gyro); + +#if defined(USE_GYRO_EXTI) + bmi270IntExtiInit(gyro); +#endif +} + +static void bmi270SpiAccInit(accDev_t *acc) +{ + // sensor is configured during gyro init + acc->acc_1G = 512 * 4; // 16G sensor scale +} + +bool bmi270SpiAccDetect(accDev_t *acc) +{ + if (acc->mpuDetectionResult.sensor != BMI_270_SPI) { + return false; + } + + acc->initFn = bmi270SpiAccInit; + acc->readFn = bmi270AccRead; + + return true; +} + + +bool bmi270SpiGyroDetect(gyroDev_t *gyro) +{ + if (gyro->mpuDetectionResult.sensor != BMI_270_SPI) { + return false; + } + + gyro->initFn = bmi270SpiGyroInit; + gyro->readFn = bmi270GyroRead; + gyro->scale = GYRO_SCALE_2000DPS; + + return true; +} + +// Used to query the status register to determine what event caused the EXTI to fire. +// When in 3.2KHz mode the interrupt is mapped to the data ready state. However the data ready +// trigger will fire for both gyro and accelerometer. So it's necessary to check this register +// to determine which event caused the interrupt. +// When in 6.4KHz mode the interrupt is configured to be the FIFO watermark size of 6 bytes. +// Since in this mode we only put gyro data in the FIFO it's sufficient to check for the FIFO +// watermark reason as an idication of gyro data ready. +uint8_t bmi270InterruptStatus(gyroDev_t *gyro) +{ + return bmi270RegisterRead(&gyro->dev, BMI270_REG_INT_STATUS_1); +} +#endif // USE_ACCGYRO_BMI270 diff --git a/src/main/drivers/accgyro/accgyro_spi_bmi270.h b/src/main/drivers/accgyro/accgyro_spi_bmi270.h new file mode 100644 index 0000000000..c0ea0495fa --- /dev/null +++ b/src/main/drivers/accgyro/accgyro_spi_bmi270.h @@ -0,0 +1,28 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#pragma once + +#include "drivers/bus.h" + +uint8_t bmi270Detect(const busDevice_t *bus); +bool bmi270SpiAccDetect(accDev_t *acc); +bool bmi270SpiGyroDetect(gyroDev_t *gyro); +uint8_t bmi270InterruptStatus(gyroDev_t *gyro); diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 9634ec73c8..83b2b1485e 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -109,13 +109,13 @@ const char * const lookupTableAccHardware[] = { "AUTO", "NONE", "ADXL345", "MPU6050", "MMA8452", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608G", "ICM20649", "ICM20689", - "ICM42605", "ICM42688P", "BMI160", "ACC_IMUF9001", "FAKE" + "ICM42605", "ICM42688P", "BMI160", "BMI270", "ACC_IMUF9001", "FAKE" }; // sync with gyroSensor_e const char * const lookupTableGyroHardware[] = { "AUTO", "NONE", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", "MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608G", "ICM20649", "ICM20689", - "ICM42605", "ICM42688P", "BMI160", "GYRO_IMUF9001", "FAKE" + "ICM42605", "ICM42688P", "BMI160", "BMI270", "GYRO_IMUF9001", "FAKE" }; #if defined(USE_SENSOR_NAMES) || defined(USE_BARO) diff --git a/src/main/pg/bus_spi.c b/src/main/pg/bus_spi.c index 3b1fcfa7a8..e8e50fc84f 100644 --- a/src/main/pg/bus_spi.c +++ b/src/main/pg/bus_spi.c @@ -100,6 +100,9 @@ ioTag_t preinitIPUList[SPI_PREINIT_IPU_COUNT] = { #ifdef BMI160_CS_PIN IO_TAG(BMI160_CS_PIN), #endif +#ifdef BMI270_CS_PIN + IO_TAG(BMI270_CS_PIN), +#endif #ifdef L3GD20_CS_PIN IO_TAG(L3GD20_CS_PIN), #endif diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 62460a182c..6a45d357b4 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -43,6 +43,7 @@ #include "drivers/accgyro/accgyro_mpu6050.h" #include "drivers/accgyro/accgyro_mpu6500.h" #include "drivers/accgyro/accgyro_spi_bmi160.h" +#include "drivers/accgyro/accgyro_spi_bmi270.h" #include "drivers/accgyro/accgyro_spi_icm20649.h" #include "drivers/accgyro/accgyro_spi_icm20689.h" #include "drivers/accgyro/accgyro_spi_icm426xx.h" @@ -299,6 +300,17 @@ bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse) { } FALLTHROUGH; #endif +#ifdef USE_ACCGYRO_BMI270 + case ACC_BMI270: + if (bmi270SpiAccDetect(dev)) { + accHardware = ACC_BMI270; +#ifdef ACC_BMI270_ALIGN + dev->accAlign = ACC_BMI270_ALIGN; +#endif + break; + } + FALLTHROUGH; +#endif #if defined(USE_ACC_SPI_ICM42605) || defined(USE_ACC_SPI_ICM42688P) case ACC_ICM42605: case ACC_ICM42688P: diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index b448d2419b..5b132c53f6 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -50,6 +50,7 @@ typedef enum { ACC_ICM42605, ACC_ICM42688P, ACC_BMI160, + ACC_BMI270, ACC_IMUF9001, ACC_FAKE } accelerationSensor_e; diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index b77528e9fd..26e5b2fd0d 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -44,6 +44,7 @@ #include "drivers/accgyro/accgyro_mpu6050.h" #include "drivers/accgyro/accgyro_mpu6500.h" #include "drivers/accgyro/accgyro_spi_bmi160.h" +#include "drivers/accgyro/accgyro_spi_bmi270.h" #include "drivers/accgyro/accgyro_spi_icm20649.h" #include "drivers/accgyro/accgyro_spi_icm20689.h" #include "drivers/accgyro/accgyro_spi_icm426xx.h" @@ -533,6 +534,17 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev) { } FALLTHROUGH; #endif +#ifdef USE_ACCGYRO_BMI270 + case GYRO_BMI270: + if (bmi270SpiGyroDetect(dev)) { + gyroHardware = GYRO_BMI270; +#ifdef GYRO_BMI270_ALIGN + dev->gyroAlign = GYRO_BMI270_ALIGN; +#endif + break; + } + FALLTHROUGH; +#endif #ifdef USE_GYRO_IMUF9001 case GYRO_IMUF9001: if (imufSpiGyroDetect(dev)) { @@ -562,7 +574,7 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev) { static bool gyroInitSensor(gyroSensor_t *gyroSensor) { gyroSensor->gyroDev.gyro_high_fsr = gyroConfig()->gyro_high_fsr; #if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) \ - || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P) || defined(USE_GYRO_IMUF9001) || defined(USE_ACCGYRO_BMI160) + || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P) || defined(USE_GYRO_IMUF9001) || defined(USE_ACCGYRO_BMI160) || defined(USE_ACCGYRO_BMI270) mpuDetect(&gyroSensor->gyroDev); mpuResetFn = gyroSensor->gyroDev.mpuConfiguration.resetFn; // must be set after mpuDetect #endif @@ -607,6 +619,7 @@ static bool gyroInitSensor(gyroSensor_t *gyroSensor) { case GYRO_MPU3050: case GYRO_L3GD20: case GYRO_BMI160: + case GYRO_BMI270: case GYRO_MPU6000: case GYRO_MPU6500: case GYRO_MPU9250: diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 0f32c699e1..ff5f8d131f 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -56,6 +56,7 @@ typedef enum { GYRO_ICM42605, GYRO_ICM42688P, GYRO_BMI160, + GYRO_BMI270, GYRO_IMUF9001, GYRO_FAKE } gyroSensor_e; diff --git a/src/main/target/IFRC_IFLIGHT_F745_AIO_V2/target.c b/src/main/target/IFRC_IFLIGHT_F745_AIO_V2/target.c new file mode 100644 index 0000000000..c57c497050 --- /dev/null +++ b/src/main/target/IFRC_IFLIGHT_F745_AIO_V2/target.c @@ -0,0 +1,44 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include + +#include "platform.h" +#include "drivers/io.h" + +#include "drivers/dma.h" +#include "drivers/timer.h" +#include "drivers/timer_def.h" + +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0 ), //LED + DEF_TIM(TIM2, CH2, PB3, TIM_USE_ANY, 0, 0 ), //CAM + + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0 ), // M1 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0, 0 ), // M2 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_MOTOR, 0, 0 ), // M3 + DEF_TIM(TIM3, CH2, PB5, TIM_USE_MOTOR, 0, 0 ), // M4 + DEF_TIM(TIM4, CH1, PD12, TIM_USE_MOTOR, 0, 0 ), // M5 + DEF_TIM(TIM4, CH2, PD13, TIM_USE_MOTOR, 0, 0 ), // M6 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MOTOR, 0, 1 ), // M7 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MOTOR, 0, 0 ), // M8 + + }; diff --git a/src/main/target/IFRC_IFLIGHT_F745_AIO_V2/target.h b/src/main/target/IFRC_IFLIGHT_F745_AIO_V2/target.h new file mode 100644 index 0000000000..75760be9f7 --- /dev/null +++ b/src/main/target/IFRC_IFLIGHT_F745_AIO_V2/target.h @@ -0,0 +1,184 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "IFRC" + +#define USBD_PRODUCT_STRING "IFLIGHT_F745_AIO_V2" + +#define LED0_PIN PC13 + +#define USE_BEEPER +#define BEEPER_PIN PD2 +#define BEEPER_INVERTED + +//**********Gyro Acc *************// + +#define USE_ACC +#define USE_GYRO +#define USE_DUAL_GYRO + +#define USE_SPI_GYRO +#define USE_ACCGYRO_BMI270 + +#define USE_EXTI +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PD0 +#define GYRO_2_EXTI_PIN PD8 +#define MPU_INT_EXTI + +#define GYRO_1_CS_PIN PA4 +#define GYRO_1_SPI_INSTANCE SPI1 +#define GYRO_2_CS_PIN PB12 +#define GYRO_2_SPI_INSTANCE SPI2 + +#define GYRO_1_ALIGN CW0_DEG +#define ACC_1_ALIGN CW0_DEG +#define GYRO_2_ALIGN CW180_DEG +#define ACC_2_ALIGN CW180_DEG + +#define USE_MPU_DATA_READY_SIGNAL +#define ENSURE_MPU_DATA_READY_IS_LOW +#define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_1 + +//**********Mag and Baro**********// + +#define USE_MAG +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define MAG_HMC5883_ALIGN CW180_DEG +#define MAG_QMC5883_ALIGN CW180_DEG +#define MAG_I2C_INSTANCE (I2CDEV_1) + +#define USE_BARO +#define USE_BARO_MS5611 +#define USE_BARO_BMP280 +#define USE_BARO_DPS310 +#define BARO_I2C_INSTANCE (I2CDEV_1) + +//**********Serial****************// + +#define USE_VCP +//#define USE_USB_DETECT +//#define USB_DETECT_PIN PC4 + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 + +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 + +#define USE_UART3 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 + +#define USE_UART4 +#define UART4_RX_PIN PA1 +#define UART4_TX_PIN PA0 + +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 + +#define USE_UART7 +#define UART7_RX_PIN PE7 +#define UART7_TX_PIN PE8 + +#define USE_UART8 +#define UART8_RX_PIN PE0 +#define UART8_TX_PIN PE1 + +#define SERIAL_PORT_COUNT 8 //VCP, USART1, USART2, USART3, UART4, USART6, USART7, USART8 + +#define USE_ESCSERIAL +#define USE_CAMERA_CONTROL +#define CAMERA_CONTROL_PIN PB3 + +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define USE_SPI_DEVICE_2 +#define USE_SPI_DEVICE_3 +#define USE_SPI_DEVICE_4 + +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 + +#define SPI4_SCK_PIN PE2 +#define SPI4_MISO_PIN PE5 +#define SPI4_MOSI_PIN PE6 + +#define USE_MAX7456 +#define MAX7456_SPI_INSTANCE SPI4 +#define MAX7456_SPI_CS_PIN PE4 +#define MAX7456_SPI_CLK (SPI_CLOCK_STANDARD) // 10MHz +#define MAX7456_RESTORE_CLK (SPI_CLOCK_FAST) + +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define FLASH_CS_PIN PA15 +#define FLASH_SPI_INSTANCE SPI3 + +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C_DEVICE_1 (I2CDEV_1) +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +#define USE_I2C_DEVICE_2 +#define I2C_DEVICE_2 (I2CDEV_2) +#define I2C2_SCL PB10 +#define I2C2_SDA PB11 + +#define USE_ADC +#define VBAT_ADC_PIN PC3 +#define CURRENT_METER_ADC_PIN PC2 +#define RSSI_ADC_PIN PC5 +#define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC +#define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ADC +#define CURRENT_METER_SCALE_DEFAULT 100 + +#define USE_LED_STRIP + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY ) +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff + +#define USABLE_TIMER_CHANNEL_COUNT 10 + +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) ) diff --git a/src/main/target/IFRC_IFLIGHT_F745_AIO_V2/target.mk b/src/main/target/IFRC_IFLIGHT_F745_AIO_V2/target.mk new file mode 100644 index 0000000000..6dc3654f9b --- /dev/null +++ b/src/main/target/IFRC_IFLIGHT_F745_AIO_V2/target.mk @@ -0,0 +1,15 @@ +F7X5XG_TARGETS += $(TARGET) +FEATURES += VCP ONBOARDFLASH + +TARGET_SRC = \ + $(ROOT)/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_maximum_fifo.c \ + drivers/accgyro/accgyro_spi_bmi270.c \ + drivers/barometer/barometer_ms5611.c \ + drivers/barometer/barometer_bmp280.c \ + drivers/barometer/barometer_bmp085.c \ + drivers/barometer/barometer_ms5611.c \ + drivers/compass/compass_hmc5883l.c \ + drivers/compass/compass_qmc5883l.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_hal.c \ + drivers/max7456.c From c9167b8fe34ab7bfbc864e748e1906d33bbd5e69 Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Thu, 11 Aug 2022 13:54:12 -0500 Subject: [PATCH 02/36] bmi270 - add IFLIGHT_F745_AIO_V2 --- src/main/target/IFLIGHT_F745_AIO_V2/target.c | 44 +++++ src/main/target/IFLIGHT_F745_AIO_V2/target.h | 184 ++++++++++++++++++ src/main/target/IFLIGHT_F745_AIO_V2/target.mk | 14 ++ 3 files changed, 242 insertions(+) create mode 100644 src/main/target/IFLIGHT_F745_AIO_V2/target.c create mode 100644 src/main/target/IFLIGHT_F745_AIO_V2/target.h create mode 100644 src/main/target/IFLIGHT_F745_AIO_V2/target.mk diff --git a/src/main/target/IFLIGHT_F745_AIO_V2/target.c b/src/main/target/IFLIGHT_F745_AIO_V2/target.c new file mode 100644 index 0000000000..c57c497050 --- /dev/null +++ b/src/main/target/IFLIGHT_F745_AIO_V2/target.c @@ -0,0 +1,44 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include + +#include "platform.h" +#include "drivers/io.h" + +#include "drivers/dma.h" +#include "drivers/timer.h" +#include "drivers/timer_def.h" + +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0 ), //LED + DEF_TIM(TIM2, CH2, PB3, TIM_USE_ANY, 0, 0 ), //CAM + + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0 ), // M1 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0, 0 ), // M2 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_MOTOR, 0, 0 ), // M3 + DEF_TIM(TIM3, CH2, PB5, TIM_USE_MOTOR, 0, 0 ), // M4 + DEF_TIM(TIM4, CH1, PD12, TIM_USE_MOTOR, 0, 0 ), // M5 + DEF_TIM(TIM4, CH2, PD13, TIM_USE_MOTOR, 0, 0 ), // M6 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MOTOR, 0, 1 ), // M7 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MOTOR, 0, 0 ), // M8 + + }; diff --git a/src/main/target/IFLIGHT_F745_AIO_V2/target.h b/src/main/target/IFLIGHT_F745_AIO_V2/target.h new file mode 100644 index 0000000000..75760be9f7 --- /dev/null +++ b/src/main/target/IFLIGHT_F745_AIO_V2/target.h @@ -0,0 +1,184 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "IFRC" + +#define USBD_PRODUCT_STRING "IFLIGHT_F745_AIO_V2" + +#define LED0_PIN PC13 + +#define USE_BEEPER +#define BEEPER_PIN PD2 +#define BEEPER_INVERTED + +//**********Gyro Acc *************// + +#define USE_ACC +#define USE_GYRO +#define USE_DUAL_GYRO + +#define USE_SPI_GYRO +#define USE_ACCGYRO_BMI270 + +#define USE_EXTI +#define USE_GYRO_EXTI +#define GYRO_1_EXTI_PIN PD0 +#define GYRO_2_EXTI_PIN PD8 +#define MPU_INT_EXTI + +#define GYRO_1_CS_PIN PA4 +#define GYRO_1_SPI_INSTANCE SPI1 +#define GYRO_2_CS_PIN PB12 +#define GYRO_2_SPI_INSTANCE SPI2 + +#define GYRO_1_ALIGN CW0_DEG +#define ACC_1_ALIGN CW0_DEG +#define GYRO_2_ALIGN CW180_DEG +#define ACC_2_ALIGN CW180_DEG + +#define USE_MPU_DATA_READY_SIGNAL +#define ENSURE_MPU_DATA_READY_IS_LOW +#define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_1 + +//**********Mag and Baro**********// + +#define USE_MAG +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define MAG_HMC5883_ALIGN CW180_DEG +#define MAG_QMC5883_ALIGN CW180_DEG +#define MAG_I2C_INSTANCE (I2CDEV_1) + +#define USE_BARO +#define USE_BARO_MS5611 +#define USE_BARO_BMP280 +#define USE_BARO_DPS310 +#define BARO_I2C_INSTANCE (I2CDEV_1) + +//**********Serial****************// + +#define USE_VCP +//#define USE_USB_DETECT +//#define USB_DETECT_PIN PC4 + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 + +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 + +#define USE_UART3 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 + +#define USE_UART4 +#define UART4_RX_PIN PA1 +#define UART4_TX_PIN PA0 + +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 + +#define USE_UART7 +#define UART7_RX_PIN PE7 +#define UART7_TX_PIN PE8 + +#define USE_UART8 +#define UART8_RX_PIN PE0 +#define UART8_TX_PIN PE1 + +#define SERIAL_PORT_COUNT 8 //VCP, USART1, USART2, USART3, UART4, USART6, USART7, USART8 + +#define USE_ESCSERIAL +#define USE_CAMERA_CONTROL +#define CAMERA_CONTROL_PIN PB3 + +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define USE_SPI_DEVICE_2 +#define USE_SPI_DEVICE_3 +#define USE_SPI_DEVICE_4 + +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 + +#define SPI4_SCK_PIN PE2 +#define SPI4_MISO_PIN PE5 +#define SPI4_MOSI_PIN PE6 + +#define USE_MAX7456 +#define MAX7456_SPI_INSTANCE SPI4 +#define MAX7456_SPI_CS_PIN PE4 +#define MAX7456_SPI_CLK (SPI_CLOCK_STANDARD) // 10MHz +#define MAX7456_RESTORE_CLK (SPI_CLOCK_FAST) + +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define FLASH_CS_PIN PA15 +#define FLASH_SPI_INSTANCE SPI3 + +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C_DEVICE_1 (I2CDEV_1) +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +#define USE_I2C_DEVICE_2 +#define I2C_DEVICE_2 (I2CDEV_2) +#define I2C2_SCL PB10 +#define I2C2_SDA PB11 + +#define USE_ADC +#define VBAT_ADC_PIN PC3 +#define CURRENT_METER_ADC_PIN PC2 +#define RSSI_ADC_PIN PC5 +#define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC +#define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ADC +#define CURRENT_METER_SCALE_DEFAULT 100 + +#define USE_LED_STRIP + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY ) +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff + +#define USABLE_TIMER_CHANNEL_COUNT 10 + +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) ) diff --git a/src/main/target/IFLIGHT_F745_AIO_V2/target.mk b/src/main/target/IFLIGHT_F745_AIO_V2/target.mk new file mode 100644 index 0000000000..6e2f072698 --- /dev/null +++ b/src/main/target/IFLIGHT_F745_AIO_V2/target.mk @@ -0,0 +1,14 @@ +F7X5XG_TARGETS += $(TARGET) +FEATURES += VCP ONBOARDFLASH + +TARGET_SRC = \ + drivers/accgyro/accgyro_spi_bmi270.c \ + drivers/barometer/barometer_ms5611.c \ + drivers/barometer/barometer_bmp280.c \ + drivers/barometer/barometer_bmp085.c \ + drivers/barometer/barometer_ms5611.c \ + drivers/compass/compass_hmc5883l.c \ + drivers/compass/compass_qmc5883l.c \ + drivers/light_ws2811strip.c \ + drivers/light_ws2811strip_hal.c \ + drivers/max7456.c From 19489bc474c139d06648ae58b0f4f7eea21c381f Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Thu, 11 Aug 2022 14:12:57 -0500 Subject: [PATCH 03/36] bmi270 - fix my obvious mistakes --- make/source.mk | 1 + src/main/drivers/accgyro/accgyro_mpu.h | 1 + 2 files changed, 2 insertions(+) diff --git a/make/source.mk b/make/source.mk index ad1a5d0f7a..dd602fab3c 100644 --- a/make/source.mk +++ b/make/source.mk @@ -198,6 +198,7 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \ drivers/accgyro/accgyro_mpu6050.c \ drivers/accgyro/accgyro_mpu6500.c \ drivers/accgyro/accgyro_spi_bmi160.c \ + drivers/accgyro/accgyro_spi_bmi270.c \ drivers/accgyro/accgyro_spi_icm20689.c \ drivers/accgyro/accgyro_spi_mpu6000.c \ drivers/accgyro/accgyro_spi_mpu6500.c \ diff --git a/src/main/drivers/accgyro/accgyro_mpu.h b/src/main/drivers/accgyro/accgyro_mpu.h index aa0f770e29..10f15a7744 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.h +++ b/src/main/drivers/accgyro/accgyro_mpu.h @@ -219,6 +219,7 @@ typedef enum { ICM_42605_SPI, ICM_42688P_SPI, BMI_160_SPI, + BMI_270_SPI, IMUF_9001_SPI, } mpuSensor_e; From c7500380d8bfe77ba7a11c28fec2b84147dbfdcc Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Thu, 11 Aug 2022 14:29:38 -0500 Subject: [PATCH 04/36] bmi270 - align ifrcf745aiov2 target --- src/main/target/IFLIGHT_F745_AIO_V2/target.mk | 1 + .../target/IFRC_IFLIGHT_F745_AIO_V2/target.c | 44 ----- .../target/IFRC_IFLIGHT_F745_AIO_V2/target.h | 184 ------------------ .../target/IFRC_IFLIGHT_F745_AIO_V2/target.mk | 15 -- 4 files changed, 1 insertion(+), 243 deletions(-) delete mode 100644 src/main/target/IFRC_IFLIGHT_F745_AIO_V2/target.c delete mode 100644 src/main/target/IFRC_IFLIGHT_F745_AIO_V2/target.h delete mode 100644 src/main/target/IFRC_IFLIGHT_F745_AIO_V2/target.mk diff --git a/src/main/target/IFLIGHT_F745_AIO_V2/target.mk b/src/main/target/IFLIGHT_F745_AIO_V2/target.mk index 6e2f072698..6dc3654f9b 100644 --- a/src/main/target/IFLIGHT_F745_AIO_V2/target.mk +++ b/src/main/target/IFLIGHT_F745_AIO_V2/target.mk @@ -2,6 +2,7 @@ F7X5XG_TARGETS += $(TARGET) FEATURES += VCP ONBOARDFLASH TARGET_SRC = \ + $(ROOT)/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_maximum_fifo.c \ drivers/accgyro/accgyro_spi_bmi270.c \ drivers/barometer/barometer_ms5611.c \ drivers/barometer/barometer_bmp280.c \ diff --git a/src/main/target/IFRC_IFLIGHT_F745_AIO_V2/target.c b/src/main/target/IFRC_IFLIGHT_F745_AIO_V2/target.c deleted file mode 100644 index c57c497050..0000000000 --- a/src/main/target/IFRC_IFLIGHT_F745_AIO_V2/target.c +++ /dev/null @@ -1,44 +0,0 @@ -/* - * This file is part of Cleanflight and Betaflight. - * - * Cleanflight and Betaflight are free software. You can redistribute - * this software and/or modify this software under the terms of the - * GNU General Public License as published by the Free Software - * Foundation, either version 3 of the License, or (at your option) - * any later version. - * - * Cleanflight and Betaflight are distributed in the hope that they - * will be useful, but WITHOUT ANY WARRANTY; without even the implied - * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. - * See the GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this software. - * - * If not, see . - */ - -#include - -#include "platform.h" -#include "drivers/io.h" - -#include "drivers/dma.h" -#include "drivers/timer.h" -#include "drivers/timer_def.h" - -const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - - DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0 ), //LED - DEF_TIM(TIM2, CH2, PB3, TIM_USE_ANY, 0, 0 ), //CAM - - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0 ), // M1 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0, 0 ), // M2 - DEF_TIM(TIM3, CH1, PB4, TIM_USE_MOTOR, 0, 0 ), // M3 - DEF_TIM(TIM3, CH2, PB5, TIM_USE_MOTOR, 0, 0 ), // M4 - DEF_TIM(TIM4, CH1, PD12, TIM_USE_MOTOR, 0, 0 ), // M5 - DEF_TIM(TIM4, CH2, PD13, TIM_USE_MOTOR, 0, 0 ), // M6 - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MOTOR, 0, 1 ), // M7 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MOTOR, 0, 0 ), // M8 - - }; diff --git a/src/main/target/IFRC_IFLIGHT_F745_AIO_V2/target.h b/src/main/target/IFRC_IFLIGHT_F745_AIO_V2/target.h deleted file mode 100644 index 75760be9f7..0000000000 --- a/src/main/target/IFRC_IFLIGHT_F745_AIO_V2/target.h +++ /dev/null @@ -1,184 +0,0 @@ -/* - * This file is part of Cleanflight and Betaflight. - * - * Cleanflight and Betaflight are free software. You can redistribute - * this software and/or modify this software under the terms of the - * GNU General Public License as published by the Free Software - * Foundation, either version 3 of the License, or (at your option) - * any later version. - * - * Cleanflight and Betaflight are distributed in the hope that they - * will be useful, but WITHOUT ANY WARRANTY; without even the implied - * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. - * See the GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this software. - * - * If not, see . - */ - -#pragma once - -#define TARGET_BOARD_IDENTIFIER "IFRC" - -#define USBD_PRODUCT_STRING "IFLIGHT_F745_AIO_V2" - -#define LED0_PIN PC13 - -#define USE_BEEPER -#define BEEPER_PIN PD2 -#define BEEPER_INVERTED - -//**********Gyro Acc *************// - -#define USE_ACC -#define USE_GYRO -#define USE_DUAL_GYRO - -#define USE_SPI_GYRO -#define USE_ACCGYRO_BMI270 - -#define USE_EXTI -#define USE_GYRO_EXTI -#define GYRO_1_EXTI_PIN PD0 -#define GYRO_2_EXTI_PIN PD8 -#define MPU_INT_EXTI - -#define GYRO_1_CS_PIN PA4 -#define GYRO_1_SPI_INSTANCE SPI1 -#define GYRO_2_CS_PIN PB12 -#define GYRO_2_SPI_INSTANCE SPI2 - -#define GYRO_1_ALIGN CW0_DEG -#define ACC_1_ALIGN CW0_DEG -#define GYRO_2_ALIGN CW180_DEG -#define ACC_2_ALIGN CW180_DEG - -#define USE_MPU_DATA_READY_SIGNAL -#define ENSURE_MPU_DATA_READY_IS_LOW -#define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_1 - -//**********Mag and Baro**********// - -#define USE_MAG -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define MAG_HMC5883_ALIGN CW180_DEG -#define MAG_QMC5883_ALIGN CW180_DEG -#define MAG_I2C_INSTANCE (I2CDEV_1) - -#define USE_BARO -#define USE_BARO_MS5611 -#define USE_BARO_BMP280 -#define USE_BARO_DPS310 -#define BARO_I2C_INSTANCE (I2CDEV_1) - -//**********Serial****************// - -#define USE_VCP -//#define USE_USB_DETECT -//#define USB_DETECT_PIN PC4 - -#define USE_UART1 -#define UART1_RX_PIN PA10 -#define UART1_TX_PIN PA9 - -#define USE_UART2 -#define UART2_RX_PIN PA3 -#define UART2_TX_PIN PA2 - -#define USE_UART3 -#define UART3_RX_PIN PB11 -#define UART3_TX_PIN PB10 - -#define USE_UART4 -#define UART4_RX_PIN PA1 -#define UART4_TX_PIN PA0 - -#define USE_UART6 -#define UART6_RX_PIN PC7 -#define UART6_TX_PIN PC6 - -#define USE_UART7 -#define UART7_RX_PIN PE7 -#define UART7_TX_PIN PE8 - -#define USE_UART8 -#define UART8_RX_PIN PE0 -#define UART8_TX_PIN PE1 - -#define SERIAL_PORT_COUNT 8 //VCP, USART1, USART2, USART3, UART4, USART6, USART7, USART8 - -#define USE_ESCSERIAL -#define USE_CAMERA_CONTROL -#define CAMERA_CONTROL_PIN PB3 - -#define USE_SPI -#define USE_SPI_DEVICE_1 -#define USE_SPI_DEVICE_2 -#define USE_SPI_DEVICE_3 -#define USE_SPI_DEVICE_4 - -#define SPI1_SCK_PIN PA5 -#define SPI1_MISO_PIN PA6 -#define SPI1_MOSI_PIN PA7 - -#define SPI2_SCK_PIN PB13 -#define SPI2_MISO_PIN PB14 -#define SPI2_MOSI_PIN PB15 - -#define SPI3_SCK_PIN PC10 -#define SPI3_MISO_PIN PC11 -#define SPI3_MOSI_PIN PC12 - -#define SPI4_SCK_PIN PE2 -#define SPI4_MISO_PIN PE5 -#define SPI4_MOSI_PIN PE6 - -#define USE_MAX7456 -#define MAX7456_SPI_INSTANCE SPI4 -#define MAX7456_SPI_CS_PIN PE4 -#define MAX7456_SPI_CLK (SPI_CLOCK_STANDARD) // 10MHz -#define MAX7456_RESTORE_CLK (SPI_CLOCK_FAST) - -#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT -#define USE_FLASHFS -#define USE_FLASH_M25P16 -#define FLASH_CS_PIN PA15 -#define FLASH_SPI_INSTANCE SPI3 - -#define USE_I2C -#define USE_I2C_DEVICE_1 -#define I2C_DEVICE_1 (I2CDEV_1) -#define I2C1_SCL PB8 -#define I2C1_SDA PB9 - -#define USE_I2C_DEVICE_2 -#define I2C_DEVICE_2 (I2CDEV_2) -#define I2C2_SCL PB10 -#define I2C2_SDA PB11 - -#define USE_ADC -#define VBAT_ADC_PIN PC3 -#define CURRENT_METER_ADC_PIN PC2 -#define RSSI_ADC_PIN PC5 -#define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC -#define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ADC -#define CURRENT_METER_SCALE_DEFAULT 100 - -#define USE_LED_STRIP - -#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY ) -#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL -#define SERIALRX_PROVIDER SERIALRX_SBUS - -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD 0xffff -#define TARGET_IO_PORTE 0xffff - -#define USABLE_TIMER_CHANNEL_COUNT 10 - -#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) ) diff --git a/src/main/target/IFRC_IFLIGHT_F745_AIO_V2/target.mk b/src/main/target/IFRC_IFLIGHT_F745_AIO_V2/target.mk deleted file mode 100644 index 6dc3654f9b..0000000000 --- a/src/main/target/IFRC_IFLIGHT_F745_AIO_V2/target.mk +++ /dev/null @@ -1,15 +0,0 @@ -F7X5XG_TARGETS += $(TARGET) -FEATURES += VCP ONBOARDFLASH - -TARGET_SRC = \ - $(ROOT)/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_maximum_fifo.c \ - drivers/accgyro/accgyro_spi_bmi270.c \ - drivers/barometer/barometer_ms5611.c \ - drivers/barometer/barometer_bmp280.c \ - drivers/barometer/barometer_bmp085.c \ - drivers/barometer/barometer_ms5611.c \ - drivers/compass/compass_hmc5883l.c \ - drivers/compass/compass_qmc5883l.c \ - drivers/light_ws2811strip.c \ - drivers/light_ws2811strip_hal.c \ - drivers/max7456.c From 4c3276c796537defa9f7d34d2aeee606751ecfa8 Mon Sep 17 00:00:00 2001 From: Peck07 <28628457+Peck07@users.noreply.github.com> Date: Mon, 15 May 2023 16:12:55 +0200 Subject: [PATCH 05/36] BMI270 should compile on IFLIGHT_F745_AIO_V2 target --- src/main/drivers/accgyro/accgyro.h | 3 + src/main/drivers/accgyro/accgyro_spi_bmi270.c | 302 ++++-------- .../accgyro/accgyro_spi_bmi270.c.newww | 456 ++++++++++++++++++ src/main/drivers/bus_spi.c | 25 + src/main/drivers/bus_spi.h | 3 + src/main/drivers/resource.h | 1 + 6 files changed, 578 insertions(+), 212 deletions(-) create mode 100644 src/main/drivers/accgyro/accgyro_spi_bmi270.c.newww diff --git a/src/main/drivers/accgyro/accgyro.h b/src/main/drivers/accgyro/accgyro.h index 2c17f4b2f5..b355a5be90 100644 --- a/src/main/drivers/accgyro/accgyro.h +++ b/src/main/drivers/accgyro/accgyro.h @@ -33,6 +33,9 @@ #include #endif +#define GYRO_SCALE_2000DPS (2000.0f / (1 << 15)) // 16.384 dps/lsb scalefactor for 2000dps sensors +#define GYRO_SCALE_4000DPS (4000.0f / (1 << 15)) // 8.192 dps/lsb scalefactor for 4000dps sensor + #ifndef MPU_I2C_INSTANCE #define MPU_I2C_INSTANCE I2C_DEVICE #endif diff --git a/src/main/drivers/accgyro/accgyro_spi_bmi270.c b/src/main/drivers/accgyro/accgyro_spi_bmi270.c index 0e97a1b46e..0bd138a48c 100644 --- a/src/main/drivers/accgyro/accgyro_spi_bmi270.c +++ b/src/main/drivers/accgyro/accgyro_spi_bmi270.c @@ -20,8 +20,6 @@ #include #include -#include -#include #include "platform.h" @@ -35,11 +33,8 @@ #include "drivers/io_impl.h" #include "drivers/nvic.h" #include "drivers/sensor.h" -#include "drivers/system.h" #include "drivers/time.h" -#include "sensors/gyro.h" - // 10 MHz max SPI frequency #define BMI270_MAX_SPI_CLK_HZ 10000000 @@ -110,9 +105,7 @@ typedef enum { BMI270_VAL_ACC_RANGE_8G = 0x02, // set acc to 8G full scale BMI270_VAL_ACC_RANGE_16G = 0x03, // set acc to 16G full scale BMI270_VAL_GYRO_CONF_ODR3200 = 0x0D, // set gyro sample rate to 3200hz - BMI270_VAL_GYRO_CONF_BWP_OSR4 = 0x00, // set gyro filter in OSR4 mode - BMI270_VAL_GYRO_CONF_BWP_OSR2 = 0x01, // set gyro filter in OSR2 mode - BMI270_VAL_GYRO_CONF_BWP_NORM = 0x02, // set gyro filter in normal mode + BMI270_VAL_GYRO_CONF_BWP = 0x02, // set gyro filter in normal mode BMI270_VAL_GYRO_CONF_NOISE_PERF = 0x01, // set gyro in high performance noise mode BMI270_VAL_GYRO_CONF_FILTER_PERF = 0x01, // set gyro in high performance filter mode @@ -130,25 +123,22 @@ typedef enum { BMI270_VAL_FIFO_WTM_1 = 0x00, // FIFO watermark MSB } bmi270ConfigValues_e; -// Need to see at least this many interrupts during initialisation to confirm EXTI connectivity -#define GYRO_EXTI_DETECT_THRESHOLD 1000 - // BMI270 register reads are 16bits with the first byte a "dummy" value 0 // that must be ignored. The result is in the second byte. -static uint8_t bmi270RegisterRead(const extDevice_t *dev, bmi270Register_e registerId) +static uint8_t bmi270RegisterRead(const busDevice_t *bus, bmi270Register_e registerId) { uint8_t data[2] = { 0, 0 }; - if (spiReadRegMskBufRB(dev, registerId, data, 2)) { + if (spiBusReadRegisterBuffer(bus, registerId, data, 2)) { return data[1]; } else { return 0; } } -static void bmi270RegisterWrite(const extDevice_t *dev, bmi270Register_e registerId, uint8_t value, unsigned delayMs) +static void bmi270RegisterWrite(const busDevice_t *bus, bmi270Register_e registerId, uint8_t value, unsigned delayMs) { - spiWriteReg(dev, registerId, value); + spiBusWriteRegister(bus, registerId, value); if (delayMs) { delay(delayMs); } @@ -156,56 +146,44 @@ static void bmi270RegisterWrite(const extDevice_t *dev, bmi270Register_e registe // Toggle the CS to switch the device into SPI mode. // Device switches initializes as I2C and switches to SPI on a low to high CS transition -static void bmi270EnableSPI(const extDevice_t *dev) +static void bmi270EnableSPI(const busDevice_t *bus) { - IOLo(dev->busType_u.spi.csnPin); + IOLo(bus->busdev_u.spi.csnPin); delay(1); - IOHi(dev->busType_u.spi.csnPin); + IOHi(bus->busdev_u.spi.csnPin); delay(10); } -uint8_t bmi270Detect(const extDevice_t *dev) +uint8_t bmi270Detect(const busDevice_t *bus) { - spiSetClkDivisor(dev, spiCalculateDivider(BMI270_MAX_SPI_CLK_HZ)); - bmi270EnableSPI(dev); + spiSetDivisor(bus->busdev_u.spi.instance, spiCalculateDivider(BMI270_MAX_SPI_CLK_HZ)); + bmi270EnableSPI(bus); - if (bmi270RegisterRead(dev, BMI270_REG_CHIP_ID) == BMI270_CHIP_ID) { + if (bmi270RegisterRead(bus, BMI270_REG_CHIP_ID) == BMI270_CHIP_ID) { return BMI_270_SPI; } return MPU_NONE; } -static void bmi270UploadConfig(const extDevice_t *dev) +static void bmi270UploadConfig(const busDevice_t *bus) { - bmi270RegisterWrite(dev, BMI270_REG_PWR_CONF, 0, 1); - bmi270RegisterWrite(dev, BMI270_REG_INIT_CTRL, 0, 1); + bmi270RegisterWrite(bus, BMI270_REG_PWR_CONF, 0, 1); + bmi270RegisterWrite(bus, BMI270_REG_INIT_CTRL, 0, 1); // Transfer the config file - spiWriteRegBuf(dev, BMI270_REG_INIT_DATA, (uint8_t *)bmi270_maximum_fifo_config_file, sizeof(bmi270_maximum_fifo_config_file)); + IOLo(bus->busdev_u.spi.csnPin); + spiTransferByte(bus->busdev_u.spi.instance, BMI270_REG_INIT_DATA); + spiTransfer(bus->busdev_u.spi.instance, bmi270_maximum_fifo_config_file, NULL, sizeof(bmi270_maximum_fifo_config_file)); + IOHi(bus->busdev_u.spi.csnPin); delay(10); - bmi270RegisterWrite(dev, BMI270_REG_INIT_CTRL, 1, 1); -} - -static uint8_t getBmiOsrMode() -{ - switch(gyroConfig()->gyro_hardware_lpf) { - case GYRO_HARDWARE_LPF_NORMAL: - return BMI270_VAL_GYRO_CONF_BWP_OSR4; - case GYRO_HARDWARE_LPF_OPTION_1: - return BMI270_VAL_GYRO_CONF_BWP_OSR2; - case GYRO_HARDWARE_LPF_OPTION_2: - return BMI270_VAL_GYRO_CONF_BWP_NORM; - case GYRO_HARDWARE_LPF_EXPERIMENTAL: - return BMI270_VAL_GYRO_CONF_BWP_NORM; - } - return 0; + bmi270RegisterWrite(bus, BMI270_REG_INIT_CTRL, 1, 1); } -static void bmi270Config(gyroDev_t *gyro) +static void bmi270Config(const gyroDev_t *gyro) { - extDevice_t *dev = &gyro->dev; + const busDevice_t *bus = &gyro->bus; // If running in hardware_lpf experimental mode then switch to FIFO-based, // 6.4KHz sampling, unfiltered data vs. the default 3.2KHz with hardware filtering @@ -217,95 +195,65 @@ static void bmi270Config(gyroDev_t *gyro) // Perform a soft reset to set all configuration to default // Delay 100ms before continuing configuration - bmi270RegisterWrite(dev, BMI270_REG_CMD, BMI270_VAL_CMD_SOFTRESET, 100); + bmi270RegisterWrite(bus, BMI270_REG_CMD, BMI270_VAL_CMD_SOFTRESET, 100); // Toggle the chip into SPI mode - bmi270EnableSPI(dev); + bmi270EnableSPI(bus); - bmi270UploadConfig(dev); + bmi270UploadConfig(bus); // Configure the FIFO if (fifoMode) { - bmi270RegisterWrite(dev, BMI270_REG_FIFO_CONFIG_0, BMI270_VAL_FIFO_CONFIG_0, 1); - bmi270RegisterWrite(dev, BMI270_REG_FIFO_CONFIG_1, BMI270_VAL_FIFO_CONFIG_1, 1); - bmi270RegisterWrite(dev, BMI270_REG_FIFO_DOWNS, BMI270_VAL_FIFO_DOWNS, 1); - bmi270RegisterWrite(dev, BMI270_REG_FIFO_WTM_0, BMI270_VAL_FIFO_WTM_0, 1); - bmi270RegisterWrite(dev, BMI270_REG_FIFO_WTM_1, BMI270_VAL_FIFO_WTM_1, 1); + bmi270RegisterWrite(bus, BMI270_REG_FIFO_CONFIG_0, BMI270_VAL_FIFO_CONFIG_0, 1); + bmi270RegisterWrite(bus, BMI270_REG_FIFO_CONFIG_1, BMI270_VAL_FIFO_CONFIG_1, 1); + bmi270RegisterWrite(bus, BMI270_REG_FIFO_DOWNS, BMI270_VAL_FIFO_DOWNS, 1); + bmi270RegisterWrite(bus, BMI270_REG_FIFO_WTM_0, BMI270_VAL_FIFO_WTM_0, 1); + bmi270RegisterWrite(bus, BMI270_REG_FIFO_WTM_1, BMI270_VAL_FIFO_WTM_1, 1); } // Configure the accelerometer - bmi270RegisterWrite(dev, BMI270_REG_ACC_CONF, (BMI270_VAL_ACC_CONF_HP << 7) | (BMI270_VAL_ACC_CONF_BWP << 4) | BMI270_VAL_ACC_CONF_ODR800, 1); + bmi270RegisterWrite(bus, BMI270_REG_ACC_CONF, (BMI270_VAL_ACC_CONF_HP << 7) | (BMI270_VAL_ACC_CONF_BWP << 4) | BMI270_VAL_ACC_CONF_ODR800, 1); // Configure the accelerometer full-scale range - bmi270RegisterWrite(dev, BMI270_REG_ACC_RANGE, BMI270_VAL_ACC_RANGE_16G, 1); + bmi270RegisterWrite(bus, BMI270_REG_ACC_RANGE, BMI270_VAL_ACC_RANGE_16G, 1); // Configure the gyro - bmi270RegisterWrite(dev, BMI270_REG_GYRO_CONF, (BMI270_VAL_GYRO_CONF_FILTER_PERF << 7) | (BMI270_VAL_GYRO_CONF_NOISE_PERF << 6) | (getBmiOsrMode() << 4) | BMI270_VAL_GYRO_CONF_ODR3200, 1); + bmi270RegisterWrite(bus, BMI270_REG_GYRO_CONF, (BMI270_VAL_GYRO_CONF_FILTER_PERF << 7) | (BMI270_VAL_GYRO_CONF_NOISE_PERF << 6) | (BMI270_VAL_GYRO_CONF_BWP << 4) | BMI270_VAL_GYRO_CONF_ODR3200, 1); // Configure the gyro full-range scale - bmi270RegisterWrite(dev, BMI270_REG_GYRO_RANGE, BMI270_VAL_GYRO_RANGE_2000DPS, 1); + bmi270RegisterWrite(bus, BMI270_REG_GYRO_RANGE, BMI270_VAL_GYRO_RANGE_2000DPS, 1); // Configure the gyro data ready interrupt if (fifoMode) { // Interrupt driven by FIFO watermark level - bmi270RegisterWrite(dev, BMI270_REG_INT_MAP_DATA, BMI270_VAL_INT_MAP_FIFO_WM_INT1, 1); + bmi270RegisterWrite(bus, BMI270_REG_INT_MAP_DATA, BMI270_VAL_INT_MAP_FIFO_WM_INT1, 1); } else { // Interrupt driven by data ready - bmi270RegisterWrite(dev, BMI270_REG_INT_MAP_DATA, BMI270_VAL_INT_MAP_DATA_DRDY_INT1, 1); + bmi270RegisterWrite(bus, BMI270_REG_INT_MAP_DATA, BMI270_VAL_INT_MAP_DATA_DRDY_INT1, 1); } // Configure the behavior of the INT1 pin - bmi270RegisterWrite(dev, BMI270_REG_INT1_IO_CTRL, BMI270_VAL_INT1_IO_CTRL_PINMODE, 1); + bmi270RegisterWrite(bus, BMI270_REG_INT1_IO_CTRL, BMI270_VAL_INT1_IO_CTRL_PINMODE, 1); // Configure the device for performance mode - bmi270RegisterWrite(dev, BMI270_REG_PWR_CONF, BMI270_VAL_PWR_CONF, 1); + bmi270RegisterWrite(bus, BMI270_REG_PWR_CONF, BMI270_VAL_PWR_CONF, 1); // Enable the gyro, accelerometer and temperature sensor - disable aux interface - bmi270RegisterWrite(dev, BMI270_REG_PWR_CTRL, BMI270_VAL_PWR_CTRL, 1); + bmi270RegisterWrite(bus, BMI270_REG_PWR_CTRL, BMI270_VAL_PWR_CTRL, 1); // Flush the FIFO if (fifoMode) { - bmi270RegisterWrite(dev, BMI270_REG_CMD, BMI270_VAL_CMD_FIFOFLUSH, 1); + bmi270RegisterWrite(bus, BMI270_REG_CMD, BMI270_VAL_CMD_FIFOFLUSH, 1); } } extiCallbackRec_t bmi270IntCallbackRec; -/* - * Gyro interrupt service routine - */ -#ifdef USE_GYRO_EXTI -// Called in ISR context -// Gyro read has just completed -busStatus_e bmi270Intcallback(uint32_t arg) -{ - gyroDev_t *gyro = (gyroDev_t *)arg; - int32_t gyroDmaDuration = cmpTimeCycles(getCycleCounter(), gyro->gyroLastEXTI); - - if (gyroDmaDuration > gyro->gyroDmaMaxDuration) { - gyro->gyroDmaMaxDuration = gyroDmaDuration; - } - - gyro->dataReady = true; - - return BUS_READY; -} - +#if defined(USE_GYRO_EXTI) && defined(USE_MPU_DATA_READY_SIGNAL) void bmi270ExtiHandler(extiCallbackRec_t *cb) { gyroDev_t *gyro = container_of(cb, gyroDev_t, exti); - // Ideally we'd use a timer to capture such information, but unfortunately the port used for EXTI interrupt does - // not have an associated timer - uint32_t nowCycles = getCycleCounter(); - gyro->gyroSyncEXTI = gyro->gyroLastEXTI + gyro->gyroDmaMaxDuration; - gyro->gyroLastEXTI = nowCycles; - - if (gyro->gyroModeSPI == GYRO_EXTI_INT_DMA) { - spiSequence(&gyro->dev, gyro->segments); - } - - gyro->detectedEXTI++; - + gyro->dataReady = true; } static void bmi270IntExtiInit(gyroDev_t *gyro) @@ -318,135 +266,63 @@ static void bmi270IntExtiInit(gyroDev_t *gyro) IOInit(mpuIntIO, OWNER_GYRO_EXTI, 0); EXTIHandlerInit(&gyro->exti, bmi270ExtiHandler); - EXTIConfig(mpuIntIO, &gyro->exti, NVIC_PRIO_MPU_INT_EXTI, IOCFG_IN_FLOATING, BETAFLIGHT_EXTI_TRIGGER_RISING); - EXTIEnable(mpuIntIO); -} -#else -void bmi270ExtiHandler(extiCallbackRec_t *cb) -{ - gyroDev_t *gyro = container_of(cb, gyroDev_t, exti); - gyro->dataReady = true; + EXTIConfig(mpuIntIO, &gyro->exti, NVIC_PRIO_MPU_INT_EXTI, IOCFG_IN_FLOATING ); + EXTIEnable(mpuIntIO, true); } #endif static bool bmi270AccRead(accDev_t *acc) { - switch (acc->gyro->gyroModeSPI) { - case GYRO_EXTI_INT: - case GYRO_EXTI_NO_INT: - { - acc->gyro->dev.txBuf[0] = BMI270_REG_ACC_DATA_X_LSB | 0x80; - - busSegment_t segments[] = { - {.u.buffers = {NULL, NULL}, 8, true, NULL}, - {.u.link = {NULL, NULL}, 0, true, NULL}, - }; - segments[0].u.buffers.txData = acc->gyro->dev.txBuf; - segments[0].u.buffers.rxData = acc->gyro->dev.rxBuf; - - spiSequence(&acc->gyro->dev, &segments[0]); - - // Wait for completion - spiWait(&acc->gyro->dev); + enum { + IDX_REG = 0, + IDX_SKIP, + IDX_ACCEL_XOUT_L, + IDX_ACCEL_XOUT_H, + IDX_ACCEL_YOUT_L, + IDX_ACCEL_YOUT_H, + IDX_ACCEL_ZOUT_L, + IDX_ACCEL_ZOUT_H, + BUFFER_SIZE, + }; - // Fall through - FALLTHROUGH; - } + uint8_t bmi270_rx_buf[BUFFER_SIZE]; + static const uint8_t bmi270_tx_buf[BUFFER_SIZE] = {BMI270_REG_ACC_DATA_X_LSB | 0x80, 0, 0, 0, 0, 0, 0, 0}; - case GYRO_EXTI_INT_DMA: - { - // If read was triggered in interrupt don't bother waiting. The worst that could happen is that we pick - // up an old value. - - // This data was read from the gyro, which is the same SPI device as the acc - uint16_t *accData = (uint16_t *)acc->gyro->dev.rxBuf; - acc->ADCRaw[X] = accData[1]; - acc->ADCRaw[Y] = accData[2]; - acc->ADCRaw[Z] = accData[3]; - break; - } + IOLo(acc->bus.busdev_u.spi.csnPin); + spiTransfer(acc->bus.busdev_u.spi.instance, bmi270_tx_buf, bmi270_rx_buf, BUFFER_SIZE); // receive response + IOHi(acc->bus.busdev_u.spi.csnPin); - case GYRO_EXTI_INIT: - default: - break; - } + acc->ADCRaw[X] = (int16_t)((bmi270_rx_buf[IDX_ACCEL_XOUT_H] << 8) | bmi270_rx_buf[IDX_ACCEL_XOUT_L]); + acc->ADCRaw[Y] = (int16_t)((bmi270_rx_buf[IDX_ACCEL_YOUT_H] << 8) | bmi270_rx_buf[IDX_ACCEL_YOUT_L]); + acc->ADCRaw[Z] = (int16_t)((bmi270_rx_buf[IDX_ACCEL_ZOUT_H] << 8) | bmi270_rx_buf[IDX_ACCEL_ZOUT_L]); return true; } static bool bmi270GyroReadRegister(gyroDev_t *gyro) { - uint16_t *gyroData = (uint16_t *)gyro->dev.rxBuf; - switch (gyro->gyroModeSPI) { - case GYRO_EXTI_INIT: - { - // Initialise the tx buffer to all 0x00 - memset(gyro->dev.txBuf, 0x00, 14); -#ifdef USE_GYRO_EXTI - // Check that minimum number of interrupts have been detected - - // We need some offset from the gyro interrupts to ensure sampling after the interrupt - gyro->gyroDmaMaxDuration = 5; - // Using DMA for gyro access upsets the scheduler on the F4 - if (gyro->detectedEXTI > GYRO_EXTI_DETECT_THRESHOLD) { - if (spiUseDMA(&gyro->dev)) { - gyro->dev.callbackArg = (uint32_t)gyro; - gyro->dev.txBuf[0] = BMI270_REG_ACC_DATA_X_LSB | 0x80; - gyro->segments[0].len = 14; - gyro->segments[0].callback = bmi270Intcallback; - gyro->segments[0].u.buffers.txData = gyro->dev.txBuf; - gyro->segments[0].u.buffers.rxData = gyro->dev.rxBuf; - gyro->segments[0].negateCS = true; - gyro->gyroModeSPI = GYRO_EXTI_INT_DMA; - } else { - // Interrupts are present, but no DMA - gyro->gyroModeSPI = GYRO_EXTI_INT; - } - } else -#endif - { - gyro->gyroModeSPI = GYRO_EXTI_NO_INT; - } - break; - } - - case GYRO_EXTI_INT: - case GYRO_EXTI_NO_INT: - { - gyro->dev.txBuf[0] = BMI270_REG_GYR_DATA_X_LSB | 0x80; - - busSegment_t segments[] = { - {.u.buffers = {NULL, NULL}, 8, true, NULL}, - {.u.link = {NULL, NULL}, 0, true, NULL}, - }; - segments[0].u.buffers.txData = gyro->dev.txBuf; - segments[0].u.buffers.rxData = gyro->dev.rxBuf; - - spiSequence(&gyro->dev, &segments[0]); - - // Wait for completion - spiWait(&gyro->dev); - - gyro->gyroADCRaw[X] = gyroData[1]; - gyro->gyroADCRaw[Y] = gyroData[2]; - gyro->gyroADCRaw[Z] = gyroData[3]; + enum { + IDX_REG = 0, + IDX_SKIP, + IDX_GYRO_XOUT_L, + IDX_GYRO_XOUT_H, + IDX_GYRO_YOUT_L, + IDX_GYRO_YOUT_H, + IDX_GYRO_ZOUT_L, + IDX_GYRO_ZOUT_H, + BUFFER_SIZE, + }; - break; - } + uint8_t bmi270_rx_buf[BUFFER_SIZE]; + static const uint8_t bmi270_tx_buf[BUFFER_SIZE] = {BMI270_REG_GYR_DATA_X_LSB | 0x80, 0, 0, 0, 0, 0, 0, 0}; - case GYRO_EXTI_INT_DMA: - { - // If read was triggered in interrupt don't bother waiting. The worst that could happen is that we pick - // up an old value. - gyro->gyroADCRaw[X] = gyroData[4]; - gyro->gyroADCRaw[Y] = gyroData[5]; - gyro->gyroADCRaw[Z] = gyroData[6]; - break; - } + IOLo(gyro->bus.busdev_u.spi.csnPin); + spiTransfer(gyro->bus.busdev_u.spi.instance, bmi270_tx_buf, bmi270_rx_buf, BUFFER_SIZE); // receive response + IOHi(gyro->bus.busdev_u.spi.csnPin); - default: - break; - } + gyro->gyroADCRaw[X] = (int16_t)((bmi270_rx_buf[IDX_GYRO_XOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_XOUT_L]); + gyro->gyroADCRaw[Y] = (int16_t)((bmi270_rx_buf[IDX_GYRO_YOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_YOUT_L]); + gyro->gyroADCRaw[Z] = (int16_t)((bmi270_rx_buf[IDX_GYRO_ZOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_ZOUT_L]); return true; } @@ -469,13 +345,15 @@ static bool bmi270GyroReadFifo(gyroDev_t *gyro) }; bool dataRead = false; - STATIC_DMA_DATA_AUTO uint8_t bmi270_tx_buf[BUFFER_SIZE] = {BMI270_REG_FIFO_LENGTH_LSB | 0x80, 0, 0, 0, 0, 0, 0, 0, 0, 0}; - STATIC_DMA_DATA_AUTO uint8_t bmi270_rx_buf[BUFFER_SIZE]; + static const uint8_t bmi270_tx_buf[BUFFER_SIZE] = {BMI270_REG_FIFO_LENGTH_LSB | 0x80, 0, 0, 0, 0, 0, 0, 0, 0, 0}; + uint8_t bmi270_rx_buf[BUFFER_SIZE]; // Burst read the FIFO length followed by the next 6 bytes containing the gyro axis data for // the first sample in the queue. It's possible for the FIFO to be empty so we need to check the // length before using the sample. - spiReadWriteBuf(&gyro->dev, (uint8_t *)bmi270_tx_buf, bmi270_rx_buf, BUFFER_SIZE); // receive response + IOLo(gyro->bus.busdev_u.spi.csnPin); + spiTransfer(gyro->bus.busdev_u.spi.instance, bmi270_tx_buf, bmi270_rx_buf, BUFFER_SIZE); // receive response + IOHi(gyro->bus.busdev_u.spi.csnPin); int fifoLength = (uint16_t)((bmi270_rx_buf[IDX_FIFO_LENGTH_H] << 8) | bmi270_rx_buf[IDX_FIFO_LENGTH_L]); @@ -507,7 +385,7 @@ static bool bmi270GyroReadFifo(gyroDev_t *gyro) // would end up in a lock state of always re-reading the same partial or invalid sample. if (fifoLength > 0) { // Partial or additional frames left - flush the FIFO - bmi270RegisterWrite(&gyro->dev, BMI270_REG_CMD, BMI270_VAL_CMD_FIFOFLUSH, 0); + bmi270RegisterWrite(&gyro->bus, BMI270_REG_CMD, BMI270_VAL_CMD_FIFOFLUSH, 0); } return dataRead; @@ -532,7 +410,7 @@ static void bmi270SpiGyroInit(gyroDev_t *gyro) { bmi270Config(gyro); -#if defined(USE_GYRO_EXTI) +#if defined(USE_GYRO_EXTI) && defined(USE_MPU_DATA_READY_SIGNAL) bmi270IntExtiInit(gyro); #endif } @@ -578,6 +456,6 @@ bool bmi270SpiGyroDetect(gyroDev_t *gyro) // watermark reason as an idication of gyro data ready. uint8_t bmi270InterruptStatus(gyroDev_t *gyro) { - return bmi270RegisterRead(&gyro->dev, BMI270_REG_INT_STATUS_1); + return bmi270RegisterRead(&gyro->bus, BMI270_REG_INT_STATUS_1); } #endif // USE_ACCGYRO_BMI270 diff --git a/src/main/drivers/accgyro/accgyro_spi_bmi270.c.newww b/src/main/drivers/accgyro/accgyro_spi_bmi270.c.newww new file mode 100644 index 0000000000..67115d7837 --- /dev/null +++ b/src/main/drivers/accgyro/accgyro_spi_bmi270.c.newww @@ -0,0 +1,456 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +/* based on the commit: https://github.com/betaflight/betaflight/commit/ab66795eebcd21aea6e292f8c0bfdba33255c133 + * as the next commit in betaflight is a big "refactor" affecting SPI handling, structures, etc + */ + +#include +#include + +#include "platform.h" + +#ifdef USE_ACCGYRO_BMI270 + +#include "drivers/accgyro/accgyro.h" +#include "drivers/accgyro/accgyro_spi_bmi270.h" +#include "drivers/bus_spi.h" +#include "drivers/exti.h" +#include "drivers/io.h" +#include "drivers/io_impl.h" +#include "drivers/nvic.h" +#include "drivers/sensor.h" +#include "drivers/time.h" +#define STATIC_DMA_DATA_AUTO static +// 10 MHz max SPI frequency +#define BMI270_MAX_SPI_CLK_HZ 10000000 + +#define BMI270_FIFO_FRAME_SIZE 6 + +#define BMI270_CONFIG_SIZE 328 + +// Declaration for the device config (microcode) that must be uploaded to the sensor +extern const uint8_t bmi270_maximum_fifo_config_file[BMI270_CONFIG_SIZE]; + +#define BMI270_CHIP_ID 0x24 + +// BMI270 registers (not the complete list) +typedef enum { + BMI270_REG_CHIP_ID = 0x00, + BMI270_REG_ERR_REG = 0x02, + BMI270_REG_STATUS = 0x03, + BMI270_REG_ACC_DATA_X_LSB = 0x0C, + BMI270_REG_GYR_DATA_X_LSB = 0x12, + BMI270_REG_SENSORTIME_0 = 0x18, + BMI270_REG_SENSORTIME_1 = 0x19, + BMI270_REG_SENSORTIME_2 = 0x1A, + BMI270_REG_EVENT = 0x1B, + BMI270_REG_INT_STATUS_0 = 0x1C, + BMI270_REG_INT_STATUS_1 = 0x1D, + BMI270_REG_INTERNAL_STATUS = 0x21, + BMI270_REG_TEMPERATURE_LSB = 0x22, + BMI270_REG_TEMPERATURE_MSB = 0x23, + BMI270_REG_FIFO_LENGTH_LSB = 0x24, + BMI270_REG_FIFO_LENGTH_MSB = 0x25, + BMI270_REG_FIFO_DATA = 0x26, + BMI270_REG_ACC_CONF = 0x40, + BMI270_REG_ACC_RANGE = 0x41, + BMI270_REG_GYRO_CONF = 0x42, + BMI270_REG_GYRO_RANGE = 0x43, + BMI270_REG_AUX_CONF = 0x44, + BMI270_REG_FIFO_DOWNS = 0x45, + BMI270_REG_FIFO_WTM_0 = 0x46, + BMI270_REG_FIFO_WTM_1 = 0x47, + BMI270_REG_FIFO_CONFIG_0 = 0x48, + BMI270_REG_FIFO_CONFIG_1 = 0x49, + BMI270_REG_SATURATION = 0x4A, + BMI270_REG_INT1_IO_CTRL = 0x53, + BMI270_REG_INT2_IO_CTRL = 0x54, + BMI270_REG_INT_LATCH = 0x55, + BMI270_REG_INT1_MAP_FEAT = 0x56, + BMI270_REG_INT2_MAP_FEAT = 0x57, + BMI270_REG_INT_MAP_DATA = 0x58, + BMI270_REG_INIT_CTRL = 0x59, + BMI270_REG_INIT_DATA = 0x5E, + BMI270_REG_ACC_SELF_TEST = 0x6D, + BMI270_REG_GYR_SELF_TEST_AXES = 0x6E, + BMI270_REG_PWR_CONF = 0x7C, + BMI270_REG_PWR_CTRL = 0x7D, + BMI270_REG_CMD = 0x7E, +} bmi270Register_e; + +// BMI270 register configuration values +typedef enum { + BMI270_VAL_CMD_SOFTRESET = 0xB6, + BMI270_VAL_CMD_FIFOFLUSH = 0xB0, + BMI270_VAL_PWR_CTRL = 0x0E, // enable gyro, acc and temp sensors + BMI270_VAL_PWR_CONF = 0x02, // disable advanced power save, enable FIFO self-wake + BMI270_VAL_ACC_CONF_ODR800 = 0x0B, // set acc sample rate to 800hz + BMI270_VAL_ACC_CONF_ODR1600 = 0x0C, // set acc sample rate to 1600hz + BMI270_VAL_ACC_CONF_BWP = 0x02, // set acc filter in normal mode + BMI270_VAL_ACC_CONF_HP = 0x01, // set acc in high performance mode + BMI270_VAL_ACC_RANGE_8G = 0x02, // set acc to 8G full scale + BMI270_VAL_ACC_RANGE_16G = 0x03, // set acc to 16G full scale + BMI270_VAL_GYRO_CONF_ODR3200 = 0x0D, // set gyro sample rate to 3200hz + BMI270_VAL_GYRO_CONF_BWP = 0x02, // set gyro filter in normal mode + BMI270_VAL_GYRO_CONF_NOISE_PERF = 0x01, // set gyro in high performance noise mode + BMI270_VAL_GYRO_CONF_FILTER_PERF = 0x01, // set gyro in high performance filter mode + + BMI270_VAL_GYRO_RANGE_2000DPS = 0x08, // set gyro to 2000dps full scale + // for some reason you have to enable the ois_range bit (bit 3) for 2000dps as well + // or else the gyro scale will be 250dps when in prefiltered FIFO mode (not documented in datasheet!) + + BMI270_VAL_INT_MAP_DATA_DRDY_INT1 = 0x04,// enable the data ready interrupt pin 1 + BMI270_VAL_INT_MAP_FIFO_WM_INT1 = 0x02, // enable the FIFO watermark interrupt pin 1 + BMI270_VAL_INT1_IO_CTRL_PINMODE = 0x0A, // active high, push-pull, output enabled, input disabled + BMI270_VAL_FIFO_CONFIG_0 = 0x00, // don't stop when full, disable sensortime frame + BMI270_VAL_FIFO_CONFIG_1 = 0x80, // only gyro data in FIFO, use headerless mode + BMI270_VAL_FIFO_DOWNS = 0x00, // select unfiltered gyro data with no downsampling (6.4KHz samples) + BMI270_VAL_FIFO_WTM_0 = 0x06, // set the FIFO watermark level to 1 gyro sample (6 bytes) + BMI270_VAL_FIFO_WTM_1 = 0x00, // FIFO watermark MSB +} bmi270ConfigValues_e; + +// BMI270 register reads are 16bits with the first byte a "dummy" value 0 +// that must be ignored. The result is in the second byte. +static uint8_t bmi270RegisterRead(const extDevice_t *dev, bmi270Register_e registerId) +{ + uint8_t data[2] = { 0, 0 }; + + if (spiReadRegMskBufRB(dev, registerId, data, 2)) { + return data[1]; + } else { + return 0; + } +} + +static void bmi270RegisterWrite(const extDevice_t *dev, bmi270Register_e registerId, uint8_t value, unsigned delayMs) +{ + spiWriteReg(dev, registerId, value); + if (delayMs) { + delay(delayMs); + } +} + +// Toggle the CS to switch the device into SPI mode. +// Device switches initializes as I2C and switches to SPI on a low to high CS transition +static void bmi270EnableSPI(const extDevice_t *dev) +{ + IOLo(dev->busType_u.spi.csnPin); + delay(1); + IOHi(dev->busType_u.spi.csnPin); + delay(10); +} + +uint8_t bmi270Detect(const extDevice_t *dev) +{ + spiSetClkDivisor(dev, spiCalculateDivider(BMI270_MAX_SPI_CLK_HZ)); + bmi270EnableSPI(dev); + + if (bmi270RegisterRead(dev, BMI270_REG_CHIP_ID) == BMI270_CHIP_ID) { + return BMI_270_SPI; + } + + return MPU_NONE; +} + +static void bmi270UploadConfig(const extDevice_t *dev) +{ + bmi270RegisterWrite(dev, BMI270_REG_PWR_CONF, 0, 1); + bmi270RegisterWrite(dev, BMI270_REG_INIT_CTRL, 0, 1); + + // Transfer the config file + spiWriteRegBuf(dev, BMI270_REG_INIT_DATA, (uint8_t *)bmi270_maximum_fifo_config_file, sizeof(bmi270_maximum_fifo_config_file)); + + delay(10); + bmi270RegisterWrite(dev, BMI270_REG_INIT_CTRL, 1, 1); +} + +static void bmi270Config(gyroDev_t *gyro) +{ + extDevice_t *dev = &gyro->dev; + + // If running in hardware_lpf experimental mode then switch to FIFO-based, + // 6.4KHz sampling, unfiltered data vs. the default 3.2KHz with hardware filtering +#ifdef USE_GYRO_DLPF_EXPERIMENTAL + const bool fifoMode = (gyro->hardware_lpf == GYRO_HARDWARE_LPF_EXPERIMENTAL); +#else + const bool fifoMode = false; +#endif + + // Perform a soft reset to set all configuration to default + // Delay 100ms before continuing configuration + bmi270RegisterWrite(dev, BMI270_REG_CMD, BMI270_VAL_CMD_SOFTRESET, 100); + + // Toggle the chip into SPI mode + bmi270EnableSPI(dev); + + bmi270UploadConfig(dev); + + // Configure the FIFO + if (fifoMode) { + bmi270RegisterWrite(dev, BMI270_REG_FIFO_CONFIG_0, BMI270_VAL_FIFO_CONFIG_0, 1); + bmi270RegisterWrite(dev, BMI270_REG_FIFO_CONFIG_1, BMI270_VAL_FIFO_CONFIG_1, 1); + bmi270RegisterWrite(dev, BMI270_REG_FIFO_DOWNS, BMI270_VAL_FIFO_DOWNS, 1); + bmi270RegisterWrite(dev, BMI270_REG_FIFO_WTM_0, BMI270_VAL_FIFO_WTM_0, 1); + bmi270RegisterWrite(dev, BMI270_REG_FIFO_WTM_1, BMI270_VAL_FIFO_WTM_1, 1); + } + + // Configure the accelerometer + bmi270RegisterWrite(dev, BMI270_REG_ACC_CONF, (BMI270_VAL_ACC_CONF_HP << 7) | (BMI270_VAL_ACC_CONF_BWP << 4) | BMI270_VAL_ACC_CONF_ODR800, 1); + + // Configure the accelerometer full-scale range + bmi270RegisterWrite(dev, BMI270_REG_ACC_RANGE, BMI270_VAL_ACC_RANGE_16G, 1); + + // Configure the gyro + bmi270RegisterWrite(dev, BMI270_REG_GYRO_CONF, (BMI270_VAL_GYRO_CONF_FILTER_PERF << 7) | (BMI270_VAL_GYRO_CONF_NOISE_PERF << 6) | (BMI270_VAL_GYRO_CONF_BWP << 4) | BMI270_VAL_GYRO_CONF_ODR3200, 1); + + // Configure the gyro full-range scale + bmi270RegisterWrite(dev, BMI270_REG_GYRO_RANGE, BMI270_VAL_GYRO_RANGE_2000DPS, 1); + + // Configure the gyro data ready interrupt + if (fifoMode) { + // Interrupt driven by FIFO watermark level + bmi270RegisterWrite(dev, BMI270_REG_INT_MAP_DATA, BMI270_VAL_INT_MAP_FIFO_WM_INT1, 1); + } else { + // Interrupt driven by data ready + bmi270RegisterWrite(dev, BMI270_REG_INT_MAP_DATA, BMI270_VAL_INT_MAP_DATA_DRDY_INT1, 1); + } + + // Configure the behavior of the INT1 pin + bmi270RegisterWrite(dev, BMI270_REG_INT1_IO_CTRL, BMI270_VAL_INT1_IO_CTRL_PINMODE, 1); + + // Configure the device for performance mode + bmi270RegisterWrite(dev, BMI270_REG_PWR_CONF, BMI270_VAL_PWR_CONF, 1); + + // Enable the gyro, accelerometer and temperature sensor - disable aux interface + bmi270RegisterWrite(dev, BMI270_REG_PWR_CTRL, BMI270_VAL_PWR_CTRL, 1); + + // Flush the FIFO + if (fifoMode) { + bmi270RegisterWrite(dev, BMI270_REG_CMD, BMI270_VAL_CMD_FIFOFLUSH, 1); + } +} + +extiCallbackRec_t bmi270IntCallbackRec; + +#if defined(USE_GYRO_EXTI) && defined(USE_MPU_DATA_READY_SIGNAL) +void bmi270ExtiHandler(extiCallbackRec_t *cb) +{ + gyroDev_t *gyro = container_of(cb, gyroDev_t, exti); + gyro->dataReady = true; +} + +static void bmi270IntExtiInit(gyroDev_t *gyro) +{ + if (gyro->mpuIntExtiTag == IO_TAG_NONE) { + return; + } + + IO_t mpuIntIO = IOGetByTag(gyro->mpuIntExtiTag); + + IOInit(mpuIntIO, OWNER_GYRO_EXTI, 0); + EXTIHandlerInit(&gyro->exti, bmi270ExtiHandler); + EXTIConfig(mpuIntIO, &gyro->exti, NVIC_PRIO_MPU_INT_EXTI, IOCFG_IN_FLOATING ); + EXTIEnable(mpuIntIO, true); +} +#endif + +static bool bmi270AccRead(accDev_t *acc) +{ + enum { + IDX_REG = 0, + IDX_SKIP, + IDX_ACCEL_XOUT_L, + IDX_ACCEL_XOUT_H, + IDX_ACCEL_YOUT_L, + IDX_ACCEL_YOUT_H, + IDX_ACCEL_ZOUT_L, + IDX_ACCEL_ZOUT_H, + BUFFER_SIZE, + }; + + STATIC_DMA_DATA_AUTO uint8_t bmi270_rx_buf[BUFFER_SIZE]; + STATIC_DMA_DATA_AUTO uint8_t bmi270_tx_buf[BUFFER_SIZE] = {BMI270_REG_ACC_DATA_X_LSB | 0x80, 0, 0, 0, 0, 0, 0, 0}; + + spiReadWriteBuf(&acc->gyro->dev, (uint8_t *)bmi270_tx_buf, bmi270_rx_buf, BUFFER_SIZE); // receive response + + acc->ADCRaw[X] = (int16_t)((bmi270_rx_buf[IDX_ACCEL_XOUT_H] << 8) | bmi270_rx_buf[IDX_ACCEL_XOUT_L]); + acc->ADCRaw[Y] = (int16_t)((bmi270_rx_buf[IDX_ACCEL_YOUT_H] << 8) | bmi270_rx_buf[IDX_ACCEL_YOUT_L]); + acc->ADCRaw[Z] = (int16_t)((bmi270_rx_buf[IDX_ACCEL_ZOUT_H] << 8) | bmi270_rx_buf[IDX_ACCEL_ZOUT_L]); + + return true; +} + +static bool bmi270GyroReadRegister(gyroDev_t *gyro) +{ + enum { + IDX_REG = 0, + IDX_SKIP, + IDX_GYRO_XOUT_L, + IDX_GYRO_XOUT_H, + IDX_GYRO_YOUT_L, + IDX_GYRO_YOUT_H, + IDX_GYRO_ZOUT_L, + IDX_GYRO_ZOUT_H, + BUFFER_SIZE, + }; + + STATIC_DMA_DATA_AUTO uint8_t bmi270_rx_buf[BUFFER_SIZE]; + STATIC_DMA_DATA_AUTO uint8_t bmi270_tx_buf[BUFFER_SIZE] = {BMI270_REG_GYR_DATA_X_LSB | 0x80, 0, 0, 0, 0, 0, 0, 0}; + + spiReadWriteBuf(&gyro->dev, (uint8_t *)bmi270_tx_buf, bmi270_rx_buf, BUFFER_SIZE); // receive response + + gyro->gyroADCRaw[X] = (int16_t)((bmi270_rx_buf[IDX_GYRO_XOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_XOUT_L]); + gyro->gyroADCRaw[Y] = (int16_t)((bmi270_rx_buf[IDX_GYRO_YOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_YOUT_L]); + gyro->gyroADCRaw[Z] = (int16_t)((bmi270_rx_buf[IDX_GYRO_ZOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_ZOUT_L]); + + return true; +} + +#ifdef USE_GYRO_DLPF_EXPERIMENTAL +static bool bmi270GyroReadFifo(gyroDev_t *gyro) +{ + enum { + IDX_REG = 0, + IDX_SKIP, + IDX_FIFO_LENGTH_L, + IDX_FIFO_LENGTH_H, + IDX_GYRO_XOUT_L, + IDX_GYRO_XOUT_H, + IDX_GYRO_YOUT_L, + IDX_GYRO_YOUT_H, + IDX_GYRO_ZOUT_L, + IDX_GYRO_ZOUT_H, + BUFFER_SIZE, + }; + + bool dataRead = false; + STATIC_DMA_DATA_AUTO uint8_t bmi270_tx_buf[BUFFER_SIZE] = {BMI270_REG_FIFO_LENGTH_LSB | 0x80, 0, 0, 0, 0, 0, 0, 0, 0, 0}; + STATIC_DMA_DATA_AUTO uint8_t bmi270_rx_buf[BUFFER_SIZE]; + + // Burst read the FIFO length followed by the next 6 bytes containing the gyro axis data for + // the first sample in the queue. It's possible for the FIFO to be empty so we need to check the + // length before using the sample. + spiReadWriteBuf(&gyro->dev, (uint8_t *)bmi270_tx_buf, bmi270_rx_buf, BUFFER_SIZE); // receive response + + int fifoLength = (uint16_t)((bmi270_rx_buf[IDX_FIFO_LENGTH_H] << 8) | bmi270_rx_buf[IDX_FIFO_LENGTH_L]); + + if (fifoLength >= BMI270_FIFO_FRAME_SIZE) { + + const int16_t gyroX = (int16_t)((bmi270_rx_buf[IDX_GYRO_XOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_XOUT_L]); + const int16_t gyroY = (int16_t)((bmi270_rx_buf[IDX_GYRO_YOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_YOUT_L]); + const int16_t gyroZ = (int16_t)((bmi270_rx_buf[IDX_GYRO_ZOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_ZOUT_L]); + + // If the FIFO data is invalid then the returned values will be 0x8000 (-32768) (pg. 43 of datasheet). + // This shouldn't happen since we're only using the data if the FIFO length indicates + // that data is available, but this safeguard is needed to prevent bad things in + // case it does happen. + if ((gyroX != INT16_MIN) || (gyroY != INT16_MIN) || (gyroZ != INT16_MIN)) { + gyro->gyroADCRaw[X] = gyroX; + gyro->gyroADCRaw[Y] = gyroY; + gyro->gyroADCRaw[Z] = gyroZ; + dataRead = true; + } + fifoLength -= BMI270_FIFO_FRAME_SIZE; + } + + // If there are additional samples in the FIFO then we don't use those for now and simply + // flush the FIFO. Under normal circumstances we only expect one sample in the FIFO since + // the gyro loop is running at the native sample rate of 6.4KHz. + // However the way the FIFO works in the sensor is that if a frame is partially read then + // it remains in the queue instead of bein removed. So if we ever got into a state where there + // was a partial frame or other unexpected data in the FIFO is may never get cleared and we + // would end up in a lock state of always re-reading the same partial or invalid sample. + if (fifoLength > 0) { + // Partial or additional frames left - flush the FIFO + bmi270RegisterWrite(&gyro->dev, BMI270_REG_CMD, BMI270_VAL_CMD_FIFOFLUSH, 0); + } + + return dataRead; +} +#endif + +static bool bmi270GyroRead(gyroDev_t *gyro) +{ +#ifdef USE_GYRO_DLPF_EXPERIMENTAL + if (gyro->hardware_lpf == GYRO_HARDWARE_LPF_EXPERIMENTAL) { + // running in 6.4KHz FIFO mode + return bmi270GyroReadFifo(gyro); + } else +#endif + { + // running in 3.2KHz register mode + return bmi270GyroReadRegister(gyro); + } +} + +static void bmi270SpiGyroInit(gyroDev_t *gyro) +{ + bmi270Config(gyro); + +#if defined(USE_GYRO_EXTI) && defined(USE_MPU_DATA_READY_SIGNAL) + bmi270IntExtiInit(gyro); +#endif +} + +static void bmi270SpiAccInit(accDev_t *acc) +{ + // sensor is configured during gyro init + acc->acc_1G = 512 * 4; // 16G sensor scale +} + +bool bmi270SpiAccDetect(accDev_t *acc) +{ + if (acc->mpuDetectionResult.sensor != BMI_270_SPI) { + return false; + } + + acc->initFn = bmi270SpiAccInit; + acc->readFn = bmi270AccRead; + + return true; +} + + +bool bmi270SpiGyroDetect(gyroDev_t *gyro) +{ + if (gyro->mpuDetectionResult.sensor != BMI_270_SPI) { + return false; + } + + gyro->initFn = bmi270SpiGyroInit; + gyro->readFn = bmi270GyroRead; + gyro->scale = GYRO_SCALE_2000DPS; + + return true; +} + +// Used to query the status register to determine what event caused the EXTI to fire. +// When in 3.2KHz mode the interrupt is mapped to the data ready state. However the data ready +// trigger will fire for both gyro and accelerometer. So it's necessary to check this register +// to determine which event caused the interrupt. +// When in 6.4KHz mode the interrupt is configured to be the FIFO watermark size of 6 bytes. +// Since in this mode we only put gyro data in the FIFO it's sufficient to check for the FIFO +// watermark reason as an idication of gyro data ready. +uint8_t bmi270InterruptStatus(gyroDev_t *gyro) +{ + return bmi270RegisterRead(&gyro->dev, BMI270_REG_INT_STATUS_1); +} +#endif // USE_ACCGYRO_BMI270 diff --git a/src/main/drivers/bus_spi.c b/src/main/drivers/bus_spi.c index d42f0e6156..bccc221f56 100644 --- a/src/main/drivers/bus_spi.c +++ b/src/main/drivers/bus_spi.c @@ -157,6 +157,31 @@ void spiResetErrorCounter(SPI_TypeDef *instance) { } } +uint16_t spiCalculateDivider(uint32_t freq) +{ +#if defined(STM32F4) || defined(STM32G4) || defined(STM32F7) + uint32_t spiClk = SystemCoreClock / 2; +#elif defined(STM32H7) + uint32_t spiClk = 100000000; +#elif defined(AT32F4) + if(freq > 36000000){ + freq = 36000000; + } + + uint32_t spiClk = system_core_clock / 2; +#else +#error "Base SPI clock not defined for this architecture" +#endif + + uint16_t divisor = 2; + + spiClk >>= 1; + + for (; (spiClk > freq) && (divisor < 256); divisor <<= 1, spiClk >>= 1); + + return divisor; +} + FAST_CODE bool spiBusWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data) { #ifdef USE_DMA_SPI_DEVICE if(USE_DMA_SPI_DEVICE == bus->busdev_u.spi.instance) { diff --git a/src/main/drivers/bus_spi.h b/src/main/drivers/bus_spi.h index 1fa4a0dab7..31b37db145 100644 --- a/src/main/drivers/bus_spi.h +++ b/src/main/drivers/bus_spi.h @@ -119,6 +119,9 @@ bool spiBusReadRegisterBuffer(const busDevice_t *bus, uint8_t reg, uint8_t *data uint8_t spiBusReadRegister(const busDevice_t *bus, uint8_t reg); void spiBusSetInstance(busDevice_t *bus, SPI_TypeDef *instance); +// Determine the divisor to use for a given bus frequency +uint16_t spiCalculateDivider(uint32_t freq); + struct spiPinConfig_s; void spiPinConfigure(const struct spiPinConfig_s *pConfig); diff --git a/src/main/drivers/resource.h b/src/main/drivers/resource.h index 8b8daaf6a8..14eaf6138e 100644 --- a/src/main/drivers/resource.h +++ b/src/main/drivers/resource.h @@ -53,6 +53,7 @@ typedef enum { OWNER_OSD_CS, OWNER_RX_SPI_CS, OWNER_SPI_CS, + OWNER_GYRO_EXTI, OWNER_MPU_EXTI, OWNER_BARO_EXTI, OWNER_COMPASS_EXTI, From f3e3165ace658506e9efa26949edcdaf8cd9786b Mon Sep 17 00:00:00 2001 From: Peck07 <28628457+Peck07@users.noreply.github.com> Date: Mon, 15 May 2023 16:13:42 +0200 Subject: [PATCH 06/36] add BMI270 support to BETAFPVF411 target --- src/main/target/BETAFPVF411/target.h | 6 ++++++ src/main/target/BETAFPVF411/target.mk | 2 ++ 2 files changed, 8 insertions(+) diff --git a/src/main/target/BETAFPVF411/target.h b/src/main/target/BETAFPVF411/target.h index 68172fdc56..2a2062848e 100644 --- a/src/main/target/BETAFPVF411/target.h +++ b/src/main/target/BETAFPVF411/target.h @@ -34,6 +34,12 @@ #define USE_SPI #define USE_SPI_DEVICE_1 +#define USE_SPI_GYRO +#define USE_ACCGYRO_BMI270 +#define BMI270_SPI_INSTANCE SPI1 +#define ACC_BMI270_ALIGN CW90_DEG +#define GYRO_BMI270_ALIGN CW90_DEG + #define SPI1_SCK_PIN PA5 #define SPI1_MISO_PIN PA6 #define SPI1_MOSI_PIN PA7 diff --git a/src/main/target/BETAFPVF411/target.mk b/src/main/target/BETAFPVF411/target.mk index 3971d0ca39..3e1d2e44a5 100644 --- a/src/main/target/BETAFPVF411/target.mk +++ b/src/main/target/BETAFPVF411/target.mk @@ -4,6 +4,8 @@ FEATURES = VCP ONBOARDFLASH TARGET_SRC = \ drivers/accgyro/accgyro_mpu.c \ drivers/accgyro/accgyro_spi_mpu6000.c \ + $(ROOT)/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_maximum_fifo.c \ + drivers/accgyro/accgyro_spi_bmi270.c \ drivers/barometer/barometer_bmp085.c \ drivers/barometer/barometer_bmp280.c \ drivers/barometer/barometer_ms5611.c \ From 973509c3fd4a0edc1a5a6a662c1f1fd946bfdb95 Mon Sep 17 00:00:00 2001 From: Roland <28628457+Peck07@users.noreply.github.com> Date: Mon, 15 May 2023 16:25:17 +0200 Subject: [PATCH 07/36] Delete accgyro_spi_bmi270.c.newww --- .../accgyro/accgyro_spi_bmi270.c.newww | 456 ------------------ 1 file changed, 456 deletions(-) delete mode 100644 src/main/drivers/accgyro/accgyro_spi_bmi270.c.newww diff --git a/src/main/drivers/accgyro/accgyro_spi_bmi270.c.newww b/src/main/drivers/accgyro/accgyro_spi_bmi270.c.newww deleted file mode 100644 index 67115d7837..0000000000 --- a/src/main/drivers/accgyro/accgyro_spi_bmi270.c.newww +++ /dev/null @@ -1,456 +0,0 @@ -/* - * This file is part of Cleanflight and Betaflight. - * - * Cleanflight and Betaflight are free software. You can redistribute - * this software and/or modify this software under the terms of the - * GNU General Public License as published by the Free Software - * Foundation, either version 3 of the License, or (at your option) - * any later version. - * - * Cleanflight and Betaflight are distributed in the hope that they - * will be useful, but WITHOUT ANY WARRANTY; without even the implied - * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. - * See the GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this software. - * - * If not, see . - */ - -/* based on the commit: https://github.com/betaflight/betaflight/commit/ab66795eebcd21aea6e292f8c0bfdba33255c133 - * as the next commit in betaflight is a big "refactor" affecting SPI handling, structures, etc - */ - -#include -#include - -#include "platform.h" - -#ifdef USE_ACCGYRO_BMI270 - -#include "drivers/accgyro/accgyro.h" -#include "drivers/accgyro/accgyro_spi_bmi270.h" -#include "drivers/bus_spi.h" -#include "drivers/exti.h" -#include "drivers/io.h" -#include "drivers/io_impl.h" -#include "drivers/nvic.h" -#include "drivers/sensor.h" -#include "drivers/time.h" -#define STATIC_DMA_DATA_AUTO static -// 10 MHz max SPI frequency -#define BMI270_MAX_SPI_CLK_HZ 10000000 - -#define BMI270_FIFO_FRAME_SIZE 6 - -#define BMI270_CONFIG_SIZE 328 - -// Declaration for the device config (microcode) that must be uploaded to the sensor -extern const uint8_t bmi270_maximum_fifo_config_file[BMI270_CONFIG_SIZE]; - -#define BMI270_CHIP_ID 0x24 - -// BMI270 registers (not the complete list) -typedef enum { - BMI270_REG_CHIP_ID = 0x00, - BMI270_REG_ERR_REG = 0x02, - BMI270_REG_STATUS = 0x03, - BMI270_REG_ACC_DATA_X_LSB = 0x0C, - BMI270_REG_GYR_DATA_X_LSB = 0x12, - BMI270_REG_SENSORTIME_0 = 0x18, - BMI270_REG_SENSORTIME_1 = 0x19, - BMI270_REG_SENSORTIME_2 = 0x1A, - BMI270_REG_EVENT = 0x1B, - BMI270_REG_INT_STATUS_0 = 0x1C, - BMI270_REG_INT_STATUS_1 = 0x1D, - BMI270_REG_INTERNAL_STATUS = 0x21, - BMI270_REG_TEMPERATURE_LSB = 0x22, - BMI270_REG_TEMPERATURE_MSB = 0x23, - BMI270_REG_FIFO_LENGTH_LSB = 0x24, - BMI270_REG_FIFO_LENGTH_MSB = 0x25, - BMI270_REG_FIFO_DATA = 0x26, - BMI270_REG_ACC_CONF = 0x40, - BMI270_REG_ACC_RANGE = 0x41, - BMI270_REG_GYRO_CONF = 0x42, - BMI270_REG_GYRO_RANGE = 0x43, - BMI270_REG_AUX_CONF = 0x44, - BMI270_REG_FIFO_DOWNS = 0x45, - BMI270_REG_FIFO_WTM_0 = 0x46, - BMI270_REG_FIFO_WTM_1 = 0x47, - BMI270_REG_FIFO_CONFIG_0 = 0x48, - BMI270_REG_FIFO_CONFIG_1 = 0x49, - BMI270_REG_SATURATION = 0x4A, - BMI270_REG_INT1_IO_CTRL = 0x53, - BMI270_REG_INT2_IO_CTRL = 0x54, - BMI270_REG_INT_LATCH = 0x55, - BMI270_REG_INT1_MAP_FEAT = 0x56, - BMI270_REG_INT2_MAP_FEAT = 0x57, - BMI270_REG_INT_MAP_DATA = 0x58, - BMI270_REG_INIT_CTRL = 0x59, - BMI270_REG_INIT_DATA = 0x5E, - BMI270_REG_ACC_SELF_TEST = 0x6D, - BMI270_REG_GYR_SELF_TEST_AXES = 0x6E, - BMI270_REG_PWR_CONF = 0x7C, - BMI270_REG_PWR_CTRL = 0x7D, - BMI270_REG_CMD = 0x7E, -} bmi270Register_e; - -// BMI270 register configuration values -typedef enum { - BMI270_VAL_CMD_SOFTRESET = 0xB6, - BMI270_VAL_CMD_FIFOFLUSH = 0xB0, - BMI270_VAL_PWR_CTRL = 0x0E, // enable gyro, acc and temp sensors - BMI270_VAL_PWR_CONF = 0x02, // disable advanced power save, enable FIFO self-wake - BMI270_VAL_ACC_CONF_ODR800 = 0x0B, // set acc sample rate to 800hz - BMI270_VAL_ACC_CONF_ODR1600 = 0x0C, // set acc sample rate to 1600hz - BMI270_VAL_ACC_CONF_BWP = 0x02, // set acc filter in normal mode - BMI270_VAL_ACC_CONF_HP = 0x01, // set acc in high performance mode - BMI270_VAL_ACC_RANGE_8G = 0x02, // set acc to 8G full scale - BMI270_VAL_ACC_RANGE_16G = 0x03, // set acc to 16G full scale - BMI270_VAL_GYRO_CONF_ODR3200 = 0x0D, // set gyro sample rate to 3200hz - BMI270_VAL_GYRO_CONF_BWP = 0x02, // set gyro filter in normal mode - BMI270_VAL_GYRO_CONF_NOISE_PERF = 0x01, // set gyro in high performance noise mode - BMI270_VAL_GYRO_CONF_FILTER_PERF = 0x01, // set gyro in high performance filter mode - - BMI270_VAL_GYRO_RANGE_2000DPS = 0x08, // set gyro to 2000dps full scale - // for some reason you have to enable the ois_range bit (bit 3) for 2000dps as well - // or else the gyro scale will be 250dps when in prefiltered FIFO mode (not documented in datasheet!) - - BMI270_VAL_INT_MAP_DATA_DRDY_INT1 = 0x04,// enable the data ready interrupt pin 1 - BMI270_VAL_INT_MAP_FIFO_WM_INT1 = 0x02, // enable the FIFO watermark interrupt pin 1 - BMI270_VAL_INT1_IO_CTRL_PINMODE = 0x0A, // active high, push-pull, output enabled, input disabled - BMI270_VAL_FIFO_CONFIG_0 = 0x00, // don't stop when full, disable sensortime frame - BMI270_VAL_FIFO_CONFIG_1 = 0x80, // only gyro data in FIFO, use headerless mode - BMI270_VAL_FIFO_DOWNS = 0x00, // select unfiltered gyro data with no downsampling (6.4KHz samples) - BMI270_VAL_FIFO_WTM_0 = 0x06, // set the FIFO watermark level to 1 gyro sample (6 bytes) - BMI270_VAL_FIFO_WTM_1 = 0x00, // FIFO watermark MSB -} bmi270ConfigValues_e; - -// BMI270 register reads are 16bits with the first byte a "dummy" value 0 -// that must be ignored. The result is in the second byte. -static uint8_t bmi270RegisterRead(const extDevice_t *dev, bmi270Register_e registerId) -{ - uint8_t data[2] = { 0, 0 }; - - if (spiReadRegMskBufRB(dev, registerId, data, 2)) { - return data[1]; - } else { - return 0; - } -} - -static void bmi270RegisterWrite(const extDevice_t *dev, bmi270Register_e registerId, uint8_t value, unsigned delayMs) -{ - spiWriteReg(dev, registerId, value); - if (delayMs) { - delay(delayMs); - } -} - -// Toggle the CS to switch the device into SPI mode. -// Device switches initializes as I2C and switches to SPI on a low to high CS transition -static void bmi270EnableSPI(const extDevice_t *dev) -{ - IOLo(dev->busType_u.spi.csnPin); - delay(1); - IOHi(dev->busType_u.spi.csnPin); - delay(10); -} - -uint8_t bmi270Detect(const extDevice_t *dev) -{ - spiSetClkDivisor(dev, spiCalculateDivider(BMI270_MAX_SPI_CLK_HZ)); - bmi270EnableSPI(dev); - - if (bmi270RegisterRead(dev, BMI270_REG_CHIP_ID) == BMI270_CHIP_ID) { - return BMI_270_SPI; - } - - return MPU_NONE; -} - -static void bmi270UploadConfig(const extDevice_t *dev) -{ - bmi270RegisterWrite(dev, BMI270_REG_PWR_CONF, 0, 1); - bmi270RegisterWrite(dev, BMI270_REG_INIT_CTRL, 0, 1); - - // Transfer the config file - spiWriteRegBuf(dev, BMI270_REG_INIT_DATA, (uint8_t *)bmi270_maximum_fifo_config_file, sizeof(bmi270_maximum_fifo_config_file)); - - delay(10); - bmi270RegisterWrite(dev, BMI270_REG_INIT_CTRL, 1, 1); -} - -static void bmi270Config(gyroDev_t *gyro) -{ - extDevice_t *dev = &gyro->dev; - - // If running in hardware_lpf experimental mode then switch to FIFO-based, - // 6.4KHz sampling, unfiltered data vs. the default 3.2KHz with hardware filtering -#ifdef USE_GYRO_DLPF_EXPERIMENTAL - const bool fifoMode = (gyro->hardware_lpf == GYRO_HARDWARE_LPF_EXPERIMENTAL); -#else - const bool fifoMode = false; -#endif - - // Perform a soft reset to set all configuration to default - // Delay 100ms before continuing configuration - bmi270RegisterWrite(dev, BMI270_REG_CMD, BMI270_VAL_CMD_SOFTRESET, 100); - - // Toggle the chip into SPI mode - bmi270EnableSPI(dev); - - bmi270UploadConfig(dev); - - // Configure the FIFO - if (fifoMode) { - bmi270RegisterWrite(dev, BMI270_REG_FIFO_CONFIG_0, BMI270_VAL_FIFO_CONFIG_0, 1); - bmi270RegisterWrite(dev, BMI270_REG_FIFO_CONFIG_1, BMI270_VAL_FIFO_CONFIG_1, 1); - bmi270RegisterWrite(dev, BMI270_REG_FIFO_DOWNS, BMI270_VAL_FIFO_DOWNS, 1); - bmi270RegisterWrite(dev, BMI270_REG_FIFO_WTM_0, BMI270_VAL_FIFO_WTM_0, 1); - bmi270RegisterWrite(dev, BMI270_REG_FIFO_WTM_1, BMI270_VAL_FIFO_WTM_1, 1); - } - - // Configure the accelerometer - bmi270RegisterWrite(dev, BMI270_REG_ACC_CONF, (BMI270_VAL_ACC_CONF_HP << 7) | (BMI270_VAL_ACC_CONF_BWP << 4) | BMI270_VAL_ACC_CONF_ODR800, 1); - - // Configure the accelerometer full-scale range - bmi270RegisterWrite(dev, BMI270_REG_ACC_RANGE, BMI270_VAL_ACC_RANGE_16G, 1); - - // Configure the gyro - bmi270RegisterWrite(dev, BMI270_REG_GYRO_CONF, (BMI270_VAL_GYRO_CONF_FILTER_PERF << 7) | (BMI270_VAL_GYRO_CONF_NOISE_PERF << 6) | (BMI270_VAL_GYRO_CONF_BWP << 4) | BMI270_VAL_GYRO_CONF_ODR3200, 1); - - // Configure the gyro full-range scale - bmi270RegisterWrite(dev, BMI270_REG_GYRO_RANGE, BMI270_VAL_GYRO_RANGE_2000DPS, 1); - - // Configure the gyro data ready interrupt - if (fifoMode) { - // Interrupt driven by FIFO watermark level - bmi270RegisterWrite(dev, BMI270_REG_INT_MAP_DATA, BMI270_VAL_INT_MAP_FIFO_WM_INT1, 1); - } else { - // Interrupt driven by data ready - bmi270RegisterWrite(dev, BMI270_REG_INT_MAP_DATA, BMI270_VAL_INT_MAP_DATA_DRDY_INT1, 1); - } - - // Configure the behavior of the INT1 pin - bmi270RegisterWrite(dev, BMI270_REG_INT1_IO_CTRL, BMI270_VAL_INT1_IO_CTRL_PINMODE, 1); - - // Configure the device for performance mode - bmi270RegisterWrite(dev, BMI270_REG_PWR_CONF, BMI270_VAL_PWR_CONF, 1); - - // Enable the gyro, accelerometer and temperature sensor - disable aux interface - bmi270RegisterWrite(dev, BMI270_REG_PWR_CTRL, BMI270_VAL_PWR_CTRL, 1); - - // Flush the FIFO - if (fifoMode) { - bmi270RegisterWrite(dev, BMI270_REG_CMD, BMI270_VAL_CMD_FIFOFLUSH, 1); - } -} - -extiCallbackRec_t bmi270IntCallbackRec; - -#if defined(USE_GYRO_EXTI) && defined(USE_MPU_DATA_READY_SIGNAL) -void bmi270ExtiHandler(extiCallbackRec_t *cb) -{ - gyroDev_t *gyro = container_of(cb, gyroDev_t, exti); - gyro->dataReady = true; -} - -static void bmi270IntExtiInit(gyroDev_t *gyro) -{ - if (gyro->mpuIntExtiTag == IO_TAG_NONE) { - return; - } - - IO_t mpuIntIO = IOGetByTag(gyro->mpuIntExtiTag); - - IOInit(mpuIntIO, OWNER_GYRO_EXTI, 0); - EXTIHandlerInit(&gyro->exti, bmi270ExtiHandler); - EXTIConfig(mpuIntIO, &gyro->exti, NVIC_PRIO_MPU_INT_EXTI, IOCFG_IN_FLOATING ); - EXTIEnable(mpuIntIO, true); -} -#endif - -static bool bmi270AccRead(accDev_t *acc) -{ - enum { - IDX_REG = 0, - IDX_SKIP, - IDX_ACCEL_XOUT_L, - IDX_ACCEL_XOUT_H, - IDX_ACCEL_YOUT_L, - IDX_ACCEL_YOUT_H, - IDX_ACCEL_ZOUT_L, - IDX_ACCEL_ZOUT_H, - BUFFER_SIZE, - }; - - STATIC_DMA_DATA_AUTO uint8_t bmi270_rx_buf[BUFFER_SIZE]; - STATIC_DMA_DATA_AUTO uint8_t bmi270_tx_buf[BUFFER_SIZE] = {BMI270_REG_ACC_DATA_X_LSB | 0x80, 0, 0, 0, 0, 0, 0, 0}; - - spiReadWriteBuf(&acc->gyro->dev, (uint8_t *)bmi270_tx_buf, bmi270_rx_buf, BUFFER_SIZE); // receive response - - acc->ADCRaw[X] = (int16_t)((bmi270_rx_buf[IDX_ACCEL_XOUT_H] << 8) | bmi270_rx_buf[IDX_ACCEL_XOUT_L]); - acc->ADCRaw[Y] = (int16_t)((bmi270_rx_buf[IDX_ACCEL_YOUT_H] << 8) | bmi270_rx_buf[IDX_ACCEL_YOUT_L]); - acc->ADCRaw[Z] = (int16_t)((bmi270_rx_buf[IDX_ACCEL_ZOUT_H] << 8) | bmi270_rx_buf[IDX_ACCEL_ZOUT_L]); - - return true; -} - -static bool bmi270GyroReadRegister(gyroDev_t *gyro) -{ - enum { - IDX_REG = 0, - IDX_SKIP, - IDX_GYRO_XOUT_L, - IDX_GYRO_XOUT_H, - IDX_GYRO_YOUT_L, - IDX_GYRO_YOUT_H, - IDX_GYRO_ZOUT_L, - IDX_GYRO_ZOUT_H, - BUFFER_SIZE, - }; - - STATIC_DMA_DATA_AUTO uint8_t bmi270_rx_buf[BUFFER_SIZE]; - STATIC_DMA_DATA_AUTO uint8_t bmi270_tx_buf[BUFFER_SIZE] = {BMI270_REG_GYR_DATA_X_LSB | 0x80, 0, 0, 0, 0, 0, 0, 0}; - - spiReadWriteBuf(&gyro->dev, (uint8_t *)bmi270_tx_buf, bmi270_rx_buf, BUFFER_SIZE); // receive response - - gyro->gyroADCRaw[X] = (int16_t)((bmi270_rx_buf[IDX_GYRO_XOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_XOUT_L]); - gyro->gyroADCRaw[Y] = (int16_t)((bmi270_rx_buf[IDX_GYRO_YOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_YOUT_L]); - gyro->gyroADCRaw[Z] = (int16_t)((bmi270_rx_buf[IDX_GYRO_ZOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_ZOUT_L]); - - return true; -} - -#ifdef USE_GYRO_DLPF_EXPERIMENTAL -static bool bmi270GyroReadFifo(gyroDev_t *gyro) -{ - enum { - IDX_REG = 0, - IDX_SKIP, - IDX_FIFO_LENGTH_L, - IDX_FIFO_LENGTH_H, - IDX_GYRO_XOUT_L, - IDX_GYRO_XOUT_H, - IDX_GYRO_YOUT_L, - IDX_GYRO_YOUT_H, - IDX_GYRO_ZOUT_L, - IDX_GYRO_ZOUT_H, - BUFFER_SIZE, - }; - - bool dataRead = false; - STATIC_DMA_DATA_AUTO uint8_t bmi270_tx_buf[BUFFER_SIZE] = {BMI270_REG_FIFO_LENGTH_LSB | 0x80, 0, 0, 0, 0, 0, 0, 0, 0, 0}; - STATIC_DMA_DATA_AUTO uint8_t bmi270_rx_buf[BUFFER_SIZE]; - - // Burst read the FIFO length followed by the next 6 bytes containing the gyro axis data for - // the first sample in the queue. It's possible for the FIFO to be empty so we need to check the - // length before using the sample. - spiReadWriteBuf(&gyro->dev, (uint8_t *)bmi270_tx_buf, bmi270_rx_buf, BUFFER_SIZE); // receive response - - int fifoLength = (uint16_t)((bmi270_rx_buf[IDX_FIFO_LENGTH_H] << 8) | bmi270_rx_buf[IDX_FIFO_LENGTH_L]); - - if (fifoLength >= BMI270_FIFO_FRAME_SIZE) { - - const int16_t gyroX = (int16_t)((bmi270_rx_buf[IDX_GYRO_XOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_XOUT_L]); - const int16_t gyroY = (int16_t)((bmi270_rx_buf[IDX_GYRO_YOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_YOUT_L]); - const int16_t gyroZ = (int16_t)((bmi270_rx_buf[IDX_GYRO_ZOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_ZOUT_L]); - - // If the FIFO data is invalid then the returned values will be 0x8000 (-32768) (pg. 43 of datasheet). - // This shouldn't happen since we're only using the data if the FIFO length indicates - // that data is available, but this safeguard is needed to prevent bad things in - // case it does happen. - if ((gyroX != INT16_MIN) || (gyroY != INT16_MIN) || (gyroZ != INT16_MIN)) { - gyro->gyroADCRaw[X] = gyroX; - gyro->gyroADCRaw[Y] = gyroY; - gyro->gyroADCRaw[Z] = gyroZ; - dataRead = true; - } - fifoLength -= BMI270_FIFO_FRAME_SIZE; - } - - // If there are additional samples in the FIFO then we don't use those for now and simply - // flush the FIFO. Under normal circumstances we only expect one sample in the FIFO since - // the gyro loop is running at the native sample rate of 6.4KHz. - // However the way the FIFO works in the sensor is that if a frame is partially read then - // it remains in the queue instead of bein removed. So if we ever got into a state where there - // was a partial frame or other unexpected data in the FIFO is may never get cleared and we - // would end up in a lock state of always re-reading the same partial or invalid sample. - if (fifoLength > 0) { - // Partial or additional frames left - flush the FIFO - bmi270RegisterWrite(&gyro->dev, BMI270_REG_CMD, BMI270_VAL_CMD_FIFOFLUSH, 0); - } - - return dataRead; -} -#endif - -static bool bmi270GyroRead(gyroDev_t *gyro) -{ -#ifdef USE_GYRO_DLPF_EXPERIMENTAL - if (gyro->hardware_lpf == GYRO_HARDWARE_LPF_EXPERIMENTAL) { - // running in 6.4KHz FIFO mode - return bmi270GyroReadFifo(gyro); - } else -#endif - { - // running in 3.2KHz register mode - return bmi270GyroReadRegister(gyro); - } -} - -static void bmi270SpiGyroInit(gyroDev_t *gyro) -{ - bmi270Config(gyro); - -#if defined(USE_GYRO_EXTI) && defined(USE_MPU_DATA_READY_SIGNAL) - bmi270IntExtiInit(gyro); -#endif -} - -static void bmi270SpiAccInit(accDev_t *acc) -{ - // sensor is configured during gyro init - acc->acc_1G = 512 * 4; // 16G sensor scale -} - -bool bmi270SpiAccDetect(accDev_t *acc) -{ - if (acc->mpuDetectionResult.sensor != BMI_270_SPI) { - return false; - } - - acc->initFn = bmi270SpiAccInit; - acc->readFn = bmi270AccRead; - - return true; -} - - -bool bmi270SpiGyroDetect(gyroDev_t *gyro) -{ - if (gyro->mpuDetectionResult.sensor != BMI_270_SPI) { - return false; - } - - gyro->initFn = bmi270SpiGyroInit; - gyro->readFn = bmi270GyroRead; - gyro->scale = GYRO_SCALE_2000DPS; - - return true; -} - -// Used to query the status register to determine what event caused the EXTI to fire. -// When in 3.2KHz mode the interrupt is mapped to the data ready state. However the data ready -// trigger will fire for both gyro and accelerometer. So it's necessary to check this register -// to determine which event caused the interrupt. -// When in 6.4KHz mode the interrupt is configured to be the FIFO watermark size of 6 bytes. -// Since in this mode we only put gyro data in the FIFO it's sufficient to check for the FIFO -// watermark reason as an idication of gyro data ready. -uint8_t bmi270InterruptStatus(gyroDev_t *gyro) -{ - return bmi270RegisterRead(&gyro->dev, BMI270_REG_INT_STATUS_1); -} -#endif // USE_ACCGYRO_BMI270 From 16a057fd3abd03e5311fc90a456bc27a76bd9666 Mon Sep 17 00:00:00 2001 From: Peck07 <28628457+Peck07@users.noreply.github.com> Date: Mon, 15 May 2023 16:41:47 +0200 Subject: [PATCH 08/36] change to OSD4 by default --- src/main/drivers/accgyro/accgyro_spi_bmi270.c | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/drivers/accgyro/accgyro_spi_bmi270.c b/src/main/drivers/accgyro/accgyro_spi_bmi270.c index 0bd138a48c..c03d35ccab 100644 --- a/src/main/drivers/accgyro/accgyro_spi_bmi270.c +++ b/src/main/drivers/accgyro/accgyro_spi_bmi270.c @@ -105,7 +105,9 @@ typedef enum { BMI270_VAL_ACC_RANGE_8G = 0x02, // set acc to 8G full scale BMI270_VAL_ACC_RANGE_16G = 0x03, // set acc to 16G full scale BMI270_VAL_GYRO_CONF_ODR3200 = 0x0D, // set gyro sample rate to 3200hz - BMI270_VAL_GYRO_CONF_BWP = 0x02, // set gyro filter in normal mode + BMI270_VAL_GYRO_CONF_BWP_OSR4 = 0x00, // set gyro filter in OSR4 mode + BMI270_VAL_GYRO_CONF_BWP_OSR2 = 0x01, // set gyro filter in OSR2 mode + BMI270_VAL_GYRO_CONF_BWP_NORM = 0x02, // set gyro filter in normal mode BMI270_VAL_GYRO_CONF_NOISE_PERF = 0x01, // set gyro in high performance noise mode BMI270_VAL_GYRO_CONF_FILTER_PERF = 0x01, // set gyro in high performance filter mode @@ -218,7 +220,7 @@ static void bmi270Config(const gyroDev_t *gyro) bmi270RegisterWrite(bus, BMI270_REG_ACC_RANGE, BMI270_VAL_ACC_RANGE_16G, 1); // Configure the gyro - bmi270RegisterWrite(bus, BMI270_REG_GYRO_CONF, (BMI270_VAL_GYRO_CONF_FILTER_PERF << 7) | (BMI270_VAL_GYRO_CONF_NOISE_PERF << 6) | (BMI270_VAL_GYRO_CONF_BWP << 4) | BMI270_VAL_GYRO_CONF_ODR3200, 1); + bmi270RegisterWrite(bus, BMI270_REG_GYRO_CONF, (BMI270_VAL_GYRO_CONF_FILTER_PERF << 7) | (BMI270_VAL_GYRO_CONF_NOISE_PERF << 6) | (BMI270_VAL_GYRO_CONF_BWP_OSR4 << 4) | BMI270_VAL_GYRO_CONF_ODR3200, 1); // Configure the gyro full-range scale bmi270RegisterWrite(bus, BMI270_REG_GYRO_RANGE, BMI270_VAL_GYRO_RANGE_2000DPS, 1); From 3ebab76f48b6e8c1a3167bd16b41c913bc616fc8 Mon Sep 17 00:00:00 2001 From: Peck07 <28628457+Peck07@users.noreply.github.com> Date: Mon, 15 May 2023 17:09:41 +0200 Subject: [PATCH 09/36] put back a working getBmiOsrMode function (discussed with nerdCopter) --- src/main/drivers/accgyro/accgyro_spi_bmi270.c | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/src/main/drivers/accgyro/accgyro_spi_bmi270.c b/src/main/drivers/accgyro/accgyro_spi_bmi270.c index c03d35ccab..80793ac2a2 100644 --- a/src/main/drivers/accgyro/accgyro_spi_bmi270.c +++ b/src/main/drivers/accgyro/accgyro_spi_bmi270.c @@ -183,6 +183,17 @@ static void bmi270UploadConfig(const busDevice_t *bus) bmi270RegisterWrite(bus, BMI270_REG_INIT_CTRL, 1, 1); } +static uint8_t getBmiOsrMode() +{ + switch(gyroConfig()->gyro_hardware_lpf) { + case GYRO_HARDWARE_LPF_NORMAL: + return BMI270_VAL_GYRO_CONF_BWP_OSR4; + case GYRO_HARDWARE_LPF_EXPERIMENTAL: + return BMI270_VAL_GYRO_CONF_BWP_NORM; + } + return 0; +} + static void bmi270Config(const gyroDev_t *gyro) { const busDevice_t *bus = &gyro->bus; @@ -220,7 +231,7 @@ static void bmi270Config(const gyroDev_t *gyro) bmi270RegisterWrite(bus, BMI270_REG_ACC_RANGE, BMI270_VAL_ACC_RANGE_16G, 1); // Configure the gyro - bmi270RegisterWrite(bus, BMI270_REG_GYRO_CONF, (BMI270_VAL_GYRO_CONF_FILTER_PERF << 7) | (BMI270_VAL_GYRO_CONF_NOISE_PERF << 6) | (BMI270_VAL_GYRO_CONF_BWP_OSR4 << 4) | BMI270_VAL_GYRO_CONF_ODR3200, 1); + bmi270RegisterWrite(bus, BMI270_REG_GYRO_CONF, (BMI270_VAL_GYRO_CONF_FILTER_PERF << 7) | (BMI270_VAL_GYRO_CONF_NOISE_PERF << 6) | (getBmiOsrMode() << 4) | BMI270_VAL_GYRO_CONF_ODR3200, 1); // Configure the gyro full-range scale bmi270RegisterWrite(bus, BMI270_REG_GYRO_RANGE, BMI270_VAL_GYRO_RANGE_2000DPS, 1); From 7b238f117d63d2da13092b97e412fdf1bf1f982f Mon Sep 17 00:00:00 2001 From: Peck07 <28628457+Peck07@users.noreply.github.com> Date: Mon, 15 May 2023 17:13:32 +0200 Subject: [PATCH 10/36] LPF_EXPERIMENTAL should give back OSR2 --- src/main/drivers/accgyro/accgyro_spi_bmi270.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/drivers/accgyro/accgyro_spi_bmi270.c b/src/main/drivers/accgyro/accgyro_spi_bmi270.c index 80793ac2a2..5bad645ef4 100644 --- a/src/main/drivers/accgyro/accgyro_spi_bmi270.c +++ b/src/main/drivers/accgyro/accgyro_spi_bmi270.c @@ -189,7 +189,7 @@ static uint8_t getBmiOsrMode() case GYRO_HARDWARE_LPF_NORMAL: return BMI270_VAL_GYRO_CONF_BWP_OSR4; case GYRO_HARDWARE_LPF_EXPERIMENTAL: - return BMI270_VAL_GYRO_CONF_BWP_NORM; + return BMI270_VAL_GYRO_CONF_BWP_OSR2; } return 0; } From 41bd0d242d37ab1fcae6bb627bc696d86588953f Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Mon, 15 May 2023 12:57:11 -0500 Subject: [PATCH 11/36] unneeded files cleanup and file move; target path change --- .../BoschSensortec/BMI270-Sensor-API/LICENSE | 30 - .../BMI270-Sensor-API/OIS_README.md | 170 - .../BMI270-Sensor-API/README.md | 14 - .../BMI270-Sensor-API/betaflight_info.txt | 9 - .../BoschSensortec/BMI270-Sensor-API/bmi270.c | 4451 ----------------- .../BoschSensortec/BMI270-Sensor-API/bmi270.h | 422 -- .../BMI270-Sensor-API/bmi270_context.c | 3122 ------------ .../BMI270-Sensor-API/bmi270_context.h | 493 -- .../BMI270-Sensor-API/bmi270_hc.c | 3165 ------------ .../BMI270-Sensor-API/bmi270_hc.h | 483 -- .../BMI270-Sensor-API/bmi270_wh.c | 3431 ------------- .../BMI270-Sensor-API/bmi270_wh.h | 402 -- .../BMI270-Sensor-API/bmi2_ois.c | 417 -- .../BMI270-Sensor-API/bmi2_ois.h | 387 -- .../examples/bmi270/accel/Makefile | 18 - .../examples/bmi270/accel/accel.c | 200 - .../examples/bmi270/accel_gyro/Makefile | 18 - .../examples/bmi270/accel_gyro/accel_gyro.c | 268 - .../bmi270/any_motion_interrupt/Makefile | 18 - .../any_motion_interrupt.c | 135 - .../examples/bmi270/common/common.c | 372 -- .../examples/bmi270/common/common.h | 122 - .../examples/bmi270/component_retrim/Makefile | 18 - .../component_retrim/component_retrim.c | 52 - .../bmi270/fifo_full_header_mode/Makefile | 18 - .../fifo_full_header_mode.c | 315 -- .../bmi270/fifo_full_headerless_mode/Makefile | 18 - .../fifo_full_headerless_mode.c | 304 -- .../fifo_watermark_header_mode/Makefile | 18 - .../fifo_watermark_header_mode.c | 335 -- .../fifo_watermark_headerless_mode/Makefile | 18 - .../fifo_watermark_headerless_mode.c | 331 -- .../examples/bmi270/gyro/Makefile | 18 - .../examples/bmi270/gyro/gyro.c | 195 - .../bmi270/no_motion_interrupt/Makefile | 18 - .../no_motion_interrupt/no_motion_interrupt.c | 134 - .../bmi270/read_aux_data_mode/Makefile | 23 - .../read_aux_data_mode/read_aux_data_mode.c | 327 -- .../bmi270/read_aux_manual_mode/Makefile | 23 - .../read_aux_manual_mode.c | 322 -- .../examples/bmi270/sig_motion/Makefile | 18 - .../examples/bmi270/sig_motion/sig_motion.c | 93 - .../examples/bmi270/step_activity/Makefile | 18 - .../bmi270/step_activity/step_activity.c | 110 - .../examples/bmi270/step_counter/Makefile | 18 - .../bmi270/step_counter/step_counter.c | 146 - .../examples/bmi270/wrist_gesture/Makefile | 18 - .../bmi270/wrist_gesture/wrist_gesture.c | 147 - .../bmi270/wrist_wear_wakeup/Makefile | 18 - .../wrist_wear_wakeup/wrist_wear_wakeup.c | 131 - .../examples/bmi270_context/accel/Makefile | 18 - .../examples/bmi270_context/accel/accel.c | 200 - .../bmi270_context/accel_gyro/Makefile | 18 - .../bmi270_context/accel_gyro/accel_gyro.c | 268 - .../activity_recognition/Makefile | 18 - .../activity_recognition.c | 154 - .../examples/bmi270_context/common/common.c | 372 -- .../examples/bmi270_context/common/common.h | 122 - .../bmi270_context/component_retrim/Makefile | 18 - .../component_retrim/component_retrim.c | 52 - .../fifo_full_header_mode/Makefile | 18 - .../fifo_full_header_mode.c | 315 -- .../fifo_full_headerless_mode/Makefile | 18 - .../fifo_full_headerless_mode.c | 304 -- .../fifo_watermark_header_mode/Makefile | 18 - .../fifo_watermark_header_mode.c | 335 -- .../fifo_watermark_headerless_mode/Makefile | 18 - .../fifo_watermark_headerless_mode.c | 331 -- .../examples/bmi270_context/gyro/Makefile | 18 - .../examples/bmi270_context/gyro/gyro.c | 195 - .../bmi270_context/step_counter/Makefile | 18 - .../step_counter/step_counter.c | 146 - .../examples/bmi270_hc/accel/Makefile | 18 - .../examples/bmi270_hc/accel/accel.c | 201 - .../examples/bmi270_hc/accel_gyro/Makefile | 18 - .../bmi270_hc/accel_gyro/accel_gyro.c | 268 - .../bmi270_hc/activity_recognition/Makefile | 18 - .../activity_recognition.c | 154 - .../examples/bmi270_hc/common/common.c | 372 -- .../examples/bmi270_hc/common/common.h | 122 - .../bmi270_hc/component_retrim/Makefile | 18 - .../component_retrim/component_retrim.c | 52 - .../bmi270_hc/fifo_full_header_mode/Makefile | 18 - .../fifo_full_header_mode.c | 316 -- .../fifo_full_headerless_mode/Makefile | 18 - .../fifo_full_headerless_mode.c | 304 -- .../fifo_watermark_header_mode/Makefile | 18 - .../fifo_watermark_header_mode.c | 335 -- .../fifo_watermark_headerless_mode/Makefile | 18 - .../fifo_watermark_headerless_mode.c | 331 -- .../examples/bmi270_hc/gyro/Makefile | 18 - .../examples/bmi270_hc/gyro/gyro.c | 195 - .../examples/bmi270_hc/step_counter/Makefile | 18 - .../bmi270_hc/step_counter/step_counter.c | 146 - .../bmi270_maximum_fifo/accel/Makefile | 18 - .../bmi270_maximum_fifo/accel/accel.c | 200 - .../bmi270_maximum_fifo/accel_gyro/Makefile | 18 - .../accel_gyro/accel_gyro.c | 268 - .../bmi270_maximum_fifo/common/common.c | 372 -- .../bmi270_maximum_fifo/common/common.h | 122 - .../component_retrim/Makefile | 18 - .../component_retrim/component_retrim.c | 52 - .../fifo_watermark_header_mode/Makefile | 18 - .../fifo_watermark_header_mode.c | 335 -- .../fifo_watermark_headerless_mode/Makefile | 18 - .../fifo_watermark_headerless_mode.c | 331 -- .../bmi270_maximum_fifo/gyro/Makefile | 18 - .../examples/bmi270_maximum_fifo/gyro/gyro.c | 195 - .../examples/bmi270_wh/accel/Makefile | 18 - .../examples/bmi270_wh/accel/accel.c | 201 - .../examples/bmi270_wh/accel_gyro/Makefile | 18 - .../bmi270_wh/accel_gyro/accel_gyro.c | 268 - .../bmi270_wh/any_motion_interrupt/Makefile | 18 - .../any_motion_interrupt.c | 134 - .../examples/bmi270_wh/common/common.c | 372 -- .../examples/bmi270_wh/common/common.h | 122 - .../bmi270_wh/fifo_full_header_mode/Makefile | 18 - .../fifo_full_header_mode.c | 316 -- .../fifo_full_headerless_mode/Makefile | 18 - .../fifo_full_headerless_mode.c | 304 -- .../fifo_watermark_header_mode/Makefile | 18 - .../fifo_watermark_header_mode.c | 335 -- .../fifo_watermark_headerless_mode/Makefile | 18 - .../fifo_watermark_headerless_mode.c | 331 -- .../examples/bmi270_wh/gyro/Makefile | 18 - .../examples/bmi270_wh/gyro/gyro.c | 195 - .../bmi270_wh/no_motion_interrupt/Makefile | 18 - .../no_motion_interrupt/no_motion_interrupt.c | 135 - .../bmi270_wh/read_aux_data_mode/Makefile | 23 - .../read_aux_data_mode/read_aux_data_mode.c | 327 -- .../bmi270_wh/read_aux_manual_mode/Makefile | 23 - .../read_aux_manual_mode.c | 322 -- .../examples/bmi270_wh/step_activity/Makefile | 18 - .../bmi270_wh/step_activity/step_activity.c | 110 - .../examples/bmi270_wh/step_counter/Makefile | 18 - .../bmi270_wh/step_counter/step_counter.c | 146 - .../bmi270_wh/wrist_wear_wakeup/Makefile | 18 - .../wrist_wear_wakeup/wrist_wear_wakeup.c | 131 - .../examples/bmi2_ois/common/common.c | 153 - .../examples/bmi2_ois/common/common.h | 92 - .../examples/bmi2_ois/ois_accel_gyro/Makefile | 18 - .../bmi2_ois/ois_accel_gyro/ois_accel_gyro.c | 118 - .../main/drivers/accgyro}/bmi2.c | 0 .../main/drivers/accgyro}/bmi2.h | 0 .../drivers/accgyro}/bmi270_maximum_fifo.c | 0 .../drivers/accgyro}/bmi270_maximum_fifo.h | 0 .../main/drivers/accgyro}/bmi2_defs.h | 0 src/main/target/BETAFPVF411/target.mk | 2 +- src/main/target/IFLIGHT_F745_AIO_V2/target.mk | 4 +- src/main/target/TMTR_TMOTORF7/target.h | 6 + src/main/target/TMTR_TMOTORF7/target.mk | 2 + 151 files changed, 11 insertions(+), 33679 deletions(-) delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/LICENSE delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/OIS_README.md delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/README.md delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/betaflight_info.txt delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/bmi270.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/bmi270.h delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_context.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_context.h delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_hc.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_hc.h delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_wh.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_wh.h delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/bmi2_ois.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/bmi2_ois.h delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/accel/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/accel/accel.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/accel_gyro/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/accel_gyro/accel_gyro.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/any_motion_interrupt/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/any_motion_interrupt/any_motion_interrupt.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/common/common.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/common/common.h delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/component_retrim/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/component_retrim/component_retrim.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_full_header_mode/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_full_header_mode/fifo_full_header_mode.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_full_headerless_mode/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_full_headerless_mode/fifo_full_headerless_mode.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_watermark_header_mode/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_watermark_header_mode/fifo_watermark_header_mode.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_watermark_headerless_mode/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/gyro/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/gyro/gyro.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/no_motion_interrupt/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/no_motion_interrupt/no_motion_interrupt.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/read_aux_data_mode/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/read_aux_data_mode/read_aux_data_mode.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/read_aux_manual_mode/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/read_aux_manual_mode/read_aux_manual_mode.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/sig_motion/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/sig_motion/sig_motion.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/step_activity/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/step_activity/step_activity.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/step_counter/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/step_counter/step_counter.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/wrist_gesture/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/wrist_gesture/wrist_gesture.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/wrist_wear_wakeup/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/wrist_wear_wakeup/wrist_wear_wakeup.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/accel/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/accel/accel.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/accel_gyro/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/accel_gyro/accel_gyro.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/activity_recognition/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/activity_recognition/activity_recognition.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/common/common.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/common/common.h delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/component_retrim/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/component_retrim/component_retrim.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_full_header_mode/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_full_header_mode/fifo_full_header_mode.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_full_headerless_mode/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_full_headerless_mode/fifo_full_headerless_mode.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_watermark_header_mode/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_watermark_header_mode/fifo_watermark_header_mode.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_watermark_headerless_mode/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/gyro/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/gyro/gyro.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/step_counter/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/step_counter/step_counter.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/accel/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/accel/accel.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/accel_gyro/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/accel_gyro/accel_gyro.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/activity_recognition/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/activity_recognition/activity_recognition.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/common/common.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/common/common.h delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/component_retrim/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/component_retrim/component_retrim.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_full_header_mode/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_full_header_mode/fifo_full_header_mode.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_full_headerless_mode/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_full_headerless_mode/fifo_full_headerless_mode.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_watermark_header_mode/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_watermark_header_mode/fifo_watermark_header_mode.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_watermark_headerless_mode/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/gyro/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/gyro/gyro.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/step_counter/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/step_counter/step_counter.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/accel/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/accel/accel.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/accel_gyro/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/accel_gyro/accel_gyro.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/common/common.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/common/common.h delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/component_retrim/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/component_retrim/component_retrim.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/fifo_watermark_header_mode/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/fifo_watermark_header_mode/fifo_watermark_header_mode.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/fifo_watermark_headerless_mode/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/gyro/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/gyro/gyro.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/accel/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/accel/accel.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/accel_gyro/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/accel_gyro/accel_gyro.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/any_motion_interrupt/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/any_motion_interrupt/any_motion_interrupt.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/common/common.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/common/common.h delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_full_header_mode/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_full_header_mode/fifo_full_header_mode.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_full_headerless_mode/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_full_headerless_mode/fifo_full_headerless_mode.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_watermark_header_mode/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_watermark_header_mode/fifo_watermark_header_mode.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_watermark_headerless_mode/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/gyro/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/gyro/gyro.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/no_motion_interrupt/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/no_motion_interrupt/no_motion_interrupt.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/read_aux_data_mode/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/read_aux_data_mode/read_aux_data_mode.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/read_aux_manual_mode/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/read_aux_manual_mode/read_aux_manual_mode.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/step_activity/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/step_activity/step_activity.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/step_counter/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/step_counter/step_counter.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/wrist_wear_wakeup/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/wrist_wear_wakeup/wrist_wear_wakeup.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi2_ois/common/common.c delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi2_ois/common/common.h delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi2_ois/ois_accel_gyro/Makefile delete mode 100644 lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi2_ois/ois_accel_gyro/ois_accel_gyro.c rename {lib/main/BoschSensortec/BMI270-Sensor-API => src/main/drivers/accgyro}/bmi2.c (100%) rename {lib/main/BoschSensortec/BMI270-Sensor-API => src/main/drivers/accgyro}/bmi2.h (100%) rename {lib/main/BoschSensortec/BMI270-Sensor-API => src/main/drivers/accgyro}/bmi270_maximum_fifo.c (100%) rename {lib/main/BoschSensortec/BMI270-Sensor-API => src/main/drivers/accgyro}/bmi270_maximum_fifo.h (100%) rename {lib/main/BoschSensortec/BMI270-Sensor-API => src/main/drivers/accgyro}/bmi2_defs.h (100%) diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/LICENSE b/lib/main/BoschSensortec/BMI270-Sensor-API/LICENSE deleted file mode 100644 index 315dfa2309..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/LICENSE +++ /dev/null @@ -1,30 +0,0 @@ -Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - -BSD-3-Clause - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - -1. Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - -2. Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. - -3. Neither the name of the copyright holder nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) -HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, -STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING -IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -POSSIBILITY OF SUCH DAMAGE. \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/OIS_README.md b/lib/main/BoschSensortec/BMI270-Sensor-API/OIS_README.md deleted file mode 100644 index 2983d0699f..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/OIS_README.md +++ /dev/null @@ -1,170 +0,0 @@ -# Sensor API for the BMI2's OIS interface - -## Table of Contents - - [Introduction](#Intro) - - [Integration details](#Integration) - - [Driver files information](#file) - - [Sensor interfaces](#interface) - - [Integration Examples](#examples) - -### Introduction - This package contains Bosch Sensortec's BMI2 Sensor API. - -### Integration details -- Integrate _bmi2.c_, _bmi2.h_, _bmi2_ois.c_, _bmi2_ois.h_, _bmi2_defs.h_ and the required variant files in your project. -- User has to include _bmi2_ois.h_ in the code to call OIS related APIs and a _variant header_ for initialization as -well as BMI2 related API calls, as shown below: -``` c -#include "bmi270.h" -#include "bmi2_ois.h" -```` -### Driver files information -- *_bmi2_ois.c_* - * This file has function definitions of OIS related API interfaces. -- *_bmi2_ois.h_* - * This header file has necessary include files, function declarations, required to make OIS related API calls. - -### Sensor interfaces -#### _Host Interface_ -- I2C interface -- SPI interface -_Note: By default, the interface is I2C._ - -#### _OIS Interface_ -- SPI interface - -### Integration examples -#### Configuring SPI/I2C for host interface. -To configure host interface, an instance of the bmi2_dev structure should be -created for initializing BMI2 sensor. "_Refer **README** for initializing BMI2 -sensor._" - -#### Configuring SPI for OIS interface. -To configure OIS interface, an instance of the bmi2_ois_dev structure should be -created. The following parameters are required to be updated in the structure, -by the user. - -Parameters | Details ---------------|----------------------------------- -_intf_ptr_ | device address reference of SPI interface -_ois_read_ | read through SPI interface -_ois_write_ | read through SPI interface -_ois_delay_us_| delay in micro seconds -_acc_en_ | for enabling accelerometer -_gyr_en_ | for enabling gyroscope - -``` c -int8_t rslt = 0; - -struct bmi2_ois_dev ois_dev = { - .intf_ptr = intf_ptr will contain the chip selection info of SPI CS pin, - .ois_read = user_spi_reg_read, - .ois_write = user_spi_reg_write, - .ois_delay_us = user_delay_us -}; -``` ->**_Important Note_**: For initializing and configuring BMI2 sensors, which is -done through host interface, the API's are to be used from bmi2.c file. Rest -of the API's, for OIS configurations and the reading of OIS data, which is done -through OIS interface, are to be used from bmi2_ois.c file. - -##### Get accelerometer and gyroscope data through OIS interface -``` c -int8_t rslt; -/* Array to enable sensor through host interface */ -uint8_t sens_list[2] = {BMI2_ACCEL, BMI2_GYRO}; -/* Array to enable sensor through OIS interface */ -uint8_t sens_sel[2] = {BMI2_OIS_ACCEL, BMI2_OIS_GYRO}; -/* Initialize the configuration structure */ -struct bmi2_sens_config sens_cfg = {0}; - -/* Initialize BMI2 */ -rslt = bmi2_init(&dev); -if (rslt != BMI2_OK) { - printf("Error: %d\n", rslt); - return; -} - -/* Enable accelerometer and gyroscope through host interface */ -rslt = bmi270_sensor_enable(sens_list, 2, &dev); -if (rslt != BMI2_OK) { - printf("Error: %d\n", rslt); - return; -} - -/* Setting of OIS Range is done through host interface */ -/* Select the gyroscope sensor for OIS Range configuration */ -sens_cfg.type = BMI2_GYRO; - -/* Get gyroscope configuration */ -rslt = bmi2_get_sensor_config(&sens_cfg, 1, &dev); -if (rslt != BMI2_OK) { - printf("Error: %d\n", rslt); - return; -} - -/* Set the desired OIS Range */ -sens_cfg.cfg.gyr.ois_range = BMI2_GYR_OIS_2000; - -/* Set gyroscope configuration for default values */ -rslt = bmi2_set_sensor_config(&sens_cfg, 1, &dev); -if (rslt != BMI2_OK) { - printf("Error: %d\n", rslt); - return; -} - -/* Enable OIS through host interface */ -rslt = bmi2_set_ois_interface(BMI2_ENABLE, &dev); -if (rslt != BMI2_OK) { - printf("Error: %d\n", rslt); - return; -} - -/* Disable Advance Power Save Mode through host interface */ -rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &dev); -if (rslt != BMI2_OK) { - printf("Error: %d\n", rslt); - return; -} - -/* Get configurations for OIS through OIS interface for default values */ -rslt = bmi2_ois_get_config(&ois_dev); -if (rslt != BMI2_OK) { - printf("Error: %d\n", rslt); - return; -} - -/* Enable accelerometer and gyroscope for reading OIS data */ -ois_dev.acc_en = BMI2_ENABLE; -ois_dev.gyr_en = BMI2_ENABLE; - -/* Set configurations for OIS through OIS interface */ -rslt = bmi2_ois_set_config(&ois_dev); -if (rslt == BMI2_OK) { - /* Get OIS accelerometer and gyroscope data through OIS interface */ - rslt = bmi2_ois_read_data(sens_sel, 2, &ois_dev); - if (rslt == BMI2_OK) { - /* Print accelerometer data */ - printf("OIS Accel x-axis = %d\t", ois_dev.acc_data.x); - printf("OIS Accel y-axis= %d\t", ois_dev.acc_data.y); - printf("OIS Accel z-axis = %d\r\n", ois_dev.acc_data.z); - - /* Print gyroscope data */ - printf("OIS Gyro x-axis = %d\t", ois_dev.gyr_data.x); - printf("OIS Gyro y-axis= %d\t", ois_dev.gyr_data.y); - printf("OIS Gyro z-axis = %d\r\n", ois_dev.gyr_data.z); - } -} - -if (rslt != BMI2_OK) { - printf("Error code: %d\n", rslt); - return; -} - -/* Enable Advance Power Save Mode through host interface */ -rslt = bmi2_set_adv_power_save(BMI2_ENABLE, &dev); -if (rslt != BMI2_OK) { - printf("Error: %d\n", rslt); - return; -} -``` \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/README.md b/lib/main/BoschSensortec/BMI270-Sensor-API/README.md deleted file mode 100644 index 193b038210..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/README.md +++ /dev/null @@ -1,14 +0,0 @@ -BMI2xy Sensor API - -> This package contains BMI2xy sensor API - -## Sensor Overview -The BMI2xy is a small, low power, low noise inertial measurement unit designed for use in mobile applications like augmented reality or indoor navigation which require highly accurate, real-time sensor data. - -### Features - -- Indoor navigation, pedestrian dead-reckoning, step-counting - -For more information refer product page [Link](https://www.bosch-sensortec.com/products/motion-sensors/imus/bmi270.html) - ---- \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/betaflight_info.txt b/lib/main/BoschSensortec/BMI270-Sensor-API/betaflight_info.txt deleted file mode 100644 index 9f8947c9d0..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/betaflight_info.txt +++ /dev/null @@ -1,9 +0,0 @@ -This library is used to support the Bosch BMI270 gyro sensor (see drivers/accgyro/accgyro_spi_bmi270.c). - -Library download location: - https://github.com/BoschSensortec/BMI270-Sensor-API - -Version: - 2.63.1 - -The only file that is compiled as part of Betaflight is bmi270_maximum_fifo.c. This file contains the device microcode that must be uploaded during initialization. diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270.c b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270.c deleted file mode 100644 index 124a984b8b..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270.c +++ /dev/null @@ -1,4451 +0,0 @@ -/** -* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. -* -* BSD-3-Clause -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* 1. Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* -* 2. Redistributions in binary form must reproduce the above copyright -* notice, this list of conditions and the following disclaimer in the -* documentation and/or other materials provided with the distribution. -* -* 3. Neither the name of the copyright holder nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) -* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, -* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING -* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -* -* @file bmi270.c -* @date 2020-11-04 -* @version v2.63.1 -* -*/ - -/***************************************************************************/ - -/*! Header files - ****************************************************************************/ -#include "bmi270.h" - -/***************************************************************************/ - -/*! Global Variable - ****************************************************************************/ - -/*! @name Global array that stores the configuration file of BMI270 */ -const uint8_t bmi270_config_file[] = { - 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x3d, 0xb1, 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x91, 0x03, 0x80, 0x2e, 0xbc, - 0xb0, 0x80, 0x2e, 0xa3, 0x03, 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x00, 0xb0, 0x50, 0x30, 0x21, 0x2e, 0x59, 0xf5, - 0x10, 0x30, 0x21, 0x2e, 0x6a, 0xf5, 0x80, 0x2e, 0x3b, 0x03, 0x00, 0x00, 0x00, 0x00, 0x08, 0x19, 0x01, 0x00, 0x22, - 0x00, 0x75, 0x00, 0x00, 0x10, 0x00, 0x10, 0xd1, 0x00, 0xb3, 0x43, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, - 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, - 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, - 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, - 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, - 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, - 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, - 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, - 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, - 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0xe0, 0x5f, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x92, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x08, 0x19, 0x00, 0x00, 0x88, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, - 0xe0, 0xaa, 0x38, 0x05, 0xe0, 0x90, 0x30, 0xfa, 0x00, 0x96, 0x00, 0x4b, 0x09, 0x11, 0x00, 0x11, 0x00, 0x02, 0x00, - 0x2d, 0x01, 0xd4, 0x7b, 0x3b, 0x01, 0xdb, 0x7a, 0x04, 0x00, 0x3f, 0x7b, 0xcd, 0x6c, 0xc3, 0x04, 0x85, 0x09, 0xc3, - 0x04, 0xec, 0xe6, 0x0c, 0x46, 0x01, 0x00, 0x27, 0x00, 0x19, 0x00, 0x96, 0x00, 0xa0, 0x00, 0x01, 0x00, 0x0c, 0x00, - 0xf0, 0x3c, 0x00, 0x01, 0x01, 0x00, 0x03, 0x00, 0x01, 0x00, 0x0e, 0x00, 0x00, 0x00, 0x32, 0x00, 0x05, 0x00, 0xee, - 0x06, 0x04, 0x00, 0xc8, 0x00, 0x00, 0x00, 0x04, 0x00, 0xa8, 0x05, 0xee, 0x06, 0x00, 0x04, 0xbc, 0x02, 0xb3, 0x00, - 0x85, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0xb4, 0x00, 0x01, 0x00, 0xb9, 0x00, 0x01, 0x00, 0x98, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x01, 0x00, 0x80, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x2e, 0x00, 0xc1, 0xfd, 0x2d, 0xde, - 0x00, 0xeb, 0x00, 0xda, 0x00, 0x00, 0x0c, 0xff, 0x0f, 0x00, 0x04, 0xc0, 0x00, 0x5b, 0xf5, 0xc9, 0x01, 0x1e, 0xf2, - 0x80, 0x00, 0x3f, 0xff, 0x19, 0xf4, 0x58, 0xf5, 0x66, 0xf5, 0x64, 0xf5, 0xc0, 0xf1, 0xf0, 0x00, 0xe0, 0x00, 0xcd, - 0x01, 0xd3, 0x01, 0xdb, 0x01, 0xff, 0x7f, 0xff, 0x01, 0xe4, 0x00, 0x74, 0xf7, 0xf3, 0x00, 0xfa, 0x00, 0xff, 0x3f, - 0xca, 0x03, 0x6c, 0x38, 0x56, 0xfe, 0x44, 0xfd, 0xbc, 0x02, 0xf9, 0x06, 0x00, 0xfc, 0x12, 0x02, 0xae, 0x01, 0x58, - 0xfa, 0x9a, 0xfd, 0x77, 0x05, 0xbb, 0x02, 0x96, 0x01, 0x95, 0x01, 0x7f, 0x01, 0x82, 0x01, 0x89, 0x01, 0x87, 0x01, - 0x88, 0x01, 0x8a, 0x01, 0x8c, 0x01, 0x8f, 0x01, 0x8d, 0x01, 0x92, 0x01, 0x91, 0x01, 0xdd, 0x00, 0x9f, 0x01, 0x7e, - 0x01, 0xdb, 0x00, 0xb6, 0x01, 0x70, 0x69, 0x26, 0xd3, 0x9c, 0x07, 0x1f, 0x05, 0x9d, 0x00, 0x00, 0x08, 0xbc, 0x05, - 0x37, 0xfa, 0xa2, 0x01, 0xaa, 0x01, 0xa1, 0x01, 0xa8, 0x01, 0xa0, 0x01, 0xa8, 0x05, 0xb4, 0x01, 0xb4, 0x01, 0xce, - 0x00, 0xd0, 0x00, 0xfc, 0x00, 0xc5, 0x01, 0xff, 0xfb, 0xb1, 0x00, 0x00, 0x38, 0x00, 0x30, 0xfd, 0xf5, 0xfc, 0xf5, - 0xcd, 0x01, 0xa0, 0x00, 0x5f, 0xff, 0x00, 0x40, 0xff, 0x00, 0x00, 0x80, 0x6d, 0x0f, 0xeb, 0x00, 0x7f, 0xff, 0xc2, - 0xf5, 0x68, 0xf7, 0xb3, 0xf1, 0x67, 0x0f, 0x5b, 0x0f, 0x61, 0x0f, 0x80, 0x0f, 0x58, 0xf7, 0x5b, 0xf7, 0x83, 0x0f, - 0x86, 0x00, 0x72, 0x0f, 0x85, 0x0f, 0xc6, 0xf1, 0x7f, 0x0f, 0x6c, 0xf7, 0x00, 0xe0, 0x00, 0xff, 0xd1, 0xf5, 0x87, - 0x0f, 0x8a, 0x0f, 0xff, 0x03, 0xf0, 0x3f, 0x8b, 0x00, 0x8e, 0x00, 0x90, 0x00, 0xb9, 0x00, 0x2d, 0xf5, 0xca, 0xf5, - 0xcb, 0x01, 0x20, 0xf2, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x30, 0x50, 0x98, 0x2e, - 0xd7, 0x0e, 0x50, 0x32, 0x98, 0x2e, 0xfa, 0x03, 0x00, 0x30, 0xf0, 0x7f, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0x00, - 0x2e, 0x01, 0x80, 0x08, 0xa2, 0xfb, 0x2f, 0x98, 0x2e, 0xba, 0x03, 0x21, 0x2e, 0x19, 0x00, 0x01, 0x2e, 0xee, 0x00, - 0x00, 0xb2, 0x07, 0x2f, 0x01, 0x2e, 0x19, 0x00, 0x00, 0xb2, 0x03, 0x2f, 0x01, 0x50, 0x03, 0x52, 0x98, 0x2e, 0x07, - 0xcc, 0x01, 0x2e, 0xdd, 0x00, 0x00, 0xb2, 0x27, 0x2f, 0x05, 0x2e, 0x8a, 0x00, 0x05, 0x52, 0x98, 0x2e, 0xc7, 0xc1, - 0x03, 0x2e, 0xe9, 0x00, 0x40, 0xb2, 0xf0, 0x7f, 0x08, 0x2f, 0x01, 0x2e, 0x19, 0x00, 0x00, 0xb2, 0x04, 0x2f, 0x00, - 0x30, 0x21, 0x2e, 0xe9, 0x00, 0x98, 0x2e, 0xb4, 0xb1, 0x01, 0x2e, 0x18, 0x00, 0x00, 0xb2, 0x10, 0x2f, 0x05, 0x50, - 0x98, 0x2e, 0x4d, 0xc3, 0x05, 0x50, 0x98, 0x2e, 0x5a, 0xc7, 0x98, 0x2e, 0xf9, 0xb4, 0x98, 0x2e, 0x54, 0xb2, 0x98, - 0x2e, 0x67, 0xb6, 0x98, 0x2e, 0x17, 0xb2, 0x10, 0x30, 0x21, 0x2e, 0x77, 0x00, 0x01, 0x2e, 0xef, 0x00, 0x00, 0xb2, - 0x04, 0x2f, 0x98, 0x2e, 0x7a, 0xb7, 0x00, 0x30, 0x21, 0x2e, 0xef, 0x00, 0x01, 0x2e, 0xd4, 0x00, 0x04, 0xae, 0x0b, - 0x2f, 0x01, 0x2e, 0xdd, 0x00, 0x00, 0xb2, 0x07, 0x2f, 0x05, 0x52, 0x98, 0x2e, 0x8e, 0x0e, 0x00, 0xb2, 0x02, 0x2f, - 0x10, 0x30, 0x21, 0x2e, 0x7d, 0x00, 0x01, 0x2e, 0x7d, 0x00, 0x00, 0x90, 0x90, 0x2e, 0xf1, 0x02, 0x01, 0x2e, 0xd7, - 0x00, 0x00, 0xb2, 0x04, 0x2f, 0x98, 0x2e, 0x2f, 0x0e, 0x00, 0x30, 0x21, 0x2e, 0x7b, 0x00, 0x01, 0x2e, 0x7b, 0x00, - 0x00, 0xb2, 0x12, 0x2f, 0x01, 0x2e, 0xd4, 0x00, 0x00, 0x90, 0x02, 0x2f, 0x98, 0x2e, 0x1f, 0x0e, 0x09, 0x2d, 0x98, - 0x2e, 0x81, 0x0d, 0x01, 0x2e, 0xd4, 0x00, 0x04, 0x90, 0x02, 0x2f, 0x50, 0x32, 0x98, 0x2e, 0xfa, 0x03, 0x00, 0x30, - 0x21, 0x2e, 0x7b, 0x00, 0x01, 0x2e, 0x7c, 0x00, 0x00, 0xb2, 0x90, 0x2e, 0x09, 0x03, 0x01, 0x2e, 0x7c, 0x00, 0x01, - 0x31, 0x01, 0x08, 0x00, 0xb2, 0x04, 0x2f, 0x98, 0x2e, 0x47, 0xcb, 0x10, 0x30, 0x21, 0x2e, 0x77, 0x00, 0x81, 0x30, - 0x01, 0x2e, 0x7c, 0x00, 0x01, 0x08, 0x00, 0xb2, 0x61, 0x2f, 0x03, 0x2e, 0x89, 0x00, 0x01, 0x2e, 0xd4, 0x00, 0x98, - 0xbc, 0x98, 0xb8, 0x05, 0xb2, 0x0f, 0x58, 0x23, 0x2f, 0x07, 0x90, 0x09, 0x54, 0x00, 0x30, 0x37, 0x2f, 0x15, 0x41, - 0x04, 0x41, 0xdc, 0xbe, 0x44, 0xbe, 0xdc, 0xba, 0x2c, 0x01, 0x61, 0x00, 0x0f, 0x56, 0x4a, 0x0f, 0x0c, 0x2f, 0xd1, - 0x42, 0x94, 0xb8, 0xc1, 0x42, 0x11, 0x30, 0x05, 0x2e, 0x6a, 0xf7, 0x2c, 0xbd, 0x2f, 0xb9, 0x80, 0xb2, 0x08, 0x22, - 0x98, 0x2e, 0xc3, 0xb7, 0x21, 0x2d, 0x61, 0x30, 0x23, 0x2e, 0xd4, 0x00, 0x98, 0x2e, 0xc3, 0xb7, 0x00, 0x30, 0x21, - 0x2e, 0x5a, 0xf5, 0x18, 0x2d, 0xe1, 0x7f, 0x50, 0x30, 0x98, 0x2e, 0xfa, 0x03, 0x0f, 0x52, 0x07, 0x50, 0x50, 0x42, - 0x70, 0x30, 0x0d, 0x54, 0x42, 0x42, 0x7e, 0x82, 0xe2, 0x6f, 0x80, 0xb2, 0x42, 0x42, 0x05, 0x2f, 0x21, 0x2e, 0xd4, - 0x00, 0x10, 0x30, 0x98, 0x2e, 0xc3, 0xb7, 0x03, 0x2d, 0x60, 0x30, 0x21, 0x2e, 0xd4, 0x00, 0x01, 0x2e, 0xd4, 0x00, - 0x06, 0x90, 0x18, 0x2f, 0x01, 0x2e, 0x76, 0x00, 0x0b, 0x54, 0x07, 0x52, 0xe0, 0x7f, 0x98, 0x2e, 0x7a, 0xc1, 0xe1, - 0x6f, 0x08, 0x1a, 0x40, 0x30, 0x08, 0x2f, 0x21, 0x2e, 0xd4, 0x00, 0x20, 0x30, 0x98, 0x2e, 0xaf, 0xb7, 0x50, 0x32, - 0x98, 0x2e, 0xfa, 0x03, 0x05, 0x2d, 0x98, 0x2e, 0x38, 0x0e, 0x00, 0x30, 0x21, 0x2e, 0xd4, 0x00, 0x00, 0x30, 0x21, - 0x2e, 0x7c, 0x00, 0x18, 0x2d, 0x01, 0x2e, 0xd4, 0x00, 0x03, 0xaa, 0x01, 0x2f, 0x98, 0x2e, 0x45, 0x0e, 0x01, 0x2e, - 0xd4, 0x00, 0x3f, 0x80, 0x03, 0xa2, 0x01, 0x2f, 0x00, 0x2e, 0x02, 0x2d, 0x98, 0x2e, 0x5b, 0x0e, 0x30, 0x30, 0x98, - 0x2e, 0xce, 0xb7, 0x00, 0x30, 0x21, 0x2e, 0x7d, 0x00, 0x50, 0x32, 0x98, 0x2e, 0xfa, 0x03, 0x01, 0x2e, 0x77, 0x00, - 0x00, 0xb2, 0x24, 0x2f, 0x98, 0x2e, 0xf5, 0xcb, 0x03, 0x2e, 0xd5, 0x00, 0x11, 0x54, 0x01, 0x0a, 0xbc, 0x84, 0x83, - 0x86, 0x21, 0x2e, 0xc9, 0x01, 0xe0, 0x40, 0x13, 0x52, 0xc4, 0x40, 0x82, 0x40, 0xa8, 0xb9, 0x52, 0x42, 0x43, 0xbe, - 0x53, 0x42, 0x04, 0x0a, 0x50, 0x42, 0xe1, 0x7f, 0xf0, 0x31, 0x41, 0x40, 0xf2, 0x6f, 0x25, 0xbd, 0x08, 0x08, 0x02, - 0x0a, 0xd0, 0x7f, 0x98, 0x2e, 0xa8, 0xcf, 0x06, 0xbc, 0xd1, 0x6f, 0xe2, 0x6f, 0x08, 0x0a, 0x80, 0x42, 0x98, 0x2e, - 0x58, 0xb7, 0x00, 0x30, 0x21, 0x2e, 0xee, 0x00, 0x21, 0x2e, 0x77, 0x00, 0x21, 0x2e, 0xdd, 0x00, 0x80, 0x2e, 0xf4, - 0x01, 0x1a, 0x24, 0x22, 0x00, 0x80, 0x2e, 0xec, 0x01, 0x10, 0x50, 0xfb, 0x7f, 0x98, 0x2e, 0xf3, 0x03, 0x57, 0x50, - 0xfb, 0x6f, 0x01, 0x30, 0x71, 0x54, 0x11, 0x42, 0x42, 0x0e, 0xfc, 0x2f, 0xc0, 0x2e, 0x01, 0x42, 0xf0, 0x5f, 0x80, - 0x2e, 0x00, 0xc1, 0xfd, 0x2d, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x9a, 0x01, - 0x34, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x20, 0x50, 0xe7, 0x7f, 0xf6, 0x7f, 0x06, 0x32, 0x0f, 0x2e, 0x61, 0xf5, 0xfe, 0x09, 0xc0, 0xb3, 0x04, - 0x2f, 0x17, 0x30, 0x2f, 0x2e, 0xef, 0x00, 0x2d, 0x2e, 0x61, 0xf5, 0xf6, 0x6f, 0xe7, 0x6f, 0xe0, 0x5f, 0xc8, 0x2e, - 0x20, 0x50, 0xe7, 0x7f, 0xf6, 0x7f, 0x46, 0x30, 0x0f, 0x2e, 0xa4, 0xf1, 0xbe, 0x09, 0x80, 0xb3, 0x06, 0x2f, 0x0d, - 0x2e, 0xd4, 0x00, 0x84, 0xaf, 0x02, 0x2f, 0x16, 0x30, 0x2d, 0x2e, 0x7b, 0x00, 0x86, 0x30, 0x2d, 0x2e, 0x60, 0xf5, - 0xf6, 0x6f, 0xe7, 0x6f, 0xe0, 0x5f, 0xc8, 0x2e, 0x01, 0x2e, 0x77, 0xf7, 0x09, 0xbc, 0x0f, 0xb8, 0x00, 0xb2, 0x10, - 0x50, 0xfb, 0x7f, 0x10, 0x30, 0x0b, 0x2f, 0x03, 0x2e, 0x8a, 0x00, 0x96, 0xbc, 0x9f, 0xb8, 0x40, 0xb2, 0x05, 0x2f, - 0x03, 0x2e, 0x68, 0xf7, 0x9e, 0xbc, 0x9f, 0xb8, 0x40, 0xb2, 0x07, 0x2f, 0x03, 0x2e, 0x7e, 0x00, 0x41, 0x90, 0x01, - 0x2f, 0x98, 0x2e, 0xdc, 0x03, 0x03, 0x2c, 0x00, 0x30, 0x21, 0x2e, 0x7e, 0x00, 0xfb, 0x6f, 0xf0, 0x5f, 0xb8, 0x2e, - 0x20, 0x50, 0xe0, 0x7f, 0xfb, 0x7f, 0x00, 0x2e, 0x27, 0x50, 0x98, 0x2e, 0x3b, 0xc8, 0x29, 0x50, 0x98, 0x2e, 0xa7, - 0xc8, 0x01, 0x50, 0x98, 0x2e, 0x55, 0xcc, 0xe1, 0x6f, 0x2b, 0x50, 0x98, 0x2e, 0xe0, 0xc9, 0xfb, 0x6f, 0x00, 0x30, - 0xe0, 0x5f, 0x21, 0x2e, 0x7e, 0x00, 0xb8, 0x2e, 0x73, 0x50, 0x01, 0x30, 0x57, 0x54, 0x11, 0x42, 0x42, 0x0e, 0xfc, - 0x2f, 0xb8, 0x2e, 0x21, 0x2e, 0x59, 0xf5, 0x10, 0x30, 0xc0, 0x2e, 0x21, 0x2e, 0x4a, 0xf1, 0x90, 0x50, 0xf7, 0x7f, - 0xe6, 0x7f, 0xd5, 0x7f, 0xc4, 0x7f, 0xb3, 0x7f, 0xa1, 0x7f, 0x90, 0x7f, 0x82, 0x7f, 0x7b, 0x7f, 0x98, 0x2e, 0x35, - 0xb7, 0x00, 0xb2, 0x90, 0x2e, 0x97, 0xb0, 0x03, 0x2e, 0x8f, 0x00, 0x07, 0x2e, 0x91, 0x00, 0x05, 0x2e, 0xb1, 0x00, - 0x3f, 0xba, 0x9f, 0xb8, 0x01, 0x2e, 0xb1, 0x00, 0xa3, 0xbd, 0x4c, 0x0a, 0x05, 0x2e, 0xb1, 0x00, 0x04, 0xbe, 0xbf, - 0xb9, 0xcb, 0x0a, 0x4f, 0xba, 0x22, 0xbd, 0x01, 0x2e, 0xb3, 0x00, 0xdc, 0x0a, 0x2f, 0xb9, 0x03, 0x2e, 0xb8, 0x00, - 0x0a, 0xbe, 0x9a, 0x0a, 0xcf, 0xb9, 0x9b, 0xbc, 0x01, 0x2e, 0x97, 0x00, 0x9f, 0xb8, 0x93, 0x0a, 0x0f, 0xbc, 0x91, - 0x0a, 0x0f, 0xb8, 0x90, 0x0a, 0x25, 0x2e, 0x18, 0x00, 0x05, 0x2e, 0xc1, 0xf5, 0x2e, 0xbd, 0x2e, 0xb9, 0x01, 0x2e, - 0x19, 0x00, 0x31, 0x30, 0x8a, 0x04, 0x00, 0x90, 0x07, 0x2f, 0x01, 0x2e, 0xd4, 0x00, 0x04, 0xa2, 0x03, 0x2f, 0x01, - 0x2e, 0x18, 0x00, 0x00, 0xb2, 0x0c, 0x2f, 0x19, 0x50, 0x05, 0x52, 0x98, 0x2e, 0x4d, 0xb7, 0x05, 0x2e, 0x78, 0x00, - 0x80, 0x90, 0x10, 0x30, 0x01, 0x2f, 0x21, 0x2e, 0x78, 0x00, 0x25, 0x2e, 0xdd, 0x00, 0x98, 0x2e, 0x3e, 0xb7, 0x00, - 0xb2, 0x02, 0x30, 0x01, 0x30, 0x04, 0x2f, 0x01, 0x2e, 0x19, 0x00, 0x00, 0xb2, 0x00, 0x2f, 0x21, 0x30, 0x01, 0x2e, - 0xea, 0x00, 0x08, 0x1a, 0x0e, 0x2f, 0x23, 0x2e, 0xea, 0x00, 0x33, 0x30, 0x1b, 0x50, 0x0b, 0x09, 0x01, 0x40, 0x17, - 0x56, 0x46, 0xbe, 0x4b, 0x08, 0x4c, 0x0a, 0x01, 0x42, 0x0a, 0x80, 0x15, 0x52, 0x01, 0x42, 0x00, 0x2e, 0x01, 0x2e, - 0x18, 0x00, 0x00, 0xb2, 0x1f, 0x2f, 0x03, 0x2e, 0xc0, 0xf5, 0xf0, 0x30, 0x48, 0x08, 0x47, 0xaa, 0x74, 0x30, 0x07, - 0x2e, 0x7a, 0x00, 0x61, 0x22, 0x4b, 0x1a, 0x05, 0x2f, 0x07, 0x2e, 0x66, 0xf5, 0xbf, 0xbd, 0xbf, 0xb9, 0xc0, 0x90, - 0x0b, 0x2f, 0x1d, 0x56, 0x2b, 0x30, 0xd2, 0x42, 0xdb, 0x42, 0x01, 0x04, 0xc2, 0x42, 0x04, 0xbd, 0xfe, 0x80, 0x81, - 0x84, 0x23, 0x2e, 0x7a, 0x00, 0x02, 0x42, 0x02, 0x32, 0x25, 0x2e, 0x62, 0xf5, 0x05, 0x2e, 0xd6, 0x00, 0x81, 0x84, - 0x25, 0x2e, 0xd6, 0x00, 0x02, 0x31, 0x25, 0x2e, 0x60, 0xf5, 0x05, 0x2e, 0x8a, 0x00, 0x0b, 0x50, 0x90, 0x08, 0x80, - 0xb2, 0x0b, 0x2f, 0x05, 0x2e, 0xca, 0xf5, 0xf0, 0x3e, 0x90, 0x08, 0x25, 0x2e, 0xca, 0xf5, 0x05, 0x2e, 0x59, 0xf5, - 0xe0, 0x3f, 0x90, 0x08, 0x25, 0x2e, 0x59, 0xf5, 0x90, 0x6f, 0xa1, 0x6f, 0xb3, 0x6f, 0xc4, 0x6f, 0xd5, 0x6f, 0xe6, - 0x6f, 0xf7, 0x6f, 0x7b, 0x6f, 0x82, 0x6f, 0x70, 0x5f, 0xc8, 0x2e, 0xc0, 0x50, 0x90, 0x7f, 0xe5, 0x7f, 0xd4, 0x7f, - 0xc3, 0x7f, 0xb1, 0x7f, 0xa2, 0x7f, 0x87, 0x7f, 0xf6, 0x7f, 0x7b, 0x7f, 0x00, 0x2e, 0x01, 0x2e, 0x60, 0xf5, 0x60, - 0x7f, 0x98, 0x2e, 0x35, 0xb7, 0x02, 0x30, 0x63, 0x6f, 0x15, 0x52, 0x50, 0x7f, 0x62, 0x7f, 0x5a, 0x2c, 0x02, 0x32, - 0x1a, 0x09, 0x00, 0xb3, 0x14, 0x2f, 0x00, 0xb2, 0x03, 0x2f, 0x09, 0x2e, 0x18, 0x00, 0x00, 0x91, 0x0c, 0x2f, 0x43, - 0x7f, 0x98, 0x2e, 0x97, 0xb7, 0x1f, 0x50, 0x02, 0x8a, 0x02, 0x32, 0x04, 0x30, 0x25, 0x2e, 0x64, 0xf5, 0x15, 0x52, - 0x50, 0x6f, 0x43, 0x6f, 0x44, 0x43, 0x25, 0x2e, 0x60, 0xf5, 0xd9, 0x08, 0xc0, 0xb2, 0x36, 0x2f, 0x98, 0x2e, 0x3e, - 0xb7, 0x00, 0xb2, 0x06, 0x2f, 0x01, 0x2e, 0x19, 0x00, 0x00, 0xb2, 0x02, 0x2f, 0x50, 0x6f, 0x00, 0x90, 0x0a, 0x2f, - 0x01, 0x2e, 0x79, 0x00, 0x00, 0x90, 0x19, 0x2f, 0x10, 0x30, 0x21, 0x2e, 0x79, 0x00, 0x00, 0x30, 0x98, 0x2e, 0xdc, - 0x03, 0x13, 0x2d, 0x01, 0x2e, 0xc3, 0xf5, 0x0c, 0xbc, 0x0f, 0xb8, 0x12, 0x30, 0x10, 0x04, 0x03, 0xb0, 0x26, 0x25, - 0x21, 0x50, 0x03, 0x52, 0x98, 0x2e, 0x4d, 0xb7, 0x10, 0x30, 0x21, 0x2e, 0xee, 0x00, 0x02, 0x30, 0x60, 0x7f, 0x25, - 0x2e, 0x79, 0x00, 0x60, 0x6f, 0x00, 0x90, 0x05, 0x2f, 0x00, 0x30, 0x21, 0x2e, 0xea, 0x00, 0x15, 0x50, 0x21, 0x2e, - 0x64, 0xf5, 0x15, 0x52, 0x23, 0x2e, 0x60, 0xf5, 0x02, 0x32, 0x50, 0x6f, 0x00, 0x90, 0x02, 0x2f, 0x03, 0x30, 0x27, - 0x2e, 0x78, 0x00, 0x07, 0x2e, 0x60, 0xf5, 0x1a, 0x09, 0x00, 0x91, 0xa3, 0x2f, 0x19, 0x09, 0x00, 0x91, 0xa0, 0x2f, - 0x90, 0x6f, 0xa2, 0x6f, 0xb1, 0x6f, 0xc3, 0x6f, 0xd4, 0x6f, 0xe5, 0x6f, 0x7b, 0x6f, 0xf6, 0x6f, 0x87, 0x6f, 0x40, - 0x5f, 0xc8, 0x2e, 0xc0, 0x50, 0xe7, 0x7f, 0xf6, 0x7f, 0x26, 0x30, 0x0f, 0x2e, 0x61, 0xf5, 0x2f, 0x2e, 0x7c, 0x00, - 0x0f, 0x2e, 0x7c, 0x00, 0xbe, 0x09, 0xa2, 0x7f, 0x80, 0x7f, 0x80, 0xb3, 0xd5, 0x7f, 0xc4, 0x7f, 0xb3, 0x7f, 0x91, - 0x7f, 0x7b, 0x7f, 0x0b, 0x2f, 0x23, 0x50, 0x1a, 0x25, 0x12, 0x40, 0x42, 0x7f, 0x74, 0x82, 0x12, 0x40, 0x52, 0x7f, - 0x00, 0x2e, 0x00, 0x40, 0x60, 0x7f, 0x98, 0x2e, 0x6a, 0xd6, 0x81, 0x30, 0x01, 0x2e, 0x7c, 0x00, 0x01, 0x08, 0x00, - 0xb2, 0x42, 0x2f, 0x03, 0x2e, 0x89, 0x00, 0x01, 0x2e, 0x89, 0x00, 0x97, 0xbc, 0x06, 0xbc, 0x9f, 0xb8, 0x0f, 0xb8, - 0x00, 0x90, 0x23, 0x2e, 0xd8, 0x00, 0x10, 0x30, 0x01, 0x30, 0x2a, 0x2f, 0x03, 0x2e, 0xd4, 0x00, 0x44, 0xb2, 0x05, - 0x2f, 0x47, 0xb2, 0x00, 0x30, 0x2d, 0x2f, 0x21, 0x2e, 0x7c, 0x00, 0x2b, 0x2d, 0x03, 0x2e, 0xfd, 0xf5, 0x9e, 0xbc, - 0x9f, 0xb8, 0x40, 0x90, 0x14, 0x2f, 0x03, 0x2e, 0xfc, 0xf5, 0x99, 0xbc, 0x9f, 0xb8, 0x40, 0x90, 0x0e, 0x2f, 0x03, - 0x2e, 0x49, 0xf1, 0x25, 0x54, 0x4a, 0x08, 0x40, 0x90, 0x08, 0x2f, 0x98, 0x2e, 0x35, 0xb7, 0x00, 0xb2, 0x10, 0x30, - 0x03, 0x2f, 0x50, 0x30, 0x21, 0x2e, 0xd4, 0x00, 0x10, 0x2d, 0x98, 0x2e, 0xaf, 0xb7, 0x00, 0x30, 0x21, 0x2e, 0x7c, - 0x00, 0x0a, 0x2d, 0x05, 0x2e, 0x69, 0xf7, 0x2d, 0xbd, 0x2f, 0xb9, 0x80, 0xb2, 0x01, 0x2f, 0x21, 0x2e, 0x7d, 0x00, - 0x23, 0x2e, 0x7c, 0x00, 0xe0, 0x31, 0x21, 0x2e, 0x61, 0xf5, 0xf6, 0x6f, 0xe7, 0x6f, 0x80, 0x6f, 0xa2, 0x6f, 0xb3, - 0x6f, 0xc4, 0x6f, 0xd5, 0x6f, 0x7b, 0x6f, 0x91, 0x6f, 0x40, 0x5f, 0xc8, 0x2e, 0x60, 0x51, 0x0a, 0x25, 0x36, 0x88, - 0xf4, 0x7f, 0xeb, 0x7f, 0x00, 0x32, 0x31, 0x52, 0x32, 0x30, 0x13, 0x30, 0x98, 0x2e, 0x15, 0xcb, 0x0a, 0x25, 0x33, - 0x84, 0xd2, 0x7f, 0x43, 0x30, 0x05, 0x50, 0x2d, 0x52, 0x98, 0x2e, 0x95, 0xc1, 0xd2, 0x6f, 0x27, 0x52, 0x98, 0x2e, - 0xd7, 0xc7, 0x2a, 0x25, 0xb0, 0x86, 0xc0, 0x7f, 0xd3, 0x7f, 0xaf, 0x84, 0x29, 0x50, 0xf1, 0x6f, 0x98, 0x2e, 0x4d, - 0xc8, 0x2a, 0x25, 0xae, 0x8a, 0xaa, 0x88, 0xf2, 0x6e, 0x2b, 0x50, 0xc1, 0x6f, 0xd3, 0x6f, 0xf4, 0x7f, 0x98, 0x2e, - 0xb6, 0xc8, 0xe0, 0x6e, 0x00, 0xb2, 0x32, 0x2f, 0x33, 0x54, 0x83, 0x86, 0xf1, 0x6f, 0xc3, 0x7f, 0x04, 0x30, 0x30, - 0x30, 0xf4, 0x7f, 0xd0, 0x7f, 0xb2, 0x7f, 0xe3, 0x30, 0xc5, 0x6f, 0x56, 0x40, 0x45, 0x41, 0x28, 0x08, 0x03, 0x14, - 0x0e, 0xb4, 0x08, 0xbc, 0x82, 0x40, 0x10, 0x0a, 0x2f, 0x54, 0x26, 0x05, 0x91, 0x7f, 0x44, 0x28, 0xa3, 0x7f, 0x98, - 0x2e, 0xd9, 0xc0, 0x08, 0xb9, 0x33, 0x30, 0x53, 0x09, 0xc1, 0x6f, 0xd3, 0x6f, 0xf4, 0x6f, 0x83, 0x17, 0x47, 0x40, - 0x6c, 0x15, 0xb2, 0x6f, 0xbe, 0x09, 0x75, 0x0b, 0x90, 0x42, 0x45, 0x42, 0x51, 0x0e, 0x32, 0xbc, 0x02, 0x89, 0xa1, - 0x6f, 0x7e, 0x86, 0xf4, 0x7f, 0xd0, 0x7f, 0xb2, 0x7f, 0x04, 0x30, 0x91, 0x6f, 0xd6, 0x2f, 0xeb, 0x6f, 0xa0, 0x5e, - 0xb8, 0x2e, 0x03, 0x2e, 0x97, 0x00, 0x1b, 0xbc, 0x60, 0x50, 0x9f, 0xbc, 0x0c, 0xb8, 0xf0, 0x7f, 0x40, 0xb2, 0xeb, - 0x7f, 0x2b, 0x2f, 0x03, 0x2e, 0x7f, 0x00, 0x41, 0x40, 0x01, 0x2e, 0xc8, 0x00, 0x01, 0x1a, 0x11, 0x2f, 0x37, 0x58, - 0x23, 0x2e, 0xc8, 0x00, 0x10, 0x41, 0xa0, 0x7f, 0x38, 0x81, 0x01, 0x41, 0xd0, 0x7f, 0xb1, 0x7f, 0x98, 0x2e, 0x64, - 0xcf, 0xd0, 0x6f, 0x07, 0x80, 0xa1, 0x6f, 0x11, 0x42, 0x00, 0x2e, 0xb1, 0x6f, 0x01, 0x42, 0x11, 0x30, 0x01, 0x2e, - 0xfc, 0x00, 0x00, 0xa8, 0x03, 0x30, 0xcb, 0x22, 0x4a, 0x25, 0x01, 0x2e, 0x7f, 0x00, 0x3c, 0x89, 0x35, 0x52, 0x05, - 0x54, 0x98, 0x2e, 0xc4, 0xce, 0xc1, 0x6f, 0xf0, 0x6f, 0x98, 0x2e, 0x95, 0xcf, 0x04, 0x2d, 0x01, 0x30, 0xf0, 0x6f, - 0x98, 0x2e, 0x95, 0xcf, 0xeb, 0x6f, 0xa0, 0x5f, 0xb8, 0x2e, 0x03, 0x2e, 0xb3, 0x00, 0x02, 0x32, 0xf0, 0x30, 0x03, - 0x31, 0x30, 0x50, 0x8a, 0x08, 0x08, 0x08, 0xcb, 0x08, 0xe0, 0x7f, 0x80, 0xb2, 0xf3, 0x7f, 0xdb, 0x7f, 0x25, 0x2f, - 0x03, 0x2e, 0xca, 0x00, 0x41, 0x90, 0x04, 0x2f, 0x01, 0x30, 0x23, 0x2e, 0xca, 0x00, 0x98, 0x2e, 0x3f, 0x03, 0xc0, - 0xb2, 0x05, 0x2f, 0x03, 0x2e, 0xda, 0x00, 0x00, 0x30, 0x41, 0x04, 0x23, 0x2e, 0xda, 0x00, 0x98, 0x2e, 0x92, 0xb2, - 0x10, 0x25, 0xf0, 0x6f, 0x00, 0xb2, 0x05, 0x2f, 0x01, 0x2e, 0xda, 0x00, 0x02, 0x30, 0x10, 0x04, 0x21, 0x2e, 0xda, - 0x00, 0x40, 0xb2, 0x01, 0x2f, 0x23, 0x2e, 0xc8, 0x01, 0xdb, 0x6f, 0xe0, 0x6f, 0xd0, 0x5f, 0x80, 0x2e, 0x95, 0xcf, - 0x01, 0x30, 0xe0, 0x6f, 0x98, 0x2e, 0x95, 0xcf, 0x11, 0x30, 0x23, 0x2e, 0xca, 0x00, 0xdb, 0x6f, 0xd0, 0x5f, 0xb8, - 0x2e, 0xd0, 0x50, 0x0a, 0x25, 0x33, 0x84, 0x55, 0x50, 0xd2, 0x7f, 0xe2, 0x7f, 0x03, 0x8c, 0xc0, 0x7f, 0xbb, 0x7f, - 0x00, 0x30, 0x05, 0x5a, 0x39, 0x54, 0x51, 0x41, 0xa5, 0x7f, 0x96, 0x7f, 0x80, 0x7f, 0x98, 0x2e, 0xd9, 0xc0, 0x05, - 0x30, 0xf5, 0x7f, 0x20, 0x25, 0x91, 0x6f, 0x3b, 0x58, 0x3d, 0x5c, 0x3b, 0x56, 0x98, 0x2e, 0x67, 0xcc, 0xc1, 0x6f, - 0xd5, 0x6f, 0x52, 0x40, 0x50, 0x43, 0xc1, 0x7f, 0xd5, 0x7f, 0x10, 0x25, 0x98, 0x2e, 0xfe, 0xc9, 0x10, 0x25, 0x98, - 0x2e, 0x74, 0xc0, 0x86, 0x6f, 0x30, 0x28, 0x92, 0x6f, 0x82, 0x8c, 0xa5, 0x6f, 0x6f, 0x52, 0x69, 0x0e, 0x39, 0x54, - 0xdb, 0x2f, 0x19, 0xa0, 0x15, 0x30, 0x03, 0x2f, 0x00, 0x30, 0x21, 0x2e, 0x81, 0x01, 0x0a, 0x2d, 0x01, 0x2e, 0x81, - 0x01, 0x05, 0x28, 0x42, 0x36, 0x21, 0x2e, 0x81, 0x01, 0x02, 0x0e, 0x01, 0x2f, 0x98, 0x2e, 0xf3, 0x03, 0x57, 0x50, - 0x12, 0x30, 0x01, 0x40, 0x98, 0x2e, 0xfe, 0xc9, 0x51, 0x6f, 0x0b, 0x5c, 0x8e, 0x0e, 0x3b, 0x6f, 0x57, 0x58, 0x02, - 0x30, 0x21, 0x2e, 0x95, 0x01, 0x45, 0x6f, 0x2a, 0x8d, 0xd2, 0x7f, 0xcb, 0x7f, 0x13, 0x2f, 0x02, 0x30, 0x3f, 0x50, - 0xd2, 0x7f, 0xa8, 0x0e, 0x0e, 0x2f, 0xc0, 0x6f, 0x53, 0x54, 0x02, 0x00, 0x51, 0x54, 0x42, 0x0e, 0x10, 0x30, 0x59, - 0x52, 0x02, 0x30, 0x01, 0x2f, 0x00, 0x2e, 0x03, 0x2d, 0x50, 0x42, 0x42, 0x42, 0x12, 0x30, 0xd2, 0x7f, 0x80, 0xb2, - 0x03, 0x2f, 0x00, 0x30, 0x21, 0x2e, 0x80, 0x01, 0x12, 0x2d, 0x01, 0x2e, 0xc9, 0x00, 0x02, 0x80, 0x05, 0x2e, 0x80, - 0x01, 0x11, 0x30, 0x91, 0x28, 0x00, 0x40, 0x25, 0x2e, 0x80, 0x01, 0x10, 0x0e, 0x05, 0x2f, 0x01, 0x2e, 0x7f, 0x01, - 0x01, 0x90, 0x01, 0x2f, 0x98, 0x2e, 0xf3, 0x03, 0x00, 0x2e, 0xa0, 0x41, 0x01, 0x90, 0xa6, 0x7f, 0x90, 0x2e, 0xe3, - 0xb4, 0x01, 0x2e, 0x95, 0x01, 0x00, 0xa8, 0x90, 0x2e, 0xe3, 0xb4, 0x5b, 0x54, 0x95, 0x80, 0x82, 0x40, 0x80, 0xb2, - 0x02, 0x40, 0x2d, 0x8c, 0x3f, 0x52, 0x96, 0x7f, 0x90, 0x2e, 0xc2, 0xb3, 0x29, 0x0e, 0x76, 0x2f, 0x01, 0x2e, 0xc9, - 0x00, 0x00, 0x40, 0x81, 0x28, 0x45, 0x52, 0xb3, 0x30, 0x98, 0x2e, 0x0f, 0xca, 0x5d, 0x54, 0x80, 0x7f, 0x00, 0x2e, - 0xa1, 0x40, 0x72, 0x7f, 0x82, 0x80, 0x82, 0x40, 0x60, 0x7f, 0x98, 0x2e, 0xfe, 0xc9, 0x10, 0x25, 0x98, 0x2e, 0x74, - 0xc0, 0x62, 0x6f, 0x05, 0x30, 0x87, 0x40, 0xc0, 0x91, 0x04, 0x30, 0x05, 0x2f, 0x05, 0x2e, 0x83, 0x01, 0x80, 0xb2, - 0x14, 0x30, 0x00, 0x2f, 0x04, 0x30, 0x05, 0x2e, 0xc9, 0x00, 0x73, 0x6f, 0x81, 0x40, 0xe2, 0x40, 0x69, 0x04, 0x11, - 0x0f, 0xe1, 0x40, 0x16, 0x30, 0xfe, 0x29, 0xcb, 0x40, 0x02, 0x2f, 0x83, 0x6f, 0x83, 0x0f, 0x22, 0x2f, 0x47, 0x56, - 0x13, 0x0f, 0x12, 0x30, 0x77, 0x2f, 0x49, 0x54, 0x42, 0x0e, 0x12, 0x30, 0x73, 0x2f, 0x00, 0x91, 0x0a, 0x2f, 0x01, - 0x2e, 0x8b, 0x01, 0x19, 0xa8, 0x02, 0x30, 0x6c, 0x2f, 0x63, 0x50, 0x00, 0x2e, 0x17, 0x42, 0x05, 0x42, 0x68, 0x2c, - 0x12, 0x30, 0x0b, 0x25, 0x08, 0x0f, 0x50, 0x30, 0x02, 0x2f, 0x21, 0x2e, 0x83, 0x01, 0x03, 0x2d, 0x40, 0x30, 0x21, - 0x2e, 0x83, 0x01, 0x2b, 0x2e, 0x85, 0x01, 0x5a, 0x2c, 0x12, 0x30, 0x00, 0x91, 0x2b, 0x25, 0x04, 0x2f, 0x63, 0x50, - 0x02, 0x30, 0x17, 0x42, 0x17, 0x2c, 0x02, 0x42, 0x98, 0x2e, 0xfe, 0xc9, 0x10, 0x25, 0x98, 0x2e, 0x74, 0xc0, 0x05, - 0x2e, 0xc9, 0x00, 0x81, 0x84, 0x5b, 0x30, 0x82, 0x40, 0x37, 0x2e, 0x83, 0x01, 0x02, 0x0e, 0x07, 0x2f, 0x5f, 0x52, - 0x40, 0x30, 0x62, 0x40, 0x41, 0x40, 0x91, 0x0e, 0x01, 0x2f, 0x21, 0x2e, 0x83, 0x01, 0x05, 0x30, 0x2b, 0x2e, 0x85, - 0x01, 0x12, 0x30, 0x36, 0x2c, 0x16, 0x30, 0x15, 0x25, 0x81, 0x7f, 0x98, 0x2e, 0xfe, 0xc9, 0x10, 0x25, 0x98, 0x2e, - 0x74, 0xc0, 0x19, 0xa2, 0x16, 0x30, 0x15, 0x2f, 0x05, 0x2e, 0x97, 0x01, 0x80, 0x6f, 0x82, 0x0e, 0x05, 0x2f, 0x01, - 0x2e, 0x86, 0x01, 0x06, 0x28, 0x21, 0x2e, 0x86, 0x01, 0x0b, 0x2d, 0x03, 0x2e, 0x87, 0x01, 0x5f, 0x54, 0x4e, 0x28, - 0x91, 0x42, 0x00, 0x2e, 0x82, 0x40, 0x90, 0x0e, 0x01, 0x2f, 0x21, 0x2e, 0x88, 0x01, 0x02, 0x30, 0x13, 0x2c, 0x05, - 0x30, 0xc0, 0x6f, 0x08, 0x1c, 0xa8, 0x0f, 0x16, 0x30, 0x05, 0x30, 0x5b, 0x50, 0x09, 0x2f, 0x02, 0x80, 0x2d, 0x2e, - 0x82, 0x01, 0x05, 0x42, 0x05, 0x80, 0x00, 0x2e, 0x02, 0x42, 0x3e, 0x80, 0x00, 0x2e, 0x06, 0x42, 0x02, 0x30, 0x90, - 0x6f, 0x3e, 0x88, 0x01, 0x40, 0x04, 0x41, 0x4c, 0x28, 0x01, 0x42, 0x07, 0x80, 0x10, 0x25, 0x24, 0x40, 0x00, 0x40, - 0x00, 0xa8, 0xf5, 0x22, 0x23, 0x29, 0x44, 0x42, 0x7a, 0x82, 0x7e, 0x88, 0x43, 0x40, 0x04, 0x41, 0x00, 0xab, 0xf5, - 0x23, 0xdf, 0x28, 0x43, 0x42, 0xd9, 0xa0, 0x14, 0x2f, 0x00, 0x90, 0x02, 0x2f, 0xd2, 0x6f, 0x81, 0xb2, 0x05, 0x2f, - 0x63, 0x54, 0x06, 0x28, 0x90, 0x42, 0x85, 0x42, 0x09, 0x2c, 0x02, 0x30, 0x5b, 0x50, 0x03, 0x80, 0x29, 0x2e, 0x7e, - 0x01, 0x2b, 0x2e, 0x82, 0x01, 0x05, 0x42, 0x12, 0x30, 0x2b, 0x2e, 0x83, 0x01, 0x45, 0x82, 0x00, 0x2e, 0x40, 0x40, - 0x7a, 0x82, 0x02, 0xa0, 0x08, 0x2f, 0x63, 0x50, 0x3b, 0x30, 0x15, 0x42, 0x05, 0x42, 0x37, 0x80, 0x37, 0x2e, 0x7e, - 0x01, 0x05, 0x42, 0x12, 0x30, 0x01, 0x2e, 0xc9, 0x00, 0x02, 0x8c, 0x40, 0x40, 0x84, 0x41, 0x7a, 0x8c, 0x04, 0x0f, - 0x03, 0x2f, 0x01, 0x2e, 0x8b, 0x01, 0x19, 0xa4, 0x04, 0x2f, 0x2b, 0x2e, 0x82, 0x01, 0x98, 0x2e, 0xf3, 0x03, 0x12, - 0x30, 0x81, 0x90, 0x61, 0x52, 0x08, 0x2f, 0x65, 0x42, 0x65, 0x42, 0x43, 0x80, 0x39, 0x84, 0x82, 0x88, 0x05, 0x42, - 0x45, 0x42, 0x85, 0x42, 0x05, 0x43, 0x00, 0x2e, 0x80, 0x41, 0x00, 0x90, 0x90, 0x2e, 0xe1, 0xb4, 0x65, 0x54, 0xc1, - 0x6f, 0x80, 0x40, 0x00, 0xb2, 0x43, 0x58, 0x69, 0x50, 0x44, 0x2f, 0x55, 0x5c, 0xb7, 0x87, 0x8c, 0x0f, 0x0d, 0x2e, - 0x96, 0x01, 0xc4, 0x40, 0x36, 0x2f, 0x41, 0x56, 0x8b, 0x0e, 0x2a, 0x2f, 0x0b, 0x52, 0xa1, 0x0e, 0x0a, 0x2f, 0x05, - 0x2e, 0x8f, 0x01, 0x14, 0x25, 0x98, 0x2e, 0xfe, 0xc9, 0x4b, 0x54, 0x02, 0x0f, 0x69, 0x50, 0x05, 0x30, 0x65, 0x54, - 0x15, 0x2f, 0x03, 0x2e, 0x8e, 0x01, 0x4d, 0x5c, 0x8e, 0x0f, 0x3a, 0x2f, 0x05, 0x2e, 0x8f, 0x01, 0x98, 0x2e, 0xfe, - 0xc9, 0x4f, 0x54, 0x82, 0x0f, 0x05, 0x30, 0x69, 0x50, 0x65, 0x54, 0x30, 0x2f, 0x6d, 0x52, 0x15, 0x30, 0x42, 0x8c, - 0x45, 0x42, 0x04, 0x30, 0x2b, 0x2c, 0x84, 0x43, 0x6b, 0x52, 0x42, 0x8c, 0x00, 0x2e, 0x85, 0x43, 0x15, 0x30, 0x24, - 0x2c, 0x45, 0x42, 0x8e, 0x0f, 0x20, 0x2f, 0x0d, 0x2e, 0x8e, 0x01, 0xb1, 0x0e, 0x1c, 0x2f, 0x23, 0x2e, 0x8e, 0x01, - 0x1a, 0x2d, 0x0e, 0x0e, 0x17, 0x2f, 0xa1, 0x0f, 0x15, 0x2f, 0x23, 0x2e, 0x8d, 0x01, 0x13, 0x2d, 0x98, 0x2e, 0x74, - 0xc0, 0x43, 0x54, 0xc2, 0x0e, 0x0a, 0x2f, 0x65, 0x50, 0x04, 0x80, 0x0b, 0x30, 0x06, 0x82, 0x0b, 0x42, 0x79, 0x80, - 0x41, 0x40, 0x12, 0x30, 0x25, 0x2e, 0x8c, 0x01, 0x01, 0x42, 0x05, 0x30, 0x69, 0x50, 0x65, 0x54, 0x84, 0x82, 0x43, - 0x84, 0xbe, 0x8c, 0x84, 0x40, 0x86, 0x41, 0x26, 0x29, 0x94, 0x42, 0xbe, 0x8e, 0xd5, 0x7f, 0x19, 0xa1, 0x43, 0x40, - 0x0b, 0x2e, 0x8c, 0x01, 0x84, 0x40, 0xc7, 0x41, 0x5d, 0x29, 0x27, 0x29, 0x45, 0x42, 0x84, 0x42, 0xc2, 0x7f, 0x01, - 0x2f, 0xc0, 0xb3, 0x1d, 0x2f, 0x05, 0x2e, 0x94, 0x01, 0x99, 0xa0, 0x01, 0x2f, 0x80, 0xb3, 0x13, 0x2f, 0x80, 0xb3, - 0x18, 0x2f, 0xc0, 0xb3, 0x16, 0x2f, 0x12, 0x40, 0x01, 0x40, 0x92, 0x7f, 0x98, 0x2e, 0x74, 0xc0, 0x92, 0x6f, 0x10, - 0x0f, 0x20, 0x30, 0x03, 0x2f, 0x10, 0x30, 0x21, 0x2e, 0x7e, 0x01, 0x0a, 0x2d, 0x21, 0x2e, 0x7e, 0x01, 0x07, 0x2d, - 0x20, 0x30, 0x21, 0x2e, 0x7e, 0x01, 0x03, 0x2d, 0x10, 0x30, 0x21, 0x2e, 0x7e, 0x01, 0xc2, 0x6f, 0x01, 0x2e, 0xc9, - 0x00, 0xbc, 0x84, 0x02, 0x80, 0x82, 0x40, 0x00, 0x40, 0x90, 0x0e, 0xd5, 0x6f, 0x02, 0x2f, 0x15, 0x30, 0x98, 0x2e, - 0xf3, 0x03, 0x41, 0x91, 0x05, 0x30, 0x07, 0x2f, 0x67, 0x50, 0x3d, 0x80, 0x2b, 0x2e, 0x8f, 0x01, 0x05, 0x42, 0x04, - 0x80, 0x00, 0x2e, 0x05, 0x42, 0x02, 0x2c, 0x00, 0x30, 0x00, 0x30, 0xa2, 0x6f, 0x98, 0x8a, 0x86, 0x40, 0x80, 0xa7, - 0x05, 0x2f, 0x98, 0x2e, 0xf3, 0x03, 0xc0, 0x30, 0x21, 0x2e, 0x95, 0x01, 0x06, 0x25, 0x1a, 0x25, 0xe2, 0x6f, 0x76, - 0x82, 0x96, 0x40, 0x56, 0x43, 0x51, 0x0e, 0xfb, 0x2f, 0xbb, 0x6f, 0x30, 0x5f, 0xb8, 0x2e, 0x01, 0x2e, 0xb8, 0x00, - 0x01, 0x31, 0x41, 0x08, 0x40, 0xb2, 0x20, 0x50, 0xf2, 0x30, 0x02, 0x08, 0xfb, 0x7f, 0x01, 0x30, 0x10, 0x2f, 0x05, - 0x2e, 0xcc, 0x00, 0x81, 0x90, 0xe0, 0x7f, 0x03, 0x2f, 0x23, 0x2e, 0xcc, 0x00, 0x98, 0x2e, 0x55, 0xb6, 0x98, 0x2e, - 0x1d, 0xb5, 0x10, 0x25, 0xfb, 0x6f, 0xe0, 0x6f, 0xe0, 0x5f, 0x80, 0x2e, 0x95, 0xcf, 0x98, 0x2e, 0x95, 0xcf, 0x10, - 0x30, 0x21, 0x2e, 0xcc, 0x00, 0xfb, 0x6f, 0xe0, 0x5f, 0xb8, 0x2e, 0x00, 0x51, 0x05, 0x58, 0xeb, 0x7f, 0x2a, 0x25, - 0x89, 0x52, 0x6f, 0x5a, 0x89, 0x50, 0x13, 0x41, 0x06, 0x40, 0xb3, 0x01, 0x16, 0x42, 0xcb, 0x16, 0x06, 0x40, 0xf3, - 0x02, 0x13, 0x42, 0x65, 0x0e, 0xf5, 0x2f, 0x05, 0x40, 0x14, 0x30, 0x2c, 0x29, 0x04, 0x42, 0x08, 0xa1, 0x00, 0x30, - 0x90, 0x2e, 0x52, 0xb6, 0xb3, 0x88, 0xb0, 0x8a, 0xb6, 0x84, 0xa4, 0x7f, 0xc4, 0x7f, 0xb5, 0x7f, 0xd5, 0x7f, 0x92, - 0x7f, 0x73, 0x30, 0x04, 0x30, 0x55, 0x40, 0x42, 0x40, 0x8a, 0x17, 0xf3, 0x08, 0x6b, 0x01, 0x90, 0x02, 0x53, 0xb8, - 0x4b, 0x82, 0xad, 0xbe, 0x71, 0x7f, 0x45, 0x0a, 0x09, 0x54, 0x84, 0x7f, 0x98, 0x2e, 0xd9, 0xc0, 0xa3, 0x6f, 0x7b, - 0x54, 0xd0, 0x42, 0xa3, 0x7f, 0xf2, 0x7f, 0x60, 0x7f, 0x20, 0x25, 0x71, 0x6f, 0x75, 0x5a, 0x77, 0x58, 0x79, 0x5c, - 0x75, 0x56, 0x98, 0x2e, 0x67, 0xcc, 0xb1, 0x6f, 0x62, 0x6f, 0x50, 0x42, 0xb1, 0x7f, 0xb3, 0x30, 0x10, 0x25, 0x98, - 0x2e, 0x0f, 0xca, 0x84, 0x6f, 0x20, 0x29, 0x71, 0x6f, 0x92, 0x6f, 0xa5, 0x6f, 0x76, 0x82, 0x6a, 0x0e, 0x73, 0x30, - 0x00, 0x30, 0xd0, 0x2f, 0xd2, 0x6f, 0xd1, 0x7f, 0xb4, 0x7f, 0x98, 0x2e, 0x2b, 0xb7, 0x15, 0xbd, 0x0b, 0xb8, 0x02, - 0x0a, 0xc2, 0x6f, 0xc0, 0x7f, 0x98, 0x2e, 0x2b, 0xb7, 0x15, 0xbd, 0x0b, 0xb8, 0x42, 0x0a, 0xc0, 0x6f, 0x08, 0x17, - 0x41, 0x18, 0x89, 0x16, 0xe1, 0x18, 0xd0, 0x18, 0xa1, 0x7f, 0x27, 0x25, 0x16, 0x25, 0x98, 0x2e, 0x79, 0xc0, 0x8b, - 0x54, 0x90, 0x7f, 0xb3, 0x30, 0x82, 0x40, 0x80, 0x90, 0x0d, 0x2f, 0x7d, 0x52, 0x92, 0x6f, 0x98, 0x2e, 0x0f, 0xca, - 0xb2, 0x6f, 0x90, 0x0e, 0x06, 0x2f, 0x8b, 0x50, 0x14, 0x30, 0x42, 0x6f, 0x51, 0x6f, 0x14, 0x42, 0x12, 0x42, 0x01, - 0x42, 0x00, 0x2e, 0x31, 0x6f, 0x98, 0x2e, 0x74, 0xc0, 0x41, 0x6f, 0x80, 0x7f, 0x98, 0x2e, 0x74, 0xc0, 0x82, 0x6f, - 0x10, 0x04, 0x43, 0x52, 0x01, 0x0f, 0x05, 0x2e, 0xcb, 0x00, 0x00, 0x30, 0x04, 0x30, 0x21, 0x2f, 0x51, 0x6f, 0x43, - 0x58, 0x8c, 0x0e, 0x04, 0x30, 0x1c, 0x2f, 0x85, 0x88, 0x41, 0x6f, 0x04, 0x41, 0x8c, 0x0f, 0x04, 0x30, 0x16, 0x2f, - 0x84, 0x88, 0x00, 0x2e, 0x04, 0x41, 0x04, 0x05, 0x8c, 0x0e, 0x04, 0x30, 0x0f, 0x2f, 0x82, 0x88, 0x31, 0x6f, 0x04, - 0x41, 0x04, 0x05, 0x8c, 0x0e, 0x04, 0x30, 0x08, 0x2f, 0x83, 0x88, 0x00, 0x2e, 0x04, 0x41, 0x8c, 0x0f, 0x04, 0x30, - 0x02, 0x2f, 0x21, 0x2e, 0xad, 0x01, 0x14, 0x30, 0x00, 0x91, 0x14, 0x2f, 0x03, 0x2e, 0xa1, 0x01, 0x41, 0x90, 0x0e, - 0x2f, 0x03, 0x2e, 0xad, 0x01, 0x14, 0x30, 0x4c, 0x28, 0x23, 0x2e, 0xad, 0x01, 0x46, 0xa0, 0x06, 0x2f, 0x81, 0x84, - 0x8d, 0x52, 0x48, 0x82, 0x82, 0x40, 0x21, 0x2e, 0xa1, 0x01, 0x42, 0x42, 0x5c, 0x2c, 0x02, 0x30, 0x05, 0x2e, 0xaa, - 0x01, 0x80, 0xb2, 0x02, 0x30, 0x55, 0x2f, 0x03, 0x2e, 0xa9, 0x01, 0x92, 0x6f, 0xb3, 0x30, 0x98, 0x2e, 0x0f, 0xca, - 0xb2, 0x6f, 0x90, 0x0f, 0x00, 0x30, 0x02, 0x30, 0x4a, 0x2f, 0xa2, 0x6f, 0x87, 0x52, 0x91, 0x00, 0x85, 0x52, 0x51, - 0x0e, 0x02, 0x2f, 0x00, 0x2e, 0x43, 0x2c, 0x02, 0x30, 0xc2, 0x6f, 0x7f, 0x52, 0x91, 0x0e, 0x02, 0x30, 0x3c, 0x2f, - 0x51, 0x6f, 0x81, 0x54, 0x98, 0x2e, 0xfe, 0xc9, 0x10, 0x25, 0xb3, 0x30, 0x21, 0x25, 0x98, 0x2e, 0x0f, 0xca, 0x32, - 0x6f, 0xc0, 0x7f, 0xb3, 0x30, 0x12, 0x25, 0x98, 0x2e, 0x0f, 0xca, 0x42, 0x6f, 0xb0, 0x7f, 0xb3, 0x30, 0x12, 0x25, - 0x98, 0x2e, 0x0f, 0xca, 0xb2, 0x6f, 0x90, 0x28, 0x83, 0x52, 0x98, 0x2e, 0xfe, 0xc9, 0xc2, 0x6f, 0x90, 0x0f, 0x00, - 0x30, 0x02, 0x30, 0x1d, 0x2f, 0x05, 0x2e, 0xa1, 0x01, 0x80, 0xb2, 0x12, 0x30, 0x0f, 0x2f, 0x42, 0x6f, 0x03, 0x2e, - 0xab, 0x01, 0x91, 0x0e, 0x02, 0x30, 0x12, 0x2f, 0x52, 0x6f, 0x03, 0x2e, 0xac, 0x01, 0x91, 0x0f, 0x02, 0x30, 0x0c, - 0x2f, 0x21, 0x2e, 0xaa, 0x01, 0x0a, 0x2c, 0x12, 0x30, 0x03, 0x2e, 0xcb, 0x00, 0x8d, 0x58, 0x08, 0x89, 0x41, 0x40, - 0x11, 0x43, 0x00, 0x43, 0x25, 0x2e, 0xa1, 0x01, 0xd4, 0x6f, 0x8f, 0x52, 0x00, 0x43, 0x3a, 0x89, 0x00, 0x2e, 0x10, - 0x43, 0x10, 0x43, 0x61, 0x0e, 0xfb, 0x2f, 0x03, 0x2e, 0xa0, 0x01, 0x11, 0x1a, 0x02, 0x2f, 0x02, 0x25, 0x21, 0x2e, - 0xa0, 0x01, 0xeb, 0x6f, 0x00, 0x5f, 0xb8, 0x2e, 0x91, 0x52, 0x10, 0x30, 0x02, 0x30, 0x95, 0x56, 0x52, 0x42, 0x4b, - 0x0e, 0xfc, 0x2f, 0x8d, 0x54, 0x88, 0x82, 0x93, 0x56, 0x80, 0x42, 0x53, 0x42, 0x40, 0x42, 0x42, 0x86, 0x83, 0x54, - 0xc0, 0x2e, 0xc2, 0x42, 0x00, 0x2e, 0xa3, 0x52, 0x00, 0x51, 0x52, 0x40, 0x47, 0x40, 0x1a, 0x25, 0x01, 0x2e, 0x97, - 0x00, 0x8f, 0xbe, 0x72, 0x86, 0xfb, 0x7f, 0x0b, 0x30, 0x7c, 0xbf, 0xa5, 0x50, 0x10, 0x08, 0xdf, 0xba, 0x70, 0x88, - 0xf8, 0xbf, 0xcb, 0x42, 0xd3, 0x7f, 0x6c, 0xbb, 0xfc, 0xbb, 0xc5, 0x0a, 0x90, 0x7f, 0x1b, 0x7f, 0x0b, 0x43, 0xc0, - 0xb2, 0xe5, 0x7f, 0xb7, 0x7f, 0xa6, 0x7f, 0xc4, 0x7f, 0x90, 0x2e, 0x1c, 0xb7, 0x07, 0x2e, 0xd2, 0x00, 0xc0, 0xb2, - 0x0b, 0x2f, 0x97, 0x52, 0x01, 0x2e, 0xcd, 0x00, 0x82, 0x7f, 0x98, 0x2e, 0xbb, 0xcc, 0x0b, 0x30, 0x37, 0x2e, 0xd2, - 0x00, 0x82, 0x6f, 0x90, 0x6f, 0x1a, 0x25, 0x00, 0xb2, 0x8b, 0x7f, 0x14, 0x2f, 0xa6, 0xbd, 0x25, 0xbd, 0xb6, 0xb9, - 0x2f, 0xb9, 0x80, 0xb2, 0xd4, 0xb0, 0x0c, 0x2f, 0x99, 0x54, 0x9b, 0x56, 0x0b, 0x30, 0x0b, 0x2e, 0xb1, 0x00, 0xa1, - 0x58, 0x9b, 0x42, 0xdb, 0x42, 0x6c, 0x09, 0x2b, 0x2e, 0xb1, 0x00, 0x8b, 0x42, 0xcb, 0x42, 0x86, 0x7f, 0x73, 0x84, - 0xa7, 0x56, 0xc3, 0x08, 0x39, 0x52, 0x05, 0x50, 0x72, 0x7f, 0x63, 0x7f, 0x98, 0x2e, 0xc2, 0xc0, 0xe1, 0x6f, 0x62, - 0x6f, 0xd1, 0x0a, 0x01, 0x2e, 0xcd, 0x00, 0xd5, 0x6f, 0xc4, 0x6f, 0x72, 0x6f, 0x97, 0x52, 0x9d, 0x5c, 0x98, 0x2e, - 0x06, 0xcd, 0x23, 0x6f, 0x90, 0x6f, 0x99, 0x52, 0xc0, 0xb2, 0x04, 0xbd, 0x54, 0x40, 0xaf, 0xb9, 0x45, 0x40, 0xe1, - 0x7f, 0x02, 0x30, 0x06, 0x2f, 0xc0, 0xb2, 0x02, 0x30, 0x03, 0x2f, 0x9b, 0x5c, 0x12, 0x30, 0x94, 0x43, 0x85, 0x43, - 0x03, 0xbf, 0x6f, 0xbb, 0x80, 0xb3, 0x20, 0x2f, 0x06, 0x6f, 0x26, 0x01, 0x16, 0x6f, 0x6e, 0x03, 0x45, 0x42, 0xc0, - 0x90, 0x29, 0x2e, 0xce, 0x00, 0x9b, 0x52, 0x14, 0x2f, 0x9b, 0x5c, 0x00, 0x2e, 0x93, 0x41, 0x86, 0x41, 0xe3, 0x04, - 0xae, 0x07, 0x80, 0xab, 0x04, 0x2f, 0x80, 0x91, 0x0a, 0x2f, 0x86, 0x6f, 0x73, 0x0f, 0x07, 0x2f, 0x83, 0x6f, 0xc0, - 0xb2, 0x04, 0x2f, 0x54, 0x42, 0x45, 0x42, 0x12, 0x30, 0x04, 0x2c, 0x11, 0x30, 0x02, 0x2c, 0x11, 0x30, 0x11, 0x30, - 0x02, 0xbc, 0x0f, 0xb8, 0xd2, 0x7f, 0x00, 0xb2, 0x0a, 0x2f, 0x01, 0x2e, 0xfc, 0x00, 0x05, 0x2e, 0xc7, 0x01, 0x10, - 0x1a, 0x02, 0x2f, 0x21, 0x2e, 0xc7, 0x01, 0x03, 0x2d, 0x02, 0x2c, 0x01, 0x30, 0x01, 0x30, 0xb0, 0x6f, 0x98, 0x2e, - 0x95, 0xcf, 0xd1, 0x6f, 0xa0, 0x6f, 0x98, 0x2e, 0x95, 0xcf, 0xe2, 0x6f, 0x9f, 0x52, 0x01, 0x2e, 0xce, 0x00, 0x82, - 0x40, 0x50, 0x42, 0x0c, 0x2c, 0x42, 0x42, 0x11, 0x30, 0x23, 0x2e, 0xd2, 0x00, 0x01, 0x30, 0xb0, 0x6f, 0x98, 0x2e, - 0x95, 0xcf, 0xa0, 0x6f, 0x01, 0x30, 0x98, 0x2e, 0x95, 0xcf, 0x00, 0x2e, 0xfb, 0x6f, 0x00, 0x5f, 0xb8, 0x2e, 0x83, - 0x86, 0x01, 0x30, 0x00, 0x30, 0x94, 0x40, 0x24, 0x18, 0x06, 0x00, 0x53, 0x0e, 0x4f, 0x02, 0xf9, 0x2f, 0xb8, 0x2e, - 0xa9, 0x52, 0x00, 0x2e, 0x60, 0x40, 0x41, 0x40, 0x0d, 0xbc, 0x98, 0xbc, 0xc0, 0x2e, 0x01, 0x0a, 0x0f, 0xb8, 0xab, - 0x52, 0x53, 0x3c, 0x52, 0x40, 0x40, 0x40, 0x4b, 0x00, 0x82, 0x16, 0x26, 0xb9, 0x01, 0xb8, 0x41, 0x40, 0x10, 0x08, - 0x97, 0xb8, 0x01, 0x08, 0xc0, 0x2e, 0x11, 0x30, 0x01, 0x08, 0x43, 0x86, 0x25, 0x40, 0x04, 0x40, 0xd8, 0xbe, 0x2c, - 0x0b, 0x22, 0x11, 0x54, 0x42, 0x03, 0x80, 0x4b, 0x0e, 0xf6, 0x2f, 0xb8, 0x2e, 0x9f, 0x50, 0x10, 0x50, 0xad, 0x52, - 0x05, 0x2e, 0xd3, 0x00, 0xfb, 0x7f, 0x00, 0x2e, 0x13, 0x40, 0x93, 0x42, 0x41, 0x0e, 0xfb, 0x2f, 0x98, 0x2e, 0xa5, - 0xb7, 0x98, 0x2e, 0x87, 0xcf, 0x01, 0x2e, 0xd9, 0x00, 0x00, 0xb2, 0xfb, 0x6f, 0x0b, 0x2f, 0x01, 0x2e, 0x69, 0xf7, - 0xb1, 0x3f, 0x01, 0x08, 0x01, 0x30, 0xf0, 0x5f, 0x23, 0x2e, 0xd9, 0x00, 0x21, 0x2e, 0x69, 0xf7, 0x80, 0x2e, 0x7a, - 0xb7, 0xf0, 0x5f, 0xb8, 0x2e, 0x01, 0x2e, 0xc0, 0xf8, 0x03, 0x2e, 0xfc, 0xf5, 0x15, 0x54, 0xaf, 0x56, 0x82, 0x08, - 0x0b, 0x2e, 0x69, 0xf7, 0xcb, 0x0a, 0xb1, 0x58, 0x80, 0x90, 0xdd, 0xbe, 0x4c, 0x08, 0x5f, 0xb9, 0x59, 0x22, 0x80, - 0x90, 0x07, 0x2f, 0x03, 0x34, 0xc3, 0x08, 0xf2, 0x3a, 0x0a, 0x08, 0x02, 0x35, 0xc0, 0x90, 0x4a, 0x0a, 0x48, 0x22, - 0xc0, 0x2e, 0x23, 0x2e, 0xfc, 0xf5, 0x10, 0x50, 0xfb, 0x7f, 0x98, 0x2e, 0x56, 0xc7, 0x98, 0x2e, 0x49, 0xc3, 0x10, - 0x30, 0xfb, 0x6f, 0xf0, 0x5f, 0x21, 0x2e, 0xcc, 0x00, 0x21, 0x2e, 0xca, 0x00, 0xb8, 0x2e, 0x03, 0x2e, 0xd3, 0x00, - 0x16, 0xb8, 0x02, 0x34, 0x4a, 0x0c, 0x21, 0x2e, 0x2d, 0xf5, 0xc0, 0x2e, 0x23, 0x2e, 0xd3, 0x00, 0x03, 0xbc, 0x21, - 0x2e, 0xd5, 0x00, 0x03, 0x2e, 0xd5, 0x00, 0x40, 0xb2, 0x10, 0x30, 0x21, 0x2e, 0x77, 0x00, 0x01, 0x30, 0x05, 0x2f, - 0x05, 0x2e, 0xd8, 0x00, 0x80, 0x90, 0x01, 0x2f, 0x23, 0x2e, 0x6f, 0xf5, 0xc0, 0x2e, 0x21, 0x2e, 0xd9, 0x00, 0x11, - 0x30, 0x81, 0x08, 0x01, 0x2e, 0x6a, 0xf7, 0x71, 0x3f, 0x23, 0xbd, 0x01, 0x08, 0x02, 0x0a, 0xc0, 0x2e, 0x21, 0x2e, - 0x6a, 0xf7, 0x30, 0x25, 0x00, 0x30, 0x21, 0x2e, 0x5a, 0xf5, 0x10, 0x50, 0x21, 0x2e, 0x7b, 0x00, 0x21, 0x2e, 0x7c, - 0x00, 0xfb, 0x7f, 0x98, 0x2e, 0xc3, 0xb7, 0x40, 0x30, 0x21, 0x2e, 0xd4, 0x00, 0xfb, 0x6f, 0xf0, 0x5f, 0x03, 0x25, - 0x80, 0x2e, 0xaf, 0xb7, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, - 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, - 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, - 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x01, 0x2e, 0x5d, 0xf7, 0x08, 0xbc, 0x80, 0xac, 0x0e, 0xbb, 0x02, 0x2f, - 0x00, 0x30, 0x41, 0x04, 0x82, 0x06, 0xc0, 0xa4, 0x00, 0x30, 0x11, 0x2f, 0x40, 0xa9, 0x03, 0x2f, 0x40, 0x91, 0x0d, - 0x2f, 0x00, 0xa7, 0x0b, 0x2f, 0x80, 0xb3, 0xb3, 0x58, 0x02, 0x2f, 0x90, 0xa1, 0x26, 0x13, 0x20, 0x23, 0x80, 0x90, - 0x10, 0x30, 0x01, 0x2f, 0xcc, 0x0e, 0x00, 0x2f, 0x00, 0x30, 0xb8, 0x2e, 0xb5, 0x50, 0x18, 0x08, 0x08, 0xbc, 0x88, - 0xb6, 0x0d, 0x17, 0xc6, 0xbd, 0x56, 0xbc, 0xb7, 0x58, 0xda, 0xba, 0x04, 0x01, 0x1d, 0x0a, 0x10, 0x50, 0x05, 0x30, - 0x32, 0x25, 0x45, 0x03, 0xfb, 0x7f, 0xf6, 0x30, 0x21, 0x25, 0x98, 0x2e, 0x37, 0xca, 0x16, 0xb5, 0x9a, 0xbc, 0x06, - 0xb8, 0x80, 0xa8, 0x41, 0x0a, 0x0e, 0x2f, 0x80, 0x90, 0x02, 0x2f, 0x2d, 0x50, 0x48, 0x0f, 0x09, 0x2f, 0xbf, 0xa0, - 0x04, 0x2f, 0xbf, 0x90, 0x06, 0x2f, 0xb7, 0x54, 0xca, 0x0f, 0x03, 0x2f, 0x00, 0x2e, 0x02, 0x2c, 0xb7, 0x52, 0x2d, - 0x52, 0xf2, 0x33, 0x98, 0x2e, 0xd9, 0xc0, 0xfb, 0x6f, 0xf1, 0x37, 0xc0, 0x2e, 0x01, 0x08, 0xf0, 0x5f, 0xbf, 0x56, - 0xb9, 0x54, 0xd0, 0x40, 0xc4, 0x40, 0x0b, 0x2e, 0xfd, 0xf3, 0xbf, 0x52, 0x90, 0x42, 0x94, 0x42, 0x95, 0x42, 0x05, - 0x30, 0xc1, 0x50, 0x0f, 0x88, 0x06, 0x40, 0x04, 0x41, 0x96, 0x42, 0xc5, 0x42, 0x48, 0xbe, 0x73, 0x30, 0x0d, 0x2e, - 0xd8, 0x00, 0x4f, 0xba, 0x84, 0x42, 0x03, 0x42, 0x81, 0xb3, 0x02, 0x2f, 0x2b, 0x2e, 0x6f, 0xf5, 0x06, 0x2d, 0x05, - 0x2e, 0x77, 0xf7, 0xbd, 0x56, 0x93, 0x08, 0x25, 0x2e, 0x77, 0xf7, 0xbb, 0x54, 0x25, 0x2e, 0xc2, 0xf5, 0x07, 0x2e, - 0xfd, 0xf3, 0x42, 0x30, 0xb4, 0x33, 0xda, 0x0a, 0x4c, 0x00, 0x27, 0x2e, 0xfd, 0xf3, 0x43, 0x40, 0xd4, 0x3f, 0xdc, - 0x08, 0x43, 0x42, 0x00, 0x2e, 0x00, 0x2e, 0x43, 0x40, 0x24, 0x30, 0xdc, 0x0a, 0x43, 0x42, 0x04, 0x80, 0x03, 0x2e, - 0xfd, 0xf3, 0x4a, 0x0a, 0x23, 0x2e, 0xfd, 0xf3, 0x61, 0x34, 0xc0, 0x2e, 0x01, 0x42, 0x00, 0x2e, 0x60, 0x50, 0x1a, - 0x25, 0x7a, 0x86, 0xe0, 0x7f, 0xf3, 0x7f, 0x03, 0x25, 0xc3, 0x52, 0x41, 0x84, 0xdb, 0x7f, 0x33, 0x30, 0x98, 0x2e, - 0x16, 0xc2, 0x1a, 0x25, 0x7d, 0x82, 0xf0, 0x6f, 0xe2, 0x6f, 0x32, 0x25, 0x16, 0x40, 0x94, 0x40, 0x26, 0x01, 0x85, - 0x40, 0x8e, 0x17, 0xc4, 0x42, 0x6e, 0x03, 0x95, 0x42, 0x41, 0x0e, 0xf4, 0x2f, 0xdb, 0x6f, 0xa0, 0x5f, 0xb8, 0x2e, - 0xb0, 0x51, 0xfb, 0x7f, 0x98, 0x2e, 0xe8, 0x0d, 0x5a, 0x25, 0x98, 0x2e, 0x0f, 0x0e, 0xcb, 0x58, 0x32, 0x87, 0xc4, - 0x7f, 0x65, 0x89, 0x6b, 0x8d, 0xc5, 0x5a, 0x65, 0x7f, 0xe1, 0x7f, 0x83, 0x7f, 0xa6, 0x7f, 0x74, 0x7f, 0xd0, 0x7f, - 0xb6, 0x7f, 0x94, 0x7f, 0x17, 0x30, 0xc7, 0x52, 0xc9, 0x54, 0x51, 0x7f, 0x00, 0x2e, 0x85, 0x6f, 0x42, 0x7f, 0x00, - 0x2e, 0x51, 0x41, 0x45, 0x81, 0x42, 0x41, 0x13, 0x40, 0x3b, 0x8a, 0x00, 0x40, 0x4b, 0x04, 0xd0, 0x06, 0xc0, 0xac, - 0x85, 0x7f, 0x02, 0x2f, 0x02, 0x30, 0x51, 0x04, 0xd3, 0x06, 0x41, 0x84, 0x05, 0x30, 0x5d, 0x02, 0xc9, 0x16, 0xdf, - 0x08, 0xd3, 0x00, 0x8d, 0x02, 0xaf, 0xbc, 0xb1, 0xb9, 0x59, 0x0a, 0x65, 0x6f, 0x11, 0x43, 0xa1, 0xb4, 0x52, 0x41, - 0x53, 0x41, 0x01, 0x43, 0x34, 0x7f, 0x65, 0x7f, 0x26, 0x31, 0xe5, 0x6f, 0xd4, 0x6f, 0x98, 0x2e, 0x37, 0xca, 0x32, - 0x6f, 0x75, 0x6f, 0x83, 0x40, 0x42, 0x41, 0x23, 0x7f, 0x12, 0x7f, 0xf6, 0x30, 0x40, 0x25, 0x51, 0x25, 0x98, 0x2e, - 0x37, 0xca, 0x14, 0x6f, 0x20, 0x05, 0x70, 0x6f, 0x25, 0x6f, 0x69, 0x07, 0xa2, 0x6f, 0x31, 0x6f, 0x0b, 0x30, 0x04, - 0x42, 0x9b, 0x42, 0x8b, 0x42, 0x55, 0x42, 0x32, 0x7f, 0x40, 0xa9, 0xc3, 0x6f, 0x71, 0x7f, 0x02, 0x30, 0xd0, 0x40, - 0xc3, 0x7f, 0x03, 0x2f, 0x40, 0x91, 0x15, 0x2f, 0x00, 0xa7, 0x13, 0x2f, 0x00, 0xa4, 0x11, 0x2f, 0x84, 0xbd, 0x98, - 0x2e, 0x79, 0xca, 0x55, 0x6f, 0xb7, 0x54, 0x54, 0x41, 0x82, 0x00, 0xf3, 0x3f, 0x45, 0x41, 0xcb, 0x02, 0xf6, 0x30, - 0x98, 0x2e, 0x37, 0xca, 0x35, 0x6f, 0xa4, 0x6f, 0x41, 0x43, 0x03, 0x2c, 0x00, 0x43, 0xa4, 0x6f, 0x35, 0x6f, 0x17, - 0x30, 0x42, 0x6f, 0x51, 0x6f, 0x93, 0x40, 0x42, 0x82, 0x00, 0x41, 0xc3, 0x00, 0x03, 0x43, 0x51, 0x7f, 0x00, 0x2e, - 0x94, 0x40, 0x41, 0x41, 0x4c, 0x02, 0xc4, 0x6f, 0xd1, 0x56, 0x63, 0x0e, 0x74, 0x6f, 0x51, 0x43, 0xa5, 0x7f, 0x8a, - 0x2f, 0x09, 0x2e, 0xd8, 0x00, 0x01, 0xb3, 0x21, 0x2f, 0xcb, 0x58, 0x90, 0x6f, 0x13, 0x41, 0xb6, 0x6f, 0xe4, 0x7f, - 0x00, 0x2e, 0x91, 0x41, 0x14, 0x40, 0x92, 0x41, 0x15, 0x40, 0x17, 0x2e, 0x6f, 0xf5, 0xb6, 0x7f, 0xd0, 0x7f, 0xcb, - 0x7f, 0x98, 0x2e, 0x00, 0x0c, 0x07, 0x15, 0xc2, 0x6f, 0x14, 0x0b, 0x29, 0x2e, 0x6f, 0xf5, 0xc3, 0xa3, 0xc1, 0x8f, - 0xe4, 0x6f, 0xd0, 0x6f, 0xe6, 0x2f, 0x14, 0x30, 0x05, 0x2e, 0x6f, 0xf5, 0x14, 0x0b, 0x29, 0x2e, 0x6f, 0xf5, 0x18, - 0x2d, 0xcd, 0x56, 0x04, 0x32, 0xb5, 0x6f, 0x1c, 0x01, 0x51, 0x41, 0x52, 0x41, 0xc3, 0x40, 0xb5, 0x7f, 0xe4, 0x7f, - 0x98, 0x2e, 0x1f, 0x0c, 0xe4, 0x6f, 0x21, 0x87, 0x00, 0x43, 0x04, 0x32, 0xcf, 0x54, 0x5a, 0x0e, 0xef, 0x2f, 0x15, - 0x54, 0x09, 0x2e, 0x77, 0xf7, 0x22, 0x0b, 0x29, 0x2e, 0x77, 0xf7, 0xfb, 0x6f, 0x50, 0x5e, 0xb8, 0x2e, 0x10, 0x50, - 0x01, 0x2e, 0xd4, 0x00, 0x00, 0xb2, 0xfb, 0x7f, 0x51, 0x2f, 0x01, 0xb2, 0x48, 0x2f, 0x02, 0xb2, 0x42, 0x2f, 0x03, - 0x90, 0x56, 0x2f, 0xd7, 0x52, 0x79, 0x80, 0x42, 0x40, 0x81, 0x84, 0x00, 0x40, 0x42, 0x42, 0x98, 0x2e, 0x93, 0x0c, - 0xd9, 0x54, 0xd7, 0x50, 0xa1, 0x40, 0x98, 0xbd, 0x82, 0x40, 0x3e, 0x82, 0xda, 0x0a, 0x44, 0x40, 0x8b, 0x16, 0xe3, - 0x00, 0x53, 0x42, 0x00, 0x2e, 0x43, 0x40, 0x9a, 0x02, 0x52, 0x42, 0x00, 0x2e, 0x41, 0x40, 0x15, 0x54, 0x4a, 0x0e, - 0x3a, 0x2f, 0x3a, 0x82, 0x00, 0x30, 0x41, 0x40, 0x21, 0x2e, 0x85, 0x0f, 0x40, 0xb2, 0x0a, 0x2f, 0x98, 0x2e, 0xb1, - 0x0c, 0x98, 0x2e, 0x45, 0x0e, 0x98, 0x2e, 0x5b, 0x0e, 0xfb, 0x6f, 0xf0, 0x5f, 0x00, 0x30, 0x80, 0x2e, 0xce, 0xb7, - 0xdd, 0x52, 0xd3, 0x54, 0x42, 0x42, 0x4f, 0x84, 0x73, 0x30, 0xdb, 0x52, 0x83, 0x42, 0x1b, 0x30, 0x6b, 0x42, 0x23, - 0x30, 0x27, 0x2e, 0xd7, 0x00, 0x37, 0x2e, 0xd4, 0x00, 0x21, 0x2e, 0xd6, 0x00, 0x7a, 0x84, 0x17, 0x2c, 0x42, 0x42, - 0x30, 0x30, 0x21, 0x2e, 0xd4, 0x00, 0x12, 0x2d, 0x21, 0x30, 0x00, 0x30, 0x23, 0x2e, 0xd4, 0x00, 0x21, 0x2e, 0x7b, - 0xf7, 0x0b, 0x2d, 0x17, 0x30, 0x98, 0x2e, 0x51, 0x0c, 0xd5, 0x50, 0x0c, 0x82, 0x72, 0x30, 0x2f, 0x2e, 0xd4, 0x00, - 0x25, 0x2e, 0x7b, 0xf7, 0x40, 0x42, 0x00, 0x2e, 0xfb, 0x6f, 0xf0, 0x5f, 0xb8, 0x2e, 0x70, 0x50, 0x0a, 0x25, 0x39, - 0x86, 0xfb, 0x7f, 0xe1, 0x32, 0x62, 0x30, 0x98, 0x2e, 0xc2, 0xc4, 0xb5, 0x56, 0xa5, 0x6f, 0xab, 0x08, 0x91, 0x6f, - 0x4b, 0x08, 0xdf, 0x56, 0xc4, 0x6f, 0x23, 0x09, 0x4d, 0xba, 0x93, 0xbc, 0x8c, 0x0b, 0xd1, 0x6f, 0x0b, 0x09, 0xcb, - 0x52, 0xe1, 0x5e, 0x56, 0x42, 0xaf, 0x09, 0x4d, 0xba, 0x23, 0xbd, 0x94, 0x0a, 0xe5, 0x6f, 0x68, 0xbb, 0xeb, 0x08, - 0xbd, 0xb9, 0x63, 0xbe, 0xfb, 0x6f, 0x52, 0x42, 0xe3, 0x0a, 0xc0, 0x2e, 0x43, 0x42, 0x90, 0x5f, 0xd1, 0x50, 0x03, - 0x2e, 0x25, 0xf3, 0x13, 0x40, 0x00, 0x40, 0x9b, 0xbc, 0x9b, 0xb4, 0x08, 0xbd, 0xb8, 0xb9, 0x98, 0xbc, 0xda, 0x0a, - 0x08, 0xb6, 0x89, 0x16, 0xc0, 0x2e, 0x19, 0x00, 0x62, 0x02, 0x10, 0x50, 0xfb, 0x7f, 0x98, 0x2e, 0x81, 0x0d, 0x01, - 0x2e, 0xd4, 0x00, 0x31, 0x30, 0x08, 0x04, 0xfb, 0x6f, 0x01, 0x30, 0xf0, 0x5f, 0x23, 0x2e, 0xd6, 0x00, 0x21, 0x2e, - 0xd7, 0x00, 0xb8, 0x2e, 0x01, 0x2e, 0xd7, 0x00, 0x03, 0x2e, 0xd6, 0x00, 0x48, 0x0e, 0x01, 0x2f, 0x80, 0x2e, 0x1f, - 0x0e, 0xb8, 0x2e, 0xe3, 0x50, 0x21, 0x34, 0x01, 0x42, 0x82, 0x30, 0xc1, 0x32, 0x25, 0x2e, 0x62, 0xf5, 0x01, 0x00, - 0x22, 0x30, 0x01, 0x40, 0x4a, 0x0a, 0x01, 0x42, 0xb8, 0x2e, 0xe3, 0x54, 0xf0, 0x3b, 0x83, 0x40, 0xd8, 0x08, 0xe5, - 0x52, 0x83, 0x42, 0x00, 0x30, 0x83, 0x30, 0x50, 0x42, 0xc4, 0x32, 0x27, 0x2e, 0x64, 0xf5, 0x94, 0x00, 0x50, 0x42, - 0x40, 0x42, 0xd3, 0x3f, 0x84, 0x40, 0x7d, 0x82, 0xe3, 0x08, 0x40, 0x42, 0x83, 0x42, 0xb8, 0x2e, 0xdd, 0x52, 0x00, - 0x30, 0x40, 0x42, 0x7c, 0x86, 0xb9, 0x52, 0x09, 0x2e, 0x70, 0x0f, 0xbf, 0x54, 0xc4, 0x42, 0xd3, 0x86, 0x54, 0x40, - 0x55, 0x40, 0x94, 0x42, 0x85, 0x42, 0x21, 0x2e, 0xd7, 0x00, 0x42, 0x40, 0x25, 0x2e, 0xfd, 0xf3, 0xc0, 0x42, 0x7e, - 0x82, 0x05, 0x2e, 0x7d, 0x00, 0x80, 0xb2, 0x14, 0x2f, 0x05, 0x2e, 0x89, 0x00, 0x27, 0xbd, 0x2f, 0xb9, 0x80, 0x90, - 0x02, 0x2f, 0x21, 0x2e, 0x6f, 0xf5, 0x0c, 0x2d, 0x07, 0x2e, 0x71, 0x0f, 0x14, 0x30, 0x1c, 0x09, 0x05, 0x2e, 0x77, - 0xf7, 0xbd, 0x56, 0x47, 0xbe, 0x93, 0x08, 0x94, 0x0a, 0x25, 0x2e, 0x77, 0xf7, 0xe7, 0x54, 0x50, 0x42, 0x4a, 0x0e, - 0xfc, 0x2f, 0xb8, 0x2e, 0x50, 0x50, 0x02, 0x30, 0x43, 0x86, 0xe5, 0x50, 0xfb, 0x7f, 0xe3, 0x7f, 0xd2, 0x7f, 0xc0, - 0x7f, 0xb1, 0x7f, 0x00, 0x2e, 0x41, 0x40, 0x00, 0x40, 0x48, 0x04, 0x98, 0x2e, 0x74, 0xc0, 0x1e, 0xaa, 0xd3, 0x6f, - 0x14, 0x30, 0xb1, 0x6f, 0xe3, 0x22, 0xc0, 0x6f, 0x52, 0x40, 0xe4, 0x6f, 0x4c, 0x0e, 0x12, 0x42, 0xd3, 0x7f, 0xeb, - 0x2f, 0x03, 0x2e, 0x86, 0x0f, 0x40, 0x90, 0x11, 0x30, 0x03, 0x2f, 0x23, 0x2e, 0x86, 0x0f, 0x02, 0x2c, 0x00, 0x30, - 0xd0, 0x6f, 0xfb, 0x6f, 0xb0, 0x5f, 0xb8, 0x2e, 0x40, 0x50, 0xf1, 0x7f, 0x0a, 0x25, 0x3c, 0x86, 0xeb, 0x7f, 0x41, - 0x33, 0x22, 0x30, 0x98, 0x2e, 0xc2, 0xc4, 0xd3, 0x6f, 0xf4, 0x30, 0xdc, 0x09, 0x47, 0x58, 0xc2, 0x6f, 0x94, 0x09, - 0xeb, 0x58, 0x6a, 0xbb, 0xdc, 0x08, 0xb4, 0xb9, 0xb1, 0xbd, 0xe9, 0x5a, 0x95, 0x08, 0x21, 0xbd, 0xf6, 0xbf, 0x77, - 0x0b, 0x51, 0xbe, 0xf1, 0x6f, 0xeb, 0x6f, 0x52, 0x42, 0x54, 0x42, 0xc0, 0x2e, 0x43, 0x42, 0xc0, 0x5f, 0x50, 0x50, - 0xf5, 0x50, 0x31, 0x30, 0x11, 0x42, 0xfb, 0x7f, 0x7b, 0x30, 0x0b, 0x42, 0x11, 0x30, 0x02, 0x80, 0x23, 0x33, 0x01, - 0x42, 0x03, 0x00, 0x07, 0x2e, 0x80, 0x03, 0x05, 0x2e, 0xd3, 0x00, 0x23, 0x52, 0xe2, 0x7f, 0xd3, 0x7f, 0xc0, 0x7f, - 0x98, 0x2e, 0xb6, 0x0e, 0xd1, 0x6f, 0x08, 0x0a, 0x1a, 0x25, 0x7b, 0x86, 0xd0, 0x7f, 0x01, 0x33, 0x12, 0x30, 0x98, - 0x2e, 0xc2, 0xc4, 0xd1, 0x6f, 0x08, 0x0a, 0x00, 0xb2, 0x0d, 0x2f, 0xe3, 0x6f, 0x01, 0x2e, 0x80, 0x03, 0x51, 0x30, - 0xc7, 0x86, 0x23, 0x2e, 0x21, 0xf2, 0x08, 0xbc, 0xc0, 0x42, 0x98, 0x2e, 0xa5, 0xb7, 0x00, 0x2e, 0x00, 0x2e, 0xd0, - 0x2e, 0xb0, 0x6f, 0x0b, 0xb8, 0x03, 0x2e, 0x1b, 0x00, 0x08, 0x1a, 0xb0, 0x7f, 0x70, 0x30, 0x04, 0x2f, 0x21, 0x2e, - 0x21, 0xf2, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0x98, 0x2e, 0x6d, 0xc0, 0x98, 0x2e, 0x5d, 0xc0, 0xed, 0x50, 0x98, - 0x2e, 0x44, 0xcb, 0xef, 0x50, 0x98, 0x2e, 0x46, 0xc3, 0xf1, 0x50, 0x98, 0x2e, 0x53, 0xc7, 0x35, 0x50, 0x98, 0x2e, - 0x64, 0xcf, 0x10, 0x30, 0x98, 0x2e, 0xdc, 0x03, 0x20, 0x26, 0xc0, 0x6f, 0x02, 0x31, 0x12, 0x42, 0xab, 0x33, 0x0b, - 0x42, 0x37, 0x80, 0x01, 0x30, 0x01, 0x42, 0xf3, 0x37, 0xf7, 0x52, 0xfb, 0x50, 0x44, 0x40, 0xa2, 0x0a, 0x42, 0x42, - 0x8b, 0x31, 0x09, 0x2e, 0x5e, 0xf7, 0xf9, 0x54, 0xe3, 0x08, 0x83, 0x42, 0x1b, 0x42, 0x23, 0x33, 0x4b, 0x00, 0xbc, - 0x84, 0x0b, 0x40, 0x33, 0x30, 0x83, 0x42, 0x0b, 0x42, 0xe0, 0x7f, 0xd1, 0x7f, 0x98, 0x2e, 0x58, 0xb7, 0xd1, 0x6f, - 0x80, 0x30, 0x40, 0x42, 0x03, 0x30, 0xe0, 0x6f, 0xf3, 0x54, 0x04, 0x30, 0x00, 0x2e, 0x00, 0x2e, 0x01, 0x89, 0x62, - 0x0e, 0xfa, 0x2f, 0x43, 0x42, 0x11, 0x30, 0xfb, 0x6f, 0xc0, 0x2e, 0x01, 0x42, 0xb0, 0x5f, 0xc1, 0x4a, 0x00, 0x00, - 0x6d, 0x57, 0x00, 0x00, 0x77, 0x8e, 0x00, 0x00, 0xe0, 0xff, 0xff, 0xff, 0xd3, 0xff, 0xff, 0xff, 0xe5, 0xff, 0xff, - 0xff, 0xee, 0xe1, 0xff, 0xff, 0x7c, 0x13, 0x00, 0x00, 0x46, 0xe6, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x2e, 0x00, 0xc1, 0x80, - 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, - 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, - 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, - 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, - 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, - 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, - 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, - 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, - 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, - 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, - 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, - 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, - 0x2e, 0x00, 0xc1 -}; - -/*! @name Global array that stores the feature input configuration of BMI270 */ -const struct bmi2_feature_config bmi270_feat_in[BMI270_MAX_FEAT_IN] = { - { .type = BMI2_CONFIG_ID, .page = BMI2_PAGE_1, .start_addr = BMI270_CONFIG_ID_STRT_ADDR }, - { .type = BMI2_MAX_BURST_LEN, .page = BMI2_PAGE_1, .start_addr = BMI270_MAX_BURST_LEN_STRT_ADDR }, - { .type = BMI2_CRT_GYRO_SELF_TEST, .page = BMI2_PAGE_1, .start_addr = BMI270_CRT_GYRO_SELF_TEST_STRT_ADDR }, - { .type = BMI2_ABORT_CRT_GYRO_SELF_TEST, .page = BMI2_PAGE_1, .start_addr = BMI270_ABORT_STRT_ADDR }, - { .type = BMI2_AXIS_MAP, .page = BMI2_PAGE_1, .start_addr = BMI270_AXIS_MAP_STRT_ADDR }, - { .type = BMI2_GYRO_SELF_OFF, .page = BMI2_PAGE_1, .start_addr = BMI270_GYRO_SELF_OFF_STRT_ADDR }, - { .type = BMI2_NVM_PROG_PREP, .page = BMI2_PAGE_1, .start_addr = BMI270_NVM_PROG_PREP_STRT_ADDR }, - { .type = BMI2_GYRO_GAIN_UPDATE, .page = BMI2_PAGE_1, .start_addr = BMI270_GYRO_GAIN_UPDATE_STRT_ADDR }, - { .type = BMI2_ANY_MOTION, .page = BMI2_PAGE_1, .start_addr = BMI270_ANY_MOT_STRT_ADDR }, - { .type = BMI2_NO_MOTION, .page = BMI2_PAGE_2, .start_addr = BMI270_NO_MOT_STRT_ADDR }, - { .type = BMI2_SIG_MOTION, .page = BMI2_PAGE_2, .start_addr = BMI270_SIG_MOT_STRT_ADDR }, - { .type = BMI2_STEP_COUNTER_PARAMS, .page = BMI2_PAGE_3, .start_addr = BMI270_STEP_CNT_1_STRT_ADDR }, - { .type = BMI2_STEP_DETECTOR, .page = BMI2_PAGE_6, .start_addr = BMI270_STEP_CNT_4_STRT_ADDR }, - { .type = BMI2_STEP_COUNTER, .page = BMI2_PAGE_6, .start_addr = BMI270_STEP_CNT_4_STRT_ADDR }, - { .type = BMI2_STEP_ACTIVITY, .page = BMI2_PAGE_6, .start_addr = BMI270_STEP_CNT_4_STRT_ADDR }, - { .type = BMI2_WRIST_GESTURE, .page = BMI2_PAGE_6, .start_addr = BMI270_WRIST_GEST_STRT_ADDR }, - { .type = BMI2_WRIST_WEAR_WAKE_UP, .page = BMI2_PAGE_7, .start_addr = BMI270_WRIST_WEAR_WAKE_UP_STRT_ADDR }, -}; - -/*! @name Global array that stores the feature output configuration */ -const struct bmi2_feature_config bmi270_feat_out[BMI270_MAX_FEAT_OUT] = { - { .type = BMI2_STEP_COUNTER, .page = BMI2_PAGE_0, .start_addr = BMI270_STEP_CNT_OUT_STRT_ADDR }, - { .type = BMI2_STEP_ACTIVITY, .page = BMI2_PAGE_0, .start_addr = BMI270_STEP_ACT_OUT_STRT_ADDR }, - { .type = BMI2_WRIST_GESTURE, .page = BMI2_PAGE_0, .start_addr = BMI270_WRIST_GEST_OUT_STRT_ADDR }, - { .type = BMI2_GYRO_GAIN_UPDATE, .page = BMI2_PAGE_0, .start_addr = BMI270_GYR_USER_GAIN_OUT_STRT_ADDR }, - { .type = BMI2_GYRO_CROSS_SENSE, .page = BMI2_PAGE_0, .start_addr = BMI270_GYRO_CROSS_SENSE_STRT_ADDR }, - { .type = BMI2_NVM_STATUS, .page = BMI2_PAGE_0, .start_addr = BMI270_NVM_VFRM_OUT_STRT_ADDR }, - { .type = BMI2_VFRM_STATUS, .page = BMI2_PAGE_0, .start_addr = BMI270_NVM_VFRM_OUT_STRT_ADDR } -}; - -/*! @name Global array that stores the feature interrupts of BMI270 */ -struct bmi2_map_int bmi270_map_int[BMI270_MAX_INT_MAP] = { - { .type = BMI2_SIG_MOTION, .sens_map_int = BMI270_INT_SIG_MOT_MASK }, - { .type = BMI2_STEP_COUNTER, .sens_map_int = BMI270_INT_STEP_COUNTER_MASK }, - { .type = BMI2_STEP_DETECTOR, .sens_map_int = BMI270_INT_STEP_DETECTOR_MASK }, - { .type = BMI2_STEP_ACTIVITY, .sens_map_int = BMI270_INT_STEP_ACT_MASK }, - { .type = BMI2_WRIST_GESTURE, .sens_map_int = BMI270_INT_WRIST_GEST_MASK }, - { .type = BMI2_WRIST_WEAR_WAKE_UP, .sens_map_int = BMI270_INT_WRIST_WEAR_WAKEUP_MASK }, - { .type = BMI2_ANY_MOTION, .sens_map_int = BMI270_INT_ANY_MOT_MASK }, - { .type = BMI2_NO_MOTION, .sens_map_int = BMI270_INT_NO_MOT_MASK }, -}; - -/******************************************************************************/ - -/*! Local Function Prototypes - ******************************************************************************/ - -/*! - * @brief This internal API is used to validate the device pointer for - * null conditions. - * - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t null_ptr_check(const struct bmi2_dev *dev); - -/*! - * @brief This internal API enables the selected sensor/features. - * - * @param[in] sensor_sel : Selects the desired sensor. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t sensor_enable(uint64_t sensor_sel, struct bmi2_dev *dev); - -/*! - * @brief This internal API disables the selected sensor/features. - * - * @param[in] sensor_sel : Selects the desired sensor. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t sensor_disable(uint64_t sensor_sel, struct bmi2_dev *dev); - -/*! - * @brief This internal API selects the sensors/features to be enabled or - * disabled. - * - * @param[in] sens_list : Pointer to select the sensor. - * @param[in] n_sens : Number of sensors selected. - * @param[out] sensor_sel : Gets the selected sensor. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t select_sensor(const uint8_t *sens_list, uint8_t n_sens, uint64_t *sensor_sel); - -/*! - * @brief This internal API is used to enable/disable any-motion feature. - * - * @param[in] dev : Structure instance of bmi2_dev. - * @param[in] enable : Enables/Disables any-motion. - * - * Enable | Description - * -------------|--------------- - * BMI2_DISABLE | Disables any-motion. - * BMI2_ENABLE | Enables any-motion. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_any_motion(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to enable/disable no-motion feature. - * - * @param[in] dev : Structure instance of bmi2_dev. - * @param[in] enable : Enables/Disables no-motion. - * - * Enable | Description - * -------------|--------------- - * BMI2_DISABLE | Disables no-motion. - * BMI2_ENABLE | Enables no-motion. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_no_motion(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to enable/disable sig-motion feature. - * - * @param[in] dev : Structure instance of bmi2_dev. - * @param[in] enable : Enables/Disables sig-motion. - * - * Enable | Description - * -------------|--------------- - * BMI2_DISABLE | Disables sig-motion. - * BMI2_ENABLE | Enables sig-motion. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_sig_motion(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to enable/disable step detector feature. - * - * @param[in] dev : Structure instance of bmi2_dev. - * @param[in] enable : Enables/Disables step-detector. - * - * Enable | Description - * -------------|--------------- - * BMI2_DISABLE | Disables step detector - * BMI2_ENABLE | Enables step detector - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_step_detector(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to enable/disable step counter feature. - * - * @param[in] dev : Structure instance of bmi2_dev. - * @param[in] enable : Enables/Disables step counter. - * - * Enable | Description - * -------------|--------------- - * BMI2_DISABLE | Disables step counter - * BMI2_ENABLE | Enables step counter - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_step_counter(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to enable/disable step activity detection. - * - * @param[in] dev : Structure instance of bmi2_dev. - * @param[in] enable : Enables/Disables step activity. - * - * Enable | Description - * -------------|--------------- - * BMI2_DISABLE | Disables step activity - * BMI2_ENABLE | Enables step activity - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_step_activity(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API gives an option to enable offset correction - * feature of gyroscope, either internally or by the host. - * - * @param[in] enable : Enables/Disables self-offset correction. - * @param[in] dev : Structure instance of bmi2_dev. - * - * enable | Description - * -------------|--------------- - * BMI2_ENABLE | gyroscope offset correction values are set internally - * BMI2_DISABLE | gyroscope offset correction values has to be set by host - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_gyro_self_offset_corr(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to enable/disable gyroscope user gain - * feature. - * - * @param[in] dev : Structure instance of bmi2_dev. - * @param[in] enable : Enables/Disables gyroscope user gain. - * - * Enable | Description - * -------------|--------------- - * BMI2_DISABLE | Disables gyroscope user gain - * BMI2_ENABLE | Enables gyroscope user gain - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_gyro_user_gain(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API enables the wrist gesture feature. - * - * @param[in] dev : Structure instance of bmi2_dev. - * @param[in] enable : Enables/Disables wrist gesture. - * - * Enable | Description - * -------------|--------------- - * BMI2_DISABLE | Disables wrist gesture - * BMI2_ENABLE | Enables wrist gesture - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_wrist_gesture(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API enables the wrist wear wake up feature. - * - * @param[in] dev : Structure instance of bmi2_dev. - * @param[in] enable : Enables/Disables wrist wear wake up. - * - * Enable | Description - * -------------|--------------- - * BMI2_DISABLE | Disables wrist wear wake up - * BMI2_ENABLE | Enables wrist wear wake up - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_wrist_wear_wake_up(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API sets any-motion configurations like axes select, - * duration, threshold and output-configuration. - * - * @param[in] config : Structure instance of bmi2_any_motion_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @verbatim - *---------------------------------------------------------------------------- - * bmi2_any_motion_config | - * Structure parameters | Description - *--------------------------|-------------------------------------------------- - * | Defines the number of consecutive data points for - * | which the threshold condition must be respected, - * | for interrupt assertion. It is expressed in 50 Hz - * duration | samples (20 msec). - * | Range is 0 to 163sec. - * | Default value is 5 = 100ms. - * -------------------------|--------------------------------------------------- - * | Slope threshold value for in 5.11g format. - * threshold | Range is 0 to 1g. - * | Default value is 0xAA = 83mg. - * -------------------------|--------------------------------------------------- - * x_sel, y_sel, z_sel | Selects the feature on a per-axis basis - * -------------------------|--------------------------------------------------- - * @endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_any_motion_config(const struct bmi2_any_motion_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API sets no-motion configurations like axes select, - * duration, threshold and output-configuration. - * - * @param[in] config : Structure instance of bmi2_no_motion_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @verbatim - *---------------------------------------------------------------------------- - * bmi2_no_motion_config | - * Structure parameters | Description - *--------------------------|-------------------------------------------------- - * | Defines the number of consecutive data points for - * | which the threshold condition must be respected, - * | for interrupt assertion. It is expressed in 50 Hz - * duration | samples (20 msec). - * | Range is 0 to 163sec. - * | Default value is 5 = 100ms. - * -------------------------|--------------------------------------------------- - * | Slope threshold value for in 5.11g format. - * threshold | Range is 0 to 1g. - * | Default value is 0xAA = 83mg. - * -------------------------|--------------------------------------------------- - * x_sel, y_sel, z_sel | Selects the feature on a per-axis basis - * -------------------------|--------------------------------------------------- - * @endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_no_motion_config(const struct bmi2_no_motion_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API sets sig-motion configurations like block-size, - * output-configuration and other parameters. - * - * @param[in] config : Structure instance of bmi2_sig_motion_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - *---------------------------------------------------------------------------- - * bmi2_sig_motion_config | - * Structure parameters | Description - * -------------------------|--------------------------------------------------- - * | Defines the duration after which the significant - * block_size | motion interrupt is triggered. It is expressed in - * | 50 Hz samples (20 ms). Default value is 0xFA=5sec. - *--------------------------|--------------------------------------------------- - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_sig_motion_config(const struct bmi2_sig_motion_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API sets step counter parameter configurations. - * - * @param[in] step_count_params : Array that stores parameters 1 to 25. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_step_count_params_config(const uint16_t *step_count_params, struct bmi2_dev *dev); - -/*! - * @brief This internal API sets step counter/detector/activity configurations. - * - * @param[in] config : Structure instance of bmi2_step_config. - * @param[in] dev : Structure instance of bmi2_dev. - * - *--------------------------------------------------------------------------- - * bmi2_step_config | - * Structure parameters | Description - *--------------------------|-------------------------------------------------- - * | The Step-counter will trigger output every time - * | the number of steps are counted. Holds implicitly - * water-mark level | a 20x factor, so the range is 0 to 10230, - * | with resolution of 20 steps. - * -------------------------|--------------------------------------------------- - * reset counter | Flag to reset the counted steps. - * -------------------------|--------------------------------------------------- - * @endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_step_config(const struct bmi2_step_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API sets wrist gesture configurations like wearable-arm, - * and output-configuration. - * - * @param[in] config : Structure instance of bmi2_wrist_gest_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @verbatim - *----------------------------------------------------------------------------- - * bmi2_wrist_gest_config | - * Structure parameters | Description - *--------------------------|-------------------------------------------------- - * | Device in left (0) or right (1) arm. By default, - * wear_arm | the wearable device is assumed to be in left arm - * | i.e. default value is 0. - * -------------------------|--------------------------------------------------- - * @endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_wrist_gest_config(const struct bmi2_wrist_gest_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API sets wrist wear wake-up configurations like - * output-configuration. - * - * @param[in] config : Structure instance of - * bmi2_wrist_wear_wake_up_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @verbatim - *----------------------------------------------------------------------------- - * bmi2_wrist_wear_wake_up_config | - * Structure parameters | Description - *----------------------------------|------------------------------------------- - * | To set the wrist wear wake-up parameters like - * | min_angle_focus, min_angle_nonfocus, - * wrist_wear_wakeup_params | angle_landscape_left, angle_landscape_right, - * | angle_potrait_up and down. - * ---------------------------------|------------------------------------------- - * @endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_wrist_wear_wake_up_config(const struct bmi2_wrist_wear_wake_up_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets any-motion configurations like axes select, - * duration, threshold and output-configuration. - * - * @param[out] config : Structure instance of bmi2_any_motion_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @verbatim - *---------------------------------------------------------------------------- - * bmi2_any_motion_config | - * Structure parameters | Description - *--------------------------|-------------------------------------------------- - * | Defines the number of consecutive data points for - * | which the threshold condition must be respected, - * | for interrupt assertion. It is expressed in 50 Hz - * duration | samples (20 msec). - * | Range is 0 to 163sec. - * | Default value is 5 = 100ms. - * -------------------------|--------------------------------------------------- - * | Slope threshold value for in 5.11g format. - * threshold | Range is 0 to 1g. - * | Default value is 0xAA = 83mg. - * -------------------------|--------------------------------------------------- - * x_sel, y_sel, z_sel | Selects the feature on a per-axis basis - * -------------------------|--------------------------------------------------- - * @endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t get_any_motion_config(struct bmi2_any_motion_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets no-motion configurations like axes select, - * duration, threshold and output-configuration. - * - * @param[out] config : Structure instance of bmi2_no_motion_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @verbatim - *---------------------------------------------------------------------------- - * bmi2_no_motion_config | - * Structure parameters | Description - *--------------------------|-------------------------------------------------- - * | Defines the number of consecutive data points for - * | which the threshold condition must be respected, - * | for interrupt assertion. It is expressed in 50 Hz - * duration | samples (20 msec). - * | Range is 0 to 163sec. - * | Default value is 5 = 100ms. - * -------------------------|--------------------------------------------------- - * | Slope threshold value for in 5.11g format. - * threshold | Range is 0 to 1g. - * | Default value is 0xAA = 83mg. - * -------------------------|--------------------------------------------------- - * x_sel, y_sel, z_sel | Selects the feature on a per-axis basis - * -------------------------|--------------------------------------------------- - * @endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t get_no_motion_config(struct bmi2_no_motion_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets sig-motion configurations like block-size, - * output-configuration and other parameters. - * - * @param[out] config : Structure instance of bmi2_sig_motion_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - *---------------------------------------------------------------------------- - * bmi2_sig_motion_config | - * Structure parameters | Description - * -------------------------|--------------------------------------------------- - * | Defines the duration after which the significant - * block_size | motion interrupt is triggered. It is expressed in - * | 50 Hz samples (20 ms). Default value is 0xFA=5sec. - *--------------------------|--------------------------------------------------- - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t get_sig_motion_config(struct bmi2_sig_motion_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets step counter parameter configurations. - * - * @param[in] step_count_params : Array that stores parameters 1 to 25. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t get_step_count_params_config(uint16_t *step_count_params, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets step counter/detector/activity configurations. - * - * @param[out] config : Structure instance of bmi2_step_config. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @verbatim - *---------------------------------------------------------------------------- - * bmi2_step_config | - * Structure parameters | Description - *--------------------------|-------------------------------------------------- - * | The Step-counter will trigger output every time - * | the number of steps are counted. Holds implicitly - * water-mark level | a 20x factor, so the range is 0 to 10230, - * | with resolution of 20 steps. - * -------------------------|--------------------------------------------------- - * reset counter | Flag to reset the counted steps. - * -------------------------|--------------------------------------------------- - * @endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t get_step_config(struct bmi2_step_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets wrist gesture configurations like wearable-arm, - * and output-configuration. - * - * @param[out] config : Structure instance of bmi2_wrist_gest_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @verbatim - *----------------------------------------------------------------------------- - * bmi2_wrist_gest_config | - * Structure parameters | Description - *--------------------------|-------------------------------------------------- - * | Device in left (0) or right (1) arm. By default, - * wear_arm | the wearable device is assumed to be in left arm - * | i.e. default value is 0. - * -------------------------|--------------------------------------------------- - * @endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t get_wrist_gest_config(struct bmi2_wrist_gest_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets wrist wear wake-up configurations like - * output-configuration. - * - * @param[out] config : Structure instance of - * bmi2_wrist_wear_wake_up_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @verbatim - *------------------------------------|--------------------------------------- - * bmi2_wrist_wear_wake_up_wh_config | - * Structure parameters | Description - *------------------------------------|------------------------------------------- - * | To get the wrist wear wake-up parameters like - * | min_angle_focus, min_angle_nonfocus, - * wrist_wear_wake_params | angle_landscape_left, angle_landscape_right, - * | angle_potrait_up and down. - * -----------------------------------|------------------------------------------- - * @endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t get_wrist_wear_wake_up_config(struct bmi2_wrist_wear_wake_up_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets the output values of wrist gesture. - * - * @param[out] wrist_gest : Pointer to the stored wrist gesture. - * @param[in] dev : Structure instance of bmi2_dev. - * - * *wrist_gest | Output - * -------------|------------ - * 0x00 | UNKNOWN - * 0x01 | PUSH_ARM_DOWN - * 0x02 | PIVOT_UP - * 0x03 | WRIST_SHAKE_JIGGLE - * 0x04 | FLICK_IN - * 0x05 | FLICK_OUT - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t get_wrist_gest_status(uint8_t *wrist_gest, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets the output values of step activity. - * - * @param[out] step_act : Pointer to the stored step activity data. - * @param[in] dev : Structure instance of bmi2_dev. - * - * *step_act | Output - * -----------|------------ - * 0x00 | STILL - * 0x01 | WALKING - * 0x02 | RUNNING - * 0x03 | UNKNOWN - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fails - */ -static int8_t get_step_activity_output(uint8_t *step_act, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets the output values of step counter. - * - * @param[out] step_count : Pointer to the stored step counter data. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t get_step_counter_output(uint32_t *step_count, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets the error status related to NVM. - * - * @param[out] nvm_err_stat : Stores the NVM error status. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t get_nvm_error_status(struct bmi2_nvm_err_status *nvm_err_stat, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets the error status related to virtual frames. - * - * @param[out] vfrm_err_stat : Stores the VFRM related error status. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t get_vfrm_error_status(struct bmi2_vfrm_err_status *vfrm_err_stat, struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to get enable status of gyroscope user gain - * update. - * - * @param[out] status : Stores status of gyroscope user gain update. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t get_user_gain_upd_status(uint8_t *status, struct bmi2_dev *dev); - -/*! - * @brief This internal API enables/disables compensation of the gain defined - * in the GAIN register. - * - * @param[in] enable : Enables/Disables gain compensation - * @param[in] dev : Structure instance of bmi2_dev. - * - * enable | Description - * -------------|--------------- - * BMI2_ENABLE | Enable gain compensation. - * BMI2_DISABLE | Disable gain compensation. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t enable_gyro_gain(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to extract the output feature configuration - * details like page and start address from the look-up table. - * - * @param[out] feat_output : Structure that stores output feature - * configurations. - * @param[in] type : Type of feature or sensor. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Returns the feature found flag. - * - * @retval BMI2_FALSE : Feature not found - * BMI2_TRUE : Feature found - */ -static uint8_t extract_output_feat_config(struct bmi2_feature_config *feat_output, - uint8_t type, - const struct bmi2_dev *dev); - -/***************************************************************************/ - -/*! User Interface Definitions - ****************************************************************************/ - -/*! - * @brief This API: - * 1) updates the device structure with address of the configuration file. - * 2) Initializes BMI270 sensor. - * 3) Writes the configuration file. - * 4) Updates the feature offset parameters in the device structure. - * 5) Updates the maximum number of pages, in the device structure. - */ -int8_t bmi270_init(struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if (rslt == BMI2_OK) - { - /* Assign chip id of BMI270 */ - dev->chip_id = BMI270_CHIP_ID; - - /* get the size of config array */ - dev->config_size = sizeof(bmi270_config_file); - - /* Enable the variant specific features if any */ - dev->variant_feature = BMI2_GYRO_CROSS_SENS_ENABLE | BMI2_CRT_RTOSK_ENABLE; - - /* An extra dummy byte is read during SPI read */ - if (dev->intf == BMI2_SPI_INTF) - { - dev->dummy_byte = 1; - } - else - { - dev->dummy_byte = 0; - } - - /* If configuration file pointer is not assigned any address */ - if (!dev->config_file_ptr) - { - /* Give the address of the configuration file array to - * the device pointer - */ - dev->config_file_ptr = bmi270_config_file; - } - - /* Initialize BMI2 sensor */ - rslt = bmi2_sec_init(dev); - if (rslt == BMI2_OK) - { - /* Assign the offsets of the feature input - * configuration to the device structure - */ - dev->feat_config = bmi270_feat_in; - - /* Assign the offsets of the feature output to - * the device structure - */ - dev->feat_output = bmi270_feat_out; - - /* Assign the maximum number of pages to the - * device structure - */ - dev->page_max = BMI270_MAX_PAGE_NUM; - - /* Assign maximum number of input sensors/ - * features to device structure - */ - dev->input_sens = BMI270_MAX_FEAT_IN; - - /* Assign maximum number of output sensors/ - * features to device structure - */ - dev->out_sens = BMI270_MAX_FEAT_OUT; - - /* Assign the offsets of the feature interrupt - * to the device structure - */ - dev->map_int = bmi270_map_int; - - /* Assign maximum number of feature interrupts - * to device structure - */ - dev->sens_int_map = BMI270_MAX_INT_MAP; - - /* Get the gyroscope cross axis sensitivity */ - rslt = bmi2_get_gyro_cross_sense(dev); - } - } - - return rslt; -} - -/*! - * @brief This API selects the sensors/features to be enabled. - */ -int8_t bmi270_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to select sensor */ - uint64_t sensor_sel = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (sens_list != NULL)) - { - /* Get the selected sensors */ - rslt = select_sensor(sens_list, n_sens, &sensor_sel); - if (rslt == BMI2_OK) - { - /* Enable the selected sensors */ - rslt = sensor_enable(sensor_sel, dev); - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API selects the sensors/features to be disabled. - */ -int8_t bmi270_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to select sensor */ - uint64_t sensor_sel = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (sens_list != NULL)) - { - /* Get the selected sensors */ - rslt = select_sensor(sens_list, n_sens, &sensor_sel); - if (rslt == BMI2_OK) - { - /* Disable the selected sensors */ - rslt = sensor_disable(sensor_sel, dev); - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API sets the sensor/feature configuration. - */ -int8_t bmi270_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define loop */ - uint8_t loop; - - /* Variable to get the status of advance power save */ - uint8_t aps_stat = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (sens_cfg != NULL)) - { - /* Get status of advance power save mode */ - aps_stat = dev->aps_status; - - for (loop = 0; loop < n_sens; loop++) - { - if ((sens_cfg[loop].type == BMI2_ACCEL) || (sens_cfg[loop].type == BMI2_GYRO) || - (sens_cfg[loop].type == BMI2_AUX) || (sens_cfg[loop].type == BMI2_GYRO_GAIN_UPDATE)) - { - rslt = bmi2_set_sensor_config(&sens_cfg[loop], 1, dev); - } - else - { - /* Disable Advance power save if enabled for auxiliary - * and feature configurations - */ - if (aps_stat == BMI2_ENABLE) - { - /* Disable advance power save if - * enabled - */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - } - - if (rslt == BMI2_OK) - { - switch (sens_cfg[loop].type) - { - /* Set any motion configuration */ - case BMI2_ANY_MOTION: - rslt = set_any_motion_config(&sens_cfg[loop].cfg.any_motion, dev); - break; - - /* Set no motion configuration */ - case BMI2_NO_MOTION: - rslt = set_no_motion_config(&sens_cfg[loop].cfg.no_motion, dev); - break; - - /* Set sig-motion configuration */ - case BMI2_SIG_MOTION: - rslt = set_sig_motion_config(&sens_cfg[loop].cfg.sig_motion, dev); - break; - - /* Set the step counter parameters */ - case BMI2_STEP_COUNTER_PARAMS: - rslt = set_step_count_params_config(sens_cfg[loop].cfg.step_counter_params, dev); - break; - - /* Set step counter/detector/activity configuration */ - case BMI2_STEP_DETECTOR: - case BMI2_STEP_COUNTER: - case BMI2_STEP_ACTIVITY: - rslt = set_step_config(&sens_cfg[loop].cfg.step_counter, dev); - break; - - /* Set the wrist gesture configuration */ - case BMI2_WRIST_GESTURE: - rslt = set_wrist_gest_config(&sens_cfg[loop].cfg.wrist_gest, dev); - break; - - /* Set the wrist wear wake-up configuration */ - case BMI2_WRIST_WEAR_WAKE_UP: - rslt = set_wrist_wear_wake_up_config(&sens_cfg[loop].cfg.wrist_wear_wake_up, dev); - break; - - default: - rslt = BMI2_E_INVALID_SENSOR; - break; - } - } - - /* Return error if any of the set configurations fail */ - if (rslt != BMI2_OK) - { - break; - } - } - } - - /* Enable Advance power save if disabled while configuring and - * not when already disabled - */ - if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) - { - rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API gets the sensor/feature configuration. - */ -int8_t bmi270_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define loop */ - uint8_t loop; - - /* Variable to get the status of advance power save */ - uint8_t aps_stat = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (sens_cfg != NULL)) - { - /* Get status of advance power save mode */ - aps_stat = dev->aps_status; - for (loop = 0; loop < n_sens; loop++) - { - if ((sens_cfg[loop].type == BMI2_ACCEL) || (sens_cfg[loop].type == BMI2_GYRO) || - (sens_cfg[loop].type == BMI2_AUX) || (sens_cfg[loop].type == BMI2_GYRO_GAIN_UPDATE)) - { - rslt = bmi2_get_sensor_config(&sens_cfg[loop], 1, dev); - } - else - { - /* Disable Advance power save if enabled for auxiliary - * and feature configurations - */ - if ((sens_cfg[loop].type >= BMI2_MAIN_SENS_MAX_NUM) || (sens_cfg[loop].type == BMI2_AUX)) - { - - if (aps_stat == BMI2_ENABLE) - { - /* Disable advance power save if - * enabled - */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - } - } - - if (rslt == BMI2_OK) - { - switch (sens_cfg[loop].type) - { - /* Get sig-motion configuration */ - case BMI2_SIG_MOTION: - rslt = get_sig_motion_config(&sens_cfg[loop].cfg.sig_motion, dev); - break; - - /* Get any motion configuration */ - case BMI2_ANY_MOTION: - rslt = get_any_motion_config(&sens_cfg[loop].cfg.any_motion, dev); - break; - - /* Get no motion configuration */ - case BMI2_NO_MOTION: - rslt = get_no_motion_config(&sens_cfg[loop].cfg.no_motion, dev); - break; - - /* Set the step counter parameters */ - case BMI2_STEP_COUNTER_PARAMS: - rslt = get_step_count_params_config(sens_cfg[loop].cfg.step_counter_params, dev); - break; - - /* Get step counter/detector/activity configuration */ - case BMI2_STEP_DETECTOR: - case BMI2_STEP_COUNTER: - case BMI2_STEP_ACTIVITY: - rslt = get_step_config(&sens_cfg[loop].cfg.step_counter, dev); - break; - - /* Get the wrist gesture configuration */ - case BMI2_WRIST_GESTURE: - rslt = get_wrist_gest_config(&sens_cfg[loop].cfg.wrist_gest, dev); - break; - - /* Get the wrist wear wake-up configuration */ - case BMI2_WRIST_WEAR_WAKE_UP: - rslt = get_wrist_wear_wake_up_config(&sens_cfg[loop].cfg.wrist_wear_wake_up, dev); - break; - - default: - rslt = BMI2_E_INVALID_SENSOR; - break; - } - } - - /* Return error if any of the get configurations fail */ - if (rslt != BMI2_OK) - { - break; - } - } - } - - /* Enable Advance power save if disabled while configuring and - * not when already disabled - */ - if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) - { - rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API gets the sensor/feature data for accelerometer, gyroscope, - * auxiliary sensor, step counter, high-g, gyroscope user-gain update, - * orientation, gyroscope cross sensitivity and error status for NVM and VFRM. - */ -int8_t bmi270_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define loop */ - uint8_t loop; - - /* Variable to get the status of advance power save */ - uint8_t aps_stat = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (sensor_data != NULL)) - { - /* Get status of advance power save mode */ - aps_stat = dev->aps_status; - for (loop = 0; loop < n_sens; loop++) - { - if ((sensor_data[loop].type == BMI2_ACCEL) || (sensor_data[loop].type == BMI2_GYRO) || - (sensor_data[loop].type == BMI2_AUX) || (sensor_data[loop].type == BMI2_GYRO_GAIN_UPDATE) || - (sensor_data[loop].type == BMI2_GYRO_CROSS_SENSE)) - { - rslt = bmi2_get_sensor_data(&sensor_data[loop], 1, dev); - } - else - { - /* Disable Advance power save if enabled for feature - * configurations - */ - if (sensor_data[loop].type >= BMI2_MAIN_SENS_MAX_NUM) - { - if (aps_stat == BMI2_ENABLE) - { - /* Disable advance power save if - * enabled - */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - } - } - - if (rslt == BMI2_OK) - { - switch (sensor_data[loop].type) - { - case BMI2_STEP_COUNTER: - - /* Get step counter output */ - rslt = get_step_counter_output(&sensor_data[loop].sens_data.step_counter_output, dev); - break; - case BMI2_STEP_ACTIVITY: - - /* Get step activity output */ - rslt = get_step_activity_output(&sensor_data[loop].sens_data.activity_output, dev); - break; - case BMI2_NVM_STATUS: - - /* Get NVM error status */ - rslt = get_nvm_error_status(&sensor_data[loop].sens_data.nvm_status, dev); - break; - case BMI2_VFRM_STATUS: - - /* Get VFRM error status */ - rslt = get_vfrm_error_status(&sensor_data[loop].sens_data.vfrm_status, dev); - break; - case BMI2_WRIST_GESTURE: - - /* Get wrist gesture status */ - rslt = get_wrist_gest_status(&sensor_data[loop].sens_data.wrist_gesture_output, dev); - break; - default: - rslt = BMI2_E_INVALID_SENSOR; - break; - } - - /* Return error if any of the get sensor data fails */ - if (rslt != BMI2_OK) - { - break; - } - } - } - - /* Enable Advance power save if disabled while - * configuring and not when already disabled - */ - if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) - { - rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - } - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API updates the gyroscope user-gain. - */ -int8_t bmi270_update_gyro_user_gain(const struct bmi2_gyro_user_gain_config *user_gain, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to select sensor */ - uint8_t sens_sel[2] = { BMI2_GYRO, BMI2_GYRO_GAIN_UPDATE }; - - /* Structure to define sensor configurations */ - struct bmi2_sens_config sens_cfg; - - /* Variable to store status of user-gain update module */ - uint8_t status = 0; - - /* Variable to define count */ - uint8_t count = 100; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (user_gain != NULL)) - { - /* Select type of feature */ - sens_cfg.type = BMI2_GYRO_GAIN_UPDATE; - - /* Get the user gain configurations */ - rslt = bmi270_get_sensor_config(&sens_cfg, 1, dev); - if (rslt == BMI2_OK) - { - /* Get the user-defined ratio */ - sens_cfg.cfg.gyro_gain_update = *user_gain; - - /* Set rate ratio for all axes */ - rslt = bmi270_set_sensor_config(&sens_cfg, 1, dev); - } - - /* Disable gyroscope */ - if (rslt == BMI2_OK) - { - rslt = bmi270_sensor_disable(&sens_sel[0], 1, dev); - } - - /* Enable gyroscope user-gain update module */ - if (rslt == BMI2_OK) - { - rslt = bmi270_sensor_enable(&sens_sel[1], 1, dev); - } - - /* Set the command to trigger the computation */ - if (rslt == BMI2_OK) - { - rslt = bmi2_set_command_register(BMI2_USR_GAIN_CMD, dev); - } - - if (rslt == BMI2_OK) - { - /* Poll until enable bit of user-gain update is 0 */ - while (count--) - { - rslt = get_user_gain_upd_status(&status, dev); - if ((rslt == BMI2_OK) && (status == 0)) - { - /* Enable compensation of gain defined - * in the GAIN register - */ - rslt = enable_gyro_gain(BMI2_ENABLE, dev); - - /* Enable gyroscope */ - if (rslt == BMI2_OK) - { - rslt = bmi270_sensor_enable(&sens_sel[0], 1, dev); - } - - break; - } - - dev->delay_us(10000, dev->intf_ptr); - } - - /* Return error if user-gain update is failed */ - if ((rslt == BMI2_OK) && (status != 0)) - { - rslt = BMI2_E_GYR_USER_GAIN_UPD_FAIL; - } - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API reads the compensated gyroscope user-gain values. - */ -int8_t bmi270_read_gyro_user_gain(struct bmi2_gyro_user_gain_data *gyr_usr_gain, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define register data */ - uint8_t reg_data[3] = { 0 }; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (gyr_usr_gain != NULL)) - { - /* Get the gyroscope compensated gain values */ - rslt = bmi2_get_regs(BMI2_GYR_USR_GAIN_0_ADDR, reg_data, 3, dev); - if (rslt == BMI2_OK) - { - /* Gyroscope user gain correction X-axis */ - gyr_usr_gain->x = (int8_t)BMI2_GET_BIT_POS0(reg_data[0], BMI2_GYR_USR_GAIN_X); - - /* Gyroscope user gain correction Y-axis */ - gyr_usr_gain->y = (int8_t)BMI2_GET_BIT_POS0(reg_data[1], BMI2_GYR_USR_GAIN_Y); - - /* Gyroscope user gain correction z-axis */ - gyr_usr_gain->z = (int8_t)BMI2_GET_BIT_POS0(reg_data[2], BMI2_GYR_USR_GAIN_Z); - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API maps/unmaps feature interrupts to that of interrupt pins. - */ -int8_t bmi270_map_feat_int(const struct bmi2_sens_int_config *sens_int, uint8_t n_sens, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define loop */ - uint8_t loop; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (sens_int != NULL)) - { - for (loop = 0; loop < n_sens; loop++) - { - switch (sens_int[loop].type) - { - case BMI2_SIG_MOTION: - case BMI2_WRIST_GESTURE: - case BMI2_ANY_MOTION: - case BMI2_NO_MOTION: - case BMI2_STEP_COUNTER: - case BMI2_STEP_DETECTOR: - case BMI2_STEP_ACTIVITY: - case BMI2_WRIST_WEAR_WAKE_UP: - - rslt = bmi2_map_feat_int(sens_int[loop].type, sens_int[loop].hw_int_pin, dev); - break; - default: - rslt = BMI2_E_INVALID_SENSOR; - break; - } - - /* Return error if interrupt mapping fails */ - if (rslt != BMI2_OK) - { - break; - } - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/***************************************************************************/ - -/*! Local Function Definitions - ****************************************************************************/ - -/*! - * @brief This internal API is used to validate the device structure pointer for - * null conditions. - */ -static int8_t null_ptr_check(const struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt = BMI2_OK; - - if ((dev == NULL) || (dev->read == NULL) || (dev->write == NULL) || (dev->delay_us == NULL)) - { - /* Device structure pointer is not valid */ - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This internal API selects the sensor/features to be enabled or - * disabled. - */ -static int8_t select_sensor(const uint8_t *sens_list, uint8_t n_sens, uint64_t *sensor_sel) -{ - /* Variable to define error */ - int8_t rslt = BMI2_OK; - - /* Variable to define loop */ - uint8_t count; - - for (count = 0; count < n_sens; count++) - { - switch (sens_list[count]) - { - case BMI2_ACCEL: - *sensor_sel |= BMI2_ACCEL_SENS_SEL; - break; - case BMI2_GYRO: - *sensor_sel |= BMI2_GYRO_SENS_SEL; - break; - case BMI2_AUX: - *sensor_sel |= BMI2_AUX_SENS_SEL; - break; - case BMI2_TEMP: - *sensor_sel |= BMI2_TEMP_SENS_SEL; - break; - case BMI2_SIG_MOTION: - *sensor_sel |= BMI2_SIG_MOTION_SEL; - break; - case BMI2_ANY_MOTION: - *sensor_sel |= BMI2_ANY_MOT_SEL; - break; - case BMI2_NO_MOTION: - *sensor_sel |= BMI2_NO_MOT_SEL; - break; - case BMI2_STEP_DETECTOR: - *sensor_sel |= BMI2_STEP_DETECT_SEL; - break; - case BMI2_STEP_COUNTER: - *sensor_sel |= BMI2_STEP_COUNT_SEL; - break; - case BMI2_STEP_ACTIVITY: - *sensor_sel |= BMI2_STEP_ACT_SEL; - break; - case BMI2_GYRO_GAIN_UPDATE: - *sensor_sel |= BMI2_GYRO_GAIN_UPDATE_SEL; - break; - case BMI2_GYRO_SELF_OFF: - *sensor_sel |= BMI2_GYRO_SELF_OFF_SEL; - break; - case BMI2_WRIST_GESTURE: - *sensor_sel |= BMI2_WRIST_GEST_SEL; - break; - case BMI2_WRIST_WEAR_WAKE_UP: - *sensor_sel |= BMI2_WRIST_WEAR_WAKE_UP_SEL; - break; - default: - rslt = BMI2_E_INVALID_SENSOR; - break; - } - } - - return rslt; -} - -/*! - * @brief This internal API enables the selected sensor/features. - */ -static int8_t sensor_enable(uint64_t sensor_sel, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to store register values */ - uint8_t reg_data = 0; - - /* Variable to define loop */ - uint8_t loop = 1; - - /* Variable to get the status of advance power save */ - uint8_t aps_stat = 0; - - rslt = bmi2_get_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); - if (rslt == BMI2_OK) - { - /* Enable accelerometer */ - if (sensor_sel & BMI2_ACCEL_SENS_SEL) - { - reg_data = BMI2_SET_BITS(reg_data, BMI2_ACC_EN, BMI2_ENABLE); - } - - /* Enable gyroscope */ - if (sensor_sel & BMI2_GYRO_SENS_SEL) - { - reg_data = BMI2_SET_BITS(reg_data, BMI2_GYR_EN, BMI2_ENABLE); - } - - /* Enable auxiliary sensor */ - if (sensor_sel & BMI2_AUX_SENS_SEL) - { - reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_AUX_EN, BMI2_ENABLE); - } - - /* Enable temperature sensor */ - if (sensor_sel & BMI2_TEMP_SENS_SEL) - { - reg_data = BMI2_SET_BITS(reg_data, BMI2_TEMP_EN, BMI2_ENABLE); - } - - /* Enable the sensors that are set in the power control register */ - if (sensor_sel & BMI2_MAIN_SENSORS) - { - rslt = bmi2_set_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); - } - } - - if ((rslt == BMI2_OK) && (sensor_sel & ~(BMI2_MAIN_SENSORS))) - { - /* Get status of advance power save mode */ - aps_stat = dev->aps_status; - if (aps_stat == BMI2_ENABLE) - { - /* Disable advance power save if enabled */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - } - - if (rslt == BMI2_OK) - { - while (loop--) - { - /* Enable sig-motion feature */ - if (sensor_sel & BMI2_SIG_MOTION_SEL) - { - rslt = set_sig_motion(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_SIG_MOTION_SEL; - } - else - { - break; - } - } - - /* Enable any motion feature */ - if (sensor_sel & BMI2_ANY_MOT_SEL) - { - rslt = set_any_motion(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_ANY_MOT_SEL; - } - else - { - break; - } - } - - /* Enable no motion feature */ - if (sensor_sel & BMI2_NO_MOT_SEL) - { - rslt = set_no_motion(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_NO_MOT_SEL; - } - else - { - break; - } - } - - /* Enable step detector feature */ - if (sensor_sel & BMI2_STEP_DETECT_SEL) - { - rslt = set_step_detector(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_STEP_DETECT_SEL; - } - else - { - break; - } - } - - /* Enable step counter feature */ - if (sensor_sel & BMI2_STEP_COUNT_SEL) - { - rslt = set_step_counter(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_STEP_COUNT_SEL; - } - else - { - break; - } - } - - /* Enable step activity feature */ - if (sensor_sel & BMI2_STEP_ACT_SEL) - { - rslt = set_step_activity(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_STEP_ACT_SEL; - } - else - { - break; - } - } - - /* Enable gyroscope user gain */ - if (sensor_sel & BMI2_GYRO_GAIN_UPDATE_SEL) - { - rslt = set_gyro_user_gain(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_GYRO_GAIN_UPDATE_SEL; - } - else - { - break; - } - } - - /* Enable gyroscope self-offset correction feature */ - if (sensor_sel & BMI2_GYRO_SELF_OFF_SEL) - { - rslt = set_gyro_self_offset_corr(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_GYRO_SELF_OFF_SEL; - } - else - { - break; - } - } - - /* Enable wrist gesture feature for wearable variant */ - if (sensor_sel & BMI2_WRIST_GEST_SEL) - { - rslt = set_wrist_gesture(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_WRIST_GEST_SEL; - } - else - { - break; - } - } - - /* Enable wrist wear wake-up feature */ - if (sensor_sel & BMI2_WRIST_WEAR_WAKE_UP_SEL) - { - rslt = set_wrist_wear_wake_up(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_WRIST_WEAR_WAKE_UP_SEL; - } - else - { - break; - } - } - } - - /* Enable Advance power save if disabled while - * configuring and not when already disabled - */ - if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) - { - rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - } - } - } - - return rslt; -} - -/*! - * @brief This internal API disables the selected sensors/features. - */ -static int8_t sensor_disable(uint64_t sensor_sel, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to store register values */ - uint8_t reg_data = 0; - - /* Variable to define loop */ - uint8_t loop = 1; - - /* Variable to get the status of advance power save */ - uint8_t aps_stat = 0; - - rslt = bmi2_get_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); - if (rslt == BMI2_OK) - { - /* Disable accelerometer */ - if (sensor_sel & BMI2_ACCEL_SENS_SEL) - { - reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_ACC_EN); - } - - /* Disable gyroscope */ - if (sensor_sel & BMI2_GYRO_SENS_SEL) - { - reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_GYR_EN); - } - - /* Disable auxiliary sensor */ - if (sensor_sel & BMI2_AUX_SENS_SEL) - { - reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_AUX_EN); - } - - /* Disable temperature sensor */ - if (sensor_sel & BMI2_TEMP_SENS_SEL) - { - reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_TEMP_EN); - } - - /* Disable the sensors that are set in the power control register */ - if (sensor_sel & BMI2_MAIN_SENSORS) - { - rslt = bmi2_set_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); - } - } - - if ((rslt == BMI2_OK) && (sensor_sel & ~(BMI2_MAIN_SENSORS))) - { - /* Get status of advance power save mode */ - aps_stat = dev->aps_status; - if (aps_stat == BMI2_ENABLE) - { - /* Disable advance power save if enabled */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - } - - if (rslt == BMI2_OK) - { - while (loop--) - { - /* Disable sig-motion feature */ - if (sensor_sel & BMI2_SIG_MOTION_SEL) - { - rslt = set_sig_motion(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat &= ~BMI2_SIG_MOTION_SEL; - } - else - { - break; - } - } - - /* Disable any-motion feature */ - if (sensor_sel & BMI2_ANY_MOT_SEL) - { - rslt = set_any_motion(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat &= ~BMI2_ANY_MOT_SEL; - } - else - { - break; - } - } - - /* Disable no-motion feature */ - if (sensor_sel & BMI2_NO_MOT_SEL) - { - rslt = set_no_motion(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat &= ~BMI2_NO_MOT_SEL; - } - else - { - break; - } - } - - /* Disable step detector feature */ - if (sensor_sel & BMI2_STEP_DETECT_SEL) - { - rslt = set_step_detector(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat &= ~BMI2_STEP_DETECT_SEL; - } - else - { - break; - } - } - - /* Disable step counter feature */ - if (sensor_sel & BMI2_STEP_COUNT_SEL) - { - rslt = set_step_counter(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat &= ~BMI2_STEP_COUNT_SEL; - } - else - { - break; - } - } - - /* Disable step activity feature */ - if (sensor_sel & BMI2_STEP_ACT_SEL) - { - rslt = set_step_activity(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat &= ~BMI2_STEP_ACT_SEL; - } - else - { - break; - } - } - - /* Disable gyroscope user gain */ - if (sensor_sel & BMI2_GYRO_GAIN_UPDATE_SEL) - { - rslt = set_gyro_user_gain(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat &= ~BMI2_GYRO_GAIN_UPDATE_SEL; - } - else - { - break; - } - } - - /* Disable gyroscope self-offset correction feature */ - if (sensor_sel & BMI2_GYRO_SELF_OFF_SEL) - { - rslt = set_gyro_self_offset_corr(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat &= ~BMI2_GYRO_SELF_OFF_SEL; - } - else - { - break; - } - } - - /* Disable wrist gesture feature for wearable variant*/ - if (sensor_sel & BMI2_WRIST_GEST_SEL) - { - rslt = set_wrist_gesture(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat &= ~BMI2_WRIST_GEST_SEL; - } - else - { - break; - } - } - - /* Enable wrist wear wake-up feature */ - if (sensor_sel & BMI2_WRIST_WEAR_WAKE_UP_SEL) - { - rslt = set_wrist_wear_wake_up(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat &= ~BMI2_WRIST_WEAR_WAKE_UP_SEL; - } - else - { - break; - } - } - - /* Enable Advance power save if disabled while - * configuring and not when already disabled - */ - if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) - { - rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - } - } - } - } - - return rslt; -} - -/*! - * @brief This internal API is used to enable/disable any motion feature. - */ -static int8_t set_any_motion(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for any-motion */ - struct bmi2_feature_config any_mot_config = { 0, 0, 0 }; - - /* Search for any-motion feature and extract its configurations details */ - feat_found = bmi2_extract_input_feat_config(&any_mot_config, BMI2_ANY_MOTION, dev); - if (feat_found) - { - /* Get the configuration from the page where any-motion feature resides */ - rslt = bmi2_get_feat_config(any_mot_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of any-motion axes */ - idx = any_mot_config.start_addr + BMI2_ANY_MOT_FEAT_EN_OFFSET; - - /* Set the feature enable bit */ - feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_ANY_NO_MOT_EN, enable); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API is used to enable/disable no-motion feature. - */ -static int8_t set_no_motion(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for no-motion */ - struct bmi2_feature_config no_mot_config = { 0, 0, 0 }; - - /* Search for no-motion feature and extract its configurations details */ - feat_found = bmi2_extract_input_feat_config(&no_mot_config, BMI2_NO_MOTION, dev); - if (feat_found) - { - /* Get the configuration from the page where any/no-motion feature resides */ - rslt = bmi2_get_feat_config(no_mot_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of no-motion axes */ - idx = no_mot_config.start_addr + BMI2_NO_MOT_FEAT_EN_OFFSET; - - /* Set the feature enable bit */ - feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_ANY_NO_MOT_EN, enable); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API is used to enable/disable step detector feature. - */ -static int8_t set_step_detector(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for step detector */ - struct bmi2_feature_config step_det_config = { 0, 0, 0 }; - - /* Search for step detector feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&step_det_config, BMI2_STEP_DETECTOR, dev); - if (feat_found) - { - /* Get the configuration from the page where step detector feature resides */ - rslt = bmi2_get_feat_config(step_det_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of step detector */ - idx = step_det_config.start_addr + BMI2_STEP_COUNT_FEAT_EN_OFFSET; - - /* Set the feature enable bit */ - feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_STEP_DET_FEAT_EN, enable); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API is used to enable/disable step counter feature. - */ -static int8_t set_step_counter(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for step counter */ - struct bmi2_feature_config step_count_config = { 0, 0, 0 }; - - /* Search for step counter feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&step_count_config, BMI2_STEP_COUNTER, dev); - if (feat_found) - { - /* Get the configuration from the page where step-counter feature resides */ - rslt = bmi2_get_feat_config(step_count_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of step counter */ - idx = step_count_config.start_addr + BMI2_STEP_COUNT_FEAT_EN_OFFSET; - - /* Set the feature enable bit */ - feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_STEP_COUNT_FEAT_EN, enable); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API is used to enable/disable sig-motion feature. - */ -static int8_t set_sig_motion(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for sig-motion */ - struct bmi2_feature_config sig_mot_config = { 0, 0, 0 }; - - /* Search for sig-motion feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&sig_mot_config, BMI2_SIG_MOTION, dev); - if (feat_found) - { - /* Get the configuration from the page where sig-motion feature resides */ - rslt = bmi2_get_feat_config(sig_mot_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of sig-motion */ - idx = sig_mot_config.start_addr + BMI2_SIG_MOT_FEAT_EN_OFFSET; - - /* Set the feature enable bit */ - feat_config[idx] = BMI2_SET_BIT_POS0(feat_config[idx], BMI2_SIG_MOT_FEAT_EN, enable); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API is used to enable/disable step activity detection. - */ -static int8_t set_step_activity(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for step activity */ - struct bmi2_feature_config step_act_config = { 0, 0, 0 }; - - /* Search for step activity feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&step_act_config, BMI2_STEP_ACTIVITY, dev); - if (feat_found) - { - /* Get the configuration from the page where step-activity - * feature resides - */ - rslt = bmi2_get_feat_config(step_act_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of step activity */ - idx = step_act_config.start_addr + BMI2_STEP_COUNT_FEAT_EN_OFFSET; - - /* Set the feature enable bit */ - feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_STEP_ACT_FEAT_EN, enable); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gives an option to enable self-offset correction - * feature of gyroscope, either internally or by the host. - */ -static int8_t set_gyro_self_offset_corr(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for self-offset correction */ - struct bmi2_feature_config self_off_corr_cfg = { 0, 0, 0 }; - - /* Search for self-offset correction and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&self_off_corr_cfg, BMI2_GYRO_SELF_OFF, dev); - if (feat_found) - { - /* Get the configuration from the page where self-offset - * correction feature resides - */ - rslt = bmi2_get_feat_config(self_off_corr_cfg.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of self-offset correction */ - idx = self_off_corr_cfg.start_addr; - - /* Set the feature enable bit */ - feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_GYR_SELF_OFF_CORR_FEAT_EN, enable); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API enables the wrist gesture feature. - */ -static int8_t set_wrist_gesture(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for wrist gesture */ - struct bmi2_feature_config wrist_gest_cfg = { 0, 0, 0 }; - - /* Search for wrist gesture and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&wrist_gest_cfg, BMI2_WRIST_GESTURE, dev); - if (feat_found) - { - /* Get the configuration from the page where wrist gesture feature resides */ - rslt = bmi2_get_feat_config(wrist_gest_cfg.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of wrist gesture */ - idx = wrist_gest_cfg.start_addr; - - /* Set the feature enable bit */ - feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_WRIST_GEST_FEAT_EN, enable); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API enables the wrist wear wake up feature. - */ -static int8_t set_wrist_wear_wake_up(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for wrist wear wake up */ - struct bmi2_feature_config wrist_wake_up_cfg = { 0, 0, 0 }; - - /* Search for wrist wear wake up and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&wrist_wake_up_cfg, BMI2_WRIST_WEAR_WAKE_UP, dev); - if (feat_found) - { - /* Get the configuration from the page where wrist wear wake up - * feature resides - */ - rslt = bmi2_get_feat_config(wrist_wake_up_cfg.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of wrist wear wake up */ - idx = wrist_wake_up_cfg.start_addr; - - /* Set the feature enable bit */ - feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_WRIST_WEAR_WAKE_UP_FEAT_EN, enable); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API is used to enable/disable gyroscope user gain - * feature. - */ -static int8_t set_gyro_user_gain(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for gyroscope user gain */ - struct bmi2_feature_config gyr_user_gain_cfg = { 0, 0, 0 }; - - /* Search for user gain feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&gyr_user_gain_cfg, BMI2_GYRO_GAIN_UPDATE, dev); - if (feat_found) - { - /* Get the configuration from the page where user gain feature resides */ - rslt = bmi2_get_feat_config(gyr_user_gain_cfg.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of user gain */ - idx = gyr_user_gain_cfg.start_addr + BMI2_GYR_USER_GAIN_FEAT_EN_OFFSET; - - /* Set the feature enable bit */ - feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_GYR_USER_GAIN_FEAT_EN, enable); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API sets any-motion configurations like axes select, - * duration, threshold and output-configuration. - */ -static int8_t set_any_motion_config(const struct bmi2_any_motion_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define count */ - uint8_t index = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for any motion */ - struct bmi2_feature_config any_mot_config = { 0, 0, 0 }; - - /* Copy the feature configuration address to a local pointer */ - uint16_t *data_p = (uint16_t *) (void *)feat_config; - - /* Search for any-motion feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&any_mot_config, BMI2_ANY_MOTION, dev); - if (feat_found) - { - /* Get the configuration from the page where any-motion feature resides */ - rslt = bmi2_get_feat_config(any_mot_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for any-motion select */ - idx = any_mot_config.start_addr; - - /* Get offset in words since all the features are set in words length */ - idx = idx / 2; - - /* Set duration */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_ANY_NO_MOT_DUR, config->duration); - - /* Set x-select */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_ANY_NO_MOT_X_SEL, config->select_x); - - /* Set y-select */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_ANY_NO_MOT_Y_SEL, config->select_y); - - /* Set z-select */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_ANY_NO_MOT_Z_SEL, config->select_z); - - /* Increment offset by 1 word to set threshold and output configuration */ - idx++; - - /* Set threshold */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_ANY_NO_MOT_THRES, config->threshold); - - /* Increment offset by 1 more word to get the total length in words */ - idx++; - - /* Get total length in bytes to copy from local pointer to the array */ - idx = (uint8_t)(idx * 2) - any_mot_config.start_addr; - - /* Copy the bytes to be set back to the array */ - for (index = 0; index < idx; index++) - { - feat_config[any_mot_config.start_addr + - index] = *((uint8_t *) data_p + any_mot_config.start_addr + index); - } - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API sets no-motion configurations like axes select, - * duration, threshold and output-configuration. - */ -static int8_t set_no_motion_config(const struct bmi2_no_motion_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define count */ - uint8_t index = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for no-motion */ - struct bmi2_feature_config no_mot_config = { 0, 0, 0 }; - - /* Copy the feature configuration address to a local pointer */ - uint16_t *data_p = (uint16_t *) (void *)feat_config; - - /* Search for no-motion feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&no_mot_config, BMI2_NO_MOTION, dev); - if (feat_found) - { - /* Get the configuration from the page where no-motion feature resides */ - rslt = bmi2_get_feat_config(no_mot_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for no-motion select */ - idx = no_mot_config.start_addr; - - /* Get offset in words since all the features are set in words length */ - idx = idx / 2; - - /* Set duration */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_ANY_NO_MOT_DUR, config->duration); - - /* Set x-select */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_ANY_NO_MOT_X_SEL, config->select_x); - - /* Set y-select */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_ANY_NO_MOT_Y_SEL, config->select_y); - - /* Set z-select */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_ANY_NO_MOT_Z_SEL, config->select_z); - - /* Increment offset by 1 word to set threshold and output configuration */ - idx++; - - /* Set threshold */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_ANY_NO_MOT_THRES, config->threshold); - - /* Increment offset by 1 more word to get the total length in words */ - idx++; - - /* Get total length in bytes to copy from local pointer to the array */ - idx = (uint8_t)(idx * 2) - no_mot_config.start_addr; - - /* Copy the bytes to be set back to the array */ - for (index = 0; index < idx; index++) - { - feat_config[no_mot_config.start_addr + - index] = *((uint8_t *) data_p + no_mot_config.start_addr + index); - } - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API sets sig-motion configurations like block-size, - * output-configuration and other parameters. - */ -static int8_t set_sig_motion_config(const struct bmi2_sig_motion_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define index */ - uint8_t index = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for sig-motion */ - struct bmi2_feature_config sig_mot_config = { 0, 0, 0 }; - - /* Copy the feature configuration address to a local pointer */ - uint16_t *data_p = (uint16_t *) (void *)feat_config; - - /* Search for sig-motion feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&sig_mot_config, BMI2_SIG_MOTION, dev); - if (feat_found) - { - /* Get the configuration from the page where sig-motion feature resides */ - rslt = bmi2_get_feat_config(sig_mot_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for sig-motion select */ - idx = sig_mot_config.start_addr; - - /* Get offset in words since all the features are set in words length */ - idx = idx / 2; - - /* Set parameter 1 */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_SIG_MOT_PARAM_1, config->block_size); - - /* Increment offset by 1 word to set parameter 2 */ - idx++; - - /* Set parameter 2 */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_SIG_MOT_PARAM_2, config->param_2); - - /* Increment offset by 1 word to set parameter 3 */ - idx++; - - /* Set parameter 3 */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_SIG_MOT_PARAM_3, config->param_3); - - /* Increment offset by 1 word to set parameter 4 */ - idx++; - - /* Set parameter 4 */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_SIG_MOT_PARAM_4, config->param_4); - - /* Increment offset by 1 word to set parameter 5 */ - idx++; - - /* Set parameter 5 */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_SIG_MOT_PARAM_5, config->param_5); - - /* Increment offset by 1 word to set output- configuration */ - idx++; - - /* Get total length in bytes to copy from local pointer to the array */ - idx = (uint8_t)(idx * 2) - sig_mot_config.start_addr; - - /* Copy the bytes to be set back to the array */ - for (index = 0; index < idx; index++) - { - feat_config[sig_mot_config.start_addr + - index] = *((uint8_t *) data_p + sig_mot_config.start_addr + index); - } - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API sets step counter parameter configurations. - */ -static int8_t set_step_count_params_config(const uint16_t *step_count_params, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt = BMI2_OK; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define index */ - uint8_t index = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for step counter parameters */ - struct bmi2_feature_config step_params_config = { 0, 0, 0 }; - - /* Variable to index the page number */ - uint8_t page_idx; - - /* Variable to define the start page */ - uint8_t start_page; - - /* Variable to define start address of the parameters */ - uint8_t start_addr; - - /* Variable to define number of bytes */ - uint8_t n_bytes = (BMI2_STEP_CNT_N_PARAMS * 2); - - /* Variable to store number of pages */ - uint8_t n_pages = (n_bytes / 16); - - /* Variable to define the end page */ - uint8_t end_page; - - /* Variable to define the remaining bytes to be read */ - uint8_t remain_len; - - /* Variable to define the maximum words(16 bytes or 8 words) to be read in a page */ - uint8_t max_len = 8; - - /* Variable index bytes in a page */ - uint8_t page_byte_idx; - - /* Variable to index the parameters */ - uint8_t param_idx = 0; - - /* Copy the feature configuration address to a local pointer */ - uint16_t *data_p = (uint16_t *) (void *)feat_config; - - /* Search for step counter parameter feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&step_params_config, BMI2_STEP_COUNTER_PARAMS, dev); - if (feat_found) - { - /* Get the start page for the step counter parameters */ - start_page = step_params_config.page; - - /* Get the end page for the step counter parameters */ - end_page = start_page + n_pages; - - /* Get the start address for the step counter parameters */ - start_addr = step_params_config.start_addr; - - /* Get the remaining length of bytes to be read */ - remain_len = (uint8_t)((n_bytes - (n_pages * 16)) + start_addr); - for (page_idx = start_page; page_idx <= end_page; page_idx++) - { - /* Get the configuration from the respective page */ - rslt = bmi2_get_feat_config(page_idx, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Start from address 0x00 when switched to next page */ - if (page_idx > start_page) - { - start_addr = 0; - } - - /* Remaining number of words to be read in the page */ - if (page_idx == end_page) - { - max_len = (remain_len / 2); - } - - /* Get offset in words since all the features are set in words length */ - page_byte_idx = start_addr / 2; - for (; page_byte_idx < max_len;) - { - /* Set parameters 1 to 25 */ - *(data_p + page_byte_idx) = BMI2_SET_BIT_POS0(*(data_p + page_byte_idx), - BMI2_STEP_COUNT_PARAMS, - step_count_params[param_idx]); - - /* Increment offset by 1 word to set to the next parameter */ - page_byte_idx++; - - /* Increment to next parameter */ - param_idx++; - } - - /* Get total length in bytes to copy from local pointer to the array */ - page_byte_idx = (uint8_t)(page_byte_idx * 2) - step_params_config.start_addr; - - /* Copy the bytes to be set back to the array */ - for (index = 0; index < page_byte_idx; index++) - { - feat_config[step_params_config.start_addr + - index] = *((uint8_t *) data_p + step_params_config.start_addr + index); - } - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/* @brief This internal API sets step counter configurations like water-mark - * level, reset-counter and output-configuration step detector and activity. - */ -static int8_t set_step_config(const struct bmi2_step_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define index */ - uint8_t index = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for step counter 4 */ - struct bmi2_feature_config step_count_config = { 0, 0, 0 }; - - /* Copy the feature configuration address to a local pointer */ - uint16_t *data_p = (uint16_t *) (void *)feat_config; - - /* Search for step counter feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&step_count_config, BMI2_STEP_COUNTER, dev); - if (feat_found) - { - /* Get the configuration from the page where step counter resides */ - rslt = bmi2_get_feat_config(step_count_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes */ - idx = step_count_config.start_addr; - - /* Get offset in words since all the features are set in words length */ - idx = idx / 2; - - /* Set water-mark level */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_STEP_COUNT_WM_LEVEL, config->watermark_level); - - /* Set reset-counter */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_STEP_COUNT_RST_CNT, config->reset_counter); - - /* Increment offset by 1 word to set output - * configuration of step detector and step activity - */ - idx++; - - /* Set step buffer size */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_STEP_BUFFER_SIZE, config->step_buffer_size); - - /* Increment offset by 1 more word to get the total length in words */ - idx++; - - /* Get total length in bytes to copy from local pointer to the array */ - idx = (uint8_t)(idx * 2) - step_count_config.start_addr; - - /* Copy the bytes to be set back to the array */ - for (index = 0; index < idx; index++) - { - feat_config[step_count_config.start_addr + - index] = *((uint8_t *) data_p + step_count_config.start_addr + index); - } - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API sets wrist gesture configurations like wearable-arm, - * and output-configuration. - */ -static int8_t set_wrist_gest_config(const struct bmi2_wrist_gest_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define index */ - uint8_t index = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for wrist gesture */ - struct bmi2_feature_config wrist_gest_config = { 0, 0, 0 }; - - /* Copy the feature configuration address to a local pointer */ - uint16_t *data_p = (uint16_t *) (void *)feat_config; - - /* Search for wrist gesture feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&wrist_gest_config, BMI2_WRIST_GESTURE, dev); - if (feat_found) - { - /* Get the configuration from the page where wrist gesture feature resides */ - rslt = bmi2_get_feat_config(wrist_gest_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for gesture select */ - idx = wrist_gest_config.start_addr; - - /* Get offset in words since all the features are set in words length */ - idx = idx / 2; - - /* Set wearable arm */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_WRIST_GEST_WEAR_ARM, config->wearable_arm); - - /* Increment offset by 1 more word to set minimum tilt angle (min_flick_peak) */ - idx++; - *(data_p + idx) = config->min_flick_peak; - - /* Increment offset by 1 more word to set min_flick_samples */ - idx++; - *(data_p + idx) = config->min_flick_samples; - - /* Increment offset by 1 more word to set max time within gesture moment has to be completed */ - idx++; - *(data_p + idx) = config->max_duration; - - /* Increment offset by 1 more word to get the total length in words */ - idx++; - - /* Get total length in bytes to copy from local pointer to the array */ - idx = (uint8_t)(idx * 2) - wrist_gest_config.start_addr; - - /* Copy the bytes to be set back to the array */ - for (index = 0; index < idx; index++) - { - feat_config[wrist_gest_config.start_addr + - index] = *((uint8_t *) data_p + wrist_gest_config.start_addr + index); - } - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API sets wrist wear wake-up configurations like - * output-configuration. - */ -static int8_t set_wrist_wear_wake_up_config(const struct bmi2_wrist_wear_wake_up_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define index */ - uint8_t index = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for wrist wear wake-up */ - struct bmi2_feature_config wrist_wake_up_config = { 0, 0, 0 }; - - /* Copy the feature configuration address to a local pointer */ - uint16_t *data_p = (uint16_t *) (void *)feat_config; - - /* Search for wrist wear wake-up feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&wrist_wake_up_config, BMI2_WRIST_WEAR_WAKE_UP, dev); - if (feat_found) - { - /* Get the configuration from the page where wrist wear wake-up feature resides */ - rslt = bmi2_get_feat_config(wrist_wake_up_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for wrist wear wake-up select */ - idx = wrist_wake_up_config.start_addr; - - /* Get offset in words since all the features are set in words length */ - idx = idx / 2; - - *(data_p + idx) = config->min_angle_focus; - - /* Increment offset by 1 more word to set min_angle_nonfocus */ - idx++; - *(data_p + idx) = config->min_angle_nonfocus; - - /* Increment offset by 1 more word to set max_tilt_lr */ - idx++; - *(data_p + idx) = config->max_tilt_lr; - - /* Increment offset by 1 more word to set max_tilt_ll */ - idx++; - *(data_p + idx) = config->max_tilt_ll; - - /* Increment offset by 1 more word to set max_tilt_pd */ - idx++; - *(data_p + idx) = config->max_tilt_pd; - - /* Increment offset by 1 more word to set max_tilt_pu */ - idx++; - *(data_p + idx) = config->max_tilt_pu; - - /* Increment offset by 1 more word to get the total length in words */ - idx++; - - /* Get total length in bytes to copy from local pointer to the array */ - idx = (uint8_t)(idx * 2) - wrist_wake_up_config.start_addr; - - /* Copy the bytes to be set back to the array */ - for (index = 0; index < idx; index++) - { - feat_config[wrist_wake_up_config.start_addr + - index] = *((uint8_t *) data_p + wrist_wake_up_config.start_addr + index); - } - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets any-motion configurations like axes select, - * duration, threshold and output-configuration. - */ -static int8_t get_any_motion_config(struct bmi2_any_motion_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define LSB */ - uint16_t lsb; - - /* Variable to define MSB */ - uint16_t msb; - - /* Variable to define a word */ - uint16_t lsb_msb; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for any-motion */ - struct bmi2_feature_config any_mot_config = { 0, 0, 0 }; - - /* Search for any-motion feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&any_mot_config, BMI2_ANY_MOTION, dev); - if (feat_found) - { - /* Get the configuration from the page where any-motion feature resides */ - rslt = bmi2_get_feat_config(any_mot_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for feature enable for any-motion */ - idx = any_mot_config.start_addr; - - /* Get word to calculate duration, x, y and z select */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get duration */ - config->duration = lsb_msb & BMI2_ANY_NO_MOT_DUR_MASK; - - /* Get x-select */ - config->select_x = (lsb_msb & BMI2_ANY_NO_MOT_X_SEL_MASK) >> BMI2_ANY_NO_MOT_X_SEL_POS; - - /* Get y-select */ - config->select_y = (lsb_msb & BMI2_ANY_NO_MOT_Y_SEL_MASK) >> BMI2_ANY_NO_MOT_Y_SEL_POS; - - /* Get z-select */ - config->select_z = (lsb_msb & BMI2_ANY_NO_MOT_Z_SEL_MASK) >> BMI2_ANY_NO_MOT_Z_SEL_POS; - - /* Get word to calculate threshold, output configuration from the same word */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get threshold */ - config->threshold = lsb_msb & BMI2_ANY_NO_MOT_THRES_MASK; - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets no-motion configurations like axes select, - * duration, threshold and output-configuration. - */ -static int8_t get_no_motion_config(struct bmi2_no_motion_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define LSB */ - uint16_t lsb = 0; - - /* Variable to define MSB */ - uint16_t msb = 0; - - /* Variable to define a word */ - uint16_t lsb_msb = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for no-motion */ - struct bmi2_feature_config no_mot_config = { 0, 0, 0 }; - - /* Search for no-motion feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&no_mot_config, BMI2_NO_MOTION, dev); - if (feat_found) - { - /* Get the configuration from the page where no-motion feature resides */ - rslt = bmi2_get_feat_config(no_mot_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for feature enable for no-motion */ - idx = no_mot_config.start_addr; - - /* Get word to calculate duration, x, y and z select */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get duration */ - config->duration = lsb_msb & BMI2_ANY_NO_MOT_DUR_MASK; - - /* Get x-select */ - config->select_x = (lsb_msb & BMI2_ANY_NO_MOT_X_SEL_MASK) >> BMI2_ANY_NO_MOT_X_SEL_POS; - - /* Get y-select */ - config->select_y = (lsb_msb & BMI2_ANY_NO_MOT_Y_SEL_MASK) >> BMI2_ANY_NO_MOT_Y_SEL_POS; - - /* Get z-select */ - config->select_z = (lsb_msb & BMI2_ANY_NO_MOT_Z_SEL_MASK) >> BMI2_ANY_NO_MOT_Z_SEL_POS; - - /* Get word to calculate threshold, output configuration from the same word */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get threshold */ - config->threshold = lsb_msb & BMI2_ANY_NO_MOT_THRES_MASK; - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets sig-motion configurations like block-size, - * output-configuration and other parameters. - */ -static int8_t get_sig_motion_config(struct bmi2_sig_motion_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define LSB */ - uint16_t lsb = 0; - - /* Variable to define MSB */ - uint16_t msb = 0; - - /* Variable to define a word */ - uint16_t lsb_msb = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration sig-motion */ - struct bmi2_feature_config sig_mot_config = { 0, 0, 0 }; - - /* Search for sig-motion feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&sig_mot_config, BMI2_SIG_MOTION, dev); - if (feat_found) - { - /* Get the configuration from the page where sig-motion feature resides */ - rslt = bmi2_get_feat_config(sig_mot_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for feature enable for sig-motion */ - idx = sig_mot_config.start_addr; - - /* Get word to calculate parameter 1 */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get parameter 1 */ - config->block_size = lsb_msb & BMI2_SIG_MOT_PARAM_1_MASK; - - /* Get word to calculate parameter 2 */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get parameter 2 */ - config->param_2 = lsb_msb & BMI2_SIG_MOT_PARAM_2_MASK; - - /* Get word to calculate parameter 3 */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get parameter 3 */ - config->param_3 = lsb_msb & BMI2_SIG_MOT_PARAM_3_MASK; - - /* Get word to calculate parameter 4 */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get parameter 4 */ - config->param_4 = lsb_msb & BMI2_SIG_MOT_PARAM_4_MASK; - - /* Get word to calculate parameter 5 */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get parameter 5 */ - config->param_5 = lsb_msb & BMI2_SIG_MOT_PARAM_5_MASK; - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets step counter parameter configurations. - */ -static int8_t get_step_count_params_config(uint16_t *step_count_params, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt = BMI2_OK; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Variable to define LSB */ - uint16_t lsb = 0; - - /* Variable to define MSB */ - uint16_t msb = 0; - - /* Variable to define a word */ - uint16_t lsb_msb = 0; - - /* Initialize feature configuration for step counter 1 */ - struct bmi2_feature_config step_params_config = { 0, 0, 0 }; - - /* Variable to index the page number */ - uint8_t page_idx; - - /* Variable to define the start page */ - uint8_t start_page; - - /* Variable to define start address of the parameters */ - uint8_t start_addr; - - /* Variable to define number of bytes */ - uint8_t n_bytes = (BMI2_STEP_CNT_N_PARAMS * 2); - - /* Variable to store number of pages */ - uint8_t n_pages = (n_bytes / 16); - - /* Variable to define the end page */ - uint8_t end_page; - - /* Variable to define the remaining bytes to be read */ - uint8_t remain_len; - - /* Variable to define the maximum words to be read in a page */ - uint8_t max_len = BMI2_FEAT_SIZE_IN_BYTES; - - /* Variable index bytes in a page */ - uint8_t page_byte_idx; - - /* Variable to index the parameters */ - uint8_t param_idx = 0; - - /* Search for step counter parameter feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&step_params_config, BMI2_STEP_COUNTER_PARAMS, dev); - if (feat_found) - { - /* Get the start page for the step counter parameters */ - start_page = step_params_config.page; - - /* Get the end page for the step counter parameters */ - end_page = start_page + n_pages; - - /* Get the start address for the step counter parameters */ - start_addr = step_params_config.start_addr; - - /* Get the remaining length of bytes to be read */ - remain_len = (uint8_t)((n_bytes - (n_pages * 16)) + start_addr); - for (page_idx = start_page; page_idx <= end_page; page_idx++) - { - /* Get the configuration from the respective page */ - rslt = bmi2_get_feat_config(page_idx, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Start from address 0x00 when switched to next page */ - if (page_idx > start_page) - { - start_addr = 0; - } - - /* Remaining number of bytes to be read in the page */ - if (page_idx == end_page) - { - max_len = remain_len; - } - - /* Get the offset */ - page_byte_idx = start_addr; - while (page_byte_idx < max_len) - { - /* Get word to calculate the parameter*/ - lsb = (uint16_t) feat_config[page_byte_idx++]; - if (page_byte_idx < max_len) - { - msb = ((uint16_t) feat_config[page_byte_idx++] << 8); - } - - lsb_msb = lsb | msb; - - /* Get parameters 1 to 25 */ - step_count_params[param_idx] = lsb_msb & BMI2_STEP_COUNT_PARAMS_MASK; - - /* Increment to next parameter */ - param_idx++; - } - } - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets step counter/detector/activity configurations. - */ -static int8_t get_step_config(struct bmi2_step_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define LSB */ - uint16_t lsb = 0; - - /* Variable to define MSB */ - uint16_t msb = 0; - - /* Variable to define a word */ - uint16_t lsb_msb = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for step counter */ - struct bmi2_feature_config step_count_config = { 0, 0, 0 }; - - /* Search for step counter 4 feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&step_count_config, BMI2_STEP_COUNTER, dev); - if (feat_found) - { - /* Get the configuration from the page where step counter 4 parameter resides */ - rslt = bmi2_get_feat_config(step_count_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for feature enable for step counter/detector/activity */ - idx = step_count_config.start_addr; - - /* Get word to calculate water-mark level and reset counter */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get water-mark level */ - config->watermark_level = lsb_msb & BMI2_STEP_COUNT_WM_LEVEL_MASK; - - /* Get reset counter */ - config->reset_counter = (lsb_msb & BMI2_STEP_COUNT_RST_CNT_MASK) >> BMI2_STEP_COUNT_RST_CNT_POS; - - /* Get word to calculate output configuration of step detector and activity */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - config->step_buffer_size = (lsb_msb & BMI2_STEP_BUFFER_SIZE_MASK) >> BMI2_STEP_BUFFER_SIZE_POS; - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets wrist gesture configurations like wearable-arm, - * and output-configuration. - */ -static int8_t get_wrist_gest_config(struct bmi2_wrist_gest_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for wrist gesture */ - struct bmi2_feature_config wrist_gest_config = { 0, 0, 0 }; - - /* Copy the feature configuration address to a local pointer */ - uint16_t *data_p = (uint16_t *) (void *)feat_config; - - /* Search for wrist gesture feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&wrist_gest_config, BMI2_WRIST_GESTURE, dev); - if (feat_found) - { - /* Get the configuration from the page where wrist gesture feature resides */ - rslt = bmi2_get_feat_config(wrist_gest_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for wrist gesture select */ - idx = wrist_gest_config.start_addr; - - /* Get offset in words since all the features are set in words length */ - idx = idx / 2; - - /* Get wearable arm */ - config->wearable_arm = (*(data_p + idx) & BMI2_WRIST_GEST_WEAR_ARM_MASK) >> BMI2_WRIST_GEST_WEAR_ARM_POS; - - /* Increment the offset by 1 word to get min_flick_peak */ - idx++; - config->min_flick_peak = *(data_p + idx); - - /* Increment the offset by 1 word to get min_flick_samples */ - idx++; - config->min_flick_samples = *(data_p + idx); - - /* Increment the offset by 1 word to get max_duration */ - idx++; - config->max_duration = *(data_p + idx); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets wrist wear wake-up configurations like - * output-configuration. - */ -static int8_t get_wrist_wear_wake_up_config(struct bmi2_wrist_wear_wake_up_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for wrist wear wake-up */ - struct bmi2_feature_config wrist_wake_up_config = { 0, 0, 0 }; - - /* Copy the feature configuration address to a local pointer */ - uint16_t *data_p = (uint16_t *) (void *)feat_config; - - /* Search for wrist wear wake-up feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&wrist_wake_up_config, BMI2_WRIST_WEAR_WAKE_UP, dev); - if (feat_found) - { - /* Get the configuration from the page where wrist wear wake-up feature resides */ - rslt = bmi2_get_feat_config(wrist_wake_up_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for wrist wear wake-up select */ - idx = wrist_wake_up_config.start_addr; - - /* Get offset in words since all the features are set in words length */ - idx = idx / 2; - - config->min_angle_focus = *(data_p + idx); - - /* Increment the offset value by 1 word to get min_angle_nonfocus */ - idx++; - config->min_angle_nonfocus = *(data_p + idx); - - /* Increment the offset value by 1 word to get max_tilt_lr */ - idx++; - config->max_tilt_lr = *(data_p + idx); - - /* Increment the offset value by 1 word to get max_tilt_ll */ - idx++; - config->max_tilt_ll = *(data_p + idx); - - /* Increment the offset value by 1 word to get max_tilt_pd */ - idx++; - config->max_tilt_pd = *(data_p + idx); - - /* Increment the offset value by 1 word to get max_tilt_pu */ - idx++; - config->max_tilt_pu = *(data_p + idx); - - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets the output values of the wrist gesture. - */ -static int8_t get_wrist_gest_status(uint8_t *wrist_gest, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variables to define index */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature output for wrist gesture */ - struct bmi2_feature_config wrist_gest_out_config = { 0, 0, 0 }; - - /* Search for wrist gesture feature and extract its configuration details */ - feat_found = extract_output_feat_config(&wrist_gest_out_config, BMI2_WRIST_GESTURE, dev); - if (feat_found) - { - /* Get the feature output configuration for wrist gesture */ - rslt = bmi2_get_feat_config(wrist_gest_out_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for wrist gesture output */ - idx = wrist_gest_out_config.start_addr; - - /* Get the wrist gesture output */ - *wrist_gest = feat_config[idx]; - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets the output values of step counter. - */ -static int8_t get_step_counter_output(uint32_t *step_count, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variables to define index */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature output for step counter */ - struct bmi2_feature_config step_cnt_out_config = { 0, 0, 0 }; - - /* Search for step counter output feature and extract its configuration details */ - feat_found = extract_output_feat_config(&step_cnt_out_config, BMI2_STEP_COUNTER, dev); - if (feat_found) - { - /* Get the feature output configuration for step-counter */ - rslt = bmi2_get_feat_config(step_cnt_out_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for step counter output */ - idx = step_cnt_out_config.start_addr; - - /* Get the step counter output in 4 bytes */ - *step_count = (uint32_t) feat_config[idx++]; - *step_count |= ((uint32_t) feat_config[idx++] << 8); - *step_count |= ((uint32_t) feat_config[idx++] << 16); - *step_count |= ((uint32_t) feat_config[idx++] << 24); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets the error status related to NVM. - */ -static int8_t get_nvm_error_status(struct bmi2_nvm_err_status *nvm_err_stat, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variables to define index */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature output for NVM error status */ - struct bmi2_feature_config nvm_err_cfg = { 0, 0, 0 }; - - /* Search for NVM error status feature and extract its configuration details */ - feat_found = extract_output_feat_config(&nvm_err_cfg, BMI2_NVM_STATUS, dev); - if (feat_found) - { - /* Get the feature output configuration for NVM error status */ - rslt = bmi2_get_feat_config(nvm_err_cfg.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for NVM error status */ - idx = nvm_err_cfg.start_addr; - - /* Increment index to get the error status */ - idx++; - - /* Error when NVM load action fails */ - nvm_err_stat->load_error = BMI2_GET_BIT_POS0(feat_config[idx], BMI2_NVM_LOAD_ERR_STATUS); - - /* Error when NVM program action fails */ - nvm_err_stat->prog_error = BMI2_GET_BITS(feat_config[idx], BMI2_NVM_PROG_ERR_STATUS); - - /* Error when NVM erase action fails */ - nvm_err_stat->erase_error = BMI2_GET_BITS(feat_config[idx], BMI2_NVM_ERASE_ERR_STATUS); - - /* Error when NVM program limit is exceeded */ - nvm_err_stat->exceed_error = BMI2_GET_BITS(feat_config[idx], BMI2_NVM_END_EXCEED_STATUS); - - /* Error when NVM privilege mode is not acquired */ - nvm_err_stat->privil_error = BMI2_GET_BITS(feat_config[idx], BMI2_NVM_PRIV_ERR_STATUS); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API is used to get enable status of gyroscope user gain - * update. - */ -static int8_t get_user_gain_upd_status(uint8_t *status, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt = BMI2_OK; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Variable to check APS status */ - uint8_t aps_stat = 0; - - /* Initialize feature configuration for gyroscope user gain */ - struct bmi2_feature_config gyr_user_gain_cfg = { 0, 0, 0 }; - - /* Search for user gain feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&gyr_user_gain_cfg, BMI2_GYRO_GAIN_UPDATE, dev); - if (feat_found) - { - /* Disable advance power save */ - aps_stat = dev->aps_status; - if (aps_stat == BMI2_ENABLE) - { - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - } - - if (rslt == BMI2_OK) - { - /* Get the configuration from the page where user gain feature resides */ - rslt = bmi2_get_feat_config(gyr_user_gain_cfg.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of user gain */ - idx = gyr_user_gain_cfg.start_addr + BMI2_GYR_USER_GAIN_FEAT_EN_OFFSET; - - /* Set the feature enable status */ - *status = BMI2_GET_BITS(feat_config[idx], BMI2_GYR_USER_GAIN_FEAT_EN); - } - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - /* Enable Advance power save if disabled while configuring and not when already disabled */ - if ((rslt == BMI2_OK) && (aps_stat == BMI2_ENABLE)) - { - rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - } - - return rslt; -} - -/*! - * @brief This internal API gets the output values of step activity. - */ -static int8_t get_step_activity_output(uint8_t *step_act, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variables to define index */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature output for step activity */ - struct bmi2_feature_config step_act_out_config = { 0, 0, 0 }; - - /* Search for step activity output feature and extract its configuration details */ - feat_found = extract_output_feat_config(&step_act_out_config, BMI2_STEP_ACTIVITY, dev); - if (feat_found) - { - /* Get the feature output configuration for step-activity */ - rslt = bmi2_get_feat_config(step_act_out_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for step activity output */ - idx = step_act_out_config.start_addr; - - /* Get the step activity output */ - *step_act = feat_config[idx]; - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets the error status related to virtual frames. - */ -static int8_t get_vfrm_error_status(struct bmi2_vfrm_err_status *vfrm_err_stat, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variables to define index */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature output for VFRM error status */ - struct bmi2_feature_config vfrm_err_cfg = { 0, 0, 0 }; - - /* Search for VFRM error status feature and extract its configuration details */ - feat_found = extract_output_feat_config(&vfrm_err_cfg, BMI2_VFRM_STATUS, dev); - if (feat_found) - { - /* Get the feature output configuration for VFRM error status */ - rslt = bmi2_get_feat_config(vfrm_err_cfg.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for VFRM error status */ - idx = vfrm_err_cfg.start_addr; - - /* Increment index to get the error status */ - idx++; - - /* Internal error while acquiring lock for FIFO */ - vfrm_err_stat->lock_error = BMI2_GET_BITS(feat_config[idx], BMI2_VFRM_LOCK_ERR_STATUS); - - /* Internal error while writing byte into FIFO */ - vfrm_err_stat->write_error = BMI2_GET_BITS(feat_config[idx], BMI2_VFRM_WRITE_ERR_STATUS); - - /* Internal error while writing into FIFO */ - vfrm_err_stat->fatal_error = BMI2_GET_BITS(feat_config[idx], BMI2_VFRM_FATAL_ERR_STATUS); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API enables/disables compensation of the gain defined - * in the GAIN register. - */ -static int8_t enable_gyro_gain(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define register data */ - uint8_t reg_data = 0; - - rslt = bmi2_get_regs(BMI2_GYR_OFF_COMP_6_ADDR, ®_data, 1, dev); - if (rslt == BMI2_OK) - { - reg_data = BMI2_SET_BITS(reg_data, BMI2_GYR_GAIN_EN, enable); - rslt = bmi2_set_regs(BMI2_GYR_OFF_COMP_6_ADDR, ®_data, 1, dev); - } - - return rslt; -} - -/*! - * @brief This internal API is used to extract the output feature configuration - * details from the look-up table. - */ -static uint8_t extract_output_feat_config(struct bmi2_feature_config *feat_output, - uint8_t type, - const struct bmi2_dev *dev) -{ - /* Variable to define loop */ - uint8_t loop = 0; - - /* Variable to set flag */ - uint8_t feat_found = BMI2_FALSE; - - /* Search for the output feature from the output configuration array */ - while (loop < dev->out_sens) - { - if (dev->feat_output[loop].type == type) - { - *feat_output = dev->feat_output[loop]; - feat_found = BMI2_TRUE; - break; - } - - loop++; - } - - /* Return flag */ - return feat_found; -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270.h b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270.h deleted file mode 100644 index f42676d65e..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270.h +++ /dev/null @@ -1,422 +0,0 @@ -/** -* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. -* -* BSD-3-Clause -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* 1. Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* -* 2. Redistributions in binary form must reproduce the above copyright -* notice, this list of conditions and the following disclaimer in the -* documentation and/or other materials provided with the distribution. -* -* 3. Neither the name of the copyright holder nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) -* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, -* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING -* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -* -* @file bmi270.h -* @date 2020-11-04 -* @version v2.63.1 -* -*/ - -/** - * \ingroup bmi2xy - * \defgroup bmi270 BMI270 - * @brief Sensor driver for BMI270 sensor - */ - -#ifndef BMI270_H_ -#define BMI270_H_ - -/*! CPP guard */ -#ifdef __cplusplus -extern "C" { -#endif - -/***************************************************************************/ - -/*! Header files - ****************************************************************************/ -#include "bmi2.h" - -/***************************************************************************/ - -/*! Macro definitions - ****************************************************************************/ - -/*! @name BMI270 Chip identifier */ -#define BMI270_CHIP_ID UINT8_C(0x24) - -/*! @name BMI270 feature input start addresses */ -#define BMI270_CONFIG_ID_STRT_ADDR UINT8_C(0x00) -#define BMI270_MAX_BURST_LEN_STRT_ADDR UINT8_C(0x02) -#define BMI270_CRT_GYRO_SELF_TEST_STRT_ADDR UINT8_C(0x03) -#define BMI270_ABORT_STRT_ADDR UINT8_C(0x03) -#define BMI270_AXIS_MAP_STRT_ADDR UINT8_C(0x04) -#define BMI270_GYRO_SELF_OFF_STRT_ADDR UINT8_C(0x05) -#define BMI270_NVM_PROG_PREP_STRT_ADDR UINT8_C(0x05) -#define BMI270_GYRO_GAIN_UPDATE_STRT_ADDR UINT8_C(0x06) -#define BMI270_ANY_MOT_STRT_ADDR UINT8_C(0x0C) -#define BMI270_NO_MOT_STRT_ADDR UINT8_C(0x00) -#define BMI270_SIG_MOT_STRT_ADDR UINT8_C(0x04) -#define BMI270_STEP_CNT_1_STRT_ADDR UINT8_C(0x00) -#define BMI270_STEP_CNT_4_STRT_ADDR UINT8_C(0x02) -#define BMI270_WRIST_GEST_STRT_ADDR UINT8_C(0x06) -#define BMI270_WRIST_WEAR_WAKE_UP_STRT_ADDR UINT8_C(0x00) - -/*! @name BMI270 feature output start addresses */ -#define BMI270_STEP_CNT_OUT_STRT_ADDR UINT8_C(0x00) -#define BMI270_STEP_ACT_OUT_STRT_ADDR UINT8_C(0x04) -#define BMI270_WRIST_GEST_OUT_STRT_ADDR UINT8_C(0x06) -#define BMI270_GYR_USER_GAIN_OUT_STRT_ADDR UINT8_C(0x08) -#define BMI270_GYRO_CROSS_SENSE_STRT_ADDR UINT8_C(0x0C) -#define BMI270_NVM_VFRM_OUT_STRT_ADDR UINT8_C(0x0E) - -/*! @name Defines maximum number of pages */ -#define BMI270_MAX_PAGE_NUM UINT8_C(8) - -/*! @name Defines maximum number of feature input configurations */ -#define BMI270_MAX_FEAT_IN UINT8_C(17) - -/*! @name Defines maximum number of feature outputs */ -#define BMI270_MAX_FEAT_OUT UINT8_C(7) - -/*! @name Mask definitions for feature interrupt status bits */ -#define BMI270_SIG_MOT_STATUS_MASK UINT8_C(0x01) -#define BMI270_STEP_CNT_STATUS_MASK UINT8_C(0x02) -#define BMI270_STEP_ACT_STATUS_MASK UINT8_C(0x04) -#define BMI270_WRIST_WAKE_UP_STATUS_MASK UINT8_C(0x08) -#define BMI270_WRIST_GEST_STATUS_MASK UINT8_C(0x10) -#define BMI270_NO_MOT_STATUS_MASK UINT8_C(0x20) -#define BMI270_ANY_MOT_STATUS_MASK UINT8_C(0x40) - -/*! @name Mask definitions for feature interrupt mapping bits */ -#define BMI270_INT_SIG_MOT_MASK UINT8_C(0x01) -#define BMI270_INT_STEP_COUNTER_MASK UINT8_C(0x02) -#define BMI270_INT_STEP_DETECTOR_MASK UINT8_C(0x02) -#define BMI270_INT_STEP_ACT_MASK UINT8_C(0x04) -#define BMI270_INT_WRIST_WEAR_WAKEUP_MASK UINT8_C(0x08) -#define BMI270_INT_WRIST_GEST_MASK UINT8_C(0x10) -#define BMI270_INT_NO_MOT_MASK UINT8_C(0x20) -#define BMI270_INT_ANY_MOT_MASK UINT8_C(0x40) - -/*! @name Defines maximum number of feature interrupts */ -#define BMI270_MAX_INT_MAP UINT8_C(8) - -/***************************************************************************/ - -/*! BMI270 User Interface function prototypes - ****************************************************************************/ - -/** - * \ingroup bmi270 - * \defgroup bmi270ApiInit Initialization - * @brief Initialize the sensor and device structure - */ - -/*! - * \ingroup bmi270ApiInit - * \page bmi270_api_bmi270_init bmi270_init - * \code - * int8_t bmi270_init(struct bmi2_dev *dev); - * \endcode - * @details This API: - * 1) updates the device structure with address of the configuration file. - * 2) Initializes BMI270 sensor. - * 3) Writes the configuration file. - * 4) Updates the feature offset parameters in the device structure. - * 5) Updates the maximum number of pages, in the device structure. - * - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi270_init(struct bmi2_dev *dev); - -/** - * \ingroup bmi270 - * \defgroup bmi270ApiSensor Feature Set - * @brief Enable / Disable features of the sensor - */ - -/*! - * \ingroup bmi270ApiSensor - * \page bmi270_api_bmi270_sensor_enable bmi270_sensor_enable - * \code - * int8_t bmi270_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); - * \endcode - * @details This API selects the sensors/features to be enabled. - * - * @param[in] sens_list : Pointer to select the sensor/feature. - * @param[in] n_sens : Number of sensors selected. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @note Sensors/features that can be enabled. - * - *@verbatim - * sens_list | Values - * ----------------------------|----------- - * BMI2_ACCEL | 0 - * BMI2_GYRO | 1 - * BMI2_AUX | 2 - * BMI2_SIG_MOTION | 3 - * BMI2_ANY_MOTION | 4 - * BMI2_NO_MOTION | 5 - * BMI2_STEP_DETECTOR | 6 - * BMI2_STEP_COUNTER | 7 - * BMI2_STEP_ACTIVITY | 8 - * BMI2_GYRO_GAIN_UPDATE | 9 - * BMI2_WRIST_GESTURE | 19 - * BMI2_WRIST_WEAR_WAKE_UP | 20 - * BMI2_GYRO_SELF_OFF | 33 - *@endverbatim - * - * @note : - * example uint8_t sens_list[2] = {BMI2_ACCEL, BMI2_GYRO}; - * uint8_t n_sens = 2; - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi270_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); - -/*! - * \ingroup bmi270ApiSensor - * \page bmi270_api_bmi270_sensor_disable bmi270_sensor_disable - * \code - * int8_t bmi270_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); - * \endcode - * @details This API selects the sensors/features to be disabled. - * - * @param[in] sens_list : Pointer to select the sensor/feature. - * @param[in] n_sens : Number of sensors selected. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @note Sensors/features that can be disabled. - * - *@verbatim - * sens_list | Values - * ----------------------------|----------- - * BMI2_ACCEL | 0 - * BMI2_GYRO | 1 - * BMI2_AUX | 2 - * BMI2_SIG_MOTION | 3 - * BMI2_ANY_MOTION | 4 - * BMI2_NO_MOTION | 5 - * BMI2_STEP_DETECTOR | 6 - * BMI2_STEP_COUNTER | 7 - * BMI2_STEP_ACTIVITY | 8 - * BMI2_GYRO_GAIN_UPDATE | 9 - * BMI2_WRIST_GESTURE | 19 - * BMI2_WRIST_WEAR_WAKE_UP | 20 - * BMI2_GYRO_SELF_OFF | 33 - *@endverbatim - * - * @note : - * example uint8_t sens_list[2] = {BMI2_ACCEL, BMI2_GYRO}; - * uint8_t n_sens = 2; - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi270_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); - -/** - * \ingroup bmi270 - * \defgroup bmi270ApiSensorC Sensor Configuration - * @brief Enable / Disable feature configuration of the sensor - */ - -/*! - * \ingroup bmi270ApiSensorC - * \page bmi270_api_bmi270_set_sensor_config bmi270_set_sensor_config - * \code - * int8_t bmi270_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); - * \endcode - * @details This API sets the sensor/feature configuration. - * - * @param[in] sens_cfg : Structure instance of bmi2_sens_config. - * @param[in] n_sens : Number of sensors selected. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @note Sensors/features that can be configured - * - *@verbatim - * sens_list | Values - * ----------------------------|----------- - * BMI2_SIG_MOTION | 3 - * BMI2_ANY_MOTION | 4 - * BMI2_NO_MOTION | 5 - * BMI2_STEP_DETECTOR | 6 - * BMI2_STEP_COUNTER | 7 - * BMI2_STEP_ACTIVITY | 8 - * BMI2_WRIST_GESTURE | 19 - * BMI2_WRIST_WEAR_WAKE_UP | 20 - * BMI2_STEP_COUNTER_PARAMS | 28 - *@endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi270_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); - -/*! - * \ingroup bmi270ApiSensorC - * \page bmi270_api_bmi270_get_sensor_config bmi270_get_sensor_config - * \code - * int8_t bmi270_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); - * \endcode - * @details This API gets the sensor/feature configuration. - * - * @param[in] sens_cfg : Structure instance of bmi2_sens_config. - * @param[in] n_sens : Number of sensors selected. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @note Sensors/features whose configurations can be read. - * - *@verbatim - * sens_list | Values - * ----------------------------|----------- - * BMI2_SIG_MOTION | 3 - * BMI2_ANY_MOTION | 4 - * BMI2_NO_MOTION | 5 - * BMI2_STEP_DETECTOR | 6 - * BMI2_STEP_COUNTER | 7 - * BMI2_STEP_ACTIVITY | 8 - * BMI2_WRIST_GESTURE | 19 - * BMI2_WRIST_WEAR_WAKE_UP | 20 - * BMI2_STEP_COUNTER_PARAMS | 28 - *@endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi270_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); - -/** - * \ingroup bmi270 - * \defgroup bmi270ApiSensorD Sensor Data - * @brief Get sensor data - */ - -/*! - * \ingroup bmi270ApiSensorD - * \page bmi270_api_bmi270_get_sensor_data bmi270_get_sensor_data - * \code - * int8_t bmi270_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev); - * \endcode - * @details This API gets the sensor/feature data for accelerometer, gyroscope, - * auxiliary sensor, step counter, high-g, gyroscope user-gain update, - * orientation, gyroscope cross sensitivity and error status for NVM and VFRM. - * - * @param[out] sensor_data : Structure instance of bmi2_sensor_data. - * @param[in] n_sens : Number of sensors selected. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @note Sensors/features whose data can be read - * - *@verbatim - * sens_list | Values - * ----------------------------|----------- - * BMI2_STEP_COUNTER | 7 - * BMI2_STEP_ACTIVITY | 8 - * BMI2_WRIST_GESTURE | 19 - * BMI2_NVM_STATUS | 38 - * BMI2_VFRM_STATUS | 39 - *@endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi270_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev); - -/** - * \ingroup bmi270 - * \defgroup bmi270ApiGyroUG Gyro User Gain - * @brief Set / Get Gyro User Gain of the sensor - */ - -/*! - * \ingroup bmi270ApiGyroUG - * \page bmi270_api_bmi270_update_gyro_user_gain bmi270_update_gyro_user_gain - * \code - * int8_t bmi270_update_gyro_user_gain(const struct bmi2_gyro_user_gain_config *user_gain, struct bmi2_dev *dev); - * \endcode - * @details This API updates the gyroscope user-gain. - * - * @param[in] user_gain : Structure that stores user-gain configurations. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi270_update_gyro_user_gain(const struct bmi2_gyro_user_gain_config *user_gain, struct bmi2_dev *dev); - -/*! - * \ingroup bmi270ApiGyroUG - * \page bmi270_api_bmi270_read_gyro_user_gain bmi270_read_gyro_user_gain - * \code - * int8_t bmi270_read_gyro_user_gain(struct bmi2_gyro_user_gain_data *gyr_usr_gain, const struct bmi2_dev *dev); - * \endcode - * @details This API reads the compensated gyroscope user-gain values. - * - * @param[out] gyr_usr_gain : Structure that stores gain values. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi270_read_gyro_user_gain(struct bmi2_gyro_user_gain_data *gyr_usr_gain, struct bmi2_dev *dev); - -/*! - * \ingroup bmi270ApiInt - * \page bmi270_api_bmi270_map_feat_int bmi270_map_feat_int - * \code - * int8_t bmi270_map_feat_int(const struct bmi2_sens_int_config *sens_int, uint8_t n_sens, struct bmi2_dev *dev) - * \endcode - * @details This API maps/unmaps feature interrupts to that of interrupt pins. - * - * @param[in] sens_int : Structure instance of bmi2_sens_int_config. - * @param[in] n_sens : Number of interrupts to be mapped. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi270_map_feat_int(const struct bmi2_sens_int_config *sens_int, uint8_t n_sens, struct bmi2_dev *dev); - -/******************************************************************************/ -/*! @name C++ Guard Macros */ -/******************************************************************************/ -#ifdef __cplusplus -} -#endif /* End of CPP guard */ - -#endif /* BMI270_H_ */ diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_context.c b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_context.c deleted file mode 100644 index 4f6e300351..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_context.c +++ /dev/null @@ -1,3122 +0,0 @@ -/** -* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. -* -* BSD-3-Clause -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* 1. Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* -* 2. Redistributions in binary form must reproduce the above copyright -* notice, this list of conditions and the following disclaimer in the -* documentation and/or other materials provided with the distribution. -* -* 3. Neither the name of the copyright holder nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) -* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, -* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING -* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -* -* @file bmi270_context.c -* @date 2020-11-04 -* @version v2.63.1 -* -*/ - -/***************************************************************************/ - -/*! Header files - ****************************************************************************/ -#include "bmi270_context.h" - -/***************************************************************************/ - -/*! Global Variable - ****************************************************************************/ - -/*! @name Global array that stores the configuration file of BMI270_CONTEXT */ -const uint8_t bmi270_context_config_file[] = { - 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x00, 0xb0, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0xc9, - 0x01, 0x80, 0x2e, 0xe2, 0x00, 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x77, 0xb0, 0x50, 0x30, 0x21, 0x2e, 0x59, 0xf5, - 0x10, 0x30, 0x21, 0x2e, 0x6a, 0xf5, 0x80, 0x2e, 0xaf, 0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0x09, 0x01, 0x00, 0x22, - 0x00, 0x76, 0x00, 0x00, 0x10, 0x00, 0x10, 0xd1, 0x00, 0xcb, 0xa7, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, - 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, - 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, - 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, - 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, - 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, - 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, - 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, - 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, - 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0xfd, 0x2d, 0x2c, 0x56, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x02, 0x08, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x04, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1a, 0x24, 0x22, 0x00, 0x80, 0x2e, 0x48, 0x02, 0x01, 0x2e, 0x49, - 0xf1, 0x0b, 0xbc, 0x10, 0x50, 0x0f, 0xb8, 0x00, 0x90, 0xfb, 0x7f, 0x07, 0x2f, 0x03, 0x2e, 0x21, 0xf2, 0x02, 0x31, - 0x4a, 0x0a, 0x23, 0x2e, 0x21, 0xf2, 0x09, 0x2c, 0x00, 0x30, 0x98, 0x2e, 0x0e, 0xc7, 0x03, 0x2e, 0x21, 0xf2, 0xf2, - 0x3e, 0x4a, 0x08, 0x23, 0x2e, 0x21, 0xf2, 0xfb, 0x6f, 0xf0, 0x5f, 0xb8, 0x2e, 0x13, 0x52, 0x00, 0x2e, 0x60, 0x40, - 0x41, 0x40, 0x0d, 0xbc, 0x98, 0xbc, 0xc0, 0x2e, 0x01, 0x0a, 0x0f, 0xb8, 0x43, 0x86, 0x25, 0x40, 0x04, 0x40, 0xd8, - 0xbe, 0x2c, 0x0b, 0x22, 0x11, 0x54, 0x42, 0x03, 0x80, 0x4b, 0x0e, 0xf6, 0x2f, 0xb8, 0x2e, 0x20, 0x50, 0xe7, 0x7f, - 0xf6, 0x7f, 0x46, 0x30, 0x0f, 0x2e, 0xa4, 0xf1, 0xbe, 0x09, 0x80, 0xb3, 0x06, 0x2f, 0x0d, 0x2e, 0x84, 0x00, 0x84, - 0xaf, 0x02, 0x2f, 0x16, 0x30, 0x2d, 0x2e, 0x7b, 0x00, 0x86, 0x30, 0x2d, 0x2e, 0x60, 0xf5, 0xf6, 0x6f, 0xe7, 0x6f, - 0xe0, 0x5f, 0xc8, 0x2e, 0x80, 0x2e, 0xfb, 0x00, 0x00, 0x30, 0xc0, 0x2e, 0x21, 0x2e, 0x8d, 0x00, 0x44, 0x47, 0x99, - 0x00, 0xff, 0x3f, 0x00, 0x0c, 0xff, 0x0f, 0x00, 0x04, 0xc0, 0x00, 0x5b, 0xf5, 0x90, 0x00, 0x1e, 0xf2, 0xfd, 0xf5, - 0x8e, 0x00, 0x96, 0x00, 0x96, 0x00, 0xe0, 0x00, 0x19, 0xf4, 0x66, 0xf5, 0x00, 0x18, 0x64, 0xf5, 0x9d, 0x00, 0x7f, - 0x00, 0x81, 0x00, 0xae, 0x00, 0xff, 0xfb, 0x21, 0x02, 0x00, 0x10, 0x00, 0x40, 0xff, 0x00, 0x00, 0x80, 0xff, 0x7f, - 0x54, 0x0f, 0xeb, 0x00, 0x7f, 0xff, 0xc2, 0xf5, 0x68, 0xf7, 0xb3, 0xf1, 0x4e, 0x0f, 0x42, 0x0f, 0x48, 0x0f, 0x80, - 0x00, 0x67, 0x0f, 0x58, 0xf7, 0x5b, 0xf7, 0x6a, 0x0f, 0x86, 0x00, 0x59, 0x0f, 0x6c, 0x0f, 0xc6, 0xf1, 0x66, 0x0f, - 0x6c, 0xf7, 0x00, 0xe0, 0x00, 0xff, 0xd1, 0xf5, 0x6e, 0x0f, 0x71, 0x0f, 0xff, 0x03, 0x00, 0xfc, 0xf0, 0x3f, 0xb9, - 0x00, 0x2d, 0xf5, 0xca, 0xf5, 0x8a, 0x00, 0x00, 0x08, 0x71, 0x7d, 0xfe, 0xc0, 0x03, 0x3f, 0x05, 0x3e, 0x49, 0x01, - 0x92, 0x02, 0xf5, 0xd6, 0xe8, 0x63, 0xd3, 0xf8, 0x2e, 0x07, 0x5c, 0xce, 0xa5, 0x67, 0x28, 0x02, 0x4e, 0x01, 0x00, - 0xf0, 0x33, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x15, 0x50, 0x10, - 0x50, 0x17, 0x52, 0x05, 0x2e, 0x7d, 0x00, 0xfb, 0x7f, 0x00, 0x2e, 0x13, 0x40, 0x93, 0x42, 0x41, 0x0e, 0xfb, 0x2f, - 0x98, 0x2e, 0x91, 0x03, 0x98, 0x2e, 0x87, 0xcf, 0x01, 0x2e, 0x89, 0x00, 0x00, 0xb2, 0x08, 0x2f, 0x01, 0x2e, 0x69, - 0xf7, 0xb1, 0x3f, 0x01, 0x08, 0x01, 0x30, 0x23, 0x2e, 0x89, 0x00, 0x21, 0x2e, 0x69, 0xf7, 0xfb, 0x6f, 0xf0, 0x5f, - 0xb8, 0x2e, 0xa0, 0x50, 0x80, 0x7f, 0xe7, 0x7f, 0xd5, 0x7f, 0xc4, 0x7f, 0xb3, 0x7f, 0xa2, 0x7f, 0x91, 0x7f, 0xf6, - 0x7f, 0x7b, 0x7f, 0x00, 0x2e, 0x01, 0x2e, 0x60, 0xf5, 0x60, 0x7f, 0x98, 0x2e, 0xce, 0x00, 0x62, 0x6f, 0x01, 0x32, - 0x91, 0x08, 0x80, 0xb2, 0x11, 0x2f, 0x00, 0xb2, 0x03, 0x2f, 0x05, 0x2e, 0x18, 0x00, 0x80, 0x90, 0x09, 0x2f, 0x60, - 0x7f, 0x98, 0x2e, 0xf9, 0x00, 0x23, 0x50, 0x01, 0x32, 0x01, 0x42, 0x02, 0x86, 0x60, 0x6f, 0x02, 0x30, 0xc2, 0x42, - 0x23, 0x2e, 0x60, 0xf5, 0x00, 0x90, 0x00, 0x30, 0x01, 0x2f, 0x21, 0x2e, 0x7a, 0x00, 0xf6, 0x6f, 0x91, 0x6f, 0xa2, - 0x6f, 0xb3, 0x6f, 0xc4, 0x6f, 0xd5, 0x6f, 0xe7, 0x6f, 0x7b, 0x6f, 0x80, 0x6f, 0x60, 0x5f, 0xc8, 0x2e, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x2d, 0x01, 0xd4, 0x7b, 0x3b, - 0x01, 0xdb, 0x7a, 0x04, 0x00, 0x3f, 0x7b, 0xcd, 0x6c, 0xc3, 0x04, 0x85, 0x09, 0xc3, 0x04, 0xec, 0xe6, 0x0c, 0x46, - 0x01, 0x00, 0x27, 0x00, 0x19, 0x00, 0x96, 0x00, 0xa0, 0x00, 0x01, 0x00, 0x0c, 0x00, 0xf0, 0x3c, 0x00, 0x01, 0x01, - 0x00, 0x03, 0x00, 0x01, 0x00, 0x0e, 0x00, 0x00, 0x00, 0x01, 0x00, 0x07, 0x09, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x01, 0x00, 0xe1, 0x06, 0x66, 0x0a, 0x0a, 0x00, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x50, 0x98, 0x2e, 0xd7, 0x0e, 0x50, 0x32, 0x98, 0x2e, - 0x48, 0x03, 0x10, 0x30, 0x21, 0x2e, 0x21, 0xf2, 0x00, 0x30, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0x00, 0x2e, 0x01, - 0x80, 0x06, 0xa2, 0xfb, 0x2f, 0x01, 0x2e, 0x9c, 0x00, 0x00, 0xb2, 0x10, 0x2f, 0x01, 0x2e, 0x18, 0x00, 0x00, 0xb2, - 0x0c, 0x2f, 0x01, 0x54, 0x03, 0x52, 0x01, 0x50, 0x98, 0x2e, 0xc2, 0xc0, 0x98, 0x2e, 0xf5, 0xb0, 0x01, 0x50, 0x98, - 0x2e, 0xd5, 0xb6, 0x10, 0x30, 0x21, 0x2e, 0x19, 0x00, 0x01, 0x2e, 0x84, 0x00, 0x04, 0xae, 0x0b, 0x2f, 0x01, 0x2e, - 0x9c, 0x00, 0x00, 0xb2, 0x07, 0x2f, 0x01, 0x52, 0x98, 0x2e, 0x8e, 0x0e, 0x00, 0xb2, 0x02, 0x2f, 0x10, 0x30, 0x21, - 0x2e, 0x79, 0x00, 0x01, 0x2e, 0x79, 0x00, 0x00, 0x90, 0x90, 0x2e, 0x14, 0x03, 0x01, 0x2e, 0x87, 0x00, 0x00, 0xb2, - 0x04, 0x2f, 0x98, 0x2e, 0x2f, 0x0e, 0x00, 0x30, 0x21, 0x2e, 0x7b, 0x00, 0x01, 0x2e, 0x7b, 0x00, 0x00, 0xb2, 0x12, - 0x2f, 0x01, 0x2e, 0x84, 0x00, 0x00, 0x90, 0x02, 0x2f, 0x98, 0x2e, 0x1f, 0x0e, 0x09, 0x2d, 0x98, 0x2e, 0x81, 0x0d, - 0x01, 0x2e, 0x84, 0x00, 0x04, 0x90, 0x02, 0x2f, 0x50, 0x32, 0x98, 0x2e, 0x48, 0x03, 0x00, 0x30, 0x21, 0x2e, 0x7b, - 0x00, 0x01, 0x2e, 0x78, 0x00, 0x00, 0xb2, 0x90, 0x2e, 0x2c, 0x03, 0x01, 0x2e, 0x78, 0x00, 0x81, 0x30, 0x01, 0x08, - 0x00, 0xb2, 0x61, 0x2f, 0x03, 0x2e, 0x24, 0x02, 0x01, 0x2e, 0x84, 0x00, 0x98, 0xbc, 0x98, 0xb8, 0x05, 0xb2, 0x0d, - 0x58, 0x23, 0x2f, 0x07, 0x90, 0x07, 0x54, 0x00, 0x30, 0x37, 0x2f, 0x15, 0x41, 0x04, 0x41, 0xdc, 0xbe, 0x44, 0xbe, - 0xdc, 0xba, 0x2c, 0x01, 0x61, 0x00, 0x0d, 0x56, 0x4a, 0x0f, 0x0c, 0x2f, 0xd1, 0x42, 0x94, 0xb8, 0xc1, 0x42, 0x11, - 0x30, 0x05, 0x2e, 0x6a, 0xf7, 0x2c, 0xbd, 0x2f, 0xb9, 0x80, 0xb2, 0x08, 0x22, 0x98, 0x2e, 0xaf, 0x03, 0x21, 0x2d, - 0x61, 0x30, 0x23, 0x2e, 0x84, 0x00, 0x98, 0x2e, 0xaf, 0x03, 0x00, 0x30, 0x21, 0x2e, 0x5a, 0xf5, 0x18, 0x2d, 0xf1, - 0x7f, 0x50, 0x30, 0x98, 0x2e, 0x48, 0x03, 0x0d, 0x52, 0x05, 0x50, 0x50, 0x42, 0x70, 0x30, 0x0b, 0x54, 0x42, 0x42, - 0x7e, 0x82, 0xf2, 0x6f, 0x80, 0xb2, 0x42, 0x42, 0x05, 0x2f, 0x21, 0x2e, 0x84, 0x00, 0x10, 0x30, 0x98, 0x2e, 0xaf, - 0x03, 0x03, 0x2d, 0x60, 0x30, 0x21, 0x2e, 0x84, 0x00, 0x01, 0x2e, 0x84, 0x00, 0x06, 0x90, 0x18, 0x2f, 0x01, 0x2e, - 0x77, 0x00, 0x09, 0x54, 0x05, 0x52, 0xf0, 0x7f, 0x98, 0x2e, 0x7a, 0xc1, 0xf1, 0x6f, 0x08, 0x1a, 0x40, 0x30, 0x08, - 0x2f, 0x21, 0x2e, 0x84, 0x00, 0x20, 0x30, 0x98, 0x2e, 0x9b, 0x03, 0x50, 0x32, 0x98, 0x2e, 0x48, 0x03, 0x05, 0x2d, - 0x98, 0x2e, 0x38, 0x0e, 0x00, 0x30, 0x21, 0x2e, 0x84, 0x00, 0x00, 0x30, 0x21, 0x2e, 0x78, 0x00, 0x18, 0x2d, 0x01, - 0x2e, 0x84, 0x00, 0x03, 0xaa, 0x01, 0x2f, 0x98, 0x2e, 0x45, 0x0e, 0x01, 0x2e, 0x84, 0x00, 0x3f, 0x80, 0x03, 0xa2, - 0x01, 0x2f, 0x00, 0x2e, 0x02, 0x2d, 0x98, 0x2e, 0x5b, 0x0e, 0x30, 0x30, 0x98, 0x2e, 0xba, 0x03, 0x00, 0x30, 0x21, - 0x2e, 0x79, 0x00, 0x50, 0x32, 0x98, 0x2e, 0x48, 0x03, 0x01, 0x2e, 0x19, 0x00, 0x00, 0xb2, 0x10, 0x2f, 0x01, 0x2e, - 0x85, 0x00, 0x21, 0x2e, 0x90, 0x00, 0x0f, 0x52, 0x7e, 0x82, 0x11, 0x50, 0x41, 0x40, 0x18, 0xb9, 0x11, 0x42, 0x02, - 0x42, 0x02, 0x80, 0x00, 0x2e, 0x01, 0x40, 0x01, 0x42, 0x98, 0x2e, 0xaa, 0x01, 0x00, 0x30, 0x21, 0x2e, 0x19, 0x00, - 0x21, 0x2e, 0x9c, 0x00, 0x80, 0x2e, 0x52, 0x02, 0x21, 0x2e, 0x59, 0xf5, 0x10, 0x30, 0xc0, 0x2e, 0x21, 0x2e, 0x4a, - 0xf1, 0x80, 0x2e, 0x00, 0xc1, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x9a, 0x01, - 0x34, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x03, 0x2e, 0x7d, 0x00, 0x16, 0xb8, 0x02, 0x34, 0x4a, 0x0c, 0x21, 0x2e, 0x2d, 0xf5, 0xc0, 0x2e, 0x23, - 0x2e, 0x7d, 0x00, 0x03, 0xbc, 0x21, 0x2e, 0x85, 0x00, 0x03, 0x2e, 0x85, 0x00, 0x40, 0xb2, 0x10, 0x30, 0x21, 0x2e, - 0x19, 0x00, 0x01, 0x30, 0x05, 0x2f, 0x05, 0x2e, 0x88, 0x00, 0x80, 0x90, 0x01, 0x2f, 0x23, 0x2e, 0x6f, 0xf5, 0xc0, - 0x2e, 0x21, 0x2e, 0x89, 0x00, 0x11, 0x30, 0x81, 0x08, 0x01, 0x2e, 0x6a, 0xf7, 0x71, 0x3f, 0x23, 0xbd, 0x01, 0x08, - 0x02, 0x0a, 0xc0, 0x2e, 0x21, 0x2e, 0x6a, 0xf7, 0x30, 0x25, 0x00, 0x30, 0x21, 0x2e, 0x5a, 0xf5, 0x10, 0x50, 0x21, - 0x2e, 0x7b, 0x00, 0x21, 0x2e, 0x78, 0x00, 0xfb, 0x7f, 0x98, 0x2e, 0xaf, 0x03, 0x40, 0x30, 0x21, 0x2e, 0x84, 0x00, - 0xfb, 0x6f, 0xf0, 0x5f, 0x03, 0x25, 0x80, 0x2e, 0x9b, 0x03, 0x0b, 0x00, 0x94, 0x02, 0x14, 0x24, 0x80, 0x00, 0x04, - 0x00, 0x04, 0x30, 0x08, 0xb8, 0x94, 0x02, 0xc0, 0x2e, 0x28, 0xbd, 0x02, 0x0a, 0x0d, 0x82, 0x02, 0x30, 0x12, 0x42, - 0x41, 0x0e, 0xfc, 0x2f, 0xb8, 0x2e, 0x95, 0x50, 0xc0, 0x2e, 0x21, 0x2e, 0xa9, 0x01, 0x02, 0x30, 0x02, 0x2c, 0x41, - 0x00, 0x12, 0x42, 0x41, 0x0e, 0xfc, 0x2f, 0xb8, 0x2e, 0x13, 0x82, 0x02, 0x30, 0x12, 0x42, 0x41, 0x0e, 0xfc, 0x2f, - 0x3f, 0x80, 0xa1, 0x30, 0xc0, 0x2e, 0x01, 0x42, 0x00, 0x2e, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, - 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0xc0, 0x50, 0xe7, 0x7f, - 0xf6, 0x7f, 0x26, 0x30, 0x0f, 0x2e, 0x61, 0xf5, 0x2f, 0x2e, 0x78, 0x00, 0x0f, 0x2e, 0x78, 0x00, 0xbe, 0x09, 0xa2, - 0x7f, 0x80, 0x7f, 0x80, 0xb3, 0xd5, 0x7f, 0xc4, 0x7f, 0xb3, 0x7f, 0x91, 0x7f, 0x7b, 0x7f, 0x0b, 0x2f, 0x19, 0x50, - 0x1a, 0x25, 0x12, 0x40, 0x42, 0x7f, 0x74, 0x82, 0x12, 0x40, 0x52, 0x7f, 0x00, 0x2e, 0x00, 0x40, 0x60, 0x7f, 0x98, - 0x2e, 0x6a, 0xd6, 0x81, 0x30, 0x01, 0x2e, 0x78, 0x00, 0x01, 0x08, 0x00, 0xb2, 0x42, 0x2f, 0x03, 0x2e, 0x24, 0x02, - 0x01, 0x2e, 0x24, 0x02, 0x97, 0xbc, 0x06, 0xbc, 0x9f, 0xb8, 0x0f, 0xb8, 0x00, 0x90, 0x23, 0x2e, 0x88, 0x00, 0x10, - 0x30, 0x01, 0x30, 0x2a, 0x2f, 0x03, 0x2e, 0x84, 0x00, 0x44, 0xb2, 0x05, 0x2f, 0x47, 0xb2, 0x00, 0x30, 0x2d, 0x2f, - 0x21, 0x2e, 0x78, 0x00, 0x2b, 0x2d, 0x03, 0x2e, 0xfd, 0xf5, 0x9e, 0xbc, 0x9f, 0xb8, 0x40, 0x90, 0x14, 0x2f, 0x03, - 0x2e, 0xfc, 0xf5, 0x99, 0xbc, 0x9f, 0xb8, 0x40, 0x90, 0x0e, 0x2f, 0x03, 0x2e, 0x49, 0xf1, 0x1b, 0x54, 0x4a, 0x08, - 0x40, 0x90, 0x08, 0x2f, 0x98, 0x2e, 0xce, 0x00, 0x00, 0xb2, 0x10, 0x30, 0x03, 0x2f, 0x50, 0x30, 0x21, 0x2e, 0x84, - 0x00, 0x10, 0x2d, 0x98, 0x2e, 0x9b, 0x03, 0x00, 0x30, 0x21, 0x2e, 0x78, 0x00, 0x0a, 0x2d, 0x05, 0x2e, 0x69, 0xf7, - 0x2d, 0xbd, 0x2f, 0xb9, 0x80, 0xb2, 0x01, 0x2f, 0x21, 0x2e, 0x79, 0x00, 0x23, 0x2e, 0x78, 0x00, 0xe0, 0x31, 0x21, - 0x2e, 0x61, 0xf5, 0xf6, 0x6f, 0xe7, 0x6f, 0x80, 0x6f, 0xa2, 0x6f, 0xb3, 0x6f, 0xc4, 0x6f, 0xd5, 0x6f, 0x7b, 0x6f, - 0x91, 0x6f, 0x40, 0x5f, 0xc8, 0x2e, 0x90, 0x50, 0xf7, 0x7f, 0xe6, 0x7f, 0xd5, 0x7f, 0xc4, 0x7f, 0xb3, 0x7f, 0xa1, - 0x7f, 0x90, 0x7f, 0x82, 0x7f, 0x7b, 0x7f, 0x98, 0x2e, 0xce, 0x00, 0x00, 0xb2, 0x10, 0x30, 0x49, 0x2f, 0x05, 0x2e, - 0x21, 0x02, 0x03, 0x2e, 0x2d, 0x02, 0x21, 0x56, 0x08, 0x08, 0x93, 0x08, 0x90, 0x0a, 0x25, 0x2e, 0x18, 0x00, 0x05, - 0x2e, 0xc1, 0xf5, 0x2e, 0xbc, 0x05, 0x2e, 0x84, 0x00, 0x84, 0xa2, 0x0e, 0xb8, 0x31, 0x30, 0x88, 0x04, 0x03, 0x2f, - 0x01, 0x2e, 0x18, 0x00, 0x00, 0xb2, 0x0c, 0x2f, 0x1d, 0x50, 0x01, 0x52, 0x98, 0x2e, 0xd7, 0x00, 0x05, 0x2e, 0x7a, - 0x00, 0x80, 0x90, 0x02, 0x2f, 0x10, 0x30, 0x21, 0x2e, 0x7a, 0x00, 0x25, 0x2e, 0x9c, 0x00, 0x05, 0x2e, 0x18, 0x00, - 0x80, 0xb2, 0x20, 0x2f, 0x01, 0x2e, 0xc0, 0xf5, 0xf2, 0x30, 0x02, 0x08, 0x07, 0xaa, 0x73, 0x30, 0x03, 0x2e, 0x7c, - 0x00, 0x18, 0x22, 0x41, 0x1a, 0x05, 0x2f, 0x03, 0x2e, 0x66, 0xf5, 0x9f, 0xbc, 0x9f, 0xb8, 0x40, 0x90, 0x0c, 0x2f, - 0x1f, 0x52, 0x03, 0x30, 0x53, 0x42, 0x2b, 0x30, 0x90, 0x04, 0x5b, 0x42, 0x21, 0x2e, 0x7c, 0x00, 0x24, 0xbd, 0x7e, - 0x80, 0x81, 0x84, 0x43, 0x42, 0x02, 0x42, 0x02, 0x32, 0x25, 0x2e, 0x62, 0xf5, 0x05, 0x2e, 0x86, 0x00, 0x81, 0x84, - 0x25, 0x2e, 0x86, 0x00, 0x02, 0x31, 0x25, 0x2e, 0x60, 0xf5, 0x05, 0x2e, 0x25, 0x02, 0x10, 0x30, 0x90, 0x08, 0x80, - 0xb2, 0x0b, 0x2f, 0x05, 0x2e, 0xca, 0xf5, 0xf0, 0x3e, 0x90, 0x08, 0x25, 0x2e, 0xca, 0xf5, 0x05, 0x2e, 0x59, 0xf5, - 0xe0, 0x3f, 0x90, 0x08, 0x25, 0x2e, 0x59, 0xf5, 0x90, 0x6f, 0xa1, 0x6f, 0xb3, 0x6f, 0xc4, 0x6f, 0xd5, 0x6f, 0xe6, - 0x6f, 0xf7, 0x6f, 0x7b, 0x6f, 0x82, 0x6f, 0x70, 0x5f, 0xc8, 0x2e, 0x2f, 0x52, 0x90, 0x50, 0x53, 0x40, 0x4a, 0x25, - 0x40, 0x40, 0x39, 0x8b, 0xfb, 0x7f, 0x0c, 0xbc, 0x21, 0x52, 0x37, 0x89, 0x0b, 0x30, 0x59, 0x08, 0x0c, 0xb8, 0xe0, - 0x7f, 0x8b, 0x7f, 0x4b, 0x43, 0x0b, 0x43, 0x40, 0xb2, 0xd1, 0x7f, 0x6e, 0x2f, 0x01, 0x2e, 0x83, 0x00, 0x00, 0xb2, - 0x0e, 0x2f, 0x25, 0x52, 0x01, 0x2e, 0x7e, 0x00, 0xc3, 0x7f, 0xb4, 0x7f, 0xa5, 0x7f, 0x98, 0x2e, 0xbb, 0xcc, 0x05, - 0x30, 0x2b, 0x2e, 0x83, 0x00, 0xc3, 0x6f, 0xd1, 0x6f, 0xb4, 0x6f, 0xa5, 0x6f, 0x36, 0xbc, 0x06, 0xb9, 0x35, 0xbc, - 0x0f, 0xb8, 0x94, 0xb0, 0xc6, 0x7f, 0x00, 0xb2, 0x0c, 0x2f, 0x27, 0x50, 0x29, 0x56, 0x0b, 0x30, 0x05, 0x2e, 0x21, - 0x02, 0x2d, 0x5c, 0x1b, 0x42, 0xdb, 0x42, 0x96, 0x08, 0x25, 0x2e, 0x21, 0x02, 0x0b, 0x42, 0xcb, 0x42, 0x00, 0x2e, - 0x31, 0x56, 0xcb, 0x08, 0x25, 0x52, 0x01, 0x2e, 0x7e, 0x00, 0x01, 0x54, 0x2b, 0x5c, 0x98, 0x2e, 0x06, 0xcd, 0xd2, - 0x6f, 0x27, 0x5a, 0x94, 0x6f, 0xa4, 0xbc, 0x53, 0x41, 0x00, 0xb3, 0x1f, 0xb8, 0x44, 0x41, 0x01, 0x30, 0xd5, 0x7f, - 0x05, 0x2f, 0x00, 0xb2, 0x03, 0x2f, 0x29, 0x5c, 0x11, 0x30, 0x93, 0x43, 0x84, 0x43, 0x23, 0xbd, 0x2f, 0xb9, 0x80, - 0xb2, 0x1c, 0x2f, 0x72, 0x6f, 0xda, 0x00, 0x82, 0x6f, 0x22, 0x03, 0x44, 0x43, 0x00, 0x90, 0x27, 0x2e, 0x7f, 0x00, - 0x29, 0x5a, 0x12, 0x2f, 0x29, 0x54, 0x00, 0x2e, 0x90, 0x40, 0x82, 0x40, 0x18, 0x04, 0xa2, 0x06, 0x80, 0xaa, 0x04, - 0x2f, 0x80, 0x90, 0x08, 0x2f, 0xc2, 0x6f, 0x50, 0x0f, 0x05, 0x2f, 0xc0, 0x6f, 0x00, 0xb2, 0x02, 0x2f, 0x53, 0x43, - 0x44, 0x43, 0x11, 0x30, 0xe0, 0x6f, 0x98, 0x2e, 0x95, 0xcf, 0xd1, 0x6f, 0x15, 0x5a, 0x09, 0x2e, 0x7f, 0x00, 0x41, - 0x40, 0x54, 0x43, 0x08, 0x2c, 0x41, 0x43, 0x15, 0x30, 0x2b, 0x2e, 0x83, 0x00, 0x01, 0x30, 0xe0, 0x6f, 0x98, 0x2e, - 0x95, 0xcf, 0x00, 0x2e, 0xfb, 0x6f, 0x70, 0x5f, 0xb8, 0x2e, 0x50, 0x86, 0xcd, 0x88, 0x34, 0x85, 0xc5, 0x40, 0x91, - 0x40, 0x8c, 0x80, 0x06, 0x41, 0x13, 0x40, 0x50, 0x50, 0x6e, 0x01, 0x82, 0x40, 0x04, 0x40, 0x34, 0x8c, 0xfb, 0x7f, - 0x98, 0x2e, 0xce, 0x03, 0xe0, 0x7f, 0x00, 0x2e, 0x91, 0x41, 0x8c, 0x81, 0x82, 0x41, 0x13, 0x40, 0x04, 0x40, 0x34, - 0x8e, 0x98, 0x2e, 0xce, 0x03, 0xc0, 0x7f, 0xd5, 0x7f, 0x13, 0x24, 0xff, 0x00, 0xd6, 0x41, 0xcc, 0x83, 0xc2, 0x41, - 0x57, 0x40, 0x74, 0x80, 0x44, 0x40, 0x11, 0x40, 0x0c, 0x8a, 0xf7, 0x01, 0x94, 0x03, 0x12, 0x24, 0x80, 0x00, 0x3a, - 0x01, 0x02, 0x30, 0xb2, 0x03, 0xce, 0x17, 0xfb, 0x08, 0x23, 0x01, 0xb2, 0x02, 0x48, 0xbb, 0x28, 0xbd, 0xf2, 0x0b, - 0x53, 0x41, 0x02, 0x40, 0x44, 0x41, 0x74, 0x8d, 0xb7, 0x7f, 0x98, 0x2e, 0xce, 0x03, 0x50, 0x25, 0x91, 0x41, 0x8c, - 0x81, 0x82, 0x41, 0x13, 0x40, 0x04, 0x40, 0x34, 0x8e, 0x98, 0x2e, 0xce, 0x03, 0x60, 0x25, 0xd1, 0x41, 0xcc, 0x81, - 0xc2, 0x41, 0x13, 0x40, 0x04, 0x40, 0x98, 0x2e, 0xce, 0x03, 0x11, 0x24, 0xb3, 0x00, 0x71, 0x0e, 0xd3, 0x6f, 0xe1, - 0x6f, 0x33, 0x2f, 0x12, 0x24, 0xdd, 0x00, 0xda, 0x0f, 0x2b, 0x2f, 0x12, 0x24, 0x8c, 0x00, 0x5a, 0x0e, 0x09, 0x2f, - 0x10, 0x24, 0x83, 0x05, 0x48, 0x0e, 0x11, 0x24, 0x7f, 0x22, 0x10, 0x24, 0x18, 0x32, 0x08, 0x22, 0x80, 0x2e, 0xd7, - 0xb4, 0x13, 0x24, 0xf4, 0x00, 0x73, 0x0e, 0x0f, 0x2f, 0x10, 0x24, 0x11, 0x10, 0x68, 0x0e, 0x10, 0x24, 0xa2, 0x30, - 0x13, 0x24, 0x97, 0x23, 0x03, 0x22, 0x13, 0x24, 0x3b, 0x04, 0x4b, 0x0e, 0x11, 0x24, 0x0f, 0x30, 0x01, 0x22, 0x80, - 0x2e, 0xd7, 0xb4, 0x11, 0x24, 0x53, 0x02, 0x41, 0x0e, 0x11, 0x24, 0xe7, 0x31, 0x10, 0x24, 0xfc, 0x25, 0x08, 0x22, - 0x80, 0x2e, 0xd7, 0xb4, 0x10, 0x24, 0xe8, 0x40, 0x80, 0x2e, 0xd7, 0xb4, 0xf2, 0x37, 0x5a, 0x0e, 0x90, 0x2e, 0x50, - 0xb3, 0x12, 0x24, 0xea, 0x00, 0x4a, 0x0e, 0x90, 0x2e, 0xc7, 0xb2, 0xc2, 0x6f, 0x14, 0x24, 0x4c, 0x0b, 0x54, 0x0e, - 0x90, 0x2e, 0xab, 0xb2, 0x14, 0x24, 0x9b, 0x00, 0x5c, 0x0e, 0x90, 0x2e, 0xa1, 0xb2, 0x14, 0x24, 0x22, 0x01, 0x4c, - 0x0e, 0x70, 0x2f, 0x82, 0xa3, 0x5e, 0x2f, 0x11, 0x24, 0xba, 0x0b, 0x51, 0x0e, 0x35, 0x2f, 0x11, 0x24, 0x03, 0x08, - 0x69, 0x0e, 0x2d, 0x2f, 0xb1, 0x6f, 0x14, 0x24, 0x90, 0x00, 0x0c, 0x0e, 0x24, 0x2f, 0x11, 0x24, 0x31, 0x08, 0x69, - 0x0e, 0x16, 0x2f, 0x11, 0x24, 0x7d, 0x01, 0x59, 0x0e, 0x0e, 0x2f, 0x11, 0x24, 0xd7, 0x0c, 0x51, 0x0e, 0x11, 0x24, - 0x9f, 0x44, 0x13, 0x24, 0x41, 0x57, 0x4b, 0x22, 0x93, 0x35, 0x43, 0x0e, 0x10, 0x24, 0xbd, 0x42, 0x08, 0x22, 0x80, - 0x2e, 0xd7, 0xb4, 0x10, 0x24, 0x1c, 0x42, 0x80, 0x2e, 0xd7, 0xb4, 0x11, 0x24, 0x47, 0x01, 0x59, 0x0e, 0x11, 0x24, - 0xa2, 0x45, 0x10, 0x24, 0x31, 0x51, 0x08, 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0x10, 0x24, 0x80, 0x41, 0x80, 0x2e, 0xd7, - 0xb4, 0x10, 0x24, 0x67, 0x54, 0x80, 0x2e, 0xd7, 0xb4, 0x11, 0x24, 0x8c, 0x08, 0xe9, 0x0f, 0x10, 0x24, 0x0a, 0x48, - 0x90, 0x2e, 0xd7, 0xb4, 0xb1, 0x6f, 0x13, 0x24, 0xe8, 0x03, 0x8b, 0x0f, 0x10, 0x24, 0xcd, 0x57, 0x90, 0x2e, 0xd7, - 0xb4, 0x73, 0x35, 0x8b, 0x0f, 0x10, 0x24, 0x6f, 0x42, 0x90, 0x2e, 0xd7, 0xb4, 0x10, 0x24, 0xa0, 0xfe, 0x08, 0x0e, - 0x10, 0x24, 0x38, 0x54, 0x13, 0x24, 0xa3, 0x46, 0x03, 0x22, 0x13, 0x24, 0x45, 0xfd, 0x0b, 0x0e, 0x11, 0x24, 0x04, - 0x43, 0x08, 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0xb1, 0x6f, 0x00, 0x3a, 0x08, 0x0e, 0x11, 0x24, 0x3d, 0x45, 0x10, 0x24, - 0x52, 0x54, 0x48, 0x22, 0x10, 0x24, 0x8f, 0x01, 0x58, 0x0e, 0x10, 0x24, 0x48, 0x44, 0x01, 0x22, 0x80, 0x2e, 0xd7, - 0xb4, 0xb1, 0x6f, 0x13, 0x24, 0xfa, 0x03, 0x0b, 0x0e, 0x11, 0x24, 0x85, 0x43, 0x13, 0x24, 0x35, 0x55, 0x4b, 0x22, - 0x11, 0xa2, 0x10, 0x24, 0xf6, 0x57, 0x08, 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0x11, 0x24, 0xa4, 0x0a, 0x69, 0x0e, 0x11, - 0x24, 0x7b, 0x5a, 0x10, 0x24, 0x5e, 0x20, 0x08, 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0x11, 0x24, 0x0f, 0x01, 0x59, 0x0e, - 0x0d, 0x2f, 0x18, 0xa2, 0x11, 0x24, 0x2b, 0x47, 0x10, 0x24, 0xf4, 0x55, 0x48, 0x22, 0x10, 0x24, 0x16, 0x0b, 0x50, - 0x0e, 0x10, 0x24, 0xc7, 0x51, 0x01, 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0x11, 0x24, 0x72, 0x0a, 0x51, 0x0e, 0x11, 0x24, - 0x85, 0x55, 0x10, 0x24, 0xb2, 0x47, 0x08, 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0x10, 0x24, 0x83, 0x00, 0x48, 0x0e, 0x53, - 0x2f, 0x11, 0x24, 0xe1, 0x07, 0x69, 0x0e, 0x2d, 0x2f, 0x95, 0xaf, 0x27, 0x2f, 0x82, 0xaf, 0x21, 0x2f, 0x11, 0x24, - 0xd7, 0x00, 0x59, 0x0e, 0x19, 0x2f, 0xb1, 0x6f, 0x10, 0x24, 0xcc, 0x03, 0x88, 0x0f, 0x10, 0x2f, 0x10, 0x24, 0xe8, - 0xfe, 0x08, 0x0e, 0x11, 0x24, 0x7e, 0x56, 0x10, 0x24, 0x94, 0x45, 0x48, 0x22, 0xc0, 0x6f, 0x13, 0x24, 0x06, 0x0b, - 0x43, 0x0e, 0x10, 0x24, 0x2f, 0x51, 0x01, 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0x10, 0x24, 0xde, 0x51, 0x80, 0x2e, 0xd7, - 0xb4, 0x10, 0x24, 0xe8, 0x54, 0x80, 0x2e, 0xd7, 0xb4, 0x10, 0x24, 0xa4, 0x52, 0x80, 0x2e, 0xd7, 0xb4, 0x10, 0x24, - 0xd0, 0x44, 0x80, 0x2e, 0xd7, 0xb4, 0x11, 0x24, 0xb8, 0x00, 0xd9, 0x0f, 0x19, 0x2f, 0xc1, 0x6f, 0x10, 0x24, 0xe7, - 0x0c, 0xc8, 0x0f, 0x10, 0x2f, 0x11, 0x24, 0xc7, 0x07, 0x69, 0x0e, 0x11, 0x24, 0xf6, 0x52, 0x10, 0x24, 0x7a, 0x12, - 0x48, 0x22, 0xb0, 0x6f, 0x13, 0x24, 0x5d, 0x02, 0x03, 0x0e, 0x10, 0x24, 0x7c, 0x54, 0x01, 0x22, 0x80, 0x2e, 0xd7, - 0xb4, 0x10, 0x24, 0x8d, 0x51, 0x80, 0x2e, 0xd7, 0xb4, 0x10, 0x24, 0x28, 0x52, 0x80, 0x2e, 0xd7, 0xb4, 0x10, 0x24, - 0xd2, 0x07, 0xe8, 0x0f, 0x28, 0x2f, 0x10, 0x24, 0xb0, 0x00, 0xd8, 0x0f, 0x20, 0x2f, 0x10, 0x24, 0xc6, 0x07, 0x68, - 0x0e, 0x18, 0x2f, 0x50, 0x35, 0x48, 0x0e, 0x11, 0x2f, 0xb1, 0x6f, 0x10, 0x24, 0xf4, 0x01, 0x08, 0x0e, 0x11, 0x24, - 0x35, 0x51, 0x10, 0x24, 0x22, 0x12, 0x48, 0x22, 0xc0, 0x6f, 0x13, 0x24, 0xe0, 0x0c, 0x43, 0x0e, 0x10, 0x24, 0x7b, - 0x50, 0x08, 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0x10, 0x24, 0x81, 0x52, 0x80, 0x2e, 0xd7, 0xb4, 0x10, 0x24, 0x3b, 0x53, - 0x80, 0x2e, 0xd7, 0xb4, 0x10, 0x24, 0x63, 0x51, 0x80, 0x2e, 0xd7, 0xb4, 0x10, 0x24, 0x27, 0x51, 0x80, 0x2e, 0xd7, - 0xb4, 0x18, 0xa2, 0x90, 0x2e, 0xdb, 0xb3, 0x12, 0x24, 0x08, 0x02, 0x4a, 0x0e, 0x37, 0x2f, 0x12, 0x24, 0x2a, 0x09, - 0x6a, 0x0e, 0x1d, 0x2f, 0x13, 0x24, 0x8e, 0x00, 0x73, 0x0e, 0x09, 0x2f, 0x11, 0x24, 0xa5, 0x01, 0x41, 0x0e, 0x11, - 0x24, 0x76, 0x32, 0x10, 0x24, 0x12, 0x25, 0x08, 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0x10, 0x24, 0xa9, 0x0d, 0x68, 0x0e, - 0x10, 0x24, 0x04, 0x27, 0x13, 0x24, 0x73, 0x20, 0x03, 0x22, 0x13, 0x24, 0x14, 0x04, 0x4b, 0x0e, 0x11, 0x24, 0x15, - 0x2c, 0x01, 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0x11, 0x24, 0xae, 0x08, 0x69, 0x0e, 0x08, 0x2f, 0xa1, 0x35, 0x71, 0x0e, - 0x11, 0x24, 0x8b, 0x2b, 0x10, 0x24, 0x07, 0x35, 0x08, 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0x91, 0x34, 0x59, 0x0e, 0x11, - 0x24, 0x7b, 0x19, 0x10, 0x24, 0x50, 0x59, 0x08, 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0x62, 0x32, 0x42, 0x0e, 0x22, 0x2f, - 0xa2, 0x32, 0x5a, 0x0e, 0x1b, 0x2f, 0x12, 0x24, 0x0b, 0x08, 0x6a, 0x0e, 0x0e, 0x2f, 0xa3, 0x34, 0x43, 0x0e, 0x10, - 0x24, 0x28, 0x2b, 0x13, 0x24, 0x20, 0x23, 0x03, 0x22, 0x13, 0x24, 0x8d, 0x01, 0x4b, 0x0e, 0x11, 0x24, 0x5c, 0x21, - 0x08, 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0x31, 0x36, 0x59, 0x0e, 0x11, 0x24, 0x43, 0x25, 0x10, 0x24, 0xfa, 0x49, 0x08, - 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0x10, 0x24, 0xc7, 0x2a, 0x80, 0x2e, 0xd7, 0xb4, 0x40, 0x36, 0x58, 0x0e, 0x09, 0x2f, - 0x11, 0x24, 0x9e, 0x08, 0x69, 0x0e, 0x11, 0x24, 0xe3, 0x54, 0x10, 0x24, 0x73, 0x22, 0x08, 0x22, 0x80, 0x2e, 0xd7, - 0xb4, 0x10, 0x24, 0x38, 0x01, 0xc8, 0x0f, 0x10, 0x2f, 0x11, 0x24, 0x11, 0x08, 0x69, 0x0e, 0x11, 0x24, 0x6e, 0x48, - 0x10, 0x24, 0x2b, 0x28, 0x48, 0x22, 0xc0, 0x6f, 0x13, 0x24, 0xc1, 0x0a, 0x43, 0x0e, 0x10, 0x24, 0x0f, 0x23, 0x08, - 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0x10, 0x24, 0xd0, 0x1a, 0x80, 0x2e, 0xd7, 0xb4, 0xe2, 0x33, 0x5a, 0x0e, 0x77, 0x2f, - 0x12, 0x24, 0x0c, 0x08, 0x6a, 0x0e, 0x2a, 0x2f, 0x12, 0x24, 0xc5, 0x00, 0x4a, 0x0e, 0x08, 0x2f, 0x11, 0x36, 0x59, - 0x0e, 0x11, 0x24, 0xfd, 0x18, 0x10, 0x24, 0x75, 0x58, 0x08, 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0xc2, 0x34, 0x5a, 0x0e, - 0x0d, 0x2f, 0x11, 0x24, 0x36, 0x08, 0x69, 0x0e, 0x11, 0x24, 0x08, 0x58, 0x13, 0x24, 0x3b, 0x54, 0x4b, 0x22, 0x01, - 0xa2, 0x10, 0x24, 0xc6, 0x52, 0x08, 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0xb3, 0x36, 0x4b, 0x0e, 0x11, 0x24, 0x0e, 0x24, - 0x13, 0x24, 0x7b, 0x50, 0x59, 0x22, 0x0e, 0xa2, 0x10, 0x24, 0xf7, 0x56, 0x01, 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0xc2, - 0x35, 0x5a, 0x0e, 0x12, 0x2f, 0x01, 0xa2, 0x0c, 0x2f, 0x84, 0xa3, 0x10, 0x24, 0xd4, 0x58, 0x13, 0x24, 0x76, 0x56, - 0x03, 0x22, 0x73, 0x36, 0x4b, 0x0e, 0x11, 0x24, 0xeb, 0x52, 0x08, 0x22, 0x80, 0x2e, 0xd7, 0xb4, 0x10, 0x24, 0x87, - 0x16, 0x80, 0x2e, 0xd7, 0xb4, 0xb0, 0x6f, 0x13, 0x24, 0x02, 0xfd, 0x03, 0x0e, 0x29, 0x2f, 0x84, 0xa3, 0xc0, 0x6f, - 0x09, 0x2f, 0x11, 0x24, 0xe4, 0x0a, 0x41, 0x0e, 0x11, 0x24, 0x5d, 0x44, 0x10, 0x24, 0x2f, 0x5a, 0x08, 0x22, 0x80, - 0x2e, 0xd7, 0xb4, 0x13, 0x24, 0x96, 0x0c, 0x43, 0x0e, 0x0e, 0x2f, 0x40, 0x33, 0x48, 0x0e, 0x10, 0x24, 0xf2, 0x18, - 0x13, 0x24, 0x31, 0x49, 0x03, 0x22, 0x13, 0x24, 0x99, 0x00, 0x4b, 0x0e, 0x11, 0x24, 0xab, 0x18, 0x01, 0x22, 0x80, - 0x2e, 0xd7, 0xb4, 0x11, 0x24, 0xc6, 0x07, 0x69, 0x0e, 0x11, 0x24, 0xb0, 0x49, 0x10, 0x24, 0xbf, 0x17, 0x08, 0x22, - 0x80, 0x2e, 0xd7, 0xb4, 0x10, 0x24, 0x03, 0x15, 0x80, 0x2e, 0xd7, 0xb4, 0xb0, 0x32, 0x48, 0x0e, 0x57, 0x2f, 0xa0, - 0x37, 0x48, 0x0e, 0x13, 0x2f, 0x83, 0xa3, 0x08, 0x2f, 0x10, 0x24, 0xe0, 0x00, 0x48, 0x0e, 0x11, 0x24, 0xf6, 0x25, - 0x10, 0x24, 0x75, 0x17, 0x71, 0x2c, 0x08, 0x22, 0x10, 0x24, 0xa0, 0x00, 0x48, 0x0e, 0x11, 0x24, 0x7f, 0x18, 0x10, - 0x24, 0xa6, 0x13, 0x68, 0x2c, 0x08, 0x22, 0x11, 0x24, 0xf9, 0x07, 0x69, 0x0e, 0x0d, 0x2f, 0x11, 0x24, 0x10, 0x08, - 0x69, 0x0e, 0x11, 0x24, 0xb1, 0x14, 0x10, 0x24, 0x8e, 0x58, 0x48, 0x22, 0x90, 0x32, 0x58, 0x0e, 0x10, 0x24, 0x6d, - 0x14, 0x56, 0x2c, 0x01, 0x22, 0xc1, 0x6f, 0x10, 0x24, 0x68, 0x0c, 0x48, 0x0e, 0xb1, 0x6f, 0x0c, 0x2f, 0xcd, 0xa2, - 0x10, 0x24, 0x23, 0x14, 0x13, 0x24, 0x8d, 0x42, 0x03, 0x22, 0x13, 0x24, 0x2a, 0xfd, 0x0b, 0x0e, 0x11, 0x24, 0x53, - 0x12, 0x43, 0x2c, 0x08, 0x22, 0x10, 0x24, 0xcc, 0x07, 0x68, 0x0e, 0x0e, 0x2f, 0x10, 0x24, 0x08, 0xfd, 0x08, 0x0e, - 0x10, 0x24, 0x08, 0x16, 0x13, 0x24, 0x83, 0x45, 0x03, 0x22, 0x13, 0x24, 0xa1, 0xfd, 0x0b, 0x0e, 0x11, 0x24, 0xa6, - 0x14, 0x30, 0x2c, 0x01, 0x22, 0x10, 0x24, 0x5b, 0x01, 0x08, 0x0e, 0x11, 0x24, 0x2f, 0x12, 0x10, 0x24, 0xdd, 0x44, - 0x27, 0x2c, 0x08, 0x22, 0xdb, 0xa2, 0x0f, 0x2f, 0xc1, 0x6f, 0x10, 0x24, 0xb2, 0x0b, 0x48, 0x0e, 0x11, 0x24, 0x21, - 0x55, 0x10, 0x24, 0xc8, 0x14, 0x48, 0x22, 0x10, 0x24, 0x4c, 0x08, 0x68, 0x0e, 0x10, 0x24, 0xe4, 0x57, 0x15, 0x2c, - 0x01, 0x22, 0x44, 0xa2, 0x0f, 0x2f, 0xc1, 0x6f, 0x10, 0x24, 0xcb, 0x0b, 0x48, 0x0e, 0x11, 0x24, 0x09, 0x58, 0x10, - 0x24, 0xe4, 0x10, 0x48, 0x22, 0x10, 0x24, 0x4d, 0x08, 0x68, 0x0e, 0x10, 0x24, 0x1a, 0x12, 0x03, 0x2c, 0x01, 0x22, - 0x10, 0x24, 0x0c, 0x10, 0xfb, 0x6f, 0xb0, 0x5f, 0xb8, 0x2e, 0xa3, 0x32, 0xc3, 0x00, 0x60, 0x51, 0xc2, 0x40, 0x81, - 0x84, 0xd3, 0x7f, 0xd2, 0x42, 0xe0, 0x7f, 0x00, 0x30, 0xc4, 0x40, 0x20, 0x02, 0xc3, 0x7f, 0xd0, 0x42, 0x42, 0x3d, - 0xc0, 0x40, 0x01, 0x80, 0xc0, 0x42, 0xda, 0x00, 0x93, 0x7f, 0xb1, 0x7f, 0xab, 0x7f, 0x98, 0x2e, 0xb3, 0xc0, 0x91, - 0x6f, 0xf3, 0x32, 0x40, 0x42, 0x00, 0xac, 0x8b, 0x00, 0x02, 0x2f, 0xe1, 0x6f, 0x39, 0x56, 0x43, 0x42, 0xa1, 0x82, - 0x91, 0x7f, 0x33, 0x33, 0x4b, 0x00, 0x81, 0x7f, 0x13, 0x3c, 0x4b, 0x00, 0x80, 0x40, 0x53, 0x34, 0xb5, 0x6f, 0x8b, - 0x00, 0x0d, 0xb0, 0x43, 0x87, 0x76, 0x7f, 0xb2, 0x7f, 0x63, 0x7f, 0x65, 0x25, 0xb5, 0x6f, 0x92, 0x41, 0x63, 0x41, - 0x64, 0x41, 0x44, 0x81, 0x56, 0x7f, 0x41, 0x7f, 0x00, 0x2e, 0x26, 0x40, 0x27, 0x40, 0x45, 0x41, 0xf7, 0x7f, 0xb0, - 0x7f, 0x98, 0x2e, 0x67, 0xcc, 0x81, 0x6f, 0x0f, 0xa4, 0x43, 0x40, 0x72, 0x6f, 0x94, 0x6f, 0x05, 0x30, 0x01, 0x2f, - 0xc0, 0xa0, 0x03, 0x2f, 0x31, 0xac, 0x07, 0x2f, 0xc0, 0xa4, 0x05, 0x2f, 0xa2, 0x00, 0xeb, 0x04, 0x80, 0x40, 0x01, - 0x80, 0x43, 0x42, 0x80, 0x42, 0x41, 0x86, 0x56, 0x6f, 0x62, 0x6f, 0x41, 0x6f, 0x42, 0x82, 0x72, 0x0e, 0x83, 0x7f, - 0xd5, 0x2f, 0x53, 0x32, 0x8b, 0x00, 0xa1, 0x86, 0x56, 0x25, 0xf0, 0x82, 0x82, 0x40, 0x8d, 0xb0, 0x52, 0x40, 0xde, - 0x00, 0x91, 0x7f, 0xb3, 0x7f, 0x85, 0x7f, 0xb3, 0x30, 0x7b, 0x52, 0x98, 0x2e, 0x5a, 0xca, 0x1a, 0x25, 0x83, 0x6f, - 0x6d, 0x82, 0xfd, 0x88, 0x50, 0x7f, 0x71, 0x7f, 0x81, 0x7f, 0x05, 0x30, 0x83, 0x30, 0x00, 0x30, 0x11, 0x41, 0x52, - 0x6f, 0x25, 0x7f, 0x30, 0x7f, 0x44, 0x7f, 0x98, 0x2e, 0x0f, 0xca, 0x73, 0x6f, 0x20, 0x25, 0x90, 0x6f, 0x7d, 0x52, - 0xd2, 0x42, 0x73, 0x7f, 0x12, 0x7f, 0x98, 0x2e, 0x86, 0xb7, 0x93, 0x6f, 0x11, 0x6f, 0xd2, 0x40, 0x0a, 0x18, 0x31, - 0x6f, 0x0e, 0x00, 0x93, 0x7f, 0x83, 0x30, 0x44, 0x6f, 0x21, 0x6f, 0x62, 0x6f, 0x62, 0x0e, 0x4f, 0x03, 0xe1, 0x2f, - 0x33, 0x52, 0x01, 0x00, 0x01, 0x30, 0x69, 0x03, 0x3a, 0x25, 0xea, 0x82, 0x92, 0x6f, 0xf0, 0x86, 0xd1, 0xbe, 0x0f, - 0xb8, 0xbd, 0x84, 0x94, 0x7f, 0x05, 0x0a, 0x23, 0x7f, 0x52, 0x7f, 0x40, 0x7f, 0x31, 0x7f, 0x71, 0x7f, 0xd3, 0x30, - 0x84, 0x6f, 0x55, 0x6f, 0x10, 0x41, 0x52, 0x41, 0x41, 0x6f, 0x55, 0x7f, 0x10, 0x7f, 0x04, 0x7f, 0x98, 0x2e, 0x0f, - 0xca, 0x11, 0x6f, 0x20, 0x25, 0x98, 0x2e, 0xfe, 0xc9, 0x31, 0x6f, 0x04, 0x6f, 0x50, 0x42, 0x31, 0x7f, 0xd3, 0x30, - 0x21, 0x6f, 0x61, 0x0e, 0xea, 0x2f, 0xb1, 0x6f, 0x41, 0x84, 0x32, 0x25, 0x90, 0x40, 0x84, 0x40, 0x71, 0x6f, 0xb4, - 0x7f, 0x72, 0x7f, 0x40, 0x7f, 0x33, 0x7f, 0x98, 0x2e, 0xb3, 0xc0, 0x53, 0x6f, 0xb1, 0x32, 0x99, 0x00, 0x83, 0xb9, - 0x41, 0x6f, 0x4b, 0x00, 0xb0, 0x6f, 0x03, 0x30, 0xc3, 0x02, 0x84, 0x40, 0xb2, 0x7f, 0xa1, 0x84, 0x0d, 0xb1, 0x52, - 0x7f, 0x56, 0x01, 0x74, 0x6f, 0x30, 0x6f, 0x92, 0x6f, 0x43, 0x8b, 0x03, 0x43, 0x01, 0x42, 0x95, 0x7f, 0xbd, 0x86, - 0x51, 0x41, 0x41, 0x7f, 0x75, 0x7f, 0x00, 0x2e, 0xd1, 0x40, 0x42, 0x41, 0x32, 0x7f, 0x23, 0x7f, 0x98, 0x2e, 0x74, - 0xc0, 0x41, 0x6f, 0xc8, 0x00, 0x90, 0x6f, 0x01, 0x30, 0x75, 0x6f, 0x32, 0x6f, 0x03, 0x42, 0x91, 0x02, 0x23, 0x6f, - 0x61, 0x6f, 0x59, 0x0e, 0x62, 0x43, 0x95, 0x7f, 0xe7, 0x2f, 0xb2, 0x6f, 0x51, 0x6f, 0x82, 0x40, 0x8d, 0xb0, 0x8e, - 0x00, 0xfd, 0x8a, 0xb2, 0x7f, 0x02, 0x30, 0x79, 0x52, 0x05, 0x25, 0x03, 0x30, 0x54, 0x40, 0xec, 0x01, 0x16, 0x40, - 0x43, 0x89, 0xc7, 0x41, 0x37, 0x18, 0x3d, 0x8b, 0x96, 0x00, 0x44, 0x0e, 0xdf, 0x02, 0xf4, 0x2f, 0x09, 0x52, 0x51, - 0x00, 0x02, 0x30, 0x9a, 0x02, 0xb5, 0x6f, 0x45, 0x87, 0x1b, 0xba, 0x25, 0xbc, 0x51, 0x6f, 0x4d, 0x8b, 0x7a, 0x82, - 0xc6, 0x40, 0x20, 0x0a, 0x30, 0x00, 0xd0, 0x42, 0x2b, 0xb5, 0xc0, 0x40, 0x82, 0x02, 0x40, 0x34, 0x08, 0x00, 0xd2, - 0x42, 0xb0, 0x7f, 0x75, 0x7f, 0x93, 0x7f, 0x00, 0x2e, 0xb5, 0x6f, 0xe2, 0x6f, 0x63, 0x41, 0x64, 0x41, 0x44, 0x8f, - 0x82, 0x40, 0xe6, 0x41, 0xc0, 0x41, 0xc4, 0x8f, 0x45, 0x41, 0xf0, 0x7f, 0xb7, 0x7f, 0x61, 0x7f, 0x98, 0x2e, 0x67, - 0xcc, 0x00, 0x18, 0x09, 0x52, 0x71, 0x00, 0x03, 0x30, 0xbb, 0x02, 0x1b, 0xba, 0x93, 0x6f, 0x25, 0xbc, 0x61, 0x6f, - 0xc5, 0x40, 0x42, 0x82, 0x20, 0x0a, 0x28, 0x00, 0xd0, 0x42, 0x2b, 0xb9, 0xc0, 0x40, 0x82, 0x02, 0xd2, 0x42, 0x93, - 0x7f, 0x00, 0x2e, 0x72, 0x6f, 0x5a, 0x0e, 0xd9, 0x2f, 0xb1, 0x6f, 0xf3, 0x3c, 0xcb, 0x00, 0xda, 0x82, 0xc3, 0x40, - 0x41, 0x40, 0x59, 0x0e, 0x50, 0x2f, 0xe1, 0x6f, 0xe3, 0x32, 0xcb, 0x00, 0xb3, 0x7f, 0x22, 0x30, 0xc0, 0x40, 0x01, - 0x80, 0xc0, 0x42, 0x02, 0xa2, 0x30, 0x2f, 0xc2, 0x42, 0x98, 0x2e, 0x83, 0xb1, 0xe1, 0x6f, 0xb3, 0x35, 0xcb, 0x00, - 0x24, 0x3d, 0xc2, 0x40, 0xdc, 0x00, 0x84, 0x40, 0x00, 0x91, 0x93, 0x7f, 0x02, 0x2f, 0x00, 0x2e, 0x06, 0x2c, 0x0c, - 0xb8, 0x30, 0x25, 0x00, 0x33, 0x48, 0x00, 0x98, 0x2e, 0xf6, 0xb6, 0x91, 0x6f, 0x90, 0x7f, 0x00, 0x2e, 0x44, 0x40, - 0x20, 0x1a, 0x15, 0x2f, 0xd3, 0x6f, 0xc1, 0x6f, 0xc3, 0x40, 0x35, 0x5a, 0x42, 0x40, 0xd3, 0x7e, 0x08, 0xbc, 0x25, - 0x09, 0xe2, 0x7e, 0xc4, 0x0a, 0x42, 0x82, 0xf3, 0x7e, 0xd1, 0x7f, 0x34, 0x30, 0x83, 0x6f, 0x82, 0x30, 0x31, 0x30, - 0x98, 0x2e, 0xb3, 0x00, 0xd1, 0x6f, 0x93, 0x6f, 0x43, 0x42, 0xf0, 0x32, 0xb1, 0x6f, 0x41, 0x82, 0xe2, 0x6f, 0x43, - 0x40, 0xc1, 0x86, 0xc2, 0xa2, 0x43, 0x42, 0x03, 0x30, 0x02, 0x2f, 0x90, 0x00, 0x00, 0x2e, 0x83, 0x42, 0x61, 0x88, - 0x42, 0x40, 0x8d, 0xb0, 0x26, 0x00, 0x98, 0x2e, 0xd9, 0x03, 0x1c, 0x83, 0x00, 0x2e, 0x43, 0x42, 0x00, 0x2e, 0xab, - 0x6f, 0xa0, 0x5e, 0xb8, 0x2e, 0xb1, 0x35, 0x40, 0x51, 0x41, 0x01, 0x02, 0x30, 0x1a, 0x25, 0x13, 0x30, 0x40, 0x25, - 0x12, 0x42, 0x45, 0x0e, 0xfc, 0x2f, 0x65, 0x34, 0x28, 0x80, 0x25, 0x01, 0x13, 0x42, 0x44, 0x0e, 0xfc, 0x2f, 0x27, - 0x80, 0x65, 0x56, 0x03, 0x42, 0x15, 0x80, 0xa3, 0x30, 0x03, 0x42, 0x04, 0x80, 0x4d, 0x56, 0x7f, 0x58, 0x13, 0x42, - 0xd4, 0x7e, 0xc2, 0x7e, 0xf2, 0x7e, 0x6c, 0x8c, 0x81, 0x56, 0x83, 0x58, 0xe3, 0x7e, 0x04, 0x7f, 0x71, 0x8a, 0x97, - 0x41, 0x17, 0x42, 0x75, 0x0e, 0xfb, 0x2f, 0x85, 0x5c, 0x87, 0x5e, 0x16, 0x7f, 0x36, 0x7f, 0x27, 0x7f, 0x00, 0x2e, - 0x89, 0x5c, 0x8b, 0x5e, 0x46, 0x7f, 0x57, 0x7f, 0x76, 0x8c, 0x57, 0x41, 0x17, 0x42, 0x6e, 0x0e, 0xfb, 0x2f, 0x8d, - 0x5a, 0x8f, 0x5e, 0x65, 0x7f, 0x87, 0x7f, 0x72, 0x7f, 0x00, 0x2e, 0x91, 0x5a, 0x93, 0x5e, 0x95, 0x7f, 0xa7, 0x7f, - 0x7b, 0x8a, 0x97, 0x41, 0x17, 0x42, 0x75, 0x0e, 0xfb, 0x2f, 0x7f, 0x5c, 0xb2, 0x7f, 0xc6, 0x7f, 0xd3, 0x7f, 0xe2, - 0x7f, 0xf4, 0x7f, 0x40, 0x82, 0x52, 0x41, 0x12, 0x42, 0x69, 0x0e, 0xfb, 0x2f, 0xc0, 0x5e, 0xb8, 0x2e, 0x03, 0x2e, - 0x2d, 0x02, 0x9f, 0xbc, 0x9f, 0xb8, 0x20, 0x50, 0x40, 0xb2, 0x14, 0x2f, 0x10, 0x25, 0x01, 0x2e, 0x8d, 0x00, 0x00, - 0x90, 0x0b, 0x2f, 0x97, 0x50, 0xf1, 0x7f, 0xeb, 0x7f, 0x98, 0x2e, 0x83, 0xb6, 0x01, 0x2e, 0x8d, 0x00, 0x01, 0x80, - 0x21, 0x2e, 0x8d, 0x00, 0xf1, 0x6f, 0xeb, 0x6f, 0xe0, 0x5f, 0x97, 0x50, 0x80, 0x2e, 0xda, 0xb4, 0x00, 0x30, 0x21, - 0x2e, 0x8d, 0x00, 0xe0, 0x5f, 0xb8, 0x2e, 0x41, 0x25, 0x42, 0x8a, 0x50, 0x50, 0x99, 0x52, 0x81, 0x80, 0x99, 0x09, - 0xf5, 0x7f, 0x52, 0x25, 0x07, 0x52, 0x03, 0x8e, 0xd9, 0x08, 0x02, 0x40, 0x03, 0x81, 0x44, 0x83, 0x6c, 0xbb, 0xda, - 0x0e, 0xe7, 0x7f, 0xdb, 0x7f, 0x20, 0x2f, 0x02, 0x41, 0x32, 0x1a, 0x1d, 0x2f, 0x42, 0x85, 0x00, 0x2e, 0x82, 0x40, - 0xda, 0x0e, 0x03, 0x30, 0x05, 0x2f, 0xf1, 0x6f, 0x06, 0x30, 0x42, 0x40, 0x81, 0x84, 0x18, 0x2c, 0x42, 0x42, 0xbf, - 0x85, 0x82, 0x00, 0x41, 0x40, 0x86, 0x40, 0x81, 0x8d, 0x86, 0x42, 0x20, 0x25, 0x13, 0x30, 0x06, 0x30, 0x97, 0x40, - 0x81, 0x8d, 0xf9, 0x0f, 0x09, 0x2f, 0x85, 0xa3, 0xf9, 0x2f, 0x03, 0x30, 0x06, 0x2c, 0x06, 0x30, 0x9b, 0x52, 0xd9, - 0x0e, 0x13, 0x30, 0x01, 0x30, 0xd9, 0x22, 0xc0, 0xb2, 0x12, 0x83, 0xc1, 0x7f, 0x03, 0x30, 0xb4, 0x7f, 0x06, 0x2f, - 0x51, 0x30, 0x70, 0x25, 0x98, 0x2e, 0xe3, 0x03, 0xff, 0x81, 0x00, 0x2e, 0x03, 0x42, 0x43, 0x8b, 0xe0, 0x6f, 0xf1, - 0x6f, 0x00, 0x40, 0x41, 0x40, 0xc8, 0x0f, 0x37, 0x2f, 0x00, 0x41, 0x80, 0xa7, 0x3c, 0x2f, 0x01, 0x83, 0x47, 0x8e, - 0x42, 0x40, 0xfa, 0x01, 0x81, 0x84, 0x08, 0x89, 0x45, 0x41, 0x55, 0x0e, 0xc6, 0x43, 0x42, 0x42, 0xf4, 0x7f, 0x00, - 0x2f, 0x43, 0x42, 0x51, 0x82, 0x70, 0x1a, 0x41, 0x40, 0x06, 0x2f, 0xc3, 0x6f, 0x41, 0x82, 0xc1, 0x42, 0xcd, 0x0e, - 0x26, 0x2f, 0xc5, 0x42, 0x25, 0x2d, 0x7f, 0x82, 0x51, 0xbb, 0xa5, 0x00, 0xce, 0x0f, 0x12, 0x2f, 0x14, 0x30, 0x05, - 0x30, 0xf7, 0x6f, 0x06, 0x30, 0x05, 0x2c, 0xe0, 0x7f, 0xd0, 0x41, 0x05, 0x1a, 0x23, 0x22, 0xb0, 0x01, 0x7a, 0x0e, - 0xf9, 0x2f, 0x71, 0x0f, 0xe0, 0x6f, 0x28, 0x22, 0x41, 0x8b, 0x71, 0x22, 0x45, 0xa7, 0xee, 0x2f, 0xb3, 0x6f, 0xc2, - 0x6f, 0xc0, 0x42, 0x81, 0x42, 0x08, 0x2d, 0x04, 0x25, 0xc4, 0x6f, 0x98, 0x2e, 0xea, 0x03, 0x00, 0x2e, 0x40, 0x41, - 0x00, 0x43, 0x00, 0x30, 0xdb, 0x6f, 0xb0, 0x5f, 0xb8, 0x2e, 0x10, 0x50, 0x03, 0x40, 0x19, 0x18, 0x37, 0x56, 0x19, - 0x05, 0x36, 0x25, 0xf7, 0x7f, 0x4a, 0x17, 0x54, 0x18, 0xec, 0x18, 0x09, 0x17, 0x01, 0x30, 0x0c, 0x07, 0xe2, 0x18, - 0xde, 0x00, 0xf2, 0x6f, 0x97, 0x02, 0x33, 0x58, 0xdc, 0x00, 0x91, 0x02, 0xbf, 0xb8, 0x21, 0xbd, 0x8a, 0x0a, 0xc0, - 0x2e, 0x02, 0x42, 0xf0, 0x5f, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, - 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, - 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, - 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, - 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, - 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, - 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, - 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, - 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, - 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, - 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x01, 0x2e, 0x5d, 0xf7, 0x08, 0xbc, 0x80, 0xac, 0x0e, 0xbb, 0x02, 0x2f, - 0x00, 0x30, 0x41, 0x04, 0x82, 0x06, 0xc0, 0xa4, 0x00, 0x30, 0x11, 0x2f, 0x40, 0xa9, 0x03, 0x2f, 0x40, 0x91, 0x0d, - 0x2f, 0x00, 0xa7, 0x0b, 0x2f, 0x80, 0xb3, 0x33, 0x58, 0x02, 0x2f, 0x90, 0xa1, 0x26, 0x13, 0x20, 0x23, 0x80, 0x90, - 0x10, 0x30, 0x01, 0x2f, 0xcc, 0x0e, 0x00, 0x2f, 0x00, 0x30, 0xb8, 0x2e, 0x35, 0x50, 0x18, 0x08, 0x08, 0xbc, 0x88, - 0xb6, 0x0d, 0x17, 0xc6, 0xbd, 0x56, 0xbc, 0x37, 0x58, 0xda, 0xba, 0x04, 0x01, 0x1d, 0x0a, 0x10, 0x50, 0x05, 0x30, - 0x32, 0x25, 0x45, 0x03, 0xfb, 0x7f, 0xf6, 0x30, 0x21, 0x25, 0x98, 0x2e, 0x37, 0xca, 0x16, 0xb5, 0x9a, 0xbc, 0x06, - 0xb8, 0x80, 0xa8, 0x41, 0x0a, 0x0e, 0x2f, 0x80, 0x90, 0x02, 0x2f, 0x39, 0x50, 0x48, 0x0f, 0x09, 0x2f, 0xbf, 0xa0, - 0x04, 0x2f, 0xbf, 0x90, 0x06, 0x2f, 0x37, 0x54, 0xca, 0x0f, 0x03, 0x2f, 0x00, 0x2e, 0x02, 0x2c, 0x37, 0x52, 0x39, - 0x52, 0xf2, 0x33, 0x98, 0x2e, 0xd9, 0xc0, 0xfb, 0x6f, 0xf1, 0x37, 0xc0, 0x2e, 0x01, 0x08, 0xf0, 0x5f, 0x41, 0x56, - 0x3b, 0x54, 0xd0, 0x40, 0xc4, 0x40, 0x0b, 0x2e, 0xfd, 0xf3, 0x41, 0x52, 0x90, 0x42, 0x94, 0x42, 0x95, 0x42, 0x05, - 0x30, 0x43, 0x50, 0x0f, 0x88, 0x06, 0x40, 0x04, 0x41, 0x96, 0x42, 0xc5, 0x42, 0x48, 0xbe, 0x73, 0x30, 0x0d, 0x2e, - 0x88, 0x00, 0x4f, 0xba, 0x84, 0x42, 0x03, 0x42, 0x81, 0xb3, 0x02, 0x2f, 0x2b, 0x2e, 0x6f, 0xf5, 0x06, 0x2d, 0x05, - 0x2e, 0x77, 0xf7, 0x3f, 0x56, 0x93, 0x08, 0x25, 0x2e, 0x77, 0xf7, 0x3d, 0x54, 0x25, 0x2e, 0xc2, 0xf5, 0x07, 0x2e, - 0xfd, 0xf3, 0x42, 0x30, 0xb4, 0x33, 0xda, 0x0a, 0x4c, 0x00, 0x27, 0x2e, 0xfd, 0xf3, 0x43, 0x40, 0xd4, 0x3f, 0xdc, - 0x08, 0x43, 0x42, 0x00, 0x2e, 0x00, 0x2e, 0x43, 0x40, 0x24, 0x30, 0xdc, 0x0a, 0x43, 0x42, 0x04, 0x80, 0x03, 0x2e, - 0xfd, 0xf3, 0x4a, 0x0a, 0x23, 0x2e, 0xfd, 0xf3, 0x61, 0x34, 0xc0, 0x2e, 0x01, 0x42, 0x00, 0x2e, 0x60, 0x50, 0x1a, - 0x25, 0x7a, 0x86, 0xe0, 0x7f, 0xf3, 0x7f, 0x03, 0x25, 0x45, 0x52, 0x41, 0x84, 0xdb, 0x7f, 0x33, 0x30, 0x98, 0x2e, - 0x16, 0xc2, 0x1a, 0x25, 0x7d, 0x82, 0xf0, 0x6f, 0xe2, 0x6f, 0x32, 0x25, 0x16, 0x40, 0x94, 0x40, 0x26, 0x01, 0x85, - 0x40, 0x8e, 0x17, 0xc4, 0x42, 0x6e, 0x03, 0x95, 0x42, 0x41, 0x0e, 0xf4, 0x2f, 0xdb, 0x6f, 0xa0, 0x5f, 0xb8, 0x2e, - 0xb0, 0x51, 0xfb, 0x7f, 0x98, 0x2e, 0xe8, 0x0d, 0x5a, 0x25, 0x98, 0x2e, 0x0f, 0x0e, 0x4f, 0x58, 0x32, 0x87, 0xc4, - 0x7f, 0x65, 0x89, 0x6b, 0x8d, 0x47, 0x5a, 0x65, 0x7f, 0xe1, 0x7f, 0x83, 0x7f, 0xa6, 0x7f, 0x74, 0x7f, 0xd0, 0x7f, - 0xb6, 0x7f, 0x94, 0x7f, 0x17, 0x30, 0x49, 0x52, 0x4b, 0x54, 0x51, 0x7f, 0x00, 0x2e, 0x85, 0x6f, 0x42, 0x7f, 0x00, - 0x2e, 0x51, 0x41, 0x45, 0x81, 0x42, 0x41, 0x13, 0x40, 0x3b, 0x8a, 0x00, 0x40, 0x4b, 0x04, 0xd0, 0x06, 0xc0, 0xac, - 0x85, 0x7f, 0x02, 0x2f, 0x02, 0x30, 0x51, 0x04, 0xd3, 0x06, 0x41, 0x84, 0x05, 0x30, 0x5d, 0x02, 0xc9, 0x16, 0xdf, - 0x08, 0xd3, 0x00, 0x8d, 0x02, 0xaf, 0xbc, 0xb1, 0xb9, 0x59, 0x0a, 0x65, 0x6f, 0x11, 0x43, 0xa1, 0xb4, 0x52, 0x41, - 0x53, 0x41, 0x01, 0x43, 0x34, 0x7f, 0x65, 0x7f, 0x26, 0x31, 0xe5, 0x6f, 0xd4, 0x6f, 0x98, 0x2e, 0x37, 0xca, 0x32, - 0x6f, 0x75, 0x6f, 0x83, 0x40, 0x42, 0x41, 0x23, 0x7f, 0x12, 0x7f, 0xf6, 0x30, 0x40, 0x25, 0x51, 0x25, 0x98, 0x2e, - 0x37, 0xca, 0x14, 0x6f, 0x20, 0x05, 0x70, 0x6f, 0x25, 0x6f, 0x69, 0x07, 0xa2, 0x6f, 0x31, 0x6f, 0x0b, 0x30, 0x04, - 0x42, 0x9b, 0x42, 0x8b, 0x42, 0x55, 0x42, 0x32, 0x7f, 0x40, 0xa9, 0xc3, 0x6f, 0x71, 0x7f, 0x02, 0x30, 0xd0, 0x40, - 0xc3, 0x7f, 0x03, 0x2f, 0x40, 0x91, 0x15, 0x2f, 0x00, 0xa7, 0x13, 0x2f, 0x00, 0xa4, 0x11, 0x2f, 0x84, 0xbd, 0x98, - 0x2e, 0x79, 0xca, 0x55, 0x6f, 0x37, 0x54, 0x54, 0x41, 0x82, 0x00, 0xf3, 0x3f, 0x45, 0x41, 0xcb, 0x02, 0xf6, 0x30, - 0x98, 0x2e, 0x37, 0xca, 0x35, 0x6f, 0xa4, 0x6f, 0x41, 0x43, 0x03, 0x2c, 0x00, 0x43, 0xa4, 0x6f, 0x35, 0x6f, 0x17, - 0x30, 0x42, 0x6f, 0x51, 0x6f, 0x93, 0x40, 0x42, 0x82, 0x00, 0x41, 0xc3, 0x00, 0x03, 0x43, 0x51, 0x7f, 0x00, 0x2e, - 0x94, 0x40, 0x41, 0x41, 0x4c, 0x02, 0xc4, 0x6f, 0x55, 0x56, 0x63, 0x0e, 0x74, 0x6f, 0x51, 0x43, 0xa5, 0x7f, 0x8a, - 0x2f, 0x09, 0x2e, 0x88, 0x00, 0x01, 0xb3, 0x21, 0x2f, 0x4f, 0x58, 0x90, 0x6f, 0x13, 0x41, 0xb6, 0x6f, 0xe4, 0x7f, - 0x00, 0x2e, 0x91, 0x41, 0x14, 0x40, 0x92, 0x41, 0x15, 0x40, 0x17, 0x2e, 0x6f, 0xf5, 0xb6, 0x7f, 0xd0, 0x7f, 0xcb, - 0x7f, 0x98, 0x2e, 0x00, 0x0c, 0x07, 0x15, 0xc2, 0x6f, 0x14, 0x0b, 0x29, 0x2e, 0x6f, 0xf5, 0xc3, 0xa3, 0xc1, 0x8f, - 0xe4, 0x6f, 0xd0, 0x6f, 0xe6, 0x2f, 0x14, 0x30, 0x05, 0x2e, 0x6f, 0xf5, 0x14, 0x0b, 0x29, 0x2e, 0x6f, 0xf5, 0x18, - 0x2d, 0x51, 0x56, 0x04, 0x32, 0xb5, 0x6f, 0x1c, 0x01, 0x51, 0x41, 0x52, 0x41, 0xc3, 0x40, 0xb5, 0x7f, 0xe4, 0x7f, - 0x98, 0x2e, 0x1f, 0x0c, 0xe4, 0x6f, 0x21, 0x87, 0x00, 0x43, 0x04, 0x32, 0x53, 0x54, 0x5a, 0x0e, 0xef, 0x2f, 0x4d, - 0x54, 0x09, 0x2e, 0x77, 0xf7, 0x22, 0x0b, 0x29, 0x2e, 0x77, 0xf7, 0xfb, 0x6f, 0x50, 0x5e, 0xb8, 0x2e, 0x10, 0x50, - 0x01, 0x2e, 0x84, 0x00, 0x00, 0xb2, 0xfb, 0x7f, 0x51, 0x2f, 0x01, 0xb2, 0x48, 0x2f, 0x02, 0xb2, 0x42, 0x2f, 0x03, - 0x90, 0x56, 0x2f, 0x5b, 0x52, 0x79, 0x80, 0x42, 0x40, 0x81, 0x84, 0x00, 0x40, 0x42, 0x42, 0x98, 0x2e, 0x93, 0x0c, - 0x5d, 0x54, 0x5b, 0x50, 0xa1, 0x40, 0x98, 0xbd, 0x82, 0x40, 0x3e, 0x82, 0xda, 0x0a, 0x44, 0x40, 0x8b, 0x16, 0xe3, - 0x00, 0x53, 0x42, 0x00, 0x2e, 0x43, 0x40, 0x9a, 0x02, 0x52, 0x42, 0x00, 0x2e, 0x41, 0x40, 0x4d, 0x54, 0x4a, 0x0e, - 0x3a, 0x2f, 0x3a, 0x82, 0x00, 0x30, 0x41, 0x40, 0x21, 0x2e, 0x6c, 0x0f, 0x40, 0xb2, 0x0a, 0x2f, 0x98, 0x2e, 0xb1, - 0x0c, 0x98, 0x2e, 0x45, 0x0e, 0x98, 0x2e, 0x5b, 0x0e, 0xfb, 0x6f, 0xf0, 0x5f, 0x00, 0x30, 0x80, 0x2e, 0xba, 0x03, - 0x61, 0x52, 0x57, 0x54, 0x42, 0x42, 0x4f, 0x84, 0x73, 0x30, 0x5f, 0x52, 0x83, 0x42, 0x1b, 0x30, 0x6b, 0x42, 0x23, - 0x30, 0x27, 0x2e, 0x87, 0x00, 0x37, 0x2e, 0x84, 0x00, 0x21, 0x2e, 0x86, 0x00, 0x7a, 0x84, 0x17, 0x2c, 0x42, 0x42, - 0x30, 0x30, 0x21, 0x2e, 0x84, 0x00, 0x12, 0x2d, 0x21, 0x30, 0x00, 0x30, 0x23, 0x2e, 0x84, 0x00, 0x21, 0x2e, 0x7b, - 0xf7, 0x0b, 0x2d, 0x17, 0x30, 0x98, 0x2e, 0x51, 0x0c, 0x59, 0x50, 0x0c, 0x82, 0x72, 0x30, 0x2f, 0x2e, 0x84, 0x00, - 0x25, 0x2e, 0x7b, 0xf7, 0x40, 0x42, 0x00, 0x2e, 0xfb, 0x6f, 0xf0, 0x5f, 0xb8, 0x2e, 0x70, 0x50, 0x0a, 0x25, 0x39, - 0x86, 0xfb, 0x7f, 0xe1, 0x32, 0x62, 0x30, 0x98, 0x2e, 0xc2, 0xc4, 0x35, 0x56, 0xa5, 0x6f, 0xab, 0x08, 0x91, 0x6f, - 0x4b, 0x08, 0x63, 0x56, 0xc4, 0x6f, 0x23, 0x09, 0x4d, 0xba, 0x93, 0xbc, 0x8c, 0x0b, 0xd1, 0x6f, 0x0b, 0x09, 0x4f, - 0x52, 0x65, 0x5e, 0x56, 0x42, 0xaf, 0x09, 0x4d, 0xba, 0x23, 0xbd, 0x94, 0x0a, 0xe5, 0x6f, 0x68, 0xbb, 0xeb, 0x08, - 0xbd, 0xb9, 0x63, 0xbe, 0xfb, 0x6f, 0x52, 0x42, 0xe3, 0x0a, 0xc0, 0x2e, 0x43, 0x42, 0x90, 0x5f, 0x55, 0x50, 0x03, - 0x2e, 0x25, 0xf3, 0x13, 0x40, 0x00, 0x40, 0x9b, 0xbc, 0x9b, 0xb4, 0x08, 0xbd, 0xb8, 0xb9, 0x98, 0xbc, 0xda, 0x0a, - 0x08, 0xb6, 0x89, 0x16, 0xc0, 0x2e, 0x19, 0x00, 0x62, 0x02, 0x10, 0x50, 0xfb, 0x7f, 0x98, 0x2e, 0x81, 0x0d, 0x01, - 0x2e, 0x84, 0x00, 0x31, 0x30, 0x08, 0x04, 0xfb, 0x6f, 0x01, 0x30, 0xf0, 0x5f, 0x23, 0x2e, 0x86, 0x00, 0x21, 0x2e, - 0x87, 0x00, 0xb8, 0x2e, 0x01, 0x2e, 0x87, 0x00, 0x03, 0x2e, 0x86, 0x00, 0x48, 0x0e, 0x01, 0x2f, 0x80, 0x2e, 0x1f, - 0x0e, 0xb8, 0x2e, 0x67, 0x50, 0x21, 0x34, 0x01, 0x42, 0x82, 0x30, 0xc1, 0x32, 0x25, 0x2e, 0x62, 0xf5, 0x01, 0x00, - 0x22, 0x30, 0x01, 0x40, 0x4a, 0x0a, 0x01, 0x42, 0xb8, 0x2e, 0x67, 0x54, 0xf0, 0x3b, 0x83, 0x40, 0xd8, 0x08, 0x69, - 0x52, 0x83, 0x42, 0x00, 0x30, 0x83, 0x30, 0x50, 0x42, 0xc4, 0x32, 0x27, 0x2e, 0x64, 0xf5, 0x94, 0x00, 0x50, 0x42, - 0x40, 0x42, 0xd3, 0x3f, 0x84, 0x40, 0x7d, 0x82, 0xe3, 0x08, 0x40, 0x42, 0x83, 0x42, 0xb8, 0x2e, 0x61, 0x52, 0x00, - 0x30, 0x40, 0x42, 0x7c, 0x86, 0x3b, 0x52, 0x09, 0x2e, 0x57, 0x0f, 0x41, 0x54, 0xc4, 0x42, 0xd3, 0x86, 0x54, 0x40, - 0x55, 0x40, 0x94, 0x42, 0x85, 0x42, 0x21, 0x2e, 0x87, 0x00, 0x42, 0x40, 0x25, 0x2e, 0xfd, 0xf3, 0xc0, 0x42, 0x7e, - 0x82, 0x05, 0x2e, 0x79, 0x00, 0x80, 0xb2, 0x14, 0x2f, 0x05, 0x2e, 0x24, 0x02, 0x27, 0xbd, 0x2f, 0xb9, 0x80, 0x90, - 0x02, 0x2f, 0x21, 0x2e, 0x6f, 0xf5, 0x0c, 0x2d, 0x07, 0x2e, 0x58, 0x0f, 0x14, 0x30, 0x1c, 0x09, 0x05, 0x2e, 0x77, - 0xf7, 0x3f, 0x56, 0x47, 0xbe, 0x93, 0x08, 0x94, 0x0a, 0x25, 0x2e, 0x77, 0xf7, 0x6b, 0x54, 0x50, 0x42, 0x4a, 0x0e, - 0xfc, 0x2f, 0xb8, 0x2e, 0x50, 0x50, 0x02, 0x30, 0x43, 0x86, 0x69, 0x50, 0xfb, 0x7f, 0xe3, 0x7f, 0xd2, 0x7f, 0xc0, - 0x7f, 0xb1, 0x7f, 0x00, 0x2e, 0x41, 0x40, 0x00, 0x40, 0x48, 0x04, 0x98, 0x2e, 0x74, 0xc0, 0x1e, 0xaa, 0xd3, 0x6f, - 0x14, 0x30, 0xb1, 0x6f, 0xe3, 0x22, 0xc0, 0x6f, 0x52, 0x40, 0xe4, 0x6f, 0x4c, 0x0e, 0x12, 0x42, 0xd3, 0x7f, 0xeb, - 0x2f, 0x03, 0x2e, 0x6d, 0x0f, 0x40, 0x90, 0x11, 0x30, 0x03, 0x2f, 0x23, 0x2e, 0x6d, 0x0f, 0x02, 0x2c, 0x00, 0x30, - 0xd0, 0x6f, 0xfb, 0x6f, 0xb0, 0x5f, 0xb8, 0x2e, 0x40, 0x50, 0xf1, 0x7f, 0x0a, 0x25, 0x3c, 0x86, 0xeb, 0x7f, 0x41, - 0x33, 0x22, 0x30, 0x98, 0x2e, 0xc2, 0xc4, 0xd3, 0x6f, 0xf4, 0x30, 0xdc, 0x09, 0x6f, 0x58, 0xc2, 0x6f, 0x94, 0x09, - 0x71, 0x58, 0x6a, 0xbb, 0xdc, 0x08, 0xb4, 0xb9, 0xb1, 0xbd, 0x6d, 0x5a, 0x95, 0x08, 0x21, 0xbd, 0xf6, 0xbf, 0x77, - 0x0b, 0x51, 0xbe, 0xf1, 0x6f, 0xeb, 0x6f, 0x52, 0x42, 0x54, 0x42, 0xc0, 0x2e, 0x43, 0x42, 0xc0, 0x5f, 0x50, 0x50, - 0x75, 0x52, 0x93, 0x30, 0x53, 0x42, 0xfb, 0x7f, 0x7b, 0x30, 0x4b, 0x42, 0x13, 0x30, 0x42, 0x82, 0x20, 0x33, 0x43, - 0x42, 0xc8, 0x00, 0x01, 0x2e, 0x80, 0x03, 0x05, 0x2e, 0x7d, 0x00, 0x19, 0x52, 0xe2, 0x7f, 0xd0, 0x7f, 0xc3, 0x7f, - 0x98, 0x2e, 0xb6, 0x0e, 0xd1, 0x6f, 0x48, 0x0a, 0xd1, 0x7f, 0x3a, 0x25, 0xfb, 0x86, 0x01, 0x33, 0x12, 0x30, 0x98, - 0x2e, 0xc2, 0xc4, 0xd1, 0x6f, 0x48, 0x0a, 0x40, 0xb2, 0x0d, 0x2f, 0xe0, 0x6f, 0x03, 0x2e, 0x80, 0x03, 0x53, 0x30, - 0x07, 0x80, 0x27, 0x2e, 0x21, 0xf2, 0x98, 0xbc, 0x01, 0x42, 0x98, 0x2e, 0x91, 0x03, 0x00, 0x2e, 0x00, 0x2e, 0xd0, - 0x2e, 0xb1, 0x6f, 0x9b, 0xb8, 0x07, 0x2e, 0x1b, 0x00, 0x19, 0x1a, 0xb1, 0x7f, 0x71, 0x30, 0x04, 0x2f, 0x23, 0x2e, - 0x21, 0xf2, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0x98, 0x2e, 0x6d, 0xc0, 0x98, 0x2e, 0x5d, 0xc0, 0x98, 0x2e, 0xdf, - 0x03, 0x20, 0x26, 0xc1, 0x6f, 0x02, 0x31, 0x52, 0x42, 0xab, 0x30, 0x4b, 0x42, 0x20, 0x33, 0x77, 0x56, 0xf1, 0x37, - 0xc4, 0x40, 0xa2, 0x0a, 0xc2, 0x42, 0xd8, 0x00, 0x01, 0x2e, 0x5e, 0xf7, 0x41, 0x08, 0x23, 0x2e, 0x94, 0x00, 0xe3, - 0x7f, 0x98, 0x2e, 0xaa, 0x01, 0xe1, 0x6f, 0x83, 0x30, 0x43, 0x42, 0x03, 0x30, 0xfb, 0x6f, 0x73, 0x50, 0x02, 0x30, - 0x00, 0x2e, 0x00, 0x2e, 0x81, 0x84, 0x50, 0x0e, 0xfa, 0x2f, 0x43, 0x42, 0x11, 0x30, 0xb0, 0x5f, 0x23, 0x2e, 0x21, - 0xf2, 0xb8, 0x2e, 0xc1, 0x4a, 0x00, 0x00, 0x6d, 0x57, 0x00, 0x00, 0x77, 0x8e, 0x00, 0x00, 0xe0, 0xff, 0xff, 0xff, - 0xd3, 0xff, 0xff, 0xff, 0xe5, 0xff, 0xff, 0xff, 0xee, 0xe1, 0xff, 0xff, 0x7c, 0x13, 0x00, 0x00, 0x46, 0xe6, 0xff, - 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, - 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, - 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, - 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, - 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, - 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, - 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, - 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, - 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, - 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, - 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, - 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, - 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, - 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, - 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, - 0xc1, 0xfd, 0x2d -}; - -/*! @name Global array that stores the feature input configuration of - * BMI270_CONTEXT - */ -const struct bmi2_feature_config bmi270_context_feat_in[BMI270_CONTEXT_MAX_FEAT_IN] = { - { .type = BMI2_CONFIG_ID, .page = BMI2_PAGE_4, .start_addr = BMI270_CONTEXT_CONFIG_ID_STRT_ADDR }, - { .type = BMI2_STEP_COUNTER_PARAMS, .page = BMI2_PAGE_1, .start_addr = BMI270_CONTEXT_STEP_CNT_1_STRT_ADDR }, - { .type = BMI2_STEP_DETECTOR, .page = BMI2_PAGE_4, .start_addr = BMI270_CONTEXT_STEP_CNT_4_STRT_ADDR }, - { .type = BMI2_STEP_COUNTER, .page = BMI2_PAGE_4, .start_addr = BMI270_CONTEXT_STEP_CNT_4_STRT_ADDR }, - { .type = BMI2_NVM_PROG_PREP, .page = BMI2_PAGE_4, .start_addr = BMI270_CONTEXT_NVM_PROG_PREP_STRT_ADDR }, - { .type = BMI2_MAX_BURST_LEN, .page = BMI2_PAGE_4, .start_addr = BMI270_CONTEXT_MAX_BURST_LEN_STRT_ADDR }, - { .type = BMI2_CRT_GYRO_SELF_TEST, .page = BMI2_PAGE_4, .start_addr = BMI270_CONTEXT_CRT_GYRO_SELF_TEST_STRT_ADDR }, - { .type = BMI2_ABORT_CRT_GYRO_SELF_TEST, .page = BMI2_PAGE_4, .start_addr = BMI270_CONTEXT_ABORT_STRT_ADDR }, - { .type = BMI2_ACTIVITY_RECOGNITION_SETTINGS, .page = BMI2_PAGE_5, - .start_addr = BMI270_CONTEXT_ACT_RGN_SETT_STRT_ADDR }, - { .type = BMI2_ACTIVITY_RECOGNITION, .page = BMI2_PAGE_5, .start_addr = BMI270_CONTEXT_ACT_RGN_STRT_ADDR }, -}; - -/*! @name Global array that stores the feature output configuration */ -const struct bmi2_feature_config bmi270_context_feat_out[BMI270_CONTEXT_MAX_FEAT_OUT] = { - { .type = BMI2_STEP_COUNTER, .page = BMI2_PAGE_0, .start_addr = BMI270_CONTEXT_STEP_CNT_OUT_STRT_ADDR }, - { .type = BMI2_GYRO_CROSS_SENSE, .page = BMI2_PAGE_0, .start_addr = BMI270_CONTEXT_GYRO_CROSS_SENSE_STRT_ADDR }, - { .type = BMI2_GYRO_GAIN_UPDATE, .page = BMI2_PAGE_0, .start_addr = BMI270_CONTEXT_GYR_USER_GAIN_OUT_STRT_ADDR }, - { .type = BMI2_NVM_STATUS, .page = BMI2_PAGE_0, .start_addr = BMI270_CONTEXT_NVM_VFRM_OUT_STRT_ADDR }, - { .type = BMI2_VFRM_STATUS, .page = BMI2_PAGE_0, .start_addr = BMI270_CONTEXT_NVM_VFRM_OUT_STRT_ADDR } -}; - -/*! @name Global array that stores the feature interrupts of BMI270_CONTEXT */ -struct bmi2_map_int bmi270_c_map_int[BMI270_C_MAX_INT_MAP] = { - { .type = BMI2_STEP_COUNTER, .sens_map_int = BMI270_C_INT_STEP_COUNTER_MASK }, - { .type = BMI2_STEP_DETECTOR, .sens_map_int = BMI270_C_INT_STEP_DETECTOR_MASK }, -}; - -/******************************************************************************/ - -/*! Local Function Prototypes - ******************************************************************************/ - -/*! - * @brief This internal API is used to validate the device pointer for - * null conditions. - * - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t null_ptr_check(const struct bmi2_dev *dev); - -/*! - * @brief This internal API enables the selected sensor/features. - * - * @param[in] sensor_sel : Selects the desired sensor. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t sensor_enable(uint64_t sensor_sel, struct bmi2_dev *dev); - -/*! - * @brief This internal API disables the selected sensor/features. - * - * @param[in] sensor_sel : Selects the desired sensor. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t sensor_disable(uint64_t sensor_sel, struct bmi2_dev *dev); - -/*! - * @brief This internal API selects the sensors/features to be enabled or - * disabled. - * - * @param[in] sens_list : Pointer to select the sensor. - * @param[in] n_sens : Number of sensors selected. - * @param[out] sensor_sel : Gets the selected sensor. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t select_sensor(const uint8_t *sens_list, uint8_t n_sens, uint64_t *sensor_sel); - -/*! - * @brief This internal API is used to enable/disable step detector feature. - * - * @param[in] dev : Structure instance of bmi2_dev. - * @param[in] enable : Enables/Disables step-detector. - * - * Enable | Description - * -------------|--------------- - * BMI2_DISABLE | Disables step detector - * BMI2_ENABLE | Enables step detector - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_step_detector(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to enable/disable step counter feature. - * - * @param[in] dev : Structure instance of bmi2_dev. - * @param[in] enable : Enables/Disables step counter. - * - * Enable | Description - * -------------|--------------- - * BMI2_DISABLE | Disables step counter - * BMI2_ENABLE | Enables step counter - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_step_counter(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to enable/disable gyroscope user gain - * feature. - * - * @param[in] dev : Structure instance of bmi2_dev. - * @param[in] enable : Enables/Disables gyroscope user gain. - * - * Enable | Description - * -------------|--------------- - * BMI2_DISABLE | Disables gyroscope user gain - * BMI2_ENABLE | Enables gyroscope user gain - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_gyro_user_gain(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API enables/disables the activity recognition feature. - * - * @param[in] enable : Enables/Disables activity recognition. - * @param[in] dev : Structure instance of bmi2_dev. - * - * enable | Description - * -------------|--------------- - * BMI2_ENABLE | Enables activity recognition. - * BMI2_DISABLE | Disables activity recognition. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_act_recog(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API sets step counter parameter configurations. - * - * @param[in] step_count_params : Array that stores parameters 1 to 25. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_step_count_params_config(const uint16_t *step_count_params, struct bmi2_dev *dev); - -/*! - * @brief This internal API sets step counter/detector/activity configurations. - * - * @param[in] config : Structure instance of bmi2_step_config. - * @param[in] dev : Structure instance of bmi2_dev. - * - *--------------------------------------------------------------------------- - * bmi2_step_config | - * Structure parameters | Description - *--------------------------|-------------------------------------------------- - * | The Step-counter will trigger output every time - * | the number of steps are counted. Holds implicitly - * water-mark level | a 20x factor, so the range is 0 to 10230, - * | with resolution of 20 steps. - * -------------------------|--------------------------------------------------- - * reset counter | Flag to reset the counted steps. - * -------------------------|--------------------------------------------------- - * @endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_step_config(const struct bmi2_step_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets step counter parameter configurations. - * - * @param[in] step_count_params : Array that stores parameters 1 to 25. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t get_step_count_params_config(uint16_t *step_count_params, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets step counter/detector/activity configurations. - * - * @param[out] config : Structure instance of bmi2_step_config. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @verbatim - *---------------------------------------------------------------------------- - * bmi2_step_config | - * Structure parameters | Description - *--------------------------|-------------------------------------------------- - * | The Step-counter will trigger output every time - * | the number of steps are counted. Holds implicitly - * water-mark level | a 20x factor, so the range is 0 to 10230, - * | with resolution of 20 steps. - * -------------------------|--------------------------------------------------- - * reset counter | Flag to reset the counted steps. - * -------------------------|--------------------------------------------------- - * @endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t get_step_config(struct bmi2_step_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to parse and store the activity recognition - * output from the FIFO data. - * - * @param[out] act_recog : Structure to retrieve output of activity - * recognition frame. - * @param[in] data_index : Index of the FIFO data which contains - * activity recognition frame. - * @param[out] fifo : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t unpack_act_recog_output(struct bmi2_act_recog_output *act_recog, - uint16_t *data_index, - const struct bmi2_fifo_frame *fifo); - -/*! - * @brief This internal API gets the output values of step counter. - * - * @param[out] step_count : Pointer to the stored step counter data. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t get_step_counter_output(uint32_t *step_count, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets the error status related to NVM. - * - * @param[out] nvm_err_stat : Stores the NVM error status. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t get_nvm_error_status(struct bmi2_nvm_err_status *nvm_err_stat, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets the error status related to virtual frames. - * - * @param[out] vfrm_err_stat : Stores the VFRM related error status. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t get_vfrm_error_status(struct bmi2_vfrm_err_status *vfrm_err_stat, struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to get enable status of gyroscope user gain - * update. - * - * @param[out] status : Stores status of gyroscope user gain update. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t get_user_gain_upd_status(uint8_t *status, struct bmi2_dev *dev); - -/*! - * @brief This internal API skips S4S frame in the FIFO data while getting - * activity recognition output. - * - * @param[in, out] frame_header : FIFO frame header. - * @param[in, out] data_index : Index value of the FIFO data bytes - * from which S4S frame header is to be - * skipped. - * @param[in] fifo : Structure instance of bmi2_fifo_frame. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t move_if_s4s_frame(const uint8_t *frame_header, uint16_t *data_index, const struct bmi2_fifo_frame *fifo); - -/*! - * @brief This internal API enables/disables compensation of the gain defined - * in the GAIN register. - * - * @param[in] enable : Enables/Disables gain compensation - * @param[in] dev : Structure instance of bmi2_dev. - * - * enable | Description - * -------------|--------------- - * BMI2_ENABLE | Enable gain compensation. - * BMI2_DISABLE | Disable gain compensation. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t enable_gyro_gain(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to extract the output feature configuration - * details like page and start address from the look-up table. - * - * @param[out] feat_output : Structure that stores output feature - * configurations. - * @param[in] type : Type of feature or sensor. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Returns the feature found flag. - * - * @retval BMI2_FALSE : Feature not found - * BMI2_TRUE : Feature found - */ -static uint8_t extract_output_feat_config(struct bmi2_feature_config *feat_output, - uint8_t type, - const struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to move the data index ahead of the - * current frame length parameter when unnecessary FIFO data appears while - * extracting the user specified data. - * - * @param[in,out] data_index : Index of the FIFO data which is to be - * moved ahead of the current frame length - * @param[in] current_frame_length : Number of bytes in the current frame. - * @param[in] fifo : Structure instance of bmi2_fifo_frame. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t move_next_frame(uint16_t *data_index, uint8_t current_frame_length, const struct bmi2_fifo_frame *fifo); - -/***************************************************************************/ - -/*! User Interface Definitions - ****************************************************************************/ - -/*! - * @brief This API: - * 1) Updates the device structure with address of the configuration file. - * 2) Initializes BMI270_CONTEXT sensor. - * 3) Writes the configuration file. - * 4) Updates the feature offset parameters in the device structure. - * 5) Updates the maximum number of pages, in the device structure. - */ -int8_t bmi270_context_init(struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if (rslt == BMI2_OK) - { - /* Assign chip id of BMI270_CONTEXT */ - dev->chip_id = BMI270_CONTEXT_CHIP_ID; - - /* Get the size of config array */ - dev->config_size = sizeof(bmi270_context_config_file); - - /* Enable the variant specific features if any */ - dev->variant_feature = BMI2_CRT_RTOSK_ENABLE | BMI2_GYRO_CROSS_SENS_ENABLE; - - /* An extra dummy byte is read during SPI read */ - if (dev->intf == BMI2_SPI_INTF) - { - dev->dummy_byte = 1; - } - else - { - dev->dummy_byte = 0; - } - - /* If configuration file pointer is not assigned any address */ - if (!dev->config_file_ptr) - { - /* Give the address of the configuration file array to - * the device pointer - */ - dev->config_file_ptr = bmi270_context_config_file; - } - - /* Initialize BMI2 sensor */ - rslt = bmi2_sec_init(dev); - if (rslt == BMI2_OK) - { - /* Assign the offsets of the feature input - * configuration to the device structure - */ - dev->feat_config = bmi270_context_feat_in; - - /* Assign the offsets of the feature output to - * the device structure - */ - dev->feat_output = bmi270_context_feat_out; - - /* Assign the maximum number of pages to the - * device structure - */ - dev->page_max = BMI270_CONTEXT_MAX_PAGE_NUM; - - /* Assign maximum number of input sensors/ - * features to device structure - */ - dev->input_sens = BMI270_CONTEXT_MAX_FEAT_IN; - - /* Assign maximum number of output sensors/ - * features to device structure - */ - dev->out_sens = BMI270_CONTEXT_MAX_FEAT_OUT; - - /* Assign the offsets of the feature interrupt - * to the device structure - */ - dev->map_int = bmi270_c_map_int; - - /* Assign maximum number of feature interrupts - * to device structure - */ - dev->sens_int_map = BMI270_C_MAX_INT_MAP; - } - } - - return rslt; -} - -/*! - * @brief This API selects the sensors/features to be enabled. - */ -int8_t bmi270_context_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to select sensor */ - uint64_t sensor_sel = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (sens_list != NULL)) - { - /* Get the selected sensors */ - rslt = select_sensor(sens_list, n_sens, &sensor_sel); - if (rslt == BMI2_OK) - { - /* Enable the selected sensors */ - rslt = sensor_enable(sensor_sel, dev); - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API selects the sensors/features to be disabled. - */ -int8_t bmi270_context_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to select sensor */ - uint64_t sensor_sel = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (sens_list != NULL)) - { - /* Get the selected sensors */ - rslt = select_sensor(sens_list, n_sens, &sensor_sel); - if (rslt == BMI2_OK) - { - /* Disable the selected sensors */ - rslt = sensor_disable(sensor_sel, dev); - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API sets the sensor/feature configuration. - */ -int8_t bmi270_context_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define loop */ - uint8_t loop; - - /* Variable to get the status of advance power save */ - uint8_t aps_stat = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (sens_cfg != NULL)) - { - /* Get status of advance power save mode */ - aps_stat = dev->aps_status; - - for (loop = 0; loop < n_sens; loop++) - { - if ((sens_cfg[loop].type == BMI2_ACCEL) || (sens_cfg[loop].type == BMI2_GYRO) || - (sens_cfg[loop].type == BMI2_AUX) || (sens_cfg[loop].type == BMI2_GYRO_GAIN_UPDATE)) - { - rslt = bmi2_set_sensor_config(&sens_cfg[loop], 1, dev); - } - else - { - /* Disable Advance power save if enabled for auxiliary - * and feature configurations - */ - if (aps_stat == BMI2_ENABLE) - { - /* Disable advance power save if - * enabled - */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - } - - if (rslt == BMI2_OK) - { - switch (sens_cfg[loop].type) - { - /* Set the step counter parameters */ - case BMI2_STEP_COUNTER_PARAMS: - rslt = set_step_count_params_config(sens_cfg[loop].cfg.step_counter_params, dev); - break; - - /* Set step counter/detector/activity configuration */ - case BMI2_STEP_DETECTOR: - case BMI2_STEP_COUNTER: - rslt = set_step_config(&sens_cfg[loop].cfg.step_counter, dev); - break; - - default: - rslt = BMI2_E_INVALID_SENSOR; - break; - } - } - - /* Return error if any of the set configurations fail */ - if (rslt != BMI2_OK) - { - break; - } - } - } - - /* Enable Advance power save if disabled while configuring and - * not when already disabled - */ - if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) - { - rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API gets the sensor/feature configuration. - */ -int8_t bmi270_context_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define loop */ - uint8_t loop; - - /* Variable to get the status of advance power save */ - uint8_t aps_stat = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (sens_cfg != NULL)) - { - /* Get status of advance power save mode */ - aps_stat = dev->aps_status; - for (loop = 0; loop < n_sens; loop++) - { - if ((sens_cfg[loop].type == BMI2_ACCEL) || (sens_cfg[loop].type == BMI2_GYRO) || - (sens_cfg[loop].type == BMI2_AUX) || (sens_cfg[loop].type == BMI2_GYRO_GAIN_UPDATE)) - { - rslt = bmi2_get_sensor_config(&sens_cfg[loop], 1, dev); - } - else - { - /* Disable Advance power save if enabled for auxiliary - * and feature configurations - */ - if ((sens_cfg[loop].type >= BMI2_MAIN_SENS_MAX_NUM) || (sens_cfg[loop].type == BMI2_AUX)) - { - - if (aps_stat == BMI2_ENABLE) - { - /* Disable advance power save if - * enabled - */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - } - } - - if (rslt == BMI2_OK) - { - switch (sens_cfg[loop].type) - { - /* Set the step counter parameters */ - case BMI2_STEP_COUNTER_PARAMS: - rslt = get_step_count_params_config(sens_cfg[loop].cfg.step_counter_params, dev); - break; - - /* Get step counter/detector/activity configuration */ - case BMI2_STEP_DETECTOR: - case BMI2_STEP_COUNTER: - rslt = get_step_config(&sens_cfg[loop].cfg.step_counter, dev); - break; - - default: - rslt = BMI2_E_INVALID_SENSOR; - break; - } - } - - /* Return error if any of the get configurations fail */ - if (rslt != BMI2_OK) - { - break; - } - } - } - - /* Enable Advance power save if disabled while configuring and - * not when already disabled - */ - if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) - { - rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API gets the sensor/feature data for accelerometer, gyroscope, - * auxiliary sensor, step counter, high-g, gyroscope user-gain update, - * orientation, gyroscope cross sensitivity and error status for NVM and VFRM. - */ -int8_t bmi270_context_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define loop */ - uint8_t loop; - - /* Variable to get the status of advance power save */ - uint8_t aps_stat = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (sensor_data != NULL)) - { - /* Get status of advance power save mode */ - aps_stat = dev->aps_status; - for (loop = 0; loop < n_sens; loop++) - { - if ((sensor_data[loop].type == BMI2_ACCEL) || (sensor_data[loop].type == BMI2_GYRO) || - (sensor_data[loop].type == BMI2_AUX) || (sensor_data[loop].type == BMI2_GYRO_GAIN_UPDATE) || - (sensor_data[loop].type == BMI2_GYRO_CROSS_SENSE)) - { - rslt = bmi2_get_sensor_data(&sensor_data[loop], 1, dev); - } - else - { - /* Disable Advance power save if enabled for feature - * configurations - */ - if (sensor_data[loop].type >= BMI2_MAIN_SENS_MAX_NUM) - { - if (aps_stat == BMI2_ENABLE) - { - /* Disable advance power save if - * enabled - */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - } - } - - if (rslt == BMI2_OK) - { - switch (sensor_data[loop].type) - { - case BMI2_STEP_COUNTER: - - /* Get step counter output */ - rslt = get_step_counter_output(&sensor_data[loop].sens_data.step_counter_output, dev); - break; - case BMI2_NVM_STATUS: - - /* Get NVM error status */ - rslt = get_nvm_error_status(&sensor_data[loop].sens_data.nvm_status, dev); - break; - case BMI2_VFRM_STATUS: - - /* Get VFRM error status */ - rslt = get_vfrm_error_status(&sensor_data[loop].sens_data.vfrm_status, dev); - break; - default: - rslt = BMI2_E_INVALID_SENSOR; - break; - } - - /* Return error if any of the get sensor data fails */ - if (rslt != BMI2_OK) - { - break; - } - } - } - - /* Enable Advance power save if disabled while - * configuring and not when already disabled - */ - if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) - { - rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - } - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This api is used for retrieving the activity recognition settings currently set. - */ -int8_t bmi270_context_get_act_recg_sett(struct bmi2_act_recg_sett *sett, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to get the status of advance power save */ - uint8_t aps_stat; - - /* Variable to set flag */ - uint8_t feat_found; - uint16_t msb_lsb; - uint8_t lsb; - uint8_t msb; - - /* Initialize feature configuration for activity recognition */ - struct bmi2_feature_config bmi2_act_recg_sett = { 0, 0, 0 }; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if (rslt == BMI2_OK) - { - - /* Search for bmi2 Abort feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&bmi2_act_recg_sett, BMI2_ACTIVITY_RECOGNITION_SETTINGS, dev); - if (feat_found) - { - aps_stat = dev->aps_status; - if (aps_stat == BMI2_ENABLE) - { - /* Disable advance power save if enabled */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - } - - /* Get the configuration from the page where activity recognition setting feature resides */ - if (rslt == BMI2_OK) - { - rslt = bmi2_get_feat_config(bmi2_act_recg_sett.page, feat_config, dev); - } - - if (rslt == BMI2_OK) - { - /* Define the offset in bytes */ - idx = bmi2_act_recg_sett.start_addr; - - /* get the status of enable/disable post processing */ - sett->act_rec_1 = BMI2_GET_BIT_POS0(feat_config[idx], BMI2_ACT_RECG_POST_PROS_EN_DIS); - - /* increment idx by 2 to point min gdi thres addres */ - idx = idx + 2; - lsb = feat_config[idx]; - idx++; - msb = feat_config[idx]; - msb_lsb = (uint16_t)(lsb | msb << 8); - sett->act_rec_2 = msb_lsb; - - /* increment idx by 1 to point max gdi thres addres */ - idx++; - lsb = feat_config[idx]; - idx++; - msb = feat_config[idx]; - msb_lsb = (uint16_t)(lsb | msb << 8); - sett->act_rec_3 = msb_lsb; - - /* increment idx by 1 to point buffer size */ - idx++; - sett->act_rec_4 = BMI2_GET_BIT_POS0(feat_config[idx], BMI2_ACT_RECG_BUFF_SIZE); - - /* increment idx by 2 to to point to min segment confidence */ - idx = idx + 2; - sett->act_rec_5 = BMI2_GET_BIT_POS0(feat_config[idx], BMI2_ACT_RECG_MIN_SEG_CONF); - } - - /* Enable Advance power save if disabled while - * configuring and not when already disabled - */ - if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) - { - rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - } - - return rslt; -} - -/*! - * @brief This api is used for setting the activity recognition settings. - */ -int8_t bmi270_context_set_act_recg_sett(const struct bmi2_act_recg_sett *sett, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to get the status of advance power save */ - uint8_t aps_stat; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for activity recognition */ - struct bmi2_feature_config bmi2_act_recg_sett = { 0, 0, 0 }; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if (rslt == BMI2_OK) - { - - /* Search for bmi2 Abort feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&bmi2_act_recg_sett, BMI2_ACTIVITY_RECOGNITION_SETTINGS, dev); - if (feat_found) - { - aps_stat = dev->aps_status; - if (aps_stat == BMI2_ENABLE) - { - /* Disable advance power save if enabled */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - } - - /* Get the configuration from the page where activity recognition setting feature resides */ - if (rslt == BMI2_OK) - { - rslt = bmi2_get_feat_config(bmi2_act_recg_sett.page, feat_config, dev); - } - - if (rslt == BMI2_OK) - { - /* Define the offset in bytes */ - idx = bmi2_act_recg_sett.start_addr; - if ((sett->act_rec_4 > 10) || (sett->act_rec_5 > 10)) - { - rslt = BMI2_E_INVALID_INPUT; - } - - if (rslt == BMI2_OK) - { - feat_config[idx] = BMI2_SET_BIT_POS0(feat_config[idx], - BMI2_ACT_RECG_POST_PROS_EN_DIS, - sett->act_rec_1); - - /* Increment idx by 2 to point min gdi thres addres */ - idx = idx + 2; - feat_config[idx] = BMI2_GET_LSB(sett->act_rec_2); - idx++; - feat_config[idx] = BMI2_GET_MSB(sett->act_rec_2); - - /* Increment idx by 1 to point max gdi thres addres */ - idx++; - feat_config[idx] = BMI2_GET_LSB(sett->act_rec_3); - idx++; - feat_config[idx] = BMI2_GET_MSB(sett->act_rec_3); - - /* Increment idx by 1 to point buffer size */ - idx++; - feat_config[idx] = BMI2_SET_BIT_POS0(feat_config[idx], BMI2_ACT_RECG_BUFF_SIZE, sett->act_rec_4); - - /* Increment idx by 2 to to point to min segment confidence */ - idx = idx + 2; - feat_config[idx] = BMI2_SET_BIT_POS0(feat_config[idx], BMI2_ACT_RECG_MIN_SEG_CONF, sett->act_rec_5); - - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - - /* Enable Advance power save if disabled while - * configuring and not when already disabled - */ - if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) - { - rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - } - - return rslt; -} - -/*! - * @brief This internal API is used to parse the activity output from the - * FIFO in header mode. - */ -int8_t bmi270_context_get_act_recog_output(struct bmi2_act_recog_output *act_recog, - uint16_t *act_frm_len, - struct bmi2_fifo_frame *fifo, - const struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define header frame */ - uint8_t frame_header = 0; - - /* Variable to index the data bytes */ - uint16_t data_index; - - /* Variable to index activity frames */ - uint16_t act_idx = 0; - - /* Variable to indicate activity frames read */ - uint16_t frame_to_read = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (act_recog != NULL) && (act_frm_len != NULL) && (fifo != NULL)) - { - - /* Store the number of frames to be read */ - frame_to_read = *act_frm_len; - for (data_index = fifo->act_recog_byte_start_idx; data_index < fifo->length;) - { - /* Get frame header byte */ - frame_header = fifo->data[data_index] & BMI2_FIFO_TAG_INTR_MASK; - - /* Skip S4S frames if S4S is enabled */ - rslt = move_if_s4s_frame(&frame_header, &data_index, fifo); - - /* Break if FIFO is empty */ - if (rslt == BMI2_W_FIFO_EMPTY) - { - break; - } - - /* Index shifted to next byte where data starts */ - data_index++; - switch (frame_header) - { - /* If header defines accelerometer frame */ - case BMI2_FIFO_HEADER_ACC_FRM: - rslt = move_next_frame(&data_index, fifo->acc_frm_len, fifo); - break; - - /* If header defines accelerometer and auxiliary frames */ - case BMI2_FIFO_HEADER_AUX_ACC_FRM: - rslt = move_next_frame(&data_index, fifo->acc_aux_frm_len, fifo); - break; - - /* If header defines accelerometer and gyroscope frames */ - case BMI2_FIFO_HEADER_GYR_ACC_FRM: - rslt = move_next_frame(&data_index, fifo->acc_gyr_frm_len, fifo); - break; - - /* If header defines accelerometer, auxiliary and gyroscope frames */ - case BMI2_FIFO_HEADER_ALL_FRM: - rslt = move_next_frame(&data_index, fifo->all_frm_len, fifo); - break; - - /* If header defines only gyroscope frame */ - case BMI2_FIFO_HEADER_GYR_FRM: - rslt = move_next_frame(&data_index, fifo->gyr_frm_len, fifo); - break; - - /* If header defines only auxiliary frame */ - case BMI2_FIFO_HEADER_AUX_FRM: - rslt = move_next_frame(&data_index, fifo->aux_frm_len, fifo); - break; - - /* If header defines auxiliary and gyroscope frame */ - case BMI2_FIFO_HEADER_AUX_GYR_FRM: - rslt = move_next_frame(&data_index, fifo->aux_gyr_frm_len, fifo); - break; - - /* If header defines sensor time frame */ - case BMI2_FIFO_HEADER_SENS_TIME_FRM: - rslt = move_next_frame(&data_index, BMI2_SENSOR_TIME_LENGTH, fifo); - break; - - /* If header defines skip frame */ - case BMI2_FIFO_HEADER_SKIP_FRM: - rslt = move_next_frame(&data_index, BMI2_FIFO_SKIP_FRM_LENGTH, fifo); - break; - - /* If header defines Input configuration frame */ - case BMI2_FIFO_HEADER_INPUT_CFG_FRM: - rslt = move_next_frame(&data_index, BMI2_FIFO_INPUT_CFG_LENGTH, fifo); - break; - - /* If header defines invalid frame or end of valid data */ - case BMI2_FIFO_HEAD_OVER_READ_MSB: - - /* Move the data index to the last byte to mark completion */ - data_index = fifo->length; - - /* FIFO is empty */ - rslt = BMI2_W_FIFO_EMPTY; - break; - - /* If header defines activity recognition frame */ - case BMI2_FIFO_VIRT_ACT_RECOG_FRM: - - /* Get the activity output */ - rslt = unpack_act_recog_output(&act_recog[(act_idx)], &data_index, fifo); - - /* Update activity frame index */ - (act_idx)++; - break; - default: - - /* Move the data index to the last byte in case of invalid values */ - data_index = fifo->length; - - /* FIFO is empty */ - rslt = BMI2_W_FIFO_EMPTY; - break; - } - - /* Number of frames to be read is complete or FIFO is empty */ - if ((frame_to_read == act_idx) || (rslt == BMI2_W_FIFO_EMPTY)) - { - break; - } - } - - /* Update the activity frame index */ - (*act_frm_len) = act_idx; - - /* Update the activity byte index */ - fifo->act_recog_byte_start_idx = data_index; - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API updates the gyroscope user-gain. - */ -int8_t bmi270_context_update_gyro_user_gain(const struct bmi2_gyro_user_gain_config *user_gain, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to select sensor */ - uint8_t sens_sel[2] = { BMI2_GYRO, BMI2_GYRO_GAIN_UPDATE }; - - /* Structure to define sensor configurations */ - struct bmi2_sens_config sens_cfg; - - /* Variable to store status of user-gain update module */ - uint8_t status = 0; - - /* Variable to define count */ - uint8_t count = 100; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (user_gain != NULL)) - { - /* Select type of feature */ - sens_cfg.type = BMI2_GYRO_GAIN_UPDATE; - - /* Get the user gain configurations */ - rslt = bmi270_context_get_sensor_config(&sens_cfg, 1, dev); - if (rslt == BMI2_OK) - { - /* Get the user-defined ratio */ - sens_cfg.cfg.gyro_gain_update = *user_gain; - - /* Set rate ratio for all axes */ - rslt = bmi270_context_set_sensor_config(&sens_cfg, 1, dev); - } - - /* Disable gyroscope */ - if (rslt == BMI2_OK) - { - rslt = bmi270_context_sensor_disable(&sens_sel[0], 1, dev); - } - - /* Enable gyroscope user-gain update module */ - if (rslt == BMI2_OK) - { - rslt = bmi270_context_sensor_enable(&sens_sel[1], 1, dev); - } - - /* Set the command to trigger the computation */ - if (rslt == BMI2_OK) - { - rslt = bmi2_set_command_register(BMI2_USR_GAIN_CMD, dev); - } - - if (rslt == BMI2_OK) - { - /* Poll until enable bit of user-gain update is 0 */ - while (count--) - { - rslt = get_user_gain_upd_status(&status, dev); - if ((rslt == BMI2_OK) && (status == 0)) - { - /* Enable compensation of gain defined - * in the GAIN register - */ - rslt = enable_gyro_gain(BMI2_ENABLE, dev); - - /* Enable gyroscope */ - if (rslt == BMI2_OK) - { - rslt = bmi270_context_sensor_enable(&sens_sel[0], 1, dev); - } - - break; - } - - dev->delay_us(10000, dev->intf_ptr); - } - - /* Return error if user-gain update is failed */ - if ((rslt == BMI2_OK) && (status != 0)) - { - rslt = BMI2_E_GYR_USER_GAIN_UPD_FAIL; - } - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API reads the compensated gyroscope user-gain values. - */ -int8_t bmi270_context_read_gyro_user_gain(struct bmi2_gyro_user_gain_data *gyr_usr_gain, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define register data */ - uint8_t reg_data[3] = { 0 }; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (gyr_usr_gain != NULL)) - { - /* Get the gyroscope compensated gain values */ - rslt = bmi2_get_regs(BMI2_GYR_USR_GAIN_0_ADDR, reg_data, 3, dev); - if (rslt == BMI2_OK) - { - /* Gyroscope user gain correction X-axis */ - gyr_usr_gain->x = (int8_t)BMI2_GET_BIT_POS0(reg_data[0], BMI2_GYR_USR_GAIN_X); - - /* Gyroscope user gain correction Y-axis */ - gyr_usr_gain->y = (int8_t)BMI2_GET_BIT_POS0(reg_data[1], BMI2_GYR_USR_GAIN_Y); - - /* Gyroscope user gain correction z-axis */ - gyr_usr_gain->z = (int8_t)BMI2_GET_BIT_POS0(reg_data[2], BMI2_GYR_USR_GAIN_Z); - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API maps/unmaps feature interrupts to that of interrupt pins. - */ -int8_t bmi270_context_map_feat_int(const struct bmi2_sens_int_config *sens_int, uint8_t n_sens, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define loop */ - uint8_t loop; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (sens_int != NULL)) - { - for (loop = 0; loop < n_sens; loop++) - { - switch (sens_int[loop].type) - { - case BMI2_STEP_COUNTER: - case BMI2_STEP_DETECTOR: - - rslt = bmi2_map_feat_int(sens_int[loop].type, sens_int[loop].hw_int_pin, dev); - break; - default: - rslt = BMI2_E_INVALID_SENSOR; - break; - } - - /* Return error if interrupt mapping fails */ - if (rslt != BMI2_OK) - { - break; - } - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/***************************************************************************/ - -/*! Local Function Definitions - ****************************************************************************/ - -/*! - * @brief This internal API is used to validate the device structure pointer for - * null conditions. - */ -static int8_t null_ptr_check(const struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt = BMI2_OK; - - if ((dev == NULL) || (dev->read == NULL) || (dev->write == NULL) || (dev->delay_us == NULL)) - { - /* Device structure pointer is not valid */ - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This internal API selects the sensor/features to be enabled or - * disabled. - */ -static int8_t select_sensor(const uint8_t *sens_list, uint8_t n_sens, uint64_t *sensor_sel) -{ - /* Variable to define error */ - int8_t rslt = BMI2_OK; - - /* Variable to define loop */ - uint8_t count; - - for (count = 0; count < n_sens; count++) - { - switch (sens_list[count]) - { - case BMI2_ACCEL: - *sensor_sel |= BMI2_ACCEL_SENS_SEL; - break; - case BMI2_GYRO: - *sensor_sel |= BMI2_GYRO_SENS_SEL; - break; - case BMI2_AUX: - *sensor_sel |= BMI2_AUX_SENS_SEL; - break; - case BMI2_TEMP: - *sensor_sel |= BMI2_TEMP_SENS_SEL; - break; - case BMI2_STEP_DETECTOR: - *sensor_sel |= BMI2_STEP_DETECT_SEL; - break; - case BMI2_STEP_COUNTER: - *sensor_sel |= BMI2_STEP_COUNT_SEL; - break; - case BMI2_GYRO_GAIN_UPDATE: - *sensor_sel |= BMI2_GYRO_GAIN_UPDATE_SEL; - break; - case BMI2_ACTIVITY_RECOGNITION: - *sensor_sel |= BMI2_ACTIVITY_RECOGNITION_SEL; - break; - default: - rslt = BMI2_E_INVALID_SENSOR; - break; - } - } - - return rslt; -} - -/*! - * @brief This internal API enables the selected sensor/features. - */ -static int8_t sensor_enable(uint64_t sensor_sel, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to store register values */ - uint8_t reg_data = 0; - - /* Variable to define loop */ - uint8_t loop = 1; - - /* Variable to get the status of advance power save */ - uint8_t aps_stat = 0; - - rslt = bmi2_get_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); - if (rslt == BMI2_OK) - { - /* Enable accelerometer */ - if (sensor_sel & BMI2_ACCEL_SENS_SEL) - { - reg_data = BMI2_SET_BITS(reg_data, BMI2_ACC_EN, BMI2_ENABLE); - } - - /* Enable gyroscope */ - if (sensor_sel & BMI2_GYRO_SENS_SEL) - { - reg_data = BMI2_SET_BITS(reg_data, BMI2_GYR_EN, BMI2_ENABLE); - } - - /* Enable auxiliary sensor */ - if (sensor_sel & BMI2_AUX_SENS_SEL) - { - reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_AUX_EN, BMI2_ENABLE); - } - - /* Enable temperature sensor */ - if (sensor_sel & BMI2_TEMP_SENS_SEL) - { - reg_data = BMI2_SET_BITS(reg_data, BMI2_TEMP_EN, BMI2_ENABLE); - } - - /* Enable the sensors that are set in the power control register */ - if (sensor_sel & BMI2_MAIN_SENSORS) - { - rslt = bmi2_set_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); - } - } - - if ((rslt == BMI2_OK) && (sensor_sel & ~(BMI2_MAIN_SENSORS))) - { - /* Get status of advance power save mode */ - aps_stat = dev->aps_status; - if (aps_stat == BMI2_ENABLE) - { - /* Disable advance power save if enabled */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - } - - if (rslt == BMI2_OK) - { - while (loop--) - { - /* Enable step detector feature */ - if (sensor_sel & BMI2_STEP_DETECT_SEL) - { - rslt = set_step_detector(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_STEP_DETECT_SEL; - } - else - { - break; - } - } - - /* Enable step counter feature */ - if (sensor_sel & BMI2_STEP_COUNT_SEL) - { - rslt = set_step_counter(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_STEP_COUNT_SEL; - } - else - { - break; - } - } - - /* Enable gyroscope user gain */ - if (sensor_sel & BMI2_GYRO_GAIN_UPDATE_SEL) - { - rslt = set_gyro_user_gain(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_GYRO_GAIN_UPDATE_SEL; - } - else - { - break; - } - } - - /* Enable activity recognition feature */ - if (sensor_sel & BMI2_ACTIVITY_RECOGNITION_SEL) - { - rslt = set_act_recog(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_ACTIVITY_RECOGNITION_SEL; - } - else - { - break; - } - } - } - - /* Enable Advance power save if disabled while - * configuring and not when already disabled - */ - if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) - { - rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - } - } - } - - return rslt; -} - -/*! - * @brief This internal API disables the selected sensors/features. - */ -static int8_t sensor_disable(uint64_t sensor_sel, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to store register values */ - uint8_t reg_data = 0; - - /* Variable to define loop */ - uint8_t loop = 1; - - /* Variable to get the status of advance power save */ - uint8_t aps_stat = 0; - - rslt = bmi2_get_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); - if (rslt == BMI2_OK) - { - /* Disable accelerometer */ - if (sensor_sel & BMI2_ACCEL_SENS_SEL) - { - reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_ACC_EN); - } - - /* Disable gyroscope */ - if (sensor_sel & BMI2_GYRO_SENS_SEL) - { - reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_GYR_EN); - } - - /* Disable auxiliary sensor */ - if (sensor_sel & BMI2_AUX_SENS_SEL) - { - reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_AUX_EN); - } - - /* Disable temperature sensor */ - if (sensor_sel & BMI2_TEMP_SENS_SEL) - { - reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_TEMP_EN); - } - - /* Disable the sensors that are set in the power control register */ - if (sensor_sel & BMI2_MAIN_SENSORS) - { - rslt = bmi2_set_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); - } - } - - if ((rslt == BMI2_OK) && (sensor_sel & ~(BMI2_MAIN_SENSORS))) - { - /* Get status of advance power save mode */ - aps_stat = dev->aps_status; - if (aps_stat == BMI2_ENABLE) - { - /* Disable advance power save if enabled */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - } - - if (rslt == BMI2_OK) - { - while (loop--) - { - /* Disable step detector feature */ - if (sensor_sel & BMI2_STEP_DETECT_SEL) - { - rslt = set_step_detector(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat &= ~BMI2_STEP_DETECT_SEL; - } - else - { - break; - } - } - - /* Disable step counter feature */ - if (sensor_sel & BMI2_STEP_COUNT_SEL) - { - rslt = set_step_counter(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat &= ~BMI2_STEP_COUNT_SEL; - } - else - { - break; - } - } - - /* Disable gyroscope user gain */ - if (sensor_sel & BMI2_GYRO_GAIN_UPDATE_SEL) - { - rslt = set_gyro_user_gain(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat &= ~BMI2_GYRO_GAIN_UPDATE_SEL; - } - else - { - break; - } - } - - if (sensor_sel & BMI2_ACTIVITY_RECOGNITION_SEL) - { - rslt = set_act_recog(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_ACTIVITY_RECOGNITION_SEL; - } - else - { - break; - } - } - - /* Enable Advance power save if disabled while - * configuring and not when already disabled - */ - if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) - { - rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - } - } - } - } - - return rslt; -} - -/*! - * @brief This internal API is used to enable/disable step detector feature. - */ -static int8_t set_step_detector(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for step detector */ - struct bmi2_feature_config step_det_config = { 0, 0, 0 }; - - /* Search for step detector feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&step_det_config, BMI2_STEP_DETECTOR, dev); - if (feat_found) - { - /* Get the configuration from the page where step detector feature resides */ - rslt = bmi2_get_feat_config(step_det_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of step detector */ - idx = step_det_config.start_addr + BMI2_STEP_COUNT_FEAT_EN_OFFSET; - - /* Set the feature enable bit */ - feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_STEP_DET_FEAT_EN, enable); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API is used to enable/disable step counter feature. - */ -static int8_t set_step_counter(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for step counter */ - struct bmi2_feature_config step_count_config = { 0, 0, 0 }; - - /* Search for step counter feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&step_count_config, BMI2_STEP_COUNTER, dev); - if (feat_found) - { - /* Get the configuration from the page where step-counter feature resides */ - rslt = bmi2_get_feat_config(step_count_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of step counter */ - idx = step_count_config.start_addr + BMI2_STEP_COUNT_FEAT_EN_OFFSET; - - /* Set the feature enable bit */ - feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_STEP_COUNT_FEAT_EN, enable); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API is used to enable/disable gyroscope user gain - * feature. - */ -static int8_t set_gyro_user_gain(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for gyroscope user gain */ - struct bmi2_feature_config gyr_user_gain_cfg = { 0, 0, 0 }; - - /* Search for user gain feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&gyr_user_gain_cfg, BMI2_GYRO_GAIN_UPDATE, dev); - if (feat_found) - { - /* Get the configuration from the page where user gain feature resides */ - rslt = bmi2_get_feat_config(gyr_user_gain_cfg.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of user gain */ - idx = gyr_user_gain_cfg.start_addr + BMI2_GYR_USER_GAIN_FEAT_EN_OFFSET; - - /* Set the feature enable bit */ - feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_GYR_USER_GAIN_FEAT_EN, enable); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API enables/disables the activity recognition feature. - */ -static int8_t set_act_recog(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for activity recognition */ - struct bmi2_feature_config act_recog_cfg = { 0, 0, 0 }; - - /* Search for activity recognition and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&act_recog_cfg, BMI2_ACTIVITY_RECOGNITION, dev); - if (feat_found) - { - /* Get the configuration from the page where activity - * recognition feature resides - */ - rslt = bmi2_get_feat_config(act_recog_cfg.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of activity recognition */ - idx = act_recog_cfg.start_addr; - - /* Set the feature enable bit */ - feat_config[idx] = BMI2_SET_BIT_POS0(feat_config[idx], BMI2_ACTIVITY_RECOG_EN, enable); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API sets step counter parameter configurations. - */ -static int8_t set_step_count_params_config(const uint16_t *step_count_params, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt = BMI2_OK; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define index */ - uint8_t index = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for step counter parameters */ - struct bmi2_feature_config step_params_config = { 0, 0, 0 }; - - /* Variable to index the page number */ - uint8_t page_idx; - - /* Variable to define the start page */ - uint8_t start_page; - - /* Variable to define start address of the parameters */ - uint8_t start_addr; - - /* Variable to define number of bytes */ - uint8_t n_bytes = (BMI2_STEP_CNT_N_PARAMS * 2); - - /* Variable to store number of pages */ - uint8_t n_pages = (n_bytes / 16); - - /* Variable to define the end page */ - uint8_t end_page; - - /* Variable to define the remaining bytes to be read */ - uint8_t remain_len; - - /* Variable to define the maximum words(16 bytes or 8 words) to be read in a page */ - uint8_t max_len = 8; - - /* Variable index bytes in a page */ - uint8_t page_byte_idx; - - /* Variable to index the parameters */ - uint8_t param_idx = 0; - - /* Copy the feature configuration address to a local pointer */ - uint16_t *data_p = (uint16_t *) (void *)feat_config; - - /* Search for step counter parameter feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&step_params_config, BMI2_STEP_COUNTER_PARAMS, dev); - if (feat_found) - { - /* Get the start page for the step counter parameters */ - start_page = step_params_config.page; - - /* Get the end page for the step counter parameters */ - end_page = start_page + n_pages; - - /* Get the start address for the step counter parameters */ - start_addr = step_params_config.start_addr; - - /* Get the remaining length of bytes to be read */ - remain_len = (uint8_t)((n_bytes - (n_pages * 16)) + start_addr); - for (page_idx = start_page; page_idx <= end_page; page_idx++) - { - /* Get the configuration from the respective page */ - rslt = bmi2_get_feat_config(page_idx, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Start from address 0x00 when switched to next page */ - if (page_idx > start_page) - { - start_addr = 0; - } - - /* Remaining number of words to be read in the page */ - if (page_idx == end_page) - { - max_len = (remain_len / 2); - } - - /* Get offset in words since all the features are set in words length */ - page_byte_idx = start_addr / 2; - for (; page_byte_idx < max_len;) - { - /* Set parameters 1 to 25 */ - *(data_p + page_byte_idx) = BMI2_SET_BIT_POS0(*(data_p + page_byte_idx), - BMI2_STEP_COUNT_PARAMS, - step_count_params[param_idx]); - - /* Increment offset by 1 word to set to the next parameter */ - page_byte_idx++; - - /* Increment to next parameter */ - param_idx++; - } - - /* Get total length in bytes to copy from local pointer to the array */ - page_byte_idx = (uint8_t)(page_byte_idx * 2) - step_params_config.start_addr; - - /* Copy the bytes to be set back to the array */ - for (index = 0; index < page_byte_idx; index++) - { - feat_config[step_params_config.start_addr + - index] = *((uint8_t *) data_p + step_params_config.start_addr + index); - } - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/* @brief This internal API sets step counter configurations like water-mark - * level, reset-counter and output-configuration step detector and activity. - */ -static int8_t set_step_config(const struct bmi2_step_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define index */ - uint8_t index = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for step counter 4 */ - struct bmi2_feature_config step_count_config = { 0, 0, 0 }; - - /* Copy the feature configuration address to a local pointer */ - uint16_t *data_p = (uint16_t *) (void *)feat_config; - - /* Search for step counter feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&step_count_config, BMI2_STEP_COUNTER, dev); - if (feat_found) - { - /* Get the configuration from the page where step counter resides */ - rslt = bmi2_get_feat_config(step_count_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes */ - idx = step_count_config.start_addr; - - /* Get offset in words since all the features are set in words length */ - idx = idx / 2; - - /* Set water-mark level */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_STEP_COUNT_WM_LEVEL, config->watermark_level); - - /* Set reset-counter */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_STEP_COUNT_RST_CNT, config->reset_counter); - - /* Increment offset by 1 word to set output - * configuration of step detector and step activity - */ - idx++; - - /* Set step buffer size */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_STEP_BUFFER_SIZE, config->step_buffer_size); - - /* Increment offset by 1 more word to get the total length in words */ - idx++; - - /* Get total length in bytes to copy from local pointer to the array */ - idx = (uint8_t)(idx * 2) - step_count_config.start_addr; - - /* Copy the bytes to be set back to the array */ - for (index = 0; index < idx; index++) - { - feat_config[step_count_config.start_addr + - index] = *((uint8_t *) data_p + step_count_config.start_addr + index); - } - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets step counter parameter configurations. - */ -static int8_t get_step_count_params_config(uint16_t *step_count_params, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt = BMI2_OK; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Variable to define LSB */ - uint16_t lsb = 0; - - /* Variable to define MSB */ - uint16_t msb = 0; - - /* Variable to define a word */ - uint16_t lsb_msb = 0; - - /* Initialize feature configuration for step counter 1 */ - struct bmi2_feature_config step_params_config = { 0, 0, 0 }; - - /* Variable to index the page number */ - uint8_t page_idx; - - /* Variable to define the start page */ - uint8_t start_page; - - /* Variable to define start address of the parameters */ - uint8_t start_addr; - - /* Variable to define number of bytes */ - uint8_t n_bytes = (BMI2_STEP_CNT_N_PARAMS * 2); - - /* Variable to store number of pages */ - uint8_t n_pages = (n_bytes / 16); - - /* Variable to define the end page */ - uint8_t end_page; - - /* Variable to define the remaining bytes to be read */ - uint8_t remain_len; - - /* Variable to define the maximum words to be read in a page */ - uint8_t max_len = BMI2_FEAT_SIZE_IN_BYTES; - - /* Variable index bytes in a page */ - uint8_t page_byte_idx; - - /* Variable to index the parameters */ - uint8_t param_idx = 0; - - /* Search for step counter parameter feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&step_params_config, BMI2_STEP_COUNTER_PARAMS, dev); - if (feat_found) - { - /* Get the start page for the step counter parameters */ - start_page = step_params_config.page; - - /* Get the end page for the step counter parameters */ - end_page = start_page + n_pages; - - /* Get the start address for the step counter parameters */ - start_addr = step_params_config.start_addr; - - /* Get the remaining length of bytes to be read */ - remain_len = (uint8_t)((n_bytes - (n_pages * 16)) + start_addr); - for (page_idx = start_page; page_idx <= end_page; page_idx++) - { - /* Get the configuration from the respective page */ - rslt = bmi2_get_feat_config(page_idx, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Start from address 0x00 when switched to next page */ - if (page_idx > start_page) - { - start_addr = 0; - } - - /* Remaining number of bytes to be read in the page */ - if (page_idx == end_page) - { - max_len = remain_len; - } - - /* Get the offset */ - page_byte_idx = start_addr; - while (page_byte_idx < max_len) - { - /* Get word to calculate the parameter*/ - lsb = (uint16_t) feat_config[page_byte_idx++]; - if (page_byte_idx < max_len) - { - msb = ((uint16_t) feat_config[page_byte_idx++] << 8); - } - - lsb_msb = lsb | msb; - - /* Get parameters 1 to 25 */ - step_count_params[param_idx] = lsb_msb & BMI2_STEP_COUNT_PARAMS_MASK; - - /* Increment to next parameter */ - param_idx++; - } - } - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets step counter/detector/activity configurations. - */ -static int8_t get_step_config(struct bmi2_step_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define LSB */ - uint16_t lsb = 0; - - /* Variable to define MSB */ - uint16_t msb = 0; - - /* Variable to define a word */ - uint16_t lsb_msb = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for step counter */ - struct bmi2_feature_config step_count_config = { 0, 0, 0 }; - - /* Search for step counter 4 feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&step_count_config, BMI2_STEP_COUNTER, dev); - if (feat_found) - { - /* Get the configuration from the page where step counter 4 parameter resides */ - rslt = bmi2_get_feat_config(step_count_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for feature enable for step counter/detector/activity */ - idx = step_count_config.start_addr; - - /* Get word to calculate water-mark level and reset counter */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get water-mark level */ - config->watermark_level = lsb_msb & BMI2_STEP_COUNT_WM_LEVEL_MASK; - - /* Get reset counter */ - config->reset_counter = (lsb_msb & BMI2_STEP_COUNT_RST_CNT_MASK) >> BMI2_STEP_COUNT_RST_CNT_POS; - - /* Get word to calculate output configuration of step detector and activity */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - config->step_buffer_size = (lsb_msb & BMI2_STEP_BUFFER_SIZE_MASK) >> BMI2_STEP_BUFFER_SIZE_POS; - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets the output values of step counter. - */ -static int8_t get_step_counter_output(uint32_t *step_count, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variables to define index */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature output for step counter */ - struct bmi2_feature_config step_cnt_out_config = { 0, 0, 0 }; - - /* Search for step counter output feature and extract its configuration details */ - feat_found = extract_output_feat_config(&step_cnt_out_config, BMI2_STEP_COUNTER, dev); - if (feat_found) - { - /* Get the feature output configuration for step-counter */ - rslt = bmi2_get_feat_config(step_cnt_out_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for step counter output */ - idx = step_cnt_out_config.start_addr; - - /* Get the step counter output in 4 bytes */ - *step_count = (uint32_t) feat_config[idx++]; - *step_count |= ((uint32_t) feat_config[idx++] << 8); - *step_count |= ((uint32_t) feat_config[idx++] << 16); - *step_count |= ((uint32_t) feat_config[idx++] << 24); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets the error status related to NVM. - */ -static int8_t get_nvm_error_status(struct bmi2_nvm_err_status *nvm_err_stat, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variables to define index */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature output for NVM error status */ - struct bmi2_feature_config nvm_err_cfg = { 0, 0, 0 }; - - /* Search for NVM error status feature and extract its configuration details */ - feat_found = extract_output_feat_config(&nvm_err_cfg, BMI2_NVM_STATUS, dev); - if (feat_found) - { - /* Get the feature output configuration for NVM error status */ - rslt = bmi2_get_feat_config(nvm_err_cfg.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for NVM error status */ - idx = nvm_err_cfg.start_addr; - - /* Increment index to get the error status */ - idx++; - - /* Error when NVM load action fails */ - nvm_err_stat->load_error = BMI2_GET_BIT_POS0(feat_config[idx], BMI2_NVM_LOAD_ERR_STATUS); - - /* Error when NVM program action fails */ - nvm_err_stat->prog_error = BMI2_GET_BITS(feat_config[idx], BMI2_NVM_PROG_ERR_STATUS); - - /* Error when NVM erase action fails */ - nvm_err_stat->erase_error = BMI2_GET_BITS(feat_config[idx], BMI2_NVM_ERASE_ERR_STATUS); - - /* Error when NVM program limit is exceeded */ - nvm_err_stat->exceed_error = BMI2_GET_BITS(feat_config[idx], BMI2_NVM_END_EXCEED_STATUS); - - /* Error when NVM privilege mode is not acquired */ - nvm_err_stat->privil_error = BMI2_GET_BITS(feat_config[idx], BMI2_NVM_PRIV_ERR_STATUS); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API is used to get enable status of gyroscope user gain - * update. - */ -static int8_t get_user_gain_upd_status(uint8_t *status, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt = BMI2_OK; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Variable to check APS status */ - uint8_t aps_stat = 0; - - /* Initialize feature configuration for gyroscope user gain */ - struct bmi2_feature_config gyr_user_gain_cfg = { 0, 0, 0 }; - - /* Search for user gain feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&gyr_user_gain_cfg, BMI2_GYRO_GAIN_UPDATE, dev); - if (feat_found) - { - /* Disable advance power save */ - aps_stat = dev->aps_status; - if (aps_stat == BMI2_ENABLE) - { - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - } - - if (rslt == BMI2_OK) - { - /* Get the configuration from the page where user gain feature resides */ - rslt = bmi2_get_feat_config(gyr_user_gain_cfg.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of user gain */ - idx = gyr_user_gain_cfg.start_addr + BMI2_GYR_USER_GAIN_FEAT_EN_OFFSET; - - /* Set the feature enable status */ - *status = BMI2_GET_BITS(feat_config[idx], BMI2_GYR_USER_GAIN_FEAT_EN); - } - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - /* Enable Advance power save if disabled while configuring and not when already disabled */ - if ((rslt == BMI2_OK) && (aps_stat == BMI2_ENABLE)) - { - rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - } - - return rslt; -} - -/*! - * @brief This internal API is used to parse and store the activity recognition - * output from the FIFO data. - */ -static int8_t unpack_act_recog_output(struct bmi2_act_recog_output *act_recog, - uint16_t *data_index, - const struct bmi2_fifo_frame *fifo) -{ - /* Variable to define error */ - int8_t rslt = BMI2_OK; - - /* Variables to define 4 bytes of sensor time */ - uint32_t time_stamp_byte4 = 0; - uint32_t time_stamp_byte3 = 0; - uint32_t time_stamp_byte2 = 0; - uint32_t time_stamp_byte1 = 0; - - /* Validate data index */ - if ((*data_index + BMI2_FIFO_VIRT_ACT_DATA_LENGTH) >= fifo->length) - { - /* Update the data index to the last byte */ - (*data_index) = fifo->length; - - /* FIFO is empty */ - rslt = BMI2_W_FIFO_EMPTY; - } - else - { - /* Get time-stamp from the activity recognition frame */ - time_stamp_byte4 = ((uint32_t)(fifo->data[(*data_index) + 3]) << 24); - time_stamp_byte3 = ((uint32_t)(fifo->data[(*data_index) + 2]) << 16); - time_stamp_byte2 = fifo->data[(*data_index) + 1] << 8; - time_stamp_byte1 = fifo->data[(*data_index)]; - - /* Update time-stamp from the virtual frame */ - act_recog->time_stamp = (time_stamp_byte4 | time_stamp_byte3 | time_stamp_byte2 | time_stamp_byte1); - - /* Move the data index by 4 bytes */ - (*data_index) = (*data_index) + BMI2_FIFO_VIRT_ACT_TIME_LENGTH; - - /* Update the previous activity from the virtual frame */ - act_recog->prev_act = fifo->data[(*data_index)]; - - /* Move the data index by 1 byte */ - (*data_index) = (*data_index) + BMI2_FIFO_VIRT_ACT_TYPE_LENGTH; - - /* Update the current activity from the virtual frame */ - act_recog->curr_act = fifo->data[(*data_index)]; - - /* Move the data index by 1 byte */ - (*data_index) = (*data_index) + BMI2_FIFO_VIRT_ACT_STAT_LENGTH; - - /* More frames could be read */ - rslt = BMI2_W_PARTIAL_READ; - } - - return rslt; -} - -/*! - * @brief This internal API gets the error status related to virtual frames. - */ -static int8_t get_vfrm_error_status(struct bmi2_vfrm_err_status *vfrm_err_stat, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variables to define index */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature output for VFRM error status */ - struct bmi2_feature_config vfrm_err_cfg = { 0, 0, 0 }; - - /* Search for VFRM error status feature and extract its configuration details */ - feat_found = extract_output_feat_config(&vfrm_err_cfg, BMI2_VFRM_STATUS, dev); - if (feat_found) - { - /* Get the feature output configuration for VFRM error status */ - rslt = bmi2_get_feat_config(vfrm_err_cfg.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for VFRM error status */ - idx = vfrm_err_cfg.start_addr; - - /* Increment index to get the error status */ - idx++; - - /* Internal error while acquiring lock for FIFO */ - vfrm_err_stat->lock_error = BMI2_GET_BITS(feat_config[idx], BMI2_VFRM_LOCK_ERR_STATUS); - - /* Internal error while writing byte into FIFO */ - vfrm_err_stat->write_error = BMI2_GET_BITS(feat_config[idx], BMI2_VFRM_WRITE_ERR_STATUS); - - /* Internal error while writing into FIFO */ - vfrm_err_stat->fatal_error = BMI2_GET_BITS(feat_config[idx], BMI2_VFRM_FATAL_ERR_STATUS); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API skips S4S frame in the FIFO data while getting - * step activity output. - */ -static int8_t move_if_s4s_frame(const uint8_t *frame_header, uint16_t *data_index, const struct bmi2_fifo_frame *fifo) -{ - /* Variable to define error */ - int8_t rslt = BMI2_OK; - - /* Variable to extract virtual header byte */ - uint8_t virtual_header_mode; - - /* Variable to define pay-load in words */ - uint8_t payload_word = 0; - - /* Variable to define pay-load in bytes */ - uint8_t payload_bytes = 0; - - /* Extract virtual header mode from the frame header */ - virtual_header_mode = BMI2_GET_BITS(*frame_header, BMI2_FIFO_VIRT_FRM_MODE); - - /* If the extracted header byte is a virtual header */ - if (virtual_header_mode == BMI2_FIFO_VIRT_FRM_MODE) - { - /* If frame header is not activity recognition header */ - if (*frame_header != 0xC8) - { - /* Extract pay-load in words from the header byte */ - payload_word = BMI2_GET_BITS(*frame_header, BMI2_FIFO_VIRT_PAYLOAD) + 1; - - /* Convert to bytes */ - payload_bytes = (uint8_t)(payload_word * 2); - - /* Move the data index by those pay-load bytes */ - rslt = move_next_frame(data_index, payload_bytes, fifo); - } - } - - return rslt; -} - -/*! - * @brief This internal API enables/disables compensation of the gain defined - * in the GAIN register. - */ -static int8_t enable_gyro_gain(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define register data */ - uint8_t reg_data = 0; - - rslt = bmi2_get_regs(BMI2_GYR_OFF_COMP_6_ADDR, ®_data, 1, dev); - if (rslt == BMI2_OK) - { - reg_data = BMI2_SET_BITS(reg_data, BMI2_GYR_GAIN_EN, enable); - rslt = bmi2_set_regs(BMI2_GYR_OFF_COMP_6_ADDR, ®_data, 1, dev); - } - - return rslt; -} - -/*! - * @brief This internal API is used to extract the output feature configuration - * details from the look-up table. - */ -static uint8_t extract_output_feat_config(struct bmi2_feature_config *feat_output, - uint8_t type, - const struct bmi2_dev *dev) -{ - /* Variable to define loop */ - uint8_t loop = 0; - - /* Variable to set flag */ - uint8_t feat_found = BMI2_FALSE; - - /* Search for the output feature from the output configuration array */ - while (loop < dev->out_sens) - { - if (dev->feat_output[loop].type == type) - { - *feat_output = dev->feat_output[loop]; - feat_found = BMI2_TRUE; - break; - } - - loop++; - } - - /* Return flag */ - return feat_found; -} - -/*! - * @brief This internal API is used to move the data index ahead of the - * current_frame_length parameter when unnecessary FIFO data appears while - * extracting the user specified data. - */ -static int8_t move_next_frame(uint16_t *data_index, uint8_t current_frame_length, const struct bmi2_fifo_frame *fifo) -{ - /* Variables to define error */ - int8_t rslt = BMI2_OK; - - /* Validate data index */ - if (((*data_index) + current_frame_length) > fifo->length) - { - /* Move the data index to the last byte */ - (*data_index) = fifo->length; - - /* FIFO is empty */ - rslt = BMI2_W_FIFO_EMPTY; - } - else - { - /* Move the data index to next frame */ - (*data_index) = (*data_index) + current_frame_length; - - /* More frames could be read */ - rslt = BMI2_W_PARTIAL_READ; - } - - return rslt; -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_context.h b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_context.h deleted file mode 100644 index 4708dc7db3..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_context.h +++ /dev/null @@ -1,493 +0,0 @@ -/** -* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. -* -* BSD-3-Clause -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* 1. Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* -* 2. Redistributions in binary form must reproduce the above copyright -* notice, this list of conditions and the following disclaimer in the -* documentation and/or other materials provided with the distribution. -* -* 3. Neither the name of the copyright holder nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) -* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, -* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING -* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -* -* @file bmi270_context.h -* @date 2020-11-04 -* @version v2.63.1 -* -*/ - -/** - * \ingroup bmi2xy - * \defgroup bmi270_context BMI270_CONTEXT - * @brief Sensor driver for BMI270_CONTEXT sensor - */ - -#ifndef BMI270_CONTEXT_H_ -#define BMI270_CONTEXT_H_ - -/*! CPP guard */ -#ifdef __cplusplus -extern "C" { -#endif - -/***************************************************************************/ - -/*! Header files - ****************************************************************************/ -#include "bmi2.h" - -/***************************************************************************/ - -/*! Macro definitions - ****************************************************************************/ - -/*! @name BMI270_CONTEXT Chip identifier */ -#define BMI270_CONTEXT_CHIP_ID UINT8_C(0x24) - -/*! @name BMI270_CONTEXT feature input start addresses */ -#define BMI270_CONTEXT_CONFIG_ID_STRT_ADDR UINT8_C(0x06) -#define BMI270_CONTEXT_STEP_CNT_1_STRT_ADDR UINT8_C(0x00) -#define BMI270_CONTEXT_STEP_CNT_4_STRT_ADDR UINT8_C(0x02) -#define BMI270_CONTEXT_MAX_BURST_LEN_STRT_ADDR UINT8_C(0x08) -#define BMI270_CONTEXT_CRT_GYRO_SELF_TEST_STRT_ADDR UINT8_C(0x09) -#define BMI270_CONTEXT_ABORT_STRT_ADDR UINT8_C(0x09) -#define BMI270_CONTEXT_NVM_PROG_PREP_STRT_ADDR UINT8_C(0x0A) -#define BMI270_CONTEXT_ACT_RGN_SETT_STRT_ADDR UINT8_C(0x00) -#define BMI270_CONTEXT_ACT_RGN_STRT_ADDR UINT8_C(0x0A) - -/*! @name BMI270_CONTEXT feature output start addresses */ -#define BMI270_CONTEXT_STEP_CNT_OUT_STRT_ADDR UINT8_C(0x00) -#define BMI270_CONTEXT_GYR_USER_GAIN_OUT_STRT_ADDR UINT8_C(0x04) -#define BMI270_CONTEXT_GYRO_CROSS_SENSE_STRT_ADDR UINT8_C(0x0C) -#define BMI270_CONTEXT_NVM_VFRM_OUT_STRT_ADDR UINT8_C(0x0E) - -/*! @name Defines maximum number of pages */ -#define BMI270_CONTEXT_MAX_PAGE_NUM UINT8_C(8) - -/*! @name Defines maximum number of feature input configurations */ -#define BMI270_CONTEXT_MAX_FEAT_IN UINT8_C(10) - -/*! @name Defines maximum number of feature outputs */ -#define BMI270_CONTEXT_MAX_FEAT_OUT UINT8_C(5) - -/*! @name Mask definitions for feature interrupt status bits */ -#define BMI270_CONTEXT_STEP_CNT_STATUS_MASK UINT8_C(0x01) - -/*! @name Mask definitions for feature interrupt mapping bits */ -#define BMI270_C_INT_STEP_COUNTER_MASK UINT8_C(0x01) -#define BMI270_C_INT_STEP_DETECTOR_MASK UINT8_C(0x01) - -/*! @name Defines maximum number of feature interrupts */ -#define BMI270_C_MAX_INT_MAP UINT8_C(2) - -/***************************************************************************/ - -/*! BMI270_CONTEXT User Interface function prototypes - ****************************************************************************/ - -/** - * \ingroup bmi270_context - * \defgroup bmi270_contextApiInit Initialization - * @brief Initialize the sensor and device structure - */ - -/*! - * \ingroup bmi270_contextApiInit - * \page bmi270_context_api_bmi270_context_init bmi270_context_init - * \code - * int8_t bmi270_context_init(struct bmi2_dev *dev); - * \endcode - * @details This API: - * 1) updates the device structure with address of the configuration file. - * 2) Initializes BMI270_CONTEXT sensor. - * 3) Writes the configuration file. - * 4) Updates the feature offset parameters in the device structure. - * 5) Updates the maximum number of pages, in the device structure. - * - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi270_context_init(struct bmi2_dev *dev); - -/** - * \ingroup bmi270_context - * \defgroup bmi270_contextApiSensor Feature Set - * @brief Enable / Disable features of the sensor - */ - -/*! - * \ingroup bmi270_contextApiSensor - * \page bmi270_context_api_bmi270_context_sensor_enable bmi270_context_sensor_enable - * \code - * int8_t bmi270_context_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); - * \endcode - * @details This API selects the sensors/features to be enabled. - * - * @param[in] sens_list : Pointer to select the sensor/feature. - * @param[in] n_sens : Number of sensors selected. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @note Sensors/features that can be enabled. - * - *@verbatim - * sens_list | Values - * ----------------------------|----------- - * BMI2_ACCEL | 0 - * BMI2_GYRO | 1 - * BMI2_AUX | 2 - * BMI2_STEP_DETECTOR | 6 - * BMI2_STEP_COUNTER | 7 - * BMI2_GYRO_GAIN_UPDATE | 9 - * BMI2_ACTIVITY_RECOGNITION | 34 - *@endverbatim - * - * @note : - * example uint8_t sens_list[2] = {BMI2_ACCEL, BMI2_GYRO}; - * uint8_t n_sens = 2; - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi270_context_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); - -/*! - * \ingroup bmi270_contextApiSensor - * \page bmi270_context_api_bmi270_context_sensor_disable bmi270_context_sensor_disable - * \code - * int8_t bmi270_context_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); - * \endcode - * @details This API selects the sensors/features to be disabled. - * - * @param[in] sens_list : Pointer to select the sensor/feature. - * @param[in] n_sens : Number of sensors selected. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @note Sensors/features that can be disabled. - * - *@verbatim - * sens_list | Values - * ----------------------------|----------- - * BMI2_ACCEL | 0 - * BMI2_GYRO | 1 - * BMI2_AUX | 2 - * BMI2_STEP_DETECTOR | 6 - * BMI2_STEP_COUNTER | 7 - * BMI2_GYRO_GAIN_UPDATE | 9 - * BMI2_ACTIVITY_RECOGNITION | 34 - *@endverbatim - * - * @note : - * example uint8_t sens_list[2] = {BMI2_ACCEL, BMI2_GYRO}; - * uint8_t n_sens = 2; - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi270_context_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); - -/** - * \ingroup bmi270_context - * \defgroup bmi270_contextApiSensorC Sensor Configuration - * @brief Enable / Disable feature configuration of the sensor - */ - -/*! - * \ingroup bmi270_contextApiSensorC - * \page bmi270_context_api_bmi270_context_set_sensor_config bmi270_context_set_sensor_config - * \code - * int8_t bmi270_context_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); - * \endcode - * @details This API sets the sensor/feature configuration. - * - * @param[in] sens_cfg : Structure instance of bmi2_sens_config. - * @param[in] n_sens : Number of sensors selected. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @note Sensors/features that can be configured - * - *@verbatim - * sens_list | Values - * ----------------------------|----------- - * BMI2_STEP_DETECTOR | 6 - * BMI2_STEP_COUNTER | 7 - * BMI2_STEP_COUNTER_PARAMS | 28 - *@endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi270_context_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); - -/*! - * \ingroup bmi270_contextApiSensorC - * \page bmi270_context_api_bmi270_context_get_sensor_config bmi270_context_get_sensor_config - * \code - * int8_t bmi270_context_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); - * \endcode - * @details This API gets the sensor/feature configuration. - * - * @param[in] sens_cfg : Structure instance of bmi2_sens_config. - * @param[in] n_sens : Number of sensors selected. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @note Sensors/features whose configurations can be read. - * - *@verbatim - * sens_list | Values - * ----------------------------|----------- - * BMI2_STEP_DETECTOR | 6 - * BMI2_STEP_COUNTER | 7 - * BMI2_STEP_COUNTER_PARAMS | 28 - *@endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi270_context_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); - -/** - * \ingroup bmi270_context - * \defgroup bmi270_contextApiSensorD Sensor Data - * @brief Get sensor data - */ - -/*! - * \ingroup bmi270_contextApiSensorD - * \page bmi270_context_api_bmi270_context_get_sensor_data bmi270_context_get_sensor_data - * \code - * int8_t bmi270_context_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev); - * \endcode - * @details This API gets the sensor/feature data for accelerometer, gyroscope, - * auxiliary sensor, step counter, high-g, gyroscope user-gain update, - * orientation, gyroscope cross sensitivity and error status for NVM and VFRM. - * - * @param[out] sensor_data : Structure instance of bmi2_sensor_data. - * @param[in] n_sens : Number of sensors selected. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @note Sensors/features whose data can be read - * - *@verbatim - * sens_list | Values - * ---------------------|----------- - * BMI2_STEP_COUNTER | 7 - * BMI2_NVM_STATUS | 38 - * BMI2_VFRM_STATUS | 39 - *@endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi270_context_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev); - -/** - * \ingroup bmi270_context - * \defgroup bmi270_contextApiARecog Activity recognition settings - * @brief Set / Get activity recognition settings of the sensor - */ - -/*! - * \ingroup bmi270_contextApiARecog - * \page bmi270_context_api_bmi270_context_get_act_recg_sett bmi270_context_get_act_recg_sett - * \code - * int8_t bmi270_context_get_act_recg_sett(struct bmi2_act_recg_sett *sett, struct bmi2_dev *dev); - * \endcode - * @details This api is used for retrieving the following activity recognition settings currently set. - * enable/disable post processing(0/1) by default -> 1(enable), - * Setting the min & max Gini's diversity index (GDI) threshold. min_GDI_tres(0-0XFFFF) by default ->(0x06e1) - * max_GDI_tres(0-0xFFFF) by default ->(0x0A66) - * buffer size for post processing. range (1-0x0A) default -> (0x0A) - * min segment confidence. range (1-0x0A) default -> (0x0A) - * - * @param[in] sett : Structure instance of bmi2_act_recg_sett. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi270_context_get_act_recg_sett(struct bmi2_act_recg_sett *sett, struct bmi2_dev *dev); - -/*! - * \ingroup bmi270_contextApiARecog - * \page bmi270_context_api_bmi270_context_set_act_recg_sett bmi270_context_set_act_recg_sett - * \code - * int8_t bmi270_context_set_act_recg_sett(const struct bmi2_act_recg_sett *sett, struct bmi2_dev *dev); - * \endcode - * @details This api is used for setting the following activity recognition settings - * enable/disable post processing(0/1) by default -> 1(enable), - * Setting the min & max Gini's diversity index (GDI) threshold. min_GDI_tres(0-0XFFFF) by default ->(0x06e1) - * max_GDI_tres(0-0xFFFF) by default ->(0x0A66) - * buffer size for post processing. range (1-0x0A) default -> (0x0A) - * min segment confidence. range (1-0x0A) default -> (0x0A) - * - * @param[in] sett : Structure instance of bmi2_act_recg_sett. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi270_context_set_act_recg_sett(const struct bmi2_act_recg_sett *sett, struct bmi2_dev *dev); - -/** - * \ingroup bmi270_contex - * \defgroup bmi270_contextApiactOut Activity Output - * @brief Activity output operations of the sensor - */ - -/*! - * \ingroup bmi270_contextApiactOut - * \page bmi270_context_api_bmi270_context_get_act_recog_output bmi270_context_get_act_recog_output - * \code - * int8_t bmi270_context_get_act_recog_output(struct bmi2_act_recog_output *act_recog, - * uint16_t *act_frm_len, - * struct bmi2_fifo_frame *fifo, - * const struct bmi2_dev *dev); - * - * \endcode - * @details This internal API is used to parse the activity output from the - * FIFO in header mode. - * - * @param[out] act_recog : Pointer to buffer where the parsed activity data - * bytes are stored. - * @param[in] act_frm_len : Number of activity frames parsed. - * @param[in] fifo : Structure instance of bmi2_fifo_frame. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @verbatim - * ---------------------------------------------------------------------------- - * bmi2_act_rec_output | - * Structure parameters | Description - *--------------------------|-------------------------------------------------- - * time_stamp | time-stamp (expressed in 50Hz ticks) - * -------------------------|--------------------------------------------------- - * type | Type of activity - * -------------------------|--------------------------------------------------- - * stat | Activity status - * -------------------------|--------------------------------------------------- - * @endverbatim - * - *@verbatim - * type | Activities - *----------|--------------------- - * 0 | UNKNOWN - * 1 | STILL - * 2 | WALK - * 3 | RUN - * 4 | BIKE - * 5 | VEHICLE - * 6 | TILTED - *@endverbatim - * - * - *@verbatim - * stat | Activity status - *----------|--------------------- - * 1 | START - * 2 | END - *@endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi270_context_get_act_recog_output(struct bmi2_act_recog_output *act_recog, - uint16_t *act_frm_len, - struct bmi2_fifo_frame *fifo, - const struct bmi2_dev *dev); - -/** - * \ingroup bmi270_contex - * \defgroup bmi270_contextApiGyroUG Gyro User Gain - * @brief Set / Get Gyro User Gain of the sensor - */ - -/*! - * \ingroup bmi270_contextApiGyroUG - * \page bmi270_context_api_bmi270_context_update_gyro_user_gain bmi270_context_update_gyro_user_gain - * \code - * int8_t bmi270_context_update_gyro_user_gain(const struct bmi2_gyro_user_gain_config *user_gain, struct bmi2_dev *dev); - * \endcode - * @details This API updates the gyroscope user-gain. - * - * @param[in] user_gain : Structure that stores user-gain configurations. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi270_context_update_gyro_user_gain(const struct bmi2_gyro_user_gain_config *user_gain, struct bmi2_dev *dev); - -/*! - * \ingroup bmi270_contextApiGyroUG - * \page bmi270_context_api_bmi270_context_read_gyro_user_gain bmi270_context_read_gyro_user_gain - * \code - * int8_t bmi270_context_read_gyro_user_gain(struct bmi2_gyro_user_gain_data *gyr_usr_gain, const struct bmi2_dev *dev); - * \endcode - * @details This API reads the compensated gyroscope user-gain values. - * - * @param[out] gyr_usr_gain : Structure that stores gain values. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi270_context_read_gyro_user_gain(struct bmi2_gyro_user_gain_data *gyr_usr_gain, struct bmi2_dev *dev); - -/*! - * \ingroup bmi270_contextApiInt - * \page bmi270_context_api_bmi270_context_map_feat_int bmi270_context_map_feat_int - * \code - * int8_t bmi270_context_map_feat_int(const struct bmi2_sens_int_config *sens_int, uint8_t n_sens, struct bmi2_dev *dev) - * \endcode - * @details This API maps/unmaps feature interrupts to that of interrupt pins. - * - * @param[in] sens_int : Structure instance of bmi2_sens_int_config. - * @param[in] n_sens : Number of interrupts to be mapped. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi270_context_map_feat_int(const struct bmi2_sens_int_config *sens_int, uint8_t n_sens, struct bmi2_dev *dev); - -/******************************************************************************/ -/*! @name C++ Guard Macros */ -/******************************************************************************/ -#ifdef __cplusplus -} -#endif /* End of CPP guard */ - -#endif /* BMI270_CONTEXT_H_ */ diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_hc.c b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_hc.c deleted file mode 100644 index a7c9172dd4..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_hc.c +++ /dev/null @@ -1,3165 +0,0 @@ -/** -* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. -* -* BSD-3-Clause -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* 1. Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* -* 2. Redistributions in binary form must reproduce the above copyright -* notice, this list of conditions and the following disclaimer in the -* documentation and/or other materials provided with the distribution. -* -* 3. Neither the name of the copyright holder nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) -* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, -* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING -* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -* -* @file bmi270_hc.c -* @date 2020-11-04 -* @version v2.63.1 -* -*/ - -/***************************************************************************/ - -/*! Header files - ****************************************************************************/ -#include "bmi270_hc.h" - -/***************************************************************************/ - -/*! Global Variable - ****************************************************************************/ - -/*! @name Global array that stores the configuration file of BMI270_huawei_context */ -const uint8_t bmi270_hc_config_file[] = { - 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x00, 0xb0, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0xa8, - 0x03, 0x80, 0x2e, 0x91, 0x03, 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x7d, 0xb0, 0x50, 0x30, 0x21, 0x2e, 0x59, 0xf5, - 0x10, 0x30, 0x21, 0x2e, 0x6a, 0xf5, 0x80, 0x2e, 0xae, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x39, 0x01, 0x00, 0x22, - 0x00, 0x76, 0x00, 0x00, 0x10, 0x00, 0x10, 0xd1, 0x00, 0x72, 0xb3, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, - 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, - 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, - 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, - 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, - 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, - 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, - 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, - 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, - 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0xfd, 0x2d, 0xd0, 0x86, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x03, 0x00, 0x08, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x01, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1a, 0x24, 0x22, 0x00, 0x80, 0x2e, 0x48, 0x02, 0x01, 0x2e, 0x49, 0xf1, 0x0b, - 0xbc, 0x10, 0x50, 0x0f, 0xb8, 0x00, 0x90, 0xfb, 0x7f, 0x07, 0x2f, 0x03, 0x2e, 0x21, 0xf2, 0x02, 0x31, 0x4a, 0x0a, - 0x23, 0x2e, 0x21, 0xf2, 0x09, 0x2c, 0x00, 0x30, 0x98, 0x2e, 0x0e, 0xc7, 0x03, 0x2e, 0x21, 0xf2, 0xf2, 0x3e, 0x4a, - 0x08, 0x23, 0x2e, 0x21, 0xf2, 0xfb, 0x6f, 0xf0, 0x5f, 0xb8, 0x2e, 0x13, 0x52, 0x00, 0x2e, 0x60, 0x40, 0x41, 0x40, - 0x0d, 0xbc, 0x98, 0xbc, 0xc0, 0x2e, 0x01, 0x0a, 0x0f, 0xb8, 0x43, 0x86, 0x25, 0x40, 0x04, 0x40, 0xd8, 0xbe, 0x2c, - 0x0b, 0x22, 0x11, 0x54, 0x42, 0x03, 0x80, 0x4b, 0x0e, 0xf6, 0x2f, 0xb8, 0x2e, 0x15, 0x50, 0x10, 0x50, 0x17, 0x52, - 0x05, 0x2e, 0x7e, 0x00, 0xfb, 0x7f, 0x00, 0x2e, 0x13, 0x40, 0x93, 0x42, 0x41, 0x0e, 0xfb, 0x2f, 0x98, 0x2e, 0xe0, - 0x03, 0x98, 0x2e, 0x87, 0xcf, 0x01, 0x2e, 0x8b, 0x00, 0x00, 0xb2, 0x08, 0x2f, 0x01, 0x2e, 0x69, 0xf7, 0xb1, 0x3f, - 0x01, 0x08, 0x01, 0x30, 0x23, 0x2e, 0x8b, 0x00, 0x21, 0x2e, 0x69, 0xf7, 0xfb, 0x6f, 0xf0, 0x5f, 0xb8, 0x2e, 0x98, - 0x00, 0xff, 0x3f, 0x00, 0x0c, 0xff, 0x0f, 0x00, 0x04, 0xc0, 0x00, 0x5b, 0xf5, 0x8f, 0x00, 0x1e, 0xf2, 0xfd, 0xf5, - 0x8d, 0x00, 0x95, 0x00, 0x95, 0x00, 0xe0, 0x00, 0x19, 0xf4, 0x66, 0xf5, 0x00, 0x18, 0xff, 0x00, 0x64, 0xf5, 0x9c, - 0x00, 0x81, 0x00, 0x83, 0x00, 0x7f, 0x00, 0xff, 0xfb, 0x21, 0x02, 0x00, 0x10, 0x00, 0x40, 0x3a, 0x0f, 0xeb, 0x00, - 0x7f, 0xff, 0xc2, 0xf5, 0x68, 0xf7, 0x34, 0x0f, 0x28, 0x0f, 0x2e, 0x0f, 0x80, 0x00, 0x4d, 0x0f, 0x58, 0xf7, 0x5b, - 0xf7, 0x50, 0x0f, 0x00, 0x80, 0xff, 0x7f, 0x86, 0x00, 0x3f, 0x0f, 0x52, 0x0f, 0xb3, 0xf1, 0x4c, 0x0f, 0x6c, 0xf7, - 0xb9, 0xf1, 0xc6, 0xf1, 0x00, 0xe0, 0x00, 0xff, 0xd1, 0xf5, 0x54, 0x0f, 0x57, 0x0f, 0xff, 0x03, 0x00, 0xfc, 0xf0, - 0x3f, 0xb9, 0x00, 0x2d, 0xf5, 0xca, 0xf5, 0x6d, 0x03, 0xf0, 0x07, 0xc3, 0x11, 0x1d, 0x08, 0x12, 0x08, 0xb7, 0x15, - 0x9c, 0x01, 0xed, 0x14, 0xc2, 0x53, 0xca, 0x10, 0xa2, 0x00, 0x74, 0x01, 0xba, 0x17, 0xb4, 0x56, 0xe8, 0x16, 0x9f, - 0x00, 0xd7, 0x13, 0x2f, 0x52, 0x8a, 0x51, 0x64, 0x01, 0xc6, 0x53, 0xf4, 0x0b, 0x06, 0x08, 0xfc, 0x57, 0xe3, 0x44, - 0xad, 0x55, 0x4c, 0x01, 0x62, 0x01, 0x0b, 0x08, 0xa0, 0x18, 0x39, 0x59, 0x56, 0x2b, 0x14, 0x01, 0x2a, 0x58, 0x5e, - 0x0c, 0xc1, 0x47, 0x16, 0x5a, 0xd2, 0x07, 0x2b, 0x21, 0x7f, 0x53, 0x03, 0x08, 0xca, 0x59, 0x1a, 0x0b, 0x5f, 0x57, - 0x36, 0x47, 0xa6, 0x17, 0x02, 0x01, 0x33, 0x49, 0xdd, 0x58, 0x71, 0x0c, 0x32, 0x08, 0xc6, 0x59, 0xd4, 0x42, 0x65, - 0x54, 0x15, 0x59, 0x64, 0x08, 0x0c, 0x08, 0x8c, 0x01, 0x98, 0x00, 0x0f, 0x48, 0x02, 0x58, 0x96, 0x0c, 0x1c, 0x43, - 0x77, 0x48, 0xe9, 0x00, 0x1e, 0x0c, 0xf5, 0x41, 0xf6, 0x46, 0x64, 0x51, 0x98, 0x41, 0xcc, 0x01, 0x66, 0x58, 0x70, - 0x45, 0x28, 0x21, 0xe7, 0x59, 0x22, 0x30, 0x00, 0x08, 0x71, 0x7d, 0x49, 0x01, 0x92, 0x02, 0xf5, 0xd6, 0xe8, 0x63, - 0x7a, 0xfa, 0x87, 0x05, 0x0f, 0xcb, 0xa3, 0x72, 0x37, 0xfc, 0xca, 0x03, 0x95, 0xc7, 0x2e, 0x6d, 0x39, 0xc4, 0xc8, - 0x3b, 0x91, 0x37, 0x29, 0x02, 0x9e, 0x01, 0x00, 0xf0, 0x33, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x2e, 0xd5, 0xb5, 0x80, 0x2e, 0x00, 0xc1, 0xfd, 0x2d, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x2d, 0x01, 0xd4, 0x7b, 0x3b, - 0x01, 0xdb, 0x7a, 0x04, 0x00, 0x3f, 0x7b, 0xcd, 0x6c, 0xc3, 0x04, 0x85, 0x09, 0xc3, 0x04, 0xec, 0xe6, 0x0c, 0x46, - 0x01, 0x00, 0x27, 0x00, 0x19, 0x00, 0x96, 0x00, 0xa0, 0x00, 0x01, 0x00, 0x0c, 0x00, 0xf0, 0x3c, 0x00, 0x01, 0x01, - 0x00, 0x03, 0x00, 0x01, 0x00, 0x0e, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x39, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0xe1, 0x06, 0x66, 0x0a, 0x0a, 0x00, 0x0a, 0x00, 0x5a, 0x8d, 0x33, - 0x73, 0xcd, 0x8c, 0x9a, 0x99, 0x71, 0x7d, 0xcd, 0x8c, 0xcd, 0x6c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x50, 0x98, 0x2e, 0xbd, 0x0e, 0x50, 0x32, 0x98, 0x2e, - 0x48, 0x03, 0x10, 0x30, 0x21, 0x2e, 0x21, 0xf2, 0x00, 0x30, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0x00, 0x2e, 0x01, - 0x80, 0x06, 0xa2, 0xfb, 0x2f, 0x01, 0x2e, 0x9b, 0x00, 0x00, 0xb2, 0x10, 0x2f, 0x01, 0x2e, 0x18, 0x00, 0x00, 0xb2, - 0x0c, 0x2f, 0x01, 0x54, 0x03, 0x52, 0x01, 0x50, 0x98, 0x2e, 0xc2, 0xc0, 0x98, 0x2e, 0x0e, 0xb1, 0x01, 0x50, 0x98, - 0x2e, 0xdd, 0xb5, 0x10, 0x30, 0x21, 0x2e, 0x19, 0x00, 0x01, 0x2e, 0x86, 0x00, 0x04, 0xae, 0x0b, 0x2f, 0x01, 0x2e, - 0x9b, 0x00, 0x00, 0xb2, 0x07, 0x2f, 0x01, 0x52, 0x98, 0x2e, 0x74, 0x0e, 0x00, 0xb2, 0x02, 0x2f, 0x10, 0x30, 0x21, - 0x2e, 0x79, 0x00, 0x01, 0x2e, 0x79, 0x00, 0x00, 0x90, 0x90, 0x2e, 0x14, 0x03, 0x01, 0x2e, 0x89, 0x00, 0x00, 0xb2, - 0x04, 0x2f, 0x98, 0x2e, 0x15, 0x0e, 0x00, 0x30, 0x21, 0x2e, 0x7c, 0x00, 0x01, 0x2e, 0x7c, 0x00, 0x00, 0xb2, 0x12, - 0x2f, 0x01, 0x2e, 0x86, 0x00, 0x00, 0x90, 0x02, 0x2f, 0x98, 0x2e, 0x05, 0x0e, 0x09, 0x2d, 0x98, 0x2e, 0x5d, 0x0d, - 0x01, 0x2e, 0x86, 0x00, 0x04, 0x90, 0x02, 0x2f, 0x50, 0x32, 0x98, 0x2e, 0x48, 0x03, 0x00, 0x30, 0x21, 0x2e, 0x7c, - 0x00, 0x01, 0x2e, 0x78, 0x00, 0x00, 0xb2, 0x90, 0x2e, 0x2c, 0x03, 0x01, 0x2e, 0x78, 0x00, 0x81, 0x30, 0x01, 0x08, - 0x00, 0xb2, 0x61, 0x2f, 0x03, 0x2e, 0x24, 0x02, 0x01, 0x2e, 0x86, 0x00, 0x98, 0xbc, 0x98, 0xb8, 0x05, 0xb2, 0x0d, - 0x58, 0x23, 0x2f, 0x07, 0x90, 0x07, 0x54, 0x00, 0x30, 0x37, 0x2f, 0x15, 0x41, 0x04, 0x41, 0xdc, 0xbe, 0x44, 0xbe, - 0xdc, 0xba, 0x2c, 0x01, 0x61, 0x00, 0x0d, 0x56, 0x4a, 0x0f, 0x0c, 0x2f, 0xd1, 0x42, 0x94, 0xb8, 0xc1, 0x42, 0x11, - 0x30, 0x05, 0x2e, 0x6a, 0xf7, 0x2c, 0xbd, 0x2f, 0xb9, 0x80, 0xb2, 0x08, 0x22, 0x98, 0x2e, 0xa4, 0xb1, 0x21, 0x2d, - 0x61, 0x30, 0x23, 0x2e, 0x86, 0x00, 0x98, 0x2e, 0xa4, 0xb1, 0x00, 0x30, 0x21, 0x2e, 0x5a, 0xf5, 0x18, 0x2d, 0xf1, - 0x7f, 0x50, 0x30, 0x98, 0x2e, 0x48, 0x03, 0x0d, 0x52, 0x05, 0x50, 0x50, 0x42, 0x70, 0x30, 0x0b, 0x54, 0x42, 0x42, - 0x7e, 0x82, 0xf2, 0x6f, 0x80, 0xb2, 0x42, 0x42, 0x05, 0x2f, 0x21, 0x2e, 0x86, 0x00, 0x10, 0x30, 0x98, 0x2e, 0xa4, - 0xb1, 0x03, 0x2d, 0x60, 0x30, 0x21, 0x2e, 0x86, 0x00, 0x01, 0x2e, 0x86, 0x00, 0x06, 0x90, 0x18, 0x2f, 0x01, 0x2e, - 0x77, 0x00, 0x09, 0x54, 0x05, 0x52, 0xf0, 0x7f, 0x98, 0x2e, 0x7a, 0xc1, 0xf1, 0x6f, 0x08, 0x1a, 0x40, 0x30, 0x08, - 0x2f, 0x21, 0x2e, 0x86, 0x00, 0x20, 0x30, 0x98, 0x2e, 0xea, 0x03, 0x50, 0x32, 0x98, 0x2e, 0x48, 0x03, 0x05, 0x2d, - 0x98, 0x2e, 0x1e, 0x0e, 0x00, 0x30, 0x21, 0x2e, 0x86, 0x00, 0x00, 0x30, 0x21, 0x2e, 0x78, 0x00, 0x18, 0x2d, 0x01, - 0x2e, 0x86, 0x00, 0x03, 0xaa, 0x01, 0x2f, 0x98, 0x2e, 0x2b, 0x0e, 0x01, 0x2e, 0x86, 0x00, 0x3f, 0x80, 0x03, 0xa2, - 0x01, 0x2f, 0x00, 0x2e, 0x02, 0x2d, 0x98, 0x2e, 0x41, 0x0e, 0x30, 0x30, 0x98, 0x2e, 0xaf, 0xb1, 0x00, 0x30, 0x21, - 0x2e, 0x79, 0x00, 0x50, 0x32, 0x98, 0x2e, 0x48, 0x03, 0x01, 0x2e, 0x19, 0x00, 0x00, 0xb2, 0x10, 0x2f, 0x01, 0x2e, - 0x87, 0x00, 0x21, 0x2e, 0x8f, 0x00, 0x0f, 0x52, 0x7e, 0x82, 0x11, 0x50, 0x41, 0x40, 0x18, 0xb9, 0x11, 0x42, 0x02, - 0x42, 0x02, 0x80, 0x00, 0x2e, 0x01, 0x40, 0x01, 0x42, 0x98, 0x2e, 0xe1, 0x00, 0x00, 0x30, 0x21, 0x2e, 0x19, 0x00, - 0x21, 0x2e, 0x9b, 0x00, 0x80, 0x2e, 0x52, 0x02, 0x21, 0x2e, 0x59, 0xf5, 0x10, 0x30, 0xc0, 0x2e, 0x21, 0x2e, 0x4a, - 0xf1, 0x80, 0x2e, 0x00, 0xc1, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x9a, 0x01, - 0x34, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x20, 0x50, 0xe7, 0x7f, 0xf6, 0x7f, 0x46, 0x30, 0x0f, 0x2e, 0xa4, 0xf1, 0xbe, 0x09, 0x80, 0xb3, 0x06, - 0x2f, 0x0d, 0x2e, 0x86, 0x00, 0x84, 0xaf, 0x02, 0x2f, 0x16, 0x30, 0x2d, 0x2e, 0x7c, 0x00, 0x86, 0x30, 0x2d, 0x2e, - 0x60, 0xf5, 0xf6, 0x6f, 0xe7, 0x6f, 0xe0, 0x5f, 0xc8, 0x2e, 0xa0, 0x50, 0x80, 0x7f, 0xe7, 0x7f, 0xd5, 0x7f, 0xc4, - 0x7f, 0xb3, 0x7f, 0xa2, 0x7f, 0x91, 0x7f, 0xf6, 0x7f, 0x7b, 0x7f, 0x00, 0x2e, 0x01, 0x2e, 0x60, 0xf5, 0x60, 0x7f, - 0x98, 0x2e, 0xcd, 0x00, 0x62, 0x6f, 0x01, 0x32, 0x91, 0x08, 0x80, 0xb2, 0x0d, 0x2f, 0x00, 0xb2, 0x03, 0x2f, 0x05, - 0x2e, 0x18, 0x00, 0x80, 0x90, 0x05, 0x2f, 0x25, 0x56, 0x02, 0x30, 0xc1, 0x42, 0xc2, 0x86, 0x00, 0x2e, 0xc2, 0x42, - 0x23, 0x2e, 0x60, 0xf5, 0x00, 0x90, 0x00, 0x30, 0x06, 0x2f, 0x21, 0x2e, 0x7a, 0x00, 0x23, 0x50, 0x21, 0x2e, 0x5a, - 0xf2, 0x98, 0x2e, 0xfb, 0x01, 0xf6, 0x6f, 0x80, 0x6f, 0x91, 0x6f, 0xa2, 0x6f, 0xb3, 0x6f, 0xc4, 0x6f, 0xd5, 0x6f, - 0xe7, 0x6f, 0x7b, 0x6f, 0x60, 0x5f, 0xc8, 0x2e, 0x03, 0x2e, 0x7e, 0x00, 0x16, 0xb8, 0x02, 0x34, 0x4a, 0x0c, 0x21, - 0x2e, 0x2d, 0xf5, 0xc0, 0x2e, 0x23, 0x2e, 0x7e, 0x00, 0x03, 0xbc, 0x21, 0x2e, 0x87, 0x00, 0x03, 0x2e, 0x87, 0x00, - 0x40, 0xb2, 0x10, 0x30, 0x21, 0x2e, 0x19, 0x00, 0x01, 0x30, 0x05, 0x2f, 0x05, 0x2e, 0x8a, 0x00, 0x80, 0x90, 0x01, - 0x2f, 0x23, 0x2e, 0x6f, 0xf5, 0xc0, 0x2e, 0x21, 0x2e, 0x8b, 0x00, 0x80, 0x2e, 0x00, 0xc1, 0xc0, 0x50, 0xe7, 0x7f, - 0xf6, 0x7f, 0x26, 0x30, 0x0f, 0x2e, 0x61, 0xf5, 0x2f, 0x2e, 0x78, 0x00, 0x0f, 0x2e, 0x78, 0x00, 0xbe, 0x09, 0xa2, - 0x7f, 0x91, 0x7f, 0x80, 0x7f, 0x80, 0xb3, 0xd5, 0x7f, 0xc4, 0x7f, 0xb3, 0x7f, 0x7b, 0x7f, 0x11, 0x2f, 0x19, 0x50, - 0x1a, 0x25, 0x12, 0x40, 0x42, 0x7f, 0x74, 0x82, 0x12, 0x40, 0x52, 0x7f, 0x00, 0x2e, 0x00, 0x40, 0x60, 0x7f, 0x98, - 0x2e, 0x6a, 0xd6, 0x01, 0x2e, 0x61, 0xf7, 0x01, 0x31, 0x01, 0x0a, 0x21, 0x2e, 0x61, 0xf7, 0x80, 0x30, 0x03, 0x2e, - 0x78, 0x00, 0x08, 0x08, 0x00, 0xb2, 0x42, 0x2f, 0x03, 0x2e, 0x24, 0x02, 0x01, 0x2e, 0x24, 0x02, 0x97, 0xbc, 0x06, - 0xbc, 0x9f, 0xb8, 0x0f, 0xb8, 0x00, 0x90, 0x23, 0x2e, 0x8a, 0x00, 0x10, 0x30, 0x01, 0x30, 0x2a, 0x2f, 0x03, 0x2e, - 0x86, 0x00, 0x44, 0xb2, 0x05, 0x2f, 0x47, 0xb2, 0x00, 0x30, 0x2d, 0x2f, 0x21, 0x2e, 0x78, 0x00, 0x2b, 0x2d, 0x03, - 0x2e, 0xfd, 0xf5, 0x9e, 0xbc, 0x9f, 0xb8, 0x40, 0x90, 0x14, 0x2f, 0x03, 0x2e, 0xfc, 0xf5, 0x99, 0xbc, 0x9f, 0xb8, - 0x40, 0x90, 0x0e, 0x2f, 0x03, 0x2e, 0x49, 0xf1, 0x1b, 0x54, 0x4a, 0x08, 0x40, 0x90, 0x08, 0x2f, 0x98, 0x2e, 0xcd, - 0x00, 0x00, 0xb2, 0x10, 0x30, 0x03, 0x2f, 0x50, 0x30, 0x21, 0x2e, 0x86, 0x00, 0x10, 0x2d, 0x98, 0x2e, 0xea, 0x03, - 0x00, 0x30, 0x21, 0x2e, 0x78, 0x00, 0x0a, 0x2d, 0x05, 0x2e, 0x69, 0xf7, 0x2d, 0xbd, 0x2f, 0xb9, 0x80, 0xb2, 0x01, - 0x2f, 0x21, 0x2e, 0x79, 0x00, 0x23, 0x2e, 0x78, 0x00, 0xe0, 0x31, 0x21, 0x2e, 0x61, 0xf5, 0xf6, 0x6f, 0xe7, 0x6f, - 0x80, 0x6f, 0xa2, 0x6f, 0xb3, 0x6f, 0xc4, 0x6f, 0xd5, 0x6f, 0x7b, 0x6f, 0x91, 0x6f, 0x40, 0x5f, 0xc8, 0x2e, 0x90, - 0x50, 0xf7, 0x7f, 0xe6, 0x7f, 0xd5, 0x7f, 0xc4, 0x7f, 0xb3, 0x7f, 0xa1, 0x7f, 0x90, 0x7f, 0x82, 0x7f, 0x7b, 0x7f, - 0x98, 0x2e, 0xcd, 0x00, 0x00, 0xb2, 0x12, 0x30, 0x5c, 0x2f, 0x01, 0x2e, 0x21, 0x02, 0x03, 0x2e, 0x28, 0x02, 0x9f, - 0xbc, 0x9f, 0xb8, 0x21, 0x56, 0x8a, 0x08, 0x03, 0x08, 0x82, 0x0a, 0x25, 0x2e, 0x18, 0x00, 0x05, 0x2e, 0xc1, 0xf5, - 0x2e, 0xbc, 0x05, 0x2e, 0x86, 0x00, 0x84, 0xa2, 0x0e, 0xb8, 0x31, 0x30, 0x88, 0x04, 0x07, 0x2f, 0x01, 0x2e, 0x18, - 0x00, 0x00, 0x90, 0x03, 0x2f, 0x01, 0x2e, 0x7b, 0x00, 0x00, 0xb2, 0x19, 0x2f, 0x1d, 0x50, 0x01, 0x52, 0x98, 0x2e, - 0xd6, 0x00, 0x05, 0x2e, 0x7a, 0x00, 0x25, 0x2e, 0x9b, 0x00, 0x05, 0x2e, 0x7a, 0x00, 0x80, 0x90, 0x02, 0x2f, 0x12, - 0x30, 0x25, 0x2e, 0x7a, 0x00, 0x01, 0x2e, 0x7b, 0x00, 0x00, 0xb2, 0x05, 0x2e, 0x18, 0x00, 0x02, 0x2f, 0x10, 0x30, - 0x21, 0x2e, 0x18, 0x00, 0x25, 0x2e, 0x7b, 0x00, 0x05, 0x2e, 0x18, 0x00, 0x80, 0xb2, 0x20, 0x2f, 0x01, 0x2e, 0xc0, - 0xf5, 0xf2, 0x30, 0x02, 0x08, 0x07, 0xaa, 0x73, 0x30, 0x03, 0x2e, 0x7d, 0x00, 0x18, 0x22, 0x41, 0x1a, 0x05, 0x2f, - 0x03, 0x2e, 0x66, 0xf5, 0x9f, 0xbc, 0x9f, 0xb8, 0x40, 0x90, 0x0c, 0x2f, 0x1f, 0x52, 0x03, 0x30, 0x53, 0x42, 0x2b, - 0x30, 0x90, 0x04, 0x5b, 0x42, 0x21, 0x2e, 0x7d, 0x00, 0x24, 0xbd, 0x7e, 0x80, 0x81, 0x84, 0x43, 0x42, 0x02, 0x42, - 0x02, 0x32, 0x25, 0x2e, 0x62, 0xf5, 0x05, 0x2e, 0x88, 0x00, 0x81, 0x84, 0x25, 0x2e, 0x88, 0x00, 0x02, 0x31, 0x25, - 0x2e, 0x60, 0xf5, 0x05, 0x2e, 0x25, 0x02, 0x10, 0x30, 0x90, 0x08, 0x80, 0xb2, 0x0b, 0x2f, 0x05, 0x2e, 0xca, 0xf5, - 0xf0, 0x3e, 0x90, 0x08, 0x25, 0x2e, 0xca, 0xf5, 0x05, 0x2e, 0x59, 0xf5, 0xe0, 0x3f, 0x90, 0x08, 0x25, 0x2e, 0x59, - 0xf5, 0x90, 0x6f, 0xa1, 0x6f, 0xb3, 0x6f, 0xc4, 0x6f, 0xd5, 0x6f, 0xe6, 0x6f, 0xf7, 0x6f, 0x7b, 0x6f, 0x82, 0x6f, - 0x70, 0x5f, 0xc8, 0x2e, 0x31, 0x50, 0xe0, 0x50, 0x12, 0x40, 0x06, 0x40, 0x1a, 0x25, 0x77, 0x88, 0x75, 0x8a, 0x21, - 0x56, 0x6c, 0xbf, 0x00, 0x30, 0xd3, 0x08, 0x6c, 0xbb, 0x60, 0x7f, 0x00, 0x43, 0x40, 0x43, 0xc0, 0xb2, 0xd6, 0x7f, - 0xe5, 0x7f, 0xf4, 0x7f, 0xc3, 0x7f, 0xbb, 0x7f, 0x74, 0x2f, 0x01, 0x2e, 0x85, 0x00, 0x00, 0xb2, 0x0b, 0x2f, 0x27, - 0x52, 0x01, 0x2e, 0x80, 0x00, 0xa2, 0x7f, 0x98, 0x2e, 0xbb, 0xcc, 0x01, 0x30, 0x23, 0x2e, 0x85, 0x00, 0xa2, 0x6f, - 0xc3, 0x6f, 0x1a, 0x25, 0x26, 0xbc, 0x86, 0xba, 0x25, 0xbc, 0x0f, 0xb8, 0x54, 0xb1, 0x00, 0xb2, 0xa6, 0x7f, 0x0c, - 0x2f, 0x29, 0x50, 0x2b, 0x54, 0x0b, 0x30, 0x0b, 0x2e, 0x21, 0x02, 0x2f, 0x58, 0x1b, 0x42, 0x9b, 0x42, 0x6c, 0x09, - 0x0b, 0x42, 0x2b, 0x2e, 0x21, 0x02, 0x8b, 0x42, 0x72, 0x84, 0x33, 0x50, 0x58, 0x09, 0x03, 0x52, 0x01, 0x50, 0x92, - 0x7f, 0x85, 0x7f, 0x98, 0x2e, 0xc2, 0xc0, 0x01, 0x2e, 0x80, 0x00, 0xf5, 0x6f, 0xe4, 0x6f, 0x83, 0x6f, 0x92, 0x6f, - 0x27, 0x52, 0x2d, 0x5c, 0x98, 0x2e, 0x06, 0xcd, 0xc3, 0x6f, 0x29, 0x50, 0x72, 0x6f, 0xb4, 0xbc, 0x14, 0x40, 0x80, - 0xb2, 0x9f, 0xba, 0x02, 0x40, 0x01, 0x30, 0xf0, 0x7f, 0x05, 0x2f, 0x40, 0xb3, 0x03, 0x2f, 0x2b, 0x5c, 0x11, 0x30, - 0x94, 0x43, 0x82, 0x43, 0xb3, 0xbd, 0xbf, 0xb9, 0xc0, 0xb2, 0x1c, 0x2f, 0x53, 0x6f, 0x23, 0x01, 0x63, 0x6f, 0x93, - 0x02, 0x02, 0x42, 0x40, 0x91, 0x29, 0x2e, 0x81, 0x00, 0x2b, 0x50, 0x12, 0x2f, 0x2b, 0x56, 0x00, 0x2e, 0xd5, 0x40, - 0xc3, 0x40, 0x65, 0x05, 0xd3, 0x06, 0xc0, 0xaa, 0x04, 0x2f, 0xc0, 0x90, 0x08, 0x2f, 0xa3, 0x6f, 0x5d, 0x0f, 0x05, - 0x2f, 0xa5, 0x6f, 0x40, 0xb3, 0x02, 0x2f, 0x14, 0x42, 0x02, 0x42, 0x11, 0x30, 0xd0, 0x6f, 0x98, 0x2e, 0x95, 0xcf, - 0xf2, 0x6f, 0x15, 0x52, 0x01, 0x2e, 0x81, 0x00, 0x82, 0x40, 0x50, 0x42, 0x08, 0x2c, 0x42, 0x42, 0x11, 0x30, 0x23, - 0x2e, 0x85, 0x00, 0x01, 0x30, 0xd0, 0x6f, 0x98, 0x2e, 0x95, 0xcf, 0x00, 0x2e, 0xbb, 0x6f, 0x20, 0x5f, 0xb8, 0x2e, - 0x11, 0x30, 0x81, 0x08, 0x01, 0x2e, 0x6a, 0xf7, 0x71, 0x3f, 0x23, 0xbd, 0x01, 0x08, 0x02, 0x0a, 0xc0, 0x2e, 0x21, - 0x2e, 0x6a, 0xf7, 0x30, 0x25, 0x00, 0x30, 0x21, 0x2e, 0x5a, 0xf5, 0x10, 0x50, 0x21, 0x2e, 0x7c, 0x00, 0x21, 0x2e, - 0x78, 0x00, 0xfb, 0x7f, 0x98, 0x2e, 0xa4, 0xb1, 0x40, 0x30, 0x21, 0x2e, 0x86, 0x00, 0xfb, 0x6f, 0xf0, 0x5f, 0x03, - 0x25, 0x80, 0x2e, 0xea, 0x03, 0x51, 0x56, 0x05, 0x40, 0x69, 0x18, 0x0d, 0x17, 0xe1, 0x18, 0x19, 0x05, 0x16, 0x25, - 0x37, 0x25, 0x4a, 0x17, 0x54, 0x18, 0xec, 0x18, 0x04, 0x30, 0x64, 0x07, 0xea, 0x18, 0x8e, 0x00, 0x5f, 0x02, 0x35, - 0x56, 0x93, 0x00, 0x4c, 0x02, 0x2f, 0xb9, 0x91, 0xbc, 0x91, 0x0a, 0x02, 0x42, 0xb8, 0x2e, 0x0b, 0x00, 0x40, 0xb3, - 0x94, 0x02, 0x0b, 0x2f, 0x50, 0xa1, 0x03, 0x2f, 0x70, 0x8b, 0x85, 0x14, 0x07, 0x2c, 0x00, 0x30, 0x04, 0x31, 0x25, - 0x05, 0x04, 0x13, 0x95, 0x14, 0x05, 0x14, 0x94, 0x0a, 0x47, 0x5a, 0x45, 0x01, 0x04, 0x30, 0x94, 0x02, 0xd8, 0xba, - 0xc0, 0x2e, 0x28, 0xbd, 0x2a, 0x0a, 0x4a, 0x18, 0x00, 0x30, 0xc1, 0x18, 0x6f, 0xb8, 0xc0, 0x2e, 0xf1, 0xbc, 0x01, - 0x0a, 0x4e, 0x86, 0x80, 0x40, 0x70, 0x50, 0xd1, 0x40, 0x86, 0x84, 0xf0, 0x7f, 0x84, 0x30, 0xce, 0x8a, 0xe2, 0x7f, - 0x20, 0x04, 0x54, 0x41, 0xc2, 0x40, 0x4c, 0x04, 0x43, 0x41, 0x93, 0x06, 0x49, 0x18, 0x03, 0x31, 0xd8, 0x04, 0x30, - 0x88, 0xd1, 0x18, 0xd4, 0x7f, 0x00, 0xb2, 0x72, 0x8b, 0xd1, 0x18, 0xc3, 0x7f, 0xbb, 0x7f, 0x0a, 0x2f, 0x10, 0xa0, - 0x03, 0x2f, 0xd1, 0x6f, 0xb9, 0x13, 0x06, 0x2c, 0x07, 0x30, 0xc1, 0x6f, 0x79, 0x14, 0xb0, 0x12, 0xf8, 0x13, 0x91, - 0x0b, 0x51, 0x41, 0x4e, 0x85, 0x00, 0xb2, 0x94, 0x40, 0xb2, 0x86, 0x82, 0x40, 0x26, 0x01, 0xa3, 0x7f, 0x97, 0x02, - 0x43, 0x41, 0x4c, 0x00, 0x9a, 0x02, 0x0a, 0x2f, 0x10, 0xa0, 0x03, 0x2f, 0xd1, 0x6f, 0x51, 0x12, 0x06, 0x2c, 0x02, - 0x30, 0xc3, 0x6f, 0xd3, 0x14, 0x48, 0x12, 0x90, 0x12, 0x4b, 0x0a, 0x98, 0x2e, 0x79, 0xc0, 0xa1, 0x6f, 0x4f, 0x8a, - 0x72, 0x8d, 0x43, 0x41, 0x42, 0x40, 0xf5, 0x6f, 0xab, 0xbc, 0x35, 0xba, 0x25, 0xb9, 0xbb, 0xbd, 0xf0, 0x7f, 0x75, - 0x25, 0x98, 0x2e, 0xdb, 0xb1, 0xd0, 0x7f, 0x57, 0x25, 0x91, 0x41, 0x8e, 0x81, 0x82, 0x41, 0x13, 0x40, 0x04, 0x40, - 0x32, 0x8c, 0x98, 0x2e, 0xdb, 0xb1, 0xc0, 0x7f, 0x57, 0x25, 0x91, 0x41, 0x8e, 0x81, 0x82, 0x41, 0x13, 0x40, 0x04, - 0x40, 0x32, 0x8c, 0x98, 0x2e, 0xdb, 0xb1, 0xa0, 0x7f, 0x57, 0x25, 0x91, 0x41, 0x8e, 0x81, 0x82, 0x41, 0x13, 0x40, - 0x04, 0x40, 0x32, 0x8c, 0x98, 0x2e, 0xdb, 0xb1, 0x90, 0x7f, 0x57, 0x25, 0x91, 0x41, 0x8e, 0x81, 0x82, 0x41, 0x13, - 0x40, 0x04, 0x40, 0x32, 0x8c, 0x98, 0x2e, 0xdb, 0xb1, 0x57, 0x25, 0x91, 0x41, 0x8e, 0x89, 0xe7, 0x6f, 0x13, 0x41, - 0x82, 0x41, 0x04, 0x41, 0xe0, 0x7f, 0x98, 0x2e, 0xdb, 0xb1, 0x57, 0x25, 0xf1, 0x6f, 0x52, 0x41, 0xf0, 0x7f, 0x98, - 0x2e, 0xf3, 0xb1, 0xd1, 0x6f, 0x52, 0x41, 0x30, 0x25, 0x98, 0x2e, 0xf3, 0xb1, 0xc1, 0x6f, 0x52, 0x41, 0xd0, 0x7f, - 0x98, 0x2e, 0xf3, 0xb1, 0xa1, 0x6f, 0x52, 0x41, 0xc0, 0x7f, 0x98, 0x2e, 0xf3, 0xb1, 0x91, 0x6f, 0x52, 0x41, 0xa0, - 0x7f, 0x98, 0x2e, 0xf3, 0xb1, 0xe1, 0x6f, 0x52, 0x41, 0x40, 0x25, 0x98, 0x2e, 0xf3, 0xb1, 0x70, 0x25, 0x42, 0x41, - 0xf1, 0x6f, 0x57, 0x25, 0x98, 0x2e, 0xf3, 0xb1, 0x7b, 0x52, 0xd9, 0x0f, 0x90, 0x2e, 0x70, 0xb3, 0x81, 0x32, 0x69, - 0x0e, 0x06, 0x2f, 0x01, 0x32, 0x41, 0x0e, 0x0d, 0x53, 0x0f, 0x55, 0x0a, 0x22, 0x80, 0x2e, 0x71, 0xb3, 0x51, 0x34, - 0x59, 0x0e, 0xd1, 0x6f, 0xc2, 0x6f, 0x90, 0x2e, 0x4f, 0xb3, 0x99, 0x5e, 0x57, 0x0e, 0x7c, 0x2f, 0xaf, 0x5e, 0x4f, - 0x0e, 0x4d, 0x2f, 0x02, 0xa2, 0x2f, 0x2f, 0xe9, 0x50, 0x60, 0x0e, 0x06, 0x2f, 0x07, 0x55, 0x4a, 0x0e, 0x09, 0x53, - 0x0b, 0x55, 0x0a, 0x22, 0x80, 0x2e, 0x71, 0xb3, 0xeb, 0x50, 0x60, 0x0e, 0x10, 0x2f, 0xfb, 0x52, 0xd1, 0x0f, 0x0a, - 0x2f, 0x45, 0xa3, 0x01, 0x53, 0x03, 0x55, 0x4a, 0x22, 0xa2, 0x6f, 0xfd, 0x50, 0x50, 0x0e, 0xff, 0x54, 0x11, 0x22, - 0x80, 0x2e, 0x71, 0xb3, 0x05, 0x51, 0x80, 0x2e, 0x71, 0xb3, 0xed, 0x54, 0x4a, 0x0e, 0x07, 0x2f, 0xa1, 0x6f, 0xf5, - 0x54, 0x4a, 0x0e, 0xf7, 0x52, 0xf9, 0x54, 0x0a, 0x22, 0x80, 0x2e, 0x71, 0xb3, 0xef, 0x52, 0x59, 0x0e, 0xf1, 0x52, - 0xf3, 0x54, 0x0a, 0x22, 0x80, 0x2e, 0x71, 0xb3, 0xcb, 0x52, 0x61, 0x0e, 0x0e, 0x2f, 0xa1, 0x6f, 0xdd, 0x54, 0xca, - 0x0f, 0x08, 0x2f, 0x01, 0xa2, 0xe1, 0x52, 0xe3, 0x54, 0x4a, 0x22, 0xdf, 0x54, 0x62, 0x0e, 0xe5, 0x54, 0x77, 0x2c, - 0x0a, 0x22, 0x75, 0x2c, 0xe7, 0x50, 0xd7, 0x52, 0x51, 0x0e, 0xd9, 0x52, 0xdb, 0x54, 0x4a, 0x22, 0x72, 0x35, 0x5a, - 0x0e, 0xd5, 0x54, 0x6b, 0x2c, 0x11, 0x22, 0xb1, 0x56, 0x53, 0x0e, 0x0a, 0x2f, 0xa1, 0x6f, 0xcf, 0x54, 0x4a, 0x0e, - 0xd1, 0x52, 0xd3, 0x54, 0x4a, 0x22, 0xcb, 0x54, 0x62, 0x0e, 0xcd, 0x54, 0x5d, 0x2c, 0x11, 0x22, 0x0c, 0xa2, 0x05, - 0x2f, 0xc5, 0x52, 0x61, 0x0e, 0xc7, 0x52, 0xc9, 0x54, 0x55, 0x2c, 0x0a, 0x22, 0xb3, 0x54, 0x62, 0x0e, 0x0a, 0x2f, - 0xa2, 0x6f, 0xbf, 0x50, 0x50, 0x0e, 0xc1, 0x54, 0xc3, 0x50, 0x90, 0x22, 0xbb, 0x50, 0x48, 0x0e, 0xbd, 0x52, 0x47, - 0x2c, 0x0a, 0x22, 0x43, 0xa3, 0xb5, 0x52, 0xb7, 0x54, 0x4a, 0x22, 0x54, 0xa3, 0xb9, 0x54, 0x3f, 0x2c, 0x0a, 0x22, - 0x41, 0xa3, 0x14, 0x2f, 0xa0, 0x37, 0x50, 0x0e, 0x0f, 0x2f, 0xa1, 0x54, 0x4a, 0x0e, 0x0a, 0x2f, 0xa7, 0x52, 0x61, - 0x0e, 0xa9, 0x52, 0xab, 0x54, 0x4a, 0x22, 0xa2, 0x6f, 0xa5, 0x50, 0x50, 0x0e, 0xad, 0x54, 0x2c, 0x2c, 0x0a, 0x22, - 0x2a, 0x2c, 0xa3, 0x50, 0x28, 0x2c, 0x9f, 0x50, 0x01, 0xa2, 0x9b, 0x52, 0x9d, 0x54, 0x23, 0x2c, 0x0a, 0x22, 0x20, - 0x33, 0x58, 0x0e, 0x09, 0x2f, 0x91, 0x50, 0x48, 0x0e, 0x93, 0x52, 0x95, 0x50, 0x48, 0x22, 0x8f, 0x50, 0x50, 0x0e, - 0x97, 0x54, 0x16, 0x2c, 0x0a, 0x22, 0x7d, 0x54, 0x62, 0x0e, 0x0e, 0x2f, 0x81, 0x54, 0xe2, 0x0f, 0x09, 0x2f, 0x87, - 0x54, 0x4a, 0x0e, 0x89, 0x52, 0x8b, 0x54, 0x4a, 0x22, 0x83, 0x54, 0x62, 0x0e, 0x85, 0x54, 0x06, 0x2c, 0x11, 0x22, - 0x04, 0x2c, 0x8d, 0x50, 0x02, 0x2c, 0x7f, 0x50, 0x11, 0x51, 0xbb, 0x6f, 0x90, 0x5f, 0xb8, 0x2e, 0x32, 0x25, 0xb0, - 0x51, 0xc2, 0x32, 0x82, 0x00, 0xe0, 0x7f, 0xd2, 0x7f, 0x04, 0x30, 0x80, 0x40, 0x01, 0x80, 0x90, 0x42, 0xc2, 0x7f, - 0x10, 0x30, 0x85, 0x40, 0x2c, 0x03, 0x94, 0x42, 0x4a, 0x25, 0x85, 0x40, 0x28, 0x28, 0x80, 0x42, 0x25, 0x3d, 0xc3, - 0x80, 0x2b, 0x89, 0x95, 0x00, 0x81, 0x7f, 0xb2, 0x7f, 0xa4, 0x7f, 0x90, 0x7f, 0x7b, 0x7f, 0x00, 0x2e, 0xd1, 0x40, - 0x03, 0x54, 0x53, 0x7f, 0x64, 0x7f, 0x98, 0x2e, 0xd9, 0xc0, 0x64, 0x6f, 0x53, 0x6f, 0x91, 0x6f, 0x10, 0x43, 0x59, - 0x0e, 0xf3, 0x2f, 0xa1, 0x6f, 0x98, 0x2e, 0xb3, 0xc0, 0xb1, 0x6f, 0x13, 0x33, 0x40, 0x42, 0x00, 0xac, 0xcb, 0x00, - 0x02, 0x2f, 0xe1, 0x6f, 0x53, 0x54, 0x42, 0x42, 0xd1, 0x3d, 0x59, 0x00, 0xc3, 0x40, 0xcf, 0xb0, 0xce, 0x00, 0x72, - 0x80, 0xc2, 0x40, 0x11, 0x40, 0x91, 0x00, 0xd2, 0x42, 0x89, 0x16, 0xc4, 0x40, 0x22, 0x03, 0x02, 0x40, 0x05, 0x33, - 0x05, 0x00, 0xd4, 0x42, 0xb3, 0x7f, 0x90, 0x7f, 0x98, 0x2e, 0xfe, 0xc9, 0x91, 0x6f, 0xd2, 0x3d, 0xb3, 0x6f, 0x00, - 0x18, 0x8a, 0x00, 0xc0, 0x40, 0xb2, 0x88, 0x06, 0x00, 0xd0, 0x42, 0xa0, 0x35, 0xc5, 0x40, 0x6f, 0x03, 0x46, 0x40, - 0x8f, 0xb1, 0x96, 0x00, 0x20, 0x00, 0xc5, 0x42, 0x92, 0x7f, 0xb0, 0x7f, 0x02, 0x32, 0x01, 0x41, 0x33, 0x30, 0x98, - 0x2e, 0x0f, 0xca, 0xb5, 0x6f, 0x11, 0x3b, 0x63, 0x41, 0x64, 0x41, 0x44, 0x85, 0x45, 0x41, 0xa6, 0x40, 0x87, 0x40, - 0x51, 0x00, 0xf7, 0x7f, 0xb1, 0x7f, 0x20, 0x25, 0x98, 0x2e, 0x67, 0xcc, 0xb1, 0x6f, 0xb3, 0x33, 0xcb, 0x00, 0xea, - 0x82, 0xc2, 0x40, 0x1a, 0xa4, 0xe5, 0x6f, 0x04, 0x30, 0x13, 0x30, 0x01, 0x2f, 0x80, 0xa0, 0x03, 0x2f, 0x2e, 0xac, - 0x0a, 0x2f, 0x80, 0xa4, 0x08, 0x2f, 0x90, 0x6f, 0x04, 0x80, 0x77, 0x34, 0x06, 0x40, 0x6f, 0x01, 0xa2, 0x04, 0xf3, - 0x28, 0x42, 0x43, 0x03, 0x42, 0xd3, 0x3d, 0x42, 0x40, 0xcb, 0x00, 0xf2, 0x82, 0x8f, 0xb0, 0xde, 0x00, 0x43, 0x80, - 0x42, 0x40, 0xb3, 0x7f, 0x90, 0x7f, 0xb3, 0x30, 0x13, 0x53, 0x98, 0x2e, 0x5a, 0xca, 0x3a, 0x25, 0xe8, 0x82, 0xee, - 0x86, 0xe2, 0x6f, 0x82, 0x84, 0x43, 0x7f, 0x20, 0x7f, 0x51, 0x7f, 0x61, 0x7f, 0x32, 0x7f, 0x05, 0x30, 0xa4, 0x6f, - 0x83, 0x30, 0x00, 0x30, 0x11, 0x41, 0x22, 0x6f, 0x14, 0x7f, 0x00, 0x7f, 0xf5, 0x7e, 0x98, 0x2e, 0x0f, 0xca, 0x33, - 0x6f, 0x51, 0x6f, 0xc3, 0x40, 0x50, 0x42, 0x51, 0x7f, 0xe0, 0x7e, 0xc0, 0x90, 0x02, 0x2f, 0x91, 0x6f, 0x00, 0x2e, - 0x40, 0x42, 0x00, 0x2e, 0xe2, 0x6e, 0x90, 0x6f, 0x15, 0x53, 0x98, 0x2e, 0xc3, 0xb1, 0x93, 0x6f, 0xe1, 0x6e, 0xd2, - 0x40, 0x0a, 0x18, 0x01, 0x6f, 0x0e, 0x00, 0x93, 0x7f, 0x83, 0x30, 0x14, 0x6f, 0xf1, 0x6e, 0x42, 0x6f, 0x62, 0x0e, - 0x4f, 0x03, 0xd9, 0x2f, 0x35, 0x52, 0xc1, 0x00, 0x01, 0x30, 0xa9, 0x02, 0x91, 0x6f, 0x7c, 0x82, 0x21, 0xbd, 0xbf, - 0xb9, 0x1b, 0x30, 0x0a, 0x25, 0xda, 0x0a, 0x5b, 0x42, 0x25, 0x80, 0x33, 0x7f, 0x51, 0x7f, 0x20, 0x7f, 0x90, 0x7f, - 0xd3, 0x30, 0x64, 0x6f, 0x55, 0x6f, 0x10, 0x41, 0x52, 0x41, 0x31, 0x6f, 0x55, 0x7f, 0x10, 0x7f, 0x04, 0x7f, 0x98, - 0x2e, 0x0f, 0xca, 0x11, 0x6f, 0x20, 0x25, 0x98, 0x2e, 0xfe, 0xc9, 0x21, 0x6f, 0x04, 0x6f, 0x50, 0x42, 0x21, 0x7f, - 0xd3, 0x30, 0xa1, 0x6f, 0x61, 0x0e, 0xea, 0x2f, 0xb1, 0x6f, 0x45, 0x84, 0x32, 0x25, 0x90, 0x40, 0x84, 0x40, 0x91, - 0x6f, 0xb4, 0x7f, 0x92, 0x7f, 0x30, 0x7f, 0x23, 0x7f, 0x98, 0x2e, 0xb3, 0xc0, 0x53, 0x6f, 0xb1, 0x32, 0x19, 0x01, - 0x83, 0xb9, 0x31, 0x6f, 0x4b, 0x00, 0x02, 0x41, 0xb0, 0x6f, 0x03, 0x30, 0xc3, 0x02, 0x8f, 0xb0, 0xb4, 0x7f, 0xd5, - 0x3d, 0x25, 0x01, 0xa2, 0x6f, 0xa4, 0x7f, 0x26, 0x01, 0x27, 0x6f, 0x90, 0x6f, 0x07, 0x89, 0xc1, 0x43, 0x94, 0x7f, - 0x03, 0x42, 0x00, 0x2e, 0x11, 0x41, 0x31, 0x7f, 0x54, 0x7f, 0x00, 0x2e, 0x91, 0x40, 0x03, 0x41, 0x23, 0x7f, 0x12, - 0x7f, 0x98, 0x2e, 0x74, 0xc0, 0x31, 0x6f, 0xc8, 0x00, 0x90, 0x6f, 0x01, 0x30, 0x54, 0x6f, 0x22, 0x6f, 0x03, 0x42, - 0xd1, 0x02, 0x41, 0x6f, 0x12, 0x6f, 0x23, 0x43, 0x94, 0x7f, 0x51, 0x0e, 0xe7, 0x2f, 0xb1, 0x6f, 0xa3, 0x6f, 0x42, - 0x40, 0x8f, 0xb0, 0xf8, 0x82, 0xde, 0x00, 0xc9, 0x86, 0x52, 0x34, 0xb3, 0x7f, 0x8a, 0x00, 0xc6, 0x86, 0xa2, 0x7f, - 0x93, 0x7f, 0x00, 0x2e, 0xa5, 0x6f, 0xe2, 0x6f, 0x63, 0x41, 0x64, 0x41, 0x44, 0x8f, 0x82, 0x40, 0xe6, 0x41, 0xc0, - 0x41, 0xc4, 0x8f, 0x45, 0x41, 0xf0, 0x7f, 0xa7, 0x7f, 0x51, 0x7f, 0x98, 0x2e, 0x67, 0xcc, 0x00, 0x18, 0x09, 0x52, - 0x71, 0x00, 0x03, 0x30, 0xbb, 0x02, 0x1b, 0xba, 0xb3, 0x6f, 0x25, 0xbc, 0x51, 0x6f, 0xc5, 0x40, 0x42, 0x82, 0x20, - 0x0a, 0x28, 0x00, 0xd0, 0x42, 0x2b, 0xb9, 0xc0, 0x40, 0x82, 0x02, 0xd2, 0x42, 0xb3, 0x7f, 0x00, 0x2e, 0x92, 0x6f, - 0x5a, 0x0e, 0xd9, 0x2f, 0xa1, 0x6f, 0x43, 0x3d, 0x8b, 0x00, 0x9a, 0x82, 0x83, 0x6f, 0x41, 0x40, 0xc3, 0x40, 0xe0, - 0x6f, 0x14, 0x33, 0x04, 0x00, 0x82, 0x40, 0x4b, 0x12, 0x51, 0x0e, 0xb0, 0x7f, 0x90, 0x2e, 0x7a, 0xb5, 0x02, 0x40, - 0x8f, 0xb0, 0xd1, 0x3d, 0x41, 0x00, 0x72, 0x30, 0xd3, 0x04, 0x73, 0x80, 0x4e, 0x00, 0x02, 0x31, 0xd3, 0x05, 0xa0, - 0x7f, 0xf0, 0x8c, 0x04, 0x40, 0x52, 0x40, 0xc0, 0xb2, 0x50, 0x40, 0x4c, 0x17, 0x96, 0x7f, 0x57, 0x7f, 0x0a, 0x2f, - 0xd0, 0xa0, 0x03, 0x2f, 0x95, 0x6f, 0x65, 0x15, 0x06, 0x2c, 0x04, 0x30, 0x56, 0x6f, 0xa6, 0x13, 0x6b, 0x15, 0x23, - 0x15, 0x6e, 0x0b, 0x14, 0x05, 0x64, 0x18, 0x45, 0x07, 0xec, 0x18, 0xc0, 0xb2, 0xec, 0x18, 0x0a, 0x2f, 0xd0, 0xa0, - 0x03, 0x2f, 0x94, 0x6f, 0xbc, 0x13, 0x06, 0x2c, 0x07, 0x30, 0x54, 0x6f, 0x3c, 0x15, 0x73, 0x13, 0xfb, 0x13, 0xac, - 0x0b, 0x44, 0x40, 0x26, 0x05, 0x54, 0x42, 0xc0, 0xb2, 0x44, 0x40, 0x27, 0x07, 0x44, 0x42, 0x08, 0x2f, 0xd0, 0xa0, - 0x02, 0x2f, 0x91, 0x6f, 0x05, 0x2c, 0x81, 0x10, 0x51, 0x6f, 0x41, 0x14, 0xd3, 0x12, 0x99, 0x0a, 0xa1, 0x6f, 0xf3, - 0x32, 0x42, 0x42, 0x4b, 0x00, 0x13, 0x30, 0x42, 0x40, 0xd3, 0x28, 0x53, 0x42, 0xa1, 0x7f, 0xc2, 0xa2, 0x30, 0x2f, - 0x82, 0x6f, 0xe1, 0x6f, 0x98, 0x2e, 0xfa, 0xb1, 0x81, 0x6f, 0x41, 0x84, 0x00, 0x2e, 0x81, 0x40, 0x40, 0x90, 0x02, - 0x2f, 0x00, 0x2e, 0x07, 0x2c, 0x0c, 0xb8, 0x30, 0x25, 0xe1, 0x6f, 0x20, 0x33, 0x48, 0x00, 0x98, 0x2e, 0x07, 0xb6, - 0xe1, 0x6f, 0xf3, 0x32, 0x4b, 0x00, 0xe0, 0x7f, 0x00, 0x2e, 0x44, 0x40, 0x20, 0x1a, 0x15, 0x2f, 0xd3, 0x6f, 0xc1, - 0x6f, 0xc3, 0x40, 0x23, 0x5a, 0x42, 0x40, 0x83, 0x7e, 0x08, 0xbc, 0x25, 0x09, 0x92, 0x7e, 0xc4, 0x0a, 0x42, 0x82, - 0xa3, 0x7e, 0xd1, 0x7f, 0x34, 0x30, 0x63, 0x6f, 0x82, 0x30, 0x31, 0x30, 0x98, 0x2e, 0xb2, 0x00, 0xd1, 0x6f, 0xe3, - 0x6f, 0x43, 0x42, 0x00, 0x2e, 0xa1, 0x6f, 0xb2, 0x6f, 0x43, 0x40, 0xc1, 0x86, 0xc2, 0xa2, 0x43, 0x42, 0x03, 0x30, - 0x00, 0x2f, 0x83, 0x42, 0xd2, 0x3d, 0x40, 0x40, 0x0f, 0xb0, 0x0a, 0x01, 0x26, 0x00, 0x05, 0x32, 0x98, 0x2e, 0x7e, - 0xb5, 0x65, 0x00, 0x00, 0x2e, 0x43, 0x42, 0x00, 0x2e, 0x7b, 0x6f, 0x50, 0x5e, 0xb8, 0x2e, 0x0f, 0x82, 0x02, 0x30, - 0x12, 0x42, 0x41, 0x0e, 0xfc, 0x2f, 0xb8, 0x2e, 0xc1, 0x35, 0x40, 0x51, 0x01, 0x01, 0x02, 0x30, 0x1a, 0x25, 0x13, - 0x30, 0x12, 0x42, 0x44, 0x0e, 0xfc, 0x2f, 0x54, 0x3a, 0x04, 0x01, 0x1d, 0x5b, 0x05, 0x7f, 0x75, 0x34, 0x13, 0x5f, - 0x07, 0x43, 0x25, 0x01, 0x47, 0x5a, 0x05, 0x43, 0x27, 0x89, 0x67, 0x5a, 0x05, 0x43, 0x18, 0x8b, 0x19, 0x59, 0x17, - 0x51, 0xd4, 0x7e, 0x43, 0x43, 0xc0, 0x7e, 0xe0, 0x7e, 0x6c, 0x88, 0x1b, 0x5d, 0xf6, 0x7e, 0x71, 0x86, 0x42, 0x81, - 0x15, 0x41, 0x15, 0x42, 0x63, 0x0e, 0xfb, 0x2f, 0x21, 0x59, 0x1f, 0x5d, 0x25, 0x5f, 0x23, 0x5b, 0x34, 0x7f, 0x16, - 0x7f, 0x45, 0x7f, 0x57, 0x7f, 0x22, 0x7f, 0x76, 0x88, 0xd5, 0x40, 0x15, 0x42, 0x5c, 0x0e, 0xfb, 0x2f, 0x29, 0x57, - 0x27, 0x5d, 0x2d, 0x5f, 0x2b, 0x5b, 0x83, 0x7f, 0x66, 0x7f, 0x95, 0x7f, 0xa7, 0x7f, 0x72, 0x7f, 0x7b, 0x86, 0x15, - 0x41, 0x15, 0x42, 0x63, 0x0e, 0xfb, 0x2f, 0x31, 0x59, 0x33, 0x5d, 0x2f, 0x5b, 0xc5, 0x7f, 0xd4, 0x7f, 0xf6, 0x7f, - 0xb2, 0x7f, 0xe2, 0x7f, 0x40, 0x82, 0xd2, 0x40, 0x12, 0x42, 0x59, 0x0e, 0xfb, 0x2f, 0xc0, 0x5e, 0xb8, 0x2e, 0x00, - 0x30, 0xc0, 0x2e, 0x21, 0x2e, 0x8c, 0x00, 0x35, 0x51, 0xc0, 0x2e, 0x21, 0x2e, 0xad, 0x00, 0x05, 0x2e, 0x28, 0x02, - 0x2f, 0xbd, 0x2f, 0xb9, 0x20, 0x50, 0x80, 0xb2, 0x16, 0x2f, 0x20, 0x25, 0x01, 0x2e, 0x8c, 0x00, 0x00, 0x90, 0x0b, - 0x2f, 0x37, 0x51, 0xf2, 0x7f, 0xeb, 0x7f, 0x98, 0x2e, 0x84, 0xb5, 0x01, 0x2e, 0x8c, 0x00, 0x01, 0x80, 0x21, 0x2e, - 0x8c, 0x00, 0xf2, 0x6f, 0xeb, 0x6f, 0xe0, 0x5f, 0x03, 0x2e, 0xad, 0x00, 0x37, 0x51, 0x80, 0x2e, 0x74, 0xb3, 0x00, - 0x30, 0x21, 0x2e, 0x8c, 0x00, 0xe0, 0x5f, 0xb8, 0x2e, 0x02, 0x30, 0x02, 0x2c, 0x41, 0x00, 0x12, 0x42, 0x41, 0x0e, - 0xfc, 0x2f, 0xb8, 0x2e, 0x81, 0x8e, 0x39, 0x51, 0x07, 0x5c, 0x58, 0x09, 0x9e, 0x09, 0xc3, 0x41, 0x50, 0x50, 0xc3, - 0x89, 0xf3, 0x0e, 0x41, 0x80, 0xf0, 0x7f, 0xdc, 0xb9, 0xe4, 0x7f, 0x44, 0x80, 0x84, 0x8e, 0x43, 0x88, 0xd1, 0x7f, - 0x05, 0x30, 0xc2, 0x7f, 0x21, 0x2f, 0xf1, 0x6f, 0x00, 0x2e, 0x41, 0x40, 0x19, 0x1a, 0x1c, 0x2f, 0x82, 0x84, 0x00, - 0x2e, 0x82, 0x40, 0xf2, 0x0e, 0x05, 0x2f, 0x02, 0x41, 0x81, 0x84, 0x01, 0x30, 0x03, 0x30, 0x22, 0x2c, 0x02, 0x43, - 0xff, 0x84, 0x42, 0x00, 0x60, 0x25, 0x43, 0x40, 0xc1, 0x86, 0x43, 0x42, 0x11, 0x30, 0xc2, 0x41, 0x03, 0x30, 0x97, - 0x41, 0xc1, 0x86, 0xfa, 0x0f, 0x13, 0x2f, 0xc5, 0xa2, 0xf9, 0x2f, 0x03, 0x30, 0x10, 0x2c, 0x01, 0x30, 0xd2, 0x6f, - 0x00, 0x2e, 0x82, 0x40, 0x80, 0x90, 0x04, 0x2f, 0xd2, 0x6f, 0xf1, 0x6f, 0x17, 0x30, 0x87, 0x42, 0x43, 0x42, 0x00, - 0x2e, 0x3b, 0x55, 0xf2, 0x0e, 0x12, 0x30, 0x55, 0x22, 0x40, 0xb2, 0xbb, 0x7f, 0x06, 0x2f, 0x51, 0x30, 0x60, 0x25, - 0x98, 0x2e, 0x00, 0xb6, 0xbf, 0x81, 0x00, 0x2e, 0x05, 0x42, 0x00, 0x2e, 0xe2, 0x6f, 0x01, 0x41, 0x82, 0x40, 0xd0, - 0x6f, 0x13, 0x88, 0xca, 0x0f, 0x49, 0x2f, 0xf2, 0x6f, 0xc0, 0xa6, 0x87, 0x40, 0x43, 0x2f, 0xc5, 0x6f, 0x02, 0x82, - 0x43, 0x8d, 0x42, 0x40, 0x81, 0x8a, 0x86, 0x41, 0x6e, 0x0e, 0x45, 0x42, 0x47, 0x8a, 0xaa, 0x00, 0x1b, 0x30, 0x83, - 0x42, 0x09, 0x84, 0xf2, 0x7f, 0x02, 0x30, 0x03, 0x2f, 0x0b, 0x43, 0x2f, 0x89, 0x00, 0x2e, 0x02, 0x43, 0x51, 0x88, - 0x41, 0x40, 0x15, 0x41, 0x40, 0xb3, 0x4e, 0x22, 0x5f, 0x1a, 0x14, 0x80, 0x03, 0x41, 0x06, 0x2f, 0xc1, 0x84, 0xd6, - 0x0e, 0x02, 0x42, 0x22, 0x2f, 0x00, 0x2e, 0x21, 0x2c, 0x06, 0x42, 0xff, 0x80, 0x41, 0x0f, 0x91, 0xb9, 0x10, 0x22, - 0xf4, 0x6f, 0x61, 0x00, 0xc3, 0x0f, 0x12, 0x2f, 0x13, 0x30, 0x04, 0x30, 0xf6, 0x6f, 0x05, 0x30, 0x05, 0x2c, 0xe7, - 0x7f, 0x97, 0x41, 0x3c, 0x1a, 0xda, 0x23, 0x6f, 0x01, 0x71, 0x0e, 0xf9, 0x2f, 0x68, 0x0f, 0xe6, 0x6f, 0xe6, 0x23, - 0x01, 0x89, 0x28, 0x22, 0x05, 0xa7, 0xee, 0x2f, 0xf2, 0x6f, 0xb8, 0x84, 0x93, 0x82, 0x87, 0x42, 0x40, 0x42, 0x08, - 0x2c, 0x07, 0x25, 0x13, 0x30, 0x50, 0x25, 0x98, 0x2e, 0xb3, 0xb6, 0x00, 0x30, 0x43, 0x43, 0x03, 0x43, 0x00, 0x2e, - 0xbb, 0x6f, 0xb0, 0x5f, 0xb8, 0x2e, 0x15, 0x82, 0x02, 0x30, 0x12, 0x42, 0x41, 0x0e, 0xfc, 0x2f, 0xb8, 0x2e, 0x80, - 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, - 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, - 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, - 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, - 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, - 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, - 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, - 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, - 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, - 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, - 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, - 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, - 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, - 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, - 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, - 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, - 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, - 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, - 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, - 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, - 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, - 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, - 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, - 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, - 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, - 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, - 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, - 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, - 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, - 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, - 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, - 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, - 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, - 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, - 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0xfd, 0x2d, 0x01, 0x2e, 0x5d, 0xf7, 0x08, 0xbc, 0x80, 0xac, 0x0e, 0xbb, 0x02, 0x2f, - 0x00, 0x30, 0x41, 0x04, 0x82, 0x06, 0xc0, 0xa4, 0x00, 0x30, 0x11, 0x2f, 0x40, 0xa9, 0x03, 0x2f, 0x40, 0x91, 0x0d, - 0x2f, 0x00, 0xa7, 0x0b, 0x2f, 0x80, 0xb3, 0x35, 0x58, 0x02, 0x2f, 0x90, 0xa1, 0x26, 0x13, 0x20, 0x23, 0x80, 0x90, - 0x10, 0x30, 0x01, 0x2f, 0xcc, 0x0e, 0x00, 0x2f, 0x00, 0x30, 0xb8, 0x2e, 0x3d, 0x56, 0x37, 0x54, 0xd0, 0x40, 0xc4, - 0x40, 0x0b, 0x2e, 0xfd, 0xf3, 0x3d, 0x52, 0x90, 0x42, 0x94, 0x42, 0x95, 0x42, 0x05, 0x30, 0x3f, 0x50, 0x0f, 0x88, - 0x06, 0x40, 0x04, 0x41, 0x96, 0x42, 0xc5, 0x42, 0x48, 0xbe, 0x73, 0x30, 0x0d, 0x2e, 0x8a, 0x00, 0x4f, 0xba, 0x84, - 0x42, 0x03, 0x42, 0x81, 0xb3, 0x02, 0x2f, 0x2b, 0x2e, 0x6f, 0xf5, 0x06, 0x2d, 0x05, 0x2e, 0x77, 0xf7, 0x3b, 0x56, - 0x93, 0x08, 0x25, 0x2e, 0x77, 0xf7, 0x39, 0x54, 0x25, 0x2e, 0xc2, 0xf5, 0x07, 0x2e, 0xfd, 0xf3, 0x42, 0x30, 0xb4, - 0x33, 0xda, 0x0a, 0x4c, 0x00, 0x27, 0x2e, 0xfd, 0xf3, 0x43, 0x40, 0xd4, 0x3f, 0xdc, 0x08, 0x43, 0x42, 0x00, 0x2e, - 0x00, 0x2e, 0x43, 0x40, 0x24, 0x30, 0xdc, 0x0a, 0x43, 0x42, 0x04, 0x80, 0x03, 0x2e, 0xfd, 0xf3, 0x4a, 0x0a, 0x23, - 0x2e, 0xfd, 0xf3, 0x61, 0x34, 0xc0, 0x2e, 0x01, 0x42, 0x00, 0x2e, 0xd0, 0x51, 0xfb, 0x7f, 0x98, 0x2e, 0xcf, 0x0d, - 0x5a, 0x25, 0x98, 0x2e, 0xf6, 0x0d, 0x6b, 0x87, 0x49, 0x54, 0xe1, 0x7f, 0xa3, 0x7f, 0xb3, 0x7f, 0xb2, 0x88, 0x41, - 0x52, 0xc2, 0x7f, 0x65, 0x8b, 0x43, 0x56, 0x84, 0x7f, 0x61, 0x7f, 0x75, 0x7f, 0xd0, 0x7f, 0x95, 0x7f, 0x53, 0x7f, - 0x14, 0x30, 0x45, 0x54, 0x81, 0x6f, 0x42, 0x7f, 0x00, 0x2e, 0x53, 0x40, 0x45, 0x8c, 0x42, 0x40, 0x90, 0x41, 0xbb, - 0x83, 0x86, 0x41, 0xd8, 0x04, 0x16, 0x06, 0x00, 0xac, 0x81, 0x7f, 0x02, 0x2f, 0x02, 0x30, 0xd3, 0x04, 0x10, 0x06, - 0xc1, 0x84, 0x01, 0x30, 0xc1, 0x02, 0x0b, 0x16, 0x04, 0x09, 0x14, 0x01, 0x99, 0x02, 0xc1, 0xb9, 0xaf, 0xbc, 0x59, - 0x0a, 0x64, 0x6f, 0x51, 0x43, 0xa1, 0xb4, 0x12, 0x41, 0x13, 0x41, 0x41, 0x43, 0x35, 0x7f, 0x64, 0x7f, 0x26, 0x31, - 0xe5, 0x6f, 0xd4, 0x6f, 0x98, 0x2e, 0x37, 0xca, 0x32, 0x6f, 0x75, 0x6f, 0x83, 0x40, 0x42, 0x41, 0x23, 0x7f, 0x12, - 0x7f, 0xf6, 0x30, 0x40, 0x25, 0x51, 0x25, 0x98, 0x2e, 0x37, 0xca, 0x14, 0x6f, 0x20, 0x05, 0x70, 0x6f, 0x25, 0x6f, - 0x69, 0x07, 0xa2, 0x6f, 0x31, 0x6f, 0x0b, 0x30, 0x04, 0x42, 0x9b, 0x42, 0x8b, 0x42, 0x55, 0x42, 0x32, 0x7f, 0x40, - 0xa9, 0xc3, 0x6f, 0x71, 0x7f, 0x02, 0x30, 0xd0, 0x40, 0xc3, 0x7f, 0x03, 0x2f, 0x40, 0x91, 0x15, 0x2f, 0x00, 0xa7, - 0x13, 0x2f, 0x00, 0xa4, 0x11, 0x2f, 0x84, 0xbd, 0x98, 0x2e, 0x79, 0xca, 0x55, 0x6f, 0x51, 0x54, 0x54, 0x41, 0x82, - 0x00, 0xf3, 0x3f, 0x45, 0x41, 0xcb, 0x02, 0xf6, 0x30, 0x98, 0x2e, 0x37, 0xca, 0x33, 0x6f, 0xa4, 0x6f, 0xc1, 0x42, - 0x03, 0x2c, 0x00, 0x43, 0xa4, 0x6f, 0x33, 0x6f, 0x00, 0x2e, 0x42, 0x6f, 0x55, 0x6f, 0x91, 0x40, 0x42, 0x8b, 0x00, - 0x41, 0x41, 0x00, 0x01, 0x43, 0x55, 0x7f, 0x14, 0x30, 0xc1, 0x40, 0x95, 0x40, 0x4d, 0x02, 0xc5, 0x6f, 0x4f, 0x50, - 0x68, 0x0e, 0x75, 0x6f, 0xd1, 0x42, 0xa3, 0x7f, 0x8a, 0x2f, 0x09, 0x2e, 0x8a, 0x00, 0x01, 0xb3, 0x22, 0x2f, 0x49, - 0x58, 0x90, 0x6f, 0x17, 0x30, 0x13, 0x41, 0xb6, 0x6f, 0xe4, 0x7f, 0x00, 0x2e, 0x91, 0x41, 0x14, 0x40, 0x92, 0x41, - 0x15, 0x40, 0x17, 0x2e, 0x6f, 0xf5, 0xb6, 0x7f, 0xd0, 0x7f, 0xcb, 0x7f, 0x98, 0x2e, 0x00, 0x0c, 0x07, 0x15, 0xc2, - 0x6f, 0x14, 0x0b, 0x29, 0x2e, 0x6f, 0xf5, 0xc3, 0xa3, 0xc1, 0x8f, 0xe4, 0x6f, 0xd0, 0x6f, 0xe6, 0x2f, 0x14, 0x30, - 0x05, 0x2e, 0x6f, 0xf5, 0x14, 0x0b, 0x29, 0x2e, 0x6f, 0xf5, 0x44, 0x2d, 0x4b, 0x54, 0x01, 0x32, 0x51, 0x58, 0x05, - 0x30, 0x23, 0x50, 0x83, 0x40, 0xd8, 0x08, 0x91, 0x01, 0xb8, 0xbd, 0x38, 0xb5, 0xe6, 0x7f, 0x0a, 0x16, 0xb1, 0x6f, - 0x2a, 0xbb, 0xa6, 0xbd, 0x1c, 0x01, 0x06, 0xbc, 0x52, 0x40, 0x06, 0x0a, 0x53, 0x40, 0x45, 0x03, 0xb1, 0x7f, 0xf6, - 0x30, 0x98, 0x2e, 0x37, 0xca, 0x1a, 0xbd, 0x16, 0xb6, 0x86, 0xba, 0x00, 0xa9, 0xaa, 0x0a, 0x53, 0x52, 0x0f, 0x2f, - 0x00, 0x91, 0x53, 0x52, 0x03, 0x2f, 0x53, 0x5a, 0x55, 0x0f, 0x53, 0x52, 0x08, 0x2f, 0x3f, 0xa1, 0x04, 0x2f, 0x3f, - 0x91, 0x03, 0x2f, 0x51, 0x58, 0xd4, 0x0f, 0x00, 0x2f, 0x51, 0x54, 0x12, 0x25, 0xf2, 0x33, 0x98, 0x2e, 0xd9, 0xc0, - 0xe4, 0x6f, 0xf5, 0x37, 0x45, 0x09, 0x21, 0x85, 0x05, 0x43, 0x05, 0x30, 0x4d, 0x52, 0x51, 0x0e, 0x01, 0x32, 0x51, - 0x58, 0xc5, 0x2f, 0x47, 0x54, 0x09, 0x2e, 0x77, 0xf7, 0x22, 0x0b, 0x29, 0x2e, 0x77, 0xf7, 0xfb, 0x6f, 0x30, 0x5e, - 0xb8, 0x2e, 0x10, 0x50, 0x01, 0x2e, 0x86, 0x00, 0x00, 0xb2, 0xfb, 0x7f, 0x5d, 0x2f, 0x01, 0xb2, 0x54, 0x2f, 0x02, - 0xb2, 0x4e, 0x2f, 0x03, 0x90, 0x63, 0x2f, 0x59, 0x50, 0x39, 0x82, 0x02, 0x40, 0x81, 0x88, 0x5b, 0x54, 0x41, 0x40, - 0x61, 0x56, 0x04, 0x42, 0x00, 0x2e, 0x94, 0x40, 0x95, 0x40, 0xd8, 0xbe, 0x2c, 0x0b, 0x45, 0x40, 0x6c, 0x01, 0x55, - 0x42, 0x0c, 0x17, 0x45, 0x40, 0x2c, 0x03, 0x54, 0x42, 0x53, 0x0e, 0xf2, 0x2f, 0x63, 0x56, 0x3e, 0x82, 0xe2, 0x40, - 0xc3, 0x40, 0x28, 0xbd, 0x93, 0x0a, 0x43, 0x40, 0xda, 0x00, 0x53, 0x42, 0x8a, 0x16, 0x43, 0x40, 0x9a, 0x02, 0x52, - 0x42, 0x00, 0x2e, 0x41, 0x40, 0x47, 0x54, 0x4a, 0x0e, 0x3b, 0x2f, 0x3a, 0x82, 0x00, 0x30, 0x41, 0x40, 0x21, 0x2e, - 0x52, 0x0f, 0x40, 0xb2, 0x0a, 0x2f, 0x98, 0x2e, 0x61, 0x0c, 0x98, 0x2e, 0x2b, 0x0e, 0x98, 0x2e, 0x41, 0x0e, 0xfb, - 0x6f, 0xf0, 0x5f, 0x00, 0x30, 0x80, 0x2e, 0xaf, 0xb1, 0x5f, 0x54, 0x55, 0x56, 0x83, 0x42, 0x8f, 0x86, 0x74, 0x30, - 0x5d, 0x54, 0xc4, 0x42, 0x11, 0x30, 0x23, 0x2e, 0x86, 0x00, 0xa1, 0x42, 0x23, 0x30, 0x27, 0x2e, 0x89, 0x00, 0x21, - 0x2e, 0x88, 0x00, 0xba, 0x82, 0x18, 0x2c, 0x81, 0x42, 0x30, 0x30, 0x21, 0x2e, 0x86, 0x00, 0x13, 0x2d, 0x21, 0x30, - 0x00, 0x30, 0x23, 0x2e, 0x86, 0x00, 0x21, 0x2e, 0x7b, 0xf7, 0x0c, 0x2d, 0x77, 0x30, 0x98, 0x2e, 0x1f, 0x0c, 0x57, - 0x50, 0x0c, 0x82, 0x12, 0x30, 0x40, 0x42, 0x25, 0x2e, 0x86, 0x00, 0x2f, 0x2e, 0x7b, 0xf7, 0xfb, 0x6f, 0xf0, 0x5f, - 0xb8, 0x2e, 0x70, 0x50, 0x0a, 0x25, 0x39, 0x86, 0xfb, 0x7f, 0xe1, 0x32, 0x62, 0x30, 0x98, 0x2e, 0xc2, 0xc4, 0x23, - 0x56, 0xa5, 0x6f, 0xab, 0x08, 0x91, 0x6f, 0x4b, 0x08, 0x65, 0x56, 0xc4, 0x6f, 0x23, 0x09, 0x4d, 0xba, 0x93, 0xbc, - 0x8c, 0x0b, 0xd1, 0x6f, 0x0b, 0x09, 0x49, 0x52, 0x67, 0x5e, 0x56, 0x42, 0xaf, 0x09, 0x4d, 0xba, 0x23, 0xbd, 0x94, - 0x0a, 0xe5, 0x6f, 0x68, 0xbb, 0xeb, 0x08, 0xbd, 0xb9, 0x63, 0xbe, 0xfb, 0x6f, 0x52, 0x42, 0xe3, 0x0a, 0xc0, 0x2e, - 0x43, 0x42, 0x90, 0x5f, 0x4f, 0x50, 0x03, 0x2e, 0x25, 0xf3, 0x12, 0x40, 0x00, 0x40, 0x28, 0xba, 0x9b, 0xbc, 0x88, - 0xbd, 0x93, 0xb4, 0xe3, 0x0a, 0x89, 0x16, 0x08, 0xb6, 0xc0, 0x2e, 0x19, 0x00, 0x62, 0x02, 0x10, 0x50, 0xfb, 0x7f, - 0x98, 0x2e, 0x5d, 0x0d, 0x01, 0x2e, 0x86, 0x00, 0x31, 0x30, 0x08, 0x04, 0xfb, 0x6f, 0x01, 0x30, 0xf0, 0x5f, 0x23, - 0x2e, 0x88, 0x00, 0x21, 0x2e, 0x89, 0x00, 0xb8, 0x2e, 0x01, 0x2e, 0x89, 0x00, 0x03, 0x2e, 0x88, 0x00, 0x48, 0x0e, - 0x01, 0x2f, 0x80, 0x2e, 0x05, 0x0e, 0xb8, 0x2e, 0x69, 0x50, 0x21, 0x34, 0x01, 0x42, 0x82, 0x30, 0xc1, 0x32, 0x25, - 0x2e, 0x62, 0xf5, 0x01, 0x00, 0x22, 0x30, 0x01, 0x40, 0x4a, 0x0a, 0x01, 0x42, 0xb8, 0x2e, 0x69, 0x54, 0xf0, 0x3b, - 0x83, 0x40, 0xd8, 0x08, 0x6b, 0x52, 0x83, 0x42, 0x00, 0x30, 0x83, 0x30, 0x50, 0x42, 0xc4, 0x32, 0x27, 0x2e, 0x64, - 0xf5, 0x94, 0x00, 0x50, 0x42, 0x40, 0x42, 0xd3, 0x3f, 0x84, 0x40, 0x7d, 0x82, 0xe3, 0x08, 0x40, 0x42, 0x83, 0x42, - 0xb8, 0x2e, 0x5f, 0x52, 0x00, 0x30, 0x40, 0x42, 0x7c, 0x86, 0x37, 0x52, 0x09, 0x2e, 0x3d, 0x0f, 0x3d, 0x54, 0xc4, - 0x42, 0xd3, 0x86, 0x54, 0x40, 0x55, 0x40, 0x94, 0x42, 0x85, 0x42, 0x21, 0x2e, 0x89, 0x00, 0x42, 0x40, 0x25, 0x2e, - 0xfd, 0xf3, 0xc0, 0x42, 0x7e, 0x82, 0x05, 0x2e, 0x79, 0x00, 0x80, 0xb2, 0x14, 0x2f, 0x05, 0x2e, 0x24, 0x02, 0x27, - 0xbd, 0x2f, 0xb9, 0x80, 0x90, 0x02, 0x2f, 0x21, 0x2e, 0x6f, 0xf5, 0x0c, 0x2d, 0x07, 0x2e, 0x3e, 0x0f, 0x14, 0x30, - 0x1c, 0x09, 0x05, 0x2e, 0x77, 0xf7, 0x3b, 0x56, 0x47, 0xbe, 0x93, 0x08, 0x94, 0x0a, 0x25, 0x2e, 0x77, 0xf7, 0x6d, - 0x54, 0x50, 0x42, 0x4a, 0x0e, 0xfc, 0x2f, 0xb8, 0x2e, 0x50, 0x50, 0x02, 0x30, 0x43, 0x86, 0x6b, 0x50, 0xfb, 0x7f, - 0xe3, 0x7f, 0xd2, 0x7f, 0xc0, 0x7f, 0xb1, 0x7f, 0x00, 0x2e, 0x41, 0x40, 0x00, 0x40, 0x48, 0x04, 0x98, 0x2e, 0x74, - 0xc0, 0x1e, 0xaa, 0xd3, 0x6f, 0x14, 0x30, 0xb1, 0x6f, 0xe3, 0x22, 0xc0, 0x6f, 0x52, 0x40, 0xe4, 0x6f, 0x4c, 0x0e, - 0x12, 0x42, 0xd3, 0x7f, 0xeb, 0x2f, 0x03, 0x2e, 0x53, 0x0f, 0x40, 0x90, 0x11, 0x30, 0x03, 0x2f, 0x23, 0x2e, 0x53, - 0x0f, 0x02, 0x2c, 0x00, 0x30, 0xd0, 0x6f, 0xfb, 0x6f, 0xb0, 0x5f, 0xb8, 0x2e, 0x40, 0x50, 0xf1, 0x7f, 0x0a, 0x25, - 0x3c, 0x86, 0xeb, 0x7f, 0x41, 0x33, 0x22, 0x30, 0x98, 0x2e, 0xc2, 0xc4, 0xd3, 0x6f, 0xf4, 0x30, 0xdc, 0x09, 0x71, - 0x58, 0xc2, 0x6f, 0x94, 0x09, 0x73, 0x58, 0x6a, 0xbb, 0xdc, 0x08, 0xb4, 0xb9, 0xb1, 0xbd, 0x6f, 0x5a, 0x95, 0x08, - 0x21, 0xbd, 0xf6, 0xbf, 0x77, 0x0b, 0x51, 0xbe, 0xf1, 0x6f, 0xeb, 0x6f, 0x52, 0x42, 0x54, 0x42, 0xc0, 0x2e, 0x43, - 0x42, 0xc0, 0x5f, 0x50, 0x50, 0x77, 0x52, 0x93, 0x30, 0x53, 0x42, 0xfb, 0x7f, 0x7b, 0x30, 0x4b, 0x42, 0x13, 0x30, - 0x42, 0x82, 0x20, 0x33, 0x43, 0x42, 0xc8, 0x00, 0x01, 0x2e, 0x80, 0x03, 0x05, 0x2e, 0x7e, 0x00, 0x19, 0x52, 0xe2, - 0x7f, 0xd0, 0x7f, 0xc3, 0x7f, 0x98, 0x2e, 0x9c, 0x0e, 0xd1, 0x6f, 0x48, 0x0a, 0xd1, 0x7f, 0x3a, 0x25, 0xfb, 0x86, - 0x01, 0x33, 0x12, 0x30, 0x98, 0x2e, 0xc2, 0xc4, 0xd1, 0x6f, 0x48, 0x0a, 0x40, 0xb2, 0x0d, 0x2f, 0xe0, 0x6f, 0x03, - 0x2e, 0x80, 0x03, 0x53, 0x30, 0x07, 0x80, 0x27, 0x2e, 0x21, 0xf2, 0x98, 0xbc, 0x01, 0x42, 0x98, 0x2e, 0xe0, 0x03, - 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0xb1, 0x6f, 0x9b, 0xb8, 0x07, 0x2e, 0x1b, 0x00, 0x19, 0x1a, 0xb1, 0x7f, 0x71, - 0x30, 0x04, 0x2f, 0x23, 0x2e, 0x21, 0xf2, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0x98, 0x2e, 0x6d, 0xc0, 0x98, 0x2e, - 0x5d, 0xc0, 0x98, 0x2e, 0xd9, 0xb5, 0x20, 0x26, 0xc1, 0x6f, 0x02, 0x31, 0x52, 0x42, 0xab, 0x30, 0x4b, 0x42, 0x20, - 0x33, 0x79, 0x56, 0xf1, 0x37, 0xc4, 0x40, 0xa2, 0x0a, 0xc2, 0x42, 0xd8, 0x00, 0x01, 0x2e, 0x5e, 0xf7, 0x41, 0x08, - 0x23, 0x2e, 0x93, 0x00, 0xe3, 0x7f, 0x98, 0x2e, 0xe1, 0x00, 0xe1, 0x6f, 0x83, 0x30, 0x43, 0x42, 0x03, 0x30, 0xfb, - 0x6f, 0x75, 0x50, 0x02, 0x30, 0x00, 0x2e, 0x00, 0x2e, 0x81, 0x84, 0x50, 0x0e, 0xfa, 0x2f, 0x43, 0x42, 0x11, 0x30, - 0xb0, 0x5f, 0x23, 0x2e, 0x21, 0xf2, 0xb8, 0x2e, 0xc1, 0x4a, 0x00, 0x00, 0x6d, 0x57, 0x00, 0x00, 0x77, 0x8e, 0x00, - 0x00, 0xe0, 0xff, 0xff, 0xff, 0xd3, 0xff, 0xff, 0xff, 0xe5, 0xff, 0xff, 0xff, 0xee, 0xe1, 0xff, 0xff, 0x7c, 0x13, - 0x00, 0x00, 0x46, 0xe6, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, - 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, - 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, - 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, - 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, - 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, - 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, - 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, - 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, - 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, - 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, - 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, - 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, - 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, - 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, - 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, - 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, - 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, - 0xc1, 0xfd, 0x2d -}; - -/*! @name Global array that stores the feature input configuration of - * BMI270_HUAWEI_CONTEXT - */ -const struct bmi2_feature_config bmi270_hc_feat_in[BMI270_HC_MAX_FEAT_IN] = { - { .type = BMI2_CONFIG_ID, .page = BMI2_PAGE_4, .start_addr = BMI270_HC_CONFIG_ID_STRT_ADDR }, - { .type = BMI2_STEP_COUNTER_PARAMS, .page = BMI2_PAGE_1, .start_addr = BMI270_HC_STEP_CNT_1_STRT_ADDR }, - { .type = BMI2_STEP_DETECTOR, .page = BMI2_PAGE_4, .start_addr = BMI270_HC_STEP_CNT_4_STRT_ADDR }, - { .type = BMI2_STEP_COUNTER, .page = BMI2_PAGE_4, .start_addr = BMI270_HC_STEP_CNT_4_STRT_ADDR }, - { .type = BMI2_NVM_PROG_PREP, .page = BMI2_PAGE_4, .start_addr = BMI270_HC_NVM_PROG_PREP_STRT_ADDR }, - { .type = BMI2_MAX_BURST_LEN, .page = BMI2_PAGE_4, .start_addr = BMI270_HC_MAX_BURST_LEN_STRT_ADDR }, - { .type = BMI2_CRT_GYRO_SELF_TEST, .page = BMI2_PAGE_4, .start_addr = BMI270_HC_CRT_GYRO_SELF_TEST_STRT_ADDR }, - { .type = BMI2_ABORT_CRT_GYRO_SELF_TEST, .page = BMI2_PAGE_4, .start_addr = BMI270_HC_ABORT_STRT_ADDR }, - { .type = BMI2_ACTIVITY_RECOGNITION_SETTINGS, .page = BMI2_PAGE_5, .start_addr = BMI270_HC_ACT_RGN_SETT_STRT_ADDR }, - { .type = BMI2_ACTIVITY_RECOGNITION, .page = BMI2_PAGE_5, .start_addr = BMI270_HC_ACT_RGN_STRT_ADDR }, -}; - -/*! @name Global array that stores the feature output configuration */ -const struct bmi2_feature_config bmi270_hc_feat_out[BMI270_HC_MAX_FEAT_OUT] = { - { .type = BMI2_STEP_COUNTER, .page = BMI2_PAGE_0, .start_addr = BMI270_HC_STEP_CNT_OUT_STRT_ADDR }, - { .type = BMI2_GYRO_CROSS_SENSE, .page = BMI2_PAGE_0, .start_addr = BMI270_HC_GYRO_CROSS_SENSE_STRT_ADDR }, - { .type = BMI2_GYRO_GAIN_UPDATE, .page = BMI2_PAGE_0, .start_addr = BMI270_HC_GYR_USER_GAIN_OUT_STRT_ADDR }, - { .type = BMI2_NVM_STATUS, .page = BMI2_PAGE_0, .start_addr = BMI270_HC_NVM_VFRM_OUT_STRT_ADDR }, - { .type = BMI2_VFRM_STATUS, .page = BMI2_PAGE_0, .start_addr = BMI270_HC_NVM_VFRM_OUT_STRT_ADDR } -}; - -/*! @name Global array that stores the feature interrupts of BMI270_HUAWEI_CONTEXT */ -struct bmi2_map_int bmi270_hc_map_int[BMI270_HC_MAX_INT_MAP] = { - { .type = BMI2_STEP_COUNTER, .sens_map_int = BMI270_HC_INT_STEP_COUNTER_MASK }, - { .type = BMI2_STEP_DETECTOR, .sens_map_int = BMI270_HC_INT_STEP_DETECTOR_MASK }, -}; - -/******************************************************************************/ - -/*! Local Function Prototypes - ******************************************************************************/ - -/*! - * @brief This internal API is used to validate the device pointer for - * null conditions. - * - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t null_ptr_check(const struct bmi2_dev *dev); - -/*! - * @brief This internal API enables the selected sensor/features. - * - * @param[in] sensor_sel : Selects the desired sensor. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t sensor_enable(uint64_t sensor_sel, struct bmi2_dev *dev); - -/*! - * @brief This internal API disables the selected sensor/features. - * - * @param[in] sensor_sel : Selects the desired sensor. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t sensor_disable(uint64_t sensor_sel, struct bmi2_dev *dev); - -/*! - * @brief This internal API selects the sensors/features to be enabled or - * disabled. - * - * @param[in] sens_list : Pointer to select the sensor. - * @param[in] n_sens : Number of sensors selected. - * @param[out] sensor_sel : Gets the selected sensor. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t select_sensor(const uint8_t *sens_list, uint8_t n_sens, uint64_t *sensor_sel); - -/*! - * @brief This internal API is used to enable/disable step detector feature. - * - * @param[in] dev : Structure instance of bmi2_dev. - * @param[in] enable : Enables/Disables step-detector. - * - * Enable | Description - * -------------|--------------- - * BMI2_DISABLE | Disables step detector - * BMI2_ENABLE | Enables step detector - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_step_detector(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to enable/disable step counter feature. - * - * @param[in] dev : Structure instance of bmi2_dev. - * @param[in] enable : Enables/Disables step counter. - * - * Enable | Description - * -------------|--------------- - * BMI2_DISABLE | Disables step counter - * BMI2_ENABLE | Enables step counter - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_step_counter(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API enables/disables the activity recognition feature. - * - * @param[in] enable : Enables/Disables activity recognition. - * @param[in] dev : Structure instance of bmi2_dev. - * - * enable | Description - * -------------|--------------- - * BMI2_ENABLE | Enables activity recognition. - * BMI2_DISABLE | Disables activity recognition. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_act_recog(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to enable/disable gyroscope user gain - * feature. - * - * @param[in] dev : Structure instance of bmi2_dev. - * @param[in] enable : Enables/Disables gyroscope user gain. - * - * Enable | Description - * -------------|--------------- - * BMI2_DISABLE | Disables gyroscope user gain - * BMI2_ENABLE | Enables gyroscope user gain - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_gyro_user_gain(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API sets step counter parameter configurations. - * - * @param[in] step_count_params : Array that stores parameters 1 to 25. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_step_count_params_config(const uint16_t *step_count_params, struct bmi2_dev *dev); - -/*! - * @brief This internal API sets step counter/detector/activity configurations. - * - * @param[in] config : Structure instance of bmi2_step_config. - * @param[in] dev : Structure instance of bmi2_dev. - * - *--------------------------------------------------------------------------- - * bmi2_step_config | - * Structure parameters | Description - *--------------------------|-------------------------------------------------- - * | The Step-counter will trigger output every time - * | the number of steps are counted. Holds implicitly - * water-mark level | a 20x factor, so the range is 0 to 10230, - * | with resolution of 20 steps. - * -------------------------|--------------------------------------------------- - * reset counter | Flag to reset the counted steps. - * -------------------------|--------------------------------------------------- - * @endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_step_config(const struct bmi2_step_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets step counter parameter configurations. - * - * @param[in] step_count_params : Array that stores parameters 1 to 25. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t get_step_count_params_config(uint16_t *step_count_params, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets step counter/detector/activity configurations. - * - * @param[out] config : Structure instance of bmi2_step_config. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @verbatim - *---------------------------------------------------------------------------- - * bmi2_step_config | - * Structure parameters | Description - *--------------------------|-------------------------------------------------- - * | The Step-counter will trigger output every time - * | the number of steps are counted. Holds implicitly - * water-mark level | a 20x factor, so the range is 0 to 10230, - * | with resolution of 20 steps. - * -------------------------|--------------------------------------------------- - * reset counter | Flag to reset the counted steps. - * -------------------------|--------------------------------------------------- - * @endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t get_step_config(struct bmi2_step_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to parse and store the activity recognition - * output from the FIFO data. - * - * @param[out] act_recog : Structure to retrieve output of activity - * recognition frame. - * @param[in] data_index : Index of the FIFO data which contains - * activity recognition frame. - * @param[out] fifo : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t unpack_act_recog_output(struct bmi2_act_recog_output *act_recog, - uint16_t *data_index, - const struct bmi2_fifo_frame *fifo); - -/*! - * @brief This internal API gets the output values of step counter. - * - * @param[out] step_count : Pointer to the stored step counter data. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t get_step_counter_output(uint32_t *step_count, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets the error status related to NVM. - * - * @param[out] nvm_err_stat : Stores the NVM error status. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t get_nvm_error_status(struct bmi2_nvm_err_status *nvm_err_stat, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets the error status related to virtual frames. - * - * @param[out] vfrm_err_stat : Stores the VFRM related error status. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t get_vfrm_error_status(struct bmi2_vfrm_err_status *vfrm_err_stat, struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to get enable status of gyroscope user gain - * update. - * - * @param[out] status : Stores status of gyroscope user gain update. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t get_user_gain_upd_status(uint8_t *status, struct bmi2_dev *dev); - -/*! - * @brief This internal API skips S4S frame in the FIFO data while getting - * activity recognition output. - * - * @param[in, out] frame_header : FIFO frame header. - * @param[in, out] data_index : Index value of the FIFO data bytes - * from which S4S frame header is to be - * skipped. - * @param[in] fifo : Structure instance of bmi2_fifo_frame. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t move_if_s4s_frame(const uint8_t *frame_header, uint16_t *data_index, const struct bmi2_fifo_frame *fifo); - -/*! - * @brief This internal API enables/disables compensation of the gain defined - * in the GAIN register. - * - * @param[in] enable : Enables/Disables gain compensation - * @param[in] dev : Structure instance of bmi2_dev. - * - * enable | Description - * -------------|--------------- - * BMI2_ENABLE | Enable gain compensation. - * BMI2_DISABLE | Disable gain compensation. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t enable_gyro_gain(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to extract the output feature configuration - * details like page and start address from the look-up table. - * - * @param[out] feat_output : Structure that stores output feature - * configurations. - * @param[in] type : Type of feature or sensor. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Returns the feature found flag. - * - * @retval BMI2_FALSE : Feature not found - * BMI2_TRUE : Feature found - */ -static uint8_t extract_output_feat_config(struct bmi2_feature_config *feat_output, - uint8_t type, - const struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to move the data index ahead of the - * current frame length parameter when unnecessary FIFO data appears while - * extracting the user specified data. - * - * @param[in,out] data_index : Index of the FIFO data which is to be - * moved ahead of the current frame length - * @param[in] current_frame_length : Number of bytes in the current frame. - * @param[in] fifo : Structure instance of bmi2_fifo_frame. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t move_next_frame(uint16_t *data_index, uint8_t current_frame_length, const struct bmi2_fifo_frame *fifo); - -/***************************************************************************/ - -/*! User Interface Definitions - ****************************************************************************/ - -/*! - * @brief This API: - * 1) Updates the device structure with address of the configuration file. - * 2) Initializes BMI270_HUAWEI_CONTEXT sensor. - * 3) Writes the configuration file. - * 4) Updates the feature offset parameters in the device structure. - * 5) Updates the maximum number of pages, in the device structure. - */ -int8_t bmi270_hc_init(struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if (rslt == BMI2_OK) - { - /* Assign chip id of BMI270_HUAWEI_CONTEXT */ - dev->chip_id = BMI270_HC_CHIP_ID; - - /* Get the size of config array */ - dev->config_size = sizeof(bmi270_hc_config_file); - - /* Enable the variant specific features if any */ - dev->variant_feature = BMI2_CRT_RTOSK_ENABLE | BMI2_GYRO_CROSS_SENS_ENABLE; - - /* An extra dummy byte is read during SPI read */ - if (dev->intf == BMI2_SPI_INTF) - { - dev->dummy_byte = 1; - } - else - { - dev->dummy_byte = 0; - } - - /* If configuration file pointer is not assigned any address */ - if (!dev->config_file_ptr) - { - /* Give the address of the configuration file array to - * the device pointer - */ - dev->config_file_ptr = bmi270_hc_config_file; - } - - /* Initialize BMI2 sensor */ - rslt = bmi2_sec_init(dev); - if (rslt == BMI2_OK) - { - /* Assign the offsets of the feature input - * configuration to the device structure - */ - dev->feat_config = bmi270_hc_feat_in; - - /* Assign the offsets of the feature output to - * the device structure - */ - dev->feat_output = bmi270_hc_feat_out; - - /* Assign the maximum number of pages to the - * device structure - */ - dev->page_max = BMI270_HC_MAX_PAGE_NUM; - - /* Assign maximum number of input sensors/ - * features to device structure - */ - dev->input_sens = BMI270_HC_MAX_FEAT_IN; - - /* Assign maximum number of output sensors/ - * features to device structure - */ - dev->out_sens = BMI270_HC_MAX_FEAT_OUT; - - /* Assign the offsets of the feature interrupt - * to the device structure - */ - dev->map_int = bmi270_hc_map_int; - - /* Assign maximum number of feature interrupts - * to device structure - */ - dev->sens_int_map = BMI270_HC_MAX_INT_MAP; - } - } - - return rslt; -} - -/*! - * @brief This API selects the sensors/features to be enabled. - */ -int8_t bmi270_hc_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to select sensor */ - uint64_t sensor_sel = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (sens_list != NULL)) - { - /* Get the selected sensors */ - rslt = select_sensor(sens_list, n_sens, &sensor_sel); - if (rslt == BMI2_OK) - { - /* Enable the selected sensors */ - rslt = sensor_enable(sensor_sel, dev); - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API selects the sensors/features to be disabled. - */ -int8_t bmi270_hc_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to select sensor */ - uint64_t sensor_sel = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (sens_list != NULL)) - { - /* Get the selected sensors */ - rslt = select_sensor(sens_list, n_sens, &sensor_sel); - if (rslt == BMI2_OK) - { - /* Disable the selected sensors */ - rslt = sensor_disable(sensor_sel, dev); - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API sets the sensor/feature configuration. - */ -int8_t bmi270_hc_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define loop */ - uint8_t loop; - - /* Variable to get the status of advance power save */ - uint8_t aps_stat = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (sens_cfg != NULL)) - { - /* Get status of advance power save mode */ - aps_stat = dev->aps_status; - - for (loop = 0; loop < n_sens; loop++) - { - if ((sens_cfg[loop].type == BMI2_ACCEL) || (sens_cfg[loop].type == BMI2_GYRO) || - (sens_cfg[loop].type == BMI2_AUX) || (sens_cfg[loop].type == BMI2_GYRO_GAIN_UPDATE)) - { - rslt = bmi2_set_sensor_config(&sens_cfg[loop], 1, dev); - } - else - { - /* Disable Advance power save if enabled for auxiliary - * and feature configurations - */ - if (aps_stat == BMI2_ENABLE) - { - /* Disable advance power save if - * enabled - */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - } - - if (rslt == BMI2_OK) - { - switch (sens_cfg[loop].type) - { - /* Set the step counter parameters */ - case BMI2_STEP_COUNTER_PARAMS: - rslt = set_step_count_params_config(sens_cfg[loop].cfg.step_counter_params, dev); - break; - - /* Set step counter/detector/activity configuration */ - case BMI2_STEP_DETECTOR: - case BMI2_STEP_COUNTER: - rslt = set_step_config(&sens_cfg[loop].cfg.step_counter, dev); - break; - - default: - rslt = BMI2_E_INVALID_SENSOR; - break; - } - } - - /* Return error if any of the set configurations fail */ - if (rslt != BMI2_OK) - { - break; - } - } - } - - /* Enable Advance power save if disabled while configuring and - * not when already disabled - */ - if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) - { - rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API gets the sensor/feature configuration. - */ -int8_t bmi270_hc_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define loop */ - uint8_t loop; - - /* Variable to get the status of advance power save */ - uint8_t aps_stat = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (sens_cfg != NULL)) - { - /* Get status of advance power save mode */ - aps_stat = dev->aps_status; - for (loop = 0; loop < n_sens; loop++) - { - if ((sens_cfg[loop].type == BMI2_ACCEL) || (sens_cfg[loop].type == BMI2_GYRO) || - (sens_cfg[loop].type == BMI2_AUX) || (sens_cfg[loop].type == BMI2_GYRO_GAIN_UPDATE)) - { - rslt = bmi2_get_sensor_config(&sens_cfg[loop], 1, dev); - } - else - { - /* Disable Advance power save if enabled for auxiliary - * and feature configurations - */ - if ((sens_cfg[loop].type >= BMI2_MAIN_SENS_MAX_NUM) || (sens_cfg[loop].type == BMI2_AUX)) - { - - if (aps_stat == BMI2_ENABLE) - { - /* Disable advance power save if - * enabled - */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - } - } - - if (rslt == BMI2_OK) - { - switch (sens_cfg[loop].type) - { - /* Set the step counter parameters */ - case BMI2_STEP_COUNTER_PARAMS: - rslt = get_step_count_params_config(sens_cfg[loop].cfg.step_counter_params, dev); - break; - - /* Get step counter/detector/activity configuration */ - case BMI2_STEP_DETECTOR: - case BMI2_STEP_COUNTER: - rslt = get_step_config(&sens_cfg[loop].cfg.step_counter, dev); - break; - - default: - rslt = BMI2_E_INVALID_SENSOR; - break; - } - } - - /* Return error if any of the get configurations fail */ - if (rslt != BMI2_OK) - { - break; - } - } - } - - /* Enable Advance power save if disabled while configuring and - * not when already disabled - */ - if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) - { - rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API gets the sensor/feature data for accelerometer, gyroscope, - * auxiliary sensor, step counter, high-g, gyroscope user-gain update, - * orientation, gyroscope cross sensitivity and error status for NVM and VFRM. - */ -int8_t bmi270_hc_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define loop */ - uint8_t loop; - - /* Variable to get the status of advance power save */ - uint8_t aps_stat = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (sensor_data != NULL)) - { - /* Get status of advance power save mode */ - aps_stat = dev->aps_status; - for (loop = 0; loop < n_sens; loop++) - { - if ((sensor_data[loop].type == BMI2_ACCEL) || (sensor_data[loop].type == BMI2_GYRO) || - (sensor_data[loop].type == BMI2_AUX) || (sensor_data[loop].type == BMI2_GYRO_GAIN_UPDATE) || - (sensor_data[loop].type == BMI2_GYRO_CROSS_SENSE)) - { - rslt = bmi2_get_sensor_data(&sensor_data[loop], 1, dev); - } - else - { - /* Disable Advance power save if enabled for feature - * configurations - */ - if (sensor_data[loop].type >= BMI2_MAIN_SENS_MAX_NUM) - { - if (aps_stat == BMI2_ENABLE) - { - /* Disable advance power save if - * enabled - */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - } - } - - if (rslt == BMI2_OK) - { - switch (sensor_data[loop].type) - { - case BMI2_STEP_COUNTER: - - /* Get step counter output */ - rslt = get_step_counter_output(&sensor_data[loop].sens_data.step_counter_output, dev); - break; - case BMI2_NVM_STATUS: - - /* Get NVM error status */ - rslt = get_nvm_error_status(&sensor_data[loop].sens_data.nvm_status, dev); - break; - case BMI2_VFRM_STATUS: - - /* Get VFRM error status */ - rslt = get_vfrm_error_status(&sensor_data[loop].sens_data.vfrm_status, dev); - break; - default: - rslt = BMI2_E_INVALID_SENSOR; - break; - } - - /* Return error if any of the get sensor data fails */ - if (rslt != BMI2_OK) - { - break; - } - } - } - - /* Enable Advance power save if disabled while - * configuring and not when already disabled - */ - if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) - { - rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - } - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This api is used for retrieving the activity recognition settings currently set for bmi270hc. - */ -int8_t bmi270_hc_get_act_recg_sett(struct bmi2_hc_act_recg_sett *sett, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to get the status of advance power save */ - uint8_t aps_stat; - - /* Variable to set flag */ - uint8_t feat_found; - uint16_t msb_lsb; - uint8_t lsb; - uint8_t msb; - - /* Initialize feature configuration for activity recognition */ - struct bmi2_feature_config bmi2_act_recg_sett = { 0, 0, 0 }; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if (rslt == BMI2_OK) - { - /* Search for activity recognition feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&bmi2_act_recg_sett, BMI2_ACTIVITY_RECOGNITION_SETTINGS, dev); - if (feat_found) - { - aps_stat = dev->aps_status; - if (aps_stat == BMI2_ENABLE) - { - /* Disable advance power save if enabled */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - } - - /* Get the configuration from the page where activity recognition setting feature resides */ - if (rslt == BMI2_OK) - { - rslt = bmi2_get_feat_config(bmi2_act_recg_sett.page, feat_config, dev); - } - - if (rslt == BMI2_OK) - { - /* Define the offset in bytes */ - idx = bmi2_act_recg_sett.start_addr; - - lsb = feat_config[idx++]; - msb = feat_config[idx++]; - msb_lsb = ((uint16_t)msb << 8) | lsb; - - sett->segment_size = msb_lsb & BMI2_HC_ACT_RECG_SEGMENT_SIZE_MASK; - - /* Increment idx by 2 to point post processing enable/disable address */ - idx = 4; - lsb = feat_config[idx++]; - msb = feat_config[idx++]; - msb_lsb = ((uint16_t)msb << 8) | lsb; - sett->pp_en = msb_lsb & BMI2_HC_ACT_RECG_PP_EN_MASK; - - /* Increment idx by 2 to point mix gdi thres addres */ - idx = 6; - lsb = feat_config[idx++]; - msb = feat_config[idx++]; - msb_lsb = ((uint16_t)msb << 8) | lsb; - sett->min_gdi_thres = msb_lsb & BMI2_HC_ACT_RECG_MIN_GDI_THRES_MASK; - - /* Increment idx by 2 to point max gdi thres addres */ - idx = 8; - lsb = feat_config[idx++]; - msb = feat_config[idx++]; - msb_lsb = ((uint16_t)msb << 8) | lsb; - sett->max_gdi_thres = msb_lsb & BMI2_HC_ACT_RECG_MAX_GDI_THRES_MASK; - - /* Increment idx by 2 to point to buffer size */ - idx = 10; - lsb = feat_config[idx++]; - msb = feat_config[idx++]; - msb_lsb = ((uint16_t)msb << 8) | lsb; - sett->buf_size = msb_lsb & BMI2_HC_ACT_RECG_BUF_SIZE_MASK; - - /* Increment idx by 2 to to point to min segment confidence */ - idx = 12; - lsb = feat_config[idx++]; - msb = feat_config[idx++]; - msb_lsb = ((uint16_t)msb << 8) | lsb; - sett->min_seg_conf = msb_lsb & BMI2_HC_ACT_RECG_MIN_SEG_CONF_MASK; - } - - /* Enable Advance power save if disabled while - * configuring and not when already disabled - */ - if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) - { - rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - } - - return rslt; -} - -/*! - * @brief This api is used for setting the activity recognition settings for bmi270hc. - */ -int8_t bmi270_hc_set_act_recg_sett(const struct bmi2_hc_act_recg_sett *sett, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to get the status of advance power save */ - uint8_t aps_stat; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Variable to define index */ - uint8_t index = 0; - - /* Initialize feature configuration for activity recognition */ - struct bmi2_feature_config bmi2_act_recg_sett = { 0, 0, 0 }; - - /* Copy the feature configuration address to a local pointer */ - uint16_t *data_p = (uint16_t *) (void *)feat_config; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if (rslt == BMI2_OK) - { - /* Search for activity recognition feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&bmi2_act_recg_sett, BMI2_ACTIVITY_RECOGNITION_SETTINGS, dev); - if (feat_found) - { - aps_stat = dev->aps_status; - if (aps_stat == BMI2_ENABLE) - { - /* Disable advance power save if enabled */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - } - - if (rslt == BMI2_OK) - { - /* Get the configuration from the page where activity recognition setting feature resides */ - rslt = bmi2_get_feat_config(bmi2_act_recg_sett.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes */ - idx = bmi2_act_recg_sett.start_addr; - - /* Get offset in words since all the features are set in words length */ - idx = idx / 2; - - /* Set segment size */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), - BMI2_HC_ACT_RECG_SEGMENT_SIZE, - sett->segment_size); - - /* Starting address of post processing represented in word length */ - idx = 2; - - /* Set post processing */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_HC_ACT_RECG_PP_EN, sett->pp_en); - - /* Starting address of min_gdi_thres represented in word length */ - idx = 3; - - /* Set minimum gdi threshold */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), - BMI2_HC_ACT_RECG_MIN_GDI_THRES, - sett->min_gdi_thres); - - /* Starting address of max_gdi_thres represented in word length */ - idx = 4; - - /* Set maximum gdi threshold */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), - BMI2_HC_ACT_RECG_MAX_GDI_THRES, - sett->max_gdi_thres); - - /* Starting address of buffer size represented in word length */ - idx = 5; - - /* Set buffer size */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_HC_ACT_RECG_BUF_SIZE, sett->buf_size); - - /* Starting address of min_seg_conf represented in word length */ - idx = 6; - - /* Set min segment confidence */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), - BMI2_HC_ACT_RECG_MIN_SEG_CONF, - sett->min_seg_conf); - - /* Increment offset by 1 more word to get the total length in words */ - idx++; - - /* Get total length in bytes to copy from local pointer to the array */ - idx = (uint8_t)(idx * 2) - bmi2_act_recg_sett.start_addr; - - /* Copy the bytes to be set back to the array */ - for (index = 0; index <= idx; index++) - { - feat_config[bmi2_act_recg_sett.start_addr + - index] = *((uint8_t *) data_p + bmi2_act_recg_sett.start_addr + index); - } - - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - - /* Enable Advance power save if disabled while - * configuring and not when already disabled - */ - if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) - { - rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - } - - return rslt; -} - -/*! - * @brief This internal API is used to parse the activity output from the - * FIFO in header mode. - */ -int8_t bmi270_hc_get_act_recog_output(struct bmi2_act_recog_output *act_recog, - uint16_t *act_frm_len, - struct bmi2_fifo_frame *fifo, - const struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define header frame */ - uint8_t frame_header = 0; - - /* Variable to index the data bytes */ - uint16_t data_index; - - /* Variable to index activity frames */ - uint16_t act_idx = 0; - - /* Variable to indicate activity frames read */ - uint16_t frame_to_read = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (act_recog != NULL) && (act_frm_len != NULL) && (fifo != NULL)) - { - - /* Store the number of frames to be read */ - frame_to_read = *act_frm_len; - for (data_index = fifo->act_recog_byte_start_idx; data_index < fifo->length;) - { - /* Get frame header byte */ - frame_header = fifo->data[data_index] & BMI2_FIFO_TAG_INTR_MASK; - - /* Skip S4S frames if S4S is enabled */ - rslt = move_if_s4s_frame(&frame_header, &data_index, fifo); - - /* Break if FIFO is empty */ - if (rslt == BMI2_W_FIFO_EMPTY) - { - break; - } - - /* Index shifted to next byte where data starts */ - data_index++; - switch (frame_header) - { - /* If header defines accelerometer frame */ - case BMI2_FIFO_HEADER_ACC_FRM: - rslt = move_next_frame(&data_index, fifo->acc_frm_len, fifo); - break; - - /* If header defines accelerometer and auxiliary frames */ - case BMI2_FIFO_HEADER_AUX_ACC_FRM: - rslt = move_next_frame(&data_index, fifo->acc_aux_frm_len, fifo); - break; - - /* If header defines accelerometer and gyroscope frames */ - case BMI2_FIFO_HEADER_GYR_ACC_FRM: - rslt = move_next_frame(&data_index, fifo->acc_gyr_frm_len, fifo); - break; - - /* If header defines accelerometer, auxiliary and gyroscope frames */ - case BMI2_FIFO_HEADER_ALL_FRM: - rslt = move_next_frame(&data_index, fifo->all_frm_len, fifo); - break; - - /* If header defines only gyroscope frame */ - case BMI2_FIFO_HEADER_GYR_FRM: - rslt = move_next_frame(&data_index, fifo->gyr_frm_len, fifo); - break; - - /* If header defines only auxiliary frame */ - case BMI2_FIFO_HEADER_AUX_FRM: - rslt = move_next_frame(&data_index, fifo->aux_frm_len, fifo); - break; - - /* If header defines auxiliary and gyroscope frame */ - case BMI2_FIFO_HEADER_AUX_GYR_FRM: - rslt = move_next_frame(&data_index, fifo->aux_gyr_frm_len, fifo); - break; - - /* If header defines sensor time frame */ - case BMI2_FIFO_HEADER_SENS_TIME_FRM: - rslt = move_next_frame(&data_index, BMI2_SENSOR_TIME_LENGTH, fifo); - break; - - /* If header defines skip frame */ - case BMI2_FIFO_HEADER_SKIP_FRM: - rslt = move_next_frame(&data_index, BMI2_FIFO_SKIP_FRM_LENGTH, fifo); - break; - - /* If header defines Input configuration frame */ - case BMI2_FIFO_HEADER_INPUT_CFG_FRM: - rslt = move_next_frame(&data_index, BMI2_FIFO_INPUT_CFG_LENGTH, fifo); - break; - - /* If header defines invalid frame or end of valid data */ - case BMI2_FIFO_HEAD_OVER_READ_MSB: - - /* Move the data index to the last byte to mark completion */ - data_index = fifo->length; - - /* FIFO is empty */ - rslt = BMI2_W_FIFO_EMPTY; - break; - - /* If header defines activity recognition frame */ - case BMI2_FIFO_VIRT_ACT_RECOG_FRM: - - /* Get the activity output */ - rslt = unpack_act_recog_output(&act_recog[(act_idx)], &data_index, fifo); - - /* Update activity frame index */ - (act_idx)++; - break; - default: - - /* Move the data index to the last byte in case of invalid values */ - data_index = fifo->length; - - /* FIFO is empty */ - rslt = BMI2_W_FIFO_EMPTY; - break; - } - - /* Number of frames to be read is complete or FIFO is empty */ - if ((frame_to_read == act_idx) || (rslt == BMI2_W_FIFO_EMPTY)) - { - break; - } - } - - /* Update the activity frame index */ - (*act_frm_len) = act_idx; - - /* Update the activity byte index */ - fifo->act_recog_byte_start_idx = data_index; - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API updates the gyroscope user-gain. - */ -int8_t bmi270_hc_update_gyro_user_gain(const struct bmi2_gyro_user_gain_config *user_gain, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to select sensor */ - uint8_t sens_sel[2] = { BMI2_GYRO, BMI2_GYRO_GAIN_UPDATE }; - - /* Structure to define sensor configurations */ - struct bmi2_sens_config sens_cfg; - - /* Variable to store status of user-gain update module */ - uint8_t status = 0; - - /* Variable to define count */ - uint8_t count = 100; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (user_gain != NULL)) - { - /* Select type of feature */ - sens_cfg.type = BMI2_GYRO_GAIN_UPDATE; - - /* Get the user gain configurations */ - rslt = bmi270_hc_get_sensor_config(&sens_cfg, 1, dev); - if (rslt == BMI2_OK) - { - /* Get the user-defined ratio */ - sens_cfg.cfg.gyro_gain_update = *user_gain; - - /* Set rate ratio for all axes */ - rslt = bmi270_hc_set_sensor_config(&sens_cfg, 1, dev); - } - - /* Disable gyroscope */ - if (rslt == BMI2_OK) - { - rslt = bmi270_hc_sensor_disable(&sens_sel[0], 1, dev); - } - - /* Enable gyroscope user-gain update module */ - if (rslt == BMI2_OK) - { - rslt = bmi270_hc_sensor_enable(&sens_sel[1], 1, dev); - } - - /* Set the command to trigger the computation */ - if (rslt == BMI2_OK) - { - rslt = bmi2_set_command_register(BMI2_USR_GAIN_CMD, dev); - } - - if (rslt == BMI2_OK) - { - /* Poll until enable bit of user-gain update is 0 */ - while (count--) - { - rslt = get_user_gain_upd_status(&status, dev); - if ((rslt == BMI2_OK) && (status == 0)) - { - /* Enable compensation of gain defined - * in the GAIN register - */ - rslt = enable_gyro_gain(BMI2_ENABLE, dev); - - /* Enable gyroscope */ - if (rslt == BMI2_OK) - { - rslt = bmi270_hc_sensor_enable(&sens_sel[0], 1, dev); - } - - break; - } - - dev->delay_us(10000, dev->intf_ptr); - } - - /* Return error if user-gain update is failed */ - if ((rslt == BMI2_OK) && (status != 0)) - { - rslt = BMI2_E_GYR_USER_GAIN_UPD_FAIL; - } - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API reads the compensated gyroscope user-gain values. - */ -int8_t bmi270_hc_read_gyro_user_gain(struct bmi2_gyro_user_gain_data *gyr_usr_gain, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define register data */ - uint8_t reg_data[3] = { 0 }; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (gyr_usr_gain != NULL)) - { - /* Get the gyroscope compensated gain values */ - rslt = bmi2_get_regs(BMI2_GYR_USR_GAIN_0_ADDR, reg_data, 3, dev); - if (rslt == BMI2_OK) - { - /* Gyroscope user gain correction X-axis */ - gyr_usr_gain->x = (int8_t)BMI2_GET_BIT_POS0(reg_data[0], BMI2_GYR_USR_GAIN_X); - - /* Gyroscope user gain correction Y-axis */ - gyr_usr_gain->y = (int8_t)BMI2_GET_BIT_POS0(reg_data[1], BMI2_GYR_USR_GAIN_Y); - - /* Gyroscope user gain correction z-axis */ - gyr_usr_gain->z = (int8_t)BMI2_GET_BIT_POS0(reg_data[2], BMI2_GYR_USR_GAIN_Z); - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API maps/unmaps feature interrupts to that of interrupt pins. - */ -int8_t bmi270_hc_map_feat_int(const struct bmi2_sens_int_config *sens_int, uint8_t n_sens, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define loop */ - uint8_t loop; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (sens_int != NULL)) - { - for (loop = 0; loop < n_sens; loop++) - { - switch (sens_int[loop].type) - { - case BMI2_STEP_COUNTER: - case BMI2_STEP_DETECTOR: - - rslt = bmi2_map_feat_int(sens_int[loop].type, sens_int[loop].hw_int_pin, dev); - break; - default: - rslt = BMI2_E_INVALID_SENSOR; - break; - } - - /* Return error if interrupt mapping fails */ - if (rslt != BMI2_OK) - { - break; - } - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/***************************************************************************/ - -/*! Local Function Definitions - ****************************************************************************/ - -/*! - * @brief This internal API is used to validate the device structure pointer for - * null conditions. - */ -static int8_t null_ptr_check(const struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt = BMI2_OK; - - if ((dev == NULL) || (dev->read == NULL) || (dev->write == NULL) || (dev->delay_us == NULL)) - { - /* Device structure pointer is not valid */ - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This internal API selects the sensor/features to be enabled or - * disabled. - */ -static int8_t select_sensor(const uint8_t *sens_list, uint8_t n_sens, uint64_t *sensor_sel) -{ - /* Variable to define error */ - int8_t rslt = BMI2_OK; - - /* Variable to define loop */ - uint8_t count; - - for (count = 0; count < n_sens; count++) - { - switch (sens_list[count]) - { - case BMI2_ACCEL: - *sensor_sel |= BMI2_ACCEL_SENS_SEL; - break; - case BMI2_GYRO: - *sensor_sel |= BMI2_GYRO_SENS_SEL; - break; - case BMI2_AUX: - *sensor_sel |= BMI2_AUX_SENS_SEL; - break; - case BMI2_TEMP: - *sensor_sel |= BMI2_TEMP_SENS_SEL; - break; - case BMI2_STEP_DETECTOR: - *sensor_sel |= BMI2_STEP_DETECT_SEL; - break; - case BMI2_STEP_COUNTER: - *sensor_sel |= BMI2_STEP_COUNT_SEL; - break; - case BMI2_GYRO_GAIN_UPDATE: - *sensor_sel |= BMI2_GYRO_GAIN_UPDATE_SEL; - break; - case BMI2_ACTIVITY_RECOGNITION: - *sensor_sel |= BMI2_ACTIVITY_RECOGNITION_SEL; - break; - default: - rslt = BMI2_E_INVALID_SENSOR; - break; - } - } - - return rslt; -} - -/*! - * @brief This internal API enables the selected sensor/features. - */ -static int8_t sensor_enable(uint64_t sensor_sel, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to store register values */ - uint8_t reg_data = 0; - - /* Variable to define loop */ - uint8_t loop = 1; - - /* Variable to get the status of advance power save */ - uint8_t aps_stat = 0; - - rslt = bmi2_get_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); - if (rslt == BMI2_OK) - { - /* Enable accelerometer */ - if (sensor_sel & BMI2_ACCEL_SENS_SEL) - { - reg_data = BMI2_SET_BITS(reg_data, BMI2_ACC_EN, BMI2_ENABLE); - } - - /* Enable gyroscope */ - if (sensor_sel & BMI2_GYRO_SENS_SEL) - { - reg_data = BMI2_SET_BITS(reg_data, BMI2_GYR_EN, BMI2_ENABLE); - } - - /* Enable auxiliary sensor */ - if (sensor_sel & BMI2_AUX_SENS_SEL) - { - reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_AUX_EN, BMI2_ENABLE); - } - - /* Enable temperature sensor */ - if (sensor_sel & BMI2_TEMP_SENS_SEL) - { - reg_data = BMI2_SET_BITS(reg_data, BMI2_TEMP_EN, BMI2_ENABLE); - } - - /* Enable the sensors that are set in the power control register */ - if (sensor_sel & BMI2_MAIN_SENSORS) - { - rslt = bmi2_set_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); - } - } - - if ((rslt == BMI2_OK) && (sensor_sel & ~(BMI2_MAIN_SENSORS))) - { - /* Get status of advance power save mode */ - aps_stat = dev->aps_status; - if (aps_stat == BMI2_ENABLE) - { - /* Disable advance power save if enabled */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - } - - if (rslt == BMI2_OK) - { - while (loop--) - { - /* Enable step detector feature */ - if (sensor_sel & BMI2_STEP_DETECT_SEL) - { - rslt = set_step_detector(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_STEP_DETECT_SEL; - } - else - { - break; - } - } - - /* Enable step counter feature */ - if (sensor_sel & BMI2_STEP_COUNT_SEL) - { - rslt = set_step_counter(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_STEP_COUNT_SEL; - } - else - { - break; - } - } - - /* Enable activity recognition feature */ - if (sensor_sel & BMI2_ACTIVITY_RECOGNITION_SEL) - { - rslt = set_act_recog(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_ACTIVITY_RECOGNITION_SEL; - } - else - { - break; - } - } - - /* Enable gyroscope user gain */ - if (sensor_sel & BMI2_GYRO_GAIN_UPDATE_SEL) - { - rslt = set_gyro_user_gain(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_GYRO_GAIN_UPDATE_SEL; - } - else - { - break; - } - } - } - - /* Enable Advance power save if disabled while - * configuring and not when already disabled - */ - if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) - { - rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - } - } - } - - return rslt; -} - -/*! - * @brief This internal API disables the selected sensors/features. - */ -static int8_t sensor_disable(uint64_t sensor_sel, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to store register values */ - uint8_t reg_data = 0; - - /* Variable to define loop */ - uint8_t loop = 1; - - /* Variable to get the status of advance power save */ - uint8_t aps_stat = 0; - - rslt = bmi2_get_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); - if (rslt == BMI2_OK) - { - /* Disable accelerometer */ - if (sensor_sel & BMI2_ACCEL_SENS_SEL) - { - reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_ACC_EN); - } - - /* Disable gyroscope */ - if (sensor_sel & BMI2_GYRO_SENS_SEL) - { - reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_GYR_EN); - } - - /* Disable auxiliary sensor */ - if (sensor_sel & BMI2_AUX_SENS_SEL) - { - reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_AUX_EN); - } - - /* Disable temperature sensor */ - if (sensor_sel & BMI2_TEMP_SENS_SEL) - { - reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_TEMP_EN); - } - - /* Disable the sensors that are set in the power control register */ - if (sensor_sel & BMI2_MAIN_SENSORS) - { - rslt = bmi2_set_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); - } - } - - if ((rslt == BMI2_OK) && (sensor_sel & ~(BMI2_MAIN_SENSORS))) - { - /* Get status of advance power save mode */ - aps_stat = dev->aps_status; - if (aps_stat == BMI2_ENABLE) - { - /* Disable advance power save if enabled */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - } - - if (rslt == BMI2_OK) - { - while (loop--) - { - /* Disable step detector feature */ - if (sensor_sel & BMI2_STEP_DETECT_SEL) - { - rslt = set_step_detector(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat &= ~BMI2_STEP_DETECT_SEL; - } - else - { - break; - } - } - - /* Disable step counter feature */ - if (sensor_sel & BMI2_STEP_COUNT_SEL) - { - rslt = set_step_counter(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat &= ~BMI2_STEP_COUNT_SEL; - } - else - { - break; - } - } - - /* Disable activity recognition feature */ - if (sensor_sel & BMI2_ACTIVITY_RECOGNITION_SEL) - { - rslt = set_act_recog(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_ACTIVITY_RECOGNITION_SEL; - } - else - { - break; - } - } - - /* Disable gyroscope user gain */ - if (sensor_sel & BMI2_GYRO_GAIN_UPDATE_SEL) - { - rslt = set_gyro_user_gain(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat &= ~BMI2_GYRO_GAIN_UPDATE_SEL; - } - else - { - break; - } - } - - /* Enable Advance power save if disabled while - * configuring and not when already disabled - */ - if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) - { - rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - } - } - } - } - - return rslt; -} - -/*! - * @brief This internal API is used to enable/disable step detector feature. - */ -static int8_t set_step_detector(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for step detector */ - struct bmi2_feature_config step_det_config = { 0, 0, 0 }; - - /* Search for step detector feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&step_det_config, BMI2_STEP_DETECTOR, dev); - if (feat_found) - { - /* Get the configuration from the page where step detector feature resides */ - rslt = bmi2_get_feat_config(step_det_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of step detector */ - idx = step_det_config.start_addr + BMI2_STEP_COUNT_FEAT_EN_OFFSET; - - /* Set the feature enable bit */ - feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_STEP_DET_FEAT_EN, enable); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API is used to enable/disable step counter feature. - */ -static int8_t set_step_counter(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for step counter */ - struct bmi2_feature_config step_count_config = { 0, 0, 0 }; - - /* Search for step counter feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&step_count_config, BMI2_STEP_COUNTER, dev); - if (feat_found) - { - /* Get the configuration from the page where step-counter feature resides */ - rslt = bmi2_get_feat_config(step_count_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of step counter */ - idx = step_count_config.start_addr + BMI2_STEP_COUNT_FEAT_EN_OFFSET; - - /* Set the feature enable bit */ - feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_STEP_COUNT_FEAT_EN, enable); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! -* @brief This internal API enables/disables the activity recognition feature. -*/ -static int8_t set_act_recog(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for activity recognition */ - struct bmi2_feature_config act_recog_cfg = { 0, 0, 0 }; - - /* Search for activity recognition and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&act_recog_cfg, BMI2_ACTIVITY_RECOGNITION, dev); - if (feat_found) - { - /* Get the configuration from the page where activity - * recognition feature resides - */ - rslt = bmi2_get_feat_config(act_recog_cfg.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of activity recognition */ - idx = act_recog_cfg.start_addr; - - /* Set the feature enable bit */ - feat_config[idx] = BMI2_SET_BIT_POS0(feat_config[idx], BMI2_ACTIVITY_RECOG_EN, enable); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API is used to enable/disable gyroscope user gain - * feature. - */ -static int8_t set_gyro_user_gain(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for gyroscope user gain */ - struct bmi2_feature_config gyr_user_gain_cfg = { 0, 0, 0 }; - - /* Search for user gain feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&gyr_user_gain_cfg, BMI2_GYRO_GAIN_UPDATE, dev); - if (feat_found) - { - /* Get the configuration from the page where user gain feature resides */ - rslt = bmi2_get_feat_config(gyr_user_gain_cfg.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of user gain */ - idx = gyr_user_gain_cfg.start_addr + BMI2_GYR_USER_GAIN_FEAT_EN_OFFSET; - - /* Set the feature enable bit */ - feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_GYR_USER_GAIN_FEAT_EN, enable); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API sets step counter parameter configurations. - */ -static int8_t set_step_count_params_config(const uint16_t *step_count_params, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt = BMI2_OK; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define index */ - uint8_t index = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for step counter parameters */ - struct bmi2_feature_config step_params_config = { 0, 0, 0 }; - - /* Variable to index the page number */ - uint8_t page_idx; - - /* Variable to define the start page */ - uint8_t start_page; - - /* Variable to define start address of the parameters */ - uint8_t start_addr; - - /* Variable to define number of bytes */ - uint8_t n_bytes = (BMI2_STEP_CNT_N_PARAMS * 2); - - /* Variable to store number of pages */ - uint8_t n_pages = (n_bytes / 16); - - /* Variable to define the end page */ - uint8_t end_page; - - /* Variable to define the remaining bytes to be read */ - uint8_t remain_len; - - /* Variable to define the maximum words(16 bytes or 8 words) to be read in a page */ - uint8_t max_len = 8; - - /* Variable index bytes in a page */ - uint8_t page_byte_idx; - - /* Variable to index the parameters */ - uint8_t param_idx = 0; - - /* Copy the feature configuration address to a local pointer */ - uint16_t *data_p = (uint16_t *) (void *)feat_config; - - /* Search for step counter parameter feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&step_params_config, BMI2_STEP_COUNTER_PARAMS, dev); - if (feat_found) - { - /* Get the start page for the step counter parameters */ - start_page = step_params_config.page; - - /* Get the end page for the step counter parameters */ - end_page = start_page + n_pages; - - /* Get the start address for the step counter parameters */ - start_addr = step_params_config.start_addr; - - /* Get the remaining length of bytes to be read */ - remain_len = (uint8_t)((n_bytes - (n_pages * 16)) + start_addr); - for (page_idx = start_page; page_idx <= end_page; page_idx++) - { - /* Get the configuration from the respective page */ - rslt = bmi2_get_feat_config(page_idx, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Start from address 0x00 when switched to next page */ - if (page_idx > start_page) - { - start_addr = 0; - } - - /* Remaining number of words to be read in the page */ - if (page_idx == end_page) - { - max_len = (remain_len / 2); - } - - /* Get offset in words since all the features are set in words length */ - page_byte_idx = start_addr / 2; - for (; page_byte_idx < max_len;) - { - /* Set parameters 1 to 25 */ - *(data_p + page_byte_idx) = BMI2_SET_BIT_POS0(*(data_p + page_byte_idx), - BMI2_STEP_COUNT_PARAMS, - step_count_params[param_idx]); - - /* Increment offset by 1 word to set to the next parameter */ - page_byte_idx++; - - /* Increment to next parameter */ - param_idx++; - } - - /* Get total length in bytes to copy from local pointer to the array */ - page_byte_idx = (uint8_t)(page_byte_idx * 2) - step_params_config.start_addr; - - /* Copy the bytes to be set back to the array */ - for (index = 0; index < page_byte_idx; index++) - { - feat_config[step_params_config.start_addr + - index] = *((uint8_t *) data_p + step_params_config.start_addr + index); - } - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/* @brief This internal API sets step counter configurations like water-mark - * level, reset-counter and output-configuration step detector and activity. - */ -static int8_t set_step_config(const struct bmi2_step_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define index */ - uint8_t index = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for step counter 4 */ - struct bmi2_feature_config step_count_config = { 0, 0, 0 }; - - /* Copy the feature configuration address to a local pointer */ - uint16_t *data_p = (uint16_t *) (void *)feat_config; - - /* Search for step counter feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&step_count_config, BMI2_STEP_COUNTER, dev); - if (feat_found) - { - /* Get the configuration from the page where step counter resides */ - rslt = bmi2_get_feat_config(step_count_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes */ - idx = step_count_config.start_addr; - - /* Get offset in words since all the features are set in words length */ - idx = idx / 2; - - /* Set water-mark level */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_STEP_COUNT_WM_LEVEL, config->watermark_level); - - /* Set reset-counter */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_STEP_COUNT_RST_CNT, config->reset_counter); - - /* Increment offset by 1 word to set output - * configuration of step detector and step activity - */ - idx++; - - /* Set step buffer size */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_STEP_BUFFER_SIZE, config->step_buffer_size); - - /* Increment offset by 1 more word to get the total length in words */ - idx++; - - /* Get total length in bytes to copy from local pointer to the array */ - idx = (uint8_t)(idx * 2) - step_count_config.start_addr; - - /* Copy the bytes to be set back to the array */ - for (index = 0; index < idx; index++) - { - feat_config[step_count_config.start_addr + - index] = *((uint8_t *) data_p + step_count_config.start_addr + index); - } - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets step counter parameter configurations. - */ -static int8_t get_step_count_params_config(uint16_t *step_count_params, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt = BMI2_OK; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Variable to define LSB */ - uint16_t lsb = 0; - - /* Variable to define MSB */ - uint16_t msb = 0; - - /* Variable to define a word */ - uint16_t lsb_msb = 0; - - /* Initialize feature configuration for step counter 1 */ - struct bmi2_feature_config step_params_config = { 0, 0, 0 }; - - /* Variable to index the page number */ - uint8_t page_idx; - - /* Variable to define the start page */ - uint8_t start_page; - - /* Variable to define start address of the parameters */ - uint8_t start_addr; - - /* Variable to define number of bytes */ - uint8_t n_bytes = (BMI2_STEP_CNT_N_PARAMS * 2); - - /* Variable to store number of pages */ - uint8_t n_pages = (n_bytes / 16); - - /* Variable to define the end page */ - uint8_t end_page; - - /* Variable to define the remaining bytes to be read */ - uint8_t remain_len; - - /* Variable to define the maximum words to be read in a page */ - uint8_t max_len = BMI2_FEAT_SIZE_IN_BYTES; - - /* Variable index bytes in a page */ - uint8_t page_byte_idx; - - /* Variable to index the parameters */ - uint8_t param_idx = 0; - - /* Search for step counter parameter feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&step_params_config, BMI2_STEP_COUNTER_PARAMS, dev); - if (feat_found) - { - /* Get the start page for the step counter parameters */ - start_page = step_params_config.page; - - /* Get the end page for the step counter parameters */ - end_page = start_page + n_pages; - - /* Get the start address for the step counter parameters */ - start_addr = step_params_config.start_addr; - - /* Get the remaining length of bytes to be read */ - remain_len = (uint8_t)((n_bytes - (n_pages * 16)) + start_addr); - for (page_idx = start_page; page_idx <= end_page; page_idx++) - { - /* Get the configuration from the respective page */ - rslt = bmi2_get_feat_config(page_idx, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Start from address 0x00 when switched to next page */ - if (page_idx > start_page) - { - start_addr = 0; - } - - /* Remaining number of bytes to be read in the page */ - if (page_idx == end_page) - { - max_len = remain_len; - } - - /* Get the offset */ - page_byte_idx = start_addr; - while (page_byte_idx < max_len) - { - /* Get word to calculate the parameter*/ - lsb = (uint16_t) feat_config[page_byte_idx++]; - if (page_byte_idx < max_len) - { - msb = ((uint16_t) feat_config[page_byte_idx++] << 8); - } - - lsb_msb = lsb | msb; - - /* Get parameters 1 to 25 */ - step_count_params[param_idx] = lsb_msb & BMI2_STEP_COUNT_PARAMS_MASK; - - /* Increment to next parameter */ - param_idx++; - } - } - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets step counter/detector/activity configurations. - */ -static int8_t get_step_config(struct bmi2_step_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define LSB */ - uint16_t lsb = 0; - - /* Variable to define MSB */ - uint16_t msb = 0; - - /* Variable to define a word */ - uint16_t lsb_msb = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for step counter */ - struct bmi2_feature_config step_count_config = { 0, 0, 0 }; - - /* Search for step counter 4 feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&step_count_config, BMI2_STEP_COUNTER, dev); - if (feat_found) - { - /* Get the configuration from the page where step counter 4 parameter resides */ - rslt = bmi2_get_feat_config(step_count_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for feature enable for step counter/detector/activity */ - idx = step_count_config.start_addr; - - /* Get word to calculate water-mark level and reset counter */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get water-mark level */ - config->watermark_level = lsb_msb & BMI2_STEP_COUNT_WM_LEVEL_MASK; - - /* Get reset counter */ - config->reset_counter = (lsb_msb & BMI2_STEP_COUNT_RST_CNT_MASK) >> BMI2_STEP_COUNT_RST_CNT_POS; - - /* Get word to calculate output configuration of step detector and activity */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - config->step_buffer_size = (lsb_msb & BMI2_STEP_BUFFER_SIZE_MASK) >> BMI2_STEP_BUFFER_SIZE_POS; - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets the output values of step counter. - */ -static int8_t get_step_counter_output(uint32_t *step_count, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variables to define index */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature output for step counter */ - struct bmi2_feature_config step_cnt_out_config = { 0, 0, 0 }; - - /* Search for step counter output feature and extract its configuration details */ - feat_found = extract_output_feat_config(&step_cnt_out_config, BMI2_STEP_COUNTER, dev); - if (feat_found) - { - /* Get the feature output configuration for step-counter */ - rslt = bmi2_get_feat_config(step_cnt_out_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for step counter output */ - idx = step_cnt_out_config.start_addr; - - /* Get the step counter output in 4 bytes */ - *step_count = (uint32_t) feat_config[idx++]; - *step_count |= ((uint32_t) feat_config[idx++] << 8); - *step_count |= ((uint32_t) feat_config[idx++] << 16); - *step_count |= ((uint32_t) feat_config[idx++] << 24); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets the error status related to NVM. - */ -static int8_t get_nvm_error_status(struct bmi2_nvm_err_status *nvm_err_stat, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variables to define index */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature output for NVM error status */ - struct bmi2_feature_config nvm_err_cfg = { 0, 0, 0 }; - - /* Search for NVM error status feature and extract its configuration details */ - feat_found = extract_output_feat_config(&nvm_err_cfg, BMI2_NVM_STATUS, dev); - if (feat_found) - { - /* Get the feature output configuration for NVM error status */ - rslt = bmi2_get_feat_config(nvm_err_cfg.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for NVM error status */ - idx = nvm_err_cfg.start_addr; - - /* Increment index to get the error status */ - idx++; - - /* Error when NVM load action fails */ - nvm_err_stat->load_error = BMI2_GET_BIT_POS0(feat_config[idx], BMI2_NVM_LOAD_ERR_STATUS); - - /* Error when NVM program action fails */ - nvm_err_stat->prog_error = BMI2_GET_BITS(feat_config[idx], BMI2_NVM_PROG_ERR_STATUS); - - /* Error when NVM erase action fails */ - nvm_err_stat->erase_error = BMI2_GET_BITS(feat_config[idx], BMI2_NVM_ERASE_ERR_STATUS); - - /* Error when NVM program limit is exceeded */ - nvm_err_stat->exceed_error = BMI2_GET_BITS(feat_config[idx], BMI2_NVM_END_EXCEED_STATUS); - - /* Error when NVM privilege mode is not acquired */ - nvm_err_stat->privil_error = BMI2_GET_BITS(feat_config[idx], BMI2_NVM_PRIV_ERR_STATUS); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API is used to get enable status of gyroscope user gain - * update. - */ -static int8_t get_user_gain_upd_status(uint8_t *status, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt = BMI2_OK; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Variable to check APS status */ - uint8_t aps_stat = 0; - - /* Initialize feature configuration for gyroscope user gain */ - struct bmi2_feature_config gyr_user_gain_cfg = { 0, 0, 0 }; - - /* Search for user gain feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&gyr_user_gain_cfg, BMI2_GYRO_GAIN_UPDATE, dev); - if (feat_found) - { - /* Disable advance power save */ - aps_stat = dev->aps_status; - if (aps_stat == BMI2_ENABLE) - { - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - } - - if (rslt == BMI2_OK) - { - /* Get the configuration from the page where user gain feature resides */ - rslt = bmi2_get_feat_config(gyr_user_gain_cfg.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of user gain */ - idx = gyr_user_gain_cfg.start_addr + BMI2_GYR_USER_GAIN_FEAT_EN_OFFSET; - - /* Set the feature enable status */ - *status = BMI2_GET_BITS(feat_config[idx], BMI2_GYR_USER_GAIN_FEAT_EN); - } - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - /* Enable Advance power save if disabled while configuring and not when already disabled */ - if ((rslt == BMI2_OK) && (aps_stat == BMI2_ENABLE)) - { - rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - } - - return rslt; -} - -/*! - * @brief This internal API is used to parse and store the activity recognition - * output from the FIFO data. - */ -static int8_t unpack_act_recog_output(struct bmi2_act_recog_output *act_recog, - uint16_t *data_index, - const struct bmi2_fifo_frame *fifo) -{ - /* Variable to define error */ - int8_t rslt = BMI2_OK; - - /* Variables to define 4 bytes of sensor time */ - uint32_t time_stamp_byte4 = 0; - uint32_t time_stamp_byte3 = 0; - uint32_t time_stamp_byte2 = 0; - uint32_t time_stamp_byte1 = 0; - - /* Validate data index */ - if ((*data_index + BMI2_FIFO_VIRT_ACT_DATA_LENGTH) >= fifo->length) - { - /* Update the data index to the last byte */ - (*data_index) = fifo->length; - - /* FIFO is empty */ - rslt = BMI2_W_FIFO_EMPTY; - } - else - { - /* Get time-stamp from the activity recognition frame */ - time_stamp_byte4 = ((uint32_t)(fifo->data[(*data_index) + 3]) << 24); - time_stamp_byte3 = ((uint32_t)(fifo->data[(*data_index) + 2]) << 16); - time_stamp_byte2 = fifo->data[(*data_index) + 1] << 8; - time_stamp_byte1 = fifo->data[(*data_index)]; - - /* Update time-stamp from the virtual frame */ - act_recog->time_stamp = (time_stamp_byte4 | time_stamp_byte3 | time_stamp_byte2 | time_stamp_byte1); - - /* Move the data index by 4 bytes */ - (*data_index) = (*data_index) + BMI2_FIFO_VIRT_ACT_TIME_LENGTH; - - /* Update the previous activity from the virtual frame */ - act_recog->prev_act = fifo->data[(*data_index)]; - - /* Move the data index by 1 byte */ - (*data_index) = (*data_index) + BMI2_FIFO_VIRT_ACT_TYPE_LENGTH; - - /* Update the current activity from the virtual frame */ - act_recog->curr_act = fifo->data[(*data_index)]; - - /* Move the data index by 1 byte */ - (*data_index) = (*data_index) + BMI2_FIFO_VIRT_ACT_STAT_LENGTH; - - /* More frames could be read */ - rslt = BMI2_W_PARTIAL_READ; - } - - return rslt; -} - -/*! - * @brief This internal API gets the error status related to virtual frames. - */ -static int8_t get_vfrm_error_status(struct bmi2_vfrm_err_status *vfrm_err_stat, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variables to define index */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature output for VFRM error status */ - struct bmi2_feature_config vfrm_err_cfg = { 0, 0, 0 }; - - /* Search for VFRM error status feature and extract its configuration details */ - feat_found = extract_output_feat_config(&vfrm_err_cfg, BMI2_VFRM_STATUS, dev); - if (feat_found) - { - /* Get the feature output configuration for VFRM error status */ - rslt = bmi2_get_feat_config(vfrm_err_cfg.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for VFRM error status */ - idx = vfrm_err_cfg.start_addr; - - /* Increment index to get the error status */ - idx++; - - /* Internal error while acquiring lock for FIFO */ - vfrm_err_stat->lock_error = BMI2_GET_BITS(feat_config[idx], BMI2_VFRM_LOCK_ERR_STATUS); - - /* Internal error while writing byte into FIFO */ - vfrm_err_stat->write_error = BMI2_GET_BITS(feat_config[idx], BMI2_VFRM_WRITE_ERR_STATUS); - - /* Internal error while writing into FIFO */ - vfrm_err_stat->fatal_error = BMI2_GET_BITS(feat_config[idx], BMI2_VFRM_FATAL_ERR_STATUS); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API skips S4S frame in the FIFO data while getting - * step activity output. - */ -static int8_t move_if_s4s_frame(const uint8_t *frame_header, uint16_t *data_index, const struct bmi2_fifo_frame *fifo) -{ - /* Variable to define error */ - int8_t rslt = BMI2_OK; - - /* Variable to extract virtual header byte */ - uint8_t virtual_header_mode; - - /* Variable to define pay-load in words */ - uint8_t payload_word = 0; - - /* Variable to define pay-load in bytes */ - uint8_t payload_bytes = 0; - - /* Extract virtual header mode from the frame header */ - virtual_header_mode = BMI2_GET_BITS(*frame_header, BMI2_FIFO_VIRT_FRM_MODE); - - /* If the extracted header byte is a virtual header */ - if (virtual_header_mode == BMI2_FIFO_VIRT_FRM_MODE) - { - /* If frame header is not activity recognition header */ - if (*frame_header != 0xC8) - { - /* Extract pay-load in words from the header byte */ - payload_word = BMI2_GET_BITS(*frame_header, BMI2_FIFO_VIRT_PAYLOAD) + 1; - - /* Convert to bytes */ - payload_bytes = (uint8_t)(payload_word * 2); - - /* Move the data index by those pay-load bytes */ - rslt = move_next_frame(data_index, payload_bytes, fifo); - } - } - - return rslt; -} - -/*! - * @brief This internal API enables/disables compensation of the gain defined - * in the GAIN register. - */ -static int8_t enable_gyro_gain(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define register data */ - uint8_t reg_data = 0; - - rslt = bmi2_get_regs(BMI2_GYR_OFF_COMP_6_ADDR, ®_data, 1, dev); - if (rslt == BMI2_OK) - { - reg_data = BMI2_SET_BITS(reg_data, BMI2_GYR_GAIN_EN, enable); - rslt = bmi2_set_regs(BMI2_GYR_OFF_COMP_6_ADDR, ®_data, 1, dev); - } - - return rslt; -} - -/*! - * @brief This internal API is used to extract the output feature configuration - * details from the look-up table. - */ -static uint8_t extract_output_feat_config(struct bmi2_feature_config *feat_output, - uint8_t type, - const struct bmi2_dev *dev) -{ - /* Variable to define loop */ - uint8_t loop = 0; - - /* Variable to set flag */ - uint8_t feat_found = BMI2_FALSE; - - /* Search for the output feature from the output configuration array */ - while (loop < dev->out_sens) - { - if (dev->feat_output[loop].type == type) - { - *feat_output = dev->feat_output[loop]; - feat_found = BMI2_TRUE; - break; - } - - loop++; - } - - /* Return flag */ - return feat_found; -} - -/*! - * @brief This internal API is used to move the data index ahead of the - * current_frame_length parameter when unnecessary FIFO data appears while - * extracting the user specified data. - */ -static int8_t move_next_frame(uint16_t *data_index, uint8_t current_frame_length, const struct bmi2_fifo_frame *fifo) -{ - /* Variables to define error */ - int8_t rslt = BMI2_OK; - - /* Validate data index */ - if (((*data_index) + current_frame_length) > fifo->length) - { - /* Move the data index to the last byte */ - (*data_index) = fifo->length; - - /* FIFO is empty */ - rslt = BMI2_W_FIFO_EMPTY; - } - else - { - /* Move the data index to next frame */ - (*data_index) = (*data_index) + current_frame_length; - - /* More frames could be read */ - rslt = BMI2_W_PARTIAL_READ; - } - - return rslt; -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_hc.h b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_hc.h deleted file mode 100644 index 2a1b5bc07b..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_hc.h +++ /dev/null @@ -1,483 +0,0 @@ -/** -* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. -* -* BSD-3-Clause -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* 1. Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* -* 2. Redistributions in binary form must reproduce the above copyright -* notice, this list of conditions and the following disclaimer in the -* documentation and/or other materials provided with the distribution. -* -* 3. Neither the name of the copyright holder nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) -* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, -* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING -* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -* -* @file bmi270_hc.h -* @date 2020-11-04 -* @version v2.63.1 -* -*/ - -/** - * \ingroup bmi2xy - * \defgroup bmi270_hc BMI270_HC - * @brief Sensor driver for BMI270_HC sensor - */ - -#ifndef BMI270_HC_H_ -#define BMI270_HC_H_ - -/*! CPP guard */ -#ifdef __cplusplus -extern "C" { -#endif - -/***************************************************************************/ - -/*! Header files - ****************************************************************************/ -#include "bmi2.h" - -/***************************************************************************/ - -/*! Macro definitions - ****************************************************************************/ - -/*! @name BMI270_HC Chip identifier */ -#define BMI270_HC_CHIP_ID UINT8_C(0x24) - -/*! @name BMI270_HC feature input start addresses */ -#define BMI270_HC_CONFIG_ID_STRT_ADDR UINT8_C(0x06) -#define BMI270_HC_STEP_CNT_1_STRT_ADDR UINT8_C(0x00) -#define BMI270_HC_STEP_CNT_4_STRT_ADDR UINT8_C(0x02) -#define BMI270_HC_MAX_BURST_LEN_STRT_ADDR UINT8_C(0x08) -#define BMI270_HC_CRT_GYRO_SELF_TEST_STRT_ADDR UINT8_C(0x09) -#define BMI270_HC_ABORT_STRT_ADDR UINT8_C(0x09) -#define BMI270_HC_NVM_PROG_PREP_STRT_ADDR UINT8_C(0x0A) -#define BMI270_HC_ACT_RGN_SETT_STRT_ADDR UINT8_C(0x02) -#define BMI270_HC_ACT_RGN_STRT_ADDR UINT8_C(0x00) - -/*! @name BMI270_HC feature output start addresses */ -#define BMI270_HC_STEP_CNT_OUT_STRT_ADDR UINT8_C(0x00) -#define BMI270_HC_GYR_USER_GAIN_OUT_STRT_ADDR UINT8_C(0x04) -#define BMI270_HC_GYRO_CROSS_SENSE_STRT_ADDR UINT8_C(0x0C) -#define BMI270_HC_NVM_VFRM_OUT_STRT_ADDR UINT8_C(0x0E) - -/*! @name Defines maximum number of pages */ -#define BMI270_HC_MAX_PAGE_NUM UINT8_C(8) - -/*! @name Defines maximum number of feature input configurations */ -#define BMI270_HC_MAX_FEAT_IN UINT8_C(10) - -/*! @name Defines maximum number of feature outputs */ -#define BMI270_HC_MAX_FEAT_OUT UINT8_C(5) - -/*! @name Mask definitions for feature interrupt status bits */ -#define BMI270_HC_STEP_CNT_STATUS_MASK UINT8_C(0x01) - -/*! @name Mask definitions for feature interrupt mapping bits */ -#define BMI270_HC_INT_STEP_COUNTER_MASK UINT8_C(0x01) -#define BMI270_HC_INT_STEP_DETECTOR_MASK UINT8_C(0x01) - -/*! @name Defines maximum number of feature interrupts */ -#define BMI270_HC_MAX_INT_MAP UINT8_C(2) - -/***************************************************************************/ - -/*! BMI270_HC User Interface function prototypes - ****************************************************************************/ - -/** - * \ingroup bmi270_hc - * \defgroup bmi270_hcApiInit Initialization - * @brief Initialize the sensor and device structure - */ - -/*! - * \ingroup bmi270_hcApiInit - * \page bmi270_hc_api_bmi270_hc_init bmi270_hc_init - * \code - * int8_t bmi270_hc_init(struct bmi2_dev *dev); - * \endcode - * @details This API: - * 1) updates the device structure with address of the configuration file. - * 2) Initializes BMI270_HC sensor. - * 3) Writes the configuration file. - * 4) Updates the feature offset parameters in the device structure. - * 5) Updates the maximum number of pages, in the device structure. - * - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi270_hc_init(struct bmi2_dev *dev); - -/** - * \ingroup bmi270_hc - * \defgroup bmi270_hcApiSensor Feature Set - * @brief Enable / Disable features of the sensor - */ - -/*! - * \ingroup bmi270_hcApiSensor - * \page bmi270_hc_api_bmi270_hc_sensor_enable bmi270_hc_sensor_enable - * \code - * int8_t bmi270_hc_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); - * \endcode - * @details This API selects the sensors/features to be enabled. - * - * @param[in] sens_list : Pointer to select the sensor/feature. - * @param[in] n_sens : Number of sensors selected. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @note Sensors/features that can be enabled. - * - *@verbatim - * sens_list | Values - * ----------------------------|----------- - * BMI2_ACCEL | 0 - * BMI2_GYRO | 1 - * BMI2_AUX | 2 - * BMI2_STEP_DETECTOR | 6 - * BMI2_STEP_COUNTER | 7 - * BMI2_GYRO_GAIN_UPDATE | 9 - * BMI2_ACTIVITY_RECOGNITION | 34 - *@endverbatim - * - * @note : - * example uint8_t sens_list[2] = {BMI2_ACCEL, BMI2_GYRO}; - * uint8_t n_sens = 2; - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi270_hc_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); - -/*! - * \ingroup bmi270_hcApiSensor - * \page bmi270_hc_api_bmi270_hc_sensor_disable bmi270_hc_sensor_disable - * \code - * int8_t bmi270_hc_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); - * \endcode - * @details This API selects the sensors/features to be disabled. - * - * @param[in] sens_list : Pointer to select the sensor/feature. - * @param[in] n_sens : Number of sensors selected. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @note Sensors/features that can be disabled. - * - *@verbatim - * sens_list | Values - * ----------------------------|----------- - * BMI2_ACCEL | 0 - * BMI2_GYRO | 1 - * BMI2_AUX | 2 - * BMI2_STEP_DETECTOR | 6 - * BMI2_STEP_COUNTER | 7 - * BMI2_GYRO_GAIN_UPDATE | 9 - * BMI2_ACTIVITY_RECOGNITION | 34 - *@endverbatim - * - * @note : - * example uint8_t sens_list[2] = {BMI2_ACCEL, BMI2_GYRO}; - * uint8_t n_sens = 2; - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi270_hc_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); - -/** - * \ingroup bmi270_hc - * \defgroup bmi270_hcApiSensorC Sensor Configuration - * @brief Enable / Disable feature configuration of the sensor - */ - -/*! - * \ingroup bmi270_hcApiSensorC - * \page bmi270_hc_api_bmi270_hc_set_sensor_config bmi270_hc_set_sensor_config - * \code - * int8_t bmi270_hc_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); - * \endcode - * @details This API sets the sensor/feature configuration. - * - * @param[in] sens_cfg : Structure instance of bmi2_sens_config. - * @param[in] n_sens : Number of sensors selected. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @note Sensors/features that can be configured - * - *@verbatim - * sens_list | Values - * ----------------------------|----------- - * BMI2_STEP_DETECTOR | 6 - * BMI2_STEP_COUNTER | 7 - * BMI2_STEP_COUNTER_PARAMS | 28 - *@endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi270_hc_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); - -/*! - * \ingroup bmi270_hcApiSensorC - * \page bmi270_hc_api_bmi270_hc_get_sensor_config bmi270_hc_get_sensor_config - * \code - * int8_t bmi270_hc_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); - * \endcode - * @details This API gets the sensor/feature configuration. - * - * @param[in] sens_cfg : Structure instance of bmi2_sens_config. - * @param[in] n_sens : Number of sensors selected. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @note Sensors/features whose configurations can be read. - * - *@verbatim - * sens_list | Values - * ----------------------------|----------- - * BMI2_STEP_DETECTOR | 6 - * BMI2_STEP_COUNTER | 7 - * BMI2_STEP_COUNTER_PARAMS | 28 - *@endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi270_hc_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); - -/** - * \ingroup bmi270_hc - * \defgroup bmi270_hcApiSensorD Sensor Data - * @brief Get sensor data - */ - -/*! - * \ingroup bmi270_hcApiSensorD - * \page bmi270_hc_api_bmi270_hc_get_sensor_data bmi270_hc_get_sensor_data - * \code - * int8_t bmi270_hc_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev); - * \endcode - * @details This API gets the sensor/feature data for accelerometer, gyroscope, - * auxiliary sensor, step counter, high-g, gyroscope user-gain update, - * orientation, gyroscope cross sensitivity and error status for NVM and VFRM. - * - * @param[out] sensor_data : Structure instance of bmi2_sensor_data. - * @param[in] n_sens : Number of sensors selected. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @note Sensors/features whose data can be read - * - *@verbatim - * sens_list | Values - * ---------------------|----------- - * BMI2_STEP_COUNTER | 7 - * BMI2_NVM_STATUS | 38 - * BMI2_VFRM_STATUS | 39 - *@endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi270_hc_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev); - -/** - * \ingroup bmi270_hc - * \defgroup bmi270_hcApiARecoghc Activity recognition settings - * @brief Set / Get activity recognition settings of bmi270hc sensor - */ - -/*! - * \ingroup bmi270_hcApiARecoghc - * \page bmi270_hc_api_bmi270_hc_get_act_recg_sett bmi270_hc_get_act_recg_sett - * \code - * int8_t bmi270_hc_get_act_recg_sett(struct bmi2_hc_act_recg_sett *sett, struct bmi2_dev *dev); - * \endcode - * @details This api is used for retrieving the following activity recognition settings currently set. - * - * @param[in] sett : Structure instance of bmi2_hc_act_recg_sett. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi270_hc_get_act_recg_sett(struct bmi2_hc_act_recg_sett *sett, struct bmi2_dev *dev); - -/*! - * \ingroup bmi270_hcApiARecoghc - * \page bmi270_hc_api_bmi270_hc_set_act_recg_sett bmi270_hc_set_act_recg_sett - * \code - * int8_t bmi270_hc_set_act_recg_sett(const struct bmi2_hc_act_recg_sett *sett, struct bmi2_dev *dev); - * \endcode - * @details This api is used for setting the following activity recognition settings - * - * @param[in] sett : Structure instance of bmi2_hc_act_recg_sett. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi270_hc_set_act_recg_sett(const struct bmi2_hc_act_recg_sett *sett, struct bmi2_dev *dev); - -/** - * \ingroup bmi270_hc - * \defgroup bmi270_hcApiactOut Activity Output - * @brief Activity output operations of the sensor - */ - -/*! - * \ingroup bmi270_hcApiactOut - * \page bmi270_hc_api_bmi270_hc_get_act_recog_output bmi270_hc_get_act_recog_output - * \code - * int8_t bmi270_hc_get_act_recog_output(struct bmi2_act_recog_output *act_recog, - * uint16_t *act_frm_len, - * struct bmi2_fifo_frame *fifo, - * const struct bmi2_dev *dev); - * - * \endcode - * @details This internal API is used to parse the activity output from the - * FIFO in header mode. - * - * @param[out] act_recog : Pointer to buffer where the parsed activity data - * bytes are stored. - * @param[in] act_frm_len : Number of activity frames parsed. - * @param[in] fifo : Structure instance of bmi2_fifo_frame. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @verbatim - * ---------------------------------------------------------------------------- - * bmi2_act_rec_output | - * Structure parameters | Description - *--------------------------|-------------------------------------------------- - * time_stamp | time-stamp (expressed in 50Hz ticks) - * -------------------------|--------------------------------------------------- - * type | Type of activity - * -------------------------|--------------------------------------------------- - * stat | Activity status - * -------------------------|--------------------------------------------------- - * @endverbatim - * - *@verbatim - * type | Activities - *----------|--------------------- - * 0 | UNKNOWN - * 1 | STILL - * 2 | WALK - * 3 | RUN - * 4 | BIKE - * 5 | VEHICLE - * 6 | TILTED - *@endverbatim - * - * - *@verbatim - * stat | Activity status - *----------|--------------------- - * 1 | START - * 2 | END - *@endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi270_hc_get_act_recog_output(struct bmi2_act_recog_output *act_recog, - uint16_t *act_frm_len, - struct bmi2_fifo_frame *fifo, - const struct bmi2_dev *dev); - -/** - * \ingroup bmi270_hc - * \defgroup bmi270_hcApiGyroUG Gyro User Gain - * @brief Set / Get Gyro User Gain of the sensor - */ - -/*! - * \ingroup bmi270_hcApiGyroUG - * \page bmi270_hc_api_bmi270_hc_update_gyro_user_gain bmi270_hc_update_gyro_user_gain - * \code - * int8_t bmi270_hc_update_gyro_user_gain(const struct bmi2_gyro_user_gain_config *user_gain, struct bmi2_dev *dev); - * \endcode - * @details This API updates the gyroscope user-gain. - * - * @param[in] user_gain : Structure that stores user-gain configurations. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi270_hc_update_gyro_user_gain(const struct bmi2_gyro_user_gain_config *user_gain, struct bmi2_dev *dev); - -/*! - * \ingroup bmi270_hcApiGyroUG - * \page bmi270_hc_api_bmi270_hc_read_gyro_user_gain bmi270_hc_read_gyro_user_gain - * \code - * int8_t bmi270_hc_read_gyro_user_gain(struct bmi2_gyro_user_gain_data *gyr_usr_gain, const struct bmi2_dev *dev); - * \endcode - * @details This API reads the compensated gyroscope user-gain values. - * - * @param[out] gyr_usr_gain : Structure that stores gain values. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi270_hc_read_gyro_user_gain(struct bmi2_gyro_user_gain_data *gyr_usr_gain, struct bmi2_dev *dev); - -/*! - * \ingroup bmi270_hcApiInt - * \page bmi270_hc_api_bmi270_hc_map_feat_int bmi270_hc_map_feat_int - * \code - * int8_t bmi270_hc_map_feat_int(const struct bmi2_sens_int_config *sens_int, uint8_t n_sens, struct bmi2_dev *dev) - * \endcode - * @details This API maps/unmaps feature interrupts to that of interrupt pins. - * - * @param[in] sens_int : Structure instance of bmi2_sens_int_config. - * @param[in] n_sens : Number of interrupts to be mapped. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi270_hc_map_feat_int(const struct bmi2_sens_int_config *sens_int, uint8_t n_sens, struct bmi2_dev *dev); - -/******************************************************************************/ -/*! @name C++ Guard Macros */ -/******************************************************************************/ -#ifdef __cplusplus -} -#endif /* End of CPP guard */ - -#endif /* BMI270_HC_H_ */ diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_wh.c b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_wh.c deleted file mode 100644 index eda23cc022..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_wh.c +++ /dev/null @@ -1,3431 +0,0 @@ -/** -* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. -* -* BSD-3-Clause -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* 1. Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* -* 2. Redistributions in binary form must reproduce the above copyright -* notice, this list of conditions and the following disclaimer in the -* documentation and/or other materials provided with the distribution. -* -* 3. Neither the name of the copyright holder nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) -* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, -* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING -* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -* -* @file bmi270_wh.c -* @date 2020-11-04 -* @version v2.63.1 -* -*/ - -/***************************************************************************/ - -/*! Header files - ****************************************************************************/ -#include "bmi270_wh.h" - -/***************************************************************************/ - -/*! Global Variable - ****************************************************************************/ - -/*! @name Global array that stores the configuration file of BMI270_WH */ -const uint8_t bmi270_wh_config_file[] = { - 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0xf8, 0x02, 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0xe1, 0x00, 0x80, 0x2e, 0xc0, - 0x02, 0x80, 0x2e, 0xe2, 0x00, 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x2c, 0x02, 0x80, 0x2e, 0xe4, 0x07, 0x59, 0xf5, - 0x10, 0x30, 0x21, 0x2e, 0x6a, 0xf5, 0x80, 0x2e, 0xdd, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3e, 0x31, 0x01, 0x00, 0x22, - 0x00, 0x80, 0x00, 0xfa, 0x07, 0xff, 0x0f, 0xd1, 0x00, 0x65, 0x9d, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, - 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, - 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, - 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, - 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, - 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, - 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, - 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, - 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, - 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, - 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0xfd, 0x2d, 0x7b, 0xaf, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x03, 0x00, 0x18, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x01, 0x12, - 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x1a, 0x24, 0x22, 0x00, 0x80, 0x2e, 0x8e, 0x01, 0xc8, 0x2e, 0xc8, 0x2e, 0xbb, 0x52, - 0x00, 0x2e, 0x60, 0x40, 0x41, 0x40, 0x0d, 0xbc, 0x98, 0xbc, 0xc0, 0x2e, 0x01, 0x0a, 0x0f, 0xb8, 0x43, 0x86, 0x25, - 0x40, 0x04, 0x40, 0xd8, 0xbe, 0x2c, 0x0b, 0x22, 0x11, 0x54, 0x42, 0x03, 0x80, 0x4b, 0x0e, 0xf6, 0x2f, 0xb8, 0x2e, - 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0xfd, 0x2d, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3e, 0x31, 0x00, 0x00, - 0x88, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0xe0, 0xaa, 0x38, 0x05, 0xe0, 0x90, 0x30, 0x04, 0x00, 0xee, - 0x06, 0xf2, 0x05, 0x80, 0x80, 0x16, 0xf1, 0x02, 0x02, 0x2d, 0x01, 0xd4, 0x7b, 0x3b, 0x01, 0xdb, 0x7a, 0x04, 0x00, - 0x3f, 0x7b, 0xcd, 0x6c, 0xc3, 0x04, 0x85, 0x09, 0xc3, 0x04, 0xec, 0xe6, 0x0c, 0x46, 0x01, 0x00, 0x27, 0x00, 0x19, - 0x00, 0x96, 0x00, 0xa0, 0x00, 0x01, 0x00, 0x0c, 0x00, 0xf0, 0x3c, 0x00, 0x01, 0x01, 0x00, 0x03, 0x00, 0x01, 0x00, - 0x0e, 0x00, 0x00, 0x00, 0x32, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x92, 0x00, 0x98, 0xf1, 0xaf, 0x00, 0xae, 0x00, 0x1e, - 0xf2, 0x19, 0xf4, 0x66, 0xf5, 0x00, 0x04, 0xff, 0x00, 0x64, 0xf5, 0x96, 0x00, 0xaa, 0x00, 0x86, 0x00, 0x99, 0x00, - 0x88, 0x00, 0x8a, 0x00, 0xff, 0x3f, 0xff, 0xfb, 0x31, 0x01, 0x00, 0x38, 0x00, 0x30, 0xfd, 0xf5, 0xb2, 0x00, 0xff, - 0x03, 0x00, 0xfc, 0xf0, 0x3f, 0x0b, 0x01, 0x0e, 0x01, 0x10, 0x01, 0xb9, 0x00, 0x2d, 0xf5, 0xca, 0xf5, 0xb0, 0x00, - 0x20, 0xf2, 0xb2, 0x00, 0xff, 0x1f, 0x00, 0x40, 0x80, 0x2e, 0xc0, 0x08, 0x1d, 0x6c, 0xad, 0x00, 0x59, 0x01, 0x31, - 0xd1, 0xff, 0x7f, 0x00, 0x08, 0xee, 0x7a, 0xe4, 0x0c, 0x12, 0x02, 0xb3, 0x00, 0xb4, 0x04, 0x62, 0x03, 0xc0, 0x02, - 0x1f, 0xf8, 0xe1, 0x07, 0xd3, 0x07, 0xd7, 0x00, 0xd3, 0x00, 0xb9, 0x00, 0xc3, 0x00, 0xc5, 0x00, 0xbd, 0x00, 0xbc, - 0x00, 0xbf, 0x00, 0xdd, 0x00, 0x95, 0x00, 0xc2, 0x00, 0xd6, 0x00, 0xf0, 0x00, 0x00, 0xff, 0x00, 0x80, 0x30, 0x50, - 0x98, 0x2e, 0xc3, 0xb0, 0x02, 0x30, 0x00, 0x30, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0x01, 0x30, 0x00, 0x2e, 0x41, - 0x82, 0x48, 0xa2, 0xfb, 0x2f, 0x03, 0x2e, 0x95, 0x00, 0x40, 0xb2, 0x1a, 0x2f, 0x05, 0x2e, 0x0a, 0x01, 0x91, 0x52, - 0x98, 0x2e, 0xc7, 0xc1, 0x05, 0x2e, 0x18, 0x00, 0x80, 0xb2, 0x02, 0x30, 0x10, 0x2f, 0xf0, 0x7f, 0x00, 0x2e, 0x91, - 0x50, 0x98, 0x2e, 0x4d, 0xc3, 0x91, 0x50, 0x98, 0x2e, 0x5a, 0xc7, 0x98, 0x2e, 0xbc, 0x03, 0x98, 0x2e, 0x00, 0xb0, - 0x10, 0x30, 0x21, 0x2e, 0x19, 0x00, 0xf0, 0x6f, 0x02, 0x30, 0x03, 0x2e, 0x85, 0x00, 0x40, 0xb2, 0x40, 0x2f, 0x03, - 0x2e, 0x85, 0x00, 0x03, 0x31, 0x4b, 0x08, 0x40, 0xb2, 0x07, 0x2f, 0xf0, 0x7f, 0x98, 0x2e, 0x47, 0xcb, 0x10, 0x30, - 0x21, 0x2e, 0x19, 0x00, 0xf0, 0x6f, 0x02, 0x30, 0x03, 0x2e, 0x85, 0x00, 0x43, 0x30, 0x4b, 0x08, 0x40, 0xb2, 0x2a, - 0x2f, 0x03, 0x2e, 0x90, 0x00, 0xf0, 0x7f, 0x40, 0xb2, 0x0e, 0x2f, 0x41, 0x90, 0x20, 0x2f, 0x05, 0x2e, 0x3f, 0x01, - 0x07, 0x2e, 0x91, 0x00, 0xa1, 0x32, 0x95, 0x58, 0x98, 0x2e, 0x97, 0xb3, 0x02, 0x30, 0x25, 0x2e, 0x90, 0x00, 0x15, - 0x2c, 0xf0, 0x6f, 0x93, 0x58, 0xa3, 0x32, 0x10, 0x41, 0x11, 0x41, 0x18, 0xb9, 0x04, 0x41, 0x98, 0xbc, 0x41, 0x0a, - 0x94, 0x0a, 0x98, 0x2e, 0xf1, 0x03, 0x12, 0x30, 0x21, 0x2e, 0x91, 0x00, 0x21, 0x2e, 0xaf, 0x00, 0x25, 0x2e, 0x90, - 0x00, 0xf0, 0x6f, 0x02, 0x30, 0x11, 0x30, 0x23, 0x2e, 0x19, 0x00, 0x25, 0x2e, 0x85, 0x00, 0x03, 0x2e, 0x19, 0x00, - 0x40, 0xb2, 0x22, 0x2f, 0xf0, 0x7f, 0x98, 0x2e, 0xf5, 0xcb, 0x97, 0x54, 0xbc, 0x84, 0x21, 0x2e, 0xae, 0x00, 0x81, - 0x40, 0x82, 0x86, 0x99, 0x54, 0x18, 0xb8, 0xc3, 0x40, 0x91, 0x42, 0x90, 0x42, 0x33, 0xbc, 0x90, 0x42, 0xe2, 0x7f, - 0xf0, 0x31, 0x82, 0x40, 0x10, 0x08, 0xf2, 0x6f, 0x25, 0xbd, 0x02, 0x0a, 0xd0, 0x7f, 0x98, 0x2e, 0xa8, 0xcf, 0x06, - 0xbc, 0xd2, 0x6f, 0xe1, 0x6f, 0x10, 0x0a, 0x40, 0x42, 0x98, 0x2e, 0x2c, 0x03, 0xf0, 0x6f, 0x02, 0x30, 0x25, 0x2e, - 0x19, 0x00, 0x25, 0x2e, 0x95, 0x00, 0x80, 0x2e, 0x93, 0x01, 0x90, 0x50, 0xf7, 0x7f, 0xe6, 0x7f, 0xd5, 0x7f, 0xc4, - 0x7f, 0xb3, 0x7f, 0xa1, 0x7f, 0x90, 0x7f, 0x82, 0x7f, 0x7b, 0x7f, 0x98, 0x2e, 0xe3, 0x00, 0x00, 0xb2, 0x65, 0x2f, - 0x05, 0x2e, 0x31, 0x01, 0x01, 0x2e, 0x11, 0x01, 0x03, 0x2e, 0x0f, 0x01, 0x8f, 0xb9, 0x23, 0xbe, 0x9f, 0xb8, 0xcb, - 0x0a, 0x24, 0xbc, 0x4f, 0xba, 0x03, 0x2e, 0x12, 0x01, 0x0f, 0xb8, 0x22, 0xbd, 0xdc, 0x0a, 0x2f, 0xb9, 0x9b, 0xbc, - 0x18, 0x0a, 0x9f, 0xb8, 0x82, 0x0a, 0x91, 0x0a, 0x25, 0x2e, 0x18, 0x00, 0x05, 0x2e, 0xc1, 0xf5, 0x2e, 0xbd, 0x2e, - 0xb9, 0x01, 0x2e, 0x18, 0x00, 0x31, 0x30, 0x8a, 0x04, 0x00, 0x90, 0x03, 0x2f, 0x01, 0x2e, 0x83, 0x00, 0x00, 0xb2, - 0x19, 0x2f, 0x9b, 0x50, 0x91, 0x52, 0x98, 0x2e, 0xec, 0x00, 0x05, 0x2e, 0x82, 0x00, 0x25, 0x2e, 0x95, 0x00, 0x05, - 0x2e, 0x82, 0x00, 0x80, 0x90, 0x02, 0x2f, 0x12, 0x30, 0x25, 0x2e, 0x82, 0x00, 0x01, 0x2e, 0x83, 0x00, 0x00, 0xb2, - 0x10, 0x30, 0x05, 0x2e, 0x18, 0x00, 0x01, 0x2f, 0x21, 0x2e, 0x18, 0x00, 0x25, 0x2e, 0x83, 0x00, 0x05, 0x2e, 0x18, - 0x00, 0x80, 0xb2, 0x20, 0x2f, 0x01, 0x2e, 0xc0, 0xf5, 0xf2, 0x30, 0x02, 0x08, 0x07, 0xaa, 0x73, 0x30, 0x03, 0x2e, - 0x84, 0x00, 0x18, 0x22, 0x41, 0x1a, 0x05, 0x2f, 0x03, 0x2e, 0x66, 0xf5, 0x9f, 0xbc, 0x9f, 0xb8, 0x40, 0x90, 0x0c, - 0x2f, 0x9d, 0x52, 0x03, 0x30, 0x53, 0x42, 0x2b, 0x30, 0x90, 0x04, 0x5b, 0x42, 0x21, 0x2e, 0x84, 0x00, 0x24, 0xbd, - 0x7e, 0x80, 0x81, 0x84, 0x43, 0x42, 0x02, 0x42, 0x02, 0x32, 0x25, 0x2e, 0x62, 0xf5, 0x9f, 0x52, 0x05, 0x2e, 0x0a, - 0x01, 0x91, 0x08, 0x00, 0x31, 0x21, 0x2e, 0x60, 0xf5, 0x80, 0xb2, 0x0b, 0x2f, 0xf2, 0x3e, 0x01, 0x2e, 0xca, 0xf5, - 0x82, 0x08, 0x25, 0x2e, 0xca, 0xf5, 0x05, 0x2e, 0x59, 0xf5, 0xe0, 0x3f, 0x90, 0x08, 0x25, 0x2e, 0x59, 0xf5, 0xa1, - 0x6f, 0xb3, 0x6f, 0xc4, 0x6f, 0xd5, 0x6f, 0xe6, 0x6f, 0xf7, 0x6f, 0x7b, 0x6f, 0x82, 0x6f, 0x90, 0x6f, 0x70, 0x5f, - 0xc8, 0x2e, 0xa0, 0x50, 0x80, 0x7f, 0xe7, 0x7f, 0xd5, 0x7f, 0xc4, 0x7f, 0xb3, 0x7f, 0xa2, 0x7f, 0x91, 0x7f, 0xf6, - 0x7f, 0x7b, 0x7f, 0x00, 0x2e, 0x01, 0x2e, 0x60, 0xf5, 0x60, 0x7f, 0x98, 0x2e, 0xe3, 0x00, 0x62, 0x6f, 0x01, 0x32, - 0x91, 0x08, 0x80, 0xb2, 0x0d, 0x2f, 0x00, 0xb2, 0x03, 0x2f, 0x05, 0x2e, 0x18, 0x00, 0x80, 0x90, 0x05, 0x2f, 0xa3, - 0x56, 0x02, 0x30, 0xc1, 0x42, 0xc2, 0x86, 0x00, 0x2e, 0xc2, 0x42, 0x23, 0x2e, 0x60, 0xf5, 0x00, 0x90, 0x00, 0x30, - 0x06, 0x2f, 0x21, 0x2e, 0x82, 0x00, 0xa1, 0x50, 0x21, 0x2e, 0x5a, 0xf2, 0x98, 0x2e, 0x3d, 0x03, 0xf6, 0x6f, 0x80, - 0x6f, 0x91, 0x6f, 0xa2, 0x6f, 0xb3, 0x6f, 0xc4, 0x6f, 0xd5, 0x6f, 0xe7, 0x6f, 0x7b, 0x6f, 0x60, 0x5f, 0xc8, 0x2e, - 0xc0, 0x50, 0xe7, 0x7f, 0xf6, 0x7f, 0x26, 0x30, 0x0f, 0x2e, 0x61, 0xf5, 0x2f, 0x2e, 0x85, 0x00, 0x0f, 0x2e, 0x85, - 0x00, 0xbe, 0x09, 0xd5, 0x7f, 0xc4, 0x7f, 0xb3, 0x7f, 0xa2, 0x7f, 0x90, 0x7f, 0x7b, 0x7f, 0x80, 0xb3, 0x81, 0x7f, - 0x11, 0x2f, 0xa5, 0x50, 0x1a, 0x25, 0x12, 0x40, 0x42, 0x7f, 0x74, 0x82, 0x12, 0x40, 0x52, 0x7f, 0x00, 0x2e, 0x00, - 0x40, 0x60, 0x7f, 0x98, 0x2e, 0x6a, 0xd6, 0x03, 0x2e, 0x61, 0xf7, 0x00, 0x31, 0x48, 0x0a, 0x23, 0x2e, 0x61, 0xf7, - 0xe1, 0x31, 0x23, 0x2e, 0x61, 0xf5, 0xf6, 0x6f, 0xe7, 0x6f, 0x81, 0x6f, 0x90, 0x6f, 0xa2, 0x6f, 0xb3, 0x6f, 0xc4, - 0x6f, 0xd5, 0x6f, 0x7b, 0x6f, 0x40, 0x5f, 0xc8, 0x2e, 0xa7, 0x50, 0x10, 0x50, 0xbd, 0x52, 0x05, 0x2e, 0x8d, 0x00, - 0xfb, 0x7f, 0x00, 0x2e, 0x13, 0x40, 0x93, 0x42, 0x41, 0x0e, 0xfb, 0x2f, 0x98, 0x2e, 0x91, 0x03, 0xfb, 0x6f, 0xf0, - 0x5f, 0x80, 0x2e, 0x87, 0xcf, 0x10, 0x50, 0xfb, 0x7f, 0x98, 0x2e, 0x56, 0xc7, 0x98, 0x2e, 0x49, 0xc3, 0x11, 0x30, - 0x30, 0x30, 0xfb, 0x6f, 0xf0, 0x5f, 0x23, 0x2e, 0x8f, 0x00, 0x21, 0x2e, 0x86, 0x00, 0x23, 0x2e, 0x19, 0x00, 0x21, - 0x2e, 0xac, 0x00, 0xb8, 0x2e, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x9a, 0x01, - 0x34, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x03, 0x2e, 0x8d, 0x00, 0x16, 0xb8, 0x02, 0x34, 0x4a, 0x0c, 0x21, 0x2e, 0x2d, 0xf5, 0xc0, 0x2e, 0x23, - 0x2e, 0x8d, 0x00, 0x40, 0x50, 0xf1, 0x7f, 0x0a, 0x25, 0x3c, 0x86, 0xeb, 0x7f, 0x41, 0x33, 0x22, 0x30, 0x98, 0x2e, - 0xc2, 0xc4, 0xd3, 0x6f, 0xf4, 0x30, 0xdc, 0x09, 0xc1, 0x58, 0xc2, 0x6f, 0x94, 0x09, 0xc3, 0x58, 0x6a, 0xbb, 0xdc, - 0x08, 0xb4, 0xb9, 0xb1, 0xbd, 0xbf, 0x5a, 0x95, 0x08, 0x21, 0xbd, 0xf6, 0xbf, 0x77, 0x0b, 0x51, 0xbe, 0xf1, 0x6f, - 0xeb, 0x6f, 0x52, 0x42, 0x54, 0x42, 0xc0, 0x2e, 0x43, 0x42, 0xc0, 0x5f, 0x03, 0x2e, 0x12, 0x01, 0x00, 0x31, 0x08, - 0x08, 0xf2, 0x30, 0x20, 0x50, 0x4a, 0x08, 0xe1, 0x7f, 0x00, 0xb2, 0xfb, 0x7f, 0x01, 0x30, 0x15, 0x2f, 0x01, 0x2e, - 0x8f, 0x00, 0x01, 0x90, 0x03, 0x2f, 0x23, 0x2e, 0x8f, 0x00, 0x98, 0x2e, 0xe7, 0x03, 0x98, 0x2e, 0xa8, 0xcf, 0x11, - 0x30, 0x41, 0x08, 0x23, 0x2e, 0xd5, 0x00, 0x98, 0x2e, 0x41, 0xb1, 0x10, 0x25, 0xfb, 0x6f, 0xe0, 0x6f, 0xe0, 0x5f, - 0x80, 0x2e, 0x95, 0xcf, 0xe0, 0x6f, 0x98, 0x2e, 0x95, 0xcf, 0x11, 0x30, 0x23, 0x2e, 0x8f, 0x00, 0xfb, 0x6f, 0xe0, - 0x5f, 0xb8, 0x2e, 0xd5, 0x50, 0x01, 0x30, 0x0f, 0x55, 0x11, 0x42, 0x42, 0x0e, 0xfc, 0x2f, 0x10, 0x30, 0xc0, 0x2e, - 0x21, 0x2e, 0xbc, 0x00, 0x40, 0x50, 0x0a, 0x25, 0x3c, 0x80, 0xfb, 0x7f, 0x01, 0x42, 0xd2, 0x7f, 0xe3, 0x7f, 0x32, - 0x30, 0x10, 0x25, 0x98, 0x2e, 0x7a, 0xc1, 0xfb, 0x6f, 0xc0, 0x5f, 0xb8, 0x2e, 0x44, 0x47, 0xb5, 0x50, 0xf0, 0x50, - 0x12, 0x40, 0x06, 0x40, 0x1a, 0x25, 0x6c, 0xbe, 0x76, 0x8a, 0x74, 0x80, 0xfb, 0x7f, 0x68, 0xbf, 0xb7, 0x56, 0x0b, - 0x30, 0xd3, 0x08, 0x4c, 0xba, 0x6c, 0xbb, 0x5b, 0x7f, 0x4b, 0x43, 0x0b, 0x42, 0xc0, 0xb2, 0xc6, 0x7f, 0xb4, 0x7f, - 0xd0, 0x7f, 0xe5, 0x7f, 0xa3, 0x7f, 0x90, 0x2e, 0xaf, 0xb0, 0x01, 0x2e, 0x8c, 0x00, 0x00, 0xb2, 0x0b, 0x2f, 0xab, - 0x52, 0x01, 0x2e, 0x87, 0x00, 0x92, 0x7f, 0x98, 0x2e, 0xbb, 0xcc, 0x01, 0x30, 0x23, 0x2e, 0x8c, 0x00, 0x92, 0x6f, - 0xa3, 0x6f, 0x1a, 0x25, 0x26, 0xbc, 0x86, 0xba, 0x25, 0xbc, 0x0f, 0xb8, 0x54, 0xb1, 0x00, 0xb2, 0x96, 0x7f, 0x0c, - 0x2f, 0xad, 0x50, 0xaf, 0x54, 0x0b, 0x30, 0x0b, 0x2e, 0x31, 0x01, 0xb3, 0x58, 0x1b, 0x42, 0x9b, 0x42, 0x6c, 0x09, - 0x0b, 0x42, 0x2b, 0x2e, 0x31, 0x01, 0x8b, 0x42, 0x71, 0x84, 0xb9, 0x50, 0x58, 0x09, 0xb1, 0x52, 0x91, 0x50, 0x82, - 0x7f, 0x75, 0x7f, 0x98, 0x2e, 0xc2, 0xc0, 0x01, 0x2e, 0x87, 0x00, 0xe5, 0x6f, 0xd4, 0x6f, 0x73, 0x6f, 0x82, 0x6f, - 0xab, 0x52, 0xa9, 0x5c, 0x98, 0x2e, 0x06, 0xcd, 0x65, 0x6f, 0xa0, 0x6f, 0xad, 0x52, 0x40, 0xb3, 0x04, 0xbd, 0x53, - 0x40, 0xaf, 0xba, 0x44, 0x40, 0xe1, 0x7f, 0x02, 0x30, 0x06, 0x2f, 0x40, 0xb3, 0x02, 0x30, 0x03, 0x2f, 0xaf, 0x5c, - 0x12, 0x30, 0x93, 0x43, 0x84, 0x43, 0x03, 0xbf, 0x6f, 0xbb, 0x80, 0xb3, 0x20, 0x2f, 0x46, 0x6f, 0xde, 0x00, 0x56, - 0x6f, 0x26, 0x03, 0x44, 0x42, 0x40, 0x91, 0x27, 0x2e, 0x88, 0x00, 0xaf, 0x52, 0x14, 0x2f, 0xaf, 0x5c, 0x00, 0x2e, - 0x95, 0x41, 0x86, 0x41, 0x5d, 0x05, 0xa6, 0x07, 0x80, 0xab, 0x04, 0x2f, 0x80, 0x91, 0x0a, 0x2f, 0x96, 0x6f, 0x75, - 0x0f, 0x07, 0x2f, 0x95, 0x6f, 0x40, 0xb3, 0x04, 0x2f, 0x53, 0x42, 0x44, 0x42, 0x12, 0x30, 0x04, 0x2c, 0x11, 0x30, - 0x02, 0x2c, 0x11, 0x30, 0x11, 0x30, 0x02, 0xbc, 0x0f, 0xb8, 0xd2, 0x7f, 0x00, 0x90, 0x06, 0x2f, 0x31, 0x30, 0x23, - 0x2e, 0xac, 0x00, 0x23, 0x2e, 0x86, 0x00, 0x0b, 0x2c, 0x01, 0x30, 0x01, 0x2e, 0x86, 0x00, 0x05, 0x2e, 0xac, 0x00, - 0x10, 0x1a, 0x02, 0x2f, 0x21, 0x2e, 0xac, 0x00, 0x01, 0x2d, 0x01, 0x30, 0xc0, 0x6f, 0x98, 0x2e, 0x95, 0xcf, 0xd1, - 0x6f, 0xb0, 0x6f, 0x98, 0x2e, 0x95, 0xcf, 0xe2, 0x6f, 0xa7, 0x52, 0x01, 0x2e, 0x88, 0x00, 0x82, 0x40, 0x50, 0x42, - 0x11, 0x2c, 0x42, 0x42, 0x10, 0x30, 0x31, 0x30, 0x21, 0x2e, 0x8c, 0x00, 0x23, 0x2e, 0xac, 0x00, 0x23, 0x2e, 0x86, - 0x00, 0xc0, 0x6f, 0x01, 0x30, 0x98, 0x2e, 0x95, 0xcf, 0xb0, 0x6f, 0x01, 0x30, 0x98, 0x2e, 0x95, 0xcf, 0x00, 0x2e, - 0xfb, 0x6f, 0x10, 0x5f, 0xb8, 0x2e, 0x50, 0x50, 0xcd, 0x50, 0x51, 0x30, 0x11, 0x42, 0xfb, 0x7f, 0x7b, 0x30, 0x0b, - 0x42, 0x11, 0x30, 0x02, 0x80, 0x23, 0x33, 0x01, 0x42, 0x03, 0x00, 0x07, 0x2e, 0x80, 0x03, 0x05, 0x2e, 0x8d, 0x00, - 0xa5, 0x52, 0xe2, 0x7f, 0xd3, 0x7f, 0xc0, 0x7f, 0x98, 0x2e, 0x9b, 0x03, 0xd1, 0x6f, 0x08, 0x0a, 0x1a, 0x25, 0x7b, - 0x86, 0xd0, 0x7f, 0x01, 0x33, 0x12, 0x30, 0x98, 0x2e, 0xc2, 0xc4, 0xd1, 0x6f, 0x08, 0x0a, 0x00, 0xb2, 0x0d, 0x2f, - 0xe3, 0x6f, 0x01, 0x2e, 0x80, 0x03, 0x51, 0x30, 0xc7, 0x86, 0x23, 0x2e, 0x21, 0xf2, 0x08, 0xbc, 0xc0, 0x42, 0x98, - 0x2e, 0x91, 0x03, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0xb0, 0x6f, 0x0b, 0xb8, 0x03, 0x2e, 0x1b, 0x00, 0x08, 0x1a, - 0xb0, 0x7f, 0x70, 0x30, 0x04, 0x2f, 0x21, 0x2e, 0x21, 0xf2, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0x98, 0x2e, 0x6d, - 0xc0, 0x98, 0x2e, 0x5d, 0xc0, 0xc5, 0x50, 0x98, 0x2e, 0x44, 0xcb, 0xc7, 0x50, 0x98, 0x2e, 0x46, 0xc3, 0xc9, 0x50, - 0x98, 0x2e, 0x53, 0xc7, 0x20, 0x26, 0xc0, 0x6f, 0x02, 0x31, 0x12, 0x42, 0x6b, 0x31, 0x0b, 0x42, 0x37, 0x80, 0x0b, - 0x30, 0x0b, 0x42, 0xf3, 0x37, 0xcf, 0x52, 0xd3, 0x50, 0x44, 0x40, 0xa2, 0x0a, 0x42, 0x42, 0x8b, 0x31, 0x09, 0x2e, - 0x5e, 0xf7, 0xd1, 0x54, 0xe3, 0x08, 0x83, 0x42, 0x1b, 0x42, 0x23, 0x33, 0x4b, 0x00, 0xbc, 0x84, 0x0b, 0x40, 0x33, - 0x30, 0x83, 0x42, 0x0b, 0x42, 0xe0, 0x7f, 0xd1, 0x7f, 0x98, 0x2e, 0x2c, 0x03, 0xd1, 0x6f, 0x80, 0x30, 0x40, 0x42, - 0x03, 0x30, 0xe0, 0x6f, 0xcb, 0x54, 0x04, 0x30, 0x00, 0x2e, 0x00, 0x2e, 0x01, 0x89, 0x62, 0x0e, 0xfa, 0x2f, 0x43, - 0x42, 0x11, 0x30, 0xfb, 0x6f, 0xc0, 0x2e, 0x01, 0x42, 0xb0, 0x5f, 0x50, 0x51, 0x91, 0x54, 0xeb, 0x7f, 0x11, 0x30, - 0x0a, 0x25, 0xff, 0x56, 0x11, 0x5d, 0x95, 0x40, 0xc4, 0x40, 0x25, 0x01, 0xd4, 0x42, 0x4d, 0x17, 0xc4, 0x40, 0x65, - 0x03, 0xd5, 0x42, 0x56, 0x0e, 0xf5, 0x2f, 0x15, 0x57, 0x02, 0x30, 0xc6, 0x40, 0xb1, 0x29, 0xe6, 0x42, 0x00, 0x2e, - 0xc3, 0x40, 0xc0, 0xb2, 0x0a, 0x23, 0x4c, 0x14, 0x71, 0x0e, 0xd4, 0x7f, 0x90, 0x2e, 0x93, 0xb3, 0x03, 0x31, 0xe1, - 0x52, 0xe3, 0x54, 0x5c, 0x05, 0x8a, 0x28, 0x2b, 0x82, 0x03, 0x57, 0xdd, 0x5e, 0x2e, 0x80, 0xde, 0x8c, 0x30, 0x89, - 0xff, 0x29, 0x96, 0x7f, 0x60, 0x7f, 0xc5, 0x7f, 0xb3, 0x7f, 0xa4, 0x7f, 0x82, 0x7f, 0x77, 0x7f, 0x51, 0x7f, 0x00, - 0x2e, 0x90, 0x41, 0x92, 0x41, 0xd3, 0x6f, 0xc0, 0xb2, 0x46, 0x7f, 0x31, 0x7f, 0x08, 0x2f, 0xd0, 0xa0, 0x02, 0x2f, - 0xa0, 0x6f, 0x05, 0x2c, 0x10, 0x10, 0xc6, 0x6f, 0x96, 0x14, 0x03, 0x12, 0x02, 0x0a, 0x05, 0x2e, 0xd3, 0x00, 0x80, - 0x90, 0xd7, 0x54, 0x40, 0x42, 0x4a, 0x2f, 0x41, 0x40, 0x98, 0x2e, 0xd9, 0xc0, 0xdb, 0x54, 0xd9, 0x52, 0x20, 0x7f, - 0x98, 0x2e, 0xfe, 0xc9, 0x72, 0x6f, 0x10, 0x25, 0x98, 0x2e, 0xfe, 0xc9, 0x22, 0x6f, 0xe3, 0x30, 0x10, 0x25, 0x98, - 0x2e, 0x0f, 0xca, 0x91, 0x6f, 0x76, 0x86, 0xd9, 0x52, 0xdd, 0x54, 0xd0, 0x42, 0x93, 0x7f, 0x98, 0x2e, 0xfe, 0xc9, - 0x22, 0x6f, 0xe3, 0x30, 0x10, 0x25, 0x98, 0x2e, 0x0f, 0xca, 0x91, 0x6f, 0xdf, 0x54, 0x40, 0x42, 0x79, 0x80, 0xd9, - 0x52, 0x90, 0x7f, 0x98, 0x2e, 0xfe, 0xc9, 0x82, 0x6f, 0x10, 0x25, 0x98, 0x2e, 0xfe, 0xc9, 0x22, 0x6f, 0xe3, 0x30, - 0x10, 0x25, 0x98, 0x2e, 0x0f, 0xca, 0x93, 0x6f, 0xe1, 0x54, 0xd9, 0x52, 0xd0, 0x42, 0x13, 0x7f, 0x98, 0x2e, 0xfe, - 0xc9, 0x22, 0x6f, 0xe3, 0x30, 0x10, 0x25, 0x98, 0x2e, 0x0f, 0xca, 0x13, 0x6f, 0xe5, 0x5e, 0xc0, 0x42, 0xf7, 0x7f, - 0x00, 0x2e, 0x22, 0x6f, 0x91, 0x6f, 0xe1, 0x5a, 0xe3, 0x58, 0xdf, 0x5c, 0xe1, 0x56, 0x98, 0x2e, 0x67, 0xcc, 0xb1, - 0x6f, 0x02, 0x2c, 0x40, 0x42, 0xb1, 0x6f, 0x41, 0x80, 0x32, 0x6f, 0x81, 0x82, 0x62, 0x6f, 0x46, 0x6f, 0x96, 0x7f, - 0x4a, 0x0e, 0xb0, 0x7f, 0x94, 0x2f, 0xf1, 0x3d, 0x01, 0x55, 0x10, 0x30, 0x53, 0x6f, 0x91, 0x00, 0x21, 0x2e, 0xd3, - 0x00, 0x66, 0x6f, 0xd1, 0x40, 0x80, 0x40, 0x01, 0x00, 0x90, 0x42, 0x09, 0x16, 0x85, 0x40, 0x28, 0x02, 0x80, 0x42, - 0x9a, 0x80, 0xd7, 0x54, 0xd6, 0x7f, 0xc3, 0x7f, 0xb0, 0x7f, 0x98, 0x2e, 0xd9, 0xc0, 0x05, 0x30, 0xf5, 0x7f, 0x20, - 0x25, 0xb1, 0x6f, 0xdd, 0x58, 0xdb, 0x5c, 0xdd, 0x56, 0x98, 0x2e, 0x67, 0xcc, 0xd6, 0x6f, 0xc3, 0x6f, 0xb2, 0x6f, - 0x61, 0x6f, 0xa7, 0x84, 0x90, 0x43, 0x59, 0x0e, 0xdf, 0x2f, 0x10, 0x30, 0x81, 0x40, 0x08, 0x28, 0x90, 0x42, 0xd2, - 0x7f, 0x02, 0xa0, 0x27, 0x2f, 0xd5, 0x54, 0x03, 0x53, 0x03, 0x30, 0x96, 0x40, 0x80, 0x40, 0x08, 0x17, 0x15, 0x30, - 0x65, 0x09, 0xb5, 0x01, 0xc3, 0x02, 0x61, 0xb8, 0x94, 0x8c, 0xb6, 0x7f, 0xbf, 0xbd, 0xd7, 0x54, 0xc1, 0x7f, 0x43, - 0x0a, 0x98, 0x2e, 0xd9, 0xc0, 0xe5, 0x54, 0xf2, 0x7f, 0x20, 0x25, 0xb1, 0x6f, 0xe1, 0x5a, 0xe3, 0x58, 0xdf, 0x5c, - 0xe1, 0x56, 0x98, 0x2e, 0x67, 0xcc, 0xb2, 0x6f, 0x03, 0x30, 0xc1, 0x6f, 0xab, 0x84, 0x0b, 0x5d, 0x93, 0x42, 0x50, - 0x42, 0x4e, 0x0e, 0x93, 0x42, 0xdb, 0x2f, 0x83, 0x42, 0x00, 0x2e, 0xd1, 0x6f, 0x98, 0x2e, 0xb3, 0xc0, 0x61, 0x6f, - 0xc0, 0x7f, 0x98, 0x2e, 0xb3, 0xc0, 0x51, 0x6f, 0xb0, 0x7f, 0x98, 0x2e, 0xb3, 0xc0, 0x00, 0xa0, 0xe7, 0x52, 0x08, - 0x22, 0xb2, 0x6f, 0xc1, 0x6f, 0xa0, 0x7f, 0xb3, 0x30, 0x98, 0x2e, 0x0f, 0xca, 0xb2, 0x6f, 0xb0, 0x7f, 0xb3, 0x30, - 0xe9, 0x52, 0x98, 0x2e, 0x5a, 0xca, 0xd6, 0x6f, 0x02, 0x30, 0x61, 0x6f, 0xd2, 0x7f, 0x90, 0x7f, 0x81, 0x7f, 0xb3, - 0x30, 0x42, 0x40, 0x91, 0x41, 0x76, 0x7f, 0x98, 0x2e, 0x0f, 0xca, 0xd2, 0x6f, 0x81, 0x6f, 0x10, 0x28, 0x41, 0x40, - 0x92, 0x6f, 0xd0, 0x7f, 0xb3, 0x30, 0x98, 0x2e, 0x0f, 0xca, 0x81, 0x6f, 0x76, 0x6f, 0x0b, 0x55, 0x50, 0x42, 0x72, - 0x0e, 0xe9, 0x2f, 0xc2, 0x6f, 0xa1, 0x6f, 0x98, 0x2e, 0xfe, 0xc9, 0x10, 0x25, 0xb3, 0x30, 0x21, 0x25, 0x98, 0x2e, - 0x0f, 0xca, 0x20, 0x25, 0x13, 0x51, 0xeb, 0x52, 0x98, 0x2e, 0xcb, 0xb3, 0xc5, 0x6f, 0x13, 0x51, 0xed, 0x5c, 0x12, - 0x40, 0x75, 0x05, 0x31, 0x30, 0x11, 0x86, 0x05, 0x5d, 0xc1, 0x7f, 0x15, 0x0f, 0x00, 0x40, 0x08, 0x2f, 0x3f, 0x80, - 0x00, 0xa8, 0x21, 0x2e, 0xc3, 0x00, 0x00, 0x30, 0x0a, 0x2f, 0x90, 0x43, 0x09, 0x2c, 0x80, 0x43, 0x01, 0x80, 0x03, - 0xa0, 0x21, 0x2e, 0xc3, 0x00, 0x10, 0x30, 0x01, 0x2f, 0x91, 0x43, 0x80, 0x43, 0x00, 0x2e, 0xef, 0x54, 0x00, 0x6f, - 0x82, 0x0f, 0x03, 0x2f, 0xf0, 0x6e, 0xf1, 0x54, 0x02, 0x0f, 0x14, 0x2f, 0xe1, 0x6e, 0xc3, 0x7f, 0x98, 0x2e, 0x74, - 0xc0, 0xc3, 0x6f, 0xf3, 0x54, 0x01, 0x30, 0xc1, 0x7f, 0xc2, 0x0f, 0x0a, 0x2f, 0xf0, 0x6e, 0xf5, 0x52, 0x81, 0x0e, - 0x11, 0x30, 0x04, 0x2f, 0x00, 0x6f, 0x00, 0xa4, 0x11, 0x30, 0x00, 0x2f, 0x21, 0x30, 0xc1, 0x7f, 0xf1, 0x80, 0xc2, - 0x40, 0x80, 0x90, 0x07, 0x2f, 0x07, 0x55, 0xb8, 0x86, 0x12, 0x30, 0xc1, 0x42, 0xd7, 0x86, 0x23, 0x2e, 0xc5, 0x00, - 0xc2, 0x42, 0x38, 0x8a, 0x02, 0x40, 0x0a, 0x1a, 0x07, 0x55, 0x00, 0x30, 0x02, 0x2f, 0x91, 0x42, 0x0e, 0x2c, 0x80, - 0x42, 0x03, 0x2e, 0xc6, 0x00, 0x12, 0x30, 0x4a, 0x28, 0x23, 0x2e, 0xc6, 0x00, 0x50, 0xa0, 0x04, 0x2f, 0x09, 0x55, - 0x89, 0x82, 0xc3, 0x6f, 0x83, 0x42, 0x40, 0x42, 0x00, 0x2e, 0x05, 0x2e, 0x8e, 0x00, 0x84, 0x8c, 0x47, 0x41, 0xa1, - 0x41, 0xa1, 0x56, 0xc0, 0xb3, 0x0b, 0x09, 0xc4, 0x05, 0x44, 0x89, 0x85, 0x41, 0xf3, 0xbf, 0x82, 0x8d, 0xa7, 0x7f, - 0x94, 0x7f, 0x0a, 0x2f, 0x09, 0x2e, 0xc4, 0x00, 0x00, 0x91, 0x06, 0x2f, 0x81, 0x80, 0xf9, 0x58, 0x0b, 0x40, 0xfb, - 0x5a, 0x84, 0x7f, 0x0c, 0x2c, 0xfb, 0x50, 0x2b, 0x09, 0x98, 0xb9, 0x44, 0x04, 0xd8, 0xba, 0x82, 0x84, 0x53, 0xbc, - 0xf7, 0x5e, 0xb3, 0xbe, 0x8b, 0x40, 0x13, 0xbe, 0x87, 0x7f, 0xb3, 0x30, 0x86, 0x41, 0xfd, 0x52, 0xb2, 0x6f, 0x76, - 0x7f, 0x60, 0x7f, 0x55, 0x7f, 0x4b, 0x7f, 0x34, 0x7f, 0x98, 0x2e, 0x0f, 0xca, 0xd1, 0x6f, 0x88, 0x0e, 0x03, 0x2f, - 0x01, 0x2e, 0xbc, 0x00, 0x00, 0xb2, 0x03, 0x2f, 0x00, 0x30, 0x21, 0x2e, 0xbe, 0x00, 0x10, 0x2d, 0x03, 0x2e, 0xbe, - 0x00, 0x10, 0x30, 0x48, 0x28, 0x72, 0x6f, 0xa8, 0xb9, 0x23, 0x2e, 0xbe, 0x00, 0x0b, 0x0e, 0xc1, 0x6f, 0x0b, 0x55, - 0x03, 0x2f, 0x90, 0x42, 0x91, 0x42, 0x00, 0x30, 0x80, 0x42, 0xb3, 0x30, 0xb2, 0x6f, 0x41, 0x6f, 0x98, 0x2e, 0x0f, - 0xca, 0xd1, 0x6f, 0x08, 0x0f, 0x03, 0x2f, 0x01, 0x2e, 0xbc, 0x00, 0x00, 0x90, 0x03, 0x2f, 0x00, 0x30, 0x21, 0x2e, - 0xc0, 0x00, 0x12, 0x2d, 0x01, 0x2e, 0xc0, 0x00, 0x71, 0x6f, 0xa1, 0x54, 0x01, 0x80, 0x4a, 0x08, 0x21, 0x2e, 0xc0, - 0x00, 0x01, 0x0e, 0x07, 0x2f, 0x0d, 0x51, 0x02, 0x80, 0x01, 0x30, 0x21, 0x42, 0x12, 0x30, 0x01, 0x42, 0x25, 0x2e, - 0xbf, 0x00, 0x91, 0x6f, 0x7e, 0x84, 0x43, 0x40, 0x82, 0x40, 0x7c, 0x80, 0x9a, 0x29, 0x03, 0x40, 0x46, 0x42, 0xc3, - 0xb2, 0x06, 0x30, 0x00, 0x30, 0x2b, 0x2f, 0xc0, 0x6f, 0x00, 0xb2, 0x00, 0x30, 0x27, 0x2f, 0x01, 0x2e, 0xc4, 0x00, - 0x00, 0x90, 0x00, 0x30, 0x05, 0x2f, 0xc2, 0x90, 0x03, 0x2f, 0xc0, 0x6f, 0x03, 0x90, 0x00, 0x30, 0x1c, 0x2f, 0x00, - 0x6f, 0x83, 0x6f, 0x83, 0x0e, 0x00, 0x30, 0x17, 0x2f, 0xf3, 0x6e, 0x50, 0x6f, 0x98, 0x0f, 0x00, 0x30, 0x12, 0x2f, - 0xa0, 0x6f, 0x98, 0x0e, 0x00, 0x30, 0x0e, 0x2f, 0xe3, 0x6e, 0x60, 0x6f, 0x98, 0x0f, 0x00, 0x30, 0x09, 0x2f, 0x30, - 0x6f, 0x98, 0x0e, 0x00, 0x30, 0x05, 0x2f, 0x80, 0xb2, 0x00, 0x30, 0x02, 0x2f, 0x2d, 0x2e, 0xbc, 0x00, 0x10, 0x30, - 0x42, 0x40, 0x82, 0xac, 0x56, 0x82, 0x01, 0x2f, 0x00, 0xb2, 0x05, 0x2f, 0x0d, 0x55, 0x82, 0x84, 0x2d, 0x2e, 0xbf, - 0x00, 0x86, 0x42, 0x00, 0x2e, 0x0f, 0x55, 0x56, 0x42, 0x56, 0x42, 0x4a, 0x0e, 0xfb, 0x2f, 0x2d, 0x2e, 0xd6, 0x00, - 0x01, 0x2d, 0x00, 0x30, 0xeb, 0x6f, 0xb0, 0x5e, 0xb8, 0x2e, 0x70, 0x50, 0x0a, 0x25, 0x39, 0x80, 0xf3, 0x7f, 0x03, - 0x42, 0xa1, 0x7f, 0xc2, 0x7f, 0xd1, 0x7f, 0x03, 0x30, 0x03, 0x43, 0xe4, 0x7f, 0xbb, 0x7f, 0x22, 0x30, 0x10, 0x25, - 0x98, 0x2e, 0x7a, 0xc1, 0xd2, 0x6f, 0x02, 0x17, 0x04, 0x08, 0xc1, 0x6f, 0x0c, 0x09, 0x04, 0x1a, 0x10, 0x30, 0x04, - 0x30, 0x20, 0x22, 0x01, 0xb2, 0x14, 0x2f, 0x17, 0x59, 0x14, 0x09, 0xf3, 0x30, 0x93, 0x08, 0x24, 0xbd, 0x44, 0xba, - 0x94, 0x0a, 0x02, 0x17, 0xf3, 0x6f, 0x4c, 0x08, 0x9a, 0x08, 0x8a, 0x0a, 0x19, 0x53, 0x51, 0x08, 0xa1, 0x58, 0x94, - 0x08, 0x28, 0xbd, 0x98, 0xb8, 0xe4, 0x6f, 0x51, 0x0a, 0x01, 0x43, 0x00, 0x2e, 0xbb, 0x6f, 0x90, 0x5f, 0xb8, 0x2e, - 0x1b, 0x57, 0x05, 0x40, 0x69, 0x18, 0x0d, 0x17, 0xe1, 0x18, 0x19, 0x05, 0x16, 0x25, 0x37, 0x25, 0x4a, 0x17, 0x54, - 0x18, 0xec, 0x18, 0x04, 0x30, 0x64, 0x07, 0xea, 0x18, 0x8e, 0x00, 0x5f, 0x02, 0xd9, 0x56, 0x93, 0x00, 0x4c, 0x02, - 0x2f, 0xb9, 0x91, 0xbc, 0x91, 0x0a, 0x02, 0x42, 0xb8, 0x2e, 0x00, 0x2e, 0x10, 0x24, 0xfa, 0x07, 0x11, 0x24, 0x00, - 0x10, 0x12, 0x24, 0x80, 0x2e, 0x13, 0x24, 0x00, 0xc1, 0x12, 0x42, 0x13, 0x42, 0x41, 0x1a, 0xfb, 0x2f, 0x10, 0x24, - 0x50, 0x32, 0x11, 0x24, 0x21, 0x2e, 0x21, 0x2e, 0x10, 0x00, 0x23, 0x2e, 0x11, 0x00, 0x80, 0x2e, 0x10, 0x00 -}; - -/*! @name Global array that stores the feature input configuration of BMI270_WH */ -const struct bmi2_feature_config bmi270_wh_feat_in[BMI270_WH_MAX_FEAT_IN] = { - { .type = BMI2_CONFIG_ID, .page = BMI2_PAGE_1, .start_addr = BMI270_WH_CONFIG_ID_STRT_ADDR }, - { .type = BMI2_MAX_BURST_LEN, .page = BMI2_PAGE_1, .start_addr = BMI270_WH_MAX_BURST_LEN_STRT_ADDR }, - { .type = BMI2_AXIS_MAP, .page = BMI2_PAGE_1, .start_addr = BMI270_WH_AXIS_MAP_STRT_ADDR }, - { .type = BMI2_NVM_PROG_PREP, .page = BMI2_PAGE_1, .start_addr = BMI270_WH_NVM_PROG_PREP_STRT_ADDR }, - { .type = BMI2_GYRO_GAIN_UPDATE, .page = BMI2_PAGE_1, .start_addr = BMI270_WH_GYRO_GAIN_UPDATE_STRT_ADDR }, - { .type = BMI2_ANY_MOTION, .page = BMI2_PAGE_1, .start_addr = BMI270_WH_ANY_MOT_STRT_ADDR }, - { .type = BMI2_NO_MOTION, .page = BMI2_PAGE_2, .start_addr = BMI270_WH_NO_MOT_STRT_ADDR }, - { .type = BMI2_STEP_COUNTER_PARAMS, .page = BMI2_PAGE_3, .start_addr = BMI270_WH_STEP_CNT_1_STRT_ADDR }, - { .type = BMI2_STEP_DETECTOR, .page = BMI2_PAGE_6, .start_addr = BMI270_WH_STEP_CNT_4_STRT_ADDR }, - { .type = BMI2_STEP_COUNTER, .page = BMI2_PAGE_6, .start_addr = BMI270_WH_STEP_CNT_4_STRT_ADDR }, - { .type = BMI2_STEP_ACTIVITY, .page = BMI2_PAGE_6, .start_addr = BMI270_WH_STEP_CNT_4_STRT_ADDR }, - { .type = BMI2_WRIST_WEAR_WAKE_UP_WH, .page = BMI2_PAGE_2, .start_addr = BMI270_WH_WRIST_WEAR_WAKE_UP_STRT_ADDR }, -}; - -/*! @name Global array that stores the feature output configuration */ -const struct bmi2_feature_config bmi270_wh_feat_out[BMI270_WH_MAX_FEAT_OUT] = { - { .type = BMI2_STEP_COUNTER, .page = BMI2_PAGE_0, .start_addr = BMI270_WH_STEP_CNT_OUT_STRT_ADDR }, - { .type = BMI2_STEP_ACTIVITY, .page = BMI2_PAGE_0, .start_addr = BMI270_WH_STEP_ACT_OUT_STRT_ADDR }, - { .type = BMI2_GYRO_GAIN_UPDATE, .page = BMI2_PAGE_0, .start_addr = BMI270_WH_GYR_USER_GAIN_OUT_STRT_ADDR }, - { .type = BMI2_GYRO_CROSS_SENSE, .page = BMI2_PAGE_0, .start_addr = BMI270_WH_GYRO_CROSS_SENSE_STRT_ADDR }, - { .type = BMI2_NVM_STATUS, .page = BMI2_PAGE_0, .start_addr = BMI270_WH_NVM_VFRM_OUT_STRT_ADDR }, - { .type = BMI2_VFRM_STATUS, .page = BMI2_PAGE_0, .start_addr = BMI270_WH_NVM_VFRM_OUT_STRT_ADDR } -}; - -/*! @name Global array that stores the feature interrupts of BMI270_WH */ -struct bmi2_map_int bmi270wh_map_int[BMI270_WH_MAX_INT_MAP] = { - { .type = BMI2_STEP_COUNTER, .sens_map_int = BMI270_WH_INT_STEP_COUNTER_MASK }, - { .type = BMI2_STEP_DETECTOR, .sens_map_int = BMI270_WH_INT_STEP_DETECTOR_MASK }, - { .type = BMI2_STEP_ACTIVITY, .sens_map_int = BMI270_WH_INT_STEP_ACT_MASK }, - { .type = BMI2_WRIST_WEAR_WAKE_UP_WH, .sens_map_int = BMI270_WH_INT_WRIST_WEAR_WAKEUP_WH_MASK }, - { .type = BMI2_ANY_MOTION, .sens_map_int = BMI270_WH_INT_ANY_MOT_MASK }, - { .type = BMI2_NO_MOTION, .sens_map_int = BMI270_WH_INT_NO_MOT_MASK }, -}; - -/******************************************************************************/ - -/*! Local Function Prototypes - ******************************************************************************/ - -/*! - * @brief This internal API is used to validate the device pointer for - * null conditions. - * - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t null_ptr_check(const struct bmi2_dev *dev); - -/*! - * @brief This internal API enables the selected sensor/features. - * - * @param[in] sensor_sel : Selects the desired sensor. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t sensor_enable(uint64_t sensor_sel, struct bmi2_dev *dev); - -/*! - * @brief This internal API disables the selected sensor/features. - * - * @param[in] sensor_sel : Selects the desired sensor. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t sensor_disable(uint64_t sensor_sel, struct bmi2_dev *dev); - -/*! - * @brief This internal API selects the sensors/features to be enabled or - * disabled. - * - * @param[in] sens_list : Pointer to select the sensor. - * @param[in] n_sens : Number of sensors selected. - * @param[out] sensor_sel : Gets the selected sensor. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t select_sensor(const uint8_t *sens_list, uint8_t n_sens, uint64_t *sensor_sel); - -/*! - * @brief This internal API is used to enable/disable any-motion feature. - * - * @param[in] dev : Structure instance of bmi2_dev. - * @param[in] enable : Enables/Disables any-motion. - * - * Enable | Description - * -------------|--------------- - * BMI2_DISABLE | Disables any-motion. - * BMI2_ENABLE | Enables any-motion. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_any_motion(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to enable/disable no-motion feature. - * - * @param[in] dev : Structure instance of bmi2_dev. - * @param[in] enable : Enables/Disables no-motion. - * - * Enable | Description - * -------------|--------------- - * BMI2_DISABLE | Disables no-motion. - * BMI2_ENABLE | Enables no-motion. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_no_motion(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to enable/disable step detector feature. - * - * @param[in] dev : Structure instance of bmi2_dev. - * @param[in] enable : Enables/Disables step-detector. - * - * Enable | Description - * -------------|--------------- - * BMI2_DISABLE | Disables step detector - * BMI2_ENABLE | Enables step detector - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_step_detector(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to enable/disable step counter feature. - * - * @param[in] dev : Structure instance of bmi2_dev. - * @param[in] enable : Enables/Disables step counter. - * - * Enable | Description - * -------------|--------------- - * BMI2_DISABLE | Disables step counter - * BMI2_ENABLE | Enables step counter - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_step_counter(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to enable/disable step activity detection. - * - * @param[in] dev : Structure instance of bmi2_dev. - * @param[in] enable : Enables/Disables step activity. - * - * Enable | Description - * -------------|--------------- - * BMI2_DISABLE | Disables step activity - * BMI2_ENABLE | Enables step activity - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_step_activity(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to enable/disable gyroscope user gain - * feature. - * - * @param[in] dev : Structure instance of bmi2_dev. - * @param[in] enable : Enables/Disables gyroscope user gain. - * - * Enable | Description - * -------------|--------------- - * BMI2_DISABLE | Disables gyroscope user gain - * BMI2_ENABLE | Enables gyroscope user gain - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_gyro_user_gain(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API enables the wrist wear wake up feature for wearable variant. - * - * @param[in] dev : Structure instance of bmi2_dev. - * @param[in] enable : Enables/Disables wrist wear wake up. - * - * Enable | Description - * -------------|--------------- - * BMI2_DISABLE | Disables wrist wear wake up - * BMI2_ENABLE | Enables wrist wear wake up - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_wrist_wear_wake_up_wh(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API sets any-motion configurations like axes select, - * duration, threshold and output-configuration. - * - * @param[in] config : Structure instance of bmi2_any_motion_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @verbatim - *---------------------------------------------------------------------------- - * bmi2_any_motion_config | - * Structure parameters | Description - *--------------------------|-------------------------------------------------- - * | Defines the number of consecutive data points for - * | which the threshold condition must be respected, - * | for interrupt assertion. It is expressed in 50 Hz - * duration | samples (20 msec). - * | Range is 0 to 163sec. - * | Default value is 5 = 100ms. - * -------------------------|--------------------------------------------------- - * | Slope threshold value for in 5.11g format. - * threshold | Range is 0 to 1g. - * | Default value is 0xAA = 83mg. - * -------------------------|--------------------------------------------------- - * x_sel, y_sel, z_sel | Selects the feature on a per-axis basis - * -------------------------|--------------------------------------------------- - * @endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_any_motion_config(const struct bmi2_any_motion_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API sets no-motion configurations like axes select, - * duration, threshold and output-configuration. - * - * @param[in] config : Structure instance of bmi2_no_motion_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @verbatim - *---------------------------------------------------------------------------- - * bmi2_no_motion_config | - * Structure parameters | Description - *--------------------------|-------------------------------------------------- - * | Defines the number of consecutive data points for - * | which the threshold condition must be respected, - * | for interrupt assertion. It is expressed in 50 Hz - * duration | samples (20 msec). - * | Range is 0 to 163sec. - * | Default value is 5 = 100ms. - * -------------------------|--------------------------------------------------- - * | Slope threshold value for in 5.11g format. - * threshold | Range is 0 to 1g. - * | Default value is 0xAA = 83mg. - * -------------------------|--------------------------------------------------- - * x_sel, y_sel, z_sel | Selects the feature on a per-axis basis - * -------------------------|--------------------------------------------------- - * @endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_no_motion_config(const struct bmi2_no_motion_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API sets step counter parameter configurations. - * - * @param[in] step_count_params : Array that stores parameters 1 to 25. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_step_count_params_config(const uint16_t *step_count_params, struct bmi2_dev *dev); - -/*! - * @brief This internal API sets step counter/detector/activity configurations. - * - * @param[in] config : Structure instance of bmi2_step_config. - * @param[in] dev : Structure instance of bmi2_dev. - * - *--------------------------------------------------------------------------- - * bmi2_step_config | - * Structure parameters | Description - *--------------------------|-------------------------------------------------- - * | The Step-counter will trigger output every time - * | the number of steps are counted. Holds implicitly - * water-mark level | a 20x factor, so the range is 0 to 10230, - * | with resolution of 20 steps. - * -------------------------|--------------------------------------------------- - * reset counter | Flag to reset the counted steps. - * -------------------------|--------------------------------------------------- - * @endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_step_config(const struct bmi2_step_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API sets wrist wear wake-up configurations like - * output-configuration for wearable variant. - * - * @param[in] config : Structure instance of - * bmi2_wrist_wear_wake_up_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @verbatim - *-------------------------------------|---------------------------------------- - * bmi2_wrist_wear_wake_up_wh_config | - * Structure parameters | Description - *-------------------------------------|------------------------------------------- - * | To set the wrist wear wake-up parameters like - * | min_angle_focus, min_angle_nonfocus, - * wrist_wear_wake_wh_params | angle_landscape_left, angle_landscape_right, - * | angle_potrait_up and down. - * ------------------------------------|------------------------------------------- - * @endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_wrist_wear_wake_up_wh_config(const struct bmi2_wrist_wear_wake_up_wh_config *config, - struct bmi2_dev *dev); - -/*! - * @brief This internal API gets any-motion configurations like axes select, - * duration, threshold and output-configuration. - * - * @param[out] config : Structure instance of bmi2_any_motion_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @verbatim - *---------------------------------------------------------------------------- - * bmi2_any_motion_config | - * Structure parameters | Description - *--------------------------|-------------------------------------------------- - * | Defines the number of consecutive data points for - * | which the threshold condition must be respected, - * | for interrupt assertion. It is expressed in 50 Hz - * duration | samples (20 msec). - * | Range is 0 to 163sec. - * | Default value is 5 = 100ms. - * -------------------------|--------------------------------------------------- - * | Slope threshold value for in 5.11g format. - * threshold | Range is 0 to 1g. - * | Default value is 0xAA = 83mg. - * -------------------------|--------------------------------------------------- - * x_sel, y_sel, z_sel | Selects the feature on a per-axis basis - * -------------------------|--------------------------------------------------- - * @endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t get_any_motion_config(struct bmi2_any_motion_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets no-motion configurations like axes select, - * duration, threshold and output-configuration. - * - * @param[out] config : Structure instance of bmi2_no_motion_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @verbatim - *---------------------------------------------------------------------------- - * bmi2_no_motion_config | - * Structure parameters | Description - *--------------------------|-------------------------------------------------- - * | Defines the number of consecutive data points for - * | which the threshold condition must be respected, - * | for interrupt assertion. It is expressed in 50 Hz - * duration | samples (20 msec). - * | Range is 0 to 163sec. - * | Default value is 5 = 100ms. - * -------------------------|--------------------------------------------------- - * | Slope threshold value for in 5.11g format. - * threshold | Range is 0 to 1g. - * | Default value is 0xAA = 83mg. - * -------------------------|--------------------------------------------------- - * x_sel, y_sel, z_sel | Selects the feature on a per-axis basis - * -------------------------|--------------------------------------------------- - * @endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t get_no_motion_config(struct bmi2_no_motion_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets step counter parameter configurations. - * - * @param[in] step_count_params : Array that stores parameters 1 to 25. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t get_step_count_params_config(uint16_t *step_count_params, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets step counter/detector/activity configurations. - * - * @param[out] config : Structure instance of bmi2_step_config. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @verbatim - *---------------------------------------------------------------------------- - * bmi2_step_config | - * Structure parameters | Description - *--------------------------|-------------------------------------------------- - * | The Step-counter will trigger output every time - * | the number of steps are counted. Holds implicitly - * water-mark level | a 20x factor, so the range is 0 to 10230, - * | with resolution of 20 steps. - * -------------------------|--------------------------------------------------- - * reset counter | Flag to reset the counted steps. - * -------------------------|--------------------------------------------------- - * @endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t get_step_config(struct bmi2_step_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets wrist wear wake-up configurations like - * output-configuration for wearable variant. - * - * @param[out] config : Structure instance of - * bmi2_wrist_wear_wake_up_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @verbatim - *----------------------------------------------------------------------------- - * bmi2_wrist_wear_wake_up_config | - * Structure parameters | Description - *----------------------------------|------------------------------------------- - * | To get the wrist wear wake-up parameters like - * | min_angle_focus, min_angle_nonfocus, - * wrist_wear_wake_params | angle_landscape_left, angle_landscape_right, - * | angle_potrait_up and down. - * ---------------------------------|------------------------------------------- - * @endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t get_wrist_wear_wake_up_wh_config(struct bmi2_wrist_wear_wake_up_wh_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets the output values of step activity. - * - * @param[out] step_act : Pointer to the stored step activity data. - * @param[in] dev : Structure instance of bmi2_dev. - * - * *step_act | Output - * -----------|------------ - * 0x00 | STILL - * 0x01 | WALKING - * 0x02 | RUNNING - * 0x03 | UNKNOWN - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t get_step_activity_output(uint8_t *step_act, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets the output values of step counter. - * - * @param[out] step_count : Pointer to the stored step counter data. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t get_step_counter_output(uint32_t *step_count, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets the error status related to NVM. - * - * @param[out] nvm_err_stat : Stores the NVM error status. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t get_nvm_error_status(struct bmi2_nvm_err_status *nvm_err_stat, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets the error status related to virtual frames. - * - * @param[out] vfrm_err_stat : Stores the VFRM related error status. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t get_vfrm_error_status(struct bmi2_vfrm_err_status *vfrm_err_stat, struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to get enable status of gyroscope user gain - * update. - * - * @param[out] status : Stores status of gyroscope user gain update. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t get_user_gain_upd_status(uint8_t *status, struct bmi2_dev *dev); - -/*! - * @brief This internal API enables/disables compensation of the gain defined - * in the GAIN register. - * - * @param[in] enable : Enables/Disables gain compensation - * @param[in] dev : Structure instance of bmi2_dev. - * - * enable | Description - * -------------|--------------- - * BMI2_ENABLE | Enable gain compensation. - * BMI2_DISABLE | Disable gain compensation. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t enable_gyro_gain(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to extract the output feature configuration - * details like page and start address from the look-up table. - * - * @param[out] feat_output : Structure that stores output feature - * configurations. - * @param[in] type : Type of feature or sensor. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Returns the feature found flag. - * - * @retval BMI2_FALSE : Feature not found - * BMI2_TRUE : Feature found - */ -static uint8_t extract_output_feat_config(struct bmi2_feature_config *feat_output, - uint8_t type, - const struct bmi2_dev *dev); - -/***************************************************************************/ - -/*! User Interface Definitions - ****************************************************************************/ - -/*! - * @brief This API: - * 1) updates the device structure with address of the configuration file. - * 2) Initializes BMI270_WH sensor. - * 3) Writes the configuration file. - * 4) Updates the feature offset parameters in the device structure. - * 5) Updates the maximum number of pages, in the device structure. - */ -int8_t bmi270_wh_init(struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if (rslt == BMI2_OK) - { - /* Assign chip id of BMI270_WH */ - dev->chip_id = BMI270_WH_CHIP_ID; - - /* get the size of config array */ - dev->config_size = sizeof(bmi270_wh_config_file); - - /* Enable the variant specific features if any */ - dev->variant_feature = BMI2_GYRO_CROSS_SENS_ENABLE | BMI2_CRT_RTOSK_ENABLE; - - /* An extra dummy byte is read during SPI read */ - if (dev->intf == BMI2_SPI_INTF) - { - dev->dummy_byte = 1; - } - else - { - dev->dummy_byte = 0; - } - - /* If configuration file pointer is not assigned any address */ - if (!dev->config_file_ptr) - { - /* Give the address of the configuration file array to - * the device pointer - */ - dev->config_file_ptr = bmi270_wh_config_file; - } - - /* Initialize BMI2 sensor */ - rslt = bmi2_sec_init(dev); - if (rslt == BMI2_OK) - { - /* Assign the offsets of the feature input - * configuration to the device structure - */ - dev->feat_config = bmi270_wh_feat_in; - - /* Assign the offsets of the feature output to - * the device structure - */ - dev->feat_output = bmi270_wh_feat_out; - - /* Assign the maximum number of pages to the - * device structure - */ - dev->page_max = BMI270_WH_MAX_PAGE_NUM; - - /* Assign maximum number of input sensors/ - * features to device structure - */ - dev->input_sens = BMI270_WH_MAX_FEAT_IN; - - /* Assign maximum number of output sensors/ - * features to device structure - */ - dev->out_sens = BMI270_WH_MAX_FEAT_OUT; - - /* Assign the offsets of the feature interrupt - * to the device structure - */ - dev->map_int = bmi270wh_map_int; - - /* Assign maximum number of feature interrupts - * to device structure - */ - dev->sens_int_map = BMI270_WH_MAX_INT_MAP; - - /* Get the gyroscope cross axis sensitivity */ - rslt = bmi2_get_gyro_cross_sense(dev); - } - } - - return rslt; -} - -/*! - * @brief This API selects the sensors/features to be enabled. - */ -int8_t bmi270_wh_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to select sensor */ - uint64_t sensor_sel = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (sens_list != NULL)) - { - /* Get the selected sensors */ - rslt = select_sensor(sens_list, n_sens, &sensor_sel); - if (rslt == BMI2_OK) - { - /* Enable the selected sensors */ - rslt = sensor_enable(sensor_sel, dev); - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API selects the sensors/features to be disabled. - */ -int8_t bmi270_wh_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to select sensor */ - uint64_t sensor_sel = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (sens_list != NULL)) - { - /* Get the selected sensors */ - rslt = select_sensor(sens_list, n_sens, &sensor_sel); - if (rslt == BMI2_OK) - { - /* Disable the selected sensors */ - rslt = sensor_disable(sensor_sel, dev); - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API sets the sensor/feature configuration. - */ -int8_t bmi270_wh_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define loop */ - uint8_t loop; - - /* Variable to get the status of advance power save */ - uint8_t aps_stat = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (sens_cfg != NULL)) - { - /* Get status of advance power save mode */ - aps_stat = dev->aps_status; - - for (loop = 0; loop < n_sens; loop++) - { - if ((sens_cfg[loop].type == BMI2_ACCEL) || (sens_cfg[loop].type == BMI2_GYRO) || - (sens_cfg[loop].type == BMI2_AUX) || (sens_cfg[loop].type == BMI2_GYRO_GAIN_UPDATE)) - { - rslt = bmi2_set_sensor_config(&sens_cfg[loop], 1, dev); - } - else - { - /* Disable Advance power save if enabled for auxiliary - * and feature configurations - */ - if (aps_stat == BMI2_ENABLE) - { - /* Disable advance power save if - * enabled - */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - } - - if (rslt == BMI2_OK) - { - switch (sens_cfg[loop].type) - { - /* Set any motion configuration */ - case BMI2_ANY_MOTION: - rslt = set_any_motion_config(&sens_cfg[loop].cfg.any_motion, dev); - break; - - /* Set no motion configuration */ - case BMI2_NO_MOTION: - rslt = set_no_motion_config(&sens_cfg[loop].cfg.no_motion, dev); - break; - - /* Set the step counter parameters */ - case BMI2_STEP_COUNTER_PARAMS: - rslt = set_step_count_params_config(sens_cfg[loop].cfg.step_counter_params, dev); - break; - - /* Set step counter/detector/activity configuration */ - case BMI2_STEP_DETECTOR: - case BMI2_STEP_COUNTER: - case BMI2_STEP_ACTIVITY: - rslt = set_step_config(&sens_cfg[loop].cfg.step_counter, dev); - break; - - /* Set the wrist wear wake-up configuration for wearable variant */ - case BMI2_WRIST_WEAR_WAKE_UP_WH: - rslt = set_wrist_wear_wake_up_wh_config(&sens_cfg[loop].cfg.wrist_wear_wake_up_wh, dev); - break; - - default: - rslt = BMI2_E_INVALID_SENSOR; - break; - } - } - - /* Return error if any of the set configurations fail */ - if (rslt != BMI2_OK) - { - break; - } - } - } - - /* Enable Advance power save if disabled while configuring and - * not when already disabled - */ - if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) - { - rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API gets the sensor/feature configuration. - */ -int8_t bmi270_wh_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define loop */ - uint8_t loop; - - /* Variable to get the status of advance power save */ - uint8_t aps_stat = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (sens_cfg != NULL)) - { - /* Get status of advance power save mode */ - aps_stat = dev->aps_status; - for (loop = 0; loop < n_sens; loop++) - { - if ((sens_cfg[loop].type == BMI2_ACCEL) || (sens_cfg[loop].type == BMI2_GYRO) || - (sens_cfg[loop].type == BMI2_AUX) || (sens_cfg[loop].type == BMI2_GYRO_GAIN_UPDATE)) - { - rslt = bmi2_get_sensor_config(&sens_cfg[loop], 1, dev); - } - else - { - /* Disable Advance power save if enabled for auxiliary - * and feature configurations - */ - if ((sens_cfg[loop].type >= BMI2_MAIN_SENS_MAX_NUM) || (sens_cfg[loop].type == BMI2_AUX)) - { - - if (aps_stat == BMI2_ENABLE) - { - /* Disable advance power save if - * enabled - */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - } - } - - if (rslt == BMI2_OK) - { - switch (sens_cfg[loop].type) - { - /* Get any motion configuration */ - case BMI2_ANY_MOTION: - rslt = get_any_motion_config(&sens_cfg[loop].cfg.any_motion, dev); - break; - - /* Get no motion configuration */ - case BMI2_NO_MOTION: - rslt = get_no_motion_config(&sens_cfg[loop].cfg.no_motion, dev); - break; - - /* Set the step counter parameters */ - case BMI2_STEP_COUNTER_PARAMS: - rslt = get_step_count_params_config(sens_cfg[loop].cfg.step_counter_params, dev); - break; - - /* Get step counter/detector/activity configuration */ - case BMI2_STEP_DETECTOR: - case BMI2_STEP_COUNTER: - case BMI2_STEP_ACTIVITY: - rslt = get_step_config(&sens_cfg[loop].cfg.step_counter, dev); - break; - - /* Get the wrist wear wake-up configuration for wearable variant */ - case BMI2_WRIST_WEAR_WAKE_UP_WH: - rslt = get_wrist_wear_wake_up_wh_config(&sens_cfg[loop].cfg.wrist_wear_wake_up_wh, dev); - break; - - default: - rslt = BMI2_E_INVALID_SENSOR; - break; - } - } - - /* Return error if any of the get configurations fail */ - if (rslt != BMI2_OK) - { - break; - } - } - } - - /* Enable Advance power save if disabled while configuring and - * not when already disabled - */ - if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) - { - rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API gets the sensor/feature data for accelerometer, gyroscope, - * auxiliary sensor, step counter, high-g, gyroscope user-gain update, - * orientation, gyroscope cross sensitivity and error status for NVM and VFRM. - */ -int8_t bmi270_wh_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define loop */ - uint8_t loop; - - /* Variable to get the status of advance power save */ - uint8_t aps_stat = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (sensor_data != NULL)) - { - /* Get status of advance power save mode */ - aps_stat = dev->aps_status; - for (loop = 0; loop < n_sens; loop++) - { - if ((sensor_data[loop].type == BMI2_ACCEL) || (sensor_data[loop].type == BMI2_GYRO) || - (sensor_data[loop].type == BMI2_AUX) || (sensor_data[loop].type == BMI2_GYRO_GAIN_UPDATE) || - (sensor_data[loop].type == BMI2_GYRO_CROSS_SENSE)) - { - rslt = bmi2_get_sensor_data(&sensor_data[loop], 1, dev); - } - else - { - /* Disable Advance power save if enabled for feature - * configurations - */ - if (sensor_data[loop].type >= BMI2_MAIN_SENS_MAX_NUM) - { - if (aps_stat == BMI2_ENABLE) - { - /* Disable advance power save if - * enabled - */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - } - } - - if (rslt == BMI2_OK) - { - switch (sensor_data[loop].type) - { - case BMI2_STEP_COUNTER: - - /* Get step counter output */ - rslt = get_step_counter_output(&sensor_data[loop].sens_data.step_counter_output, dev); - break; - case BMI2_STEP_ACTIVITY: - - /* Get step activity output */ - rslt = get_step_activity_output(&sensor_data[loop].sens_data.activity_output, dev); - break; - case BMI2_NVM_STATUS: - - /* Get NVM error status */ - rslt = get_nvm_error_status(&sensor_data[loop].sens_data.nvm_status, dev); - break; - case BMI2_VFRM_STATUS: - - /* Get VFRM error status */ - rslt = get_vfrm_error_status(&sensor_data[loop].sens_data.vfrm_status, dev); - break; - default: - rslt = BMI2_E_INVALID_SENSOR; - break; - } - - /* Return error if any of the get sensor data fails */ - if (rslt != BMI2_OK) - { - break; - } - } - } - - /* Enable Advance power save if disabled while - * configuring and not when already disabled - */ - if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) - { - rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - } - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API updates the gyroscope user-gain. - */ -int8_t bmi270_wh_update_gyro_user_gain(const struct bmi2_gyro_user_gain_config *user_gain, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to select sensor */ - uint8_t sens_sel[2] = { BMI2_GYRO, BMI2_GYRO_GAIN_UPDATE }; - - /* Structure to define sensor configurations */ - struct bmi2_sens_config sens_cfg; - - /* Variable to store status of user-gain update module */ - uint8_t status = 0; - - /* Variable to define count */ - uint8_t count = 100; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (user_gain != NULL)) - { - /* Select type of feature */ - sens_cfg.type = BMI2_GYRO_GAIN_UPDATE; - - /* Get the user gain configurations */ - rslt = bmi270_wh_get_sensor_config(&sens_cfg, 1, dev); - if (rslt == BMI2_OK) - { - /* Get the user-defined ratio */ - sens_cfg.cfg.gyro_gain_update = *user_gain; - - /* Set rate ratio for all axes */ - rslt = bmi270_wh_set_sensor_config(&sens_cfg, 1, dev); - } - - /* Disable gyroscope */ - if (rslt == BMI2_OK) - { - rslt = bmi270_wh_sensor_disable(&sens_sel[0], 1, dev); - } - - /* Enable gyroscope user-gain update module */ - if (rslt == BMI2_OK) - { - rslt = bmi270_wh_sensor_enable(&sens_sel[1], 1, dev); - } - - /* Set the command to trigger the computation */ - if (rslt == BMI2_OK) - { - rslt = bmi2_set_command_register(BMI2_USR_GAIN_CMD, dev); - } - - if (rslt == BMI2_OK) - { - /* Poll until enable bit of user-gain update is 0 */ - while (count--) - { - rslt = get_user_gain_upd_status(&status, dev); - if ((rslt == BMI2_OK) && (status == 0)) - { - /* Enable compensation of gain defined - * in the GAIN register - */ - rslt = enable_gyro_gain(BMI2_ENABLE, dev); - - /* Enable gyroscope */ - if (rslt == BMI2_OK) - { - rslt = bmi270_wh_sensor_enable(&sens_sel[0], 1, dev); - } - - break; - } - - dev->delay_us(10000, dev->intf_ptr); - } - - /* Return error if user-gain update is failed */ - if ((rslt == BMI2_OK) && (status != 0)) - { - rslt = BMI2_E_GYR_USER_GAIN_UPD_FAIL; - } - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API reads the compensated gyroscope user-gain values. - */ -int8_t bmi270_wh_read_gyro_user_gain(struct bmi2_gyro_user_gain_data *gyr_usr_gain, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define register data */ - uint8_t reg_data[3] = { 0 }; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (gyr_usr_gain != NULL)) - { - /* Get the gyroscope compensated gain values */ - rslt = bmi2_get_regs(BMI2_GYR_USR_GAIN_0_ADDR, reg_data, 3, dev); - if (rslt == BMI2_OK) - { - /* Gyroscope user gain correction X-axis */ - gyr_usr_gain->x = (int8_t)BMI2_GET_BIT_POS0(reg_data[0], BMI2_GYR_USR_GAIN_X); - - /* Gyroscope user gain correction Y-axis */ - gyr_usr_gain->y = (int8_t)BMI2_GET_BIT_POS0(reg_data[1], BMI2_GYR_USR_GAIN_Y); - - /* Gyroscope user gain correction z-axis */ - gyr_usr_gain->z = (int8_t)BMI2_GET_BIT_POS0(reg_data[2], BMI2_GYR_USR_GAIN_Z); - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API maps/unmaps feature interrupts to that of interrupt pins. - */ -int8_t bmi270_wh_map_feat_int(const struct bmi2_sens_int_config *sens_int, uint8_t n_sens, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define loop */ - uint8_t loop; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (sens_int != NULL)) - { - for (loop = 0; loop < n_sens; loop++) - { - switch (sens_int[loop].type) - { - case BMI2_ANY_MOTION: - case BMI2_NO_MOTION: - case BMI2_STEP_COUNTER: - case BMI2_STEP_DETECTOR: - case BMI2_STEP_ACTIVITY: - case BMI2_WRIST_WEAR_WAKE_UP_WH: - - rslt = bmi2_map_feat_int(sens_int[loop].type, sens_int[loop].hw_int_pin, dev); - break; - default: - rslt = BMI2_E_INVALID_SENSOR; - break; - } - - /* Return error if interrupt mapping fails */ - if (rslt != BMI2_OK) - { - break; - } - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/***************************************************************************/ - -/*! Local Function Definitions - ****************************************************************************/ - -/*! - * @brief This internal API is used to validate the device structure pointer for - * null conditions. - */ -static int8_t null_ptr_check(const struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt = BMI2_OK; - - if ((dev == NULL) || (dev->read == NULL) || (dev->write == NULL) || (dev->delay_us == NULL)) - { - /* Device structure pointer is not valid */ - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This internal API selects the sensor/features to be enabled or - * disabled. - */ -static int8_t select_sensor(const uint8_t *sens_list, uint8_t n_sens, uint64_t *sensor_sel) -{ - /* Variable to define error */ - int8_t rslt = BMI2_OK; - - /* Variable to define loop */ - uint8_t count; - - for (count = 0; count < n_sens; count++) - { - switch (sens_list[count]) - { - case BMI2_ACCEL: - *sensor_sel |= BMI2_ACCEL_SENS_SEL; - break; - case BMI2_GYRO: - *sensor_sel |= BMI2_GYRO_SENS_SEL; - break; - case BMI2_AUX: - *sensor_sel |= BMI2_AUX_SENS_SEL; - break; - case BMI2_TEMP: - *sensor_sel |= BMI2_TEMP_SENS_SEL; - break; - case BMI2_ANY_MOTION: - *sensor_sel |= BMI2_ANY_MOT_SEL; - break; - case BMI2_NO_MOTION: - *sensor_sel |= BMI2_NO_MOT_SEL; - break; - case BMI2_STEP_DETECTOR: - *sensor_sel |= BMI2_STEP_DETECT_SEL; - break; - case BMI2_STEP_COUNTER: - *sensor_sel |= BMI2_STEP_COUNT_SEL; - break; - case BMI2_STEP_ACTIVITY: - *sensor_sel |= BMI2_STEP_ACT_SEL; - break; - case BMI2_GYRO_GAIN_UPDATE: - *sensor_sel |= BMI2_GYRO_GAIN_UPDATE_SEL; - break; - case BMI2_WRIST_WEAR_WAKE_UP_WH: - *sensor_sel |= BMI2_WRIST_WEAR_WAKE_UP_WH_SEL; - break; - default: - rslt = BMI2_E_INVALID_SENSOR; - break; - } - } - - return rslt; -} - -/*! - * @brief This internal API enables the selected sensor/features. - */ -static int8_t sensor_enable(uint64_t sensor_sel, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to store register values */ - uint8_t reg_data = 0; - - /* Variable to define loop */ - uint8_t loop = 1; - - /* Variable to get the status of advance power save */ - uint8_t aps_stat = 0; - - rslt = bmi2_get_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); - if (rslt == BMI2_OK) - { - /* Enable accelerometer */ - if (sensor_sel & BMI2_ACCEL_SENS_SEL) - { - reg_data = BMI2_SET_BITS(reg_data, BMI2_ACC_EN, BMI2_ENABLE); - } - - /* Enable gyroscope */ - if (sensor_sel & BMI2_GYRO_SENS_SEL) - { - reg_data = BMI2_SET_BITS(reg_data, BMI2_GYR_EN, BMI2_ENABLE); - } - - /* Enable auxiliary sensor */ - if (sensor_sel & BMI2_AUX_SENS_SEL) - { - reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_AUX_EN, BMI2_ENABLE); - } - - /* Enable temperature sensor */ - if (sensor_sel & BMI2_TEMP_SENS_SEL) - { - reg_data = BMI2_SET_BITS(reg_data, BMI2_TEMP_EN, BMI2_ENABLE); - } - - /* Enable the sensors that are set in the power control register */ - if (sensor_sel & BMI2_MAIN_SENSORS) - { - rslt = bmi2_set_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); - } - } - - if ((rslt == BMI2_OK) && (sensor_sel & ~(BMI2_MAIN_SENSORS))) - { - /* Get status of advance power save mode */ - aps_stat = dev->aps_status; - if (aps_stat == BMI2_ENABLE) - { - /* Disable advance power save if enabled */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - } - - if (rslt == BMI2_OK) - { - while (loop--) - { - /* Enable any motion feature */ - if (sensor_sel & BMI2_ANY_MOT_SEL) - { - rslt = set_any_motion(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_ANY_MOT_SEL; - } - else - { - break; - } - } - - /* Enable no motion feature */ - if (sensor_sel & BMI2_NO_MOT_SEL) - { - rslt = set_no_motion(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_NO_MOT_SEL; - } - else - { - break; - } - } - - /* Enable step detector feature */ - if (sensor_sel & BMI2_STEP_DETECT_SEL) - { - rslt = set_step_detector(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_STEP_DETECT_SEL; - } - else - { - break; - } - } - - /* Enable step counter feature */ - if (sensor_sel & BMI2_STEP_COUNT_SEL) - { - rslt = set_step_counter(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_STEP_COUNT_SEL; - } - else - { - break; - } - } - - /* Enable step activity feature */ - if (sensor_sel & BMI2_STEP_ACT_SEL) - { - rslt = set_step_activity(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_STEP_ACT_SEL; - } - else - { - break; - } - } - - /* Enable gyroscope user gain */ - if (sensor_sel & BMI2_GYRO_GAIN_UPDATE_SEL) - { - rslt = set_gyro_user_gain(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_GYRO_GAIN_UPDATE_SEL; - } - else - { - break; - } - } - - /* Enable wrist wear wake-up feature for wearable variant */ - if (sensor_sel & BMI2_WRIST_WEAR_WAKE_UP_WH_SEL) - { - rslt = set_wrist_wear_wake_up_wh(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat |= BMI2_WRIST_WEAR_WAKE_UP_WH_SEL; - } - else - { - break; - } - } - } - - /* Enable Advance power save if disabled while - * configuring and not when already disabled - */ - if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) - { - rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - } - } - } - - return rslt; -} - -/*! - * @brief This internal API disables the selected sensors/features. - */ -static int8_t sensor_disable(uint64_t sensor_sel, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to store register values */ - uint8_t reg_data = 0; - - /* Variable to define loop */ - uint8_t loop = 1; - - /* Variable to get the status of advance power save */ - uint8_t aps_stat = 0; - - rslt = bmi2_get_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); - if (rslt == BMI2_OK) - { - /* Disable accelerometer */ - if (sensor_sel & BMI2_ACCEL_SENS_SEL) - { - reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_ACC_EN); - } - - /* Disable gyroscope */ - if (sensor_sel & BMI2_GYRO_SENS_SEL) - { - reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_GYR_EN); - } - - /* Disable auxiliary sensor */ - if (sensor_sel & BMI2_AUX_SENS_SEL) - { - reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_AUX_EN); - } - - /* Disable temperature sensor */ - if (sensor_sel & BMI2_TEMP_SENS_SEL) - { - reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_TEMP_EN); - } - - /* Disable the sensors that are set in the power control register */ - if (sensor_sel & BMI2_MAIN_SENSORS) - { - rslt = bmi2_set_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); - } - } - - if ((rslt == BMI2_OK) && (sensor_sel & ~(BMI2_MAIN_SENSORS))) - { - /* Get status of advance power save mode */ - aps_stat = dev->aps_status; - if (aps_stat == BMI2_ENABLE) - { - /* Disable advance power save if enabled */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - } - - if (rslt == BMI2_OK) - { - while (loop--) - { - /* Disable any-motion feature */ - if (sensor_sel & BMI2_ANY_MOT_SEL) - { - rslt = set_any_motion(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat &= ~BMI2_ANY_MOT_SEL; - } - else - { - break; - } - } - - /* Disable no-motion feature */ - if (sensor_sel & BMI2_NO_MOT_SEL) - { - rslt = set_no_motion(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat &= ~BMI2_NO_MOT_SEL; - } - else - { - break; - } - } - - /* Disable step detector feature */ - if (sensor_sel & BMI2_STEP_DETECT_SEL) - { - rslt = set_step_detector(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat &= ~BMI2_STEP_DETECT_SEL; - } - else - { - break; - } - } - - /* Disable step counter feature */ - if (sensor_sel & BMI2_STEP_COUNT_SEL) - { - rslt = set_step_counter(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat &= ~BMI2_STEP_COUNT_SEL; - } - else - { - break; - } - } - - /* Disable step activity feature */ - if (sensor_sel & BMI2_STEP_ACT_SEL) - { - rslt = set_step_activity(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat &= ~BMI2_STEP_ACT_SEL; - } - else - { - break; - } - } - - /* Disable gyroscope user gain */ - if (sensor_sel & BMI2_GYRO_GAIN_UPDATE_SEL) - { - rslt = set_gyro_user_gain(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat &= ~BMI2_GYRO_GAIN_UPDATE_SEL; - } - else - { - break; - } - } - - /* Disable wrist wear wake-up feature for wearable variant */ - if (sensor_sel & BMI2_WRIST_WEAR_WAKE_UP_WH_SEL) - { - rslt = set_wrist_wear_wake_up_wh(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - dev->sens_en_stat &= ~BMI2_WRIST_WEAR_WAKE_UP_WH_SEL; - } - else - { - break; - } - } - - /* Enable Advance power save if disabled while - * configuring and not when already disabled - */ - if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) - { - rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - } - } - } - } - - return rslt; -} - -/*! - * @brief This internal API is used to enable/disable any motion feature. - */ -static int8_t set_any_motion(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for any-motion */ - struct bmi2_feature_config any_mot_config = { 0, 0, 0 }; - - /* Search for any-motion feature and extract its configurations details */ - feat_found = bmi2_extract_input_feat_config(&any_mot_config, BMI2_ANY_MOTION, dev); - if (feat_found) - { - /* Get the configuration from the page where any-motion feature resides */ - rslt = bmi2_get_feat_config(any_mot_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of any-motion axes */ - idx = any_mot_config.start_addr + BMI2_ANY_MOT_FEAT_EN_OFFSET; - - /* Set the feature enable bit */ - feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_ANY_NO_MOT_EN, enable); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API is used to enable/disable no-motion feature. - */ -static int8_t set_no_motion(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for no-motion */ - struct bmi2_feature_config no_mot_config = { 0, 0, 0 }; - - /* Search for no-motion feature and extract its configurations details */ - feat_found = bmi2_extract_input_feat_config(&no_mot_config, BMI2_NO_MOTION, dev); - if (feat_found) - { - /* Get the configuration from the page where any/no-motion feature resides */ - rslt = bmi2_get_feat_config(no_mot_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of no-motion axes */ - idx = no_mot_config.start_addr + BMI2_NO_MOT_FEAT_EN_OFFSET; - - /* Set the feature enable bit */ - feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_ANY_NO_MOT_EN, enable); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API is used to enable/disable step detector feature. - */ -static int8_t set_step_detector(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for step detector */ - struct bmi2_feature_config step_det_config = { 0, 0, 0 }; - - /* Search for step detector feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&step_det_config, BMI2_STEP_DETECTOR, dev); - if (feat_found) - { - /* Get the configuration from the page where step detector feature resides */ - rslt = bmi2_get_feat_config(step_det_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of step detector */ - idx = step_det_config.start_addr + BMI2_STEP_COUNT_FEAT_EN_OFFSET; - - /* Set the feature enable bit */ - feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_STEP_DET_FEAT_EN, enable); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API is used to enable/disable step counter feature. - */ -static int8_t set_step_counter(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for step counter */ - struct bmi2_feature_config step_count_config = { 0, 0, 0 }; - - /* Search for step counter feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&step_count_config, BMI2_STEP_COUNTER, dev); - if (feat_found) - { - /* Get the configuration from the page where step-counter feature resides */ - rslt = bmi2_get_feat_config(step_count_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of step counter */ - idx = step_count_config.start_addr + BMI2_STEP_COUNT_FEAT_EN_OFFSET; - - /* Set the feature enable bit */ - feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_STEP_COUNT_FEAT_EN, enable); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API is used to enable/disable step activity detection. - */ -static int8_t set_step_activity(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for step activity */ - struct bmi2_feature_config step_act_config = { 0, 0, 0 }; - - /* Search for step activity feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&step_act_config, BMI2_STEP_ACTIVITY, dev); - if (feat_found) - { - /* Get the configuration from the page where step-activity - * feature resides - */ - rslt = bmi2_get_feat_config(step_act_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of step activity */ - idx = step_act_config.start_addr + BMI2_STEP_COUNT_FEAT_EN_OFFSET; - - /* Set the feature enable bit */ - feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_STEP_ACT_FEAT_EN, enable); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API enables the wrist wear wake up feature. - */ -static int8_t set_wrist_wear_wake_up_wh(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for wrist wear wake up */ - struct bmi2_feature_config wrist_wake_up_cfg = { 0, 0, 0 }; - - /* Search for wrist wear wake up and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&wrist_wake_up_cfg, BMI2_WRIST_WEAR_WAKE_UP_WH, dev); - if (feat_found) - { - /* Get the configuration from the page where wrist wear wake up - * feature resides - */ - rslt = bmi2_get_feat_config(wrist_wake_up_cfg.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of wrist wear wake up */ - idx = wrist_wake_up_cfg.start_addr; - - /* Set the feature enable bit */ - feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_WRIST_WEAR_WAKE_UP_FEAT_EN, enable); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API is used to enable/disable gyroscope user gain - * feature. - */ -static int8_t set_gyro_user_gain(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for gyroscope user gain */ - struct bmi2_feature_config gyr_user_gain_cfg = { 0, 0, 0 }; - - /* Search for user gain feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&gyr_user_gain_cfg, BMI2_GYRO_GAIN_UPDATE, dev); - if (feat_found) - { - /* Get the configuration from the page where user gain feature resides */ - rslt = bmi2_get_feat_config(gyr_user_gain_cfg.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of user gain */ - idx = gyr_user_gain_cfg.start_addr + BMI2_GYR_USER_GAIN_FEAT_EN_OFFSET; - - /* Set the feature enable bit */ - feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_GYR_USER_GAIN_FEAT_EN, enable); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API sets any-motion configurations like axes select, - * duration, threshold and output-configuration. - */ -static int8_t set_any_motion_config(const struct bmi2_any_motion_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define count */ - uint8_t index = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for any motion */ - struct bmi2_feature_config any_mot_config = { 0, 0, 0 }; - - /* Copy the feature configuration address to a local pointer */ - uint16_t *data_p = (uint16_t *) (void *)feat_config; - - /* Search for any-motion feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&any_mot_config, BMI2_ANY_MOTION, dev); - if (feat_found) - { - /* Get the configuration from the page where any-motion feature resides */ - rslt = bmi2_get_feat_config(any_mot_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for any-motion select */ - idx = any_mot_config.start_addr; - - /* Get offset in words since all the features are set in words length */ - idx = idx / 2; - - /* Set duration */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_ANY_NO_MOT_DUR, config->duration); - - /* Set x-select */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_ANY_NO_MOT_X_SEL, config->select_x); - - /* Set y-select */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_ANY_NO_MOT_Y_SEL, config->select_y); - - /* Set z-select */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_ANY_NO_MOT_Z_SEL, config->select_z); - - /* Increment offset by 1 word to set threshold and output configuration */ - idx++; - - /* Set threshold */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_ANY_NO_MOT_THRES, config->threshold); - - /* Increment offset by 1 more word to get the total length in words */ - idx++; - - /* Get total length in bytes to copy from local pointer to the array */ - idx = (uint8_t)(idx * 2) - any_mot_config.start_addr; - - /* Copy the bytes to be set back to the array */ - for (index = 0; index < idx; index++) - { - feat_config[any_mot_config.start_addr + - index] = *((uint8_t *) data_p + any_mot_config.start_addr + index); - } - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API sets no-motion configurations like axes select, - * duration, threshold and output-configuration. - */ -static int8_t set_no_motion_config(const struct bmi2_no_motion_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define count */ - uint8_t index = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for no-motion */ - struct bmi2_feature_config no_mot_config = { 0, 0, 0 }; - - /* Copy the feature configuration address to a local pointer */ - uint16_t *data_p = (uint16_t *) (void *)feat_config; - - /* Search for no-motion feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&no_mot_config, BMI2_NO_MOTION, dev); - if (feat_found) - { - /* Get the configuration from the page where no-motion feature resides */ - rslt = bmi2_get_feat_config(no_mot_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for no-motion select */ - idx = no_mot_config.start_addr; - - /* Get offset in words since all the features are set in words length */ - idx = idx / 2; - - /* Set duration */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_ANY_NO_MOT_DUR, config->duration); - - /* Set x-select */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_ANY_NO_MOT_X_SEL, config->select_x); - - /* Set y-select */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_ANY_NO_MOT_Y_SEL, config->select_y); - - /* Set z-select */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_ANY_NO_MOT_Z_SEL, config->select_z); - - /* Increment offset by 1 word to set threshold and output configuration */ - idx++; - - /* Set threshold */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_ANY_NO_MOT_THRES, config->threshold); - - /* Increment offset by 1 more word to get the total length in words */ - idx++; - - /* Get total length in bytes to copy from local pointer to the array */ - idx = (uint8_t)(idx * 2) - no_mot_config.start_addr; - - /* Copy the bytes to be set back to the array */ - for (index = 0; index < idx; index++) - { - feat_config[no_mot_config.start_addr + - index] = *((uint8_t *) data_p + no_mot_config.start_addr + index); - } - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API sets step counter parameter configurations. - */ -static int8_t set_step_count_params_config(const uint16_t *step_count_params, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt = BMI2_OK; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define index */ - uint8_t index = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for step counter parameters */ - struct bmi2_feature_config step_params_config = { 0, 0, 0 }; - - /* Variable to index the page number */ - uint8_t page_idx; - - /* Variable to define the start page */ - uint8_t start_page; - - /* Variable to define start address of the parameters */ - uint8_t start_addr; - - /* Variable to define number of bytes */ - uint8_t n_bytes = (BMI2_STEP_CNT_N_PARAMS * 2); - - /* Variable to store number of pages */ - uint8_t n_pages = (n_bytes / 16); - - /* Variable to define the end page */ - uint8_t end_page; - - /* Variable to define the remaining bytes to be read */ - uint8_t remain_len; - - /* Variable to define the maximum words(16 bytes or 8 words) to be read in a page */ - uint8_t max_len = 8; - - /* Variable index bytes in a page */ - uint8_t page_byte_idx; - - /* Variable to index the parameters */ - uint8_t param_idx = 0; - - /* Copy the feature configuration address to a local pointer */ - uint16_t *data_p = (uint16_t *) (void *)feat_config; - - /* Search for step counter parameter feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&step_params_config, BMI2_STEP_COUNTER_PARAMS, dev); - if (feat_found) - { - /* Get the start page for the step counter parameters */ - start_page = step_params_config.page; - - /* Get the end page for the step counter parameters */ - end_page = start_page + n_pages; - - /* Get the start address for the step counter parameters */ - start_addr = step_params_config.start_addr; - - /* Get the remaining length of bytes to be read */ - remain_len = (uint8_t)((n_bytes - (n_pages * 16)) + start_addr); - for (page_idx = start_page; page_idx <= end_page; page_idx++) - { - /* Get the configuration from the respective page */ - rslt = bmi2_get_feat_config(page_idx, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Start from address 0x00 when switched to next page */ - if (page_idx > start_page) - { - start_addr = 0; - } - - /* Remaining number of words to be read in the page */ - if (page_idx == end_page) - { - max_len = (remain_len / 2); - } - - /* Get offset in words since all the features are set in words length */ - page_byte_idx = start_addr / 2; - for (; page_byte_idx < max_len;) - { - /* Set parameters 1 to 25 */ - *(data_p + page_byte_idx) = BMI2_SET_BIT_POS0(*(data_p + page_byte_idx), - BMI2_STEP_COUNT_PARAMS, - step_count_params[param_idx]); - - /* Increment offset by 1 word to set to the next parameter */ - page_byte_idx++; - - /* Increment to next parameter */ - param_idx++; - } - - /* Get total length in bytes to copy from local pointer to the array */ - page_byte_idx = (uint8_t)(page_byte_idx * 2) - step_params_config.start_addr; - - /* Copy the bytes to be set back to the array */ - for (index = 0; index < page_byte_idx; index++) - { - feat_config[step_params_config.start_addr + - index] = *((uint8_t *) data_p + step_params_config.start_addr + index); - } - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/* @brief This internal API sets step counter configurations like water-mark - * level, reset-counter and output-configuration step detector and activity. - */ -static int8_t set_step_config(const struct bmi2_step_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define index */ - uint8_t index = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for step counter 4 */ - struct bmi2_feature_config step_count_config = { 0, 0, 0 }; - - /* Copy the feature configuration address to a local pointer */ - uint16_t *data_p = (uint16_t *) (void *)feat_config; - - /* Search for step counter feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&step_count_config, BMI2_STEP_COUNTER, dev); - if (feat_found) - { - /* Get the configuration from the page where step counter resides */ - rslt = bmi2_get_feat_config(step_count_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes */ - idx = step_count_config.start_addr; - - /* Get offset in words since all the features are set in words length */ - idx = idx / 2; - - /* Set water-mark level */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_STEP_COUNT_WM_LEVEL, config->watermark_level); - - /* Set reset-counter */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_STEP_COUNT_RST_CNT, config->reset_counter); - - /* Increment offset by 1 word to set output - * configuration of step detector and step activity - */ - idx++; - - /* Set step buffer size */ - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_STEP_BUFFER_SIZE, config->step_buffer_size); - - /* Increment offset by 1 more word to get the total length in words */ - idx++; - - /* Get total length in bytes to copy from local pointer to the array */ - idx = (uint8_t)(idx * 2) - step_count_config.start_addr; - - /* Copy the bytes to be set back to the array */ - for (index = 0; index < idx; index++) - { - feat_config[step_count_config.start_addr + - index] = *((uint8_t *) data_p + step_count_config.start_addr + index); - } - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API sets wrist wear wake-up configurations like - * output-configuration for wearable variant. - */ -static int8_t set_wrist_wear_wake_up_wh_config(const struct bmi2_wrist_wear_wake_up_wh_config *config, - struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define index */ - uint8_t index = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for wrist wear wake-up */ - struct bmi2_feature_config wrist_wake_up_config = { 0, 0, 0 }; - - /* Copy the feature configuration address to a local pointer */ - uint16_t *data_p = (uint16_t *) (void *)feat_config; - - /* Search for wrist wear wake-up feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&wrist_wake_up_config, BMI2_WRIST_WEAR_WAKE_UP_WH, dev); - if (feat_found) - { - /* Get the configuration from the page where wrist wear wake-up feature resides */ - rslt = bmi2_get_feat_config(wrist_wake_up_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for wrist wear wake-up select */ - idx = wrist_wake_up_config.start_addr; - - /* Get offset in words since all the features are set in words length */ - idx = idx / 2; - - *(data_p + idx) = config->min_angle_focus; - - /* Increment offset by 1 more word to set min_angle_nonfocus */ - idx++; - *(data_p + idx) = config->min_angle_nonfocus; - - /* Increment offset by 1 more word to set angle landscape right and angle landscape left */ - idx++; - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_WRIST_WAKE_UP_ANGLE_LR, config->angle_lr); - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_WRIST_WAKE_UP_ANGLE_LL, config->angle_ll); - - /* Increment offset by 1 more word to set angle portrait down and angle portrait left */ - idx++; - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_WRIST_WAKE_UP_ANGLE_PD, config->angle_pd); - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_WRIST_WAKE_UP_ANGLE_PU, config->angle_pu); - - /* Increment offset by 1 more word to set min duration moved and min duration quite */ - idx++; - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_WRIST_WAKE_UP_MIN_DUR_MOVED, config->min_dur_mov); - *(data_p + idx) = BMI2_SET_BITS(*(data_p + idx), BMI2_WRIST_WAKE_UP_MIN_DUR_QUITE, config->min_dur_quite); - - /* Increment offset by 1 more word to get the total length in words */ - idx++; - - /* Get total length in bytes to copy from local pointer to the array */ - idx = (uint8_t)(idx * 2) - wrist_wake_up_config.start_addr; - - /* Copy the bytes to be set back to the array */ - for (index = 0; index < idx; index++) - { - feat_config[wrist_wake_up_config.start_addr + - index] = *((uint8_t *) data_p + wrist_wake_up_config.start_addr + index); - } - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets any-motion configurations like axes select, - * duration, threshold and output-configuration. - */ -static int8_t get_any_motion_config(struct bmi2_any_motion_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define LSB */ - uint16_t lsb; - - /* Variable to define MSB */ - uint16_t msb; - - /* Variable to define a word */ - uint16_t lsb_msb; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for any-motion */ - struct bmi2_feature_config any_mot_config = { 0, 0, 0 }; - - /* Search for any-motion feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&any_mot_config, BMI2_ANY_MOTION, dev); - if (feat_found) - { - /* Get the configuration from the page where any-motion feature resides */ - rslt = bmi2_get_feat_config(any_mot_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for feature enable for any-motion */ - idx = any_mot_config.start_addr; - - /* Get word to calculate duration, x, y and z select */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get duration */ - config->duration = lsb_msb & BMI2_ANY_NO_MOT_DUR_MASK; - - /* Get x-select */ - config->select_x = (lsb_msb & BMI2_ANY_NO_MOT_X_SEL_MASK) >> BMI2_ANY_NO_MOT_X_SEL_POS; - - /* Get y-select */ - config->select_y = (lsb_msb & BMI2_ANY_NO_MOT_Y_SEL_MASK) >> BMI2_ANY_NO_MOT_Y_SEL_POS; - - /* Get z-select */ - config->select_z = (lsb_msb & BMI2_ANY_NO_MOT_Z_SEL_MASK) >> BMI2_ANY_NO_MOT_Z_SEL_POS; - - /* Get word to calculate threshold, output configuration from the same word */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get threshold */ - config->threshold = lsb_msb & BMI2_ANY_NO_MOT_THRES_MASK; - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets no-motion configurations like axes select, - * duration, threshold and output-configuration. - */ -static int8_t get_no_motion_config(struct bmi2_no_motion_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define LSB */ - uint16_t lsb = 0; - - /* Variable to define MSB */ - uint16_t msb = 0; - - /* Variable to define a word */ - uint16_t lsb_msb = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for no-motion */ - struct bmi2_feature_config no_mot_config = { 0, 0, 0 }; - - /* Search for no-motion feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&no_mot_config, BMI2_NO_MOTION, dev); - if (feat_found) - { - /* Get the configuration from the page where no-motion feature resides */ - rslt = bmi2_get_feat_config(no_mot_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for feature enable for no-motion */ - idx = no_mot_config.start_addr; - - /* Get word to calculate duration, x, y and z select */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get duration */ - config->duration = lsb_msb & BMI2_ANY_NO_MOT_DUR_MASK; - - /* Get x-select */ - config->select_x = (lsb_msb & BMI2_ANY_NO_MOT_X_SEL_MASK) >> BMI2_ANY_NO_MOT_X_SEL_POS; - - /* Get y-select */ - config->select_y = (lsb_msb & BMI2_ANY_NO_MOT_Y_SEL_MASK) >> BMI2_ANY_NO_MOT_Y_SEL_POS; - - /* Get z-select */ - config->select_z = (lsb_msb & BMI2_ANY_NO_MOT_Z_SEL_MASK) >> BMI2_ANY_NO_MOT_Z_SEL_POS; - - /* Get word to calculate threshold, output configuration from the same word */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get threshold */ - config->threshold = lsb_msb & BMI2_ANY_NO_MOT_THRES_MASK; - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets step counter parameter configurations. - */ -static int8_t get_step_count_params_config(uint16_t *step_count_params, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt = BMI2_OK; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Variable to define LSB */ - uint16_t lsb = 0; - - /* Variable to define MSB */ - uint16_t msb = 0; - - /* Variable to define a word */ - uint16_t lsb_msb = 0; - - /* Initialize feature configuration for step counter 1 */ - struct bmi2_feature_config step_params_config = { 0, 0, 0 }; - - /* Variable to index the page number */ - uint8_t page_idx; - - /* Variable to define the start page */ - uint8_t start_page; - - /* Variable to define start address of the parameters */ - uint8_t start_addr; - - /* Variable to define number of bytes */ - uint8_t n_bytes = (BMI2_STEP_CNT_N_PARAMS * 2); - - /* Variable to store number of pages */ - uint8_t n_pages = (n_bytes / 16); - - /* Variable to define the end page */ - uint8_t end_page; - - /* Variable to define the remaining bytes to be read */ - uint8_t remain_len; - - /* Variable to define the maximum words to be read in a page */ - uint8_t max_len = BMI2_FEAT_SIZE_IN_BYTES; - - /* Variable index bytes in a page */ - uint8_t page_byte_idx; - - /* Variable to index the parameters */ - uint8_t param_idx = 0; - - /* Search for step counter parameter feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&step_params_config, BMI2_STEP_COUNTER_PARAMS, dev); - if (feat_found) - { - /* Get the start page for the step counter parameters */ - start_page = step_params_config.page; - - /* Get the end page for the step counter parameters */ - end_page = start_page + n_pages; - - /* Get the start address for the step counter parameters */ - start_addr = step_params_config.start_addr; - - /* Get the remaining length of bytes to be read */ - remain_len = (uint8_t)((n_bytes - (n_pages * 16)) + start_addr); - for (page_idx = start_page; page_idx <= end_page; page_idx++) - { - /* Get the configuration from the respective page */ - rslt = bmi2_get_feat_config(page_idx, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Start from address 0x00 when switched to next page */ - if (page_idx > start_page) - { - start_addr = 0; - } - - /* Remaining number of bytes to be read in the page */ - if (page_idx == end_page) - { - max_len = remain_len; - } - - /* Get the offset */ - page_byte_idx = start_addr; - while (page_byte_idx < max_len) - { - /* Get word to calculate the parameter*/ - lsb = (uint16_t) feat_config[page_byte_idx++]; - if (page_byte_idx < max_len) - { - msb = ((uint16_t) feat_config[page_byte_idx++] << 8); - } - - lsb_msb = lsb | msb; - - /* Get parameters 1 to 25 */ - step_count_params[param_idx] = lsb_msb & BMI2_STEP_COUNT_PARAMS_MASK; - - /* Increment to next parameter */ - param_idx++; - } - } - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets step counter/detector/activity configurations. - */ -static int8_t get_step_config(struct bmi2_step_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define LSB */ - uint16_t lsb = 0; - - /* Variable to define MSB */ - uint16_t msb = 0; - - /* Variable to define a word */ - uint16_t lsb_msb = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for step counter */ - struct bmi2_feature_config step_count_config = { 0, 0, 0 }; - - /* Search for step counter 4 feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&step_count_config, BMI2_STEP_COUNTER, dev); - if (feat_found) - { - /* Get the configuration from the page where step counter 4 parameter resides */ - rslt = bmi2_get_feat_config(step_count_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for feature enable for step counter/detector/activity */ - idx = step_count_config.start_addr; - - /* Get word to calculate water-mark level and reset counter */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get water-mark level */ - config->watermark_level = lsb_msb & BMI2_STEP_COUNT_WM_LEVEL_MASK; - - /* Get reset counter */ - config->reset_counter = (lsb_msb & BMI2_STEP_COUNT_RST_CNT_MASK) >> BMI2_STEP_COUNT_RST_CNT_POS; - - /* Get word to calculate output configuration of step detector and activity */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - config->step_buffer_size = (lsb_msb & BMI2_STEP_BUFFER_SIZE_MASK) >> BMI2_STEP_BUFFER_SIZE_POS; - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets wrist wear wake-up configurations like min_angle_focus, - * min_angle_nonfocus for wearable variant. - */ -static int8_t get_wrist_wear_wake_up_wh_config(struct bmi2_wrist_wear_wake_up_wh_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for wrist wear wake-up */ - struct bmi2_feature_config wrist_wake_up_config = { 0, 0, 0 }; - - /* Copy the feature configuration address to a local pointer */ - uint16_t *data_p = (uint16_t *) (void *)feat_config; - - /* Search for wrist wear wake-up feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&wrist_wake_up_config, BMI2_WRIST_WEAR_WAKE_UP_WH, dev); - if (feat_found) - { - /* Get the configuration from the page where wrist wear wake-up feature resides */ - rslt = bmi2_get_feat_config(wrist_wake_up_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for wrist wear wake-up select */ - idx = wrist_wake_up_config.start_addr; - - /* Get offset in words since all the features are set in words length */ - idx = idx / 2; - - config->min_angle_focus = *(data_p + idx); - - /* Increment the offset value by 1 word to get min_angle_nonfocus */ - idx++; - config->min_angle_nonfocus = *(data_p + idx); - - /* Increment the offset value by 1 word to get angle landscape right and angle landscape left */ - idx++; - config->angle_lr = BMI2_GET_BIT_POS0(*(data_p + idx), BMI2_WRIST_WAKE_UP_ANGLE_LR); - config->angle_ll = BMI2_GET_BITS(*(data_p + idx), BMI2_WRIST_WAKE_UP_ANGLE_LL); - - /* Increment the offset value by 1 word to get angle portrait down and angle portrait up */ - idx++; - config->angle_pd = BMI2_GET_BIT_POS0(*(data_p + idx), BMI2_WRIST_WAKE_UP_ANGLE_PD); - config->angle_pu = BMI2_GET_BITS(*(data_p + idx), BMI2_WRIST_WAKE_UP_ANGLE_PU); - - /* Increment the offset value by 1 word to get min duration quite and min duration moved */ - idx++; - config->min_dur_mov = BMI2_GET_BIT_POS0(*(data_p + idx), BMI2_WRIST_WAKE_UP_MIN_DUR_MOVED); - config->min_dur_quite = BMI2_GET_BITS(*(data_p + idx), BMI2_WRIST_WAKE_UP_MIN_DUR_QUITE); - - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets the output values of step counter. - */ -static int8_t get_step_counter_output(uint32_t *step_count, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variables to define index */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature output for step counter */ - struct bmi2_feature_config step_cnt_out_config = { 0, 0, 0 }; - - /* Search for step counter output feature and extract its configuration details */ - feat_found = extract_output_feat_config(&step_cnt_out_config, BMI2_STEP_COUNTER, dev); - if (feat_found) - { - /* Get the feature output configuration for step-counter */ - rslt = bmi2_get_feat_config(step_cnt_out_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for step counter output */ - idx = step_cnt_out_config.start_addr; - - /* Get the step counter output in 4 bytes */ - *step_count = (uint32_t) feat_config[idx++]; - *step_count |= ((uint32_t) feat_config[idx++] << 8); - *step_count |= ((uint32_t) feat_config[idx++] << 16); - *step_count |= ((uint32_t) feat_config[idx++] << 24); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets the error status related to NVM. - */ -static int8_t get_nvm_error_status(struct bmi2_nvm_err_status *nvm_err_stat, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variables to define index */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature output for NVM error status */ - struct bmi2_feature_config nvm_err_cfg = { 0, 0, 0 }; - - /* Search for NVM error status feature and extract its configuration details */ - feat_found = extract_output_feat_config(&nvm_err_cfg, BMI2_NVM_STATUS, dev); - if (feat_found) - { - /* Get the feature output configuration for NVM error status */ - rslt = bmi2_get_feat_config(nvm_err_cfg.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for NVM error status */ - idx = nvm_err_cfg.start_addr; - - /* Increment index to get the error status */ - idx++; - - /* Error when NVM load action fails */ - nvm_err_stat->load_error = BMI2_GET_BIT_POS0(feat_config[idx], BMI2_NVM_LOAD_ERR_STATUS); - - /* Error when NVM program action fails */ - nvm_err_stat->prog_error = BMI2_GET_BITS(feat_config[idx], BMI2_NVM_PROG_ERR_STATUS); - - /* Error when NVM erase action fails */ - nvm_err_stat->erase_error = BMI2_GET_BITS(feat_config[idx], BMI2_NVM_ERASE_ERR_STATUS); - - /* Error when NVM program limit is exceeded */ - nvm_err_stat->exceed_error = BMI2_GET_BITS(feat_config[idx], BMI2_NVM_END_EXCEED_STATUS); - - /* Error when NVM privilege mode is not acquired */ - nvm_err_stat->privil_error = BMI2_GET_BITS(feat_config[idx], BMI2_NVM_PRIV_ERR_STATUS); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API is used to get enable status of gyroscope user gain - * update. - */ -static int8_t get_user_gain_upd_status(uint8_t *status, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt = BMI2_OK; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Variable to check APS status */ - uint8_t aps_stat = 0; - - /* Initialize feature configuration for gyroscope user gain */ - struct bmi2_feature_config gyr_user_gain_cfg = { 0, 0, 0 }; - - /* Search for user gain feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&gyr_user_gain_cfg, BMI2_GYRO_GAIN_UPDATE, dev); - if (feat_found) - { - /* Disable advance power save */ - aps_stat = dev->aps_status; - if (aps_stat == BMI2_ENABLE) - { - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - } - - if (rslt == BMI2_OK) - { - /* Get the configuration from the page where user gain feature resides */ - rslt = bmi2_get_feat_config(gyr_user_gain_cfg.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for enable/disable of user gain */ - idx = gyr_user_gain_cfg.start_addr + BMI2_GYR_USER_GAIN_FEAT_EN_OFFSET; - - /* Set the feature enable status */ - *status = BMI2_GET_BITS(feat_config[idx], BMI2_GYR_USER_GAIN_FEAT_EN); - } - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - /* Enable Advance power save if disabled while configuring and not when already disabled */ - if ((rslt == BMI2_OK) && (aps_stat == BMI2_ENABLE)) - { - rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - } - - return rslt; -} - -/*! - * @brief This internal API gets the output values of step activity. - */ -static int8_t get_step_activity_output(uint8_t *step_act, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variables to define index */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature output for step activity */ - struct bmi2_feature_config step_act_out_config = { 0, 0, 0 }; - - /* Search for step activity output feature and extract its configuration details */ - feat_found = extract_output_feat_config(&step_act_out_config, BMI2_STEP_ACTIVITY, dev); - if (feat_found) - { - /* Get the feature output configuration for step-activity */ - rslt = bmi2_get_feat_config(step_act_out_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for step activity output */ - idx = step_act_out_config.start_addr; - - /* Get the step activity output */ - *step_act = feat_config[idx]; - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets the error status related to virtual frames. - */ -static int8_t get_vfrm_error_status(struct bmi2_vfrm_err_status *vfrm_err_stat, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variables to define index */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature output for VFRM error status */ - struct bmi2_feature_config vfrm_err_cfg = { 0, 0, 0 }; - - /* Search for VFRM error status feature and extract its configuration details */ - feat_found = extract_output_feat_config(&vfrm_err_cfg, BMI2_VFRM_STATUS, dev); - if (feat_found) - { - /* Get the feature output configuration for VFRM error status */ - rslt = bmi2_get_feat_config(vfrm_err_cfg.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for VFRM error status */ - idx = vfrm_err_cfg.start_addr; - - /* Increment index to get the error status */ - idx++; - - /* Internal error while acquiring lock for FIFO */ - vfrm_err_stat->lock_error = BMI2_GET_BITS(feat_config[idx], BMI2_VFRM_LOCK_ERR_STATUS); - - /* Internal error while writing byte into FIFO */ - vfrm_err_stat->write_error = BMI2_GET_BITS(feat_config[idx], BMI2_VFRM_WRITE_ERR_STATUS); - - /* Internal error while writing into FIFO */ - vfrm_err_stat->fatal_error = BMI2_GET_BITS(feat_config[idx], BMI2_VFRM_FATAL_ERR_STATUS); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API enables/disables compensation of the gain defined - * in the GAIN register. - */ -static int8_t enable_gyro_gain(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define register data */ - uint8_t reg_data = 0; - - rslt = bmi2_get_regs(BMI2_GYR_OFF_COMP_6_ADDR, ®_data, 1, dev); - if (rslt == BMI2_OK) - { - reg_data = BMI2_SET_BITS(reg_data, BMI2_GYR_GAIN_EN, enable); - rslt = bmi2_set_regs(BMI2_GYR_OFF_COMP_6_ADDR, ®_data, 1, dev); - } - - return rslt; -} - -/*! - * @brief This internal API is used to extract the output feature configuration - * details from the look-up table. - */ -static uint8_t extract_output_feat_config(struct bmi2_feature_config *feat_output, - uint8_t type, - const struct bmi2_dev *dev) -{ - /* Variable to define loop */ - uint8_t loop = 0; - - /* Variable to set flag */ - uint8_t feat_found = BMI2_FALSE; - - /* Search for the output feature from the output configuration array */ - while (loop < dev->out_sens) - { - if (dev->feat_output[loop].type == type) - { - *feat_output = dev->feat_output[loop]; - feat_found = BMI2_TRUE; - break; - } - - loop++; - } - - /* Return flag */ - return feat_found; -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_wh.h b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_wh.h deleted file mode 100644 index d46251bbd1..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_wh.h +++ /dev/null @@ -1,402 +0,0 @@ -/** -* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. -* -* BSD-3-Clause -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* 1. Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* -* 2. Redistributions in binary form must reproduce the above copyright -* notice, this list of conditions and the following disclaimer in the -* documentation and/or other materials provided with the distribution. -* -* 3. Neither the name of the copyright holder nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) -* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, -* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING -* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -* -* @file bmi270_wh.h -* @date 2020-11-04 -* @version v2.63.1 -* -*/ - -/** - * \ingroup bmi2xy - * \defgroup bmi270_wh BMI270_WH - * @brief Sensor driver for BMI270_WH sensor - */ - -#ifndef BMI270_WH_H_ -#define BMI270_WH_H_ - -/*! CPP guard */ -#ifdef __cplusplus -extern "C" { -#endif - -/***************************************************************************/ - -/*! Header files - ****************************************************************************/ -#include "bmi2.h" - -/***************************************************************************/ - -/*! Macro definitions - ****************************************************************************/ - -/*! @name BMI270_WH Chip identifier */ -#define BMI270_WH_CHIP_ID UINT8_C(0x24) - -/*! @name BMI270_WH feature input start addresses */ -#define BMI270_WH_CONFIG_ID_STRT_ADDR UINT8_C(0x00) -#define BMI270_WH_MAX_BURST_LEN_STRT_ADDR UINT8_C(0x02) -#define BMI270_WH_AXIS_MAP_STRT_ADDR UINT8_C(0x04) -#define BMI270_WH_NVM_PROG_PREP_STRT_ADDR UINT8_C(0x05) -#define BMI270_WH_GYRO_GAIN_UPDATE_STRT_ADDR UINT8_C(0x06) -#define BMI270_WH_ANY_MOT_STRT_ADDR UINT8_C(0x0C) -#define BMI270_WH_NO_MOT_STRT_ADDR UINT8_C(0x00) -#define BMI270_WH_STEP_CNT_1_STRT_ADDR UINT8_C(0x00) -#define BMI270_WH_STEP_CNT_4_STRT_ADDR UINT8_C(0x02) -#define BMI270_WH_WRIST_WEAR_WAKE_UP_STRT_ADDR UINT8_C(0x04) - -/*! @name BMI270_WH feature output start addresses */ -#define BMI270_WH_STEP_CNT_OUT_STRT_ADDR UINT8_C(0x00) -#define BMI270_WH_STEP_ACT_OUT_STRT_ADDR UINT8_C(0x04) -#define BMI270_WH_WRIST_GEST_OUT_STRT_ADDR UINT8_C(0x06) -#define BMI270_WH_GYR_USER_GAIN_OUT_STRT_ADDR UINT8_C(0x08) -#define BMI270_WH_GYRO_CROSS_SENSE_STRT_ADDR UINT8_C(0x0C) -#define BMI270_WH_NVM_VFRM_OUT_STRT_ADDR UINT8_C(0x0E) - -/*! @name Defines maximum number of pages */ -#define BMI270_WH_MAX_PAGE_NUM UINT8_C(8) - -/*! @name Defines maximum number of feature input configurations */ -#define BMI270_WH_MAX_FEAT_IN UINT8_C(12) - -/*! @name Defines maximum number of feature outputs */ -#define BMI270_WH_MAX_FEAT_OUT UINT8_C(6) - -/*! @name Mask definitions for feature interrupt status bits */ -#define BMI270_WH_STEP_CNT_STATUS_MASK UINT8_C(0x02) -#define BMI270_WH_STEP_ACT_STATUS_MASK UINT8_C(0x04) -#define BMI270_WH_WRIST_WEAR_WAKEUP_WH_STATUS_MASK UINT8_C(0x08) -#define BMI270_WH_NO_MOT_STATUS_MASK UINT8_C(0x20) -#define BMI270_WH_ANY_MOT_STATUS_MASK UINT8_C(0x40) - -/*! @name Mask definitions for feature interrupt mapping bits */ -#define BMI270_WH_INT_STEP_COUNTER_MASK UINT8_C(0x02) -#define BMI270_WH_INT_STEP_DETECTOR_MASK UINT8_C(0x02) -#define BMI270_WH_INT_STEP_ACT_MASK UINT8_C(0x04) -#define BMI270_WH_INT_WRIST_WEAR_WAKEUP_WH_MASK UINT8_C(0x08) -#define BMI270_WH_INT_NO_MOT_MASK UINT8_C(0x20) -#define BMI270_WH_INT_ANY_MOT_MASK UINT8_C(0x40) - -/*! @name Defines maximum number of feature interrupts */ -#define BMI270_WH_MAX_INT_MAP UINT8_C(6) - -/***************************************************************************/ - -/*! BMI270_WH User Interface function prototypes - ****************************************************************************/ - -/** - * \ingroup bmi270_wh - * \defgroup bmi270_whApiInit Initialization - * @brief Initialize the sensor and device structure - */ - -/*! - * \ingroup bmi270_whApiInit - * \page bmi270_wh_api_bmi270_wh_init bmi270_wh_init - * \code - * int8_t bmi270_wh_init(struct bmi2_dev *dev); - * \endcode - * @details This API: - * 1) updates the device structure with address of the configuration file. - * 2) Initializes BMI270_WH sensor. - * 3) Writes the configuration file. - * 4) Updates the feature offset parameters in the device structure. - * 5) Updates the maximum number of pages, in the device structure. - * - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi270_wh_init(struct bmi2_dev *dev); - -/** - * \ingroup bmi270_wh - * \defgroup bmi270_whApiSensor Feature Set - * @brief Enable / Disable features of the sensor - */ - -/*! - * \ingroup bmi270_whApiSensor - * \page bmi270_wh_api_bmi270_wh_sensor_enable bmi270_wh_sensor_enable - * \code - * int8_t bmi270_wh_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); - * \endcode - * @details This API selects the sensors/features to be enabled. - * - * @param[in] sens_list : Pointer to select the sensor/feature. - * @param[in] n_sens : Number of sensors selected. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @note Sensors/features that can be enabled. - * - *@verbatim - * sens_list | Values - * ----------------------------|----------- - * BMI2_ACCEL | 0 - * BMI2_GYRO | 1 - * BMI2_AUX | 2 - * BMI2_ANY_MOTION | 4 - * BMI2_NO_MOTION | 5 - * BMI2_STEP_DETECTOR | 6 - * BMI2_STEP_COUNTER | 7 - * BMI2_STEP_ACTIVITY | 8 - * BMI2_GYRO_GAIN_UPDATE | 9 - * BMI2_WRIST_WEAR_WAKE_UP_WH | 21 - *@endverbatim - * - * @note : - * example uint8_t sens_list[2] = {BMI2_ACCEL, BMI2_GYRO}; - * uint8_t n_sens = 2; - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi270_wh_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); - -/*! - * \ingroup bmi270_whApiSensor - * \page bmi270_wh_api_bmi270_wh_sensor_disable bmi270_wh_sensor_disable - * \code - * int8_t bmi270_wh_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); - * \endcode - * @details This API selects the sensors/features to be disabled. - * - * @param[in] sens_list : Pointer to select the sensor/feature. - * @param[in] n_sens : Number of sensors selected. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @note Sensors/features that can be disabled. - * - *@verbatim - * sens_list | Values - * ----------------------------|----------- - * BMI2_ACCEL | 0 - * BMI2_GYRO | 1 - * BMI2_AUX | 2 - * BMI2_ANY_MOTION | 4 - * BMI2_NO_MOTION | 5 - * BMI2_STEP_DETECTOR | 6 - * BMI2_STEP_COUNTER | 7 - * BMI2_STEP_ACTIVITY | 8 - * BMI2_GYRO_GAIN_UPDATE | 9 - * BMI2_WRIST_WEAR_WAKE_UP_WH | 21 - *@endverbatim - * - * @note : - * example uint8_t sens_list[2] = {BMI2_ACCEL, BMI2_GYRO}; - * uint8_t n_sens = 2; - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi270_wh_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); - -/** - * \ingroup bmi270_wh - * \defgroup bmi270_whApiSensorC Sensor Configuration - * @brief Enable / Disable feature configuration of the sensor - */ - -/*! - * \ingroup bmi270_whApiSensorC - * \page bmi270_wh_api_bmi270_wh_set_sensor_config bmi270_wh_set_sensor_config - * \code - * int8_t bmi270_wh_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); - * \endcode - * @details This API sets the sensor/feature configuration. - * - * @param[in] sens_cfg : Structure instance of bmi2_sens_config. - * @param[in] n_sens : Number of sensors selected. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @note Sensors/features that can be configured - * - *@verbatim - * sens_list | Values - * ----------------------------|----------- - * BMI2_ANY_MOTION | 4 - * BMI2_NO_MOTION | 5 - * BMI2_STEP_DETECTOR | 6 - * BMI2_STEP_COUNTER | 7 - * BMI2_STEP_ACTIVITY | 8 - * BMI2_WRIST_WEAR_WAKE_UP_WH | 21 - * BMI2_STEP_COUNTER_PARAMS | 28 - *@endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi270_wh_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); - -/*! - * \ingroup bmi270_whApiSensorC - * \page bmi270_wh_api_bmi270_wh_get_sensor_config bmi270_wh_get_sensor_config - * \code - * int8_t bmi270_wh_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); - * \endcode - * @details This API gets the sensor/feature configuration. - * - * @param[in] sens_cfg : Structure instance of bmi2_sens_config. - * @param[in] n_sens : Number of sensors selected. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @note Sensors/features whose configurations can be read. - * - *@verbatim - * sens_list | Values - * ----------------------------|----------- - * BMI2_ANY_MOTION | 4 - * BMI2_NO_MOTION | 5 - * BMI2_STEP_DETECTOR | 6 - * BMI2_STEP_COUNTER | 7 - * BMI2_STEP_ACTIVITY | 8 - * BMI2_WRIST_WEAR_WAKE_UP_WH | 21 - * BMI2_STEP_COUNTER_PARAMS | 28 - *@endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi270_wh_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); - -/** - * \ingroup bmi270_wh - * \defgroup bmi270_whApiSensorD Sensor Data - * @brief Get sensor data - */ - -/*! - * \ingroup bmi270_whApiSensorD - * \page bmi270_wh_api_bmi270_wh_get_sensor_data bmi270_wh_get_sensor_data - * \code - * int8_t bmi270_wh_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev); - * \endcode - * @details This API gets the sensor/feature data for accelerometer, gyroscope, - * auxiliary sensor, step counter, high-g, gyroscope user-gain update, - * orientation, gyroscope cross sensitivity and error status for NVM and VFRM. - * - * @param[out] sensor_data : Structure instance of bmi2_sensor_data. - * @param[in] n_sens : Number of sensors selected. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @note Sensors/features whose data can be read - * - *@verbatim - * sens_list | Values - * ---------------------|----------- - * BMI2_STEP_COUNTER | 7 - * BMI2_STEP_ACTIVITY | 8 - * BMI2_NVM_STATUS | 38 - * BMI2_VFRM_STATUS | 39 - *@endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi270_wh_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev); - -/** - * \ingroup bmi270_wh - * \defgroup bmi270_whApiGyroUG Gyro User Gain - * @brief Set / Get Gyro User Gain of the sensor - */ - -/*! - * \ingroup bmi270_whApiGyroUG - * \page bmi270_wh_api_bmi270_wh_update_gyro_user_gain bmi270_wh_update_gyro_user_gain - * \code - * int8_t bmi270_wh_update_gyro_user_gain(const struct bmi2_gyro_user_gain_config *user_gain, struct bmi2_dev *dev); - * \endcode - * @details This API updates the gyroscope user-gain. - * - * @param[in] user_gain : Structure that stores user-gain configurations. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi270_wh_update_gyro_user_gain(const struct bmi2_gyro_user_gain_config *user_gain, struct bmi2_dev *dev); - -/*! - * \ingroup bmi270_whApiGyroUG - * \page bmi270_wh_api_bmi270_wh_read_gyro_user_gain bmi270_wh_read_gyro_user_gain - * \code - * int8_t bmi270_wh_read_gyro_user_gain(struct bmi2_gyro_user_gain_data *gyr_usr_gain, const struct bmi2_dev *dev); - * \endcode - * @details This API reads the compensated gyroscope user-gain values. - * - * @param[out] gyr_usr_gain : Structure that stores gain values. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi270_wh_read_gyro_user_gain(struct bmi2_gyro_user_gain_data *gyr_usr_gain, struct bmi2_dev *dev); - -/*! - * \ingroup bmi270_whApiInt - * \page bmi270_wh_api_bmi270_wh_map_feat_int bmi270_wh_map_feat_int - * \code - * int8_t bmi270_wh_map_feat_int(const struct bmi2_sens_int_config *sens_int, uint8_t n_sens, struct bmi2_dev *dev) - * \endcode - * @details This API maps/unmaps feature interrupts to that of interrupt pins. - * - * @param[in] sens_int : Structure instance of bmi2_sens_int_config. - * @param[in] n_sens : Number of interrupts to be mapped. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi270_wh_map_feat_int(const struct bmi2_sens_int_config *sens_int, uint8_t n_sens, struct bmi2_dev *dev); - -/******************************************************************************/ -/*! @name C++ Guard Macros */ -/******************************************************************************/ -#ifdef __cplusplus -} -#endif /* End of CPP guard */ - -#endif /* BMI270_WH_H_ */ diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2_ois.c b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2_ois.c deleted file mode 100644 index 04350db08d..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2_ois.c +++ /dev/null @@ -1,417 +0,0 @@ -/** - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * BSD-3-Clause - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * - * 3. Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES - * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) - * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, - * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING - * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * @file bmi2_ois.c - * @date 2020-05-05 - * @version v2.23.2 - * - */ - -/******************************************************************************/ - -/*! @name Header Files */ -/******************************************************************************/ -#include "bmi2_ois.h" - -/******************************************************************************/ - -/*! Local Function Prototypes - ******************************************************************************/ - -/*! - * @brief This internal API gets the OIS accelerometer and the gyroscope data. - * - * @param[out] ois_data : Structure instance of bmi2_sens_axes_data. - * @param[in] reg_addr : Register address where data is stored. - * @param[in] ois_dev : Structure instance of bmi2_ois_dev. - * @param[in] ois_gyr_cross_sens_zx :"gyroscope cross sensitivity value which was calculated during - * bmi2xy_init(), refer the example ois_accel_gyro.c for more info" - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t get_ois_acc_gyr_data(struct bmi2_ois_sens_axes_data *ois_data, - uint8_t reg_addr, - struct bmi2_ois_dev *ois_dev, - int16_t ois_gyr_cross_sens_zx); - -/*! - * @brief This internal API is used to validate the OIS device pointer for null - * conditions. - * - * @param[in] ois_dev : Structure instance of bmi2_ois_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t null_ptr_check(const struct bmi2_ois_dev *ois_dev); - -/*! - * @brief This internal API corrects the gyroscope cross-axis sensitivity - * between the z and the x axis. - * - * @param[in] ois_dev : Structure instance of bmi2_ois_dev. - * @param[in] ois_gyr_cross_sens_zx : "gyroscope cross sensitivity value which was calculated during bmi2xy_init(), - * refer the example ois_accel_gyro.c for more info" - * - */ -static void comp_gyro_cross_axis_sensitivity(struct bmi2_ois_sens_axes_data *ois_data, int16_t ois_gyr_cross_sens_zx); - -/******************************************************************************/ -/*! @name User Interface Definitions */ -/******************************************************************************/ - -/*! - * @brief This API reads the data from the given OIS register address of bmi2 - * sensor. - */ -int8_t bmi2_ois_get_regs(uint8_t ois_reg_addr, uint8_t *ois_reg_data, uint16_t data_len, struct bmi2_ois_dev *ois_dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define dummy byte for SPI read */ - uint8_t dummy_byte = 1; - - /* Variable to define temporary length */ - uint16_t temp_len = data_len + dummy_byte; - - /* Variable to define temporary buffer */ - uint8_t temp_buf[temp_len]; - - /* Variable to index bytes read */ - uint16_t index = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(ois_dev); - if ((rslt == BMI2_OIS_OK) && (ois_reg_data != NULL)) - { - /* Configuring reg_addr for SPI Interface */ - ois_reg_addr = (ois_reg_addr | BMI2_OIS_SPI_RD_MASK); - - /* Read from OIS register through OIS interface */ - ois_dev->intf_rslt = ois_dev->ois_read(ois_reg_addr, temp_buf, temp_len, ois_dev->intf_ptr); - if (ois_dev->intf_rslt == BMI2_INTF_RET_SUCCESS) - { - /* Read the data from the position next to dummy byte */ - while (index < data_len) - { - ois_reg_data[index] = temp_buf[index + dummy_byte]; - index++; - } - } - else - { - rslt = BMI2_OIS_E_COM_FAIL; - } - } - else - { - rslt = BMI2_OIS_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API writes data to the given OIS register address of bmi2 sensor. - */ -int8_t bmi2_ois_set_regs(uint8_t ois_reg_addr, - const uint8_t *ois_reg_data, - uint16_t data_len, - struct bmi2_ois_dev *ois_dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Null-pointer check */ - rslt = null_ptr_check(ois_dev); - if ((rslt == BMI2_OIS_OK) && (ois_reg_data != NULL)) - { - /* Configuring reg_addr for SPI Interface */ - ois_reg_addr = (ois_reg_addr & BMI2_OIS_SPI_WR_MASK); - - /* Burst write to OIS register through OIS interface */ - ois_dev->intf_rslt = ois_dev->ois_write(ois_reg_addr, ois_reg_data, data_len, ois_dev->intf_ptr); - if (ois_dev->intf_rslt != BMI2_INTF_RET_SUCCESS) - { - rslt = BMI2_OIS_E_COM_FAIL; - } - } - else - { - rslt = BMI2_OIS_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API enables/disables accelerometer/gyroscope data read through - * OIS interface. - */ -int8_t bmi2_ois_set_config(struct bmi2_ois_dev *ois_dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to store data */ - uint8_t reg_data = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(ois_dev); - if (rslt == BMI2_OIS_OK) - { - rslt = bmi2_ois_get_regs(BMI2_OIS_CONFIG_ADDR, ®_data, 1, ois_dev); - if (rslt == BMI2_OIS_OK) - { - /* Enable/Disable Low pass filter */ - reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_OIS_LP_FILTER_EN, ois_dev->lp_filter_en); - - /* Configures Low pass filter cut-off frequency */ - reg_data = BMI2_OIS_SET_BITS(reg_data, BMI2_OIS_LP_FILTER_CONFIG, ois_dev->lp_filter_config); - - /* Low pass filter - mute on secondary interface */ - reg_data = BMI2_OIS_SET_BITS(reg_data, BMI2_OIS_LP_FILTER_MUTE, ois_dev->lp_filter_mute); - - /* Enable/Disable ois on accelerometer */ - reg_data = BMI2_OIS_SET_BITS(reg_data, BMI2_OIS_ACC_EN, ois_dev->acc_en); - - /* Enable/Disable ois on gyroscope */ - reg_data = BMI2_OIS_SET_BITS(reg_data, BMI2_OIS_GYR_EN, ois_dev->gyr_en); - - /* Set the OIS configurations */ - rslt = bmi2_ois_set_regs(BMI2_OIS_CONFIG_ADDR, ®_data, 1, ois_dev); - } - } - - return rslt; -} - -/*! - * @brief This API gets the status of accelerometer/gyroscope enable for data - * read through OIS interface. - */ -int8_t bmi2_ois_get_config(struct bmi2_ois_dev *ois_dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to store data */ - uint8_t reg_data = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(ois_dev); - if (rslt == BMI2_OIS_OK) - { - rslt = bmi2_ois_get_regs(BMI2_OIS_CONFIG_ADDR, ®_data, 1, ois_dev); - if (rslt == BMI2_OIS_OK) - { - /* Get the status of Low pass filter enable */ - ois_dev->lp_filter_en = BMI2_GET_BIT_POS0(reg_data, BMI2_OIS_LP_FILTER_EN); - - /* Get the status of Low pass filter cut-off frequency */ - ois_dev->lp_filter_config = BMI2_OIS_GET_BITS(reg_data, BMI2_OIS_LP_FILTER_CONFIG); - - /* Get the status of Low pass filter mute */ - ois_dev->lp_filter_mute = BMI2_OIS_GET_BITS(reg_data, BMI2_OIS_LP_FILTER_MUTE); - - /* Get the status of accelerometer enable */ - ois_dev->acc_en = BMI2_OIS_GET_BITS(reg_data, BMI2_OIS_ACC_EN); - - /* Get the status of gyroscope enable */ - ois_dev->gyr_en = BMI2_OIS_GET_BITS(reg_data, BMI2_OIS_GYR_EN); - } - } - - return rslt; -} - -/*! - * @brief This API reads accelerometer/gyroscope data through OIS interface. - */ -int8_t bmi2_ois_read_data(const uint8_t *sens_sel, - uint8_t n_sens, - struct bmi2_ois_dev *ois_dev, - int16_t gyr_cross_sens_zx) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define loop */ - uint8_t loop = 0; - - /* Variable to update register address */ - uint8_t reg_addr = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(ois_dev); - if ((rslt == BMI2_OIS_OK) && (sens_sel != NULL)) - { - - for (loop = 0; loop < n_sens; loop++) - { - switch (sens_sel[loop]) - { - case BMI2_OIS_ACCEL: - - /* Update OIS accelerometer address */ - reg_addr = BMI2_OIS_ACC_X_LSB_ADDR; - - /* Get OIS accelerometer data */ - rslt = get_ois_acc_gyr_data(&ois_dev->acc_data, reg_addr, ois_dev, 0); - break; - case BMI2_OIS_GYRO: - - /* Update OIS gyroscope address */ - reg_addr = BMI2_OIS_GYR_X_LSB_ADDR; - - /* Get OIS gyroscope data */ - rslt = get_ois_acc_gyr_data(&ois_dev->gyr_data, reg_addr, ois_dev, gyr_cross_sens_zx); - break; - default: - rslt = BMI2_OIS_E_INVALID_SENSOR; - break; - } - - /* Return error if any of the get sensor data fails */ - if (rslt != BMI2_OIS_OK) - { - break; - } - } - } - else - { - rslt = BMI2_OIS_E_NULL_PTR; - } - - return rslt; -} - -/***************************************************************************/ - -/*! Local Function Definitions - ****************************************************************************/ - -/*! @cond DOXYGEN_SUPRESS */ - -/* Suppressing doxygen warnings triggered for same static function names present across various sensor variant - * directories */ - -/*! - * @brief This internal API gets the accelerometer and the gyroscope data. - */ -static int8_t get_ois_acc_gyr_data(struct bmi2_ois_sens_axes_data *ois_data, - uint8_t reg_addr, - struct bmi2_ois_dev *ois_dev, - int16_t ois_gyr_cross_sens_zx) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variables to store MSB value */ - uint8_t msb; - - /* Variables to store LSB value */ - uint8_t lsb; - - /* Variables to store both MSB and LSB value */ - uint16_t msb_lsb; - - /* Variables to define index */ - uint8_t index = 0; - - /* Array to define data stored in register */ - uint8_t reg_data[BMI2_OIS_ACC_GYR_NUM_BYTES] = { 0 }; - - /* Read the sensor data */ - rslt = bmi2_ois_get_regs(reg_addr, reg_data, BMI2_OIS_ACC_GYR_NUM_BYTES, ois_dev); - if (rslt == BMI2_OIS_OK) - { - /* Read x-axis data */ - lsb = reg_data[index++]; - msb = reg_data[index++]; - msb_lsb = ((uint16_t)msb << 8) | (uint16_t)lsb; - ois_data->x = (int16_t)msb_lsb; - - /* Read y-axis data */ - lsb = reg_data[index++]; - msb = reg_data[index++]; - msb_lsb = ((uint16_t)msb << 8) | (uint16_t)lsb; - ois_data->y = (int16_t)msb_lsb; - - /* Read z-axis data */ - lsb = reg_data[index++]; - msb = reg_data[index++]; - msb_lsb = ((uint16_t)msb << 8) | (uint16_t)lsb; - ois_data->z = (int16_t)msb_lsb; - - comp_gyro_cross_axis_sensitivity(ois_data, ois_gyr_cross_sens_zx); - } - - return rslt; -} - -/*! - * @brief This internal API is used to validate the device structure pointer for - * null conditions. - */ -static int8_t null_ptr_check(const struct bmi2_ois_dev *ois_dev) -{ - /* Variable to define error */ - int8_t rslt = BMI2_OIS_OK; - - if ((ois_dev == NULL) || (ois_dev->ois_read == NULL) || (ois_dev->ois_write == NULL) || - (ois_dev->ois_delay_us == NULL)) - { - /* Device structure pointer is NULL */ - rslt = BMI2_OIS_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This internal API corrects the gyroscope cross-axis sensitivity - * between the z and the x axis. - */ -static void comp_gyro_cross_axis_sensitivity(struct bmi2_ois_sens_axes_data *ois_data, int16_t ois_gyr_cross_sens_zx) -{ - - /* Get the compensated gyroscope x-axis */ - ois_data->x = ois_data->x - (int16_t)(((int32_t)ois_gyr_cross_sens_zx * (int32_t)ois_data->z) / 512); -} - -/*! @endcond */ diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2_ois.h b/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2_ois.h deleted file mode 100644 index 9b700cd955..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2_ois.h +++ /dev/null @@ -1,387 +0,0 @@ -/** - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * BSD-3-Clause - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * - * 3. Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES - * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) - * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, - * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING - * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * @file bmi2_ois.h - * @date 2020-05-05 - * @version v2.23.2 - * - */ - -/** - * \ingroup bmi2xy - * \defgroup bmi2_ois BMI2_OIS - * @brief Sensor driver for BMI2_OIS sensor - */ -#ifndef _BMI2_OIS_H -#define _BMI2_OIS_H - -/*! CPP guard */ -#ifdef __cplusplus -extern "C" { -#endif - -/***************************************************************************/ - -/*! Header files - ****************************************************************************/ -#ifdef __KERNEL__ -#include -#include -#else -#include -#include -#endif - -/******************************************************************************/ -/*! @name Macros */ -/******************************************************************************/ - -#ifndef BMI2_INTF_RETURN_TYPE -#define BMI2_INTF_RETURN_TYPE int8_t -#endif - -/*! @name Utility macros */ -#define BMI2_OIS_SET_BITS(reg_data, bitname, data) \ - ((reg_data & ~(bitname##_MASK)) | \ - ((data << bitname##_POS) & bitname##_MASK)) - -#define BMI2_OIS_GET_BITS(reg_data, bitname) \ - ((reg_data & (bitname##_MASK)) >> \ - (bitname##_POS)) - -#define BMI2_SET_BIT_POS0(reg_data, bitname, data) \ - ((reg_data & ~(bitname##_MASK)) | \ - (data & bitname##_MASK)) - -#define BMI2_GET_BIT_POS0(reg_data, bitname) (reg_data & (bitname##_MASK)) - -/*! @name For enable and disable */ -#define BMI2_OIS_ENABLE UINT8_C(1) -#define BMI2_OIS_DISABLE UINT8_C(0) - -/*! @name To define sensor interface success code */ -#define BMI2_INTF_RET_SUCCESS INT8_C(0) - -/*! @name To define success code */ -#define BMI2_OIS_OK UINT8_C(0) - -/*! @name To define error codes */ -#define BMI2_OIS_E_NULL_PTR INT8_C(-1) -#define BMI2_OIS_E_COM_FAIL INT8_C(-2) -#define BMI2_OIS_E_INVALID_SENSOR INT8_C(-8) - -/*! @name Mask definitions for SPI read/write address for OIS */ -#define BMI2_OIS_SPI_RD_MASK UINT8_C(0x80) -#define BMI2_OIS_SPI_WR_MASK UINT8_C(0x7F) - -/*! @name BMI2 OIS data bytes */ -#define BMI2_OIS_ACC_GYR_NUM_BYTES UINT8_C(6) - -/*! @name Macros to select sensor for OIS data read */ -#define BMI2_OIS_ACCEL UINT8_C(0x01) -#define BMI2_OIS_GYRO UINT8_C(0x02) - -/*! @name Macros to define OIS register addresses */ -#define BMI2_OIS_CONFIG_ADDR UINT8_C(0x40) -#define BMI2_OIS_ACC_X_LSB_ADDR UINT8_C(0x0C) -#define BMI2_OIS_GYR_X_LSB_ADDR UINT8_C(0x12) - -/*! @name Mask definitions for OIS configurations */ -#define BMI2_OIS_GYR_EN_MASK UINT8_C(0x40) -#define BMI2_OIS_ACC_EN_MASK UINT8_C(0x80) - -/*! @name Bit Positions for OIS configurations */ -#define BMI2_OIS_GYR_EN_POS UINT8_C(0x06) -#define BMI2_OIS_ACC_EN_POS UINT8_C(0x07) - -/*! Low pass filter configuration position and mask */ -#define BMI2_OIS_LP_FILTER_EN_POS UINT8_C(0x00) -#define BMI2_OIS_LP_FILTER_EN_MASK UINT8_C(0x01) - -#define BMI2_OIS_LP_FILTER_CONFIG_POS UINT8_C(0x01) -#define BMI2_OIS_LP_FILTER_CONFIG_MASK UINT8_C(0x06) - -#define BMI2_OIS_LP_FILTER_MUTE_POS UINT8_C(0x05) -#define BMI2_OIS_LP_FILTER_MUTE_MASK UINT8_C(0x20) - -/******************************************************************************/ -/*! @name Function Pointers */ -/******************************************************************************/ - -/*! - * @brief Bus communication function pointer which should be mapped to - * the platform specific read functions of the user - * - * @param[in] reg_addr : Register address from which data is read. - * @param[out] reg_data : Pointer to data buffer where read data is stored. - * @param[in] len : Number of bytes of data to be read. - * @param[in, out] intf_ptr : Void pointer that can enable the linking of descriptors - * for interface related call backs. - * - * retval = BMI2_INTF_RET_SUCCESS -> Success - * retval != BMI2_INTF_RET_SUCCESS -> Failure - * - */ -typedef BMI2_INTF_RETURN_TYPE (*bmi2_ois_read_fptr_t)(uint8_t reg_addr, uint8_t *data, uint32_t len, void *intf_ptr); - -/*! - * @brief Bus communication function pointer which should be mapped to - * the platform specific write functions of the user - * - * @param[in] reg_addr : Register address to which the data is written. - * @param[in] reg_data : Pointer to data buffer in which data to be written - * is stored. - * @param[in] len : Number of bytes of data to be written. - * @param[in, out] intf_ptr : Void pointer that can enable the linking of descriptors - * for interface related call backs - * - * retval = BMI2_INTF_RET_SUCCESS -> Success - * retval != BMI2_INTF_RET_SUCCESS -> Failure - * - */ -typedef BMI2_INTF_RETURN_TYPE (*bmi2_ois_write_fptr_t)(uint8_t reg_addr, const uint8_t *data, uint32_t len, - void *intf_ptr); - -/*! - * @brief Delay function pointer which should be mapped to - * delay function of the user - * - * @param[in] period : Delay in microseconds. - * @param[in, out] intf_ptr : Void pointer that can enable the linking of descriptors - * for interface related call backs - * - */ -typedef void (*bmi2_ois_delay_fptr_t)(uint32_t period, void *intf_ptr); - -/******************************************************************************/ -/*! @name Structure Declarations */ -/******************************************************************************/ -/*! @name Structure to define accelerometer and gyroscope sensor axes for OIS */ -struct bmi2_ois_sens_axes_data -{ - /*! Data in x-axis */ - int16_t x; - - /*! Data in y-axis */ - int16_t y; - - /*! Data in z-axis */ - int16_t z; - -}; - -/*! @name Structure to define bmi2 OIS sensor configurations */ -struct bmi2_ois_dev -{ - /*! Read function pointer */ - bmi2_ois_read_fptr_t ois_read; - - /*! Write function pointer */ - bmi2_ois_write_fptr_t ois_write; - - /*! Delay function pointer */ - bmi2_ois_delay_fptr_t ois_delay_us; - - /*! Low pass filter en/dis */ - uint8_t lp_filter_en; - - /*! Void interface pointer */ - void *intf_ptr; - - /*! To store interface pointer error */ - int8_t intf_rslt; - - /*! Low pass filter cut-off frequency */ - uint8_t lp_filter_config; - - /*! Low pass filter mute */ - uint8_t lp_filter_mute; - - /*! Accelerometer enable for OIS */ - uint8_t acc_en; - - /*! Gyroscope enable for OIS */ - uint8_t gyr_en; - - /*! Accelerometer data axes */ - struct bmi2_ois_sens_axes_data acc_data; - - /*! Gyroscope data axes */ - struct bmi2_ois_sens_axes_data gyr_data; -}; - -/***************************************************************************/ - -/*! BMI2 OIS User Interface function prototypes - ****************************************************************************/ - -/** - * \ingroup bmi2_ois - * \defgroup bmi2_oisApiRegs Registers - * @brief Read data from the given OIS register address of bmi2 - */ - -/*! - * \ingroup bmi2_oisApiRegs - * \page bmi2_ois_api_bmi2_ois_get_regs bmi2_ois_get_regs - * \code - * int8_t bmi2_ois_get_regs(uint8_t ois_reg_addr, - * uint8_t *ois_reg_data, - * uint16_t data_len, - * const struct bmi2_ois_dev *ois_dev); - * \endcode - * @details This API reads the data from the given OIS register address of bmi2 - * sensor. - * - * @param[in] ois_reg_addr : OIS register address from which data is read. - * @param[out] ois_reg_data : Pointer to data buffer where read data is stored. - * @param[in] data_len : No. of bytes of data to be read. - * @param[in] ois_dev : Structure instance of bmi2_ois_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_ois_get_regs(uint8_t ois_reg_addr, uint8_t *ois_reg_data, uint16_t data_len, struct bmi2_ois_dev *ois_dev); - -/*! - * \ingroup bmi2_oisApiRegs - * \page bmi2_ois_api_bmi2_ois_set_regs bmi2_ois_set_regs - * \code - * int8_t bmi2_ois_set_regs(uint8_t ois_reg_addr, - * uint8_t *ois_reg_data, - * uint16_t data_len, - * const struct bmi2_ois_dev *ois_dev); - * \endcode - * @details This API writes data to the given OIS register address of bmi2 sensor. - * - * @param[in] ois_reg_addr : OIS register address to which the data is written. - * @param[in] ois_reg_data : Pointer to data buffer in which data to be written - * is stored. - * @param[in] data_len : No. of bytes of data to be written. - * @param[in] ois_dev : Structure instance of bmi2_ois_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_ois_set_regs(uint8_t ois_reg_addr, - const uint8_t *ois_reg_data, - uint16_t data_len, - struct bmi2_ois_dev *ois_dev); - -/** - * \ingroup bmi2_ois - * \defgroup bmi2_oisApiConfig Status update - * @brief Get / Set the status of Enable / Disable accelerometer / gyroscope data read through OIS interface - */ - -/*! - * \ingroup bmi2_oisApiConfig - * \page bmi2_ois_api_bmi2_ois_set_config bmi2_ois_set_config - * \code - * int8_t bmi2_ois_set_config(const struct bmi2_ois_dev *ois_dev); - * \endcode - * @details This API sets the status of enable/disable accelerometer/gyroscope data read through - * OIS interface. - * - * @param[in] ois_dev : Structure instance of bmi2_ois_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_ois_set_config(struct bmi2_ois_dev *ois_dev); - -/*! - * \ingroup bmi2_oisApiConfig - * \page bmi2_ois_api_bmi2_ois_get_config bmi2_ois_get_config - * \code - * int8_t bmi2_ois_get_config(struct bmi2_ois_dev *ois_dev); - * \endcode - * @details This API gets the status of accelerometer/gyroscope enable for data - * read through OIS interface. - * - * @param[in, out] ois_dev : Structure instance of bmi2_ois_dev. - * - * @note Enabling and disabling is done during OIS structure initialization. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_ois_get_config(struct bmi2_ois_dev *ois_dev); - -/** - * \ingroup bmi2_ois - * \defgroup bmi2_oisApiRead Data read - * @brief Read accelerometer / gyroscope data through OIS interface - */ - -/*! - * \ingroup bmi2_oisApiRead - * \page bmi2_ois_api_bmi2_ois_read_data bmi2_ois_read_data - * \code - * int8_t bmi2_ois_read_data(const uint8_t *sens_sel, - * uint8_t n_sens, - * struct bmi2_ois_dev *ois_dev, - * int16_t gyr_cross_sens_zx); - * \endcode - * @details This API reads accelerometer/gyroscope data through OIS interface. - * - * @param[in] sens_sel : Select sensor whose data is to be read. - * @param[in] n_sens : Number of sensors selected. - * @param[in, out] ois_dev : Structure instance of bmi2_ois_dev. - * @param[in] gyr_cross_sens_zx : Store the gyroscope cross sensitivity values taken from the bmi2xy - * (refer bmi2_ois example). - * - *@verbatim - * sens_sel | Value - * ---------------|--------------- - * BMI2_OIS_ACCEL | 0x01 - * BMI2_OIS_GYRO | 0x02 - *@endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_ois_read_data(const uint8_t *sens_sel, - uint8_t n_sens, - struct bmi2_ois_dev *ois_dev, - int16_t gyr_cross_sens_zx); - -#ifdef __cplusplus -} -#endif /* End of CPP guard */ - -#endif /* End of _BMI2_OIS_H */ diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/accel/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/accel/Makefile deleted file mode 100644 index c63d602ce5..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/accel/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= accel.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/accel/accel.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/accel/accel.c deleted file mode 100644 index ea385d7441..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/accel/accel.c +++ /dev/null @@ -1,200 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270.h" -#include "common.h" - -/******************************************************************************/ -/*! Macro definition */ - -/*! Earth's gravity in m/s^2 */ -#define GRAVITY_EARTH (9.80665f) - -/******************************************************************************/ -/*! Static Function Declaration */ - -/*! - * @brief This internal API is used to set configurations for accel. - * - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Status of execution. - */ -static int8_t set_accel_config(struct bmi2_dev *bmi2_dev); - -/*! - * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at - * range 2G, 4G, 8G or 16G. - * - * @param[in] val : LSB from each axis. - * @param[in] g_range : Gravity range. - * @param[in] bit_width : Resolution for accel. - * - * @return Gravity. - */ -static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width); - -/******************************************************************************/ -/*! Functions */ - -/* This function starts the execution of program. */ -int main(void) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Variable to define limit to print accel data. */ - uint8_t limit = 20; - - /* Assign accel sensor to variable. */ - uint8_t sensor_list = BMI2_ACCEL; - - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Create an instance of sensor data structure. */ - struct bmi2_sensor_data sensor_data = { 0 }; - - /* Initialize the interrupt status of accel. */ - uint16_t int_status = 0; - - uint8_t indx = 0; - float x = 0, y = 0, z = 0; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270. */ - rslt = bmi270_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Enable the accel sensor. */ - rslt = bmi270_sensor_enable(&sensor_list, 1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Accel configuration settings. */ - rslt = set_accel_config(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Assign accel sensor. */ - sensor_data.type = BMI2_ACCEL; - printf("Accel and m/s2 data \n"); - printf("Accel data collected at 2G Range with 16-bit resolution\n"); - - /* Loop to print the accel data when interrupt occurs. */ - while (indx <= limit) - { - /* To get the status of accel data ready interrupt. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* To check the accel data ready interrupt status and print the status for 10 samples. */ - if (int_status & BMI2_ACC_DRDY_INT_MASK) - { - /* Get accelerometer data for x, y and z axis. */ - rslt = bmi270_get_sensor_data(&sensor_data, 1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - printf("\nAcc_X = %d\t", sensor_data.sens_data.acc.x); - printf("Acc_Y = %d\t", sensor_data.sens_data.acc.y); - printf("Acc_Z = %d\r\n", sensor_data.sens_data.acc.z); - - /* Converting lsb to meter per second squared for 16 bit accelerometer at 2G range. */ - x = lsb_to_mps2(sensor_data.sens_data.acc.x, 2, bmi2_dev.resolution); - y = lsb_to_mps2(sensor_data.sens_data.acc.y, 2, bmi2_dev.resolution); - z = lsb_to_mps2(sensor_data.sens_data.acc.z, 2, bmi2_dev.resolution); - - /* Print the data in m/s2. */ - printf("\nAcc_ms2_X = %4.2f, Acc_ms2_Y = %4.2f, Acc_ms2_Z = %4.2f\n", x, y, z); - - indx++; - } - } - } - } - - bmi2_coines_deinit(); - - return rslt; -} - -/*! - * @brief This internal API is used to set configurations for accel. - */ -static int8_t set_accel_config(struct bmi2_dev *bmi2_dev) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Structure to define accelerometer configuration. */ - struct bmi2_sens_config config; - - /* Configure the type of feature. */ - config.type = BMI2_ACCEL; - - /* Get default configurations for the type of feature selected. */ - rslt = bmi270_get_sensor_config(&config, 1, bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* NOTE: The user can change the following configuration parameters according to their requirement. */ - /* Set Output Data Rate */ - config.cfg.acc.odr = BMI2_ACC_ODR_200HZ; - - /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ - config.cfg.acc.range = BMI2_ACC_RANGE_2G; - - /* The bandwidth parameter is used to configure the number of sensor samples that are averaged - * if it is set to 2, then 2^(bandwidth parameter) samples - * are averaged, resulting in 4 averaged samples. - * Note1 : For more information, refer the datasheet. - * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but - * this has an adverse effect on the power consumed. - */ - config.cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; - - /* Enable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - * For more info refer datasheet. - */ - config.cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; - - /* Set the accel configurations. */ - rslt = bmi270_set_sensor_config(&config, 1, bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Map data ready interrupt to interrupt pin. */ - rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT1, bmi2_dev); - bmi2_error_codes_print_result(rslt); - } - - return rslt; -} - -/*! - * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at - * range 2G, 4G, 8G or 16G. - */ -static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width) -{ - float half_scale = ((float)(1 << bit_width) / 2.0f); - - return (GRAVITY_EARTH * val * g_range) / half_scale; -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/accel_gyro/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/accel_gyro/Makefile deleted file mode 100644 index c0617d02e2..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/accel_gyro/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= accel_gyro.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/accel_gyro/accel_gyro.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/accel_gyro/accel_gyro.c deleted file mode 100644 index 1e7c9dfc06..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/accel_gyro/accel_gyro.c +++ /dev/null @@ -1,268 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270.h" -#include "common.h" - -/******************************************************************************/ -/*! Macro definition */ - -/*! Earth's gravity in m/s^2 */ -#define GRAVITY_EARTH (9.80665f) - -/*! Macros to select the sensors */ -#define ACCEL UINT8_C(0x00) -#define GYRO UINT8_C(0x01) - -/******************************************************************************/ -/*! Static Function Declaration */ - -/*! - * @brief This internal API is used to set configurations for accel. - * - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Status of execution. - */ -static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev); - -/*! - * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at - * range 2G, 4G, 8G or 16G. - * - * @param[in] val : LSB from each axis. - * @param[in] g_range : Gravity range. - * @param[in] bit_width : Resolution for accel. - * - * @return Gravity. - */ -static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width); - -/*! - * @brief This function converts lsb to degree per second for 16 bit gyro at - * range 125, 250, 500, 1000 or 2000dps. - * - * @param[in] val : LSB from each axis. - * @param[in] dps : Degree per second. - * @param[in] bit_width : Resolution for gyro. - * - * @return Degree per second. - */ -static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width); - -/******************************************************************************/ -/*! Functions */ - -/* This function starts the execution of program. */ -int main(void) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Variable to define limit to print accel data. */ - uint8_t limit = 10; - - /* Assign accel and gyro sensor to variable. */ - uint8_t sensor_list[2] = { BMI2_ACCEL, BMI2_GYRO }; - - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Create an instance of sensor data structure. */ - struct bmi2_sensor_data sensor_data[2] = { { 0 } }; - - /* Initialize the interrupt status of accel and gyro. */ - uint16_t int_status = 0; - - uint8_t indx = 1; - - float x = 0, y = 0, z = 0; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_SPI_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270. */ - rslt = bmi270_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Enable the accel and gyro sensor. */ - rslt = bmi270_sensor_enable(sensor_list, 2, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Accel and gyro configuration settings. */ - rslt = set_accel_gyro_config(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Assign accel and gyro sensor. */ - sensor_data[ACCEL].type = BMI2_ACCEL; - sensor_data[GYRO].type = BMI2_GYRO; - - /* Loop to print accel and gyro data when interrupt occurs. */ - while (indx <= limit) - { - /* To get the data ready interrupt status of accel and gyro. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* To check the data ready interrupt status and print the status for 10 samples. */ - if ((int_status & BMI2_ACC_DRDY_INT_MASK) && (int_status & BMI2_GYR_DRDY_INT_MASK)) - { - /* Get accel and gyro data for x, y and z axis. */ - rslt = bmi270_get_sensor_data(sensor_data, 2, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - printf("\n******* Accel(Raw and m/s2) Gyro(Raw and dps) data : %d *******\n", indx); - - printf("\nAcc_x = %d\t", sensor_data[ACCEL].sens_data.acc.x); - printf("Acc_y = %d\t", sensor_data[ACCEL].sens_data.acc.y); - printf("Acc_z = %d", sensor_data[ACCEL].sens_data.acc.z); - - /* Converting lsb to meter per second squared for 16 bit accelerometer at 2G range. */ - x = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.x, 2, bmi2_dev.resolution); - y = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.y, 2, bmi2_dev.resolution); - z = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.z, 2, bmi2_dev.resolution); - - /* Print the data in m/s2. */ - printf("\nAcc_ms2_X = %4.2f, Acc_ms2_Y = %4.2f, Acc_ms2_Z = %4.2f\n", x, y, z); - - printf("\nGyr_X = %d\t", sensor_data[GYRO].sens_data.gyr.x); - printf("Gyr_Y = %d\t", sensor_data[GYRO].sens_data.gyr.y); - printf("Gyr_Z= %d\n", sensor_data[GYRO].sens_data.gyr.z); - - /* Converting lsb to degree per second for 16 bit gyro at 2000dps range. */ - x = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.x, 2000, bmi2_dev.resolution); - y = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.y, 2000, bmi2_dev.resolution); - z = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.z, 2000, bmi2_dev.resolution); - - /* Print the data in dps. */ - printf("Gyro_DPS_X = %4.2f, Gyro_DPS_Y = %4.2f, Gyro_DPS_Z = %4.2f\n", x, y, z); - - indx++; - } - } - } - } - - bmi2_coines_deinit(); - - return rslt; -} - -/*! - * @brief This internal API is used to set configurations for accel and gyro. - */ -static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Structure to define accelerometer and gyro configuration. */ - struct bmi2_sens_config config[2]; - - /* Configure the type of feature. */ - config[ACCEL].type = BMI2_ACCEL; - config[GYRO].type = BMI2_GYRO; - - /* Get default configurations for the type of feature selected. */ - rslt = bmi270_get_sensor_config(config, 2, bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Map data ready interrupt to interrupt pin. */ - rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT1, bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* NOTE: The user can change the following configuration parameters according to their requirement. */ - /* Set Output Data Rate */ - config[ACCEL].cfg.acc.odr = BMI2_ACC_ODR_200HZ; - - /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ - config[ACCEL].cfg.acc.range = BMI2_ACC_RANGE_2G; - - /* The bandwidth parameter is used to configure the number of sensor samples that are averaged - * if it is set to 2, then 2^(bandwidth parameter) samples - * are averaged, resulting in 4 averaged samples. - * Note1 : For more information, refer the datasheet. - * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but - * this has an adverse effect on the power consumed. - */ - config[ACCEL].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; - - /* Enable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - * For more info refer datasheet. - */ - config[ACCEL].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; - - /* The user can change the following configuration parameters according to their requirement. */ - /* Set Output Data Rate */ - config[GYRO].cfg.gyr.odr = BMI2_GYR_ODR_200HZ; - - /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ - config[GYRO].cfg.gyr.range = BMI2_GYR_RANGE_2000; - - /* Gyroscope bandwidth parameters. By default the gyro bandwidth is in normal mode. */ - config[GYRO].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; - - /* Enable/Disable the noise performance mode for precision yaw rate sensing - * There are two modes - * 0 -> Ultra low power mode(Default) - * 1 -> High performance mode - */ - config[GYRO].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; - - /* Enable/Disable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - */ - config[GYRO].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; - - /* Set the accel and gyro configurations. */ - rslt = bmi270_set_sensor_config(config, 2, bmi2_dev); - bmi2_error_codes_print_result(rslt); - } - - return rslt; -} - -/*! - * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at - * range 2G, 4G, 8G or 16G. - */ -static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width) -{ - float half_scale = ((float)(1 << bit_width) / 2.0f); - - return (GRAVITY_EARTH * val * g_range) / half_scale; -} - -/*! - * @brief This function converts lsb to degree per second for 16 bit gyro at - * range 125, 250, 500, 1000 or 2000dps. - */ -static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width) -{ - float half_scale = ((float)(1 << bit_width) / 2.0f); - - return (dps / ((half_scale) + BMI2_GYR_RANGE_2000)) * (val); -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/any_motion_interrupt/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/any_motion_interrupt/Makefile deleted file mode 100644 index 9158f05773..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/any_motion_interrupt/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= any_motion_interrupt.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/any_motion_interrupt/any_motion_interrupt.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/any_motion_interrupt/any_motion_interrupt.c deleted file mode 100644 index 062b29eb31..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/any_motion_interrupt/any_motion_interrupt.c +++ /dev/null @@ -1,135 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270.h" -#include "common.h" - -/******************************************************************************/ -/*! Static Function Declaration */ - -/*! - * @brief This internal API is used to set configurations for any-motion. - * - * @param[in] bmi2_dev : Structure instance of bmi2_dev. - * - * @return Status of execution. - */ -static int8_t set_feature_config(struct bmi2_dev *bmi2_dev); - -/******************************************************************************/ -/*! Functions */ - -/* This function starts the execution of program. */ -int main(void) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Accel sensor and any-motion feature are listed in array. */ - uint8_t sens_list[2] = { BMI2_ACCEL, BMI2_ANY_MOTION }; - - /* Variable to get any-motion interrupt status. */ - uint16_t int_status = 0; - - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Select features and their pins to be mapped to. */ - struct bmi2_sens_int_config sens_int = { .type = BMI2_ANY_MOTION, .hw_int_pin = BMI2_INT1 }; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270. */ - rslt = bmi270_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Enable the selected sensors. */ - rslt = bmi270_sensor_enable(sens_list, 2, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Set feature configurations for any-motion. */ - rslt = set_feature_config(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Map the feature interrupt for any-motion. */ - rslt = bmi270_map_feat_int(&sens_int, 1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - printf("Move the board\n"); - - /* Loop to get any-motion interrupt. */ - do - { - /* Clear buffer. */ - int_status = 0; - - /* To get the interrupt status of any-motion. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* To check the interrupt status of any-motion. */ - if (int_status & BMI270_ANY_MOT_STATUS_MASK) - { - printf("Any-motion interrupt is generated\n"); - break; - } - } while (rslt == BMI2_OK); - } - } - } - - bmi2_coines_deinit(); - - return rslt; -} - -/*! - * @brief This internal API is used to set configurations for any-motion. - */ -static int8_t set_feature_config(struct bmi2_dev *bmi2_dev) -{ - - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Structure to define the type of sensor and its configurations. */ - struct bmi2_sens_config config; - - /* Configure the type of feature. */ - config.type = BMI2_ANY_MOTION; - - /* Get default configurations for the type of feature selected. */ - rslt = bmi270_get_sensor_config(&config, 1, bmi2_dev); - bmi2_error_codes_print_result(rslt); - if (rslt == BMI2_OK) - { - /* NOTE: The user can change the following configuration parameters according to their requirement. */ - /* 1LSB equals 20ms. Default is 100ms, setting to 80ms. */ - config.cfg.any_motion.duration = 0x04; - - /* 1LSB equals to 0.48mg. Default is 83mg, setting to 50mg. */ - config.cfg.any_motion.threshold = 0x68; - - /* Set new configurations. */ - rslt = bmi270_set_sensor_config(&config, 1, bmi2_dev); - bmi2_error_codes_print_result(rslt); - } - - return rslt; -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/common/common.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/common/common.c deleted file mode 100644 index afff37bc84..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/common/common.c +++ /dev/null @@ -1,372 +0,0 @@ -/** - * Copyright (C) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - */ - -#include -#include -#include - -#include "coines.h" -#include "bmi2_defs.h" - -/******************************************************************************/ -/*! Macro definitions */ -#define BMI2XY_SHUTTLE_ID UINT16_C(0x1B8) - -/*! Macro that defines read write length */ -#define READ_WRITE_LEN UINT8_C(46) - -/******************************************************************************/ -/*! Static variable definition */ - -/*! Variable that holds the I2C device address or SPI chip selection */ -static uint8_t dev_addr; - -/******************************************************************************/ -/*! User interface functions */ - -/*! - * I2C read function map to COINES platform - */ -BMI2_INTF_RETURN_TYPE bmi2_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr) -{ - uint8_t dev_addr = *(uint8_t*)intf_ptr; - - return coines_read_i2c(dev_addr, reg_addr, reg_data, (uint16_t)len); -} - -/*! - * I2C write function map to COINES platform - */ -BMI2_INTF_RETURN_TYPE bmi2_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr) -{ - uint8_t dev_addr = *(uint8_t*)intf_ptr; - - return coines_write_i2c(dev_addr, reg_addr, (uint8_t *)reg_data, (uint16_t)len); -} - -/*! - * SPI read function map to COINES platform - */ -BMI2_INTF_RETURN_TYPE bmi2_spi_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr) -{ - uint8_t dev_addr = *(uint8_t*)intf_ptr; - - return coines_read_spi(dev_addr, reg_addr, reg_data, (uint16_t)len); -} - -/*! - * SPI write function map to COINES platform - */ -BMI2_INTF_RETURN_TYPE bmi2_spi_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr) -{ - uint8_t dev_addr = *(uint8_t*)intf_ptr; - - return coines_write_spi(dev_addr, reg_addr, (uint8_t *)reg_data, (uint16_t)len); -} - -/*! - * Delay function map to COINES platform - */ -void bmi2_delay_us(uint32_t period, void *intf_ptr) -{ - coines_delay_usec(period); -} - -/*! - * @brief Function to select the interface between SPI and I2C. - * Also to initialize coines platform - */ -int8_t bmi2_interface_init(struct bmi2_dev *bmi, uint8_t intf) -{ - int8_t rslt = BMI2_OK; - struct coines_board_info board_info; - - if (bmi != NULL) - { - int16_t result = coines_open_comm_intf(COINES_COMM_INTF_USB); - if (result < COINES_SUCCESS) - { - printf( - "\n Unable to connect with Application Board ! \n" " 1. Check if the board is connected and powered on. \n" " 2. Check if Application Board USB driver is installed. \n" - " 3. Check if board is in use by another application. (Insufficient permissions to access USB) \n"); - exit(result); - } - - result = coines_get_board_info(&board_info); - -#if defined(PC) - setbuf(stdout, NULL); -#endif - - if (result == COINES_SUCCESS) - { - if ((board_info.shuttle_id != BMI2XY_SHUTTLE_ID)) - { - printf("! Warning invalid sensor shuttle \n ," "This application will not support this sensor \n"); - exit(COINES_E_FAILURE); - } - } - - coines_set_shuttleboard_vdd_vddio_config(0, 0); - coines_delay_msec(100); - - /* Bus configuration : I2C */ - if (intf == BMI2_I2C_INTF) - { - printf("I2C Interface \n"); - - /* To initialize the user I2C function */ - dev_addr = BMI2_I2C_PRIM_ADDR; - bmi->intf = BMI2_I2C_INTF; - bmi->read = bmi2_i2c_read; - bmi->write = bmi2_i2c_write; - coines_config_i2c_bus(COINES_I2C_BUS_0, COINES_I2C_FAST_MODE); - } - /* Bus configuration : SPI */ - else if (intf == BMI2_SPI_INTF) - { - printf("SPI Interface \n"); - - /* To initialize the user SPI function */ - dev_addr = COINES_SHUTTLE_PIN_7; - bmi->intf = BMI2_SPI_INTF; - bmi->read = bmi2_spi_read; - bmi->write = bmi2_spi_write; - coines_config_spi_bus(COINES_SPI_BUS_0, COINES_SPI_SPEED_5_MHZ, COINES_SPI_MODE3); - coines_set_pin_config(COINES_SHUTTLE_PIN_7, COINES_PIN_DIRECTION_OUT, COINES_PIN_VALUE_HIGH); - } - - /* Assign device address to interface pointer */ - bmi->intf_ptr = &dev_addr; - - /* Configure delay in microseconds */ - bmi->delay_us = bmi2_delay_us; - - /* Configure max read/write length (in bytes) ( Supported length depends on target machine) */ - bmi->read_write_len = READ_WRITE_LEN; - - /* Assign to NULL to load the default config file. */ - bmi->config_file_ptr = NULL; - - coines_delay_usec(10000); - - coines_set_shuttleboard_vdd_vddio_config(3300, 3300); - - coines_delay_usec(10000); - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; - -} - -/*! - * @brief Prints the execution status of the APIs. - */ -void bmi2_error_codes_print_result(int8_t rslt) -{ - switch (rslt) - { - case BMI2_OK: - - /* Do nothing */ - break; - - case BMI2_W_FIFO_EMPTY: - printf("Warning [%d] : FIFO empty\r\n", rslt); - break; - case BMI2_W_PARTIAL_READ: - printf("Warning [%d] : FIFO partial read\r\n", rslt); - break; - case BMI2_E_NULL_PTR: - printf( - "Error [%d] : Null pointer error. It occurs when the user tries to assign value (not address) to a pointer," " which has been initialized to NULL.\r\n", - rslt); - break; - - case BMI2_E_COM_FAIL: - printf( - "Error [%d] : Communication failure error. It occurs due to read/write operation failure and also due " "to power failure during communication\r\n", - rslt); - break; - - case BMI2_E_DEV_NOT_FOUND: - printf("Error [%d] : Device not found error. It occurs when the device chip id is incorrectly read\r\n", - rslt); - break; - - case BMI2_E_INVALID_SENSOR: - printf( - "Error [%d] : Invalid sensor error. It occurs when there is a mismatch in the requested feature with the " "available one\r\n", - rslt); - break; - - case BMI2_E_SELF_TEST_FAIL: - printf( - "Error [%d] : Self-test failed error. It occurs when the validation of accel self-test data is " "not satisfied\r\n", - rslt); - break; - - case BMI2_E_INVALID_INT_PIN: - printf( - "Error [%d] : Invalid interrupt pin error. It occurs when the user tries to configure interrupt pins " "apart from INT1 and INT2\r\n", - rslt); - break; - - case BMI2_E_OUT_OF_RANGE: - printf( - "Error [%d] : Out of range error. It occurs when the data exceeds from filtered or unfiltered data from " "fifo and also when the range exceeds the maximum range for accel and gyro while performing FOC\r\n", - rslt); - break; - - case BMI2_E_ACC_INVALID_CFG: - printf( - "Error [%d] : Invalid Accel configuration error. It occurs when there is an error in accel configuration" " register which could be one among range, BW or filter performance in reg address 0x40\r\n", - rslt); - break; - - case BMI2_E_GYRO_INVALID_CFG: - printf( - "Error [%d] : Invalid Gyro configuration error. It occurs when there is a error in gyro configuration" "register which could be one among range, BW or filter performance in reg address 0x42\r\n", - rslt); - break; - - case BMI2_E_ACC_GYR_INVALID_CFG: - printf( - "Error [%d] : Invalid Accel-Gyro configuration error. It occurs when there is a error in accel and gyro" " configuration registers which could be one among range, BW or filter performance in reg address 0x40 " "and 0x42\r\n", - rslt); - break; - - case BMI2_E_CONFIG_LOAD: - printf( - "Error [%d] : Configuration load error. It occurs when failure observed while loading the configuration " "into the sensor\r\n", - rslt); - break; - - case BMI2_E_INVALID_PAGE: - printf( - "Error [%d] : Invalid page error. It occurs due to failure in writing the correct feature configuration " "from selected page\r\n", - rslt); - break; - - case BMI2_E_SET_APS_FAIL: - printf( - "Error [%d] : APS failure error. It occurs due to failure in write of advance power mode configuration " "register\r\n", - rslt); - break; - - case BMI2_E_AUX_INVALID_CFG: - printf( - "Error [%d] : Invalid AUX configuration error. It occurs when the auxiliary interface settings are not " "enabled properly\r\n", - rslt); - break; - - case BMI2_E_AUX_BUSY: - printf( - "Error [%d] : AUX busy error. It occurs when the auxiliary interface buses are engaged while configuring" " the AUX\r\n", - rslt); - break; - - case BMI2_E_REMAP_ERROR: - printf( - "Error [%d] : Remap error. It occurs due to failure in assigning the remap axes data for all the axes " "after change in axis position\r\n", - rslt); - break; - - case BMI2_E_GYR_USER_GAIN_UPD_FAIL: - printf( - "Error [%d] : Gyro user gain update fail error. It occurs when the reading of user gain update status " "fails\r\n", - rslt); - break; - - case BMI2_E_SELF_TEST_NOT_DONE: - printf( - "Error [%d] : Self-test not done error. It occurs when the self-test process is ongoing or not " "completed\r\n", - rslt); - break; - - case BMI2_E_INVALID_INPUT: - printf("Error [%d] : Invalid input error. It occurs when the sensor input validity fails\r\n", rslt); - break; - - case BMI2_E_INVALID_STATUS: - printf("Error [%d] : Invalid status error. It occurs when the feature/sensor validity fails\r\n", rslt); - break; - - case BMI2_E_CRT_ERROR: - printf("Error [%d] : CRT error. It occurs when the CRT test has failed\r\n", rslt); - break; - - case BMI2_E_ST_ALREADY_RUNNING: - printf( - "Error [%d] : Self-test already running error. It occurs when the self-test is already running and " "another has been initiated\r\n", - rslt); - break; - - case BMI2_E_CRT_READY_FOR_DL_FAIL_ABORT: - printf( - "Error [%d] : CRT ready for download fail abort error. It occurs when download in CRT fails due to wrong " "address location\r\n", - rslt); - break; - - case BMI2_E_DL_ERROR: - printf( - "Error [%d] : Download error. It occurs when write length exceeds that of the maximum burst length\r\n", - rslt); - break; - - case BMI2_E_PRECON_ERROR: - printf( - "Error [%d] : Pre-conditional error. It occurs when precondition to start the feature was not " "completed\r\n", - rslt); - break; - - case BMI2_E_ABORT_ERROR: - printf("Error [%d] : Abort error. It occurs when the device was shaken during CRT test\r\n", rslt); - break; - - case BMI2_E_WRITE_CYCLE_ONGOING: - printf( - "Error [%d] : Write cycle ongoing error. It occurs when the write cycle is already running and another " "has been initiated\r\n", - rslt); - break; - - case BMI2_E_ST_NOT_RUNING: - printf( - "Error [%d] : Self-test is not running error. It occurs when self-test running is disabled while it's " "running\r\n", - rslt); - break; - - case BMI2_E_DATA_RDY_INT_FAILED: - printf( - "Error [%d] : Data ready interrupt error. It occurs when the sample count exceeds the FOC sample limit " "and data ready status is not updated\r\n", - rslt); - break; - - case BMI2_E_INVALID_FOC_POSITION: - printf( - "Error [%d] : Invalid FOC position error. It occurs when average FOC data is obtained for the wrong" " axes\r\n", - rslt); - break; - - default: - printf("Error [%d] : Unknown error code\r\n", rslt); - break; - } -} - -/*! - * @brief Deinitializes coines platform - * - * @return void. - */ -void bmi2_coines_deinit(void) -{ - coines_close_comm_intf(COINES_COMM_INTF_USB); -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/common/common.h b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/common/common.h deleted file mode 100644 index 763983da4a..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/common/common.h +++ /dev/null @@ -1,122 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -#ifndef _COMMON_H -#define _COMMON_H - -/*! CPP guard */ -#ifdef __cplusplus -extern "C" { -#endif - -#include -#include "bmi2.h" - -/*! - * @brief Function for reading the sensor's registers through I2C bus. - * - * @param[in] reg_addr : Register address. - * @param[out] reg_data : Pointer to the data buffer to store the read data. - * @param[in] length : No of bytes to read. - * @param[in] intf_ptr : Interface pointer - * - * @return Status of execution - * @retval = BMI2_INTF_RET_SUCCESS -> Success - * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info - * - */ -BMI2_INTF_RETURN_TYPE bmi2_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr); - -/*! - * @brief Function for writing the sensor's registers through I2C bus. - * - * @param[in] reg_addr : Register address. - * @param[in] reg_data : Pointer to the data buffer whose value is to be written. - * @param[in] length : No of bytes to write. - * @param[in] intf_ptr : Interface pointer - * - * @return Status of execution - * @retval = BMI2_INTF_RET_SUCCESS -> Success - * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info - * - */ -BMI2_INTF_RETURN_TYPE bmi2_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr); - -/*! - * @brief Function for reading the sensor's registers through SPI bus. - * - * @param[in] reg_addr : Register address. - * @param[out] reg_data : Pointer to the data buffer to store the read data. - * @param[in] length : No of bytes to read. - * @param[in] intf_ptr : Interface pointer - * - * @return Status of execution - * @retval = BMI2_INTF_RET_SUCCESS -> Success - * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info - * - */ -BMI2_INTF_RETURN_TYPE bmi2_spi_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr); - -/*! - * @brief Function for writing the sensor's registers through SPI bus. - * - * @param[in] reg_addr : Register address. - * @param[in] reg_data : Pointer to the data buffer whose data has to be written. - * @param[in] length : No of bytes to write. - * @param[in] intf_ptr : Interface pointer - * - * @return Status of execution - * @retval = BMI2_INTF_RET_SUCCESS -> Success - * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info - * - */ -BMI2_INTF_RETURN_TYPE bmi2_spi_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr); - -/*! - * @brief This function provides the delay for required time (Microsecond) as per the input provided in some of the - * APIs. - * - * @param[in] period_us : The required wait time in microsecond. - * @param[in] intf_ptr : Interface pointer - * - * @return void. - * - */ -void bmi2_delay_us(uint32_t period, void *intf_ptr); - -/*! - * @brief Function to select the interface between SPI and I2C. - * - * @param[in] bma : Structure instance of bmi2_dev - * @param[in] intf : Interface selection parameter - * - * @return Status of execution - * @retval 0 -> Success - * @retval < 0 -> Failure Info - */ -int8_t bmi2_interface_init(struct bmi2_dev *bma, uint8_t intf); - -/*! - * @brief Prints the execution status of the APIs. - * - * @param[in] rslt : Error code returned by the API whose execution status has to be printed. - * - * @return void. - */ -void bmi2_error_codes_print_result(int8_t rslt); - -/*! - * @brief Deinitializes coines platform - * - * @return void. - */ -void bmi2_coines_deinit(void); - -#ifdef __cplusplus -} -#endif /* End of CPP guard */ - -#endif /* _COMMON_H */ diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/component_retrim/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/component_retrim/Makefile deleted file mode 100644 index aad6a0b877..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/component_retrim/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= component_retrim.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/component_retrim/component_retrim.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/component_retrim/component_retrim.c deleted file mode 100644 index d85c148d2a..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/component_retrim/component_retrim.c +++ /dev/null @@ -1,52 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270.h" -#include "common.h" - -/******************************************************************************/ -/*! Functions */ - -/* This function starts the execution of program. */ -int main(void) -{ - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270. */ - rslt = bmi270_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* This API runs the CRT process. */ - rslt = bmi2_do_crt(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Do not move the board while doing CRT. If so, it will throw an abort error. */ - if (rslt == BMI2_OK) - { - printf("CRT successfully completed."); - } - } - - bmi2_coines_deinit(); - - return rslt; -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_full_header_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_full_header_mode/Makefile deleted file mode 100644 index 83a8f49ff9..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_full_header_mode/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= fifo_full_header_mode.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_full_header_mode/fifo_full_header_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_full_header_mode/fifo_full_header_mode.c deleted file mode 100644 index 2118c241df..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_full_header_mode/fifo_full_header_mode.c +++ /dev/null @@ -1,315 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270.h" -#include "common.h" - -/******************************************************************************/ -/*! Macros */ - -/*! Buffer size allocated to store raw FIFO data. */ -#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) - -/*! Length of data to be read from FIFO. */ -#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) - -/*! Number of accel frames to be extracted from FIFO. */ - -/*! Calculation for frame count: Total frame count = Fifo buffer size(2048)/ Total frames(6 Accel, 6 Gyro and 1 header, - * totaling to 13) which equals to 157. - */ -#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(157) - -/*! Number of gyro frames to be extracted from FIFO. */ -#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(157) - -/*! Macro to read sensortime byte in FIFO. */ -#define SENSORTIME_OVERHEAD_BYTE UINT8_C(3) - -/******************************************************************************/ -/*! Static Function Declaration */ - -/*! - * @brief This internal API is used to set configurations for accel and gyro. - * @param[in] dev : Structure instance of bmi2_dev. - * @return Status of execution. - */ -static int8_t set_accel_gyro_config(struct bmi2_dev *dev); - -/******************************************************************************/ - -/* This function starts the execution of program. */ -int main(void) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - uint16_t index = 0; - uint16_t fifo_length = 0; - uint16_t config = 0; - - /* To read sensortime, extra 3 bytes are added to fifo buffer. */ - uint16_t fifo_buffer_size = BMI2_FIFO_RAW_DATA_BUFFER_SIZE + SENSORTIME_OVERHEAD_BYTE; - - /* Variable to get fifo full interrupt status. */ - uint16_t int_status = 0; - - uint16_t accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; - - uint16_t gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; - - /* Variable to store the available frame length count in FIFO */ - uint8_t frame_count; - - int8_t try = 1; - - /* Number of bytes of FIFO data. */ - uint8_t fifo_data[fifo_buffer_size]; - - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Array of accelerometer frames -> Total bytes = - * 157 * (6 axes + 1 header bytes) = 1099 bytes */ - struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; - - /* Array of gyro frames -> Total bytes = - * 157 * (6 axes + 1 header bytes) = 1099 bytes */ - struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; - - /* Initialize FIFO frame structure. */ - struct bmi2_fifo_frame fifoframe = { 0 }; - - /* Accel and gyro sensor are listed in array. */ - uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270. */ - rslt = bmi270_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Enable accel and gyro sensor. */ - rslt = bmi270_sensor_enable(sensor_sel, 2, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Configuration settings for accel and gyro. */ - rslt = set_accel_gyro_config(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Before setting FIFO, disable the advance power save mode. */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Initially disable all configurations in fifo. */ - rslt = bmi2_get_fifo_config(&config, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Initially disable all configurations in fifo. */ - rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Update FIFO structure. */ - /* Mapping the buffer to store the fifo data. */ - fifoframe.data = fifo_data; - - /* Length of FIFO frame. */ - /* To read sensortime, extra 3 bytes are added to fifo user length. */ - fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH + SENSORTIME_OVERHEAD_BYTE; - - /* Set FIFO configuration by enabling accel, gyro and timestamp. - * NOTE 1: The header mode is enabled by default. - * NOTE 2: By default the FIFO operating mode is in FIFO mode. - * NOTE 3: Sensortime is enabled by default */ - printf("FIFO is configured in header mode\n"); - rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Map FIFO full interrupt. */ - fifoframe.data_int_map = BMI2_FFULL_INT; - rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - while (try <= 10) - { - /* Read FIFO data on interrupt. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if ((rslt == BMI2_OK) && (int_status & BMI2_FFULL_INT_STATUS_MASK)) - { - printf("\nIteration : %d\n", try); - - accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; - - gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; - - rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - printf("\nFIFO data bytes available : %d \n", fifo_length); - printf("\nFIFO data bytes requested : %d \n", fifoframe.length); - - /* Read FIFO data. */ - rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Read FIFO data on interrupt. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - printf("\nFIFO accel frames requested : %d \n", accel_frame_length); - - /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ - rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); - printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); - - printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); - - /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ - rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); - printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); - - /* Calculating the frame count from the available bytes in FIFO - * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len + header_byte)) */ - frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH + 1)); - printf("\nAvailable frame count from available bytes: %d\n", frame_count); - - printf("\nExtracted accel frames\n"); - - if (accel_frame_length > frame_count) - { - accel_frame_length = frame_count; - } - - /* Print the parsed accelerometer data from the FIFO buffer. */ - for (index = 0; index < accel_frame_length; index++) - { - printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, - fifo_accel_data[index].y, fifo_accel_data[index].z); - } - - printf("\nExtracted gyro frames\n"); - - if (gyro_frame_length > frame_count) - { - gyro_frame_length = frame_count; - } - - /* Print the parsed gyro data from the FIFO buffer. */ - for (index = 0; index < gyro_frame_length; index++) - { - printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", - index, - fifo_gyro_data[index].x, - fifo_gyro_data[index].y, - fifo_gyro_data[index].z); - } - - /* Print control frames like sensor time and skipped frame count. */ - printf("\nSkipped frame count = %d\n", fifoframe.skipped_frame_count); - - printf("Sensor time = %lu\r\n", fifoframe.sensor_time); - - try++; - } - } - } - - bmi2_coines_deinit(); - - return rslt; -} - -/*! - * @brief This internal API is used to set configurations for accel and gyro. - */ -static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Structure to define accel and gyro configurations. */ - struct bmi2_sens_config config[2]; - - /* Configure the type of feature. */ - config[0].type = BMI2_ACCEL; - config[1].type = BMI2_GYRO; - - /* Get default configurations for the type of feature selected. */ - rslt = bmi270_get_sensor_config(config, 2, bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* NOTE: The user can change the following configuration parameters according to their requirement. */ - /* Accel configuration settings. */ - /* Set Output Data Rate */ - config[0].cfg.acc.odr = BMI2_ACC_ODR_25HZ; - - /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ - config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; - - /* The bandwidth parameter is used to configure the number of sensor samples that are averaged - * if it is set to 2, then 2^(bandwidth parameter) samples - * are averaged, resulting in 4 averaged samples - * Note1 : For more information, refer the datasheet. - * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but - * this has an adverse effect on the power consumed. - */ - config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; - - /* Enable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - * For more info refer datasheet. - */ - config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; - - /* Gyro configuration settings. */ - /* Set Output Data Rate */ - config[1].cfg.gyr.odr = BMI2_GYR_ODR_25HZ; - - /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ - config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; - - /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ - config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; - - /* Enable/Disable the noise performance mode for precision yaw rate sensing - * There are two modes - * 0 -> Ultra low power mode(Default) - * 1 -> High performance mode - */ - config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; - - /* Enable/Disable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - */ - config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; - - /* Set new configurations. */ - rslt = bmi270_set_sensor_config(config, 2, bmi2_dev); - bmi2_error_codes_print_result(rslt); - } - - return rslt; -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_full_headerless_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_full_headerless_mode/Makefile deleted file mode 100644 index ae9a413fe5..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_full_headerless_mode/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= fifo_full_headerless_mode.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_full_headerless_mode/fifo_full_headerless_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_full_headerless_mode/fifo_full_headerless_mode.c deleted file mode 100644 index 53dddc4ba6..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_full_headerless_mode/fifo_full_headerless_mode.c +++ /dev/null @@ -1,304 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270.h" -#include "common.h" - -/******************************************************************************/ -/*! Macros */ - -/*! Buffer size allocated to store raw FIFO data */ -#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) - -/*! Length of data to be read from FIFO */ -#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) - -/*! Number of accel frames to be extracted from FIFO */ - -/*! Calculation for frame count: Total frame count = Fifo buffer size(2048)/ Total frames(6 Accel, 6 Gyro totaling to - * 12) which equals to 170. - */ -#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(169) - -/*! Number of gyro frames to be extracted from FIFO */ -#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(169) - -/******************************************************************************/ -/*! Static Function Declaration */ - -/*! - * @brief This internal API is used to set configurations for accel and gyro. - * @param[in] dev : Structure instance of bmi2_dev. - * @return Status of execution. - */ -static int8_t set_accel_gyro_config(struct bmi2_dev *dev); - -/******************************************************************************/ - -/* This function starts the execution of program. */ -int main(void) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - uint16_t index = 0; - - uint16_t fifo_length = 0; - - uint8_t try = 1; - - /* Variable to get fifo full interrupt status. */ - uint16_t int_status = 0; - - uint16_t accel_frame_length; - - uint16_t gyro_frame_length; - - /* Variable to store the available frame length count in FIFO */ - uint8_t frame_count; - - /* Number of bytes of FIFO data. */ - uint8_t fifo_data[BMI2_FIFO_RAW_DATA_BUFFER_SIZE] = { 0 }; - - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Array of accelerometer frames -> Total bytes = - * 170 * (6 axes bytes) = 1020 bytes */ - struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; - - /* Array of gyro frames -> Total bytes = - * 170 * (6 axes bytes) = 1020 bytes */ - struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; - - /* Initialize FIFO frame structure. */ - struct bmi2_fifo_frame fifoframe = { 0 }; - - /* Accel and gyro sensor are listed in array. */ - uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270. */ - rslt = bmi270_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Enable accel and gyro sensor. */ - rslt = bmi270_sensor_enable(sensor_sel, 2, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Configuration settings for accel and gyro. */ - rslt = set_accel_gyro_config(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Before setting FIFO, disable the advance power save mode. */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Initially disable all configurations in fifo. */ - rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Update FIFO structure. */ - /* Mapping the buffer to store the fifo data. */ - fifoframe.data = fifo_data; - - /* Length of FIFO frame. */ - fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH; - - /* Set FIFO configuration by enabling accel, gyro. - * NOTE 1: The header mode is enabled by default. - * NOTE 2: By default the FIFO operating mode is FIFO mode. */ - rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - printf("FIFO is configured in headerless mode\n"); - - /* To enable headerless mode, disable the header. */ - if (rslt == BMI2_OK) - { - rslt = bmi2_set_fifo_config(BMI2_FIFO_HEADER_EN, BMI2_DISABLE, &bmi2_dev); - } - - /* Map FIFO full interrupt. */ - fifoframe.data_int_map = BMI2_FFULL_INT; - rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - while (try <= 10) - { - /* Read FIFO data on interrupt. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if ((rslt == BMI2_OK) && (int_status & BMI2_FFULL_INT_STATUS_MASK)) - { - printf("\nIteration : %d\n", try); - - rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; - gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; - - printf("\nFIFO data bytes available : %d \n", fifo_length); - printf("\nFIFO data bytes requested : %d \n", fifoframe.length); - - /* Read FIFO data. */ - rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Read FIFO data on interrupt. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - printf("\nFIFO accel frames requested : %d \n", accel_frame_length); - - /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ - rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); - printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); - - printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); - - /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ - rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); - printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); - - /* Calculating the frame count from the available bytes in FIFO - * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len)) */ - frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH)); - printf("\nAvailable frame count from available bytes: %d\n", frame_count); - - printf("\nExtracted accel frames\n"); - - if (accel_frame_length > frame_count) - { - accel_frame_length = frame_count; - } - - /* Print the parsed accelerometer data from the FIFO buffer. */ - for (index = 0; index < accel_frame_length; index++) - { - printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, - fifo_accel_data[index].y, fifo_accel_data[index].z); - } - - printf("\nExtracted gyro frames\n"); - - if (gyro_frame_length > frame_count) - { - gyro_frame_length = frame_count; - } - - /* Print the parsed gyro data from the FIFO buffer. */ - for (index = 0; index < gyro_frame_length; index++) - { - printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", - index, - fifo_gyro_data[index].x, - fifo_gyro_data[index].y, - fifo_gyro_data[index].z); - } - } - - try++; - } - } - - bmi2_coines_deinit(); - - return rslt; -} - -/*! - * @brief This internal API is used to set configurations for accel and gyro. - */ -static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Structure to define accel and gyro configurations. */ - struct bmi2_sens_config config[2]; - - /* Configure the type of feature. */ - config[0].type = BMI2_ACCEL; - config[1].type = BMI2_GYRO; - - /* Get default configurations for the type of feature selected. */ - rslt = bmi270_get_sensor_config(config, 2, bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* NOTE: The user can change the following configuration parameters according to their requirement. */ - /* Accel configuration settings. */ - /* Set Output Data Rate */ - config[0].cfg.acc.odr = BMI2_ACC_ODR_100HZ; - - /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ - config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; - - /* The bandwidth parameter is used to configure the number of sensor samples that are averaged - * if it is set to 2, then 2^(bandwidth parameter) samples - * are averaged, resulting in 4 averaged samples - * Note1 : For more information, refer the datasheet. - * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but - * this has an adverse effect on the power consumed. - */ - config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; - - /* Enable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - * For more info refer datasheet. - */ - config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; - - /* Gyro configuration settings. */ - /* Set Output Data Rate */ - config[1].cfg.gyr.odr = BMI2_GYR_ODR_100HZ; - - /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ - config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; - - /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ - config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; - - /* Enable/Disable the noise performance mode for precision yaw rate sensing - * There are two modes - * 0 -> Ultra low power mode(Default) - * 1 -> High performance mode - */ - config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; - - /* Enable/Disable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - */ - config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; - - /* Set new configurations. */ - rslt = bmi270_set_sensor_config(config, 2, bmi2_dev); - bmi2_error_codes_print_result(rslt); - } - - return rslt; -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_watermark_header_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_watermark_header_mode/Makefile deleted file mode 100644 index 08158205b5..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_watermark_header_mode/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= fifo_watermark_header_mode.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_watermark_header_mode/fifo_watermark_header_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_watermark_header_mode/fifo_watermark_header_mode.c deleted file mode 100644 index faaeb307ea..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_watermark_header_mode/fifo_watermark_header_mode.c +++ /dev/null @@ -1,335 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270.h" -#include "common.h" - -/******************************************************************************/ -/*! Macros */ - -/*! Buffer size allocated to store raw FIFO data */ -#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) - -/*! Length of data to be read from FIFO */ -#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) - -/*! Number of accel frames to be extracted from FIFO */ - -/*! - * Calculation: - * fifo_watermark_level = 650, accel_frame_len = 6, gyro_frame_len = 6 header_byte = 1. - * fifo_accel_frame_count = (650 / (6 + 6 + 1 )) = 50 frames - * NOTE: Extra frames are read in order to get sensor time - */ -#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(55) - -/*! Number of gyro frames to be extracted from FIFO */ -#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(55) - -/*! Setting a watermark level in FIFO */ -#define BMI270_FIFO_WATERMARK_LEVEL UINT16_C(650) - -/*! Macro to read sensortime byte in FIFO */ -#define SENSORTIME_OVERHEAD_BYTE UINT8_C(3) - -/******************************************************************************/ -/*! Static Function Declaration */ - -/*! - * @brief This internal API is used to set configurations for accel and gyro. - * @param[in] dev : Structure instance of bmi2_dev. - * @return Status of execution. - */ -static int8_t set_accel_gyro_config(struct bmi2_dev *dev); - -/******************************************************************************/ -/*! Functions */ - -/* This function starts the execution of program. */ -int main(void) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Variable to store the available frame length count in FIFO */ - uint8_t frame_count; - - /* Variable to index bytes. */ - uint16_t index = 0; - - uint16_t fifo_length = 0; - - uint16_t accel_frame_length; - - uint16_t gyro_frame_length; - - uint8_t try = 1; - - /* To read sensortime, extra 3 bytes are added to fifo buffer */ - uint16_t fifo_buffer_size = BMI2_FIFO_RAW_DATA_BUFFER_SIZE + SENSORTIME_OVERHEAD_BYTE; - - /* Number of bytes of FIFO data. */ - uint8_t fifo_data[fifo_buffer_size]; - - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Array of accelerometer frames -> Total bytes = - * 55 * (6 axes bytes) = 330 bytes */ - struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; - - /* Array of gyro frames -> Total bytes = - * 55 * (6 axes bytes) = 330 bytes */ - struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; - - /* Initialize FIFO frame structure. */ - struct bmi2_fifo_frame fifoframe = { 0 }; - - /* Accel and gyro sensor are listed in array. */ - uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; - - /* Variable to get fifo water-mark interrupt status. */ - uint16_t int_status = 0; - - uint16_t watermark = 0; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270. */ - rslt = bmi270_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Enable accel and gyro sensor. */ - rslt = bmi270_sensor_enable(sensor_sel, 2, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Configuration settings for accel and gyro. */ - rslt = set_accel_gyro_config(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Before setting FIFO, disable the advance power save mode. */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Initially disable all configurations in fifo. */ - rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Update FIFO structure. */ - /* Mapping the buffer to store the fifo data. */ - fifoframe.data = fifo_data; - - /* Length of FIFO frame. */ - /* To read sensortime, extra 3 bytes are added to fifo user length. */ - fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH + SENSORTIME_OVERHEAD_BYTE; - - /* Set FIFO configuration by enabling accel, gyro and timestamp. - * NOTE 1: The header mode is enabled by default. - * NOTE 2: By default the FIFO operating mode is FIFO mode. - * NOTE 3: Sensortime is enabled by default */ - printf("FIFO is configured in header mode\n"); - rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* FIFO water-mark interrupt is enabled. */ - fifoframe.data_int_map = BMI2_FWM_INT; - - /* Map water-mark interrupt to the required interrupt pin. */ - rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Set water-mark level. */ - fifoframe.wm_lvl = BMI270_FIFO_WATERMARK_LEVEL; - - /* Set the water-mark level if water-mark interrupt is mapped. */ - rslt = bmi2_set_fifo_wm(fifoframe.wm_lvl, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - rslt = bmi2_get_fifo_wm(&watermark, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - printf("\nFIFO watermark level is %d\n", watermark); - - while (try <= 10) - { - /* Read FIFO data on interrupt. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* To check the status of FIFO watermark interrupt. */ - if ((rslt == BMI2_OK) && (int_status & BMI2_FWM_INT_STATUS_MASK)) - { - printf("\nIteration : %d\n", try); - - printf("\nWatermark interrupt occurred\n"); - - rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; - gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; - - printf("\nFIFO data bytes available : %d \n", fifo_length); - printf("\nFIFO data bytes requested : %d \n", fifoframe.length); - - /* Read FIFO data. */ - rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Read FIFO data on interrupt. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - printf("\nFIFO accel frames requested : %d \n", accel_frame_length); - - /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ - rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); - printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); - - printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); - - /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ - rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); - printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); - - /* Calculating the frame count from the available bytes in FIFO - * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len + header_byte)) */ - frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH + 1)); - printf("\nAvailable frame count from available bytes: %d\n", frame_count); - - printf("\nExracted accel frames\n"); - - if (accel_frame_length > frame_count) - { - accel_frame_length = frame_count; - } - - /* Print the parsed accelerometer data from the FIFO buffer. */ - for (index = 0; index < accel_frame_length; index++) - { - printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, - fifo_accel_data[index].y, fifo_accel_data[index].z); - } - - printf("\nExtracted gyro frames\n"); - - if (gyro_frame_length > frame_count) - { - gyro_frame_length = frame_count; - } - - /* Print the parsed gyro data from the FIFO buffer. */ - for (index = 0; index < gyro_frame_length; index++) - { - printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", - index, - fifo_gyro_data[index].x, - fifo_gyro_data[index].y, - fifo_gyro_data[index].z); - } - - /* Print control frames like sensor time and skipped frame count. */ - printf("Skipped frame count = %d\n", fifoframe.skipped_frame_count); - printf("Sensor time = %lu\r\n", fifoframe.sensor_time); - - try++; - } - } - } - - bmi2_coines_deinit(); - - return rslt; -} - -/*! - * @brief This internal API is used to set configurations for accel and gyro. - */ -static int8_t set_accel_gyro_config(struct bmi2_dev *dev) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Structure to define accel and gyro configurations. */ - struct bmi2_sens_config config[2]; - - /* Configure the type of feature. */ - config[0].type = BMI2_ACCEL; - config[1].type = BMI2_GYRO; - - /* Get default configurations for the type of feature selected. */ - rslt = bmi270_get_sensor_config(config, 2, dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* NOTE: The user can change the following configuration parameters according to their requirement. */ - /* Accel configuration settings. */ - /* Set Output Data Rate */ - config[0].cfg.acc.odr = BMI2_ACC_ODR_25HZ; - - /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ - config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; - - /* The bandwidth parameter is used to configure the number of sensor samples that are averaged - * if it is set to 2, then 2^(bandwidth parameter) samples - * are averaged, resulting in 4 averaged samples - * Note1 : For more information, refer the datasheet. - * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but - * this has an adverse effect on the power consumed. - */ - config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; - - /* Enable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - * For more info refer datasheet. - */ - config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; - - /* Gyro configuration settings. */ - /* Set Output Data Rate */ - config[1].cfg.gyr.odr = BMI2_GYR_ODR_25HZ; - - /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ - config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; - - /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ - config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; - - /* Enable/Disable the noise performance mode for precision yaw rate sensing - * There are two modes - * 0 -> Ultra low power mode(Default) - * 1 -> High performance mode - */ - config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; - - /* Enable/Disable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - */ - config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; - - /* Set new configurations. */ - rslt = bmi270_set_sensor_config(config, 2, dev); - bmi2_error_codes_print_result(rslt); - } - - return rslt; -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_watermark_headerless_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_watermark_headerless_mode/Makefile deleted file mode 100644 index 527a18ab8e..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_watermark_headerless_mode/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= fifo_watermark_headerless_mode.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c deleted file mode 100644 index b3adf89659..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c +++ /dev/null @@ -1,331 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270.h" -#include "common.h" - -/******************************************************************************/ -/*! Macros */ - -/*! Buffer size allocated to store raw FIFO data */ -#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) - -/*! Length of data to be read from FIFO */ -#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) - -/*! Number of accel frames to be extracted from FIFO */ - -/*! - * Calculation: - * fifo_watermark_level = 650, accel_frame_len = 6, gyro_frame_len = 6. - * fifo_accel_frame_count = (650 / (6 + 6 )) = 54 frames - */ -#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(55) - -/*! Number of gyro frames to be extracted from FIFO */ -#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(55) - -/*! Setting a watermark level in FIFO */ -#define BMI270_FIFO_WATERMARK_LEVEL UINT16_C(650) - -/******************************************************************************/ -/*! Static Function Declaration */ - -/*! - * @brief This internal API is used to set configurations for accel and gyro. - * @param[in] dev : Structure instance of bmi2_dev. - * @return Status of execution. - */ -static int8_t set_accel_gyro_config(struct bmi2_dev *dev); - -/******************************************************************************/ -/*! Functions */ - -/* This function starts the execution of program. */ -int main(void) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Variable to index bytes. */ - uint16_t index = 0; - - /* Variable to store the available frame length count in FIFO */ - uint8_t frame_count; - - uint16_t fifo_length = 0; - - uint16_t accel_frame_length; - - uint16_t gyro_frame_length; - - uint8_t try = 1; - - /* Number of bytes of FIFO data. */ - uint8_t fifo_data[BMI2_FIFO_RAW_DATA_BUFFER_SIZE] = { 0 }; - - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Array of accelerometer frames -> Total bytes = - * 50 * (6 axes bytes) = 300 bytes */ - struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; - - /* Array of gyro frames -> Total bytes = - * 50 * (6 axes bytes) = 300 bytes */ - struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; - - /* Initialize FIFO frame structure. */ - struct bmi2_fifo_frame fifoframe = { 0 }; - - /* Accel and gyro sensor are listed in array. */ - uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; - - /* Variable to get fifo water-mark interrupt status. */ - uint16_t int_status = 0; - - uint16_t watermark = 0; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270. */ - rslt = bmi270_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Enable accel and gyro sensor. */ - rslt = bmi270_sensor_enable(sensor_sel, 2, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Configuration settings for accel and gyro. */ - rslt = set_accel_gyro_config(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Before setting FIFO, disable the advance power save mode. */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Initially disable all configurations in fifo. */ - rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Update FIFO structure. */ - /* Mapping the buffer to store the fifo data. */ - fifoframe.data = fifo_data; - - /* Length of FIFO frame. */ - fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH; - - /* Set FIFO configuration by enabling accel, gyro. - * NOTE 1: The header mode is enabled by default. - * NOTE 2: By default the FIFO operating mode is FIFO mode. */ - rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - printf("FIFO is configured in headerless mode\n"); - - /* To enable headerless mode, disable the header. */ - if (rslt == BMI2_OK) - { - rslt = bmi2_set_fifo_config(BMI2_FIFO_HEADER_EN, BMI2_DISABLE, &bmi2_dev); - } - - /* FIFO water-mark interrupt is enabled. */ - fifoframe.data_int_map = BMI2_FWM_INT; - - /* Map water-mark interrupt to the required interrupt pin. */ - rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Set water-mark level. */ - fifoframe.wm_lvl = BMI270_FIFO_WATERMARK_LEVEL; - - fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH; - - /* Set the water-mark level if water-mark interrupt is mapped. */ - rslt = bmi2_set_fifo_wm(fifoframe.wm_lvl, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - rslt = bmi2_get_fifo_wm(&watermark, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - printf("\nFIFO watermark level is %d\n", watermark); - - while (try <= 10) - { - /* Read FIFO data on interrupt. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* To check the status of FIFO watermark interrupt. */ - if ((rslt == BMI2_OK) && (int_status & BMI2_FWM_INT_STATUS_MASK)) - { - printf("\nIteration : %d\n", try); - - printf("\nWatermark interrupt occurred\n"); - - rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; - gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; - - printf("\nFIFO data bytes available : %d \n", fifo_length); - printf("\nFIFO data bytes requested : %d \n", fifoframe.length); - - /* Read FIFO data. */ - rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Read FIFO data on interrupt. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - printf("\nFIFO accel frames requested : %d \n", accel_frame_length); - - /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ - rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); - printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); - - printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); - - /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ - rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); - printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); - - /* Calculating the frame count from the available bytes in FIFO - * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len)) */ - frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH)); - printf("\nAvailable frame count from available bytes: %d\n", frame_count); - - printf("\nExracted accel frames\n"); - - if (accel_frame_length > frame_count) - { - accel_frame_length = frame_count; - } - - /* Print the parsed accelerometer data from the FIFO buffer. */ - for (index = 0; index < accel_frame_length; index++) - { - printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, - fifo_accel_data[index].y, fifo_accel_data[index].z); - } - - printf("\nExtracted gyro frames\n"); - - if (gyro_frame_length > frame_count) - { - gyro_frame_length = frame_count; - } - - /* Print the parsed gyro data from the FIFO buffer. */ - for (index = 0; index < gyro_frame_length; index++) - { - printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", - index, - fifo_gyro_data[index].x, - fifo_gyro_data[index].y, - fifo_gyro_data[index].z); - } - } - - try++; - } - } - - bmi2_coines_deinit(); - - return rslt; -} - -/*! - * @brief This internal API is used to set configurations for accel. - */ -static int8_t set_accel_gyro_config(struct bmi2_dev *dev) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Structure to define accel and gyro configurations. */ - struct bmi2_sens_config config[2]; - - /* Configure the type of feature. */ - config[0].type = BMI2_ACCEL; - config[1].type = BMI2_GYRO; - - /* Get default configurations for the type of feature selected. */ - rslt = bmi270_get_sensor_config(config, 2, dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* NOTE: The user can change the following configuration parameter according to their requirement. */ - /* Accel configuration settings. */ - /* Set Output Data Rate */ - config[0].cfg.acc.odr = BMI2_ACC_ODR_100HZ; - - /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ - config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; - - /* The bandwidth parameter is used to configure the number of sensor samples that are averaged - * if it is set to 2, then 2^(bandwidth parameter) samples - * are averaged, resulting in 4 averaged samples - * Note1 : For more information, refer the datasheet. - * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but - * this has an adverse effect on the power consumed. - */ - config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; - - /* Enable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - * For more info refer datasheet. - */ - config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; - - /* Gyro configuration settings. */ - /* Set Output Data Rate */ - config[1].cfg.gyr.odr = BMI2_GYR_ODR_100HZ; - - /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ - config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; - - /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ - config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; - - /* Enable/Disable the noise performance mode for precision yaw rate sensing - * There are two modes - * 0 -> Ultra low power mode(Default) - * 1 -> High performance mode - */ - config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; - - /* Enable/Disable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - */ - config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; - - /* Set new configurations. */ - rslt = bmi270_set_sensor_config(config, 2, dev); - bmi2_error_codes_print_result(rslt); - } - - return rslt; -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/gyro/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/gyro/Makefile deleted file mode 100644 index e1f03b8bd2..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/gyro/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= gyro.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/gyro/gyro.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/gyro/gyro.c deleted file mode 100644 index 1bb0ca471b..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/gyro/gyro.c +++ /dev/null @@ -1,195 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270.h" -#include "common.h" - -/******************************************************************************/ -/*! Static Function Declaration */ - -/*! - * @brief This internal API is used to set configurations for gyro. - * - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Status of execution. - */ -static int8_t set_gyro_config(struct bmi2_dev *dev); - -/*! - * @brief This function converts lsb to degree per second for 16 bit gyro at - * range 125, 250, 500, 1000 or 2000dps. - * - * @param[in] val : LSB from each axis. - * @param[in] dps : Degree per second. - * @param[in] bit_width : Resolution for gyro. - * - * @return Degree per second. - */ -static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width); - -/******************************************************************************/ -/*! Functions */ - -/* This function starts the execution of program. */ -int main(void) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Variable to define limit to print gyro data. */ - uint8_t limit = 10; - - uint8_t indx = 0; - - float x = 0, y = 0, z = 0; - - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Create an instance of sensor data structure. */ - struct bmi2_sensor_data sensor_data = { 0 }; - - /* Assign gyro sensor to variable. */ - uint8_t sens_list = BMI2_GYRO; - - /* Initialize the interrupt status of gyro. */ - uint16_t int_status = 0; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270. */ - rslt = bmi270_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Enable the selected sensors. */ - rslt = bmi270_sensor_enable(&sens_list, 1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Gyro configuration settings. */ - rslt = set_gyro_config(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Select gyro sensor. */ - sensor_data.type = BMI2_GYRO; - printf("Gyro and DPS data\n"); - printf("Gyro data collected at Range 2000 dps with 16-bit resolution\n"); - - /* Loop to print gyro data when interrupt occurs. */ - while (indx <= limit) - { - /* To get the data ready interrupt status of gyro. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* To check the data ready interrupt status and print the status for 10 samples. */ - if (int_status & BMI2_GYR_DRDY_INT_MASK) - { - /* Get gyro data for x, y and z axis. */ - rslt = bmi270_get_sensor_data(&sensor_data, 1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - printf("\nGyr_X = %d\t", sensor_data.sens_data.gyr.x); - printf("Gyr_Y = %d\t", sensor_data.sens_data.gyr.y); - printf("Gyr_Z = %d\r\n", sensor_data.sens_data.gyr.z); - - /* Converting lsb to degree per second for 16 bit gyro at 2000dps range. */ - x = lsb_to_dps(sensor_data.sens_data.gyr.x, 2000, bmi2_dev.resolution); - y = lsb_to_dps(sensor_data.sens_data.gyr.y, 2000, bmi2_dev.resolution); - z = lsb_to_dps(sensor_data.sens_data.gyr.z, 2000, bmi2_dev.resolution); - - /* Print the data in dps. */ - printf("Gyr_DPS_X = %4.2f, Gyr_DPS_Y = %4.2f, Gyr_DPS_Z = %4.2f\n", x, y, z); - - indx++; - } - } - } - } - - bmi2_coines_deinit(); - - return rslt; -} - -/*! - * @brief This internal API is used to set configurations for gyro. - */ -static int8_t set_gyro_config(struct bmi2_dev *dev) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Structure to define the type of sensor and its configurations. */ - struct bmi2_sens_config config; - - /* Configure the type of feature. */ - config.type = BMI2_GYRO; - - /* Get default configurations for the type of feature selected. */ - rslt = bmi270_get_sensor_config(&config, 1, dev); - bmi2_error_codes_print_result(rslt); - - /* Map data ready interrupt to interrupt pin. */ - rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT2, dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* The user can change the following configuration parameters according to their requirement. */ - /* Set Output Data Rate */ - config.cfg.gyr.odr = BMI2_GYR_ODR_100HZ; - - /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ - config.cfg.gyr.range = BMI2_GYR_RANGE_2000; - - /* Gyroscope bandwidth parameters. By default the gyro bandwidth is in normal mode. */ - config.cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; - - /* Enable/Disable the noise performance mode for precision yaw rate sensing - * There are two modes - * 0 -> Ultra low power mode(Default) - * 1 -> High performance mode - */ - config.cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; - - /* Enable/Disable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - */ - config.cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; - - /* Set the gyro configurations. */ - rslt = bmi270_set_sensor_config(&config, 1, dev); - } - - return rslt; -} - -/*! - * @brief This function converts lsb to degree per second for 16 bit gyro at - * range 125, 250, 500, 1000 or 2000dps. - */ -static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width) -{ - float half_scale = ((float)(1 << bit_width) / 2.0f); - - return (dps / ((half_scale) + BMI2_GYR_RANGE_2000)) * (val); -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/no_motion_interrupt/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/no_motion_interrupt/Makefile deleted file mode 100644 index c5df6dfb3c..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/no_motion_interrupt/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= no_motion_interrupt.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/no_motion_interrupt/no_motion_interrupt.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/no_motion_interrupt/no_motion_interrupt.c deleted file mode 100644 index 5c46306602..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/no_motion_interrupt/no_motion_interrupt.c +++ /dev/null @@ -1,134 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270.h" -#include "common.h" - -/******************************************************************************/ -/*! Static Function Declaration */ - -/*! - * @brief This internal API is used to set configurations for no-motion. - * - * @param[in] bmi2_dev : Structure instance of bmi2_dev. - * - * @return Status of execution. - */ -static int8_t set_feature_config(struct bmi2_dev *bmi2_dev); - -/******************************************************************************/ -/*! Functions */ - -/* This function starts the execution of program. */ -int main(void) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Accel sensor and no-motion feature are listed in array. */ - uint8_t sens_list[2] = { BMI2_ACCEL, BMI2_NO_MOTION }; - - /* Variable to get no-motion interrupt status. */ - uint16_t int_status = 0; - - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Select features and their pins to be mapped to. */ - struct bmi2_sens_int_config sens_int = { .type = BMI2_NO_MOTION, .hw_int_pin = BMI2_INT1 }; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270. */ - rslt = bmi270_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Enable the selected sensors. */ - rslt = bmi270_sensor_enable(sens_list, 2, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Set feature configurations for no-motion. */ - rslt = set_feature_config(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Map the feature interrupt for no-motion. */ - rslt = bmi270_map_feat_int(&sens_int, 1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - printf("Do not move the board\n"); - - /* Loop to get no-motion interrupt. */ - do - { - /* Clear buffer. */ - int_status = 0; - - /* To get the interrupt status of no-motion. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* To check the interrupt status of no-motion. */ - if (int_status & BMI270_NO_MOT_STATUS_MASK) - { - printf("No-motion interrupt is generated\n"); - break; - } - } while (rslt == BMI2_OK); - } - } - } - - bmi2_coines_deinit(); - - return rslt; -} - -/*! - * @brief This internal API is used to set configurations for no-motion. - */ -static int8_t set_feature_config(struct bmi2_dev *bmi2_dev) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Structure to define the type of sensor and its configurations. */ - struct bmi2_sens_config config; - - /* Configure the type of feature. */ - config.type = BMI2_NO_MOTION; - - /* Get default configurations for the type of feature selected. */ - rslt = bmi270_get_sensor_config(&config, 1, bmi2_dev); - bmi2_error_codes_print_result(rslt); - if (rslt == BMI2_OK) - { - /* NOTE: The user can change the following configuration parameters according to their requirement. */ - /* 1LSB equals 20ms. Default is 100ms, setting to 80ms. */ - config.cfg.no_motion.duration = 0x04; - - /* 1LSB equals to 0.48mg. Default is 83mg, setting to 50mg. */ - config.cfg.no_motion.threshold = 0x68; - - /* Set new configurations. */ - rslt = bmi270_set_sensor_config(&config, 1, bmi2_dev); - bmi2_error_codes_print_result(rslt); - } - - return rslt; -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/read_aux_data_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/read_aux_data_mode/Makefile deleted file mode 100644 index db669c09dc..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/read_aux_data_mode/Makefile +++ /dev/null @@ -1,23 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= read_aux_data_mode.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -BMM150_SOURCE ?= ../../../bmm150 - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270.c \ -$(BMM150_SOURCE)/bmm150.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(BMM150_SOURCE) \ -$(COMMON_LOCATION)/common - - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/read_aux_data_mode/read_aux_data_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/read_aux_data_mode/read_aux_data_mode.c deleted file mode 100644 index a542f97c61..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/read_aux_data_mode/read_aux_data_mode.c +++ /dev/null @@ -1,327 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270.h" -#include "bmm150.h" -#include "coines.h" -#include "common.h" - -/******************************************************************************/ -/*! Macro definition */ - -/*! Macros to select the sensors */ -#define ACCEL UINT8_C(0x00) -#define GYRO UINT8_C(0x01) -#define AUX UINT8_C(0x02) - -/*! Earth's gravity in m/s^2 */ -#define GRAVITY_EARTH (9.80665f) - -/*****************************************************************************/ -/*! Structure declaration */ - -/* Sensor initialization configuration. */ -struct bmi2_dev bmi2_dev; - -/*******************************************************************************/ -/*! Functions */ - -/** - * user_aux_read - Reads data from auxiliary sensor in manual mode. - * - * @param[in] reg_addr : Register address. - * @param[out] aux_data : Aux data pointer to store the read data. - * @param[in] length : No of bytes to read. - * @param[in] intf_ptr : Interface pointer - * - * @return Status of execution - * @retval 0 -> Success - * @retval < 0 -> Failure Info - */ -static int8_t user_aux_read(uint8_t reg_addr, uint8_t *aux_data, uint32_t len, void *intf_ptr); - -/** - * user_aux_write - Writes data to the auxiliary sensor in manual mode. - * - * @param[in] reg_addr : Register address. - * @param[out] aux_data : Aux data pointer to store the data being written. - * @param[in] length : No of bytes to read. - * @param[in] intf_ptr : Interface pointer - * - * @return Status of execution - * @retval 0 -> Success - * @retval < 0 -> Failure Info - */ -static int8_t user_aux_write(uint8_t reg_addr, const uint8_t *aux_data, uint32_t len, void *intf_ptr); - -/*! - * @brief This function provides the delay for required time (Microsecond) as per the input provided in some of the - * APIs. - * - * @param[in] period_us : The required wait time in microsecond. - * @param[in] intf_ptr : Interface pointer - * - * @return void. - */ -static void user_delay_us(uint32_t period, void *intf_ptr); - -/*! - * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at - * range 2G, 4G, 8G or 16G. - * - * @param[in] val : LSB from each axis. - * @param[in] g_range : Gravity range. - * @param[in] bit_width : Resolution for accel. - * - * @return Gravity. - */ -static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width); - -/*! - * @brief This function converts lsb to degree per second for 16 bit gyro at - * range 125, 250, 500, 1000 or 2000dps. - * - * @param[in] val : LSB from each axis. - * @param[in] dps : Degree per second. - * @param[in] bit_width : Resolution for gyro. - * - * @return Degree per second. - */ -static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width); - -/* This function starts the execution of program. */ -int main(void) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Variable to select the pull-up resistor which is set to trim register */ - uint8_t regdata; - - /* Variable to define limit to print aux data. */ - uint8_t limit = 20; - - /* Accel, Gyro and Aux sensors are listed in array. */ - uint8_t sensor_list[3] = { BMI2_ACCEL, BMI2_GYRO, BMI2_AUX }; - - /* Structure to define the type of the sensor and its configurations. */ - struct bmi2_sens_config config[3]; - - /* Sensor initialization configuration. */ - struct bmm150_dev bmm150_dev; - - /* bmm150 settings configuration */ - struct bmm150_settings settings; - - /* bmm150 magnetometer data */ - struct bmm150_mag_data mag_data; - - /* Structure to define type of sensor and their respective data. */ - struct bmi2_sensor_data sensor_data[3]; - - /* Variables to define read the accel and gyro data in float */ - float x = 0, y = 0, z = 0; - - config[ACCEL].type = BMI2_ACCEL; - config[GYRO].type = BMI2_GYRO; - config[AUX].type = BMI2_AUX; - - /* To enable the i2c interface settings for bmm150. */ - uint8_t bmm150_dev_addr = BMM150_DEFAULT_I2C_ADDRESS; - bmm150_dev.intf_ptr = &bmm150_dev_addr; - bmm150_dev.read = user_aux_read; - bmm150_dev.write = user_aux_write; - bmm150_dev.delay_us = user_delay_us; - - /* As per datasheet, aux interface with bmi270 will support only for I2C */ - bmm150_dev.intf = BMM150_I2C_INTF; - - sensor_data[ACCEL].type = BMI2_ACCEL; - sensor_data[GYRO].type = BMI2_GYRO; - sensor_data[AUX].type = BMI2_AUX; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270. */ - rslt = bmi270_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Pull-up resistor 2k is set to the trim regiter */ - regdata = BMI2_ASDA_PUPSEL_2K; - rslt = bmi2_set_regs(BMI2_AUX_IF_TRIM, ®data, 1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Enable the accel, gyro and aux sensor. */ - rslt = bmi270_sensor_enable(sensor_list, 3, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Get default configurations for the type of feature selected. */ - rslt = bmi270_get_sensor_config(config, 3, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Configurations for accel. */ - config[ACCEL].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; - config[ACCEL].cfg.acc.bwp = BMI2_ACC_OSR2_AVG2; - config[ACCEL].cfg.acc.odr = BMI2_ACC_ODR_100HZ; - config[ACCEL].cfg.acc.range = BMI2_ACC_RANGE_2G; - - /* Configurations for gyro. */ - config[GYRO].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; - config[GYRO].cfg.gyr.noise_perf = BMI2_GYR_RANGE_2000; - config[GYRO].cfg.gyr.bwp = BMI2_GYR_OSR2_MODE; - config[GYRO].cfg.gyr.odr = BMI2_GYR_ODR_100HZ; - config[GYRO].cfg.gyr.range = BMI2_GYR_RANGE_2000; - config[GYRO].cfg.gyr.ois_range = BMI2_GYR_OIS_2000; - - /* Configurations for aux. */ - config[AUX].cfg.aux.odr = BMI2_AUX_ODR_100HZ; - config[AUX].cfg.aux.aux_en = BMI2_ENABLE; - config[AUX].cfg.aux.i2c_device_addr = BMM150_DEFAULT_I2C_ADDRESS; - config[AUX].cfg.aux.manual_en = BMI2_ENABLE; - config[AUX].cfg.aux.fcu_write_en = BMI2_ENABLE; - config[AUX].cfg.aux.man_rd_burst = BMI2_AUX_READ_LEN_3; - config[AUX].cfg.aux.read_addr = BMM150_REG_DATA_X_LSB; - - /* Set new configurations for accel, gyro and aux. */ - rslt = bmi270_set_sensor_config(config, 3, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmm150. */ - rslt = bmm150_init(&bmm150_dev); - bmi2_error_codes_print_result(rslt); - - /* Set the power mode to normal mode. */ - settings.pwr_mode = BMM150_POWERMODE_NORMAL; - rslt = bmm150_set_op_mode(&settings, &bmm150_dev); - bmi2_error_codes_print_result(rslt); - - rslt = bmi270_get_sensor_config(config, 3, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Disable manual mode so that the data mode is enabled. */ - config[AUX].cfg.aux.manual_en = BMI2_DISABLE; - - /* Set the aux configurations. */ - rslt = bmi270_set_sensor_config(config, 3, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - printf("MAGNETOMETER, ACCEL, AND GYRO DATA IN DATA MODE\n"); - - if (bmm150_dev.chip_id == BMM150_CHIP_ID) - { - while (limit--) - { - /* Delay has been added to get aux, accel and gyro data at the interval of every 0.05 second. */ - bmi2_dev.delay_us(50000, bmi2_dev.intf_ptr); - - rslt = bmi270_get_sensor_data(sensor_data, 3, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - printf("\nAcc_x = %d\t", sensor_data[ACCEL].sens_data.acc.x); - printf("Acc_y = %d\t", sensor_data[ACCEL].sens_data.acc.y); - printf("Acc_z = %d", sensor_data[ACCEL].sens_data.acc.z); - - /* Converting lsb to meter per second squared for 16 bit accelerometer at 2G range. */ - x = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.x, 2, bmi2_dev.resolution); - y = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.y, 2, bmi2_dev.resolution); - z = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.z, 2, bmi2_dev.resolution); - - /* Print the data in m/s2. */ - printf("\nAcc_ms2_X = %4.2f, Acc_ms2_Y = %4.2f, Acc_ms2_Z = %4.2f\n", x, y, z); - - printf("\nGyr_X = %d\t", sensor_data[GYRO].sens_data.gyr.x); - printf("Gyr_Y = %d\t", sensor_data[GYRO].sens_data.gyr.y); - printf("Gyr_Z= %d", sensor_data[GYRO].sens_data.gyr.z); - - /* Converting lsb to degree per second for 16 bit gyro at 2000dps range. */ - x = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.x, 2000, bmi2_dev.resolution); - y = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.y, 2000, bmi2_dev.resolution); - z = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.z, 2000, bmi2_dev.resolution); - - /* Print the data in dps. */ - printf("\nGyro_DPS_X = %4.2f, Gyro_DPS_Y = %4.2f, Gyro_DPS_Z = %4.2f\n", x, y, z); - - /* Compensating the raw auxiliary data available from the BMM150 API. */ - rslt = bmm150_aux_mag_data(sensor_data[AUX].sens_data.aux_data, &mag_data, &bmm150_dev); - bmi2_error_codes_print_result(rslt); - printf("\nMag_x_axis = %d \t Mag_y_axis = %d \t Mag_z_axis = %d \t\n", - mag_data.x, - mag_data.y, - mag_data.z); - } - } - } - - bmi2_coines_deinit(); - - return rslt; -} - -/*! - * @brief This function reads the data from auxiliary sensor in manual mode. - */ -static int8_t user_aux_read(uint8_t reg_addr, uint8_t *aux_data, uint32_t len, void *intf_ptr) -{ - int8_t rslt; - - /* Discarding the parameter id as it is redundant */ - rslt = bmi2_read_aux_man_mode(reg_addr, aux_data, len, &bmi2_dev); - - return rslt; -} - -/*! - * @brief This function writes the data to auxiliary sensor in manual mode. - */ -static int8_t user_aux_write(uint8_t reg_addr, const uint8_t *aux_data, uint32_t len, void *intf_ptr) -{ - int8_t rslt; - - /* Discarding the parameter id as it is redundant */ - rslt = bmi2_write_aux_man_mode(reg_addr, aux_data, len, &bmi2_dev); - - return rslt; -} - -/*! - * Delay function map to COINES platform - */ -static void user_delay_us(uint32_t period, void *intf_ptr) -{ - coines_delay_usec(period); -} - -/*! - * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at - * range 2G, 4G, 8G or 16G. - */ -static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width) -{ - float half_scale = ((float)(1 << bit_width) / 2.0f); - - return (GRAVITY_EARTH * val * g_range) / half_scale; -} - -/*! - * @brief This function converts lsb to degree per second for 16 bit gyro at - * range 125, 250, 500, 1000 or 2000dps. - */ -static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width) -{ - float half_scale = ((float)(1 << bit_width) / 2.0f); - - return (dps / ((half_scale) + BMI2_GYR_RANGE_2000)) * (val); -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/read_aux_manual_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/read_aux_manual_mode/Makefile deleted file mode 100644 index cc9e6e4e74..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/read_aux_manual_mode/Makefile +++ /dev/null @@ -1,23 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= read_aux_manual_mode.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -BMM150_SOURCE ?= ../../../bmm150 - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270.c \ -$(BMM150_SOURCE)/bmm150.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(BMM150_SOURCE) \ -$(COMMON_LOCATION)/common - - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/read_aux_manual_mode/read_aux_manual_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/read_aux_manual_mode/read_aux_manual_mode.c deleted file mode 100644 index ced8f59c7a..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/read_aux_manual_mode/read_aux_manual_mode.c +++ /dev/null @@ -1,322 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270.h" -#include "bmm150.h" -#include "coines.h" -#include "common.h" - -/******************************************************************************/ -/*! Macro definition */ - -/*! Macros to select the sensors */ -#define ACCEL UINT8_C(0x00) -#define GYRO UINT8_C(0x01) -#define AUX UINT8_C(0x02) - -/*! Earth's gravity in m/s^2 */ -#define GRAVITY_EARTH (9.80665f) - -/*****************************************************************************/ -/*! Structure declaration */ - -/* Sensor initialization configuration. */ -struct bmi2_dev bmi2_dev; - -/******************************************************************************/ -/*! Functions */ - -/** - * user_aux_read - Reads data from auxiliary sensor in manual mode. - * - * @param[in] reg_addr : Register address. - * @param[out] aux_data : Aux data pointer to store the read data. - * @param[in] length : No of bytes to read. - * @param[in] intf_ptr : Interface pointer - * - * @return Status of execution - * @retval 0 -> Success - * @retval < 0 -> Failure Info - */ -static int8_t user_aux_read(uint8_t reg_addr, uint8_t *aux_data, uint32_t len, void *intf_ptr); - -/** - * user_aux_write - Writes data to the auxiliary sensor in manual mode. - * - * @param[in] reg_addr : Register address. - * @param[out] aux_data : Aux data pointer to store the data being written. - * @param[in] length : No of bytes to read. - * @param[in] intf_ptr : Interface pointer - * - * @return Status of execution - * @retval 0 -> Success - * @retval < 0 -> Failure Info - */ -static int8_t user_aux_write(uint8_t reg_addr, const uint8_t *aux_data, uint32_t len, void *intf_ptr); - -/*! - * @brief This function provides the delay for required time (Microsecond) as per the input provided in some of the - * APIs. - * - * @param[in] period_us : The required wait time in microsecond. - * @param[in] intf_ptr : Interface pointer - * - * @return void. - */ -static void user_delay_us(uint32_t period, void *intf_ptr); - -/*! - * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at - * range 2G, 4G, 8G or 16G. - * - * @param[in] val : LSB from each axis. - * @param[in] g_range : Gravity range. - * @param[in] bit_width : Resolution for accel. - * - * @return Gravity. - */ -static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width); - -/*! - * @brief This function converts lsb to degree per second for 16 bit gyro at - * range 125, 250, 500, 1000 or 2000dps. - * - * @param[in] val : LSB from each axis. - * @param[in] dps : Degree per second. - * @param[in] bit_width : Resolution for gyro. - * - * @return Degree per second. - */ -static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width); - -/* This function starts the execution of program. */ -int main(void) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Variable to select the pull-up resistor which is set to trim register */ - uint8_t regdata; - - /* Variable to define limit to print aux data. */ - uint8_t limit = 20; - - /* Accel, Gyro and Aux sensors are listed in array. */ - uint8_t sensor_list[3] = { BMI2_ACCEL, BMI2_GYRO, BMI2_AUX }; - - /* Sensor initialization configuration. */ - struct bmm150_dev bmm150_dev; - - /* bmm150 settings configuration */ - struct bmm150_settings settings; - - /* bmm150 magnetometer data */ - struct bmm150_mag_data mag_data; - - /* Structure to define the type of the sensor and its configurations. */ - struct bmi2_sens_config config[3]; - - /* Structure to define type of sensor and their respective data. */ - struct bmi2_sensor_data sensor_data[2]; - - /* Variables to define read the accel and gyro data in float */ - float x = 0, y = 0, z = 0; - - config[ACCEL].type = BMI2_ACCEL; - config[GYRO].type = BMI2_GYRO; - config[AUX].type = BMI2_AUX; - - sensor_data[ACCEL].type = BMI2_ACCEL; - sensor_data[GYRO].type = BMI2_GYRO; - - /* Array of eight bytes to store x, y, z and r axis aux data. */ - uint8_t aux_data[8] = { 0 }; - - /* To enable the i2c interface settings for bmm150. */ - uint8_t bmm150_dev_addr = BMM150_DEFAULT_I2C_ADDRESS; - bmm150_dev.intf_ptr = &bmm150_dev_addr; - bmm150_dev.read = user_aux_read; - bmm150_dev.write = user_aux_write; - bmm150_dev.delay_us = user_delay_us; - - /* As per datasheet, aux interface with bmi270 will support only for I2C */ - bmm150_dev.intf = BMM150_I2C_INTF; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270. */ - rslt = bmi270_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Pull-up resistor 2k is set to the trim register */ - regdata = BMI2_ASDA_PUPSEL_2K; - rslt = bmi2_set_regs(BMI2_AUX_IF_TRIM, ®data, 1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Enable the accel, gyro and aux sensor. */ - rslt = bmi270_sensor_enable(sensor_list, 3, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Get default configurations for the type of feature selected. */ - rslt = bmi270_get_sensor_config(config, 3, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Configurations for accel. */ - config[ACCEL].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; - config[ACCEL].cfg.acc.bwp = BMI2_ACC_OSR2_AVG2; - config[ACCEL].cfg.acc.odr = BMI2_ACC_ODR_100HZ; - config[ACCEL].cfg.acc.range = BMI2_ACC_RANGE_2G; - - /* Configurations for gyro. */ - config[GYRO].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; - config[GYRO].cfg.gyr.noise_perf = BMI2_GYR_RANGE_2000; - config[GYRO].cfg.gyr.bwp = BMI2_GYR_OSR2_MODE; - config[GYRO].cfg.gyr.odr = BMI2_GYR_ODR_100HZ; - config[GYRO].cfg.gyr.range = BMI2_GYR_RANGE_2000; - config[GYRO].cfg.gyr.ois_range = BMI2_GYR_OIS_2000; - - /* Configurations for aux. */ - config[AUX].cfg.aux.odr = BMI2_AUX_ODR_100HZ; - config[AUX].cfg.aux.aux_en = BMI2_ENABLE; - config[AUX].cfg.aux.i2c_device_addr = BMM150_DEFAULT_I2C_ADDRESS; - config[AUX].cfg.aux.manual_en = BMI2_ENABLE; - config[AUX].cfg.aux.fcu_write_en = BMI2_ENABLE; - config[AUX].cfg.aux.man_rd_burst = BMI2_AUX_READ_LEN_3; - - /* Set new configurations for accel, gyro and aux. */ - rslt = bmi270_set_sensor_config(config, 3, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmm150. */ - rslt = bmm150_init(&bmm150_dev); - bmi2_error_codes_print_result(rslt); - - /* Set the power mode to normal mode. */ - settings.pwr_mode = BMM150_POWERMODE_NORMAL; - rslt = bmm150_set_op_mode(&settings, &bmm150_dev); - bmi2_error_codes_print_result(rslt); - - printf("MAGNETOMETER, ACCEL AND GYRO DATA IN MANUAL MODE\n"); - - if (bmm150_dev.chip_id == BMM150_CHIP_ID) - { - while (limit--) - { - /* Delay has been added to get aux, accel and gyro data at the interval of every 0.05 second. */ - bmi2_dev.delay_us(50000, bmi2_dev.intf_ptr); - - rslt = bmi270_get_sensor_data(sensor_data, 2, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - printf("\nAcc_x = %d\t", sensor_data[ACCEL].sens_data.acc.x); - printf("Acc_y = %d\t", sensor_data[ACCEL].sens_data.acc.y); - printf("Acc_z = %d", sensor_data[ACCEL].sens_data.acc.z); - - /* Converting lsb to meter per second squared for 16 bit accelerometer at 2G range. */ - x = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.x, 2, bmi2_dev.resolution); - y = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.y, 2, bmi2_dev.resolution); - z = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.z, 2, bmi2_dev.resolution); - - /* Print the data in m/s2. */ - printf("\nAcc_ms2_X = %4.2f, Acc_ms2_Y = %4.2f, Acc_ms2_Z = %4.2f\n", x, y, z); - - printf("\nGyr_X = %d\t", sensor_data[GYRO].sens_data.gyr.x); - printf("Gyr_Y = %d\t", sensor_data[GYRO].sens_data.gyr.y); - printf("Gyr_Z = %d", sensor_data[GYRO].sens_data.gyr.z); - - /* Converting lsb to degree per second for 16 bit gyro at 2000dps range. */ - x = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.x, 2000, bmi2_dev.resolution); - y = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.y, 2000, bmi2_dev.resolution); - z = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.z, 2000, bmi2_dev.resolution); - - /* Print the data in dps. */ - printf("\nGyro_DPS_X = %4.2f, Gyro_DPS_Y = %4.2f, Gyro_DPS_Z = %4.2f\n", x, y, z); - } - - /* Read aux data from the bmm150 data registers. */ - rslt = bmi2_read_aux_man_mode(BMM150_REG_DATA_X_LSB, aux_data, 8, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - if (rslt == BMI2_OK) - { - /* Compensating the raw auxiliary data available from the BMM150 API. */ - rslt = bmm150_aux_mag_data(aux_data, &mag_data, &bmm150_dev); - bmi2_error_codes_print_result(rslt); - printf("Mag_x_axis = %d \t Mag_y_axis = %d \t Mag_z_axis = %d \t\n", mag_data.x, mag_data.y, - mag_data.z); - } - } - } - - bmi2_coines_deinit(); - - return rslt; -} - -/*! - * @brief This function reads the data from auxiliary sensor in manual mode. - */ -int8_t user_aux_read(uint8_t reg_addr, uint8_t *aux_data, uint32_t len, void *intf_ptr) -{ - int8_t rslt; - - /* Discarding the parameter id as it is redundant */ - rslt = bmi2_read_aux_man_mode(reg_addr, aux_data, len, &bmi2_dev); - - return rslt; -} - -/*! - * @brief This function writes the data to auxiliary sensor in manual mode. - */ -int8_t user_aux_write(uint8_t reg_addr, const uint8_t *aux_data, uint32_t len, void *intf_ptr) -{ - int8_t rslt; - - /* Discarding the parameter id as it is redundant */ - rslt = bmi2_write_aux_man_mode(reg_addr, aux_data, len, &bmi2_dev); - - return rslt; -} - -/*! - * Delay function map to COINES platform - */ -static void user_delay_us(uint32_t period, void *intf_ptr) -{ - coines_delay_usec(period); -} - -/*! - * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at - * range 2G, 4G, 8G or 16G. - */ -static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width) -{ - float half_scale = ((float)(1 << bit_width) / 2.0f); - - return (GRAVITY_EARTH * val * g_range) / half_scale; -} - -/*! - * @brief This function converts lsb to degree per second for 16 bit gyro at - * range 125, 250, 500, 1000 or 2000dps. - */ -static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width) -{ - float half_scale = ((float)(1 << bit_width) / 2.0f); - - return (dps / ((half_scale) + BMI2_GYR_RANGE_2000)) * (val); -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/sig_motion/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/sig_motion/Makefile deleted file mode 100644 index 97edcc2d16..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/sig_motion/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= sig_motion.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/sig_motion/sig_motion.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/sig_motion/sig_motion.c deleted file mode 100644 index 47e1c2864d..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/sig_motion/sig_motion.c +++ /dev/null @@ -1,93 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270.h" -#include "common.h" - -/******************************************************************************/ -/*! Functions */ - -/* This function starts the execution of program. */ -int main(void) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Structure to define the type of sensor and its configurations. */ - struct bmi2_sens_config config; - - /* Accel sensor and significant-motion feature are listed in array. */ - uint8_t sens_list[2] = { BMI2_ACCEL, BMI2_SIG_MOTION }; - - /* Variable to get significant-motion interrupt status. */ - uint16_t int_status = 0; - - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Select features and their pins to be mapped to. */ - struct bmi2_sens_int_config sens_int = { .type = BMI2_SIG_MOTION, .hw_int_pin = BMI2_INT1 }; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270. */ - rslt = bmi270_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Enable the selected sensors. */ - rslt = bmi270_sensor_enable(sens_list, 2, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Configure the type of feature. */ - config.type = BMI2_SIG_MOTION; - - if (rslt == BMI2_OK) - { - /* Get default configurations for the type of feature selected. */ - rslt = bmi270_get_sensor_config(&config, 1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Map the feature interrupt for significant-motion. */ - rslt = bmi270_map_feat_int(&sens_int, 1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* By default the significant-motion interrupt occurs when the sensor is in motion for about 5 seconds. - * */ - printf("Move the board for 5 secs in any direction\n"); - - do - { - /* To get the interrupt status of significant-motion. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* To check the interrupt status of significant-motion. */ - if (int_status & BMI270_SIG_MOT_STATUS_MASK) - { - printf("Significant motion interrupt is generated\n"); - break; - } - } while (rslt == BMI2_OK); - } - } - } - - bmi2_coines_deinit(); - - return rslt; -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/step_activity/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/step_activity/Makefile deleted file mode 100644 index 91962d5026..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/step_activity/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= step_activity.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/step_activity/step_activity.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/step_activity/step_activity.c deleted file mode 100644 index 87775b706b..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/step_activity/step_activity.c +++ /dev/null @@ -1,110 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270.h" -#include "common.h" - -/******************************************************************************/ -/*! Functions */ -/* This function starts the execution of program. */ -int main(void) -{ - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Structure to define the type of sensor and their respective data. */ - struct bmi2_sensor_data sensor_data; - - /* Structure to define the type of sensor and its configurations. */ - struct bmi2_sens_config config; - - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Accel sensor and step activity feature are listed in array. */ - uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_STEP_ACTIVITY }; - - /* Type of sensor to get step activity data. */ - sensor_data.type = BMI2_STEP_ACTIVITY; - - /* Variable to get step activity interrupt status. */ - uint16_t int_status = 0; - - uint16_t loop = 10; - - /* Select features and their pins to be mapped to. */ - struct bmi2_sens_int_config sens_int = { .type = BMI2_STEP_ACTIVITY, .hw_int_pin = BMI2_INT2 }; - - /* The step activities are listed in array. */ - const char *activity_output[4] = { "BMI2_STILL", "BMI2_WALK", "BMI2_RUN", "BMI2_UNKNOWN" }; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270. */ - rslt = bmi270_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Enable the selected sensor. */ - rslt = bmi270_sensor_enable(sensor_sel, 2, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Configure the type of sensor. */ - config.type = BMI2_STEP_ACTIVITY; - - if (rslt == BMI2_OK) - { - /* Get default configurations for the type of feature selected. */ - rslt = bmi270_get_sensor_config(&config, 1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Map the feature interrupt for step activity. */ - rslt = bmi270_map_feat_int(&sens_int, 1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - printf("\nMove the board in steps to perform step activity:\n"); - - /* Loop to get step activity. */ - while (loop) - { - /* To get the interrupt status of step detector. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* To check the interrupt status of step detector. */ - if (int_status & BMI270_STEP_ACT_STATUS_MASK) - { - printf("Step detected\n"); - - /* Get step activity output. */ - rslt = bmi270_get_sensor_data(&sensor_data, 1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Print the step activity output. */ - printf("Step activity = %s\n", activity_output[sensor_data.sens_data.activity_output]); - - loop--; - } - } - } - } - } - - bmi2_coines_deinit(); - - return rslt; -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/step_counter/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/step_counter/Makefile deleted file mode 100644 index b47fb0e9d1..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/step_counter/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= step_counter.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/step_counter/step_counter.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/step_counter/step_counter.c deleted file mode 100644 index b08f958a22..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/step_counter/step_counter.c +++ /dev/null @@ -1,146 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270.h" -#include "common.h" - -/******************************************************************************/ -/*! Static function declaration */ - -/*! - * @brief This internal API is used to set configurations for step counter. - * - * @param[in] bmi2_dev : Structure instance of bmi2_dev. - * - * @return Status of execution. - */ -static int8_t set_feature_config(struct bmi2_dev *bmi2_dev); - -/******************************************************************************/ -/*! Functions */ - -/* This function starts the execution of program. */ -int main(void) -{ - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Structure to define type of sensor and their respective data. */ - struct bmi2_sensor_data sensor_data; - - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Accel sensor and step counter feature are listed in array. */ - uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_STEP_COUNTER }; - - /* Variable to get step counter interrupt status. */ - uint16_t int_status = 0; - - /* Select features and their pins to be mapped to. */ - struct bmi2_sens_int_config sens_int = { .type = BMI2_STEP_COUNTER, .hw_int_pin = BMI2_INT2 }; - - /* Type of sensor to get step counter data. */ - sensor_data.type = BMI2_STEP_COUNTER; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270. */ - rslt = bmi270_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Enable the selected sensor. */ - rslt = bmi270_sensor_enable(sensor_sel, 2, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Set the feature configuration for step counter. */ - rslt = set_feature_config(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - printf("Step counter watermark level set to 1 (20 steps)\n"); - - if (rslt == BMI2_OK) - { - /* Map the step counter feature interrupt. */ - rslt = bmi270_map_feat_int(&sens_int, 1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Move the board in steps for 20 times to get step counter interrupt. */ - printf("Move the board in steps\n"); - - /* Loop to get number of steps counted. */ - do - { - /* To get the interrupt status of the step counter. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* To check the interrupt status of the step counter. */ - if (int_status & BMI270_STEP_CNT_STATUS_MASK) - { - printf("Step counter interrupt occurred when watermark level (20 steps) is reached\n"); - - /* Get step counter output. */ - rslt = bmi270_get_sensor_data(&sensor_data, 1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Print the step counter output. */ - printf("No of steps counted = %ld", sensor_data.sens_data.step_counter_output); - break; - } - } while (rslt == BMI2_OK); - } - } - } - - bmi2_coines_deinit(); - - return rslt; -} - -/*! - * @brief This internal API is used to set configurations for step counter. - */ -static int8_t set_feature_config(struct bmi2_dev *bmi2_dev) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Structure to define the type of sensor and its configurations. */ - struct bmi2_sens_config config; - - /* Configure the type of sensor. */ - config.type = BMI2_STEP_COUNTER; - - /* Get default configurations for the type of feature selected. */ - rslt = bmi270_get_sensor_config(&config, 1, bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Setting water-mark level to 1 for step counter to get interrupt after 20 step counts. Every 20 steps once - * output triggers. */ - config.cfg.step_counter.watermark_level = 1; - - /* Set new configuration. */ - rslt = bmi270_set_sensor_config(&config, 1, bmi2_dev); - bmi2_error_codes_print_result(rslt); - } - - return rslt; -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/wrist_gesture/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/wrist_gesture/Makefile deleted file mode 100644 index 0f959907ba..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/wrist_gesture/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= wrist_gesture.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/wrist_gesture/wrist_gesture.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/wrist_gesture/wrist_gesture.c deleted file mode 100644 index be494b8be9..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/wrist_gesture/wrist_gesture.c +++ /dev/null @@ -1,147 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270.h" -#include "common.h" - -/******************************************************************************/ -/*! Static function declaration */ - -/*! - * @brief This internal API is used to set the sensor configuration. - * - * @param[in] bmi2_dev : Structure instance of bmi2_dev. - * - * @return Status of execution. - */ -static int8_t bmi2_set_config(struct bmi2_dev *bmi2_dev); - -/******************************************************************************/ -/*! Functions */ - -/* This function starts the execution of program */ -int main(void) -{ - /* Variable to define result */ - int8_t rslt; - - /* Initialize status of wrist gesture interrupt */ - uint16_t int_status = 0; - - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Select features and their pins to be mapped to */ - struct bmi2_sens_int_config sens_int = { .type = BMI2_WRIST_GESTURE, .hw_int_pin = BMI2_INT1 }; - - /* Sensor data structure */ - struct bmi2_sensor_data sens_data = { 0 }; - - /* The gesture movements are listed in array */ - const char *gesture_output[6] = - { "unknown_gesture", "push_arm_down", "pivot_up", "wrist_shake_jiggle", "flick_in", "flick_out" }; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270. */ - rslt = bmi270_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - sens_data.type = BMI2_WRIST_GESTURE; - - /* Set the sensor configuration */ - rslt = bmi2_set_config(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Map the feature interrupt */ - rslt = bmi270_map_feat_int(&sens_int, 1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - printf("Flip the board in portrait/landscape mode:\n"); - - /* Loop to print the wrist gesture data when interrupt occurs */ - while (1) - { - /* Get the interrupt status of the wrist gesture */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if ((rslt == BMI2_OK) && (int_status & BMI270_WRIST_GEST_STATUS_MASK)) - { - printf("Wrist gesture detected\n"); - - /* Get wrist gesture output */ - rslt = bmi270_get_sensor_data(&sens_data, 1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - printf("Wrist gesture = %d\r\n", sens_data.sens_data.wrist_gesture_output); - - printf("Gesture output = %s\n", gesture_output[sens_data.sens_data.wrist_gesture_output]); - break; - } - } - } - } - } - - bmi2_coines_deinit(); - - return rslt; -} - -/*! - * @brief This internal API sets the sensor configuration - */ -static int8_t bmi2_set_config(struct bmi2_dev *bmi2_dev) -{ - /* Variable to define result */ - int8_t rslt; - - /* List the sensors which are required to enable */ - uint8_t sens_list[2] = { BMI2_ACCEL, BMI2_WRIST_GESTURE }; - - /* Structure to define the type of the sensor and its configurations */ - struct bmi2_sens_config config; - - /* Configure type of feature */ - config.type = BMI2_WRIST_GESTURE; - - /* Enable the selected sensors */ - rslt = bmi270_sensor_enable(sens_list, 2, bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Get default configurations for the type of feature selected */ - rslt = bmi270_get_sensor_config(&config, 1, bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - config.cfg.wrist_gest.wearable_arm = BMI2_ARM_LEFT; - - /* Set the new configuration along with interrupt mapping */ - rslt = bmi270_set_sensor_config(&config, 1, bmi2_dev); - bmi2_error_codes_print_result(rslt); - } - } - - return rslt; -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/wrist_wear_wakeup/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/wrist_wear_wakeup/Makefile deleted file mode 100644 index 1e20da2ea8..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/wrist_wear_wakeup/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= wrist_wear_wakeup.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/wrist_wear_wakeup/wrist_wear_wakeup.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/wrist_wear_wakeup/wrist_wear_wakeup.c deleted file mode 100644 index 3547b2d309..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270/wrist_wear_wakeup/wrist_wear_wakeup.c +++ /dev/null @@ -1,131 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270.h" -#include "common.h" - -/******************************************************************************/ -/*! Static function declaration */ - -/*! - * @brief This internal API is used to set the sensor configuration. - * - * @param[in] bmi2_dev : Structure instance of bmi2_dev. - * - * @return Status of execution. - */ -static int8_t bmi2_set_config(struct bmi2_dev *bmi2_dev); - -/******************************************************************************/ -/*! Functions */ - -/* This function starts the execution of program */ -int main(void) -{ - /* Variable to define result */ - int8_t rslt; - - /* Initialize status of wrist wear wakeup interrupt */ - uint16_t int_status = 0; - - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Select features and their pins to be mapped to */ - struct bmi2_sens_int_config sens_int = { .type = BMI2_WRIST_WEAR_WAKE_UP, .hw_int_pin = BMI2_INT1 }; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270. */ - rslt = bmi270_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Set the sensor configuration */ - rslt = bmi2_set_config(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Map the feature interrupt */ - rslt = bmi270_map_feat_int(&sens_int, 1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - printf("Tilt the board in landscape position to trigger wrist wear wakeup\n"); - - /* Loop to print the wrist wear wakeup data when interrupt occurs */ - while (1) - { - int_status = 0; - - /* To get the interrupt status of the wrist wear wakeup */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* To check the interrupt status of the wrist gesture */ - if ((rslt == BMI2_OK) && (int_status & BMI270_WRIST_WAKE_UP_STATUS_MASK)) - { - printf("Wrist wear wakeup detected\n"); - break; - } - } - } - } - } - - bmi2_coines_deinit(); - - return rslt; -} - -/*! - * @brief This internal API sets the sensor configuration - */ -static int8_t bmi2_set_config(struct bmi2_dev *bmi2_dev) -{ - /* Variable to define result */ - int8_t rslt; - - /* List the sensors which are required to enable */ - uint8_t sens_list[2] = { BMI2_ACCEL, BMI2_WRIST_WEAR_WAKE_UP }; - - /* Structure to define the type of the sensor and its configurations */ - struct bmi2_sens_config config; - - /* Configure type of feature */ - config.type = BMI2_WRIST_WEAR_WAKE_UP; - - /* Enable the selected sensors */ - rslt = bmi270_sensor_enable(sens_list, 2, bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Get default configurations for the type of feature selected */ - rslt = bmi270_get_sensor_config(&config, 1, bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Set the new configuration */ - rslt = bmi270_set_sensor_config(&config, 1, bmi2_dev); - bmi2_error_codes_print_result(rslt); - } - } - - return rslt; -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/accel/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/accel/Makefile deleted file mode 100644 index d215172455..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/accel/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= accel.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270_context.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/accel/accel.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/accel/accel.c deleted file mode 100644 index 5b36fffe42..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/accel/accel.c +++ /dev/null @@ -1,200 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270_context.h" -#include "common.h" - -/******************************************************************************/ -/*! Macro definition */ - -/*! Earth's gravity in m/s^2 */ -#define GRAVITY_EARTH (9.80665f) - -/******************************************************************************/ -/*! Static Function Declaration */ - -/*! - * @brief This internal API is used to set configurations for accel. - * - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Status of execution. - */ -static int8_t set_accel_config(struct bmi2_dev *bmi2_dev); - -/*! - * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at - * range 2G, 4G, 8G or 16G. - * - * @param[in] val : LSB from each axis. - * @param[in] g_range : Gravity range. - * @param[in] bit_width : Resolution for accel. - * - * @return Gravity. - */ -static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width); - -/******************************************************************************/ -/*! Functions */ - -/* This function starts the execution of program. */ -int main(void) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Variable to define limit to print accel data. */ - uint8_t limit = 20; - - /* Assign accel sensor to variable. */ - uint8_t sensor_list = BMI2_ACCEL; - - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Create an instance of sensor data structure. */ - struct bmi2_sensor_data sensor_data = { 0 }; - - /* Initialize the interrupt status of accel. */ - uint16_t int_status = 0; - - uint8_t indx = 0; - float x = 0, y = 0, z = 0; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270_context. */ - rslt = bmi270_context_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Enable the accel sensor. */ - rslt = bmi270_context_sensor_enable(&sensor_list, 1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Accel configuration settings. */ - rslt = set_accel_config(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Assign accel sensor. */ - sensor_data.type = BMI2_ACCEL; - printf("Accel and m/s2 data \n"); - printf("Accel data collected at 2G Range with 16-bit resolution\n"); - - /* Loop to print the accel data when interrupt occurs. */ - while (indx <= limit) - { - /* To get the status of accel data ready interrupt. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* To check the accel data ready interrupt status and print the status for 10 samples. */ - if (int_status & BMI2_ACC_DRDY_INT_MASK) - { - /* Get accelerometer data for x, y and z axis. */ - rslt = bmi270_context_get_sensor_data(&sensor_data, 1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - printf("\nAcc_X = %d\t", sensor_data.sens_data.acc.x); - printf("Acc_Y = %d\t", sensor_data.sens_data.acc.y); - printf("Acc_Z = %d\r\n", sensor_data.sens_data.acc.z); - - /* Converting lsb to meter per second squared for 16 bit accelerometer at 2G range. */ - x = lsb_to_mps2(sensor_data.sens_data.acc.x, 2, bmi2_dev.resolution); - y = lsb_to_mps2(sensor_data.sens_data.acc.y, 2, bmi2_dev.resolution); - z = lsb_to_mps2(sensor_data.sens_data.acc.z, 2, bmi2_dev.resolution); - - /* Print the data in m/s2. */ - printf("\nAcc_ms2_X = %4.2f, Acc_ms2_Y = %4.2f, Acc_ms2_Z = %4.2f\n", x, y, z); - - indx++; - } - } - } - } - - bmi2_coines_deinit(); - - return rslt; -} - -/*! - * @brief This internal API is used to set configurations for accel. - */ -static int8_t set_accel_config(struct bmi2_dev *bmi2_dev) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Structure to define accelerometer configuration. */ - struct bmi2_sens_config config; - - /* Configure the type of feature. */ - config.type = BMI2_ACCEL; - - /* Get default configurations for the type of feature selected. */ - rslt = bmi270_context_get_sensor_config(&config, 1, bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* NOTE: The user can change the following configuration parameters according to their requirement. */ - /* Set Output Data Rate */ - config.cfg.acc.odr = BMI2_ACC_ODR_200HZ; - - /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ - config.cfg.acc.range = BMI2_ACC_RANGE_2G; - - /* The bandwidth parameter is used to configure the number of sensor samples that are averaged - * if it is set to 2, then 2^(bandwidth parameter) samples - * are averaged, resulting in 4 averaged samples. - * Note1 : For more information, refer the datasheet. - * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but - * this has an adverse effect on the power consumed. - */ - config.cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; - - /* Enable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - * For more info refer datasheet. - */ - config.cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; - - /* Set the accel configurations. */ - rslt = bmi270_context_set_sensor_config(&config, 1, bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Map data ready interrupt to interrupt pin. */ - rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT1, bmi2_dev); - bmi2_error_codes_print_result(rslt); - } - - return rslt; -} - -/*! - * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at - * range 2G, 4G, 8G or 16G. - */ -static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width) -{ - float half_scale = ((float)(1 << bit_width) / 2.0f); - - return (GRAVITY_EARTH * val * g_range) / half_scale; -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/accel_gyro/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/accel_gyro/Makefile deleted file mode 100644 index 2eed195611..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/accel_gyro/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= accel_gyro.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270_context.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/accel_gyro/accel_gyro.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/accel_gyro/accel_gyro.c deleted file mode 100644 index 7458969efb..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/accel_gyro/accel_gyro.c +++ /dev/null @@ -1,268 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270_context.h" -#include "common.h" - -/******************************************************************************/ -/*! Macro definition */ - -/*! Earth's gravity in m/s^2 */ -#define GRAVITY_EARTH (9.80665f) - -/*! Macros to select the sensors */ -#define ACCEL UINT8_C(0x00) -#define GYRO UINT8_C(0x01) - -/******************************************************************************/ -/*! Static Function Declaration */ - -/*! - * @brief This internal API is used to set configurations for accel. - * - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Status of execution. - */ -static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev); - -/*! - * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at - * range 2G, 4G, 8G or 16G. - * - * @param[in] val : LSB from each axis. - * @param[in] g_range : Gravity range. - * @param[in] bit_width : Resolution for accel. - * - * @return Gravity. - */ -static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width); - -/*! - * @brief This function converts lsb to degree per second for 16 bit gyro at - * range 125, 250, 500, 1000 or 2000dps. - * - * @param[in] val : LSB from each axis. - * @param[in] dps : Degree per second. - * @param[in] bit_width : Resolution for gyro. - * - * @return Degree per second. - */ -static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width); - -/******************************************************************************/ -/*! Functions */ - -/* This function starts the execution of program. */ -int main(void) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Variable to define limit to print accel data. */ - uint8_t limit = 10; - - /* Assign accel and gyro sensor to variable. */ - uint8_t sensor_list[2] = { BMI2_ACCEL, BMI2_GYRO }; - - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Create an instance of sensor data structure. */ - struct bmi2_sensor_data sensor_data[2] = { { 0 } }; - - /* Initialize the interrupt status of accel and gyro. */ - uint16_t int_status = 0; - - uint8_t indx = 1; - - float x = 0, y = 0, z = 0; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270_context. */ - rslt = bmi270_context_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Enable the accel and gyro sensor. */ - rslt = bmi270_context_sensor_enable(sensor_list, 2, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Accel and gyro configuration settings. */ - rslt = set_accel_gyro_config(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Assign accel and gyro sensor. */ - sensor_data[ACCEL].type = BMI2_ACCEL; - sensor_data[GYRO].type = BMI2_GYRO; - - /* Loop to print accel and gyro data when interrupt occurs. */ - while (indx <= limit) - { - /* To get the data ready interrupt status of accel and gyro. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* To check the data ready interrupt status and print the status for 10 samples. */ - if ((int_status & BMI2_ACC_DRDY_INT_MASK) && (int_status & BMI2_GYR_DRDY_INT_MASK)) - { - /* Get accel and gyro data for x, y and z axis. */ - rslt = bmi270_context_get_sensor_data(sensor_data, 2, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - printf("\n******* Accel(Raw and m/s2) Gyro(Raw and dps) data : %d *******\n", indx); - - printf("\nAcc_x = %d\t", sensor_data[ACCEL].sens_data.acc.x); - printf("Acc_y = %d\t", sensor_data[ACCEL].sens_data.acc.y); - printf("Acc_z = %d", sensor_data[ACCEL].sens_data.acc.z); - - /* Converting lsb to meter per second squared for 16 bit accelerometer at 2G range. */ - x = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.x, 2, bmi2_dev.resolution); - y = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.y, 2, bmi2_dev.resolution); - z = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.z, 2, bmi2_dev.resolution); - - /* Print the data in m/s2. */ - printf("\nAcc_ms2_X = %4.2f, Acc_ms2_Y = %4.2f, Acc_ms2_Z = %4.2f\n", x, y, z); - - printf("\nGyr_X = %d\t", sensor_data[GYRO].sens_data.gyr.x); - printf("Gyr_Y = %d\t", sensor_data[GYRO].sens_data.gyr.y); - printf("Gyr_Z = %d\n", sensor_data[GYRO].sens_data.gyr.z); - - /* Converting lsb to degree per second for 16 bit gyro at 2000dps range. */ - x = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.x, 2000, bmi2_dev.resolution); - y = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.y, 2000, bmi2_dev.resolution); - z = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.z, 2000, bmi2_dev.resolution); - - /* Print the data in dps. */ - printf("Gyro_DPS_X = %4.2f, Gyro_DPS_Y = %4.2f, Gyro_DPS_Z = %4.2f\n", x, y, z); - - indx++; - } - } - } - } - - bmi2_coines_deinit(); - - return rslt; -} - -/*! - * @brief This internal API is used to set configurations for accel and gyro. - */ -static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Structure to define accelerometer and gyro configuration. */ - struct bmi2_sens_config config[2]; - - /* Configure the type of feature. */ - config[ACCEL].type = BMI2_ACCEL; - config[GYRO].type = BMI2_GYRO; - - /* Get default configurations for the type of feature selected. */ - rslt = bmi270_context_get_sensor_config(config, 2, bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Map data ready interrupt to interrupt pin. */ - rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT1, bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* NOTE: The user can change the following configuration parameters according to their requirement. */ - /* Set Output Data Rate */ - config[ACCEL].cfg.acc.odr = BMI2_ACC_ODR_200HZ; - - /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ - config[ACCEL].cfg.acc.range = BMI2_ACC_RANGE_2G; - - /* The bandwidth parameter is used to configure the number of sensor samples that are averaged - * if it is set to 2, then 2^(bandwidth parameter) samples - * are averaged, resulting in 4 averaged samples. - * Note1 : For more information, refer the datasheet. - * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but - * this has an adverse effect on the power consumed. - */ - config[ACCEL].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; - - /* Enable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - * For more info refer datasheet. - */ - config[ACCEL].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; - - /* The user can change the following configuration parameters according to their requirement. */ - /* Set Output Data Rate */ - config[GYRO].cfg.gyr.odr = BMI2_GYR_ODR_200HZ; - - /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ - config[GYRO].cfg.gyr.range = BMI2_GYR_RANGE_2000; - - /* Gyroscope bandwidth parameters. By default the gyro bandwidth is in normal mode. */ - config[GYRO].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; - - /* Enable/Disable the noise performance mode for precision yaw rate sensing - * There are two modes - * 0 -> Ultra low power mode(Default) - * 1 -> High performance mode - */ - config[GYRO].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; - - /* Enable/Disable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - */ - config[GYRO].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; - - /* Set the accel and gyro configurations. */ - rslt = bmi270_context_set_sensor_config(config, 2, bmi2_dev); - bmi2_error_codes_print_result(rslt); - } - - return rslt; -} - -/*! - * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at - * range 2G, 4G, 8G or 16G. - */ -static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width) -{ - float half_scale = ((float)(1 << bit_width) / 2.0f); - - return (GRAVITY_EARTH * val * g_range) / half_scale; -} - -/*! - * @brief This function converts lsb to degree per second for 16 bit gyro at - * range 125, 250, 500, 1000 or 2000dps. - */ -static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width) -{ - float half_scale = ((float)(1 << bit_width) / 2.0f); - - return (dps / ((half_scale) + BMI2_GYR_RANGE_2000)) * (val); -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/activity_recognition/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/activity_recognition/Makefile deleted file mode 100644 index 28ed939cd6..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/activity_recognition/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= activity_recognition.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270_context.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/activity_recognition/activity_recognition.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/activity_recognition/activity_recognition.c deleted file mode 100644 index 9f0714d384..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/activity_recognition/activity_recognition.c +++ /dev/null @@ -1,154 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270_context.h" -#include "common.h" - -/******************************************************************************/ -/*! Static function declaration */ - -/*! - * @brief This internal API is used to set configurations for step counter. - * - * @param[in] bmi2_dev : Structure instance of bmi2_dev. - * - * @return Status of execution. - */ -static int8_t set_config(struct bmi2_dev *bmi2_dev); - -/******************************************************************************/ -/*! Functions */ -/* This function starts the execution of program. */ -int main(void) -{ - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Variable to define index for FIFO data */ - uint8_t count; - - /* Various Activity recognitions are listed in array. */ - const char *activity_reg_output[6] = { "OTHERS", "STILL", "WALKING", "RUNNING", "ON_BICYCLE", "IN_VEHICLE" }; - - /* Accel sensor and step activity feature are listed in array. */ - uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_ACTIVITY_RECOGNITION }; - - /* Array to define FIFO data */ - uint8_t fifo_data[516] = { 0 }; - - /* Variable to store number of activity frames */ - uint16_t act_frames; - - /* Structure to define a FIFO */ - struct bmi2_fifo_frame fifoframe = { 0 }; - - /* Structure to define output for activity recognition */ - struct bmi2_act_recog_output act_recog_data[5] = { { 0 } }; - - uint16_t fifo_length; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270_context. */ - rslt = bmi270_context_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Enable the selected sensor. */ - rslt = bmi270_context_sensor_enable(sensor_sel, 2, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - rslt = set_config(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Update FIFO structure */ - fifoframe.data = fifo_data; - fifoframe.length = 516; - - rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); - printf("Fifo length = %d \n", fifo_length); - - /* Read FIFO data */ - rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - while (rslt != BMI2_W_FIFO_EMPTY) - { - /* Provide the number of frames to be read */ - act_frames = 5; - - printf("Requested FIFO data frames : %d\n", act_frames); - - /* Get the activity output */ - rslt = bmi270_context_get_act_recog_output(act_recog_data, &act_frames, &fifoframe, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - printf("Parsed FIFO data frames : %d\r\n", act_frames); - - for (count = 0; count < act_frames; count++) - { - printf( - "Activity Recognition output[%d]:Sensor time: %lu\t Previous activity: %s\t current: %s\n", - count, - act_recog_data[count].time_stamp, - activity_reg_output[act_recog_data[count].prev_act], - activity_reg_output[act_recog_data[count].curr_act]); - } - } - } - } - } - - bmi2_coines_deinit(); - - return rslt; -} - -static int8_t set_config(struct bmi2_dev *bmi2_dev) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - uint8_t temp_data[2]; - - /* Disable advanced power save */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Reset FIFO */ - rslt = bmi2_set_command_register(BMI2_FIFO_FLUSH_CMD, bmi2_dev); - bmi2_error_codes_print_result(rslt); - - printf("Move the board for activity recognition for 30 sec :\n"); - bmi2_dev->delay_us(30000000, bmi2_dev->intf_ptr); - - if (rslt == BMI2_OK) - { - rslt = bmi2_get_regs(BMI2_FIFO_CONFIG_0_ADDR, temp_data, 2, bmi2_dev); - bmi2_error_codes_print_result(rslt); - } - } - - return rslt; -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/common/common.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/common/common.c deleted file mode 100644 index afff37bc84..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/common/common.c +++ /dev/null @@ -1,372 +0,0 @@ -/** - * Copyright (C) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - */ - -#include -#include -#include - -#include "coines.h" -#include "bmi2_defs.h" - -/******************************************************************************/ -/*! Macro definitions */ -#define BMI2XY_SHUTTLE_ID UINT16_C(0x1B8) - -/*! Macro that defines read write length */ -#define READ_WRITE_LEN UINT8_C(46) - -/******************************************************************************/ -/*! Static variable definition */ - -/*! Variable that holds the I2C device address or SPI chip selection */ -static uint8_t dev_addr; - -/******************************************************************************/ -/*! User interface functions */ - -/*! - * I2C read function map to COINES platform - */ -BMI2_INTF_RETURN_TYPE bmi2_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr) -{ - uint8_t dev_addr = *(uint8_t*)intf_ptr; - - return coines_read_i2c(dev_addr, reg_addr, reg_data, (uint16_t)len); -} - -/*! - * I2C write function map to COINES platform - */ -BMI2_INTF_RETURN_TYPE bmi2_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr) -{ - uint8_t dev_addr = *(uint8_t*)intf_ptr; - - return coines_write_i2c(dev_addr, reg_addr, (uint8_t *)reg_data, (uint16_t)len); -} - -/*! - * SPI read function map to COINES platform - */ -BMI2_INTF_RETURN_TYPE bmi2_spi_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr) -{ - uint8_t dev_addr = *(uint8_t*)intf_ptr; - - return coines_read_spi(dev_addr, reg_addr, reg_data, (uint16_t)len); -} - -/*! - * SPI write function map to COINES platform - */ -BMI2_INTF_RETURN_TYPE bmi2_spi_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr) -{ - uint8_t dev_addr = *(uint8_t*)intf_ptr; - - return coines_write_spi(dev_addr, reg_addr, (uint8_t *)reg_data, (uint16_t)len); -} - -/*! - * Delay function map to COINES platform - */ -void bmi2_delay_us(uint32_t period, void *intf_ptr) -{ - coines_delay_usec(period); -} - -/*! - * @brief Function to select the interface between SPI and I2C. - * Also to initialize coines platform - */ -int8_t bmi2_interface_init(struct bmi2_dev *bmi, uint8_t intf) -{ - int8_t rslt = BMI2_OK; - struct coines_board_info board_info; - - if (bmi != NULL) - { - int16_t result = coines_open_comm_intf(COINES_COMM_INTF_USB); - if (result < COINES_SUCCESS) - { - printf( - "\n Unable to connect with Application Board ! \n" " 1. Check if the board is connected and powered on. \n" " 2. Check if Application Board USB driver is installed. \n" - " 3. Check if board is in use by another application. (Insufficient permissions to access USB) \n"); - exit(result); - } - - result = coines_get_board_info(&board_info); - -#if defined(PC) - setbuf(stdout, NULL); -#endif - - if (result == COINES_SUCCESS) - { - if ((board_info.shuttle_id != BMI2XY_SHUTTLE_ID)) - { - printf("! Warning invalid sensor shuttle \n ," "This application will not support this sensor \n"); - exit(COINES_E_FAILURE); - } - } - - coines_set_shuttleboard_vdd_vddio_config(0, 0); - coines_delay_msec(100); - - /* Bus configuration : I2C */ - if (intf == BMI2_I2C_INTF) - { - printf("I2C Interface \n"); - - /* To initialize the user I2C function */ - dev_addr = BMI2_I2C_PRIM_ADDR; - bmi->intf = BMI2_I2C_INTF; - bmi->read = bmi2_i2c_read; - bmi->write = bmi2_i2c_write; - coines_config_i2c_bus(COINES_I2C_BUS_0, COINES_I2C_FAST_MODE); - } - /* Bus configuration : SPI */ - else if (intf == BMI2_SPI_INTF) - { - printf("SPI Interface \n"); - - /* To initialize the user SPI function */ - dev_addr = COINES_SHUTTLE_PIN_7; - bmi->intf = BMI2_SPI_INTF; - bmi->read = bmi2_spi_read; - bmi->write = bmi2_spi_write; - coines_config_spi_bus(COINES_SPI_BUS_0, COINES_SPI_SPEED_5_MHZ, COINES_SPI_MODE3); - coines_set_pin_config(COINES_SHUTTLE_PIN_7, COINES_PIN_DIRECTION_OUT, COINES_PIN_VALUE_HIGH); - } - - /* Assign device address to interface pointer */ - bmi->intf_ptr = &dev_addr; - - /* Configure delay in microseconds */ - bmi->delay_us = bmi2_delay_us; - - /* Configure max read/write length (in bytes) ( Supported length depends on target machine) */ - bmi->read_write_len = READ_WRITE_LEN; - - /* Assign to NULL to load the default config file. */ - bmi->config_file_ptr = NULL; - - coines_delay_usec(10000); - - coines_set_shuttleboard_vdd_vddio_config(3300, 3300); - - coines_delay_usec(10000); - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; - -} - -/*! - * @brief Prints the execution status of the APIs. - */ -void bmi2_error_codes_print_result(int8_t rslt) -{ - switch (rslt) - { - case BMI2_OK: - - /* Do nothing */ - break; - - case BMI2_W_FIFO_EMPTY: - printf("Warning [%d] : FIFO empty\r\n", rslt); - break; - case BMI2_W_PARTIAL_READ: - printf("Warning [%d] : FIFO partial read\r\n", rslt); - break; - case BMI2_E_NULL_PTR: - printf( - "Error [%d] : Null pointer error. It occurs when the user tries to assign value (not address) to a pointer," " which has been initialized to NULL.\r\n", - rslt); - break; - - case BMI2_E_COM_FAIL: - printf( - "Error [%d] : Communication failure error. It occurs due to read/write operation failure and also due " "to power failure during communication\r\n", - rslt); - break; - - case BMI2_E_DEV_NOT_FOUND: - printf("Error [%d] : Device not found error. It occurs when the device chip id is incorrectly read\r\n", - rslt); - break; - - case BMI2_E_INVALID_SENSOR: - printf( - "Error [%d] : Invalid sensor error. It occurs when there is a mismatch in the requested feature with the " "available one\r\n", - rslt); - break; - - case BMI2_E_SELF_TEST_FAIL: - printf( - "Error [%d] : Self-test failed error. It occurs when the validation of accel self-test data is " "not satisfied\r\n", - rslt); - break; - - case BMI2_E_INVALID_INT_PIN: - printf( - "Error [%d] : Invalid interrupt pin error. It occurs when the user tries to configure interrupt pins " "apart from INT1 and INT2\r\n", - rslt); - break; - - case BMI2_E_OUT_OF_RANGE: - printf( - "Error [%d] : Out of range error. It occurs when the data exceeds from filtered or unfiltered data from " "fifo and also when the range exceeds the maximum range for accel and gyro while performing FOC\r\n", - rslt); - break; - - case BMI2_E_ACC_INVALID_CFG: - printf( - "Error [%d] : Invalid Accel configuration error. It occurs when there is an error in accel configuration" " register which could be one among range, BW or filter performance in reg address 0x40\r\n", - rslt); - break; - - case BMI2_E_GYRO_INVALID_CFG: - printf( - "Error [%d] : Invalid Gyro configuration error. It occurs when there is a error in gyro configuration" "register which could be one among range, BW or filter performance in reg address 0x42\r\n", - rslt); - break; - - case BMI2_E_ACC_GYR_INVALID_CFG: - printf( - "Error [%d] : Invalid Accel-Gyro configuration error. It occurs when there is a error in accel and gyro" " configuration registers which could be one among range, BW or filter performance in reg address 0x40 " "and 0x42\r\n", - rslt); - break; - - case BMI2_E_CONFIG_LOAD: - printf( - "Error [%d] : Configuration load error. It occurs when failure observed while loading the configuration " "into the sensor\r\n", - rslt); - break; - - case BMI2_E_INVALID_PAGE: - printf( - "Error [%d] : Invalid page error. It occurs due to failure in writing the correct feature configuration " "from selected page\r\n", - rslt); - break; - - case BMI2_E_SET_APS_FAIL: - printf( - "Error [%d] : APS failure error. It occurs due to failure in write of advance power mode configuration " "register\r\n", - rslt); - break; - - case BMI2_E_AUX_INVALID_CFG: - printf( - "Error [%d] : Invalid AUX configuration error. It occurs when the auxiliary interface settings are not " "enabled properly\r\n", - rslt); - break; - - case BMI2_E_AUX_BUSY: - printf( - "Error [%d] : AUX busy error. It occurs when the auxiliary interface buses are engaged while configuring" " the AUX\r\n", - rslt); - break; - - case BMI2_E_REMAP_ERROR: - printf( - "Error [%d] : Remap error. It occurs due to failure in assigning the remap axes data for all the axes " "after change in axis position\r\n", - rslt); - break; - - case BMI2_E_GYR_USER_GAIN_UPD_FAIL: - printf( - "Error [%d] : Gyro user gain update fail error. It occurs when the reading of user gain update status " "fails\r\n", - rslt); - break; - - case BMI2_E_SELF_TEST_NOT_DONE: - printf( - "Error [%d] : Self-test not done error. It occurs when the self-test process is ongoing or not " "completed\r\n", - rslt); - break; - - case BMI2_E_INVALID_INPUT: - printf("Error [%d] : Invalid input error. It occurs when the sensor input validity fails\r\n", rslt); - break; - - case BMI2_E_INVALID_STATUS: - printf("Error [%d] : Invalid status error. It occurs when the feature/sensor validity fails\r\n", rslt); - break; - - case BMI2_E_CRT_ERROR: - printf("Error [%d] : CRT error. It occurs when the CRT test has failed\r\n", rslt); - break; - - case BMI2_E_ST_ALREADY_RUNNING: - printf( - "Error [%d] : Self-test already running error. It occurs when the self-test is already running and " "another has been initiated\r\n", - rslt); - break; - - case BMI2_E_CRT_READY_FOR_DL_FAIL_ABORT: - printf( - "Error [%d] : CRT ready for download fail abort error. It occurs when download in CRT fails due to wrong " "address location\r\n", - rslt); - break; - - case BMI2_E_DL_ERROR: - printf( - "Error [%d] : Download error. It occurs when write length exceeds that of the maximum burst length\r\n", - rslt); - break; - - case BMI2_E_PRECON_ERROR: - printf( - "Error [%d] : Pre-conditional error. It occurs when precondition to start the feature was not " "completed\r\n", - rslt); - break; - - case BMI2_E_ABORT_ERROR: - printf("Error [%d] : Abort error. It occurs when the device was shaken during CRT test\r\n", rslt); - break; - - case BMI2_E_WRITE_CYCLE_ONGOING: - printf( - "Error [%d] : Write cycle ongoing error. It occurs when the write cycle is already running and another " "has been initiated\r\n", - rslt); - break; - - case BMI2_E_ST_NOT_RUNING: - printf( - "Error [%d] : Self-test is not running error. It occurs when self-test running is disabled while it's " "running\r\n", - rslt); - break; - - case BMI2_E_DATA_RDY_INT_FAILED: - printf( - "Error [%d] : Data ready interrupt error. It occurs when the sample count exceeds the FOC sample limit " "and data ready status is not updated\r\n", - rslt); - break; - - case BMI2_E_INVALID_FOC_POSITION: - printf( - "Error [%d] : Invalid FOC position error. It occurs when average FOC data is obtained for the wrong" " axes\r\n", - rslt); - break; - - default: - printf("Error [%d] : Unknown error code\r\n", rslt); - break; - } -} - -/*! - * @brief Deinitializes coines platform - * - * @return void. - */ -void bmi2_coines_deinit(void) -{ - coines_close_comm_intf(COINES_COMM_INTF_USB); -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/common/common.h b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/common/common.h deleted file mode 100644 index 763983da4a..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/common/common.h +++ /dev/null @@ -1,122 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -#ifndef _COMMON_H -#define _COMMON_H - -/*! CPP guard */ -#ifdef __cplusplus -extern "C" { -#endif - -#include -#include "bmi2.h" - -/*! - * @brief Function for reading the sensor's registers through I2C bus. - * - * @param[in] reg_addr : Register address. - * @param[out] reg_data : Pointer to the data buffer to store the read data. - * @param[in] length : No of bytes to read. - * @param[in] intf_ptr : Interface pointer - * - * @return Status of execution - * @retval = BMI2_INTF_RET_SUCCESS -> Success - * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info - * - */ -BMI2_INTF_RETURN_TYPE bmi2_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr); - -/*! - * @brief Function for writing the sensor's registers through I2C bus. - * - * @param[in] reg_addr : Register address. - * @param[in] reg_data : Pointer to the data buffer whose value is to be written. - * @param[in] length : No of bytes to write. - * @param[in] intf_ptr : Interface pointer - * - * @return Status of execution - * @retval = BMI2_INTF_RET_SUCCESS -> Success - * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info - * - */ -BMI2_INTF_RETURN_TYPE bmi2_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr); - -/*! - * @brief Function for reading the sensor's registers through SPI bus. - * - * @param[in] reg_addr : Register address. - * @param[out] reg_data : Pointer to the data buffer to store the read data. - * @param[in] length : No of bytes to read. - * @param[in] intf_ptr : Interface pointer - * - * @return Status of execution - * @retval = BMI2_INTF_RET_SUCCESS -> Success - * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info - * - */ -BMI2_INTF_RETURN_TYPE bmi2_spi_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr); - -/*! - * @brief Function for writing the sensor's registers through SPI bus. - * - * @param[in] reg_addr : Register address. - * @param[in] reg_data : Pointer to the data buffer whose data has to be written. - * @param[in] length : No of bytes to write. - * @param[in] intf_ptr : Interface pointer - * - * @return Status of execution - * @retval = BMI2_INTF_RET_SUCCESS -> Success - * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info - * - */ -BMI2_INTF_RETURN_TYPE bmi2_spi_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr); - -/*! - * @brief This function provides the delay for required time (Microsecond) as per the input provided in some of the - * APIs. - * - * @param[in] period_us : The required wait time in microsecond. - * @param[in] intf_ptr : Interface pointer - * - * @return void. - * - */ -void bmi2_delay_us(uint32_t period, void *intf_ptr); - -/*! - * @brief Function to select the interface between SPI and I2C. - * - * @param[in] bma : Structure instance of bmi2_dev - * @param[in] intf : Interface selection parameter - * - * @return Status of execution - * @retval 0 -> Success - * @retval < 0 -> Failure Info - */ -int8_t bmi2_interface_init(struct bmi2_dev *bma, uint8_t intf); - -/*! - * @brief Prints the execution status of the APIs. - * - * @param[in] rslt : Error code returned by the API whose execution status has to be printed. - * - * @return void. - */ -void bmi2_error_codes_print_result(int8_t rslt); - -/*! - * @brief Deinitializes coines platform - * - * @return void. - */ -void bmi2_coines_deinit(void); - -#ifdef __cplusplus -} -#endif /* End of CPP guard */ - -#endif /* _COMMON_H */ diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/component_retrim/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/component_retrim/Makefile deleted file mode 100644 index 61db2e1351..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/component_retrim/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= component_retrim.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270_context.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/component_retrim/component_retrim.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/component_retrim/component_retrim.c deleted file mode 100644 index 94ed043772..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/component_retrim/component_retrim.c +++ /dev/null @@ -1,52 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270_context.h" -#include "common.h" - -/******************************************************************************/ -/*! Functions */ - -/* This function starts the execution of program. */ -int main(void) -{ - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270_context. */ - rslt = bmi270_context_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* This API runs the CRT process. */ - rslt = bmi2_do_crt(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Do not move the board while doing CRT. If so, it will throw an abort error. */ - if (rslt == BMI2_OK) - { - printf("CRT successfully completed."); - } - } - - bmi2_coines_deinit(); - - return rslt; -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_full_header_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_full_header_mode/Makefile deleted file mode 100644 index 2d328d5f8a..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_full_header_mode/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= fifo_full_header_mode.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270_context.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_full_header_mode/fifo_full_header_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_full_header_mode/fifo_full_header_mode.c deleted file mode 100644 index 3a5a810ec7..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_full_header_mode/fifo_full_header_mode.c +++ /dev/null @@ -1,315 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270_context.h" -#include "common.h" - -/******************************************************************************/ -/*! Macros */ - -/*! Buffer size allocated to store raw FIFO data. */ -#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) - -/*! Length of data to be read from FIFO. */ -#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) - -/*! Number of accel frames to be extracted from FIFO. */ - -/*! Calculation for frame count: Total frame count = Fifo buffer size(2048)/ Total frames(6 Accel, 6 Gyro and 1 header, - * totaling to 13) which equals to 157. - */ -#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(157) - -/*! Number of gyro frames to be extracted from FIFO. */ -#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(157) - -/*! Macro to read sensortime byte in FIFO. */ -#define SENSORTIME_OVERHEAD_BYTE UINT8_C(3) - -/******************************************************************************/ -/*! Static Function Declaration */ - -/*! - * @brief This internal API is used to set configurations for accel and gyro. - * @param[in] dev : Structure instance of bmi2_dev. - * @return Status of execution. - */ -static int8_t set_accel_gyro_config(struct bmi2_dev *dev); - -/******************************************************************************/ - -/* This function starts the execution of program. */ -int main(void) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - uint16_t index = 0; - uint16_t fifo_length = 0; - uint16_t config = 0; - - /* To read sensortime, extra 3 bytes are added to fifo buffer. */ - uint16_t fifo_buffer_size = BMI2_FIFO_RAW_DATA_BUFFER_SIZE + SENSORTIME_OVERHEAD_BYTE; - - /* Variable to get fifo full interrupt status. */ - uint16_t int_status = 0; - - uint16_t accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; - - uint16_t gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; - - /* Variable to store the available frame length count in FIFO */ - uint8_t frame_count; - - int8_t try = 1; - - /* Number of bytes of FIFO data. */ - uint8_t fifo_data[fifo_buffer_size]; - - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Array of accelerometer frames -> Total bytes = - * 157 * (6 axes + 1 header bytes) = 1099 bytes */ - struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; - - /* Array of gyro frames -> Total bytes = - * 157 * (6 axes + 1 header bytes) = 1099 bytes */ - struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; - - /* Initialize FIFO frame structure. */ - struct bmi2_fifo_frame fifoframe = { 0 }; - - /* Accel and gyro sensor are listed in array. */ - uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270_context. */ - rslt = bmi270_context_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Enable accel and gyro sensor. */ - rslt = bmi270_context_sensor_enable(sensor_sel, 2, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Configuration settings for accel and gyro. */ - rslt = set_accel_gyro_config(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Before setting FIFO, disable the advance power save mode. */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Initially disable all configurations in fifo. */ - rslt = bmi2_get_fifo_config(&config, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Initially disable all configurations in fifo. */ - rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Update FIFO structure. */ - /* Mapping the buffer to store the fifo data. */ - fifoframe.data = fifo_data; - - /* Length of FIFO frame. */ - /* To read sensortime, extra 3 bytes are added to fifo user length. */ - fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH + SENSORTIME_OVERHEAD_BYTE; - - /* Set FIFO configuration by enabling accel, gyro and timestamp. - * NOTE 1: The header mode is enabled by default. - * NOTE 2: By default the FIFO operating mode is in FIFO mode. - * NOTE 3: Sensortime is enabled by default */ - printf("FIFO is configured in header mode\n"); - rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Map FIFO full interrupt. */ - fifoframe.data_int_map = BMI2_FFULL_INT; - rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - while (try <= 10) - { - /* Read FIFO data on interrupt. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if ((rslt == BMI2_OK) && (int_status & BMI2_FFULL_INT_STATUS_MASK)) - { - printf("\nIteration : %d\n", try); - - accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; - - gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; - - rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - printf("\nFIFO data bytes available : %d \n", fifo_length); - printf("\nFIFO data bytes requested : %d \n", fifoframe.length); - - /* Read FIFO data. */ - rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Read FIFO data on interrupt. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - printf("\nFIFO accel frames requested : %d \n", accel_frame_length); - - /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ - rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); - printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); - - printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); - - /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ - rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); - printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); - - /* Calculating the frame count from the available bytes in FIFO - * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len + header_byte)) */ - frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH + 1)); - printf("\nAvailable frame count from available bytes: %d\n", frame_count); - - printf("\nExtracted accel frames\n"); - - if (accel_frame_length > frame_count) - { - accel_frame_length = frame_count; - } - - /* Print the parsed accelerometer data from the FIFO buffer. */ - for (index = 0; index < accel_frame_length; index++) - { - printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, - fifo_accel_data[index].y, fifo_accel_data[index].z); - } - - printf("\nExtracted gyro frames\n"); - - if (gyro_frame_length > frame_count) - { - gyro_frame_length = frame_count; - } - - /* Print the parsed gyro data from the FIFO buffer. */ - for (index = 0; index < gyro_frame_length; index++) - { - printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", - index, - fifo_gyro_data[index].x, - fifo_gyro_data[index].y, - fifo_gyro_data[index].z); - } - - /* Print control frames like sensor time and skipped frame count. */ - printf("\nSkipped frame count = %d\n", fifoframe.skipped_frame_count); - - printf("Sensor time = %lu\r\n", fifoframe.sensor_time); - - try++; - } - } - } - - bmi2_coines_deinit(); - - return rslt; -} - -/*! - * @brief This internal API is used to set configurations for accel and gyro. - */ -static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Structure to define accel and gyro configurations. */ - struct bmi2_sens_config config[2]; - - /* Configure the type of feature. */ - config[0].type = BMI2_ACCEL; - config[1].type = BMI2_GYRO; - - /* Get default configurations for the type of feature selected. */ - rslt = bmi270_context_get_sensor_config(config, 2, bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* NOTE: The user can change the following configuration parameters according to their requirement. */ - /* Accel configuration settings. */ - /* Set Output Data Rate */ - config[0].cfg.acc.odr = BMI2_ACC_ODR_25HZ; - - /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ - config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; - - /* The bandwidth parameter is used to configure the number of sensor samples that are averaged - * if it is set to 2, then 2^(bandwidth parameter) samples - * are averaged, resulting in 4 averaged samples - * Note1 : For more information, refer the datasheet. - * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but - * this has an adverse effect on the power consumed. - */ - config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; - - /* Enable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - * For more info refer datasheet. - */ - config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; - - /* Gyro configuration settings. */ - /* Set Output Data Rate */ - config[1].cfg.gyr.odr = BMI2_GYR_ODR_50HZ; - - /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ - config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; - - /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ - config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; - - /* Enable/Disable the noise performance mode for precision yaw rate sensing - * There are two modes - * 0 -> Ultra low power mode(Default) - * 1 -> High performance mode - */ - config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; - - /* Enable/Disable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - */ - config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; - - /* Set new configurations. */ - rslt = bmi270_context_set_sensor_config(config, 2, bmi2_dev); - bmi2_error_codes_print_result(rslt); - } - - return rslt; -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_full_headerless_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_full_headerless_mode/Makefile deleted file mode 100644 index 8f1f9609dd..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_full_headerless_mode/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= fifo_full_headerless_mode.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270_context.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_full_headerless_mode/fifo_full_headerless_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_full_headerless_mode/fifo_full_headerless_mode.c deleted file mode 100644 index bfc11455a8..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_full_headerless_mode/fifo_full_headerless_mode.c +++ /dev/null @@ -1,304 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270_context.h" -#include "common.h" - -/******************************************************************************/ -/*! Macros */ - -/*! Buffer size allocated to store raw FIFO data */ -#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) - -/*! Length of data to be read from FIFO */ -#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) - -/*! Number of accel frames to be extracted from FIFO */ - -/*! Calculation for frame count: Total frame count = Fifo buffer size(2048)/ Total frames(6 Accel, 6 Gyro totaling to - * 12) which equals to 170. - */ -#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(169) - -/*! Number of gyro frames to be extracted from FIFO */ -#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(169) - -/******************************************************************************/ -/*! Static Function Declaration */ - -/*! - * @brief This internal API is used to set configurations for accel and gyro. - * @param[in] dev : Structure instance of bmi2_dev. - * @return Status of execution. - */ -static int8_t set_accel_gyro_config(struct bmi2_dev *dev); - -/******************************************************************************/ - -/* This function starts the execution of program. */ -int main(void) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - uint16_t index = 0; - - uint16_t fifo_length = 0; - - uint8_t try = 1; - - /* Variable to get fifo full interrupt status. */ - uint16_t int_status = 0; - - uint16_t accel_frame_length; - - uint16_t gyro_frame_length; - - /* Variable to store the available frame length count in FIFO */ - uint8_t frame_count; - - /* Number of bytes of FIFO data. */ - uint8_t fifo_data[BMI2_FIFO_RAW_DATA_BUFFER_SIZE] = { 0 }; - - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Array of accelerometer frames -> Total bytes = - * 170 * (6 axes bytes) = 1020 bytes */ - struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; - - /* Array of gyro frames -> Total bytes = - * 170 * (6 axes bytes) = 1020 bytes */ - struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; - - /* Initialize FIFO frame structure. */ - struct bmi2_fifo_frame fifoframe = { 0 }; - - /* Accel and gyro sensor are listed in array. */ - uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270_context. */ - rslt = bmi270_context_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Enable accel and gyro sensor. */ - rslt = bmi270_context_sensor_enable(sensor_sel, 2, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Configuration settings for accel and gyro. */ - rslt = set_accel_gyro_config(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Before setting FIFO, disable the advance power save mode. */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Initially disable all configurations in fifo. */ - rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Update FIFO structure. */ - /* Mapping the buffer to store the fifo data. */ - fifoframe.data = fifo_data; - - /* Length of FIFO frame. */ - fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH; - - /* Set FIFO configuration by enabling accel, gyro. - * NOTE 1: The header mode is enabled by default. - * NOTE 2: By default the FIFO operating mode is FIFO mode. */ - rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - printf("FIFO is configured in headerless mode\n"); - - /* To enable headerless mode, disable the header. */ - if (rslt == BMI2_OK) - { - rslt = bmi2_set_fifo_config(BMI2_FIFO_HEADER_EN, BMI2_DISABLE, &bmi2_dev); - } - - /* Map FIFO full interrupt. */ - fifoframe.data_int_map = BMI2_FFULL_INT; - rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - while (try <= 10) - { - /* Read FIFO data on interrupt. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if ((rslt == BMI2_OK) && (int_status & BMI2_FFULL_INT_STATUS_MASK)) - { - printf("\nIteration : %d\n", try); - - rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; - gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; - - printf("\nFIFO data bytes available : %d \n", fifo_length); - printf("\nFIFO data bytes requested : %d \n", fifoframe.length); - - /* Read FIFO data. */ - rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Read FIFO data on interrupt. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - printf("\nFIFO accel frames requested : %d \n", accel_frame_length); - - /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ - rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); - printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); - - printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); - - /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ - rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); - printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); - - /* Calculating the frame count from the available bytes in FIFO - * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len)) */ - frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH)); - printf("\nAvailable frame count from available bytes: %d\n", frame_count); - - printf("\nExtracted accel frames\n"); - - if (accel_frame_length > frame_count) - { - accel_frame_length = frame_count; - } - - /* Print the parsed accelerometer data from the FIFO buffer. */ - for (index = 0; index < accel_frame_length; index++) - { - printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, - fifo_accel_data[index].y, fifo_accel_data[index].z); - } - - printf("\nExtracted gyro frames\n"); - - if (gyro_frame_length > frame_count) - { - gyro_frame_length = frame_count; - } - - /* Print the parsed gyro data from the FIFO buffer. */ - for (index = 0; index < gyro_frame_length; index++) - { - printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", - index, - fifo_gyro_data[index].x, - fifo_gyro_data[index].y, - fifo_gyro_data[index].z); - } - } - - try++; - } - } - - bmi2_coines_deinit(); - - return rslt; -} - -/*! - * @brief This internal API is used to set configurations for accel and gyro. - */ -static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Structure to define accel and gyro configurations. */ - struct bmi2_sens_config config[2]; - - /* Configure the type of feature. */ - config[0].type = BMI2_ACCEL; - config[1].type = BMI2_GYRO; - - /* Get default configurations for the type of feature selected. */ - rslt = bmi270_context_get_sensor_config(config, 2, bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* NOTE: The user can change the following configuration parameters according to their requirement. */ - /* Accel configuration settings. */ - /* Set Output Data Rate */ - config[0].cfg.acc.odr = BMI2_ACC_ODR_100HZ; - - /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ - config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; - - /* The bandwidth parameter is used to configure the number of sensor samples that are averaged - * if it is set to 2, then 2^(bandwidth parameter) samples - * are averaged, resulting in 4 averaged samples - * Note1 : For more information, refer the datasheet. - * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but - * this has an adverse effect on the power consumed. - */ - config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; - - /* Enable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - * For more info refer datasheet. - */ - config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; - - /* Gyro configuration settings. */ - /* Set Output Data Rate */ - config[1].cfg.gyr.odr = BMI2_GYR_ODR_100HZ; - - /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ - config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; - - /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ - config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; - - /* Enable/Disable the noise performance mode for precision yaw rate sensing - * There are two modes - * 0 -> Ultra low power mode(Default) - * 1 -> High performance mode - */ - config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; - - /* Enable/Disable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - */ - config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; - - /* Set new configurations. */ - rslt = bmi270_context_set_sensor_config(config, 2, bmi2_dev); - bmi2_error_codes_print_result(rslt); - } - - return rslt; -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_watermark_header_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_watermark_header_mode/Makefile deleted file mode 100644 index 38c5ce6a59..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_watermark_header_mode/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= fifo_watermark_header_mode.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270_context.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_watermark_header_mode/fifo_watermark_header_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_watermark_header_mode/fifo_watermark_header_mode.c deleted file mode 100644 index 59ff8f758a..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_watermark_header_mode/fifo_watermark_header_mode.c +++ /dev/null @@ -1,335 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270_context.h" -#include "common.h" - -/******************************************************************************/ -/*! Macros */ - -/*! Buffer size allocated to store raw FIFO data */ -#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) - -/*! Length of data to be read from FIFO */ -#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) - -/*! Number of accel frames to be extracted from FIFO */ - -/*! - * Calculation: - * fifo_watermark_level = 650, accel_frame_len = 6, gyro_frame_len = 6 header_byte = 1. - * fifo_accel_frame_count = (650 / (6 + 6 + 1 )) = 50 frames - * NOTE: Extra frames are read in order to get sensor time - */ -#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(55) - -/*! Number of gyro frames to be extracted from FIFO */ -#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(55) - -/*! Setting a watermark level in FIFO */ -#define BMI270_FIFO_WATERMARK_LEVEL UINT16_C(650) - -/*! Macro to read sensortime byte in FIFO */ -#define SENSORTIME_OVERHEAD_BYTE UINT8_C(3) - -/******************************************************************************/ -/*! Static Function Declaration */ - -/*! - * @brief This internal API is used to set configurations for accel and gyro. - * @param[in] dev : Structure instance of bmi2_dev. - * @return Status of execution. - */ -static int8_t set_accel_gyro_config(struct bmi2_dev *dev); - -/******************************************************************************/ -/*! Functions */ - -/* This function starts the execution of program. */ -int main(void) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Variable to store the available frame length count in FIFO */ - uint8_t frame_count; - - /* Variable to index bytes. */ - uint16_t index = 0; - - uint16_t fifo_length = 0; - - uint16_t accel_frame_length; - - uint16_t gyro_frame_length; - - uint8_t try = 1; - - /* To read sensortime, extra 3 bytes are added to fifo buffer */ - uint16_t fifo_buffer_size = BMI2_FIFO_RAW_DATA_BUFFER_SIZE + SENSORTIME_OVERHEAD_BYTE; - - /* Number of bytes of FIFO data. */ - uint8_t fifo_data[fifo_buffer_size]; - - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Array of accelerometer frames -> Total bytes = - * 55 * (6 axes bytes) = 330 bytes */ - struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; - - /* Array of gyro frames -> Total bytes = - * 55 * (6 axes bytes) = 330 bytes */ - struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; - - /* Initialize FIFO frame structure. */ - struct bmi2_fifo_frame fifoframe = { 0 }; - - /* Accel and gyro sensor are listed in array. */ - uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; - - /* Variable to get fifo water-mark interrupt status. */ - uint16_t int_status = 0; - - uint16_t watermark = 0; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270_context. */ - rslt = bmi270_context_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Enable accel and gyro sensor. */ - rslt = bmi270_context_sensor_enable(sensor_sel, 2, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Configuration settings for accel and gyro. */ - rslt = set_accel_gyro_config(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Before setting FIFO, disable the advance power save mode. */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Initially disable all configurations in fifo. */ - rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Update FIFO structure. */ - /* Mapping the buffer to store the fifo data. */ - fifoframe.data = fifo_data; - - /* Length of FIFO frame. */ - /* To read sensortime, extra 3 bytes are added to fifo user length. */ - fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH + SENSORTIME_OVERHEAD_BYTE; - - /* Set FIFO configuration by enabling accel, gyro and timestamp. - * NOTE 1: The header mode is enabled by default. - * NOTE 2: By default the FIFO operating mode is FIFO mode. - * NOTE 3: Sensortime is enabled by default */ - printf("FIFO is configured in header mode\n"); - rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* FIFO water-mark interrupt is enabled. */ - fifoframe.data_int_map = BMI2_FWM_INT; - - /* Map water-mark interrupt to the required interrupt pin. */ - rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Set water-mark level. */ - fifoframe.wm_lvl = BMI270_FIFO_WATERMARK_LEVEL; - - /* Set the water-mark level if water-mark interrupt is mapped. */ - rslt = bmi2_set_fifo_wm(fifoframe.wm_lvl, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - rslt = bmi2_get_fifo_wm(&watermark, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - printf("\nFIFO watermark level is %d\n", watermark); - - while (try <= 10) - { - /* Read FIFO data on interrupt. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* To check the status of FIFO watermark interrupt. */ - if ((rslt == BMI2_OK) && (int_status & BMI2_FWM_INT_STATUS_MASK)) - { - printf("\nIteration : %d\n", try); - - printf("\nWatermark interrupt occurred\n"); - - rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; - gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; - - printf("\nFIFO data bytes available : %d \n", fifo_length); - printf("\nFIFO data bytes requested : %d \n", fifoframe.length); - - /* Read FIFO data. */ - rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Read FIFO data on interrupt. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - printf("\nFIFO accel frames requested : %d \n", accel_frame_length); - - /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ - rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); - printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); - - printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); - - /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ - rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); - printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); - - /* Calculating the frame count from the available bytes in FIFO - * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len + header_byte)) */ - frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH + 1)); - printf("\nAvailable frame count from available bytes: %d\n", frame_count); - - printf("\nExracted accel frames\n"); - - if (accel_frame_length > frame_count) - { - accel_frame_length = frame_count; - } - - /* Print the parsed accelerometer data from the FIFO buffer. */ - for (index = 0; index < accel_frame_length; index++) - { - printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, - fifo_accel_data[index].y, fifo_accel_data[index].z); - } - - printf("\nExtracted gyro frames\n"); - - if (gyro_frame_length > frame_count) - { - gyro_frame_length = frame_count; - } - - /* Print the parsed gyro data from the FIFO buffer. */ - for (index = 0; index < gyro_frame_length; index++) - { - printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", - index, - fifo_gyro_data[index].x, - fifo_gyro_data[index].y, - fifo_gyro_data[index].z); - } - - /* Print control frames like sensor time and skipped frame count. */ - printf("Skipped frame count = %d\n", fifoframe.skipped_frame_count); - printf("Sensor time = %lu\r\n", fifoframe.sensor_time); - - try++; - } - } - } - - bmi2_coines_deinit(); - - return rslt; -} - -/*! - * @brief This internal API is used to set configurations for accel and gyro. - */ -static int8_t set_accel_gyro_config(struct bmi2_dev *dev) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Structure to define accel and gyro configurations. */ - struct bmi2_sens_config config[2]; - - /* Configure the type of feature. */ - config[0].type = BMI2_ACCEL; - config[1].type = BMI2_GYRO; - - /* Get default configurations for the type of feature selected. */ - rslt = bmi270_context_get_sensor_config(config, 2, dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* NOTE: The user can change the following configuration parameters according to their requirement. */ - /* Accel configuration settings. */ - /* Set Output Data Rate */ - config[0].cfg.acc.odr = BMI2_ACC_ODR_25HZ; - - /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ - config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; - - /* The bandwidth parameter is used to configure the number of sensor samples that are averaged - * if it is set to 2, then 2^(bandwidth parameter) samples - * are averaged, resulting in 4 averaged samples - * Note1 : For more information, refer the datasheet. - * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but - * this has an adverse effect on the power consumed. - */ - config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; - - /* Enable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - * For more info refer datasheet. - */ - config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; - - /* Gyro configuration settings. */ - /* Set Output Data Rate */ - config[1].cfg.gyr.odr = BMI2_GYR_ODR_25HZ; - - /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ - config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; - - /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ - config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; - - /* Enable/Disable the noise performance mode for precision yaw rate sensing - * There are two modes - * 0 -> Ultra low power mode(Default) - * 1 -> High performance mode - */ - config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; - - /* Enable/Disable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - */ - config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; - - /* Set new configurations. */ - rslt = bmi270_context_set_sensor_config(config, 2, dev); - bmi2_error_codes_print_result(rslt); - } - - return rslt; -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_watermark_headerless_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_watermark_headerless_mode/Makefile deleted file mode 100644 index 9d5e86e564..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_watermark_headerless_mode/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= fifo_watermark_headerless_mode.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270_context.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c deleted file mode 100644 index 878fe43bb0..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c +++ /dev/null @@ -1,331 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270_context.h" -#include "common.h" - -/******************************************************************************/ -/*! Macros */ - -/*! Buffer size allocated to store raw FIFO data */ -#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) - -/*! Length of data to be read from FIFO */ -#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) - -/*! Number of accel frames to be extracted from FIFO */ - -/*! - * Calculation: - * fifo_watermark_level = 650, accel_frame_len = 6, gyro_frame_len = 6. - * fifo_accel_frame_count = (650 / (6 + 6 )) = 54 frames - */ -#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(55) - -/*! Number of gyro frames to be extracted from FIFO */ -#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(55) - -/*! Setting a watermark level in FIFO */ -#define BMI270_FIFO_WATERMARK_LEVEL UINT16_C(650) - -/******************************************************************************/ -/*! Static Function Declaration */ - -/*! - * @brief This internal API is used to set configurations for accel and gyro. - * @param[in] dev : Structure instance of bmi2_dev. - * @return Status of execution. - */ -static int8_t set_accel_gyro_config(struct bmi2_dev *dev); - -/******************************************************************************/ -/*! Functions */ - -/* This function starts the execution of program. */ -int main(void) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Variable to index bytes. */ - uint16_t index = 0; - - /* Variable to store the available frame length count in FIFO */ - uint8_t frame_count; - - uint16_t fifo_length = 0; - - uint16_t accel_frame_length; - - uint16_t gyro_frame_length; - - uint8_t try = 1; - - /* Number of bytes of FIFO data. */ - uint8_t fifo_data[BMI2_FIFO_RAW_DATA_BUFFER_SIZE] = { 0 }; - - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Array of accelerometer frames -> Total bytes = - * 50 * (6 axes bytes) = 300 bytes */ - struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; - - /* Array of gyro frames -> Total bytes = - * 50 * (6 axes bytes) = 300 bytes */ - struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; - - /* Initialize FIFO frame structure. */ - struct bmi2_fifo_frame fifoframe = { 0 }; - - /* Accel and gyro sensor are listed in array. */ - uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; - - /* Variable to get fifo water-mark interrupt status. */ - uint16_t int_status = 0; - - uint16_t watermark = 0; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270_context. */ - rslt = bmi270_context_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Enable accel and gyro sensor. */ - rslt = bmi270_context_sensor_enable(sensor_sel, 2, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Configuration settings for accel and gyro. */ - rslt = set_accel_gyro_config(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Before setting FIFO, disable the advance power save mode. */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Initially disable all configurations in fifo. */ - rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Update FIFO structure. */ - /* Mapping the buffer to store the fifo data. */ - fifoframe.data = fifo_data; - - /* Length of FIFO frame. */ - fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH; - - /* Set FIFO configuration by enabling accel, gyro. - * NOTE 1: The header mode is enabled by default. - * NOTE 2: By default the FIFO operating mode is FIFO mode. */ - rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - printf("FIFO is configured in headerless mode\n"); - - /* To enable headerless mode, disable the header. */ - if (rslt == BMI2_OK) - { - rslt = bmi2_set_fifo_config(BMI2_FIFO_HEADER_EN, BMI2_DISABLE, &bmi2_dev); - } - - /* FIFO water-mark interrupt is enabled. */ - fifoframe.data_int_map = BMI2_FWM_INT; - - /* Map water-mark interrupt to the required interrupt pin. */ - rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Set water-mark level. */ - fifoframe.wm_lvl = BMI270_FIFO_WATERMARK_LEVEL; - - fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH; - - /* Set the water-mark level if water-mark interrupt is mapped. */ - rslt = bmi2_set_fifo_wm(fifoframe.wm_lvl, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - rslt = bmi2_get_fifo_wm(&watermark, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - printf("\nFIFO watermark level is %d\n", watermark); - - while (try <= 10) - { - /* Read FIFO data on interrupt. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* To check the status of FIFO watermark interrupt. */ - if ((rslt == BMI2_OK) && (int_status & BMI2_FWM_INT_STATUS_MASK)) - { - printf("\nIteration : %d\n", try); - - printf("\nWatermark interrupt occurred\n"); - - rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; - gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; - - printf("\nFIFO data bytes available : %d \n", fifo_length); - printf("\nFIFO data bytes requested : %d \n", fifoframe.length); - - /* Read FIFO data. */ - rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Read FIFO data on interrupt. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - printf("\nFIFO accel frames requested : %d \n", accel_frame_length); - - /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ - rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); - printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); - - printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); - - /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ - rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); - printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); - - /* Calculating the frame count from the available bytes in FIFO - * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len)) */ - frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH)); - printf("\nAvailable frame count from available bytes: %d\n", frame_count); - - printf("\nExracted accel frames\n"); - - if (accel_frame_length > frame_count) - { - accel_frame_length = frame_count; - } - - /* Print the parsed accelerometer data from the FIFO buffer. */ - for (index = 0; index < accel_frame_length; index++) - { - printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, - fifo_accel_data[index].y, fifo_accel_data[index].z); - } - - printf("\nExtracted gyro frames\n"); - - if (gyro_frame_length > frame_count) - { - gyro_frame_length = frame_count; - } - - /* Print the parsed gyro data from the FIFO buffer. */ - for (index = 0; index < gyro_frame_length; index++) - { - printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", - index, - fifo_gyro_data[index].x, - fifo_gyro_data[index].y, - fifo_gyro_data[index].z); - } - } - - try++; - } - } - - bmi2_coines_deinit(); - - return rslt; -} - -/*! - * @brief This internal API is used to set configurations for accel. - */ -static int8_t set_accel_gyro_config(struct bmi2_dev *dev) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Structure to define accel and gyro configurations. */ - struct bmi2_sens_config config[2]; - - /* Configure the type of feature. */ - config[0].type = BMI2_ACCEL; - config[1].type = BMI2_GYRO; - - /* Get default configurations for the type of feature selected. */ - rslt = bmi270_context_get_sensor_config(config, 2, dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* NOTE: The user can change the following configuration parameter according to their requirement. */ - /* Accel configuration settings. */ - /* Set Output Data Rate */ - config[0].cfg.acc.odr = BMI2_ACC_ODR_100HZ; - - /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ - config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; - - /* The bandwidth parameter is used to configure the number of sensor samples that are averaged - * if it is set to 2, then 2^(bandwidth parameter) samples - * are averaged, resulting in 4 averaged samples - * Note1 : For more information, refer the datasheet. - * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but - * this has an adverse effect on the power consumed. - */ - config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; - - /* Enable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - * For more info refer datasheet. - */ - config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; - - /* Gyro configuration settings. */ - /* Set Output Data Rate */ - config[1].cfg.gyr.odr = BMI2_GYR_ODR_100HZ; - - /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ - config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; - - /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ - config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; - - /* Enable/Disable the noise performance mode for precision yaw rate sensing - * There are two modes - * 0 -> Ultra low power mode(Default) - * 1 -> High performance mode - */ - config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; - - /* Enable/Disable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - */ - config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; - - /* Set new configurations. */ - rslt = bmi270_context_set_sensor_config(config, 2, dev); - bmi2_error_codes_print_result(rslt); - } - - return rslt; -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/gyro/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/gyro/Makefile deleted file mode 100644 index 135fe5ba3f..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/gyro/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= gyro.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270_context.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/gyro/gyro.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/gyro/gyro.c deleted file mode 100644 index 0a057e4647..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/gyro/gyro.c +++ /dev/null @@ -1,195 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270_context.h" -#include "common.h" - -/******************************************************************************/ -/*! Static Function Declaration */ - -/*! - * @brief This internal API is used to set configurations for gyro. - * - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Status of execution. - */ -static int8_t set_gyro_config(struct bmi2_dev *dev); - -/*! - * @brief This function converts lsb to degree per second for 16 bit gyro at - * range 125, 250, 500, 1000 or 2000dps. - * - * @param[in] val : LSB from each axis. - * @param[in] dps : Degree per second. - * @param[in] bit_width : Resolution for gyro. - * - * @return Degree per second. - */ -static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width); - -/******************************************************************************/ -/*! Functions */ - -/* This function starts the execution of program. */ -int main(void) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Variable to define limit to print gyro data. */ - uint8_t limit = 10; - - uint8_t indx = 0; - - float x = 0, y = 0, z = 0; - - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Create an instance of sensor data structure. */ - struct bmi2_sensor_data sensor_data = { 0 }; - - /* Assign gyro sensor to variable. */ - uint8_t sens_list = BMI2_GYRO; - - /* Initialize the interrupt status of gyro. */ - uint16_t int_status = 0; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270_context. */ - rslt = bmi270_context_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Enable the selected sensors. */ - rslt = bmi270_context_sensor_enable(&sens_list, 1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Gyro configuration settings. */ - rslt = set_gyro_config(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Select gyro sensor. */ - sensor_data.type = BMI2_GYRO; - printf("Gyro and DPS data\n"); - printf("Gyro data collected at Range 2000 dps with 16-bit resolution\n"); - - /* Loop to print gyro data when interrupt occurs. */ - while (indx <= limit) - { - /* To get the data ready interrupt status of gyro. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* To check the data ready interrupt status and print the status for 10 samples. */ - if (int_status & BMI2_GYR_DRDY_INT_MASK) - { - /* Get gyro data for x, y and z axis. */ - rslt = bmi270_context_get_sensor_data(&sensor_data, 1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - printf("\nGyr_X = %d\t", sensor_data.sens_data.gyr.x); - printf("Gyr_Y = %d\t", sensor_data.sens_data.gyr.y); - printf("Gyr_Z = %d\r\n", sensor_data.sens_data.gyr.z); - - /* Converting lsb to degree per second for 16 bit gyro at 2000dps range. */ - x = lsb_to_dps(sensor_data.sens_data.gyr.x, 2000, bmi2_dev.resolution); - y = lsb_to_dps(sensor_data.sens_data.gyr.y, 2000, bmi2_dev.resolution); - z = lsb_to_dps(sensor_data.sens_data.gyr.z, 2000, bmi2_dev.resolution); - - /* Print the data in dps. */ - printf("Gyr_DPS_X = %4.2f, Gyr_DPS_Y = %4.2f, Gyr_DPS_Z = %4.2f\n", x, y, z); - - indx++; - } - } - } - } - - bmi2_coines_deinit(); - - return rslt; -} - -/*! - * @brief This internal API is used to set configurations for gyro. - */ -static int8_t set_gyro_config(struct bmi2_dev *dev) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Structure to define the type of sensor and its configurations. */ - struct bmi2_sens_config config; - - /* Configure the type of feature. */ - config.type = BMI2_GYRO; - - /* Get default configurations for the type of feature selected. */ - rslt = bmi270_context_get_sensor_config(&config, 1, dev); - bmi2_error_codes_print_result(rslt); - - /* Map data ready interrupt to interrupt pin. */ - rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT2, dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* The user can change the following configuration parameters according to their requirement. */ - /* Set Output Data Rate */ - config.cfg.gyr.odr = BMI2_GYR_ODR_200HZ; - - /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ - config.cfg.gyr.range = BMI2_GYR_RANGE_2000; - - /* Gyroscope bandwidth parameters. By default the gyro bandwidth is in normal mode. */ - config.cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; - - /* Enable/Disable the noise performance mode for precision yaw rate sensing - * There are two modes - * 0 -> Ultra low power mode(Default) - * 1 -> High performance mode - */ - config.cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; - - /* Enable/Disable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - */ - config.cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; - - /* Set the gyro configurations. */ - rslt = bmi270_context_set_sensor_config(&config, 1, dev); - } - - return rslt; -} - -/*! - * @brief This function converts lsb to degree per second for 16 bit gyro at - * range 125, 250, 500, 1000 or 2000dps. - */ -static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width) -{ - float half_scale = ((float)(1 << bit_width) / 2.0f); - - return (dps / ((half_scale) + BMI2_GYR_RANGE_2000)) * (val); -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/step_counter/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/step_counter/Makefile deleted file mode 100644 index 1e8be48cdc..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/step_counter/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= step_counter.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270_context.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/step_counter/step_counter.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/step_counter/step_counter.c deleted file mode 100644 index 2513b028ac..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_context/step_counter/step_counter.c +++ /dev/null @@ -1,146 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270_context.h" -#include "common.h" - -/******************************************************************************/ -/*! Static function declaration */ - -/*! - * @brief This internal API is used to set configurations for step counter. - * - * @param[in] bmi2_dev : Structure instance of bmi2_dev. - * - * @return Status of execution. - */ -static int8_t set_feature_config(struct bmi2_dev *bmi2_dev); - -/******************************************************************************/ -/*! Functions */ - -/* This function starts the execution of program. */ -int main(void) -{ - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Structure to define type of sensor and their respective data. */ - struct bmi2_sensor_data sensor_data; - - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Accel sensor and step counter feature are listed in array. */ - uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_STEP_COUNTER }; - - /* Variable to get step counter interrupt status. */ - uint16_t int_status = 0; - - /* Select features and their pins to be mapped to. */ - struct bmi2_sens_int_config sens_int = { .type = BMI2_STEP_COUNTER, .hw_int_pin = BMI2_INT2 }; - - /* Type of sensor to get step counter data. */ - sensor_data.type = BMI2_STEP_COUNTER; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270_context. */ - rslt = bmi270_context_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Enable the selected sensor. */ - rslt = bmi270_context_sensor_enable(sensor_sel, 2, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Set the feature configuration for step counter. */ - rslt = set_feature_config(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - printf("Step counter watermark level set to 1 (20 steps)\n"); - - if (rslt == BMI2_OK) - { - /* Map the step counter feature interrupt. */ - rslt = bmi270_context_map_feat_int(&sens_int, 1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Move the board in steps for 20 times to get step counter interrupt. */ - printf("Move the board in steps\n"); - - /* Loop to get number of steps counted. */ - do - { - /* To get the interrupt status of the step counter. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* To check the interrupt status of the step counter. */ - if (int_status & BMI270_CONTEXT_STEP_CNT_STATUS_MASK) - { - printf("Step counter interrupt occurred when watermark level (20 steps) is reached\n"); - - /* Get step counter output. */ - rslt = bmi270_context_get_sensor_data(&sensor_data, 1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Print the step counter output. */ - printf("No of steps counted = %ld", sensor_data.sens_data.step_counter_output); - break; - } - } while (rslt == BMI2_OK); - } - } - } - - bmi2_coines_deinit(); - - return rslt; -} - -/*! - * @brief This internal API is used to set configurations for step counter. - */ -static int8_t set_feature_config(struct bmi2_dev *bmi2_dev) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Structure to define the type of sensor and its configurations. */ - struct bmi2_sens_config config; - - /* Configure the type of sensor. */ - config.type = BMI2_STEP_COUNTER; - - /* Get default configurations for the type of feature selected. */ - rslt = bmi270_context_get_sensor_config(&config, 1, bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Setting water-mark level to 1 for step counter to get interrupt after 20 step counts. Every 20 steps once - * output triggers. */ - config.cfg.step_counter.watermark_level = 1; - - /* Set new configuration. */ - rslt = bmi270_context_set_sensor_config(&config, 1, bmi2_dev); - bmi2_error_codes_print_result(rslt); - } - - return rslt; -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/accel/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/accel/Makefile deleted file mode 100644 index d4cc64caf4..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/accel/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= accel.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270_hc.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/accel/accel.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/accel/accel.c deleted file mode 100644 index 553db37724..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/accel/accel.c +++ /dev/null @@ -1,201 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270_hc.h" -#include "common.h" - -/******************************************************************************/ -/*! Macro definition */ - -/*! Earth's gravity in m/s^2 */ -#define GRAVITY_EARTH (9.80665f) - -/******************************************************************************/ -/*! Static Function Declaration */ - -/*! - * @brief This internal API is used to set configurations for accel. - * - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Status of execution. - */ -static int8_t set_accel_config(struct bmi2_dev *bmi2_dev); - -/*! - * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at - * range 2G, 4G, 8G or 16G. - * - * @param[in] val : LSB from each axis. - * @param[in] g_range : Gravity range. - * @param[in] bit_width : Resolution for accel. - * - * @return Gravity. - */ -static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width); - -/******************************************************************************/ -/*! Functions */ - -/* This function starts the execution of program. */ -int main(void) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Variable to define limit to print accel data. */ - uint8_t limit = 20; - - /* Assign accel sensor to variable. */ - uint8_t sensor_list = BMI2_ACCEL; - - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Create an instance of sensor data structure. */ - struct bmi2_sensor_data sensor_data = { 0 }; - - /* Initialize the interrupt status of accel. */ - uint16_t int_status = 0; - - uint8_t indx = 0; - float x = 0, y = 0, z = 0; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270_hc. */ - rslt = bmi270_hc_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Enable the accel sensor. */ - rslt = bmi270_hc_sensor_enable(&sensor_list, 1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Accel configuration settings. */ - rslt = set_accel_config(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Assign accel sensor. */ - sensor_data.type = BMI2_ACCEL; - printf("Accel and m/s2 data \n"); - printf("Accel data collected at 2G Range with 16-bit resolution\n"); - - /* Loop to print the accel data when interrupt occurs. */ - while (indx <= limit) - { - /* To get the status of accel data ready interrupt. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* To check the accel data ready interrupt status and print the status for 10 samples. */ - if (int_status & BMI2_ACC_DRDY_INT_MASK) - { - /* Get accelerometer data for x, y and z axis. */ - rslt = bmi270_hc_get_sensor_data(&sensor_data, 1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - printf("\nAcc_X = %d\t", sensor_data.sens_data.acc.x); - printf("Acc_Y = %d\t", sensor_data.sens_data.acc.y); - printf("Acc_Z = %d\r\n", sensor_data.sens_data.acc.z); - - /* Converting lsb to meter per second squared for 16 bit accelerometer at 2G range. */ - x = lsb_to_mps2(sensor_data.sens_data.acc.x, 2, bmi2_dev.resolution); - y = lsb_to_mps2(sensor_data.sens_data.acc.y, 2, bmi2_dev.resolution); - z = lsb_to_mps2(sensor_data.sens_data.acc.z, 2, bmi2_dev.resolution); - - /* Print the data in m/s2. */ - printf("\nAcc_ms2_X = %4.2f, Acc_ms2_Y = %4.2f, Acc_ms2_Z = %4.2f\n", x, y, z); - - indx++; - } - } - } - } - - bmi2_coines_deinit(); - - return rslt; -} - -/*! - * @brief This internal API is used to set configurations for accel. - */ -static int8_t set_accel_config(struct bmi2_dev *bmi2_dev) -{ - - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Structure to define accelerometer configuration. */ - struct bmi2_sens_config config; - - /* Configure the type of feature. */ - config.type = BMI2_ACCEL; - - /* Get default configurations for the type of feature selected. */ - rslt = bmi270_hc_get_sensor_config(&config, 1, bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* NOTE: The user can change the following configuration parameters according to their requirement. */ - /* Set Output Data Rate */ - config.cfg.acc.odr = BMI2_ACC_ODR_200HZ; - - /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ - config.cfg.acc.range = BMI2_ACC_RANGE_2G; - - /* The bandwidth parameter is used to configure the number of sensor samples that are averaged - * if it is set to 2, then 2^(bandwidth parameter) samples - * are averaged, resulting in 4 averaged samples. - * Note1 : For more information, refer the datasheet. - * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but - * this has an adverse effect on the power consumed. - */ - config.cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; - - /* Enable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - * For more info refer datasheet. - */ - config.cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; - - /* Set the accel configurations. */ - rslt = bmi270_hc_set_sensor_config(&config, 1, bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Map data ready interrupt to interrupt pin. */ - rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT1, bmi2_dev); - bmi2_error_codes_print_result(rslt); - } - - return rslt; -} - -/*! - * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at - * range 2G, 4G, 8G or 16G. - */ -static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width) -{ - float half_scale = ((float)(1 << bit_width) / 2.0f); - - return (GRAVITY_EARTH * val * g_range) / half_scale; -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/accel_gyro/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/accel_gyro/Makefile deleted file mode 100644 index 19eb609eae..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/accel_gyro/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= accel_gyro.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270_hc.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/accel_gyro/accel_gyro.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/accel_gyro/accel_gyro.c deleted file mode 100644 index c92a1b82d5..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/accel_gyro/accel_gyro.c +++ /dev/null @@ -1,268 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270_hc.h" -#include "common.h" - -/******************************************************************************/ -/*! Macro definition */ - -/*! Earth's gravity in m/s^2 */ -#define GRAVITY_EARTH (9.80665f) - -/*! Macros to select the sensors */ -#define ACCEL UINT8_C(0x00) -#define GYRO UINT8_C(0x01) - -/******************************************************************************/ -/*! Static Function Declaration */ - -/*! - * @brief This internal API is used to set configurations for accel. - * - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Status of execution. - */ -static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev); - -/*! - * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at - * range 2G, 4G, 8G or 16G. - * - * @param[in] val : LSB from each axis. - * @param[in] g_range : Gravity range. - * @param[in] bit_width : Resolution for accel. - * - * @return Gravity. - */ -static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width); - -/*! - * @brief This function converts lsb to degree per second for 16 bit gyro at - * range 125, 250, 500, 1000 or 2000dps. - * - * @param[in] val : LSB from each axis. - * @param[in] dps : Degree per second. - * @param[in] bit_width : Resolution for gyro. - * - * @return Degree per second. - */ -static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width); - -/******************************************************************************/ -/*! Functions */ - -/* This function starts the execution of program. */ -int main(void) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Variable to define limit to print accel data. */ - uint8_t limit = 10; - - /* Assign accel and gyro sensor to variable. */ - uint8_t sensor_list[2] = { BMI2_ACCEL, BMI2_GYRO }; - - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Create an instance of sensor data structure. */ - struct bmi2_sensor_data sensor_data[2] = { { 0 } }; - - /* Initialize the interrupt status of accel and gyro. */ - uint16_t int_status = 0; - - uint8_t indx = 1; - - float x = 0, y = 0, z = 0; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270_hc. */ - rslt = bmi270_hc_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Enable the accel and gyro sensor. */ - rslt = bmi270_hc_sensor_enable(sensor_list, 2, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Accel and gyro configuration settings. */ - rslt = set_accel_gyro_config(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Assign accel and gyro sensor. */ - sensor_data[ACCEL].type = BMI2_ACCEL; - sensor_data[GYRO].type = BMI2_GYRO; - - /* Loop to print accel and gyro data when interrupt occurs. */ - while (indx <= limit) - { - /* To get the data ready interrupt status of accel and gyro. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* To check the data ready interrupt status and print the status for 10 samples. */ - if ((int_status & BMI2_ACC_DRDY_INT_MASK) && (int_status & BMI2_GYR_DRDY_INT_MASK)) - { - /* Get accel and gyro data for x, y and z axis. */ - rslt = bmi270_hc_get_sensor_data(sensor_data, 2, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - printf("\n******* Accel(Raw and m/s2) Gyro(Raw and dps) data : %d *******\n", indx); - - printf("\nAcc_x = %d\t", sensor_data[ACCEL].sens_data.acc.x); - printf("Acc_y = %d\t", sensor_data[ACCEL].sens_data.acc.y); - printf("Acc_z = %d", sensor_data[ACCEL].sens_data.acc.z); - - /* Converting lsb to meter per second squared for 16 bit accelerometer at 2G range. */ - x = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.x, 2, bmi2_dev.resolution); - y = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.y, 2, bmi2_dev.resolution); - z = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.z, 2, bmi2_dev.resolution); - - /* Print the data in m/s2. */ - printf("\nAcc_ms2_X = %4.2f, Acc_ms2_Y = %4.2f, Acc_ms2_Z = %4.2f\n", x, y, z); - - printf("\nGyr_X = %d\t", sensor_data[GYRO].sens_data.gyr.x); - printf("Gyr_Y = %d\t", sensor_data[GYRO].sens_data.gyr.y); - printf("Gyr_Z = %d\n", sensor_data[GYRO].sens_data.gyr.z); - - /* Converting lsb to degree per second for 16 bit gyro at 2000dps range. */ - x = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.x, 2000, bmi2_dev.resolution); - y = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.y, 2000, bmi2_dev.resolution); - z = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.z, 2000, bmi2_dev.resolution); - - /* Print the data in dps. */ - printf("Gyro_DPS_X = %4.2f, Gyro_DPS_Y = %4.2f, Gyro_DPS_Z = %4.2f\n", x, y, z); - - indx++; - } - } - } - } - - bmi2_coines_deinit(); - - return rslt; -} - -/*! - * @brief This internal API is used to set configurations for accel and gyro. - */ -static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Structure to define accelerometer and gyro configuration. */ - struct bmi2_sens_config config[2]; - - /* Configure the type of feature. */ - config[ACCEL].type = BMI2_ACCEL; - config[GYRO].type = BMI2_GYRO; - - /* Get default configurations for the type of feature selected. */ - rslt = bmi270_hc_get_sensor_config(config, 2, bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Map data ready interrupt to interrupt pin. */ - rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT1, bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* NOTE: The user can change the following configuration parameters according to their requirement. */ - /* Set Output Data Rate */ - config[ACCEL].cfg.acc.odr = BMI2_ACC_ODR_200HZ; - - /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ - config[ACCEL].cfg.acc.range = BMI2_ACC_RANGE_2G; - - /* The bandwidth parameter is used to configure the number of sensor samples that are averaged - * if it is set to 2, then 2^(bandwidth parameter) samples - * are averaged, resulting in 4 averaged samples. - * Note1 : For more information, refer the datasheet. - * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but - * this has an adverse effect on the power consumed. - */ - config[ACCEL].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; - - /* Enable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - * For more info refer datasheet. - */ - config[ACCEL].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; - - /* The user can change the following configuration parameters according to their requirement. */ - /* Set Output Data Rate */ - config[GYRO].cfg.gyr.odr = BMI2_GYR_ODR_200HZ; - - /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ - config[GYRO].cfg.gyr.range = BMI2_GYR_RANGE_2000; - - /* Gyroscope bandwidth parameters. By default the gyro bandwidth is in normal mode. */ - config[GYRO].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; - - /* Enable/Disable the noise performance mode for precision yaw rate sensing - * There are two modes - * 0 -> Ultra low power mode(Default) - * 1 -> High performance mode - */ - config[GYRO].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; - - /* Enable/Disable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - */ - config[GYRO].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; - - /* Set the accel and gyro configurations. */ - rslt = bmi270_hc_set_sensor_config(config, 2, bmi2_dev); - bmi2_error_codes_print_result(rslt); - } - - return rslt; -} - -/*! - * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at - * range 2G, 4G, 8G or 16G. - */ -static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width) -{ - float half_scale = ((float)(1 << bit_width) / 2.0f); - - return (GRAVITY_EARTH * val * g_range) / half_scale; -} - -/*! - * @brief This function converts lsb to degree per second for 16 bit gyro at - * range 125, 250, 500, 1000 or 2000dps. - */ -static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width) -{ - float half_scale = ((float)(1 << bit_width) / 2.0f); - - return (dps / ((half_scale) + BMI2_GYR_RANGE_2000)) * (val); -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/activity_recognition/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/activity_recognition/Makefile deleted file mode 100644 index ae036b47c1..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/activity_recognition/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= activity_recognition.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270_hc.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/activity_recognition/activity_recognition.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/activity_recognition/activity_recognition.c deleted file mode 100644 index 11750ca538..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/activity_recognition/activity_recognition.c +++ /dev/null @@ -1,154 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270_hc.h" -#include "common.h" - -/******************************************************************************/ -/*! Static function declaration */ - -/*! - * @brief This internal API is used to set configurations for step counter. - * - * @param[in] bmi2_dev : Structure instance of bmi2_dev. - * - * @return Status of execution. - */ -static int8_t set_config(struct bmi2_dev *bmi2_dev); - -/******************************************************************************/ -/*! Functions */ -/* This function starts the execution of program. */ -int main(void) -{ - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Variable to define index for FIFO data */ - uint8_t count; - - /* Various Activity recognitions are listed in array. */ - const char *activity_reg_output[6] = { "OTHERS", "STILL", "WALKING", "RUNNING", "ON_BICYCLE", "IN_VEHICLE" }; - - /* Accel sensor and step activity feature are listed in array. */ - uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_ACTIVITY_RECOGNITION }; - - /* Array to define FIFO data */ - uint8_t fifo_data[516] = { 0 }; - - /* Variable to store number of activity frames */ - uint16_t act_frames; - - /* Structure to define a FIFO */ - struct bmi2_fifo_frame fifoframe = { 0 }; - - /* Structure to define output for activity recognition */ - struct bmi2_act_recog_output act_recog_data[5] = { { 0 } }; - - uint16_t fifo_length; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270_hc. */ - rslt = bmi270_hc_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Enable the selected sensor. */ - rslt = bmi270_hc_sensor_enable(sensor_sel, 2, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - rslt = set_config(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Update FIFO structure */ - fifoframe.data = fifo_data; - fifoframe.length = 516; - - rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); - printf("Fifo length = %d \n", fifo_length); - - /* Read FIFO data */ - rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - while (rslt != BMI2_W_FIFO_EMPTY) - { - /* Provide the number of frames to be read */ - act_frames = 5; - - printf("Requested FIFO data frames : %d\n", act_frames); - - /* Get the activity output */ - rslt = bmi270_hc_get_act_recog_output(act_recog_data, &act_frames, &fifoframe, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - printf("Parsed FIFO data frames : %d\r\n", act_frames); - - for (count = 0; count < act_frames; count++) - { - printf( - "Activity Recognition output[%d]:Sensor time: %lu\t Previous activity: %s\t current: %s\n", - count, - act_recog_data[count].time_stamp, - activity_reg_output[act_recog_data[count].prev_act], - activity_reg_output[act_recog_data[count].curr_act]); - } - } - } - } - } - - bmi2_coines_deinit(); - - return rslt; -} - -static int8_t set_config(struct bmi2_dev *bmi2_dev) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - uint8_t temp_data[2]; - - /* Disable advanced power save */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Reset FIFO */ - rslt = bmi2_set_command_register(BMI2_FIFO_FLUSH_CMD, bmi2_dev); - bmi2_error_codes_print_result(rslt); - - printf("Move the board for activity recognition for 30 sec :\n"); - bmi2_dev->delay_us(30000000, bmi2_dev->intf_ptr); - - if (rslt == BMI2_OK) - { - rslt = bmi2_get_regs(BMI2_FIFO_CONFIG_0_ADDR, temp_data, 2, bmi2_dev); - bmi2_error_codes_print_result(rslt); - } - } - - return rslt; -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/common/common.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/common/common.c deleted file mode 100644 index afff37bc84..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/common/common.c +++ /dev/null @@ -1,372 +0,0 @@ -/** - * Copyright (C) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - */ - -#include -#include -#include - -#include "coines.h" -#include "bmi2_defs.h" - -/******************************************************************************/ -/*! Macro definitions */ -#define BMI2XY_SHUTTLE_ID UINT16_C(0x1B8) - -/*! Macro that defines read write length */ -#define READ_WRITE_LEN UINT8_C(46) - -/******************************************************************************/ -/*! Static variable definition */ - -/*! Variable that holds the I2C device address or SPI chip selection */ -static uint8_t dev_addr; - -/******************************************************************************/ -/*! User interface functions */ - -/*! - * I2C read function map to COINES platform - */ -BMI2_INTF_RETURN_TYPE bmi2_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr) -{ - uint8_t dev_addr = *(uint8_t*)intf_ptr; - - return coines_read_i2c(dev_addr, reg_addr, reg_data, (uint16_t)len); -} - -/*! - * I2C write function map to COINES platform - */ -BMI2_INTF_RETURN_TYPE bmi2_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr) -{ - uint8_t dev_addr = *(uint8_t*)intf_ptr; - - return coines_write_i2c(dev_addr, reg_addr, (uint8_t *)reg_data, (uint16_t)len); -} - -/*! - * SPI read function map to COINES platform - */ -BMI2_INTF_RETURN_TYPE bmi2_spi_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr) -{ - uint8_t dev_addr = *(uint8_t*)intf_ptr; - - return coines_read_spi(dev_addr, reg_addr, reg_data, (uint16_t)len); -} - -/*! - * SPI write function map to COINES platform - */ -BMI2_INTF_RETURN_TYPE bmi2_spi_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr) -{ - uint8_t dev_addr = *(uint8_t*)intf_ptr; - - return coines_write_spi(dev_addr, reg_addr, (uint8_t *)reg_data, (uint16_t)len); -} - -/*! - * Delay function map to COINES platform - */ -void bmi2_delay_us(uint32_t period, void *intf_ptr) -{ - coines_delay_usec(period); -} - -/*! - * @brief Function to select the interface between SPI and I2C. - * Also to initialize coines platform - */ -int8_t bmi2_interface_init(struct bmi2_dev *bmi, uint8_t intf) -{ - int8_t rslt = BMI2_OK; - struct coines_board_info board_info; - - if (bmi != NULL) - { - int16_t result = coines_open_comm_intf(COINES_COMM_INTF_USB); - if (result < COINES_SUCCESS) - { - printf( - "\n Unable to connect with Application Board ! \n" " 1. Check if the board is connected and powered on. \n" " 2. Check if Application Board USB driver is installed. \n" - " 3. Check if board is in use by another application. (Insufficient permissions to access USB) \n"); - exit(result); - } - - result = coines_get_board_info(&board_info); - -#if defined(PC) - setbuf(stdout, NULL); -#endif - - if (result == COINES_SUCCESS) - { - if ((board_info.shuttle_id != BMI2XY_SHUTTLE_ID)) - { - printf("! Warning invalid sensor shuttle \n ," "This application will not support this sensor \n"); - exit(COINES_E_FAILURE); - } - } - - coines_set_shuttleboard_vdd_vddio_config(0, 0); - coines_delay_msec(100); - - /* Bus configuration : I2C */ - if (intf == BMI2_I2C_INTF) - { - printf("I2C Interface \n"); - - /* To initialize the user I2C function */ - dev_addr = BMI2_I2C_PRIM_ADDR; - bmi->intf = BMI2_I2C_INTF; - bmi->read = bmi2_i2c_read; - bmi->write = bmi2_i2c_write; - coines_config_i2c_bus(COINES_I2C_BUS_0, COINES_I2C_FAST_MODE); - } - /* Bus configuration : SPI */ - else if (intf == BMI2_SPI_INTF) - { - printf("SPI Interface \n"); - - /* To initialize the user SPI function */ - dev_addr = COINES_SHUTTLE_PIN_7; - bmi->intf = BMI2_SPI_INTF; - bmi->read = bmi2_spi_read; - bmi->write = bmi2_spi_write; - coines_config_spi_bus(COINES_SPI_BUS_0, COINES_SPI_SPEED_5_MHZ, COINES_SPI_MODE3); - coines_set_pin_config(COINES_SHUTTLE_PIN_7, COINES_PIN_DIRECTION_OUT, COINES_PIN_VALUE_HIGH); - } - - /* Assign device address to interface pointer */ - bmi->intf_ptr = &dev_addr; - - /* Configure delay in microseconds */ - bmi->delay_us = bmi2_delay_us; - - /* Configure max read/write length (in bytes) ( Supported length depends on target machine) */ - bmi->read_write_len = READ_WRITE_LEN; - - /* Assign to NULL to load the default config file. */ - bmi->config_file_ptr = NULL; - - coines_delay_usec(10000); - - coines_set_shuttleboard_vdd_vddio_config(3300, 3300); - - coines_delay_usec(10000); - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; - -} - -/*! - * @brief Prints the execution status of the APIs. - */ -void bmi2_error_codes_print_result(int8_t rslt) -{ - switch (rslt) - { - case BMI2_OK: - - /* Do nothing */ - break; - - case BMI2_W_FIFO_EMPTY: - printf("Warning [%d] : FIFO empty\r\n", rslt); - break; - case BMI2_W_PARTIAL_READ: - printf("Warning [%d] : FIFO partial read\r\n", rslt); - break; - case BMI2_E_NULL_PTR: - printf( - "Error [%d] : Null pointer error. It occurs when the user tries to assign value (not address) to a pointer," " which has been initialized to NULL.\r\n", - rslt); - break; - - case BMI2_E_COM_FAIL: - printf( - "Error [%d] : Communication failure error. It occurs due to read/write operation failure and also due " "to power failure during communication\r\n", - rslt); - break; - - case BMI2_E_DEV_NOT_FOUND: - printf("Error [%d] : Device not found error. It occurs when the device chip id is incorrectly read\r\n", - rslt); - break; - - case BMI2_E_INVALID_SENSOR: - printf( - "Error [%d] : Invalid sensor error. It occurs when there is a mismatch in the requested feature with the " "available one\r\n", - rslt); - break; - - case BMI2_E_SELF_TEST_FAIL: - printf( - "Error [%d] : Self-test failed error. It occurs when the validation of accel self-test data is " "not satisfied\r\n", - rslt); - break; - - case BMI2_E_INVALID_INT_PIN: - printf( - "Error [%d] : Invalid interrupt pin error. It occurs when the user tries to configure interrupt pins " "apart from INT1 and INT2\r\n", - rslt); - break; - - case BMI2_E_OUT_OF_RANGE: - printf( - "Error [%d] : Out of range error. It occurs when the data exceeds from filtered or unfiltered data from " "fifo and also when the range exceeds the maximum range for accel and gyro while performing FOC\r\n", - rslt); - break; - - case BMI2_E_ACC_INVALID_CFG: - printf( - "Error [%d] : Invalid Accel configuration error. It occurs when there is an error in accel configuration" " register which could be one among range, BW or filter performance in reg address 0x40\r\n", - rslt); - break; - - case BMI2_E_GYRO_INVALID_CFG: - printf( - "Error [%d] : Invalid Gyro configuration error. It occurs when there is a error in gyro configuration" "register which could be one among range, BW or filter performance in reg address 0x42\r\n", - rslt); - break; - - case BMI2_E_ACC_GYR_INVALID_CFG: - printf( - "Error [%d] : Invalid Accel-Gyro configuration error. It occurs when there is a error in accel and gyro" " configuration registers which could be one among range, BW or filter performance in reg address 0x40 " "and 0x42\r\n", - rslt); - break; - - case BMI2_E_CONFIG_LOAD: - printf( - "Error [%d] : Configuration load error. It occurs when failure observed while loading the configuration " "into the sensor\r\n", - rslt); - break; - - case BMI2_E_INVALID_PAGE: - printf( - "Error [%d] : Invalid page error. It occurs due to failure in writing the correct feature configuration " "from selected page\r\n", - rslt); - break; - - case BMI2_E_SET_APS_FAIL: - printf( - "Error [%d] : APS failure error. It occurs due to failure in write of advance power mode configuration " "register\r\n", - rslt); - break; - - case BMI2_E_AUX_INVALID_CFG: - printf( - "Error [%d] : Invalid AUX configuration error. It occurs when the auxiliary interface settings are not " "enabled properly\r\n", - rslt); - break; - - case BMI2_E_AUX_BUSY: - printf( - "Error [%d] : AUX busy error. It occurs when the auxiliary interface buses are engaged while configuring" " the AUX\r\n", - rslt); - break; - - case BMI2_E_REMAP_ERROR: - printf( - "Error [%d] : Remap error. It occurs due to failure in assigning the remap axes data for all the axes " "after change in axis position\r\n", - rslt); - break; - - case BMI2_E_GYR_USER_GAIN_UPD_FAIL: - printf( - "Error [%d] : Gyro user gain update fail error. It occurs when the reading of user gain update status " "fails\r\n", - rslt); - break; - - case BMI2_E_SELF_TEST_NOT_DONE: - printf( - "Error [%d] : Self-test not done error. It occurs when the self-test process is ongoing or not " "completed\r\n", - rslt); - break; - - case BMI2_E_INVALID_INPUT: - printf("Error [%d] : Invalid input error. It occurs when the sensor input validity fails\r\n", rslt); - break; - - case BMI2_E_INVALID_STATUS: - printf("Error [%d] : Invalid status error. It occurs when the feature/sensor validity fails\r\n", rslt); - break; - - case BMI2_E_CRT_ERROR: - printf("Error [%d] : CRT error. It occurs when the CRT test has failed\r\n", rslt); - break; - - case BMI2_E_ST_ALREADY_RUNNING: - printf( - "Error [%d] : Self-test already running error. It occurs when the self-test is already running and " "another has been initiated\r\n", - rslt); - break; - - case BMI2_E_CRT_READY_FOR_DL_FAIL_ABORT: - printf( - "Error [%d] : CRT ready for download fail abort error. It occurs when download in CRT fails due to wrong " "address location\r\n", - rslt); - break; - - case BMI2_E_DL_ERROR: - printf( - "Error [%d] : Download error. It occurs when write length exceeds that of the maximum burst length\r\n", - rslt); - break; - - case BMI2_E_PRECON_ERROR: - printf( - "Error [%d] : Pre-conditional error. It occurs when precondition to start the feature was not " "completed\r\n", - rslt); - break; - - case BMI2_E_ABORT_ERROR: - printf("Error [%d] : Abort error. It occurs when the device was shaken during CRT test\r\n", rslt); - break; - - case BMI2_E_WRITE_CYCLE_ONGOING: - printf( - "Error [%d] : Write cycle ongoing error. It occurs when the write cycle is already running and another " "has been initiated\r\n", - rslt); - break; - - case BMI2_E_ST_NOT_RUNING: - printf( - "Error [%d] : Self-test is not running error. It occurs when self-test running is disabled while it's " "running\r\n", - rslt); - break; - - case BMI2_E_DATA_RDY_INT_FAILED: - printf( - "Error [%d] : Data ready interrupt error. It occurs when the sample count exceeds the FOC sample limit " "and data ready status is not updated\r\n", - rslt); - break; - - case BMI2_E_INVALID_FOC_POSITION: - printf( - "Error [%d] : Invalid FOC position error. It occurs when average FOC data is obtained for the wrong" " axes\r\n", - rslt); - break; - - default: - printf("Error [%d] : Unknown error code\r\n", rslt); - break; - } -} - -/*! - * @brief Deinitializes coines platform - * - * @return void. - */ -void bmi2_coines_deinit(void) -{ - coines_close_comm_intf(COINES_COMM_INTF_USB); -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/common/common.h b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/common/common.h deleted file mode 100644 index 763983da4a..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/common/common.h +++ /dev/null @@ -1,122 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -#ifndef _COMMON_H -#define _COMMON_H - -/*! CPP guard */ -#ifdef __cplusplus -extern "C" { -#endif - -#include -#include "bmi2.h" - -/*! - * @brief Function for reading the sensor's registers through I2C bus. - * - * @param[in] reg_addr : Register address. - * @param[out] reg_data : Pointer to the data buffer to store the read data. - * @param[in] length : No of bytes to read. - * @param[in] intf_ptr : Interface pointer - * - * @return Status of execution - * @retval = BMI2_INTF_RET_SUCCESS -> Success - * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info - * - */ -BMI2_INTF_RETURN_TYPE bmi2_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr); - -/*! - * @brief Function for writing the sensor's registers through I2C bus. - * - * @param[in] reg_addr : Register address. - * @param[in] reg_data : Pointer to the data buffer whose value is to be written. - * @param[in] length : No of bytes to write. - * @param[in] intf_ptr : Interface pointer - * - * @return Status of execution - * @retval = BMI2_INTF_RET_SUCCESS -> Success - * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info - * - */ -BMI2_INTF_RETURN_TYPE bmi2_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr); - -/*! - * @brief Function for reading the sensor's registers through SPI bus. - * - * @param[in] reg_addr : Register address. - * @param[out] reg_data : Pointer to the data buffer to store the read data. - * @param[in] length : No of bytes to read. - * @param[in] intf_ptr : Interface pointer - * - * @return Status of execution - * @retval = BMI2_INTF_RET_SUCCESS -> Success - * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info - * - */ -BMI2_INTF_RETURN_TYPE bmi2_spi_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr); - -/*! - * @brief Function for writing the sensor's registers through SPI bus. - * - * @param[in] reg_addr : Register address. - * @param[in] reg_data : Pointer to the data buffer whose data has to be written. - * @param[in] length : No of bytes to write. - * @param[in] intf_ptr : Interface pointer - * - * @return Status of execution - * @retval = BMI2_INTF_RET_SUCCESS -> Success - * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info - * - */ -BMI2_INTF_RETURN_TYPE bmi2_spi_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr); - -/*! - * @brief This function provides the delay for required time (Microsecond) as per the input provided in some of the - * APIs. - * - * @param[in] period_us : The required wait time in microsecond. - * @param[in] intf_ptr : Interface pointer - * - * @return void. - * - */ -void bmi2_delay_us(uint32_t period, void *intf_ptr); - -/*! - * @brief Function to select the interface between SPI and I2C. - * - * @param[in] bma : Structure instance of bmi2_dev - * @param[in] intf : Interface selection parameter - * - * @return Status of execution - * @retval 0 -> Success - * @retval < 0 -> Failure Info - */ -int8_t bmi2_interface_init(struct bmi2_dev *bma, uint8_t intf); - -/*! - * @brief Prints the execution status of the APIs. - * - * @param[in] rslt : Error code returned by the API whose execution status has to be printed. - * - * @return void. - */ -void bmi2_error_codes_print_result(int8_t rslt); - -/*! - * @brief Deinitializes coines platform - * - * @return void. - */ -void bmi2_coines_deinit(void); - -#ifdef __cplusplus -} -#endif /* End of CPP guard */ - -#endif /* _COMMON_H */ diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/component_retrim/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/component_retrim/Makefile deleted file mode 100644 index 633a817299..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/component_retrim/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= component_retrim.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270_hc.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/component_retrim/component_retrim.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/component_retrim/component_retrim.c deleted file mode 100644 index 6084574f4a..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/component_retrim/component_retrim.c +++ /dev/null @@ -1,52 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270_hc.h" -#include "common.h" - -/******************************************************************************/ -/*! Functions */ - -/* This function starts the execution of program. */ -int main(void) -{ - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270_hc. */ - rslt = bmi270_hc_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* This API runs the CRT process. */ - rslt = bmi2_do_crt(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Do not move the board while doing CRT. If so, it will throw an abort error. */ - if (rslt == BMI2_OK) - { - printf("CRT successfully completed."); - } - } - - bmi2_coines_deinit(); - - return rslt; -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_full_header_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_full_header_mode/Makefile deleted file mode 100644 index 00794a37a0..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_full_header_mode/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= fifo_full_header_mode.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270_hc.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_full_header_mode/fifo_full_header_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_full_header_mode/fifo_full_header_mode.c deleted file mode 100644 index 6f11642843..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_full_header_mode/fifo_full_header_mode.c +++ /dev/null @@ -1,316 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270_hc.h" -#include "common.h" - -/******************************************************************************/ -/*! Macros */ - -/*! Buffer size allocated to store raw FIFO data. */ -#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) - -/*! Length of data to be read from FIFO. */ -#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) - -/*! Number of accel frames to be extracted from FIFO. */ - -/*! Calculation for frame count: Total frame count = Fifo buffer size(2048)/ Total frames(6 Accel, 6 Gyro and 1 header, - * totaling to 13) which equals to 157. - */ -#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(157) - -/*! Number of gyro frames to be extracted from FIFO. */ -#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(157) - -/*! Macro to read sensortime byte in FIFO. */ -#define SENSORTIME_OVERHEAD_BYTE UINT8_C(3) - -/******************************************************************************/ -/*! Static Function Declaration */ - -/*! - * @brief This internal API is used to set configurations for accel and gyro. - * @param[in] dev : Structure instance of bmi2_dev. - * @return Status of execution. - */ -static int8_t set_accel_gyro_config(struct bmi2_dev *dev); - -/******************************************************************************/ - -/* This function starts the execution of program. */ -int main(void) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - uint16_t index = 0; - uint16_t fifo_length = 0; - uint16_t config = 0; - - /* To read sensortime, extra 3 bytes are added to fifo buffer. */ - uint16_t fifo_buffer_size = BMI2_FIFO_RAW_DATA_BUFFER_SIZE + SENSORTIME_OVERHEAD_BYTE; - - /* Variable to get fifo full interrupt status. */ - uint16_t int_status = 0; - - uint16_t accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; - - uint16_t gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; - - /* Variable to store the available frame length count in FIFO */ - uint8_t frame_count; - - int8_t try = 1; - - /* Number of bytes of FIFO data. */ - uint8_t fifo_data[fifo_buffer_size]; - - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Array of accelerometer frames -> Total bytes = - * 157 * (6 axes + 1 header bytes) = 1099 bytes */ - struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; - - /* Array of gyro frames -> Total bytes = - * 157 * (6 axes + 1 header bytes) = 1099 bytes */ - struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; - - /* Initialize FIFO frame structure. */ - struct bmi2_fifo_frame fifoframe = { 0 }; - - /* Accel and gyro sensor are listed in array. */ - uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270_hc. */ - rslt = bmi270_hc_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Enable accel and gyro sensor. */ - rslt = bmi270_hc_sensor_enable(sensor_sel, 2, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Configuration settings for accel and gyro. */ - rslt = set_accel_gyro_config(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Before setting FIFO, disable the advance power save mode. */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Initially disable all configurations in fifo. */ - rslt = bmi2_get_fifo_config(&config, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Initially disable all configurations in fifo. */ - rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Update FIFO structure. */ - /* Mapping the buffer to store the fifo data. */ - fifoframe.data = fifo_data; - - /* Length of FIFO frame. */ - /* To read sensortime, extra 3 bytes are added to fifo user length. */ - fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH + SENSORTIME_OVERHEAD_BYTE; - - /* Set FIFO configuration by enabling accel, gyro and timestamp. - * NOTE 1: The header mode is enabled by default. - * NOTE 2: By default the FIFO operating mode is in FIFO mode. - * NOTE 3: Sensortime is enabled by default */ - printf("FIFO is configured in header mode\n"); - rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Map FIFO full interrupt. */ - fifoframe.data_int_map = BMI2_FFULL_INT; - rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - while (try <= 10) - { - /* Read FIFO data on interrupt. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if ((rslt == BMI2_OK) && (int_status & BMI2_FFULL_INT_STATUS_MASK)) - { - printf("\nIteration : %d\n", try); - - accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; - - gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; - - rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - printf("\nFIFO data bytes available : %d \n", fifo_length); - printf("\nFIFO data bytes requested : %d \n", fifoframe.length); - - /* Read FIFO data. */ - rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Read FIFO data on interrupt. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - printf("\nFIFO accel frames requested : %d \n", accel_frame_length); - - /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ - rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); - printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); - - printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); - - /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ - rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); - printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); - - /* Calculating the frame count from the available bytes in FIFO - * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len + header_byte)) */ - frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH + 1)); - printf("\nAvailable frame count from available bytes: %d\n", frame_count); - - printf("\nExtracted accel frames\n"); - - if (accel_frame_length > frame_count) - { - accel_frame_length = frame_count; - } - - /* Print the parsed accelerometer data from the FIFO buffer. */ - for (index = 0; index < accel_frame_length; index++) - { - printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, - fifo_accel_data[index].y, fifo_accel_data[index].z); - } - - printf("\nExtracted gyro frames\n"); - - if (gyro_frame_length > frame_count) - { - gyro_frame_length = frame_count; - } - - /* Print the parsed gyro data from the FIFO buffer. */ - for (index = 0; index < gyro_frame_length; index++) - { - printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", - index, - fifo_gyro_data[index].x, - fifo_gyro_data[index].y, - fifo_gyro_data[index].z); - } - - /* Print control frames like sensor time and skipped frame count. */ - printf("\nSkipped frame count = %d\n", fifoframe.skipped_frame_count); - - printf("Sensor time = %lu\r\n", fifoframe.sensor_time); - - try++; - } - } - } - - bmi2_coines_deinit(); - - return rslt; -} - -/*! - * @brief This internal API is used to set configurations for accel and gyro. - */ -static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Structure to define accel and gyro configurations. */ - struct bmi2_sens_config config[2]; - - /* Configure the type of feature. */ - config[0].type = BMI2_ACCEL; - config[1].type = BMI2_GYRO; - - /* Get default configurations for the type of feature selected. */ - rslt = bmi270_hc_get_sensor_config(config, 2, bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - - /* NOTE: The user can change the following configuration parameters according to their requirement. */ - /* Accel configuration settings. */ - /* Set Output Data Rate */ - config[0].cfg.acc.odr = BMI2_ACC_ODR_25HZ; - - /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ - config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; - - /* The bandwidth parameter is used to configure the number of sensor samples that are averaged - * if it is set to 2, then 2^(bandwidth parameter) samples - * are averaged, resulting in 4 averaged samples - * Note1 : For more information, refer the datasheet. - * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but - * this has an adverse effect on the power consumed. - */ - config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; - - /* Enable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - * For more info refer datasheet. - */ - config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; - - /* Gyro configuration settings. */ - /* Set Output Data Rate */ - config[1].cfg.gyr.odr = BMI2_GYR_ODR_25HZ; - - /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ - config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; - - /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ - config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; - - /* Enable/Disable the noise performance mode for precision yaw rate sensing - * There are two modes - * 0 -> Ultra low power mode(Default) - * 1 -> High performance mode - */ - config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; - - /* Enable/Disable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - */ - config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; - - /* Set new configurations. */ - rslt = bmi270_hc_set_sensor_config(config, 2, bmi2_dev); - bmi2_error_codes_print_result(rslt); - } - - return rslt; -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_full_headerless_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_full_headerless_mode/Makefile deleted file mode 100644 index 5eefc71ab8..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_full_headerless_mode/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= fifo_full_headerless_mode.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270_hc.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_full_headerless_mode/fifo_full_headerless_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_full_headerless_mode/fifo_full_headerless_mode.c deleted file mode 100644 index 1bce488be0..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_full_headerless_mode/fifo_full_headerless_mode.c +++ /dev/null @@ -1,304 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270_hc.h" -#include "common.h" - -/******************************************************************************/ -/*! Macros */ - -/*! Buffer size allocated to store raw FIFO data */ -#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) - -/*! Length of data to be read from FIFO */ -#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) - -/*! Number of accel frames to be extracted from FIFO */ - -/*! Calculation for frame count: Total frame count = Fifo buffer size(2048)/ Total frames(6 Accel, 6 Gyro totaling to - * 12) which equals to 170. - */ -#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(169) - -/*! Number of gyro frames to be extracted from FIFO */ -#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(169) - -/******************************************************************************/ -/*! Static Function Declaration */ - -/*! - * @brief This internal API is used to set configurations for accel and gyro. - * @param[in] dev : Structure instance of bmi2_dev. - * @return Status of execution. - */ -static int8_t set_accel_gyro_config(struct bmi2_dev *dev); - -/******************************************************************************/ - -/* This function starts the execution of program. */ -int main(void) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - uint16_t index = 0; - - uint16_t fifo_length = 0; - - uint8_t try = 1; - - /* Variable to get fifo full interrupt status. */ - uint16_t int_status = 0; - - uint16_t accel_frame_length; - - uint16_t gyro_frame_length; - - /* Variable to store the available frame length count in FIFO */ - uint8_t frame_count; - - /* Number of bytes of FIFO data. */ - uint8_t fifo_data[BMI2_FIFO_RAW_DATA_BUFFER_SIZE] = { 0 }; - - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Array of accelerometer frames -> Total bytes = - * 170 * (6 axes bytes) = 1020 bytes */ - struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; - - /* Array of gyro frames -> Total bytes = - * 170 * (6 axes bytes) = 1020 bytes */ - struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; - - /* Initialize FIFO frame structure. */ - struct bmi2_fifo_frame fifoframe = { 0 }; - - /* Accel and gyro sensor are listed in array. */ - uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270_hc. */ - rslt = bmi270_hc_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Enable accel and gyro sensor. */ - rslt = bmi270_hc_sensor_enable(sensor_sel, 2, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Configuration settings for accel and gyro. */ - rslt = set_accel_gyro_config(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Before setting FIFO, disable the advance power save mode. */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Initially disable all configurations in fifo. */ - rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Update FIFO structure. */ - /* Mapping the buffer to store the fifo data. */ - fifoframe.data = fifo_data; - - /* Length of FIFO frame. */ - fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH; - - /* Set FIFO configuration by enabling accel, gyro. - * NOTE 1: The header mode is enabled by default. - * NOTE 2: By default the FIFO operating mode is FIFO mode. */ - rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - printf("FIFO is configured in headerless mode\n"); - - /* To enable headerless mode, disable the header. */ - if (rslt == BMI2_OK) - { - rslt = bmi2_set_fifo_config(BMI2_FIFO_HEADER_EN, BMI2_DISABLE, &bmi2_dev); - } - - /* Map FIFO full interrupt. */ - fifoframe.data_int_map = BMI2_FFULL_INT; - rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - while (try <= 10) - { - /* Read FIFO data on interrupt. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if ((rslt == BMI2_OK) && (int_status & BMI2_FFULL_INT_STATUS_MASK)) - { - printf("\nIteration : %d\n", try); - - rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; - gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; - - printf("\nFIFO data bytes available : %d \n", fifo_length); - printf("\nFIFO data bytes requested : %d \n", fifoframe.length); - - /* Read FIFO data. */ - rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Read FIFO data on interrupt. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - printf("\nFIFO accel frames requested : %d \n", accel_frame_length); - - /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ - rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); - printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); - - printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); - - /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ - rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); - printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); - - /* Calculating the frame count from the available bytes in FIFO - * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len)) */ - frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH)); - printf("\nAvailable frame count from available bytes: %d\n", frame_count); - - printf("\nExtracted accel frames\n"); - - if (accel_frame_length > frame_count) - { - accel_frame_length = frame_count; - } - - /* Print the parsed accelerometer data from the FIFO buffer. */ - for (index = 0; index < accel_frame_length; index++) - { - printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, - fifo_accel_data[index].y, fifo_accel_data[index].z); - } - - printf("\nExtracted gyro frames\n"); - - if (gyro_frame_length > frame_count) - { - gyro_frame_length = frame_count; - } - - /* Print the parsed gyro data from the FIFO buffer. */ - for (index = 0; index < gyro_frame_length; index++) - { - printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", - index, - fifo_gyro_data[index].x, - fifo_gyro_data[index].y, - fifo_gyro_data[index].z); - } - } - - try++; - } - } - - bmi2_coines_deinit(); - - return rslt; -} - -/*! - * @brief This internal API is used to set configurations for accel and gyro. - */ -static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Structure to define accel and gyro configurations. */ - struct bmi2_sens_config config[2]; - - /* Configure the type of feature. */ - config[0].type = BMI2_ACCEL; - config[1].type = BMI2_GYRO; - - /* Get default configurations for the type of feature selected. */ - rslt = bmi270_hc_get_sensor_config(config, 2, bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* NOTE: The user can change the following configuration parameters according to their requirement. */ - /* Accel configuration settings. */ - /* Set Output Data Rate */ - config[0].cfg.acc.odr = BMI2_ACC_ODR_100HZ; - - /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ - config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; - - /* The bandwidth parameter is used to configure the number of sensor samples that are averaged - * if it is set to 2, then 2^(bandwidth parameter) samples - * are averaged, resulting in 4 averaged samples - * Note1 : For more information, refer the datasheet. - * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but - * this has an adverse effect on the power consumed. - */ - config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; - - /* Enable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - * For more info refer datasheet. - */ - config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; - - /* Gyro configuration settings. */ - /* Set Output Data Rate */ - config[1].cfg.gyr.odr = BMI2_GYR_ODR_100HZ; - - /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ - config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; - - /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ - config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; - - /* Enable/Disable the noise performance mode for precision yaw rate sensing - * There are two modes - * 0 -> Ultra low power mode(Default) - * 1 -> High performance mode - */ - config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; - - /* Enable/Disable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - */ - config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; - - /* Set new configurations. */ - rslt = bmi270_hc_set_sensor_config(config, 2, bmi2_dev); - bmi2_error_codes_print_result(rslt); - } - - return rslt; -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_watermark_header_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_watermark_header_mode/Makefile deleted file mode 100644 index 8dea084da5..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_watermark_header_mode/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= fifo_watermark_header_mode.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270_hc.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_watermark_header_mode/fifo_watermark_header_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_watermark_header_mode/fifo_watermark_header_mode.c deleted file mode 100644 index 46599f21cd..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_watermark_header_mode/fifo_watermark_header_mode.c +++ /dev/null @@ -1,335 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270_hc.h" -#include "common.h" - -/******************************************************************************/ -/*! Macros */ - -/*! Buffer size allocated to store raw FIFO data */ -#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) - -/*! Length of data to be read from FIFO */ -#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) - -/*! Number of accel frames to be extracted from FIFO */ - -/*! - * Calculation: - * fifo_watermark_level = 650, accel_frame_len = 6, gyro_frame_len = 6 header_byte = 1. - * fifo_accel_frame_count = (650 / (6 + 6 + 1 )) = 50 frames - * NOTE: Extra frames are read in order to get sensor time - */ -#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(55) - -/*! Number of gyro frames to be extracted from FIFO */ -#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(55) - -/*! Setting a watermark level in FIFO */ -#define BMI270_FIFO_WATERMARK_LEVEL UINT16_C(650) - -/*! Macro to read sensortime byte in FIFO */ -#define SENSORTIME_OVERHEAD_BYTE UINT8_C(3) - -/******************************************************************************/ -/*! Static Function Declaration */ - -/*! - * @brief This internal API is used to set configurations for accel and gyro. - * @param[in] dev : Structure instance of bmi2_dev. - * @return Status of execution. - */ -static int8_t set_accel_gyro_config(struct bmi2_dev *dev); - -/******************************************************************************/ -/*! Functions */ - -/* This function starts the execution of program. */ -int main(void) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Variable to store the available frame length count in FIFO */ - uint8_t frame_count; - - /* Variable to index bytes. */ - uint16_t index = 0; - - uint16_t fifo_length = 0; - - uint16_t accel_frame_length; - - uint16_t gyro_frame_length; - - uint8_t try = 1; - - /* To read sensortime, extra 3 bytes are added to fifo buffer */ - uint16_t fifo_buffer_size = BMI2_FIFO_RAW_DATA_BUFFER_SIZE + SENSORTIME_OVERHEAD_BYTE; - - /* Number of bytes of FIFO data. */ - uint8_t fifo_data[fifo_buffer_size]; - - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Array of accelerometer frames -> Total bytes = - * 55 * (6 axes bytes) = 330 bytes */ - struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; - - /* Array of gyro frames -> Total bytes = - * 55 * (6 axes bytes) = 330 bytes */ - struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; - - /* Initialize FIFO frame structure. */ - struct bmi2_fifo_frame fifoframe = { 0 }; - - /* Accel and gyro sensor are listed in array. */ - uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; - - /* Variable to get fifo water-mark interrupt status. */ - uint16_t int_status = 0; - - uint16_t watermark = 0; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270_hc. */ - rslt = bmi270_hc_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Enable accel and gyro sensor. */ - rslt = bmi270_hc_sensor_enable(sensor_sel, 2, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Configuration settings for accel and gyro. */ - rslt = set_accel_gyro_config(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Before setting FIFO, disable the advance power save mode. */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Initially disable all configurations in fifo. */ - rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Update FIFO structure. */ - /* Mapping the buffer to store the fifo data. */ - fifoframe.data = fifo_data; - - /* Length of FIFO frame. */ - /* To read sensortime, extra 3 bytes are added to fifo user length. */ - fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH + SENSORTIME_OVERHEAD_BYTE; - - /* Set FIFO configuration by enabling accel, gyro and timestamp. - * NOTE 1: The header mode is enabled by default. - * NOTE 2: By default the FIFO operating mode is FIFO mode. - * NOTE 3: Sensortime is enabled by default */ - printf("FIFO is configured in header mode\n"); - rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* FIFO water-mark interrupt is enabled. */ - fifoframe.data_int_map = BMI2_FWM_INT; - - /* Map water-mark interrupt to the required interrupt pin. */ - rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Set water-mark level. */ - fifoframe.wm_lvl = BMI270_FIFO_WATERMARK_LEVEL; - - /* Set the water-mark level if water-mark interrupt is mapped. */ - rslt = bmi2_set_fifo_wm(fifoframe.wm_lvl, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - rslt = bmi2_get_fifo_wm(&watermark, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - printf("\nFIFO watermark level is %d\n", watermark); - - while (try <= 10) - { - /* Read FIFO data on interrupt. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* To check the status of FIFO watermark interrupt. */ - if ((rslt == BMI2_OK) && (int_status & BMI2_FWM_INT_STATUS_MASK)) - { - printf("\nIteration : %d\n", try); - - printf("\nWatermark interrupt occurred\n"); - - rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; - gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; - - printf("\nFIFO data bytes available : %d \n", fifo_length); - printf("\nFIFO data bytes requested : %d \n", fifoframe.length); - - /* Read FIFO data. */ - rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Read FIFO data on interrupt. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - printf("\nFIFO accel frames requested : %d \n", accel_frame_length); - - /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ - rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); - printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); - - printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); - - /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ - rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); - printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); - - /* Calculating the frame count from the available bytes in FIFO - * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len + header_byte)) */ - frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH + 1)); - printf("\nAvailable frame count from available bytes: %d\n", frame_count); - - printf("\nExracted accel frames\n"); - - if (accel_frame_length > frame_count) - { - accel_frame_length = frame_count; - } - - /* Print the parsed accelerometer data from the FIFO buffer. */ - for (index = 0; index < accel_frame_length; index++) - { - printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, - fifo_accel_data[index].y, fifo_accel_data[index].z); - } - - printf("\nExtracted gyro frames\n"); - - if (gyro_frame_length > frame_count) - { - gyro_frame_length = frame_count; - } - - /* Print the parsed gyro data from the FIFO buffer. */ - for (index = 0; index < gyro_frame_length; index++) - { - printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", - index, - fifo_gyro_data[index].x, - fifo_gyro_data[index].y, - fifo_gyro_data[index].z); - } - - /* Print control frames like sensor time and skipped frame count. */ - printf("Skipped frame count = %d\n", fifoframe.skipped_frame_count); - printf("Sensor time = %lu\r\n", fifoframe.sensor_time); - - try++; - } - } - } - - bmi2_coines_deinit(); - - return rslt; -} - -/*! - * @brief This internal API is used to set configurations for accel and gyro. - */ -static int8_t set_accel_gyro_config(struct bmi2_dev *dev) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Structure to define accel and gyro configurations. */ - struct bmi2_sens_config config[2]; - - /* Configure the type of feature. */ - config[0].type = BMI2_ACCEL; - config[1].type = BMI2_GYRO; - - /* Get default configurations for the type of feature selected. */ - rslt = bmi270_hc_get_sensor_config(config, 2, dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* NOTE: The user can change the following configuration parameters according to their requirement. */ - /* Accel configuration settings. */ - /* Set Output Data Rate */ - config[0].cfg.acc.odr = BMI2_ACC_ODR_25HZ; - - /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ - config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; - - /* The bandwidth parameter is used to configure the number of sensor samples that are averaged - * if it is set to 2, then 2^(bandwidth parameter) samples - * are averaged, resulting in 4 averaged samples - * Note1 : For more information, refer the datasheet. - * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but - * this has an adverse effect on the power consumed. - */ - config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; - - /* Enable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - * For more info refer datasheet. - */ - config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; - - /* Gyro configuration settings. */ - /* Set Output Data Rate */ - config[1].cfg.gyr.odr = BMI2_GYR_ODR_25HZ; - - /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ - config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; - - /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ - config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; - - /* Enable/Disable the noise performance mode for precision yaw rate sensing - * There are two modes - * 0 -> Ultra low power mode(Default) - * 1 -> High performance mode - */ - config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; - - /* Enable/Disable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - */ - config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; - - /* Set new configurations. */ - rslt = bmi270_hc_set_sensor_config(config, 2, dev); - bmi2_error_codes_print_result(rslt); - } - - return rslt; -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_watermark_headerless_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_watermark_headerless_mode/Makefile deleted file mode 100644 index bd842a8170..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_watermark_headerless_mode/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= fifo_watermark_headerless_mode.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270_hc.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c deleted file mode 100644 index bb0bfd1118..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c +++ /dev/null @@ -1,331 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270_hc.h" -#include "common.h" - -/******************************************************************************/ -/*! Macros */ - -/*! Buffer size allocated to store raw FIFO data */ -#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) - -/*! Length of data to be read from FIFO */ -#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) - -/*! Number of accel frames to be extracted from FIFO */ - -/*! - * Calculation: - * fifo_watermark_level = 650, accel_frame_len = 6, gyro_frame_len = 6. - * fifo_accel_frame_count = (650 / (6 + 6 )) = 54 frames - */ -#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(55) - -/*! Number of gyro frames to be extracted from FIFO */ -#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(55) - -/*! Setting a watermark level in FIFO */ -#define BMI270_FIFO_WATERMARK_LEVEL UINT16_C(650) - -/******************************************************************************/ -/*! Static Function Declaration */ - -/*! - * @brief This internal API is used to set configurations for accel and gyro. - * @param[in] dev : Structure instance of bmi2_dev. - * @return Status of execution. - */ -static int8_t set_accel_gyro_config(struct bmi2_dev *dev); - -/******************************************************************************/ -/*! Functions */ - -/* This function starts the execution of program. */ -int main(void) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Variable to index bytes. */ - uint16_t index = 0; - - /* Variable to store the available frame length count in FIFO */ - uint8_t frame_count; - - uint16_t fifo_length = 0; - - uint16_t accel_frame_length; - - uint16_t gyro_frame_length; - - uint8_t try = 1; - - /* Number of bytes of FIFO data. */ - uint8_t fifo_data[BMI2_FIFO_RAW_DATA_BUFFER_SIZE] = { 0 }; - - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Array of accelerometer frames -> Total bytes = - * 50 * (6 axes bytes) = 300 bytes */ - struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; - - /* Array of gyro frames -> Total bytes = - * 50 * (6 axes bytes) = 300 bytes */ - struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; - - /* Initialize FIFO frame structure. */ - struct bmi2_fifo_frame fifoframe = { 0 }; - - /* Accel and gyro sensor are listed in array. */ - uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; - - /* Variable to get fifo water-mark interrupt status. */ - uint16_t int_status = 0; - - uint16_t watermark = 0; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270_hc. */ - rslt = bmi270_hc_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Enable accel and gyro sensor. */ - rslt = bmi270_hc_sensor_enable(sensor_sel, 2, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Configuration settings for accel and gyro. */ - rslt = set_accel_gyro_config(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Before setting FIFO, disable the advance power save mode. */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Initially disable all configurations in fifo. */ - rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Update FIFO structure. */ - /* Mapping the buffer to store the fifo data. */ - fifoframe.data = fifo_data; - - /* Length of FIFO frame. */ - fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH; - - /* Set FIFO configuration by enabling accel, gyro. - * NOTE 1: The header mode is enabled by default. - * NOTE 2: By default the FIFO operating mode is FIFO mode. */ - rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - printf("FIFO is configured in headerless mode\n"); - - /* To enable headerless mode, disable the header. */ - if (rslt == BMI2_OK) - { - rslt = bmi2_set_fifo_config(BMI2_FIFO_HEADER_EN, BMI2_DISABLE, &bmi2_dev); - } - - /* FIFO water-mark interrupt is enabled. */ - fifoframe.data_int_map = BMI2_FWM_INT; - - /* Map water-mark interrupt to the required interrupt pin. */ - rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Set water-mark level. */ - fifoframe.wm_lvl = BMI270_FIFO_WATERMARK_LEVEL; - - fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH; - - /* Set the water-mark level if water-mark interrupt is mapped. */ - rslt = bmi2_set_fifo_wm(fifoframe.wm_lvl, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - rslt = bmi2_get_fifo_wm(&watermark, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - printf("\nFIFO watermark level is %d\n", watermark); - - while (try <= 10) - { - /* Read FIFO data on interrupt. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* To check the status of FIFO watermark interrupt. */ - if ((rslt == BMI2_OK) && (int_status & BMI2_FWM_INT_STATUS_MASK)) - { - printf("\nIteration : %d\n", try); - - printf("\nWatermark interrupt occurred\n"); - - rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; - gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; - - printf("\nFIFO data bytes available : %d \n", fifo_length); - printf("\nFIFO data bytes requested : %d \n", fifoframe.length); - - /* Read FIFO data. */ - rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Read FIFO data on interrupt. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - printf("\nFIFO accel frames requested : %d \n", accel_frame_length); - - /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ - rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); - printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); - - printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); - - /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ - rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); - printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); - - /* Calculating the frame count from the available bytes in FIFO - * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len)) */ - frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH)); - printf("\nAvailable frame count from available bytes: %d\n", frame_count); - - printf("\nExracted accel frames\n"); - - if (accel_frame_length > frame_count) - { - accel_frame_length = frame_count; - } - - /* Print the parsed accelerometer data from the FIFO buffer. */ - for (index = 0; index < accel_frame_length; index++) - { - printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, - fifo_accel_data[index].y, fifo_accel_data[index].z); - } - - printf("\nExtracted gyro frames\n"); - - if (gyro_frame_length > frame_count) - { - gyro_frame_length = frame_count; - } - - /* Print the parsed gyro data from the FIFO buffer. */ - for (index = 0; index < gyro_frame_length; index++) - { - printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", - index, - fifo_gyro_data[index].x, - fifo_gyro_data[index].y, - fifo_gyro_data[index].z); - } - } - - try++; - } - } - - bmi2_coines_deinit(); - - return rslt; -} - -/*! - * @brief This internal API is used to set configurations for accel. - */ -static int8_t set_accel_gyro_config(struct bmi2_dev *dev) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Structure to define accel and gyro configurations. */ - struct bmi2_sens_config config[2]; - - /* Configure the type of feature. */ - config[0].type = BMI2_ACCEL; - config[1].type = BMI2_GYRO; - - /* Get default configurations for the type of feature selected. */ - rslt = bmi270_hc_get_sensor_config(config, 2, dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* NOTE: The user can change the following configuration parameter according to their requirement. */ - /* Accel configuration settings. */ - /* Set Output Data Rate */ - config[0].cfg.acc.odr = BMI2_ACC_ODR_100HZ; - - /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ - config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; - - /* The bandwidth parameter is used to configure the number of sensor samples that are averaged - * if it is set to 2, then 2^(bandwidth parameter) samples - * are averaged, resulting in 4 averaged samples - * Note1 : For more information, refer the datasheet. - * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but - * this has an adverse effect on the power consumed. - */ - config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; - - /* Enable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - * For more info refer datasheet. - */ - config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; - - /* Gyro configuration settings. */ - /* Set Output Data Rate */ - config[1].cfg.gyr.odr = BMI2_GYR_ODR_100HZ; - - /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ - config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; - - /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ - config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; - - /* Enable/Disable the noise performance mode for precision yaw rate sensing - * There are two modes - * 0 -> Ultra low power mode(Default) - * 1 -> High performance mode - */ - config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; - - /* Enable/Disable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - */ - config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; - - /* Set new configurations. */ - rslt = bmi270_hc_set_sensor_config(config, 2, dev); - bmi2_error_codes_print_result(rslt); - } - - return rslt; -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/gyro/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/gyro/Makefile deleted file mode 100644 index cb6bfaa3e1..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/gyro/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= gyro.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270_hc.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/gyro/gyro.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/gyro/gyro.c deleted file mode 100644 index c7c50277fd..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/gyro/gyro.c +++ /dev/null @@ -1,195 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270_hc.h" -#include "common.h" - -/******************************************************************************/ -/*! Static Function Declaration */ - -/*! - * @brief This internal API is used to set configurations for gyro. - * - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Status of execution. - */ -static int8_t set_gyro_config(struct bmi2_dev *dev); - -/*! - * @brief This function converts lsb to degree per second for 16 bit gyro at - * range 125, 250, 500, 1000 or 2000dps. - * - * @param[in] val : LSB from each axis. - * @param[in] dps : Degree per second. - * @param[in] bit_width : Resolution for gyro. - * - * @return Degree per second. - */ -static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width); - -/******************************************************************************/ -/*! Functions */ - -/* This function starts the execution of program. */ -int main(void) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Variable to define limit to print gyro data. */ - uint8_t limit = 10; - - uint8_t indx = 0; - - float x = 0, y = 0, z = 0; - - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Create an instance of sensor data structure. */ - struct bmi2_sensor_data sensor_data = { 0 }; - - /* Assign gyro sensor to variable. */ - uint8_t sens_list = BMI2_GYRO; - - /* Initialize the interrupt status of gyro. */ - uint16_t int_status = 0; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270_hc. */ - rslt = bmi270_hc_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Enable the selected sensors. */ - rslt = bmi270_hc_sensor_enable(&sens_list, 1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Gyro configuration settings. */ - rslt = set_gyro_config(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Select gyro sensor. */ - sensor_data.type = BMI2_GYRO; - printf("Gyro and DPS data\n"); - printf("Gyro data collected at Range 2000 dps with 16-bit resolution\n"); - - /* Loop to print gyro data when interrupt occurs. */ - while (indx <= limit) - { - /* To get the data ready interrupt status of gyro. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* To check the data ready interrupt status and print the status for 10 samples. */ - if (int_status & BMI2_GYR_DRDY_INT_MASK) - { - /* Get gyro data for x, y and z axis. */ - rslt = bmi270_hc_get_sensor_data(&sensor_data, 1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - printf("\nGyr_X = %d\t", sensor_data.sens_data.gyr.x); - printf("Gyr_Y = %d\t", sensor_data.sens_data.gyr.y); - printf("Gyr_Z = %d\r\n", sensor_data.sens_data.gyr.z); - - /* Converting lsb to degree per second for 16 bit gyro at 2000dps range. */ - x = lsb_to_dps(sensor_data.sens_data.gyr.x, 2000, bmi2_dev.resolution); - y = lsb_to_dps(sensor_data.sens_data.gyr.y, 2000, bmi2_dev.resolution); - z = lsb_to_dps(sensor_data.sens_data.gyr.z, 2000, bmi2_dev.resolution); - - /* Print the data in dps. */ - printf("Gyr_DPS_X = %4.2f, Gyr_DPS_Y = %4.2f, Gyr_DPS_Z = %4.2f\n", x, y, z); - - indx++; - } - } - } - } - - bmi2_coines_deinit(); - - return rslt; -} - -/*! - * @brief This internal API is used to set configurations for gyro. - */ -static int8_t set_gyro_config(struct bmi2_dev *dev) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Structure to define the type of sensor and its configurations. */ - struct bmi2_sens_config config; - - /* Configure the type of feature. */ - config.type = BMI2_GYRO; - - /* Get default configurations for the type of feature selected. */ - rslt = bmi270_hc_get_sensor_config(&config, 1, dev); - bmi2_error_codes_print_result(rslt); - - /* Map data ready interrupt to interrupt pin. */ - rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT2, dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* The user can change the following configuration parameters according to their requirement. */ - /* Set Output Data Rate */ - config.cfg.gyr.odr = BMI2_GYR_ODR_200HZ; - - /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ - config.cfg.gyr.range = BMI2_GYR_RANGE_2000; - - /* Gyroscope bandwidth parameters. By default the gyro bandwidth is in normal mode. */ - config.cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; - - /* Enable/Disable the noise performance mode for precision yaw rate sensing - * There are two modes - * 0 -> Ultra low power mode(Default) - * 1 -> High performance mode - */ - config.cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; - - /* Enable/Disable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - */ - config.cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; - - /* Set the gyro configurations. */ - rslt = bmi270_hc_set_sensor_config(&config, 1, dev); - } - - return rslt; -} - -/*! - * @brief This function converts lsb to degree per second for 16 bit gyro at - * range 125, 250, 500, 1000 or 2000dps. - */ -static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width) -{ - float half_scale = ((float)(1 << bit_width) / 2.0f); - - return (dps / ((half_scale) + BMI2_GYR_RANGE_2000)) * (val); -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/step_counter/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/step_counter/Makefile deleted file mode 100644 index fc47b9bb1d..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/step_counter/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= step_counter.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270_hc.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/step_counter/step_counter.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/step_counter/step_counter.c deleted file mode 100644 index 785dc28ffc..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_hc/step_counter/step_counter.c +++ /dev/null @@ -1,146 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270_hc.h" -#include "common.h" - -/******************************************************************************/ -/*! Static function declaration */ - -/*! - * @brief This internal API is used to set configurations for step counter. - * - * @param[in] bmi2_dev : Structure instance of bmi2_dev. - * - * @return Status of execution. - */ -static int8_t set_feature_config(struct bmi2_dev *bmi2_dev); - -/******************************************************************************/ -/*! Functions */ - -/* This function starts the execution of program. */ -int main(void) -{ - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Structure to define type of sensor and their respective data. */ - struct bmi2_sensor_data sensor_data; - - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Accel sensor and step counter feature are listed in array. */ - uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_STEP_COUNTER }; - - /* Variable to get step counter interrupt status. */ - uint16_t int_status = 0; - - /* Select features and their pins to be mapped to. */ - struct bmi2_sens_int_config sens_int = { .type = BMI2_STEP_COUNTER, .hw_int_pin = BMI2_INT2 }; - - /* Type of sensor to get step counter data. */ - sensor_data.type = BMI2_STEP_COUNTER; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270_hc. */ - rslt = bmi270_hc_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Enable the selected sensor. */ - rslt = bmi270_hc_sensor_enable(sensor_sel, 2, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Set the feature configuration for step counter. */ - rslt = set_feature_config(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - printf("Step counter watermark level set to 1 (20 steps)\n"); - - if (rslt == BMI2_OK) - { - /* Map the step counter feature interrupt. */ - rslt = bmi270_hc_map_feat_int(&sens_int, 1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Move the board in steps for 20 times to get step counter interrupt. */ - printf("Move the board in steps\n"); - - /* Loop to get number of steps counted. */ - do - { - /* To get the interrupt status of the step counter. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* To check the interrupt status of the step counter. */ - if (int_status & BMI270_HC_STEP_CNT_STATUS_MASK) - { - printf("Step counter interrupt occurred when watermark level (20 steps) is reached\n"); - - /* Get step counter output. */ - rslt = bmi270_hc_get_sensor_data(&sensor_data, 1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Print the step counter output. */ - printf("No of steps counted = %ld", sensor_data.sens_data.step_counter_output); - break; - } - } while (rslt == BMI2_OK); - } - } - } - - bmi2_coines_deinit(); - - return rslt; -} - -/*! - * @brief This internal API is used to set configurations for step counter. - */ -static int8_t set_feature_config(struct bmi2_dev *bmi2_dev) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Structure to define the type of sensor and its configurations. */ - struct bmi2_sens_config config; - - /* Configure the type of sensor. */ - config.type = BMI2_STEP_COUNTER; - - /* Get default configurations for the type of feature selected. */ - rslt = bmi270_hc_get_sensor_config(&config, 1, bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Setting water-mark level to 1 for step counter to get interrupt after 20 step counts. Every 20 steps once - * output triggers. */ - config.cfg.step_counter.watermark_level = 1; - - /* Set new configuration. */ - rslt = bmi270_hc_set_sensor_config(&config, 1, bmi2_dev); - bmi2_error_codes_print_result(rslt); - } - - return rslt; -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/accel/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/accel/Makefile deleted file mode 100644 index e4400a45ba..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/accel/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= accel.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270_maximum_fifo.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/accel/accel.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/accel/accel.c deleted file mode 100644 index c32a43a41f..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/accel/accel.c +++ /dev/null @@ -1,200 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270_maximum_fifo.h" -#include "common.h" - -/******************************************************************************/ -/*! Macro definition */ - -/*! Earth's gravity in m/s^2 */ -#define GRAVITY_EARTH (9.80665f) - -/******************************************************************************/ -/*! Static Function Declaration */ - -/*! - * @brief This internal API is used to set configurations for accel. - * - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Status of execution. - */ -static int8_t set_accel_config(struct bmi2_dev *bmi2_dev); - -/*! - * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at - * range 2G, 4G, 8G or 16G. - * - * @param[in] val : LSB from each axis. - * @param[in] g_range : Gravity range. - * @param[in] bit_width : Resolution for accel. - * - * @return Gravity. - */ -static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width); - -/******************************************************************************/ -/*! Functions */ - -/* This function starts the execution of program. */ -int main(void) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Variable to define limit to print accel data. */ - uint8_t limit = 20; - - /* Assign accel sensor to variable. */ - uint8_t sensor_list = BMI2_ACCEL; - - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Create an instance of sensor data structure. */ - struct bmi2_sensor_data sensor_data = { 0 }; - - /* Initialize the interrupt status of accel. */ - uint16_t int_status = 0; - - uint8_t indx = 0; - float x = 0, y = 0, z = 0; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270_maximum_fifo. */ - rslt = bmi270_maximum_fifo_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Enable the accel sensor. */ - rslt = bmi2_sensor_enable(&sensor_list, 1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Accel configuration settings. */ - rslt = set_accel_config(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Assign accel sensor. */ - sensor_data.type = BMI2_ACCEL; - printf("Accel and m/s2 data \n"); - printf("Accel data collected at 2G Range with 16-bit resolution\n"); - - /* Loop to print the accel data when interrupt occurs. */ - while (indx <= limit) - { - /* To get the status of accel data ready interrupt. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* To check the accel data ready interrupt status and print the status for 10 samples. */ - if (int_status & BMI2_ACC_DRDY_INT_MASK) - { - /* Get accelerometer data for x, y and z axis. */ - rslt = bmi2_get_sensor_data(&sensor_data, 1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - printf("\nAcc_X = %d\t", sensor_data.sens_data.acc.x); - printf("Acc_Y = %d\t", sensor_data.sens_data.acc.y); - printf("Acc_Z = %d\r\n", sensor_data.sens_data.acc.z); - - /* Converting lsb to meter per second squared for 16 bit accelerometer at 2G range. */ - x = lsb_to_mps2(sensor_data.sens_data.acc.x, 2, bmi2_dev.resolution); - y = lsb_to_mps2(sensor_data.sens_data.acc.y, 2, bmi2_dev.resolution); - z = lsb_to_mps2(sensor_data.sens_data.acc.z, 2, bmi2_dev.resolution); - - /* Print the data in m/s2. */ - printf("\nAcc_ms2_X = %4.2f, Acc_ms2_Y = %4.2f, Acc_ms2_Z = %4.2f\n", x, y, z); - - indx++; - } - } - } - } - - bmi2_coines_deinit(); - - return rslt; -} - -/*! - * @brief This internal API is used to set configurations for accel. - */ -static int8_t set_accel_config(struct bmi2_dev *bmi2_dev) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Structure to define accelerometer configuration. */ - struct bmi2_sens_config config; - - /* Configure the type of feature. */ - config.type = BMI2_ACCEL; - - /* Get default configurations for the type of feature selected. */ - rslt = bmi2_get_sensor_config(&config, 1, bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* NOTE: The user can change the following configuration parameters according to their requirement. */ - /* Set Output Data Rate */ - config.cfg.acc.odr = BMI2_ACC_ODR_200HZ; - - /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ - config.cfg.acc.range = BMI2_ACC_RANGE_2G; - - /* The bandwidth parameter is used to configure the number of sensor samples that are averaged - * if it is set to 2, then 2^(bandwidth parameter) samples - * are averaged, resulting in 4 averaged samples. - * Note1 : For more information, refer the datasheet. - * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but - * this has an adverse effect on the power consumed. - */ - config.cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; - - /* Enable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - * For more info refer datasheet. - */ - config.cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; - - /* Set the accel configurations. */ - rslt = bmi2_set_sensor_config(&config, 1, bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Map data ready interrupt to interrupt pin. */ - rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT1, bmi2_dev); - bmi2_error_codes_print_result(rslt); - } - - return rslt; -} - -/*! - * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at - * range 2G, 4G, 8G or 16G. - */ -static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width) -{ - float half_scale = ((float)(1 << bit_width) / 2.0f); - - return (GRAVITY_EARTH * val * g_range) / half_scale; -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/accel_gyro/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/accel_gyro/Makefile deleted file mode 100644 index a33f36617a..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/accel_gyro/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= accel_gyro.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270_maximum_fifo.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/accel_gyro/accel_gyro.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/accel_gyro/accel_gyro.c deleted file mode 100644 index b77ea1d810..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/accel_gyro/accel_gyro.c +++ /dev/null @@ -1,268 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270_maximum_fifo.h" -#include "common.h" - -/******************************************************************************/ -/*! Macro definition */ - -/*! Earth's gravity in m/s^2 */ -#define GRAVITY_EARTH (9.80665f) - -/*! Macros to select the sensors */ -#define ACCEL UINT8_C(0x00) -#define GYRO UINT8_C(0x01) - -/******************************************************************************/ -/*! Static Function Declaration */ - -/*! - * @brief This internal API is used to set configurations for accel. - * - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Status of execution. - */ -static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev); - -/*! - * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at - * range 2G, 4G, 8G or 16G. - * - * @param[in] val : LSB from each axis. - * @param[in] g_range : Gravity range. - * @param[in] bit_width : Resolution for accel. - * - * @return Gravity. - */ -static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width); - -/*! - * @brief This function converts lsb to degree per second for 16 bit gyro at - * range 125, 250, 500, 1000 or 2000dps. - * - * @param[in] val : LSB from each axis. - * @param[in] dps : Degree per second. - * @param[in] bit_width : Resolution for gyro. - * - * @return Degree per second. - */ -static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width); - -/******************************************************************************/ -/*! Functions */ - -/* This function starts the execution of program. */ -int main(void) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Variable to define limit to print accel data. */ - uint8_t limit = 10; - - /* Assign accel and gyro sensor to variable. */ - uint8_t sensor_list[2] = { BMI2_ACCEL, BMI2_GYRO }; - - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Create an instance of sensor data structure. */ - struct bmi2_sensor_data sensor_data[2] = { { 0 } }; - - /* Initialize the interrupt status of accel and gyro. */ - uint16_t int_status = 0; - - uint8_t indx = 1; - - float x = 0, y = 0, z = 0; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270_maximum_fifo. */ - rslt = bmi270_maximum_fifo_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Enable the accel and gyro sensor. */ - rslt = bmi2_sensor_enable(sensor_list, 2, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Accel and gyro configuration settings. */ - rslt = set_accel_gyro_config(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Assign accel and gyro sensor. */ - sensor_data[ACCEL].type = BMI2_ACCEL; - sensor_data[GYRO].type = BMI2_GYRO; - - /* Loop to print accel and gyro data when interrupt occurs. */ - while (indx <= limit) - { - /* To get the data ready interrupt status of accel and gyro. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* To check the data ready interrupt status and print the status for 10 samples. */ - if ((int_status & BMI2_ACC_DRDY_INT_MASK) && (int_status & BMI2_GYR_DRDY_INT_MASK)) - { - /* Get accel and gyro data for x, y and z axis. */ - rslt = bmi2_get_sensor_data(sensor_data, 2, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - printf("\n******* Accel(Raw and m/s2) Gyro(Raw and dps) data : %d *******\n", indx); - - printf("\nAcc_x = %d\t", sensor_data[ACCEL].sens_data.acc.x); - printf("Acc_y = %d\t", sensor_data[ACCEL].sens_data.acc.y); - printf("Acc_z = %d", sensor_data[ACCEL].sens_data.acc.z); - - /* Converting lsb to meter per second squared for 16 bit accelerometer at 2G range. */ - x = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.x, 2, bmi2_dev.resolution); - y = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.y, 2, bmi2_dev.resolution); - z = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.z, 2, bmi2_dev.resolution); - - /* Print the data in m/s2. */ - printf("\nAcc_ms2_X = %4.2f, Acc_ms2_Y = %4.2f, Acc_ms2_Z = %4.2f\n", x, y, z); - - printf("\nGyr_X = %d\t", sensor_data[GYRO].sens_data.gyr.x); - printf("Gyr_Y = %d\t", sensor_data[GYRO].sens_data.gyr.y); - printf("Gyr_Z= %d\n", sensor_data[GYRO].sens_data.gyr.z); - - /* Converting lsb to degree per second for 16 bit gyro at 2000dps range. */ - x = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.x, 2000, bmi2_dev.resolution); - y = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.y, 2000, bmi2_dev.resolution); - z = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.z, 2000, bmi2_dev.resolution); - - /* Print the data in dps. */ - printf("Gyro_DPS_X = %4.2f, Gyro_DPS_Y = %4.2f, Gyro_DPS_Z = %4.2f\n", x, y, z); - - indx++; - } - } - } - } - - bmi2_coines_deinit(); - - return rslt; -} - -/*! - * @brief This internal API is used to set configurations for accel and gyro. - */ -static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Structure to define accelerometer and gyro configuration. */ - struct bmi2_sens_config config[2]; - - /* Configure the type of feature. */ - config[ACCEL].type = BMI2_ACCEL; - config[GYRO].type = BMI2_GYRO; - - /* Get default configurations for the type of feature selected. */ - rslt = bmi2_get_sensor_config(config, 2, bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Map data ready interrupt to interrupt pin. */ - rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT1, bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* NOTE: The user can change the following configuration parameters according to their requirement. */ - /* Set Output Data Rate */ - config[ACCEL].cfg.acc.odr = BMI2_ACC_ODR_200HZ; - - /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ - config[ACCEL].cfg.acc.range = BMI2_ACC_RANGE_2G; - - /* The bandwidth parameter is used to configure the number of sensor samples that are averaged - * if it is set to 2, then 2^(bandwidth parameter) samples - * are averaged, resulting in 4 averaged samples. - * Note1 : For more information, refer the datasheet. - * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but - * this has an adverse effect on the power consumed. - */ - config[ACCEL].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; - - /* Enable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - * For more info refer datasheet. - */ - config[ACCEL].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; - - /* The user can change the following configuration parameters according to their requirement. */ - /* Set Output Data Rate */ - config[GYRO].cfg.gyr.odr = BMI2_GYR_ODR_200HZ; - - /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ - config[GYRO].cfg.gyr.range = BMI2_GYR_RANGE_2000; - - /* Gyroscope bandwidth parameters. By default the gyro bandwidth is in normal mode. */ - config[GYRO].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; - - /* Enable/Disable the noise performance mode for precision yaw rate sensing - * There are two modes - * 0 -> Ultra low power mode(Default) - * 1 -> High performance mode - */ - config[GYRO].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; - - /* Enable/Disable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - */ - config[GYRO].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; - - /* Set the accel and gyro configurations. */ - rslt = bmi2_set_sensor_config(config, 2, bmi2_dev); - bmi2_error_codes_print_result(rslt); - } - - return rslt; -} - -/*! - * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at - * range 2G, 4G, 8G or 16G. - */ -static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width) -{ - float half_scale = ((float)(1 << bit_width) / 2.0f); - - return (GRAVITY_EARTH * val * g_range) / half_scale; -} - -/*! - * @brief This function converts lsb to degree per second for 16 bit gyro at - * range 125, 250, 500, 1000 or 2000dps. - */ -static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width) -{ - float half_scale = ((float)(1 << bit_width) / 2.0f); - - return (dps / ((half_scale) + BMI2_GYR_RANGE_2000)) * (val); -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/common/common.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/common/common.c deleted file mode 100644 index afff37bc84..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/common/common.c +++ /dev/null @@ -1,372 +0,0 @@ -/** - * Copyright (C) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - */ - -#include -#include -#include - -#include "coines.h" -#include "bmi2_defs.h" - -/******************************************************************************/ -/*! Macro definitions */ -#define BMI2XY_SHUTTLE_ID UINT16_C(0x1B8) - -/*! Macro that defines read write length */ -#define READ_WRITE_LEN UINT8_C(46) - -/******************************************************************************/ -/*! Static variable definition */ - -/*! Variable that holds the I2C device address or SPI chip selection */ -static uint8_t dev_addr; - -/******************************************************************************/ -/*! User interface functions */ - -/*! - * I2C read function map to COINES platform - */ -BMI2_INTF_RETURN_TYPE bmi2_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr) -{ - uint8_t dev_addr = *(uint8_t*)intf_ptr; - - return coines_read_i2c(dev_addr, reg_addr, reg_data, (uint16_t)len); -} - -/*! - * I2C write function map to COINES platform - */ -BMI2_INTF_RETURN_TYPE bmi2_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr) -{ - uint8_t dev_addr = *(uint8_t*)intf_ptr; - - return coines_write_i2c(dev_addr, reg_addr, (uint8_t *)reg_data, (uint16_t)len); -} - -/*! - * SPI read function map to COINES platform - */ -BMI2_INTF_RETURN_TYPE bmi2_spi_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr) -{ - uint8_t dev_addr = *(uint8_t*)intf_ptr; - - return coines_read_spi(dev_addr, reg_addr, reg_data, (uint16_t)len); -} - -/*! - * SPI write function map to COINES platform - */ -BMI2_INTF_RETURN_TYPE bmi2_spi_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr) -{ - uint8_t dev_addr = *(uint8_t*)intf_ptr; - - return coines_write_spi(dev_addr, reg_addr, (uint8_t *)reg_data, (uint16_t)len); -} - -/*! - * Delay function map to COINES platform - */ -void bmi2_delay_us(uint32_t period, void *intf_ptr) -{ - coines_delay_usec(period); -} - -/*! - * @brief Function to select the interface between SPI and I2C. - * Also to initialize coines platform - */ -int8_t bmi2_interface_init(struct bmi2_dev *bmi, uint8_t intf) -{ - int8_t rslt = BMI2_OK; - struct coines_board_info board_info; - - if (bmi != NULL) - { - int16_t result = coines_open_comm_intf(COINES_COMM_INTF_USB); - if (result < COINES_SUCCESS) - { - printf( - "\n Unable to connect with Application Board ! \n" " 1. Check if the board is connected and powered on. \n" " 2. Check if Application Board USB driver is installed. \n" - " 3. Check if board is in use by another application. (Insufficient permissions to access USB) \n"); - exit(result); - } - - result = coines_get_board_info(&board_info); - -#if defined(PC) - setbuf(stdout, NULL); -#endif - - if (result == COINES_SUCCESS) - { - if ((board_info.shuttle_id != BMI2XY_SHUTTLE_ID)) - { - printf("! Warning invalid sensor shuttle \n ," "This application will not support this sensor \n"); - exit(COINES_E_FAILURE); - } - } - - coines_set_shuttleboard_vdd_vddio_config(0, 0); - coines_delay_msec(100); - - /* Bus configuration : I2C */ - if (intf == BMI2_I2C_INTF) - { - printf("I2C Interface \n"); - - /* To initialize the user I2C function */ - dev_addr = BMI2_I2C_PRIM_ADDR; - bmi->intf = BMI2_I2C_INTF; - bmi->read = bmi2_i2c_read; - bmi->write = bmi2_i2c_write; - coines_config_i2c_bus(COINES_I2C_BUS_0, COINES_I2C_FAST_MODE); - } - /* Bus configuration : SPI */ - else if (intf == BMI2_SPI_INTF) - { - printf("SPI Interface \n"); - - /* To initialize the user SPI function */ - dev_addr = COINES_SHUTTLE_PIN_7; - bmi->intf = BMI2_SPI_INTF; - bmi->read = bmi2_spi_read; - bmi->write = bmi2_spi_write; - coines_config_spi_bus(COINES_SPI_BUS_0, COINES_SPI_SPEED_5_MHZ, COINES_SPI_MODE3); - coines_set_pin_config(COINES_SHUTTLE_PIN_7, COINES_PIN_DIRECTION_OUT, COINES_PIN_VALUE_HIGH); - } - - /* Assign device address to interface pointer */ - bmi->intf_ptr = &dev_addr; - - /* Configure delay in microseconds */ - bmi->delay_us = bmi2_delay_us; - - /* Configure max read/write length (in bytes) ( Supported length depends on target machine) */ - bmi->read_write_len = READ_WRITE_LEN; - - /* Assign to NULL to load the default config file. */ - bmi->config_file_ptr = NULL; - - coines_delay_usec(10000); - - coines_set_shuttleboard_vdd_vddio_config(3300, 3300); - - coines_delay_usec(10000); - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; - -} - -/*! - * @brief Prints the execution status of the APIs. - */ -void bmi2_error_codes_print_result(int8_t rslt) -{ - switch (rslt) - { - case BMI2_OK: - - /* Do nothing */ - break; - - case BMI2_W_FIFO_EMPTY: - printf("Warning [%d] : FIFO empty\r\n", rslt); - break; - case BMI2_W_PARTIAL_READ: - printf("Warning [%d] : FIFO partial read\r\n", rslt); - break; - case BMI2_E_NULL_PTR: - printf( - "Error [%d] : Null pointer error. It occurs when the user tries to assign value (not address) to a pointer," " which has been initialized to NULL.\r\n", - rslt); - break; - - case BMI2_E_COM_FAIL: - printf( - "Error [%d] : Communication failure error. It occurs due to read/write operation failure and also due " "to power failure during communication\r\n", - rslt); - break; - - case BMI2_E_DEV_NOT_FOUND: - printf("Error [%d] : Device not found error. It occurs when the device chip id is incorrectly read\r\n", - rslt); - break; - - case BMI2_E_INVALID_SENSOR: - printf( - "Error [%d] : Invalid sensor error. It occurs when there is a mismatch in the requested feature with the " "available one\r\n", - rslt); - break; - - case BMI2_E_SELF_TEST_FAIL: - printf( - "Error [%d] : Self-test failed error. It occurs when the validation of accel self-test data is " "not satisfied\r\n", - rslt); - break; - - case BMI2_E_INVALID_INT_PIN: - printf( - "Error [%d] : Invalid interrupt pin error. It occurs when the user tries to configure interrupt pins " "apart from INT1 and INT2\r\n", - rslt); - break; - - case BMI2_E_OUT_OF_RANGE: - printf( - "Error [%d] : Out of range error. It occurs when the data exceeds from filtered or unfiltered data from " "fifo and also when the range exceeds the maximum range for accel and gyro while performing FOC\r\n", - rslt); - break; - - case BMI2_E_ACC_INVALID_CFG: - printf( - "Error [%d] : Invalid Accel configuration error. It occurs when there is an error in accel configuration" " register which could be one among range, BW or filter performance in reg address 0x40\r\n", - rslt); - break; - - case BMI2_E_GYRO_INVALID_CFG: - printf( - "Error [%d] : Invalid Gyro configuration error. It occurs when there is a error in gyro configuration" "register which could be one among range, BW or filter performance in reg address 0x42\r\n", - rslt); - break; - - case BMI2_E_ACC_GYR_INVALID_CFG: - printf( - "Error [%d] : Invalid Accel-Gyro configuration error. It occurs when there is a error in accel and gyro" " configuration registers which could be one among range, BW or filter performance in reg address 0x40 " "and 0x42\r\n", - rslt); - break; - - case BMI2_E_CONFIG_LOAD: - printf( - "Error [%d] : Configuration load error. It occurs when failure observed while loading the configuration " "into the sensor\r\n", - rslt); - break; - - case BMI2_E_INVALID_PAGE: - printf( - "Error [%d] : Invalid page error. It occurs due to failure in writing the correct feature configuration " "from selected page\r\n", - rslt); - break; - - case BMI2_E_SET_APS_FAIL: - printf( - "Error [%d] : APS failure error. It occurs due to failure in write of advance power mode configuration " "register\r\n", - rslt); - break; - - case BMI2_E_AUX_INVALID_CFG: - printf( - "Error [%d] : Invalid AUX configuration error. It occurs when the auxiliary interface settings are not " "enabled properly\r\n", - rslt); - break; - - case BMI2_E_AUX_BUSY: - printf( - "Error [%d] : AUX busy error. It occurs when the auxiliary interface buses are engaged while configuring" " the AUX\r\n", - rslt); - break; - - case BMI2_E_REMAP_ERROR: - printf( - "Error [%d] : Remap error. It occurs due to failure in assigning the remap axes data for all the axes " "after change in axis position\r\n", - rslt); - break; - - case BMI2_E_GYR_USER_GAIN_UPD_FAIL: - printf( - "Error [%d] : Gyro user gain update fail error. It occurs when the reading of user gain update status " "fails\r\n", - rslt); - break; - - case BMI2_E_SELF_TEST_NOT_DONE: - printf( - "Error [%d] : Self-test not done error. It occurs when the self-test process is ongoing or not " "completed\r\n", - rslt); - break; - - case BMI2_E_INVALID_INPUT: - printf("Error [%d] : Invalid input error. It occurs when the sensor input validity fails\r\n", rslt); - break; - - case BMI2_E_INVALID_STATUS: - printf("Error [%d] : Invalid status error. It occurs when the feature/sensor validity fails\r\n", rslt); - break; - - case BMI2_E_CRT_ERROR: - printf("Error [%d] : CRT error. It occurs when the CRT test has failed\r\n", rslt); - break; - - case BMI2_E_ST_ALREADY_RUNNING: - printf( - "Error [%d] : Self-test already running error. It occurs when the self-test is already running and " "another has been initiated\r\n", - rslt); - break; - - case BMI2_E_CRT_READY_FOR_DL_FAIL_ABORT: - printf( - "Error [%d] : CRT ready for download fail abort error. It occurs when download in CRT fails due to wrong " "address location\r\n", - rslt); - break; - - case BMI2_E_DL_ERROR: - printf( - "Error [%d] : Download error. It occurs when write length exceeds that of the maximum burst length\r\n", - rslt); - break; - - case BMI2_E_PRECON_ERROR: - printf( - "Error [%d] : Pre-conditional error. It occurs when precondition to start the feature was not " "completed\r\n", - rslt); - break; - - case BMI2_E_ABORT_ERROR: - printf("Error [%d] : Abort error. It occurs when the device was shaken during CRT test\r\n", rslt); - break; - - case BMI2_E_WRITE_CYCLE_ONGOING: - printf( - "Error [%d] : Write cycle ongoing error. It occurs when the write cycle is already running and another " "has been initiated\r\n", - rslt); - break; - - case BMI2_E_ST_NOT_RUNING: - printf( - "Error [%d] : Self-test is not running error. It occurs when self-test running is disabled while it's " "running\r\n", - rslt); - break; - - case BMI2_E_DATA_RDY_INT_FAILED: - printf( - "Error [%d] : Data ready interrupt error. It occurs when the sample count exceeds the FOC sample limit " "and data ready status is not updated\r\n", - rslt); - break; - - case BMI2_E_INVALID_FOC_POSITION: - printf( - "Error [%d] : Invalid FOC position error. It occurs when average FOC data is obtained for the wrong" " axes\r\n", - rslt); - break; - - default: - printf("Error [%d] : Unknown error code\r\n", rslt); - break; - } -} - -/*! - * @brief Deinitializes coines platform - * - * @return void. - */ -void bmi2_coines_deinit(void) -{ - coines_close_comm_intf(COINES_COMM_INTF_USB); -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/common/common.h b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/common/common.h deleted file mode 100644 index 763983da4a..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/common/common.h +++ /dev/null @@ -1,122 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -#ifndef _COMMON_H -#define _COMMON_H - -/*! CPP guard */ -#ifdef __cplusplus -extern "C" { -#endif - -#include -#include "bmi2.h" - -/*! - * @brief Function for reading the sensor's registers through I2C bus. - * - * @param[in] reg_addr : Register address. - * @param[out] reg_data : Pointer to the data buffer to store the read data. - * @param[in] length : No of bytes to read. - * @param[in] intf_ptr : Interface pointer - * - * @return Status of execution - * @retval = BMI2_INTF_RET_SUCCESS -> Success - * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info - * - */ -BMI2_INTF_RETURN_TYPE bmi2_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr); - -/*! - * @brief Function for writing the sensor's registers through I2C bus. - * - * @param[in] reg_addr : Register address. - * @param[in] reg_data : Pointer to the data buffer whose value is to be written. - * @param[in] length : No of bytes to write. - * @param[in] intf_ptr : Interface pointer - * - * @return Status of execution - * @retval = BMI2_INTF_RET_SUCCESS -> Success - * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info - * - */ -BMI2_INTF_RETURN_TYPE bmi2_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr); - -/*! - * @brief Function for reading the sensor's registers through SPI bus. - * - * @param[in] reg_addr : Register address. - * @param[out] reg_data : Pointer to the data buffer to store the read data. - * @param[in] length : No of bytes to read. - * @param[in] intf_ptr : Interface pointer - * - * @return Status of execution - * @retval = BMI2_INTF_RET_SUCCESS -> Success - * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info - * - */ -BMI2_INTF_RETURN_TYPE bmi2_spi_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr); - -/*! - * @brief Function for writing the sensor's registers through SPI bus. - * - * @param[in] reg_addr : Register address. - * @param[in] reg_data : Pointer to the data buffer whose data has to be written. - * @param[in] length : No of bytes to write. - * @param[in] intf_ptr : Interface pointer - * - * @return Status of execution - * @retval = BMI2_INTF_RET_SUCCESS -> Success - * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info - * - */ -BMI2_INTF_RETURN_TYPE bmi2_spi_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr); - -/*! - * @brief This function provides the delay for required time (Microsecond) as per the input provided in some of the - * APIs. - * - * @param[in] period_us : The required wait time in microsecond. - * @param[in] intf_ptr : Interface pointer - * - * @return void. - * - */ -void bmi2_delay_us(uint32_t period, void *intf_ptr); - -/*! - * @brief Function to select the interface between SPI and I2C. - * - * @param[in] bma : Structure instance of bmi2_dev - * @param[in] intf : Interface selection parameter - * - * @return Status of execution - * @retval 0 -> Success - * @retval < 0 -> Failure Info - */ -int8_t bmi2_interface_init(struct bmi2_dev *bma, uint8_t intf); - -/*! - * @brief Prints the execution status of the APIs. - * - * @param[in] rslt : Error code returned by the API whose execution status has to be printed. - * - * @return void. - */ -void bmi2_error_codes_print_result(int8_t rslt); - -/*! - * @brief Deinitializes coines platform - * - * @return void. - */ -void bmi2_coines_deinit(void); - -#ifdef __cplusplus -} -#endif /* End of CPP guard */ - -#endif /* _COMMON_H */ diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/component_retrim/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/component_retrim/Makefile deleted file mode 100644 index 0729972a20..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/component_retrim/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= component_retrim.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270_maximum_fifo.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/component_retrim/component_retrim.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/component_retrim/component_retrim.c deleted file mode 100644 index ea24ab2e9f..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/component_retrim/component_retrim.c +++ /dev/null @@ -1,52 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270_maximum_fifo.h" -#include "common.h" - -/******************************************************************************/ -/*! Functions */ - -/* This function starts the execution of program. */ -int main(void) -{ - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270_maximum_fifo. */ - rslt = bmi270_maximum_fifo_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* This API runs the CRT process. */ - rslt = bmi2_do_crt(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Do not move the board while doing CRT. If so, it will throw an abort error. */ - if (rslt == BMI2_OK) - { - printf("CRT successfully completed."); - } - } - - bmi2_coines_deinit(); - - return rslt; -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/fifo_watermark_header_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/fifo_watermark_header_mode/Makefile deleted file mode 100644 index 131538f30c..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/fifo_watermark_header_mode/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= fifo_watermark_header_mode.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270_maximum_fifo.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/fifo_watermark_header_mode/fifo_watermark_header_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/fifo_watermark_header_mode/fifo_watermark_header_mode.c deleted file mode 100644 index f031379f78..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/fifo_watermark_header_mode/fifo_watermark_header_mode.c +++ /dev/null @@ -1,335 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270_maximum_fifo.h" -#include "common.h" - -/******************************************************************************/ -/*! Macros */ - -/*! Buffer size allocated to store raw FIFO data */ -#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) - -/*! Length of data to be read from FIFO */ -#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) - -/*! Number of accel frames to be extracted from FIFO */ - -/*! - * Calculation: - * fifo_watermark_level = 650, accel_frame_len = 6, gyro_frame_len = 6 header_byte = 1. - * fifo_accel_frame_count = (650 / (6 + 6 + 1 )) = 50 frames - * NOTE: Extra frames are read in order to get sensor time - */ -#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(55) - -/*! Number of gyro frames to be extracted from FIFO */ -#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(55) - -/*! Setting a watermark level in FIFO */ -#define BMI270_FIFO_WATERMARK_LEVEL UINT16_C(650) - -/*! Macro to read sensortime byte in FIFO */ -#define SENSORTIME_OVERHEAD_BYTE UINT8_C(3) - -/******************************************************************************/ -/*! Static Function Declaration */ - -/*! - * @brief This internal API is used to set configurations for accel and gyro. - * @param[in] dev : Structure instance of bmi2_dev. - * @return Status of execution. - */ -static int8_t set_accel_gyro_config(struct bmi2_dev *dev); - -/******************************************************************************/ -/*! Functions */ - -/* This function starts the execution of program. */ -int main(void) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Variable to store the available frame length count in FIFO */ - uint8_t frame_count; - - /* Variable to index bytes. */ - uint16_t index = 0; - - uint16_t fifo_length = 0; - - uint16_t accel_frame_length; - - uint16_t gyro_frame_length; - - uint8_t try = 1; - - /* To read sensortime, extra 3 bytes are added to fifo buffer */ - uint16_t fifo_buffer_size = BMI2_FIFO_RAW_DATA_BUFFER_SIZE + SENSORTIME_OVERHEAD_BYTE; - - /* Number of bytes of FIFO data. */ - uint8_t fifo_data[fifo_buffer_size]; - - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Array of accelerometer frames -> Total bytes = - * 55 * (6 axes bytes) = 330 bytes */ - struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; - - /* Array of gyro frames -> Total bytes = - * 55 * (6 axes bytes) = 330 bytes */ - struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; - - /* Initialize FIFO frame structure. */ - struct bmi2_fifo_frame fifoframe = { 0 }; - - /* Accel and gyro sensor are listed in array. */ - uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; - - /* Variable to get fifo water-mark interrupt status. */ - uint16_t int_status = 0; - - uint16_t watermark = 0; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270_maximum_fifo. */ - rslt = bmi270_maximum_fifo_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Enable accel and gyro sensor. */ - rslt = bmi2_sensor_enable(sensor_sel, 2, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Configuration settings for accel and gyro. */ - rslt = set_accel_gyro_config(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Before setting FIFO, disable the advance power save mode. */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Initially disable all configurations in fifo. */ - rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Update FIFO structure. */ - /* Mapping the buffer to store the fifo data. */ - fifoframe.data = fifo_data; - - /* Length of FIFO frame. */ - /* To read sensortime, extra 3 bytes are added to fifo user length. */ - fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH + SENSORTIME_OVERHEAD_BYTE; - - /* Set FIFO configuration by enabling accel, gyro and timestamp. - * NOTE 1: The header mode is enabled by default. - * NOTE 2: By default the FIFO operating mode is FIFO mode. - * NOTE 3: Sensortime is enabled by default */ - printf("FIFO is configured in header mode\n"); - rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* FIFO water-mark interrupt is enabled. */ - fifoframe.data_int_map = BMI2_FWM_INT; - - /* Map water-mark interrupt to the required interrupt pin. */ - rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Set water-mark level. */ - fifoframe.wm_lvl = BMI270_FIFO_WATERMARK_LEVEL; - - /* Set the water-mark level if water-mark interrupt is mapped. */ - rslt = bmi2_set_fifo_wm(fifoframe.wm_lvl, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - rslt = bmi2_get_fifo_wm(&watermark, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - printf("\nFIFO watermark level is %d\n", watermark); - - while (try <= 10) - { - /* Read FIFO data on interrupt. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* To check the status of FIFO watermark interrupt. */ - if ((rslt == BMI2_OK) && (int_status & BMI2_FWM_INT_STATUS_MASK)) - { - printf("\nIteration : %d\n", try); - - printf("\nWatermark interrupt occurred\n"); - - rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; - gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; - - printf("\nFIFO data bytes available : %d \n", fifo_length); - printf("\nFIFO data bytes requested : %d \n", fifoframe.length); - - /* Read FIFO data. */ - rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Read FIFO data on interrupt. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - printf("\nFIFO accel frames requested : %d \n", accel_frame_length); - - /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ - rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); - printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); - - printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); - - /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ - rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); - printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); - - /* Calculating the frame count from the available bytes in FIFO - * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len + header_byte)) */ - frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH + 1)); - printf("\nAvailable frame count from available bytes: %d\n", frame_count); - - printf("\nExracted accel frames\n"); - - if (accel_frame_length > frame_count) - { - accel_frame_length = frame_count; - } - - /* Print the parsed accelerometer data from the FIFO buffer. */ - for (index = 0; index < accel_frame_length; index++) - { - printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, - fifo_accel_data[index].y, fifo_accel_data[index].z); - } - - printf("\nExtracted gyro frames\n"); - - if (gyro_frame_length > frame_count) - { - gyro_frame_length = frame_count; - } - - /* Print the parsed gyro data from the FIFO buffer. */ - for (index = 0; index < gyro_frame_length; index++) - { - printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", - index, - fifo_gyro_data[index].x, - fifo_gyro_data[index].y, - fifo_gyro_data[index].z); - } - - /* Print control frames like sensor time and skipped frame count. */ - printf("Skipped frame count = %d\n", fifoframe.skipped_frame_count); - printf("Sensor time = %lu\r\n", fifoframe.sensor_time); - - try++; - } - } - } - - bmi2_coines_deinit(); - - return rslt; -} - -/*! - * @brief This internal API is used to set configurations for accel and gyro. - */ -static int8_t set_accel_gyro_config(struct bmi2_dev *dev) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Structure to define accel and gyro configurations. */ - struct bmi2_sens_config config[2]; - - /* Configure the type of feature. */ - config[0].type = BMI2_ACCEL; - config[1].type = BMI2_GYRO; - - /* Get default configurations for the type of feature selected. */ - rslt = bmi2_get_sensor_config(config, 2, dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* NOTE: The user can change the following configuration parameters according to their requirement. */ - /* Accel configuration settings. */ - /* Set Output Data Rate */ - config[0].cfg.acc.odr = BMI2_ACC_ODR_25HZ; - - /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ - config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; - - /* The bandwidth parameter is used to configure the number of sensor samples that are averaged - * if it is set to 2, then 2^(bandwidth parameter) samples - * are averaged, resulting in 4 averaged samples - * Note1 : For more information, refer the datasheet. - * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but - * this has an adverse effect on the power consumed. - */ - config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; - - /* Enable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - * For more info refer datasheet. - */ - config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; - - /* Gyro configuration settings. */ - /* Set Output Data Rate */ - config[1].cfg.gyr.odr = BMI2_GYR_ODR_25HZ; - - /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ - config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; - - /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ - config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; - - /* Enable/Disable the noise performance mode for precision yaw rate sensing - * There are two modes - * 0 -> Ultra low power mode(Default) - * 1 -> High performance mode - */ - config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; - - /* Enable/Disable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - */ - config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; - - /* Set new configurations. */ - rslt = bmi2_set_sensor_config(config, 2, dev); - bmi2_error_codes_print_result(rslt); - } - - return rslt; -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/fifo_watermark_headerless_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/fifo_watermark_headerless_mode/Makefile deleted file mode 100644 index 9790a92d4b..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/fifo_watermark_headerless_mode/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= fifo_watermark_headerless_mode.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270_maximum_fifo.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c deleted file mode 100644 index 541ff11163..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c +++ /dev/null @@ -1,331 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270_maximum_fifo.h" -#include "common.h" - -/******************************************************************************/ -/*! Macros */ - -/*! Buffer size allocated to store raw FIFO data */ -#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) - -/*! Length of data to be read from FIFO */ -#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) - -/*! Number of accel frames to be extracted from FIFO */ - -/*! - * Calculation: - * fifo_watermark_level = 650, accel_frame_len = 6, gyro_frame_len = 6. - * fifo_accel_frame_count = (650 / (6 + 6 )) = 54 frames - */ -#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(55) - -/*! Number of gyro frames to be extracted from FIFO */ -#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(55) - -/*! Setting a watermark level in FIFO */ -#define BMI270_FIFO_WATERMARK_LEVEL UINT16_C(650) - -/******************************************************************************/ -/*! Static Function Declaration */ - -/*! - * @brief This internal API is used to set configurations for accel and gyro. - * @param[in] dev : Structure instance of bmi2_dev. - * @return Status of execution. - */ -static int8_t set_accel_gyro_config(struct bmi2_dev *dev); - -/******************************************************************************/ -/*! Functions */ - -/* This function starts the execution of program. */ -int main(void) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Variable to index bytes. */ - uint16_t index = 0; - - /* Variable to store the available frame length count in FIFO */ - uint8_t frame_count; - - uint16_t fifo_length = 0; - - uint16_t accel_frame_length; - - uint16_t gyro_frame_length; - - uint8_t try = 1; - - /* Number of bytes of FIFO data. */ - uint8_t fifo_data[BMI2_FIFO_RAW_DATA_BUFFER_SIZE] = { 0 }; - - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Array of accelerometer frames -> Total bytes = - * 50 * (6 axes bytes) = 300 bytes */ - struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; - - /* Array of gyro frames -> Total bytes = - * 50 * (6 axes bytes) = 300 bytes */ - struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; - - /* Initialize FIFO frame structure. */ - struct bmi2_fifo_frame fifoframe = { 0 }; - - /* Accel and gyro sensor are listed in array. */ - uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; - - /* Variable to get fifo water-mark interrupt status. */ - uint16_t int_status = 0; - - uint16_t watermark = 0; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270. */ - rslt = bmi270_maximum_fifo_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Enable accel and gyro sensor. */ - rslt = bmi2_sensor_enable(sensor_sel, 2, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Configuration settings for accel and gyro. */ - rslt = set_accel_gyro_config(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Before setting FIFO, disable the advance power save mode. */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Initially disable all configurations in fifo. */ - rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Update FIFO structure. */ - /* Mapping the buffer to store the fifo data. */ - fifoframe.data = fifo_data; - - /* Length of FIFO frame. */ - fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH; - - /* Set FIFO configuration by enabling accel, gyro. - * NOTE 1: The header mode is enabled by default. - * NOTE 2: By default the FIFO operating mode is FIFO mode. */ - rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - printf("FIFO is configured in headerless mode\n"); - - /* To enable headerless mode, disable the header. */ - if (rslt == BMI2_OK) - { - rslt = bmi2_set_fifo_config(BMI2_FIFO_HEADER_EN, BMI2_DISABLE, &bmi2_dev); - } - - /* FIFO water-mark interrupt is enabled. */ - fifoframe.data_int_map = BMI2_FWM_INT; - - /* Map water-mark interrupt to the required interrupt pin. */ - rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Set water-mark level. */ - fifoframe.wm_lvl = BMI270_FIFO_WATERMARK_LEVEL; - - fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH; - - /* Set the water-mark level if water-mark interrupt is mapped. */ - rslt = bmi2_set_fifo_wm(fifoframe.wm_lvl, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - rslt = bmi2_get_fifo_wm(&watermark, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - printf("\nFIFO watermark level is %d\n", watermark); - - while (try <= 10) - { - /* Read FIFO data on interrupt. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* To check the status of FIFO watermark interrupt. */ - if ((rslt == BMI2_OK) && (int_status & BMI2_FWM_INT_STATUS_MASK)) - { - printf("\nIteration : %d\n", try); - - printf("\nWatermark interrupt occurred\n"); - - rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; - gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; - - printf("\nFIFO data bytes available : %d \n", fifo_length); - printf("\nFIFO data bytes requested : %d \n", fifoframe.length); - - /* Read FIFO data. */ - rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Read FIFO data on interrupt. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - printf("\nFIFO accel frames requested : %d \n", accel_frame_length); - - /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ - rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); - printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); - - printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); - - /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ - rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); - printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); - - /* Calculating the frame count from the available bytes in FIFO - * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len)) */ - frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH)); - printf("\nAvailable frame count from available bytes: %d\n", frame_count); - - printf("\nExracted accel frames\n"); - - if (accel_frame_length > frame_count) - { - accel_frame_length = frame_count; - } - - /* Print the parsed accelerometer data from the FIFO buffer. */ - for (index = 0; index < accel_frame_length; index++) - { - printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, - fifo_accel_data[index].y, fifo_accel_data[index].z); - } - - printf("\nExtracted gyro frames\n"); - - if (gyro_frame_length > frame_count) - { - gyro_frame_length = frame_count; - } - - /* Print the parsed gyro data from the FIFO buffer. */ - for (index = 0; index < gyro_frame_length; index++) - { - printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", - index, - fifo_gyro_data[index].x, - fifo_gyro_data[index].y, - fifo_gyro_data[index].z); - } - } - - try++; - } - } - - bmi2_coines_deinit(); - - return rslt; -} - -/*! - * @brief This internal API is used to set configurations for accel. - */ -static int8_t set_accel_gyro_config(struct bmi2_dev *dev) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Structure to define accel and gyro configurations. */ - struct bmi2_sens_config config[2]; - - /* Configure the type of feature. */ - config[0].type = BMI2_ACCEL; - config[1].type = BMI2_GYRO; - - /* Get default configurations for the type of feature selected. */ - rslt = bmi2_get_sensor_config(config, 2, dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* NOTE: The user can change the following configuration parameter according to their requirement. */ - /* Accel configuration settings. */ - /* Set Output Data Rate */ - config[0].cfg.acc.odr = BMI2_ACC_ODR_100HZ; - - /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ - config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; - - /* The bandwidth parameter is used to configure the number of sensor samples that are averaged - * if it is set to 2, then 2^(bandwidth parameter) samples - * are averaged, resulting in 4 averaged samples - * Note1 : For more information, refer the datasheet. - * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but - * this has an adverse effect on the power consumed. - */ - config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; - - /* Enable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - * For more info refer datasheet. - */ - config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; - - /* Gyro configuration settings. */ - /* Set Output Data Rate */ - config[1].cfg.gyr.odr = BMI2_GYR_ODR_100HZ; - - /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ - config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; - - /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ - config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; - - /* Enable/Disable the noise performance mode for precision yaw rate sensing - * There are two modes - * 0 -> Ultra low power mode(Default) - * 1 -> High performance mode - */ - config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; - - /* Enable/Disable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - */ - config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; - - /* Set new configurations. */ - rslt = bmi2_set_sensor_config(config, 2, dev); - bmi2_error_codes_print_result(rslt); - } - - return rslt; -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/gyro/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/gyro/Makefile deleted file mode 100644 index 5ab6e7fd76..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/gyro/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= gyro.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270_maximum_fifo.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/gyro/gyro.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/gyro/gyro.c deleted file mode 100644 index 8b56f08429..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_maximum_fifo/gyro/gyro.c +++ /dev/null @@ -1,195 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270_maximum_fifo.h" -#include "common.h" - -/******************************************************************************/ -/*! Static Function Declaration */ - -/*! - * @brief This internal API is used to set configurations for gyro. - * - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Status of execution. - */ -static int8_t set_gyro_config(struct bmi2_dev *dev); - -/*! - * @brief This function converts lsb to degree per second for 16 bit gyro at - * range 125, 250, 500, 1000 or 2000dps. - * - * @param[in] val : LSB from each axis. - * @param[in] dps : Degree per second. - * @param[in] bit_width : Resolution for gyro. - * - * @return Degree per second. - */ -static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width); - -/******************************************************************************/ -/*! Functions */ - -/* This function starts the execution of program. */ -int main(void) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Variable to define limit to print gyro data. */ - uint8_t limit = 10; - - uint8_t indx = 0; - - float x = 0, y = 0, z = 0; - - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Create an instance of sensor data structure. */ - struct bmi2_sensor_data sensor_data = { 0 }; - - /* Assign gyro sensor to variable. */ - uint8_t sens_list = BMI2_GYRO; - - /* Initialize the interrupt status of gyro. */ - uint16_t int_status = 0; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270_maximum_fifo_init */ - rslt = bmi270_maximum_fifo_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Enable the selected sensors. */ - rslt = bmi2_sensor_enable(&sens_list, 1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Gyro configuration settings. */ - rslt = set_gyro_config(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Select gyro sensor. */ - sensor_data.type = BMI2_GYRO; - printf("Gyro and DPS data\n"); - printf("Gyro data collected at Range 2000 dps with 16-bit resolution\n"); - - /* Loop to print gyro data when interrupt occurs. */ - while (indx <= limit) - { - /* To get the data ready interrupt status of gyro. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* To check the data ready interrupt status and print the status for 10 samples. */ - if (int_status & BMI2_GYR_DRDY_INT_MASK) - { - /* Get gyro data for x, y and z axis. */ - rslt = bmi2_get_sensor_data(&sensor_data, 1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - printf("\nGyr_X = %d\t", sensor_data.sens_data.gyr.x); - printf("Gyr_Y = %d\t", sensor_data.sens_data.gyr.y); - printf("Gyr_Z = %d\r\n", sensor_data.sens_data.gyr.z); - - /* Converting lsb to degree per second for 16 bit gyro at 2000dps range. */ - x = lsb_to_dps(sensor_data.sens_data.gyr.x, 2000, bmi2_dev.resolution); - y = lsb_to_dps(sensor_data.sens_data.gyr.y, 2000, bmi2_dev.resolution); - z = lsb_to_dps(sensor_data.sens_data.gyr.z, 2000, bmi2_dev.resolution); - - /* Print the data in dps. */ - printf("Gyr_DPS_X = %4.2f, Gyr_DPS_Y = %4.2f, Gyr_DPS_Z = %4.2f\n", x, y, z); - - indx++; - } - } - } - } - - bmi2_coines_deinit(); - - return rslt; -} - -/*! - * @brief This internal API is used to set configurations for gyro. - */ -static int8_t set_gyro_config(struct bmi2_dev *dev) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Structure to define the type of sensor and its configurations. */ - struct bmi2_sens_config config; - - /* Configure the type of feature. */ - config.type = BMI2_GYRO; - - /* Get default configurations for the type of feature selected. */ - rslt = bmi2_get_sensor_config(&config, 1, dev); - bmi2_error_codes_print_result(rslt); - - /* Map data ready interrupt to interrupt pin. */ - rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT2, dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* The user can change the following configuration parameters according to their requirement. */ - /* Set Output Data Rate */ - config.cfg.gyr.odr = BMI2_GYR_ODR_200HZ; - - /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ - config.cfg.gyr.range = BMI2_GYR_RANGE_2000; - - /* Gyroscope bandwidth parameters. By default the gyro bandwidth is in normal mode. */ - config.cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; - - /* Enable/Disable the noise performance mode for precision yaw rate sensing - * There are two modes - * 0 -> Ultra low power mode(Default) - * 1 -> High performance mode - */ - config.cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; - - /* Enable/Disable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - */ - config.cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; - - /* Set the gyro configurations. */ - rslt = bmi2_set_sensor_config(&config, 1, dev); - } - - return rslt; -} - -/*! - * @brief This function converts lsb to degree per second for 16 bit gyro at - * range 125, 250, 500, 1000 or 2000dps. - */ -static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width) -{ - float half_scale = ((float)(1 << bit_width) / 2.0f); - - return (dps / ((half_scale) + BMI2_GYR_RANGE_2000)) * (val); -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/accel/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/accel/Makefile deleted file mode 100644 index 3ab55f00e4..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/accel/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= accel.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270_wh.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/accel/accel.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/accel/accel.c deleted file mode 100644 index cf6825229e..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/accel/accel.c +++ /dev/null @@ -1,201 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270_wh.h" -#include "common.h" - -/******************************************************************************/ -/*! Macro definition */ - -/*! Earth's gravity in m/s^2 */ -#define GRAVITY_EARTH (9.80665f) - -/******************************************************************************/ -/*! Static Function Declaration */ - -/*! - * @brief This internal API is used to set configurations for accel. - * - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Status of execution. - */ -static int8_t set_accel_config(struct bmi2_dev *bmi2_dev); - -/*! - * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at - * range 2G, 4G, 8G or 16G. - * - * @param[in] val : LSB from each axis. - * @param[in] g_range : Gravity range. - * @param[in] bit_width : Resolution for accel. - * - * @return Gravity. - */ -static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width); - -/******************************************************************************/ -/*! Functions */ - -/* This function starts the execution of program. */ -int main(void) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Variable to define limit to print accel data. */ - uint8_t limit = 20; - - /* Assign accel sensor to variable. */ - uint8_t sensor_list = BMI2_ACCEL; - - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Create an instance of sensor data structure. */ - struct bmi2_sensor_data sensor_data = { 0 }; - - /* Initialize the interrupt status of accel. */ - uint16_t int_status = 0; - - uint8_t indx = 0; - float x = 0, y = 0, z = 0; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270_wh. */ - rslt = bmi270_wh_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Enable the accel sensor. */ - rslt = bmi270_wh_sensor_enable(&sensor_list, 1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Accel configuration settings. */ - rslt = set_accel_config(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Assign accel sensor. */ - sensor_data.type = BMI2_ACCEL; - printf("Accel and m/s2 data \n"); - printf("Accel data collected at 2G Range with 16-bit resolution\n"); - - /* Loop to print the accel data when interrupt occurs. */ - while (indx <= limit) - { - /* To get the status of accel data ready interrupt. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* To check the accel data ready interrupt status and print the status for 10 samples. */ - if (int_status & BMI2_ACC_DRDY_INT_MASK) - { - /* Get accelerometer data for x, y and z axis. */ - rslt = bmi270_wh_get_sensor_data(&sensor_data, 1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - printf("\nAcc_X = %d\t", sensor_data.sens_data.acc.x); - printf("Acc_Y = %d\t", sensor_data.sens_data.acc.y); - printf("Acc_Z = %d\r\n", sensor_data.sens_data.acc.z); - - /* Converting lsb to meter per second squared for 16 bit accelerometer at 2G range. */ - x = lsb_to_mps2(sensor_data.sens_data.acc.x, 2, bmi2_dev.resolution); - y = lsb_to_mps2(sensor_data.sens_data.acc.y, 2, bmi2_dev.resolution); - z = lsb_to_mps2(sensor_data.sens_data.acc.z, 2, bmi2_dev.resolution); - - /* Print the data in m/s2. */ - printf("\nAcc_ms2_X = %4.2f, Acc_ms2_Y = %4.2f, Acc_ms2_Z = %4.2f\n", x, y, z); - - indx++; - } - } - } - } - - bmi2_coines_deinit(); - - return rslt; -} - -/*! - * @brief This internal API is used to set configurations for accel. - */ -static int8_t set_accel_config(struct bmi2_dev *bmi2_dev) -{ - - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Structure to define accelerometer configuration. */ - struct bmi2_sens_config config; - - /* Configure the type of feature. */ - config.type = BMI2_ACCEL; - - /* Get default configurations for the type of feature selected. */ - rslt = bmi270_wh_get_sensor_config(&config, 1, bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* NOTE: The user can change the following configuration parameters according to their requirement. */ - /* Set Output Data Rate */ - config.cfg.acc.odr = BMI2_ACC_ODR_200HZ; - - /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ - config.cfg.acc.range = BMI2_ACC_RANGE_2G; - - /* The bandwidth parameter is used to configure the number of sensor samples that are averaged - * if it is set to 2, then 2^(bandwidth parameter) samples - * are averaged, resulting in 4 averaged samples. - * Note1 : For more information, refer the datasheet. - * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but - * this has an adverse effect on the power consumed. - */ - config.cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; - - /* Enable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - * For more info refer datasheet. - */ - config.cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; - - /* Set the accel configurations. */ - rslt = bmi270_wh_set_sensor_config(&config, 1, bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Map data ready interrupt to interrupt pin. */ - rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT1, bmi2_dev); - bmi2_error_codes_print_result(rslt); - } - - return rslt; -} - -/*! - * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at - * range 2G, 4G, 8G or 16G. - */ -static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width) -{ - float half_scale = ((float)(1 << bit_width) / 2.0f); - - return (GRAVITY_EARTH * val * g_range) / half_scale; -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/accel_gyro/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/accel_gyro/Makefile deleted file mode 100644 index 1d3ea28c5f..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/accel_gyro/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= accel_gyro.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270_wh.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/accel_gyro/accel_gyro.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/accel_gyro/accel_gyro.c deleted file mode 100644 index e543cca7cf..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/accel_gyro/accel_gyro.c +++ /dev/null @@ -1,268 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270_wh.h" -#include "common.h" - -/******************************************************************************/ -/*! Macro definition */ - -/*! Earth's gravity in m/s^2 */ -#define GRAVITY_EARTH (9.80665f) - -/*! Macros to select the sensors */ -#define ACCEL UINT8_C(0x00) -#define GYRO UINT8_C(0x01) - -/******************************************************************************/ -/*! Static Function Declaration */ - -/*! - * @brief This internal API is used to set configurations for accel. - * - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Status of execution. - */ -static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev); - -/*! - * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at - * range 2G, 4G, 8G or 16G. - * - * @param[in] val : LSB from each axis. - * @param[in] g_range : Gravity range. - * @param[in] bit_width : Resolution for accel. - * - * @return Gravity. - */ -static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width); - -/*! - * @brief This function converts lsb to degree per second for 16 bit gyro at - * range 125, 250, 500, 1000 or 2000dps. - * - * @param[in] val : LSB from each axis. - * @param[in] dps : Degree per second. - * @param[in] bit_width : Resolution for gyro. - * - * @return Degree per second. - */ -static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width); - -/******************************************************************************/ -/*! Functions */ - -/* This function starts the execution of program. */ -int main(void) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Variable to define limit to print accel data. */ - uint8_t limit = 10; - - /* Assign accel and gyro sensor to variable. */ - uint8_t sensor_list[2] = { BMI2_ACCEL, BMI2_GYRO }; - - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Create an instance of sensor data structure. */ - struct bmi2_sensor_data sensor_data[2] = { { 0 } }; - - /* Initialize the interrupt status of accel and gyro. */ - uint16_t int_status = 0; - - uint8_t indx = 1; - - float x = 0, y = 0, z = 0; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270. */ - rslt = bmi270_wh_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Enable the accel and gyro sensor. */ - rslt = bmi270_wh_sensor_enable(sensor_list, 2, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Accel and gyro configuration settings. */ - rslt = set_accel_gyro_config(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Assign accel and gyro sensor. */ - sensor_data[ACCEL].type = BMI2_ACCEL; - sensor_data[GYRO].type = BMI2_GYRO; - - /* Loop to print accel and gyro data when interrupt occurs. */ - while (indx <= limit) - { - /* To get the data ready interrupt status of accel and gyro. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* To check the data ready interrupt status and print the status for 10 samples. */ - if ((int_status & BMI2_ACC_DRDY_INT_MASK) && (int_status & BMI2_GYR_DRDY_INT_MASK)) - { - /* Get accel and gyro data for x, y and z axis. */ - rslt = bmi270_wh_get_sensor_data(sensor_data, 2, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - printf("\n******* Accel(Raw and m/s2) Gyro(Raw and dps) data : %d *******\n", indx); - - printf("\nAcc_x = %d\t", sensor_data[ACCEL].sens_data.acc.x); - printf("Acc_y = %d\t", sensor_data[ACCEL].sens_data.acc.y); - printf("Acc_z = %d", sensor_data[ACCEL].sens_data.acc.z); - - /* Converting lsb to meter per second squared for 16 bit accelerometer at 2G range. */ - x = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.x, 2, bmi2_dev.resolution); - y = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.y, 2, bmi2_dev.resolution); - z = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.z, 2, bmi2_dev.resolution); - - /* Print the data in m/s2. */ - printf("\nAcc_ms2_X = %4.2f, Acc_ms2_Y = %4.2f, Acc_ms2_Z = %4.2f\n", x, y, z); - - printf("\nGyr_X = %d\t", sensor_data[GYRO].sens_data.gyr.x); - printf("Gyr_Y = %d\t", sensor_data[GYRO].sens_data.gyr.y); - printf("Gyr_Z= %d\n", sensor_data[GYRO].sens_data.gyr.z); - - /* Converting lsb to degree per second for 16 bit gyro at 2000dps range. */ - x = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.x, 2000, bmi2_dev.resolution); - y = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.y, 2000, bmi2_dev.resolution); - z = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.z, 2000, bmi2_dev.resolution); - - /* Print the data in dps. */ - printf("Gyro_DPS_X = %4.2f, Gyro_DPS_Y = %4.2f, Gyro_DPS_Z = %4.2f\n", x, y, z); - - indx++; - } - } - } - } - - bmi2_coines_deinit(); - - return rslt; -} - -/*! - * @brief This internal API is used to set configurations for accel and gyro. - */ -static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Structure to define accelerometer and gyro configuration. */ - struct bmi2_sens_config config[2]; - - /* Configure the type of feature. */ - config[ACCEL].type = BMI2_ACCEL; - config[GYRO].type = BMI2_GYRO; - - /* Get default configurations for the type of feature selected. */ - rslt = bmi270_wh_get_sensor_config(config, 2, bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Map data ready interrupt to interrupt pin. */ - rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT1, bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* NOTE: The user can change the following configuration parameters according to their requirement. */ - /* Set Output Data Rate */ - config[ACCEL].cfg.acc.odr = BMI2_ACC_ODR_200HZ; - - /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ - config[ACCEL].cfg.acc.range = BMI2_ACC_RANGE_2G; - - /* The bandwidth parameter is used to configure the number of sensor samples that are averaged - * if it is set to 2, then 2^(bandwidth parameter) samples - * are averaged, resulting in 4 averaged samples. - * Note1 : For more information, refer the datasheet. - * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but - * this has an adverse effect on the power consumed. - */ - config[ACCEL].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; - - /* Enable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - * For more info refer datasheet. - */ - config[ACCEL].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; - - /* The user can change the following configuration parameters according to their requirement. */ - /* Set Output Data Rate */ - config[GYRO].cfg.gyr.odr = BMI2_GYR_ODR_200HZ; - - /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ - config[GYRO].cfg.gyr.range = BMI2_GYR_RANGE_2000; - - /* Gyroscope bandwidth parameters. By default the gyro bandwidth is in normal mode. */ - config[GYRO].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; - - /* Enable/Disable the noise performance mode for precision yaw rate sensing - * There are two modes - * 0 -> Ultra low power mode(Default) - * 1 -> High performance mode - */ - config[GYRO].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; - - /* Enable/Disable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - */ - config[GYRO].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; - - /* Set the accel and gyro configurations. */ - rslt = bmi270_wh_set_sensor_config(config, 2, bmi2_dev); - bmi2_error_codes_print_result(rslt); - } - - return rslt; -} - -/*! - * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at - * range 2G, 4G, 8G or 16G. - */ -static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width) -{ - float half_scale = ((float)(1 << bit_width) / 2.0f); - - return (GRAVITY_EARTH * val * g_range) / half_scale; -} - -/*! - * @brief This function converts lsb to degree per second for 16 bit gyro at - * range 125, 250, 500, 1000 or 2000dps. - */ -static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width) -{ - float half_scale = ((float)(1 << bit_width) / 2.0f); - - return (dps / ((half_scale) + BMI2_GYR_RANGE_2000)) * (val); -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/any_motion_interrupt/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/any_motion_interrupt/Makefile deleted file mode 100644 index 1598c62cbf..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/any_motion_interrupt/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= any_motion_interrupt.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270_wh.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/any_motion_interrupt/any_motion_interrupt.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/any_motion_interrupt/any_motion_interrupt.c deleted file mode 100644 index d1953e322c..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/any_motion_interrupt/any_motion_interrupt.c +++ /dev/null @@ -1,134 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270_wh.h" -#include "common.h" - -/******************************************************************************/ -/*! Static Function Declaration */ - -/*! - * @brief This internal API is used to set configurations for any-motion. - * - * @param[in] bmi2_dev : Structure instance of bmi2_dev. - * - * @return Status of execution. - */ -static int8_t set_feature_config(struct bmi2_dev *bmi2_dev); - -/******************************************************************************/ -/*! Functions */ - -/* This function starts the execution of program. */ -int main(void) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Accel sensor and any-motion feature are listed in array. */ - uint8_t sens_list[2] = { BMI2_ACCEL, BMI2_ANY_MOTION }; - - /* Variable to get any-motion interrupt status. */ - uint16_t int_status = 0; - - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Select features and their pins to be mapped to. */ - struct bmi2_sens_int_config sens_int = { .type = BMI2_ANY_MOTION, .hw_int_pin = BMI2_INT1 }; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270_wh. */ - rslt = bmi270_wh_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Enable the selected sensors. */ - rslt = bmi270_wh_sensor_enable(sens_list, 2, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Set feature configurations for any-motion. */ - rslt = set_feature_config(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Map the feature interrupt for any-motion. */ - rslt = bmi270_wh_map_feat_int(&sens_int, 1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - printf("Move the board\n"); - - /* Loop to get any-motion interrupt. */ - do - { - /* Clear buffer. */ - int_status = 0; - - /* To get the interrupt status of any-motion. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* To check the interrupt status of any-motion. */ - if (int_status & BMI270_WH_ANY_MOT_STATUS_MASK) - { - printf("Any-motion interrupt is generated\n"); - break; - } - } while (rslt == BMI2_OK); - } - } - } - - bmi2_coines_deinit(); - - return rslt; -} - -/*! - * @brief This internal API is used to set configurations for any-motion. - */ -static int8_t set_feature_config(struct bmi2_dev *bmi2_dev) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Structure to define the type of sensor and its configurations. */ - struct bmi2_sens_config config; - - /* Configure the type of feature. */ - config.type = BMI2_ANY_MOTION; - - /* Get default configurations for the type of feature selected. */ - rslt = bmi270_wh_get_sensor_config(&config, 1, bmi2_dev); - bmi2_error_codes_print_result(rslt); - if (rslt == BMI2_OK) - { - /* NOTE: The user can change the following configuration parameters according to their requirement. */ - /* 1LSB equals 20ms. Default is 100ms, setting to 80ms. */ - config.cfg.any_motion.duration = 0x04; - - /* 1LSB equals to 0.48mg. Default is 83mg, setting to 50mg. */ - config.cfg.any_motion.threshold = 0x68; - - /* Set new configurations. */ - rslt = bmi270_wh_set_sensor_config(&config, 1, bmi2_dev); - bmi2_error_codes_print_result(rslt); - } - - return rslt; -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/common/common.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/common/common.c deleted file mode 100644 index afff37bc84..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/common/common.c +++ /dev/null @@ -1,372 +0,0 @@ -/** - * Copyright (C) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - */ - -#include -#include -#include - -#include "coines.h" -#include "bmi2_defs.h" - -/******************************************************************************/ -/*! Macro definitions */ -#define BMI2XY_SHUTTLE_ID UINT16_C(0x1B8) - -/*! Macro that defines read write length */ -#define READ_WRITE_LEN UINT8_C(46) - -/******************************************************************************/ -/*! Static variable definition */ - -/*! Variable that holds the I2C device address or SPI chip selection */ -static uint8_t dev_addr; - -/******************************************************************************/ -/*! User interface functions */ - -/*! - * I2C read function map to COINES platform - */ -BMI2_INTF_RETURN_TYPE bmi2_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr) -{ - uint8_t dev_addr = *(uint8_t*)intf_ptr; - - return coines_read_i2c(dev_addr, reg_addr, reg_data, (uint16_t)len); -} - -/*! - * I2C write function map to COINES platform - */ -BMI2_INTF_RETURN_TYPE bmi2_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr) -{ - uint8_t dev_addr = *(uint8_t*)intf_ptr; - - return coines_write_i2c(dev_addr, reg_addr, (uint8_t *)reg_data, (uint16_t)len); -} - -/*! - * SPI read function map to COINES platform - */ -BMI2_INTF_RETURN_TYPE bmi2_spi_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr) -{ - uint8_t dev_addr = *(uint8_t*)intf_ptr; - - return coines_read_spi(dev_addr, reg_addr, reg_data, (uint16_t)len); -} - -/*! - * SPI write function map to COINES platform - */ -BMI2_INTF_RETURN_TYPE bmi2_spi_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr) -{ - uint8_t dev_addr = *(uint8_t*)intf_ptr; - - return coines_write_spi(dev_addr, reg_addr, (uint8_t *)reg_data, (uint16_t)len); -} - -/*! - * Delay function map to COINES platform - */ -void bmi2_delay_us(uint32_t period, void *intf_ptr) -{ - coines_delay_usec(period); -} - -/*! - * @brief Function to select the interface between SPI and I2C. - * Also to initialize coines platform - */ -int8_t bmi2_interface_init(struct bmi2_dev *bmi, uint8_t intf) -{ - int8_t rslt = BMI2_OK; - struct coines_board_info board_info; - - if (bmi != NULL) - { - int16_t result = coines_open_comm_intf(COINES_COMM_INTF_USB); - if (result < COINES_SUCCESS) - { - printf( - "\n Unable to connect with Application Board ! \n" " 1. Check if the board is connected and powered on. \n" " 2. Check if Application Board USB driver is installed. \n" - " 3. Check if board is in use by another application. (Insufficient permissions to access USB) \n"); - exit(result); - } - - result = coines_get_board_info(&board_info); - -#if defined(PC) - setbuf(stdout, NULL); -#endif - - if (result == COINES_SUCCESS) - { - if ((board_info.shuttle_id != BMI2XY_SHUTTLE_ID)) - { - printf("! Warning invalid sensor shuttle \n ," "This application will not support this sensor \n"); - exit(COINES_E_FAILURE); - } - } - - coines_set_shuttleboard_vdd_vddio_config(0, 0); - coines_delay_msec(100); - - /* Bus configuration : I2C */ - if (intf == BMI2_I2C_INTF) - { - printf("I2C Interface \n"); - - /* To initialize the user I2C function */ - dev_addr = BMI2_I2C_PRIM_ADDR; - bmi->intf = BMI2_I2C_INTF; - bmi->read = bmi2_i2c_read; - bmi->write = bmi2_i2c_write; - coines_config_i2c_bus(COINES_I2C_BUS_0, COINES_I2C_FAST_MODE); - } - /* Bus configuration : SPI */ - else if (intf == BMI2_SPI_INTF) - { - printf("SPI Interface \n"); - - /* To initialize the user SPI function */ - dev_addr = COINES_SHUTTLE_PIN_7; - bmi->intf = BMI2_SPI_INTF; - bmi->read = bmi2_spi_read; - bmi->write = bmi2_spi_write; - coines_config_spi_bus(COINES_SPI_BUS_0, COINES_SPI_SPEED_5_MHZ, COINES_SPI_MODE3); - coines_set_pin_config(COINES_SHUTTLE_PIN_7, COINES_PIN_DIRECTION_OUT, COINES_PIN_VALUE_HIGH); - } - - /* Assign device address to interface pointer */ - bmi->intf_ptr = &dev_addr; - - /* Configure delay in microseconds */ - bmi->delay_us = bmi2_delay_us; - - /* Configure max read/write length (in bytes) ( Supported length depends on target machine) */ - bmi->read_write_len = READ_WRITE_LEN; - - /* Assign to NULL to load the default config file. */ - bmi->config_file_ptr = NULL; - - coines_delay_usec(10000); - - coines_set_shuttleboard_vdd_vddio_config(3300, 3300); - - coines_delay_usec(10000); - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; - -} - -/*! - * @brief Prints the execution status of the APIs. - */ -void bmi2_error_codes_print_result(int8_t rslt) -{ - switch (rslt) - { - case BMI2_OK: - - /* Do nothing */ - break; - - case BMI2_W_FIFO_EMPTY: - printf("Warning [%d] : FIFO empty\r\n", rslt); - break; - case BMI2_W_PARTIAL_READ: - printf("Warning [%d] : FIFO partial read\r\n", rslt); - break; - case BMI2_E_NULL_PTR: - printf( - "Error [%d] : Null pointer error. It occurs when the user tries to assign value (not address) to a pointer," " which has been initialized to NULL.\r\n", - rslt); - break; - - case BMI2_E_COM_FAIL: - printf( - "Error [%d] : Communication failure error. It occurs due to read/write operation failure and also due " "to power failure during communication\r\n", - rslt); - break; - - case BMI2_E_DEV_NOT_FOUND: - printf("Error [%d] : Device not found error. It occurs when the device chip id is incorrectly read\r\n", - rslt); - break; - - case BMI2_E_INVALID_SENSOR: - printf( - "Error [%d] : Invalid sensor error. It occurs when there is a mismatch in the requested feature with the " "available one\r\n", - rslt); - break; - - case BMI2_E_SELF_TEST_FAIL: - printf( - "Error [%d] : Self-test failed error. It occurs when the validation of accel self-test data is " "not satisfied\r\n", - rslt); - break; - - case BMI2_E_INVALID_INT_PIN: - printf( - "Error [%d] : Invalid interrupt pin error. It occurs when the user tries to configure interrupt pins " "apart from INT1 and INT2\r\n", - rslt); - break; - - case BMI2_E_OUT_OF_RANGE: - printf( - "Error [%d] : Out of range error. It occurs when the data exceeds from filtered or unfiltered data from " "fifo and also when the range exceeds the maximum range for accel and gyro while performing FOC\r\n", - rslt); - break; - - case BMI2_E_ACC_INVALID_CFG: - printf( - "Error [%d] : Invalid Accel configuration error. It occurs when there is an error in accel configuration" " register which could be one among range, BW or filter performance in reg address 0x40\r\n", - rslt); - break; - - case BMI2_E_GYRO_INVALID_CFG: - printf( - "Error [%d] : Invalid Gyro configuration error. It occurs when there is a error in gyro configuration" "register which could be one among range, BW or filter performance in reg address 0x42\r\n", - rslt); - break; - - case BMI2_E_ACC_GYR_INVALID_CFG: - printf( - "Error [%d] : Invalid Accel-Gyro configuration error. It occurs when there is a error in accel and gyro" " configuration registers which could be one among range, BW or filter performance in reg address 0x40 " "and 0x42\r\n", - rslt); - break; - - case BMI2_E_CONFIG_LOAD: - printf( - "Error [%d] : Configuration load error. It occurs when failure observed while loading the configuration " "into the sensor\r\n", - rslt); - break; - - case BMI2_E_INVALID_PAGE: - printf( - "Error [%d] : Invalid page error. It occurs due to failure in writing the correct feature configuration " "from selected page\r\n", - rslt); - break; - - case BMI2_E_SET_APS_FAIL: - printf( - "Error [%d] : APS failure error. It occurs due to failure in write of advance power mode configuration " "register\r\n", - rslt); - break; - - case BMI2_E_AUX_INVALID_CFG: - printf( - "Error [%d] : Invalid AUX configuration error. It occurs when the auxiliary interface settings are not " "enabled properly\r\n", - rslt); - break; - - case BMI2_E_AUX_BUSY: - printf( - "Error [%d] : AUX busy error. It occurs when the auxiliary interface buses are engaged while configuring" " the AUX\r\n", - rslt); - break; - - case BMI2_E_REMAP_ERROR: - printf( - "Error [%d] : Remap error. It occurs due to failure in assigning the remap axes data for all the axes " "after change in axis position\r\n", - rslt); - break; - - case BMI2_E_GYR_USER_GAIN_UPD_FAIL: - printf( - "Error [%d] : Gyro user gain update fail error. It occurs when the reading of user gain update status " "fails\r\n", - rslt); - break; - - case BMI2_E_SELF_TEST_NOT_DONE: - printf( - "Error [%d] : Self-test not done error. It occurs when the self-test process is ongoing or not " "completed\r\n", - rslt); - break; - - case BMI2_E_INVALID_INPUT: - printf("Error [%d] : Invalid input error. It occurs when the sensor input validity fails\r\n", rslt); - break; - - case BMI2_E_INVALID_STATUS: - printf("Error [%d] : Invalid status error. It occurs when the feature/sensor validity fails\r\n", rslt); - break; - - case BMI2_E_CRT_ERROR: - printf("Error [%d] : CRT error. It occurs when the CRT test has failed\r\n", rslt); - break; - - case BMI2_E_ST_ALREADY_RUNNING: - printf( - "Error [%d] : Self-test already running error. It occurs when the self-test is already running and " "another has been initiated\r\n", - rslt); - break; - - case BMI2_E_CRT_READY_FOR_DL_FAIL_ABORT: - printf( - "Error [%d] : CRT ready for download fail abort error. It occurs when download in CRT fails due to wrong " "address location\r\n", - rslt); - break; - - case BMI2_E_DL_ERROR: - printf( - "Error [%d] : Download error. It occurs when write length exceeds that of the maximum burst length\r\n", - rslt); - break; - - case BMI2_E_PRECON_ERROR: - printf( - "Error [%d] : Pre-conditional error. It occurs when precondition to start the feature was not " "completed\r\n", - rslt); - break; - - case BMI2_E_ABORT_ERROR: - printf("Error [%d] : Abort error. It occurs when the device was shaken during CRT test\r\n", rslt); - break; - - case BMI2_E_WRITE_CYCLE_ONGOING: - printf( - "Error [%d] : Write cycle ongoing error. It occurs when the write cycle is already running and another " "has been initiated\r\n", - rslt); - break; - - case BMI2_E_ST_NOT_RUNING: - printf( - "Error [%d] : Self-test is not running error. It occurs when self-test running is disabled while it's " "running\r\n", - rslt); - break; - - case BMI2_E_DATA_RDY_INT_FAILED: - printf( - "Error [%d] : Data ready interrupt error. It occurs when the sample count exceeds the FOC sample limit " "and data ready status is not updated\r\n", - rslt); - break; - - case BMI2_E_INVALID_FOC_POSITION: - printf( - "Error [%d] : Invalid FOC position error. It occurs when average FOC data is obtained for the wrong" " axes\r\n", - rslt); - break; - - default: - printf("Error [%d] : Unknown error code\r\n", rslt); - break; - } -} - -/*! - * @brief Deinitializes coines platform - * - * @return void. - */ -void bmi2_coines_deinit(void) -{ - coines_close_comm_intf(COINES_COMM_INTF_USB); -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/common/common.h b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/common/common.h deleted file mode 100644 index 763983da4a..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/common/common.h +++ /dev/null @@ -1,122 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -#ifndef _COMMON_H -#define _COMMON_H - -/*! CPP guard */ -#ifdef __cplusplus -extern "C" { -#endif - -#include -#include "bmi2.h" - -/*! - * @brief Function for reading the sensor's registers through I2C bus. - * - * @param[in] reg_addr : Register address. - * @param[out] reg_data : Pointer to the data buffer to store the read data. - * @param[in] length : No of bytes to read. - * @param[in] intf_ptr : Interface pointer - * - * @return Status of execution - * @retval = BMI2_INTF_RET_SUCCESS -> Success - * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info - * - */ -BMI2_INTF_RETURN_TYPE bmi2_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr); - -/*! - * @brief Function for writing the sensor's registers through I2C bus. - * - * @param[in] reg_addr : Register address. - * @param[in] reg_data : Pointer to the data buffer whose value is to be written. - * @param[in] length : No of bytes to write. - * @param[in] intf_ptr : Interface pointer - * - * @return Status of execution - * @retval = BMI2_INTF_RET_SUCCESS -> Success - * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info - * - */ -BMI2_INTF_RETURN_TYPE bmi2_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr); - -/*! - * @brief Function for reading the sensor's registers through SPI bus. - * - * @param[in] reg_addr : Register address. - * @param[out] reg_data : Pointer to the data buffer to store the read data. - * @param[in] length : No of bytes to read. - * @param[in] intf_ptr : Interface pointer - * - * @return Status of execution - * @retval = BMI2_INTF_RET_SUCCESS -> Success - * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info - * - */ -BMI2_INTF_RETURN_TYPE bmi2_spi_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr); - -/*! - * @brief Function for writing the sensor's registers through SPI bus. - * - * @param[in] reg_addr : Register address. - * @param[in] reg_data : Pointer to the data buffer whose data has to be written. - * @param[in] length : No of bytes to write. - * @param[in] intf_ptr : Interface pointer - * - * @return Status of execution - * @retval = BMI2_INTF_RET_SUCCESS -> Success - * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info - * - */ -BMI2_INTF_RETURN_TYPE bmi2_spi_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr); - -/*! - * @brief This function provides the delay for required time (Microsecond) as per the input provided in some of the - * APIs. - * - * @param[in] period_us : The required wait time in microsecond. - * @param[in] intf_ptr : Interface pointer - * - * @return void. - * - */ -void bmi2_delay_us(uint32_t period, void *intf_ptr); - -/*! - * @brief Function to select the interface between SPI and I2C. - * - * @param[in] bma : Structure instance of bmi2_dev - * @param[in] intf : Interface selection parameter - * - * @return Status of execution - * @retval 0 -> Success - * @retval < 0 -> Failure Info - */ -int8_t bmi2_interface_init(struct bmi2_dev *bma, uint8_t intf); - -/*! - * @brief Prints the execution status of the APIs. - * - * @param[in] rslt : Error code returned by the API whose execution status has to be printed. - * - * @return void. - */ -void bmi2_error_codes_print_result(int8_t rslt); - -/*! - * @brief Deinitializes coines platform - * - * @return void. - */ -void bmi2_coines_deinit(void); - -#ifdef __cplusplus -} -#endif /* End of CPP guard */ - -#endif /* _COMMON_H */ diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_full_header_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_full_header_mode/Makefile deleted file mode 100644 index bde7c19627..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_full_header_mode/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= fifo_full_header_mode.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270_wh.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_full_header_mode/fifo_full_header_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_full_header_mode/fifo_full_header_mode.c deleted file mode 100644 index ab331eaeed..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_full_header_mode/fifo_full_header_mode.c +++ /dev/null @@ -1,316 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270_wh.h" -#include "common.h" - -/******************************************************************************/ -/*! Macros */ - -/*! Buffer size allocated to store raw FIFO data. */ -#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) - -/*! Length of data to be read from FIFO. */ -#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) - -/*! Number of accel frames to be extracted from FIFO. */ - -/*! Calculation for frame count: Total frame count = Fifo buffer size(2048)/ Total frames(6 Accel, 6 Gyro and 1 header, - * totaling to 13) which equals to 157. - */ -#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(157) - -/*! Number of gyro frames to be extracted from FIFO. */ -#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(157) - -/*! Macro to read sensortime byte in FIFO. */ -#define SENSORTIME_OVERHEAD_BYTE UINT8_C(3) - -/******************************************************************************/ -/*! Static Function Declaration */ - -/*! - * @brief This internal API is used to set configurations for accel and gyro. - * @param[in] dev : Structure instance of bmi2_dev. - * @return Status of execution. - */ -static int8_t set_accel_gyro_config(struct bmi2_dev *dev); - -/******************************************************************************/ - -/* This function starts the execution of program. */ -int main(void) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - uint16_t index = 0; - uint16_t fifo_length = 0; - uint16_t config = 0; - - /* To read sensortime, extra 3 bytes are added to fifo buffer. */ - uint16_t fifo_buffer_size = BMI2_FIFO_RAW_DATA_BUFFER_SIZE + SENSORTIME_OVERHEAD_BYTE; - - /* Variable to get fifo full interrupt status. */ - uint16_t int_status = 0; - - uint16_t accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; - - uint16_t gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; - - /* Variable to store the available frame length count in FIFO */ - uint8_t frame_count; - - int8_t try = 1; - - /* Number of bytes of FIFO data. */ - uint8_t fifo_data[fifo_buffer_size]; - - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Array of accelerometer frames -> Total bytes = - * 157 * (6 axes + 1 header bytes) = 1099 bytes */ - struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; - - /* Array of gyro frames -> Total bytes = - * 157 * (6 axes + 1 header bytes) = 1099 bytes */ - struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; - - /* Initialize FIFO frame structure. */ - struct bmi2_fifo_frame fifoframe = { 0 }; - - /* Accel and gyro sensor are listed in array. */ - uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270_wh. */ - rslt = bmi270_wh_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Enable accel and gyro sensor. */ - rslt = bmi270_wh_sensor_enable(sensor_sel, 2, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Configuration settings for accel and gyro. */ - rslt = set_accel_gyro_config(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Before setting FIFO, disable the advance power save mode. */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Initially disable all configurations in fifo. */ - rslt = bmi2_get_fifo_config(&config, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Initially disable all configurations in fifo. */ - rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Update FIFO structure. */ - /* Mapping the buffer to store the fifo data. */ - fifoframe.data = fifo_data; - - /* Length of FIFO frame. */ - /* To read sensortime, extra 3 bytes are added to fifo user length. */ - fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH + SENSORTIME_OVERHEAD_BYTE; - - /* Set FIFO configuration by enabling accel, gyro and timestamp. - * NOTE 1: The header mode is enabled by default. - * NOTE 2: By default the FIFO operating mode is in FIFO mode. - * NOTE 3: Sensortime is enabled by default */ - printf("FIFO is configured in header mode\n"); - rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Map FIFO full interrupt. */ - fifoframe.data_int_map = BMI2_FFULL_INT; - rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - while (try <= 10) - { - /* Read FIFO data on interrupt. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if ((rslt == BMI2_OK) && (int_status & BMI2_FFULL_INT_STATUS_MASK)) - { - printf("\nIteration : %d\n", try); - - accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; - - gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; - - rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - printf("\nFIFO data bytes available : %d \n", fifo_length); - printf("\nFIFO data bytes requested : %d \n", fifoframe.length); - - /* Read FIFO data. */ - rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Read FIFO data on interrupt. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - printf("\nFIFO accel frames requested : %d \n", accel_frame_length); - - /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ - rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); - printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); - - printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); - - /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ - rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); - printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); - - /* Calculating the frame count from the available bytes in FIFO - * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len + header_byte)) */ - frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH + 1)); - printf("\nAvailable frame count from available bytes: %d\n", frame_count); - - printf("\nExtracted accel frames\n"); - - if (accel_frame_length > frame_count) - { - accel_frame_length = frame_count; - } - - /* Print the parsed accelerometer data from the FIFO buffer. */ - for (index = 0; index < accel_frame_length; index++) - { - printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, - fifo_accel_data[index].y, fifo_accel_data[index].z); - } - - printf("\nExtracted gyro frames\n"); - - if (gyro_frame_length > frame_count) - { - gyro_frame_length = frame_count; - } - - /* Print the parsed gyro data from the FIFO buffer. */ - for (index = 0; index < gyro_frame_length; index++) - { - printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", - index, - fifo_gyro_data[index].x, - fifo_gyro_data[index].y, - fifo_gyro_data[index].z); - } - - /* Print control frames like sensor time and skipped frame count. */ - printf("\nSkipped frame count = %d\n", fifoframe.skipped_frame_count); - - printf("Sensor time = %lu\r\n", fifoframe.sensor_time); - - try++; - } - } - } - - bmi2_coines_deinit(); - - return rslt; -} - -/*! - * @brief This internal API is used to set configurations for accel and gyro. - */ -static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Structure to define accel and gyro configurations. */ - struct bmi2_sens_config config[2]; - - /* Configure the type of feature. */ - config[0].type = BMI2_ACCEL; - config[1].type = BMI2_GYRO; - - /* Get default configurations for the type of feature selected. */ - rslt = bmi270_wh_get_sensor_config(config, 2, bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - - /* NOTE: The user can change the following configuration parameters according to their requirement. */ - /* Accel configuration settings. */ - /* Set Output Data Rate */ - config[0].cfg.acc.odr = BMI2_ACC_ODR_25HZ; - - /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ - config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; - - /* The bandwidth parameter is used to configure the number of sensor samples that are averaged - * if it is set to 2, then 2^(bandwidth parameter) samples - * are averaged, resulting in 4 averaged samples - * Note1 : For more information, refer the datasheet. - * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but - * this has an adverse effect on the power consumed. - */ - config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; - - /* Enable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - * For more info refer datasheet. - */ - config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; - - /* Gyro configuration settings. */ - /* Set Output Data Rate */ - config[1].cfg.gyr.odr = BMI2_GYR_ODR_25HZ; - - /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ - config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; - - /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ - config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; - - /* Enable/Disable the noise performance mode for precision yaw rate sensing - * There are two modes - * 0 -> Ultra low power mode(Default) - * 1 -> High performance mode - */ - config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; - - /* Enable/Disable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - */ - config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; - - /* Set new configurations. */ - rslt = bmi270_wh_set_sensor_config(config, 2, bmi2_dev); - bmi2_error_codes_print_result(rslt); - } - - return rslt; -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_full_headerless_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_full_headerless_mode/Makefile deleted file mode 100644 index 5c407e9094..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_full_headerless_mode/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= fifo_full_headerless_mode.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270_wh.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_full_headerless_mode/fifo_full_headerless_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_full_headerless_mode/fifo_full_headerless_mode.c deleted file mode 100644 index 14b0b22b3d..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_full_headerless_mode/fifo_full_headerless_mode.c +++ /dev/null @@ -1,304 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270_wh.h" -#include "common.h" - -/******************************************************************************/ -/*! Macros */ - -/*! Buffer size allocated to store raw FIFO data */ -#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) - -/*! Length of data to be read from FIFO */ -#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) - -/*! Number of accel frames to be extracted from FIFO */ - -/*! Calculation for frame count: Total frame count = Fifo buffer size(2048)/ Total frames(6 Accel, 6 Gyro totaling to - * 12) which equals to 170. - */ -#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(169) - -/*! Number of gyro frames to be extracted from FIFO */ -#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(169) - -/******************************************************************************/ -/*! Static Function Declaration */ - -/*! - * @brief This internal API is used to set configurations for accel and gyro. - * @param[in] dev : Structure instance of bmi2_dev. - * @return Status of execution. - */ -static int8_t set_accel_gyro_config(struct bmi2_dev *dev); - -/******************************************************************************/ - -/* This function starts the execution of program. */ -int main(void) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - uint16_t index = 0; - - uint16_t fifo_length = 0; - - uint8_t try = 1; - - /* Variable to get fifo full interrupt status. */ - uint16_t int_status = 0; - - uint16_t accel_frame_length; - - uint16_t gyro_frame_length; - - /* Variable to store the available frame length count in FIFO */ - uint8_t frame_count; - - /* Number of bytes of FIFO data. */ - uint8_t fifo_data[BMI2_FIFO_RAW_DATA_BUFFER_SIZE] = { 0 }; - - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Array of accelerometer frames -> Total bytes = - * 170 * (6 axes bytes) = 1020 bytes */ - struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; - - /* Array of gyro frames -> Total bytes = - * 170 * (6 axes bytes) = 1020 bytes */ - struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; - - /* Initialize FIFO frame structure. */ - struct bmi2_fifo_frame fifoframe = { 0 }; - - /* Accel and gyro sensor are listed in array. */ - uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270_wh. */ - rslt = bmi270_wh_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Enable accel and gyro sensor. */ - rslt = bmi270_wh_sensor_enable(sensor_sel, 2, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Configuration settings for accel and gyro. */ - rslt = set_accel_gyro_config(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Before setting FIFO, disable the advance power save mode. */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Initially disable all configurations in fifo. */ - rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Update FIFO structure. */ - /* Mapping the buffer to store the fifo data. */ - fifoframe.data = fifo_data; - - /* Length of FIFO frame. */ - fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH; - - /* Set FIFO configuration by enabling accel, gyro. - * NOTE 1: The header mode is enabled by default. - * NOTE 2: By default the FIFO operating mode is FIFO mode. */ - rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - printf("FIFO is configured in headerless mode\n"); - - /* To enable headerless mode, disable the header. */ - if (rslt == BMI2_OK) - { - rslt = bmi2_set_fifo_config(BMI2_FIFO_HEADER_EN, BMI2_DISABLE, &bmi2_dev); - } - - /* Map FIFO full interrupt. */ - fifoframe.data_int_map = BMI2_FFULL_INT; - rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - while (try <= 10) - { - /* Read FIFO data on interrupt. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if ((rslt == BMI2_OK) && (int_status & BMI2_FFULL_INT_STATUS_MASK)) - { - printf("\nIteration : %d\n", try); - - rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; - gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; - - printf("\nFIFO data bytes available : %d \n", fifo_length); - printf("\nFIFO data bytes requested : %d \n", fifoframe.length); - - /* Read FIFO data. */ - rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Read FIFO data on interrupt. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - printf("\nFIFO accel frames requested : %d \n", accel_frame_length); - - /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ - rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); - printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); - - printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); - - /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ - rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); - printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); - - /* Calculating the frame count from the available bytes in FIFO - * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len)) */ - frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH)); - printf("\nAvailable frame count from available bytes: %d\n", frame_count); - - printf("\nExtracted accel frames\n"); - - if (accel_frame_length > frame_count) - { - accel_frame_length = frame_count; - } - - /* Print the parsed accelerometer data from the FIFO buffer. */ - for (index = 0; index < accel_frame_length; index++) - { - printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, - fifo_accel_data[index].y, fifo_accel_data[index].z); - } - - printf("\nExtracted gyro frames\n"); - - if (gyro_frame_length > frame_count) - { - gyro_frame_length = frame_count; - } - - /* Print the parsed gyro data from the FIFO buffer. */ - for (index = 0; index < gyro_frame_length; index++) - { - printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", - index, - fifo_gyro_data[index].x, - fifo_gyro_data[index].y, - fifo_gyro_data[index].z); - } - } - - try++; - } - } - - bmi2_coines_deinit(); - - return rslt; -} - -/*! - * @brief This internal API is used to set configurations for accel and gyro. - */ -static int8_t set_accel_gyro_config(struct bmi2_dev *bmi2_dev) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Structure to define accel and gyro configurations. */ - struct bmi2_sens_config config[2]; - - /* Configure the type of feature. */ - config[0].type = BMI2_ACCEL; - config[1].type = BMI2_GYRO; - - /* Get default configurations for the type of feature selected. */ - rslt = bmi270_wh_get_sensor_config(config, 2, bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* NOTE: The user can change the following configuration parameters according to their requirement. */ - /* Accel configuration settings. */ - /* Set Output Data Rate */ - config[0].cfg.acc.odr = BMI2_ACC_ODR_100HZ; - - /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ - config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; - - /* The bandwidth parameter is used to configure the number of sensor samples that are averaged - * if it is set to 2, then 2^(bandwidth parameter) samples - * are averaged, resulting in 4 averaged samples - * Note1 : For more information, refer the datasheet. - * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but - * this has an adverse effect on the power consumed. - */ - config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; - - /* Enable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - * For more info refer datasheet. - */ - config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; - - /* Gyro configuration settings. */ - /* Set Output Data Rate */ - config[1].cfg.gyr.odr = BMI2_GYR_ODR_100HZ; - - /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ - config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; - - /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ - config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; - - /* Enable/Disable the noise performance mode for precision yaw rate sensing - * There are two modes - * 0 -> Ultra low power mode(Default) - * 1 -> High performance mode - */ - config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; - - /* Enable/Disable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - */ - config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; - - /* Set new configurations. */ - rslt = bmi270_wh_set_sensor_config(config, 2, bmi2_dev); - bmi2_error_codes_print_result(rslt); - } - - return rslt; -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_watermark_header_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_watermark_header_mode/Makefile deleted file mode 100644 index e0a42e369a..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_watermark_header_mode/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= fifo_watermark_header_mode.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270_wh.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_watermark_header_mode/fifo_watermark_header_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_watermark_header_mode/fifo_watermark_header_mode.c deleted file mode 100644 index f33bb5ebff..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_watermark_header_mode/fifo_watermark_header_mode.c +++ /dev/null @@ -1,335 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270_wh.h" -#include "common.h" - -/******************************************************************************/ -/*! Macros */ - -/*! Buffer size allocated to store raw FIFO data */ -#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) - -/*! Length of data to be read from FIFO */ -#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) - -/*! Number of accel frames to be extracted from FIFO */ - -/*! - * Calculation: - * fifo_watermark_level = 650, accel_frame_len = 6, gyro_frame_len = 6 header_byte = 1. - * fifo_accel_frame_count = (650 / (6 + 6 + 1 )) = 50 frames - * NOTE: Extra frames are read in order to get sensor time - */ -#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(55) - -/*! Number of gyro frames to be extracted from FIFO */ -#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(55) - -/*! Setting a watermark level in FIFO */ -#define BMI270_FIFO_WATERMARK_LEVEL UINT16_C(650) - -/*! Macro to read sensortime byte in FIFO */ -#define SENSORTIME_OVERHEAD_BYTE UINT8_C(3) - -/******************************************************************************/ -/*! Static Function Declaration */ - -/*! - * @brief This internal API is used to set configurations for accel and gyro. - * @param[in] dev : Structure instance of bmi2_dev. - * @return Status of execution. - */ -static int8_t set_accel_gyro_config(struct bmi2_dev *dev); - -/******************************************************************************/ -/*! Functions */ - -/* This function starts the execution of program. */ -int main(void) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Variable to store the available frame length count in FIFO */ - uint8_t frame_count; - - /* Variable to index bytes. */ - uint16_t index = 0; - - uint16_t fifo_length = 0; - - uint16_t accel_frame_length; - - uint16_t gyro_frame_length; - - uint8_t try = 1; - - /* To read sensortime, extra 3 bytes are added to fifo buffer */ - uint16_t fifo_buffer_size = BMI2_FIFO_RAW_DATA_BUFFER_SIZE + SENSORTIME_OVERHEAD_BYTE; - - /* Number of bytes of FIFO data. */ - uint8_t fifo_data[fifo_buffer_size]; - - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Array of accelerometer frames -> Total bytes = - * 55 * (6 axes bytes) = 330 bytes */ - struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; - - /* Array of gyro frames -> Total bytes = - * 55 * (6 axes bytes) = 330 bytes */ - struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; - - /* Initialize FIFO frame structure. */ - struct bmi2_fifo_frame fifoframe = { 0 }; - - /* Accel and gyro sensor are listed in array. */ - uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; - - /* Variable to get fifo water-mark interrupt status. */ - uint16_t int_status = 0; - - uint16_t watermark = 0; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270_wh. */ - rslt = bmi270_wh_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Enable accel and gyro sensor. */ - rslt = bmi270_wh_sensor_enable(sensor_sel, 2, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Configuration settings for accel and gyro. */ - rslt = set_accel_gyro_config(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Before setting FIFO, disable the advance power save mode. */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Initially disable all configurations in fifo. */ - rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Update FIFO structure. */ - /* Mapping the buffer to store the fifo data. */ - fifoframe.data = fifo_data; - - /* Length of FIFO frame. */ - /* To read sensortime, extra 3 bytes are added to fifo user length. */ - fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH + SENSORTIME_OVERHEAD_BYTE; - - /* Set FIFO configuration by enabling accel, gyro and timestamp. - * NOTE 1: The header mode is enabled by default. - * NOTE 2: By default the FIFO operating mode is FIFO mode. - * NOTE 3: Sensortime is enabled by default */ - printf("FIFO is configured in header mode\n"); - rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* FIFO water-mark interrupt is enabled. */ - fifoframe.data_int_map = BMI2_FWM_INT; - - /* Map water-mark interrupt to the required interrupt pin. */ - rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Set water-mark level. */ - fifoframe.wm_lvl = BMI270_FIFO_WATERMARK_LEVEL; - - /* Set the water-mark level if water-mark interrupt is mapped. */ - rslt = bmi2_set_fifo_wm(fifoframe.wm_lvl, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - rslt = bmi2_get_fifo_wm(&watermark, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - printf("\nFIFO watermark level is %d\n", watermark); - - while (try <= 10) - { - /* Read FIFO data on interrupt. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* To check the status of FIFO watermark interrupt. */ - if ((rslt == BMI2_OK) && (int_status & BMI2_FWM_INT_STATUS_MASK)) - { - printf("\nIteration : %d\n", try); - - printf("\nWatermark interrupt occurred\n"); - - rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; - gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; - - printf("\nFIFO data bytes available : %d \n", fifo_length); - printf("\nFIFO data bytes requested : %d \n", fifoframe.length); - - /* Read FIFO data. */ - rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Read FIFO data on interrupt. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - printf("\nFIFO accel frames requested : %d \n", accel_frame_length); - - /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ - rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); - printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); - - printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); - - /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ - rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); - printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); - - /* Calculating the frame count from the available bytes in FIFO - * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len + header_byte)) */ - frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH + 1)); - printf("\nAvailable frame count from available bytes: %d\n", frame_count); - - printf("\nExracted accel frames\n"); - - if (accel_frame_length > frame_count) - { - accel_frame_length = frame_count; - } - - /* Print the parsed accelerometer data from the FIFO buffer. */ - for (index = 0; index < accel_frame_length; index++) - { - printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, - fifo_accel_data[index].y, fifo_accel_data[index].z); - } - - printf("\nExtracted gyro frames\n"); - - if (gyro_frame_length > frame_count) - { - gyro_frame_length = frame_count; - } - - /* Print the parsed gyro data from the FIFO buffer. */ - for (index = 0; index < gyro_frame_length; index++) - { - printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", - index, - fifo_gyro_data[index].x, - fifo_gyro_data[index].y, - fifo_gyro_data[index].z); - } - - /* Print control frames like sensor time and skipped frame count. */ - printf("Skipped frame count = %d\n", fifoframe.skipped_frame_count); - printf("Sensor time = %lu\r\n", fifoframe.sensor_time); - - try++; - } - } - } - - bmi2_coines_deinit(); - - return rslt; -} - -/*! - * @brief This internal API is used to set configurations for accel and gyro. - */ -static int8_t set_accel_gyro_config(struct bmi2_dev *dev) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Structure to define accel and gyro configurations. */ - struct bmi2_sens_config config[2]; - - /* Configure the type of feature. */ - config[0].type = BMI2_ACCEL; - config[1].type = BMI2_GYRO; - - /* Get default configurations for the type of feature selected. */ - rslt = bmi270_wh_get_sensor_config(config, 2, dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* NOTE: The user can change the following configuration parameters according to their requirement. */ - /* Accel configuration settings. */ - /* Set Output Data Rate */ - config[0].cfg.acc.odr = BMI2_ACC_ODR_25HZ; - - /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ - config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; - - /* The bandwidth parameter is used to configure the number of sensor samples that are averaged - * if it is set to 2, then 2^(bandwidth parameter) samples - * are averaged, resulting in 4 averaged samples - * Note1 : For more information, refer the datasheet. - * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but - * this has an adverse effect on the power consumed. - */ - config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; - - /* Enable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - * For more info refer datasheet. - */ - config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; - - /* Gyro configuration settings. */ - /* Set Output Data Rate */ - config[1].cfg.gyr.odr = BMI2_GYR_ODR_25HZ; - - /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ - config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; - - /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ - config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; - - /* Enable/Disable the noise performance mode for precision yaw rate sensing - * There are two modes - * 0 -> Ultra low power mode(Default) - * 1 -> High performance mode - */ - config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; - - /* Enable/Disable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - */ - config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; - - /* Set new configurations. */ - rslt = bmi270_wh_set_sensor_config(config, 2, dev); - bmi2_error_codes_print_result(rslt); - } - - return rslt; -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_watermark_headerless_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_watermark_headerless_mode/Makefile deleted file mode 100644 index ce97737eab..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_watermark_headerless_mode/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= fifo_watermark_headerless_mode.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270_wh.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c deleted file mode 100644 index 160c01de5b..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/fifo_watermark_headerless_mode/fifo_watermark_headerless_mode.c +++ /dev/null @@ -1,331 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270_wh.h" -#include "common.h" - -/******************************************************************************/ -/*! Macros */ - -/*! Buffer size allocated to store raw FIFO data */ -#define BMI2_FIFO_RAW_DATA_BUFFER_SIZE UINT16_C(2048) - -/*! Length of data to be read from FIFO */ -#define BMI2_FIFO_RAW_DATA_USER_LENGTH UINT16_C(2048) - -/*! Number of accel frames to be extracted from FIFO */ - -/*! - * Calculation: - * fifo_watermark_level = 650, accel_frame_len = 6, gyro_frame_len = 6. - * fifo_accel_frame_count = (650 / (6 + 6 )) = 54 frames - */ -#define BMI2_FIFO_ACCEL_FRAME_COUNT UINT8_C(55) - -/*! Number of gyro frames to be extracted from FIFO */ -#define BMI2_FIFO_GYRO_FRAME_COUNT UINT8_C(55) - -/*! Setting a watermark level in FIFO */ -#define BMI270_FIFO_WATERMARK_LEVEL UINT16_C(650) - -/******************************************************************************/ -/*! Static Function Declaration */ - -/*! - * @brief This internal API is used to set configurations for accel and gyro. - * @param[in] dev : Structure instance of bmi2_dev. - * @return Status of execution. - */ -static int8_t set_accel_gyro_config(struct bmi2_dev *dev); - -/******************************************************************************/ -/*! Functions */ - -/* This function starts the execution of program. */ -int main(void) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Variable to index bytes. */ - uint16_t index = 0; - - /* Variable to store the available frame length count in FIFO */ - uint8_t frame_count; - - uint16_t fifo_length = 0; - - uint16_t accel_frame_length; - - uint16_t gyro_frame_length; - - uint8_t try = 1; - - /* Number of bytes of FIFO data. */ - uint8_t fifo_data[BMI2_FIFO_RAW_DATA_BUFFER_SIZE] = { 0 }; - - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Array of accelerometer frames -> Total bytes = - * 50 * (6 axes bytes) = 300 bytes */ - struct bmi2_sens_axes_data fifo_accel_data[BMI2_FIFO_ACCEL_FRAME_COUNT] = { { 0 } }; - - /* Array of gyro frames -> Total bytes = - * 50 * (6 axes bytes) = 300 bytes */ - struct bmi2_sens_axes_data fifo_gyro_data[BMI2_FIFO_GYRO_FRAME_COUNT] = { { 0 } }; - - /* Initialize FIFO frame structure. */ - struct bmi2_fifo_frame fifoframe = { 0 }; - - /* Accel and gyro sensor are listed in array. */ - uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_GYRO }; - - /* Variable to get fifo water-mark interrupt status. */ - uint16_t int_status = 0; - - uint16_t watermark = 0; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270_wh. */ - rslt = bmi270_wh_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Enable accel and gyro sensor. */ - rslt = bmi270_wh_sensor_enable(sensor_sel, 2, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Configuration settings for accel and gyro. */ - rslt = set_accel_gyro_config(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Before setting FIFO, disable the advance power save mode. */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Initially disable all configurations in fifo. */ - rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Update FIFO structure. */ - /* Mapping the buffer to store the fifo data. */ - fifoframe.data = fifo_data; - - /* Length of FIFO frame. */ - fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH; - - /* Set FIFO configuration by enabling accel, gyro. - * NOTE 1: The header mode is enabled by default. - * NOTE 2: By default the FIFO operating mode is FIFO mode. */ - rslt = bmi2_set_fifo_config(BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN, BMI2_ENABLE, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - printf("FIFO is configured in headerless mode\n"); - - /* To enable headerless mode, disable the header. */ - if (rslt == BMI2_OK) - { - rslt = bmi2_set_fifo_config(BMI2_FIFO_HEADER_EN, BMI2_DISABLE, &bmi2_dev); - } - - /* FIFO water-mark interrupt is enabled. */ - fifoframe.data_int_map = BMI2_FWM_INT; - - /* Map water-mark interrupt to the required interrupt pin. */ - rslt = bmi2_map_data_int(fifoframe.data_int_map, BMI2_INT1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Set water-mark level. */ - fifoframe.wm_lvl = BMI270_FIFO_WATERMARK_LEVEL; - - fifoframe.length = BMI2_FIFO_RAW_DATA_USER_LENGTH; - - /* Set the water-mark level if water-mark interrupt is mapped. */ - rslt = bmi2_set_fifo_wm(fifoframe.wm_lvl, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - rslt = bmi2_get_fifo_wm(&watermark, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - printf("\nFIFO watermark level is %d\n", watermark); - - while (try <= 10) - { - /* Read FIFO data on interrupt. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* To check the status of FIFO watermark interrupt. */ - if ((rslt == BMI2_OK) && (int_status & BMI2_FWM_INT_STATUS_MASK)) - { - printf("\nIteration : %d\n", try); - - printf("\nWatermark interrupt occurred\n"); - - rslt = bmi2_get_fifo_length(&fifo_length, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - accel_frame_length = BMI2_FIFO_ACCEL_FRAME_COUNT; - gyro_frame_length = BMI2_FIFO_GYRO_FRAME_COUNT; - - printf("\nFIFO data bytes available : %d \n", fifo_length); - printf("\nFIFO data bytes requested : %d \n", fifoframe.length); - - /* Read FIFO data. */ - rslt = bmi2_read_fifo_data(&fifoframe, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Read FIFO data on interrupt. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - printf("\nFIFO accel frames requested : %d \n", accel_frame_length); - - /* Parse the FIFO data to extract accelerometer data from the FIFO buffer. */ - rslt = bmi2_extract_accel(fifo_accel_data, &accel_frame_length, &fifoframe, &bmi2_dev); - printf("\nFIFO accel frames extracted : %d \n", accel_frame_length); - - printf("\nFIFO gyro frames requested : %d \n", gyro_frame_length); - - /* Parse the FIFO data to extract gyro data from the FIFO buffer. */ - rslt = bmi2_extract_gyro(fifo_gyro_data, &gyro_frame_length, &fifoframe, &bmi2_dev); - printf("\nFIFO gyro frames extracted : %d \n", gyro_frame_length); - - /* Calculating the frame count from the available bytes in FIFO - * frame_length = (available_fifo_bytes / (acc_frame_len + gyro_frame_len)) */ - frame_count = (fifo_length / (BMI2_FIFO_ACC_GYR_LENGTH)); - printf("\nAvailable frame count from available bytes: %d\n", frame_count); - - printf("\nExracted accel frames\n"); - - if (accel_frame_length > frame_count) - { - accel_frame_length = frame_count; - } - - /* Print the parsed accelerometer data from the FIFO buffer. */ - for (index = 0; index < accel_frame_length; index++) - { - printf("ACCEL[%d] X : %d\t Y : %d\t Z : %d\n", index, fifo_accel_data[index].x, - fifo_accel_data[index].y, fifo_accel_data[index].z); - } - - printf("\nExtracted gyro frames\n"); - - if (gyro_frame_length > frame_count) - { - gyro_frame_length = frame_count; - } - - /* Print the parsed gyro data from the FIFO buffer. */ - for (index = 0; index < gyro_frame_length; index++) - { - printf("GYRO[%d] X : %d\t Y : %d\t Z : %d\n", - index, - fifo_gyro_data[index].x, - fifo_gyro_data[index].y, - fifo_gyro_data[index].z); - } - } - - try++; - } - } - - bmi2_coines_deinit(); - - return rslt; -} - -/*! - * @brief This internal API is used to set configurations for accel. - */ -static int8_t set_accel_gyro_config(struct bmi2_dev *dev) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Structure to define accel and gyro configurations. */ - struct bmi2_sens_config config[2]; - - /* Configure the type of feature. */ - config[0].type = BMI2_ACCEL; - config[1].type = BMI2_GYRO; - - /* Get default configurations for the type of feature selected. */ - rslt = bmi270_wh_get_sensor_config(config, 2, dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* NOTE: The user can change the following configuration parameter according to their requirement. */ - /* Accel configuration settings. */ - /* Set Output Data Rate */ - config[0].cfg.acc.odr = BMI2_ACC_ODR_100HZ; - - /* Gravity range of the sensor (+/- 2G, 4G, 8G, 16G). */ - config[0].cfg.acc.range = BMI2_ACC_RANGE_2G; - - /* The bandwidth parameter is used to configure the number of sensor samples that are averaged - * if it is set to 2, then 2^(bandwidth parameter) samples - * are averaged, resulting in 4 averaged samples - * Note1 : For more information, refer the datasheet. - * Note2 : A higher number of averaged samples will result in a lower noise level of the signal, but - * this has an adverse effect on the power consumed. - */ - config[0].cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; - - /* Enable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - * For more info refer datasheet. - */ - config[0].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; - - /* Gyro configuration settings. */ - /* Set Output Data Rate */ - config[1].cfg.gyr.odr = BMI2_GYR_ODR_100HZ; - - /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ - config[1].cfg.gyr.range = BMI2_GYR_RANGE_2000; - - /* Gyroscope Bandwidth parameters. By default the gyro bandwidth is in normal mode. */ - config[1].cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; - - /* Enable/Disable the noise performance mode for precision yaw rate sensing - * There are two modes - * 0 -> Ultra low power mode(Default) - * 1 -> High performance mode - */ - config[1].cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; - - /* Enable/Disable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - */ - config[1].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; - - /* Set new configurations. */ - rslt = bmi270_wh_set_sensor_config(config, 2, dev); - bmi2_error_codes_print_result(rslt); - } - - return rslt; -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/gyro/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/gyro/Makefile deleted file mode 100644 index 9192877d58..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/gyro/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= gyro.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270_wh.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/gyro/gyro.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/gyro/gyro.c deleted file mode 100644 index d1ce9fe16f..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/gyro/gyro.c +++ /dev/null @@ -1,195 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270_wh.h" -#include "common.h" - -/******************************************************************************/ -/*! Static Function Declaration */ - -/*! - * @brief This internal API is used to set configurations for gyro. - * - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Status of execution. - */ -static int8_t set_gyro_config(struct bmi2_dev *dev); - -/*! - * @brief This function converts lsb to degree per second for 16 bit gyro at - * range 125, 250, 500, 1000 or 2000dps. - * - * @param[in] val : LSB from each axis. - * @param[in] dps : Degree per second. - * @param[in] bit_width : Resolution for gyro. - * - * @return Degree per second. - */ -static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width); - -/******************************************************************************/ -/*! Functions */ - -/* This function starts the execution of program. */ -int main(void) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Variable to define limit to print gyro data. */ - uint8_t limit = 10; - - uint8_t indx = 0; - - float x = 0, y = 0, z = 0; - - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Create an instance of sensor data structure. */ - struct bmi2_sensor_data sensor_data = { 0 }; - - /* Assign gyro sensor to variable. */ - uint8_t sens_list = BMI2_GYRO; - - /* Initialize the interrupt status of gyro. */ - uint16_t int_status = 0; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270_wh. */ - rslt = bmi270_wh_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Enable the selected sensors. */ - rslt = bmi270_wh_sensor_enable(&sens_list, 1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Gyro configuration settings. */ - rslt = set_gyro_config(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Select gyro sensor. */ - sensor_data.type = BMI2_GYRO; - printf("Gyro and DPS data\n"); - printf("Gyro data collected at Range 2000 dps with 16-bit resolution\n"); - - /* Loop to print gyro data when interrupt occurs. */ - while (indx <= limit) - { - /* To get the data ready interrupt status of gyro. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* To check the data ready interrupt status and print the status for 10 samples. */ - if (int_status & BMI2_GYR_DRDY_INT_MASK) - { - /* Get gyro data for x, y and z axis. */ - rslt = bmi270_wh_get_sensor_data(&sensor_data, 1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - printf("\nGyr_X = %d\t", sensor_data.sens_data.gyr.x); - printf("Gyr_Y = %d\t", sensor_data.sens_data.gyr.y); - printf("Gyr_Z = %d\r\n", sensor_data.sens_data.gyr.z); - - /* Converting lsb to degree per second for 16 bit gyro at 2000dps range. */ - x = lsb_to_dps(sensor_data.sens_data.gyr.x, 2000, bmi2_dev.resolution); - y = lsb_to_dps(sensor_data.sens_data.gyr.y, 2000, bmi2_dev.resolution); - z = lsb_to_dps(sensor_data.sens_data.gyr.z, 2000, bmi2_dev.resolution); - - /* Print the data in dps. */ - printf("Gyr_DPS_X = %4.2f, Gyr_DPS_Y = %4.2f, Gyr_DPS_Z = %4.2f\n", x, y, z); - - indx++; - } - } - } - } - - bmi2_coines_deinit(); - - return rslt; -} - -/*! - * @brief This internal API is used to set configurations for gyro. - */ -static int8_t set_gyro_config(struct bmi2_dev *dev) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Structure to define the type of sensor and its configurations. */ - struct bmi2_sens_config config; - - /* Configure the type of feature. */ - config.type = BMI2_GYRO; - - /* Get default configurations for the type of feature selected. */ - rslt = bmi270_wh_get_sensor_config(&config, 1, dev); - bmi2_error_codes_print_result(rslt); - - /* Map data ready interrupt to interrupt pin. */ - rslt = bmi2_map_data_int(BMI2_DRDY_INT, BMI2_INT2, dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* The user can change the following configuration parameters according to their requirement. */ - /* Set Output Data Rate */ - config.cfg.gyr.odr = BMI2_GYR_ODR_200HZ; - - /* Gyroscope Angular Rate Measurement Range.By default the range is 2000dps. */ - config.cfg.gyr.range = BMI2_GYR_RANGE_2000; - - /* Gyroscope bandwidth parameters. By default the gyro bandwidth is in normal mode. */ - config.cfg.gyr.bwp = BMI2_GYR_NORMAL_MODE; - - /* Enable/Disable the noise performance mode for precision yaw rate sensing - * There are two modes - * 0 -> Ultra low power mode(Default) - * 1 -> High performance mode - */ - config.cfg.gyr.noise_perf = BMI2_POWER_OPT_MODE; - - /* Enable/Disable the filter performance mode where averaging of samples - * will be done based on above set bandwidth and ODR. - * There are two modes - * 0 -> Ultra low power mode - * 1 -> High performance mode(Default) - */ - config.cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; - - /* Set the gyro configurations. */ - rslt = bmi270_wh_set_sensor_config(&config, 1, dev); - } - - return rslt; -} - -/*! - * @brief This function converts lsb to degree per second for 16 bit gyro at - * range 125, 250, 500, 1000 or 2000dps. - */ -static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width) -{ - float half_scale = ((float)(1 << bit_width) / 2.0f); - - return (dps / ((half_scale) + BMI2_GYR_RANGE_2000)) * (val); -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/no_motion_interrupt/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/no_motion_interrupt/Makefile deleted file mode 100644 index 61b7016bb2..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/no_motion_interrupt/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= no_motion_interrupt.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270_wh.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/no_motion_interrupt/no_motion_interrupt.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/no_motion_interrupt/no_motion_interrupt.c deleted file mode 100644 index 5f584de28c..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/no_motion_interrupt/no_motion_interrupt.c +++ /dev/null @@ -1,135 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270_wh.h" -#include "common.h" - -/******************************************************************************/ -/*! Static Function Declaration */ - -/*! - * @brief This internal API is used to set configurations for no-motion. - * - * @param[in] bmi2_dev : Structure instance of bmi2_dev. - * - * @return Status of execution. - */ -static int8_t set_feature_config(struct bmi2_dev *bmi2_dev); - -/******************************************************************************/ -/*! Functions */ - -/* This function starts the execution of program. */ -int main(void) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Accel sensor and no-motion feature are listed in array. */ - uint8_t sens_list[2] = { BMI2_ACCEL, BMI2_NO_MOTION }; - - /* Variable to get no-motion interrupt status. */ - uint16_t int_status = 0; - - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Select features and their pins to be mapped to. */ - struct bmi2_sens_int_config sens_int = { .type = BMI2_NO_MOTION, .hw_int_pin = BMI2_INT1 }; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270_wh. */ - rslt = bmi270_wh_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Enable the selected sensors. */ - rslt = bmi270_wh_sensor_enable(sens_list, 2, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Set feature configurations for no-motion. */ - rslt = set_feature_config(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Map the feature interrupt for no-motion. */ - rslt = bmi270_wh_map_feat_int(&sens_int, 1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - printf("Do not move the board\n"); - - /* Loop to get no-motion interrupt. */ - do - { - /* Clear buffer. */ - int_status = 0; - - /* To get the interrupt status of no-motion. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* To check the interrupt status of no-motion. */ - if (int_status & BMI270_WH_NO_MOT_STATUS_MASK) - { - printf("No-motion interrupt is generated\n"); - break; - } - } while (rslt == BMI2_OK); - } - } - } - - bmi2_coines_deinit(); - - return rslt; -} - -/*! - * @brief This internal API is used to set configurations for no-motion. - */ -static int8_t set_feature_config(struct bmi2_dev *bmi2_dev) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Structure to define the type of sensor and its configurations. */ - struct bmi2_sens_config config; - - /* Configure the type of feature. */ - config.type = BMI2_NO_MOTION; - - /* Get default configurations for the type of feature selected. */ - rslt = bmi270_wh_get_sensor_config(&config, 1, bmi2_dev); - bmi2_error_codes_print_result(rslt); - if (rslt == BMI2_OK) - { - - /* NOTE: The user can change the following configuration parameters according to their requirement. */ - /* 1LSB equals 20ms. Default is 100ms, setting to 80ms. */ - config.cfg.no_motion.duration = 0x04; - - /* 1LSB equals to 0.48mg. Default is 83mg, setting to 50mg. */ - config.cfg.no_motion.threshold = 0x68; - - /* Set new configurations. */ - rslt = bmi270_wh_set_sensor_config(&config, 1, bmi2_dev); - bmi2_error_codes_print_result(rslt); - } - - return rslt; -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/read_aux_data_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/read_aux_data_mode/Makefile deleted file mode 100644 index b995b42dd9..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/read_aux_data_mode/Makefile +++ /dev/null @@ -1,23 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= read_aux_data_mode.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -BMM150_SOURCE ?= ../../../bmm150 - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270_wh.c \ -$(BMM150_SOURCE)/bmm150.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(BMM150_SOURCE) \ -$(COMMON_LOCATION)/common - - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/read_aux_data_mode/read_aux_data_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/read_aux_data_mode/read_aux_data_mode.c deleted file mode 100644 index 6d8cd67f35..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/read_aux_data_mode/read_aux_data_mode.c +++ /dev/null @@ -1,327 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270_wh.h" -#include "bmm150.h" -#include "coines.h" -#include "common.h" - -/******************************************************************************/ -/*! Macro definition */ - -/*! Macros to select the sensors */ -#define ACCEL UINT8_C(0x00) -#define GYRO UINT8_C(0x01) -#define AUX UINT8_C(0x02) - -/*! Earth's gravity in m/s^2 */ -#define GRAVITY_EARTH (9.80665f) - -/*****************************************************************************/ -/*! Structure declaration */ - -/* Sensor initialization configuration. */ -struct bmi2_dev bmi2_dev; - -/*******************************************************************************/ -/*! Functions */ - -/** - * user_aux_read - Reads data from auxiliary sensor in manual mode. - * - * @param[in] reg_addr : Register address. - * @param[out] aux_data : Aux data pointer to store the read data. - * @param[in] length : No of bytes to read. - * @param[in] intf_ptr : Interface pointer - * - * @return Status of execution - * @retval 0 -> Success - * @retval < 0 -> Failure Info - */ -static int8_t user_aux_read(uint8_t reg_addr, uint8_t *aux_data, uint32_t len, void *intf_ptr); - -/** - * user_aux_write - Writes data to the auxiliary sensor in manual mode. - * - * @param[in] reg_addr : Register address. - * @param[out] aux_data : Aux data pointer to store the data being written. - * @param[in] length : No of bytes to read. - * @param[in] intf_ptr : Interface pointer - * - * @return Status of execution - * @retval 0 -> Success - * @retval < 0 -> Failure Info - */ -static int8_t user_aux_write(uint8_t reg_addr, const uint8_t *aux_data, uint32_t len, void *intf_ptr); - -/*! - * @brief This function provides the delay for required time (Microsecond) as per the input provided in some of the - * APIs. - * - * @param[in] period_us : The required wait time in microsecond. - * @param[in] intf_ptr : Interface pointer - * - * @return void. - */ -static void user_delay_us(uint32_t period, void *intf_ptr); - -/*! - * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at - * range 2G, 4G, 8G or 16G. - * - * @param[in] val : LSB from each axis. - * @param[in] g_range : Gravity range. - * @param[in] bit_width : Resolution for accel. - * - * @return Gravity. - */ -static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width); - -/*! - * @brief This function converts lsb to degree per second for 16 bit gyro at - * range 125, 250, 500, 1000 or 2000dps. - * - * @param[in] val : LSB from each axis. - * @param[in] dps : Degree per second. - * @param[in] bit_width : Resolution for gyro. - * - * @return Degree per second. - */ -static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width); - -/* This function starts the execution of program. */ -int main(void) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Variable to select the pull-up resistor which is set to trim register */ - uint8_t regdata; - - /* Variable to define limit to print aux data. */ - uint8_t limit = 20; - - /* Accel, Gyro and Aux sensors are listed in array. */ - uint8_t sensor_list[3] = { BMI2_ACCEL, BMI2_GYRO, BMI2_AUX }; - - /* Structure to define the type of the sensor and its configurations. */ - struct bmi2_sens_config config[3]; - - /* Sensor initialization configuration. */ - struct bmm150_dev bmm150_dev; - - /* bmm150 settings configuration */ - struct bmm150_settings settings; - - /* bmm150 magnetometer data */ - struct bmm150_mag_data mag_data; - - /* Structure to define type of sensor and their respective data. */ - struct bmi2_sensor_data sensor_data[3]; - - /* Variables to define read the accel and gyro data in float */ - float x = 0, y = 0, z = 0; - - config[ACCEL].type = BMI2_ACCEL; - config[GYRO].type = BMI2_GYRO; - config[AUX].type = BMI2_AUX; - - /* To enable the i2c interface settings for bmm150. */ - uint8_t bmm150_dev_addr = BMM150_DEFAULT_I2C_ADDRESS; - bmm150_dev.intf_ptr = &bmm150_dev_addr; - bmm150_dev.read = user_aux_read; - bmm150_dev.write = user_aux_write; - bmm150_dev.delay_us = user_delay_us; - - /* As per datasheet, aux interface with bmi270_wh will support only for I2C */ - bmm150_dev.intf = BMM150_I2C_INTF; - - sensor_data[ACCEL].type = BMI2_ACCEL; - sensor_data[GYRO].type = BMI2_GYRO; - sensor_data[AUX].type = BMI2_AUX; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270_wh. */ - rslt = bmi270_wh_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Pull-up resistor 2k is set to the trim regiter */ - regdata = BMI2_ASDA_PUPSEL_2K; - rslt = bmi2_set_regs(BMI2_AUX_IF_TRIM, ®data, 1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Enable the accel, gyro and aux sensor. */ - rslt = bmi270_wh_sensor_enable(sensor_list, 3, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Get default configurations for the type of feature selected. */ - rslt = bmi270_wh_get_sensor_config(config, 3, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Configurations for accel. */ - config[ACCEL].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; - config[ACCEL].cfg.acc.bwp = BMI2_ACC_OSR2_AVG2; - config[ACCEL].cfg.acc.odr = BMI2_ACC_ODR_100HZ; - config[ACCEL].cfg.acc.range = BMI2_ACC_RANGE_2G; - - /* Configurations for gyro. */ - config[GYRO].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; - config[GYRO].cfg.gyr.noise_perf = BMI2_GYR_RANGE_2000; - config[GYRO].cfg.gyr.bwp = BMI2_GYR_OSR2_MODE; - config[GYRO].cfg.gyr.odr = BMI2_GYR_ODR_100HZ; - config[GYRO].cfg.gyr.range = BMI2_GYR_RANGE_2000; - config[GYRO].cfg.gyr.ois_range = BMI2_GYR_OIS_2000; - - /* Configurations for aux. */ - config[AUX].cfg.aux.odr = BMI2_AUX_ODR_100HZ; - config[AUX].cfg.aux.aux_en = BMI2_ENABLE; - config[AUX].cfg.aux.i2c_device_addr = BMM150_DEFAULT_I2C_ADDRESS; - config[AUX].cfg.aux.manual_en = BMI2_ENABLE; - config[AUX].cfg.aux.fcu_write_en = BMI2_ENABLE; - config[AUX].cfg.aux.man_rd_burst = BMI2_AUX_READ_LEN_3; - config[AUX].cfg.aux.read_addr = BMM150_REG_DATA_X_LSB; - - /* Set new configurations for accel, gyro and aux. */ - rslt = bmi270_wh_set_sensor_config(config, 3, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmm150. */ - rslt = bmm150_init(&bmm150_dev); - bmi2_error_codes_print_result(rslt); - - /* Set the power mode to normal mode. */ - settings.pwr_mode = BMM150_POWERMODE_NORMAL; - rslt = bmm150_set_op_mode(&settings, &bmm150_dev); - bmi2_error_codes_print_result(rslt); - - rslt = bmi270_wh_get_sensor_config(config, 3, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Disable manual mode so that the data mode is enabled. */ - config[AUX].cfg.aux.manual_en = BMI2_DISABLE; - - /* Set the aux configurations. */ - rslt = bmi270_wh_set_sensor_config(config, 3, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - printf("MAGNETOMETER, ACCEL, AND GYRO DATA IN DATA MODE\n"); - - if (bmm150_dev.chip_id == BMM150_CHIP_ID) - { - while (limit--) - { - /* Delay has been added to get aux, accel and gyro data at the interval of every 0.05 second. */ - bmi2_dev.delay_us(50000, bmi2_dev.intf_ptr); - - rslt = bmi270_wh_get_sensor_data(sensor_data, 3, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - printf("\nAcc_x = %d\t", sensor_data[ACCEL].sens_data.acc.x); - printf("Acc_y = %d\t", sensor_data[ACCEL].sens_data.acc.y); - printf("Acc_z = %d", sensor_data[ACCEL].sens_data.acc.z); - - /* Converting lsb to meter per second squared for 16 bit accelerometer at 2G range. */ - x = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.x, 2, bmi2_dev.resolution); - y = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.y, 2, bmi2_dev.resolution); - z = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.z, 2, bmi2_dev.resolution); - - /* Print the data in m/s2. */ - printf("\nAcc_ms2_X = %4.2f, Acc_ms2_Y = %4.2f, Acc_ms2_Z = %4.2f\n", x, y, z); - - printf("\nGyr_X = %d\t", sensor_data[GYRO].sens_data.gyr.x); - printf("Gyr_Y = %d\t", sensor_data[GYRO].sens_data.gyr.y); - printf("Gyr_Z= %d", sensor_data[GYRO].sens_data.gyr.z); - - /* Converting lsb to degree per second for 16 bit gyro at 2000dps range. */ - x = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.x, 2000, bmi2_dev.resolution); - y = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.y, 2000, bmi2_dev.resolution); - z = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.z, 2000, bmi2_dev.resolution); - - /* Print the data in dps. */ - printf("\nGyro_DPS_X = %4.2f, Gyro_DPS_Y = %4.2f, Gyro_DPS_Z = %4.2f\n", x, y, z); - - /* Compensating the raw auxiliary data available from the BMM150 API. */ - rslt = bmm150_aux_mag_data(sensor_data[AUX].sens_data.aux_data, &mag_data, &bmm150_dev); - bmi2_error_codes_print_result(rslt); - printf("\nMag_x_axis = %d \t Mag_y_axis = %d \t Mag_z_axis = %d \t\n", - mag_data.x, - mag_data.y, - mag_data.z); - } - } - } - - bmi2_coines_deinit(); - - return rslt; -} - -/*! - * @brief This function reads the data from auxiliary sensor in manual mode. - */ -static int8_t user_aux_read(uint8_t reg_addr, uint8_t *aux_data, uint32_t len, void *intf_ptr) -{ - int8_t rslt; - - /* Discarding the parameter id as it is redundant */ - rslt = bmi2_read_aux_man_mode(reg_addr, aux_data, len, &bmi2_dev); - - return rslt; -} - -/*! - * @brief This function writes the data to auxiliary sensor in manual mode. - */ -static int8_t user_aux_write(uint8_t reg_addr, const uint8_t *aux_data, uint32_t len, void *intf_ptr) -{ - int8_t rslt; - - /* Discarding the parameter id as it is redundant */ - rslt = bmi2_write_aux_man_mode(reg_addr, aux_data, len, &bmi2_dev); - - return rslt; -} - -/*! - * Delay function map to COINES platform - */ -static void user_delay_us(uint32_t period, void *intf_ptr) -{ - coines_delay_usec(period); -} - -/*! - * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at - * range 2G, 4G, 8G or 16G. - */ -static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width) -{ - float half_scale = ((float)(1 << bit_width) / 2.0f); - - return (GRAVITY_EARTH * val * g_range) / half_scale; -} - -/*! - * @brief This function converts lsb to degree per second for 16 bit gyro at - * range 125, 250, 500, 1000 or 2000dps. - */ -static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width) -{ - float half_scale = ((float)(1 << bit_width) / 2.0f); - - return (dps / ((half_scale) + BMI2_GYR_RANGE_2000)) * (val); -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/read_aux_manual_mode/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/read_aux_manual_mode/Makefile deleted file mode 100644 index 843a2ce8c7..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/read_aux_manual_mode/Makefile +++ /dev/null @@ -1,23 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= read_aux_manual_mode.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -BMM150_SOURCE ?= ../../../bmm150 - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270_wh.c \ -$(BMM150_SOURCE)/bmm150.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(BMM150_SOURCE) \ -$(COMMON_LOCATION)/common - - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/read_aux_manual_mode/read_aux_manual_mode.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/read_aux_manual_mode/read_aux_manual_mode.c deleted file mode 100644 index 524eb8d2ff..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/read_aux_manual_mode/read_aux_manual_mode.c +++ /dev/null @@ -1,322 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270_wh.h" -#include "bmm150.h" -#include "coines.h" -#include "common.h" - -/******************************************************************************/ -/*! Macro definition */ - -/*! Macros to select the sensors */ -#define ACCEL UINT8_C(0x00) -#define GYRO UINT8_C(0x01) -#define AUX UINT8_C(0x02) - -/*! Earth's gravity in m/s^2 */ -#define GRAVITY_EARTH (9.80665f) - -/*****************************************************************************/ -/*! Structure declaration */ - -/* Sensor initialization configuration. */ -struct bmi2_dev bmi2_dev; - -/******************************************************************************/ -/*! Functions */ - -/** - * user_aux_read - Reads data from auxiliary sensor in manual mode. - * - * @param[in] reg_addr : Register address. - * @param[out] aux_data : Aux data pointer to store the read data. - * @param[in] length : No of bytes to read. - * @param[in] intf_ptr : Interface pointer - * - * @return Status of execution - * @retval 0 -> Success - * @retval < 0 -> Failure Info - */ -static int8_t user_aux_read(uint8_t reg_addr, uint8_t *aux_data, uint32_t len, void *intf_ptr); - -/** - * user_aux_write - Writes data to the auxiliary sensor in manual mode. - * - * @param[in] reg_addr : Register address. - * @param[out] aux_data : Aux data pointer to store the data being written. - * @param[in] length : No of bytes to read. - * @param[in] intf_ptr : Interface pointer - * - * @return Status of execution - * @retval 0 -> Success - * @retval < 0 -> Failure Info - */ -static int8_t user_aux_write(uint8_t reg_addr, const uint8_t *aux_data, uint32_t len, void *intf_ptr); - -/*! - * @brief This function provides the delay for required time (Microsecond) as per the input provided in some of the - * APIs. - * - * @param[in] period_us : The required wait time in microsecond. - * @param[in] intf_ptr : Interface pointer - * - * @return void. - */ -static void user_delay_us(uint32_t period, void *intf_ptr); - -/*! - * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at - * range 2G, 4G, 8G or 16G. - * - * @param[in] val : LSB from each axis. - * @param[in] g_range : Gravity range. - * @param[in] bit_width : Resolution for accel. - * - * @return Gravity. - */ -static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width); - -/*! - * @brief This function converts lsb to degree per second for 16 bit gyro at - * range 125, 250, 500, 1000 or 2000dps. - * - * @param[in] val : LSB from each axis. - * @param[in] dps : Degree per second. - * @param[in] bit_width : Resolution for gyro. - * - * @return Degree per second. - */ -static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width); - -/* This function starts the execution of program. */ -int main(void) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Variable to select the pull-up resistor which is set to trim register */ - uint8_t regdata; - - /* Variable to define limit to print aux data. */ - uint8_t limit = 20; - - /* Accel, Gyro and Aux sensors are listed in array. */ - uint8_t sensor_list[3] = { BMI2_ACCEL, BMI2_GYRO, BMI2_AUX }; - - /* Sensor initialization configuration. */ - struct bmm150_dev bmm150_dev; - - /* bmm150 settings configuration */ - struct bmm150_settings settings; - - /* bmm150 magnetometer data */ - struct bmm150_mag_data mag_data; - - /* Structure to define the type of the sensor and its configurations. */ - struct bmi2_sens_config config[3]; - - /* Structure to define type of sensor and their respective data. */ - struct bmi2_sensor_data sensor_data[2]; - - /* Variables to define read the accel and gyro data in float */ - float x = 0, y = 0, z = 0; - - config[ACCEL].type = BMI2_ACCEL; - config[GYRO].type = BMI2_GYRO; - config[AUX].type = BMI2_AUX; - - sensor_data[ACCEL].type = BMI2_ACCEL; - sensor_data[GYRO].type = BMI2_GYRO; - - /* Array of eight bytes to store x, y, z and r axis aux data. */ - uint8_t aux_data[8] = { 0 }; - - /* To enable the i2c interface settings for bmm150. */ - uint8_t bmm150_dev_addr = BMM150_DEFAULT_I2C_ADDRESS; - bmm150_dev.intf_ptr = &bmm150_dev_addr; - bmm150_dev.read = user_aux_read; - bmm150_dev.write = user_aux_write; - bmm150_dev.delay_us = user_delay_us; - - /* As per datasheet, aux interface with bmi270_wh will support only for I2C */ - bmm150_dev.intf = BMM150_I2C_INTF; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_SPI_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270_wh. */ - rslt = bmi270_wh_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Pull-up resistor 2k is set to the trim register */ - regdata = BMI2_ASDA_PUPSEL_2K; - rslt = bmi2_set_regs(BMI2_AUX_IF_TRIM, ®data, 1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Enable the accel, gyro and aux sensor. */ - rslt = bmi270_wh_sensor_enable(sensor_list, 3, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Get default configurations for the type of feature selected. */ - rslt = bmi270_wh_get_sensor_config(config, 3, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Configurations for accel. */ - config[ACCEL].cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; - config[ACCEL].cfg.acc.bwp = BMI2_ACC_OSR2_AVG2; - config[ACCEL].cfg.acc.odr = BMI2_ACC_ODR_100HZ; - config[ACCEL].cfg.acc.range = BMI2_ACC_RANGE_2G; - - /* Configurations for gyro. */ - config[GYRO].cfg.gyr.filter_perf = BMI2_PERF_OPT_MODE; - config[GYRO].cfg.gyr.noise_perf = BMI2_GYR_RANGE_2000; - config[GYRO].cfg.gyr.bwp = BMI2_GYR_OSR2_MODE; - config[GYRO].cfg.gyr.odr = BMI2_GYR_ODR_100HZ; - config[GYRO].cfg.gyr.range = BMI2_GYR_RANGE_2000; - config[GYRO].cfg.gyr.ois_range = BMI2_GYR_OIS_2000; - - /* Configurations for aux. */ - config[AUX].cfg.aux.odr = BMI2_AUX_ODR_100HZ; - config[AUX].cfg.aux.aux_en = BMI2_ENABLE; - config[AUX].cfg.aux.i2c_device_addr = BMM150_DEFAULT_I2C_ADDRESS; - config[AUX].cfg.aux.manual_en = BMI2_ENABLE; - config[AUX].cfg.aux.fcu_write_en = BMI2_ENABLE; - config[AUX].cfg.aux.man_rd_burst = BMI2_AUX_READ_LEN_3; - - /* Set new configurations for accel, gyro and aux. */ - rslt = bmi270_wh_set_sensor_config(config, 3, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmm150. */ - rslt = bmm150_init(&bmm150_dev); - bmi2_error_codes_print_result(rslt); - - /* Set the power mode to normal mode. */ - settings.pwr_mode = BMM150_POWERMODE_NORMAL; - rslt = bmm150_set_op_mode(&settings, &bmm150_dev); - bmi2_error_codes_print_result(rslt); - - printf("MAGNETOMETER, ACCEL AND GYRO DATA IN MANUAL MODE\n"); - - if (bmm150_dev.chip_id == BMM150_CHIP_ID) - { - while (limit--) - { - /* Delay has been added to get aux, accel and gyro data at the interval of every 0.05 second. */ - bmi2_dev.delay_us(50000, bmi2_dev.intf_ptr); - - rslt = bmi270_wh_get_sensor_data(sensor_data, 2, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - printf("\nAcc_x = %d\t", sensor_data[ACCEL].sens_data.acc.x); - printf("Acc_y = %d\t", sensor_data[ACCEL].sens_data.acc.y); - printf("Acc_z = %d", sensor_data[ACCEL].sens_data.acc.z); - - /* Converting lsb to meter per second squared for 16 bit accelerometer at 2G range. */ - x = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.x, 2, bmi2_dev.resolution); - y = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.y, 2, bmi2_dev.resolution); - z = lsb_to_mps2(sensor_data[ACCEL].sens_data.acc.z, 2, bmi2_dev.resolution); - - /* Print the data in m/s2. */ - printf("\nAcc_ms2_X = %4.2f, Acc_ms2_Y = %4.2f, Acc_ms2_Z = %4.2f\n", x, y, z); - - printf("\nGyr_X = %d\t", sensor_data[GYRO].sens_data.gyr.x); - printf("Gyr_Y = %d\t", sensor_data[GYRO].sens_data.gyr.y); - printf("Gyr_Z = %d", sensor_data[GYRO].sens_data.gyr.z); - - /* Converting lsb to degree per second for 16 bit gyro at 2000dps range. */ - x = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.x, 2000, bmi2_dev.resolution); - y = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.y, 2000, bmi2_dev.resolution); - z = lsb_to_dps(sensor_data[GYRO].sens_data.gyr.z, 2000, bmi2_dev.resolution); - - /* Print the data in dps. */ - printf("\nGyro_DPS_X = %4.2f, Gyro_DPS_Y = %4.2f, Gyro_DPS_Z = %4.2f\n", x, y, z); - } - - /* Read aux data from the bmm150 data registers. */ - rslt = bmi2_read_aux_man_mode(BMM150_REG_DATA_X_LSB, aux_data, 8, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - if (rslt == BMI2_OK) - { - /* Compensating the raw auxiliary data available from the BMM150 API. */ - rslt = bmm150_aux_mag_data(aux_data, &mag_data, &bmm150_dev); - bmi2_error_codes_print_result(rslt); - printf("Mag_x_axis = %d \t Mag_y_axis = %d \t Mag_z_axis = %d \t\n", mag_data.x, mag_data.y, - mag_data.z); - } - } - } - - bmi2_coines_deinit(); - - return rslt; -} - -/*! - * @brief This function reads the data from auxiliary sensor in manual mode. - */ -int8_t user_aux_read(uint8_t reg_addr, uint8_t *aux_data, uint32_t len, void *intf_ptr) -{ - int8_t rslt; - - /* Discarding the parameter id as it is redundant */ - rslt = bmi2_read_aux_man_mode(reg_addr, aux_data, len, &bmi2_dev); - - return rslt; -} - -/*! - * @brief This function writes the data to auxiliary sensor in manual mode. - */ -int8_t user_aux_write(uint8_t reg_addr, const uint8_t *aux_data, uint32_t len, void *intf_ptr) -{ - int8_t rslt; - - /* Discarding the parameter id as it is redundant */ - rslt = bmi2_write_aux_man_mode(reg_addr, aux_data, len, &bmi2_dev); - - return rslt; -} - -/*! - * Delay function map to COINES platform - */ -static void user_delay_us(uint32_t period, void *intf_ptr) -{ - coines_delay_usec(period); -} - -/*! - * @brief This function converts lsb to meter per second squared for 16 bit accelerometer at - * range 2G, 4G, 8G or 16G. - */ -static float lsb_to_mps2(int16_t val, float g_range, uint8_t bit_width) -{ - float half_scale = ((float)(1 << bit_width) / 2.0f); - - return (GRAVITY_EARTH * val * g_range) / half_scale; -} - -/*! - * @brief This function converts lsb to degree per second for 16 bit gyro at - * range 125, 250, 500, 1000 or 2000dps. - */ -static float lsb_to_dps(int16_t val, float dps, uint8_t bit_width) -{ - float half_scale = ((float)(1 << bit_width) / 2.0f); - - return (dps / ((half_scale) + BMI2_GYR_RANGE_2000)) * (val); -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/step_activity/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/step_activity/Makefile deleted file mode 100644 index a4fcf100cd..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/step_activity/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= step_activity.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270_wh.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/step_activity/step_activity.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/step_activity/step_activity.c deleted file mode 100644 index 0c3e9c7558..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/step_activity/step_activity.c +++ /dev/null @@ -1,110 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270_wh.h" -#include "common.h" - -/******************************************************************************/ -/*! Functions */ -/* This function starts the execution of program. */ -int main(void) -{ - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Structure to define the type of sensor and their respective data. */ - struct bmi2_sensor_data sensor_data; - - /* Structure to define the type of sensor and its configurations. */ - struct bmi2_sens_config config; - - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Accel sensor and step activity feature are listed in array. */ - uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_STEP_ACTIVITY }; - - /* Type of sensor to get step activity data. */ - sensor_data.type = BMI2_STEP_ACTIVITY; - - /* Variable to get step activity interrupt status. */ - uint16_t int_status = 0; - - uint16_t loop = 10; - - /* Select features and their pins to be mapped to. */ - struct bmi2_sens_int_config sens_int = { .type = BMI2_STEP_ACTIVITY, .hw_int_pin = BMI2_INT2 }; - - /* The step activities are listed in array. */ - const char *activity_output[4] = { "BMI2_STILL", "BMI2_WALK", "BMI2_RUN", "BMI2_UNKNOWN" }; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270. */ - rslt = bmi270_wh_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Enable the selected sensor. */ - rslt = bmi270_wh_sensor_enable(sensor_sel, 2, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Configure the type of sensor. */ - config.type = BMI2_STEP_ACTIVITY; - - if (rslt == BMI2_OK) - { - /* Get default configurations for the type of feature selected. */ - rslt = bmi270_wh_get_sensor_config(&config, 1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Map the feature interrupt for step activity. */ - rslt = bmi270_wh_map_feat_int(&sens_int, 1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - printf("\nMove the board in steps to perform step activity:\n"); - - /* Loop to get step activity. */ - while (loop) - { - /* To get the interrupt status of step detector. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* To check the interrupt status of step detector. */ - if (int_status & BMI270_WH_STEP_ACT_STATUS_MASK) - { - printf("Step detected\n"); - - /* Get step activity output. */ - rslt = bmi270_wh_get_sensor_data(&sensor_data, 1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Print the step activity output. */ - printf("Step activity = %s\n", activity_output[sensor_data.sens_data.activity_output]); - - loop--; - } - } - } - } - } - - bmi2_coines_deinit(); - - return rslt; -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/step_counter/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/step_counter/Makefile deleted file mode 100644 index 1c16cc62a2..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/step_counter/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= step_counter.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270_wh.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/step_counter/step_counter.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/step_counter/step_counter.c deleted file mode 100644 index 8d5f968fb7..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/step_counter/step_counter.c +++ /dev/null @@ -1,146 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270_wh.h" -#include "common.h" - -/******************************************************************************/ -/*! Static function declaration */ - -/*! - * @brief This internal API is used to set configurations for step counter. - * - * @param[in] bmi2_dev : Structure instance of bmi2_dev. - * - * @return Status of execution. - */ -static int8_t set_feature_config(struct bmi2_dev *bmi2_dev); - -/******************************************************************************/ -/*! Functions */ - -/* This function starts the execution of program. */ -int main(void) -{ - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Structure to define type of sensor and their respective data. */ - struct bmi2_sensor_data sensor_data; - - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Accel sensor and step counter feature are listed in array. */ - uint8_t sensor_sel[2] = { BMI2_ACCEL, BMI2_STEP_COUNTER }; - - /* Variable to get step counter interrupt status. */ - uint16_t int_status = 0; - - /* Select features and their pins to be mapped to. */ - struct bmi2_sens_int_config sens_int = { .type = BMI2_STEP_COUNTER, .hw_int_pin = BMI2_INT2 }; - - /* Type of sensor to get step counter data. */ - sensor_data.type = BMI2_STEP_COUNTER; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270_wh. */ - rslt = bmi270_wh_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Enable the selected sensor. */ - rslt = bmi270_wh_sensor_enable(sensor_sel, 2, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Set the feature configuration for step counter. */ - rslt = set_feature_config(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - printf("Step counter watermark level set to 1 (20 steps)\n"); - - if (rslt == BMI2_OK) - { - /* Map the step counter feature interrupt. */ - rslt = bmi270_wh_map_feat_int(&sens_int, 1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Move the board in steps for 20 times to get step counter interrupt. */ - printf("Move the board in steps\n"); - - /* Loop to get number of steps counted. */ - do - { - /* To get the interrupt status of the step counter. */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* To check the interrupt status of the step counter. */ - if (int_status & BMI270_WH_STEP_CNT_STATUS_MASK) - { - printf("Step counter interrupt occurred when watermark level (20 steps) is reached\n"); - - /* Get step counter output. */ - rslt = bmi270_wh_get_sensor_data(&sensor_data, 1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* Print the step counter output. */ - printf("No of steps counted = %ld", sensor_data.sens_data.step_counter_output); - break; - } - } while (rslt == BMI2_OK); - } - } - } - - bmi2_coines_deinit(); - - return rslt; -} - -/*! - * @brief This internal API is used to set configurations for step counter. - */ -static int8_t set_feature_config(struct bmi2_dev *bmi2_dev) -{ - /* Status of api are returned to this variable. */ - int8_t rslt; - - /* Structure to define the type of sensor and its configurations. */ - struct bmi2_sens_config config; - - /* Configure the type of sensor. */ - config.type = BMI2_STEP_COUNTER; - - /* Get default configurations for the type of feature selected. */ - rslt = bmi270_wh_get_sensor_config(&config, 1, bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Setting water-mark level to 1 for step counter to get interrupt after 20 step counts. Every 20 steps once - * output triggers. */ - config.cfg.step_counter.watermark_level = 1; - - /* Set new configuration. */ - rslt = bmi270_wh_set_sensor_config(&config, 1, bmi2_dev); - bmi2_error_codes_print_result(rslt); - } - - return rslt; -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/wrist_wear_wakeup/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/wrist_wear_wakeup/Makefile deleted file mode 100644 index 65d3561d83..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/wrist_wear_wakeup/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= wrist_wear_wakeup.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi270_wh.c \ -$(COMMON_LOCATION)/common/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/wrist_wear_wakeup/wrist_wear_wakeup.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/wrist_wear_wakeup/wrist_wear_wakeup.c deleted file mode 100644 index bee1191d94..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi270_wh/wrist_wear_wakeup/wrist_wear_wakeup.c +++ /dev/null @@ -1,131 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -/*! Header Files */ -#include -#include "bmi270_wh.h" -#include "common.h" - -/******************************************************************************/ -/*! Static function declaration */ - -/*! - * @brief This internal API is used to set the sensor configuration. - * - * @param[in] bmi2_dev : Structure instance of bmi2_dev. - * - * @return Status of execution. - */ -static int8_t bmi2_set_config(struct bmi2_dev *bmi2_dev); - -/******************************************************************************/ -/*! Functions */ - -/* This function starts the execution of program */ -int main(void) -{ - /* Variable to define result */ - int8_t rslt; - - /* Initialize status of wrist wear wakeup interrupt */ - uint16_t int_status = 0; - - /* Sensor initialization configuration. */ - struct bmi2_dev bmi2_dev; - - /* Select features and their pins to be mapped to */ - struct bmi2_sens_int_config sens_int = { .type = BMI2_WRIST_WEAR_WAKE_UP_WH, .hw_int_pin = BMI2_INT1 }; - - /* Interface reference is given as a parameter - * For I2C : BMI2_I2C_INTF - * For SPI : BMI2_SPI_INTF - */ - rslt = bmi2_interface_init(&bmi2_dev, BMI2_I2C_INTF); - bmi2_error_codes_print_result(rslt); - - /* Initialize bmi270_wh. */ - rslt = bmi270_wh_init(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Set the sensor configuration */ - rslt = bmi2_set_config(&bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Map the feature interrupt */ - rslt = bmi270_wh_map_feat_int(&sens_int, 1, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - printf("Tilt the board in landscape position to trigger wrist wear wakeup\n"); - - /* Loop to print the wrist wear wakeup data when interrupt occurs */ - while (1) - { - int_status = 0; - - /* To get the interrupt status of the wrist wear wakeup */ - rslt = bmi2_get_int_status(&int_status, &bmi2_dev); - bmi2_error_codes_print_result(rslt); - - /* To check the interrupt status of the wrist gesture */ - if ((rslt == BMI2_OK) && (int_status & BMI270_WH_WRIST_WEAR_WAKEUP_WH_STATUS_MASK)) - { - printf("Wrist wear wakeup detected\n"); - break; - } - } - } - } - } - - bmi2_coines_deinit(); - - return rslt; -} - -/*! - * @brief This internal API sets the sensor configuration - */ -static int8_t bmi2_set_config(struct bmi2_dev *bmi2_dev) -{ - /* Variable to define result */ - int8_t rslt; - - /* List the sensors which are required to enable */ - uint8_t sens_list[2] = { BMI2_ACCEL, BMI2_WRIST_WEAR_WAKE_UP_WH }; - - /* Structure to define the type of the sensor and its configurations */ - struct bmi2_sens_config config; - - /* Configure type of feature */ - config.type = BMI2_WRIST_WEAR_WAKE_UP_WH; - - /* Enable the selected sensors */ - rslt = bmi270_wh_sensor_enable(sens_list, 2, bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Get default configurations for the type of feature selected */ - rslt = bmi270_wh_get_sensor_config(&config, 1, bmi2_dev); - bmi2_error_codes_print_result(rslt); - - if (rslt == BMI2_OK) - { - /* Set the new configuration */ - rslt = bmi270_wh_set_sensor_config(&config, 1, bmi2_dev); - bmi2_error_codes_print_result(rslt); - } - } - - return rslt; -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi2_ois/common/common.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi2_ois/common/common.c deleted file mode 100644 index 1a43947a99..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi2_ois/common/common.c +++ /dev/null @@ -1,153 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -#include -#include -#include - -#include "coines.h" -#include "common.h" - -/******************************************************************************/ -/*! Global declaration */ - -/*! Device address for primary and secondary interface */ -static uint8_t ois_dev_addr; - -/******************************************************************************/ -/*! Functions */ - -/*! - * SPI read function map to COINES platform - */ -int8_t bmi2_ois_spi_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr) -{ - uint8_t dev_addr = *(uint8_t*)intf_ptr; - - return coines_read_spi(dev_addr, reg_addr, reg_data, (uint16_t)len); -} - -/*! - * SPI write function map to COINES platform - */ -int8_t bmi2_ois_spi_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr) -{ - uint8_t dev_addr = *(uint8_t*)intf_ptr; - - return coines_write_spi(dev_addr, reg_addr, (uint8_t *)reg_data, (uint16_t)len); -} - -/*! - * Delay function map to COINES platform - */ -void bmi2_ois_delay_us(uint32_t period, void *intf_ptr) -{ - coines_delay_usec(period); -} - -/******************************************************************************/ -int8_t bmi2_ois_init(struct bmi2_ois_dev *ois_dev) -{ - int8_t rslt; - - if (ois_dev != NULL) - { - int16_t result = coines_open_comm_intf(COINES_COMM_INTF_USB); - if (result < COINES_SUCCESS) - { - printf( - "\n Unable to connect with Application Board ! \n" " 1. Check if the board is connected and powered on. \n" " 2. Check if Application Board USB driver is installed. \n" - " 3. Check if board is in use by another application. (Insufficient permissions to access USB) \n"); - exit(result); - } - - coines_set_shuttleboard_vdd_vddio_config(0, 0); - coines_delay_msec(100); - - printf("SPI Interface \n"); - - /* To initialize the user SPI function */ - ois_dev_addr = COINES_SHUTTLE_PIN_8; /*COINES_SHUTTLE_PIN_7 */ - ois_dev->ois_read = bmi2_ois_spi_read; - ois_dev->ois_write = bmi2_ois_spi_write; - - /* Assign device address to interface pointer */ - ois_dev->intf_ptr = &ois_dev_addr; - - /* Configure delay in microseconds */ - ois_dev->ois_delay_us = bmi2_ois_delay_us; - - /* SDO pin is made low for selecting I2C address 0x76*/ - coines_set_pin_config(COINES_SHUTTLE_PIN_8, COINES_PIN_DIRECTION_OUT, COINES_PIN_VALUE_LOW); - - /* coines_config_spi_bus(COINES_SPI_BUS_0, COINES_SPI_SPEED_5_MHZ, COINES_SPI_MODE3); */ - coines_config_spi_bus(COINES_SPI_BUS_0, COINES_SPI_SPEED_7_5_MHZ, COINES_SPI_MODE0); - coines_delay_msec(10); - - /* PS pin is made high for selecting I2C protocol (gyroscope)*/ - coines_set_pin_config(COINES_SHUTTLE_PIN_9, COINES_PIN_DIRECTION_OUT, COINES_PIN_VALUE_HIGH); - - coines_delay_usec(10000); - - coines_set_shuttleboard_vdd_vddio_config(3300, 3300); - - coines_delay_usec(10000); - - } - else - { - rslt = BMI2_OIS_E_NULL_PTR; - } - - return rslt; -} - -/******************************************************************************/ - -/*! - * @brief This internal API prints the execution status - */ -void bmi2_ois_error_codes_print_result(const char api_name[], int8_t rslt) -{ - if (rslt != BMI2_OIS_OK) - { - printf("%s\t", api_name); - - if (rslt == BMI2_OIS_E_NULL_PTR) - { - printf("Error [%d] : Null pointer error.\r\n", rslt); - printf( - "It occurs when the user tries to assign value (not address) to a pointer, which has been initialized to NULL.\r\n"); - } - else if (rslt == BMI2_OIS_E_COM_FAIL) - { - printf("Error [%d] : Communication failure error.\r\n", rslt); - printf( - "It occurs due to read/write operation failure and also due to power failure during communication\r\n"); - } - else if (rslt == BMI2_OIS_E_INVALID_SENSOR) - { - printf( - "Error [%d] : Invalid sensor error. It occurs when there is a mismatch in the requested feature with the available one\r\n", - rslt); - } - else - { - printf("Error [%d] : Unknown error code\r\n", rslt); - } - } -} - -/*! - * @brief Deinitializes coines platform - * - * @return void. - */ -void bmi2_ois_coines_deinit(void) -{ - coines_close_comm_intf(COINES_COMM_INTF_USB); -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi2_ois/common/common.h b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi2_ois/common/common.h deleted file mode 100644 index 82a843bbf5..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi2_ois/common/common.h +++ /dev/null @@ -1,92 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -#ifndef _COMMON_H -#define _COMMON_H - -/*! CPP guard */ -#ifdef __cplusplus -extern "C" { -#endif - -#include -#include "bmi2_ois.h" - -/*! - * @brief Function for reading the sensor's registers through SPI bus. - * - * @param[in] reg_addr : Register address. - * @param[out] reg_data : Pointer to the data buffer to store the read data. - * @param[in] length : No of bytes to read. - * @param[in] intf_ptr : Interface pointer - * - * @return Status of execution - * @retval = BMI2_INTF_RET_SUCCESS -> Success - * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info - * - */ -int8_t bmi2_ois_spi_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr); - -/*! - * @brief Function for writing the sensor's registers through SPI bus. - * - * @param[in] reg_addr : Register address. - * @param[in] reg_data : Pointer to the data buffer whose data has to be written. - * @param[in] length : No of bytes to write. - * @param[in] intf_ptr : Interface pointer - * - * @return Status of execution - * @retval = BMI2_INTF_RET_SUCCESS -> Success - * @retval != BMI2_INTF_RET_SUCCESS -> Failure Info - * - */ -int8_t bmi2_ois_spi_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr); - -/*! - * @brief This function provides the delay for required time (Microsecond) as per the input provided in some of the - * APIs. - * - * @param[in] period_us : The required wait time in microsecond. - * @param[in] intf_ptr : Interface pointer - * - * @return void. - * - */ -void bmi2_ois_delay_us(uint32_t period, void *intf_ptr); - -/*! - * @brief Function to initialize device structure and coines platform - * - * @param[in] bma : Structure instance of bmi2_dev - * - * @return Status of execution - * @retval 0 -> Success - * @retval < 0 -> Failure Info - */ -int8_t bmi2_ois_init(struct bmi2_ois_dev *ois_dev); - -/*! - * @brief Prints the execution status of the APIs. - * - * @param[in] api_name : API name with return status - * @param[in] rslt : Error code returned by the API whose execution status has to be printed. - * - * @return void. - */ -void bmi2_ois_error_codes_print_result(const char api_name[], int8_t rslt); - -/*! - * @brief Deinitializes coines platform - * - * @return void. - */ -void bmi2_ois_coines_deinit(void); - -#ifdef __cplusplus -} -#endif /* End of CPP guard */ - -#endif /* End _COMMON_H */ diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi2_ois/ois_accel_gyro/Makefile b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi2_ois/ois_accel_gyro/Makefile deleted file mode 100644 index 65cd946442..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi2_ois/ois_accel_gyro/Makefile +++ /dev/null @@ -1,18 +0,0 @@ -COINES_INSTALL_PATH ?= ../../../.. - -EXAMPLE_FILE ?= ois_accel_gyro.c - -API_LOCATION ?= ../.. - -COMMON_LOCATION ?= .. - -C_SRCS += \ -$(API_LOCATION)/bmi2.c \ -$(API_LOCATION)/bmi2_ois.c \ -$(COMMON_LOCATION)/common.c - -INCLUDEPATHS += \ -$(API_LOCATION) \ -$(COMMON_LOCATION)/common - -include $(COINES_INSTALL_PATH)/coines.mk \ No newline at end of file diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi2_ois/ois_accel_gyro/ois_accel_gyro.c b/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi2_ois/ois_accel_gyro/ois_accel_gyro.c deleted file mode 100644 index bdb9d8f936..0000000000 --- a/lib/main/BoschSensortec/BMI270-Sensor-API/examples/bmi2_ois/ois_accel_gyro/ois_accel_gyro.c +++ /dev/null @@ -1,118 +0,0 @@ -/**\ - * Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. - * - * SPDX-License-Identifier: BSD-3-Clause - **/ - -/******************************************************************************/ -#include - -#include "bmi2_ois.h" -#include "bmi2_ois_common.h" - -/******************************************************************************/ -/*! Macro definition */ - -/*! Macro that holds the total number of accel x,y and z axes sample counts to be printed */ -#define ACCEL_GYRO_SAMPLE_COUNT UINT8_C(50) - -/******************************************************************************/ -/*! Functions */ - -int main(void) -{ - int8_t rslt; - - struct bmi2_ois_dev ois_dev; - - int8_t idx; - - /* To store the gyroscope cross sensitivity value */ - int16_t ois_gyr_cross_sens_zx = 0; - - /* Array to enable sensor through OIS interface */ - uint8_t sens_sel[2] = { BMI2_OIS_ACCEL, BMI2_OIS_GYRO }; - - /* Variable that holds the accel and gyro sample count */ - uint8_t n_data = ACCEL_GYRO_SAMPLE_COUNT; - - /* Initialize the device structure */ - rslt = bmi2_ois_init(&ois_dev); - bmi2_ois_error_codes_print_result("bmi2_ois_init", rslt); - - /* Get configurations for OIS */ - rslt = bmi2_ois_get_config(&ois_dev); - bmi2_ois_error_codes_print_result("bmi2_ois_get_config", rslt); - - /* OIS configurations */ - ois_dev.acc_en = BMI2_OIS_ENABLE; - ois_dev.gyr_en = BMI2_OIS_ENABLE; - ois_dev.lp_filter_en = BMI2_OIS_ENABLE; - - /* Set configurations for OIS */ - rslt = bmi2_ois_set_config(&ois_dev); - bmi2_ois_error_codes_print_result("bmi2_ois_set_config", rslt); - - /* Get configurations for OIS */ - rslt = bmi2_ois_get_config(&ois_dev); - bmi2_ois_error_codes_print_result("bmi2_ois_get_config", rslt); - - if (rslt == BMI2_OIS_OK) - { - for (idx = 0; idx < n_data; idx++) - { - ois_dev.ois_delay_us(156, ois_dev.intf_ptr); - - /* Accel ODR is 1600hz and gyro ODR is 6400hz.Delay required for - * accel 156us and 625us for gyro. - * taken modules from accel and gyro ODR which results every 4th sample accel and gyro - * read happens, rest of three samples gyro data alone will be read */ - if (idx % 4 == 0) - { - /* Get OIS accelerometer and gyro data through OIS interface - * @note for sensor which support gyro cross axes sensitivity pass the - * gyr_cross_sens_zx from the bmi2_dev structure */ - rslt = bmi2_ois_read_data(sens_sel, 2, &ois_dev, ois_gyr_cross_sens_zx); - bmi2_ois_error_codes_print_result("bmi2_ois_read_data", rslt); - - if (rslt == BMI2_OIS_OK) - { - printf("\n"); - - /* Print accelerometer data */ - printf("OIS Accel x-axis = %d ", ois_dev.acc_data.x); - printf("OIS Accel y-axis = %d ", ois_dev.acc_data.y); - printf("OIS Accel z-axis = %d \n", ois_dev.acc_data.z); - - /* Print gyro data */ - printf("OIS Gyro x-axis = %d ", ois_dev.gyr_data.x); - printf("OIS Gyro y-axis = %d ", ois_dev.gyr_data.y); - printf("OIS Gyro z-axis = %d \n", ois_dev.gyr_data.z); - - printf("\n"); - } - } - else - { - /* Get OIS gyro data through OIS interface - * @note for sensor which support gyro cross axes sensitivity pass the - * gyr_cross_sens_zx from the bmi2_dev structure */ - rslt = bmi2_ois_read_data(&sens_sel[1], 1, &ois_dev, ois_gyr_cross_sens_zx); - bmi2_ois_error_codes_print_result("bmi2_ois_read_data", rslt); - if (rslt == BMI2_OIS_OK) - { - /* Print gyro data */ - printf("OIS Gyro x-axis = %d ", ois_dev.gyr_data.x); - printf("OIS Gyro y-axis = %d ", ois_dev.gyr_data.y); - printf("OIS Gyro z-axis = %d ", ois_dev.gyr_data.z); - - printf("\n"); - } - } - } - } - - bmi2_ois_coines_deinit(); - - return rslt; -} diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2.c b/src/main/drivers/accgyro/bmi2.c similarity index 100% rename from lib/main/BoschSensortec/BMI270-Sensor-API/bmi2.c rename to src/main/drivers/accgyro/bmi2.c diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2.h b/src/main/drivers/accgyro/bmi2.h similarity index 100% rename from lib/main/BoschSensortec/BMI270-Sensor-API/bmi2.h rename to src/main/drivers/accgyro/bmi2.h diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_maximum_fifo.c b/src/main/drivers/accgyro/bmi270_maximum_fifo.c similarity index 100% rename from lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_maximum_fifo.c rename to src/main/drivers/accgyro/bmi270_maximum_fifo.c diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_maximum_fifo.h b/src/main/drivers/accgyro/bmi270_maximum_fifo.h similarity index 100% rename from lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_maximum_fifo.h rename to src/main/drivers/accgyro/bmi270_maximum_fifo.h diff --git a/lib/main/BoschSensortec/BMI270-Sensor-API/bmi2_defs.h b/src/main/drivers/accgyro/bmi2_defs.h similarity index 100% rename from lib/main/BoschSensortec/BMI270-Sensor-API/bmi2_defs.h rename to src/main/drivers/accgyro/bmi2_defs.h diff --git a/src/main/target/BETAFPVF411/target.mk b/src/main/target/BETAFPVF411/target.mk index 3e1d2e44a5..4008921362 100644 --- a/src/main/target/BETAFPVF411/target.mk +++ b/src/main/target/BETAFPVF411/target.mk @@ -4,7 +4,7 @@ FEATURES = VCP ONBOARDFLASH TARGET_SRC = \ drivers/accgyro/accgyro_mpu.c \ drivers/accgyro/accgyro_spi_mpu6000.c \ - $(ROOT)/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_maximum_fifo.c \ + drivers/accgyro/bmi270_maximum_fifo.c \ drivers/accgyro/accgyro_spi_bmi270.c \ drivers/barometer/barometer_bmp085.c \ drivers/barometer/barometer_bmp280.c \ diff --git a/src/main/target/IFLIGHT_F745_AIO_V2/target.mk b/src/main/target/IFLIGHT_F745_AIO_V2/target.mk index 6dc3654f9b..deb69abcb4 100644 --- a/src/main/target/IFLIGHT_F745_AIO_V2/target.mk +++ b/src/main/target/IFLIGHT_F745_AIO_V2/target.mk @@ -2,11 +2,11 @@ F7X5XG_TARGETS += $(TARGET) FEATURES += VCP ONBOARDFLASH TARGET_SRC = \ - $(ROOT)/lib/main/BoschSensortec/BMI270-Sensor-API/bmi270_maximum_fifo.c \ + drivers/accgyro/bmi270_maximum_fifo.c \ drivers/accgyro/accgyro_spi_bmi270.c \ drivers/barometer/barometer_ms5611.c \ drivers/barometer/barometer_bmp280.c \ - drivers/barometer/barometer_bmp085.c \ + drivers/barometer/barometer_bmp085.c \ drivers/barometer/barometer_ms5611.c \ drivers/compass/compass_hmc5883l.c \ drivers/compass/compass_qmc5883l.c \ diff --git a/src/main/target/TMTR_TMOTORF7/target.h b/src/main/target/TMTR_TMOTORF7/target.h index 1353cbead9..5e8a58685d 100644 --- a/src/main/target/TMTR_TMOTORF7/target.h +++ b/src/main/target/TMTR_TMOTORF7/target.h @@ -50,6 +50,12 @@ #define ACC_MPU6000_ALIGN CW0_DEG #define GYRO_MPU6000_ALIGN CW0_DEG +#define USE_SPI_GYRO +#define USE_ACCGYRO_BMI270 +#define BMI270_SPI_INSTANCE SPI1 +#define ACC_BMI270_ALIGN CW0_DEG +#define GYRO_BMI270_ALIGN CW0_DEG + #define USE_ACC #define USE_ACC_SPI_MPU6000 #define USE_ACC_SPI_MPU6500 diff --git a/src/main/target/TMTR_TMOTORF7/target.mk b/src/main/target/TMTR_TMOTORF7/target.mk index 15077cfbd3..faa81c689f 100644 --- a/src/main/target/TMTR_TMOTORF7/target.mk +++ b/src/main/target/TMTR_TMOTORF7/target.mk @@ -1,6 +1,8 @@ F7X2RE_TARGETS += $(TARGET) FEATURES = VCP ONBOARDFLASH TARGET_SRC = \ + drivers/accgyro/bmi270_maximum_fifo.c \ + drivers/accgyro/accgyro_spi_bmi270.c \ drivers/accgyro/accgyro_mpu6500.c \ drivers/accgyro/accgyro_spi_mpu6000.c \ drivers/accgyro/accgyro_spi_mpu6500.c \ From 3cdc84145eeabce900b24e9d6b55a7924c5eda6b Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Mon, 15 May 2023 13:40:19 -0500 Subject: [PATCH 12/36] reorganize gyroRateKHz with switch/case; add bmi270 3.2 KHz --- src/main/drivers/accgyro/gyro_sync.c | 28 +++++++++++++++++++--------- 1 file changed, 19 insertions(+), 9 deletions(-) diff --git a/src/main/drivers/accgyro/gyro_sync.c b/src/main/drivers/accgyro/gyro_sync.c index bc9ad37f27..fe36341caa 100644 --- a/src/main/drivers/accgyro/gyro_sync.c +++ b/src/main/drivers/accgyro/gyro_sync.c @@ -53,16 +53,26 @@ uint32_t gyroSetSampleRate(gyroDev_t *gyro, uint8_t lpf, uint8_t gyroSyncDenomin } gyro->mpuDividerDrops = gyroSyncDenominator - 1; gyro->gyroRateKHz = lpfNoneOr256 ? GYRO_RATE_8_kHz : GYRO_RATE_1_kHz; - //20649 is a weird gyro - if (gyro->mpuDetectionResult.sensor == ICM_20649_SPI) { - gyro->gyroRateKHz = lpfNoneOr256 ? GYRO_RATE_9_kHz : GYRO_RATE_1100_Hz; - } else if (gyro->mpuDetectionResult.sensor == BMI_160_SPI && lpfNoneOr256) { - //brainFPV is also a weird gyro - gyro->gyroRateKHz = GYRO_RATE_3200_Hz; - } else if (gyro_use_32khz) { - //use full 32k - gyro->gyroRateKHz = GYRO_RATE_32_kHz; + + + + switch (gyro->mpuDetectionResult.sensor) { + case ICM_20649_SPI: //20649 is a weird gyro + gyro->gyroRateKHz = lpfNoneOr256 ? GYRO_RATE_9_kHz : GYRO_RATE_1100_Hz; + break; + case BMI_160_SPI: //brainFPV is also a weird gyro + if (lpfNoneOr256) { gyro->gyroRateKHz = GYRO_RATE_3200_Hz; } + break; + case BMI_270_SPI: //bmi270 + gyro->gyroRateKHz = GYRO_RATE_3200_Hz; + break; + default: + if (gyro_use_32khz) { + //use full 32k + gyro->gyroRateKHz = GYRO_RATE_32_kHz; + } } + // return the targetLooptime (expected cycleTime) return (uint32_t)(gyroSyncDenominator * gyro->gyroRateKHz); } From 26b4b03b938a7a707e001572a52fb3e530a227e1 Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Mon, 15 May 2023 13:57:38 -0500 Subject: [PATCH 13/36] bmi270 - more file cleanup --- ...m_fifo.c => accgyro_bmi270_maximum_fifo.c} | 3 +- src/main/drivers/accgyro/bmi2.c | 10338 ---------------- src/main/drivers/accgyro/bmi2.h | 1513 --- .../drivers/accgyro/bmi270_maximum_fifo.h | 117 - src/main/drivers/accgyro/bmi2_defs.h | 2475 ---- src/main/target/BETAFPVF411/target.mk | 2 +- src/main/target/IFLIGHT_F745_AIO_V2/target.mk | 2 +- src/main/target/TMTR_TMOTORF7/target.mk | 2 +- 8 files changed, 5 insertions(+), 14447 deletions(-) rename src/main/drivers/accgyro/{bmi270_maximum_fifo.c => accgyro_bmi270_maximum_fifo.c} (99%) delete mode 100644 src/main/drivers/accgyro/bmi2.c delete mode 100644 src/main/drivers/accgyro/bmi2.h delete mode 100644 src/main/drivers/accgyro/bmi270_maximum_fifo.h delete mode 100644 src/main/drivers/accgyro/bmi2_defs.h diff --git a/src/main/drivers/accgyro/bmi270_maximum_fifo.c b/src/main/drivers/accgyro/accgyro_bmi270_maximum_fifo.c similarity index 99% rename from src/main/drivers/accgyro/bmi270_maximum_fifo.c rename to src/main/drivers/accgyro/accgyro_bmi270_maximum_fifo.c index c97e4bdd8b..0305f68f1a 100644 --- a/src/main/drivers/accgyro/bmi270_maximum_fifo.c +++ b/src/main/drivers/accgyro/accgyro_bmi270_maximum_fifo.c @@ -40,7 +40,8 @@ /*! Header files ****************************************************************************/ -#include "bmi270_maximum_fifo.h" +// #include "bmi270_maximum_fifo.h" +#include "platform.h" /***************************************************************************/ diff --git a/src/main/drivers/accgyro/bmi2.c b/src/main/drivers/accgyro/bmi2.c deleted file mode 100644 index 556039a1a4..0000000000 --- a/src/main/drivers/accgyro/bmi2.c +++ /dev/null @@ -1,10338 +0,0 @@ -/** -* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. -* -* BSD-3-Clause -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* 1. Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* -* 2. Redistributions in binary form must reproduce the above copyright -* notice, this list of conditions and the following disclaimer in the -* documentation and/or other materials provided with the distribution. -* -* 3. Neither the name of the copyright holder nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) -* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, -* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING -* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -* -* @file bmi2.c -* @date 2020-11-04 -* @version v2.63.1 -* -*/ - -/******************************************************************************/ - -/*! @name Header Files */ -/******************************************************************************/ -#include "bmi2.h" - -/***************************************************************************/ - -/*! Local structures - ****************************************************************************/ - -/*! @name Structure to define the difference in accelerometer values */ -struct bmi2_selftest_delta_limit -{ - /*! X data */ - int32_t x; - - /*! Y data */ - int32_t y; - - /*! Z data */ - int32_t z; -}; - -/*! @name Structure to store temporary accelerometer/gyroscope values */ -struct bmi2_foc_temp_value -{ - /*! X data */ - int32_t x; - - /*! Y data */ - int32_t y; - - /*! Z data */ - int32_t z; -}; - -/*! @name Structure to store accelerometer data deviation from ideal value */ -struct bmi2_offset_delta -{ - /*! X axis */ - int16_t x; - - /*! Y axis */ - int16_t y; - - /*! Z axis */ - int16_t z; -}; - -/*! @name Structure to store accelerometer offset values */ -struct bmi2_accel_offset -{ - /*! offset X data */ - uint8_t x; - - /*! offset Y data */ - uint8_t y; - - /*! offset Z data */ - uint8_t z; -}; - -/******************************************************************************/ - -/*! Local Function Prototypes - ******************************************************************************/ - -/*! - * @brief This internal API writes the configuration file. - * - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t write_config_file(struct bmi2_dev *dev); - -/*! - * @brief This internal API enables/disables the loading of the configuration - * file. - * - * @param[in] enable : To enable/disable configuration load. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_config_load(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API loads the configuration file. - * - * @param[in] config_data : Pointer to the configuration file. - * @param[in] index : Variable to define array index. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t upload_file(const uint8_t *config_data, uint16_t index, uint16_t write_len, struct bmi2_dev *dev); - -/*! - * @brief This internal API sets accelerometer configurations like ODR, - * bandwidth, performance mode and g-range. - * - * @param[in,out] config : Structure instance of bmi2_accel_config. - * @param[in,out] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_accel_config(struct bmi2_accel_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API validates bandwidth and performance mode of the - * accelerometer set by the user. - * - * @param[in, out] bandwidth : Pointer to bandwidth value set by the user. - * @param[in, out] perf_mode : Pointer to performance mode set by the user. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @note dev->info contains two warnings: BMI2_I_MIN_VALUE and BMI2_I_MAX_VALUE - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t validate_bw_perf_mode(uint8_t *bandwidth, uint8_t *perf_mode, struct bmi2_dev *dev); - -/*! - * @brief This internal API validates ODR and range of the accelerometer set by - * the user. - * - * @param[in, out] odr : Pointer to ODR value set by the user. - * @param[in, out] range : Pointer to range value set by the user. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @note dev->info contains two warnings: BMI2_I_MIN_VALUE and BMI2_I_MAX_VALUE - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t validate_odr_range(uint8_t *odr, uint8_t *range, struct bmi2_dev *dev); - -/*! - * @brief This internal API sets gyroscope configurations like ODR, bandwidth, - * low power/high performance mode, performance mode and range. - * - * @param[in,out] config : Structure instance of bmi2_gyro_config. - * @param[in,out] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_gyro_config(struct bmi2_gyro_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API validates bandwidth, performance mode, low power/ - * high performance mode, ODR, and range set by the user. - * - * @param[in] config : Structure instance of bmi2_gyro_config. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @note dev->info contains two warnings: BMI2_I_MIN_VALUE and BMI2_I_MAX_VALUE - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t validate_gyro_config(struct bmi2_gyro_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API shows the error status when illegal sensor - * configuration is set. - * - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t cfg_error_status(struct bmi2_dev *dev); - -/*! - * @brief This internal API: - * 1) Enables/Disables auxiliary interface. - * 2) Sets auxiliary interface configurations like I2C address, manual/auto - * mode enable, manual burst read length, AUX burst read length and AUX read - * address. - * 3)It maps/un-maps data interrupts to that of hardware interrupt line. - * - * @param[in] config : Structure instance of bmi2_aux_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_aux_config(struct bmi2_aux_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API sets gyroscope user-gain configurations like gain - * update value for x, y and z-axis. - * - * @param[in] config : Structure instance of bmi2_gyro_user_gain_config. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @verbatim - *---------------------------------------------------------------------------- - * bmi2_gyro_user_gain_config| - * Structure parameters | Description - *--------------------------|-------------------------------------------------- - * ratio_x | Gain update value for x-axis - * -------------------------|--------------------------------------------------- - * ratio_y | Gain update value for y-axis - * -------------------------|--------------------------------------------------- - * ratio_z | Gain update value for z-axis - * @endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_gyro_user_gain_config(const struct bmi2_gyro_user_gain_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API enables/disables auxiliary interface. - * - * @param[in] config : Structure instance of bmi2_aux_config. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_aux_interface(const struct bmi2_aux_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API sets auxiliary configurations like manual/auto mode - * FCU write command enable and read burst length for both data and manual mode. - * - * @param[in] config : Structure instance of bmi2_aux_config. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @note Auxiliary sensor should not be busy when configuring aux_i2c_addr, - * man_rd_burst_len, aux_rd_burst_len and aux_rd_addr. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t config_aux_interface(const struct bmi2_aux_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API triggers read out offset and sets ODR of the - * auxiliary sensor. - * - * @param[in] config : Structure instance of bmi2_aux_config. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t config_aux(const struct bmi2_aux_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API validates auxiliary configuration set by the user. - * - * @param[in, out] config : Structure instance of bmi2_aux_config. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @note dev->info contains two warnings: BMI2_I_MIN_VALUE and BMI2_I_MAX_VALUE - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t validate_aux_config(struct bmi2_aux_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets accelerometer configurations like ODR, - * bandwidth, performance mode and g-range. - * - * @param[out] config : Structure instance of bmi2_accel_config. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t get_accel_config(struct bmi2_accel_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets gyroscope configurations like ODR, bandwidth, - * low power/ high performance mode, performance mode and range. - * - * @param[out] config : Structure instance of bmi2_gyro_config. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t get_gyro_config(struct bmi2_gyro_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API: - * 1) Gets the status of auxiliary interface enable. - * 2) Gets auxiliary interface configurations like I2C address, manual/auto - * mode enable, manual burst read length, AUX burst read length and AUX read - * address. - * 3) Gets ODR and offset. - * - * @param[out] config : Structure instance of bmi2_aux_config. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t get_aux_config(struct bmi2_aux_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets gyroscope user-gain configurations like gain - * update value for x, y and z-axis. - * - * @param[out] config : Structure instance of bmi2_gyro_user_gain_config. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @verbatim - *---------------------------------------------------------------------------- - * bmi2_gyro_user_gain_config| - * Structure parameters | Description - *-------------------------|-------------------------------------------------- - * ratio_x | Gain update value for x-axis - * ------------------------|--------------------------------------------------- - * ratio_y | Gain update value for y-axis - * ------------------------|--------------------------------------------------- - * ratio_z | Gain update value for z-axis - * - * @endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t get_gyro_gain_update_config(struct bmi2_gyro_user_gain_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets the enable status of auxiliary interface. - * - * @param[out] config : Structure instance of bmi2_aux_config. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t get_aux_interface(struct bmi2_aux_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets auxiliary configurations like manual/auto mode - * FCU write command enable and read burst length for both data and manual mode. - * - * @param[out] config : Structure instance of bmi2_aux_config. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t get_aux_interface_config(struct bmi2_aux_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets read out offset and ODR of the auxiliary - * sensor. - * - * @param[out] config : Structure instance of bmi2_aux_config. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t get_aux_cfg(struct bmi2_aux_config *config, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets the saturation status for the gyroscope user - * gain update. - * - * @param[out] user_gain_stat : Stores the saturation status of the axes. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t get_gyro_gain_update_status(struct bmi2_gyr_user_gain_status *user_gain, struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to extract the output feature configuration - * details like page and start address from the look-up table. - * - * @param[out] feat_output : Structure that stores output feature - * configurations. - * @param[in] type : Type of feature or sensor. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Returns the feature found flag. - * - * @retval BMI2_FALSE : Feature not found - * BMI2_TRUE : Feature found - */ -static uint8_t extract_output_feat_config(struct bmi2_feature_config *feat_output, - uint8_t type, - const struct bmi2_dev *dev); - -/*! - * @brief This internal API gets the cross sensitivity coefficient between - * gyroscope's X and Z axes. - * - * @param[out] cross_sense : Pointer to the stored cross sensitivity - * coefficient. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t get_gyro_cross_sense(int16_t *cross_sense, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets the accelerometer data from the register. - * - * @param[out] data : Structure instance of sensor_data. - * @param[in] reg_addr : Register address where data is stored. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t get_accel_sensor_data(struct bmi2_sens_axes_data *data, uint8_t reg_addr, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets the gyroscope data from the register. - * - * @param[out] data : Structure instance of sensor_data. - * @param[in] reg_addr : Register address where data is stored. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t get_gyro_sensor_data(struct bmi2_sens_axes_data *data, uint8_t reg_addr, struct bmi2_dev *dev); - -/*! - * @brief This internal API gets the accelerometer/gyroscope data. - * - * @param[out] data : Structure instance of sensor_data. - * @param[in] reg_data : Data stored in the register. - * - * @return None - * - * @retval None - */ -static void get_acc_gyr_data(struct bmi2_sens_axes_data *data, const uint8_t *reg_data); - -/*! - * @brief This internal API gets the re-mapped accelerometer/gyroscope data. - * - * @param[out] data : Structure instance of sensor_data. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return None - * - * @retval None - */ -static void get_remapped_data(struct bmi2_sens_axes_data *data, const struct bmi2_dev *dev); - -/*! - * @brief This internal API reads the user-defined bytes of data from the given - * register address of auxiliary sensor in data mode. - * - * @param[out] aux_data : Pointer to the stored auxiliary data. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t read_aux_data_mode(uint8_t *aux_data, struct bmi2_dev *dev); - -/*! - * @brief This internal API reads the user-defined bytes of data from the given - * register address of auxiliary sensor in manual mode. - * - * @param[in] reg_addr : AUX address from where data is read. - * @param[out] aux_data : Pointer to the stored auxiliary data. - * @param[in] len : Total bytes to be read. - * @param[in] burst_len : Bytes of data to be read in bursts. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t read_aux_data(uint8_t reg_addr, uint8_t *aux_data, uint16_t len, uint8_t burst_len, struct bmi2_dev *dev); - -/*! - * @brief This internal API checks the busy status of auxiliary sensor and sets - * the auxiliary register addresses when not busy. - * - * @param[in] reg_addr : Address in which AUX register address is - * set. - * @param[in] reg_data : Auxiliary register address to be set when AUX is - * not busy. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_if_aux_not_busy(uint8_t reg_addr, uint8_t reg_data, struct bmi2_dev *dev); - -/*! - * @brief his internal API maps the actual burst read length with that of the - * register value set by user. - * - * @param[out] len : Actual burst length. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t map_read_len(uint8_t *len, const struct bmi2_dev *dev); - -/*! - * @brief This internal API writes AUX write address and the user-defined bytes - * of data to the AUX sensor in manual mode. - * - * @note Change of BMI2_AUX_WR_ADDR is only allowed if AUX is not busy. - * - * @param[in] reg_addr : AUX address in which data is to be written. - * @param[in] reg_data : Data to be written - * @param[in] dev : Structure instance of bmi2_dev. - * - * @note Change of BMI2_AUX_WR_ADDR is only allowed if AUX is not busy. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t write_aux_data(uint8_t reg_addr, uint8_t reg_data, struct bmi2_dev *dev); - -/*! - * @brief This internal API maps/unmaps feature interrupts to that of interrupt - * pins. - * - * @param[in] int_pin : Interrupt pin selected. - * @param[in] feat_int : Type of feature interrupt to be mapped. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t map_feat_int(uint8_t *reg_data_array, enum bmi2_hw_int_pin int_pin, uint8_t int_mask); - -/*! - * @brief This internal API computes the number of bytes of accelerometer FIFO - * data which is to be parsed in header-less mode. - * - * @param[out] start_idx : The start index for parsing data. - * @param[out] len : Number of bytes to be parsed. - * @param[in] acc_count : Number of accelerometer frames to be read. - * @param[in] fifo : Structure instance of bmi2_fifo_frame. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t parse_fifo_accel_len(uint16_t *start_idx, - uint16_t *len, - const uint16_t *acc_count, - const struct bmi2_fifo_frame *fifo); - -/*! - * @brief This internal API is used to parse accelerometer data from the FIFO - * data in header mode. - * - * @param[out] acc : Structure instance of bmi2_sens_axes_data where - * the parsed accelerometer data bytes are stored. - * @param[in] accel_length : Number of accelerometer frames (x,y,z data). - * @param[in] fifo : Structure instance of bmi2_fifo_frame. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t extract_accel_header_mode(struct bmi2_sens_axes_data *acc, - uint16_t *accel_length, - struct bmi2_fifo_frame *fifo, - const struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to parse the accelerometer data from the - * FIFO data in both header and header-less mode. It updates the current data - * byte to be parsed. - * - * @param[in,out] acc : Structure instance of bmi2_sens_axes_data where - * where the parsed data bytes are stored. - * @param[in,out] idx : Index value of number of bytes parsed. - * @param[in,out] acc_idx : Index value of accelerometer data (x,y,z axes) - * frame to be parsed. - * @param[in] frame : Either data is enabled by user in header-less - * mode or header frame value in header mode. - * @param[in] fifo : Structure instance of bmi2_fifo_frame. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t unpack_accel_frame(struct bmi2_sens_axes_data *acc, - uint16_t *idx, - uint16_t *acc_idx, - uint8_t frame, - const struct bmi2_fifo_frame *fifo, - const struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to parse accelerometer data from the FIFO - * data. - * - * @param[out] acc : Structure instance of bmi2_sens_axes_data - * where the parsed data bytes are stored. - * @param[in] data_start_index : Index value of the accelerometer data bytes - * which is to be parsed from the FIFO data. - * @param[in] fifo : Structure instance of bmi2_fifo_frame. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return None - * @retval None - */ -static void unpack_accel_data(struct bmi2_sens_axes_data *acc, - uint16_t data_start_index, - const struct bmi2_fifo_frame *fifo, - const struct bmi2_dev *dev); - -/*! - * @brief This internal API computes the number of bytes of gyroscope FIFO data - * which is to be parsed in header-less mode. - * - * @param[out] start_idx : The start index for parsing data. - * @param[out] len : Number of bytes to be parsed. - * @param[in] gyr_count : Number of gyroscope frames to be read. - * @param[in] frame : Either data enabled by user in header-less - * mode or header frame value in header mode. - * @param[in] fifo : Structure instance of bmi2_fifo_frame. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t parse_fifo_gyro_len(uint16_t *start_idx, - uint16_t(*len), - const uint16_t *gyr_count, - const struct bmi2_fifo_frame *fifo); - -/*! - * @brief This internal API is used to parse the gyroscope data from the FIFO - * data in both header and header-less mode It updates the current data byte to - * be parsed. - * - * @param[in,out] gyr : Structure instance of bmi2_sens_axes_data. - * @param[in,out] idx : Index value of number of bytes parsed - * @param[in,out] gyr_idx : Index value of gyroscope data (x,y,z axes) - * frame to be parsed. - * @param[in] frame : Either data is enabled by user in header-less - * mode or header frame value in header mode. - * @param[in] fifo : Structure instance of bmi2_fifo_frame. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t unpack_gyro_frame(struct bmi2_sens_axes_data *gyr, - uint16_t *idx, - uint16_t *gyr_idx, - uint8_t frame, - const struct bmi2_fifo_frame *fifo, - const struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to parse gyroscope data from the FIFO data. - * - * @param[out] gyr : Structure instance of bmi2_sens_axes_data where - * the parsed gyroscope data bytes are stored. - * @param[in] data_start_index : Index value of the gyroscope data bytes - * which is to be parsed from the FIFO data. - * @param[in] fifo : Structure instance of bmi2_fifo_frame. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return None - * @retval None - */ -static void unpack_gyro_data(struct bmi2_sens_axes_data *gyr, - uint16_t data_start_index, - const struct bmi2_fifo_frame *fifo, - const struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to parse the gyroscope data from the - * FIFO data in header mode. - * - * @param[out] gyr : Structure instance of bmi2_sens_axes_data where - * the parsed gyroscope data bytes are stored. - * @param[in] gyro_length : Number of gyroscope frames (x,y,z data). - * @param[in] fifo : Structure instance of bmi2_fifo_frame. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t extract_gyro_header_mode(struct bmi2_sens_axes_data *gyr, - uint16_t *gyro_length, - struct bmi2_fifo_frame *fifo, - const struct bmi2_dev *dev); - -/*! - * @brief This API computes the number of bytes of auxiliary FIFO data - * which is to be parsed in header-less mode. - * - * @param[out] start_idx : The start index for parsing data. - * @param[out] len : Number of bytes to be parsed. - * @param[in] aux_count : Number of accelerometer frames to be read. - * @param[in] fifo : Structure instance of bmi2_fifo_frame. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t parse_fifo_aux_len(uint16_t *start_idx, - uint16_t(*len), - const uint16_t *aux_count, - const struct bmi2_fifo_frame *fifo); - -/*! - * @brief This API is used to parse auxiliary data from the FIFO data. - * - * @param[out] aux : Pointer to buffer where the parsed auxiliary data - * bytes are stored. - * @param[in] aux_length : Number of auxiliary frames (x,y,z data). - * @param[in] fifo : Structure instance of bmi2_fifo_frame. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t extract_aux_header_mode(struct bmi2_aux_fifo_data *aux, - uint16_t *aux_length, - struct bmi2_fifo_frame *fifo, - const struct bmi2_dev *dev); - -/*! - * @brief This API is used to parse the auxiliary data from the FIFO data in - * both header and header-less mode. It updates the current data byte to be - * parsed. - * - * @param[out] aux : Pointer to structure where the parsed auxiliary data - * bytes are stored. - * @param[in,out] idx : Index value of number of bytes parsed - * @param[in,out] aux_idx : Index value of auxiliary data (x,y,z axes) - * frame to be parsed - * @param[in] frame : Either data is enabled by user in header-less - * mode or header frame value in header mode. - * @param[in] fifo : Structure instance of bmi2_fifo_frame. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t unpack_aux_frame(struct bmi2_aux_fifo_data *aux, - uint16_t *idx, - uint16_t *aux_idx, - uint8_t frame, - const struct bmi2_fifo_frame *fifo, - const struct bmi2_dev *dev); - -/*! - * @brief This API is used to parse auxiliary data from the FIFO data. - * - * @param[out] aux : Pointer to structure where the parsed - * auxiliary data bytes are stored. - * @param[in] data_start_index : Index value of the auxiliary data bytes which - * is to be parsed from the FIFO data. - * @param[in] fifo : Structure instance of bmi2_fifo_frame. - * - * @return None - * @retval None - */ -static void unpack_aux_data(struct bmi2_aux_fifo_data *aux, - uint16_t data_start_index, - const struct bmi2_fifo_frame *fifo); - -/*! - * @brief This internal API is used to reset the FIFO related configurations - * in the FIFO frame structure for the next FIFO read. - * - * @param[in, out] fifo : Structure instance of bmi2_fifo_frame. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return None - * @retval None - */ -static void reset_fifo_frame_structure(struct bmi2_fifo_frame *fifo, const struct bmi2_dev *dev); - -/*! - * @brief This internal API checks whether the FIFO data read is an empty frame. - * If empty frame, index is moved to the last byte. - * - * @param[in,out] data_index : The index of the current data to be parsed from - * FIFO data. - * @param[in] fifo : Structure instance of bmi2_fifo_frame. - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_W_FIFO_EMPTY - Warning : FIFO is empty - * @retval BMI2_W_PARTIAL_READ - Warning : There are more frames to be read - */ -static int8_t check_empty_fifo(uint16_t *data_index, const struct bmi2_fifo_frame *fifo); - -/*! - * @brief This internal API is used to move the data index ahead of the - * current frame length parameter when unnecessary FIFO data appears while - * extracting the user specified data. - * - * @param[in,out] data_index : Index of the FIFO data which is to be - * moved ahead of the current frame length - * @param[in] current_frame_length : Number of bytes in the current frame. - * @param[in] fifo : Structure instance of bmi2_fifo_frame. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t move_next_frame(uint16_t *data_index, uint8_t current_frame_length, const struct bmi2_fifo_frame *fifo); - -/*! - * @brief This internal API is used to parse and store the sensor time from the - * FIFO data. - * - * @param[in,out] data_index : Index of the FIFO data which has the sensor time. - * @param[in] fifo : Structure instance of bmi2_fifo_frame. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t unpack_sensortime_frame(uint16_t *data_index, struct bmi2_fifo_frame *fifo); - -/*! - * @brief This internal API is used to parse and store the skipped frame count - * from the FIFO data. - * - * @param[in,out] data_index : Index of the FIFO data which contains skipped - * frame count. - * @param[in] fifo : Structure instance of bmi2_fifo_frame. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t unpack_skipped_frame(uint16_t *data_index, struct bmi2_fifo_frame *fifo); - -/*! - * @brief This internal API enables and configures the accelerometer which is - * needed for self-test operation. It also sets the amplitude for the self-test. - * - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t pre_self_test_config(struct bmi2_dev *dev); - -/*! - * @brief This internal API performs the steps needed for self-test operation - * before reading the accelerometer self-test data. - * - * @param[in] sign : Selects sign of self-test excitation - * @param[in] dev : Structure instance of bmi2_dev. - * - * sign | Description - * -------------|--------------- - * BMI2_ENABLE | positive excitation - * BMI2_DISABLE | negative excitation - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t self_test_config(uint8_t sign, struct bmi2_dev *dev); - -/*! - * @brief This internal API enables or disables the accelerometer self-test - * feature in the sensor. - * - * @param[in] enable : Enables/ Disables self-test. - * @param[in] dev : Structure instance of bmi2_dev. - * - * sign | Description - * -------------|--------------- - * BMI2_ENABLE | Enables self-test - * BMI2_DISABLE | Disables self-test - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_accel_self_test_enable(uint8_t enable, struct bmi2_dev *dev); - -/*! - * @brief This internal API selects the sign for accelerometer self-test - * excitation. - * - * @param[in] sign : Selects sign of self-test excitation - * @param[in] dev : Structure instance of bmi2_dev. - * - * sign | Description - * -------------|--------------- - * BMI2_ENABLE | positive excitation - * BMI2_DISABLE | negative excitation - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_acc_self_test_sign(uint8_t sign, struct bmi2_dev *dev); - -/*! - * @brief This internal API sets the amplitude of the accelerometer self-test - * deflection in the sensor. - * - * @param[in] amp : Select amplitude of the self-test deflection. - * @param[in] dev : Structure instance of bmi2_dev. - * - * amp | Description - * -------------|--------------- - * BMI2_ENABLE | self-test amplitude is high - * BMI2_DISABLE | self-test amplitude is low - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_accel_self_test_amp(uint8_t amp, struct bmi2_dev *dev); - -/*! - * @brief This internal API reads the accelerometer data for x,y and z axis from - * the sensor. The data units is in LSB format. - * - * @param[out] accel : Buffer to store the acceleration value. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t read_accel_xyz(struct bmi2_sens_axes_data *accel, struct bmi2_dev *dev); - -/*! - * @brief This internal API converts LSB value of accelerometer axes to form - * 'g' to 'mg' for self-test. - * - * @param[in] acc_data_diff : Stores the acceleration value difference in g. - * @param[out]acc_data_diff_mg : Stores the acceleration value difference in mg. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return None - * @retval None - */ -static void convert_lsb_g(const struct bmi2_selftest_delta_limit *acc_data_diff, - struct bmi2_selftest_delta_limit *acc_data_diff_mg, - const struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to calculate the power of a value. - * - * @param[in] base : base for power calculation. - * @param[in] resolution : exponent for power calculation. - * - * @return the calculated power - * @retval the power value - */ -static int32_t power(int16_t base, uint8_t resolution); - -/*! - * @brief This internal API validates the accelerometer self-test data and - * decides the result of self-test operation. - * - * @param[in] accel_data_diff : Stores the acceleration value difference. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t validate_self_test(const struct bmi2_selftest_delta_limit *accel_data_diff); - -/*! - * @brief This internal API gets the re-mapped x, y and z axes from the sensor. - * - * @param[out] remap : Structure that stores local copy of re-mapped axes. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t get_remap_axes(struct bmi2_axes_remap *remap, struct bmi2_dev *dev); - -/*! - * @brief This internal API sets the re-mapped x, y and z axes in the sensor. - * - * @param[in] remap : Structure that stores local copy of re-mapped axes. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_remap_axes(const struct bmi2_axes_remap *remap, struct bmi2_dev *dev); - -/*! - * @brief Interface to get max burst length - * - * @param[in] max_burst_len : Pointer to store max burst length - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t get_maxburst_len(uint8_t *max_burst_len, struct bmi2_dev *dev); - -/*! - * @brief This internal API sets the max burst length. - * - * @param[in] write_len_byte : read & write length - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_maxburst_len(const uint16_t write_len_byte, struct bmi2_dev *dev); - -/*! - * @brief This internal API parses virtual frame header from the FIFO data. - * - * @param[in, out] frame_header : FIFO frame header. - * @param[in, out] data_index : Index value of the FIFO data bytes - * from which sensor frame header is to be parsed - * @param[in] fifo : Structure instance of bmi2_fifo_frame. - * - * @return None - * @retval None - */ -static void parse_if_virtual_header(uint8_t *frame_header, uint16_t *data_index, const struct bmi2_fifo_frame *fifo); - -/*! - * @brief This internal API gets sensor time from the accelerometer and - * gyroscope virtual frames and updates in the data structure. - * - * @param[out] sens : Sensor data structure - * @param[in, out] idx : Index of FIFO from where the data is to retrieved. - * @param[in] fifo : Structure instance of bmi2_fifo_frame. - * - * @return None - * @retval None - */ -static void unpack_virt_sensor_time(struct bmi2_sens_axes_data *sens, uint16_t *idx, - const struct bmi2_fifo_frame *fifo); - -/*! - * @brief This internal API gets sensor time from the auxiliary virtual - * frames and updates in the data structure. - * - * @param[out] aux : Auxiliary sensor data structure - * @param[in, out] idx : Index of FIFO from where the data is to retrieved. - * @param[in] fifo : Structure instance of bmi2_fifo_frame. - * - * @return None - * @retval None - */ -static void unpack_virt_aux_sensor_time(struct bmi2_aux_fifo_data *aux, - uint16_t *idx, - const struct bmi2_fifo_frame *fifo); - -/*! - * @brief This internal API corrects the gyroscope cross-axis sensitivity - * between the z and the x axis. - * - * @param[in] dev : Structure instance of bmi2_dev. - * @param[out] gyr_data : Structure instance of gyroscope data - * - * @return Result of API execution status - * - * @return None - * @retval None - */ -static void comp_gyro_cross_axis_sensitivity(struct bmi2_sens_axes_data *gyr_data, const struct bmi2_dev *dev); - -/*! - * @brief This internal API saves the configurations before performing FOC. - * - * @param[out] acc_cfg : Accelerometer configuration value - * @param[out] aps : Advance power mode value - * @param[out] acc_en : Accelerometer enable value - * @param[in] dev : Structure instance of bmi2_dev - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t save_accel_foc_config(struct bmi2_accel_config *acc_cfg, - uint8_t *aps, - uint8_t *acc_en, - struct bmi2_dev *dev); - -/*! - * @brief This internal API performs Fast Offset Compensation for accelerometer. - * - * @param[in] accel_g_value : This parameter selects the accel foc - * axis to be performed - * - * input format is {x, y, z, sign}. '1' to enable. '0' to disable - * - * eg to choose x axis {1, 0, 0, 0} - * eg to choose -x axis {1, 0, 0, 1} - * - * @param[in] acc_cfg : Accelerometer configuration value - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t perform_accel_foc(const struct bmi2_accel_foc_g_value *accel_g_value, - const struct bmi2_accel_config *acc_cfg, - struct bmi2_dev *dev); - -/*! - * @brief This internal sets configurations for performing accelerometer FOC. - * - * @param[in] dev : Structure instance of bmi2_dev - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_accel_foc_config(struct bmi2_dev *dev); - -/*! - * @brief This internal API enables/disables the offset compensation for - * filtered and un-filtered accelerometer data. - * - * @param[in] offset_en : enables/disables offset compensation. - * @param[in] dev : Structure instance of bmi2_dev - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_accel_offset_comp(uint8_t offset_en, struct bmi2_dev *dev); - -/*! - * @brief This internal API converts the range value into accelerometer - * corresponding integer value. - * - * @param[in] range_in : Input range value. - * @param[out] range_out : Stores the integer value of range. - * - * @return None - * @retval None - */ -static void map_accel_range(uint8_t range_in, uint8_t *range_out); - -/*! - * @brief This internal API compensate the accelerometer data against gravity. - * - * @param[in] lsb_per_g : LSB value pre 1g. - * @param[in] g_val : G reference value of all axis. - * @param[in] data : Accelerometer data - * @param[out] comp_data : Stores the data that is compensated by taking the - * difference in accelerometer data and lsb_per_g - * value. - * - * @return None - * @retval None - */ -static void comp_for_gravity(uint16_t lsb_per_g, - const struct bmi2_accel_foc_g_value *g_val, - const struct bmi2_sens_axes_data *data, - struct bmi2_offset_delta *comp_data); - -/*! - * @brief This internal API scales the compensated accelerometer data according - * to the offset register resolution. - * - * @param[in] range : G-range of the accelerometer. - * @param[out] comp_data : Data that is compensated by taking the - * difference in accelerometer data and lsb_per_g - * value. - * @param[out] data : Stores offset data - * - * @return None - * @retval None - */ -static void scale_accel_offset(uint8_t range, const struct bmi2_offset_delta *comp_data, - struct bmi2_accel_offset *data); - -/*! - * @brief This internal API finds the bit position of 3.9mg according to given - * range and resolution. - * - * @param[in] range : G-range of the accelerometer. - * - * @return Result of API execution status - * @retval Bit position of 3.9mg - */ -static int8_t get_bit_pos_3_9mg(uint8_t range); - -/*! - * @brief This internal API inverts the accelerometer offset data. - * - * @param[out] offset_data : Stores the inverted offset data - * - * @return None - * @retval None - */ -static void invert_accel_offset(struct bmi2_accel_offset *offset_data); - -/*! - * @brief This internal API writes the offset data in the offset compensation - * register. - * - * @param[in] offset : offset data - * @param[in] dev : Structure instance of bmi2_dev - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t write_accel_offset(const struct bmi2_accel_offset *offset, struct bmi2_dev *dev); - -/*! - * @brief This internal API restores the configurations saved before performing - * accelerometer FOC. - * - * @param[in] acc_cfg : Accelerometer configuration value - * @param[in] acc_en : Accelerometer enable value - * @param[in] aps : Advance power mode value - * @param[in] dev : Structure instance of bmi2_dev - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t restore_accel_foc_config(struct bmi2_accel_config *acc_cfg, - uint8_t aps, - uint8_t acc_en, - struct bmi2_dev *dev); - -/*! - * @brief This internal API saves the configurations before performing gyroscope - * FOC. - * - * @param[out] gyr_cfg : Gyroscope configuration value - * @param[out] gyr_en : Gyroscope enable value - * @param[out] aps : Advance power mode value - * @param[in] dev : Structure instance of bmi2_dev - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t save_gyro_config(struct bmi2_gyro_config *gyr_cfg, uint8_t *aps, uint8_t *gyr_en, struct bmi2_dev *dev); - -/*! - * @brief This internal sets configurations for performing gyroscope FOC. - * - * @param[in] dev : Structure instance of bmi2_dev - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_gyro_foc_config(struct bmi2_dev *dev); - -/*! - * @brief This internal API inverts the gyroscope offset data. - * - * @param[out] offset_data : Stores the inverted offset data - * - * @return None - * @retval None - */ -static void invert_gyro_offset(struct bmi2_sens_axes_data *offset_data); - -/*! - * @brief This internal API restores the gyroscope configurations saved - * before performing FOC. - * - * @param[in] gyr_cfg : Gyroscope configuration value - * @param[in] gyr_en : Gyroscope enable value - * @param[in] aps : Advance power mode value - * @param[in] dev : Structure instance of bmi2_dev - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t restore_gyro_config(struct bmi2_gyro_config *gyr_cfg, uint8_t aps, uint8_t gyr_en, struct bmi2_dev *dev); - -/*! - * @brief This internal API saturates the gyroscope data value before writing to - * to 10 bit offset register. - * - * @param[in, out] gyr_off : Gyroscope data to be stored in offset register - * - * @return None - * @retval None - */ -static void saturate_gyro_data(struct bmi2_sens_axes_data *gyr_off); - -/*! - * @brief This internal API reads the gyroscope data for x, y and z axis from - * the sensor. - * - * @param[out] gyro : Buffer to store the gyroscope value. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t read_gyro_xyz(struct bmi2_sens_axes_data *gyro, struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to check the boundary conditions. - * - * @param[in] dev : Structure instance of bmi2_dev. - * @param[in,out] val : Pointer to the value to be validated. - * @param[in] min : minimum value. - * @param[in] max : maximum value. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t check_boundary_val(uint8_t *val, uint8_t min, uint8_t max, struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to validate the device pointer for - * null conditions. - * - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t null_ptr_check(const struct bmi2_dev *dev); - -/*! - * @brief This updates the result for CRT or gyro self-test. - * - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t crt_gyro_st_update_result(struct bmi2_dev *dev); - -/*! - * @brief This function is to get the st_status status. - * - * @param[in] *st_status: gets the crt running status - * @param[in] dev : Structure instance of bmi2_dev - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t get_st_running(uint8_t *st_status, struct bmi2_dev *dev); - -/*! - * @brief This function is to set crt bit to running. - * - * @param[in] enable - * @param[in] dev : Structure instance of bmi2_dev - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_st_running(uint8_t st_status, struct bmi2_dev *dev); - -/*! - * @brief This function is to make the initial changes for CRT. - * Disable the gyro, OIS, aps - * Note: For the purpose of preparing CRT Gyro, OIS and APS are disabled - * - * @param[in] dev : Structure instance of bmi2_dev - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t crt_prepare_setup(struct bmi2_dev *dev); - -static int8_t do_gtrigger_test(uint8_t gyro_st_crt, struct bmi2_dev *dev); - -/*! - * @brief This function is to get the rdy for dl bit status - * this will toggle from 0 to 1 and visevers according to the - * dowload status - * - * @param[in] *rdy_for_dl: gets the rdy_for_dl status - * @param[in] dev : Structure instance of bmi2_dev - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t get_rdy_for_dl(uint8_t *rdy_for_dl, struct bmi2_dev *dev); - -/*! - * @brief This function is to write the config file in the given location for crt. - * which inter checks the status of the rdy_for_dl bit and also the crt running, and - * wirtes the given size. - * - * @param[in] write_len: length of the words to be written - * @param[in] config_file_size: length of the words to be written - * @param[in] start_index: provide the start index from where config file has to written - * @param[in] dev : Structure instance of bmi2_dev - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t write_crt_config_file(uint16_t write_len, - uint16_t config_file_size, - uint16_t start_index, - struct bmi2_dev *dev); - -/*! - * @brief This function is to check for rdy_for_dl bit to toggle for CRT process - * - * @param[in] retry_complete: wait for given time to toggle - * @param[in] download_ready: get the status for rdy_for_dl - * @param[in] dev : Structure instance of bmi2_dev - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t wait_rdy_for_dl_toggle(uint8_t retry_complete, uint8_t download_ready, struct bmi2_dev *dev); - -/*! - * @brief This function is to wait till the CRT or gyro self-test process is completed - * - * @param[in] retry_complete: wait for given time to complete the crt process - * @param[in] dev : Structure instance of bmi2_dev - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t wait_st_running(uint8_t retry_complete, struct bmi2_dev *dev); - -/*! - * @brief This function is to complete the crt process if max burst length is not zero - * this checks for the crt status and rdy_for_dl bit to toggle - * - * @param[in] last_byte_flag: to provide the last toggled state - * @param[in] dev : Structure instance of bmi2_dev - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t process_crt_download(uint8_t last_byte_flag, struct bmi2_dev *dev); - -/*! - * @brief This api is used to enable the gyro self-test or crt. - * - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t select_self_test(uint8_t gyro_st_crt, struct bmi2_dev *dev); - -/*! - * @brief This api is used to enable/disable abort. - * - * @param[in] abort_enable : variable to enable the abort feature. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t abort_bmi2(uint8_t abort_enable, struct bmi2_dev *dev); - -/*! - * @brief This api is use to wait till gyro self-test is completed and update the status of gyro - * self-test. - * - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t gyro_self_test_completed(struct bmi2_gyro_self_test_status *gyro_st_result, struct bmi2_dev *dev); - -/*! - * @brief This api is used to trigger the preparation for system for NVM programming. - * - * @param[out] nvm_prep : pointer to variable to store the status of nvm_prep_prog. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t set_nvm_prep_prog(uint8_t nvm_prep, struct bmi2_dev *dev); - -/*! - * @brief This api validates accel foc position as per the range - * - * @param[in] sens_list : Sensor type - * @param[in] accel_g_axis : accel axis to foc. NA for gyro foc - * @param[in] avg_foc_data : average value of sensor sample datas - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t validate_foc_position(uint8_t sens_list, - const struct bmi2_accel_foc_g_value *accel_g_axis, - struct bmi2_sens_axes_data avg_foc_data, - struct bmi2_dev *dev); - -/*! - * @brief This api validates accel foc axis given as input - * - * @param[in] avg_foc_data : average value of sensor sample datas - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t validate_foc_accel_axis(int16_t avg_foc_data, struct bmi2_dev *dev); - -/*! - * @brief This api is used to verify the right position of the sensor before doing accel foc - * - * @param[in] dev : Structure instance of bmi2_dev. - * @param[in] sens_list: Sensor type - * @param[in] accel_g_axis: Accel Foc axis and sign input - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t verify_foc_position(uint8_t sens_list, - const struct bmi2_accel_foc_g_value *accel_g_axis, - struct bmi2_dev *dev); - -/*! - * @brief This API reads and provides average for 128 samples of sensor data for foc operation - * gyro. - * - * @param[in] sens_list : Sensor type. - * @param[in] bmi2_dev: Structure instance of bmi2_dev. - * @param[in] temp_foc_data: to store data samples - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t get_average_of_sensor_data(uint8_t sens_list, - struct bmi2_foc_temp_value *temp_foc_data, - struct bmi2_dev *dev); - -/*! - * @brief This internal api gets major and minor version for config file - * - * @param[out] config_major : Pointer to store the major version - * @param[out] config_minor : Pointer to store the minor version - * @param[in] dev : Structure instance of bmi2_dev - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t extract_config_file(uint8_t *config_major, uint8_t *config_minor, struct bmi2_dev *dev); - -/*! - * @brief This internal API is used to map the interrupts to the sensor. - * - * @param[in] map_int : Structure instance of bmi2_map_int. - * @param[in] type : Type of feature or sensor. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return None - * @retval None - */ -static void extract_feat_int_map(struct bmi2_map_int *map_int, uint8_t type, const struct bmi2_dev *dev); - -/*! - * @brief This internal API selects the sensors/features to be enabled or - * disabled. - * - * @param[in] sens_list : Pointer to select the sensor. - * @param[in] n_sens : Number of sensors selected. - * @param[out] sensor_sel : Gets the selected sensor. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t select_sensor(const uint8_t *sens_list, uint8_t n_sens, uint64_t *sensor_sel); - -/*! - * @brief This internal API enables the selected sensor/features. - * - * @param[in] sensor_sel : Selects the desired sensor. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t sensor_enable(uint64_t sensor_sel, struct bmi2_dev *dev); - -/*! - * @brief This internal API disables the selected sensor/features. - * - * @param[in] sensor_sel : Selects the desired sensor. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t sensor_disable(uint64_t sensor_sel, struct bmi2_dev *dev); - -/******************************************************************************/ -/*! @name User Interface Definitions */ -/******************************************************************************/ - -/*! - * @brief This API is the entry point for bmi2 sensor. It selects between - * I2C/SPI interface, based on user selection. It reads and validates the - * chip-id of the sensor. - */ -int8_t bmi2_sec_init(struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to assign chip id */ - uint8_t chip_id = 0; - - /* Structure to define the default values for axes re-mapping */ - struct bmi2_axes_remap axes_remap = { - .x_axis = BMI2_MAP_X_AXIS, .x_axis_sign = BMI2_POS_SIGN, .y_axis = BMI2_MAP_Y_AXIS, - .y_axis_sign = BMI2_POS_SIGN, .z_axis = BMI2_MAP_Z_AXIS, .z_axis_sign = BMI2_POS_SIGN - }; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if (rslt == BMI2_OK) - { - /* Perform soft-reset to bring all register values to their - * default values - */ - rslt = bmi2_soft_reset(dev); - - if (rslt == BMI2_OK) - { - /* Read chip-id of the BMI2 sensor */ - rslt = bmi2_get_regs(BMI2_CHIP_ID_ADDR, &chip_id, 1, dev); - - if (rslt == BMI2_OK) - { - /* Validate chip-id */ - if (chip_id == dev->chip_id) - { - /* Assign resolution to the structure */ - dev->resolution = 16; - - /* Set manual enable flag */ - dev->aux_man_en = 1; - - /* Set the default values for axis - * re-mapping in the device structure - */ - dev->remap = axes_remap; - } - else - { - /* Storing the chip-id value read from - * the register to identify the sensor - */ - dev->chip_id = chip_id; - rslt = BMI2_E_DEV_NOT_FOUND; - } - } - } - } - - return rslt; -} - -/*! - * @brief This API reads the data from the given register address of bmi2 - * sensor. - * - * @note For most of the registers auto address increment applies, with the - * exception of a few special registers, which trap the address. For e.g., - * Register address - 0x26, 0x5E. - */ -int8_t bmi2_get_regs(uint8_t reg_addr, uint8_t *data, uint16_t len, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define loop */ - uint16_t index = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (data != NULL)) - { - /* Variable to define temporary length */ - uint16_t temp_len = len + dev->dummy_byte; - - /* Variable to define temporary buffer */ - uint8_t temp_buf[temp_len]; - - /* Configuring reg_addr for SPI Interface */ - if (dev->intf == BMI2_SPI_INTF) - { - reg_addr = (reg_addr | BMI2_SPI_RD_MASK); - } - - dev->intf_rslt = dev->read(reg_addr, temp_buf, temp_len, dev->intf_ptr); - - if (dev->aps_status == BMI2_ENABLE) - { - dev->delay_us(450, dev->intf_ptr); - } - else - { - dev->delay_us(2, dev->intf_ptr); - } - - if (dev->intf_rslt == BMI2_INTF_RET_SUCCESS) - { - /* Read the data from the position next to dummy byte */ - while (index < len) - { - data[index] = temp_buf[index + dev->dummy_byte]; - index++; - } - } - else - { - rslt = BMI2_E_COM_FAIL; - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API writes data to the given register address of bmi2 sensor. - */ -int8_t bmi2_set_regs(uint8_t reg_addr, const uint8_t *data, uint16_t len, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (data != NULL)) - { - /* Configuring reg_addr for SPI Interface */ - if (dev->intf == BMI2_SPI_INTF) - { - reg_addr = (reg_addr & BMI2_SPI_WR_MASK); - } - - dev->intf_rslt = dev->write(reg_addr, data, len, dev->intf_ptr); - - /* Delay for Low power mode of the sensor is 450 us */ - if (dev->aps_status == BMI2_ENABLE) - { - dev->delay_us(450, dev->intf_ptr); - } - /* Delay for Normal mode of the sensor is 2 us */ - else - { - dev->delay_us(2, dev->intf_ptr); - } - - /* updating the advance power saver flag */ - if (reg_addr == BMI2_PWR_CONF_ADDR) - { - if (*data & BMI2_ADV_POW_EN_MASK) - { - dev->aps_status = BMI2_ENABLE; - } - else - { - dev->aps_status = BMI2_DISABLE; - } - } - - if (dev->intf_rslt != BMI2_INTF_RET_SUCCESS) - { - rslt = BMI2_E_COM_FAIL; - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API resets bmi2 sensor. All registers are overwritten with - * their default values. - */ -int8_t bmi2_soft_reset(struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define soft reset value */ - uint8_t data = BMI2_SOFT_RESET_CMD; - - /* Variable to read the dummy byte */ - uint8_t dummy_read = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if (rslt == BMI2_OK) - { - /* Reset bmi2 device */ - rslt = bmi2_set_regs(BMI2_CMD_REG_ADDR, &data, 1, dev); - dev->delay_us(2000, dev->intf_ptr); - - /* set APS flag as after soft reset the sensor is on advance power save mode */ - dev->aps_status = BMI2_ENABLE; - - /* Performing a dummy read to bring interface back to SPI from - * I2C after a soft-reset - */ - if ((rslt == BMI2_OK) && (dev->intf == BMI2_SPI_INTF)) - { - rslt = bmi2_get_regs(BMI2_CHIP_ID_ADDR, &dummy_read, 1, dev); - } - - if (rslt == BMI2_OK) - { - /* Write the configuration file */ - rslt = bmi2_write_config_file(dev); - } - - /* Reset the sensor status flag in the device structure */ - if (rslt == BMI2_OK) - { - dev->sens_en_stat = 0; - } - } - - return rslt; -} - -/*! - * @brief This API is used to get the config file major and minor version information. - */ -int8_t bmi2_get_config_file_version(uint8_t *config_major, uint8_t *config_minor, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* NULL pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (config_major != NULL) && (config_minor != NULL)) - { - /* Extract the config file identification from the dmr page and get the major and minor version */ - rslt = extract_config_file(config_major, config_minor, dev); - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API enables/disables the advance power save mode in the sensor. - */ -int8_t bmi2_set_adv_power_save(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to store data */ - uint8_t reg_data = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if (rslt == BMI2_OK) - { - rslt = bmi2_get_regs(BMI2_PWR_CONF_ADDR, ®_data, 1, dev); - if (rslt == BMI2_OK) - { - reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_ADV_POW_EN, enable); - rslt = bmi2_set_regs(BMI2_PWR_CONF_ADDR, ®_data, 1, dev); - - if (rslt != BMI2_OK) - { - /* Return error if enable/disable APS fails */ - rslt = BMI2_E_SET_APS_FAIL; - } - - if (rslt == BMI2_OK) - { - dev->aps_status = BMI2_GET_BIT_POS0(reg_data, BMI2_ADV_POW_EN); - } - } - } - - return rslt; -} - -/*! - * @brief This API gets the status of advance power save mode in the sensor. - */ -int8_t bmi2_get_adv_power_save(uint8_t *aps_status, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to store data */ - uint8_t reg_data = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (aps_status != NULL)) - { - rslt = bmi2_get_regs(BMI2_PWR_CONF_ADDR, ®_data, 1, dev); - if (rslt == BMI2_OK) - { - *aps_status = BMI2_GET_BIT_POS0(reg_data, BMI2_ADV_POW_EN); - dev->aps_status = *aps_status; - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API loads the configuration file into the bmi2 sensor. - */ -int8_t bmi2_write_config_file(struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to know the load status */ - uint8_t load_status = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (dev->config_size != 0)) - { - /* Bytes written are multiples of 2 */ - if ((dev->read_write_len % 2) != 0) - { - dev->read_write_len = dev->read_write_len - 1; - } - - if (dev->read_write_len < 2) - { - dev->read_write_len = 2; - } - - /* Write the configuration file */ - rslt = write_config_file(dev); - if (rslt == BMI2_OK) - { - /* Check the configuration load status */ - rslt = bmi2_get_internal_status(&load_status, dev); - - /* Return error if loading not successful */ - if ((rslt == BMI2_OK) && (!(load_status & BMI2_CONFIG_LOAD_SUCCESS))) - { - rslt = BMI2_E_CONFIG_LOAD; - } - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API sets: - * 1) The input output configuration of the selected interrupt pin: - * INT1 or INT2. - * 2) The interrupt mode: permanently latched or non-latched. - */ -int8_t bmi2_set_int_pin_config(const struct bmi2_int_pin_config *int_cfg, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define data array */ - uint8_t data_array[3] = { 0 }; - - /* Variable to store register data */ - uint8_t reg_data = 0; - - /* Variable to define type of interrupt pin */ - uint8_t int_pin = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (int_cfg != NULL)) - { - /* Copy the pin type to a local variable */ - int_pin = int_cfg->pin_type; - if ((int_pin > BMI2_INT_NONE) && (int_pin < BMI2_INT_PIN_MAX)) - { - /* Get the previous configuration data */ - rslt = bmi2_get_regs(BMI2_INT1_IO_CTRL_ADDR, data_array, 3, dev); - if (rslt == BMI2_OK) - { - /* Set interrupt pin 1 configuration */ - if ((int_pin == BMI2_INT1) || (int_pin == BMI2_INT_BOTH)) - { - /* Configure active low or high */ - reg_data = BMI2_SET_BITS(data_array[0], BMI2_INT_LEVEL, int_cfg->pin_cfg[0].lvl); - - /* Configure push-pull or open drain */ - reg_data = BMI2_SET_BITS(reg_data, BMI2_INT_OPEN_DRAIN, int_cfg->pin_cfg[0].od); - - /* Configure output enable */ - reg_data = BMI2_SET_BITS(reg_data, BMI2_INT_OUTPUT_EN, int_cfg->pin_cfg[0].output_en); - - /* Configure input enable */ - reg_data = BMI2_SET_BITS(reg_data, BMI2_INT_INPUT_EN, int_cfg->pin_cfg[0].input_en); - - /* Copy the data to be written in the respective array */ - data_array[0] = reg_data; - } - - /* Set interrupt pin 2 configuration */ - if ((int_pin == BMI2_INT2) || (int_pin == BMI2_INT_BOTH)) - { - /* Configure active low or high */ - reg_data = BMI2_SET_BITS(data_array[1], BMI2_INT_LEVEL, int_cfg->pin_cfg[1].lvl); - - /* Configure push-pull or open drain */ - reg_data = BMI2_SET_BITS(reg_data, BMI2_INT_OPEN_DRAIN, int_cfg->pin_cfg[1].od); - - /* Configure output enable */ - reg_data = BMI2_SET_BITS(reg_data, BMI2_INT_OUTPUT_EN, int_cfg->pin_cfg[1].output_en); - - /* Configure input enable */ - reg_data = BMI2_SET_BITS(reg_data, BMI2_INT_INPUT_EN, int_cfg->pin_cfg[1].input_en); - - /* Copy the data to be written in the respective array */ - data_array[1] = reg_data; - } - - /* Configure the interrupt mode */ - data_array[2] = BMI2_SET_BIT_POS0(data_array[2], BMI2_INT_LATCH, int_cfg->int_latch); - - /* Set the configurations simultaneously as - * INT1_IO_CTRL, INT2_IO_CTRL, and INT_LATCH lie - * in consecutive addresses - */ - rslt = bmi2_set_regs(BMI2_INT1_IO_CTRL_ADDR, data_array, 3, dev); - } - } - else - { - rslt = BMI2_E_INVALID_INT_PIN; - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API gets: - * 1) The input output configuration of the selected interrupt pin: - * INT1 or INT2. - * 2) The interrupt mode: permanently latched or non-latched. - */ -int8_t bmi2_get_int_pin_config(struct bmi2_int_pin_config *int_cfg, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define data array */ - uint8_t data_array[3] = { 0 }; - - /* Variable to define type of interrupt pin */ - uint8_t int_pin = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (int_cfg != NULL)) - { - /* Copy the pin type to a local variable */ - int_pin = int_cfg->pin_type; - - /* Get the previous configuration data */ - rslt = bmi2_get_regs(BMI2_INT1_IO_CTRL_ADDR, data_array, 3, dev); - if (rslt == BMI2_OK) - { - /* Get interrupt pin 1 configuration */ - if ((int_pin == BMI2_INT1) || (int_pin == BMI2_INT_BOTH)) - { - /* Get active low or high */ - int_cfg->pin_cfg[0].lvl = BMI2_GET_BITS(data_array[0], BMI2_INT_LEVEL); - - /* Get push-pull or open drain */ - int_cfg->pin_cfg[0].od = BMI2_GET_BITS(data_array[0], BMI2_INT_OPEN_DRAIN); - - /* Get output enable */ - int_cfg->pin_cfg[0].output_en = BMI2_GET_BITS(data_array[0], BMI2_INT_OUTPUT_EN); - - /* Get input enable */ - int_cfg->pin_cfg[0].input_en = BMI2_GET_BITS(data_array[0], BMI2_INT_INPUT_EN); - } - - /* Get interrupt pin 2 configuration */ - if ((int_pin == BMI2_INT2) || (int_pin == BMI2_INT_BOTH)) - { - /* Get active low or high */ - int_cfg->pin_cfg[1].lvl = BMI2_GET_BITS(data_array[1], BMI2_INT_LEVEL); - - /* Get push-pull or open drain */ - int_cfg->pin_cfg[1].od = BMI2_GET_BITS(data_array[1], BMI2_INT_OPEN_DRAIN); - - /* Get output enable */ - int_cfg->pin_cfg[1].output_en = BMI2_GET_BITS(data_array[1], BMI2_INT_OUTPUT_EN); - - /* Get input enable */ - int_cfg->pin_cfg[1].input_en = BMI2_GET_BITS(data_array[1], BMI2_INT_INPUT_EN); - } - - /* Get interrupt mode */ - int_cfg->int_latch = BMI2_GET_BIT_POS0(data_array[2], BMI2_INT_LATCH); - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API gets the interrupt status of both feature and data - * interrupts - */ -int8_t bmi2_get_int_status(uint16_t *int_status, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to store data */ - uint8_t data_array[2] = { 0 }; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (int_status != NULL)) - { - /* Get the interrupt status */ - rslt = bmi2_get_regs(BMI2_INT_STATUS_0_ADDR, data_array, 2, dev); - if (rslt == BMI2_OK) - { - *int_status = (uint16_t) data_array[0] | ((uint16_t) data_array[1] << 8); - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API selects the sensors/features to be enabled. - */ -int8_t bmi2_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to select sensor */ - uint64_t sensor_sel = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (sens_list != NULL)) - { - /* Get the selected sensors */ - rslt = select_sensor(sens_list, n_sens, &sensor_sel); - if (rslt == BMI2_OK) - { - /* Enable the selected sensors */ - rslt = sensor_enable(sensor_sel, dev); - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API selects the sensors/features to be disabled. - */ -int8_t bmi2_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to select sensor */ - uint64_t sensor_sel = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (sens_list != NULL)) - { - /* Get the selected sensors */ - rslt = select_sensor(sens_list, n_sens, &sensor_sel); - if (rslt == BMI2_OK) - { - /* Disable the selected sensors */ - rslt = sensor_disable(sensor_sel, dev); - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API sets the sensor/feature configuration. - */ -int8_t bmi2_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define loop */ - uint8_t loop; - - /* Variable to get the status of advance power save */ - uint8_t aps_stat = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (sens_cfg != NULL)) - { - /* Get status of advance power save mode */ - aps_stat = dev->aps_status; - - for (loop = 0; loop < n_sens; loop++) - { - /* Disable Advance power save if enabled for auxiliary - * and feature configurations - */ - if (aps_stat == BMI2_ENABLE) - { - /* Disable advance power save if - * enabled - */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - } - - if (rslt == BMI2_OK) - { - switch (sens_cfg[loop].type) - { - /* Set accelerometer configuration */ - case BMI2_ACCEL: - rslt = set_accel_config(&sens_cfg[loop].cfg.acc, dev); - break; - - /* Set gyroscope configuration */ - case BMI2_GYRO: - rslt = set_gyro_config(&sens_cfg[loop].cfg.gyr, dev); - break; - - /* Set auxiliary configuration */ - case BMI2_AUX: - rslt = set_aux_config(&sens_cfg[loop].cfg.aux, dev); - break; - - /* Set gyroscope user gain configuration */ - case BMI2_GYRO_GAIN_UPDATE: - rslt = set_gyro_user_gain_config(&sens_cfg[loop].cfg.gyro_gain_update, dev); - break; - - default: - rslt = BMI2_E_INVALID_SENSOR; - break; - } - } - - /* Return error if any of the set configurations fail */ - if (rslt != BMI2_OK) - { - break; - } - } - - /* Enable Advance power save if disabled while configuring and - * not when already disabled - */ - if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) - { - rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API gets the sensor/feature configuration. - */ -int8_t bmi2_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define loop */ - uint8_t loop; - - /* Variable to get the status of advance power save */ - uint8_t aps_stat = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (sens_cfg != NULL)) - { - /* Get status of advance power save mode */ - aps_stat = dev->aps_status; - for (loop = 0; loop < n_sens; loop++) - { - /* Disable Advance power save if enabled for auxiliary - * and feature configurations - */ - if ((sens_cfg[loop].type >= BMI2_MAIN_SENS_MAX_NUM) || (sens_cfg[loop].type == BMI2_AUX)) - { - - if (aps_stat == BMI2_ENABLE) - { - /* Disable advance power save if - * enabled - */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - } - } - - if (rslt == BMI2_OK) - { - switch (sens_cfg[loop].type) - { - /* Get accelerometer configuration */ - case BMI2_ACCEL: - rslt = get_accel_config(&sens_cfg[loop].cfg.acc, dev); - break; - - /* Get gyroscope configuration */ - case BMI2_GYRO: - rslt = get_gyro_config(&sens_cfg[loop].cfg.gyr, dev); - break; - - /* Get auxiliary configuration */ - case BMI2_AUX: - rslt = get_aux_config(&sens_cfg[loop].cfg.aux, dev); - break; - - /* Get gyroscope user gain configuration */ - case BMI2_GYRO_GAIN_UPDATE: - rslt = get_gyro_gain_update_config(&sens_cfg[loop].cfg.gyro_gain_update, dev); - break; - - default: - rslt = BMI2_E_INVALID_SENSOR; - break; - } - } - - /* Return error if any of the get configurations fail */ - if (rslt != BMI2_OK) - { - break; - } - } - - /* Enable Advance power save if disabled while configuring and - * not when already disabled - */ - if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) - { - rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API gets the sensor/feature data for accelerometer, gyroscope, - * auxiliary sensor, step counter, high-g, gyroscope user-gain update, - * orientation, gyroscope cross sensitivity and error status for NVM and VFRM. - */ -int8_t bmi2_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define loop */ - uint8_t loop; - - /* Variable to get the status of advance power save */ - uint8_t aps_stat = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (sensor_data != NULL)) - { - /* Get status of advance power save mode */ - aps_stat = dev->aps_status; - for (loop = 0; loop < n_sens; loop++) - { - /* Disable Advance power save if enabled for feature - * configurations - */ - if (sensor_data[loop].type >= BMI2_MAIN_SENS_MAX_NUM) - { - if (aps_stat == BMI2_ENABLE) - { - /* Disable advance power save if - * enabled - */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - } - } - - if (rslt == BMI2_OK) - { - switch (sensor_data[loop].type) - { - case BMI2_ACCEL: - - /* Get accelerometer data */ - rslt = get_accel_sensor_data(&sensor_data[loop].sens_data.acc, BMI2_ACC_X_LSB_ADDR, dev); - break; - case BMI2_GYRO: - - /* Get gyroscope data */ - rslt = get_gyro_sensor_data(&sensor_data[loop].sens_data.gyr, BMI2_GYR_X_LSB_ADDR, dev); - break; - case BMI2_AUX: - - /* Get auxiliary sensor data in data mode */ - rslt = read_aux_data_mode(sensor_data[loop].sens_data.aux_data, dev); - break; - - case BMI2_GYRO_CROSS_SENSE: - - /* Get Gyroscope cross sense value of z axis */ - rslt = get_gyro_cross_sense(&sensor_data[loop].sens_data.correction_factor_zx, dev); - break; - - case BMI2_GYRO_GAIN_UPDATE: - - /* Get saturation status of gyroscope user gain update */ - rslt = get_gyro_gain_update_status(&sensor_data[loop].sens_data.gyro_user_gain_status, dev); - break; - default: - rslt = BMI2_E_INVALID_SENSOR; - break; - } - - /* Return error if any of the get sensor data fails */ - if (rslt != BMI2_OK) - { - break; - } - } - - /* Enable Advance power save if disabled while - * configuring and not when already disabled - */ - if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) - { - rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - } - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API sets the FIFO configuration in the sensor. - */ -int8_t bmi2_set_fifo_config(uint16_t config, uint8_t enable, struct bmi2_dev *dev) -{ - int8_t rslt; - uint8_t data[2] = { 0 }; - uint8_t max_burst_len = 0; - - /* Variable to store data of FIFO configuration register 0 */ - uint8_t fifo_config_0 = (uint8_t)(config & BMI2_FIFO_CONFIG_0_MASK); - - /* Variable to store data of FIFO configuration register 1 */ - uint8_t fifo_config_1 = (uint8_t)((config & BMI2_FIFO_CONFIG_1_MASK) >> 8); - - rslt = null_ptr_check(dev); - if (rslt == BMI2_OK) - { - rslt = bmi2_get_regs(BMI2_FIFO_CONFIG_0_ADDR, data, BMI2_FIFO_CONFIG_LENGTH, dev); - if (rslt == BMI2_OK) - { - /* Get data to set FIFO configuration register 0 */ - if (fifo_config_0 > 0) - { - if (enable == BMI2_ENABLE) - { - data[0] = data[0] | fifo_config_0; - } - else - { - data[0] = data[0] & (~fifo_config_0); - } - } - - /* Get data to set FIFO configuration register 1 */ - if (enable == BMI2_ENABLE) - { - data[1] = data[1] | fifo_config_1; - if (dev->variant_feature & BMI2_CRT_RTOSK_ENABLE) - { - - /* Burst length is needed for CRT - * FIFO enable will reset the default values - * So configure the max burst length again. - */ - rslt = get_maxburst_len(&max_burst_len, dev); - if (rslt == BMI2_OK && max_burst_len == 0) - { - rslt = set_maxburst_len(BMI2_CRT_MIN_BURST_WORD_LENGTH, dev); - } - } - } - else - { - data[1] = data[1] & (~fifo_config_1); - } - - /* Set the FIFO configurations */ - if (rslt == BMI2_OK) - { - rslt = bmi2_set_regs(BMI2_FIFO_CONFIG_0_ADDR, data, 2, dev); - } - } - } - - return rslt; -} - -/*! - * @brief This API reads the FIFO configuration from the sensor. - */ -int8_t bmi2_get_fifo_config(uint16_t *fifo_config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to store data */ - uint8_t data[2] = { 0 }; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (fifo_config != NULL)) - { - /* Get the FIFO configuration value */ - rslt = bmi2_get_regs(BMI2_FIFO_CONFIG_0_ADDR, data, BMI2_FIFO_CONFIG_LENGTH, dev); - if (rslt == BMI2_OK) - { - (*fifo_config) = (uint16_t)((uint16_t) data[0] & BMI2_FIFO_CONFIG_0_MASK); - (*fifo_config) |= (uint16_t)(((uint16_t) data[1] << 8) & BMI2_FIFO_CONFIG_1_MASK); - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API reads the FIFO data. - */ -int8_t bmi2_read_fifo_data(struct bmi2_fifo_frame *fifo, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to store FIFO configuration data */ - uint8_t config_data[2] = { 0 }; - - /* Variable to define FIFO address */ - uint8_t addr = BMI2_FIFO_DATA_ADDR; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (fifo != NULL)) - { - /* Clear the FIFO data structure */ - reset_fifo_frame_structure(fifo, dev); - - /* Read FIFO data */ - rslt = bmi2_get_regs(addr, fifo->data, fifo->length, dev); - - if (rslt == BMI2_OK) - { - - /* Get the set FIFO frame configurations */ - rslt = bmi2_get_regs(BMI2_FIFO_CONFIG_0_ADDR, config_data, 2, dev); - if (rslt == BMI2_OK) - { - /* Get FIFO header status */ - fifo->header_enable = (uint8_t)((config_data[1]) & (BMI2_FIFO_HEADER_EN >> 8)); - - /* Get sensor enable status, of which the data is to be read */ - fifo->data_enable = - (uint16_t)(((config_data[0]) | ((uint16_t) config_data[1] << 8)) & BMI2_FIFO_ALL_EN); - } - } - else - { - rslt = BMI2_E_COM_FAIL; - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API parses and extracts the accelerometer frames from FIFO data - * read by the "bmi2_read_fifo_data" API and stores it in the "accel_data" - * structure instance. - */ -int8_t bmi2_extract_accel(struct bmi2_sens_axes_data *accel_data, - uint16_t *accel_length, - struct bmi2_fifo_frame *fifo, - const struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to index the bytes */ - uint16_t data_index = 0; - - /* Variable to index accelerometer frames */ - uint16_t accel_index = 0; - - /* Variable to store the number of bytes to be read */ - uint16_t data_read_length = 0; - - /* Variable to define the data enable byte */ - uint8_t data_enable = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (accel_data != NULL) && (accel_length != NULL) && (fifo != NULL)) - { - /* Parsing the FIFO data in header-less mode */ - if (fifo->header_enable == 0) - { - - /* Get the number of accelerometer bytes to be read */ - rslt = parse_fifo_accel_len(&data_index, &data_read_length, accel_length, fifo); - - /* Convert word to byte since all sensor enables are in a byte */ - data_enable = (uint8_t)(fifo->data_enable >> 8); - for (; (data_index < data_read_length) && (rslt != BMI2_W_FIFO_EMPTY);) - { - /* Unpack frame to get the accelerometer data */ - rslt = unpack_accel_frame(accel_data, &data_index, &accel_index, data_enable, fifo, dev); - - if (rslt != BMI2_W_FIFO_EMPTY) - { - /* Check for the availability of next two bytes of FIFO data */ - rslt = check_empty_fifo(&data_index, fifo); - } - } - - /* Update number of accelerometer frames to be read */ - (*accel_length) = accel_index; - - /* Update the accelerometer byte index */ - fifo->acc_byte_start_idx = data_index; - } - else - { - /* Parsing the FIFO data in header mode */ - rslt = extract_accel_header_mode(accel_data, accel_length, fifo, dev); - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API parses and extracts the gyroscope frames from FIFO data - * read by the "bmi2_read_fifo_data" API and stores it in the "gyro_data" - * structure instance. - */ -int8_t bmi2_extract_gyro(struct bmi2_sens_axes_data *gyro_data, - uint16_t *gyro_length, - struct bmi2_fifo_frame *fifo, - const struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to index the bytes */ - uint16_t data_index = 0; - - /* Variable to index gyroscope frames */ - uint16_t gyro_index = 0; - - /* Variable to store the number of bytes to be read */ - uint16_t data_read_length = 0; - - /* Variable to define the data enable byte */ - uint8_t data_enable = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (gyro_data != NULL) && (gyro_length != NULL) && (fifo != NULL)) - { - /* Parsing the FIFO data in header-less mode */ - if (fifo->header_enable == 0) - { - /* Get the number of gyro bytes to be read */ - rslt = parse_fifo_gyro_len(&data_index, &data_read_length, gyro_length, fifo); - - /* Convert word to byte since all sensor enables are in a byte */ - data_enable = (uint8_t)(fifo->data_enable >> 8); - for (; (data_index < data_read_length) && (rslt != BMI2_W_FIFO_EMPTY);) - { - /* Unpack frame to get gyroscope data */ - rslt = unpack_gyro_frame(gyro_data, &data_index, &gyro_index, data_enable, fifo, dev); - if (rslt != BMI2_W_FIFO_EMPTY) - { - /* Check for the availability of next two bytes of FIFO data */ - rslt = check_empty_fifo(&data_index, fifo); - } - } - - /* Update number of gyroscope frames to be read */ - (*gyro_length) = gyro_index; - - /* Update the gyroscope byte index */ - fifo->acc_byte_start_idx = data_index; - } - else - { - /* Parsing the FIFO data in header mode */ - rslt = extract_gyro_header_mode(gyro_data, gyro_length, fifo, dev); - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API parses and extracts the auxiliary frames from FIFO data - * read by the "bmi2_read_fifo_data" API and stores it in "aux_data" buffer. - */ -int8_t bmi2_extract_aux(struct bmi2_aux_fifo_data *aux, - uint16_t *aux_length, - struct bmi2_fifo_frame *fifo, - const struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to index the bytes */ - uint16_t data_index = 0; - - /* Variable to index auxiliary frames */ - uint16_t aux_index = 0; - - /* Variable to store the number of bytes to be read */ - uint16_t data_read_length = 0; - - /* Variable to define the data enable byte */ - uint8_t data_enable = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (aux != NULL) && (aux_length != NULL) && (fifo != NULL)) - { - /* Parsing the FIFO data in header-less mode */ - if (fifo->header_enable == 0) - { - rslt = parse_fifo_aux_len(&data_index, &data_read_length, aux_length, fifo); - - /* Convert word to byte since all sensor enables are in - * a byte - */ - data_enable = (uint8_t)(fifo->data_enable >> 8); - for (; (data_index < data_read_length) && (rslt != BMI2_W_FIFO_EMPTY);) - { - /* Unpack frame to get auxiliary data */ - rslt = unpack_aux_frame(aux, &data_index, &aux_index, data_enable, fifo, dev); - if (rslt != BMI2_W_FIFO_EMPTY) - { - /* Check for the availability of next - * two bytes of FIFO data - */ - rslt = check_empty_fifo(&data_index, fifo); - } - } - - /* Update number of auxiliary frames to be read */ - *aux_length = aux_index; - - /* Update the auxiliary byte index */ - fifo->aux_byte_start_idx = data_index; - } - else - { - /* Parsing the FIFO data in header mode */ - rslt = extract_aux_header_mode(aux, aux_length, fifo, dev); - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API writes the available sensor specific commands to the sensor. - */ -int8_t bmi2_set_command_register(uint8_t command, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if (rslt == BMI2_OK) - { - /* Set the command in the command register */ - rslt = bmi2_set_regs(BMI2_CMD_REG_ADDR, &command, 1, dev); - } - - return rslt; -} - -/* - * @brief This API sets the FIFO self wake up functionality in the sensor. - */ -int8_t bmi2_set_fifo_self_wake_up(uint8_t fifo_self_wake_up, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to store data */ - uint8_t data = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if (rslt == BMI2_OK) - { - /* Set FIFO self wake-up */ - rslt = bmi2_get_regs(BMI2_PWR_CONF_ADDR, &data, 1, dev); - if (rslt == BMI2_OK) - { - data = BMI2_SET_BITS(data, BMI2_FIFO_SELF_WAKE_UP, fifo_self_wake_up); - rslt = bmi2_set_regs(BMI2_PWR_CONF_ADDR, &data, 1, dev); - } - } - - return rslt; -} - -/*! - * @brief This API gets the status of FIFO self wake up functionality from - * the sensor. - */ -int8_t bmi2_get_fifo_self_wake_up(uint8_t *fifo_self_wake_up, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to store data */ - uint8_t data = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (fifo_self_wake_up != NULL)) - { - /* Get the status of FIFO self wake-up */ - rslt = bmi2_get_regs(BMI2_PWR_CONF_ADDR, &data, 1, dev); - if (rslt == BMI2_OK) - { - (*fifo_self_wake_up) = BMI2_GET_BITS(data, BMI2_FIFO_SELF_WAKE_UP); - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API sets the FIFO water-mark level in the sensor. - */ -int8_t bmi2_set_fifo_wm(uint16_t fifo_wm, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to store data */ - uint8_t data[2] = { 0 }; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if (rslt == BMI2_OK) - { - /* Get LSB value of FIFO water-mark */ - data[0] = BMI2_GET_LSB(fifo_wm); - - /* Get MSB value of FIFO water-mark */ - data[1] = BMI2_GET_MSB(fifo_wm); - - /* Set the FIFO water-mark level */ - rslt = bmi2_set_regs(BMI2_FIFO_WTM_0_ADDR, data, 2, dev); - } - - return rslt; -} - -/*! - * @brief This API reads the FIFO water mark level set in the sensor. - */ -int8_t bmi2_get_fifo_wm(uint16_t *fifo_wm, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to to store data */ - uint8_t data[2] = { 0 }; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (fifo_wm != NULL)) - { - /* Read the FIFO water mark level */ - rslt = bmi2_get_regs(BMI2_FIFO_WTM_0_ADDR, data, BMI2_FIFO_WM_LENGTH, dev); - if (rslt == BMI2_OK) - { - (*fifo_wm) = (uint16_t)((uint16_t) data[1] << 8) | (data[0]); - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API sets either filtered or un-filtered FIFO accelerometer or - * gyroscope data. - */ -int8_t bmi2_set_fifo_filter_data(uint8_t sens_sel, uint8_t fifo_filter_data, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to store data */ - uint8_t data = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if (rslt == BMI2_OK) - { - switch (sens_sel) - { - case BMI2_ACCEL: - - /* Validate filter mode */ - if (fifo_filter_data <= BMI2_MAX_VALUE_FIFO_FILTER) - { - /* Set the accelerometer FIFO filter data */ - rslt = bmi2_get_regs(BMI2_FIFO_DOWNS_ADDR, &data, 1, dev); - if (rslt == BMI2_OK) - { - data = BMI2_SET_BITS(data, BMI2_ACC_FIFO_FILT_DATA, fifo_filter_data); - rslt = bmi2_set_regs(BMI2_FIFO_DOWNS_ADDR, &data, 1, dev); - } - } - else - { - rslt = BMI2_E_OUT_OF_RANGE; - } - - break; - case BMI2_GYRO: - - /* Validate filter mode */ - if (fifo_filter_data <= BMI2_MAX_VALUE_FIFO_FILTER) - { - /* Set the gyroscope FIFO filter data */ - rslt = bmi2_get_regs(BMI2_FIFO_DOWNS_ADDR, &data, 1, dev); - if (rslt == BMI2_OK) - { - data = BMI2_SET_BITS(data, BMI2_GYR_FIFO_FILT_DATA, fifo_filter_data); - rslt = bmi2_set_regs(BMI2_FIFO_DOWNS_ADDR, &data, 1, dev); - } - } - else - { - rslt = BMI2_E_OUT_OF_RANGE; - } - - break; - default: - rslt = BMI2_E_INVALID_SENSOR; - break; - } - } - - return rslt; -} - -/*! - * @brief This API gets the FIFO accelerometer or gyroscope filter data. - */ -int8_t bmi2_get_fifo_filter_data(uint8_t sens_sel, uint8_t *fifo_filter_data, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to store FIFO filter mode */ - uint8_t data = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (fifo_filter_data != NULL)) - { - switch (sens_sel) - { - case BMI2_ACCEL: - - /* Read the accelerometer FIFO filter data */ - rslt = bmi2_get_regs(BMI2_FIFO_DOWNS_ADDR, &data, 1, dev); - if (rslt == BMI2_OK) - { - (*fifo_filter_data) = BMI2_GET_BITS(data, BMI2_ACC_FIFO_FILT_DATA); - } - - break; - case BMI2_GYRO: - - /* Read the gyroscope FIFO filter data */ - rslt = bmi2_get_regs(BMI2_FIFO_DOWNS_ADDR, &data, 1, dev); - if (rslt == BMI2_OK) - { - (*fifo_filter_data) = BMI2_GET_BITS(data, BMI2_GYR_FIFO_FILT_DATA); - } - - break; - default: - rslt = BMI2_E_INVALID_SENSOR; - break; - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API sets the down-sampling rates for accelerometer or gyroscope - * FIFO data. - */ -int8_t bmi2_set_fifo_down_sample(uint8_t sens_sel, uint8_t fifo_down_samp, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to store sampling rate */ - uint8_t data = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if (rslt == BMI2_OK) - { - switch (sens_sel) - { - case BMI2_ACCEL: - - /* Set the accelerometer FIFO down sampling rate */ - rslt = bmi2_get_regs(BMI2_FIFO_DOWNS_ADDR, &data, 1, dev); - if (rslt == BMI2_OK) - { - data = BMI2_SET_BITS(data, BMI2_ACC_FIFO_DOWNS, fifo_down_samp); - rslt = bmi2_set_regs(BMI2_FIFO_DOWNS_ADDR, &data, 1, dev); - } - - break; - case BMI2_GYRO: - - /* Set the gyroscope FIFO down sampling rate */ - rslt = bmi2_get_regs(BMI2_FIFO_DOWNS_ADDR, &data, 1, dev); - if (rslt == BMI2_OK) - { - data = BMI2_SET_BIT_POS0(data, BMI2_GYR_FIFO_DOWNS, fifo_down_samp); - rslt = bmi2_set_regs(BMI2_FIFO_DOWNS_ADDR, &data, 1, dev); - } - - break; - default: - rslt = BMI2_E_INVALID_SENSOR; - break; - } - } - - return rslt; -} - -/*! - * @brief This API reads the down sampling rates which is configured for - * accelerometer or gyroscope FIFO data. - */ -int8_t bmi2_get_fifo_down_sample(uint8_t sens_sel, uint8_t *fifo_down_samp, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to store sampling rate */ - uint8_t data = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (fifo_down_samp != NULL)) - { - switch (sens_sel) - { - case BMI2_ACCEL: - - /* Read the accelerometer FIFO down data sampling rate */ - rslt = bmi2_get_regs(BMI2_FIFO_DOWNS_ADDR, &data, 1, dev); - if (rslt == BMI2_OK) - { - (*fifo_down_samp) = BMI2_GET_BITS(data, BMI2_ACC_FIFO_DOWNS); - } - - break; - case BMI2_GYRO: - - /* Read the gyroscope FIFO down data sampling rate */ - rslt = bmi2_get_regs(BMI2_FIFO_DOWNS_ADDR, &data, 1, dev); - if (rslt == BMI2_OK) - { - (*fifo_down_samp) = BMI2_GET_BIT_POS0(data, BMI2_GYR_FIFO_DOWNS); - } - - break; - default: - rslt = BMI2_E_INVALID_SENSOR; - break; - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API gets the length of FIFO data available in the sensor in - * bytes. - */ -int8_t bmi2_get_fifo_length(uint16_t *fifo_length, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define byte index */ - uint8_t index = 0; - - /* Array to store FIFO data length */ - uint8_t data[BMI2_FIFO_DATA_LENGTH] = { 0 }; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (fifo_length != NULL)) - { - /* Read FIFO length */ - rslt = bmi2_get_regs(BMI2_FIFO_LENGTH_0_ADDR, data, BMI2_FIFO_DATA_LENGTH, dev); - if (rslt == BMI2_OK) - { - /* Get the MSB byte index */ - index = BMI2_FIFO_LENGTH_MSB_BYTE; - - /* Get the MSB byte of FIFO length */ - data[index] = BMI2_GET_BIT_POS0(data[index], BMI2_FIFO_BYTE_COUNTER_MSB); - - /* Get total FIFO length */ - (*fifo_length) = ((data[index] << 8) | data[index - 1]); - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API reads the user-defined bytes of data from the given register - * address of auxiliary sensor in manual mode. - * - * @note Change of BMI2_AUX_RD_ADDR is only allowed if AUX is not busy. - */ -int8_t bmi2_read_aux_man_mode(uint8_t reg_addr, uint8_t *aux_data, uint16_t len, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to store burst length */ - uint8_t burst_len = 0; - - /* Variable to define APS status */ - uint8_t aps_stat = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (aux_data != NULL)) - { - /* Validate if manual mode */ - if (dev->aux_man_en) - { - /* Get status of advance power save mode */ - aps_stat = dev->aps_status; - if (aps_stat == BMI2_ENABLE) - { - /* Disable APS if enabled */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - } - - if (rslt == BMI2_OK) - { - /* Map the register value set to that of burst - * length - */ - rslt = map_read_len(&burst_len, dev); - if (rslt == BMI2_OK) - { - /* Read auxiliary data */ - rslt = read_aux_data(reg_addr, aux_data, len, burst_len, dev); - } - } - - /* Enable Advance power save if disabled for reading - * data and not when already disabled - */ - if ((rslt == BMI2_OK) && (aps_stat == BMI2_ENABLE)) - { - rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - } - } - else - { - rslt = BMI2_E_AUX_INVALID_CFG; - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API writes the user-defined bytes of data and the address of - * auxiliary sensor where data is to be written in manual mode. - * - * @note Change of BMI2_AUX_WR_ADDR is only allowed if AUX is not busy. - */ -int8_t bmi2_write_aux_man_mode(uint8_t reg_addr, const uint8_t *aux_data, uint16_t len, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define loop */ - uint8_t loop = 0; - - /* Variable to define APS status */ - uint8_t aps_stat = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (aux_data != NULL)) - { - /* Validate if manual mode */ - if (dev->aux_man_en) - { - /* Get status of advance power save mode */ - aps_stat = dev->aps_status; - if (aps_stat == BMI2_ENABLE) - { - /* Disable APS if enabled */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - } - - /* Byte write data in the corresponding address */ - if (rslt == BMI2_OK) - { - for (; ((loop < len) && (rslt == BMI2_OK)); loop++) - { - rslt = write_aux_data((reg_addr + loop), aux_data[loop], dev); - dev->delay_us(1000, dev->intf_ptr); - } - } - - /* Enable Advance power save if disabled for writing - * data and not when already disabled - */ - if ((rslt == BMI2_OK) && (aps_stat == BMI2_ENABLE)) - { - rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - } - } - else - { - rslt = BMI2_E_AUX_INVALID_CFG; - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API writes the user-defined bytes of data and the address of - * auxiliary sensor where data is to be written, from an interleaved input, - * in manual mode. - * - * @note Change of BMI2_AUX_WR_ADDR is only allowed if AUX is not busy. - */ -int8_t bmi2_write_aux_interleaved(uint8_t reg_addr, const uint8_t *aux_data, uint16_t len, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define loop */ - uint8_t loop = 1; - - /* Variable to define APS status */ - uint8_t aps_stat = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (aux_data != NULL)) - { - /* Validate if manual mode */ - if (dev->aux_man_en) - { - /* Get status of advance power save mode */ - aps_stat = dev->aps_status; - if (aps_stat == BMI2_ENABLE) - { - /* Disable APS if enabled */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - } - - if (rslt == BMI2_OK) - { - /* Write the start register address extracted - * from the interleaved data - */ - rslt = write_aux_data(reg_addr, aux_data[0], dev); - - /* Extract the remaining address and data from - * the interleaved data and write it in the - * corresponding addresses byte by byte - */ - for (; ((loop < len) && (rslt == BMI2_OK)); loop += 2) - { - rslt = write_aux_data(aux_data[loop], aux_data[loop + 1], dev); - dev->delay_us(1000, dev->intf_ptr); - } - - /* Enable Advance power save if disabled for - * writing data and not when already disabled - */ - if ((rslt == BMI2_OK) && (aps_stat == BMI2_ENABLE)) - { - rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - } - } - } - else - { - rslt = BMI2_E_AUX_INVALID_CFG; - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API gets the data ready status of accelerometer, gyroscope, - * auxiliary, command decoder and busy status of auxiliary. - */ -int8_t bmi2_get_status(uint8_t *status, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (status != NULL)) - { - rslt = bmi2_get_regs(BMI2_STATUS_ADDR, status, 1, dev); - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API enables/disables OIS interface. - */ -int8_t bmi2_set_ois_interface(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to store data */ - uint8_t reg_data = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if (rslt == BMI2_OK) - { - rslt = bmi2_get_regs(BMI2_IF_CONF_ADDR, ®_data, 1, dev); - if (rslt == BMI2_OK) - { - /* Enable/Disable OIS interface */ - reg_data = BMI2_SET_BITS(reg_data, BMI2_OIS_IF_EN, enable); - if (enable) - { - /* Disable auxiliary interface if OIS is enabled */ - reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_AUX_IF_EN); - } - - /* Set the OIS interface configurations */ - rslt = bmi2_set_regs(BMI2_IF_CONF_ADDR, ®_data, 1, dev); - } - } - - return rslt; -} - -/*! - * @brief This API can be used to write sync commands like ODR, sync period, - * frequency and phase, resolution ratio, sync time and delay time. - */ -int8_t bmi2_write_sync_commands(const uint8_t *command, uint8_t n_comm, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (command != NULL)) - { - rslt = bmi2_set_regs(BMI2_SYNC_COMMAND_ADDR, command, n_comm, dev); - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API performs self-test to check the proper functionality of the - * accelerometer sensor. - */ -int8_t bmi2_perform_accel_self_test(struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to store self-test result */ - int8_t st_rslt = 0; - - /* Structure to define positive accelerometer axes */ - struct bmi2_sens_axes_data positive = { 0, 0, 0, 0 }; - - /* Structure to define negative accelerometer axes */ - struct bmi2_sens_axes_data negative = { 0, 0, 0, 0 }; - - /* Structure for difference of accelerometer values in g */ - struct bmi2_selftest_delta_limit accel_data_diff = { 0, 0, 0 }; - - /* Structure for difference of accelerometer values in mg */ - struct bmi2_selftest_delta_limit accel_data_diff_mg = { 0, 0, 0 }; - - /* Initialize the polarity of self-test as positive */ - int8_t sign = BMI2_ENABLE; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if (rslt == BMI2_OK) - { - /* Sets the configuration required before enabling self-test */ - rslt = pre_self_test_config(dev); - - /* Wait for greater than 2 milliseconds */ - dev->delay_us(3000, dev->intf_ptr); - if (rslt == BMI2_OK) - { - do - { - /* Select positive first, then negative polarity - * after enabling self-test - */ - rslt = self_test_config((uint8_t) sign, dev); - if (rslt == BMI2_OK) - { - /* Wait for greater than 50 milli-sec */ - dev->delay_us(51000, dev->intf_ptr); - - /* If polarity is positive */ - if (sign == BMI2_ENABLE) - { - /* Read and store positive acceleration value */ - rslt = read_accel_xyz(&positive, dev); - } - /* If polarity is negative */ - else if (sign == BMI2_DISABLE) - { - /* Read and store negative acceleration value */ - rslt = read_accel_xyz(&negative, dev); - } - } - else - { - /* Break if error */ - break; - } - - /* Break if error */ - if (rslt != BMI2_OK) - { - break; - } - - /* Turn the polarity of self-test negative */ - sign--; - } while (sign >= 0); - if (rslt == BMI2_OK) - { - /* Subtract -ve acceleration values from that of +ve values */ - accel_data_diff.x = (positive.x) - (negative.x); - accel_data_diff.y = (positive.y) - (negative.y); - accel_data_diff.z = (positive.z) - (negative.z); - - /* Convert differences of acceleration values - * from 'g' to 'mg' - */ - convert_lsb_g(&accel_data_diff, &accel_data_diff_mg, dev); - - /* Validate self-test for acceleration values - * in mg and get the self-test result - */ - st_rslt = validate_self_test(&accel_data_diff_mg); - - /* Trigger a soft reset after performing self-test */ - rslt = bmi2_soft_reset(dev); - - /* Return the self-test result */ - if (rslt == BMI2_OK) - { - rslt = st_rslt; - } - } - } - } - - return rslt; -} - -/*! - * @brief This API maps/unmaps feature interrupts to that of interrupt pins. - */ -int8_t bmi2_map_feat_int(uint8_t type, enum bmi2_hw_int_pin hw_int_pin, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define the value of feature interrupts */ - uint8_t feat_int = 0; - - /* Array to store the interrupt mask bits */ - uint8_t data_array[2] = { 0 }; - - /* Structure to define map the interrupts */ - struct bmi2_map_int map_int = { 0 }; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if (rslt == BMI2_OK) - { - /* Read interrupt map1 and map2 and register */ - rslt = bmi2_get_regs(BMI2_INT1_MAP_FEAT_ADDR, data_array, 2, dev); - - if (rslt == BMI2_OK) - { - /* Get the value of the feature interrupt to be mapped */ - extract_feat_int_map(&map_int, type, dev); - - feat_int = map_int.sens_map_int; - - /* Map the interrupts */ - rslt = map_feat_int(data_array, hw_int_pin, feat_int); - - /* Map the interrupts to INT1 and INT2 map register */ - if (rslt == BMI2_OK) - { - rslt = bmi2_set_regs(BMI2_INT1_MAP_FEAT_ADDR, &data_array[0], 1, dev); - if (rslt == BMI2_OK) - { - rslt = bmi2_set_regs(BMI2_INT2_MAP_FEAT_ADDR, &data_array[1], 1, dev); - } - } - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API maps/un-maps data interrupts to that of interrupt pins. - */ -int8_t bmi2_map_data_int(uint8_t data_int, enum bmi2_hw_int_pin int_pin, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to mask interrupt pin 1 - lower nibble */ - uint8_t int1_mask = data_int; - - /* Variable to mask interrupt pin 2 - higher nibble */ - uint8_t int2_mask = (uint8_t)(data_int << 4); - - /* Variable to store register data */ - uint8_t reg_data = 0; - - /* Read interrupt map1 and map2 and register */ - rslt = bmi2_get_regs(BMI2_INT_MAP_DATA_ADDR, ®_data, 1, dev); - if (rslt == BMI2_OK) - { - if (int_pin < BMI2_INT_PIN_MAX) - { - switch (int_pin) - { - case BMI2_INT_NONE: - - /* Un-Map the corresponding data - * interrupt to both interrupt pin 1 and 2 - */ - reg_data &= ~(int1_mask | int2_mask); - break; - case BMI2_INT1: - - /* Map the corresponding data interrupt to - * interrupt pin 1 - */ - reg_data |= int1_mask; - break; - case BMI2_INT2: - - /* Map the corresponding data interrupt to - * interrupt pin 2 - */ - reg_data |= int2_mask; - break; - case BMI2_INT_BOTH: - - /* Map the corresponding data - * interrupt to both interrupt pin 1 and 2 - */ - reg_data |= (int1_mask | int2_mask); - break; - default: - break; - } - - /* Set the interrupts in the map register */ - rslt = bmi2_set_regs(BMI2_INT_MAP_DATA_ADDR, ®_data, 1, dev); - } - else - { - /* Return error if invalid pin selection */ - rslt = BMI2_E_INVALID_INT_PIN; - } - } - - return rslt; -} - -/*! - * @brief This API gets the re-mapped x, y and z axes from the sensor and - * updates the values in the device structure. - */ -int8_t bmi2_get_remap_axes(struct bmi2_remap *remapped_axis, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Initialize the local structure for axis re-mapping */ - struct bmi2_axes_remap remap = { 0, 0, 0, 0, 0, 0 }; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (remapped_axis != NULL)) - { - /* Get the re-mapped axes from the sensor */ - rslt = get_remap_axes(&remap, dev); - if (rslt == BMI2_OK) - { - /* Store the re-mapped x-axis value in device structure - * and its user-value in the interface structure - */ - switch (remap.x_axis) - { - case BMI2_MAP_X_AXIS: - - /* If mapped to x-axis */ - dev->remap.x_axis = BMI2_MAP_X_AXIS; - remapped_axis->x = BMI2_X; - break; - case BMI2_MAP_Y_AXIS: - - /* If mapped to y-axis */ - dev->remap.x_axis = BMI2_MAP_Y_AXIS; - remapped_axis->x = BMI2_Y; - break; - case BMI2_MAP_Z_AXIS: - - /* If mapped to z-axis */ - dev->remap.x_axis = BMI2_MAP_Z_AXIS; - remapped_axis->x = BMI2_Z; - break; - default: - break; - } - - /* Store the re-mapped x-axis sign in device structure - * and its user-value in the interface structure - */ - if (remap.x_axis_sign) - { - /* If x-axis is mapped to -ve sign */ - dev->remap.x_axis_sign = BMI2_NEG_SIGN; - remapped_axis->x |= BMI2_AXIS_SIGN; - } - else - { - dev->remap.x_axis_sign = BMI2_POS_SIGN; - } - - /* Store the re-mapped y-axis value in device structure - * and its user-value in the interface structure - */ - switch (remap.y_axis) - { - case BMI2_MAP_X_AXIS: - - /* If mapped to x-axis */ - dev->remap.y_axis = BMI2_MAP_X_AXIS; - remapped_axis->y = BMI2_X; - break; - case BMI2_MAP_Y_AXIS: - - /* If mapped to y-axis */ - dev->remap.y_axis = BMI2_MAP_Y_AXIS; - remapped_axis->y = BMI2_Y; - break; - case BMI2_MAP_Z_AXIS: - - /* If mapped to z-axis */ - dev->remap.y_axis = BMI2_MAP_Z_AXIS; - remapped_axis->y = BMI2_Z; - break; - default: - break; - } - - /* Store the re-mapped y-axis sign in device structure - * and its user-value in the interface structure - */ - if (remap.y_axis_sign) - { - /* If y-axis is mapped to -ve sign */ - dev->remap.y_axis_sign = BMI2_NEG_SIGN; - remapped_axis->y |= BMI2_AXIS_SIGN; - } - else - { - dev->remap.y_axis_sign = BMI2_POS_SIGN; - } - - /* Store the re-mapped z-axis value in device structure - * and its user-value in the interface structure - */ - switch (remap.z_axis) - { - case BMI2_MAP_X_AXIS: - - /* If mapped to x-axis */ - dev->remap.z_axis = BMI2_MAP_X_AXIS; - remapped_axis->z = BMI2_X; - break; - case BMI2_MAP_Y_AXIS: - - /* If mapped to y-axis */ - dev->remap.z_axis = BMI2_MAP_Y_AXIS; - remapped_axis->z = BMI2_Y; - break; - case BMI2_MAP_Z_AXIS: - - /* If mapped to z-axis */ - dev->remap.z_axis = BMI2_MAP_Z_AXIS; - remapped_axis->z = BMI2_Z; - break; - default: - break; - } - - /* Store the re-mapped z-axis sign in device structure - * and its user-value in the interface structure - */ - if (remap.z_axis_sign) - { - /* If z-axis is mapped to -ve sign */ - dev->remap.z_axis_sign = BMI2_NEG_SIGN; - remapped_axis->z |= BMI2_AXIS_SIGN; - } - else - { - dev->remap.z_axis_sign = BMI2_POS_SIGN; - } - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API sets the re-mapped x, y and z axes to the sensor and - * updates the them in the device structure. - */ -int8_t bmi2_set_remap_axes(const struct bmi2_remap *remapped_axis, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to store all the re-mapped axes */ - uint8_t remap_axes = 0; - - /* Variable to store the re-mapped x-axes */ - uint8_t remap_x = 0; - - /* Variable to store the re-mapped y-axes */ - uint8_t remap_y = 0; - - /* Variable to store the re-mapped z-axes */ - uint8_t remap_z = 0; - - /* Initialize the local structure for axis re-mapping */ - struct bmi2_axes_remap remap = { 0, 0, 0, 0, 0, 0 }; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (remapped_axis != NULL)) - { - /* Check whether all the axes are re-mapped */ - remap_axes = remapped_axis->x | remapped_axis->y | remapped_axis->z; - - /* If all the axes are re-mapped */ - if ((remap_axes & BMI2_AXIS_MASK) == BMI2_AXIS_MASK) - { - /* Get the re-mapped value of x, y and z axis */ - remap_x = remapped_axis->x & BMI2_AXIS_MASK; - remap_y = remapped_axis->y & BMI2_AXIS_MASK; - remap_z = remapped_axis->z & BMI2_AXIS_MASK; - - /* Store the value of re-mapped x-axis in both - * device structure and the local structure - */ - switch (remap_x) - { - case BMI2_X: - - /* If mapped to x-axis */ - dev->remap.x_axis = BMI2_MAP_X_AXIS; - remap.x_axis = BMI2_MAP_X_AXIS; - break; - case BMI2_Y: - - /* If mapped to y-axis */ - dev->remap.x_axis = BMI2_MAP_Y_AXIS; - remap.x_axis = BMI2_MAP_Y_AXIS; - break; - case BMI2_Z: - - /* If mapped to z-axis */ - dev->remap.x_axis = BMI2_MAP_Z_AXIS; - remap.x_axis = BMI2_MAP_Z_AXIS; - break; - default: - break; - } - - /* Store the re-mapped x-axis sign in the device - * structure and its value in local structure - */ - if (remapped_axis->x & BMI2_AXIS_SIGN) - { - /* If x-axis is mapped to -ve sign */ - dev->remap.x_axis_sign = BMI2_NEG_SIGN; - remap.x_axis_sign = BMI2_MAP_NEGATIVE; - } - else - { - dev->remap.x_axis_sign = BMI2_POS_SIGN; - remap.x_axis_sign = BMI2_MAP_POSITIVE; - } - - /* Store the value of re-mapped y-axis in both - * device structure and the local structure - */ - switch (remap_y) - { - case BMI2_X: - - /* If mapped to x-axis */ - dev->remap.y_axis = BMI2_MAP_X_AXIS; - remap.y_axis = BMI2_MAP_X_AXIS; - break; - case BMI2_Y: - - /* If mapped to y-axis */ - dev->remap.y_axis = BMI2_MAP_Y_AXIS; - remap.y_axis = BMI2_MAP_Y_AXIS; - break; - case BMI2_Z: - - /* If mapped to z-axis */ - dev->remap.y_axis = BMI2_MAP_Z_AXIS; - remap.y_axis = BMI2_MAP_Z_AXIS; - break; - default: - break; - } - - /* Store the re-mapped y-axis sign in the device - * structure and its value in local structure - */ - if (remapped_axis->y & BMI2_AXIS_SIGN) - { - /* If y-axis is mapped to -ve sign */ - dev->remap.y_axis_sign = BMI2_NEG_SIGN; - remap.y_axis_sign = BMI2_MAP_NEGATIVE; - } - else - { - dev->remap.y_axis_sign = BMI2_POS_SIGN; - remap.y_axis_sign = BMI2_MAP_POSITIVE; - } - - /* Store the value of re-mapped z-axis in both - * device structure and the local structure - */ - switch (remap_z) - { - case BMI2_X: - - /* If mapped to x-axis */ - dev->remap.z_axis = BMI2_MAP_X_AXIS; - remap.z_axis = BMI2_MAP_X_AXIS; - break; - case BMI2_Y: - - /* If mapped to y-axis */ - dev->remap.z_axis = BMI2_MAP_Y_AXIS; - remap.z_axis = BMI2_MAP_Y_AXIS; - break; - case BMI2_Z: - - /* If mapped to z-axis */ - dev->remap.z_axis = BMI2_MAP_Z_AXIS; - remap.z_axis = BMI2_MAP_Z_AXIS; - break; - default: - break; - } - - /* Store the re-mapped z-axis sign in the device - * structure and its value in local structure - */ - if (remapped_axis->z & BMI2_AXIS_SIGN) - { - /* If z-axis is mapped to -ve sign */ - dev->remap.z_axis_sign = BMI2_NEG_SIGN; - remap.z_axis_sign = BMI2_MAP_NEGATIVE; - } - else - { - dev->remap.z_axis_sign = BMI2_POS_SIGN; - remap.z_axis_sign = BMI2_MAP_POSITIVE; - } - - /* Set the re-mapped axes in the sensor */ - rslt = set_remap_axes(&remap, dev); - } - else - { - rslt = BMI2_E_REMAP_ERROR; - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API enables/disables gyroscope offset compensation. It adds the - * offsets defined in the offset register with gyroscope data. - */ -int8_t bmi2_set_gyro_offset_comp(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define register data */ - uint8_t reg_data = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if (rslt == BMI2_OK) - { - /* Get the status of gyroscope offset enable */ - rslt = bmi2_get_regs(BMI2_GYR_OFF_COMP_6_ADDR, ®_data, 1, dev); - if (rslt == BMI2_OK) - { - reg_data = BMI2_SET_BITS(reg_data, BMI2_GYR_OFF_COMP_EN, enable); - - /* Enable/Disable gyroscope offset compensation */ - rslt = bmi2_set_regs(BMI2_GYR_OFF_COMP_6_ADDR, ®_data, 1, dev); - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API reads the gyroscope bias values for each axis which is used - * for gyroscope offset compensation. - */ -int8_t bmi2_read_gyro_offset_comp_axes(struct bmi2_sens_axes_data *gyr_off_comp_axes, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define register data */ - uint8_t reg_data[4] = { 0 }; - - /* Variable to store LSB value of offset compensation for x-axis */ - uint8_t gyr_off_lsb_x; - - /* Variable to store LSB value of offset compensation for y-axis */ - uint8_t gyr_off_lsb_y; - - /* Variable to store LSB value of offset compensation for z-axis */ - uint8_t gyr_off_lsb_z; - - /* Variable to store MSB value of offset compensation for x-axis */ - uint8_t gyr_off_msb_x; - - /* Variable to store MSB value of offset compensation for y-axis */ - uint8_t gyr_off_msb_y; - - /* Variable to store MSB value of offset compensation for z-axis */ - uint8_t gyr_off_msb_z; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (gyr_off_comp_axes != NULL)) - { - /* Get the gyroscope compensated offset values */ - rslt = bmi2_get_regs(BMI2_GYR_OFF_COMP_3_ADDR, reg_data, 4, dev); - if (rslt == BMI2_OK) - { - /* Get LSB and MSB values of offset compensation for - * x, y and z axis - */ - gyr_off_lsb_x = reg_data[0]; - gyr_off_lsb_y = reg_data[1]; - gyr_off_lsb_z = reg_data[2]; - gyr_off_msb_x = reg_data[3] & BMI2_GYR_OFF_COMP_MSB_X_MASK; - gyr_off_msb_y = reg_data[3] & BMI2_GYR_OFF_COMP_MSB_Y_MASK; - gyr_off_msb_z = reg_data[3] & BMI2_GYR_OFF_COMP_MSB_Z_MASK; - - /* Gyroscope offset compensation value for x-axis */ - gyr_off_comp_axes->x = (int16_t)(((uint16_t) gyr_off_msb_x << 8) | gyr_off_lsb_x); - - /* Gyroscope offset compensation value for y-axis */ - gyr_off_comp_axes->y = (int16_t)(((uint16_t) gyr_off_msb_y << 6) | gyr_off_lsb_y); - - /* Gyroscope offset compensation value for z-axis */ - gyr_off_comp_axes->z = (int16_t)(((uint16_t) gyr_off_msb_z << 4) | gyr_off_lsb_z); - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API writes the gyroscope bias values for each axis which is used - * for gyroscope offset compensation. - */ -int8_t bmi2_write_gyro_offset_comp_axes(const struct bmi2_sens_axes_data *gyr_off_comp_axes, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define register data */ - uint8_t reg_data[4] = { 0 }; - - /* Variable to store MSB value of offset compensation for x-axis */ - uint8_t gyr_off_msb_x; - - /* Variable to store MSB value of offset compensation for y-axis */ - uint8_t gyr_off_msb_y; - - /* Variable to store MSB value of offset compensation for z-axis */ - uint8_t gyr_off_msb_z; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (gyr_off_comp_axes != NULL)) - { - /* Get the MSB values of gyroscope compensated offset values */ - rslt = bmi2_get_regs(BMI2_GYR_OFF_COMP_6_ADDR, ®_data[3], 1, dev); - if (rslt == BMI2_OK) - { - /* Get MSB value of x-axis from user-input */ - gyr_off_msb_x = (uint8_t)((gyr_off_comp_axes->x & BMI2_GYR_OFF_COMP_MSB_MASK) >> 8); - - /* Get MSB value of y-axis from user-input */ - gyr_off_msb_y = (uint8_t)((gyr_off_comp_axes->y & BMI2_GYR_OFF_COMP_MSB_MASK) >> 8); - - /* Get MSB value of z-axis from user-input */ - gyr_off_msb_z = (uint8_t)((gyr_off_comp_axes->z & BMI2_GYR_OFF_COMP_MSB_MASK) >> 8); - - /* Get LSB value of x-axis from user-input */ - reg_data[0] = (uint8_t)(gyr_off_comp_axes->x & BMI2_GYR_OFF_COMP_LSB_MASK); - - /* Get LSB value of y-axis from user-input */ - reg_data[1] = (uint8_t)(gyr_off_comp_axes->y & BMI2_GYR_OFF_COMP_LSB_MASK); - - /* Get LSB value of z-axis from user-input */ - reg_data[2] = (uint8_t)(gyr_off_comp_axes->z & BMI2_GYR_OFF_COMP_LSB_MASK); - - /* Get MSB value of x-axis to be set */ - reg_data[3] = BMI2_SET_BIT_POS0(reg_data[3], BMI2_GYR_OFF_COMP_MSB_X, gyr_off_msb_x); - - /* Get MSB value of y-axis to be set */ - reg_data[3] = BMI2_SET_BITS(reg_data[3], BMI2_GYR_OFF_COMP_MSB_Y, gyr_off_msb_y); - - /* Get MSB value of z-axis to be set */ - reg_data[3] = BMI2_SET_BITS(reg_data[3], BMI2_GYR_OFF_COMP_MSB_Z, gyr_off_msb_z); - - /* Set the offset compensation values of axes */ - rslt = bmi2_set_regs(BMI2_GYR_OFF_COMP_3_ADDR, reg_data, 4, dev); - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API updates the cross sensitivity coefficient between gyroscope's - * X and Z axes. - */ -int8_t bmi2_get_gyro_cross_sense(struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt = BMI2_OK; - struct bmi2_sensor_data data; - - /* Check if the feature is supported by this variant */ - if (dev->variant_feature & BMI2_GYRO_CROSS_SENS_ENABLE) - { - rslt = null_ptr_check(dev); - if (rslt == BMI2_OK) - { - /* Select the feature whose data is to be acquired */ - data.type = BMI2_GYRO_CROSS_SENSE; - - /* Get the respective data */ - rslt = bmi2_get_sensor_data(&data, 1, dev); - if (rslt == BMI2_OK) - { - /* Update the gyroscope cross sense value of z axis - * in the device structure - */ - dev->gyr_cross_sens_zx = data.sens_data.correction_factor_zx; - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - } - - return rslt; -} - -/*! - * @brief This API gets Error bits and message indicating internal status. - */ -int8_t bmi2_get_internal_status(uint8_t *int_stat, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (int_stat != NULL)) - { - /* Delay to read the internal status */ - dev->delay_us(20000, dev->intf_ptr); - - /* Get the error bits and message */ - rslt = bmi2_get_regs(BMI2_INTERNAL_STATUS_ADDR, int_stat, 1, dev); - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! @cond DOXYGEN_SUPRESS */ - -/* Suppressing doxygen warnings triggered for same static function names present across various sensor variant - * directories */ - -/*! - * @brief This API verifies and allows only the correct position to do Fast Offset Compensation for - * accelerometer & gyro. - */ -static int8_t verify_foc_position(uint8_t sens_list, - const struct bmi2_accel_foc_g_value *accel_g_axis, - struct bmi2_dev *dev) -{ - int8_t rslt; - - struct bmi2_sens_axes_data avg_foc_data = { 0 }; - struct bmi2_foc_temp_value temp_foc_data = { 0 }; - - rslt = null_ptr_check(dev); - if (rslt == BMI2_OK) - { - /* Enable sensor */ - rslt = bmi2_sensor_enable(&sens_list, 1, dev); - } - - if (rslt == BMI2_OK) - { - - rslt = get_average_of_sensor_data(sens_list, &temp_foc_data, dev); - if (rslt == BMI2_OK) - { - if (sens_list == BMI2_ACCEL) - { - - /* Taking modulus to make negative values as positive */ - if ((accel_g_axis->x == 1) && (accel_g_axis->sign == 1)) - { - temp_foc_data.x = temp_foc_data.x * -1; - } - else if ((accel_g_axis->y == 1) && (accel_g_axis->sign == 1)) - { - temp_foc_data.y = temp_foc_data.y * -1; - } - else if ((accel_g_axis->z == 1) && (accel_g_axis->sign == 1)) - { - temp_foc_data.z = temp_foc_data.z * -1; - } - } - - /* Typecasting into 16bit */ - avg_foc_data.x = (int16_t)(temp_foc_data.x); - avg_foc_data.y = (int16_t)(temp_foc_data.y); - avg_foc_data.z = (int16_t)(temp_foc_data.z); - - rslt = validate_foc_position(sens_list, accel_g_axis, avg_foc_data, dev); - } - } - - return rslt; -} - -/*! @endcond */ - -/*! - * @brief This API performs Fast Offset Compensation for accelerometer. - */ -int8_t bmi2_perform_accel_foc(const struct bmi2_accel_foc_g_value *accel_g_value, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Structure to define the accelerometer configurations */ - struct bmi2_accel_config acc_cfg = { 0, 0, 0, 0 }; - - /* Variable to store status of advance power save */ - uint8_t aps = 0; - - /* Variable to store status of accelerometer enable */ - uint8_t acc_en = 0; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (accel_g_value != NULL)) - { - /* Check for input validity */ - if ((((BMI2_ABS(accel_g_value->x)) + (BMI2_ABS(accel_g_value->y)) + (BMI2_ABS(accel_g_value->z))) == 1) && - ((accel_g_value->sign == 1) || (accel_g_value->sign == 0))) - { - rslt = verify_foc_position(BMI2_ACCEL, accel_g_value, dev); - if (rslt == BMI2_OK) - { - - /* Save accelerometer configurations, accelerometer - * enable status and advance power save status - */ - rslt = save_accel_foc_config(&acc_cfg, &aps, &acc_en, dev); - } - - /* Set configurations for FOC */ - if (rslt == BMI2_OK) - { - rslt = set_accel_foc_config(dev); - } - - /* Perform accelerometer FOC */ - if (rslt == BMI2_OK) - { - rslt = perform_accel_foc(accel_g_value, &acc_cfg, dev); - } - - /* Restore the saved configurations */ - if (rslt == BMI2_OK) - { - rslt = restore_accel_foc_config(&acc_cfg, aps, acc_en, dev); - } - } - else - { - rslt = BMI2_E_INVALID_INPUT; - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API performs Fast Offset Compensation for gyroscope. - */ -int8_t bmi2_perform_gyro_foc(struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Structure to define the gyroscope configurations */ - struct bmi2_gyro_config gyr_cfg = { 0, 0, 0, 0, 0, 0 }; - - /* Variable to store status of advance power save */ - uint8_t aps = 0; - - /* Variable to store status of gyroscope enable */ - uint8_t gyr_en = 0; - - /* Array of structure to store gyroscope data */ - struct bmi2_sens_axes_data gyr_value[128]; - - /* Structure to store gyroscope data temporarily */ - struct bmi2_foc_temp_value temp = { 0, 0, 0 }; - - /* Variable to store status read from the status register */ - uint8_t reg_status = 0; - - /* Variable to define count */ - uint8_t loop = 0; - - /* Structure to store the offset values to be stored in the register */ - struct bmi2_sens_axes_data gyro_offset = { 0, 0, 0, 0 }; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if (rslt == BMI2_OK) - { - /* Argument2 is not applicable for gyro */ - rslt = verify_foc_position(BMI2_GYRO, 0, dev); - if (rslt == BMI2_OK) - { - /* Save gyroscope configurations, gyroscope enable - * status and advance power save status - */ - rslt = save_gyro_config(&gyr_cfg, &aps, &gyr_en, dev); - - /* Set configurations for gyroscope FOC */ - if (rslt == BMI2_OK) - { - rslt = set_gyro_foc_config(dev); - } - - /* Perform FOC */ - if (rslt == BMI2_OK) - { - for (loop = 0; loop < 128; loop++) - { - /* Giving a delay of more than 40ms since ODR is configured as 25Hz */ - dev->delay_us(50000, dev->intf_ptr); - - /* Get gyroscope data ready interrupt status */ - rslt = bmi2_get_status(®_status, dev); - - /* Read 128 samples of gyroscope data on data ready interrupt */ - if ((rslt == BMI2_OK) && (reg_status & BMI2_DRDY_GYR)) - { - rslt = read_gyro_xyz(&gyr_value[loop], dev); - if (rslt == BMI2_OK) - { - /* Store the data in a temporary structure */ - temp.x = temp.x + (int32_t)gyr_value[loop].x; - temp.y = temp.y + (int32_t)gyr_value[loop].y; - temp.z = temp.z + (int32_t)gyr_value[loop].z; - } - } - - if (rslt != BMI2_OK) - { - break; - } - else if ((reg_status & BMI2_DRDY_GYR) != BMI2_DRDY_GYR) - { - rslt = BMI2_E_INVALID_STATUS; - break; - } - } - - if (rslt == BMI2_OK) - { - /* Take average of x, y and z data for lesser - * noise. It is same as offset data since lsb/dps - * is same for both data and offset register - */ - gyro_offset.x = (int16_t)(temp.x / 128); - gyro_offset.y = (int16_t)(temp.y / 128); - gyro_offset.z = (int16_t)(temp.z / 128); - - /* Saturate gyroscope data since the offset - * registers are of 10 bit value where as the - * gyroscope data is of 16 bit value - */ - saturate_gyro_data(&gyro_offset); - - /* Invert the gyroscope offset data */ - invert_gyro_offset(&gyro_offset); - - /* Write offset data in the gyroscope offset - * compensation register - */ - rslt = bmi2_write_gyro_offset_comp_axes(&gyro_offset, dev); - } - - /* Enable gyroscope offset compensation */ - if (rslt == BMI2_OK) - { - rslt = bmi2_set_gyro_offset_comp(BMI2_ENABLE, dev); - } - - /* Restore the saved gyroscope configurations */ - if (rslt == BMI2_OK) - { - rslt = restore_gyro_config(&gyr_cfg, aps, gyr_en, dev); - } - } - } - } - - return rslt; -} - -/*! - * @brief This API is used to get the feature configuration from the - * selected page. - */ -int8_t bmi2_get_feat_config(uint8_t sw_page, uint8_t *feat_config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define bytes remaining to read */ - uint8_t bytes_remain = BMI2_FEAT_SIZE_IN_BYTES; - - /* Variable to define the read-write length */ - uint8_t read_write_len = 0; - - /* Variable to define the feature configuration address */ - uint8_t addr = BMI2_FEATURES_REG_ADDR; - - /* Variable to define index */ - uint8_t index = 0; - - if ((feat_config == NULL) || (dev == NULL)) - { - rslt = BMI2_E_NULL_PTR; - } - else - { - /* Check whether the page is valid */ - if (sw_page < dev->page_max) - { - /* Switch page */ - rslt = bmi2_set_regs(BMI2_FEAT_PAGE_ADDR, &sw_page, 1, dev); - - /* If user length is less than feature length */ - if ((rslt == BMI2_OK) && (dev->read_write_len < BMI2_FEAT_SIZE_IN_BYTES)) - { - /* Read-write should be even */ - if ((dev->read_write_len % 2) != 0) - { - dev->read_write_len--; - } - - while (bytes_remain > 0) - { - if (bytes_remain >= dev->read_write_len) - { - /* Read from the page */ - rslt = bmi2_get_regs(addr, &feat_config[index], dev->read_write_len, dev); - - /* Update index */ - index += (uint8_t) dev->read_write_len; - - /* Update address */ - addr += (uint8_t) dev->read_write_len; - - /* Update read-write length */ - read_write_len += (uint8_t) dev->read_write_len; - } - else - { - /* Read from the page */ - rslt = bmi2_get_regs(addr, (uint8_t *) (feat_config + index), (uint16_t) bytes_remain, dev); - - /* Update read-write length */ - read_write_len += bytes_remain; - } - - /* Remaining bytes */ - bytes_remain = BMI2_FEAT_SIZE_IN_BYTES - read_write_len; - - if (rslt != BMI2_OK) - { - break; - } - } - } - else if (rslt == BMI2_OK) - { - /* Get configuration from the page */ - rslt = bmi2_get_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_PAGE; - } - } - - return rslt; -} - -/*! - * @brief This API is used to extract the input feature configuration - * details from the look-up table. - */ -uint8_t bmi2_extract_input_feat_config(struct bmi2_feature_config *feat_config, uint8_t type, - const struct bmi2_dev *dev) -{ - /* Variable to define loop */ - uint8_t loop = 0; - - /* Variable to set flag */ - uint8_t feat_found = BMI2_FALSE; - - /* Search for the input feature from the input configuration array */ - while (loop < dev->input_sens) - { - if (dev->feat_config[loop].type == type) - { - *feat_config = dev->feat_config[loop]; - feat_found = BMI2_TRUE; - break; - } - - loop++; - } - - /* Return flag */ - return feat_found; -} - -/***************************************************************************/ - -/*! Local Function Definitions - ****************************************************************************/ - -/*! @cond DOXYGEN_SUPRESS */ - -/* Suppressing doxygen warnings triggered for same static function names present across various sensor variant - * directories */ - -/*! - * @brief This internal API writes the configuration file. - */ -static int8_t write_config_file(struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to update the configuration file index */ - uint16_t index = 0; - - /* config file size */ - uint16_t config_size = dev->config_size; - - /* Variable to get the remainder */ - uint8_t remain = (uint8_t)(config_size % dev->read_write_len); - - /* Variable to get the balance bytes */ - uint16_t bal_byte = 0; - - /* Variable to define temporary read/write length */ - uint16_t read_write_len = 0; - - /* Disable advanced power save mode */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - /* Disable loading of the configuration */ - rslt = set_config_load(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - if (!remain) - { - /* Write the configuration file */ - for (index = 0; (index < config_size) && (rslt == BMI2_OK); index += dev->read_write_len) - { - rslt = upload_file((dev->config_file_ptr + index), index, dev->read_write_len, dev); - } - } - else - { - /* Get the balance bytes */ - bal_byte = (uint16_t) config_size - (uint16_t) remain; - - /* Write the configuration file for the balancem bytes */ - for (index = 0; (index < bal_byte) && (rslt == BMI2_OK); index += dev->read_write_len) - { - rslt = upload_file((dev->config_file_ptr + index), index, dev->read_write_len, dev); - } - - if (rslt == BMI2_OK) - { - /* Update length in a temporary variable */ - read_write_len = dev->read_write_len; - - /* Write the remaining bytes in 2 bytes length */ - dev->read_write_len = 2; - - /* Write the configuration file for the remaining bytes */ - for (index = bal_byte; - (index < config_size) && (rslt == BMI2_OK); - index += dev->read_write_len) - { - rslt = upload_file((dev->config_file_ptr + index), index, dev->read_write_len, dev); - } - - /* Restore the user set length back from the temporary variable */ - dev->read_write_len = read_write_len; - } - } - - if (rslt == BMI2_OK) - { - /* Enable loading of the configuration */ - rslt = set_config_load(BMI2_ENABLE, dev); - - /* Wait till ASIC is initialized */ - dev->delay_us(150000, dev->intf_ptr); - if (rslt == BMI2_OK) - { - /* Enable advanced power save mode */ - rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - } - } - } - } - - return rslt; -} - -/*! - * @brief This internal API enables/disables the loading of the configuration - * file. - */ -static int8_t set_config_load(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to store data */ - uint8_t reg_data = 0; - - rslt = bmi2_get_regs(BMI2_INIT_CTRL_ADDR, ®_data, 1, dev); - if (rslt == BMI2_OK) - { - reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_CONF_LOAD_EN, enable); - rslt = bmi2_set_regs(BMI2_INIT_CTRL_ADDR, ®_data, 1, dev); - } - - return rslt; -} - -/*! - * @brief This internal API loads the configuration file. - */ -static int8_t upload_file(const uint8_t *config_data, uint16_t index, uint16_t write_len, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to store address */ - uint8_t addr_array[2] = { 0 }; - - if (config_data != NULL) - { - /* Store 0 to 3 bits of address in first byte */ - addr_array[0] = (uint8_t)((index / 2) & 0x0F); - - /* Store 4 to 11 bits of address in the second byte */ - addr_array[1] = (uint8_t)((index / 2) >> 4); - - /* Write the 2 bytes of address in consecutive locations */ - rslt = bmi2_set_regs(BMI2_INIT_ADDR_0, addr_array, 2, dev); - if (rslt == BMI2_OK) - { - /* Burst write configuration file data corresponding to user set length */ - rslt = bmi2_set_regs(BMI2_INIT_DATA_ADDR, (uint8_t *)config_data, write_len, dev); - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This internal API validates bandwidth and performance mode of the - * accelerometer set by the user. - */ -static int8_t validate_bw_perf_mode(uint8_t *bandwidth, uint8_t *perf_mode, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Validate and auto-correct performance mode */ - rslt = check_boundary_val(perf_mode, BMI2_POWER_OPT_MODE, BMI2_PERF_OPT_MODE, dev); - if (rslt == BMI2_OK) - { - /* Validate and auto-correct bandwidth parameter */ - if (*perf_mode == BMI2_PERF_OPT_MODE) - { - /* Validate for continuous filter mode */ - rslt = check_boundary_val(bandwidth, BMI2_ACC_OSR4_AVG1, BMI2_ACC_CIC_AVG8, dev); - } - else - { - /* Validate for CIC averaging mode */ - rslt = check_boundary_val(bandwidth, BMI2_ACC_OSR4_AVG1, BMI2_ACC_RES_AVG128, dev); - } - } - - return rslt; -} - -/*! - * @brief This internal API validates ODR and range of the accelerometer set by - * the user. - */ -static int8_t validate_odr_range(uint8_t *odr, uint8_t *range, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Validate and auto correct ODR */ - rslt = check_boundary_val(odr, BMI2_ACC_ODR_0_78HZ, BMI2_ACC_ODR_1600HZ, dev); - if (rslt == BMI2_OK) - { - /* Validate and auto correct Range */ - rslt = check_boundary_val(range, BMI2_ACC_RANGE_2G, BMI2_ACC_RANGE_16G, dev); - } - - return rslt; -} - -/*! - * @brief This internal API validates bandwidth, performance mode, low power/ - * high performance mode, ODR, and range set by the user. - */ -static int8_t validate_gyro_config(struct bmi2_gyro_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Validate and auto-correct performance mode */ - rslt = check_boundary_val(&config->filter_perf, BMI2_POWER_OPT_MODE, BMI2_PERF_OPT_MODE, dev); - if (rslt == BMI2_OK) - { - /* Validate and auto-correct bandwidth parameter */ - rslt = check_boundary_val(&config->bwp, BMI2_GYR_OSR4_MODE, BMI2_GYR_CIC_MODE, dev); - if (rslt == BMI2_OK) - { - /* Validate and auto-correct low power/high-performance parameter */ - rslt = check_boundary_val(&config->noise_perf, BMI2_POWER_OPT_MODE, BMI2_PERF_OPT_MODE, dev); - if (rslt == BMI2_OK) - { - /* Validate and auto-correct ODR parameter */ - rslt = check_boundary_val(&config->odr, BMI2_GYR_ODR_25HZ, BMI2_GYR_ODR_3200HZ, dev); - if (rslt == BMI2_OK) - { - /* Validate and auto-correct OIS range */ - rslt = check_boundary_val(&config->ois_range, BMI2_GYR_OIS_250, BMI2_GYR_OIS_2000, dev); - if (rslt == BMI2_OK) - { - /* Validate and auto-correct range parameter */ - rslt = check_boundary_val(&config->range, BMI2_GYR_RANGE_2000, BMI2_GYR_RANGE_125, dev); - } - } - } - } - } - - return rslt; -} - -/*! - * @brief This internal API shows the error status when illegal sensor - * configuration is set. - */ -static int8_t cfg_error_status(struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to store data */ - uint8_t reg_data; - - /* Get error status of the set sensor configuration */ - rslt = bmi2_get_regs(BMI2_EVENT_ADDR, ®_data, 1, dev); - if (rslt == BMI2_OK) - { - reg_data = BMI2_GET_BITS(reg_data, BMI2_EVENT_FLAG); - switch (reg_data) - { - case BMI2_NO_ERROR: - rslt = BMI2_OK; - break; - case BMI2_ACC_ERROR: - rslt = BMI2_E_ACC_INVALID_CFG; - break; - case BMI2_GYR_ERROR: - rslt = BMI2_E_GYRO_INVALID_CFG; - break; - case BMI2_ACC_GYR_ERROR: - rslt = BMI2_E_ACC_GYR_INVALID_CFG; - break; - default: - break; - } - } - - return rslt; -} - -/*! - * @brief This internal API: - * 1) Enables/Disables auxiliary interface. - * 2) Sets auxiliary interface configurations like I2C address, manual/auto - * mode enable, manual burst read length, AUX burst read length and AUX read - * address. - * 3)It maps/un-maps data interrupts to that of hardware interrupt line. - */ -static int8_t set_aux_config(struct bmi2_aux_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Validate auxiliary configurations */ - rslt = validate_aux_config(config, dev); - if (rslt == BMI2_OK) - { - /* Enable/Disable auxiliary interface */ - rslt = set_aux_interface(config, dev); - if (rslt == BMI2_OK) - { - /* Set the auxiliary interface configurations */ - rslt = config_aux_interface(config, dev); - if (rslt == BMI2_OK) - { - /* Set read out offset and ODR */ - rslt = config_aux(config, dev); - } - } - } - - return rslt; -} - -/*! - * @brief This internal API sets gyroscope user-gain configurations like gain - * update value for x, y and z-axis. - */ -static int8_t set_gyro_user_gain_config(const struct bmi2_gyro_user_gain_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define index */ - uint8_t index = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for user-gain */ - struct bmi2_feature_config user_gain_config = { 0, 0, 0 }; - - /* Copy the feature configuration address to a local pointer */ - uint16_t *data_p = (uint16_t *) (void *)feat_config; - - /* Search for user-gain feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&user_gain_config, BMI2_GYRO_GAIN_UPDATE, dev); - if (feat_found) - { - /* Get the configuration from the page where user-gain feature resides */ - rslt = bmi2_get_feat_config(user_gain_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for user-gain select */ - idx = user_gain_config.start_addr; - - /* Get offset in words since all the features are set in words length */ - idx = idx / 2; - - /* Set ratio_x */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_GYR_USER_GAIN_RATIO_X, config->ratio_x); - - /* Increment offset by 1 word to set ratio_y */ - idx++; - - /* Set ratio_y */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_GYR_USER_GAIN_RATIO_Y, config->ratio_y); - - /* Increment offset by 1 word to set ratio_z */ - idx++; - - /* Set ratio_z */ - *(data_p + idx) = BMI2_SET_BIT_POS0(*(data_p + idx), BMI2_GYR_USER_GAIN_RATIO_Z, config->ratio_z); - - /* Increment offset by 1 more word to get the total length in words */ - idx++; - - /* Get total length in bytes to copy from local pointer to the array */ - idx = (uint8_t)(idx * 2) - user_gain_config.start_addr; - - /* Copy the bytes to be set back to the array */ - for (index = 0; index < idx; index++) - { - feat_config[user_gain_config.start_addr + - index] = *((uint8_t *) data_p + user_gain_config.start_addr + index); - } - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(BMI2_FEATURES_REG_ADDR, feat_config, BMI2_FEAT_SIZE_IN_BYTES, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API enables/disables auxiliary interface. - */ -static int8_t set_aux_interface(const struct bmi2_aux_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to store data */ - uint8_t reg_data; - - rslt = bmi2_get_regs(BMI2_IF_CONF_ADDR, ®_data, 1, dev); - if (rslt == BMI2_OK) - { - reg_data = BMI2_SET_BITS(reg_data, BMI2_AUX_IF_EN, config->aux_en); - - /* Enable/Disable auxiliary interface */ - rslt = bmi2_set_regs(BMI2_IF_CONF_ADDR, ®_data, 1, dev); - } - - return rslt; -} - -/*! - * @brief This internal API sets auxiliary configurations like manual/auto mode - * FCU write command enable and read burst length for both data and manual mode. - * - * @note Auxiliary sensor should not be busy when configuring aux_i2c_addr, - * man_rd_burst_len, aux_rd_burst_len and aux_rd_addr. - */ -static int8_t config_aux_interface(const struct bmi2_aux_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to store data */ - uint8_t reg_data[2] = { 0 }; - - /* Variable to store status */ - uint8_t status = 0; - - /* Variable to define count */ - uint8_t count = 0; - - rslt = bmi2_get_regs(BMI2_AUX_DEV_ID_ADDR, reg_data, 2, dev); - if (rslt == BMI2_OK) - { - /* Set I2C address for AUX sensor */ - reg_data[0] = BMI2_SET_BITS(reg_data[0], BMI2_AUX_SET_I2C_ADDR, config->i2c_device_addr); - - /* Set the AUX IF to either manual or auto mode */ - reg_data[1] = BMI2_SET_BITS(reg_data[1], BMI2_AUX_MAN_MODE_EN, config->manual_en); - - /* Enables FCU write command on AUX IF for auxiliary sensors that need a trigger */ - reg_data[1] = BMI2_SET_BITS(reg_data[1], BMI2_AUX_FCU_WR_EN, config->fcu_write_en); - - /* Set the burst read length for manual mode */ - reg_data[1] = BMI2_SET_BITS(reg_data[1], BMI2_AUX_MAN_READ_BURST, config->man_rd_burst); - - /* Set the burst read length for data mode */ - reg_data[1] = BMI2_SET_BIT_POS0(reg_data[1], BMI2_AUX_READ_BURST, config->aux_rd_burst); - for (;;) - { - /* Check if auxiliary sensor is busy */ - rslt = bmi2_get_status(&status, dev); - if ((rslt == BMI2_OK) && (!(status & BMI2_AUX_BUSY))) - { - /* Set the configurations if AUX is not busy */ - rslt = bmi2_set_regs(BMI2_AUX_DEV_ID_ADDR, reg_data, 2, dev); - dev->delay_us(1000, dev->intf_ptr); - if (rslt == BMI2_OK) - { - /* If data mode */ - if (!config->manual_en) - { - /* Disable manual enable flag in device structure */ - dev->aux_man_en = 0; - - /* Set the read address of the AUX sensor */ - rslt = bmi2_set_regs(BMI2_AUX_RD_ADDR, (uint8_t *) &config->read_addr, 1, dev); - dev->delay_us(1000, dev->intf_ptr); - } - else - { - /* Enable manual enable flag in device structure */ - dev->aux_man_en = 1; - - /* Update manual read burst length in device structure */ - dev->aux_man_rd_burst_len = config->man_rd_burst; - } - } - - /* Break after setting the register */ - break; - } - - /* Increment count after every 10 seconds */ - dev->delay_us(10000, dev->intf_ptr); - count++; - - /* Break after 2 seconds if AUX still busy - since slowest ODR is 0.78Hz*/ - if (count > 20) - { - rslt = BMI2_E_AUX_BUSY; - break; - } - } - } - - return rslt; -} - -/*! - * @brief This internal API triggers read out offset and sets ODR of the - * auxiliary sensor. - */ -static int8_t config_aux(const struct bmi2_aux_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to store data */ - uint8_t reg_data; - - rslt = bmi2_get_regs(BMI2_AUX_CONF_ADDR, ®_data, 1, dev); - if (rslt == BMI2_OK) - { - /* Trigger read out offset */ - reg_data = BMI2_SET_BITS(reg_data, BMI2_AUX_OFFSET_READ_OUT, config->offset); - - /* Set ODR */ - reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_AUX_ODR_EN, config->odr); - - /* Set auxiliary configuration register */ - rslt = bmi2_set_regs(BMI2_AUX_CONF_ADDR, ®_data, 1, dev); - dev->delay_us(1000, dev->intf_ptr); - } - - return rslt; -} - -/*! - * @brief This internal API checks the busy status of auxiliary sensor and sets - * the auxiliary register addresses when not busy. - */ -static int8_t set_if_aux_not_busy(uint8_t reg_addr, uint8_t reg_data, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to get status of AUX_BUSY */ - uint8_t status = 0; - - /* Variable to define count for time-out */ - uint8_t count = 0; - - for (;;) - { - /* Check if AUX is busy */ - rslt = bmi2_get_status(&status, dev); - - /* Set the registers if not busy */ - if ((rslt == BMI2_OK) && (!(status & BMI2_AUX_BUSY))) - { - rslt = bmi2_set_regs(reg_addr, ®_data, 1, dev); - dev->delay_us(1000, dev->intf_ptr); - - /* Break after setting the register */ - break; - } - - /* Increment count after every 10 seconds */ - dev->delay_us(10000, dev->intf_ptr); - count++; - - /* Break after 2 seconds if AUX still busy - since slowest ODR is 0.78Hz*/ - if (count > 20) - { - rslt = BMI2_E_AUX_BUSY; - break; - } - } - - return rslt; -} - -/*! - * @brief This internal API validates auxiliary configuration set by the user. - */ -static int8_t validate_aux_config(struct bmi2_aux_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Validate ODR for auxiliary sensor */ - rslt = check_boundary_val(&config->odr, BMI2_AUX_ODR_0_78HZ, BMI2_AUX_ODR_800HZ, dev); - - return rslt; -} - -/*! - * @brief This internal API gets accelerometer configurations like ODR, - * bandwidth, performance mode and g-range. - */ -static int8_t get_accel_config(struct bmi2_accel_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to store data */ - uint8_t data_array[2] = { 0 }; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (config != NULL)) - { - /* Read the sensor configuration details */ - rslt = bmi2_get_regs(BMI2_ACC_CONF_ADDR, data_array, 2, dev); - if (rslt == BMI2_OK) - { - /* Get accelerometer performance mode */ - config->filter_perf = BMI2_GET_BITS(data_array[0], BMI2_ACC_FILTER_PERF_MODE); - - /* Get accelerometer bandwidth */ - config->bwp = BMI2_GET_BITS(data_array[0], BMI2_ACC_BW_PARAM); - - /* Get accelerometer ODR */ - config->odr = BMI2_GET_BIT_POS0(data_array[0], BMI2_ACC_ODR); - - /* Get accelerometer range */ - config->range = BMI2_GET_BIT_POS0(data_array[1], BMI2_ACC_RANGE); - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This internal API gets gyroscope configurations like ODR, bandwidth, - * low power/high performance mode, performance mode and range. - */ -static int8_t get_gyro_config(struct bmi2_gyro_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to store data */ - uint8_t data_array[2] = { 0 }; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (config != NULL)) - { - /* Read the sensor configuration details */ - rslt = bmi2_get_regs(BMI2_GYR_CONF_ADDR, data_array, 2, dev); - if (rslt == BMI2_OK) - { - /* Get gyroscope performance mode */ - config->filter_perf = BMI2_GET_BITS(data_array[0], BMI2_GYR_FILTER_PERF_MODE); - - /* Get gyroscope noise performance mode */ - config->noise_perf = BMI2_GET_BITS(data_array[0], BMI2_GYR_NOISE_PERF_MODE); - - /* Get gyroscope bandwidth */ - config->bwp = BMI2_GET_BITS(data_array[0], BMI2_GYR_BW_PARAM); - - /* Get gyroscope ODR */ - config->odr = BMI2_GET_BIT_POS0(data_array[0], BMI2_GYR_ODR); - - /* Get gyroscope OIS range */ - config->ois_range = BMI2_GET_BITS(data_array[1], BMI2_GYR_OIS_RANGE); - - /* Get gyroscope range */ - config->range = BMI2_GET_BIT_POS0(data_array[1], BMI2_GYR_RANGE); - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This internal API: - * 1) Gets the status of auxiliary interface enable. - * 2) Gets auxiliary interface configurations like I2C address, manual/auto - * mode enable, manual burst read length, AUX burst read length and AUX read - * address. - * 3) Gets ODR and offset. - */ -static int8_t get_aux_config(struct bmi2_aux_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - if ((rslt == BMI2_OK) && (config != NULL)) - { - /* Get enable status of auxiliary interface */ - rslt = get_aux_interface(config, dev); - if (rslt == BMI2_OK) - { - /* Get the auxiliary interface configurations */ - rslt = get_aux_interface_config(config, dev); - if (rslt == BMI2_OK) - { - /* Get read out offset and ODR */ - rslt = get_aux_cfg(config, dev); - } - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This internal API gets gyroscope user-gain configurations like gain - * update value for x, y and z-axis. - */ -static int8_t get_gyro_gain_update_config(struct bmi2_gyro_user_gain_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define LSB */ - uint16_t lsb = 0; - - /* Variable to define MSB */ - uint16_t msb = 0; - - /* Variable to define a word */ - uint16_t lsb_msb = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for user-gain */ - struct bmi2_feature_config user_gain_config = { 0, 0, 0 }; - - /* Search for user-gain feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&user_gain_config, BMI2_GYRO_GAIN_UPDATE, dev); - if (feat_found) - { - /* Get the configuration from the page where user-gain feature resides */ - rslt = bmi2_get_feat_config(user_gain_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for user-gain select */ - idx = user_gain_config.start_addr; - - /* Get word to calculate ratio_x */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get ratio_x */ - config->ratio_x = lsb_msb & BMI2_GYR_USER_GAIN_RATIO_X_MASK; - - /* Get word to calculate ratio_y */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get ratio_y */ - config->ratio_y = lsb_msb & BMI2_GYR_USER_GAIN_RATIO_Y_MASK; - - /* Get word to calculate ratio_z */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get ratio_z */ - config->ratio_z = lsb_msb & BMI2_GYR_USER_GAIN_RATIO_Z_MASK; - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API gets the enable status of auxiliary interface. - */ -static int8_t get_aux_interface(struct bmi2_aux_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to store data */ - uint8_t reg_data; - - /* Get the enable status of auxiliary interface */ - rslt = bmi2_get_regs(BMI2_IF_CONF_ADDR, ®_data, 1, dev); - if (rslt == BMI2_OK) - { - config->aux_en = BMI2_GET_BITS(reg_data, BMI2_AUX_IF_EN); - } - - return rslt; -} - -/*! - * @brief This internal API gets auxiliary configurations like manual/auto mode - * FCU write command enable and read burst length for both data and manual mode. - */ -static int8_t get_aux_interface_config(struct bmi2_aux_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to store data */ - uint8_t reg_data[2] = { 0 }; - - rslt = bmi2_get_regs(BMI2_AUX_DEV_ID_ADDR, reg_data, 2, dev); - if (rslt == BMI2_OK) - { - /* Get I2C address for auxiliary sensor */ - config->i2c_device_addr = BMI2_GET_BITS(reg_data[0], BMI2_AUX_SET_I2C_ADDR); - - /* Get the AUX IF to either manual or auto mode */ - config->manual_en = BMI2_GET_BITS(reg_data[1], BMI2_AUX_MAN_MODE_EN); - - /* Enables FCU write command on AUX IF for auxiliary sensors that need a trigger */ - config->fcu_write_en = BMI2_GET_BITS(reg_data[1], BMI2_AUX_FCU_WR_EN); - - /* Get the burst read length for manual mode */ - config->man_rd_burst = BMI2_GET_BITS(reg_data[1], BMI2_AUX_MAN_READ_BURST); - - /* Get the burst read length for data mode */ - config->aux_rd_burst = BMI2_GET_BIT_POS0(reg_data[1], BMI2_AUX_READ_BURST); - - /* If data mode, get the read address of the auxiliary sensor from where data is to be read */ - if (!config->manual_en) - { - rslt = bmi2_get_regs(BMI2_AUX_RD_ADDR, &config->read_addr, 1, dev); - } - } - - return rslt; -} - -/*! - * @brief This internal API gets read out offset and ODR of the auxiliary - * sensor. - */ -static int8_t get_aux_cfg(struct bmi2_aux_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to store data */ - uint8_t reg_data; - - rslt = bmi2_get_regs(BMI2_AUX_CONF_ADDR, ®_data, 1, dev); - if (rslt == BMI2_OK) - { - /* Get read out offset */ - config->offset = BMI2_GET_BITS(reg_data, BMI2_AUX_OFFSET_READ_OUT); - - /* Get ODR */ - config->odr = BMI2_GET_BIT_POS0(reg_data, BMI2_AUX_ODR_EN); - } - - return rslt; -} - -/*! - * @brief This internal API maps/un-maps feature interrupts to that of interrupt - * pins. - */ -static int8_t map_feat_int(uint8_t *reg_data_array, enum bmi2_hw_int_pin int_pin, uint8_t int_mask) -{ - /* Variable to define error */ - int8_t rslt = BMI2_OK; - - /* Check for NULL error */ - if (reg_data_array != NULL) - { - /* Check validity on interrupt pin selection */ - if (int_pin < BMI2_INT_PIN_MAX) - { - switch (int_pin) - { - case BMI2_INT_NONE: - - /* Un-Map the corresponding feature interrupt to interrupt pin 1 and 2 */ - reg_data_array[0] &= ~(int_mask); - reg_data_array[1] &= ~(int_mask); - break; - case BMI2_INT1: - - /* Map the corresponding feature interrupt to interrupt pin 1 */ - reg_data_array[0] |= int_mask; - - /* Un-map the corresponding feature interrupt to interrupt pin 2 */ - reg_data_array[1] &= ~(int_mask); - break; - case BMI2_INT2: - - /* Map the corresponding feature interrupt to interrupt pin 2 */ - reg_data_array[1] |= int_mask; - - /* Un-map the corresponding feature interrupt to interrupt pin 1 */ - reg_data_array[0] &= ~(int_mask); - break; - case BMI2_INT_BOTH: - - /* Map the corresponding feature interrupt to interrupt pin 1 and 2 */ - reg_data_array[0] |= int_mask; - reg_data_array[1] |= int_mask; - break; - default: - break; - } - } - else - { - /* Return error if invalid pin selection */ - rslt = BMI2_E_INVALID_INT_PIN; - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This internal API gets the accelerometer data from the register. - */ -static int8_t get_accel_sensor_data(struct bmi2_sens_axes_data *data, uint8_t reg_addr, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define data stored in register */ - uint8_t reg_data[BMI2_ACC_GYR_NUM_BYTES] = { 0 }; - - /* Read the sensor data */ - rslt = bmi2_get_regs(reg_addr, reg_data, BMI2_ACC_GYR_NUM_BYTES, dev); - if (rslt == BMI2_OK) - { - /* Get accelerometer data from the register */ - get_acc_gyr_data(data, reg_data); - - /* Get the re-mapped accelerometer data */ - get_remapped_data(data, dev); - } - - return rslt; -} - -/*! - * @brief This internal API gets the gyroscope data from the register. - */ -static int8_t get_gyro_sensor_data(struct bmi2_sens_axes_data *data, uint8_t reg_addr, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define data stored in register */ - uint8_t reg_data[BMI2_ACC_GYR_NUM_BYTES] = { 0 }; - - /* Read the sensor data */ - rslt = bmi2_get_regs(reg_addr, reg_data, BMI2_ACC_GYR_NUM_BYTES, dev); - if (rslt == BMI2_OK) - { - /* Get gyroscope data from the register */ - get_acc_gyr_data(data, reg_data); - - /* Get the compensated gyroscope data */ - comp_gyro_cross_axis_sensitivity(data, dev); - - /* Get the re-mapped gyroscope data */ - get_remapped_data(data, dev); - - } - - return rslt; -} - -/*! - * @brief This internal API gets the accelerometer/gyroscope data. - */ -static void get_acc_gyr_data(struct bmi2_sens_axes_data *data, const uint8_t *reg_data) -{ - /* Variables to store msb value */ - uint8_t msb; - - /* Variables to store lsb value */ - uint8_t lsb; - - /* Variables to store both msb and lsb value */ - uint16_t msb_lsb; - - /* Variables to define index */ - uint8_t index = 0; - - /* Read x-axis data */ - lsb = reg_data[index++]; - msb = reg_data[index++]; - msb_lsb = ((uint16_t) msb << 8) | (uint16_t) lsb; - data->x = (int16_t) msb_lsb; - - /* Read y-axis data */ - lsb = reg_data[index++]; - msb = reg_data[index++]; - msb_lsb = ((uint16_t) msb << 8) | (uint16_t) lsb; - data->y = (int16_t) msb_lsb; - - /* Read z-axis data */ - lsb = reg_data[index++]; - msb = reg_data[index++]; - msb_lsb = ((uint16_t) msb << 8) | (uint16_t) lsb; - data->z = (int16_t) msb_lsb; -} - -/*! - * @brief This internal API gets the re-mapped accelerometer/gyroscope data. - */ -static void get_remapped_data(struct bmi2_sens_axes_data *data, const struct bmi2_dev *dev) -{ - /* Array to defined the re-mapped sensor data */ - int16_t remap_data[3] = { 0 }; - int16_t pos_multiplier = INT16_C(1); - int16_t neg_multiplier = INT16_C(-1); - - /* Fill the array with the un-mapped sensor data */ - remap_data[0] = data->x; - remap_data[1] = data->y; - remap_data[2] = data->z; - - /* Get the re-mapped x axis data */ - if (dev->remap.x_axis_sign == BMI2_POS_SIGN) - { - data->x = (int16_t)(remap_data[dev->remap.x_axis] * pos_multiplier); - } - else - { - data->x = (int16_t)(remap_data[dev->remap.x_axis] * neg_multiplier); - } - - /* Get the re-mapped y axis data */ - if (dev->remap.y_axis_sign == BMI2_POS_SIGN) - { - data->y = (int16_t)(remap_data[dev->remap.y_axis] * pos_multiplier); - } - else - { - data->y = (int16_t)(remap_data[dev->remap.y_axis] * neg_multiplier); - } - - /* Get the re-mapped z axis data */ - if (dev->remap.z_axis_sign == BMI2_POS_SIGN) - { - data->z = (int16_t)(remap_data[dev->remap.z_axis] * pos_multiplier); - } - else - { - data->z = (int16_t)(remap_data[dev->remap.z_axis] * neg_multiplier); - } -} - -/*! - * @brief This internal API reads the user-defined bytes of data from the given - * register address of auxiliary sensor in manual mode. - */ -static int8_t read_aux_data(uint8_t reg_addr, uint8_t *aux_data, uint16_t len, uint8_t burst_len, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt = BMI2_OK; - - /* Array to store the register data */ - uint8_t reg_data[15] = { 0 }; - - /* Variable to define number of bytes to read */ - uint16_t read_length = 0; - - /* Variable to define loop */ - uint8_t loop = 0; - - /* Variable to define counts to get the correct array index */ - uint8_t count = 0; - - /* Variable to define index for the array */ - uint8_t idx = 0; - - while (len > 0) - { - /* Set the read address if AUX is not busy */ - rslt = set_if_aux_not_busy(BMI2_AUX_RD_ADDR, reg_addr, dev); - if (rslt == BMI2_OK) - { - /* Read data from bmi2 data register */ - rslt = bmi2_get_regs(BMI2_AUX_X_LSB_ADDR, reg_data, (uint16_t) burst_len, dev); - dev->delay_us(1000, dev->intf_ptr); - if (rslt == BMI2_OK) - { - /* Get number of bytes to be read */ - if (len < burst_len) - { - read_length = (uint8_t) len; - } - else - { - read_length = burst_len; - } - - /* Update array index and store the data */ - for (loop = 0; loop < read_length; loop++) - { - idx = loop + count; - aux_data[idx] = reg_data[loop]; - } - } - } - - /* Update address for the next read */ - reg_addr += burst_len; - - /* Update count for the array index */ - count += burst_len; - - /* Update no of bytes left to read */ - len -= read_length; - } - - return rslt; -} - -/*! - * @brief This internal API writes AUX write address and the user-defined bytes - * of data to the AUX sensor in manual mode. - * - * @note Change of BMI2_AUX_WR_ADDR is only allowed if AUX is not busy. - */ -static int8_t write_aux_data(uint8_t reg_addr, uint8_t reg_data, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Write data to be written to the AUX sensor in bmi2 register */ - rslt = bmi2_set_regs(BMI2_AUX_WR_DATA_ADDR, ®_data, 1, dev); - if (rslt == BMI2_OK) - { - /* Write the AUX address where data is to be stored when AUX is not busy */ - rslt = set_if_aux_not_busy(BMI2_AUX_WR_ADDR, reg_addr, dev); - } - - return rslt; -} - -/*! - * @brief This internal API reads the user-defined bytes of data from the given - * register address of auxiliary sensor in data mode. - */ -static int8_t read_aux_data_mode(uint8_t *aux_data, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variables to define loop */ - uint8_t count = 0; - - /* Variables to define index */ - uint8_t index = 0; - - /* Array to define data stored in register */ - uint8_t reg_data[BMI2_AUX_NUM_BYTES] = { 0 }; - - /* Check if data mode */ - if (!dev->aux_man_en) - { - /* Read the auxiliary sensor data */ - rslt = bmi2_get_regs(BMI2_AUX_X_LSB_ADDR, reg_data, BMI2_AUX_NUM_BYTES, dev); - if (rslt == BMI2_OK) - { - /* Get the 8 bytes of auxiliary data */ - do - { - *(aux_data + count++) = *(reg_data + index++); - } while (count < BMI2_AUX_NUM_BYTES); - } - } - else - { - rslt = BMI2_E_AUX_INVALID_CFG; - } - - return rslt; -} - -/*! - * @brief This internal API maps the actual burst read length with that of the - * register value set by user. - */ -static int8_t map_read_len(uint8_t *len, const struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt = BMI2_OK; - - /* Get the burst read length against the values set by the user */ - switch (dev->aux_man_rd_burst_len) - { - case BMI2_AUX_READ_LEN_0: - *len = 1; - break; - case BMI2_AUX_READ_LEN_1: - *len = 2; - break; - case BMI2_AUX_READ_LEN_2: - *len = 6; - break; - case BMI2_AUX_READ_LEN_3: - *len = 8; - break; - default: - rslt = BMI2_E_AUX_INVALID_CFG; - break; - } - - return rslt; -} - -/*! - * @brief This internal API computes the number of bytes of accelerometer FIFO - * data which is to be parsed in header-less mode. - */ -static int8_t parse_fifo_accel_len(uint16_t *start_idx, - uint16_t *len, - const uint16_t *acc_count, - const struct bmi2_fifo_frame *fifo) -{ - /* Variable to define error */ - int8_t rslt = BMI2_OK; - - /* Data start index */ - (*start_idx) = fifo->acc_byte_start_idx; - - /* If only accelerometer is enabled */ - if (fifo->data_enable == BMI2_FIFO_ACC_EN) - { - /* Number of bytes to be read */ - (*len) = (uint16_t)((*acc_count) * BMI2_FIFO_ACC_LENGTH); - } - /* If only accelerometer and auxiliary are enabled */ - else if (fifo->data_enable == (BMI2_FIFO_ACC_EN | BMI2_FIFO_AUX_EN)) - { - /* Number of bytes to be read */ - (*len) = (uint16_t)((*acc_count) * BMI2_FIFO_ACC_AUX_LENGTH); - } - /* If only accelerometer and gyroscope are enabled */ - else if (fifo->data_enable == (BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN)) - { - /* Number of bytes to be read */ - (*len) = (uint16_t)((*acc_count) * BMI2_FIFO_ACC_GYR_LENGTH); - } - /* If only accelerometer, gyroscope and auxiliary are enabled */ - else if (fifo->data_enable == (BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN | BMI2_FIFO_AUX_EN)) - { - /* Number of bytes to be read */ - (*len) = (uint16_t)((*acc_count) * BMI2_FIFO_ALL_LENGTH); - } - else - { - /* Move the data index to the last byte to mark completion when - * no sensors or sensors apart from accelerometer are enabled - */ - (*start_idx) = fifo->length; - - /* FIFO is empty */ - rslt = BMI2_W_FIFO_EMPTY; - } - - /* If more data is requested than available */ - if ((*len) > fifo->length) - { - (*len) = fifo->length; - } - - return rslt; -} - -/*! - * @brief This internal API is used to parse the accelerometer data from the - * FIFO in header mode. - */ -static int8_t extract_accel_header_mode(struct bmi2_sens_axes_data *acc, - uint16_t *accel_length, - struct bmi2_fifo_frame *fifo, - const struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt = BMI2_OK; - - /* Variable to define header frame */ - uint8_t frame_header = 0; - - /* Variable to index the data bytes */ - uint16_t data_index; - - /* Variable to index accelerometer frames */ - uint16_t accel_index = 0; - - /* Variable to indicate accelerometer frames read */ - uint16_t frame_to_read = *accel_length; - - for (data_index = fifo->acc_byte_start_idx; data_index < fifo->length;) - { - /* Get frame header byte */ - frame_header = fifo->data[data_index] & BMI2_FIFO_TAG_INTR_MASK; - - /* Parse virtual header if S4S is enabled */ - parse_if_virtual_header(&frame_header, &data_index, fifo); - - /* Index shifted to next byte where data starts */ - data_index++; - switch (frame_header) - { - /* If header defines accelerometer frame */ - case BMI2_FIFO_HEADER_ACC_FRM: - case BMI2_FIFO_HEADER_AUX_ACC_FRM: - case BMI2_FIFO_HEADER_GYR_ACC_FRM: - case BMI2_FIFO_HEADER_ALL_FRM: - - /* Unpack from normal frames */ - rslt = unpack_accel_frame(acc, &data_index, &accel_index, frame_header, fifo, dev); - break; - - /* If header defines only gyroscope frame */ - case BMI2_FIFO_HEADER_GYR_FRM: - rslt = move_next_frame(&data_index, fifo->gyr_frm_len, fifo); - break; - - /* If header defines only auxiliary frame */ - case BMI2_FIFO_HEADER_AUX_FRM: - rslt = move_next_frame(&data_index, fifo->aux_frm_len, fifo); - break; - - /* If header defines only auxiliary and gyroscope frame */ - case BMI2_FIFO_HEADER_AUX_GYR_FRM: - rslt = move_next_frame(&data_index, fifo->aux_gyr_frm_len, fifo); - break; - - /* If header defines sensor time frame */ - case BMI2_FIFO_HEADER_SENS_TIME_FRM: - rslt = unpack_sensortime_frame(&data_index, fifo); - break; - - /* If header defines skip frame */ - case BMI2_FIFO_HEADER_SKIP_FRM: - rslt = unpack_skipped_frame(&data_index, fifo); - break; - - /* If header defines Input configuration frame */ - case BMI2_FIFO_HEADER_INPUT_CFG_FRM: - rslt = move_next_frame(&data_index, BMI2_FIFO_INPUT_CFG_LENGTH, fifo); - break; - - /* If header defines invalid frame or end of valid data */ - case BMI2_FIFO_HEAD_OVER_READ_MSB: - - /* Move the data index to the last byte to mark completion */ - data_index = fifo->length; - - /* FIFO is empty */ - rslt = BMI2_W_FIFO_EMPTY; - break; - case BMI2_FIFO_VIRT_ACT_RECOG_FRM: - rslt = move_next_frame(&data_index, BMI2_FIFO_VIRT_ACT_DATA_LENGTH, fifo); - break; - default: - - /* Move the data index to the last byte in case of invalid values */ - data_index = fifo->length; - - /* FIFO is empty */ - rslt = BMI2_W_FIFO_EMPTY; - break; - } - - /* Break if Number of frames to be read is complete or FIFO is mpty */ - if ((frame_to_read == accel_index) || (rslt == BMI2_W_FIFO_EMPTY)) - { - break; - } - } - - /* Update the accelerometer frame index */ - (*accel_length) = accel_index; - - /* Update the accelerometer byte index */ - fifo->acc_byte_start_idx = data_index; - - return rslt; -} - -/*! - * @brief This internal API is used to parse the accelerometer data from the - * FIFO data in both header and header-less mode. It updates the current data - * byte to be parsed. - */ -static int8_t unpack_accel_frame(struct bmi2_sens_axes_data *acc, - uint16_t *idx, - uint16_t *acc_idx, - uint8_t frame, - const struct bmi2_fifo_frame *fifo, - const struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt = BMI2_OK; - - switch (frame) - { - /* If frame contains only accelerometer data */ - case BMI2_FIFO_HEADER_ACC_FRM: - case BMI2_FIFO_HEAD_LESS_ACC_FRM: - - /* Partially read, then skip the data */ - if (((*idx) + fifo->acc_frm_len) > fifo->length) - { - /* Update the data index as complete*/ - (*idx) = fifo->length; - - /* FIFO is empty */ - rslt = BMI2_W_FIFO_EMPTY; - break; - } - - /* Get the accelerometer data */ - unpack_accel_data(&acc[(*acc_idx)], *idx, fifo, dev); - - /* Update data index */ - (*idx) = (*idx) + BMI2_FIFO_ACC_LENGTH; - - /* Get virtual sensor time if S4S is enabled */ - if (dev->sens_en_stat & BMI2_EXT_SENS_SEL) - { - unpack_virt_sensor_time(&acc[(*acc_idx)], idx, fifo); - } - - /* Update accelerometer frame index */ - (*acc_idx)++; - - /* More frames could be read */ - rslt = BMI2_W_PARTIAL_READ; - break; - - /* If frame contains accelerometer and gyroscope data */ - case BMI2_FIFO_HEADER_GYR_ACC_FRM: - case BMI2_FIFO_HEAD_LESS_GYR_ACC_FRM: - - /* Partially read, then skip the data */ - if (((*idx) + fifo->acc_gyr_frm_len) > fifo->length) - { - /* Move the data index to the last byte */ - (*idx) = fifo->length; - - /* FIFO is empty */ - rslt = BMI2_W_FIFO_EMPTY; - break; - } - - /* Get the accelerometer data */ - unpack_accel_data(&acc[(*acc_idx)], ((*idx) + BMI2_FIFO_GYR_LENGTH), fifo, dev); - - /* Update data index */ - (*idx) = (*idx) + BMI2_FIFO_ACC_GYR_LENGTH; - - /* Get virtual sensor time if S4S is enabled */ - if (dev->sens_en_stat & BMI2_EXT_SENS_SEL) - { - unpack_virt_sensor_time(&acc[(*acc_idx)], idx, fifo); - } - - /* Update accelerometer frame index */ - (*acc_idx)++; - - /* More frames could be read */ - rslt = BMI2_W_PARTIAL_READ; - break; - - /* If frame contains accelerometer and auxiliary data */ - case BMI2_FIFO_HEADER_AUX_ACC_FRM: - case BMI2_FIFO_HEAD_LESS_AUX_ACC_FRM: - - /* Partially read, then skip the data */ - if (((*idx) + fifo->acc_aux_frm_len) > fifo->length) - { - /* Move the data index to the last byte */ - (*idx) = fifo->length; - - /* FIFO is empty */ - rslt = BMI2_W_FIFO_EMPTY; - break; - } - - /* Get the accelerometer data */ - unpack_accel_data(&acc[(*acc_idx)], ((*idx) + BMI2_FIFO_AUX_LENGTH), fifo, dev); - - /* Update data index */ - (*idx) = (*idx) + BMI2_FIFO_ACC_AUX_LENGTH; - - /* Get virtual sensor time if S4S is enabled */ - if (dev->sens_en_stat & BMI2_EXT_SENS_SEL) - { - unpack_virt_sensor_time(&acc[(*acc_idx)], idx, fifo); - } - - /* Update accelerometer frame index */ - (*acc_idx)++; - - /* More frames could be read */ - rslt = BMI2_W_PARTIAL_READ; - break; - - /* If frame contains accelerometer, gyroscope and auxiliary data */ - case BMI2_FIFO_HEADER_ALL_FRM: - case BMI2_FIFO_HEAD_LESS_ALL_FRM: - - /* Partially read, then skip the data*/ - if ((*idx + fifo->all_frm_len) > fifo->length) - { - /* Move the data index to the last byte */ - (*idx) = fifo->length; - - /* FIFO is empty */ - rslt = BMI2_W_FIFO_EMPTY; - break; - } - - /* Get the accelerometer data */ - unpack_accel_data(&acc[(*acc_idx)], ((*idx) + BMI2_FIFO_GYR_AUX_LENGTH), fifo, dev); - - /* Update data index */ - (*idx) = (*idx) + BMI2_FIFO_ALL_LENGTH; - - /* Get virtual sensor time if S4S is enabled */ - if (dev->sens_en_stat & BMI2_EXT_SENS_SEL) - { - unpack_virt_sensor_time(&acc[(*acc_idx)], idx, fifo); - } - - /* Update accelerometer frame index */ - (*acc_idx)++; - - /* More frames could be read */ - rslt = BMI2_W_PARTIAL_READ; - break; - - /* If frame contains gyroscope and auxiliary data */ - case BMI2_FIFO_HEADER_AUX_GYR_FRM: - case BMI2_FIFO_HEAD_LESS_GYR_AUX_FRM: - - /* Update data index */ - (*idx) = (*idx) + fifo->aux_gyr_frm_len; - - /* More frames could be read */ - rslt = BMI2_W_PARTIAL_READ; - break; - - /* If frame contains only auxiliary data */ - case BMI2_FIFO_HEADER_AUX_FRM: - case BMI2_FIFO_HEAD_LESS_AUX_FRM: - - /* Update data index */ - (*idx) = (*idx) + fifo->aux_frm_len; - - /* More frames could be read */ - rslt = BMI2_W_PARTIAL_READ; - break; - - /* If frame contains only gyroscope data */ - case BMI2_FIFO_HEADER_GYR_FRM: - case BMI2_FIFO_HEAD_LESS_GYR_FRM: - - /* Update data index */ - (*idx) = (*idx) + fifo->gyr_frm_len; - - /* More frames could be read */ - rslt = BMI2_W_PARTIAL_READ; - break; - default: - - /* Move the data index to the last byte in case of invalid values */ - (*idx) = fifo->length; - - /* FIFO is empty */ - rslt = BMI2_W_FIFO_EMPTY; - break; - } - - return rslt; -} - -/*! - * @brief This internal API is used to parse accelerometer data from the - * FIFO data. - */ -static void unpack_accel_data(struct bmi2_sens_axes_data *acc, - uint16_t data_start_index, - const struct bmi2_fifo_frame *fifo, - const struct bmi2_dev *dev) -{ - /* Variables to store LSB value */ - uint16_t data_lsb; - - /* Variables to store MSB value */ - uint16_t data_msb; - - /* Accelerometer raw x data */ - data_lsb = fifo->data[data_start_index++]; - data_msb = fifo->data[data_start_index++]; - acc->x = (int16_t)((data_msb << 8) | data_lsb); - - /* Accelerometer raw y data */ - data_lsb = fifo->data[data_start_index++]; - data_msb = fifo->data[data_start_index++]; - acc->y = (int16_t)((data_msb << 8) | data_lsb); - - /* Accelerometer raw z data */ - data_lsb = fifo->data[data_start_index++]; - data_msb = fifo->data[data_start_index++]; - acc->z = (int16_t)((data_msb << 8) | data_lsb); - - /* Get the re-mapped accelerometer data */ - get_remapped_data(acc, dev); -} - -/*! - * @brief This internal API computes the number of bytes of gyroscope FIFO data - * which is to be parsed in header-less mode. - */ -static int8_t parse_fifo_gyro_len(uint16_t *start_idx, - uint16_t(*len), - const uint16_t *gyr_count, - const struct bmi2_fifo_frame *fifo) -{ - /* Variable to define error */ - int8_t rslt = BMI2_OK; - - /* Data start index */ - (*start_idx) = fifo->gyr_byte_start_idx; - - /* If only gyroscope is enabled */ - if (fifo->data_enable == BMI2_FIFO_GYR_EN) - { - /* Number of bytes to be read */ - (*len) = (uint16_t)((*gyr_count) * BMI2_FIFO_GYR_LENGTH); - } - /* If only gyroscope and auxiliary are enabled */ - else if (fifo->data_enable == (BMI2_FIFO_GYR_EN | BMI2_FIFO_AUX_EN)) - { - /* Number of bytes to be read */ - (*len) = (uint16_t)((*gyr_count) * BMI2_FIFO_GYR_AUX_LENGTH); - } - /* If only accelerometer and gyroscope are enabled */ - else if (fifo->data_enable == (BMI2_FIFO_ACC_EN | BMI2_FIFO_GYR_EN)) - { - /* Number of bytes to be read */ - (*len) = (uint16_t)((*gyr_count) * BMI2_FIFO_ACC_GYR_LENGTH); - } - /* If only accelerometer, gyroscope and auxiliary are enabled */ - else if (fifo->data_enable == (BMI2_FIFO_GYR_EN | BMI2_FIFO_AUX_EN | BMI2_FIFO_ACC_EN)) - { - /* Number of bytes to be read */ - (*len) = (uint16_t)((*gyr_count) * BMI2_FIFO_ALL_LENGTH); - } - else - { - /* Move the data index to the last byte to mark completion when - * no sensors or sensors apart from gyroscope are enabled - */ - (*start_idx) = fifo->length; - - /* FIFO is empty */ - rslt = BMI2_W_FIFO_EMPTY; - } - - /* If more data is requested than available */ - if (((*len)) > fifo->length) - { - (*len) = fifo->length; - } - - return rslt; -} - -/*! - * @brief This internal API is used to parse the gyroscope data from the - * FIFO data in header mode. - */ -static int8_t extract_gyro_header_mode(struct bmi2_sens_axes_data *gyr, - uint16_t *gyro_length, - struct bmi2_fifo_frame *fifo, - const struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt = BMI2_OK; - - /* Variable to define header frame */ - uint8_t frame_header = 0; - - /* Variable to index the data bytes */ - uint16_t data_index; - - /* Variable to index gyroscope frames */ - uint16_t gyro_index = 0; - - /* Variable to indicate gyroscope frames read */ - uint16_t frame_to_read = (*gyro_length); - - for (data_index = fifo->gyr_byte_start_idx; data_index < fifo->length;) - { - /* Get frame header byte */ - frame_header = fifo->data[data_index] & BMI2_FIFO_TAG_INTR_MASK; - - /* Parse virtual header if S4S is enabled */ - parse_if_virtual_header(&frame_header, &data_index, fifo); - - /* Index shifted to next byte where data starts */ - data_index++; - switch (frame_header) - { - /* If header defines gyroscope frame */ - case BMI2_FIFO_HEADER_GYR_FRM: - case BMI2_FIFO_HEADER_GYR_ACC_FRM: - case BMI2_FIFO_HEADER_AUX_GYR_FRM: - case BMI2_FIFO_HEADER_ALL_FRM: - - /* Unpack from normal frames */ - rslt = unpack_gyro_frame(gyr, &data_index, &gyro_index, frame_header, fifo, dev); - break; - - /* If header defines only accelerometer frame */ - case BMI2_FIFO_HEADER_ACC_FRM: - rslt = move_next_frame(&data_index, fifo->acc_frm_len, fifo); - break; - - /* If header defines only auxiliary frame */ - case BMI2_FIFO_HEADER_AUX_FRM: - rslt = move_next_frame(&data_index, fifo->aux_frm_len, fifo); - break; - - /* If header defines only auxiliary and accelerometer frame */ - case BMI2_FIFO_HEADER_AUX_ACC_FRM: - rslt = move_next_frame(&data_index, fifo->acc_aux_frm_len, fifo); - break; - - /* If header defines sensor time frame */ - case BMI2_FIFO_HEADER_SENS_TIME_FRM: - rslt = unpack_sensortime_frame(&data_index, fifo); - break; - - /* If header defines skip frame */ - case BMI2_FIFO_HEADER_SKIP_FRM: - rslt = unpack_skipped_frame(&data_index, fifo); - break; - - /* If header defines Input configuration frame */ - case BMI2_FIFO_HEADER_INPUT_CFG_FRM: - rslt = move_next_frame(&data_index, BMI2_FIFO_INPUT_CFG_LENGTH, fifo); - break; - - /* If header defines invalid frame or end of valid data */ - case BMI2_FIFO_HEAD_OVER_READ_MSB: - - /* Move the data index to the last byte */ - data_index = fifo->length; - - /* FIFO is empty */ - rslt = BMI2_W_FIFO_EMPTY; - break; - case BMI2_FIFO_VIRT_ACT_RECOG_FRM: - rslt = move_next_frame(&data_index, BMI2_FIFO_VIRT_ACT_DATA_LENGTH, fifo); - break; - default: - - /* Move the data index to the last byte in case of invalid values */ - data_index = fifo->length; - - /* FIFO is empty */ - rslt = BMI2_W_FIFO_EMPTY; - break; - } - - /* Break if number of frames to be read is complete or FIFO is empty */ - if ((frame_to_read == gyro_index) || (rslt == BMI2_W_FIFO_EMPTY)) - { - break; - } - } - - /* Update the gyroscope frame index */ - (*gyro_length) = gyro_index; - - /* Update the gyroscope byte index */ - fifo->gyr_byte_start_idx = data_index; - - return rslt; -} - -/*! - * @brief This internal API is used to parse the gyroscope data from the FIFO - * data in both header and header-less mode. It updates the current data byte to - * be parsed. - */ -static int8_t unpack_gyro_frame(struct bmi2_sens_axes_data *gyr, - uint16_t *idx, - uint16_t *gyr_idx, - uint8_t frame, - const struct bmi2_fifo_frame *fifo, - const struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt = BMI2_OK; - - switch (frame) - { - /* If frame contains only gyroscope data */ - case BMI2_FIFO_HEADER_GYR_FRM: - case BMI2_FIFO_HEAD_LESS_GYR_FRM: - - /* Partially read, then skip the data */ - if (((*idx) + fifo->gyr_frm_len) > fifo->length) - { - /* Update the data index as complete*/ - (*idx) = fifo->length; - - /* FIFO is empty */ - rslt = BMI2_W_FIFO_EMPTY; - break; - } - - /* Get the gyroscope data */ - unpack_gyro_data(&gyr[(*gyr_idx)], *idx, fifo, dev); - - /* Update data index */ - (*idx) = (*idx) + BMI2_FIFO_GYR_LENGTH; - - /* Get virtual sensor time if S4S is enabled */ - if (dev->sens_en_stat & BMI2_EXT_SENS_SEL) - { - unpack_virt_sensor_time(&gyr[(*gyr_idx)], idx, fifo); - } - - /* Update gyroscope frame index */ - (*gyr_idx)++; - - /* More frames could be read */ - rslt = BMI2_W_PARTIAL_READ; - break; - - /* If frame contains accelerometer and gyroscope data */ - case BMI2_FIFO_HEADER_GYR_ACC_FRM: - case BMI2_FIFO_HEAD_LESS_GYR_ACC_FRM: - - /* Partially read, then skip the data */ - if (((*idx) + fifo->acc_gyr_frm_len) > fifo->length) - { - /* Move the data index to the last byte */ - (*idx) = fifo->length; - - /* FIFO is empty */ - rslt = BMI2_W_FIFO_EMPTY; - break; - } - - /* Get the gyroscope data */ - unpack_gyro_data(&gyr[(*gyr_idx)], (*idx), fifo, dev); - - /* Update data index */ - (*idx) = (*idx) + BMI2_FIFO_ACC_GYR_LENGTH; - - /* Get virtual sensor time if S4S is enabled */ - if (dev->sens_en_stat & BMI2_EXT_SENS_SEL) - { - unpack_virt_sensor_time(&gyr[(*gyr_idx)], idx, fifo); - } - - /* Update gyroscope frame index */ - (*gyr_idx)++; - - /* More frames could be read */ - rslt = BMI2_W_PARTIAL_READ; - break; - - /* If frame contains gyroscope and auxiliary data */ - case BMI2_FIFO_HEADER_AUX_GYR_FRM: - case BMI2_FIFO_HEAD_LESS_GYR_AUX_FRM: - - /* Partially read, then skip the data */ - if (((*idx) + fifo->aux_gyr_frm_len) > fifo->length) - { - /* Move the data index to the last byte */ - (*idx) = fifo->length; - - /* FIFO is empty */ - rslt = BMI2_W_FIFO_EMPTY; - break; - } - - /* Get the gyroscope data */ - unpack_gyro_data(&gyr[(*gyr_idx)], ((*idx) + BMI2_FIFO_AUX_LENGTH), fifo, dev); - - /* Update data index */ - (*idx) = (*idx) + BMI2_FIFO_GYR_AUX_LENGTH; - - /* Get virtual sensor time if S4S is enabled */ - if (dev->sens_en_stat & BMI2_EXT_SENS_SEL) - { - unpack_virt_sensor_time(&gyr[(*gyr_idx)], idx, fifo); - } - - /* Update gyroscope frame index */ - (*gyr_idx)++; - - /* More frames could be read */ - rslt = BMI2_W_PARTIAL_READ; - break; - - /* If frame contains accelerometer, gyroscope and auxiliary data */ - case BMI2_FIFO_HEADER_ALL_FRM: - case BMI2_FIFO_HEAD_LESS_ALL_FRM: - - /* Partially read, then skip the data*/ - if ((*idx + fifo->all_frm_len) > fifo->length) - { - /* Move the data index to the last byte */ - (*idx) = fifo->length; - - /* FIFO is empty */ - rslt = BMI2_W_FIFO_EMPTY; - break; - } - - /* Get the gyroscope data */ - unpack_gyro_data(&gyr[(*gyr_idx)], ((*idx) + BMI2_FIFO_AUX_LENGTH), fifo, dev); - - /* Update data index */ - (*idx) = (*idx) + BMI2_FIFO_ALL_LENGTH; - - /* Get virtual sensor time if S4S is enabled */ - if (dev->sens_en_stat & BMI2_EXT_SENS_SEL) - { - unpack_virt_sensor_time(&gyr[(*gyr_idx)], idx, fifo); - } - - /* Update gyroscope frame index */ - (*gyr_idx)++; - - /* More frames could be read */ - rslt = BMI2_W_PARTIAL_READ; - break; - - /* If frame contains accelerometer and auxiliary data */ - case BMI2_FIFO_HEADER_AUX_ACC_FRM: - case BMI2_FIFO_HEAD_LESS_AUX_ACC_FRM: - - /* Update data index */ - (*idx) = (*idx) + fifo->acc_aux_frm_len; - - /* More frames could be read */ - rslt = BMI2_W_PARTIAL_READ; - break; - - /* If frame contains only auxiliary data */ - case BMI2_FIFO_HEADER_AUX_FRM: - case BMI2_FIFO_HEAD_LESS_AUX_FRM: - - /* Update data index */ - (*idx) = (*idx) + fifo->aux_frm_len; - - /* More frames could be read */ - rslt = BMI2_W_PARTIAL_READ; - break; - - /* If frame contains only accelerometer data */ - case BMI2_FIFO_HEADER_ACC_FRM: - case BMI2_FIFO_HEAD_LESS_ACC_FRM: - - /* Update data index */ - (*idx) = (*idx) + fifo->acc_frm_len; - - /* More frames could be read */ - rslt = BMI2_W_PARTIAL_READ; - break; - default: - - /* Move the data index to the last byte in case of invalid values */ - (*idx) = fifo->length; - - /* FIFO is empty */ - rslt = BMI2_W_FIFO_EMPTY; - break; - } - - return rslt; -} - -/*! - * @brief This internal API is used to parse gyroscope data from the FIFO data. - */ -static void unpack_gyro_data(struct bmi2_sens_axes_data *gyr, - uint16_t data_start_index, - const struct bmi2_fifo_frame *fifo, - const struct bmi2_dev *dev) -{ - /* Variables to store LSB value */ - uint16_t data_lsb; - - /* Variables to store MSB value */ - uint16_t data_msb; - - /* Gyroscope raw x data */ - data_lsb = fifo->data[data_start_index++]; - data_msb = fifo->data[data_start_index++]; - gyr->x = (int16_t)((data_msb << 8) | data_lsb); - - /* Gyroscope raw y data */ - data_lsb = fifo->data[data_start_index++]; - data_msb = fifo->data[data_start_index++]; - gyr->y = (int16_t)((data_msb << 8) | data_lsb); - - /* Gyroscope raw z data */ - data_lsb = fifo->data[data_start_index++]; - data_msb = fifo->data[data_start_index++]; - gyr->z = (int16_t)((data_msb << 8) | data_lsb); - - /* Get the compensated gyroscope data */ - comp_gyro_cross_axis_sensitivity(gyr, dev); - - /* Get the re-mapped gyroscope data */ - get_remapped_data(gyr, dev); -} - -/*! - * @brief This API computes the number of bytes of auxiliary FIFO data which is - * to be parsed in header-less mode. - */ -static int8_t parse_fifo_aux_len(uint16_t *start_idx, - uint16_t(*len), - const uint16_t *aux_count, - const struct bmi2_fifo_frame *fifo) -{ - /* Variable to define error */ - int8_t rslt = BMI2_OK; - - /* Data start index */ - *start_idx = fifo->aux_byte_start_idx; - - /* If only auxiliary is enabled */ - if (fifo->data_enable == BMI2_FIFO_AUX_EN) - { - /* Number of bytes to be read */ - (*len) = (uint16_t)((*aux_count) * BMI2_FIFO_AUX_LENGTH); - } - /* If only accelerometer and auxiliary are enabled */ - else if (fifo->data_enable == (BMI2_FIFO_AUX_EN | BMI2_FIFO_ACC_EN)) - { - /* Number of bytes to be read */ - (*len) = (uint16_t)((*aux_count) * BMI2_FIFO_ACC_AUX_LENGTH); - } - /* If only accelerometer and gyroscope are enabled */ - else if (fifo->data_enable == (BMI2_FIFO_AUX_EN | BMI2_FIFO_GYR_EN)) - { - /* Number of bytes to be read */ - (*len) = (uint16_t)((*aux_count) * BMI2_FIFO_GYR_AUX_LENGTH); - } - /* If only accelerometer, gyroscope and auxiliary are enabled */ - else if (fifo->data_enable == (BMI2_FIFO_AUX_EN | BMI2_FIFO_GYR_EN | BMI2_FIFO_ACC_EN)) - { - /* Number of bytes to be read */ - (*len) = (uint16_t)((*aux_count) * BMI2_FIFO_ALL_LENGTH); - } - else - { - /* Move the data index to the last byte to mark completion when - * no sensors or sensors apart from gyroscope are enabled - */ - (*start_idx) = fifo->length; - - /* FIFO is empty */ - rslt = BMI2_W_FIFO_EMPTY; - } - - /* If more data is requested than available */ - if (((*len)) > fifo->length) - { - (*len) = fifo->length; - } - - return rslt; -} - -/*! - * @brief This API is used to parse the auxiliary data from the FIFO data in - * header mode. - */ -static int8_t extract_aux_header_mode(struct bmi2_aux_fifo_data *aux, - uint16_t *aux_len, - struct bmi2_fifo_frame *fifo, - const struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt = BMI2_OK; - - /* Variable to define header frame */ - uint8_t frame_header = 0; - - /* Variable to index the data bytes */ - uint16_t data_index; - - /* Variable to index gyroscope frames */ - uint16_t aux_index = 0; - - /* Variable to indicate auxiliary frames read */ - uint16_t frame_to_read = *aux_len; - - for (data_index = fifo->aux_byte_start_idx; data_index < fifo->length;) - { - /* Get frame header byte */ - frame_header = fifo->data[data_index] & BMI2_FIFO_TAG_INTR_MASK; - - /* Parse virtual header if S4S is enabled */ - parse_if_virtual_header(&frame_header, &data_index, fifo); - - /* Index shifted to next byte where data starts */ - data_index++; - switch (frame_header) - { - /* If header defines auxiliary frame */ - case BMI2_FIFO_HEADER_AUX_FRM: - case BMI2_FIFO_HEADER_AUX_ACC_FRM: - case BMI2_FIFO_HEADER_AUX_GYR_FRM: - case BMI2_FIFO_HEADER_ALL_FRM: - - /* Unpack from normal frames */ - rslt = unpack_aux_frame(aux, &data_index, &aux_index, frame_header, fifo, dev); - break; - - /* If header defines only accelerometer frame */ - case BMI2_FIFO_HEADER_ACC_FRM: - rslt = move_next_frame(&data_index, fifo->acc_frm_len, fifo); - break; - - /* If header defines only gyroscope frame */ - case BMI2_FIFO_HEADER_GYR_FRM: - rslt = move_next_frame(&data_index, fifo->gyr_frm_len, fifo); - break; - - /* If header defines only gyroscope and accelerometer frame */ - case BMI2_FIFO_HEADER_GYR_ACC_FRM: - rslt = move_next_frame(&data_index, fifo->acc_gyr_frm_len, fifo); - break; - - /* If header defines sensor time frame */ - case BMI2_FIFO_HEADER_SENS_TIME_FRM: - rslt = unpack_sensortime_frame(&data_index, fifo); - break; - - /* If header defines skip frame */ - case BMI2_FIFO_HEADER_SKIP_FRM: - rslt = unpack_skipped_frame(&data_index, fifo); - break; - - /* If header defines Input configuration frame */ - case BMI2_FIFO_HEADER_INPUT_CFG_FRM: - rslt = move_next_frame(&data_index, BMI2_FIFO_INPUT_CFG_LENGTH, fifo); - break; - - /* If header defines invalid frame or end of valid data */ - case BMI2_FIFO_HEAD_OVER_READ_MSB: - - /* Move the data index to the last byte */ - data_index = fifo->length; - - /* FIFO is empty */ - rslt = BMI2_W_FIFO_EMPTY; - break; - case BMI2_FIFO_VIRT_ACT_RECOG_FRM: - rslt = move_next_frame(&data_index, BMI2_FIFO_VIRT_ACT_DATA_LENGTH, fifo); - break; - default: - - /* Move the data index to the last byte in case - * of invalid values - */ - data_index = fifo->length; - - /* FIFO is empty */ - rslt = BMI2_W_FIFO_EMPTY; - break; - } - - /* Break if number of frames to be read is complete or FIFO is - * empty - */ - if ((frame_to_read == aux_index) || (rslt == BMI2_W_FIFO_EMPTY)) - { - break; - } - } - - /* Update the gyroscope frame index */ - (*aux_len) = aux_index; - - /* Update the gyroscope byte index */ - fifo->aux_byte_start_idx = data_index; - - return rslt; -} - -/*! - * @brief This API is used to parse the auxiliary frame from the FIFO data in - * both header mode and header-less mode and update the data_index value which - * is used to store the index of the current data byte which is parsed. - */ -static int8_t unpack_aux_frame(struct bmi2_aux_fifo_data *aux, - uint16_t *idx, - uint16_t *aux_idx, - uint8_t frame, - const struct bmi2_fifo_frame *fifo, - const struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt = BMI2_OK; - - switch (frame) - { - /* If frame contains only auxiliary data */ - case BMI2_FIFO_HEADER_AUX_FRM: - case BMI2_FIFO_HEAD_LESS_AUX_FRM: - - /* Partially read, then skip the data */ - if (((*idx) + fifo->aux_frm_len) > fifo->length) - { - /* Update the data index as complete*/ - (*idx) = fifo->length; - - /* FIFO is empty */ - rslt = BMI2_W_FIFO_EMPTY; - break; - } - - /* Get the auxiliary data */ - unpack_aux_data(&aux[(*aux_idx)], (*idx), fifo); - - /* Update data index */ - (*idx) = (*idx) + BMI2_FIFO_AUX_LENGTH; - - /* Get virtual sensor time if S4S is enabled */ - if (dev->sens_en_stat & BMI2_EXT_SENS_SEL) - { - unpack_virt_aux_sensor_time(&aux[(*aux_idx)], idx, fifo); - } - - /* Update auxiliary frame index */ - (*aux_idx)++; - - /* More frames could be read */ - rslt = BMI2_W_PARTIAL_READ; - break; - - /* If frame contains accelerometer and auxiliary data */ - case BMI2_FIFO_HEADER_AUX_ACC_FRM: - case BMI2_FIFO_HEAD_LESS_AUX_ACC_FRM: - - /* Partially read, then skip the data */ - if (((*idx) + fifo->acc_aux_frm_len) > fifo->length) - { - /* Move the data index to the last byte */ - (*idx) = fifo->length; - - /* FIFO is empty */ - rslt = BMI2_W_FIFO_EMPTY; - break; - } - - /* Get the auxiliary data */ - unpack_aux_data(&aux[(*aux_idx)], (*idx), fifo); - - /* Update data index */ - (*idx) = (*idx) + BMI2_FIFO_ACC_AUX_LENGTH; - - /* Get virtual sensor time if S4S is enabled */ - if (dev->sens_en_stat & BMI2_EXT_SENS_SEL) - { - unpack_virt_aux_sensor_time(&aux[(*aux_idx)], idx, fifo); - } - - /* Update auxiliary frame index */ - (*aux_idx)++; - - /* More frames could be read */ - rslt = BMI2_W_PARTIAL_READ; - break; - - /* If frame contains gyroscope and auxiliary data */ - case BMI2_FIFO_HEADER_AUX_GYR_FRM: - case BMI2_FIFO_HEAD_LESS_GYR_AUX_FRM: - - /* Partially read, then skip the data */ - if (((*idx) + fifo->aux_gyr_frm_len) > fifo->length) - { - /* Move the data index to the last byte */ - (*idx) = fifo->length; - - /* FIFO is empty */ - rslt = BMI2_W_FIFO_EMPTY; - break; - } - - /* Get the auxiliary data */ - unpack_aux_data(&aux[(*aux_idx)], (*idx), fifo); - - /* Update data index */ - (*idx) = (*idx) + BMI2_FIFO_GYR_AUX_LENGTH; - - /* Get virtual sensor time if S4S is enabled */ - if (dev->sens_en_stat & BMI2_EXT_SENS_SEL) - { - unpack_virt_aux_sensor_time(&aux[(*aux_idx)], idx, fifo); - } - - /* Update auxiliary frame index */ - (*aux_idx)++; - - /* More frames could be read */ - rslt = BMI2_W_PARTIAL_READ; - break; - - /* If frame contains accelerometer, gyroscope and auxiliary data */ - case BMI2_FIFO_HEADER_ALL_FRM: - case BMI2_FIFO_HEAD_LESS_ALL_FRM: - - /* Partially read, then skip the data */ - if ((*idx + fifo->all_frm_len) > fifo->length) - { - /* Move the data index to the last byte */ - (*idx) = fifo->length; - - /* FIFO is empty */ - rslt = BMI2_W_FIFO_EMPTY; - break; - } - - /* Get the auxiliary data */ - unpack_aux_data(&aux[(*aux_idx)], (*idx), fifo); - - /* Update data index */ - (*idx) = (*idx) + BMI2_FIFO_ALL_LENGTH; - - /* Get virtual sensor time if S4S is enabled */ - if (dev->sens_en_stat & BMI2_EXT_SENS_SEL) - { - unpack_virt_aux_sensor_time(&aux[(*aux_idx)], idx, fifo); - } - - /* Update auxiliary frame index */ - (*aux_idx)++; - - /* More frames could be read */ - rslt = BMI2_W_PARTIAL_READ; - break; - - /* If frame contains only accelerometer data */ - case BMI2_FIFO_HEADER_ACC_FRM: - case BMI2_FIFO_HEAD_LESS_ACC_FRM: - - /* Update data index */ - (*idx) = (*idx) + fifo->acc_frm_len; - - /* More frames could be read */ - rslt = BMI2_W_PARTIAL_READ; - break; - - /* If frame contains only gyroscope data */ - case BMI2_FIFO_HEADER_GYR_FRM: - case BMI2_FIFO_HEAD_LESS_GYR_FRM: - - /* Update data index */ - (*idx) = (*idx) + fifo->gyr_frm_len; - - /* More frames could be read */ - rslt = BMI2_W_PARTIAL_READ; - break; - - /* If frame contains accelerometer and gyroscope data */ - case BMI2_FIFO_HEADER_GYR_ACC_FRM: - case BMI2_FIFO_HEAD_LESS_GYR_ACC_FRM: - - /* Update data index */ - (*idx) = (*idx) + fifo->acc_gyr_frm_len; - - /* More frames could be read */ - rslt = BMI2_W_PARTIAL_READ; - break; - default: - - /* Move the data index to the last byte in case of - * invalid values - */ - (*idx) = fifo->length; - - /* FIFO is empty */ - rslt = BMI2_W_FIFO_EMPTY; - break; - } - - return rslt; -} - -/*! - * @brief This internal API is used to parse auxiliary data from the FIFO data. - */ -static void unpack_aux_data(struct bmi2_aux_fifo_data *aux, - uint16_t data_start_index, - const struct bmi2_fifo_frame *fifo) -{ - /* Variables to store index */ - uint16_t idx = 0; - - /* Get auxiliary data */ - for (; idx < 8; idx++) - { - aux->data[idx] = fifo->data[data_start_index++]; - } -} - -/*! - * @brief This internal API parses virtual frame header from the FIFO data. - */ -static void parse_if_virtual_header(uint8_t *frame_header, uint16_t *data_index, const struct bmi2_fifo_frame *fifo) -{ - /* Variable to extract virtual header byte */ - uint8_t virtual_header_mode; - - /* Extract virtual header mode from the frame header */ - virtual_header_mode = BMI2_GET_BITS(*frame_header, BMI2_FIFO_VIRT_FRM_MODE); - - /* If the extracted header byte is a virtual header */ - if (virtual_header_mode == BMI2_FIFO_VIRT_FRM_MODE) - { - /* If frame header is not activity recognition header */ - if (*frame_header != 0xC8) - { - /* Index shifted to next byte where sensor frame is present */ - (*data_index) = (*data_index) + 1; - - /* Get the sensor frame header */ - *frame_header = fifo->data[*data_index] & BMI2_FIFO_TAG_INTR_MASK; - } - } -} - -/*! - * @brief This internal API gets sensor time from the accelerometer and - * gyroscope virtual frames and updates in the data structure. - */ -static void unpack_virt_sensor_time(struct bmi2_sens_axes_data *sens, uint16_t *idx, const struct bmi2_fifo_frame *fifo) -{ - /* Variables to define 3 bytes of sensor time */ - uint32_t sensor_time_byte3; - uint16_t sensor_time_byte2; - uint8_t sensor_time_byte1; - - /* Get sensor time from the FIFO data */ - sensor_time_byte3 = (uint32_t)(fifo->data[(*idx) + BMI2_SENSOR_TIME_MSB_BYTE] << 16); - sensor_time_byte2 = (uint16_t) fifo->data[(*idx) + BMI2_SENSOR_TIME_XLSB_BYTE] << 8; - sensor_time_byte1 = fifo->data[(*idx)]; - - /* Store sensor time in the sensor data structure */ - sens->virt_sens_time = (uint32_t)(sensor_time_byte3 | sensor_time_byte2 | sensor_time_byte1); - - /* Move the data index by 3 bytes */ - (*idx) = (*idx) + BMI2_SENSOR_TIME_LENGTH; -} - -/*! - * @brief This internal API gets sensor time from the auxiliary virtual - * frames and updates in the data structure. - */ -static void unpack_virt_aux_sensor_time(struct bmi2_aux_fifo_data *aux, - uint16_t *idx, - const struct bmi2_fifo_frame *fifo) -{ - /* Variables to define 3 bytes of sensor time */ - uint32_t sensor_time_byte3; - uint16_t sensor_time_byte2; - uint8_t sensor_time_byte1; - - /* Get sensor time from the FIFO data */ - sensor_time_byte3 = (uint32_t)(fifo->data[(*idx) + BMI2_SENSOR_TIME_MSB_BYTE] << 16); - sensor_time_byte2 = (uint16_t) fifo->data[(*idx) + BMI2_SENSOR_TIME_XLSB_BYTE] << 8; - sensor_time_byte1 = fifo->data[(*idx)]; - - /* Store sensor time in the sensor data structure */ - aux->virt_sens_time = (uint32_t)(sensor_time_byte3 | sensor_time_byte2 | sensor_time_byte1); - - /* Move the data index by 3 bytes */ - (*idx) = (*idx) + BMI2_SENSOR_TIME_LENGTH; -} - -/*! - * @brief This internal API is used to reset the FIFO related configurations in - * the FIFO frame structure for the next FIFO read. - */ -static void reset_fifo_frame_structure(struct bmi2_fifo_frame *fifo, const struct bmi2_dev *dev) -{ - /* Reset FIFO data structure */ - fifo->acc_byte_start_idx = 0; - fifo->aux_byte_start_idx = 0; - fifo->gyr_byte_start_idx = 0; - fifo->sensor_time = 0; - fifo->skipped_frame_count = 0; - fifo->act_recog_byte_start_idx = 0; - - /* If S4S is enabled */ - if ((dev->sens_en_stat & BMI2_EXT_SENS_SEL) == BMI2_EXT_SENS_SEL) - { - fifo->acc_frm_len = BMI2_FIFO_VIRT_ACC_LENGTH; - fifo->gyr_frm_len = BMI2_FIFO_VIRT_GYR_LENGTH; - fifo->aux_frm_len = BMI2_FIFO_VIRT_AUX_LENGTH; - fifo->acc_gyr_frm_len = BMI2_FIFO_VIRT_ACC_GYR_LENGTH; - fifo->acc_aux_frm_len = BMI2_FIFO_VIRT_ACC_AUX_LENGTH; - fifo->aux_gyr_frm_len = BMI2_FIFO_VIRT_GYR_AUX_LENGTH; - fifo->all_frm_len = BMI2_FIFO_VIRT_ALL_LENGTH; - - /* If S4S is not enabled */ - } - else - { - fifo->acc_frm_len = BMI2_FIFO_ACC_LENGTH; - fifo->gyr_frm_len = BMI2_FIFO_GYR_LENGTH; - fifo->aux_frm_len = BMI2_FIFO_AUX_LENGTH; - fifo->acc_gyr_frm_len = BMI2_FIFO_ACC_GYR_LENGTH; - fifo->acc_aux_frm_len = BMI2_FIFO_ACC_AUX_LENGTH; - fifo->aux_gyr_frm_len = BMI2_FIFO_GYR_AUX_LENGTH; - fifo->all_frm_len = BMI2_FIFO_ALL_LENGTH; - } -} - -/*! - * @brief This API internal checks whether the FIFO data read is an empty frame. - * If empty frame, index is moved to the last byte. - */ -static int8_t check_empty_fifo(uint16_t *data_index, const struct bmi2_fifo_frame *fifo) -{ - /* Variables to define error */ - int8_t rslt = BMI2_OK; - - /* Validate data index */ - if (((*data_index) + 6) < fifo->length) - { - /* Check if FIFO is empty */ - if (((fifo->data[(*data_index)] == BMI2_FIFO_MSB_CONFIG_CHECK) && - (fifo->data[(*data_index) + 1] == BMI2_FIFO_LSB_CONFIG_CHECK)) && - ((fifo->data[(*data_index) + 2] == BMI2_FIFO_MSB_CONFIG_CHECK) && - (fifo->data[(*data_index) + 3] == BMI2_FIFO_LSB_CONFIG_CHECK)) && - ((fifo->data[(*data_index) + 4] == BMI2_FIFO_MSB_CONFIG_CHECK) && - (fifo->data[(*data_index) + 5] == BMI2_FIFO_LSB_CONFIG_CHECK))) - { - /* Move the data index to the last byte to mark completion */ - (*data_index) = fifo->length; - - /* FIFO is empty */ - rslt = BMI2_W_FIFO_EMPTY; - } - else - { - /* More frames could be read */ - rslt = BMI2_W_PARTIAL_READ; - } - } - - return rslt; -} - -/*! - * @brief This internal API is used to move the data index ahead of the - * current_frame_length parameter when unnecessary FIFO data appears while - * extracting the user specified data. - */ -static int8_t move_next_frame(uint16_t *data_index, uint8_t current_frame_length, const struct bmi2_fifo_frame *fifo) -{ - /* Variables to define error */ - int8_t rslt = BMI2_OK; - - /* Validate data index */ - if (((*data_index) + current_frame_length) > fifo->length) - { - /* Move the data index to the last byte */ - (*data_index) = fifo->length; - - /* FIFO is empty */ - rslt = BMI2_W_FIFO_EMPTY; - } - else - { - /* Move the data index to next frame */ - (*data_index) = (*data_index) + current_frame_length; - - /* More frames could be read */ - rslt = BMI2_W_PARTIAL_READ; - } - - return rslt; -} - -/*! - * @brief This internal API is used to parse and store the sensor time from the - * FIFO data. - */ -static int8_t unpack_sensortime_frame(uint16_t *data_index, struct bmi2_fifo_frame *fifo) -{ - /* Variables to define error */ - int8_t rslt = BMI2_OK; - - /* Variables to define 3 bytes of sensor time */ - uint32_t sensor_time_byte3 = 0; - uint16_t sensor_time_byte2 = 0; - uint8_t sensor_time_byte1 = 0; - - /* Validate data index */ - if (((*data_index) + BMI2_SENSOR_TIME_LENGTH) > fifo->length) - { - /* Move the data index to the last byte */ - (*data_index) = fifo->length; - - /* FIFO is empty */ - rslt = BMI2_W_FIFO_EMPTY; - } - else - { - /* Get sensor time from the FIFO data */ - sensor_time_byte3 = fifo->data[(*data_index) + BMI2_SENSOR_TIME_MSB_BYTE] << 16; - sensor_time_byte2 = fifo->data[(*data_index) + BMI2_SENSOR_TIME_XLSB_BYTE] << 8; - sensor_time_byte1 = fifo->data[(*data_index)]; - - /* Update sensor time in the FIFO structure */ - fifo->sensor_time = (uint32_t)(sensor_time_byte3 | sensor_time_byte2 | sensor_time_byte1); - - /* Move the data index by 3 bytes */ - (*data_index) = (*data_index) + BMI2_SENSOR_TIME_LENGTH; - - /* More frames could be read */ - rslt = BMI2_W_PARTIAL_READ; - } - - return rslt; -} - -/*! - * @brief This internal API is used to parse and store the skipped frame count - * from the FIFO data. - */ -static int8_t unpack_skipped_frame(uint16_t *data_index, struct bmi2_fifo_frame *fifo) -{ - /* Variables to define error */ - int8_t rslt = BMI2_OK; - - /* Validate data index */ - if ((*data_index) >= fifo->length) - { - /* Update the data index to the last byte */ - (*data_index) = fifo->length; - - /* FIFO is empty */ - rslt = BMI2_W_FIFO_EMPTY; - } - else - { - /* Update skipped frame count in the FIFO structure */ - fifo->skipped_frame_count = fifo->data[(*data_index)]; - - /* Move the data index by 1 byte */ - (*data_index) = (*data_index) + 1; - - /* More frames could be read */ - rslt = BMI2_W_PARTIAL_READ; - } - - return rslt; -} - -/*! - * @brief This internal API enables and configures the accelerometer which is - * needed for self-test operation. It also sets the amplitude for the self-test. - */ -static int8_t pre_self_test_config(struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Structure to define sensor configurations */ - struct bmi2_sens_config sens_cfg; - - /* List the sensors to be selected */ - uint8_t sens_sel = BMI2_ACCEL; - - /* Enable accelerometer */ - rslt = bmi2_sensor_enable(&sens_sel, 1, dev); - dev->delay_us(1000, dev->intf_ptr); - - /* Enable self-test amplitude */ - if (rslt == BMI2_OK) - { - rslt = set_accel_self_test_amp(BMI2_ENABLE, dev); - } - - if (rslt == BMI2_OK) - { - /* Select accelerometer for sensor configurations */ - sens_cfg.type = BMI2_ACCEL; - - /* Get the default values */ - rslt = bmi2_get_sensor_config(&sens_cfg, 1, dev); - if (rslt == BMI2_OK) - { - /* Set the configurations required for self-test */ - sens_cfg.cfg.acc.odr = BMI2_ACC_ODR_1600HZ; - sens_cfg.cfg.acc.bwp = BMI2_ACC_NORMAL_AVG4; - sens_cfg.cfg.acc.filter_perf = BMI2_PERF_OPT_MODE; - sens_cfg.cfg.acc.range = BMI2_ACC_RANGE_16G; - - /* Set accelerometer configurations */ - rslt = bmi2_set_sensor_config(&sens_cfg, 1, dev); - } - } - - return rslt; -} - -/*! - * @brief This internal API performs the steps needed for self-test operation - * before reading the accelerometer self-test data. - */ -static int8_t self_test_config(uint8_t sign, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Enable the accelerometer self-test feature */ - rslt = set_accel_self_test_enable(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - /* Selects the sign of accelerometer self-test excitation */ - rslt = set_acc_self_test_sign(sign, dev); - } - - return rslt; -} - -/*! - * @brief This internal API enables or disables the accelerometer self-test - * feature in the sensor. - */ -static int8_t set_accel_self_test_enable(uint8_t enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define data */ - uint8_t data = 0; - - /* Enable/Disable self-test feature */ - rslt = bmi2_get_regs(BMI2_ACC_SELF_TEST_ADDR, &data, 1, dev); - if (rslt == BMI2_OK) - { - data = BMI2_SET_BIT_POS0(data, BMI2_ACC_SELF_TEST_EN, enable); - rslt = bmi2_set_regs(BMI2_ACC_SELF_TEST_ADDR, &data, 1, dev); - } - - return rslt; -} - -/*! - * @brief This internal API selects the sign for accelerometer self-test - * excitation. - */ -static int8_t set_acc_self_test_sign(uint8_t sign, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define data */ - uint8_t data = 0; - - /* Select the sign for self-test excitation */ - rslt = bmi2_get_regs(BMI2_ACC_SELF_TEST_ADDR, &data, 1, dev); - if (rslt == BMI2_OK) - { - data = BMI2_SET_BITS(data, BMI2_ACC_SELF_TEST_SIGN, sign); - rslt = bmi2_set_regs(BMI2_ACC_SELF_TEST_ADDR, &data, 1, dev); - } - - return rslt; -} - -/*! - * @brief This internal API sets the amplitude of the accelerometer self-test - * deflection in the sensor. - */ -static int8_t set_accel_self_test_amp(uint8_t amp, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define data */ - uint8_t data = 0; - - /* Select amplitude of the self-test deflection */ - rslt = bmi2_get_regs(BMI2_ACC_SELF_TEST_ADDR, &data, 1, dev); - if (rslt == BMI2_OK) - { - data = BMI2_SET_BITS(data, BMI2_ACC_SELF_TEST_AMP, amp); - rslt = bmi2_set_regs(BMI2_ACC_SELF_TEST_ADDR, &data, 1, dev); - } - - return rslt; -} - -/*! - * @brief This internal API reads the accelerometer data for x,y and z axis from - * the sensor. The data units is in LSB format. - */ -static int8_t read_accel_xyz(struct bmi2_sens_axes_data *accel, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define LSB */ - uint16_t lsb = 0; - - /* Variable to define MSB */ - uint16_t msb = 0; - - /* Array to define data buffer */ - uint8_t data[BMI2_ACC_GYR_NUM_BYTES] = { 0 }; - - rslt = bmi2_get_regs(BMI2_ACC_X_LSB_ADDR, data, BMI2_ACC_GYR_NUM_BYTES, dev); - if (rslt == BMI2_OK) - { - /* Accelerometer data x axis */ - msb = data[1]; - lsb = data[0]; - accel->x = (int16_t)((msb << 8) | lsb); - - /* Accelerometer data y axis */ - msb = data[3]; - lsb = data[2]; - accel->y = (int16_t)((msb << 8) | lsb); - - /* Accelerometer data z axis */ - msb = data[5]; - lsb = data[4]; - accel->z = (int16_t)((msb << 8) | lsb); - } - - return rslt; -} - -/*! - * @brief This internal API reads the gyroscope data for x, y and z axis from - * the sensor. The data units is in LSB format. - */ -static int8_t read_gyro_xyz(struct bmi2_sens_axes_data *gyro, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to define LSB */ - uint16_t lsb = 0; - - /* Variable to define MSB */ - uint16_t msb = 0; - - /* Array to define data buffer */ - uint8_t data[BMI2_ACC_GYR_NUM_BYTES] = { 0 }; - - rslt = bmi2_get_regs(BMI2_GYR_X_LSB_ADDR, data, BMI2_ACC_GYR_NUM_BYTES, dev); - if (rslt == BMI2_OK) - { - /* Gyroscope data x axis */ - msb = data[1]; - lsb = data[0]; - gyro->x = (int16_t)((msb << 8) | lsb); - - /* Gyroscope data y axis */ - msb = data[3]; - lsb = data[2]; - gyro->y = (int16_t)((msb << 8) | lsb); - - /* Gyroscope data z axis */ - msb = data[5]; - lsb = data[4]; - gyro->z = (int16_t)((msb << 8) | lsb); - } - - return rslt; -} - -/*! - * @brief This internal API converts LSB value of accelerometer axes to form - * 'g' to 'mg' for self-test. - */ -static void convert_lsb_g(const struct bmi2_selftest_delta_limit *acc_data_diff, - struct bmi2_selftest_delta_limit *acc_data_diff_mg, - const struct bmi2_dev *dev) -{ - /* Variable to define LSB/g value of axes */ - uint32_t lsb_per_g; - - /* Range considered for self-test is +/-16g */ - uint8_t range = BMI2_ACC_SELF_TEST_RANGE; - - /* lsb_per_g for the respective resolution and 16g range */ - lsb_per_g = (uint32_t)(power(2, dev->resolution) / (2 * range)); - - /* Accelerometer x value in mg */ - acc_data_diff_mg->x = (acc_data_diff->x / (int32_t) lsb_per_g) * 1000; - - /* Accelerometer y value in mg */ - acc_data_diff_mg->y = (acc_data_diff->y / (int32_t) lsb_per_g) * 1000; - - /* Accelerometer z value in mg */ - acc_data_diff_mg->z = (acc_data_diff->z / (int32_t) lsb_per_g) * 1000; -} - -/*! - * @brief This internal API is used to calculate the power of a value. - */ -static int32_t power(int16_t base, uint8_t resolution) -{ - /* Initialize loop */ - uint8_t loop = 1; - - /* Initialize variable to store the power of 2 value */ - int32_t value = 1; - - for (; loop <= resolution; loop++) - { - value = (int32_t)(value * base); - } - - return value; -} - -/*! - * @brief This internal API validates the accelerometer self-test data and - * decides the result of self-test operation. - */ -static int8_t validate_self_test(const struct bmi2_selftest_delta_limit *accel_data_diff) -{ - /* Variable to define error */ - int8_t rslt; - - /* As per the data sheet, The actually measured signal differences should be significantly - * larger than the minimum differences for each axis in order for the self-test to pass. - */ - if ((accel_data_diff->x > BMI2_ST_ACC_X_SIG_MIN_DIFF) && (accel_data_diff->y < BMI2_ST_ACC_Y_SIG_MIN_DIFF) && - (accel_data_diff->z > BMI2_ST_ACC_Z_SIG_MIN_DIFF)) - { - /* Self-test pass */ - rslt = BMI2_OK; - } - else - { - /* Self-test fail*/ - rslt = BMI2_E_SELF_TEST_FAIL; - } - - return rslt; -} - -/*! - * @brief This internal API gets the re-mapped x, y and z axes from the sensor. - */ -static int8_t get_remap_axes(struct bmi2_axes_remap *remap, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt = BMI2_OK; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for axis re-mapping */ - struct bmi2_feature_config remap_config = { 0, 0, 0 }; - - /* Variable to get the status of advance power save */ - uint8_t aps_stat; - - /* Get status of advance power save mode */ - aps_stat = dev->aps_status; - if (aps_stat == BMI2_ENABLE) - { - /* Disable advance power save if enabled */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - } - - if (rslt == BMI2_OK) - { - /* Search for axis re-mapping and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&remap_config, BMI2_AXIS_MAP, dev); - if (feat_found) - { - rslt = bmi2_get_feat_config(remap_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for axis re-mapping */ - idx = remap_config.start_addr; - - /* Get the re-mapped x-axis */ - remap->x_axis = BMI2_GET_BIT_POS0(feat_config[idx], BMI2_X_AXIS); - - /* Get the re-mapped x-axis polarity */ - remap->x_axis_sign = BMI2_GET_BITS(feat_config[idx], BMI2_X_AXIS_SIGN); - - /* Get the re-mapped y-axis */ - remap->y_axis = BMI2_GET_BITS(feat_config[idx], BMI2_Y_AXIS); - - /* Get the re-mapped y-axis polarity */ - remap->y_axis_sign = BMI2_GET_BITS(feat_config[idx], BMI2_Y_AXIS_SIGN); - - /* Get the re-mapped z-axis */ - remap->z_axis = BMI2_GET_BITS(feat_config[idx], BMI2_Z_AXIS); - - /* Increment byte to fetch the next data */ - idx++; - - /* Get the re-mapped z-axis polarity */ - remap->z_axis_sign = BMI2_GET_BIT_POS0(feat_config[idx], BMI2_Z_AXIS_SIGN); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - /* Enable Advance power save if disabled while configuring and - * not when already disabled - */ - if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) - { - rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - } - } - - return rslt; -} - -/*! - * @brief This internal API sets the re-mapped x, y and z axes in the sensor. - */ -static int8_t set_remap_axes(const struct bmi2_axes_remap *remap, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt = BMI2_OK; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define the register address */ - uint8_t reg_addr = 0; - - /* Variable to set the re-mapped x-axes in the sensor */ - uint8_t x_axis = 0; - - /* Variable to set the re-mapped y-axes in the sensor */ - uint8_t y_axis = 0; - - /* Variable to set the re-mapped z-axes in the sensor */ - uint8_t z_axis = 0; - - /* Variable to set the re-mapped x-axes sign in the sensor */ - uint8_t x_axis_sign = 0; - - /* Variable to set the re-mapped y-axes sign in the sensor */ - uint8_t y_axis_sign = 0; - - /* Variable to set the re-mapped z-axes sign in the sensor */ - uint8_t z_axis_sign = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature configuration for axis re-mapping */ - struct bmi2_feature_config remap_config = { 0, 0, 0 }; - - /* Variable to get the status of advance power save */ - uint8_t aps_stat; - - /* Get status of advance power save mode */ - aps_stat = dev->aps_status; - if (aps_stat == BMI2_ENABLE) - { - /* Disable advance power save if enabled */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - } - - if (rslt == BMI2_OK) - { - /* Search for axis-re-mapping and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&remap_config, BMI2_AXIS_MAP, dev); - if (feat_found) - { - /* Get the configuration from the page where axis re-mapping feature resides */ - rslt = bmi2_get_feat_config(remap_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes */ - idx = remap_config.start_addr; - - /* Set the value of re-mapped x-axis */ - x_axis = remap->x_axis & BMI2_X_AXIS_MASK; - - /* Set the value of re-mapped x-axis sign */ - x_axis_sign = ((remap->x_axis_sign << BMI2_X_AXIS_SIGN_POS) & BMI2_X_AXIS_SIGN_MASK); - - /* Set the value of re-mapped y-axis */ - y_axis = ((remap->y_axis << BMI2_Y_AXIS_POS) & BMI2_Y_AXIS_MASK); - - /* Set the value of re-mapped y-axis sign */ - y_axis_sign = ((remap->y_axis_sign << BMI2_Y_AXIS_SIGN_POS) & BMI2_Y_AXIS_SIGN_MASK); - - /* Set the value of re-mapped z-axis */ - z_axis = ((remap->z_axis << BMI2_Z_AXIS_POS) & BMI2_Z_AXIS_MASK); - - /* Set the value of re-mapped z-axis sign */ - z_axis_sign = remap->z_axis_sign & BMI2_Z_AXIS_SIGN_MASK; - - /* Arrange axes in the first byte */ - feat_config[idx] = x_axis | x_axis_sign | y_axis | y_axis_sign | z_axis; - - /* Increment the index */ - idx++; - - /* Cannot OR in the second byte since it holds - * gyroscope self-offset correction bit - */ - feat_config[idx] = BMI2_SET_BIT_POS0(feat_config[idx], BMI2_Z_AXIS_SIGN, z_axis_sign); - - /* Update the register address */ - reg_addr = BMI2_FEATURES_REG_ADDR + remap_config.start_addr; - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(reg_addr, &feat_config[remap_config.start_addr], 2, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - /* Enable Advance power save if disabled while configuring and - * not when already disabled - */ - if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) - { - rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - } - } - - return rslt; -} - -/*! - * @brief This internal API corrects the gyroscope cross-axis sensitivity - * between the z and the x axis. - */ -static void comp_gyro_cross_axis_sensitivity(struct bmi2_sens_axes_data *gyr_data, const struct bmi2_dev *dev) -{ - /* Get the compensated gyroscope x-axis */ - gyr_data->x = gyr_data->x - (int16_t)(((int32_t) dev->gyr_cross_sens_zx * (int32_t) gyr_data->z) / 512); -} - -/*! - * @brief This internal API is used to validate the boundary conditions. - */ -static int8_t check_boundary_val(uint8_t *val, uint8_t min, uint8_t max, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt = BMI2_OK; - - if (val != NULL) - { - /* Check if value is below minimum value */ - if (*val < min) - { - /* Auto correct the invalid value to minimum value */ - *val = min; - dev->info |= BMI2_I_MIN_VALUE; - } - - /* Check if value is above maximum value */ - if (*val > max) - { - /* Auto correct the invalid value to maximum value */ - *val = max; - dev->info |= BMI2_I_MAX_VALUE; - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This internal API saves the configurations before performing FOC. - */ -static int8_t save_accel_foc_config(struct bmi2_accel_config *acc_cfg, - uint8_t *aps, - uint8_t *acc_en, - struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to get the status from PWR_CTRL register */ - uint8_t pwr_ctrl_data = 0; - - /* Get accelerometer configurations to be saved */ - rslt = get_accel_config(acc_cfg, dev); - if (rslt == BMI2_OK) - { - /* Get accelerometer enable status to be saved */ - rslt = bmi2_get_regs(BMI2_PWR_CTRL_ADDR, &pwr_ctrl_data, 1, dev); - *acc_en = BMI2_GET_BITS(pwr_ctrl_data, BMI2_ACC_EN); - - /* Get advance power save mode to be saved */ - if (rslt == BMI2_OK) - { - rslt = bmi2_get_adv_power_save(aps, dev); - } - } - - return rslt; -} - -/*! - * @brief This internal sets configurations for performing accelerometer FOC. - */ -static int8_t set_accel_foc_config(struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to select the sensor */ - uint8_t sens_list = BMI2_ACCEL; - - /* Variable to set the accelerometer configuration value */ - uint8_t acc_conf_data = BMI2_FOC_ACC_CONF_VAL; - - /* Disabling offset compensation */ - rslt = set_accel_offset_comp(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - /* Set accelerometer configurations to 50Hz, continuous mode, CIC mode */ - rslt = bmi2_set_regs(BMI2_ACC_CONF_ADDR, &acc_conf_data, 1, dev); - if (rslt == BMI2_OK) - { - /* Set accelerometer to normal mode by enabling it */ - rslt = bmi2_sensor_enable(&sens_list, 1, dev); - - if (rslt == BMI2_OK) - { - /* Disable advance power save mode */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - } - } - } - - return rslt; -} - -/*! - * @brief This internal API performs Fast Offset Compensation for accelerometer. - */ -static int8_t perform_accel_foc(const struct bmi2_accel_foc_g_value *accel_g_value, - const struct bmi2_accel_config *acc_cfg, - struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt = BMI2_E_INVALID_STATUS; - - /* Variable to define count */ - uint8_t loop; - - /* Variable to store status read from the status register */ - uint8_t reg_status = 0; - - /* Array of structure to store accelerometer data */ - struct bmi2_sens_axes_data accel_value[128] = { { 0 } }; - - /* Structure to store accelerometer data temporarily */ - struct bmi2_foc_temp_value temp = { 0, 0, 0 }; - - /* Structure to store the average of accelerometer data */ - struct bmi2_sens_axes_data accel_avg = { 0, 0, 0, 0 }; - - /* Variable to define LSB per g value */ - uint16_t lsb_per_g = 0; - - /* Variable to define range */ - uint8_t range = 0; - - /* Structure to store accelerometer data deviation from ideal value */ - struct bmi2_offset_delta delta = { 0, 0, 0 }; - - /* Structure to store accelerometer offset values */ - struct bmi2_accel_offset offset = { 0, 0, 0 }; - - /* Variable tries max 5 times for interrupt then generates timeout */ - uint8_t try_cnt; - - for (loop = 0; loop < 128; loop++) - { - try_cnt = 5; - while (try_cnt && (!(reg_status & BMI2_DRDY_ACC))) - { - /* 20ms delay for 50Hz ODR */ - dev->delay_us(20000, dev->intf_ptr); - rslt = bmi2_get_status(®_status, dev); - try_cnt--; - } - - if ((rslt == BMI2_OK) && (reg_status & BMI2_DRDY_ACC)) - { - rslt = read_accel_xyz(&accel_value[loop], dev); - } - - if (rslt == BMI2_OK) - { - rslt = read_accel_xyz(&accel_value[loop], dev); - } - - if (rslt == BMI2_OK) - { - /* Store the data in a temporary structure */ - temp.x = temp.x + (int32_t)accel_value[loop].x; - temp.y = temp.y + (int32_t)accel_value[loop].y; - temp.z = temp.z + (int32_t)accel_value[loop].z; - } - else - { - break; - } - } - - if (rslt == BMI2_OK) - { - /* Take average of x, y and z data for lesser noise */ - accel_avg.x = (int16_t)(temp.x / 128); - accel_avg.y = (int16_t)(temp.y / 128); - accel_avg.z = (int16_t)(temp.z / 128); - - /* Get the exact range value */ - map_accel_range(acc_cfg->range, &range); - - /* Get the smallest possible measurable acceleration level given the range and - * resolution */ - lsb_per_g = (uint16_t)(power(2, dev->resolution) / (2 * range)); - - /* Compensate acceleration data against gravity */ - comp_for_gravity(lsb_per_g, accel_g_value, &accel_avg, &delta); - - /* Scale according to offset register resolution */ - scale_accel_offset(range, &delta, &offset); - - /* Invert the accelerometer offset data */ - invert_accel_offset(&offset); - - /* Write offset data in the offset compensation register */ - rslt = write_accel_offset(&offset, dev); - - /* Enable offset compensation */ - if (rslt == BMI2_OK) - { - rslt = set_accel_offset_comp(BMI2_ENABLE, dev); - } - } - - return rslt; -} - -/*! - * @brief This internal API enables/disables the offset compensation for - * filtered and un-filtered accelerometer data. - */ -static int8_t set_accel_offset_comp(uint8_t offset_en, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to store data */ - uint8_t data = 0; - - /* Enable/Disable offset compensation */ - rslt = bmi2_get_regs(BMI2_NV_CONF_ADDR, &data, 1, dev); - if (rslt == BMI2_OK) - { - data = BMI2_SET_BITS(data, BMI2_NV_ACC_OFFSET, offset_en); - rslt = bmi2_set_regs(BMI2_NV_CONF_ADDR, &data, 1, dev); - } - - return rslt; -} - -/*! - * @brief This internal API converts the accelerometer range value into - * corresponding integer value. - */ -static void map_accel_range(uint8_t range_in, uint8_t *range_out) -{ - switch (range_in) - { - case BMI2_ACC_RANGE_2G: - *range_out = 2; - break; - case BMI2_ACC_RANGE_4G: - *range_out = 4; - break; - case BMI2_ACC_RANGE_8G: - *range_out = 8; - break; - case BMI2_ACC_RANGE_16G: - *range_out = 16; - break; - default: - - /* By default RANGE 8G is set */ - *range_out = 8; - break; - } -} - -/*! - * @brief This internal API compensate the accelerometer data against gravity. - */ -static void comp_for_gravity(uint16_t lsb_per_g, - const struct bmi2_accel_foc_g_value *g_val, - const struct bmi2_sens_axes_data *data, - struct bmi2_offset_delta *comp_data) -{ - /* Array to store the accelerometer values in LSB */ - int16_t accel_value_lsb[3] = { 0 }; - - /* Convert g-value to LSB */ - accel_value_lsb[BMI2_X_AXIS] = (int16_t)(lsb_per_g * g_val->x); - accel_value_lsb[BMI2_Y_AXIS] = (int16_t)(lsb_per_g * g_val->y); - accel_value_lsb[BMI2_Z_AXIS] = (int16_t)(lsb_per_g * g_val->z); - - /* Get the compensated values for X, Y and Z axis */ - comp_data->x = (data->x - accel_value_lsb[BMI2_X_AXIS]); - comp_data->y = (data->y - accel_value_lsb[BMI2_Y_AXIS]); - comp_data->z = (data->z - accel_value_lsb[BMI2_Z_AXIS]); -} - -/*! - * @brief This internal API scales the compensated accelerometer data according - * to the offset register resolution. - * - * @note The bit position is always greater than 0 since accelerometer data is - * 16 bit wide. - */ -static void scale_accel_offset(uint8_t range, const struct bmi2_offset_delta *comp_data, struct bmi2_accel_offset *data) -{ - /* Variable to store the position of bit having 3.9mg resolution */ - int8_t bit_pos_3_9mg; - - /* Variable to store the position previous of bit having 3.9mg resolution */ - int8_t bit_pos_3_9mg_prev_bit; - - /* Variable to store the round-off value */ - uint8_t round_off; - - /* Find the bit position of 3.9mg */ - bit_pos_3_9mg = get_bit_pos_3_9mg(range); - - /* Round off, consider if the next bit is high */ - bit_pos_3_9mg_prev_bit = bit_pos_3_9mg - 1; - round_off = (uint8_t)(power(2, ((uint8_t) bit_pos_3_9mg_prev_bit))); - - /* Scale according to offset register resolution */ - data->x = (uint8_t)((comp_data->x + round_off) / power(2, ((uint8_t) bit_pos_3_9mg))); - data->y = (uint8_t)((comp_data->y + round_off) / power(2, ((uint8_t) bit_pos_3_9mg))); - data->z = (uint8_t)((comp_data->z + round_off) / power(2, ((uint8_t) bit_pos_3_9mg))); -} - -/*! - * @brief This internal API finds the bit position of 3.9mg according to given - * range and resolution. - */ -static int8_t get_bit_pos_3_9mg(uint8_t range) -{ - /* Variable to store the bit position of 3.9mg resolution */ - int8_t bit_pos_3_9mg; - - /* Variable to shift the bits according to the resolution */ - uint32_t divisor = 1; - - /* Scaling factor to get the bit position of 3.9 mg resolution */ - int16_t scale_factor = -1; - - /* Variable to store temporary value */ - uint16_t temp; - - /* Shift left by the times of resolution */ - divisor = divisor << 16; - - /* Get the bit position to be shifted */ - temp = (uint16_t)(divisor / (range * 256)); - - /* Get the scaling factor until bit position is shifted to last bit */ - while (temp != 1) - { - scale_factor++; - temp = temp >> 1; - } - - /* Scaling factor is the bit position of 3.9 mg resolution */ - bit_pos_3_9mg = (int8_t) scale_factor; - - return bit_pos_3_9mg; -} - -/*! - * @brief This internal API inverts the accelerometer offset data. - */ -static void invert_accel_offset(struct bmi2_accel_offset *offset_data) -{ - /* Get the offset data */ - offset_data->x = (uint8_t)((offset_data->x) * (-1)); - offset_data->y = (uint8_t)((offset_data->y) * (-1)); - offset_data->z = (uint8_t)((offset_data->z) * (-1)); -} - -/*! - * @brief This internal API writes the offset data in the offset compensation - * register. - */ -static int8_t write_accel_offset(const struct bmi2_accel_offset *offset, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to store the offset data */ - uint8_t data_array[3] = { 0 }; - - data_array[0] = offset->x; - data_array[1] = offset->y; - data_array[2] = offset->z; - - /* Offset values are written in the offset register */ - rslt = bmi2_set_regs(BMI2_ACC_OFF_COMP_0_ADDR, data_array, 3, dev); - - return rslt; -} - -/*! - * @brief This internal API restores the configurations saved before performing - * accelerometer FOC. - */ -static int8_t restore_accel_foc_config(struct bmi2_accel_config *acc_cfg, - uint8_t aps, - uint8_t acc_en, - struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to get the status from PWR_CTRL register */ - uint8_t pwr_ctrl_data = 0; - - /* Restore the saved accelerometer configurations */ - rslt = set_accel_config(acc_cfg, dev); - if (rslt == BMI2_OK) - { - /* Restore the saved accelerometer enable status */ - rslt = bmi2_get_regs(BMI2_PWR_CTRL_ADDR, &pwr_ctrl_data, 1, dev); - if (rslt == BMI2_OK) - { - pwr_ctrl_data = BMI2_SET_BITS(pwr_ctrl_data, BMI2_ACC_EN, acc_en); - rslt = bmi2_set_regs(BMI2_PWR_CTRL_ADDR, &pwr_ctrl_data, 1, dev); - - /* Restore the saved advance power save */ - if (rslt == BMI2_OK) - { - rslt = bmi2_set_adv_power_save(aps, dev); - } - } - } - - return rslt; -} - -/*! - * @brief This internal API sets accelerometer configurations like ODR, - * bandwidth, performance mode and g-range. - */ -static int8_t set_accel_config(struct bmi2_accel_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to store data */ - uint8_t reg_data; - - /* Array to store the default value of accelerometer configuration - * reserved registers - */ - uint8_t data_array[2] = { 0 }; - - /* Validate bandwidth and performance mode */ - rslt = validate_bw_perf_mode(&config->bwp, &config->filter_perf, dev); - if (rslt == BMI2_OK) - { - /* Validate ODR and range */ - rslt = validate_odr_range(&config->odr, &config->range, dev); - if (rslt == BMI2_OK) - { - /* Set accelerometer performance mode */ - reg_data = BMI2_SET_BITS(data_array[0], BMI2_ACC_FILTER_PERF_MODE, config->filter_perf); - - /* Set accelerometer bandwidth */ - reg_data = BMI2_SET_BITS(reg_data, BMI2_ACC_BW_PARAM, config->bwp); - - /* Set accelerometer ODR */ - reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_ACC_ODR, config->odr); - - /* Copy the register data to the array */ - data_array[0] = reg_data; - - /* Set accelerometer range */ - reg_data = BMI2_SET_BIT_POS0(data_array[1], BMI2_ACC_RANGE, config->range); - - /* Copy the register data to the array */ - data_array[1] = reg_data; - - /* Write accelerometer configuration to ACC_CONFand - * ACC_RANGE registers simultaneously as they lie in consecutive places - */ - rslt = bmi2_set_regs(BMI2_ACC_CONF_ADDR, data_array, 2, dev); - - /* Get error status to check for invalid configurations */ - if (rslt == BMI2_OK) - { - rslt = cfg_error_status(dev); - } - } - } - - return rslt; -} - -/*! - * @brief This internal API sets gyroscope configurations like ODR, bandwidth, - * low power/high performance mode, performance mode and range. It also - * maps/un-maps data interrupts to that of hardware interrupt line. - */ -static int8_t set_gyro_config(struct bmi2_gyro_config *config, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to store data */ - uint8_t reg_data; - - /* Array to store the default value of gyroscope configuration reserved registers */ - uint8_t data_array[2] = { 0 }; - - /* Validate gyroscope configurations */ - rslt = validate_gyro_config(config, dev); - if (rslt == BMI2_OK) - { - /* Set gyroscope performance mode */ - reg_data = BMI2_SET_BITS(data_array[0], BMI2_GYR_FILTER_PERF_MODE, config->filter_perf); - - /* Set gyroscope noise performance mode */ - reg_data = BMI2_SET_BITS(reg_data, BMI2_GYR_NOISE_PERF_MODE, config->noise_perf); - - /* Set gyroscope bandwidth */ - reg_data = BMI2_SET_BITS(reg_data, BMI2_GYR_BW_PARAM, config->bwp); - - /* Set gyroscope ODR */ - reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_GYR_ODR, config->odr); - - /* Copy the register data to the array */ - data_array[0] = reg_data; - - /* Set gyroscope OIS range */ - reg_data = BMI2_SET_BITS(data_array[1], BMI2_GYR_OIS_RANGE, config->ois_range); - - /* Set gyroscope range */ - reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_GYR_RANGE, config->range); - - /* Copy the register data to the array */ - data_array[1] = reg_data; - - /* Write accelerometer configuration to GYR_CONF and GYR_RANGE - * registers simultaneously as they lie in consecutive places - */ - rslt = bmi2_set_regs(BMI2_GYR_CONF_ADDR, data_array, 2, dev); - - /* Get error status to check for invalid configurations */ - if (rslt == BMI2_OK) - { - rslt = cfg_error_status(dev); - } - } - - return rslt; -} - -/*! - * @brief This internal API saves the configurations before performing gyroscope - * FOC. - */ -static int8_t save_gyro_config(struct bmi2_gyro_config *gyr_cfg, uint8_t *aps, uint8_t *gyr_en, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to get the status from PWR_CTRL register */ - uint8_t pwr_ctrl_data = 0; - - /* Get gyroscope configurations to be saved */ - rslt = get_gyro_config(gyr_cfg, dev); - if (rslt == BMI2_OK) - { - /* Get gyroscope enable status to be saved */ - rslt = bmi2_get_regs(BMI2_PWR_CTRL_ADDR, &pwr_ctrl_data, 1, dev); - *gyr_en = BMI2_GET_BITS(pwr_ctrl_data, BMI2_GYR_EN); - - /* Get advance power save mode to be saved */ - if (rslt == BMI2_OK) - { - rslt = bmi2_get_adv_power_save(aps, dev); - } - } - - return rslt; -} - -/*! - * @brief This internal sets configurations for performing gyroscope FOC. - */ -static int8_t set_gyro_foc_config(struct bmi2_dev *dev) -{ - int8_t rslt; - - /* Variable to select the sensor */ - uint8_t sens_list = BMI2_GYRO; - - /* Array to set the gyroscope configuration value (ODR, Performance mode - * and bandwidth) and gyroscope range - */ - uint8_t gyr_conf_data[2] = { BMI2_FOC_GYR_CONF_VAL, BMI2_GYR_RANGE_2000 }; - - /* Disabling gyroscope offset compensation */ - rslt = bmi2_set_gyro_offset_comp(BMI2_DISABLE, dev); - if (rslt == BMI2_OK) - { - /* Set gyroscope configurations to 25Hz, continuous mode, - * CIC mode, and 2000 dps range - */ - rslt = bmi2_set_regs(BMI2_GYR_CONF_ADDR, gyr_conf_data, 2, dev); - if (rslt == BMI2_OK) - { - /* Set gyroscope to normal mode by enabling it */ - rslt = bmi2_sensor_enable(&sens_list, 1, dev); - - if (rslt == BMI2_OK) - { - /* Disable advance power save mode */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - } - } - } - - return rslt; -} - -/*! - * @brief This internal API inverts the gyroscope offset data. - */ -static void invert_gyro_offset(struct bmi2_sens_axes_data *offset_data) -{ - /* Invert the values */ - offset_data->x = (int16_t)((offset_data->x) * (-1)); - offset_data->y = (int16_t)((offset_data->y) * (-1)); - offset_data->z = (int16_t)((offset_data->z) * (-1)); -} - -/*! - * @brief This internal API restores the gyroscope configurations saved - * before performing FOC. - */ -static int8_t restore_gyro_config(struct bmi2_gyro_config *gyr_cfg, uint8_t aps, uint8_t gyr_en, struct bmi2_dev *dev) -{ - int8_t rslt; - uint8_t pwr_ctrl_data = 0; - - /* Restore the saved gyroscope configurations */ - rslt = set_gyro_config(gyr_cfg, dev); - if (rslt == BMI2_OK) - { - /* Restore the saved gyroscope enable status */ - rslt = bmi2_get_regs(BMI2_PWR_CTRL_ADDR, &pwr_ctrl_data, 1, dev); - if (rslt == BMI2_OK) - { - pwr_ctrl_data = BMI2_SET_BITS(pwr_ctrl_data, BMI2_GYR_EN, gyr_en); - rslt = bmi2_set_regs(BMI2_PWR_CTRL_ADDR, &pwr_ctrl_data, 1, dev); - - /* Restore the saved advance power save */ - if (rslt == BMI2_OK) - { - rslt = bmi2_set_adv_power_save(aps, dev); - } - } - } - - return rslt; -} - -/*! - * @brief This internal API saturates the gyroscope data value before writing to - * to 10 bit offset register. - */ -static void saturate_gyro_data(struct bmi2_sens_axes_data *gyr_off) -{ - if (gyr_off->x > 511) - { - gyr_off->x = 511; - } - - if (gyr_off->x < -512) - { - gyr_off->x = -512; - } - - if (gyr_off->y > 511) - { - gyr_off->y = 511; - } - - if (gyr_off->y < -512) - { - gyr_off->y = -512; - } - - if (gyr_off->z > 511) - { - gyr_off->z = 511; - } - - if (gyr_off->z < -512) - { - gyr_off->z = -512; - } -} - -/*! - * @brief This internal API is used to validate the device structure pointer for - * null conditions. - */ -static int8_t null_ptr_check(const struct bmi2_dev *dev) -{ - int8_t rslt = BMI2_OK; - - if ((dev == NULL) || (dev->read == NULL) || (dev->write == NULL) || (dev->delay_us == NULL)) - { - /* Device structure pointer is not valid */ - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This internal API is to get the status of st_status from gry_crt_conf register - */ -static int8_t get_st_running(uint8_t *st_status, struct bmi2_dev *dev) -{ - int8_t rslt; - uint8_t reg_data = 0; - - rslt = null_ptr_check(dev); - if (rslt == BMI2_OK) - { - /* Get the status of crt running */ - rslt = bmi2_get_regs(BMI2_GYR_CRT_CONF_ADDR, ®_data, 1, dev); - if (rslt == BMI2_OK) - { - (*st_status) = BMI2_GET_BITS(reg_data, BMI2_GYR_CRT_RUNNING); - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API enables/disables the CRT running. - */ -static int8_t set_st_running(uint8_t st_status, struct bmi2_dev *dev) -{ - int8_t rslt; - uint8_t reg_data = 0; - - rslt = null_ptr_check(dev); - if (rslt == BMI2_OK) - { - rslt = bmi2_get_regs(BMI2_GYR_CRT_CONF_ADDR, ®_data, 1, dev); - if (rslt == BMI2_OK) - { - reg_data = BMI2_SET_BITS(reg_data, BMI2_GYR_CRT_RUNNING, st_status); - rslt = bmi2_set_regs(BMI2_GYR_CRT_CONF_ADDR, ®_data, 1, dev); - } - } - - return rslt; -} - -/*! - * @brief This API gets the status of rdy for dl bit. - */ -static int8_t get_rdy_for_dl(uint8_t *rdy_for_dl, struct bmi2_dev *dev) -{ - int8_t rslt; - uint8_t reg_data = 0; - - rslt = null_ptr_check(dev); - if (rslt == BMI2_OK) - { - /* Get the status of rdy_fo_dl */ - rslt = bmi2_get_regs(BMI2_GYR_CRT_CONF_ADDR, ®_data, 1, dev); - if (rslt == BMI2_OK) - { - (*rdy_for_dl) = BMI2_GET_BITS(reg_data, BMI2_GYR_RDY_FOR_DL); - } - } - else - { - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} - -/*! - * @brief This API does the crt process if max burst length is not zero. - */ -static int8_t process_crt_download(uint8_t last_byte_flag, struct bmi2_dev *dev) -{ - int8_t rslt; - uint8_t rdy_for_dl = 0; - uint8_t cmd = BMI2_G_TRIGGER_CMD; - - rslt = null_ptr_check(dev); - if (rslt == BMI2_OK) - { - rslt = get_rdy_for_dl(&rdy_for_dl, dev); - } - - /* Trigger next CRT command */ - if (rslt == BMI2_OK) - { - rslt = bmi2_set_regs(BMI2_CMD_REG_ADDR, &cmd, 1, dev); - } - - if ((!last_byte_flag) && (rslt == BMI2_OK)) - { - rslt = wait_rdy_for_dl_toggle(BMI2_CRT_READY_FOR_DOWNLOAD_RETRY, rdy_for_dl, dev); - } - - return rslt; -} - -/*! - * @brief This API to write the 2kb size of crt configuration - */ -static int8_t write_crt_config_file(uint16_t write_len, - uint16_t config_file_size, - uint16_t start_index, - struct bmi2_dev *dev) -{ - int8_t rslt = BMI2_OK; - uint16_t index = 0; - uint8_t last_byte_flag = 0; - uint8_t remain = (uint8_t)(config_file_size % write_len); - uint16_t balance_byte = 0; - - if (!remain) - { - - /* Write the configuration file */ - for (index = start_index; - (index < (start_index + config_file_size)) && (rslt == BMI2_OK); - index += write_len) - { - rslt = upload_file((dev->config_file_ptr + index), index, write_len, dev); - if (index >= ((start_index + config_file_size) - (write_len))) - { - last_byte_flag = 1; - } - - if (rslt == BMI2_OK) - { - rslt = process_crt_download(last_byte_flag, dev); - } - } - } - else - { - /* Get the balance bytes */ - balance_byte = (uint16_t)start_index + (uint16_t)config_file_size - (uint16_t)remain; - - /* Write the configuration file for the balance bytes */ - for (index = start_index; (index < balance_byte) && (rslt == BMI2_OK); index += write_len) - { - rslt = upload_file((dev->config_file_ptr + index), index, write_len, dev); - if (rslt == BMI2_OK) - { - rslt = process_crt_download(last_byte_flag, dev); - } - } - - if (rslt == BMI2_OK) - { - /* Write the remaining bytes in 2 bytes length */ - write_len = 2; - rslt = set_maxburst_len(write_len, dev); - - /* Write the configuration file for the remaining bytes */ - for (index = balance_byte; - (index < (start_index + config_file_size)) && (rslt == BMI2_OK); - index += write_len) - { - rslt = upload_file((dev->config_file_ptr + index), index, write_len, dev); - if (index < ((start_index + config_file_size) - write_len)) - { - last_byte_flag = 1; - } - - if (rslt == BMI2_OK) - { - rslt = process_crt_download(last_byte_flag, dev); - } - } - } - } - - return rslt; -} - -/*! - * @brief This API is to wait till the rdy for dl bit toggles after every pack of bytes. - */ -static int8_t wait_rdy_for_dl_toggle(uint8_t retry_complete, uint8_t download_ready, struct bmi2_dev *dev) -{ - int8_t rslt = BMI2_OK; - uint8_t dl_ready = 0; - uint8_t st_status = 0; - - while ((rslt == BMI2_OK) && (retry_complete--)) - { - rslt = get_rdy_for_dl(&dl_ready, dev); - if (download_ready != dl_ready) - { - break; - } - - dev->delay_us(BMI2_CRT_READY_FOR_DOWNLOAD_US, dev->intf_ptr); - } - - if ((rslt == BMI2_OK) && (download_ready == dl_ready)) - { - rslt = BMI2_E_CRT_READY_FOR_DL_FAIL_ABORT; - } - - if (rslt == BMI2_OK) - { - rslt = get_st_running(&st_status, dev); - if ((rslt == BMI2_OK) && (st_status == 0)) - { - rslt = BMI2_E_ST_ALREADY_RUNNING; - } - } - - return rslt; -} - -/*! - * @brief This API is to wait till crt status complete. - */ -static int8_t wait_st_running(uint8_t retry_complete, struct bmi2_dev *dev) -{ - uint8_t st_status = 1; - int8_t rslt = BMI2_OK; - - while (retry_complete--) - { - rslt = get_st_running(&st_status, dev); - if ((rslt == BMI2_OK) && (st_status == 0)) - { - break; - } - - dev->delay_us(BMI2_CRT_WAIT_RUNNING_US, dev->intf_ptr); - } - - if ((rslt == BMI2_OK) && (st_status == 1)) - { - rslt = BMI2_E_ST_ALREADY_RUNNING; - } - - return rslt; -} - -/*! - * @brief This api is used to perform gyroscope self-test. - */ -int8_t bmi2_do_gyro_st(struct bmi2_dev *dev) -{ - int8_t rslt; - - rslt = do_gtrigger_test(BMI2_SELECT_GYRO_SELF_TEST, dev); - - return rslt; -} - -/*! - * @brief This API is to run the CRT process for both max burst length 0 and non zero condition. - */ -int8_t bmi2_do_crt(struct bmi2_dev *dev) -{ - int8_t rslt; - - rslt = do_gtrigger_test(BMI2_SELECT_CRT, dev); - - return rslt; -} - -/*! - * @brief This API is to run the crt process for both max burst length 0 and non zero condition. - */ -static int8_t do_gtrigger_test(uint8_t gyro_st_crt, struct bmi2_dev *dev) -{ - int8_t rslt; - int8_t rslt_crt = BMI2_OK; - uint8_t st_status = 0; - uint8_t max_burst_length = 0; - uint8_t download_ready = 0; - uint8_t cmd = BMI2_G_TRIGGER_CMD; - struct bmi2_gyro_self_test_status gyro_st_result = { 0 }; - - /* Variable to get the status of advance power save */ - uint8_t aps_stat = 0; - - rslt = null_ptr_check(dev); - if (rslt == BMI2_OK) - { - /* Check if the variant supports this feature */ - if (dev->variant_feature & BMI2_CRT_RTOSK_ENABLE) - { - /* Get status of advance power save mode */ - aps_stat = dev->aps_status; - if (aps_stat == BMI2_ENABLE) - { - /* Disable advance power save if enabled */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - } - - /* Get max burst length */ - if (rslt == BMI2_OK) - { - rslt = get_maxburst_len(&max_burst_length, dev); - } - - /* Checking for CRT running status */ - if (rslt == BMI2_OK) - { - rslt = get_st_running(&st_status, dev); - } - - /* CRT is not running and Max burst length is zero */ - if (st_status == 0) - { - if (rslt == BMI2_OK) - { - rslt = set_st_running(BMI2_ENABLE, dev); - } - - /* Preparing the setup */ - if (rslt == BMI2_OK) - { - rslt = crt_prepare_setup(dev); - } - - /* Enable the gyro self-test, CRT */ - if (rslt == BMI2_OK) - { - rslt = select_self_test(gyro_st_crt, dev); - } - - /* Check if FIFO is unchanged by checking the max burst length */ - if ((rslt == BMI2_OK) && (max_burst_length == 0)) - { - /* Trigger CRT */ - rslt = bmi2_set_regs(BMI2_CMD_REG_ADDR, &cmd, 1, dev); - if (rslt == BMI2_OK) - { - /* Wait until st_status = 0 or time out is 2 seconds */ - rslt = wait_st_running(BMI2_CRT_WAIT_RUNNING_RETRY_EXECUTION, dev); - - /* CRT Running wait & check is successful */ - if (rslt == BMI2_OK) - { - rslt = crt_gyro_st_update_result(dev); - } - } - } - else - { - /* FIFO may be used */ - if (rslt == BMI2_OK) - { - if (dev->read_write_len < 2) - { - dev->read_write_len = 2; - } - - if (dev->read_write_len > (BMI2_CRT_MAX_BURST_WORD_LENGTH * 2)) - { - dev->read_write_len = BMI2_CRT_MAX_BURST_WORD_LENGTH * 2; - } - - /* Reset the max burst length to default value */ - rslt = set_maxburst_len(dev->read_write_len, dev); - } - - if (rslt == BMI2_OK) - { - rslt = get_rdy_for_dl(&download_ready, dev); - } - - /* Trigger CRT */ - if (rslt == BMI2_OK) - { - rslt = bmi2_set_regs(BMI2_CMD_REG_ADDR, &cmd, 1, dev); - } - - /* Wait till either ready for download toggle or crt running = 0 */ - if (rslt == BMI2_OK) - { - rslt = wait_rdy_for_dl_toggle(BMI2_CRT_READY_FOR_DOWNLOAD_RETRY, download_ready, dev); - if (rslt == BMI2_OK) - { - rslt = write_crt_config_file(dev->read_write_len, BMI2_CRT_CONFIG_FILE_SIZE, 0x1800, dev); - } - - if (rslt == BMI2_OK) - { - rslt = wait_st_running(BMI2_CRT_WAIT_RUNNING_RETRY_EXECUTION, dev); - rslt_crt = crt_gyro_st_update_result(dev); - if (rslt == BMI2_OK) - { - rslt = rslt_crt; - } - } - } - } - } - else - { - rslt = BMI2_E_ST_ALREADY_RUNNING; - } - - if (rslt == BMI2_OK) - { - if (gyro_st_crt == BMI2_SELECT_GYRO_SELF_TEST) - { - rslt = gyro_self_test_completed(&gyro_st_result, dev); - } - } - - /* Enable Advance power save if disabled while configuring and - * not when already disabled - */ - if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) - { - rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - } - } - } - - return rslt; -} - -/*! - * @brief This API to set up environment for processing the crt. - */ -static int8_t crt_prepare_setup(struct bmi2_dev *dev) -{ - int8_t rslt; - - /* Variable to select the sensor */ - uint8_t sens_list = BMI2_GYRO; - - rslt = null_ptr_check(dev); - - if (rslt == BMI2_OK) - { - /* Disable gyroscope */ - rslt = bmi2_sensor_disable(&sens_list, 1, dev); - } - - /* Disable FIFO for all sensors */ - if (rslt == BMI2_OK) - { - rslt = bmi2_set_fifo_config(BMI2_FIFO_ALL_EN, BMI2_DISABLE, dev); - } - - if (rslt == BMI2_OK) - { - /* Enable accelerometer */ - sens_list = BMI2_ACCEL; - rslt = bmi2_sensor_enable(&sens_list, 1, dev); - } - - if (rslt == BMI2_OK) - { - /* Disable Abort after 1 msec */ - dev->delay_us(1000, dev->intf_ptr); - rslt = abort_bmi2(BMI2_DISABLE, dev); - } - - return rslt; -} - -/*! - * @brief This API is to update the CRT or gyro self-test final result. - */ -static int8_t crt_gyro_st_update_result(struct bmi2_dev *dev) -{ - int8_t rslt; - struct bmi2_gyr_user_gain_status user_gain_stat = { 0, 0, 0, 0 }; - - rslt = null_ptr_check(dev); - - /* CRT status has to be read from the config register map */ - if (rslt == BMI2_OK) - { - rslt = get_gyro_gain_update_status(&user_gain_stat, dev); - } - - if (rslt == BMI2_OK) - { - switch (user_gain_stat.g_trigger_status) - { - case BMI2_G_TRIGGER_NO_ERROR: - - /* CRT is successful - Reset the Max Burst Length */ - rslt = set_maxburst_len(0, dev); - break; - - case BMI2_G_TRIGGER_DL_ERROR: - - /* CRT is Download Error - Keep non zero value for Max Burst Length */ - rslt = set_maxburst_len(dev->read_write_len, dev); - if (rslt == BMI2_OK) - { - rslt = BMI2_E_DL_ERROR; - } - - break; - case BMI2_G_TRIGGER_ABORT_ERROR: - - /* Command is aborted either by host via the block bit or due to motion - * detection. Keep non zero value for Max Burst Length - */ - rslt = set_maxburst_len(dev->read_write_len, dev); - if (rslt == BMI2_OK) - { - rslt = BMI2_E_ABORT_ERROR; - } - - break; - - case BMI2_G_TRIGGER_PRECON_ERROR: - - /* Pre-condition to start the feature was not completed. */ - rslt = BMI2_E_PRECON_ERROR; - break; - - default: - rslt = BMI2_E_INVALID_STATUS; - - break; - } - } - - return rslt; -} - -/*! - * @brief This internal API gets the max burst length. - */ -static int8_t get_maxburst_len(uint8_t *max_burst_len, struct bmi2_dev *dev) -{ - int8_t rslt = BMI2_OK; - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - uint8_t idx = 0; - uint8_t feat_found = 0; - struct bmi2_feature_config maxburst_length_bytes = { 0, 0, 0 }; - uint8_t aps_stat; - - if ((dev->variant_feature & BMI2_CRT_IN_FIFO_NOT_REQ) != 0) - { - *max_burst_len = 0; - - return BMI2_OK; - } - - /* Get status of advance power save mode */ - aps_stat = dev->aps_status; - if (aps_stat == BMI2_ENABLE) - { - /* Disable advance power save if enabled */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - } - - if (rslt == BMI2_OK) - { - /* Search for max burst length */ - feat_found = bmi2_extract_input_feat_config(&maxburst_length_bytes, BMI2_MAX_BURST_LEN, dev); - if (feat_found) - { - rslt = bmi2_get_feat_config(maxburst_length_bytes.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for max burst length */ - idx = maxburst_length_bytes.start_addr; - - /* Get the max burst length */ - *max_burst_len = feat_config[idx]; - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - /* Enable Advance power save if disabled while configuring and - * not when already disabled - */ - if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) - { - rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - } - } - - return rslt; -} - -/*! - * @brief This internal API sets the max burst length. - */ -static int8_t set_maxburst_len(const uint16_t write_len_byte, struct bmi2_dev *dev) -{ - int8_t rslt = BMI2_OK; - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - uint8_t idx = 0; - uint8_t reg_addr = 0; - uint8_t max_burst_len = 0; - uint8_t feat_found = 0; - struct bmi2_feature_config maxburst_length_bytes = { 0, 0, 0 }; - uint8_t aps_stat; - uint16_t burst_len = write_len_byte / 2; - - /* for variant that support crt outside fifo, do not modify the max burst len */ - if ((dev->variant_feature & BMI2_CRT_IN_FIFO_NOT_REQ) != 0) - { - return BMI2_OK; - } - - /* Max burst length is only 1 byte */ - if (burst_len > BMI2_CRT_MAX_BURST_WORD_LENGTH) - { - max_burst_len = UINT8_C(0xFF); - } - else - { - max_burst_len = (uint8_t)burst_len; - } - - /* Get status of advance power save mode */ - aps_stat = dev->aps_status; - if (aps_stat == BMI2_ENABLE) - { - /* Disable advance power save if enabled */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - } - - if (rslt == BMI2_OK) - { - /* Search for axis-re-mapping and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&maxburst_length_bytes, BMI2_MAX_BURST_LEN, dev); - if (feat_found) - { - /* Get the configuration from the page where axis - * re-mapping feature resides - */ - rslt = bmi2_get_feat_config(maxburst_length_bytes.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes */ - idx = maxburst_length_bytes.start_addr; - - /* update Max burst length */ - feat_config[idx] = max_burst_len; - - /* Update the register address */ - reg_addr = BMI2_FEATURES_REG_ADDR + maxburst_length_bytes.start_addr; - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(reg_addr, &feat_config[maxburst_length_bytes.start_addr], 2, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - /* Enable Advance power save if disabled while configuring and - * not when already disabled - */ - if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) - { - rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - } - } - - return rslt; -} - -/*! - * @brief This api is used to trigger the preparation for system for NVM programming. - */ -static int8_t set_nvm_prep_prog(uint8_t nvm_prep, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt = BMI2_OK; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - uint8_t reg_addr = 0; - - /* Initialize feature configuration for nvm preparation*/ - struct bmi2_feature_config nvm_config = { 0, 0, 0 }; - - /* Search for bmi2 gyro self offset correction feature as nvm program preparation feature is - * present in the same Word and extract its configuration details - */ - feat_found = bmi2_extract_input_feat_config(&nvm_config, BMI2_NVM_PROG_PREP, dev); - if (feat_found) - { - /* Get the configuration from the page where nvm preparation feature enable feature - * resides */ - rslt = bmi2_get_feat_config(nvm_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for nvm preparation feature enable */ - idx = nvm_config.start_addr; - - /* update nvm_prog_prep enable bit */ - feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_NVM_PREP_FEATURE_EN, nvm_prep); - - /* Update the register address */ - reg_addr = BMI2_FEATURES_REG_ADDR + nvm_config.start_addr - 1; - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(reg_addr, &feat_config[nvm_config.start_addr - 1], 2, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This api is used to enable the CRT. - */ -static int8_t select_self_test(uint8_t gyro_st_crt, struct bmi2_dev *dev) -{ - int8_t rslt; - - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - uint8_t idx = 0; - - uint8_t feat_found; - uint8_t reg_addr = 0; - - struct bmi2_feature_config gyro_self_test_crt_config = { 0, 0, 0 }; - - /* Search for bmi2 crt gyro self-test feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&gyro_self_test_crt_config, BMI2_CRT_GYRO_SELF_TEST, dev); - if (feat_found) - { - /* Get the configuration from the page where gyro self-test and crt enable feature - * resides */ - rslt = bmi2_get_feat_config(gyro_self_test_crt_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes */ - idx = gyro_self_test_crt_config.start_addr; - - /* update the gyro self-test crt enable bit */ - feat_config[idx] = BMI2_SET_BIT_POS0(feat_config[idx], BMI2_GYRO_SELF_TEST_CRT_EN, gyro_st_crt); - - /* Update the register address */ - reg_addr = BMI2_FEATURES_REG_ADDR + (gyro_self_test_crt_config.start_addr - 1); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(reg_addr, &feat_config[gyro_self_test_crt_config.start_addr - 1], 2, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This api is used to abort ongoing crt or gyro self-test. - */ -int8_t bmi2_abort_crt_gyro_st(struct bmi2_dev *dev) -{ - int8_t rslt = BMI2_OK; - uint8_t aps_stat; - uint8_t st_running = 0; - uint8_t cmd = BMI2_G_TRIGGER_CMD; - - /* Get status of advance power save mode */ - aps_stat = dev->aps_status; - if (aps_stat == BMI2_ENABLE) - { - /* Disable advance power save if enabled */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - } - - /* Checking for ST running status */ - if (rslt == BMI2_OK) - { - rslt = get_st_running(&st_running, dev); - if (rslt == BMI2_OK) - { - /* ST is not running */ - if (st_running == 0) - { - rslt = BMI2_E_ST_NOT_RUNING; - } - } - } - - if (rslt == BMI2_OK) - { - rslt = abort_bmi2(BMI2_ENABLE, dev); - } - - /* send the g trigger command */ - if (rslt == BMI2_OK) - { - rslt = bmi2_set_regs(BMI2_CMD_REG_ADDR, &cmd, 1, dev); - } - - if (rslt == BMI2_OK) - { - /* wait until st_status = 0 or time out is 2 seconds */ - rslt = wait_st_running(BMI2_CRT_WAIT_RUNNING_RETRY_EXECUTION, dev); - } - - /* Check G trigger status for error */ - if (rslt == BMI2_OK) - { - rslt = crt_gyro_st_update_result(dev); - if (rslt == BMI2_E_ABORT_ERROR) - { - rslt = BMI2_OK; - } - else - { - rslt = BMI2_E_ABORT_ERROR; - } - } - - /* Enable Advance power save if disabled while configuring and - * not when already disabled - */ - if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) - { - rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - } - - return rslt; -} - -/*! - * @brief This api is used to enable/disable abort. - */ -static int8_t abort_bmi2(uint8_t abort_enable, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - uint8_t reg_addr = 0; - - /* Initialize feature configuration for blocking a feature */ - struct bmi2_feature_config block_config = { 0, 0, 0 }; - - /* Search for bmi2 Abort feature and extract its configuration details */ - feat_found = bmi2_extract_input_feat_config(&block_config, BMI2_ABORT_CRT_GYRO_SELF_TEST, dev); - if (feat_found) - { - /* Get the configuration from the page where abort(block) feature resides */ - rslt = bmi2_get_feat_config(block_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes */ - idx = block_config.start_addr; - - /* update the gyro self-test crt abort enable bit */ - feat_config[idx] = BMI2_SET_BITS(feat_config[idx], BMI2_ABORT_FEATURE_EN, abort_enable); - - /* Update the register address */ - reg_addr = BMI2_FEATURES_REG_ADDR + (block_config.start_addr - 1); - - /* Set the configuration back to the page */ - rslt = bmi2_set_regs(reg_addr, &feat_config[block_config.start_addr - 1], 2, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This api is use to wait till gyro self-test is completed and update the status of gyro - * self-test. - */ -static int8_t gyro_self_test_completed(struct bmi2_gyro_self_test_status *gyro_st_result, struct bmi2_dev *dev) -{ - int8_t rslt; - uint8_t reg_data; - - rslt = bmi2_get_regs(BMI2_GYR_SELF_TEST_AXES_ADDR, ®_data, 1, dev); - if (rslt == BMI2_OK) - { - gyro_st_result->gyr_st_axes_done = BMI2_GET_BIT_POS0(reg_data, BMI2_GYR_ST_AXES_DONE); - if (gyro_st_result->gyr_st_axes_done == 0x01) - { - gyro_st_result->gyr_axis_x_ok = BMI2_GET_BITS(reg_data, BMI2_GYR_AXIS_X_OK); - gyro_st_result->gyr_axis_y_ok = BMI2_GET_BITS(reg_data, BMI2_GYR_AXIS_Y_OK); - gyro_st_result->gyr_axis_z_ok = BMI2_GET_BITS(reg_data, BMI2_GYR_AXIS_Z_OK); - } - else - { - rslt = BMI2_E_SELF_TEST_NOT_DONE; - } - } - - return rslt; -} - -/*! - * @brief This api validates accel foc position as per the range - */ -static int8_t validate_foc_position(uint8_t sens_list, - const struct bmi2_accel_foc_g_value *accel_g_axis, - struct bmi2_sens_axes_data avg_foc_data, - struct bmi2_dev *dev) -{ - int8_t rslt = BMI2_E_INVALID_INPUT; - - if (sens_list == BMI2_ACCEL) - { - if (accel_g_axis->x == 1) - { - rslt = validate_foc_accel_axis(avg_foc_data.x, dev); - } - else if (accel_g_axis->y == 1) - { - rslt = validate_foc_accel_axis(avg_foc_data.y, dev); - } - else - { - rslt = validate_foc_accel_axis(avg_foc_data.z, dev); - } - } - else if (sens_list == BMI2_GYRO) - { - if (((avg_foc_data.x >= BMI2_GYRO_FOC_NOISE_LIMIT_NEGATIVE) && - (avg_foc_data.x <= BMI2_GYRO_FOC_NOISE_LIMIT_POSITIVE)) && - ((avg_foc_data.y >= BMI2_GYRO_FOC_NOISE_LIMIT_NEGATIVE) && - (avg_foc_data.y <= BMI2_GYRO_FOC_NOISE_LIMIT_POSITIVE)) && - ((avg_foc_data.z >= BMI2_GYRO_FOC_NOISE_LIMIT_NEGATIVE) && - (avg_foc_data.z <= BMI2_GYRO_FOC_NOISE_LIMIT_POSITIVE))) - { - rslt = BMI2_OK; - } - else - { - rslt = BMI2_E_INVALID_FOC_POSITION; - } - } - - return rslt; -} - -/*! - * @brief This api validates depends on accel foc access input - */ -static int8_t validate_foc_accel_axis(int16_t avg_foc_data, struct bmi2_dev *dev) -{ - struct bmi2_sens_config sens_cfg = { 0 }; - uint8_t range; - int8_t rslt; - - sens_cfg.type = BMI2_ACCEL; - rslt = bmi2_get_sensor_config(&sens_cfg, 1, dev); - range = sens_cfg.cfg.acc.range; - - /* reference LSB value of 16G */ - if ((range == BMI2_ACC_RANGE_2G) && (avg_foc_data > BMI2_ACC_2G_MIN_NOISE_LIMIT) && - (avg_foc_data < BMI2_ACC_2G_MAX_NOISE_LIMIT)) - { - rslt = BMI2_OK; - } - /* reference LSB value of 16G */ - else if ((range == BMI2_ACC_RANGE_4G) && (avg_foc_data > BMI2_ACC_4G_MIN_NOISE_LIMIT) && - (avg_foc_data < BMI2_ACC_4G_MAX_NOISE_LIMIT)) - { - rslt = BMI2_OK; - } - /* reference LSB value of 16G */ - else if ((range == BMI2_ACC_RANGE_8G) && (avg_foc_data > BMI2_ACC_8G_MIN_NOISE_LIMIT) && - (avg_foc_data < BMI2_ACC_8G_MAX_NOISE_LIMIT)) - { - rslt = BMI2_OK; - } - /* reference LSB value of 16G */ - else if ((range == BMI2_ACC_RANGE_16G) && (avg_foc_data > BMI2_ACC_16G_MIN_NOISE_LIMIT) && - (avg_foc_data < BMI2_ACC_16G_MAX_NOISE_LIMIT)) - { - rslt = BMI2_OK; - } - else - { - rslt = BMI2_E_INVALID_FOC_POSITION; - } - - return rslt; -} - -/*! @brief This api is used for programming the non volatile memory(nvm) */ -int8_t bmi2_nvm_prog(struct bmi2_dev *dev) -{ - int8_t rslt = BMI2_OK; - - /* Variable to get the status of advance power save */ - uint8_t aps_stat; - uint8_t status; - uint8_t cmd_rdy; - uint8_t reg_data; - uint8_t write_timeout = 100; - - /* Get status of advance power save mode */ - aps_stat = dev->aps_status; - if (aps_stat == BMI2_ENABLE) - { - /* Disable advance power save if enabled */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - } - - /* Check the Write status and proceed only if there is no ongoing write cycle */ - if (rslt == BMI2_OK) - { - rslt = bmi2_get_status(&status, dev); - - cmd_rdy = BMI2_GET_BITS(status, BMI2_CMD_RDY); - if (cmd_rdy) - { - rslt = set_nvm_prep_prog(BMI2_ENABLE, dev); - if (rslt == BMI2_OK) - { - dev->delay_us(40000, dev->intf_ptr); - - /* Set the NVM_CONF.nvm_prog_en bit in order to enable the NVM - * programming */ - reg_data = BMI2_NVM_UNLOCK_ENABLE; - rslt = bmi2_set_regs(BMI2_NVM_CONF_ADDR, ®_data, 1, dev); - if (rslt == BMI2_OK) - { - /* Send NVM prog command to command register */ - reg_data = BMI2_NVM_PROG_CMD; - rslt = bmi2_set_regs(BMI2_CMD_REG_ADDR, ®_data, 1, dev); - } - - /* Wait till write operation is completed */ - if (rslt == BMI2_OK) - { - while (write_timeout--) - { - rslt = bmi2_get_status(&status, dev); - if (rslt == BMI2_OK) - { - cmd_rdy = BMI2_GET_BITS(status, BMI2_CMD_RDY); - - /* Nvm is complete once cmd_rdy is 1, break if 1 */ - if (cmd_rdy) - { - break; - } - - /* Wait till cmd_rdy becomes 1 indicating - * nvm process completes */ - dev->delay_us(20000, dev->intf_ptr); - } - } - } - - if ((rslt == BMI2_OK) && (cmd_rdy != BMI2_TRUE)) - { - rslt = BMI2_E_WRITE_CYCLE_ONGOING; - } - } - } - else - { - rslt = BMI2_E_WRITE_CYCLE_ONGOING; - } - } - - if (rslt == BMI2_OK) - { - /* perform soft reset */ - rslt = bmi2_soft_reset(dev); - } - - /* Enable Advance power save if disabled while configuring and not when already disabled */ - if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) - { - rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - } - - return rslt; -} - -/*! - * @brief This API reads and provides average for 128 samples of sensor data for foc operation - * gyro. - */ -static int8_t get_average_of_sensor_data(uint8_t sens_list, - struct bmi2_foc_temp_value *temp_foc_data, - struct bmi2_dev *dev) -{ - int8_t rslt = 0; - struct bmi2_sensor_data sensor_data = { 0 }; - uint8_t sample_count = 0; - uint8_t datardy_try_cnt; - uint8_t drdy_status = 0; - uint8_t sensor_drdy = 0; - - sensor_data.type = sens_list; - if (sens_list == BMI2_ACCEL) - { - sensor_drdy = BMI2_DRDY_ACC; - } - else - { - sensor_drdy = BMI2_DRDY_GYR; - } - - /* Read sensor values before FOC */ - while (sample_count < BMI2_FOC_SAMPLE_LIMIT) - { - datardy_try_cnt = 5; - do - { - dev->delay_us(20000, dev->intf_ptr); - rslt = bmi2_get_status(&drdy_status, dev); - datardy_try_cnt--; - } while ((rslt == BMI2_OK) && (!(drdy_status & sensor_drdy)) && (datardy_try_cnt)); - - if ((rslt != BMI2_OK) || (datardy_try_cnt == 0)) - { - rslt = BMI2_E_DATA_RDY_INT_FAILED; - break; - } - - rslt = bmi2_get_sensor_data(&sensor_data, 1, dev); - - if (rslt == BMI2_OK) - { - if (sensor_data.type == BMI2_ACCEL) - { - temp_foc_data->x += sensor_data.sens_data.acc.x; - temp_foc_data->y += sensor_data.sens_data.acc.y; - temp_foc_data->z += sensor_data.sens_data.acc.z; - } - else if (sensor_data.type == BMI2_GYRO) - { - temp_foc_data->x += sensor_data.sens_data.gyr.x; - temp_foc_data->y += sensor_data.sens_data.gyr.y; - temp_foc_data->z += sensor_data.sens_data.gyr.z; - } - } - else - { - break; - } - - sample_count++; - } - - if (rslt == BMI2_OK) - { - temp_foc_data->x = (temp_foc_data->x / BMI2_FOC_SAMPLE_LIMIT); - temp_foc_data->y = (temp_foc_data->y / BMI2_FOC_SAMPLE_LIMIT); - temp_foc_data->z = (temp_foc_data->z / BMI2_FOC_SAMPLE_LIMIT); - } - - return rslt; -} - -/*! - * @brief This internal API extract the identification feature from the DMR page - * and retrieve the config file major and minor version. - */ -static int8_t extract_config_file(uint8_t *config_major, uint8_t *config_minor, struct bmi2_dev *dev) -{ - /* Variable to define the result */ - int8_t rslt = BMI2_OK; - - /* Variable to define the array offset */ - uint8_t idx = 0; - - /* Variable to define LSB */ - uint16_t lsb = 0; - - /* Variable to define MSB */ - uint16_t msb = 0; - - /* Variable to define a word */ - uint16_t lsb_msb = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Variable to define advance power save mode status */ - uint8_t aps_stat; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Initialize feature configuration for config file identification */ - struct bmi2_feature_config config_id = { 0, 0, 0 }; - - /* Check the power mode status */ - aps_stat = dev->aps_status; - if (aps_stat == BMI2_ENABLE) - { - /* Disable advance power save if enabled */ - rslt = bmi2_set_adv_power_save(BMI2_DISABLE, dev); - } - - if (rslt == BMI2_OK) - { - /* Search for config file identification feature and extract its configuration - * details */ - feat_found = bmi2_extract_input_feat_config(&config_id, BMI2_CONFIG_ID, dev); - if (feat_found) - { - /* Get the configuration from the page where config file identification - * feature resides */ - rslt = bmi2_get_feat_config(config_id.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset for config file identification */ - idx = config_id.start_addr; - - /* Get word to calculate config file identification */ - lsb = (uint16_t) feat_config[idx++]; - msb = ((uint16_t) feat_config[idx++] << 8); - lsb_msb = lsb | msb; - - /* Get major and minor version */ - *config_major = BMI2_GET_BITS(lsb_msb, BMI2_CONFIG_MAJOR); - *config_minor = BMI2_GET_BIT_POS0(lsb, BMI2_CONFIG_MINOR); - } - } - - /* Enable Advance power save if disabled while configuring and - * not when already disabled - */ - if ((aps_stat == BMI2_ENABLE) && (rslt == BMI2_OK)) - { - rslt = bmi2_set_adv_power_save(BMI2_ENABLE, dev); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - *@brief This internal API is used to map the interrupts to the sensor. - */ -static void extract_feat_int_map(struct bmi2_map_int *map_int, uint8_t type, const struct bmi2_dev *dev) -{ - /* Variable to define loop */ - uint8_t loop = 0; - - /* Search for the interrupts from the input configuration array */ - while (loop < dev->sens_int_map) - { - if (dev->map_int[loop].type == type) - { - *map_int = dev->map_int[loop]; - break; - } - - loop++; - } -} - -/*! - * @brief This internal API gets the saturation status for the gyroscope user - * gain update. - */ -static int8_t get_gyro_gain_update_status(struct bmi2_gyr_user_gain_status *user_gain_stat, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variables to define index */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - /* Initialize feature output for gyroscope user gain status */ - struct bmi2_feature_config user_gain_cfg = { 0, 0, 0 }; - - /* Search for gyroscope user gain status output feature and extract its - * configuration details - */ - feat_found = extract_output_feat_config(&user_gain_cfg, BMI2_GYRO_GAIN_UPDATE, dev); - if (feat_found) - { - /* Get the feature output configuration for gyroscope user gain status */ - rslt = bmi2_get_feat_config(user_gain_cfg.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for gyroscope user gain status */ - idx = user_gain_cfg.start_addr; - - /* Get the saturation status for x-axis */ - user_gain_stat->sat_x = BMI2_GET_BIT_POS0(feat_config[idx], BMI2_GYR_USER_GAIN_SAT_STAT_X); - - /* Get the saturation status for y-axis */ - user_gain_stat->sat_y = BMI2_GET_BITS(feat_config[idx], BMI2_GYR_USER_GAIN_SAT_STAT_Y); - - /* Get the saturation status for z-axis */ - user_gain_stat->sat_z = BMI2_GET_BITS(feat_config[idx], BMI2_GYR_USER_GAIN_SAT_STAT_Z); - - /* Get g trigger status */ - user_gain_stat->g_trigger_status = BMI2_GET_BITS(feat_config[idx], BMI2_G_TRIGGER_STAT); - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - - return rslt; -} - -/*! - * @brief This internal API is used to extract the output feature configuration - * details from the look-up table. - */ -static uint8_t extract_output_feat_config(struct bmi2_feature_config *feat_output, - uint8_t type, - const struct bmi2_dev *dev) -{ - /* Variable to define loop */ - uint8_t loop = 0; - - /* Variable to set flag */ - uint8_t feat_found = BMI2_FALSE; - - /* Search for the output feature from the output configuration array */ - while (loop < dev->out_sens) - { - if (dev->feat_output[loop].type == type) - { - *feat_output = dev->feat_output[loop]; - feat_found = BMI2_TRUE; - break; - } - - loop++; - } - - /* Return flag */ - return feat_found; -} - -/*! - * @brief This internal API gets the cross sensitivity coefficient between - * gyroscope's X and Z axes. - */ -static int8_t get_gyro_cross_sense(int16_t *cross_sense, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Array to define the feature configuration */ - uint8_t feat_config[BMI2_FEAT_SIZE_IN_BYTES] = { 0 }; - - /* Variable to define index */ - uint8_t idx = 0; - - /* Variable to set flag */ - uint8_t feat_found; - - uint8_t corr_fact_zx; - - /* Initialize feature output for gyroscope cross sensitivity */ - struct bmi2_feature_config cross_sense_out_config = { 0, 0, 0 }; - - if (dev->variant_feature & BMI2_MAXIMUM_FIFO_VARIANT) - { - /* For maximum_fifo variant fetch the correction factor from GPIO0 */ - rslt = bmi2_get_regs(BMI2_GYR_CAS_GPIO0_ADDR, &corr_fact_zx, 1, dev); - if (rslt == BMI2_OK) - { - /* Get the gyroscope cross sensitivity coefficient */ - if (corr_fact_zx & BMI2_GYRO_CROSS_AXES_SENSE_SIGN_BIT_MASK) - { - *cross_sense = (int16_t)(((int16_t)corr_fact_zx) - 128); - } - else - { - *cross_sense = (int16_t)(corr_fact_zx); - } - } - } - else - { - /* Search for gyroscope cross sensitivity feature and extract its configuration details */ - feat_found = extract_output_feat_config(&cross_sense_out_config, BMI2_GYRO_CROSS_SENSE, dev); - if (feat_found) - { - /* Get the feature output configuration for gyroscope cross sensitivity - * feature */ - rslt = bmi2_get_feat_config(cross_sense_out_config.page, feat_config, dev); - if (rslt == BMI2_OK) - { - /* Define the offset in bytes for gyroscope cross sensitivity output */ - idx = cross_sense_out_config.start_addr; - - /* discard the MSB as GYR_CAS is of only 7 bit */ - feat_config[idx] = feat_config[idx] & BMI2_GYRO_CROSS_AXES_SENSE_MASK; - - /* Get the gyroscope cross sensitivity coefficient */ - if (feat_config[idx] & BMI2_GYRO_CROSS_AXES_SENSE_SIGN_BIT_MASK) - { - *cross_sense = (int16_t)(((int16_t)feat_config[idx]) - 128); - } - else - { - *cross_sense = (int16_t)(feat_config[idx]); - } - } - } - else - { - rslt = BMI2_E_INVALID_SENSOR; - } - } - - return rslt; -} - -/*! - * @brief This internal API selects the sensor/features to be enabled or - * disabled. - */ -static int8_t select_sensor(const uint8_t *sens_list, uint8_t n_sens, uint64_t *sensor_sel) -{ - /* Variable to define error */ - int8_t rslt = BMI2_OK; - - /* Variable to define loop */ - uint8_t count; - - for (count = 0; count < n_sens; count++) - { - switch (sens_list[count]) - { - case BMI2_ACCEL: - *sensor_sel |= BMI2_ACCEL_SENS_SEL; - break; - case BMI2_GYRO: - *sensor_sel |= BMI2_GYRO_SENS_SEL; - break; - case BMI2_AUX: - *sensor_sel |= BMI2_AUX_SENS_SEL; - break; - case BMI2_TEMP: - *sensor_sel |= BMI2_TEMP_SENS_SEL; - break; - default: - rslt = BMI2_E_INVALID_SENSOR; - break; - } - } - - return rslt; -} - -/*! - * @brief This internal API enables the selected sensor/features. - */ -static int8_t sensor_enable(uint64_t sensor_sel, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to store register values */ - uint8_t reg_data = 0; - - rslt = bmi2_get_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); - if (rslt == BMI2_OK) - { - /* Enable accelerometer */ - if (sensor_sel & BMI2_ACCEL_SENS_SEL) - { - reg_data = BMI2_SET_BITS(reg_data, BMI2_ACC_EN, BMI2_ENABLE); - } - - /* Enable gyroscope */ - if (sensor_sel & BMI2_GYRO_SENS_SEL) - { - reg_data = BMI2_SET_BITS(reg_data, BMI2_GYR_EN, BMI2_ENABLE); - } - - /* Enable auxiliary sensor */ - if (sensor_sel & BMI2_AUX_SENS_SEL) - { - reg_data = BMI2_SET_BIT_POS0(reg_data, BMI2_AUX_EN, BMI2_ENABLE); - } - - /* Enable temperature sensor */ - if (sensor_sel & BMI2_TEMP_SENS_SEL) - { - reg_data = BMI2_SET_BITS(reg_data, BMI2_TEMP_EN, BMI2_ENABLE); - } - - /* Enable the sensors that are set in the power control register */ - if (sensor_sel & BMI2_MAIN_SENSORS) - { - rslt = bmi2_set_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); - } - } - - return rslt; -} - -/*! - * @brief This internal API disables the selected sensors/features. - */ -static int8_t sensor_disable(uint64_t sensor_sel, struct bmi2_dev *dev) -{ - /* Variable to define error */ - int8_t rslt; - - /* Variable to store register values */ - uint8_t reg_data = 0; - - rslt = bmi2_get_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); - if (rslt == BMI2_OK) - { - /* Disable accelerometer */ - if (sensor_sel & BMI2_ACCEL_SENS_SEL) - { - reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_ACC_EN); - } - - /* Disable gyroscope */ - if (sensor_sel & BMI2_GYRO_SENS_SEL) - { - reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_GYR_EN); - } - - /* Disable auxiliary sensor */ - if (sensor_sel & BMI2_AUX_SENS_SEL) - { - reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_AUX_EN); - } - - /* Disable temperature sensor */ - if (sensor_sel & BMI2_TEMP_SENS_SEL) - { - reg_data = BMI2_SET_BIT_VAL0(reg_data, BMI2_TEMP_EN); - } - - /* Enable the sensors that are set in the power control register */ - if (sensor_sel & BMI2_MAIN_SENSORS) - { - rslt = bmi2_set_regs(BMI2_PWR_CTRL_ADDR, ®_data, 1, dev); - } - } - - return rslt; -} - -/*! @endcond */ diff --git a/src/main/drivers/accgyro/bmi2.h b/src/main/drivers/accgyro/bmi2.h deleted file mode 100644 index b1fe627d62..0000000000 --- a/src/main/drivers/accgyro/bmi2.h +++ /dev/null @@ -1,1513 +0,0 @@ -/** -* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. -* -* BSD-3-Clause -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* 1. Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* -* 2. Redistributions in binary form must reproduce the above copyright -* notice, this list of conditions and the following disclaimer in the -* documentation and/or other materials provided with the distribution. -* -* 3. Neither the name of the copyright holder nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) -* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, -* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING -* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -* -* @file bmi2.h -* @date 2020-11-04 -* @version v2.63.1 -* -*/ - -/*! - * @defgroup bmi2xy BMI2XY - */ - -/** - * \ingroup bmi2xy - * \defgroup bmi2 BMI2 - * @brief Sensor driver for BMI2 sensor - */ - -#ifndef BMI2_H_ -#define BMI2_H_ - -/*! CPP guard */ -#ifdef __cplusplus -extern "C" { -#endif - -/***************************************************************************/ - -/*! Header files - ****************************************************************************/ -#include "bmi2_defs.h" - -/***************************************************************************/ - -/*! BMI2XY User Interface function prototypes - ****************************************************************************/ - -/** - * \ingroup bmi2 - * \defgroup bmi2ApiInit Initialization - * @brief Initialize the sensor and device structure - */ - -/*! - * \ingroup bmi2ApiInit - * \page bmi2_api_bmi2_sec_init bmi2_sec_init - * \code - * int8_t bmi2_sec_init(struct bmi2_dev *dev); - * \endcode - * @details This API is the entry point for bmi2 sensor. It selects between - * I2C/SPI interface, based on user selection. It also reads the chip-id of - * the sensor. - * - * @param[in,out] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_sec_init(struct bmi2_dev *dev); - -/** - * \ingroup bmi2 - * \defgroup bmi2ApiRegs Registers - * @brief Set / Get data from the given register address of the sensor - */ - -/*! - * \ingroup bmi2ApiRegs - * \page bmi2_api_bmi2_get_regs bmi2_get_regs - * \code - * int8_t bmi2_get_regs(uint8_t reg_addr, uint8_t *data, uint16_t len, const struct bmi2_dev *dev); - * \endcode - * @details This API reads the data from the given register address of bmi2 - * sensor. - * - * @param[in] reg_addr : Register address from which data is read. - * @param[out] data : Pointer to data buffer where read data is stored. - * @param[in] len : No. of bytes of data to be read. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @note For most of the registers auto address increment applies, with the - * exception of a few special registers, which trap the address. For e.g., - * Register address - 0x26, 0x5E. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_get_regs(uint8_t reg_addr, uint8_t *data, uint16_t len, struct bmi2_dev *dev); - -/*! - * \ingroup bmi2ApiRegs - * \page bmi2_api_bmi2_set_regs bmi2_set_regs - * \code - * int8_t bmi2_set_regs(uint8_t reg_addr, const uint8_t *data, uint16_t len, struct bmi2_dev *dev); - * \endcode - * @details This API writes data to the given register address of bmi2 sensor. - * - * @param[in] reg_addr : Register address to which the data is written. - * @param[in] data : Pointer to data buffer in which data to be written - * is stored. - * @param[in] len : No. of bytes of data to be written. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_set_regs(uint8_t reg_addr, const uint8_t *data, uint16_t len, struct bmi2_dev *dev); - -/** - * \ingroup bmi2 - * \defgroup bmi2ApiSR Soft reset - * @brief Set / Get data from the given register address of the sensor - */ - -/*! - * \ingroup bmi2ApiSR - * \page bmi2_api_bmi2_soft_reset bmi2_soft_reset - * \code - * int8_t bmi2_soft_reset(struct bmi2_dev *dev); - * \endcode - * @details This API resets bmi2 sensor. All registers are overwritten with - * their default values. - * - * @note If selected interface is SPI, an extra dummy byte is read to bring the - * interface back to SPI from default, after the soft reset command. - * - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_soft_reset(struct bmi2_dev *dev); - -/** - * \ingroup bmi2 - * \defgroup bmi2ApiConfig Configuration - * @brief Functions related to configuration of the sensor - */ - -/*! - * \ingroup bmi2ApiConfig - * \page bmi2_api_bmi2_get_config_file_version bmi2_get_config_file_version - * \code - * int8_t bmi2_get_config_file_version(uint8_t *config_major, uint8_t *config_minor, struct bmi2_dev *dev); - * \endcode - * @details This API is used to get the config file major and minor information. - * - * @param[in] dev : Structure instance of bmi2_dev. - * @param[out] config_major : pointer to data buffer to store the config major. - * @param[out] config_minor : pointer to data buffer to store the config minor. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_get_config_file_version(uint8_t *config_major, uint8_t *config_minor, struct bmi2_dev *dev); - -/** - * \ingroup bmi2 - * \defgroup bmi2ApiPowersave Advanced power save mode - * @brief Set / Get Advanced power save mode of the sensor - */ - -/*! - * \ingroup bmi2ApiPowersave - * \page bmi2_api_bmi2_set_adv_power_save bmi2_set_adv_power_save - * \code - * int8_t bmi2_set_adv_power_save(uint8_t enable, struct bmi2_dev *dev); - * \endcode - * @details This API enables/disables the advance power save mode in the sensor. - * - * @param[in] enable : To enable/disable advance power mode. - * @param[in] dev : Structure instance of bmi2_dev. - * - *@verbatim - * Enable | Description - * -------------|--------------- - * BMI2_DISABLE | Disables advance power save. - * BMI2_ENABLE | Enables advance power save. - *@endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_set_adv_power_save(uint8_t enable, struct bmi2_dev *dev); - -/*! - * \ingroup bmi2ApiPowersave - * \page bmi2_api_bmi2_get_adv_power_save bmi2_get_adv_power_save - * \code - * int8_t bmi2_get_adv_power_save(uint8_t *aps_status, struct bmi2_dev *dev); - * \endcode - * @details This API gets the status of advance power save mode in the sensor. - * - * @param[out] aps_status : Pointer to get the status of APS mode. - * @param[in] dev : Structure instance of bmi2_dev. - * - *@verbatim - * aps_status | Description - * -------------|--------------- - * BMI2_DISABLE | Advance power save disabled. - * BMI2_ENABLE | Advance power save enabled. - *@endverbatim - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_SET_APS_FAIL - Error: Set Advance Power Save Fail - */ -int8_t bmi2_get_adv_power_save(uint8_t *aps_status, struct bmi2_dev *dev); - -/*! - * \ingroup bmi2ApiConfig - * \page bmi2_api_bmi2_write_config_file bmi2_write_config_file - * \code - * int8_t bmi2_write_config_file(struct bmi2_dev *dev); - * \endcode - * @details This API loads the configuration file to the bmi2 sensor. - * - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_write_config_file(struct bmi2_dev *dev); - -/** - * \ingroup bmi2 - * \defgroup bmi2ApiInt Interrupt - * @brief Interrupt operations of the sensor - */ - -/*! - * \ingroup bmi2ApiInt - * \page bmi2_api_bmi2_set_int_pin_config bmi2_set_int_pin_config - * \code - * int8_t bmi2_set_int_pin_config(const struct bmi2_int_pin_config *int_cfg, struct bmi2_dev *dev); - * \endcode - * @details This API sets: - * 1) The input output configuration of the selected interrupt pin: - * INT1 or INT2. - * 2) The interrupt mode: permanently latched or non-latched. - * - * @param[in] int_cfg : Structure instance of bmi2_int_pin_config. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_set_int_pin_config(const struct bmi2_int_pin_config *int_cfg, struct bmi2_dev *dev); - -/*! - * \ingroup bmi2ApiInt - * \page bmi2_api_bmi2_get_int_pin_config bmi2_get_int_pin_config - * \code - * int8_t bmi2_get_int_pin_config(struct bmi2_int_pin_config *int_cfg, const struct bmi2_dev *dev); - * \endcode - * @details This API gets: - * 1) The input output configuration of the selected interrupt pin: - * INT1 or INT2. - * 2) The interrupt mode: permanently latched or non-latched. - * - * @param[in,out] int_cfg : Structure instance of bmi2_int_pin_config. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_get_int_pin_config(struct bmi2_int_pin_config *int_cfg, struct bmi2_dev *dev); - -/*! - * \ingroup bmi2ApiInt - * \page bmi2_api_bmi2_get_int_status bmi2_get_int_status - * \code - * int8_t bmi2_get_int_status(uint16_t *int_status, const struct bmi2_dev *dev); - * \endcode - * @details This API gets the interrupt status of both feature and data - * interrupts. - * - * @param[out] int_status : Pointer to get the status of the interrupts. - * @param[in] dev : Structure instance of bmi2_dev. - * - *@verbatim - * int_status | Status - * -----------|------------ - * 0x00 | BIT0 - * 0x01 | BIT1 - * 0x02 | BIT2 - * 0x03 | BIT3 - * 0x04 | BIT4 - * 0x05 | BIT5 - * 0x06 | BIT6 - * 0x07 | BIT7 - *@endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_get_int_status(uint16_t *int_status, struct bmi2_dev *dev); - -/** - * \ingroup bmi2 - * \defgroup bmi2ApiSensorC Sensor Configuration - * @brief Enable / Disable feature configuration of the sensor - */ - -/*! - * \ingroup bmi2ApiSensorC - * \page bmi2_api_bmi2_set_sensor_config bmi2_set_sensor_config - * \code - * int8_t bmi2_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); - * \endcode - * @details This API sets the sensor/feature configuration. - * - * @param[in] sens_cfg : Structure instance of bmi2_sens_config. - * @param[in] n_sens : Number of sensors selected. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @note Sensors/features that can be configured - * - *@verbatim - * sens_list | Values - * ----------------------------|----------- - * BMI2_ACCEL | 0 - * BMI2_GYRO | 1 - * BMI2_AUX | 2 - * BMI2_GYRO_GAIN_UPDATE | 9 - *@endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_set_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); - -/*! - * \ingroup bmi2ApiSensorC - * \page bmi2_api_bmi2_get_sensor_config bmi2_get_sensor_config - * \code - * int8_t bmi2_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); - * \endcode - * @details This API gets the sensor/feature configuration. - * - * @param[in] sens_cfg : Structure instance of bmi2_sens_config. - * @param[in] n_sens : Number of sensors selected. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @note Sensors/features whose configurations can be read. - * - *@verbatim - * sens_list | Values - * -------------------------|----------- - * BMI2_ACCEL | 0 - * BMI2_GYRO | 1 - * BMI2_AUX | 2 - * BMI2_GYRO_GAIN_UPDATE | 9 - *@endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_get_sensor_config(struct bmi2_sens_config *sens_cfg, uint8_t n_sens, struct bmi2_dev *dev); - -/** - * \ingroup bmi2 - * \defgroup bmi2ApiSensor Feature Set - * @brief Enable / Disable features of the sensor - */ - -/*! - * \ingroup bmi2ApiSensor - * \page bmi2_api_bmi2_sensor_enable bmi2_sensor_enable - * \code - * int8_t bmi2_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); - * \endcode - * @details This API selects the sensors/features to be enabled. - * - * @param[in] sens_list : Pointer to select the sensor/feature. - * @param[in] n_sens : Number of sensors selected. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @note Sensors/features that can be enabled. - * - *@verbatim - * sens_list | Values - * -------------------------|----------- - * BMI2_ACCEL | 0 - * BMI2_GYRO | 1 - * BMI2_AUX | 2 - * BMI2_TEMP | 31 - *@endverbatim - * - * @note : - * example uint8_t sens_list[2] = {BMI2_ACCEL, BMI2_GYRO}; - * uint8_t n_sens = 2; - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_sensor_enable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); - -/*! - * \ingroup bmi2ApiSensor - * \page bmi2_api_bmi2_sensor_disable bmi2_sensor_disable - * \code - * int8_t bmi2_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); - * \endcode - * @details This API selects the sensors/features to be disabled. - * - * @param[in] sens_list : Pointer to select the sensor/feature. - * @param[in] n_sens : Number of sensors selected. - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @note Sensors/features that can be disabled. - * - *@verbatim - * sens_list | Values - * ----------------------------|----------- - * BMI2_ACCEL | 0 - * BMI2_GYRO | 1 - * BMI2_AUX | 2 - * BMI2_TEMP | 31 - *@endverbatim - * - * @note : - * example uint8_t sens_list[2] = {BMI2_ACCEL, BMI2_GYRO}; - * uint8_t n_sens = 2; - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_sensor_disable(const uint8_t *sens_list, uint8_t n_sens, struct bmi2_dev *dev); - -/** - * \ingroup bmi2 - * \defgroup bmi2ApiSensorD Sensor Data - * @brief Get sensor data - */ - -/*! - * \ingroup bmi2ApiSensorD - * \page bmi2_api_bmi2_get_sensor_data bmi2_get_sensor_data - * \code - * int8_t bmi2_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev); - * \endcode - * @details This API gets the sensor/feature data for accelerometer, gyroscope, - * auxiliary sensor, step counter, high-g, gyroscope user-gain update, - * orientation, gyroscope cross sensitivity and error status for NVM and VFRM. - * - * @param[out] sensor_data : Structure instance of bmi2_sensor_data. - * @param[in] n_sens : Number of sensors selected. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @note Sensors/features whose data can be read - * - *@verbatim - * sens_list | Values - * ---------------------|----------- - * BMI2_ACCEL | 0 - * BMI2_GYRO | 1 - * BMI2_AUX | 2 - * BMI2_GYRO_GAIN_UPDATE| 12 - * BMI2_GYRO_CROSS_SENSE| 28 - *@endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_get_sensor_data(struct bmi2_sensor_data *sensor_data, uint8_t n_sens, struct bmi2_dev *dev); - -/** - * \ingroup bmi2 - * \defgroup bmi2ApiFIFO FIFO - * @brief FIFO operations of the sensor - */ - -/*! - * \ingroup bmi2ApiFIFO - * \page bmi2_api_bmi2_set_fifo_config bmi2_set_fifo_config - * \code - * int8_t bmi2_set_fifo_config(uint16_t config, uint8_t enable, struct bmi2_dev *dev); - * \endcode - * @details This API sets the FIFO configuration in the sensor. - * - * @param[in] config : FIFO configurations to be enabled/disabled. - * @param[in] enable : Enable/Disable FIFO configurations. - * @param[in] dev : Structure instance of bmi2_dev. - * - *@verbatim - * enable | Description - * -------------|--------------- - * BMI2_DISABLE | Disables FIFO configuration. - * BMI2_ENABLE | Enables FIFO configuration. - *@endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_set_fifo_config(uint16_t config, uint8_t enable, struct bmi2_dev *dev); - -/*! - * \ingroup bmi2ApiFIFO - * \page bmi2_api_bmi2_get_fifo_config bmi2_get_fifo_config - * \code - * int8_t bmi2_get_fifo_config(uint16_t *fifo_config, const struct bmi2_dev *dev); - * \endcode - * @details This API gets the FIFO configuration from the sensor. - * - * @param[out] fifo_config : Pointer variable to get FIFO configuration value. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_get_fifo_config(uint16_t *fifo_config, struct bmi2_dev *dev); - -/*! - * \ingroup bmi2ApiFIFO - * \page bmi2_api_bmi2_read_fifo_data bmi2_read_fifo_data - * \code - * int8_t bmi2_read_fifo_data(struct bmi2_fifo_frame *fifo, const struct bmi2_dev *dev); - * \endcode - * @details This API reads FIFO data. - * - * @param[in, out] fifo : Structure instance of bmi2_fifo_frame. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @note APS has to be disabled before calling this function. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_read_fifo_data(struct bmi2_fifo_frame *fifo, struct bmi2_dev *dev); - -/*! - * \ingroup bmi2ApiFIFO - * \page bmi2_api_bmi2_extract_accel bmi2_extract_accel - * \code - * int8_t bmi2_extract_accel(struct bmi2_sens_axes_data *accel_data, - * uint16_t *accel_length, - * struct bmi2_fifo_frame *fifo, - * const struct bmi2_dev *dev); - * \endcode - * @details This API parses and extracts the accelerometer frames from FIFO data read by - * the "bmi2_read_fifo_data" API and stores it in the "accel_data" structure - * instance. - * - * @param[out] accel_data : Structure instance of bmi2_sens_axes_data - * where the parsed data bytes are stored. - * @param[in,out] accel_length : Number of accelerometer frames. - * @param[in,out] fifo : Structure instance of bmi2_fifo_frame. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_extract_accel(struct bmi2_sens_axes_data *accel_data, - uint16_t *accel_length, - struct bmi2_fifo_frame *fifo, - const struct bmi2_dev *dev); - -/*! - * \ingroup bmi2ApiFIFO - * \page bmi2_api_bmi2_extract_aux bmi2_extract_aux - * \code - * int8_t bmi2_extract_aux(struct bmi2_aux_fifo_data *aux, - * uint16_t *aux_length, - * struct bmi2_fifo_frame *fifo, - * const struct bmi2_dev *dev); - * - * \endcode - * @details This API parses and extracts the auxiliary frames from FIFO data - * read by the "bmi2_read_fifo_data" API and stores it in "aux_data" buffer. - * - * @param[out] aux : Pointer to structure where the parsed auxiliary - * data bytes are stored. - * @param[in,out] aux_length : Number of auxiliary frames. - * @param[in,out] fifo : Structure instance of bmi2_fifo_frame. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_extract_aux(struct bmi2_aux_fifo_data *aux, - uint16_t *aux_length, - struct bmi2_fifo_frame *fifo, - const struct bmi2_dev *dev); - -/*! - * \ingroup bmi2ApiFIFO - * \page bmi2_api_bmi2_extract_gyro bmi2_extract_gyro - * \code - * int8_t bmi2_extract_gyro(struct bmi2_sens_axes_data *gyro_data, - * uint16_t *gyro_length, - * struct bmi2_fifo_frame *fifo, - * const struct bmi2_dev *dev); - * \endcode - * @details This API parses and extracts the gyroscope frames from FIFO data read by the - * "bmi2_read_fifo_data" API and stores it in the "gyro_data" - * structure instance. - * - * @param[out] gyro_data : Structure instance of bmi2_sens_axes_data - * where the parsed data bytes are stored. - * @param[in,out] gyro_length : Number of gyroscope frames. - * @param[in,out] fifo : Structure instance of bmi2_fifo_frame. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_extract_gyro(struct bmi2_sens_axes_data *gyro_data, - uint16_t *gyro_length, - struct bmi2_fifo_frame *fifo, - const struct bmi2_dev *dev); - -/** - * \ingroup bmi2 - * \defgroup bmi2ApiCmd Command Register - * @brief Write commands to the sensor - */ - -/*! - * \ingroup bmi2ApiCmd - * \page bmi2_api_bmi2_set_command_register bmi2_set_command_register - * \code - * int8_t bmi2_set_command_register(uint8_t command, struct bmi2_dev *dev); - * \endcode - * @details This API writes the available sensor specific commands to the sensor. - * - * @param[in] command : Commands to be given to the sensor. - * @param[in] dev : Structure instance of bmi2_dev. - * - *@verbatim - * Commands | Values - * ---------------------|--------------------- - * BMI2_SOFT_RESET_CMD | 0xB6 - * BMI2_FIFO_FLUSH_CMD | 0xB0 - * BMI2_USR_GAIN_CMD | 0x03 - *@endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_set_command_register(uint8_t command, struct bmi2_dev *dev); - -/*! - * \ingroup bmi2ApiFIFO - * \page bmi2_api_bmi2_set_fifo_self_wake_up bmi2_set_fifo_self_wake_up - * \code - * int8_t bmi2_set_fifo_self_wake_up(uint8_t fifo_self_wake_up, struct bmi2_dev *dev); - * \endcode - * @details This API sets the FIFO self wake up functionality in the sensor. - * - * @param[in] fifo_self_wake_up : Variable to set FIFO self wake-up. - * @param[in] dev : Structure instance of bmi2_dev. - * - *@verbatim - * fifo_self_wake_up | Description - * -------------------|--------------- - * BMI2_DISABLE | Disables self wake-up. - * BMI2_ENABLE | Enables self wake-up. - *@endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_set_fifo_self_wake_up(uint8_t fifo_self_wake_up, struct bmi2_dev *dev); - -/*! - * \ingroup bmi2ApiFIFO - * \page bmi2_api_bmi2_get_fifo_self_wake_up bmi2_get_fifo_self_wake_up - * \code - * int8_t bmi2_get_fifo_self_wake_up(uint8_t *fifo_self_wake_up, const struct bmi2_dev *dev); - * \endcode - * @details This API gets the FIFO self wake up functionality from the sensor. - * - * @param[out] fifo_self_wake_up : Pointer variable to get the status of FIFO - * self wake-up. - * @param[in] dev : Structure instance of bmi2_dev. - * - *@verbatim - * fifo_self_wake_up | Description - * -------------------|--------------- - * BMI2_DISABLE | Self wake-up disabled - * BMI2_ENABLE | Self wake-up enabled. - *@endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_get_fifo_self_wake_up(uint8_t *fifo_self_wake_up, struct bmi2_dev *dev); - -/*! - * \ingroup bmi2ApiFIFO - * \page bmi2_api_bmi2_set_fifo_wm bmi2_set_fifo_wm - * \code - * int8_t bmi2_set_fifo_wm(uint16_t fifo_wm, struct bmi2_dev *dev); - * \endcode - * @details This API sets the FIFO water mark level which is set in the sensor. - * - * @param[in] fifo_wm : Variable to set FIFO water-mark level. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_set_fifo_wm(uint16_t fifo_wm, struct bmi2_dev *dev); - -/*! - * \ingroup bmi2ApiFIFO - * \page bmi2_api_bmi2_get_fifo_wm bmi2_get_fifo_wm - * \code - * int8_t bmi2_get_fifo_wm(uint16_t *fifo_wm, const struct bmi2_dev *dev); - * \endcode - * @details This API gets the FIFO water mark level which is set in the sensor. - * - * @param[out] fifo_wm : Pointer variable to store FIFO water-mark level. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_get_fifo_wm(uint16_t *fifo_wm, struct bmi2_dev *dev); - -/*! - * \ingroup bmi2ApiFIFO - * \page bmi2_api_bmi2_set_fifo_filter_data bmi2_set_fifo_filter_data - * \code - * int8_t bmi2_set_fifo_filter_data(uint8_t sens_sel, uint8_t fifo_filter_data, struct bmi2_dev *dev); - * \endcode - * @details This API sets either filtered or un-filtered FIFO accelerometer or - * gyroscope data. - * - * @param[in] sens_sel : Selects either accelerometer or - * gyroscope sensor. - * @param[in] fifo_filter_data : Variable to set the filter data. - * @param[in] dev : Structure instance of bmi2_dev. - * - *@verbatim - * sens_sel | values - * -----------------|---------- - * BMI2_ACCEL | 0x01 - * BMI2_GYRO | 0x02 - *@endverbatim - * - *@verbatim - * Value | fifo_filter_data - * ---------|--------------------- - * 0x00 | Un-filtered data - * 0x01 | Filtered data - *@endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_set_fifo_filter_data(uint8_t sens_sel, uint8_t fifo_filter_data, struct bmi2_dev *dev); - -/*! - * \ingroup bmi2ApiFIFO - * \page bmi2_api_bmi2_get_fifo_filter_data bmi2_get_fifo_filter_data - * \code - * int8_t bmi2_get_fifo_filter_data(uint8_t sens_sel, uint8_t *fifo_filter_data, const struct bmi2_dev *dev); - * \endcode - * @details This API gets the FIFO accelerometer or gyroscope filter data. - * - * @param[in] sens_sel : Selects either accelerometer or - * gyroscope sensor. - * @param[out] fifo_filter_data : Pointer variable to get the filter data. - * @param[in] dev : Structure instance of bmi2_dev. - * - *@verbatim - * sens_sel | values - * -----------------|---------- - * BMI2_ACCEL | 0x01 - * BMI2_GYRO | 0x02 - *@endverbatim - * - *@verbatim - * Value | fifo_filter_data - * ---------|--------------------- - * 0x00 | Un-filtered data - * 0x01 | Filtered data - *@endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_get_fifo_filter_data(uint8_t sens_sel, uint8_t *fifo_filter_data, struct bmi2_dev *dev); - -/*! - * \ingroup bmi2ApiFIFO - * \page bmi2_api_bmi2_set_fifo_down_sample bmi2_set_fifo_down_sample - * \code - * int8_t bmi2_set_fifo_down_sample(uint8_t sens_sel, uint8_t fifo_down_samp, struct bmi2_dev *dev); - * \endcode - * @details This API sets the down sampling rate for FIFO accelerometer or - * gyroscope FIFO data. - * - * @param[in] sens_sel : Selects either either accelerometer or - * gyroscope sensor. - * @param[in] fifo_down_samp : Variable to set the down sampling rate. - * @param[in] dev : Structure instance of bmi2_dev. - * - *@verbatim - * sens_sel | values - * ----------------|---------- - * BMI2_ACCEL | 0x01 - * BMI2_GYRO | 0x02 - *@endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_set_fifo_down_sample(uint8_t sens_sel, uint8_t fifo_down_samp, struct bmi2_dev *dev); - -/*! - * \ingroup bmi2ApiFIFO - * \page bmi2_api_bmi2_get_fifo_down_sample bmi2_get_fifo_down_sample - * \code - * int8_t bmi2_get_fifo_down_sample(uint8_t sens_sel, uint8_t *fifo_down_samp, const struct bmi2_dev *dev); - * \endcode - * @details This API gets the down sampling rate, configured for FIFO - * accelerometer or gyroscope data. - * - * @param[in] sens_sel : Selects either either accelerometer or - * gyroscope sensor. - * @param[out] fifo_down_samp : Pointer variable to store the down sampling rate - * @param[in] dev : Structure instance of bmi2_dev. - * - *@verbatim - * sens_sel | values - * ----------------|---------- - * BMI2_ACCEL | 0x01 - * BMI2_GYRO | 0x02 - *@endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_get_fifo_down_sample(uint8_t sens_sel, uint8_t *fifo_down_samp, struct bmi2_dev *dev); - -/*! - * \ingroup bmi2ApiFIFO - * \page bmi2_api_bmi2_get_fifo_length bmi2_get_fifo_length - * \code - * int8_t bmi2_get_fifo_length(uint16_t *fifo_length, const struct bmi2_dev *dev); - * \endcode - * @details This API gets the length of FIFO data available in the sensor in - * bytes. - * - * @param[out] fifo_length : Pointer variable to store the value of FIFO byte - * counter. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @note The byte counter is updated each time a complete frame is read or - * written. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_get_fifo_length(uint16_t *fifo_length, struct bmi2_dev *dev); - -/** - * \ingroup bmi2 - * \defgroup bmi2ApiOIS OIS - * @brief OIS operations of the sensor - */ - -/*! - * \ingroup bmi2ApiOIS - * \page bmi2_api_bmi2_set_ois_interface bmi2_set_ois_interface - * \code - * int8_t bmi2_set_ois_interface(uint8_t enable, struct bmi2_dev *dev); - * \endcode - * @details This API enables/disables OIS interface. - * - * @param[in] enable : To enable/disable OIS interface. - * @param[in] dev : Structure instance of bmi2_dev. - * - *@verbatim - * Enable | Description - * -------------|--------------- - * BMI2_DISABLE | Disables OIS interface. - * BMI2_ENABLE | Enables OIS interface. - *@endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_set_ois_interface(uint8_t enable, struct bmi2_dev *dev); - -/** - * \ingroup bmi2 - * \defgroup bmi2ApiAux Auxiliary sensor - * @brief Auxiliary sensor operations of the sensor - */ - -/*! - * \ingroup bmi2ApiAux - * \page bmi2_api_bmi2_read_aux_man_mode bmi2_read_aux_man_mode - * \code - * int8_t bmi2_read_aux_man_mode(uint8_t reg_addr, uint8_t *aux_data, uint16_t len, struct bmi2_dev *dev); - * \endcode - * @details This API reads the user-defined bytes of data from the given register - * address of auxiliary sensor in manual mode. - * - * @param[in] reg_addr : Address from where data is read. - * @param[out] aux_data : Pointer to the stored buffer. - * @param[in] len : Total length of data to be read. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @note Change of BMI2_AUX_RD_ADDR is only allowed if AUX is not busy. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_read_aux_man_mode(uint8_t reg_addr, uint8_t *aux_data, uint16_t len, struct bmi2_dev *dev); - -/*! - * \ingroup bmi2ApiAux - * \page bmi2_api_bmi2_write_aux_man_mode bmi2_write_aux_man_mode - * \code - * int8_t bmi2_write_aux_man_mode(uint8_t reg_addr, const uint8_t *aux_data, uint16_t len, struct bmi2_dev *dev); - * \endcode - * @details This API writes the user-defined bytes of data and the address of - * auxiliary sensor where data is to be written in manual mode. - * - * @param[in] reg_addr : AUX address where data is to be written. - * @param[in] aux_data : Pointer to data to be written. - * @param[in] len : Total length of data to be written. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @note Change of BMI2_AUX_WR_ADDR is only allowed if AUX is not busy. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_write_aux_man_mode(uint8_t reg_addr, const uint8_t *aux_data, uint16_t len, struct bmi2_dev *dev); - -/*! - * \ingroup bmi2ApiAux - * \page bmi2_api_bmi2_write_aux_interleaved bmi2_write_aux_interleaved - * \code - * int8_t bmi2_write_aux_interleaved(uint8_t reg_addr, const uint8_t *aux_data, uint16_t len, struct bmi2_dev *dev); - * \endcode - * @details This API writes the user-defined bytes of data and the address of - * auxiliary sensor where data is to be written, from an interleaved input, - * in manual mode. - * - * @param[in] reg_addr : AUX address where data is to be written. - * @param[in] aux_data : Pointer to data to be written. - * @param[in] len : Total length of data to be written. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @note Change of BMI2_AUX_WR_ADDR is only allowed if AUX is not busy. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_write_aux_interleaved(uint8_t reg_addr, const uint8_t *aux_data, uint16_t len, struct bmi2_dev *dev); - -/** - * \ingroup bmi2 - * \defgroup bmi2ApiStatus Sensor Status - * @brief Get sensor status - */ - -/*! - * \ingroup bmi2ApiStatus - * \page bmi2_api_bmi2_get_status bmi2_get_status - * \code - * int8_t bmi2_get_status(uint8_t *status, const struct bmi2_dev *dev); - * \endcode - * @details This API gets the data ready status of accelerometer, gyroscope, - * auxiliary, ready status of command decoder and busy status of auxiliary. - * - * @param[out] status : Pointer variable to the status. - * @param[in] dev : Structure instance of bmi2_dev. - * - *@verbatim - * Value | Status - * ---------|--------------------- - * 0x80 | DRDY_ACC - * 0x40 | DRDY_GYR - * 0x20 | DRDY_AUX - * 0x10 | CMD_RDY - * 0x04 | AUX_BUSY - *@endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_get_status(uint8_t *status, struct bmi2_dev *dev); - -/** - * \ingroup bmi2 - * \defgroup bmi2ApiWSync Sync commands - * @brief Write sync commands - */ - -/*! - * \ingroup bmi2ApiWSync - * \page bmi2_api_bmi2_write_sync_commands bmi2_write_sync_commands - * \code - * int8_t bmi2_write_sync_commands(const uint8_t *command, uint8_t n_comm, struct bmi2_dev *dev); - * \endcode - * @details This API can be used to write sync commands like ODR, sync period, - * frequency and phase, resolution ratio, sync time and delay time. - * - * @param[in] command : Sync command to be written. - * @param[in] n_comm : Length of the command. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_write_sync_commands(const uint8_t *command, uint8_t n_comm, struct bmi2_dev *dev); - -/** - * \ingroup bmi2 - * \defgroup bmi2ApiASelftest Accel self test - * @brief Perform accel self test - */ - -/*! - * \ingroup bmi2ApiASelftest - * \page bmi2_api_bmi2_perform_accel_self_test bmi2_perform_accel_self_test - * \code - * int8_t bmi2_perform_accel_self_test(struct bmi2_dev *dev); - * \endcode - * @details This API performs self-test to check the proper functionality of the - * accelerometer sensor. - * - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_perform_accel_self_test(struct bmi2_dev *dev); - -/*! - * \ingroup bmi2ApiInt - * \page bmi2_api_bmi2_map_feat_int bmi2_map_feat_int - * \code - * int8_t bmi2_map_feat_int(const struct bmi2_sens_int_config *sens_int, struct bmi2_dev *dev); - * \endcode - * @details This API maps/unmaps feature interrupts to that of interrupt pins. - * - * @param[in] sens_int : Structure instance of bmi2_sens_int_config. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_map_feat_int(uint8_t type, enum bmi2_hw_int_pin hw_int_pin, struct bmi2_dev *dev); - -/*! - * \ingroup bmi2ApiInt - * \page bmi2_api_bmi2_map_data_int bmi2_map_data_int - * \code - * int8_t bmi2_map_data_int(uint8_t data_int, enum bmi2_hw_int_pin int_pin, struct bmi2_dev *dev); - * \endcode - * @details This API maps/un-maps data interrupts to that of interrupt pins. - * - * @param[in] int_pin : Interrupt pin selected. - * @param[in] data_int : Type of data interrupt to be mapped. - * @param[in] dev : Structure instance of bmi2_dev. - * - *@verbatim - * data_int | Mask values - * ---------------------|--------------------- - * BMI2_FFULL_INT | 0x01 - * BMI2_FWM_INT | 0x02 - * BMI2_DRDY_INT | 0x04 - * BMI2_ERR_INT | 0x08 - *@endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_map_data_int(uint8_t data_int, enum bmi2_hw_int_pin int_pin, struct bmi2_dev *dev); - -/** - * \ingroup bmi2 - * \defgroup bmi2ApiRemap Remap Axes - * @brief Set / Get remap axes values from the sensor - */ - -/*! - * \ingroup bmi2ApiRemap - * \page bmi2_api_bmi2_get_remap_axes bmi2_get_remap_axes - * \code - * int8_t bmi2_get_remap_axes(struct bmi2_remap *remapped_axis, struct bmi2_dev *dev); - * \endcode - * @details This API gets the re-mapped x, y and z axes from the sensor and - * updates the values in the device structure. - * - * @param[out] remapped_axis : Structure that stores re-mapped axes. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_get_remap_axes(struct bmi2_remap *remapped_axis, struct bmi2_dev *dev); - -/*! - * \ingroup bmi2ApiRemap - * \page bmi2_api_bmi2_set_remap_axes bmi2_set_remap_axes - * \code - * int8_t bmi2_set_remap_axes(const struct bmi2_remap *remapped_axis, struct bmi2_dev *dev); - * \endcode - * @details This API sets the re-mapped x, y and z axes to the sensor and - * updates them in the device structure. - * - * @param[in] remapped_axis : Structure that stores re-mapped axes. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_set_remap_axes(const struct bmi2_remap *remapped_axis, struct bmi2_dev *dev); - -/** - * \ingroup bmi2 - * \defgroup bmi2ApiGyroOC Gyro Offset Compensation - * @brief Gyro Offset Compensation operations of the sensor - */ - -/*! - * \ingroup bmi2ApiGyroOC - * \page bmi2_api_bmi2_set_gyro_offset_comp bmi2_set_gyro_offset_comp - * \code - * int8_t bmi2_set_gyro_offset_comp(uint8_t enable, struct bmi2_dev *dev); - * \endcode - * @details This API enables/disables gyroscope offset compensation. It adds the - * offsets defined in the offset register with gyroscope data. - * - * @param[in] enable : Enables/Disables gyroscope offset compensation. - * @param[in] dev : Structure instance of bmi2_dev. - * - *@verbatim - * enable | Description - * -------------|--------------- - * BMI2_ENABLE | Enables gyroscope offset compensation. - * BMI2_DISABLE | Disables gyroscope offset compensation. - *@endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_set_gyro_offset_comp(uint8_t enable, struct bmi2_dev *dev); - -/*! - * \ingroup bmi2ApiGyroOC - * \page bmi2_api_bmi2_read_gyro_offset_comp_axes bmi2_read_gyro_offset_comp_axes - * \code - * int8_t bmi2_read_gyro_offset_comp_axes(struct bmi2_sens_axes_data *gyr_off_comp_axes, const struct bmi2_dev *dev); - * \endcode - * @details This API reads the gyroscope bias values for each axis which is used - * for gyroscope offset compensation. - * - * @param[out] gyr_off_comp_axes: Structure to store gyroscope offset - * compensated values. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_read_gyro_offset_comp_axes(struct bmi2_sens_axes_data *gyr_off_comp_axes, struct bmi2_dev *dev); - -/*! - * \ingroup bmi2ApiGyroOC - * \page bmi2_api_bmi2_write_gyro_offset_comp_axes bmi2_write_gyro_offset_comp_axes - * \code - * int8_t bmi2_write_gyro_offset_comp_axes(const struct bmi2_sens_axes_data *gyr_off_comp_axes, struct bmi2_dev *dev); - * \endcode - * @details This API writes the gyroscope bias values for each axis which is used - * for gyroscope offset compensation. - * - * @param[in] gyr_off_comp_axes : Structure to store gyroscope offset - * compensated values. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_write_gyro_offset_comp_axes(const struct bmi2_sens_axes_data *gyr_off_comp_axes, struct bmi2_dev *dev); - -/** - * \ingroup bmi2 - * \defgroup bmi2ApiGyroCS Gyro cross sensitivity - * @brief Gyro Cross sensitivity operation - */ - -/*! - * \ingroup bmi2ApiGyroCS - * \page bmi2_api_bmi2_get_gyro_cross_sense bmi2_get_gyro_cross_sense - * \code - * int8_t bmi2_get_gyro_cross_sense(struct bmi2_dev *dev); - * \endcode - * @details This API updates the cross sensitivity coefficient between gyroscope's - * X and Z axes. - * - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_get_gyro_cross_sense(struct bmi2_dev *dev); - -/** - * \ingroup bmi2 - * \defgroup bmi2ApiInts Internal Status - * @brief Get Internal Status of the sensor - */ - -/*! - * \ingroup bmi2ApiInts - * \page bmi2_api_bmi2_get_internal_status bmi2_get_internal_status - * \code - * int8_t bmi2_get_internal_status(uint8_t *int_stat, const struct bmi2_dev *dev); - * \endcode - * @details This API gets error bits and message indicating internal status. - * - * @param[in] dev : Structure instance of bmi2_dev. - * @param[out] int_stat : Pointer variable to store error bits and - * message. - * - *@verbatim - * Internal status | *int_stat - * ---------------------|--------------------- - * BMI2_NOT_INIT | 0x00 - * BMI2_INIT_OK | 0x01 - * BMI2_INIT_ERR | 0x02 - * BMI2_DRV_ERR | 0x03 - * BMI2_SNS_STOP | 0x04 - * BMI2_NVM_ERROR | 0x05 - * BMI2_START_UP_ERROR | 0x06 - * BMI2_COMPAT_ERROR | 0x07 - * BMI2_VFM_SKIPPED | 0x10 - * BMI2_AXES_MAP_ERROR | 0x20 - * BMI2_ODR_50_HZ_ERROR | 0x40 - * BMI2_ODR_HIGH_ERROR | 0x80 - *@endverbatim - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_get_internal_status(uint8_t *int_stat, struct bmi2_dev *dev); - -/** - * \ingroup bmi2 - * \defgroup bmi2ApiFOC FOC - * @brief FOC operations of the sensor - */ - -/*! - * \ingroup bmi2ApiFOC - * \page bmi2_api_bmi2_perform_accel_foc bmi2_perform_accel_foc - * \code - * int8_t bmi2_perform_accel_foc(const struct bmi2_accel_foc_g_value *accel_g_value, struct bmi2_dev *dev); - * \endcode - * @details This API performs Fast Offset Compensation for accelerometer. - * - * @param[in] accel_g_value : This parameter selects the accel foc - * axis to be performed - * - * input format is {x, y, z, sign}. '1' to enable. '0' to disable - * - * eg to choose x axis {1, 0, 0, 0} - * eg to choose -x axis {1, 0, 0, 1} - * - * - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_perform_accel_foc(const struct bmi2_accel_foc_g_value *accel_g_value, struct bmi2_dev *dev); - -/*! - * \ingroup bmi2ApiFOC - * \page bmi2_api_bmi2_perform_gyro_foc bmi2_perform_gyro_foc - * \code - * int8_t bmi2_perform_gyro_foc(struct bmi2_dev *dev); - * \endcode - * @details This API performs Fast Offset Compensation for gyroscope. - * - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_perform_gyro_foc(struct bmi2_dev *dev); - -/** - * \ingroup bmi2 - * \defgroup bmi2ApiCRT CRT - * @brief CRT operations of the sensor - */ - -/*! - * \ingroup bmi2ApiCRT - * \page bmi2_api_bmi2_do_crt bmi2_do_crt - * \code - * int8_t bmi2_do_crt(struct bmi2_dev *dev); - * \endcode - * @details API performs Component Re-Trim calibration (CRT). - * - * - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - * - * @note CRT calibration takes approximately 500ms & maximum time out configured as 2 seconds - */ -int8_t bmi2_do_crt(struct bmi2_dev *dev); - -/** - * \ingroup bmi2 - * \defgroup bmi2ApiCRTSt CRT and self test - * @brief Enable / Abort CRT and self test operations of gyroscope - */ - -/*! - * \ingroup bmi2ApiCRTSt - * \page bmi2_api_bmi2_set_gyro_self_test_crt bmi2_set_gyro_self_test_crt - * \code - * int8_t bmi2_set_gyro_self_test_crt - * \endcode - * @details This api is used to enable the gyro self-test and crt. - * *gyro_self_test_crt -> 0 then gyro self test enable - * *gyro_self_test_crt -> 1 then CRT enable - * - * @param[in] gyro_self_test_crt : enable the gyro self test or crt. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_set_gyro_self_test_crt(uint8_t *gyro_self_test_crt, struct bmi2_dev *dev); - -/*! - * \ingroup bmi2ApiCRTSt - * \page bmi2_api_bmi2_abort_crt_gyro_st bmi2_abort_crt_gyro_st - * \code - * int8_t bmi2_abort_crt_gyro_st(struct bmi2_dev *dev); - * \endcode - * @details This api is used to abort ongoing crt or gyro self test. - * - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_abort_crt_gyro_st(struct bmi2_dev *dev); - -/*! - * \ingroup bmi2ApiASelftest - * \page bmi2_api_bmi2_do_gyro_st bmi2_do_gyro_st - * \code - * int8_t bmi2_do_gyro_st - * \endcode - * @details this api is used to perform gyroscope self test. - * - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_do_gyro_st(struct bmi2_dev *dev); - -/** - * \ingroup bmi2 - * \defgroup bmi2ApiNVM NVM - * @brief NVM operations of the sensor - */ - -/*! - * \ingroup bmi2ApiNVM - * \page bmi2_api_bmi2_nvm_prog bmi2_nvm_prog - * \code - * int8_t bmi2_nvm_prog - * \endcode - * @details This api is used for programming the non volatile memory(nvm) - * - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_nvm_prog(struct bmi2_dev *dev); - -/*! - * @brief This API extracts the input feature configuration - * details like page and start address from the look-up table. - * - * @param[out] feat_config : Structure that stores feature configurations. - * @param[in] type : Type of feature or sensor. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Returns the feature found flag. - * - * @retval BMI2_FALSE : Feature not found - * BMI2_TRUE : Feature found - */ -uint8_t bmi2_extract_input_feat_config(struct bmi2_feature_config *feat_config, uint8_t type, - const struct bmi2_dev *dev); - -/*! - * @brief This API is used to get the feature configuration from the - * selected page. - * - * @param[in] sw_page : Switches to the desired page. - * @param[out] feat_config : Pointer to the feature configuration. - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi2_get_feat_config(uint8_t sw_page, uint8_t *feat_config, struct bmi2_dev *dev); - -#ifdef __cplusplus -} -#endif /* End of CPP guard */ - -#endif /* BMI2_H_ */ diff --git a/src/main/drivers/accgyro/bmi270_maximum_fifo.h b/src/main/drivers/accgyro/bmi270_maximum_fifo.h deleted file mode 100644 index e912a96a63..0000000000 --- a/src/main/drivers/accgyro/bmi270_maximum_fifo.h +++ /dev/null @@ -1,117 +0,0 @@ -/** -* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. -* -* BSD-3-Clause -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* 1. Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* -* 2. Redistributions in binary form must reproduce the above copyright -* notice, this list of conditions and the following disclaimer in the -* documentation and/or other materials provided with the distribution. -* -* 3. Neither the name of the copyright holder nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) -* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, -* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING -* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -* -* @file bmi270_maximum_fifo.h -* @date 2020-11-04 -* @version v2.63.1 -* -*/ - -/** - * \ingroup bmi2xy - * \defgroup bmi270_maximum_fifo BMI270_MAXIMUM_FIFO - * @brief Sensor driver for BMI270_MAXIMUM_FIFO sensor - */ - -#ifndef BMI270_MAXIMUM_FIFO_H_ -#define BMI270_MAXIMUM_FIFO_H_ - -/*! CPP guard */ -#ifdef __cplusplus -extern "C" { -#endif - -/***************************************************************************/ - -/*! Header files - ****************************************************************************/ -#include "bmi2.h" - -/***************************************************************************/ - -/*! Macro definitions - ****************************************************************************/ - -/*! @name BMI270 Chip identifier */ -#define BMI270_MAXIMUM_FIFO_CHIP_ID UINT8_C(0x24) - -/*! @name Defines maximum number of pages */ -#define BMI270_MAXIMUM_FIFO_MAX_PAGE_NUM UINT8_C(0) - -/*! @name Defines maximum number of feature input configurations */ -#define BMI270_MAXIMUM_FIFO_MAX_FEAT_IN UINT8_C(0) - -/*! @name Defines maximum number of feature outputs */ -#define BMI270_MAXIMUM_FIFO_MAX_FEAT_OUT UINT8_C(0) - -/*! @name Mask definitions for feature interrupt status bits */ - -/***************************************************************************/ - -/*! BMI270 User Interface function prototypes - ****************************************************************************/ - -/** - * \ingroup bmi270_maximum_fifo - * \defgroup bmi270_maximum_fifoApiInit Initialization - * @brief Initialize the sensor and device structure - */ - -/*! - * \ingroup bmi270_maximum_fifoApiInit - * \page bmi270_maximum_fifo_api_bmi270_maximum_fifo_init bmi270_maximum_fifo_init - * \code - * int8_t bmi270_maximum_fifo_init(struct bmi2_dev *dev); - * \endcode - * @details This API: - * 1) updates the device structure with address of the configuration file. - * 2) Initializes BMI270 sensor. - * 3) Writes the configuration file. - * 4) Updates the feature offset parameters in the device structure. - * 5) Updates the maximum number of pages, in the device structure. - * - * @param[in, out] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -int8_t bmi270_maximum_fifo_init(struct bmi2_dev *dev); - -/******************************************************************************/ -/*! @name C++ Guard Macros */ -/******************************************************************************/ -#ifdef __cplusplus -} -#endif /* End of CPP guard */ - -#endif /* BMI270_MAXIMUM_FIFO_H_ */ diff --git a/src/main/drivers/accgyro/bmi2_defs.h b/src/main/drivers/accgyro/bmi2_defs.h deleted file mode 100644 index 889bf82564..0000000000 --- a/src/main/drivers/accgyro/bmi2_defs.h +++ /dev/null @@ -1,2475 +0,0 @@ -/** -* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. -* -* BSD-3-Clause -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* 1. Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* -* 2. Redistributions in binary form must reproduce the above copyright -* notice, this list of conditions and the following disclaimer in the -* documentation and/or other materials provided with the distribution. -* -* 3. Neither the name of the copyright holder nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) -* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, -* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING -* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -* -* @file bmi2_defs.h -* @date 2020-11-04 -* @version v2.63.1 -* -*/ - -#ifndef BMI2_DEFS_H_ -#define BMI2_DEFS_H_ - -/******************************************************************************/ -/*! @name Header includes */ -/******************************************************************************/ -#ifdef __KERNEL__ -#include -#include -#else -#include -#include -#endif - -/******************************************************************************/ -/*! @name Common macros */ -/******************************************************************************/ -#ifdef __KERNEL__ -#if !defined(UINT8_C) && !defined(INT8_C) -#define INT8_C(x) S8_C(x) -#define UINT8_C(x) U8_C(x) -#endif - -#if !defined(UINT16_C) && !defined(INT16_C) -#define INT16_C(x) S16_C(x) -#define UINT16_C(x) U16_C(x) -#endif - -#if !defined(INT32_C) && !defined(UINT32_C) -#define INT32_C(x) S32_C(x) -#define UINT32_C(x) U32_C(x) -#endif - -#if !defined(INT64_C) && !defined(UINT64_C) -#define INT64_C(x) S64_C(x) -#define UINT64_C(x) U64_C(x) -#endif -#endif - -/*! @name C standard macros */ -#ifndef NULL -#ifdef __cplusplus -#define NULL 0 -#else -#define NULL ((void *) 0) -#endif -#endif - -/******************************************************************************/ -/*! @name General Macro Definitions */ -/******************************************************************************/ -/*! @name Utility macros */ -#define BMI2_SET_BITS(reg_data, bitname, data) \ - ((reg_data & ~(bitname##_MASK)) | \ - ((data << bitname##_POS) & bitname##_MASK)) - -#define BMI2_GET_BITS(reg_data, bitname) \ - ((reg_data & (bitname##_MASK)) >> \ - (bitname##_POS)) - -#define BMI2_SET_BIT_POS0(reg_data, bitname, data) \ - ((reg_data & ~(bitname##_MASK)) | \ - (data & bitname##_MASK)) - -#define BMI2_GET_BIT_POS0(reg_data, bitname) (reg_data & (bitname##_MASK)) -#define BMI2_SET_BIT_VAL0(reg_data, bitname) (reg_data & ~(bitname##_MASK)) - -/*! @name For getting LSB and MSB */ -#define BMI2_GET_LSB(var) (uint8_t)(var & BMI2_SET_LOW_BYTE) -#define BMI2_GET_MSB(var) (uint8_t)((var & BMI2_SET_HIGH_BYTE) >> 8) - -#ifndef BMI2_INTF_RETURN_TYPE -#define BMI2_INTF_RETURN_TYPE int8_t -#endif - -/*! @name For defining absolute values */ -#define BMI2_ABS(a) ((a) > 0 ? (a) : -(a)) - -/*! @name LSB and MSB mask definitions */ -#define BMI2_SET_LOW_BYTE UINT16_C(0x00FF) -#define BMI2_SET_HIGH_BYTE UINT16_C(0xFF00) -#define BMI2_SET_LOW_NIBBLE UINT8_C(0x0F) - -/*! @name For enable and disable */ -#define BMI2_ENABLE UINT8_C(1) -#define BMI2_DISABLE UINT8_C(0) - -/*! @name To define TRUE or FALSE */ -#define BMI2_TRUE UINT8_C(1) -#define BMI2_FALSE UINT8_C(0) - -/*! @name To define sensor interface success code */ -#define BMI2_INTF_RET_SUCCESS INT8_C(0) - -/*! @name To define success code */ -#define BMI2_OK INT8_C(0) - -/*! @name To define error codes */ -#define BMI2_E_NULL_PTR INT8_C(-1) -#define BMI2_E_COM_FAIL INT8_C(-2) -#define BMI2_E_DEV_NOT_FOUND INT8_C(-3) -#define BMI2_E_OUT_OF_RANGE INT8_C(-4) -#define BMI2_E_ACC_INVALID_CFG INT8_C(-5) -#define BMI2_E_GYRO_INVALID_CFG INT8_C(-6) -#define BMI2_E_ACC_GYR_INVALID_CFG INT8_C(-7) -#define BMI2_E_INVALID_SENSOR INT8_C(-8) -#define BMI2_E_CONFIG_LOAD INT8_C(-9) -#define BMI2_E_INVALID_PAGE INT8_C(-10) -#define BMI2_E_INVALID_FEAT_BIT INT8_C(-11) -#define BMI2_E_INVALID_INT_PIN INT8_C(-12) -#define BMI2_E_SET_APS_FAIL INT8_C(-13) -#define BMI2_E_AUX_INVALID_CFG INT8_C(-14) -#define BMI2_E_AUX_BUSY INT8_C(-15) -#define BMI2_E_SELF_TEST_FAIL INT8_C(-16) -#define BMI2_E_REMAP_ERROR INT8_C(-17) -#define BMI2_E_GYR_USER_GAIN_UPD_FAIL INT8_C(-18) -#define BMI2_E_SELF_TEST_NOT_DONE INT8_C(-19) -#define BMI2_E_INVALID_INPUT INT8_C(-20) -#define BMI2_E_INVALID_STATUS INT8_C(-21) -#define BMI2_E_CRT_ERROR INT8_C(-22) -#define BMI2_E_ST_ALREADY_RUNNING INT8_C(-23) -#define BMI2_E_CRT_READY_FOR_DL_FAIL_ABORT INT8_C(-24) -#define BMI2_E_DL_ERROR INT8_C(-25) -#define BMI2_E_PRECON_ERROR INT8_C(-26) -#define BMI2_E_ABORT_ERROR INT8_C(-27) -#define BMI2_E_GYRO_SELF_TEST_ERROR INT8_C(-28) -#define BMI2_E_GYRO_SELF_TEST_TIMEOUT INT8_C(-29) -#define BMI2_E_WRITE_CYCLE_ONGOING INT8_C(-30) -#define BMI2_E_WRITE_CYCLE_TIMEOUT INT8_C(-31) -#define BMI2_E_ST_NOT_RUNING INT8_C(-32) -#define BMI2_E_DATA_RDY_INT_FAILED INT8_C(-33) -#define BMI2_E_INVALID_FOC_POSITION INT8_C(-34) - -/*! @name To define warnings for FIFO activity */ -#define BMI2_W_FIFO_EMPTY INT8_C(1) -#define BMI2_W_PARTIAL_READ INT8_C(2) - -/*! @name Bit wise to define information */ -#define BMI2_I_MIN_VALUE UINT8_C(1) -#define BMI2_I_MAX_VALUE UINT8_C(2) - -/*! @name BMI2 register addresses */ -#define BMI2_CHIP_ID_ADDR UINT8_C(0x00) -#define BMI2_STATUS_ADDR UINT8_C(0x03) -#define BMI2_AUX_X_LSB_ADDR UINT8_C(0x04) -#define BMI2_ACC_X_LSB_ADDR UINT8_C(0x0C) -#define BMI2_GYR_X_LSB_ADDR UINT8_C(0x12) -#define BMI2_EVENT_ADDR UINT8_C(0x1B) -#define BMI2_INT_STATUS_0_ADDR UINT8_C(0x1C) -#define BMI2_INT_STATUS_1_ADDR UINT8_C(0x1D) -#define BMI2_SC_OUT_0_ADDR UINT8_C(0x1E) -#define BMI2_SYNC_COMMAND_ADDR UINT8_C(0x1E) -#define BMI2_GYR_CAS_GPIO0_ADDR UINT8_C(0x1E) -#define BMI2_INTERNAL_STATUS_ADDR UINT8_C(0x21) -#define BMI2_FIFO_LENGTH_0_ADDR UINT8_C(0X24) -#define BMI2_FIFO_DATA_ADDR UINT8_C(0X26) -#define BMI2_FEAT_PAGE_ADDR UINT8_C(0x2F) -#define BMI2_FEATURES_REG_ADDR UINT8_C(0x30) -#define BMI2_ACC_CONF_ADDR UINT8_C(0x40) -#define BMI2_GYR_CONF_ADDR UINT8_C(0x42) -#define BMI2_AUX_CONF_ADDR UINT8_C(0x44) -#define BMI2_FIFO_DOWNS_ADDR UINT8_C(0X45) -#define BMI2_FIFO_WTM_0_ADDR UINT8_C(0X46) -#define BMI2_FIFO_WTM_1_ADDR UINT8_C(0X47) -#define BMI2_FIFO_CONFIG_0_ADDR UINT8_C(0X48) -#define BMI2_FIFO_CONFIG_1_ADDR UINT8_C(0X49) -#define BMI2_AUX_DEV_ID_ADDR UINT8_C(0x4B) -#define BMI2_AUX_IF_CONF_ADDR UINT8_C(0x4C) -#define BMI2_AUX_RD_ADDR UINT8_C(0x4D) -#define BMI2_AUX_WR_ADDR UINT8_C(0x4E) -#define BMI2_AUX_WR_DATA_ADDR UINT8_C(0x4F) -#define BMI2_INT1_IO_CTRL_ADDR UINT8_C(0x53) -#define BMI2_INT2_IO_CTRL_ADDR UINT8_C(0x54) -#define BMI2_INT_LATCH_ADDR UINT8_C(0x55) -#define BMI2_INT1_MAP_FEAT_ADDR UINT8_C(0x56) -#define BMI2_INT2_MAP_FEAT_ADDR UINT8_C(0x57) -#define BMI2_INT_MAP_DATA_ADDR UINT8_C(0x58) -#define BMI2_INIT_CTRL_ADDR UINT8_C(0x59) -#define BMI2_INIT_ADDR_0 UINT8_C(0x5B) -#define BMI2_INIT_ADDR_1 UINT8_C(0x5C) -#define BMI2_INIT_DATA_ADDR UINT8_C(0x5E) -#define BMI2_AUX_IF_TRIM UINT8_C(0x68) -#define BMI2_GYR_CRT_CONF_ADDR UINT8_C(0X69) -#define BMI2_NVM_CONF_ADDR UINT8_C(0x6A) -#define BMI2_IF_CONF_ADDR UINT8_C(0X6B) -#define BMI2_ACC_SELF_TEST_ADDR UINT8_C(0X6D) -#define BMI2_GYR_SELF_TEST_AXES_ADDR UINT8_C(0x6E) -#define BMI2_SELF_TEST_MEMS_ADDR UINT8_C(0X6F) -#define BMI2_NV_CONF_ADDR UINT8_C(0x70) -#define BMI2_ACC_OFF_COMP_0_ADDR UINT8_C(0X71) -#define BMI2_GYR_OFF_COMP_3_ADDR UINT8_C(0X74) -#define BMI2_GYR_OFF_COMP_6_ADDR UINT8_C(0X77) -#define BMI2_GYR_USR_GAIN_0_ADDR UINT8_C(0X78) -#define BMI2_PWR_CONF_ADDR UINT8_C(0x7C) -#define BMI2_PWR_CTRL_ADDR UINT8_C(0x7D) -#define BMI2_CMD_REG_ADDR UINT8_C(0x7E) - -/*! @name BMI2 I2C address */ -#define BMI2_I2C_PRIM_ADDR UINT8_C(0x68) -#define BMI2_I2C_SEC_ADDR UINT8_C(0x69) - -/*! @name BMI2 Commands */ -#define BMI2_G_TRIGGER_CMD UINT8_C(0x02) -#define BMI2_USR_GAIN_CMD UINT8_C(0x03) -#define BMI2_NVM_PROG_CMD UINT8_C(0xA0) -#define BMI2_SOFT_RESET_CMD UINT8_C(0xB6) -#define BMI2_FIFO_FLUSH_CMD UINT8_C(0xB0) - -/*! @name BMI2 sensor data bytes */ - -#define BMI2_ACC_GYR_NUM_BYTES UINT8_C(6) -#define BMI2_AUX_NUM_BYTES UINT8_C(8) -#define BMI2_CRT_CONFIG_FILE_SIZE UINT16_C(2048) -#define BMI2_FEAT_SIZE_IN_BYTES UINT8_C(16) -#define BMI2_ACC_CONFIG_LENGTH UINT8_C(2) - -/*! @name BMI2 configuration load status */ -#define BMI2_CONFIG_LOAD_SUCCESS UINT8_C(1) - -/*! @name To define BMI2 pages */ -#define BMI2_PAGE_0 UINT8_C(0) -#define BMI2_PAGE_1 UINT8_C(1) -#define BMI2_PAGE_2 UINT8_C(2) -#define BMI2_PAGE_3 UINT8_C(3) -#define BMI2_PAGE_4 UINT8_C(4) -#define BMI2_PAGE_5 UINT8_C(5) -#define BMI2_PAGE_6 UINT8_C(6) -#define BMI2_PAGE_7 UINT8_C(7) - -/*! @name Array Parameter DefinItions */ -#define BMI2_SENSOR_TIME_LSB_BYTE UINT8_C(0) -#define BMI2_SENSOR_TIME_XLSB_BYTE UINT8_C(1) -#define BMI2_SENSOR_TIME_MSB_BYTE UINT8_C(2) - -/*! @name Mask definitions for Gyro CRT */ -#define BMI2_GYR_RDY_FOR_DL_MASK UINT8_C(0x08) -#define BMI2_GYR_CRT_RUNNING_MASK UINT8_C(0x04) - -/*! @name mask definition for status register */ -#define BMI2_AUX_BUSY_MASK UINT8_C(0x04) -#define BMI2_CMD_RDY_MASK UINT8_C(0x10) -#define BMI2_DRDY_AUX_MASK UINT8_C(0x20) -#define BMI2_DRDY_GYR_MASK UINT8_C(0x40) -#define BMI2_DRDY_ACC_MASK UINT8_C(0x80) - -/*! @name Mask definitions for SPI read/write address */ -#define BMI2_SPI_RD_MASK UINT8_C(0x80) -#define BMI2_SPI_WR_MASK UINT8_C(0x7F) - -/*! @name Mask definitions for power configuration register */ -#define BMI2_ADV_POW_EN_MASK UINT8_C(0x01) - -/*! @name Mask definitions for initialization control register */ -#define BMI2_CONF_LOAD_EN_MASK UINT8_C(0x01) - -/*! @name Mask definitions for power control register */ -#define BMI2_AUX_EN_MASK UINT8_C(0x01) -#define BMI2_GYR_EN_MASK UINT8_C(0x02) -#define BMI2_ACC_EN_MASK UINT8_C(0x04) -#define BMI2_TEMP_EN_MASK UINT8_C(0x08) - -/*! @name Mask definitions for sensor event flags */ -#define BMI2_EVENT_FLAG_MASK UINT8_C(0x1C) - -/*! @name Mask definitions to switch page */ -#define BMI2_SWITCH_PAGE_EN_MASK UINT8_C(0x07) - -/*! @name Mask definitions of NVM register */ -#define BMI2_NV_ACC_OFFSET_MASK UINT8_C(0x08) - -/*! @name Mask definition for config version */ -#define BMI2_CONFIG_MAJOR_MASK UINT16_C(0x3C0) -#define BMI2_CONFIG_MINOR_MASK UINT8_C(0x3F) - -/*! @name mask and bit position for activity recognition settings */ -#define BMI2_ACT_RECG_POST_PROS_EN_DIS_MASK UINT8_C(0x01) -#define BMI2_ACT_RECG_BUFF_SIZE_MASK UINT8_C(0x0F) -#define BMI2_ACT_RECG_MIN_SEG_CONF_MASK UINT8_C(0x0F) - -/*! @name mask and bit position for activity recognition hc settings */ -#define BMI2_HC_ACT_RECG_SEGMENT_SIZE_MASK UINT8_C(0x03) -#define BMI2_HC_ACT_RECG_PP_EN_MASK UINT8_C(0x01) -#define BMI2_HC_ACT_RECG_MIN_GDI_THRES_MASK UINT16_C(0xFFFF) -#define BMI2_HC_ACT_RECG_MAX_GDI_THRES_MASK UINT16_C(0xFFFF) -#define BMI2_HC_ACT_RECG_BUF_SIZE_MASK UINT16_C(0xFFFF) -#define BMI2_HC_ACT_RECG_MIN_SEG_CONF_MASK UINT16_C(0xFFFF) - -#define BMI2_GYRO_CROSS_AXES_SENSE_MASK UINT8_C(0x7F) -#define BMI2_GYRO_CROSS_AXES_SENSE_SIGN_BIT_MASK UINT8_C(0x40) - -/*! @name Bit position definitions for Gyro CRT */ -#define BMI2_GYR_RDY_FOR_DL_POS UINT8_C(0x03) -#define BMI2_GYR_CRT_RUNNING_POS UINT8_C(0x02) - -/*! @name Bit position for status register*/ -#define BMI2_AUX_BUSY_POS UINT8_C(0x02) -#define BMI2_CMD_RDY_POS UINT8_C(0x04) -#define BMI2_DRDY_AUX_POS UINT8_C(0x05) -#define BMI2_DRDY_GYR_POS UINT8_C(0x06) -#define BMI2_DRDY_ACC_POS UINT8_C(0x07) - -/*! @name Bit position definitions for power control register */ -#define BMI2_GYR_EN_POS UINT8_C(0x01) -#define BMI2_ACC_EN_POS UINT8_C(0x02) -#define BMI2_TEMP_EN_POS UINT8_C(0x03) - -/*! @name Bit position definitions for sensor event flags */ -#define BMI2_EVENT_FLAG_POS UINT8_C(0x02) - -/*! @name Bit position definitions of NVM register */ -#define BMI2_NV_ACC_OFFSET_POS UINT8_C(0x03) - -/*! @name Bit position for major version from config */ -#define BMI2_CONFIG_MAJOR_POS UINT8_C(0x06) - -/*! @name Accelerometer and Gyroscope Filter/Noise performance modes */ -/* Power optimized mode */ -#define BMI2_POWER_OPT_MODE UINT8_C(0) - -/* Performance optimized */ -#define BMI2_PERF_OPT_MODE UINT8_C(1) - -/*! @name index for config major minor information */ -#define BMI2_CONFIG_INFO_LOWER UINT8_C(52) -#define BMI2_CONFIG_INFO_HIGHER UINT8_C(53) - -/*! @name Sensor status */ -#define BMI2_DRDY_ACC UINT8_C(0x80) -#define BMI2_DRDY_GYR UINT8_C(0x40) -#define BMI2_DRDY_AUX UINT8_C(0x20) -#define BMI2_CMD_RDY UINT8_C(0x10) -#define BMI2_AUX_BUSY UINT8_C(0x04) - -/*! @name Macro to define accelerometer configuration value for FOC */ -#define BMI2_FOC_ACC_CONF_VAL UINT8_C(0xB7) - -/*! @name Macro to define gyroscope configuration value for FOC */ -#define BMI2_FOC_GYR_CONF_VAL UINT8_C(0xB6) - -/*! @name Macro to define X Y and Z axis for an array */ -#define BMI2_X_AXIS UINT8_C(0) -#define BMI2_Y_AXIS UINT8_C(1) -#define BMI2_Z_AXIS UINT8_C(2) - -/******************************************************************************/ -/*! @name Sensor Macro Definitions */ -/******************************************************************************/ -/*! @name Macros to define BMI2 sensor/feature types */ -#define BMI2_ACCEL UINT8_C(0) -#define BMI2_GYRO UINT8_C(1) -#define BMI2_AUX UINT8_C(2) -#define BMI2_SIG_MOTION UINT8_C(3) -#define BMI2_ANY_MOTION UINT8_C(4) -#define BMI2_NO_MOTION UINT8_C(5) -#define BMI2_STEP_DETECTOR UINT8_C(6) -#define BMI2_STEP_COUNTER UINT8_C(7) -#define BMI2_STEP_ACTIVITY UINT8_C(8) -#define BMI2_GYRO_GAIN_UPDATE UINT8_C(9) -#define BMI2_TILT UINT8_C(10) -#define BMI2_UP_HOLD_TO_WAKE UINT8_C(11) -#define BMI2_GLANCE_DETECTOR UINT8_C(12) -#define BMI2_WAKE_UP UINT8_C(13) -#define BMI2_ORIENTATION UINT8_C(14) -#define BMI2_HIGH_G UINT8_C(15) -#define BMI2_LOW_G UINT8_C(16) -#define BMI2_FLAT UINT8_C(17) -#define BMI2_EXT_SENS_SYNC UINT8_C(18) -#define BMI2_WRIST_GESTURE UINT8_C(19) -#define BMI2_WRIST_WEAR_WAKE_UP UINT8_C(20) -#define BMI2_WRIST_WEAR_WAKE_UP_WH UINT8_C(21) -#define BMI2_WRIST_GESTURE_WH UINT8_C(22) -#define BMI2_PRIMARY_OIS UINT8_C(23) -#define BMI2_FREE_FALL_DET UINT8_C(24) -#define BMI2_SINGLE_TAP UINT8_C(25) -#define BMI2_DOUBLE_TAP UINT8_C(26) -#define BMI2_TRIPLE_TAP UINT8_C(27) -#define BMI2_TAP UINT8_C(28) - -/* Non virtual sensor features */ -#define BMI2_STEP_COUNTER_PARAMS UINT8_C(29) -#define BMI2_TAP_DETECTOR_1 UINT8_C(30) -#define BMI2_TAP_DETECTOR_2 UINT8_C(31) -#define BMI2_TEMP UINT8_C(32) -#define BMI2_ACCEL_SELF_TEST UINT8_C(33) -#define BMI2_GYRO_SELF_OFF UINT8_C(34) -#define BMI2_ACTIVITY_RECOGNITION UINT8_C(35) -#define BMI2_MAX_BURST_LEN UINT8_C(36) -#define BMI2_SENS_MAX_NUM UINT8_C(37) -#define BMI2_AXIS_MAP UINT8_C(38) -#define BMI2_NVM_STATUS UINT8_C(39) -#define BMI2_VFRM_STATUS UINT8_C(40) -#define BMI2_GYRO_CROSS_SENSE UINT8_C(41) -#define BMI2_CRT_GYRO_SELF_TEST UINT8_C(42) -#define BMI2_ABORT_CRT_GYRO_SELF_TEST UINT8_C(43) -#define BMI2_NVM_PROG_PREP UINT8_C(44) -#define BMI2_ACTIVITY_RECOGNITION_SETTINGS UINT8_C(45) -#define BMI2_OIS_OUTPUT UINT8_C(46) -#define BMI2_CONFIG_ID UINT8_C(47) - -/*! @name Bit wise for selecting BMI2 sensors/features */ -#define BMI2_ACCEL_SENS_SEL (1) -#define BMI2_GYRO_SENS_SEL (1 << BMI2_GYRO) -#define BMI2_AUX_SENS_SEL (1 << BMI2_AUX) -#define BMI2_TEMP_SENS_SEL ((uint64_t)1 << BMI2_TEMP) -#define BMI2_ANY_MOT_SEL (1 << BMI2_ANY_MOTION) -#define BMI2_NO_MOT_SEL (1 << BMI2_NO_MOTION) -#define BMI2_TILT_SEL (1 << BMI2_TILT) -#define BMI2_ORIENT_SEL (1 << BMI2_ORIENTATION) -#define BMI2_SIG_MOTION_SEL (1 << BMI2_SIG_MOTION) -#define BMI2_STEP_DETECT_SEL (1 << BMI2_STEP_DETECTOR) -#define BMI2_STEP_COUNT_SEL (1 << BMI2_STEP_COUNTER) -#define BMI2_STEP_ACT_SEL (1 << BMI2_STEP_ACTIVITY) -#define BMI2_GYRO_GAIN_UPDATE_SEL (1 << BMI2_GYRO_GAIN_UPDATE) -#define BMI2_UP_HOLD_TO_WAKE_SEL (1 << BMI2_UP_HOLD_TO_WAKE) -#define BMI2_GLANCE_DET_SEL (1 << BMI2_GLANCE_DETECTOR) -#define BMI2_WAKE_UP_SEL (1 << BMI2_WAKE_UP) -#define BMI2_TAP_SEL (1 << BMI2_TAP) -#define BMI2_HIGH_G_SEL (1 << BMI2_HIGH_G) -#define BMI2_LOW_G_SEL (1 << BMI2_LOW_G) -#define BMI2_FLAT_SEL (1 << BMI2_FLAT) -#define BMI2_EXT_SENS_SEL (1 << BMI2_EXT_SENS_SYNC) -#define BMI2_SINGLE_TAP_SEL (1 << BMI2_SINGLE_TAP) -#define BMI2_DOUBLE_TAP_SEL (1 << BMI2_DOUBLE_TAP) -#define BMI2_TRIPLE_TAP_SEL (1 << BMI2_TRIPLE_TAP) -#define BMI2_GYRO_SELF_OFF_SEL ((uint64_t)1 << BMI2_GYRO_SELF_OFF) -#define BMI2_WRIST_GEST_SEL (1 << BMI2_WRIST_GESTURE) -#define BMI2_WRIST_WEAR_WAKE_UP_SEL (1 << BMI2_WRIST_WEAR_WAKE_UP) -#define BMI2_ACTIVITY_RECOGNITION_SEL ((uint64_t)1 << BMI2_ACTIVITY_RECOGNITION) -#define BMI2_ACCEL_SELF_TEST_SEL ((uint64_t)1 << BMI2_ACCEL_SELF_TEST) -#define BMI2_WRIST_GEST_W_SEL (1 << BMI2_WRIST_GESTURE_WH) -#define BMI2_WRIST_WEAR_WAKE_UP_WH_SEL (1 << BMI2_WRIST_WEAR_WAKE_UP_WH) -#define BMI2_PRIMARY_OIS_SEL (1 << BMI2_PRIMARY_OIS) -#define BMI2_FREE_FALL_DET_SEL (1 << BMI2_FREE_FALL_DET) - -/*! @name Mask definitions for BMI2 wake-up feature configuration for bmi260 */ -#define BMI2_WAKEUP_SENSITIVITY_MASK UINT8_C(0x0E) -#define BMI2_WAKEUP_SINGLE_TAP_EN_MASK UINT8_C(0x01) -#define BMI2_WAKEUP_DOUBLE_TAP_EN_MASK UINT8_C(0x02) -#define BMI2_WAKEUP_TRIPLE_TAP_EN_MASK UINT8_C(0x04) -#define BMI2_WAKEUP_DATA_REG_EN_MASK UINT8_C(0x08) -#define BMI2_WAKEUP_AXIS_SEL_MASK UINT8_C(0x03) - -/*! @name Bit position definitions for BMI2 wake-up feature configuration for bmi260 */ -#define BMI2_WAKEUP_SENSITIVITY_POS UINT8_C(0x01) -#define BMI2_WAKEUP_DOUBLE_TAP_EN_POS UINT8_C(0x01) -#define BMI2_WAKEUP_TRIPLE_TAP_EN_POS UINT8_C(0x02) -#define BMI2_WAKEUP_DATA_REG_EN_POS UINT8_C(0x03) - -/*! @name Mask definitions for BMI2 tap feature configuration for bmi260t */ -#define BMI2_TAP_SENSITIVITY_MASK UINT8_C(0x0E) -#define BMI2_TAP_SINGLE_TAP_EN_MASK UINT8_C(0x01) -#define BMI2_TAP_DOUBLE_TAP_EN_MASK UINT8_C(0x02) -#define BMI2_TAP_TRIPLE_TAP_EN_MASK UINT8_C(0x04) -#define BMI2_TAP_DATA_REG_EN_MASK UINT8_C(0x08) -#define BMI2_TAP_AXIS_SEL_MASK UINT8_C(0x03) - -/*! @name Bit position definitions for BMI2 tap feature configuration for bmi260t */ -#define BMI2_TAP_SENSITIVITY_POS UINT8_C(0x01) -#define BMI2_TAP_DOUBLE_TAP_EN_POS UINT8_C(0x01) -#define BMI2_TAP_TRIPLE_TAP_EN_POS UINT8_C(0x02) -#define BMI2_TAP_DATA_REG_EN_POS UINT8_C(0x03) - -/*! @name Mask definitions for BMI2 wake-up feature configuration for other than bmi261 */ -#define BMI2_WAKE_UP_SENSITIVITY_MASK UINT16_C(0x000E) -#define BMI2_WAKE_UP_SINGLE_TAP_EN_MASK UINT16_C(0x0010) - -/*! @name Bit position definitions for BMI2 wake-up feature configuration for other than bmi261 */ -#define BMI2_WAKE_UP_SENSITIVITY_POS UINT8_C(0x01) -#define BMI2_WAKE_UP_SINGLE_TAP_EN_POS UINT8_C(0x04) - -/*! @name Offsets from feature start address for BMI2 feature enable/disable */ -#define BMI2_ANY_MOT_FEAT_EN_OFFSET UINT8_C(0x03) -#define BMI2_NO_MOT_FEAT_EN_OFFSET UINT8_C(0x03) -#define BMI2_SIG_MOT_FEAT_EN_OFFSET UINT8_C(0x0A) -#define BMI2_STEP_COUNT_FEAT_EN_OFFSET UINT8_C(0x01) -#define BMI2_GYR_USER_GAIN_FEAT_EN_OFFSET UINT8_C(0x05) -#define BMI2_HIGH_G_FEAT_EN_OFFSET UINT8_C(0x03) -#define BMI2_LOW_G_FEAT_EN_OFFSET UINT8_C(0x03) -#define BMI2_TILT_FEAT_EN_OFFSET UINT8_C(0x00) - -/*! @name Mask definitions for BMI2 feature enable/disable */ -#define BMI2_ANY_NO_MOT_EN_MASK UINT8_C(0x80) -#define BMI2_TILT_FEAT_EN_MASK UINT8_C(0x01) -#define BMI2_ORIENT_FEAT_EN_MASK UINT8_C(0x01) -#define BMI2_SIG_MOT_FEAT_EN_MASK UINT8_C(0x01) -#define BMI2_STEP_DET_FEAT_EN_MASK UINT8_C(0x08) -#define BMI2_STEP_COUNT_FEAT_EN_MASK UINT8_C(0x10) -#define BMI2_STEP_ACT_FEAT_EN_MASK UINT8_C(0x20) -#define BMI2_GYR_USER_GAIN_FEAT_EN_MASK UINT8_C(0x08) -#define BMI2_UP_HOLD_TO_WAKE_FEAT_EN_MASK UINT8_C(0x01) -#define BMI2_GLANCE_FEAT_EN_MASK UINT8_C(0x01) -#define BMI2_WAKE_UP_FEAT_EN_MASK UINT8_C(0x01) -#define BMI2_TAP_FEAT_EN_MASK UINT8_C(0x01) -#define BMI2_HIGH_G_FEAT_EN_MASK UINT8_C(0x80) -#define BMI2_LOW_G_FEAT_EN_MASK UINT8_C(0x10) -#define BMI2_FLAT_FEAT_EN_MASK UINT8_C(0x01) -#define BMI2_EXT_SENS_SYNC_FEAT_EN_MASK UINT8_C(0x01) -#define BMI2_GYR_SELF_OFF_CORR_FEAT_EN_MASK UINT8_C(0x02) -#define BMI2_WRIST_GEST_FEAT_EN_MASK UINT8_C(0x20) -#define BMI2_WRIST_WEAR_WAKE_UP_FEAT_EN_MASK UINT8_C(0x10) -#define BMI2_ACTIVITY_RECOG_EN_MASK UINT8_C(0x01) -#define BMI2_ACC_SELF_TEST_FEAT_EN_MASK UINT8_C(0x02) -#define BMI2_GYRO_SELF_TEST_CRT_EN_MASK UINT8_C(0x01) -#define BMI2_ABORT_FEATURE_EN_MASK UINT8_C(0x02) -#define BMI2_NVM_PREP_FEATURE_EN_MASK UINT8_C(0x04) -#define BMI2_FREE_FALL_DET_FEAT_EN_MASK UINT8_C(0x01) -#define BMI2_WRIST_GEST_WH_FEAT_EN_MASK UINT8_C(0x02) - -/*! @name Bit position definitions for BMI2 feature enable/disable */ -#define BMI2_ANY_NO_MOT_EN_POS UINT8_C(0x07) -#define BMI2_STEP_DET_FEAT_EN_POS UINT8_C(0x03) -#define BMI2_STEP_COUNT_FEAT_EN_POS UINT8_C(0x04) -#define BMI2_STEP_ACT_FEAT_EN_POS UINT8_C(0x05) -#define BMI2_GYR_USER_GAIN_FEAT_EN_POS UINT8_C(0x03) -#define BMI2_HIGH_G_FEAT_EN_POS UINT8_C(0x07) -#define BMI2_LOW_G_FEAT_EN_POS UINT8_C(0x04) -#define BMI2_GYR_SELF_OFF_CORR_FEAT_EN_POS UINT8_C(0x01) -#define BMI2_WRIST_GEST_FEAT_EN_POS UINT8_C(0x05) -#define BMI2_WRIST_WEAR_WAKE_UP_FEAT_EN_POS UINT8_C(0x04) -#define BMI2_ACC_SELF_TEST_FEAT_EN_POS UINT8_C(0x01) -#define BMI2_ABORT_FEATURE_EN_POS UINT8_C(0x1) -#define BMI2_NVM_PREP_FEATURE_EN_POS UINT8_C(0x02) -#define BMI2_WRIST_GEST_WH_FEAT_EN_POS UINT8_C(0x01) - -/*! Primary OIS low pass filter configuration position and mask */ -#define BMI2_LP_FILTER_EN_MASK UINT8_C(0x01) - -#define BMI2_LP_FILTER_CONFIG_POS UINT8_C(0x01) -#define BMI2_LP_FILTER_CONFIG_MASK UINT8_C(0x06) - -#define BMI2_PRIMARY_OIS_GYR_EN_POS UINT8_C(0x06) -#define BMI2_PRIMARY_OIS_GYR_EN_MASK UINT8_C(0x40) - -#define BMI2_PRIMARY_OIS_ACC_EN_POS UINT8_C(0x07) -#define BMI2_PRIMARY_OIS_ACC_EN_MASK UINT8_C(0x80) - -/*! @name Mask definitions for BMI2 any and no-motion feature configuration */ -#define BMI2_ANY_NO_MOT_DUR_MASK UINT16_C(0x1FFF) -#define BMI2_ANY_NO_MOT_X_SEL_MASK UINT16_C(0x2000) -#define BMI2_ANY_NO_MOT_Y_SEL_MASK UINT16_C(0x4000) -#define BMI2_ANY_NO_MOT_Z_SEL_MASK UINT16_C(0x8000) -#define BMI2_ANY_NO_MOT_THRES_MASK UINT16_C(0x07FF) -#define BMI2_ANY_MOT_INT_MASK UINT8_C(0x40) - -/*! @name Mask definitions for BMI2 no-motion interrupt mapping */ -#define BMI2_NO_MOT_INT_MASK UINT8_C(0x20) - -/*! @name Bit position definitions for BMI2 any and no-motion feature - * configuration - */ -#define BMI2_ANY_NO_MOT_X_SEL_POS UINT8_C(0x0D) -#define BMI2_ANY_NO_MOT_Y_SEL_POS UINT8_C(0x0E) -#define BMI2_ANY_NO_MOT_Z_SEL_POS UINT8_C(0x0F) - -/*! @name Mask definitions for BMI2 orientation feature configuration */ -#define BMI2_ORIENT_UP_DOWN_MASK UINT16_C(0x0002) -#define BMI2_ORIENT_SYMM_MODE_MASK UINT16_C(0x000C) -#define BMI2_ORIENT_BLOCK_MODE_MASK UINT16_C(0x0030) -#define BMI2_ORIENT_THETA_MASK UINT16_C(0x0FC0) -#define BMI2_ORIENT_HYST_MASK UINT16_C(0x07FF) - -/*! @name Bit position definitions for BMI2 orientation feature configuration */ -#define BMI2_ORIENT_UP_DOWN_POS UINT8_C(0x01) -#define BMI2_ORIENT_SYMM_MODE_POS UINT8_C(0x02) -#define BMI2_ORIENT_BLOCK_MODE_POS UINT8_C(0x04) -#define BMI2_ORIENT_THETA_POS UINT8_C(0x06) - -/*! @name Mask definitions for BMI2 sig-motion feature configuration */ -#define BMI2_SIG_MOT_PARAM_1_MASK UINT16_C(0xFFFF) -#define BMI2_SIG_MOT_PARAM_2_MASK UINT16_C(0xFFFF) -#define BMI2_SIG_MOT_PARAM_3_MASK UINT16_C(0xFFFF) -#define BMI2_SIG_MOT_PARAM_4_MASK UINT16_C(0xFFFF) -#define BMI2_SIG_MOT_PARAM_5_MASK UINT16_C(0xFFFF) - -/*! @name Mask definitions for BMI2 parameter configurations */ -#define BMI2_STEP_COUNT_PARAMS_MASK UINT16_C(0xFFFF) - -/*! @name Mask definitions for BMI2 step-counter/detector feature configuration */ -#define BMI2_STEP_COUNT_WM_LEVEL_MASK UINT16_C(0x03FF) -#define BMI2_STEP_COUNT_RST_CNT_MASK UINT16_C(0x0400) -#define BMI2_STEP_BUFFER_SIZE_MASK UINT16_C(0XFF00) -#define BMI2_STEP_COUNT_INT_MASK UINT8_C(0x02) -#define BMI2_STEP_ACT_INT_MASK UINT8_C(0x04) - -/*! @name Bit position definitions for BMI2 step-counter/detector feature - * configuration - */ -#define BMI2_STEP_COUNT_RST_CNT_POS UINT8_C(0x0A) -#define BMI2_STEP_BUFFER_SIZE_POS UINT8_C(0X08) - -/*! @name Mask definitions for BMI2 gyroscope user gain feature - * configuration - */ -#define BMI2_GYR_USER_GAIN_RATIO_X_MASK UINT16_C(0x07FF) -#define BMI2_GYR_USER_GAIN_RATIO_Y_MASK UINT16_C(0x07FF) -#define BMI2_GYR_USER_GAIN_RATIO_Z_MASK UINT16_C(0x07FF) - -/*! @name Mask definitions for BMI2 gyroscope user gain saturation status */ -#define BMI2_GYR_USER_GAIN_SAT_STAT_X_MASK UINT8_C(0x01) -#define BMI2_GYR_USER_GAIN_SAT_STAT_Y_MASK UINT8_C(0x02) -#define BMI2_GYR_USER_GAIN_SAT_STAT_Z_MASK UINT8_C(0x04) -#define BMI2_G_TRIGGER_STAT_MASK UINT8_C(0x38) - -/*! @name Bit position definitions for BMI2 gyroscope user gain saturation status */ -#define BMI2_GYR_USER_GAIN_SAT_STAT_Y_POS UINT8_C(0x01) -#define BMI2_GYR_USER_GAIN_SAT_STAT_Z_POS UINT8_C(0x02) -#define BMI2_G_TRIGGER_STAT_POS UINT8_C(0x03) - -/*! @name Mask definitions for MSB values of BMI2 gyroscope compensation */ -#define BMI2_GYR_OFF_COMP_MSB_X_MASK UINT8_C(0x03) -#define BMI2_GYR_OFF_COMP_MSB_Y_MASK UINT8_C(0x0C) -#define BMI2_GYR_OFF_COMP_MSB_Z_MASK UINT8_C(0x30) - -/*! @name Bit positions for MSB values of BMI2 gyroscope compensation */ -#define BMI2_GYR_OFF_COMP_MSB_Y_POS UINT8_C(0x02) -#define BMI2_GYR_OFF_COMP_MSB_Z_POS UINT8_C(0x04) - -/*! @name Mask definitions for MSB values of BMI2 gyroscope compensation from user input */ -#define BMI2_GYR_OFF_COMP_MSB_MASK UINT16_C(0x0300) -#define BMI2_GYR_OFF_COMP_LSB_MASK UINT16_C(0x00FF) - -/*! @name Mask definitions for BMI2 orientation status */ -#define BMI2_ORIENT_DETECT_MASK UINT8_C(0x03) -#define BMI2_ORIENT_FACE_UP_DWN_MASK UINT8_C(0x04) - -/*! @name Bit position definitions for BMI2 orientation status */ -#define BMI2_ORIENT_FACE_UP_DWN_POS UINT8_C(0x02) - -/*! @name Mask definitions for NVM-VFRM error status */ -#define BMI2_NVM_LOAD_ERR_STATUS_MASK UINT8_C(0x01) -#define BMI2_NVM_PROG_ERR_STATUS_MASK UINT8_C(0x02) -#define BMI2_NVM_ERASE_ERR_STATUS_MASK UINT8_C(0x04) -#define BMI2_NVM_END_EXCEED_STATUS_MASK UINT8_C(0x08) -#define BMI2_NVM_PRIV_ERR_STATUS_MASK UINT8_C(0x10) -#define BMI2_VFRM_LOCK_ERR_STATUS_MASK UINT8_C(0x20) -#define BMI2_VFRM_WRITE_ERR_STATUS_MASK UINT8_C(0x40) -#define BMI2_VFRM_FATAL_ERR_STATUS_MASK UINT8_C(0x80) - -/*! @name Bit positions for NVM-VFRM error status */ -#define BMI2_NVM_PROG_ERR_STATUS_POS UINT8_C(0x01) -#define BMI2_NVM_ERASE_ERR_STATUS_POS UINT8_C(0x02) -#define BMI2_NVM_END_EXCEED_STATUS_POS UINT8_C(0x03) -#define BMI2_NVM_PRIV_ERR_STATUS_POS UINT8_C(0x04) -#define BMI2_VFRM_LOCK_ERR_STATUS_POS UINT8_C(0x05) -#define BMI2_VFRM_WRITE_ERR_STATUS_POS UINT8_C(0x06) -#define BMI2_VFRM_FATAL_ERR_STATUS_POS UINT8_C(0x07) - -/*! @name Mask definitions for accelerometer self-test status */ -#define BMI2_ACC_SELF_TEST_DONE_MASK UINT8_C(0x01) -#define BMI2_ACC_X_OK_MASK UINT8_C(0x02) -#define BMI2_ACC_Y_OK_MASK UINT8_C(0x04) -#define BMI2_ACC_Z_OK_MASK UINT8_C(0x08) - -/*! @name Bit Positions for accelerometer self-test status */ -#define BMI2_ACC_X_OK_POS UINT8_C(0x01) -#define BMI2_ACC_Y_OK_POS UINT8_C(0x02) -#define BMI2_ACC_Z_OK_POS UINT8_C(0x03) - -/*! @name Mask definitions for BMI2 high-g feature configuration */ -#define BMI2_HIGH_G_THRES_MASK UINT16_C(0x7FFF) -#define BMI2_HIGH_G_HYST_MASK UINT16_C(0x0FFF) -#define BMI2_HIGH_G_X_SEL_MASK UINT16_C(0x1000) -#define BMI2_HIGH_G_Y_SEL_MASK UINT16_C(0x2000) -#define BMI2_HIGH_G_Z_SEL_MASK UINT16_C(0x4000) -#define BMI2_HIGH_G_DUR_MASK UINT16_C(0x0FFF) - -/*! @name Bit position definitions for BMI2 high-g feature configuration */ -#define BMI2_HIGH_G_X_SEL_POS UINT8_C(0x0C) -#define BMI2_HIGH_G_Y_SEL_POS UINT8_C(0x0D) -#define BMI2_HIGH_G_Z_SEL_POS UINT8_C(0x0E) - -/*! @name Mask definitions for BMI2 low-g feature configuration */ -#define BMI2_LOW_G_THRES_MASK UINT16_C(0x7FFF) -#define BMI2_LOW_G_HYST_MASK UINT16_C(0x0FFF) -#define BMI2_LOW_G_DUR_MASK UINT16_C(0x0FFF) - -/*! @name Mask definitions for BMI2 free-fall detection feature configuration */ -#define BMI2_FREE_FALL_ACCEL_SETT_MASK UINT16_C(0xFFFF) - -/*! @name Mask definitions for BMI2 flat feature configuration */ -#define BMI2_FLAT_THETA_MASK UINT16_C(0x007E) -#define BMI2_FLAT_BLOCK_MASK UINT16_C(0x0180) -#define BMI2_FLAT_HYST_MASK UINT16_C(0x003F) -#define BMI2_FLAT_HOLD_TIME_MASK UINT16_C(0x3FC0) - -/*! @name Bit position definitions for BMI2 flat feature configuration */ -#define BMI2_FLAT_THETA_POS UINT8_C(0x01) -#define BMI2_FLAT_BLOCK_POS UINT8_C(0x07) -#define BMI2_FLAT_HOLD_TIME_POS UINT8_C(0x06) - -/*! @name Mask definitions for BMI2 wrist gesture configuration */ -#define BMI2_WRIST_GEST_WEAR_ARM_MASK UINT16_C(0x0010) - -/*! @name Bit position definitions for wrist gesture configuration */ -#define BMI2_WRIST_GEST_WEAR_ARM_POS UINT8_C(0x04) - -/*! @name Mask definitions for BMI2 wrist gesture wh configuration */ -#define BMI2_WRIST_GEST_WH_DEVICE_POS_MASK UINT16_C(0x0001) -#define BMI2_WRIST_GEST_WH_INT UINT8_C(0x10) -#define BMI2_WRIST_GEST_WH_START_ADD UINT8_C(0x08) - -/*! @name Mask definitions for BMI2 wrist wear wake-up configuration */ -#define BMI2_WRIST_WAKE_UP_WH_INT_MASK UINT8_C(0x08) - -/*! @name Mask definition for BMI2 wrist wear wake-up configuration for wearable variant */ -#define BMI2_WRIST_WAKE_UP_ANGLE_LR_MASK UINT16_C(0x00FF) -#define BMI2_WRIST_WAKE_UP_ANGLE_LL_MASK UINT16_C(0xFF00) -#define BMI2_WRIST_WAKE_UP_ANGLE_PD_MASK UINT16_C(0x00FF) -#define BMI2_WRIST_WAKE_UP_ANGLE_PU_MASK UINT16_C(0xFF00) -#define BMI2_WRIST_WAKE_UP_MIN_DUR_MOVED_MASK UINT16_C(0x00FF) -#define BMI2_WRIST_WAKE_UP_MIN_DUR_QUITE_MASK UINT16_C(0xFF00) - -/*! @name Bit position definition for BMI2 wrist wear wake-up configuration for wearable variant */ -#define BMI2_WRIST_WAKE_UP_ANGLE_LL_POS UINT16_C(0x0008) -#define BMI2_WRIST_WAKE_UP_ANGLE_PU_POS UINT16_C(0x0008) -#define BMI2_WRIST_WAKE_UP_MIN_DUR_QUITE_POS UINT16_C(0x0008) - -/*! @name Macros to define values of BMI2 axis and its sign for re-map - * settings - */ -#define BMI2_MAP_X_AXIS UINT8_C(0x00) -#define BMI2_MAP_Y_AXIS UINT8_C(0x01) -#define BMI2_MAP_Z_AXIS UINT8_C(0x02) -#define BMI2_MAP_POSITIVE UINT8_C(0x00) -#define BMI2_MAP_NEGATIVE UINT8_C(0x01) - -/*! @name Mask definitions of BMI2 axis re-mapping */ -#define BMI2_X_AXIS_MASK UINT8_C(0x03) -#define BMI2_X_AXIS_SIGN_MASK UINT8_C(0x04) -#define BMI2_Y_AXIS_MASK UINT8_C(0x18) -#define BMI2_Y_AXIS_SIGN_MASK UINT8_C(0x20) -#define BMI2_Z_AXIS_MASK UINT8_C(0xC0) -#define BMI2_Z_AXIS_SIGN_MASK UINT8_C(0x01) - -/*! @name Bit position definitions of BMI2 axis re-mapping */ -#define BMI2_X_AXIS_SIGN_POS UINT8_C(0x02) -#define BMI2_Y_AXIS_POS UINT8_C(0x03) -#define BMI2_Y_AXIS_SIGN_POS UINT8_C(0x05) -#define BMI2_Z_AXIS_POS UINT8_C(0x06) - -/*! @name Macros to define polarity */ -#define BMI2_NEG_SIGN UINT8_C(1) -#define BMI2_POS_SIGN UINT8_C(0) - -/*! @name Macro to define related to CRT */ -#define BMI2_CRT_READY_FOR_DOWNLOAD_US UINT16_C(2000) -#define BMI2_CRT_READY_FOR_DOWNLOAD_RETRY UINT8_C(100) - -#define BMI2_CRT_WAIT_RUNNING_US UINT16_C(10000) -#define BMI2_CRT_WAIT_RUNNING_RETRY_EXECUTION UINT8_C(200) - -#define BMI2_CRT_MIN_BURST_WORD_LENGTH UINT8_C(2) -#define BMI2_CRT_MAX_BURST_WORD_LENGTH UINT16_C(255) - -#define BMI2_ACC_FOC_2G_REF UINT16_C(16384) -#define BMI2_ACC_FOC_4G_REF UINT16_C(8192) -#define BMI2_ACC_FOC_8G_REF UINT16_C(4096) -#define BMI2_ACC_FOC_16G_REF UINT16_C(2048) - -#define BMI2_GYRO_FOC_NOISE_LIMIT_NEGATIVE INT8_C(-20) -#define BMI2_GYRO_FOC_NOISE_LIMIT_POSITIVE INT8_C(20) - -/* reference value with positive and negative noise range in lsb */ -#define BMI2_ACC_2G_MAX_NOISE_LIMIT (BMI2_ACC_FOC_2G_REF + UINT16_C(255)) -#define BMI2_ACC_2G_MIN_NOISE_LIMIT (BMI2_ACC_FOC_2G_REF - UINT16_C(255)) -#define BMI2_ACC_4G_MAX_NOISE_LIMIT (BMI2_ACC_FOC_4G_REF + UINT16_C(255)) -#define BMI2_ACC_4G_MIN_NOISE_LIMIT (BMI2_ACC_FOC_4G_REF - UINT16_C(255)) -#define BMI2_ACC_8G_MAX_NOISE_LIMIT (BMI2_ACC_FOC_8G_REF + UINT16_C(255)) -#define BMI2_ACC_8G_MIN_NOISE_LIMIT (BMI2_ACC_FOC_8G_REF - UINT16_C(255)) -#define BMI2_ACC_16G_MAX_NOISE_LIMIT (BMI2_ACC_FOC_16G_REF + UINT16_C(255)) -#define BMI2_ACC_16G_MIN_NOISE_LIMIT (BMI2_ACC_FOC_16G_REF - UINT16_C(255)) - -#define BMI2_FOC_SAMPLE_LIMIT UINT8_C(128) - -/*! @name Bit wise selection of BMI2 sensors */ -#define BMI2_MAIN_SENSORS \ - (BMI2_ACCEL_SENS_SEL | BMI2_GYRO_SENS_SEL \ - | BMI2_AUX_SENS_SEL | BMI2_TEMP_SENS_SEL) - -/*! @name Maximum number of BMI2 main sensors */ -#define BMI2_MAIN_SENS_MAX_NUM UINT8_C(4) - -/*! @name Macro to specify the number of step counter parameters */ -#define BMI2_STEP_CNT_N_PARAMS UINT8_C(25) - -/*! @name Macro to specify the number of free-fall accel setting parameters */ -#define BMI2_FREE_FALL_ACCEL_SET_PARAMS UINT8_C(7) - -#define BMI2_SELECT_GYRO_SELF_TEST UINT8_C(0) -#define BMI2_SELECT_CRT UINT8_C(1) - -/*! @name Macro for NVM enable */ -#define BMI2_NVM_UNLOCK_ENABLE UINT8_C(0x02) -#define BMI2_NVM_UNLOCK_DISABLE UINT8_C(0x00) - -/*! @name macro to select between gyro self test and CRT */ -#define BMI2_GYRO_SELF_TEST_SEL UINT8_C(0) -#define BMI2_CRT_SEL UINT8_C(1) - -/******************************************************************************/ -/*! @name Accelerometer Macro Definitions */ -/******************************************************************************/ -/*! @name Accelerometer Bandwidth parameters */ -#define BMI2_ACC_OSR4_AVG1 UINT8_C(0x00) -#define BMI2_ACC_OSR2_AVG2 UINT8_C(0x01) -#define BMI2_ACC_NORMAL_AVG4 UINT8_C(0x02) -#define BMI2_ACC_CIC_AVG8 UINT8_C(0x03) -#define BMI2_ACC_RES_AVG16 UINT8_C(0x04) -#define BMI2_ACC_RES_AVG32 UINT8_C(0x05) -#define BMI2_ACC_RES_AVG64 UINT8_C(0x06) -#define BMI2_ACC_RES_AVG128 UINT8_C(0x07) - -/*! @name Accelerometer Output Data Rate */ -#define BMI2_ACC_ODR_0_78HZ UINT8_C(0x01) -#define BMI2_ACC_ODR_1_56HZ UINT8_C(0x02) -#define BMI2_ACC_ODR_3_12HZ UINT8_C(0x03) -#define BMI2_ACC_ODR_6_25HZ UINT8_C(0x04) -#define BMI2_ACC_ODR_12_5HZ UINT8_C(0x05) -#define BMI2_ACC_ODR_25HZ UINT8_C(0x06) -#define BMI2_ACC_ODR_50HZ UINT8_C(0x07) -#define BMI2_ACC_ODR_100HZ UINT8_C(0x08) -#define BMI2_ACC_ODR_200HZ UINT8_C(0x09) -#define BMI2_ACC_ODR_400HZ UINT8_C(0x0A) -#define BMI2_ACC_ODR_800HZ UINT8_C(0x0B) -#define BMI2_ACC_ODR_1600HZ UINT8_C(0x0C) - -/*! @name Accelerometer G Range */ -#define BMI2_ACC_RANGE_2G UINT8_C(0x00) -#define BMI2_ACC_RANGE_4G UINT8_C(0x01) -#define BMI2_ACC_RANGE_8G UINT8_C(0x02) -#define BMI2_ACC_RANGE_16G UINT8_C(0x03) - -/*! @name Mask definitions for accelerometer configuration register */ -#define BMI2_ACC_RANGE_MASK UINT8_C(0x03) -#define BMI2_ACC_ODR_MASK UINT8_C(0x0F) -#define BMI2_ACC_BW_PARAM_MASK UINT8_C(0x70) -#define BMI2_ACC_FILTER_PERF_MODE_MASK UINT8_C(0x80) - -/*! @name Bit position definitions for accelerometer configuration register */ -#define BMI2_ACC_BW_PARAM_POS UINT8_C(0x04) -#define BMI2_ACC_FILTER_PERF_MODE_POS UINT8_C(0x07) - -/*! @name Self test macro to define range */ -#define BMI2_ACC_SELF_TEST_RANGE UINT8_C(16) - -/*! @name Self test macro to show resulting minimum and maximum difference - * signal of the axes in mg - */ -#define BMI2_ST_ACC_X_SIG_MIN_DIFF INT16_C(16000) -#define BMI2_ST_ACC_Y_SIG_MIN_DIFF INT16_C(-15000) -#define BMI2_ST_ACC_Z_SIG_MIN_DIFF INT16_C(10000) - -/*! @name Mask definitions for accelerometer self-test */ -#define BMI2_ACC_SELF_TEST_EN_MASK UINT8_C(0x01) -#define BMI2_ACC_SELF_TEST_SIGN_MASK UINT8_C(0x04) -#define BMI2_ACC_SELF_TEST_AMP_MASK UINT8_C(0x08) - -/*! @name Bit Positions for accelerometer self-test */ -#define BMI2_ACC_SELF_TEST_SIGN_POS UINT8_C(0x02) -#define BMI2_ACC_SELF_TEST_AMP_POS UINT8_C(0x03) - -/*! @name MASK definition for gyro self test status */ -#define BMI2_GYR_ST_AXES_DONE_MASK UINT8_C(0X01) -#define BMI2_GYR_AXIS_X_OK_MASK UINT8_C(0x02) -#define BMI2_GYR_AXIS_Y_OK_MASK UINT8_C(0x04) -#define BMI2_GYR_AXIS_Z_OK_MASK UINT8_C(0x08) - -/*! @name Bit position for gyro self test status */ -#define BMI2_GYR_AXIS_X_OK_POS UINT8_C(0x01) -#define BMI2_GYR_AXIS_Y_OK_POS UINT8_C(0x02) -#define BMI2_GYR_AXIS_Z_OK_POS UINT8_C(0x03) - -/******************************************************************************/ -/*! @name Gyroscope Macro Definitions */ -/******************************************************************************/ -/*! @name Gyroscope Bandwidth parameters */ -#define BMI2_GYR_OSR4_MODE UINT8_C(0x00) -#define BMI2_GYR_OSR2_MODE UINT8_C(0x01) -#define BMI2_GYR_NORMAL_MODE UINT8_C(0x02) -#define BMI2_GYR_CIC_MODE UINT8_C(0x03) - -/*! @name Gyroscope Output Data Rate */ -#define BMI2_GYR_ODR_25HZ UINT8_C(0x06) -#define BMI2_GYR_ODR_50HZ UINT8_C(0x07) -#define BMI2_GYR_ODR_100HZ UINT8_C(0x08) -#define BMI2_GYR_ODR_200HZ UINT8_C(0x09) -#define BMI2_GYR_ODR_400HZ UINT8_C(0x0A) -#define BMI2_GYR_ODR_800HZ UINT8_C(0x0B) -#define BMI2_GYR_ODR_1600HZ UINT8_C(0x0C) -#define BMI2_GYR_ODR_3200HZ UINT8_C(0x0D) - -/*! @name Gyroscope OIS Range */ -#define BMI2_GYR_OIS_250 UINT8_C(0x00) -#define BMI2_GYR_OIS_2000 UINT8_C(0x01) - -/*! @name Gyroscope Angular Rate Measurement Range */ -#define BMI2_GYR_RANGE_2000 UINT8_C(0x00) -#define BMI2_GYR_RANGE_1000 UINT8_C(0x01) -#define BMI2_GYR_RANGE_500 UINT8_C(0x02) -#define BMI2_GYR_RANGE_250 UINT8_C(0x03) -#define BMI2_GYR_RANGE_125 UINT8_C(0x04) - -/*! @name Mask definitions for gyroscope configuration register */ -#define BMI2_GYR_RANGE_MASK UINT8_C(0x07) -#define BMI2_GYR_OIS_RANGE_MASK UINT8_C(0x08) -#define BMI2_GYR_ODR_MASK UINT8_C(0x0F) -#define BMI2_GYR_BW_PARAM_MASK UINT8_C(0x30) -#define BMI2_GYR_NOISE_PERF_MODE_MASK UINT8_C(0x40) -#define BMI2_GYR_FILTER_PERF_MODE_MASK UINT8_C(0x80) - -/*! @name Bit position definitions for gyroscope configuration register */ -#define BMI2_GYR_OIS_RANGE_POS UINT8_C(0x03) -#define BMI2_GYR_BW_PARAM_POS UINT8_C(0x04) -#define BMI2_GYR_NOISE_PERF_MODE_POS UINT8_C(0x06) -#define BMI2_GYR_FILTER_PERF_MODE_POS UINT8_C(0x07) - -/******************************************************************************/ -/*! @name Auxiliary Macro Definitions */ -/******************************************************************************/ -/*! @name Auxiliary Output Data Rate */ -#define BMI2_AUX_ODR_RESERVED UINT8_C(0x00) -#define BMI2_AUX_ODR_0_78HZ UINT8_C(0x01) -#define BMI2_AUX_ODR_1_56HZ UINT8_C(0x02) -#define BMI2_AUX_ODR_3_12HZ UINT8_C(0x03) -#define BMI2_AUX_ODR_6_25HZ UINT8_C(0x04) -#define BMI2_AUX_ODR_12_5HZ UINT8_C(0x05) -#define BMI2_AUX_ODR_25HZ UINT8_C(0x06) -#define BMI2_AUX_ODR_50HZ UINT8_C(0x07) -#define BMI2_AUX_ODR_100HZ UINT8_C(0x08) -#define BMI2_AUX_ODR_200HZ UINT8_C(0x09) -#define BMI2_AUX_ODR_400HZ UINT8_C(0x0A) -#define BMI2_AUX_ODR_800HZ UINT8_C(0x0B) - -/*! @name Macro to define burst read lengths for both manual and auto modes */ -#define BMI2_AUX_READ_LEN_0 UINT8_C(0x00) -#define BMI2_AUX_READ_LEN_1 UINT8_C(0x01) -#define BMI2_AUX_READ_LEN_2 UINT8_C(0x02) -#define BMI2_AUX_READ_LEN_3 UINT8_C(0x03) - -/*! @name Mask definitions for auxiliary interface configuration register */ -#define BMI2_AUX_SET_I2C_ADDR_MASK UINT8_C(0xFE) -#define BMI2_AUX_MAN_MODE_EN_MASK UINT8_C(0x80) -#define BMI2_AUX_FCU_WR_EN_MASK UINT8_C(0x40) -#define BMI2_AUX_MAN_READ_BURST_MASK UINT8_C(0x0C) -#define BMI2_AUX_READ_BURST_MASK UINT8_C(0x03) -#define BMI2_AUX_ODR_EN_MASK UINT8_C(0x0F) -#define BMI2_AUX_OFFSET_READ_OUT_MASK UINT8_C(0xF0) - -/*! @name Bit positions for auxiliary interface configuration register */ -#define BMI2_AUX_SET_I2C_ADDR_POS UINT8_C(0x01) -#define BMI2_AUX_MAN_MODE_EN_POS UINT8_C(0x07) -#define BMI2_AUX_FCU_WR_EN_POS UINT8_C(0x06) -#define BMI2_AUX_MAN_READ_BURST_POS UINT8_C(0x02) -#define BMI2_AUX_OFFSET_READ_OUT_POS UINT8_C(0x04) - -/******************************************************************************/ -/*! @name FIFO Macro Definitions */ -/******************************************************************************/ -/*! @name Macros to define virtual FIFO frame mode */ -#define BMI2_FIFO_VIRT_FRM_MODE UINT8_C(0x03) - -/*! @name FIFO Header Mask definitions */ -#define BMI2_FIFO_HEADER_ACC_FRM UINT8_C(0x84) -#define BMI2_FIFO_HEADER_AUX_FRM UINT8_C(0x90) -#define BMI2_FIFO_HEADER_GYR_FRM UINT8_C(0x88) -#define BMI2_FIFO_HEADER_GYR_ACC_FRM UINT8_C(0x8C) -#define BMI2_FIFO_HEADER_AUX_ACC_FRM UINT8_C(0x94) -#define BMI2_FIFO_HEADER_AUX_GYR_FRM UINT8_C(0x98) -#define BMI2_FIFO_HEADER_ALL_FRM UINT8_C(0x9C) -#define BMI2_FIFO_HEADER_SENS_TIME_FRM UINT8_C(0x44) -#define BMI2_FIFO_HEADER_SKIP_FRM UINT8_C(0x40) -#define BMI2_FIFO_HEADER_INPUT_CFG_FRM UINT8_C(0x48) -#define BMI2_FIFO_HEAD_OVER_READ_MSB UINT8_C(0x80) -#define BMI2_FIFO_VIRT_ACT_RECOG_FRM UINT8_C(0xC8) - -/*! @name BMI2 sensor selection for header-less frames */ -#define BMI2_FIFO_HEAD_LESS_ACC_FRM UINT8_C(0x40) -#define BMI2_FIFO_HEAD_LESS_AUX_FRM UINT8_C(0x20) -#define BMI2_FIFO_HEAD_LESS_GYR_FRM UINT8_C(0x80) -#define BMI2_FIFO_HEAD_LESS_GYR_AUX_FRM UINT8_C(0xA0) -#define BMI2_FIFO_HEAD_LESS_GYR_ACC_FRM UINT8_C(0xC0) -#define BMI2_FIFO_HEAD_LESS_AUX_ACC_FRM UINT8_C(0x60) -#define BMI2_FIFO_HEAD_LESS_ALL_FRM UINT8_C(0xE0) - -/*! @name Mask definitions for FIFO frame content configuration */ -#define BMI2_FIFO_STOP_ON_FULL UINT16_C(0x0001) -#define BMI2_FIFO_TIME_EN UINT16_C(0x0002) -#define BMI2_FIFO_TAG_INT1 UINT16_C(0x0300) -#define BMI2_FIFO_TAG_INT2 UINT16_C(0x0C00) -#define BMI2_FIFO_HEADER_EN UINT16_C(0x1000) -#define BMI2_FIFO_AUX_EN UINT16_C(0x2000) -#define BMI2_FIFO_ACC_EN UINT16_C(0x4000) -#define BMI2_FIFO_GYR_EN UINT16_C(0x8000) -#define BMI2_FIFO_ALL_EN UINT16_C(0xE000) - -/*! @name FIFO sensor data lengths */ -#define BMI2_FIFO_ACC_LENGTH UINT8_C(6) -#define BMI2_FIFO_GYR_LENGTH UINT8_C(6) -#define BMI2_FIFO_AUX_LENGTH UINT8_C(8) -#define BMI2_FIFO_ACC_AUX_LENGTH UINT8_C(14) -#define BMI2_FIFO_GYR_AUX_LENGTH UINT8_C(14) -#define BMI2_FIFO_ACC_GYR_LENGTH UINT8_C(12) -#define BMI2_FIFO_ALL_LENGTH UINT8_C(20) -#define BMI2_SENSOR_TIME_LENGTH UINT8_C(3) -#define BMI2_FIFO_CONFIG_LENGTH UINT8_C(2) -#define BMI2_FIFO_WM_LENGTH UINT8_C(2) -#define BMI2_MAX_VALUE_FIFO_FILTER UINT8_C(1) -#define BMI2_FIFO_DATA_LENGTH UINT8_C(2) -#define BMI2_FIFO_LENGTH_MSB_BYTE UINT8_C(1) -#define BMI2_FIFO_INPUT_CFG_LENGTH UINT8_C(4) -#define BMI2_FIFO_SKIP_FRM_LENGTH UINT8_C(1) - -/*! @name FIFO sensor virtual data lengths: sensor data plus sensor time */ -#define BMI2_FIFO_VIRT_ACC_LENGTH UINT8_C(9) -#define BMI2_FIFO_VIRT_GYR_LENGTH UINT8_C(9) -#define BMI2_FIFO_VIRT_AUX_LENGTH UINT8_C(11) -#define BMI2_FIFO_VIRT_ACC_AUX_LENGTH UINT8_C(17) -#define BMI2_FIFO_VIRT_GYR_AUX_LENGTH UINT8_C(17) -#define BMI2_FIFO_VIRT_ACC_GYR_LENGTH UINT8_C(15) -#define BMI2_FIFO_VIRT_ALL_LENGTH UINT8_C(23) - -/*! @name FIFO sensor virtual data lengths: activity recognition */ -#define BMI2_FIFO_VIRT_ACT_DATA_LENGTH UINT8_C(6) -#define BMI2_FIFO_VIRT_ACT_TIME_LENGTH UINT8_C(4) -#define BMI2_FIFO_VIRT_ACT_TYPE_LENGTH UINT8_C(1) -#define BMI2_FIFO_VIRT_ACT_STAT_LENGTH UINT8_C(1) - -/*! @name BMI2 FIFO data filter modes */ -#define BMI2_FIFO_UNFILTERED_DATA UINT8_C(0) -#define BMI2_FIFO_FILTERED_DATA UINT8_C(1) - -/*! @name FIFO frame masks */ -#define BMI2_FIFO_LSB_CONFIG_CHECK UINT8_C(0x00) -#define BMI2_FIFO_MSB_CONFIG_CHECK UINT8_C(0x80) -#define BMI2_FIFO_TAG_INTR_MASK UINT8_C(0xFF) - -/*! @name BMI2 Mask definitions of FIFO configuration registers */ -#define BMI2_FIFO_CONFIG_0_MASK UINT16_C(0x0003) -#define BMI2_FIFO_CONFIG_1_MASK UINT16_C(0xFF00) - -/*! @name FIFO self wake-up mask definition */ -#define BMI2_FIFO_SELF_WAKE_UP_MASK UINT8_C(0x02) - -/*! @name FIFO down sampling mask definition */ -#define BMI2_ACC_FIFO_DOWNS_MASK UINT8_C(0x70) -#define BMI2_GYR_FIFO_DOWNS_MASK UINT8_C(0x07) - -/*! @name FIFO down sampling bit positions */ -#define BMI2_ACC_FIFO_DOWNS_POS UINT8_C(0x04) - -/*! @name FIFO filter mask definition */ -#define BMI2_ACC_FIFO_FILT_DATA_MASK UINT8_C(0x80) -#define BMI2_GYR_FIFO_FILT_DATA_MASK UINT8_C(0x08) - -/*! @name FIFO filter bit positions */ -#define BMI2_ACC_FIFO_FILT_DATA_POS UINT8_C(0x07) -#define BMI2_GYR_FIFO_FILT_DATA_POS UINT8_C(0x03) - -/*! @name FIFO byte counter mask definition */ -#define BMI2_FIFO_BYTE_COUNTER_MSB_MASK UINT8_C(0x3F) - -/*! @name FIFO self wake-up bit positions */ -#define BMI2_FIFO_SELF_WAKE_UP_POS UINT8_C(0x01) - -/*! @name Mask Definitions for Virtual FIFO frames */ -#define BMI2_FIFO_VIRT_FRM_MODE_MASK UINT8_C(0xC0) -#define BMI2_FIFO_VIRT_PAYLOAD_MASK UINT8_C(0x3C) - -/*! @name Bit Positions for Virtual FIFO frames */ -#define BMI2_FIFO_VIRT_FRM_MODE_POS UINT8_C(0x06) -#define BMI2_FIFO_VIRT_PAYLOAD_POS UINT8_C(0x02) - -/******************************************************************************/ -/*! @name Interrupt Macro Definitions */ -/******************************************************************************/ -/*! @name BMI2 Interrupt Modes */ -/* Non latched */ -#define BMI2_INT_NON_LATCH UINT8_C(0) - -/* Permanently latched */ -#define BMI2_INT_LATCH UINT8_C(1) - -/*! @name BMI2 Interrupt Pin Behavior */ -#define BMI2_INT_PUSH_PULL UINT8_C(0) -#define BMI2_INT_OPEN_DRAIN UINT8_C(1) - -/*! @name BMI2 Interrupt Pin Level */ -#define BMI2_INT_ACTIVE_LOW UINT8_C(0) -#define BMI2_INT_ACTIVE_HIGH UINT8_C(1) - -/*! @name BMI2 Interrupt Output Enable */ -#define BMI2_INT_OUTPUT_DISABLE UINT8_C(0) -#define BMI2_INT_OUTPUT_ENABLE UINT8_C(1) - -/*! @name BMI2 Interrupt Input Enable */ -#define BMI2_INT_INPUT_DISABLE UINT8_C(0) -#define BMI2_INT_INPUT_ENABLE UINT8_C(1) - -/*! @name Mask definitions for interrupt pin configuration */ -#define BMI2_INT_LATCH_MASK UINT8_C(0x01) -#define BMI2_INT_LEVEL_MASK UINT8_C(0x02) -#define BMI2_INT_OPEN_DRAIN_MASK UINT8_C(0x04) -#define BMI2_INT_OUTPUT_EN_MASK UINT8_C(0x08) -#define BMI2_INT_INPUT_EN_MASK UINT8_C(0x10) - -/*! @name Bit position definitions for interrupt pin configuration */ -#define BMI2_INT_LEVEL_POS UINT8_C(0x01) -#define BMI2_INT_OPEN_DRAIN_POS UINT8_C(0x02) -#define BMI2_INT_OUTPUT_EN_POS UINT8_C(0x03) -#define BMI2_INT_INPUT_EN_POS UINT8_C(0x04) - -/*! @name Mask definitions for data interrupt mapping */ -#define BMI2_FFULL_INT UINT8_C(0x01) -#define BMI2_FWM_INT UINT8_C(0x02) -#define BMI2_DRDY_INT UINT8_C(0x04) -#define BMI2_ERR_INT UINT8_C(0x08) - -/*! @name Mask definitions for data interrupt status bits */ -#define BMI2_FFULL_INT_STATUS_MASK UINT16_C(0x0100) -#define BMI2_FWM_INT_STATUS_MASK UINT16_C(0x0200) -#define BMI2_ERR_INT_STATUS_MASK UINT16_C(0x0400) -#define BMI2_AUX_DRDY_INT_MASK UINT16_C(0x2000) -#define BMI2_GYR_DRDY_INT_MASK UINT16_C(0x4000) -#define BMI2_ACC_DRDY_INT_MASK UINT16_C(0x8000) - -/*! @name Maximum number of interrupt pins */ -#define BMI2_INT_PIN_MAX_NUM UINT8_C(2) - -/*! @name Macro for mapping feature interrupts */ -#define BMI2_FEAT_BIT_DISABLE UINT8_C(0) -#define BMI2_FEAT_BIT0 UINT8_C(1) -#define BMI2_FEAT_BIT1 UINT8_C(2) -#define BMI2_FEAT_BIT2 UINT8_C(3) -#define BMI2_FEAT_BIT3 UINT8_C(4) -#define BMI2_FEAT_BIT4 UINT8_C(5) -#define BMI2_FEAT_BIT5 UINT8_C(6) -#define BMI2_FEAT_BIT6 UINT8_C(7) -#define BMI2_FEAT_BIT7 UINT8_C(8) -#define BMI2_FEAT_BIT_MAX UINT8_C(9) - -/******************************************************************************/ -/*! @name OIS Interface Macro Definitions */ -/******************************************************************************/ -/*! @name Mask definitions for interface configuration register */ -#define BMI2_OIS_IF_EN_MASK UINT8_C(0x10) -#define BMI2_AUX_IF_EN_MASK UINT8_C(0x20) - -/*! @name Bit positions for OIS interface enable */ -#define BMI2_OIS_IF_EN_POS UINT8_C(0x04) -#define BMI2_AUX_IF_EN_POS UINT8_C(0x05) - -/******************************************************************************/ -/*! @name Macro Definitions for Axes re-mapping */ -/******************************************************************************/ -/*! @name Macros for the user-defined values of axes and their polarities */ -#define BMI2_X UINT8_C(0x01) -#define BMI2_NEG_X UINT8_C(0x09) -#define BMI2_Y UINT8_C(0x02) -#define BMI2_NEG_Y UINT8_C(0x0A) -#define BMI2_Z UINT8_C(0x04) -#define BMI2_NEG_Z UINT8_C(0x0C) -#define BMI2_AXIS_MASK UINT8_C(0x07) -#define BMI2_AXIS_SIGN UINT8_C(0x08) - -/******************************************************************************/ -/*! @name Macro Definitions for offset and gain compensation */ -/******************************************************************************/ -/*! @name Mask definitions of gyroscope offset compensation registers */ -#define BMI2_GYR_GAIN_EN_MASK UINT8_C(0x80) -#define BMI2_GYR_OFF_COMP_EN_MASK UINT8_C(0x40) - -/*! @name Bit positions of gyroscope offset compensation registers */ -#define BMI2_GYR_OFF_COMP_EN_POS UINT8_C(0x06) - -/*! @name Mask definitions of gyroscope user-gain registers */ -#define BMI2_GYR_USR_GAIN_X_MASK UINT8_C(0x7F) -#define BMI2_GYR_USR_GAIN_Y_MASK UINT8_C(0x7F) -#define BMI2_GYR_USR_GAIN_Z_MASK UINT8_C(0x7F) - -/*! @name Bit positions of gyroscope offset compensation registers */ -#define BMI2_GYR_GAIN_EN_POS UINT8_C(0x07) - -/******************************************************************************/ -/*! @name Macro Definitions for internal status */ -/******************************************************************************/ -#define BMI2_NOT_INIT UINT8_C(0x00) -#define BMI2_INIT_OK UINT8_C(0x01) -#define BMI2_INIT_ERR UINT8_C(0x02) -#define BMI2_DRV_ERR UINT8_C(0x03) -#define BMI2_SNS_STOP UINT8_C(0x04) -#define BMI2_NVM_ERROR UINT8_C(0x05) -#define BMI2_START_UP_ERROR UINT8_C(0x06) -#define BMI2_COMPAT_ERROR UINT8_C(0x07) -#define BMI2_VFM_SKIPPED UINT8_C(0x10) -#define BMI2_AXES_MAP_ERROR UINT8_C(0x20) -#define BMI2_ODR_50_HZ_ERROR UINT8_C(0x40) -#define BMI2_ODR_HIGH_ERROR UINT8_C(0x80) - -/******************************************************************************/ -/*! @name error status form gyro gain update status. */ -/******************************************************************************/ -#define BMI2_G_TRIGGER_NO_ERROR UINT8_C(0x00) - -#define BMI2_G_TRIGGER_PRECON_ERROR UINT8_C(0x01) -#define BMI2_G_TRIGGER_DL_ERROR UINT8_C(0x02) -#define BMI2_G_TRIGGER_ABORT_ERROR UINT8_C(0x03) - -/******************************************************************************/ -/*! @name Variant specific features selection macros */ -/******************************************************************************/ -#define BMI2_CRT_RTOSK_ENABLE UINT8_C(0x01) -#define BMI2_GYRO_CROSS_SENS_ENABLE UINT8_C(0x02) -#define BMI2_GYRO_USER_GAIN_ENABLE UINT8_C(0x08) -#define BMI2_NO_FEATURE_ENABLE UINT8_C(0x00) -#define BMI2_CRT_IN_FIFO_NOT_REQ UINT8_C(0x10) -#define BMI2_MAXIMUM_FIFO_VARIANT UINT8_C(0x20) - -/*! Pull-up configuration for ASDA */ -#define BMI2_ASDA_PUPSEL_OFF UINT8_C(0x00) -#define BMI2_ASDA_PUPSEL_40K UINT8_C(0x01) -#define BMI2_ASDA_PUPSEL_10K UINT8_C(0x02) -#define BMI2_ASDA_PUPSEL_2K UINT8_C(0x03) - -/******************************************************************************/ -/*! @name Function Pointers */ -/******************************************************************************/ - -/*! - * @brief Bus communication function pointer which should be mapped to - * the platform specific read functions of the user - * - * @param[in] reg_addr : Register address from which data is read. - * @param[out] reg_data : Pointer to data buffer where read data is stored. - * @param[in] len : Number of bytes of data to be read. - * @param[in, out] intf_ptr : Void pointer that can enable the linking of descriptors - * for interface related call backs. - * - * retval = BMA4_INTF_RET_SUCCESS -> Success - * retval != BMA4_INTF_RET_SUCCESS -> Failure - * - */ -typedef BMI2_INTF_RETURN_TYPE (*bmi2_read_fptr_t)(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr); - -/*! - * @brief Bus communication function pointer which should be mapped to - * the platform specific write functions of the user - * - * @param[in] reg_addr : Register address to which the data is written. - * @param[in] reg_data : Pointer to data buffer in which data to be written - * is stored. - * @param[in] len : Number of bytes of data to be written. - * @param[in, out] intf_ptr : Void pointer that can enable the linking of descriptors - * for interface related call backs - * - * retval = BMA4_INTF_RET_SUCCESS -> Success - * retval != BMA4_INTF_RET_SUCCESS -> Failure - * - */ -typedef BMI2_INTF_RETURN_TYPE (*bmi2_write_fptr_t)(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, - void *intf_ptr); - -/*! - * @brief Delay function pointer which should be mapped to - * delay function of the user - * - * @param[in] period : Delay in microseconds. - * @param[in, out] intf_ptr : Void pointer that can enable the linking of descriptors - * for interface related call backs - * - */ -typedef void (*bmi2_delay_fptr_t)(uint32_t period, void *intf_ptr); - -/*! - * @brief To get the configurations for wake_up feature, since wakeup feature is different for bmi260 and bmi261. - * - * @param[out] wake_up : Void pointer to store bmi2_wake_up_config structure. - * @param[in, out] bmi2_dev : Void pointer to store bmi2_dev structure. - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - * - */ -typedef int8_t (*bmi2_wake_up_fptr_t)(void *wake_up, void *bmi2_dev); - -/*! - * @brief To get the configurations for tap feature. - * - * @param[out] tap : Void pointer to store bmi2_tap_config structure. - * @param[in, out] bmi2_dev : Void pointer to store bmi2_dev structure. - * - * @return Result of API execution status - * - * @retval BMI2_OK - Success. - * @retval BMI2_E_COM_FAIL - Error: Communication fail - * @retval BMI2_E_NULL_PTR - Error: Null pointer error - * @retval BMI2_E_INVALID_PAGE - Error: Invalid Page - * - */ -typedef int8_t (*bmi2_tap_fptr_t)(void *tap, void *bmi2_dev); - -/******************************************************************************/ -/*! @name Enum Declarations */ -/******************************************************************************/ -/*! @name Enum to define BMI2 sensor interfaces */ -enum bmi2_intf { - BMI2_SPI_INTF = 0, - BMI2_I2C_INTF, - BMI2_I3C_INTF -}; - -/*! @name Enum to define BMI2 sensor configuration errors for accelerometer - * and gyroscope - */ -enum bmi2_sensor_config_error { - BMI2_NO_ERROR, - BMI2_ACC_ERROR, - BMI2_GYR_ERROR, - BMI2_ACC_GYR_ERROR -}; - -/*! @name Enum to define interrupt lines */ -enum bmi2_hw_int_pin { - BMI2_INT_NONE, - BMI2_INT1, - BMI2_INT2, - BMI2_INT_BOTH, - BMI2_INT_PIN_MAX -}; - -/*! @name Enum for the position of the wearable device */ -enum bmi2_wear_arm_pos { - BMI2_ARM_LEFT, - BMI2_ARM_RIGHT -}; - -/*! @name Enum to display type of activity recognition */ -enum bmi2_act_recog_type { - BMI2_ACT_UNKNOWN, - BMI2_ACT_STILL, - BMI2_ACT_WALK, - BMI2_ACT_RUN, - BMI2_ACT_BIKE, - BMI2_ACT_VEHICLE, - BMI2_ACT_TILTED -}; - -/*! @name Enum to display activity recognition status */ -enum bmi2_act_recog_stat { - BMI2_ACT_START = 1, - BMI2_ACT_END -}; - -/******************************************************************************/ -/*! @name Structure Declarations */ -/******************************************************************************/ -/*! @name Structure to store the compensated user-gain data of gyroscope */ -struct bmi2_gyro_user_gain_data -{ - /*! x-axis */ - int8_t x; - - /*! y-axis */ - int8_t y; - - /*! z-axis */ - int8_t z; -}; - -/*! @name Structure to store the re-mapped axis */ -struct bmi2_remap -{ - /*! Re-mapped x-axis */ - uint8_t x; - - /*! Re-mapped y-axis */ - uint8_t y; - - /*! Re-mapped z-axis */ - uint8_t z; -}; - -/*! @name Structure to store the value of re-mapped axis and its sign */ -struct bmi2_axes_remap -{ - /*! Re-mapped x-axis */ - uint8_t x_axis; - - /*! Re-mapped y-axis */ - uint8_t y_axis; - - /*! Re-mapped z-axis */ - uint8_t z_axis; - - /*! Re-mapped x-axis sign */ - uint8_t x_axis_sign; - - /*! Re-mapped y-axis sign */ - uint8_t y_axis_sign; - - /*! Re-mapped z-axis sign */ - uint8_t z_axis_sign; -}; - -/*! @name Structure to define the type of sensor and its interrupt pin */ -struct bmi2_sens_int_config -{ - /*! Defines the type of sensor */ - uint8_t type; - - /*! Type of interrupt pin */ - enum bmi2_hw_int_pin hw_int_pin; -}; - -/*! @name Structure to define output for activity recognition */ -struct bmi2_act_recog_output -{ - /*! Time stamp */ - uint32_t time_stamp; - - /*! current activity */ - uint8_t curr_act; - - /*! previous activity */ - uint8_t prev_act; -}; - -/*! @name Structure to define FIFO frame configuration */ -struct bmi2_fifo_frame -{ - /*! Pointer to FIFO data */ - uint8_t *data; - - /*! Number of user defined bytes of FIFO to be read */ - uint16_t length; - - /*! Defines header/header-less mode */ - uint8_t header_enable; - - /*! Enables type of data to be streamed - accelerometer, auxiliary or - * gyroscope - */ - uint16_t data_enable; - - /*! To index accelerometer bytes */ - uint16_t acc_byte_start_idx; - - /*! To index activity output bytes */ - uint16_t act_recog_byte_start_idx; - - /*! To index auxiliary bytes */ - uint16_t aux_byte_start_idx; - - /*! To index gyroscope bytes */ - uint16_t gyr_byte_start_idx; - - /*! FIFO sensor time */ - uint32_t sensor_time; - - /*! Skipped frame count */ - uint8_t skipped_frame_count; - - /*! Type of data interrupt to be mapped */ - uint8_t data_int_map; - - /*! Water-mark level for water-mark interrupt */ - uint16_t wm_lvl; - - /*! Accelerometer frame length */ - uint8_t acc_frm_len; - - /*! Gyroscope frame length */ - uint8_t gyr_frm_len; - - /*! Auxiliary frame length */ - uint8_t aux_frm_len; - - /*! Accelerometer and gyroscope frame length */ - uint8_t acc_gyr_frm_len; - - /*! Accelerometer and auxiliary frame length */ - uint8_t acc_aux_frm_len; - - /*! Gyroscope and auxiliary frame length */ - uint8_t aux_gyr_frm_len; - - /*! Accelerometer, Gyroscope and auxiliary frame length */ - uint8_t all_frm_len; -}; - -/*! @name Structure to define Interrupt pin configuration */ -struct bmi2_int_pin_cfg -{ - /*! Configure level of interrupt pin */ - uint8_t lvl; - - /*! Configure behavior of interrupt pin */ - uint8_t od; - - /*! Output enable for interrupt pin */ - uint8_t output_en; - - /*! Input enable for interrupt pin */ - uint8_t input_en; -}; - -/*! @name Structure to define interrupt pin type, mode and configurations */ -struct bmi2_int_pin_config -{ - /*! Interrupt pin type: INT1 or INT2 or BOTH */ - uint8_t pin_type; - - /*! Latched or non-latched mode*/ - uint8_t int_latch; - - /*! Structure to define Interrupt pin configuration */ - struct bmi2_int_pin_cfg pin_cfg[BMI2_INT_PIN_MAX_NUM]; -}; - -/*! @name Structure to define an array of 8 auxiliary data bytes */ -struct bmi2_aux_fifo_data -{ - /*! Auxiliary data */ - uint8_t data[8]; - - /*! Sensor time for virtual frames */ - uint32_t virt_sens_time; -}; - -/*! @name Structure to define accelerometer and gyroscope sensor axes and - * sensor time for virtual frames - */ -struct bmi2_sens_axes_data -{ - /*! Data in x-axis */ - int16_t x; - - /*! Data in y-axis */ - int16_t y; - - /*! Data in z-axis */ - int16_t z; - - /*! Sensor time for virtual frames */ - uint32_t virt_sens_time; -}; - -/*! @name Structure to define gyroscope saturation status of user gain */ -struct bmi2_gyr_user_gain_status -{ - /*! Status in x-axis */ - uint8_t sat_x; - - /*! Status in y-axis */ - uint8_t sat_y; - - /*! Status in z-axis */ - uint8_t sat_z; - - /*! G trigger status */ - uint8_t g_trigger_status; -}; - -/*! @name Structure to store the status of gyro self test result */ -struct bmi2_gyro_self_test_status -{ - /*! gyro self test axes done */ - uint8_t gyr_st_axes_done : 1; - - /*! status of gyro X-axis self test */ - uint8_t gyr_axis_x_ok : 1; - - /*! status of gyro Y-axis self test */ - uint8_t gyr_axis_y_ok : 1; - - /*! status of gyro Z-axis self test */ - uint8_t gyr_axis_z_ok : 1; -}; - -/*! @name Structure to define NVM error status */ -struct bmi2_nvm_err_status -{ - /*! NVM load action error */ - uint8_t load_error; - - /*! NVM program action error */ - uint8_t prog_error; - - /*! NVM erase action error */ - uint8_t erase_error; - - /*! NVM program limit exceeded */ - uint8_t exceed_error; - - /*! NVM privilege error */ - uint8_t privil_error; -}; - -/*! @name Structure to define VFRM error status */ -struct bmi2_vfrm_err_status -{ - /*! VFRM lock acquire error */ - uint8_t lock_error; - - /*! VFRM write error */ - uint8_t write_error; - - /*! VFRM fatal err */ - uint8_t fatal_error; -}; - -/*! @name Structure to define accelerometer self test feature status */ -struct bmi2_acc_self_test_status -{ - /*! Accelerometer test completed */ - uint8_t acc_self_test_done; - - /*! Bit is set to 1 when accelerometer X-axis test passed */ - uint8_t acc_x_ok; - - /*! Bit is set to 1 when accelerometer y-axis test passed */ - uint8_t acc_y_ok; - - /*! Bit is set to 1 when accelerometer z-axis test passed */ - uint8_t acc_z_ok; -}; - -/*! @name Structure to define orientation output */ -struct bmi2_orientation_output -{ - /*! Orientation portrait landscape */ - uint8_t portrait_landscape; - - /*! Orientation face-up down */ - uint8_t faceup_down; -}; - -/*! @name Structure to define OIS output */ -struct bmi2_ois_output -{ - /*! OIS accel x axis */ - int16_t ois_acc_x; - - /*! OIS accel y axis */ - int16_t ois_acc_y; - - /*! OIS accel z axis */ - int16_t ois_acc_z; - - /*! ois gyro x axis */ - int16_t ois_gyro_x; - - /*! OIS gyro y axis */ - int16_t ois_gyro_y; - - /*! OIS gyro z axis */ - int16_t ois_gyro_z; -}; - -/*! @name Union to define BMI2 sensor data */ -union bmi2_sens_data -{ - /*! Accelerometer axes data */ - struct bmi2_sens_axes_data acc; - - /*! Gyroscope axes data */ - struct bmi2_sens_axes_data gyr; - - /*! Auxiliary sensor data */ - uint8_t aux_data[BMI2_AUX_NUM_BYTES]; - - /*! Step counter output */ - uint32_t step_counter_output; - - /*! Step activity output */ - uint8_t activity_output; - - /*! Orientation output */ - struct bmi2_orientation_output orient_output; - - /*! High-g output */ - uint8_t high_g_output; - - /*! Gyroscope user gain saturation status */ - struct bmi2_gyr_user_gain_status gyro_user_gain_status; - - /*! NVM error status */ - struct bmi2_nvm_err_status nvm_status; - - /*! Virtual frame error status */ - struct bmi2_vfrm_err_status vfrm_status; - - /*! Wrist gesture output */ - uint8_t wrist_gesture_output; - - /*! Gyroscope cross sense value of z axis */ - int16_t correction_factor_zx; - - /*! Accelerometer self test feature status */ - struct bmi2_acc_self_test_status accel_self_test_output; - - /*! OIS output */ - struct bmi2_ois_output ois_output; -}; - -/*! @name Structure to define type of sensor and their respective data */ -struct bmi2_sensor_data -{ - /*! Defines the type of sensor */ - uint8_t type; - - /*! Defines various sensor data */ - union bmi2_sens_data sens_data; -}; - -/*! @name Structure to define accelerometer configuration */ -struct bmi2_accel_config -{ - /*! Output data rate in Hz */ - uint8_t odr; - - /*! Bandwidth parameter */ - uint8_t bwp; - - /*! Filter performance mode */ - uint8_t filter_perf; - - /*! g-range */ - uint8_t range; -}; - -/*! @name Structure to define gyroscope configuration */ -struct bmi2_gyro_config -{ - /*! Output data rate in Hz */ - uint8_t odr; - - /*! Bandwidth parameter */ - uint8_t bwp; - - /*! Filter performance mode */ - uint8_t filter_perf; - - /*! OIS Range */ - uint8_t ois_range; - - /*! Gyroscope Range */ - uint8_t range; - - /*! Selects noise performance */ - uint8_t noise_perf; -}; - -/*! @name Structure to define auxiliary sensor configuration */ -struct bmi2_aux_config -{ - /*! Enable/Disable auxiliary interface */ - uint8_t aux_en; - - /*! Manual or Auto mode*/ - uint8_t manual_en; - - /*! Enables FCU write command on auxiliary interface */ - uint8_t fcu_write_en; - - /*! Read burst length for manual mode */ - uint8_t man_rd_burst; - - /*! Read burst length for data mode */ - uint8_t aux_rd_burst; - - /*! Output data rate */ - uint8_t odr; - - /*! Read-out offset */ - uint8_t offset; - - /*! I2c address of auxiliary sensor */ - uint8_t i2c_device_addr; - - /*! Read address of auxiliary sensor */ - uint8_t read_addr; -}; - -/*! @name Structure to define any-motion configuration */ -struct bmi2_any_motion_config -{ - /*! Duration in 50Hz samples(20msec) */ - uint16_t duration; - - /*! Acceleration slope threshold */ - uint16_t threshold; - - /*! To select per x-axis */ - uint16_t select_x; - - /*! To select per y-axis */ - uint16_t select_y; - - /*! To select per z-axis */ - uint16_t select_z; -}; - -/*! @name Structure to define no-motion configuration */ -struct bmi2_no_motion_config -{ - /*! Duration in 50Hz samples(20msec) */ - uint16_t duration; - - /*! Acceleration slope threshold */ - uint16_t threshold; - - /*! To select per x-axis */ - uint16_t select_x; - - /*! To select per y-axis */ - uint16_t select_y; - - /*! To select per z-axis */ - uint16_t select_z; -}; - -/*! @name Structure to define sig-motion configuration */ -struct bmi2_sig_motion_config -{ - /*! Block size */ - uint16_t block_size; - - /*! Parameter 2 */ - uint16_t param_2; - - /*! Parameter 3 */ - uint16_t param_3; - - /*! Parameter 4 */ - uint16_t param_4; - - /*! Parameter 5 */ - uint16_t param_5; -}; - -/*! @name Structure to define step counter/detector/activity configuration */ -struct bmi2_step_config -{ - /*! Water-mark level */ - uint16_t watermark_level; - - /*! Reset counter */ - uint16_t reset_counter; - - /*! Step buffer size */ - uint8_t step_buffer_size; -}; - -/*! @name Structure to define gyroscope user gain configuration */ -struct bmi2_gyro_user_gain_config -{ - /*! Gain update value for x-axis */ - uint16_t ratio_x; - - /*! Gain update value for y-axis */ - uint16_t ratio_y; - - /*! Gain update value for z-axis */ - uint16_t ratio_z; -}; - -/*! @name Structure to define wake-up configuration */ -struct bmi2_wake_up_config -{ - /*! Wake-up sensitivity for bmi261 */ - uint16_t sensitivity; - - /*! Tap feature for BMI261 - * For Single tap, single_tap_en = 1 - * For Double tap, single_tap_en = 0 - */ - uint16_t single_tap_en; - - /*! Enable -> Filtered tap data, Disable -> Unfiltered data */ - uint16_t data_reg_en; - - /*! Scaling factor of threshold */ - uint16_t tap_sens_thres; - - /*! Maximum duration between each taps */ - uint16_t max_gest_dur; - - /*! Minimum quite time between the two gesture detection */ - uint16_t quite_time_after_gest; - - /*! Wait time */ - uint16_t wait_for_timeout; - - /*! Axis selection */ - uint16_t axis_sel; -}; - -/*! @name Structure to define tap configuration */ -struct bmi2_tap_config -{ - /*! Tap sensitivity */ - uint16_t sensitivity; - - /*! Tap feature. - * For Single tap, single_tap_en = 1 - * For Double tap, single_tap_en = 0 - */ - uint16_t single_tap_en; - - /*! Enable -> Filtered tap data, Disable -> Unfiltered data */ - uint16_t data_reg_en; - - /*! Scaling factor of threshold */ - uint16_t tap_sens_thres; - - /*! Maximum duration between each taps */ - uint16_t max_gest_dur; - - /*! Minimum quite time between the two gesture detection */ - uint16_t quite_time_after_gest; - - /*! Wait time */ - uint16_t wait_for_timeout; - - /*! Axis selection */ - uint16_t axis_sel; -}; - -/*! @name Structure to define orientation configuration */ -struct bmi2_orient_config -{ - /*! Upside/down detection */ - uint16_t ud_en; - - /*! Symmetrical, high or low Symmetrical */ - uint16_t mode; - - /*! Blocking mode */ - uint16_t blocking; - - /*! Threshold angle */ - uint16_t theta; - - /*! Acceleration hysteresis for orientation detection */ - uint16_t hysteresis; -}; - -/*! @name Structure to define high-g configuration */ -struct bmi2_high_g_config -{ - /*! Acceleration threshold */ - uint16_t threshold; - - /*! Hysteresis */ - uint16_t hysteresis; - - /*! To select per x-axis */ - uint16_t select_x; - - /*! To select per y-axis */ - uint16_t select_y; - - /*! To select per z-axis */ - uint16_t select_z; - - /*! Duration interval */ - uint16_t duration; -}; - -/*! @name Structure to define low-g configuration */ -struct bmi2_low_g_config -{ - /*! Acceleration threshold */ - uint16_t threshold; - - /*! Hysteresis */ - uint16_t hysteresis; - - /*! Duration interval */ - uint16_t duration; -}; - -/*! @name Structure to define flat configuration */ -struct bmi2_flat_config -{ - /*! Theta angle for flat detection */ - uint16_t theta; - - /*! Blocking mode */ - uint16_t blocking; - - /*! Hysteresis for theta flat detection */ - uint16_t hysteresis; - - /*! Holds the duration in 50Hz samples(20msec) */ - uint16_t hold_time; -}; - -/*! @name Structure to define wrist gesture configuration */ -struct bmi2_wrist_gest_config -{ - /*! Wearable arm (left or right) */ - uint16_t wearable_arm; - - /*! Sine of the minimum tilt angle in portrait down direction of the device when wrist is rolled - * away from user. The configuration parameter is scaled by 2048 i.e. 2048 * sin(angle). - * Range is 1448 to 1774. Default value is 1774. */ - uint16_t min_flick_peak; - - /*! Value of minimum time difference between wrist roll-out and roll-in movement during flick gesture. - * Range is 3 to 5 samples at 50Hz. Default value is 4 (i.e. 0.08 seconds). */ - uint16_t min_flick_samples; - - /*! Maximum time within which gesture movement has to be completed. Range is 150 to 250 samples at 50Hz. - * Default value is 200 (i.e. 4 seconds). */ - uint16_t max_duration; -}; - -/*! @name Structure to define wrist wear wake-up configuration */ -struct bmi2_wrist_wear_wake_up_config -{ - /*! Cosine of min expected attitude change of the device within 1 second time window when - * moving within focus position. - * The parameter is scaled by 2048 i.e. 2048 * cos(angle). Range is 1024 to 1774. - * Default is 1448. */ - uint16_t min_angle_focus; - - /*! Cosine of min expected attitude change of the device within 1 second time window when - * moving from non-focus to focus position. - * The parameter is scaled by 2048 i.e. 2048 * cos(angle). Range is 1448 to 1856. - * Default value is 1774. */ - uint16_t min_angle_nonfocus; - - /*! Sine of the max allowed downward tilt angle in landscape right direction of the device, - * when it is in focus position - * (i.e. user is able to comfortably look at the dial of wear device). - * The configuration parameter is scaled by 2048 i.e. 2048 * sin(angle). Range is 700 to 1024. - * Default value is 1024. */ - uint16_t max_tilt_lr; - - /*! Sine of the max allowed downward tilt angle in landscape left direction of the device, - * when it is in focus position - * (i.e. user is able to comfortably look at the dial of wear device). - * The configuration parameter is scaled by 2048 i.e. 2048 * sin(angle). Range is 700 to - * 1024. Default value is 700. */ - uint16_t max_tilt_ll; - - /*! Sine of the max allowed backward tilt angle in portrait down direction of the device, - * when it is in focus position - * (i.e. user is able to comfortably look at the dial of wear device). - * The configuration parameter is scaled by 2048 i.e. 2048 * sin(angle). Range is 0 to179. - * Default value is 179. */ - uint16_t max_tilt_pd; - - /*! Sine of the maximum allowed forward tilt angle in portrait up direction of the - * device, when it is in focus position - * (i.e. user is able to comfortably look at the dial of wear device). - * The configuration parameter is scaled by 2048 i.e. 2048 * sin(angle). Range is 1774 to 1978. - * Default value is 1925. */ - uint16_t max_tilt_pu; -}; - -/*! @name Structure to define wrist gesture configuration for wearable variant */ -struct bmi2_wrist_gest_w_config -{ - /*! Wearable arm (left or right) */ - uint8_t device_position; - - /*! Minimum threshold for flick peak on y-axis */ - uint16_t min_flick_peak_y_threshold; - - /*! Minimum threshold for flick peak on z-axis */ - uint16_t min_flick_peak_z_threshold; - - /*! Maximum expected value of positive gravitational acceleration on x-axis - * when arm is in focus pose */ - uint16_t gravity_bounds_x_pos; - - /*! Maximum expected value of negative gravitational acceleration on x-axis - * when arm is in focus pose */ - uint16_t gravity_bounds_x_neg; - - /*! Maximum expected value of negative gravitational acceleration on y-axis - * when arm is in focus pose */ - uint16_t gravity_bounds_y_neg; - - /*! Maximum expected value of negative gravitational acceleration on z-axis - * when arm is in focus pose */ - uint16_t gravity_bounds_z_neg; - - /*! Exponential smoothing coefficient for adaptive peak threshold decay */ - uint16_t flick_peak_decay_coeff; - - /*! Exponential smoothing coefficient for acceleration mean estimation */ - uint16_t lp_mean_filter_coeff; - - /*! Maximum duration between 2 peaks of jiggle in samples @50Hz */ - uint16_t max_duration_jiggle_peaks; -}; - -/*! @name Structure to define wrist wear wake-up configuration for wearable configuration */ -struct bmi2_wrist_wear_wake_up_wh_config -{ - /*! Cosine of min expected attitude change of the device within 1 second time window when - * moving within focus position. - * The parameter is scaled by 2048 i.e. 2048 * cos(angle). Range is 1024 to 1774. - * Default is 1448. */ - uint16_t min_angle_focus; - - /*! Cosine of min expected attitude change of the device within 1 second time window when - * moving from non-focus to focus position. - * The parameter is scaled by 2048 i.e. 2048 * cos(angle). Range is 1448 to 1856. - * Default value is 1774. */ - uint16_t min_angle_nonfocus; - - /*! Sine of the max allowed downward tilt angle in landscape right direction of the device, - * when it is in focus position (i.e. user is able to comfortably look at the dial of wear device). - * The configuration parameter is scaled by 256 i.e. 256 * sin(angle). Range is 88 to 128. - * Default value is 128. */ - uint8_t angle_lr; - - /*! Sine of the max allowed downward tilt angle in landscape left direction of the device, - * when it is in focus position (i.e. user is able to comfortably look at the dial of wear device). - * The configuration parameter is scaled by 256 i.e. 256 * sin(angle). Range is 88 to 128. - * Default value is 128. */ - uint8_t angle_ll; - - /*! Sine of the max allowed backward tilt angle in portrait down direction of the device, - * when it is in focus position (i.e. user is able to comfortably look at the dial of wear device). - * The configuration parameter is scaled by 256 i.e. 256 * sin(angle). Range is 0 to 179. - * Default value is 22. */ - uint8_t angle_pd; - - /*! Sine of the maximum allowed forward tilt angle in portrait up direction of the device, - * when it is in focus position (i.e. user is able to comfortably look at the dial of wear device). - * The configuration parameter is scaled by 256 i.e. 256 * sin(angle). Range is 222 to 247. - * Default value is 241. */ - uint8_t angle_pu; - - /*! Minimum duration the arm should be moved while performing gesture. Range: 1 to 10, - * resolution = 20 ms. - * Default 2(40 ms)*/ - uint8_t min_dur_mov; - - /*! Minimum duration the arm should be static between two consecutive gestures. Range: 1 to - * 10, resolution = 20 ms - * Default 2(40 ms)*/ - uint8_t min_dur_quite; -}; - -/*! @name Structure to define primary OIS configuration */ -struct bmi2_primary_ois_config -{ - uint8_t lp_filter_enable; - - uint8_t lp_filter_config; - - uint8_t primary_ois_reserved; - - uint8_t primary_ois_gyro_en; - - uint8_t primary_ois_accel_en; -}; - -/*! @name Structure to configure free-fall detection settings */ -struct bmi2_free_fall_det_config -{ - /*! free-fall accel settings */ - uint16_t freefall_accel_settings[BMI2_FREE_FALL_ACCEL_SET_PARAMS]; -}; - -/*! @name Union to define the sensor configurations */ -union bmi2_sens_config_types -{ - /*! Accelerometer configuration */ - struct bmi2_accel_config acc; - - /*! Gyroscope configuration */ - struct bmi2_gyro_config gyr; - - /*! Auxiliary configuration */ - struct bmi2_aux_config aux; - - /*! Any-motion configuration */ - struct bmi2_any_motion_config any_motion; - - /*! No-motion configuration */ - struct bmi2_no_motion_config no_motion; - - /*! Sig_motion configuration */ - struct bmi2_sig_motion_config sig_motion; - - /*! Step counter parameter configuration */ - uint16_t step_counter_params[BMI2_STEP_CNT_N_PARAMS]; - - /*! Step counter/detector/activity configuration */ - struct bmi2_step_config step_counter; - - /*! Gyroscope user gain configuration */ - struct bmi2_gyro_user_gain_config gyro_gain_update; - - /*! Wake-up configuration */ - struct bmi2_wake_up_config tap; - - /*! Tap configuration */ - struct bmi2_tap_config tap_conf; - - /*! Orientation configuration */ - struct bmi2_orient_config orientation; - - /*! High-g configuration */ - struct bmi2_high_g_config high_g; - - /*! Low-g configuration */ - struct bmi2_low_g_config low_g; - - /*! Flat configuration */ - struct bmi2_flat_config flat; - - /*! Wrist gesture configuration */ - struct bmi2_wrist_gest_config wrist_gest; - - /*! Wrist wear wake-up configuration */ - struct bmi2_wrist_wear_wake_up_config wrist_wear_wake_up; - - /*! Wrist gesture configuration for wearable variant */ - struct bmi2_wrist_gest_w_config wrist_gest_w; - - /*! Wrist wear wake-up configuration for wearable variant */ - struct bmi2_wrist_wear_wake_up_wh_config wrist_wear_wake_up_wh; - - /*! Primary OIS configuration */ - struct bmi2_primary_ois_config primary_ois; - - /* Free-fall detection configurations */ - struct bmi2_free_fall_det_config free_fall_det; -}; - -/*! @name Structure to define the type of the sensor and its configurations */ -struct bmi2_sens_config -{ - /*! Defines the type of sensor */ - uint8_t type; - - /*! Defines various sensor configurations */ - union bmi2_sens_config_types cfg; -}; - -/*! @name Structure to define the feature configuration */ -struct bmi2_feature_config -{ - /*! Defines the type of sensor */ - uint8_t type; - - /*! Page to where the feature is mapped */ - uint8_t page; - - /*! Address of the feature */ - uint8_t start_addr; -}; - -/*! @name Structure to define the feature interrupt configurations */ -struct bmi2_map_int -{ - /*! Defines the type of sensor */ - uint8_t type; - - /*! Defines the feature interrupt */ - uint8_t sens_map_int; -}; - -/*! @name Structure to define BMI2 sensor configurations */ -struct bmi2_dev -{ - /*! Chip id of BMI2 */ - uint8_t chip_id; - - /*! The interface pointer is used to enable the user - * to link their interface descriptors for reference during the - * implementation of the read and write interfaces to the - * hardware. - */ - void *intf_ptr; - - /*! To store warnings */ - uint8_t info; - - /*! Type of Interface */ - enum bmi2_intf intf; - - /*! To store interface pointer error */ - BMI2_INTF_RETURN_TYPE intf_rslt; - - /*! For switching from I2C to SPI */ - uint8_t dummy_byte; - - /*! Resolution for FOC */ - uint8_t resolution; - - /*! User set read/write length */ - uint16_t read_write_len; - - /*! Pointer to the configuration data buffer address */ - const uint8_t *config_file_ptr; - - /*! To define maximum page number */ - uint8_t page_max; - - /*! To define maximum number of input sensors/features */ - uint8_t input_sens; - - /*! To define maximum number of output sensors/features */ - uint8_t out_sens; - - /*! Indicate manual enable for auxiliary communication */ - uint8_t aux_man_en; - - /*! Defines manual read burst length for auxiliary communication */ - uint8_t aux_man_rd_burst_len; - - /*! Array of feature input configuration structure */ - const struct bmi2_feature_config *feat_config; - - /*! Array of feature output configuration structure */ - const struct bmi2_feature_config *feat_output; - - /*! Structure to maintain a copy of the re-mapped axis */ - struct bmi2_axes_remap remap; - - /*! Flag to hold enable status of sensors */ - uint64_t sens_en_stat; - - /*! Read function pointer */ - bmi2_read_fptr_t read; - - /*! Write function pointer */ - bmi2_write_fptr_t write; - - /*! Delay function pointer */ - bmi2_delay_fptr_t delay_us; - - /*! To store the gyroscope cross sensitivity value */ - int16_t gyr_cross_sens_zx; - - /* gyro enable status, used as a flag in CRT enabling and aborting */ - uint8_t gyro_en : 1; - - /* advance power saving mode status, used as a flag in CRT enabling and aborting */ - uint8_t aps_status; - - /* used as a flag to enable variant specific features like crt */ - uint16_t variant_feature; - - /* To store hold the size of config file */ - uint16_t config_size; - - /*! Function pointer to get wakeup configurations */ - bmi2_wake_up_fptr_t get_wakeup_config; - - /*! Function pointer to set wakeup configurations */ - bmi2_wake_up_fptr_t set_wakeup_config; - - /*! Function pointer to get tap configurations */ - bmi2_tap_fptr_t get_tap_config; - - /*! Function pointer to set tap configurations */ - bmi2_tap_fptr_t set_tap_config; - - /*! Array of feature interrupts configuration structure */ - struct bmi2_map_int *map_int; - - /*! To define maximum number of interrupts */ - uint8_t sens_int_map; -}; - -/*! @name Structure to enable an accel axis for foc */ -struct bmi2_accel_foc_g_value -{ - /*! '0' to disable x axis and '1' to enable x axis */ - uint8_t x; - - /*! '0' to disable y axis and '1' to enable y axis */ - uint8_t y; - - /*! '0' to disable z axis and '1' to enable z axis */ - uint8_t z; - - /*! '0' for positive input and '1' for negative input */ - uint8_t sign; -}; - -/*! @name Structure to configure activity recognition settings */ -struct bmi2_act_recg_sett -{ - /*! Activity recognition register 1 */ - uint8_t act_rec_1 : 1; - - /*! Activity recognition register 2 */ - uint16_t act_rec_2; - - /*! Activity recognition register 3 */ - uint16_t act_rec_3; - - /*! Activity recognition register 4 */ - uint8_t act_rec_4 : 4; - - /*! Activity recognition register 5 */ - uint8_t act_rec_5 : 4; -}; - -/*! @name Structure to configure activity recognition settings for bmi270hc */ -struct bmi2_hc_act_recg_sett -{ - /*! Static segment size for activity classification. */ - uint8_t segment_size; - - /*! Enable/Disable post processing of the activity detected */ - uint8_t pp_en; - - /*! Minimum threshold of the Gini's diversity index (GDI) */ - uint16_t min_gdi_thres; - - /*! Maximum threshold of the Gini's diversity index (GDI) */ - uint16_t max_gdi_thres; - - /*! Buffer size for post processing of the activity detected */ - uint16_t buf_size; - - /*! Minimum segments belonging to a certain activity type */ - uint16_t min_seg_conf; -}; - -#endif /* BMI2_DEFS_H_ */ diff --git a/src/main/target/BETAFPVF411/target.mk b/src/main/target/BETAFPVF411/target.mk index 4008921362..2eafab1ba8 100644 --- a/src/main/target/BETAFPVF411/target.mk +++ b/src/main/target/BETAFPVF411/target.mk @@ -4,7 +4,7 @@ FEATURES = VCP ONBOARDFLASH TARGET_SRC = \ drivers/accgyro/accgyro_mpu.c \ drivers/accgyro/accgyro_spi_mpu6000.c \ - drivers/accgyro/bmi270_maximum_fifo.c \ + drivers/accgyro/accgyro_bmi270_maximum_fifo.c \ drivers/accgyro/accgyro_spi_bmi270.c \ drivers/barometer/barometer_bmp085.c \ drivers/barometer/barometer_bmp280.c \ diff --git a/src/main/target/IFLIGHT_F745_AIO_V2/target.mk b/src/main/target/IFLIGHT_F745_AIO_V2/target.mk index deb69abcb4..908df66294 100644 --- a/src/main/target/IFLIGHT_F745_AIO_V2/target.mk +++ b/src/main/target/IFLIGHT_F745_AIO_V2/target.mk @@ -2,7 +2,7 @@ F7X5XG_TARGETS += $(TARGET) FEATURES += VCP ONBOARDFLASH TARGET_SRC = \ - drivers/accgyro/bmi270_maximum_fifo.c \ + drivers/accgyro/accgyro_bmi270_maximum_fifo.c \ drivers/accgyro/accgyro_spi_bmi270.c \ drivers/barometer/barometer_ms5611.c \ drivers/barometer/barometer_bmp280.c \ diff --git a/src/main/target/TMTR_TMOTORF7/target.mk b/src/main/target/TMTR_TMOTORF7/target.mk index faa81c689f..31b5f262e1 100644 --- a/src/main/target/TMTR_TMOTORF7/target.mk +++ b/src/main/target/TMTR_TMOTORF7/target.mk @@ -1,7 +1,7 @@ F7X2RE_TARGETS += $(TARGET) FEATURES = VCP ONBOARDFLASH TARGET_SRC = \ - drivers/accgyro/bmi270_maximum_fifo.c \ + drivers/accgyro/accgyro_bmi270_maximum_fifo.c \ drivers/accgyro/accgyro_spi_bmi270.c \ drivers/accgyro/accgyro_mpu6500.c \ drivers/accgyro/accgyro_spi_mpu6000.c \ From b3895001018f45ae7f895f830d0cab51a2b81283 Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Mon, 15 May 2023 14:16:47 -0500 Subject: [PATCH 14/36] bmi270 - switch/case - samplingTime --- src/main/fc/config.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 5ccab8f8b5..37ca42323d 100644 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -434,6 +434,7 @@ void validateAndFixGyroConfig(void) { samplingTime = 1.0f / 9000.0f; break; case BMI_160_SPI: + case BMI_270_SPI: samplingTime = 0.0003125f; break; default: From 4a34f0de6a783ec24d74c56fa9c7252f4bbc94b7 Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Mon, 15 May 2023 15:45:51 -0500 Subject: [PATCH 15/36] bmi270 targets - really not sure about this CS_PIN stuff --- src/main/target/BETAFPVF411/target.h | 1 + src/main/target/TMTR_TMOTORF7/target.h | 1 + 2 files changed, 2 insertions(+) diff --git a/src/main/target/BETAFPVF411/target.h b/src/main/target/BETAFPVF411/target.h index 2a2062848e..ddd244caef 100644 --- a/src/main/target/BETAFPVF411/target.h +++ b/src/main/target/BETAFPVF411/target.h @@ -36,6 +36,7 @@ #define USE_SPI_GYRO #define USE_ACCGYRO_BMI270 +#define BMI270_CS_PIN PA4 #define BMI270_SPI_INSTANCE SPI1 #define ACC_BMI270_ALIGN CW90_DEG #define GYRO_BMI270_ALIGN CW90_DEG diff --git a/src/main/target/TMTR_TMOTORF7/target.h b/src/main/target/TMTR_TMOTORF7/target.h index 5e8a58685d..9dbeb0d6ac 100644 --- a/src/main/target/TMTR_TMOTORF7/target.h +++ b/src/main/target/TMTR_TMOTORF7/target.h @@ -52,6 +52,7 @@ #define USE_SPI_GYRO #define USE_ACCGYRO_BMI270 +#define BMI270_CS_PIN PA4 #define BMI270_SPI_INSTANCE SPI1 #define ACC_BMI270_ALIGN CW0_DEG #define GYRO_BMI270_ALIGN CW0_DEG From ffbef9ce1ee032e095b383e82aa97a7e05523d3d Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Wed, 17 May 2023 08:26:55 -0500 Subject: [PATCH 16/36] BMI270 - FOXEERF745_AIO (V2) --- .../target/FOXEERF745_AIO/FOXEERF745_AIO_V2.mk | 0 src/main/target/FOXEERF745_AIO/target.h | 15 +++++++++++++++ src/main/target/FOXEERF745_AIO/target.mk | 6 ++++-- 3 files changed, 19 insertions(+), 2 deletions(-) create mode 100644 src/main/target/FOXEERF745_AIO/FOXEERF745_AIO_V2.mk diff --git a/src/main/target/FOXEERF745_AIO/FOXEERF745_AIO_V2.mk b/src/main/target/FOXEERF745_AIO/FOXEERF745_AIO_V2.mk new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/main/target/FOXEERF745_AIO/target.h b/src/main/target/FOXEERF745_AIO/target.h index bd56acc8ef..6994d23949 100644 --- a/src/main/target/FOXEERF745_AIO/target.h +++ b/src/main/target/FOXEERF745_AIO/target.h @@ -21,7 +21,12 @@ #pragma once #define TARGET_BOARD_IDENTIFIER "FOXE" + + #if defined(FOXEERF745_AIO_V2) + #define USBD_PRODUCT_STRING "FOXEERF745_AIO_V2" + #else #define USBD_PRODUCT_STRING "FOXEERF745_AIO" + #endif #define LED0_PIN PC13 @@ -55,6 +60,16 @@ //#define MAG_HMC5883_ALIGN CW270_DEG_FLIP //#define MAG_ALIGN CW180_DEG //not sure if this command will work or if should be more specific to mag + #if defined(FOXEERF745_AIO_V2) + //BMI270 + #define USE_SPI_GYRO + #define USE_ACCGYRO_BMI270 + #define BMI270_CS_PIN PA15 + #define BMI270_SPI_INSTANCE SPI3 + #define ACC_BMI270_ALIGN CW180_DEG + #define GYRO_BMI270_ALIGN CW180_DEG + #endif + #define USE_BARO #define USE_BARO_BMP280 #define BARO_I2C_INSTANCE (I2CDEV_1) diff --git a/src/main/target/FOXEERF745_AIO/target.mk b/src/main/target/FOXEERF745_AIO/target.mk index e4faff73d9..348e36c404 100644 --- a/src/main/target/FOXEERF745_AIO/target.mk +++ b/src/main/target/FOXEERF745_AIO/target.mk @@ -2,11 +2,13 @@ F7X5XG_TARGETS += $(TARGET) FEATURES += VCP ONBOARDFLASH TARGET_SRC = \ + drivers/accgyro/accgyro_bmi270_maximum_fifo.c \ + drivers/accgyro/accgyro_spi_bmi270.c \ drivers/accgyro/accgyro_spi_mpu6000.c \ - drivers/accgyro/accgyro_mpu.c \ + drivers/accgyro/accgyro_mpu.c \ drivers/barometer/barometer_bmp280.c \ drivers/compass/compass_hmc5883l.c \ drivers/compass/compass_qmc5883l.c \ - drivers/compass/compass_lis3mdl.c \ + drivers/compass/compass_lis3mdl.c \ drivers/light_ws2811strip.c \ drivers/max7456.c From 44e6c32f4203fc325ce7844c1311298af092d9b6 Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Thu, 18 May 2023 11:43:59 -0500 Subject: [PATCH 17/36] BMI270 - NBD_INFINITYAIOV2PRO --- src/main/target/NBD_INFINITYAIOV2PRO/target.c | 39 +++++ src/main/target/NBD_INFINITYAIOV2PRO/target.h | 147 ++++++++++++++++++ .../target/NBD_INFINITYAIOV2PRO/target.mk | 15 ++ 3 files changed, 201 insertions(+) create mode 100644 src/main/target/NBD_INFINITYAIOV2PRO/target.c create mode 100644 src/main/target/NBD_INFINITYAIOV2PRO/target.h create mode 100644 src/main/target/NBD_INFINITYAIOV2PRO/target.mk diff --git a/src/main/target/NBD_INFINITYAIOV2PRO/target.c b/src/main/target/NBD_INFINITYAIOV2PRO/target.c new file mode 100644 index 0000000000..abad368d32 --- /dev/null +++ b/src/main/target/NBD_INFINITYAIOV2PRO/target.c @@ -0,0 +1,39 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include + +#include "platform.h" +#include "drivers/io.h" + +#include "drivers/dma.h" +#include "drivers/timer.h" +#include "drivers/timer_def.h" + +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + +DEF_TIM(TIM1, CH2, PA9, TIM_USE_LED, 0, 0 ), // LED_STRIP, + +DEF_TIM(TIM3, CH4, PC9, TIM_USE_MOTOR, 0, 0 ), // M1 +DEF_TIM(TIM3, CH3, PC8, TIM_USE_MOTOR, 0, 0 ), // M2 +DEF_TIM(TIM3, CH2, PC7, TIM_USE_MOTOR, 0, 0 ), // M3 +DEF_TIM(TIM3, CH1, PC6, TIM_USE_MOTOR, 0, 0 ), // M4 + +}; diff --git a/src/main/target/NBD_INFINITYAIOV2PRO/target.h b/src/main/target/NBD_INFINITYAIOV2PRO/target.h new file mode 100644 index 0000000000..d90cb83be9 --- /dev/null +++ b/src/main/target/NBD_INFINITYAIOV2PRO/target.h @@ -0,0 +1,147 @@ +/* +* This file is part of Cleanflight and Betaflight. +* +* Cleanflight and Betaflight are free software. You can redistribute +* this software and/or modify this software under the terms of the +* GNU General Public License as published by the Free Software +* Foundation, either version 3 of the License, or (at your option) +* any later version. +* +* Cleanflight and Betaflight are distributed in the hope that they +* will be useful, but WITHOUT ANY WARRANTY; without even the implied +* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. +* See the GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with this software. +* +* If not, see . +*/ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "NEBD" + +#define USBD_PRODUCT_STRING "NBD_INFINITYAIOV2PRO" + +#define LED0_PIN PC0 + +#define USE_BEEPER +#define BEEPER_PIN PD13 +#define BEEPER_INVERTED + +#define USE_SPI_GYRO +#define USE_ACCGYRO_BMI270 +#define BMI270_CS_PIN PE11 +#define BMI270_SPI_INSTANCE SPI4 +#define ACC_BMI270_ALIGN CW270_DEG +#define GYRO_BMI270_ALIGN CW270_DEG + +// dual gyro +// gyro 1 +#define GYRO_1_CS_PIN BMI270_CS_PIN +#define GYRO_1_SPI_INSTANCE BMI270_SPI_INSTANCE +#define GYRO_1_EXTI_PIN PB1 +#define GYRO_1_ALIGN GYRO_BMI270_ALIGN +#define ACC_1_ALIGN ACC_BMI270_ALIGN + +// gyro 2 +#define GYRO_2_CS_PIN PB12 +#define GYRO_2_SPI_INSTANCE SPI2 +#define GYRO_2_EXTI_PIN PD0 +#define GYRO_2_ALIGN GYRO_BMI270_ALIGN +#define ACC_2_ALIGN ACC_BMI270_ALIGN + +#define USE_VCP +#define USE_USB_DETECT + +#define USE_UART1 +#define UART1_RX_PIN PB7 + +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 + +#define USE_UART3 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 + +#define USE_UART5 +#define UART5_RX_PIN PD2 + +#define USE_UART7 +#define UART7_TX_PIN PE8 + +#define USE_UART8 +#define UART8_RX_PIN PE0 +#define UART8_TX_PIN PE1 + +#define SERIAL_PORT_COUNT 7 //VCP, USART1, USART2, USART3, UART5, USART7, UART8 + +#define USE_ESC_SENSOR + +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define USE_SPI_DEVICE_2 +#define USE_SPI_DEVICE_3 +#define USE_SPI_DEVICE_4 + +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PD6 + +#define SPI4_SCK_PIN PE12 +#define SPI4_MISO_PIN PE13 +#define SPI4_MOSI_PIN PE14 + +#define USE_MAX7456 +#define MAX7456_SPI_INSTANCE SPI3 +#define MAX7456_SPI_CS_PIN PA15 +#define MAX7456_SPI_CLK (SPI_CLOCK_STANDARD) // 10MHz +#define MAX7456_RESTORE_CLK (SPI_CLOCK_FAST) + +//#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT +#define USE_FLASHFS +#define USE_FLASH_W25Q128FV //official +#define FLASH_CS_PIN PB0 +#define FLASH_SPI_INSTANCE SPI1 + +#define USE_FLASH_W25Q //testing +#define USE_FLASH_W25M512 //testing +#define USE_FLASH_M25P16 //testing + +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C_DEVICE_1 (I2CDEV_1) +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +#define USE_ADC +#define VBAT_ADC_PIN PC1 +#define CURRENT_METER_ADC_PIN PC2 +#define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC +#define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ADC +#define CURRENT_METER_SCALE_DEFAULT 100 + +#define BEEPER_PWM_HZ 5400 + +#define USE_LED_STRIP +#define LED_STRIP_PIN PA9 + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff + +#define USABLE_TIMER_CHANNEL_COUNT 7 +#define USED_TIMERS ( TIM_N(1) | TIM_N(3) | TIM_N(4) ) diff --git a/src/main/target/NBD_INFINITYAIOV2PRO/target.mk b/src/main/target/NBD_INFINITYAIOV2PRO/target.mk new file mode 100644 index 0000000000..6c7c440f2d --- /dev/null +++ b/src/main/target/NBD_INFINITYAIOV2PRO/target.mk @@ -0,0 +1,15 @@ +F7X5XG_TARGETS += $(TARGET) +FEATURES += VCP ONBOARDFLASH + +TARGET_SRC = \ + drivers/accgyro/accgyro_bmi270_maximum_fifo.c \ + drivers/accgyro/accgyro_spi_bmi270.c \ + drivers/light_ws2811strip.c \ + drivers/max7456.c \ +drivers/compass/compass_fake.c \ +drivers/compass/compass_ak8963.c \ +drivers/compass/compass_ak8975.c \ +drivers/compass/compass_hmc5883l.c \ +drivers/compass/compass_lis3mdl.c \ +drivers/compass/compass_qmc5883l.c \ + From 1600ac9587f59e46400595d3c6cc40b41a57ec89 Mon Sep 17 00:00:00 2001 From: BeauBrewski <85623381+BeauBrewski@users.noreply.github.com> Date: Thu, 25 May 2023 10:32:55 -0500 Subject: [PATCH 18/36] added SkystarsF7 target updates --- src/main/target/SKYSTARSF7HD/target.h | 7 + src/main/target/SKYSTARSF7HD/target.mk | 2 + src/main/target/SKYSTARSF7HDPRO/config.c | 38 ++++ src/main/target/SKYSTARSF7HDPRO/target.c | 43 +++++ src/main/target/SKYSTARSF7HDPRO/target.h | 205 ++++++++++++++++++++++ src/main/target/SKYSTARSF7HDPRO/target.mk | 17 ++ 6 files changed, 312 insertions(+) create mode 100644 src/main/target/SKYSTARSF7HDPRO/config.c create mode 100644 src/main/target/SKYSTARSF7HDPRO/target.c create mode 100644 src/main/target/SKYSTARSF7HDPRO/target.h create mode 100644 src/main/target/SKYSTARSF7HDPRO/target.mk diff --git a/src/main/target/SKYSTARSF7HD/target.h b/src/main/target/SKYSTARSF7HD/target.h index bf68dc5fc4..31d1d6f3fa 100644 --- a/src/main/target/SKYSTARSF7HD/target.h +++ b/src/main/target/SKYSTARSF7HD/target.h @@ -53,6 +53,13 @@ #define USE_ACC +#define USE_SPI_GYRO +#define USE_ACCGYRO_BMI270 +#define BMI270_CS_PIN PA4 +#define BMI270_SPI_INSTANCE SPI1 +#define ACC_BMI270_ALIGN CW180_DEG +#define GYRO_BMI270_ALIGN CW180_DEG + #define MPU6000_CS_PIN PA4 #define MPU6000_SPI_INSTANCE SPI1 diff --git a/src/main/target/SKYSTARSF7HD/target.mk b/src/main/target/SKYSTARSF7HD/target.mk index 1b54266132..a9c5cf138f 100644 --- a/src/main/target/SKYSTARSF7HD/target.mk +++ b/src/main/target/SKYSTARSF7HD/target.mk @@ -2,6 +2,8 @@ F7X2RE_TARGETS += $(TARGET) FEATURES += VCP ONBOARDFLASH TARGET_SRC = \ + drivers/accgyro/accgyro_bmi270_maximum_fifo.c \ + drivers/accgyro/accgyro_spi_bmi270.c \ drivers/accgyro/accgyro_mpu.c \ drivers/accgyro/accgyro_spi_mpu6000.c \ drivers/barometer/barometer_bmp085.c \ diff --git a/src/main/target/SKYSTARSF7HDPRO/config.c b/src/main/target/SKYSTARSF7HDPRO/config.c new file mode 100644 index 0000000000..55d138b578 --- /dev/null +++ b/src/main/target/SKYSTARSF7HDPRO/config.c @@ -0,0 +1,38 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include + +#include "platform.h" + +#define USE_TARGET_CONFIG + +#include "io/serial.h" +#include "pg/pinio.h" +#include "pg/piniobox.h" +#include "target.h" + + +void targetConfiguration(void) { + + pinioBoxConfigMutable()->permanentId[0] = 40; + pinioConfigMutable()->config[0] = 129; + +} diff --git a/src/main/target/SKYSTARSF7HDPRO/target.c b/src/main/target/SKYSTARSF7HDPRO/target.c new file mode 100644 index 0000000000..57c3a5a336 --- /dev/null +++ b/src/main/target/SKYSTARSF7HDPRO/target.c @@ -0,0 +1,43 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include + +#include "platform.h" +#include "drivers/io.h" + +#include "drivers/dma.h" +#include "drivers/timer.h" +#include "drivers/timer_def.h" + +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + + DEF_TIM(TIM3, CH1, PB4, TIM_USE_PPM, 0, 0 ), // PPM IN + + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MOTOR, 0, 0 ), // S1_OUT – UP2-1 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MOTOR, 0, 0 ), // S2_OUT – UP2-1 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_MOTOR, 0, 0 ), // S3_OUT – UP2-5 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MOTOR, 0, 0 ), // S4_OUT – UP2-5 + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_ANY, 0, 0 ), // FC CAM – DMA1_ST7 + + DEF_TIM(TIM2, CH2, PB3, TIM_USE_LED, 0, 0 ), // LED_STRIP – DMA1_ST6 + +}; diff --git a/src/main/target/SKYSTARSF7HDPRO/target.h b/src/main/target/SKYSTARSF7HDPRO/target.h new file mode 100644 index 0000000000..2c3d169a21 --- /dev/null +++ b/src/main/target/SKYSTARSF7HDPRO/target.h @@ -0,0 +1,205 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#pragma once + +#define USE_TARGET_CONFIG + +#define TARGET_BOARD_IDENTIFIER "SSHP" +#define USBD_PRODUCT_STRING "SKST7HDPRO" + +// ******* LEDs and BEEPER ******** + +#define LED0_PIN PC15 +#define LED1_PIN PC14 + +#define USE_BEEPER +#define BEEPER_PIN PB2 +#define BEEPER_INVERTED + +//#define ENABLE_DSHOT_DMAR true + +#define USE_PINIO +#define PINIO1_PIN PA14 // Bluetooth mode control, PB0 is connected to the 36 pin (P2.0) of the Bluetooth chip. Replace PB0 with the pin for your flight control and 36-pin connection + +#define USE_CAMERA_CONTROL +#define CAMERA_CONTROL_PIN PA8 // define dedicated camera osd pin + + +// ******* GYRO and ACC ******** + +#define USE_DUAL_GYRO +#define USE_EXTI +#define GYRO_1_EXTI_PIN PC4 +#define GYRO_2_EXTI_PIN PC0 +#define MPU_INT_EXTI + +#define GYRO_1_CS_PIN PA4 +#define GYRO_1_SPI_INSTANCE SPI1 +#define GYRO_2_CS_PIN PC13 +#define GYRO_2_SPI_INSTANCE SPI1 + +#define USE_GYRO +#define USE_GYRO_SPI_MPU6000 +#define USE_GYRO_SPI_MPU6500 +#define USE_SPI_GYRO +#define USE_ACCGYRO_BMI270 + + +#define USE_ACC +#define USE_ACC_SPI_MPU6000 +#define USE_ACC_SPI_MPU6500 + +#define GYRO_1_ALIGN CW90_DEG_FLIP +#define ACC_1_ALIGN CW90_DEG_FLIP + +#define GYRO_2_ALIGN CW0_DEG +#define ACC_2_ALIGN CW0_DEG + +#define USE_MPU_DATA_READY_SIGNAL +#define ENSURE_MPU_DATA_READY_IS_LOW +#define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_1 + +// *************** Baro ************************ + +#define USE_SPI + +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_BARO +#define USE_BARO_SPI_BMP280 +#define DEFAULT_BARO_SPI_BMP280 +#define BMP280_SPI_INSTANCE SPI2 +#define BMP280_CS_PIN PB1 + +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + + +//*********** Magnetometer / Compass ************* +#define USE_MAG +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define MAG_I2C_INSTANCE (I2CDEV_1) + +// ******* SERIAL ******** + +#define USE_VCP + +#define USE_UART1 +#define USE_UART2 +#define USE_UART3 +#define USE_UART4 +#define USE_UART5 +#define USE_UART6 + +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 + +#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 + +#define UART4_TX_PIN PA0 +#define UART4_RX_PIN PA1 + +#define UART5_TX_PIN PC12 +#define UART5_RX_PIN PD2 + +#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 + +#define USE_SOFTSERIAL1 +#define USE_SOFTSERIAL2 + +#define SERIAL_PORT_COUNT 9 //VCP, UART1-UART6 , 2 x Soft Serial + +// ******* SPI ******** + +#define USE_SPI + +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + + +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PB5 + +// ******* ADC ******** + +#define USE_ADC +#define ADC_INSTANCE ADC2 +#define ADC2_DMA_STREAM DMA2_Stream3 + +#define VBAT_ADC_PIN PC1 +#define RSSI_ADC_PIN PC2 +#define CURRENT_METER_ADC_PIN PC3 + +// ******* OSD ******** + +#define USE_MAX7456 +#define MAX7456_SPI_INSTANCE SPI2 +#define MAX7456_SPI_CS_PIN PB12 +#define MAX7456_SPI_CLK ( SPI_CLOCK_STANDARD ) +#define MAX7456_RESTORE_CLK ( SPI_CLOCK_FAST ) + +//******* FLASH ******** + +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define FLASH_CS_PIN PA15 +#define FLASH_SPI_INSTANCE SPI3 + +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +// ******* FEATURES ******** + +#define USE_OSD + +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define SERIALRX_UART SERIAL_PORT_USART1 +#define SERIALRX_PROVIDER SERIALRX_SBUS + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_SOFTSERIAL) +#define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC +#define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ADC +#define CURRENT_METER_SCALE_DEFAULT 290 + + +#define USE_ESCSERIAL + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff + +#define USABLE_TIMER_CHANNEL_COUNT 7 +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3)| TIM_N(4) | TIM_N(8) ) diff --git a/src/main/target/SKYSTARSF7HDPRO/target.mk b/src/main/target/SKYSTARSF7HDPRO/target.mk new file mode 100644 index 0000000000..108b015201 --- /dev/null +++ b/src/main/target/SKYSTARSF7HDPRO/target.mk @@ -0,0 +1,17 @@ +F7X2RE_TARGETS += $(TARGET) +FEATURES += VCP ONBOARDFLASH + +TARGET_SRC = \ + drivers/accgyro/accgyro_bmi270_maximum_fifo.c \ + drivers/accgyro/accgyro_spi_bmi270.c \ + drivers/accgyro/accgyro_spi_mpu6000.c \ + drivers/accgyro/accgyro_mpu.c \ + drivers/accgyro/accgyro_mpu6500.c \ + drivers/accgyro/accgyro_spi_mpu6500.c \ + drivers/barometer/barometer_bmp280.c \ + drivers/barometer/barometer_ms5611.c \ + drivers/barometer/barometer_bmp085.c \ + drivers/compass/compass_hmc5883l.c \ + drivers/compass/compass_qmc5883l.c \ + drivers/light_ws2811strip.c \ + drivers/max7456.c From 2740f8c1ab5c1f352baf3e6415ddf0e6c629a3a8 Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Tue, 20 Jun 2023 14:39:27 -0500 Subject: [PATCH 19/36] fix FLYWOOF411_5IN1_AIO align --- src/main/drivers/bus_spi.c | 25 ------------------- src/main/target/FLYWOOF411_5IN1_AIO/target.h | 4 +-- src/main/target/FLYWOOF411_5IN1_AIO/target.mk | 18 +++++++------ 3 files changed, 12 insertions(+), 35 deletions(-) diff --git a/src/main/drivers/bus_spi.c b/src/main/drivers/bus_spi.c index bccc221f56..d42f0e6156 100644 --- a/src/main/drivers/bus_spi.c +++ b/src/main/drivers/bus_spi.c @@ -157,31 +157,6 @@ void spiResetErrorCounter(SPI_TypeDef *instance) { } } -uint16_t spiCalculateDivider(uint32_t freq) -{ -#if defined(STM32F4) || defined(STM32G4) || defined(STM32F7) - uint32_t spiClk = SystemCoreClock / 2; -#elif defined(STM32H7) - uint32_t spiClk = 100000000; -#elif defined(AT32F4) - if(freq > 36000000){ - freq = 36000000; - } - - uint32_t spiClk = system_core_clock / 2; -#else -#error "Base SPI clock not defined for this architecture" -#endif - - uint16_t divisor = 2; - - spiClk >>= 1; - - for (; (spiClk > freq) && (divisor < 256); divisor <<= 1, spiClk >>= 1); - - return divisor; -} - FAST_CODE bool spiBusWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data) { #ifdef USE_DMA_SPI_DEVICE if(USE_DMA_SPI_DEVICE == bus->busdev_u.spi.instance) { diff --git a/src/main/target/FLYWOOF411_5IN1_AIO/target.h b/src/main/target/FLYWOOF411_5IN1_AIO/target.h index fc3d6d1f90..ba562fa63e 100644 --- a/src/main/target/FLYWOOF411_5IN1_AIO/target.h +++ b/src/main/target/FLYWOOF411_5IN1_AIO/target.h @@ -66,8 +66,8 @@ #define ICM42688P_SPI_INSTANCE SPI1 #define ICM42688P_CS_PIN PB2 -#define ACC_ICM426XX_ALIGN CW90_DEG -#define GYRO_ICM426XX_ALIGN CW90_DEG +#define ACC_ICM42688P_ALIGN CW90_DEG +#define GYRO_ICM42688P_ALIGN CW90_DEG // *************** Baro ************************** #define USE_I2C diff --git a/src/main/target/FLYWOOF411_5IN1_AIO/target.mk b/src/main/target/FLYWOOF411_5IN1_AIO/target.mk index 19d3a639b7..a69689a1d8 100644 --- a/src/main/target/FLYWOOF411_5IN1_AIO/target.mk +++ b/src/main/target/FLYWOOF411_5IN1_AIO/target.mk @@ -3,14 +3,16 @@ F411_TARGETS += $(TARGET) FEATURES += VCP TARGET_SRC = \ - drivers/accgyro/accgyro_mpu.c \ - drivers/accgyro/accgyro_spi_mpu6000.c \ - drivers/accgyro/accgyro_spi_icm20689.c \ - drivers/accgyro/accgyro_spi_icm426xx.c \ - drivers/barometer/barometer_bmp085.c \ - drivers/barometer/barometer_bmp280.c \ - drivers/barometer/barometer_ms5611.c \ - drivers/compass/compass_hmc5883l.c \ + drivers/accgyro/accgyro_mpu.c \ + drivers/accgyro/accgyro_bmi270_maximum_fifo.c \ + drivers/accgyro/accgyro_spi_bmi270.c \ + drivers/accgyro/accgyro_spi_icm20689.c \ + drivers/accgyro/accgyro_spi_icm426xx.c \ + drivers/accgyro/accgyro_spi_mpu6000.c \ + drivers/barometer/barometer_bmp085.c \ + drivers/barometer/barometer_bmp280.c \ + drivers/barometer/barometer_ms5611.c \ + drivers/compass/compass_hmc5883l.c \ drivers/compass/compass_qmc5883l.c \ drivers/max7456.c \ drivers/rx/rx_cc2500.c \ From 0fd54a028a049cbd8ea7fd6d5c44088bd150883b Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Tue, 6 Jun 2023 15:41:36 -0500 Subject: [PATCH 20/36] BMI270 - gyro/pid denom default 1 --- src/main/flight/pid.c | 2 +- src/main/sensors/gyro.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 19b51d18f7..fac0213e8f 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -86,7 +86,7 @@ PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_PID_CONFIG, 2); #ifdef STM32F10X #define PID_PROCESS_DENOM_DEFAULT 1 -#elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P) +#elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P) || defined(USE_ACCGYRO_BMI160) || defined(USE_ACCGYRO_BMI270) #define PID_PROCESS_DENOM_DEFAULT 1 #else #define PID_PROCESS_DENOM_DEFAULT 2 diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 26e5b2fd0d..aa7eceaa2d 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -210,7 +210,7 @@ static void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int typ #ifdef STM32F10X #define GYRO_SYNC_DENOM_DEFAULT 8 #elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) \ - || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P) + || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P) || defined(USE_ACCGYRO_BMI160) || defined(USE_ACCGYRO_BMI270) #define GYRO_SYNC_DENOM_DEFAULT 1 #else #define GYRO_SYNC_DENOM_DEFAULT 3 From 97f660eb3d219803b3c4700bf338e6ddd0054f2b Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Tue, 13 Jun 2023 09:32:04 -0500 Subject: [PATCH 21/36] BMI270 - GYRO_HARDWARE_LPF_EXPERIMENTAL 3 --- src/main/drivers/accgyro/accgyro.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/drivers/accgyro/accgyro.h b/src/main/drivers/accgyro/accgyro.h index b355a5be90..0bcf72738c 100644 --- a/src/main/drivers/accgyro/accgyro.h +++ b/src/main/drivers/accgyro/accgyro.h @@ -43,7 +43,7 @@ #define GYRO_HARDWARE_LPF_NORMAL 0 #define GYRO_HARDWARE_LPF_1KHZ_SAMPLE 2 -#if defined(USE_GYRO_SPI_ICM42688P) +#if defined(USE_GYRO_SPI_ICM42688P) || defined(USE_ACCGYRO_BMI270) #define GYRO_HARDWARE_LPF_EXPERIMENTAL 3 #else #define GYRO_HARDWARE_LPF_EXPERIMENTAL 1 From 6ea948ff0dee5aee07c56202f70456b02bb0e393 Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Wed, 21 Jun 2023 09:33:01 -0500 Subject: [PATCH 22/36] BMI270 - deduplicate spiCalculateDivider in bus_spi.h --- src/main/drivers/bus_spi.h | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/main/drivers/bus_spi.h b/src/main/drivers/bus_spi.h index 31b37db145..1fa4a0dab7 100644 --- a/src/main/drivers/bus_spi.h +++ b/src/main/drivers/bus_spi.h @@ -119,9 +119,6 @@ bool spiBusReadRegisterBuffer(const busDevice_t *bus, uint8_t reg, uint8_t *data uint8_t spiBusReadRegister(const busDevice_t *bus, uint8_t reg); void spiBusSetInstance(busDevice_t *bus, SPI_TypeDef *instance); -// Determine the divisor to use for a given bus frequency -uint16_t spiCalculateDivider(uint32_t freq); - struct spiPinConfig_s; void spiPinConfigure(const struct spiPinConfig_s *pConfig); From f28be02f5f67094e2640718005a2be39b64c7576 Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Wed, 21 Jun 2023 09:50:44 -0500 Subject: [PATCH 23/36] BMI270 - remove OWNER_GYRO_EXTI from resource.h to fix breakage --- src/main/drivers/resource.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/drivers/resource.h b/src/main/drivers/resource.h index 14eaf6138e..8b8daaf6a8 100644 --- a/src/main/drivers/resource.h +++ b/src/main/drivers/resource.h @@ -53,7 +53,6 @@ typedef enum { OWNER_OSD_CS, OWNER_RX_SPI_CS, OWNER_SPI_CS, - OWNER_GYRO_EXTI, OWNER_MPU_EXTI, OWNER_BARO_EXTI, OWNER_COMPASS_EXTI, From 11b769b88bdd50091cd0790ff115e2384e854435 Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Thu, 22 Jun 2023 14:27:46 -0500 Subject: [PATCH 24/36] BMI270 - gyro_hardware_lpf options (USE_GYRO_DLPF_EXPERIMENTAL) --- src/main/interface/settings.c | 4 ++-- src/main/target/common_fc_pre.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 83b2b1485e..599b075a1c 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -252,10 +252,10 @@ static const char * const lookupTableRxSpi[] = { static const char * const lookupTableGyroHardwareLpf[] = { "NORMAL", -#if defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P) +#if defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P) || defined(USE_ACCGYRO_BMI270) "OPTION1", "OPTION2", -#if defined(USE_GYRO_SPI_ICM42688P) && defined(USE_GYRO_DLPF_EXPERIMENTAL) // will need bmi270 logic as well +#if ( defined(USE_GYRO_SPI_ICM42688P) || defined(USE_ACCGYRO_BMI270) ) && defined(USE_GYRO_DLPF_EXPERIMENTAL) // icm42688p & bmi270 experimental "EXPERIMENTAL", #endif #else diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h index 426ec9b767..4d378c10ac 100644 --- a/src/main/target/common_fc_pre.h +++ b/src/main/target/common_fc_pre.h @@ -229,5 +229,5 @@ #define USE_CMS_GPS_RESCUE_MENU #endif -// ICM42688P define +// ICM42688P & BMI270 experimental define #define USE_GYRO_DLPF_EXPERIMENTAL From 4db3460daff349b26733030329b3997e2ac930be Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Tue, 27 Jun 2023 13:44:47 -0500 Subject: [PATCH 25/36] BMI270 - replace OWNER_GYRO_EXTI with OWNER_MPU_EXTI --- src/main/drivers/accgyro/accgyro_spi_bmi270.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/drivers/accgyro/accgyro_spi_bmi270.c b/src/main/drivers/accgyro/accgyro_spi_bmi270.c index 5bad645ef4..56fcebfb4c 100644 --- a/src/main/drivers/accgyro/accgyro_spi_bmi270.c +++ b/src/main/drivers/accgyro/accgyro_spi_bmi270.c @@ -277,7 +277,7 @@ static void bmi270IntExtiInit(gyroDev_t *gyro) IO_t mpuIntIO = IOGetByTag(gyro->mpuIntExtiTag); - IOInit(mpuIntIO, OWNER_GYRO_EXTI, 0); + IOInit(mpuIntIO, OWNER_MPU_EXTI, 0); EXTIHandlerInit(&gyro->exti, bmi270ExtiHandler); EXTIConfig(mpuIntIO, &gyro->exti, NVIC_PRIO_MPU_INT_EXTI, IOCFG_IN_FLOATING ); EXTIEnable(mpuIntIO, true); From f4ccac93d8ed1cdcb3283854fc156d3efb62987c Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Tue, 11 Jul 2023 15:36:26 -0500 Subject: [PATCH 26/36] BMI270 - fix NBD_INFINITYAIOV2PRO dual-gyro defines --- src/main/target/NBD_INFINITYAIOV2PRO/target.h | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/src/main/target/NBD_INFINITYAIOV2PRO/target.h b/src/main/target/NBD_INFINITYAIOV2PRO/target.h index d90cb83be9..fac5460376 100644 --- a/src/main/target/NBD_INFINITYAIOV2PRO/target.h +++ b/src/main/target/NBD_INFINITYAIOV2PRO/target.h @@ -38,19 +38,21 @@ #define GYRO_BMI270_ALIGN CW270_DEG // dual gyro +#define USE_DUAL_GYRO + // gyro 1 -#define GYRO_1_CS_PIN BMI270_CS_PIN -#define GYRO_1_SPI_INSTANCE BMI270_SPI_INSTANCE +#define GYRO_1_CS_PIN PE11 +#define GYRO_1_SPI_INSTANCE SPI4 #define GYRO_1_EXTI_PIN PB1 -#define GYRO_1_ALIGN GYRO_BMI270_ALIGN -#define ACC_1_ALIGN ACC_BMI270_ALIGN +#define GYRO_1_ALIGN CW270_DEG +#define ACC_1_ALIGN CW270_DEG // gyro 2 #define GYRO_2_CS_PIN PB12 #define GYRO_2_SPI_INSTANCE SPI2 #define GYRO_2_EXTI_PIN PD0 -#define GYRO_2_ALIGN GYRO_BMI270_ALIGN -#define ACC_2_ALIGN ACC_BMI270_ALIGN +#define GYRO_2_ALIGN CW270_DEG +#define ACC_2_ALIGN CW270_DEG #define USE_VCP #define USE_USB_DETECT From 98e549f4c79efc97e4b9366b8b3766c7e92307c9 Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Wed, 19 Jul 2023 08:20:06 -0500 Subject: [PATCH 27/36] fix IFLIGHT_F745_AIO_V2 .mk --- src/main/target/IFLIGHT_F745_AIO_V2/target.mk | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/target/IFLIGHT_F745_AIO_V2/target.mk b/src/main/target/IFLIGHT_F745_AIO_V2/target.mk index f964a5e50a..a2ee8ce99b 100644 --- a/src/main/target/IFLIGHT_F745_AIO_V2/target.mk +++ b/src/main/target/IFLIGHT_F745_AIO_V2/target.mk @@ -2,7 +2,6 @@ F7X5XG_TARGETS += $(TARGET) FEATURES += VCP ONBOARDFLASH TARGET_SRC = \ - drivers/accgyro/accgyro_bmi270_maximum_fifo.c \ drivers/accgyro/accgyro_spi_bmi270.c \ drivers/accgyro/accgyro_spi_mpu6000.c \ From 945016fa4150a94b3cf0f840ca95281ffe25808a Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Wed, 23 Aug 2023 09:52:23 -0500 Subject: [PATCH 28/36] BMI270 - add bmi gyros to accgyro_mpu.h ifdef --- src/main/drivers/accgyro/accgyro_mpu.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/drivers/accgyro/accgyro_mpu.h b/src/main/drivers/accgyro/accgyro_mpu.h index 10f15a7744..b85e6d4ca4 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.h +++ b/src/main/drivers/accgyro/accgyro_mpu.h @@ -29,7 +29,7 @@ //#define DEBUG_MPU_DATA_READY_INTERRUPT #if defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20649) \ - || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P) + || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P) || defined(USE_ACCGYRO_BMI160) || defined(USE_ACCGYRO_BMI270) #define GYRO_USES_SPI #endif From 206f7cd2d5815ced82c6293be7b31203a23ac0c7 Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Mon, 28 Aug 2023 14:09:14 -0500 Subject: [PATCH 29/36] BMI270 - fix detect fixes requirement for mpu6000 defines --- src/main/drivers/accgyro/accgyro_spi_bmi270.c | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/main/drivers/accgyro/accgyro_spi_bmi270.c b/src/main/drivers/accgyro/accgyro_spi_bmi270.c index 56fcebfb4c..db8a29f1ab 100644 --- a/src/main/drivers/accgyro/accgyro_spi_bmi270.c +++ b/src/main/drivers/accgyro/accgyro_spi_bmi270.c @@ -158,6 +158,12 @@ static void bmi270EnableSPI(const busDevice_t *bus) uint8_t bmi270Detect(const busDevice_t *bus) { +#ifndef USE_DUAL_GYRO + IOInit(bus->busdev_u.spi.csnPin, OWNER_MPU_CS, 0); + IOConfigGPIO(bus->busdev_u.spi.csnPin, SPI_IO_CS_CFG); + IOHi(bus->busdev_u.spi.csnPin); +#endif + spiSetDivisor(bus->busdev_u.spi.instance, spiCalculateDivider(BMI270_MAX_SPI_CLK_HZ)); bmi270EnableSPI(bus); From 95d5bcbed5fbdd21ba5181b8162bb423cf8b6302 Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Wed, 27 Sep 2023 09:53:08 -0500 Subject: [PATCH 30/36] BMI270 - ensure over --- src/main/drivers/accgyro/accgyro_bmi270_maximum_fifo.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/drivers/accgyro/accgyro_bmi270_maximum_fifo.c b/src/main/drivers/accgyro/accgyro_bmi270_maximum_fifo.c index 0305f68f1a..530726ad33 100644 --- a/src/main/drivers/accgyro/accgyro_bmi270_maximum_fifo.c +++ b/src/main/drivers/accgyro/accgyro_bmi270_maximum_fifo.c @@ -98,7 +98,7 @@ const struct bmi2_feature_config bmi270_maximum_fifo_feat_out[BMI270_MAXIMUM_FIF * @retval 0 -> Success * @retval < 0 -> Fail */ -static int8_t null_ptr_check(const struct bmi2_dev *dev); +static int8_t null_ptr_check(const struct bmi2_dev *bus); /***************************************************************************/ @@ -113,7 +113,7 @@ static int8_t null_ptr_check(const struct bmi2_dev *dev); * 4) Updates the feature offset parameters in the device structure. * 5) Updates the maximum number of pages, in the device structure. */ -int8_t bmi270_maximum_fifo_init(struct bmi2_dev *dev) +int8_t bmi270_maximum_fifo_init(struct bmi2_dev *bus) { /* Variable to define result */ int8_t rslt; @@ -197,7 +197,7 @@ int8_t bmi270_maximum_fifo_init(struct bmi2_dev *dev) * @brief This internal API is used to validate the device structure pointer for * null conditions. */ -static int8_t null_ptr_check(const struct bmi2_dev *dev) +static int8_t null_ptr_check(const struct bmi2_dev *bus) { /* Variable to define result */ int8_t rslt = BMI2_OK; From 21b0b52afa7a39e93c730b1d0e309b85d7bf0c02 Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Wed, 27 Sep 2023 14:37:34 -0500 Subject: [PATCH 31/36] BMI270 - move bmi270_maximum_fifo_config_file constant --- .../accgyro/accgyro_bmi270_maximum_fifo.c | 213 ------------------ src/main/drivers/accgyro/accgyro_spi_bmi270.c | 21 +- 2 files changed, 20 insertions(+), 214 deletions(-) delete mode 100644 src/main/drivers/accgyro/accgyro_bmi270_maximum_fifo.c diff --git a/src/main/drivers/accgyro/accgyro_bmi270_maximum_fifo.c b/src/main/drivers/accgyro/accgyro_bmi270_maximum_fifo.c deleted file mode 100644 index 530726ad33..0000000000 --- a/src/main/drivers/accgyro/accgyro_bmi270_maximum_fifo.c +++ /dev/null @@ -1,213 +0,0 @@ -/** -* Copyright (c) 2020 Bosch Sensortec GmbH. All rights reserved. -* -* BSD-3-Clause -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions are met: -* -* 1. Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* -* 2. Redistributions in binary form must reproduce the above copyright -* notice, this list of conditions and the following disclaimer in the -* documentation and/or other materials provided with the distribution. -* -* 3. Neither the name of the copyright holder nor the names of its -* contributors may be used to endorse or promote products derived from -* this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) -* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, -* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING -* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -* -* @file bmi270_maximum_fifo.c -* @date 2020-11-04 -* @version v2.63.1 -* -*/ - -/***************************************************************************/ - -/*! Header files - ****************************************************************************/ -// #include "bmi270_maximum_fifo.h" -#include "platform.h" - -/***************************************************************************/ - -/*! Global Variable - ****************************************************************************/ - -/*! @name Global array that stores the configuration file of BMI270 */ -const uint8_t bmi270_maximum_fifo_config_file[] = { - 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x1a, 0x00, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, - 0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0x90, 0x32, 0x21, 0x2e, 0x59, 0xf5, - 0x10, 0x30, 0x21, 0x2e, 0x6a, 0xf5, 0x1a, 0x24, 0x22, 0x00, 0x80, 0x2e, 0x3b, 0x00, 0xc8, 0x2e, 0x44, 0x47, 0x22, - 0x00, 0x37, 0x00, 0xa4, 0x00, 0xff, 0x0f, 0xd1, 0x00, 0x07, 0xad, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, - 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, - 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x11, 0x24, 0xfc, 0xf5, 0x80, 0x30, 0x40, 0x42, 0x50, 0x50, 0x00, 0x30, 0x12, 0x24, 0xeb, - 0x00, 0x03, 0x30, 0x00, 0x2e, 0xc1, 0x86, 0x5a, 0x0e, 0xfb, 0x2f, 0x21, 0x2e, 0xfc, 0xf5, 0x13, 0x24, 0x63, 0xf5, - 0xe0, 0x3c, 0x48, 0x00, 0x22, 0x30, 0xf7, 0x80, 0xc2, 0x42, 0xe1, 0x7f, 0x3a, 0x25, 0xfc, 0x86, 0xf0, 0x7f, 0x41, - 0x33, 0x98, 0x2e, 0xc2, 0xc4, 0xd6, 0x6f, 0xf1, 0x30, 0xf1, 0x08, 0xc4, 0x6f, 0x11, 0x24, 0xff, 0x03, 0x12, 0x24, - 0x00, 0xfc, 0x61, 0x09, 0xa2, 0x08, 0x36, 0xbe, 0x2a, 0xb9, 0x13, 0x24, 0x38, 0x00, 0x64, 0xbb, 0xd1, 0xbe, 0x94, - 0x0a, 0x71, 0x08, 0xd5, 0x42, 0x21, 0xbd, 0x91, 0xbc, 0xd2, 0x42, 0xc1, 0x42, 0x00, 0xb2, 0xfe, 0x82, 0x05, 0x2f, - 0x50, 0x30, 0x21, 0x2e, 0x21, 0xf2, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0xf0, 0x6f, 0x02, 0x30, 0x02, 0x42, 0x20, - 0x26, 0xe0, 0x6f, 0x02, 0x31, 0x03, 0x40, 0x9a, 0x0a, 0x02, 0x42, 0xf0, 0x37, 0x05, 0x2e, 0x5e, 0xf7, 0x10, 0x08, - 0x12, 0x24, 0x1e, 0xf2, 0x80, 0x42, 0x83, 0x84, 0xf1, 0x7f, 0x0a, 0x25, 0x13, 0x30, 0x83, 0x42, 0x3b, 0x82, 0xf0, - 0x6f, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0x12, 0x40, 0x52, 0x42, 0x00, 0x2e, 0x12, 0x40, 0x52, 0x42, 0x3e, 0x84, - 0x00, 0x40, 0x40, 0x42, 0x7e, 0x82, 0xe1, 0x7f, 0xf2, 0x7f, 0x98, 0x2e, 0x6a, 0xd6, 0x21, 0x30, 0x23, 0x2e, 0x61, - 0xf5, 0xeb, 0x2c, 0xe1, 0x6f -}; - -// The rest of this is not needed, avoid compiler errors due to pedantic settings -#if false - -/*! @name Global array that stores the feature input configuration of BMI270 */ -const struct bmi2_feature_config bmi270_maximum_fifo_feat_in[BMI270_MAXIMUM_FIFO_MAX_FEAT_IN] = { - -}; - -/*! @name Global array that stores the feature output configuration */ -const struct bmi2_feature_config bmi270_maximum_fifo_feat_out[BMI270_MAXIMUM_FIFO_MAX_FEAT_OUT] = { - -}; - -/******************************************************************************/ - -/*! Local Function Prototypes - ******************************************************************************/ - -/*! - * @brief This internal API is used to validate the device pointer for - * null conditions. - * - * @param[in] dev : Structure instance of bmi2_dev. - * - * @return Result of API execution status - * @retval 0 -> Success - * @retval < 0 -> Fail - */ -static int8_t null_ptr_check(const struct bmi2_dev *bus); - -/***************************************************************************/ - -/*! User Interface Definitions - ****************************************************************************/ - -/*! - * @brief This API: - * 1) updates the device structure with address of the configuration file. - * 2) Initializes BMI270 sensor. - * 3) Writes the configuration file. - * 4) Updates the feature offset parameters in the device structure. - * 5) Updates the maximum number of pages, in the device structure. - */ -int8_t bmi270_maximum_fifo_init(struct bmi2_dev *bus) -{ - /* Variable to define result */ - int8_t rslt; - - /* Null-pointer check */ - rslt = null_ptr_check(dev); - - if (rslt == BMI2_OK) - { - /* Assign chip id of BMI270 */ - dev->chip_id = BMI270_MAXIMUM_FIFO_CHIP_ID; - - dev->config_size = sizeof(bmi270_maximum_fifo_config_file); - - /* Enable the variant specific features if any */ - dev->variant_feature = BMI2_GYRO_CROSS_SENS_ENABLE | BMI2_MAXIMUM_FIFO_VARIANT; - - /* An extra dummy byte is read during SPI read */ - if (dev->intf == BMI2_SPI_INTF) - { - dev->dummy_byte = 1; - } - else - { - dev->dummy_byte = 0; - } - - /* If configuration file pointer is not assigned any address */ - if (!dev->config_file_ptr) - { - /* Give the address of the configuration file array to - * the device pointer - */ - dev->config_file_ptr = bmi270_maximum_fifo_config_file; - } - - /* Initialize BMI2 sensor */ - rslt = bmi2_sec_init(dev); - - if (rslt == BMI2_OK) - { - /* Assign the offsets of the feature input - * configuration to the device structure - */ - dev->feat_config = bmi270_maximum_fifo_feat_in; - - /* Assign the offsets of the feature output to - * the device structure - */ - dev->feat_output = bmi270_maximum_fifo_feat_out; - - /* Assign the maximum number of pages to the - * device structure - */ - dev->page_max = BMI270_MAXIMUM_FIFO_MAX_PAGE_NUM; - - /* Assign maximum number of input sensors/ - * features to device structure - */ - dev->input_sens = BMI270_MAXIMUM_FIFO_MAX_FEAT_IN; - - /* Assign maximum number of output sensors/ - * features to device structure - */ - dev->out_sens = BMI270_MAXIMUM_FIFO_MAX_FEAT_OUT; - - /* Get the gyroscope cross axis sensitivity */ - rslt = bmi2_get_gyro_cross_sense(dev); - } - } - - return rslt; -} - -/***************************************************************************/ - -/*! Local Function Definitions - ****************************************************************************/ - -/*! - * @brief This internal API is used to validate the device structure pointer for - * null conditions. - */ -static int8_t null_ptr_check(const struct bmi2_dev *bus) -{ - /* Variable to define result */ - int8_t rslt = BMI2_OK; - - if ((dev == NULL) || (dev->read == NULL) || (dev->write == NULL) || (dev->delay_us == NULL)) - { - /* Device structure pointer is not valid */ - rslt = BMI2_E_NULL_PTR; - } - - return rslt; -} -#endif diff --git a/src/main/drivers/accgyro/accgyro_spi_bmi270.c b/src/main/drivers/accgyro/accgyro_spi_bmi270.c index db8a29f1ab..b7e4749770 100644 --- a/src/main/drivers/accgyro/accgyro_spi_bmi270.c +++ b/src/main/drivers/accgyro/accgyro_spi_bmi270.c @@ -43,7 +43,26 @@ #define BMI270_CONFIG_SIZE 328 // Declaration for the device config (microcode) that must be uploaded to the sensor -extern const uint8_t bmi270_maximum_fifo_config_file[BMI270_CONFIG_SIZE]; +const uint8_t bmi270_maximum_fifo_config_file[] = { + 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x1a, 0x00, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, + 0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0x90, 0x32, 0x21, 0x2e, 0x59, 0xf5, + 0x10, 0x30, 0x21, 0x2e, 0x6a, 0xf5, 0x1a, 0x24, 0x22, 0x00, 0x80, 0x2e, 0x3b, 0x00, 0xc8, 0x2e, 0x44, 0x47, 0x22, + 0x00, 0x37, 0x00, 0xa4, 0x00, 0xff, 0x0f, 0xd1, 0x00, 0x07, 0xad, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x11, 0x24, 0xfc, 0xf5, 0x80, 0x30, 0x40, 0x42, 0x50, 0x50, 0x00, 0x30, 0x12, 0x24, 0xeb, + 0x00, 0x03, 0x30, 0x00, 0x2e, 0xc1, 0x86, 0x5a, 0x0e, 0xfb, 0x2f, 0x21, 0x2e, 0xfc, 0xf5, 0x13, 0x24, 0x63, 0xf5, + 0xe0, 0x3c, 0x48, 0x00, 0x22, 0x30, 0xf7, 0x80, 0xc2, 0x42, 0xe1, 0x7f, 0x3a, 0x25, 0xfc, 0x86, 0xf0, 0x7f, 0x41, + 0x33, 0x98, 0x2e, 0xc2, 0xc4, 0xd6, 0x6f, 0xf1, 0x30, 0xf1, 0x08, 0xc4, 0x6f, 0x11, 0x24, 0xff, 0x03, 0x12, 0x24, + 0x00, 0xfc, 0x61, 0x09, 0xa2, 0x08, 0x36, 0xbe, 0x2a, 0xb9, 0x13, 0x24, 0x38, 0x00, 0x64, 0xbb, 0xd1, 0xbe, 0x94, + 0x0a, 0x71, 0x08, 0xd5, 0x42, 0x21, 0xbd, 0x91, 0xbc, 0xd2, 0x42, 0xc1, 0x42, 0x00, 0xb2, 0xfe, 0x82, 0x05, 0x2f, + 0x50, 0x30, 0x21, 0x2e, 0x21, 0xf2, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0xf0, 0x6f, 0x02, 0x30, 0x02, 0x42, 0x20, + 0x26, 0xe0, 0x6f, 0x02, 0x31, 0x03, 0x40, 0x9a, 0x0a, 0x02, 0x42, 0xf0, 0x37, 0x05, 0x2e, 0x5e, 0xf7, 0x10, 0x08, + 0x12, 0x24, 0x1e, 0xf2, 0x80, 0x42, 0x83, 0x84, 0xf1, 0x7f, 0x0a, 0x25, 0x13, 0x30, 0x83, 0x42, 0x3b, 0x82, 0xf0, + 0x6f, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0x12, 0x40, 0x52, 0x42, 0x00, 0x2e, 0x12, 0x40, 0x52, 0x42, 0x3e, 0x84, + 0x00, 0x40, 0x40, 0x42, 0x7e, 0x82, 0xe1, 0x7f, 0xf2, 0x7f, 0x98, 0x2e, 0x6a, 0xd6, 0x21, 0x30, 0x23, 0x2e, 0x61, + 0xf5, 0xeb, 0x2c, 0xe1, 0x6f +}; #define BMI270_CHIP_ID 0x24 From aaf935d9ceab739146aec5f1887b9e371c9b5594 Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Wed, 27 Sep 2023 15:46:48 -0500 Subject: [PATCH 32/36] BMI270 - remove extraneous fifo.c file inclusions --- src/main/target/BETAFPVF411/target.mk | 1 - src/main/target/FLYWOOF411_5IN1_AIO/target.mk | 27 +++++++++---------- src/main/target/FOXEERF745_AIO/target.mk | 1 - src/main/target/IFLIGHT_F745_AIO_V2/target.mk | 1 - .../target/NBD_INFINITYAIOV2PRO/target.mk | 13 +++++---- src/main/target/SKYSTARSF7HD/target.mk | 5 ++-- src/main/target/SKYSTARSF7HDPRO/target.mk | 13 +++++---- src/main/target/TMTR_TMOTORF7/target.mk | 1 - 8 files changed, 27 insertions(+), 35 deletions(-) diff --git a/src/main/target/BETAFPVF411/target.mk b/src/main/target/BETAFPVF411/target.mk index 2eafab1ba8..ab77872afb 100644 --- a/src/main/target/BETAFPVF411/target.mk +++ b/src/main/target/BETAFPVF411/target.mk @@ -4,7 +4,6 @@ FEATURES = VCP ONBOARDFLASH TARGET_SRC = \ drivers/accgyro/accgyro_mpu.c \ drivers/accgyro/accgyro_spi_mpu6000.c \ - drivers/accgyro/accgyro_bmi270_maximum_fifo.c \ drivers/accgyro/accgyro_spi_bmi270.c \ drivers/barometer/barometer_bmp085.c \ drivers/barometer/barometer_bmp280.c \ diff --git a/src/main/target/FLYWOOF411_5IN1_AIO/target.mk b/src/main/target/FLYWOOF411_5IN1_AIO/target.mk index a69689a1d8..a0f0b8aff2 100644 --- a/src/main/target/FLYWOOF411_5IN1_AIO/target.mk +++ b/src/main/target/FLYWOOF411_5IN1_AIO/target.mk @@ -3,24 +3,23 @@ F411_TARGETS += $(TARGET) FEATURES += VCP TARGET_SRC = \ - drivers/accgyro/accgyro_mpu.c \ - drivers/accgyro/accgyro_bmi270_maximum_fifo.c \ - drivers/accgyro/accgyro_spi_bmi270.c \ - drivers/accgyro/accgyro_spi_icm20689.c \ - drivers/accgyro/accgyro_spi_icm426xx.c \ - drivers/accgyro/accgyro_spi_mpu6000.c \ - drivers/barometer/barometer_bmp085.c \ - drivers/barometer/barometer_bmp280.c \ - drivers/barometer/barometer_ms5611.c \ - drivers/compass/compass_hmc5883l.c \ + drivers/accgyro/accgyro_mpu.c \ + drivers/accgyro/accgyro_spi_bmi270.c \ + drivers/accgyro/accgyro_spi_icm20689.c \ + drivers/accgyro/accgyro_spi_icm426xx.c \ + drivers/accgyro/accgyro_spi_mpu6000.c \ + drivers/barometer/barometer_bmp085.c \ + drivers/barometer/barometer_bmp280.c \ + drivers/barometer/barometer_ms5611.c \ + drivers/compass/compass_hmc5883l.c \ drivers/compass/compass_qmc5883l.c \ drivers/max7456.c \ drivers/rx/rx_cc2500.c \ - rx/cc2500_common.c \ + rx/cc2500_common.c \ rx/cc2500_frsky_shared.c \ rx/cc2500_frsky_d.c \ - rx/cc2500_frsky_x.c \ - rx/cc2500_redpine.c \ + rx/cc2500_frsky_x.c \ + rx/cc2500_redpine.c \ rx/cc2500_sfhss.c\ drivers/rx/rx_a7105.c \ - drivers/light_ws2811strip.c + drivers/light_ws2811strip.c diff --git a/src/main/target/FOXEERF745_AIO/target.mk b/src/main/target/FOXEERF745_AIO/target.mk index 348e36c404..01b1543813 100644 --- a/src/main/target/FOXEERF745_AIO/target.mk +++ b/src/main/target/FOXEERF745_AIO/target.mk @@ -2,7 +2,6 @@ F7X5XG_TARGETS += $(TARGET) FEATURES += VCP ONBOARDFLASH TARGET_SRC = \ - drivers/accgyro/accgyro_bmi270_maximum_fifo.c \ drivers/accgyro/accgyro_spi_bmi270.c \ drivers/accgyro/accgyro_spi_mpu6000.c \ drivers/accgyro/accgyro_mpu.c \ diff --git a/src/main/target/IFLIGHT_F745_AIO_V2/target.mk b/src/main/target/IFLIGHT_F745_AIO_V2/target.mk index a2ee8ce99b..7cfd60c081 100644 --- a/src/main/target/IFLIGHT_F745_AIO_V2/target.mk +++ b/src/main/target/IFLIGHT_F745_AIO_V2/target.mk @@ -2,7 +2,6 @@ F7X5XG_TARGETS += $(TARGET) FEATURES += VCP ONBOARDFLASH TARGET_SRC = \ - drivers/accgyro/accgyro_bmi270_maximum_fifo.c \ drivers/accgyro/accgyro_spi_bmi270.c \ drivers/accgyro/accgyro_spi_mpu6000.c \ drivers/accgyro/accgyro_mpu.c \ diff --git a/src/main/target/NBD_INFINITYAIOV2PRO/target.mk b/src/main/target/NBD_INFINITYAIOV2PRO/target.mk index 6c7c440f2d..eb8fe54ef3 100644 --- a/src/main/target/NBD_INFINITYAIOV2PRO/target.mk +++ b/src/main/target/NBD_INFINITYAIOV2PRO/target.mk @@ -2,14 +2,13 @@ F7X5XG_TARGETS += $(TARGET) FEATURES += VCP ONBOARDFLASH TARGET_SRC = \ - drivers/accgyro/accgyro_bmi270_maximum_fifo.c \ drivers/accgyro/accgyro_spi_bmi270.c \ drivers/light_ws2811strip.c \ drivers/max7456.c \ -drivers/compass/compass_fake.c \ -drivers/compass/compass_ak8963.c \ -drivers/compass/compass_ak8975.c \ -drivers/compass/compass_hmc5883l.c \ -drivers/compass/compass_lis3mdl.c \ -drivers/compass/compass_qmc5883l.c \ + drivers/compass/compass_fake.c \ + drivers/compass/compass_ak8963.c \ + drivers/compass/compass_ak8975.c \ + drivers/compass/compass_hmc5883l.c \ + drivers/compass/compass_lis3mdl.c \ + drivers/compass/compass_qmc5883l.c \ diff --git a/src/main/target/SKYSTARSF7HD/target.mk b/src/main/target/SKYSTARSF7HD/target.mk index a9c5cf138f..1f9b3af4ff 100644 --- a/src/main/target/SKYSTARSF7HD/target.mk +++ b/src/main/target/SKYSTARSF7HD/target.mk @@ -2,8 +2,7 @@ F7X2RE_TARGETS += $(TARGET) FEATURES += VCP ONBOARDFLASH TARGET_SRC = \ - drivers/accgyro/accgyro_bmi270_maximum_fifo.c \ - drivers/accgyro/accgyro_spi_bmi270.c \ + drivers/accgyro/accgyro_spi_bmi270.c \ drivers/accgyro/accgyro_mpu.c \ drivers/accgyro/accgyro_spi_mpu6000.c \ drivers/barometer/barometer_bmp085.c \ @@ -11,5 +10,5 @@ TARGET_SRC = \ drivers/barometer/barometer_ms5611.c \ drivers/compass/compass_hmc5883l.c \ drivers/compass/compass_qmc5883l.c \ - drivers/light_ws2811strip.c \ + drivers/light_ws2811strip.c \ drivers/max7456.c diff --git a/src/main/target/SKYSTARSF7HDPRO/target.mk b/src/main/target/SKYSTARSF7HDPRO/target.mk index 108b015201..383eb5da06 100644 --- a/src/main/target/SKYSTARSF7HDPRO/target.mk +++ b/src/main/target/SKYSTARSF7HDPRO/target.mk @@ -2,16 +2,15 @@ F7X2RE_TARGETS += $(TARGET) FEATURES += VCP ONBOARDFLASH TARGET_SRC = \ - drivers/accgyro/accgyro_bmi270_maximum_fifo.c \ - drivers/accgyro/accgyro_spi_bmi270.c \ - drivers/accgyro/accgyro_spi_mpu6000.c \ + drivers/accgyro/accgyro_spi_bmi270.c \ + drivers/accgyro/accgyro_spi_mpu6000.c \ drivers/accgyro/accgyro_mpu.c \ - drivers/accgyro/accgyro_mpu6500.c \ - drivers/accgyro/accgyro_spi_mpu6500.c \ + drivers/accgyro/accgyro_mpu6500.c \ + drivers/accgyro/accgyro_spi_mpu6500.c \ drivers/barometer/barometer_bmp280.c \ drivers/barometer/barometer_ms5611.c \ - drivers/barometer/barometer_bmp085.c \ + drivers/barometer/barometer_bmp085.c \ drivers/compass/compass_hmc5883l.c \ drivers/compass/compass_qmc5883l.c \ - drivers/light_ws2811strip.c \ + drivers/light_ws2811strip.c \ drivers/max7456.c diff --git a/src/main/target/TMTR_TMOTORF7/target.mk b/src/main/target/TMTR_TMOTORF7/target.mk index 31b5f262e1..8d266e1396 100644 --- a/src/main/target/TMTR_TMOTORF7/target.mk +++ b/src/main/target/TMTR_TMOTORF7/target.mk @@ -1,7 +1,6 @@ F7X2RE_TARGETS += $(TARGET) FEATURES = VCP ONBOARDFLASH TARGET_SRC = \ - drivers/accgyro/accgyro_bmi270_maximum_fifo.c \ drivers/accgyro/accgyro_spi_bmi270.c \ drivers/accgyro/accgyro_mpu6500.c \ drivers/accgyro/accgyro_spi_mpu6000.c \ From aa3daa87aed4b8475d5cd08fae64e920ed560ab0 Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Thu, 28 Sep 2023 09:28:08 -0500 Subject: [PATCH 33/36] BMI270 - comment spiCalculateDivider --- src/main/drivers/bus_spi.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/drivers/bus_spi.c b/src/main/drivers/bus_spi.c index d42f0e6156..bd1a82d1ed 100644 --- a/src/main/drivers/bus_spi.c +++ b/src/main/drivers/bus_spi.c @@ -252,6 +252,7 @@ void spiBusSetInstance(busDevice_t *bus, SPI_TypeDef *instance) { bus->busdev_u.spi.instance = instance; } +// icm42688p and bmi270 porting uint16_t spiCalculateDivider(uint32_t freq) { #if defined(STM32F4) || defined(STM32G4) || defined(STM32F7) From dc51cc9dd9d5ba42c50ca7e5c2b804ecd5905274 Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Thu, 28 Sep 2023 09:29:05 -0500 Subject: [PATCH 34/36] BMI270 - sync targets --- src/main/target/NBD_INFINITYAIOV2PRO/target.h | 4 ++++ src/main/target/NBD_INFINITYAIOV2PRO/target.mk | 1 - src/main/target/SKYSTARSF7HDPRO/target.mk | 2 +- 3 files changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/target/NBD_INFINITYAIOV2PRO/target.h b/src/main/target/NBD_INFINITYAIOV2PRO/target.h index fac5460376..2c35079099 100644 --- a/src/main/target/NBD_INFINITYAIOV2PRO/target.h +++ b/src/main/target/NBD_INFINITYAIOV2PRO/target.h @@ -40,6 +40,10 @@ // dual gyro #define USE_DUAL_GYRO +#define USE_EXTI //REQUIRED when USE_GYRO_EXTI +#define USE_GYRO_EXTI +#define USE_MPU_DATA_READY_SIGNAL + // gyro 1 #define GYRO_1_CS_PIN PE11 #define GYRO_1_SPI_INSTANCE SPI4 diff --git a/src/main/target/NBD_INFINITYAIOV2PRO/target.mk b/src/main/target/NBD_INFINITYAIOV2PRO/target.mk index eb8fe54ef3..ff6b7c6818 100644 --- a/src/main/target/NBD_INFINITYAIOV2PRO/target.mk +++ b/src/main/target/NBD_INFINITYAIOV2PRO/target.mk @@ -11,4 +11,3 @@ TARGET_SRC = \ drivers/compass/compass_hmc5883l.c \ drivers/compass/compass_lis3mdl.c \ drivers/compass/compass_qmc5883l.c \ - diff --git a/src/main/target/SKYSTARSF7HDPRO/target.mk b/src/main/target/SKYSTARSF7HDPRO/target.mk index 383eb5da06..6d549c15d0 100644 --- a/src/main/target/SKYSTARSF7HDPRO/target.mk +++ b/src/main/target/SKYSTARSF7HDPRO/target.mk @@ -8,7 +8,7 @@ TARGET_SRC = \ drivers/accgyro/accgyro_mpu6500.c \ drivers/accgyro/accgyro_spi_mpu6500.c \ drivers/barometer/barometer_bmp280.c \ - drivers/barometer/barometer_ms5611.c \ + drivers/barometer/barometer_ms5611.c \ drivers/barometer/barometer_bmp085.c \ drivers/compass/compass_hmc5883l.c \ drivers/compass/compass_qmc5883l.c \ From b5ac87a0734ee6f27ece8eb1d3b86b1da98a2d06 Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Tue, 31 Oct 2023 15:43:17 -0500 Subject: [PATCH 35/36] BMI270 - remove targets and targets modification from this PR --- src/main/target/BETAFPVF411/target.h | 7 - src/main/target/BETAFPVF411/target.mk | 1 - src/main/target/FLYWOOF411_5IN1_AIO/target.mk | 25 +-- .../FOXEERF745_AIO/FOXEERF745_AIO_V2.mk | 0 src/main/target/FOXEERF745_AIO/target.h | 15 -- src/main/target/FOXEERF745_AIO/target.mk | 5 +- src/main/target/IFLIGHT_F745_AIO_V2/target.h | 2 +- src/main/target/IFLIGHT_F745_AIO_V2/target.mk | 2 +- src/main/target/NBD_INFINITYAIOV2PRO/target.c | 39 ---- src/main/target/NBD_INFINITYAIOV2PRO/target.h | 153 ------------- .../target/NBD_INFINITYAIOV2PRO/target.mk | 13 -- src/main/target/SKYSTARSF7HD/target.h | 7 - src/main/target/SKYSTARSF7HD/target.mk | 3 +- src/main/target/SKYSTARSF7HDPRO/config.c | 38 ---- src/main/target/SKYSTARSF7HDPRO/target.c | 43 ---- src/main/target/SKYSTARSF7HDPRO/target.h | 205 ------------------ src/main/target/SKYSTARSF7HDPRO/target.mk | 16 -- src/main/target/TMTR_TMOTORF7/target.h | 7 - src/main/target/TMTR_TMOTORF7/target.mk | 1 - 19 files changed, 17 insertions(+), 565 deletions(-) delete mode 100644 src/main/target/FOXEERF745_AIO/FOXEERF745_AIO_V2.mk delete mode 100644 src/main/target/NBD_INFINITYAIOV2PRO/target.c delete mode 100644 src/main/target/NBD_INFINITYAIOV2PRO/target.h delete mode 100644 src/main/target/NBD_INFINITYAIOV2PRO/target.mk delete mode 100644 src/main/target/SKYSTARSF7HDPRO/config.c delete mode 100644 src/main/target/SKYSTARSF7HDPRO/target.c delete mode 100644 src/main/target/SKYSTARSF7HDPRO/target.h delete mode 100644 src/main/target/SKYSTARSF7HDPRO/target.mk diff --git a/src/main/target/BETAFPVF411/target.h b/src/main/target/BETAFPVF411/target.h index ddd244caef..68172fdc56 100644 --- a/src/main/target/BETAFPVF411/target.h +++ b/src/main/target/BETAFPVF411/target.h @@ -34,13 +34,6 @@ #define USE_SPI #define USE_SPI_DEVICE_1 -#define USE_SPI_GYRO -#define USE_ACCGYRO_BMI270 -#define BMI270_CS_PIN PA4 -#define BMI270_SPI_INSTANCE SPI1 -#define ACC_BMI270_ALIGN CW90_DEG -#define GYRO_BMI270_ALIGN CW90_DEG - #define SPI1_SCK_PIN PA5 #define SPI1_MISO_PIN PA6 #define SPI1_MOSI_PIN PA7 diff --git a/src/main/target/BETAFPVF411/target.mk b/src/main/target/BETAFPVF411/target.mk index ab77872afb..3971d0ca39 100644 --- a/src/main/target/BETAFPVF411/target.mk +++ b/src/main/target/BETAFPVF411/target.mk @@ -4,7 +4,6 @@ FEATURES = VCP ONBOARDFLASH TARGET_SRC = \ drivers/accgyro/accgyro_mpu.c \ drivers/accgyro/accgyro_spi_mpu6000.c \ - drivers/accgyro/accgyro_spi_bmi270.c \ drivers/barometer/barometer_bmp085.c \ drivers/barometer/barometer_bmp280.c \ drivers/barometer/barometer_ms5611.c \ diff --git a/src/main/target/FLYWOOF411_5IN1_AIO/target.mk b/src/main/target/FLYWOOF411_5IN1_AIO/target.mk index a0f0b8aff2..19d3a639b7 100644 --- a/src/main/target/FLYWOOF411_5IN1_AIO/target.mk +++ b/src/main/target/FLYWOOF411_5IN1_AIO/target.mk @@ -3,23 +3,22 @@ F411_TARGETS += $(TARGET) FEATURES += VCP TARGET_SRC = \ - drivers/accgyro/accgyro_mpu.c \ - drivers/accgyro/accgyro_spi_bmi270.c \ - drivers/accgyro/accgyro_spi_icm20689.c \ - drivers/accgyro/accgyro_spi_icm426xx.c \ - drivers/accgyro/accgyro_spi_mpu6000.c \ - drivers/barometer/barometer_bmp085.c \ - drivers/barometer/barometer_bmp280.c \ - drivers/barometer/barometer_ms5611.c \ - drivers/compass/compass_hmc5883l.c \ + drivers/accgyro/accgyro_mpu.c \ + drivers/accgyro/accgyro_spi_mpu6000.c \ + drivers/accgyro/accgyro_spi_icm20689.c \ + drivers/accgyro/accgyro_spi_icm426xx.c \ + drivers/barometer/barometer_bmp085.c \ + drivers/barometer/barometer_bmp280.c \ + drivers/barometer/barometer_ms5611.c \ + drivers/compass/compass_hmc5883l.c \ drivers/compass/compass_qmc5883l.c \ drivers/max7456.c \ drivers/rx/rx_cc2500.c \ - rx/cc2500_common.c \ + rx/cc2500_common.c \ rx/cc2500_frsky_shared.c \ rx/cc2500_frsky_d.c \ - rx/cc2500_frsky_x.c \ - rx/cc2500_redpine.c \ + rx/cc2500_frsky_x.c \ + rx/cc2500_redpine.c \ rx/cc2500_sfhss.c\ drivers/rx/rx_a7105.c \ - drivers/light_ws2811strip.c + drivers/light_ws2811strip.c diff --git a/src/main/target/FOXEERF745_AIO/FOXEERF745_AIO_V2.mk b/src/main/target/FOXEERF745_AIO/FOXEERF745_AIO_V2.mk deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/src/main/target/FOXEERF745_AIO/target.h b/src/main/target/FOXEERF745_AIO/target.h index 6994d23949..bd56acc8ef 100644 --- a/src/main/target/FOXEERF745_AIO/target.h +++ b/src/main/target/FOXEERF745_AIO/target.h @@ -21,12 +21,7 @@ #pragma once #define TARGET_BOARD_IDENTIFIER "FOXE" - - #if defined(FOXEERF745_AIO_V2) - #define USBD_PRODUCT_STRING "FOXEERF745_AIO_V2" - #else #define USBD_PRODUCT_STRING "FOXEERF745_AIO" - #endif #define LED0_PIN PC13 @@ -60,16 +55,6 @@ //#define MAG_HMC5883_ALIGN CW270_DEG_FLIP //#define MAG_ALIGN CW180_DEG //not sure if this command will work or if should be more specific to mag - #if defined(FOXEERF745_AIO_V2) - //BMI270 - #define USE_SPI_GYRO - #define USE_ACCGYRO_BMI270 - #define BMI270_CS_PIN PA15 - #define BMI270_SPI_INSTANCE SPI3 - #define ACC_BMI270_ALIGN CW180_DEG - #define GYRO_BMI270_ALIGN CW180_DEG - #endif - #define USE_BARO #define USE_BARO_BMP280 #define BARO_I2C_INSTANCE (I2CDEV_1) diff --git a/src/main/target/FOXEERF745_AIO/target.mk b/src/main/target/FOXEERF745_AIO/target.mk index 01b1543813..e4faff73d9 100644 --- a/src/main/target/FOXEERF745_AIO/target.mk +++ b/src/main/target/FOXEERF745_AIO/target.mk @@ -2,12 +2,11 @@ F7X5XG_TARGETS += $(TARGET) FEATURES += VCP ONBOARDFLASH TARGET_SRC = \ - drivers/accgyro/accgyro_spi_bmi270.c \ drivers/accgyro/accgyro_spi_mpu6000.c \ - drivers/accgyro/accgyro_mpu.c \ + drivers/accgyro/accgyro_mpu.c \ drivers/barometer/barometer_bmp280.c \ drivers/compass/compass_hmc5883l.c \ drivers/compass/compass_qmc5883l.c \ - drivers/compass/compass_lis3mdl.c \ + drivers/compass/compass_lis3mdl.c \ drivers/light_ws2811strip.c \ drivers/max7456.c diff --git a/src/main/target/IFLIGHT_F745_AIO_V2/target.h b/src/main/target/IFLIGHT_F745_AIO_V2/target.h index e771a75672..6eec787ed9 100644 --- a/src/main/target/IFLIGHT_F745_AIO_V2/target.h +++ b/src/main/target/IFLIGHT_F745_AIO_V2/target.h @@ -39,7 +39,7 @@ #define USE_DUAL_GYRO #define USE_SPI_GYRO -#define USE_ACCGYRO_BMI270 +//#define USE_ACCGYRO_BMI270 #define USE_GYRO_SPI_MPU6000 #define USE_ACC_SPI_MPU6000 diff --git a/src/main/target/IFLIGHT_F745_AIO_V2/target.mk b/src/main/target/IFLIGHT_F745_AIO_V2/target.mk index 7cfd60c081..525f68127c 100644 --- a/src/main/target/IFLIGHT_F745_AIO_V2/target.mk +++ b/src/main/target/IFLIGHT_F745_AIO_V2/target.mk @@ -2,12 +2,12 @@ F7X5XG_TARGETS += $(TARGET) FEATURES += VCP ONBOARDFLASH TARGET_SRC = \ - drivers/accgyro/accgyro_spi_bmi270.c \ drivers/accgyro/accgyro_spi_mpu6000.c \ drivers/accgyro/accgyro_mpu.c \ drivers/barometer/barometer_ms5611.c \ drivers/barometer/barometer_bmp280.c \ drivers/barometer/barometer_bmp085.c \ + drivers/barometer/barometer_ms5611.c \ drivers/compass/compass_hmc5883l.c \ drivers/compass/compass_qmc5883l.c \ drivers/light_ws2811strip.c \ diff --git a/src/main/target/NBD_INFINITYAIOV2PRO/target.c b/src/main/target/NBD_INFINITYAIOV2PRO/target.c deleted file mode 100644 index abad368d32..0000000000 --- a/src/main/target/NBD_INFINITYAIOV2PRO/target.c +++ /dev/null @@ -1,39 +0,0 @@ -/* - * This file is part of Cleanflight and Betaflight. - * - * Cleanflight and Betaflight are free software. You can redistribute - * this software and/or modify this software under the terms of the - * GNU General Public License as published by the Free Software - * Foundation, either version 3 of the License, or (at your option) - * any later version. - * - * Cleanflight and Betaflight are distributed in the hope that they - * will be useful, but WITHOUT ANY WARRANTY; without even the implied - * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. - * See the GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this software. - * - * If not, see . - */ - -#include - -#include "platform.h" -#include "drivers/io.h" - -#include "drivers/dma.h" -#include "drivers/timer.h" -#include "drivers/timer_def.h" - -const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - -DEF_TIM(TIM1, CH2, PA9, TIM_USE_LED, 0, 0 ), // LED_STRIP, - -DEF_TIM(TIM3, CH4, PC9, TIM_USE_MOTOR, 0, 0 ), // M1 -DEF_TIM(TIM3, CH3, PC8, TIM_USE_MOTOR, 0, 0 ), // M2 -DEF_TIM(TIM3, CH2, PC7, TIM_USE_MOTOR, 0, 0 ), // M3 -DEF_TIM(TIM3, CH1, PC6, TIM_USE_MOTOR, 0, 0 ), // M4 - -}; diff --git a/src/main/target/NBD_INFINITYAIOV2PRO/target.h b/src/main/target/NBD_INFINITYAIOV2PRO/target.h deleted file mode 100644 index 2c35079099..0000000000 --- a/src/main/target/NBD_INFINITYAIOV2PRO/target.h +++ /dev/null @@ -1,153 +0,0 @@ -/* -* This file is part of Cleanflight and Betaflight. -* -* Cleanflight and Betaflight are free software. You can redistribute -* this software and/or modify this software under the terms of the -* GNU General Public License as published by the Free Software -* Foundation, either version 3 of the License, or (at your option) -* any later version. -* -* Cleanflight and Betaflight are distributed in the hope that they -* will be useful, but WITHOUT ANY WARRANTY; without even the implied -* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. -* See the GNU General Public License for more details. -* -* You should have received a copy of the GNU General Public License -* along with this software. -* -* If not, see . -*/ - -#pragma once - -#define TARGET_BOARD_IDENTIFIER "NEBD" - -#define USBD_PRODUCT_STRING "NBD_INFINITYAIOV2PRO" - -#define LED0_PIN PC0 - -#define USE_BEEPER -#define BEEPER_PIN PD13 -#define BEEPER_INVERTED - -#define USE_SPI_GYRO -#define USE_ACCGYRO_BMI270 -#define BMI270_CS_PIN PE11 -#define BMI270_SPI_INSTANCE SPI4 -#define ACC_BMI270_ALIGN CW270_DEG -#define GYRO_BMI270_ALIGN CW270_DEG - -// dual gyro -#define USE_DUAL_GYRO - -#define USE_EXTI //REQUIRED when USE_GYRO_EXTI -#define USE_GYRO_EXTI -#define USE_MPU_DATA_READY_SIGNAL - -// gyro 1 -#define GYRO_1_CS_PIN PE11 -#define GYRO_1_SPI_INSTANCE SPI4 -#define GYRO_1_EXTI_PIN PB1 -#define GYRO_1_ALIGN CW270_DEG -#define ACC_1_ALIGN CW270_DEG - -// gyro 2 -#define GYRO_2_CS_PIN PB12 -#define GYRO_2_SPI_INSTANCE SPI2 -#define GYRO_2_EXTI_PIN PD0 -#define GYRO_2_ALIGN CW270_DEG -#define ACC_2_ALIGN CW270_DEG - -#define USE_VCP -#define USE_USB_DETECT - -#define USE_UART1 -#define UART1_RX_PIN PB7 - -#define USE_UART2 -#define UART2_RX_PIN PA3 -#define UART2_TX_PIN PA2 - -#define USE_UART3 -#define UART3_RX_PIN PB11 -#define UART3_TX_PIN PB10 - -#define USE_UART5 -#define UART5_RX_PIN PD2 - -#define USE_UART7 -#define UART7_TX_PIN PE8 - -#define USE_UART8 -#define UART8_RX_PIN PE0 -#define UART8_TX_PIN PE1 - -#define SERIAL_PORT_COUNT 7 //VCP, USART1, USART2, USART3, UART5, USART7, UART8 - -#define USE_ESC_SENSOR - -#define USE_SPI -#define USE_SPI_DEVICE_1 -#define USE_SPI_DEVICE_2 -#define USE_SPI_DEVICE_3 -#define USE_SPI_DEVICE_4 - -#define SPI1_SCK_PIN PA5 -#define SPI1_MISO_PIN PA6 -#define SPI1_MOSI_PIN PA7 - -#define SPI2_SCK_PIN PB13 -#define SPI2_MISO_PIN PB14 -#define SPI2_MOSI_PIN PB15 - -#define SPI3_SCK_PIN PB3 -#define SPI3_MISO_PIN PB4 -#define SPI3_MOSI_PIN PD6 - -#define SPI4_SCK_PIN PE12 -#define SPI4_MISO_PIN PE13 -#define SPI4_MOSI_PIN PE14 - -#define USE_MAX7456 -#define MAX7456_SPI_INSTANCE SPI3 -#define MAX7456_SPI_CS_PIN PA15 -#define MAX7456_SPI_CLK (SPI_CLOCK_STANDARD) // 10MHz -#define MAX7456_RESTORE_CLK (SPI_CLOCK_FAST) - -//#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT -#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT -#define USE_FLASHFS -#define USE_FLASH_W25Q128FV //official -#define FLASH_CS_PIN PB0 -#define FLASH_SPI_INSTANCE SPI1 - -#define USE_FLASH_W25Q //testing -#define USE_FLASH_W25M512 //testing -#define USE_FLASH_M25P16 //testing - -#define USE_I2C -#define USE_I2C_DEVICE_1 -#define I2C_DEVICE_1 (I2CDEV_1) -#define I2C1_SCL PB8 -#define I2C1_SDA PB9 - -#define USE_ADC -#define VBAT_ADC_PIN PC1 -#define CURRENT_METER_ADC_PIN PC2 -#define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC -#define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ADC -#define CURRENT_METER_SCALE_DEFAULT 100 - -#define BEEPER_PWM_HZ 5400 - -#define USE_LED_STRIP -#define LED_STRIP_PIN PA9 - -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD 0xffff -#define TARGET_IO_PORTE 0xffff - -#define USABLE_TIMER_CHANNEL_COUNT 7 -#define USED_TIMERS ( TIM_N(1) | TIM_N(3) | TIM_N(4) ) diff --git a/src/main/target/NBD_INFINITYAIOV2PRO/target.mk b/src/main/target/NBD_INFINITYAIOV2PRO/target.mk deleted file mode 100644 index ff6b7c6818..0000000000 --- a/src/main/target/NBD_INFINITYAIOV2PRO/target.mk +++ /dev/null @@ -1,13 +0,0 @@ -F7X5XG_TARGETS += $(TARGET) -FEATURES += VCP ONBOARDFLASH - -TARGET_SRC = \ - drivers/accgyro/accgyro_spi_bmi270.c \ - drivers/light_ws2811strip.c \ - drivers/max7456.c \ - drivers/compass/compass_fake.c \ - drivers/compass/compass_ak8963.c \ - drivers/compass/compass_ak8975.c \ - drivers/compass/compass_hmc5883l.c \ - drivers/compass/compass_lis3mdl.c \ - drivers/compass/compass_qmc5883l.c \ diff --git a/src/main/target/SKYSTARSF7HD/target.h b/src/main/target/SKYSTARSF7HD/target.h index 31d1d6f3fa..bf68dc5fc4 100644 --- a/src/main/target/SKYSTARSF7HD/target.h +++ b/src/main/target/SKYSTARSF7HD/target.h @@ -53,13 +53,6 @@ #define USE_ACC -#define USE_SPI_GYRO -#define USE_ACCGYRO_BMI270 -#define BMI270_CS_PIN PA4 -#define BMI270_SPI_INSTANCE SPI1 -#define ACC_BMI270_ALIGN CW180_DEG -#define GYRO_BMI270_ALIGN CW180_DEG - #define MPU6000_CS_PIN PA4 #define MPU6000_SPI_INSTANCE SPI1 diff --git a/src/main/target/SKYSTARSF7HD/target.mk b/src/main/target/SKYSTARSF7HD/target.mk index 1f9b3af4ff..1b54266132 100644 --- a/src/main/target/SKYSTARSF7HD/target.mk +++ b/src/main/target/SKYSTARSF7HD/target.mk @@ -2,7 +2,6 @@ F7X2RE_TARGETS += $(TARGET) FEATURES += VCP ONBOARDFLASH TARGET_SRC = \ - drivers/accgyro/accgyro_spi_bmi270.c \ drivers/accgyro/accgyro_mpu.c \ drivers/accgyro/accgyro_spi_mpu6000.c \ drivers/barometer/barometer_bmp085.c \ @@ -10,5 +9,5 @@ TARGET_SRC = \ drivers/barometer/barometer_ms5611.c \ drivers/compass/compass_hmc5883l.c \ drivers/compass/compass_qmc5883l.c \ - drivers/light_ws2811strip.c \ + drivers/light_ws2811strip.c \ drivers/max7456.c diff --git a/src/main/target/SKYSTARSF7HDPRO/config.c b/src/main/target/SKYSTARSF7HDPRO/config.c deleted file mode 100644 index 55d138b578..0000000000 --- a/src/main/target/SKYSTARSF7HDPRO/config.c +++ /dev/null @@ -1,38 +0,0 @@ -/* - * This file is part of Cleanflight and Betaflight. - * - * Cleanflight and Betaflight are free software. You can redistribute - * this software and/or modify this software under the terms of the - * GNU General Public License as published by the Free Software - * Foundation, either version 3 of the License, or (at your option) - * any later version. - * - * Cleanflight and Betaflight are distributed in the hope that they - * will be useful, but WITHOUT ANY WARRANTY; without even the implied - * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. - * See the GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this software. - * - * If not, see . - */ - -#include - -#include "platform.h" - -#define USE_TARGET_CONFIG - -#include "io/serial.h" -#include "pg/pinio.h" -#include "pg/piniobox.h" -#include "target.h" - - -void targetConfiguration(void) { - - pinioBoxConfigMutable()->permanentId[0] = 40; - pinioConfigMutable()->config[0] = 129; - -} diff --git a/src/main/target/SKYSTARSF7HDPRO/target.c b/src/main/target/SKYSTARSF7HDPRO/target.c deleted file mode 100644 index 57c3a5a336..0000000000 --- a/src/main/target/SKYSTARSF7HDPRO/target.c +++ /dev/null @@ -1,43 +0,0 @@ -/* - * This file is part of Cleanflight and Betaflight. - * - * Cleanflight and Betaflight are free software. You can redistribute - * this software and/or modify this software under the terms of the - * GNU General Public License as published by the Free Software - * Foundation, either version 3 of the License, or (at your option) - * any later version. - * - * Cleanflight and Betaflight are distributed in the hope that they - * will be useful, but WITHOUT ANY WARRANTY; without even the implied - * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. - * See the GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this software. - * - * If not, see . - */ - -#include - -#include "platform.h" -#include "drivers/io.h" - -#include "drivers/dma.h" -#include "drivers/timer.h" -#include "drivers/timer_def.h" - -const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { - - DEF_TIM(TIM3, CH1, PB4, TIM_USE_PPM, 0, 0 ), // PPM IN - - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MOTOR, 0, 0 ), // S1_OUT – UP2-1 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MOTOR, 0, 0 ), // S2_OUT – UP2-1 - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MOTOR, 0, 0 ), // S3_OUT – UP2-5 - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MOTOR, 0, 0 ), // S4_OUT – UP2-5 - - DEF_TIM(TIM1, CH1, PA8, TIM_USE_ANY, 0, 0 ), // FC CAM – DMA1_ST7 - - DEF_TIM(TIM2, CH2, PB3, TIM_USE_LED, 0, 0 ), // LED_STRIP – DMA1_ST6 - -}; diff --git a/src/main/target/SKYSTARSF7HDPRO/target.h b/src/main/target/SKYSTARSF7HDPRO/target.h deleted file mode 100644 index 2c3d169a21..0000000000 --- a/src/main/target/SKYSTARSF7HDPRO/target.h +++ /dev/null @@ -1,205 +0,0 @@ -/* - * This file is part of Cleanflight and Betaflight. - * - * Cleanflight and Betaflight are free software. You can redistribute - * this software and/or modify this software under the terms of the - * GNU General Public License as published by the Free Software - * Foundation, either version 3 of the License, or (at your option) - * any later version. - * - * Cleanflight and Betaflight are distributed in the hope that they - * will be useful, but WITHOUT ANY WARRANTY; without even the implied - * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. - * See the GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this software. - * - * If not, see . - */ - -#pragma once - -#define USE_TARGET_CONFIG - -#define TARGET_BOARD_IDENTIFIER "SSHP" -#define USBD_PRODUCT_STRING "SKST7HDPRO" - -// ******* LEDs and BEEPER ******** - -#define LED0_PIN PC15 -#define LED1_PIN PC14 - -#define USE_BEEPER -#define BEEPER_PIN PB2 -#define BEEPER_INVERTED - -//#define ENABLE_DSHOT_DMAR true - -#define USE_PINIO -#define PINIO1_PIN PA14 // Bluetooth mode control, PB0 is connected to the 36 pin (P2.0) of the Bluetooth chip. Replace PB0 with the pin for your flight control and 36-pin connection - -#define USE_CAMERA_CONTROL -#define CAMERA_CONTROL_PIN PA8 // define dedicated camera osd pin - - -// ******* GYRO and ACC ******** - -#define USE_DUAL_GYRO -#define USE_EXTI -#define GYRO_1_EXTI_PIN PC4 -#define GYRO_2_EXTI_PIN PC0 -#define MPU_INT_EXTI - -#define GYRO_1_CS_PIN PA4 -#define GYRO_1_SPI_INSTANCE SPI1 -#define GYRO_2_CS_PIN PC13 -#define GYRO_2_SPI_INSTANCE SPI1 - -#define USE_GYRO -#define USE_GYRO_SPI_MPU6000 -#define USE_GYRO_SPI_MPU6500 -#define USE_SPI_GYRO -#define USE_ACCGYRO_BMI270 - - -#define USE_ACC -#define USE_ACC_SPI_MPU6000 -#define USE_ACC_SPI_MPU6500 - -#define GYRO_1_ALIGN CW90_DEG_FLIP -#define ACC_1_ALIGN CW90_DEG_FLIP - -#define GYRO_2_ALIGN CW0_DEG -#define ACC_2_ALIGN CW0_DEG - -#define USE_MPU_DATA_READY_SIGNAL -#define ENSURE_MPU_DATA_READY_IS_LOW -#define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_1 - -// *************** Baro ************************ - -#define USE_SPI - -#define USE_SPI_DEVICE_2 -#define SPI2_SCK_PIN PB13 -#define SPI2_MISO_PIN PB14 -#define SPI2_MOSI_PIN PB15 - -#define USE_BARO -#define USE_BARO_SPI_BMP280 -#define DEFAULT_BARO_SPI_BMP280 -#define BMP280_SPI_INSTANCE SPI2 -#define BMP280_CS_PIN PB1 - -#define USE_I2C -#define USE_I2C_DEVICE_1 -#define I2C1_SCL PB8 -#define I2C1_SDA PB9 - - -//*********** Magnetometer / Compass ************* -#define USE_MAG -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define MAG_I2C_INSTANCE (I2CDEV_1) - -// ******* SERIAL ******** - -#define USE_VCP - -#define USE_UART1 -#define USE_UART2 -#define USE_UART3 -#define USE_UART4 -#define USE_UART5 -#define USE_UART6 - -#define UART1_TX_PIN PA9 -#define UART1_RX_PIN PA10 - -#define UART2_TX_PIN PA2 -#define UART2_RX_PIN PA3 - -#define UART3_TX_PIN PB10 -#define UART3_RX_PIN PB11 - -#define UART4_TX_PIN PA0 -#define UART4_RX_PIN PA1 - -#define UART5_TX_PIN PC12 -#define UART5_RX_PIN PD2 - -#define UART6_TX_PIN PC6 -#define UART6_RX_PIN PC7 - -#define USE_SOFTSERIAL1 -#define USE_SOFTSERIAL2 - -#define SERIAL_PORT_COUNT 9 //VCP, UART1-UART6 , 2 x Soft Serial - -// ******* SPI ******** - -#define USE_SPI - -#define USE_SPI_DEVICE_1 -#define SPI1_SCK_PIN PA5 -#define SPI1_MISO_PIN PA6 -#define SPI1_MOSI_PIN PA7 - - -#define USE_SPI_DEVICE_3 -#define SPI3_SCK_PIN PC10 -#define SPI3_MISO_PIN PC11 -#define SPI3_MOSI_PIN PB5 - -// ******* ADC ******** - -#define USE_ADC -#define ADC_INSTANCE ADC2 -#define ADC2_DMA_STREAM DMA2_Stream3 - -#define VBAT_ADC_PIN PC1 -#define RSSI_ADC_PIN PC2 -#define CURRENT_METER_ADC_PIN PC3 - -// ******* OSD ******** - -#define USE_MAX7456 -#define MAX7456_SPI_INSTANCE SPI2 -#define MAX7456_SPI_CS_PIN PB12 -#define MAX7456_SPI_CLK ( SPI_CLOCK_STANDARD ) -#define MAX7456_RESTORE_CLK ( SPI_CLOCK_FAST ) - -//******* FLASH ******** - -#define USE_FLASHFS -#define USE_FLASH_M25P16 -#define FLASH_CS_PIN PA15 -#define FLASH_SPI_INSTANCE SPI3 - -#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT - -// ******* FEATURES ******** - -#define USE_OSD - -#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL -#define SERIALRX_UART SERIAL_PORT_USART1 -#define SERIALRX_PROVIDER SERIALRX_SBUS - -#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_SOFTSERIAL) -#define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC -#define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ADC -#define CURRENT_METER_SCALE_DEFAULT 290 - - -#define USE_ESCSERIAL - -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD 0xffff - -#define USABLE_TIMER_CHANNEL_COUNT 7 -#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3)| TIM_N(4) | TIM_N(8) ) diff --git a/src/main/target/SKYSTARSF7HDPRO/target.mk b/src/main/target/SKYSTARSF7HDPRO/target.mk deleted file mode 100644 index 6d549c15d0..0000000000 --- a/src/main/target/SKYSTARSF7HDPRO/target.mk +++ /dev/null @@ -1,16 +0,0 @@ -F7X2RE_TARGETS += $(TARGET) -FEATURES += VCP ONBOARDFLASH - -TARGET_SRC = \ - drivers/accgyro/accgyro_spi_bmi270.c \ - drivers/accgyro/accgyro_spi_mpu6000.c \ - drivers/accgyro/accgyro_mpu.c \ - drivers/accgyro/accgyro_mpu6500.c \ - drivers/accgyro/accgyro_spi_mpu6500.c \ - drivers/barometer/barometer_bmp280.c \ - drivers/barometer/barometer_ms5611.c \ - drivers/barometer/barometer_bmp085.c \ - drivers/compass/compass_hmc5883l.c \ - drivers/compass/compass_qmc5883l.c \ - drivers/light_ws2811strip.c \ - drivers/max7456.c diff --git a/src/main/target/TMTR_TMOTORF7/target.h b/src/main/target/TMTR_TMOTORF7/target.h index 9dbeb0d6ac..1353cbead9 100644 --- a/src/main/target/TMTR_TMOTORF7/target.h +++ b/src/main/target/TMTR_TMOTORF7/target.h @@ -50,13 +50,6 @@ #define ACC_MPU6000_ALIGN CW0_DEG #define GYRO_MPU6000_ALIGN CW0_DEG -#define USE_SPI_GYRO -#define USE_ACCGYRO_BMI270 -#define BMI270_CS_PIN PA4 -#define BMI270_SPI_INSTANCE SPI1 -#define ACC_BMI270_ALIGN CW0_DEG -#define GYRO_BMI270_ALIGN CW0_DEG - #define USE_ACC #define USE_ACC_SPI_MPU6000 #define USE_ACC_SPI_MPU6500 diff --git a/src/main/target/TMTR_TMOTORF7/target.mk b/src/main/target/TMTR_TMOTORF7/target.mk index 8d266e1396..15077cfbd3 100644 --- a/src/main/target/TMTR_TMOTORF7/target.mk +++ b/src/main/target/TMTR_TMOTORF7/target.mk @@ -1,7 +1,6 @@ F7X2RE_TARGETS += $(TARGET) FEATURES = VCP ONBOARDFLASH TARGET_SRC = \ - drivers/accgyro/accgyro_spi_bmi270.c \ drivers/accgyro/accgyro_mpu6500.c \ drivers/accgyro/accgyro_spi_mpu6000.c \ drivers/accgyro/accgyro_spi_mpu6500.c \ From dd6f3c304e948079e808d4fd4c44ded13678f0de Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Tue, 31 Oct 2023 15:55:32 -0500 Subject: [PATCH 36/36] BMI270 - emptyline cleanup --- src/main/drivers/accgyro/gyro_sync.c | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/drivers/accgyro/gyro_sync.c b/src/main/drivers/accgyro/gyro_sync.c index fe36341caa..3653c24d43 100644 --- a/src/main/drivers/accgyro/gyro_sync.c +++ b/src/main/drivers/accgyro/gyro_sync.c @@ -54,8 +54,6 @@ uint32_t gyroSetSampleRate(gyroDev_t *gyro, uint8_t lpf, uint8_t gyroSyncDenomin gyro->mpuDividerDrops = gyroSyncDenominator - 1; gyro->gyroRateKHz = lpfNoneOr256 ? GYRO_RATE_8_kHz : GYRO_RATE_1_kHz; - - switch (gyro->mpuDetectionResult.sensor) { case ICM_20649_SPI: //20649 is a weird gyro gyro->gyroRateKHz = lpfNoneOr256 ? GYRO_RATE_9_kHz : GYRO_RATE_1100_Hz;