/**
 * @file    imu.c
 * @brief   LSM6DS3TR-C IMU module
 * @author  Kenny Tran
 * @date    Feb 28 2024
 * @version 0.0.1
 *
 * @section LICENSES
 * Copyright (C) Morgan Advanced Programmable Systems, Inc.
 */


/*--------------------------------------------------------------------------------------------------
 * INCLUDE FILES
 *------------------------------------------------------------------------------------------------*/
#include <stdint.h>
#include <stdbool.h>
#include <string.h>
#include <stdlib.h>

#include <zephyr/kernel.h>
#include <zephyr/timing/timing.h>

#include "imu.h"
#include "imu_reg.h"
#include "mcu.h"

#include "../app/motion.h"
#include "../app/shot_capture.h"

#include "../lib/errno.h"
#include "../lib/print.h"
#include "../lib/debug.h"
#include "../lib/fs.h"

#include "../wrappers/i2c.h"
#include "../wrappers/gpio.h"


/*--------------------------------------------------------------------------------------------------
 * MACROS & CONSTANTS
 *------------------------------------------------------------------------------------------------*/
K_SEM_DEFINE(_imu_int_sem, 0, 1);
K_SEM_DEFINE(shot_capture_sem, 0, 4);


/*--------------------------------------------------------------------------------------------------
 * THREAD DEFINITIONS
 *------------------------------------------------------------------------------------------------*/
K_THREAD_STACK_DEFINE(_imu_task_stack, IMU_TASK_STACK_SIZE);
K_THREAD_STACK_DEFINE(_imu_interrrupt_handler_task_stack, IMU_INTERRUPT_HANDLER_TASK_STACK_SIZE);


/*--------------------------------------------------------------------------------------------------
 * TYPE DEFINITIONS & ENUMERATIONS
 *------------------------------------------------------------------------------------------------*/
/* Defines the physical location of a bit field in the IMU registers */
typedef struct _imu_field_loc {
    const uint8_t reg_addr    : 8; // 2-digit hexidecimal address of the register in the IMU
    const uint8_t field_start : 3; // the MSB of the field can be from 0th to 7th bit so 3 bits is enough
    const uint8_t field_len   : 4; // bit length is 1 to 8
} __attribute__((packed, aligned(1))) _imu_field_loc_s;

/* list of all the fields that can be configured */
typedef struct _imu_fields {
    /* Accelerometer */
    const _imu_field_loc_s fs_xl_f;
    const _imu_field_loc_s odr_xl_f;
    const _imu_field_loc_s lpf1_bw_sel_f;
    const _imu_field_loc_s lpf2_xl_en_f;
    const _imu_field_loc_s hp_slope_xl_en_f;
    const _imu_field_loc_s hpcf_xl_f;
    const _imu_field_loc_s input_composite_f;

    /* Gyroscope */
    const _imu_field_loc_s fs_125_f;      // FS_125  CTRL2_G (0x11)
    const _imu_field_loc_s fs_g_f;        // FS_G    CTRL2_G (0x11)
    const _imu_field_loc_s odr_g_f;       // ODR_G   CTRL2_G (0x11)
    const _imu_field_loc_s hp_en_g_f;     // HP_EN_G CTRL7_G (0x16)
    const _imu_field_loc_s hpm_g_f;       // HPM_G   CTRL7_G (0x16)
    const _imu_field_loc_s lpf1_sel_g_f;  // HPM_G   CTRL4_C (0x13)
    const _imu_field_loc_s ftype_f;       // FTYPE   CTRL6_C (0x15)

    /* FIFO */
    const _imu_field_loc_s fifo_thresh_low_f;
    const _imu_field_loc_s fifo_thresh_high_f;
    const _imu_field_loc_s dec_fifo_gyro_f;
    const _imu_field_loc_s dec_fifo_xl_f;
    const _imu_field_loc_s odr_fifo_f;
    const _imu_field_loc_s fifo_mode_f;

    /* OTHERS*/
    const _imu_field_loc_s int1_fth_f;
    const _imu_field_loc_s sw_reset_f;
    const _imu_field_loc_s bdu_f;
    const _imu_field_loc_s interrupts_enable_f;
    const _imu_field_loc_s slope_fds_f;
    const _imu_field_loc_s tap_x_en_f;
    const _imu_field_loc_s tap_y_en_f;
    const _imu_field_loc_s tap_z_en_f;
    const _imu_field_loc_s lir_f;
    const _imu_field_loc_s tap_ths_f;
    const _imu_field_loc_s quiet_f;
    const _imu_field_loc_s shock_f;
    const _imu_field_loc_s single_double_tap_f;
    const _imu_field_loc_s wk_ths_f;
    const _imu_field_loc_s wake_dur_f;
    const _imu_field_loc_s int1_single_tap_f;
    const _imu_field_loc_s int1_wu_f;
    const _imu_field_loc_s int1_double_tap_f;
} _imu_fields_s;

typedef struct _imu_field_value {
    /* Accelerometer */
    /* Accelerometer full-scale range value (g)
     * 0: +/- 2
     * 1: +/- 16
     * 2: +/- 4
     * 3: +/- 8
     */
    LSM6DS3TR_C_FS_XL_e fs_xl_v;
    /* Accelerometer ODR value (Hz)
     * 0: power-down
     * 1: 12.5
     * 2: 26
     * 3: 52
     * 4: 104
     * 5: 208
     * 6: 416
     * 7: 833
     * 8: 1666
     * 9: 3332
     * 10: 6664
     * 11: 1.6 (low power only)
     */
    LSM6DS3TR_C_ODR_XL_e odr_xl_v;
    /* Accelerometer low-pass filter 1 bandwidth selection (Hz)
     * 0: odr/2
     * 1: odr/4
     */
    LSM6DS3TR_C_LPF1_BW_SEL_e lpf1_bw_sel_v;
    /* Accelerometer low-pass filter LPF2 selection, default: 0
     * Only valid whe xl_hp_slope_xl_en is set to 0
     * 0: disable
     * 1: enable
     */
    LSM6DS3TR_C_LPF2_XL_EN_e lpf2_xl_en_v;
    /* Accelerometer slope filter / high-pass filter selection, default: 0
     * 0: low-pass path
     * 1: high-pass path
     */
    LSM6DS3TR_C_HP_SLOPE_XL_EN_e hp_slope_xl_en_v;
    /* Accelerometer LPF2 and high-pass filter configuration and cutoff setting, default: 0
     * LPF2 bandwith, only when hp_slope_xl_en_v is set to 0 and lpf2_xl_en_v is set to 1:
     * 0: ODR/50
     * 1: ODR/100
     * 2: ODR/9
     * 3: ODR/400
     * slope fileter or high-pass bandwith, only when accel_hp_slope_xl_en is set to 1
     * 0: ODR/4, slope filter
     * 1: ODR/100, HPF
     * 2: ODR/9, HPF
     * 3: ODR/400, HPF
     */
    LSM6DS3TR_C_HPCF_XL_e hpcf_xl_v;
    /* Composite filter input selection. default: 0
     * only valid when hp_slope_xl_en_v is set to 0 and lpf2_xl_en is set to 1
     * 0: ODR/2 low pass filtered sent to composite filter (low latency)
     * 1: ODR/4 low pass filtered sent to composite filter (low noise)
     */
    LSM6DS3TR_C_INPUT_COMPOSITE_e input_composite_v;


    /* Gyroscope */
    /* Gyroscope full-scale range value (dps)
     * 0: +/- 125
     * 1: +/- 250
     * 2: +/- 500
     * 3: +/- 1000
     * 4: +/- 2000
     */
    LSM6DS3TR_C_FS_G_ABSTR_e fs_g_v;
    /* Gyroscope ODR value (Hz)
     * 0: power-down
     * 1: 12.5
     * 2: 26
     * 3: 52
     * 4: 104
     * 5: 208
     * 6: 416
     * 7: 833
     * 8: 1666
     * 9: 3332
     * 10: 6664
     */
    LSM6DS3TR_C_ODR_G_e odr_g_v;
    /* Gyroscope digital high-pass filter enable, default: 0
     * 0: disable
     * 1: enable
     */
    LSM6DS3TR_C_HP_EN_G_e hp_en_g_v;
    /* Gyroscope digital HP filter cutoff selection selection (Hz), default: 0
     * valid only when hp_en_g_v is set to 1
     * 0: 16m
     * 1: 65m
     * 2: 260m
     * 3: 1.04Hz
     */
    LSM6DS3TR_C_HPM_G_e hpm_g_v;
    /* gyroscope digital LPF1 enable, default: 0
     * 0: disable
     * 1: enable
     */
    LSM6DS3TR_C_LPF1_SEL_G_e lpf1_sel_g_v;
    /* Gyroscope digital LPF (LPF1) bandwidth selection (Hz)
     * Depends on odr_g and only valid when lpf1_sel_g_v is set to 1
     ***    800   1.6k    3.3k    6.6k
     * 0:   245   315     343     351
     * 1:   195   224     234     237
     * 2:   155   168     172     173
     * 3:   293   505     925     937
     */
    LSM6DS3TR_C_FTYPE_e ftype_v;

    /* FIFO */
    /* FIFO threshold value. 0 to 2047 16-bit samples */
    uint16_t fifo_threshold_v;
    /* Gyro FIFO (first data set) decimation setting, default: 0
     * 0: Gyro sensor not in FIFO
     * 1: No decimation
     * 2: Decimation with factor 2
     * 3: Decimation with factor 3
     * 4: Decimation with factor 4
     * 5: Decimation with factor 8
     * 6: Decimation with factor 16
     * 7: Decimation with factor 32
     */
    LSM6DS3TR_C_DEC_FIFO_GYRO_e  dec_fifo_gyro_v;
    /* Accelerometer FIFO (second data set) decimation setting, default: 0
     * 0: Accelerometer sensor not in FIFO
     * 1: No decimation
     * 2: Decimation with factor 2
     * 3: Decimation with factor 3
     * 4: Decimation with factor 4
     * 5: Decimation with factor 8
     * 6: Decimation with factor 16
     * 7: Decimation with factor 32
     */
    LSM6DS3TR_C_DEC_FIFO_XL_e  dec_fifo_xl_v;
    /* FIFO ODR selection
     * If the device is working at an ODR slower than the one selected,
     * FIFO ODR is limited to that ODR value.
     * 0: FIFO disabled
     * 1: 12.5 Hz
     * 2: 26 Hz
     * 3: 52 Hz
     * 4: 104 Hz
     * 5: 208 Hz
     * 6: 416 Hz
     * 7: 833 Hz
     * 8: 1.66 kHz
     * 9: 3.33 kHz
     * 10: 6.66 kHz
     */
    LSM6DS3TR_C_ODR_FIFO_e  odr_fifo_v;
    /* FIFO mode selection
     * 0: Bypass mode. FIFO disabled
     * 1: FIFO mode. Stops collecting data when FIFO is full
     * 3: Continuous mode until trigger is deasserted, then FIFO mode
     * 4: Bypass mode until trigger is deasserted, then Continuous mode
     * 6: Continuous mode. If the FIFO is full, the new sample overwrites the older one
     */
    LSM6DS3TR_C_FIFO_MODE_e fifo_mode_v;

    /* others */
    /* FIFO threshold interrupt on INT1 pin
     * 0: diable
     * 1: enable
     */
    LSM6DS3TR_C_INT1_FTH_e int1_fth_v;
    /* Software reset
     * 0: normal mode
     * 1: reset all registers to default values
     */
    LSM6DS3TR_C_SW_RESET_e sw_reset_v;
    /* Block data update (BDU) enable.
     * 0: continuous update
     * 1: output registers not updated until MSB and LSB have been read
     */

    LSM6DS3TR_C_BDU_e bdu_v;
    /* Enable basic interrupts (6D, tap, double tap, free fall, wake up)
     * 0: disable
     * 1: enable
     */
    LSM6DS3TR_C_INTERRUPTS_ENABLE_e interrupts_enable_v;
    /* HPF or SLOPE filter selection on wake-up and Activity/Inactivity functions. Refer to
     * Default value: 0
     * 0: SLOPE filter applied; 1: HPF applied
     */
    LSM6DS3TR_C_SLOPE_FDS_e slope_fds_v;
    /* Tap detection on X axis enable
     * 0: disable
     * 1: enable
     */
    LSM6DS3TR_C_TAP_X_EN_e tap_x_en_v;
    /* Tap detection on Y axis enable
     * 0: disable
     * 1: enable
     */
    LSM6DS3TR_C_TAP_Y_EN_e tap_y_en_v;
    /* Tap detection on Z axis enable
     * 0: disable
     * 1: enable
     */
    LSM6DS3TR_C_TAP_Z_EN_e tap_z_en_v;
    /* Latch interrupt request
     * 0: interrupt request not latched
     * 1: interrupt request latched
     */
    LSM6DS3TR_C_LIR_e lir_v;
    /* Threshold for tap recognition.
     * 1 LSB corresponds to FS_XL/2^5
     * max value is 2**5 - 1 = 31
     */
    uint8_t tap_ths_v;
    /* Expected quiet time after a tap detection
     * Quiet time is the time after the first detected tap in which there must not be any
     * overthreshold event. The default value of these bits is 00b which corresponds to
     * 2*ODR_XL time. If the QUIET[1:0] bits are set to a different value, 1LSB
     * corresponds to 4*ODR_XL time
     */
    LSM6DS3TR_C_QUIET_e quiet_v;
    /* Maximum duration of overthreshold event.
     * Maximum duration is the maximum time of an overthreshold signal detection to be
     * recognized as a tap event. The default value of these bits is 00b which corresponds
     * to 4*ODR_XL time. If the SHOCK[1:0] bits are set to a different value, 1LSB
     * corresponds to 8*ODR_XL time.
     */
    LSM6DS3TR_C_SHOCK_e shock_v;
    /* Single and double-tap detection enable
     * 0: only single-tap detection enabled
     * 1: both single and double-tap detection enabled
     */
    LSM6DS3TR_C_SINGLE_DOUBLE_TAP_e single_double_tap_v;
    /* Threshold for wakeup. 
     * 1 LSB corresponds to FS_XL/2^6 max value is 2**6 - 1 = 63
     */
    uint8_t wk_ths_v;
    /* Wake up duration event.
     * The filtered samples must be greater than wk_ths_v
     * for this duration to be considered as a wake up 
     * event. 1LSB = 1 ODR_time
     */
    LSM6DS3TR_C_WAKE_DUR_e wake_dur_v;
    /* Single-tap recongnition routing on INT1 pin
     * 0: routing of single-tap event to INT1 pin disabled
     * 1: routing of single-tap event to INT1 pin enabled
     */
    LSM6DS3TR_C_INT1_SINGLE_TAP_e int1_single_tap_v;
    /* Routing of wakeup event on INT1. Default value: 0
     * 0: routing of wakeup event on INT1 disabled;
     * 1: routing of wakeup event on INT1 enabled
     */
    LSM6DS3TR_C_INT1_WU_e int1_wu_v;
    /* Double-tap recongnition routing on INT1 pin
     * 0: routing of double-tap event to INT1 pin disabled
     * 1: routing of double-tap event to INT1 pin enabled
     */
    LSM6DS3TR_C_INT1_DOUBLE_TAP_e int1_double_tap_v;
} _imu_field_value_s;

typedef struct _imu_config {
    _imu_field_value_s field_v;
    uint8_t temp_enabled;
    uint16_t temp_sensitivity;
    /* compensation matrix (A^1/2) */
    float A_comp[3][3];
    float bias[3];
    /* Interrupt Eanble Flags
     * the bits in the flag indicates which interrupt is enabled
     * and routed to the INT1 pin
     */
    atomic_t interrupt_mode_flags;
} _imu_config_s;


/*--------------------------------------------------------------------------------------------------
 * PRIVATE CONSTANTS & VARIABLES
 *------------------------------------------------------------------------------------------------*/
static bool _imu_init = false;
static atomic_t _imu_motion_int_enable = ATOMIC_INIT(0);

static const _imu_fields_s _imu_fields = {

    /* Accelerometer */
    .odr_xl_f            = {.reg_addr = LSM6DS3TR_C_CTRL1_XL, .field_start = 4, .field_len = 4},
    .fs_xl_f             = {.reg_addr = LSM6DS3TR_C_CTRL1_XL, .field_start = 2, .field_len = 2},
    .lpf1_bw_sel_f       = {.reg_addr = LSM6DS3TR_C_CTRL1_XL, .field_start = 1, .field_len = 1},
    .lpf2_xl_en_f        = {.reg_addr = LSM6DS3TR_C_CTRL8_XL, .field_start = 7, .field_len = 1},
    .hp_slope_xl_en_f    = {.reg_addr = LSM6DS3TR_C_CTRL8_XL, .field_start = 2, .field_len = 1},
    .hpcf_xl_f           = {.reg_addr = LSM6DS3TR_C_CTRL8_XL, .field_start = 5, .field_len = 2},
    .input_composite_f   = {.reg_addr = LSM6DS3TR_C_CTRL8_XL, .field_start = 3, .field_len = 1},

    /* Gyroscope */
    .fs_125_f            = {.reg_addr = LSM6DS3TR_C_CTRL2_G, .field_start = 1, .field_len = 1},
    .fs_g_f              = {.reg_addr = LSM6DS3TR_C_CTRL2_G, .field_start = 2, .field_len = 2},
    .odr_g_f             = {.reg_addr = LSM6DS3TR_C_CTRL2_G, .field_start = 4, .field_len = 4},
    .hp_en_g_f           = {.reg_addr = LSM6DS3TR_C_CTRL7_G, .field_start = 6, .field_len = 1},
    .hpm_g_f             = {.reg_addr = LSM6DS3TR_C_CTRL7_G, .field_start = 4, .field_len = 2},
    .lpf1_sel_g_f        = {.reg_addr = LSM6DS3TR_C_CTRL4_C, .field_start = 1, .field_len = 1},
    .ftype_f             = {.reg_addr = LSM6DS3TR_C_CTRL6_C, .field_start = 0, .field_len = 2},

    /* FIFO */
    .fifo_thresh_low_f   = {.reg_addr = LSM6DS3TR_C_FIFO_CTRL1, .field_start = 0, .field_len = 8},
    .fifo_thresh_high_f  = {.reg_addr = LSM6DS3TR_C_FIFO_CTRL2, .field_start = 0, .field_len = 3},
    .dec_fifo_gyro_f     = {.reg_addr = LSM6DS3TR_C_FIFO_CTRL3, .field_start = 3, .field_len = 3},
    .dec_fifo_xl_f       = {.reg_addr = LSM6DS3TR_C_FIFO_CTRL3, .field_start = 0, .field_len = 3},
    .odr_fifo_f          = {.reg_addr = LSM6DS3TR_C_FIFO_CTRL5, .field_start = 3, .field_len = 4},
    .fifo_mode_f         = {.reg_addr = LSM6DS3TR_C_FIFO_CTRL5, .field_start = 0, .field_len = 3},

    /* Others */
    .int1_fth_f          = {.reg_addr = LSM6DS3TR_C_INT1_CTRL,   .field_start = 3, .field_len = 1},
    .sw_reset_f          = {.reg_addr = LSM6DS3TR_C_CTRL3_C,     .field_start = 0, .field_len = 1},
    .bdu_f               = {.reg_addr = LSM6DS3TR_C_CTRL3_C,     .field_start = 6, .field_len = 1},
    .interrupts_enable_f = {.reg_addr = LSM6DS3TR_C_TAP_CFG,     .field_start = 7, .field_len = 1},
    .slope_fds_f         = {.reg_addr = LSM6DS3TR_C_TAP_CFG,     .field_start = 4, .field_len = 1},
    .tap_x_en_f          = {.reg_addr = LSM6DS3TR_C_TAP_CFG,     .field_start = 3, .field_len = 1},
    .tap_y_en_f          = {.reg_addr = LSM6DS3TR_C_TAP_CFG,     .field_start = 2, .field_len = 1},
    .tap_z_en_f          = {.reg_addr = LSM6DS3TR_C_TAP_CFG,     .field_start = 1, .field_len = 1},
    .lir_f               = {.reg_addr = LSM6DS3TR_C_TAP_CFG,     .field_start = 0, .field_len = 1},
    .tap_ths_f           = {.reg_addr = LSM6DS3TR_C_TAP_THS_6D,  .field_start = 0, .field_len = 5},
    .quiet_f             = {.reg_addr = LSM6DS3TR_C_INT_DUR2,    .field_start = 2, .field_len = 2},
    .shock_f             = {.reg_addr = LSM6DS3TR_C_INT_DUR2,    .field_start = 0, .field_len = 2},
    .single_double_tap_f = {.reg_addr = LSM6DS3TR_C_WAKE_UP_THS, .field_start = 7, .field_len = 1},
    .wk_ths_f            = {.reg_addr = LSM6DS3TR_C_WAKE_UP_THS, .field_start = 0, .field_len = 6},
    .wake_dur_f          = {.reg_addr = LSM6DS3TR_C_WAKE_UP_DUR, .field_start = 5, .field_len = 2},
    .int1_single_tap_f   = {.reg_addr = LSM6DS3TR_C_MD1_CFG,     .field_start = 6, .field_len = 1},
    .int1_wu_f           = {.reg_addr = LSM6DS3TR_C_MD1_CFG,     .field_start = 5, .field_len = 1},
    .int1_double_tap_f   = {.reg_addr = LSM6DS3TR_C_MD1_CFG,     .field_start = 3, .field_len = 1},
};

/* default field values */
static const _imu_field_value_s _imu_field_v_default = {
    /* Accelerometer */
    .fs_xl_v             = LSM6DS3TR_C_FS_XL_2g,
    .odr_xl_v            = LSM6DS3TR_C_ODR_XL_416Hz,
    .lpf1_bw_sel_v       = LSM6DS3TR_C_LPF1_BW_SEL_ODR_DIV_4,
    .lpf2_xl_en_v        = LSM6DS3TR_C_LPF2_XL_EN_DISABLED,
    .hp_slope_xl_en_v    = LSM6DS3TR_C_HP_SLOPE_XL_EN_DISABLED,
    .hpcf_xl_v           = LSM6DS3TR_C_HPCF_XL_00,
    .input_composite_v   = LSM6DS3TR_C_INPUT_COMPOSITE_LOW_LATENCY,

    /* Gyroscope */
    .fs_g_v              = LSM6DS3TR_C_FS_G_ABSTR_250dps,
    .odr_g_v             = LSM6DS3TR_C_ODR_G_416Hz,
    .hp_en_g_v           = LSM6DS3TR_C_HP_EN_G_DISABLE,
    .hpm_g_v             = LSM6DS3TR_C_HPM_G_16mHz,
    .lpf1_sel_g_v        = LSM6DS3TR_C_LPF1_SEL_G_DISABLE,
    .ftype_v             = LSM6DS3TR_C_FTYPE_0,

    /* FIFO */
    .fifo_threshold_v    = 0,
    .dec_fifo_gyro_v     = LSM6DS3TR_C_DEC_FIFO_GYRO_DATA_NOT_IN_FIFO,
    .dec_fifo_xl_v       = LSM6DS3TR_C_DEC_FIFO_XL_DATA_NOT_IN_FIFO,
    .odr_fifo_v          = LSM6DS3TR_C_ODR_FIFO_DISABLE,
    .fifo_mode_v         = LSM6DS3TR_C_FIFO_MODE_BYPASS,

    /* Others */
    .int1_fth_v          = LSM6DS3TR_C_INT1_FTH_DISABLED,
    .sw_reset_v          = LSM6DS3TR_C_SW_RESET_NORMAL_MODE,
    .bdu_v               = LSM6DS3TR_C_BDU_CONTINUOUS,
    .interrupts_enable_v = LSM6DS3TR_C_INTERRUPTS_DISABLE,
    .tap_x_en_v          = LSM6DS3TR_C_TAP_X_EN_DISABLED,
    .tap_y_en_v          = LSM6DS3TR_C_TAP_Y_EN_DISABLED,
    .tap_z_en_v          = LSM6DS3TR_C_TAP_Z_EN_DISABLED,
    .lir_v               = LSM6DS3TR_C_LIR_DISABLED,
    .tap_ths_v           = 0,
    .quiet_v             = LSM6DS3TR_C_QUIET_2_OVER_ODR_XL,
    .shock_v             = LSM6DS3TR_C_SHOCK_4_OVER_ODR_XL,
    .single_double_tap_v = LSM6DS3TR_C_SINGLE_DOUBLE_TAP_SINGLE_ONLY,
    .wk_ths_v            = 2,
    .wake_dur_v          = LSM6DS3TR_C_WAKE_DUR_1_ODR_XL,
    .int1_single_tap_v   = LSM6DS3TR_C_INT1_SINGLE_TAP_DISABLED,
    .int1_double_tap_v   = LSM6DS3TR_C_INT1_DOUBLE_TAP_DISABLED
};

static _imu_config_s _imu_cfg = {
    .temp_enabled = 1,
    .A_comp = { {1.0f, 0.0f, 0.0f},
                {0.0f, 1.0f, 0.0f},
                {0.0f, 0.0f, 1.0f},
    },
    .bias = {0.0f},
    .interrupt_mode_flags = ATOMIC_INIT(0)
};

static IMU_cb_t _imu_wakeup_cb         = NULL;
static bool     _imu_data_dump         = false;
static bool     _imu_data_dump_hex     = false;
static uint32_t _imu_data_dump_time_ms = 0;

/* variables for various statistics */
static atomic_t _interrupt_count = ATOMIC_INIT(0);
static bool _imu_statistic_dump = false;
static atomic_t _imu_fifo_is_set = ATOMIC_INIT(0);

static volatile IMU_int_source_e _imu_int_source = IMU_INT_SOURCE_NONE;

/*--------------------------------------------------------------------------------------------------
 * PUBLIC VARIABLE DEFINITIOPNS (allocates memory)
 *------------------------------------------------------------------------------------------------*/
volatile bool busy_wait_active = false;
volatile uint32_t imu_task_counter = 0;
volatile uint32_t imu_task_during_busy_wait_counter = 0;
volatile uint32_t _interface_task_counter = 0;
volatile uint32_t _interface_task_during_i2c_counter = 0;


/*--------------------------------------------------------------------------------------------------
 * PRIVATE FUNCTION PROTOTYPES
 *------------------------------------------------------------------------------------------------*/
static void    _imu_task(void *p1, void *p2, void *p3);
static void    _imu_interrupt_handler_task(void *p1, void *p2, void *p3);
static ERRNO_e _imu_who_am_i(void);
static ERRNO_e _imu_cfg_init(void);
static float   _imu_xl_raw_to_float(int16_t raw);
static float   _imu_gyro_raw_to_float(int16_t raw);
static void    _imu_config_write (void);
static void    _imu_isr(void);


/*--------------------------------------------------------------------------------------------------
 * PUBLIC FUNCTION DEFINITIOPNS
 *------------------------------------------------------------------------------------------------*/
ERRNO_e IMU_init (void)
{
    ERRNO_e err;

    if (_imu_init) {
        return ERRNO_ALREADY_INITIALIZED;
    }

    if (I2C_init (I2C_CH_1) < ERRNO_SUCCESS) {
        return ERRNO_I2C_FAILURE;
    }

    /* Initialize GPIO for use with IMU interrupt pin. */
    if (GPIO_init () < ERRNO_SUCCESS) {
        return ERRNO_GPIO_FAILURE;
    }

    /* Perform an ID check. */
    while (_imu_who_am_i() != ERRNO_SUCCESS) {
        static uint32_t cnt = 0;
        k_busy_wait (10000);

        if (cnt++ == 50)
        {
            PRINT (ERROR, "IMU WHO AM I failure\r\n");
            return ERRNO_I2C_FAILURE;
        }
    }

    /* Set init flag before IMU_reset so it doesn't fail. */
    _imu_init = true;

    err = IMU_reset();
    if (err != ERRNO_SUCCESS) {
        return ERRNO_I2C_FAILURE;
    }

    /* Register callback for IMU interrupts. */
    if (GPIO_cfg (GPIO_PIN_IMU_INT, GPIO_DIRECTION_IN, GPIO_PULLDOWN, GPIO_INIT_LOW, GPIO_INT_RISING_EDGE, _imu_isr)) {
        PRINT(CRITICAL, "Failure to register IMU interrupt callback.\r\n");
        return ERRNO_GPIO_FAILURE;
    }

    /* Configure LSM6DS3 with default settings. */
    err = _imu_cfg_init();
    if (err != ERRNO_SUCCESS) {
        return err;
    }

    return ERRNO_SUCCESS;
}

ERRNO_e IMU_int_cb_register(IMU_int_source_e src, IMU_cb_t cb)
{
    if (!_imu_init) {
        return ERRNO_NOT_INITIALIZED;
    }

    switch (src)
    {
        case IMU_INT_SOURCE_WAKEUP:
            _imu_wakeup_cb = cb;
            break;

        default:
            return ERRNO_INVALID_PARAM;
    }

    return ERRNO_SUCCESS;
}

ERRNO_e IMU_gyro_enable(bool en)
{
    ERRNO_e err;

    if (!_imu_init) {
        return ERRNO_NOT_INITIALIZED;
    }

    if (en) {
        if (_imu_cfg.field_v.odr_g_v == LSM6DS3TR_C_ODR_G_POWER_DOWN) {
            _imu_cfg.field_v.odr_g_v = LSM6DS3TR_C_ODR_G_416Hz;
        }
        err = I2C_field_write(I2C_CH_1, LSM6DS3_I2C_DEV_ADDR,
                              _imu_fields.odr_g_f.reg_addr,
                              (uint8_t)_imu_cfg.field_v.odr_g_v,
                              _imu_fields.odr_g_f.field_start,
                              _imu_fields.odr_g_f.field_len);

    } else {
        _imu_cfg.field_v.odr_g_v = LSM6DS3TR_C_ODR_G_POWER_DOWN;
        err = I2C_field_write(I2C_CH_1, LSM6DS3_I2C_DEV_ADDR,
                              _imu_fields.odr_g_f.reg_addr,
                              (uint8_t)_imu_cfg.field_v.odr_g_v,
                              _imu_fields.odr_g_f.field_start,
                              _imu_fields.odr_g_f.field_len);

    }

    return err;
}

ERRNO_e IMU_is_gyro_enabled(bool *en)
{
    ERRNO_e err;
    uint8_t gyro_enabled;

    err = I2C_field_read(I2C_CH_1, LSM6DS3_I2C_DEV_ADDR,
                        _imu_fields.odr_g_f.reg_addr,
                        &gyro_enabled,
                        _imu_fields.odr_g_f.field_start,
                        _imu_fields.odr_g_f.field_len);

    *en = (bool) gyro_enabled;

    return err;
}

ERRNO_e IMU_set_fs_g(LSM6DS3TR_C_FS_G_ABSTR_e value)
{
    ERRNO_e err;
    uint8_t fs_125 = 0, fs_main = 0;

    if (!_imu_init) {
        return ERRNO_NOT_INITIALIZED;
    }

    switch (value) {
        case LSM6DS3TR_C_FS_G_ABSTR_125dps:  fs_125 = 1; fs_main = 0; break;
        case LSM6DS3TR_C_FS_G_ABSTR_250dps:  fs_main = 0; break;
        case LSM6DS3TR_C_FS_G_ABSTR_500dps:  fs_main = 1; break;
        case LSM6DS3TR_C_FS_G_ABSTR_1000dps: fs_main = 2; break;
        case LSM6DS3TR_C_FS_G_ABSTR_2000dps: fs_main = 3; break;
        default: return ERRNO_INVALID_PARAM;
    }

    /* update runtime value */
    _imu_cfg.field_v.fs_g_v = value;
    err = I2C_field_write(I2C_CH_1, LSM6DS3_I2C_DEV_ADDR,
                          _imu_fields.fs_125_f.reg_addr,
                          fs_125,
                          _imu_fields.fs_125_f.field_start,
                          _imu_fields.fs_125_f.field_len);
    if (err == ERRNO_SUCCESS) {
        err = I2C_field_write(I2C_CH_1, LSM6DS3_I2C_DEV_ADDR,
                              _imu_fields.fs_g_f.reg_addr,
                              fs_main,
                              _imu_fields.fs_g_f.field_start,
                              _imu_fields.fs_g_f.field_len);
    }

    return err;
}

ERRNO_e IMU_get_fs_g(LSM6DS3TR_C_FS_G_ABSTR_e *value)
{
    ERRNO_e err;
    uint8_t fs_125, fs_main;
    err = I2C_field_read(I2C_CH_1,
                         LSM6DS3_I2C_DEV_ADDR,
                         _imu_fields.fs_125_f.reg_addr,
                         &fs_125,
                         _imu_fields.fs_125_f.field_start,
                         _imu_fields.fs_125_f.field_len);

    if (err == ERRNO_SUCCESS) {
        err = I2C_field_read(I2C_CH_1,
                             LSM6DS3_I2C_DEV_ADDR,
                             _imu_fields.fs_g_f.reg_addr,
                             &fs_main,
                             _imu_fields.fs_g_f.field_start,
                             _imu_fields.fs_g_f.field_len);
        if (err == ERRNO_SUCCESS) {
            if (fs_125) {
                *value = LSM6DS3TR_C_FS_G_ABSTR_125dps;
            } else {
                switch(fs_main) {
                    case 0: *value = LSM6DS3TR_C_FS_G_ABSTR_250dps;  break;
                    case 1: *value = LSM6DS3TR_C_FS_G_ABSTR_500dps;  break;
                    case 2: *value = LSM6DS3TR_C_FS_G_ABSTR_1000dps; break;
                    case 3: *value = LSM6DS3TR_C_FS_G_ABSTR_2000dps; break;
                }
            }
            /* update runtime value */
            _imu_cfg.field_v.fs_g_v = *value;
        }
    }

    return err;
}

ERRNO_e IMU_set_odr_g(LSM6DS3TR_C_ODR_G_e value)
{
    ERRNO_e err;

    if (!_imu_init) {
        return ERRNO_NOT_INITIALIZED;
    }

    switch (value) {
        case LSM6DS3TR_C_ODR_G_POWER_DOWN: break;
        case LSM6DS3TR_C_ODR_G_12_5Hz:     break;
        case LSM6DS3TR_C_ODR_G_26Hz:       break;
        case LSM6DS3TR_C_ODR_G_52Hz:       break;
        case LSM6DS3TR_C_ODR_G_104Hz:      break;
        case LSM6DS3TR_C_ODR_G_208Hz:      break;
        case LSM6DS3TR_C_ODR_G_416Hz:      break;
        case LSM6DS3TR_C_ODR_G_833Hz:      break;
        case LSM6DS3TR_C_ODR_G_1666Hz:     break;
        case LSM6DS3TR_C_ODR_G_3332Hz:     break;
        case LSM6DS3TR_C_ODR_G_6664Hz:     break;
        default: return ERRNO_INVALID_PARAM;
    }

    _imu_cfg.field_v.odr_g_v = value;
    err = I2C_field_write(I2C_CH_1,
                          LSM6DS3_I2C_DEV_ADDR,
                          _imu_fields.odr_g_f.reg_addr,
                          (uint8_t)value,
                          _imu_fields.odr_g_f.field_start,
                          _imu_fields.odr_g_f.field_len);
    return err;
}

ERRNO_e IMU_get_odr_g(LSM6DS3TR_C_ODR_G_e *value)
{
    ERRNO_e err;
    uint8_t raw;

    err = I2C_field_read(I2C_CH_1,
                         LSM6DS3_I2C_DEV_ADDR,
                         _imu_fields.odr_g_f.reg_addr,
                         &raw,
                         _imu_fields.odr_g_f.field_start,
                         _imu_fields.odr_g_f.field_len);

    *value = (LSM6DS3TR_C_ODR_G_e)raw;
    _imu_cfg.field_v.odr_g_v = *value;
    return err;
}

ERRNO_e IMU_set_hp_en_g(LSM6DS3TR_C_HP_EN_G_e value)
{
    ERRNO_e err;

    if (!_imu_init) {
        return ERRNO_NOT_INITIALIZED;
    }

    switch (value) {
        case LSM6DS3TR_C_HP_EN_G_DISABLE: break;
        case LSM6DS3TR_C_HP_EN_G_ENABLE:  break;
        default: return ERRNO_INVALID_PARAM;
    }

    if (_imu_cfg.field_v.hp_en_g_v != value) {
        _imu_cfg.field_v.hp_en_g_v = value;
    }

    err = I2C_field_write(I2C_CH_1, LSM6DS3_I2C_DEV_ADDR,
                          _imu_fields.hp_en_g_f.reg_addr,
                          (uint8_t)value,
                          _imu_fields.hp_en_g_f.field_start,
                          _imu_fields.hp_en_g_f.field_len);
    return err;
}

ERRNO_e IMU_get_hp_en_g(LSM6DS3TR_C_HP_EN_G_e *value)
{
    ERRNO_e err;
    uint8_t raw;

    err = I2C_field_read(I2C_CH_1, LSM6DS3_I2C_DEV_ADDR,
                         _imu_fields.hp_en_g_f.reg_addr,
                         &raw,
                         _imu_fields.hp_en_g_f.field_start,
                         _imu_fields.hp_en_g_f.field_len);
    *value = (LSM6DS3TR_C_HP_EN_G_e)raw;

    if (_imu_cfg.field_v.hp_en_g_v != *value) {
        _imu_cfg.field_v.hp_en_g_v = *value;
    }

    return err;
}

ERRNO_e IMU_set_hpm_g(LSM6DS3TR_C_HPM_G_e value)
{
    ERRNO_e err;

    if (!_imu_init) {
        return ERRNO_NOT_INITIALIZED;
    }

    switch (value) {
        case LSM6DS3TR_C_HPM_G_16mHz:  break;
        case LSM6DS3TR_C_HPM_G_65mHz:  break;
        case LSM6DS3TR_C_HPM_G_260mHz: break;
        case LSM6DS3TR_C_HPM_G_1_04Hz: break;
        default: return ERRNO_INVALID_PARAM;
    }

    if (_imu_cfg.field_v.hpm_g_v != value) {
        _imu_cfg.field_v.hpm_g_v = value;
    }

    err = I2C_field_write(I2C_CH_1, LSM6DS3_I2C_DEV_ADDR,
                          _imu_fields.hpm_g_f.reg_addr,
                          (uint8_t)value,
                          _imu_fields.hpm_g_f.field_start,
                          _imu_fields.hpm_g_f.field_len);
    return err;
}

ERRNO_e IMU_get_hpm_g(LSM6DS3TR_C_HPM_G_e *value)
{
    ERRNO_e err;
    uint8_t raw;

    err = I2C_field_read(I2C_CH_1, LSM6DS3_I2C_DEV_ADDR,
                         _imu_fields.hpm_g_f.reg_addr,
                         &raw,
                         _imu_fields.hpm_g_f.field_start,
                         _imu_fields.hpm_g_f.field_len);
    *value = (LSM6DS3TR_C_HPM_G_e)raw;

    if (_imu_cfg.field_v.hpm_g_v != *value) {
        _imu_cfg.field_v.hpm_g_v = *value;
    }

    return err;
}

ERRNO_e IMU_set_lpf1_sel_g(LSM6DS3TR_C_LPF1_SEL_G_e value)
{
    ERRNO_e err;

    if (!_imu_init) {
        return ERRNO_NOT_INITIALIZED;
    }

    switch (value) {
        case LSM6DS3TR_C_LPF1_SEL_G_DISABLE: break;
        case LSM6DS3TR_C_LPF1_SEL_G_ENABLE:  break;
        default: return ERRNO_INVALID_PARAM;
    }

    if (_imu_cfg.field_v.lpf1_sel_g_v != value) {
        _imu_cfg.field_v.lpf1_sel_g_v = value;
    }

    err = I2C_field_write(I2C_CH_1, LSM6DS3_I2C_DEV_ADDR,
                          _imu_fields.lpf1_sel_g_f.reg_addr,
                          (uint8_t)value,
                          _imu_fields.lpf1_sel_g_f.field_start,
                          _imu_fields.lpf1_sel_g_f.field_len);
    return err;
}

ERRNO_e IMU_get_lpf1_sel_g(LSM6DS3TR_C_LPF1_SEL_G_e *value)
{
    ERRNO_e err;
    uint8_t raw;

    err = I2C_field_read(I2C_CH_1, LSM6DS3_I2C_DEV_ADDR,
                         _imu_fields.lpf1_sel_g_f.reg_addr,
                         &raw,
                         _imu_fields.lpf1_sel_g_f.field_start,
                         _imu_fields.lpf1_sel_g_f.field_len);
    *value = (LSM6DS3TR_C_LPF1_SEL_G_e)raw;

    if (_imu_cfg.field_v.lpf1_sel_g_v != *value) {
        _imu_cfg.field_v.lpf1_sel_g_v = *value;
    }

    return err;
}

ERRNO_e IMU_set_ftype(LSM6DS3TR_C_FTYPE_e value)
{
    ERRNO_e err;

    if (!_imu_init) {
        return ERRNO_NOT_INITIALIZED;
    }

    switch (value) {
        case LSM6DS3TR_C_FTYPE_0: break;
        case LSM6DS3TR_C_FTYPE_1: break;
        case LSM6DS3TR_C_FTYPE_2: break;
        case LSM6DS3TR_C_FTYPE_3: break;
        default: return ERRNO_INVALID_PARAM;
    }

    if (_imu_cfg.field_v.ftype_v != value) {
        _imu_cfg.field_v.ftype_v = value;
    }

    err = I2C_field_write(I2C_CH_1, LSM6DS3_I2C_DEV_ADDR,
                          _imu_fields.ftype_f.reg_addr,
                          (uint8_t)value,
                          _imu_fields.ftype_f.field_start,
                          _imu_fields.ftype_f.field_len);
    return err;
}

ERRNO_e IMU_get_ftype(LSM6DS3TR_C_FTYPE_e *value)
{
    ERRNO_e err;
    uint8_t raw;

    err = I2C_field_read(I2C_CH_1, LSM6DS3_I2C_DEV_ADDR,
                         _imu_fields.ftype_f.reg_addr,
                         &raw,
                         _imu_fields.ftype_f.field_start,
                         _imu_fields.ftype_f.field_len);
    *value = (LSM6DS3TR_C_FTYPE_e)raw;

    if (_imu_cfg.field_v.ftype_v != *value) {
        _imu_cfg.field_v.ftype_v = *value;
    }

    return err;
}

ERRNO_e IMU_xl_enable(bool en)
{
    ERRNO_e err;

    if (!_imu_init) {
        return ERRNO_NOT_INITIALIZED;
    }

    if (en) {
        if (_imu_cfg.field_v.odr_xl_v == LSM6DS3TR_C_ODR_XL_POWER_DOWN) {
            _imu_cfg.field_v.odr_xl_v = LSM6DS3TR_C_ODR_XL_416Hz;
        }
        err = I2C_field_write(I2C_CH_1, LSM6DS3_I2C_DEV_ADDR,
                              _imu_fields.odr_xl_f.reg_addr,
                              (uint8_t)_imu_cfg.field_v.odr_xl_v,
                              _imu_fields.odr_xl_f.field_start,
                              _imu_fields.odr_xl_f.field_len);
    } else {
        _imu_cfg.field_v.odr_xl_v = LSM6DS3TR_C_ODR_XL_POWER_DOWN;
        err = I2C_field_write(I2C_CH_1, LSM6DS3_I2C_DEV_ADDR,
                              _imu_fields.odr_xl_f.reg_addr,
                              (uint8_t)_imu_cfg.field_v.odr_xl_v,
                              _imu_fields.odr_xl_f.field_start,
                              _imu_fields.odr_xl_f.field_len);
    }

    return err;
}

ERRNO_e IMU_is_xl_enabled(bool *en)
{
    uint8_t xl_enabled;
    ERRNO_e err;

    err = I2C_field_read(I2C_CH_1, LSM6DS3_I2C_DEV_ADDR,
                        _imu_fields.odr_xl_f.reg_addr,
                        &xl_enabled,
                        _imu_fields.odr_xl_f.field_start,
                        _imu_fields.odr_xl_f.field_len);

    return (bool) xl_enabled;
}

ERRNO_e IMU_set_fs_xl(LSM6DS3TR_C_FS_XL_e value)
{
    ERRNO_e err;

    if (!_imu_init) {
        return ERRNO_NOT_INITIALIZED;
    }

    switch (value) {
        case LSM6DS3TR_C_FS_XL_2g:  break;
        case LSM6DS3TR_C_FS_XL_16g: break;
        case LSM6DS3TR_C_FS_XL_4g:  break;
        case LSM6DS3TR_C_FS_XL_8g:  break;
        default: return ERRNO_INVALID_PARAM;
    }

    /* update runtime value */
    _imu_cfg.field_v.fs_xl_v = value;

    err = I2C_field_write(I2C_CH_1,
                          LSM6DS3_I2C_DEV_ADDR,
                          _imu_fields.fs_xl_f.reg_addr,
                          (uint8_t)value,
                          _imu_fields.fs_xl_f.field_start,
                          _imu_fields.fs_xl_f.field_len);

    return err;
}

ERRNO_e IMU_get_fs_xl(LSM6DS3TR_C_FS_XL_e *value)
{
    ERRNO_e err;
    uint8_t raw;

    err = I2C_field_read(I2C_CH_1,
                         LSM6DS3_I2C_DEV_ADDR,
                         _imu_fields.fs_xl_f.reg_addr,
                         &raw,
                         _imu_fields.fs_xl_f.field_start,
                         _imu_fields.fs_xl_f.field_len);
    *value = (LSM6DS3TR_C_FS_XL_e)raw;
    _imu_cfg.field_v.fs_xl_v = *value;

    return err;
}

ERRNO_e IMU_set_odr_xl(LSM6DS3TR_C_ODR_XL_e value)
{
    ERRNO_e err;

    if (!_imu_init) {
        return ERRNO_NOT_INITIALIZED;
    }

    switch (value) {
        case LSM6DS3TR_C_ODR_XL_POWER_DOWN: break;
        case LSM6DS3TR_C_ODR_XL_12_5Hz:     break;
        case LSM6DS3TR_C_ODR_XL_26Hz:       break;
        case LSM6DS3TR_C_ODR_XL_52Hz:       break;
        case LSM6DS3TR_C_ODR_XL_104Hz:      break;
        case LSM6DS3TR_C_ODR_XL_208Hz:      break;
        case LSM6DS3TR_C_ODR_XL_416Hz:      break;
        case LSM6DS3TR_C_ODR_XL_833Hz:      break;
        case LSM6DS3TR_C_ODR_XL_1666Hz:     break;
        case LSM6DS3TR_C_ODR_XL_3332Hz:     break;
        case LSM6DS3TR_C_ODR_XL_6664Hz:     break;
        case LSM6DS3TR_C_ODR_XL_1_6Hz:      break;
        default: return ERRNO_INVALID_PARAM;
    }

    _imu_cfg.field_v.odr_xl_v = value;
    err = I2C_field_write(I2C_CH_1,
                          LSM6DS3_I2C_DEV_ADDR,
                          _imu_fields.odr_xl_f.reg_addr,
                          (uint8_t)value,
                          _imu_fields.odr_xl_f.field_start,
                          _imu_fields.odr_xl_f.field_len);
    return err;
}

ERRNO_e IMU_get_odr_xl(LSM6DS3TR_C_ODR_XL_e *value)
{
    ERRNO_e err;
    uint8_t raw;

    err = I2C_field_read(I2C_CH_1,
                         LSM6DS3_I2C_DEV_ADDR,
                         _imu_fields.odr_xl_f.reg_addr,
                         &raw,
                         _imu_fields.odr_xl_f.field_start,
                         _imu_fields.odr_xl_f.field_len);

    *value = (LSM6DS3TR_C_ODR_XL_e)raw;
    _imu_cfg.field_v.odr_xl_v = *value;
    return err;
}

ERRNO_e IMU_set_lpf1_bw_sel(LSM6DS3TR_C_LPF1_BW_SEL_e value)
{
    ERRNO_e err;

    if (!_imu_init) {
        return ERRNO_NOT_INITIALIZED;
    }

    switch (value) {
        case LSM6DS3TR_C_LPF1_BW_SEL_ODR_DIV_2: break;
        case LSM6DS3TR_C_LPF1_BW_SEL_ODR_DIV_4: break;
        default: return ERRNO_INVALID_PARAM;
    }

    _imu_cfg.field_v.lpf1_bw_sel_v = value;
    err = I2C_field_write(I2C_CH_1,
                          LSM6DS3_I2C_DEV_ADDR,
                          _imu_fields.lpf1_bw_sel_f.reg_addr,
                          (uint8_t)value,
                          _imu_fields.lpf1_bw_sel_f.field_start,
                          _imu_fields.lpf1_bw_sel_f.field_len);
    return err;
}

ERRNO_e IMU_get_lpf1_bw_sel(LSM6DS3TR_C_LPF1_BW_SEL_e *value)
{
    ERRNO_e err;
    uint8_t raw;

    err = I2C_field_read(I2C_CH_1,
                         LSM6DS3_I2C_DEV_ADDR,
                         _imu_fields.lpf1_bw_sel_f.reg_addr,
                         &raw,
                         _imu_fields.lpf1_bw_sel_f.field_start,
                         _imu_fields.lpf1_bw_sel_f.field_len);

    *value = (LSM6DS3TR_C_LPF1_BW_SEL_e)raw;
    _imu_cfg.field_v.lpf1_bw_sel_v = *value;
    return err;
}

ERRNO_e IMU_set_lpf2_xl_en(LSM6DS3TR_C_LPF2_XL_EN_e value)
{
    ERRNO_e err;

    if (!_imu_init) {
        return ERRNO_NOT_INITIALIZED;
    }

    switch (value) {
        case LSM6DS3TR_C_LPF2_XL_EN_DISABLED: break;
        case LSM6DS3TR_C_LPF2_XL_EN_ENABLED: break;
        default: return ERRNO_INVALID_PARAM;
    }

    _imu_cfg.field_v.lpf2_xl_en_v = value;
    err = I2C_field_write(I2C_CH_1,
                          LSM6DS3_I2C_DEV_ADDR,
                          _imu_fields.lpf2_xl_en_f.reg_addr,
                          (uint8_t)value,
                          _imu_fields.lpf2_xl_en_f.field_start,
                          _imu_fields.lpf2_xl_en_f.field_len);
    return err;
}

ERRNO_e IMU_get_lpf2_xl_en(LSM6DS3TR_C_LPF2_XL_EN_e *value)
{
    ERRNO_e err;
    uint8_t raw;

    err = I2C_field_read(I2C_CH_1,
                         LSM6DS3_I2C_DEV_ADDR,
                         _imu_fields.lpf2_xl_en_f.reg_addr,
                         &raw,
                         _imu_fields.lpf2_xl_en_f.field_start,
                         _imu_fields.lpf2_xl_en_f.field_len);

    *value = (LSM6DS3TR_C_LPF2_XL_EN_e)raw;
    _imu_cfg.field_v.lpf2_xl_en_v = *value;
    return err;
}

ERRNO_e IMU_set_hp_slope_xl_en(LSM6DS3TR_C_HP_SLOPE_XL_EN_e value)
{
    ERRNO_e err;

    if (!_imu_init) {
        return ERRNO_NOT_INITIALIZED;
    }

    switch (value) {
        case LSM6DS3TR_C_HP_SLOPE_XL_EN_DISABLED: break;
        case LSM6DS3TR_C_HP_SLOPE_XL_EN_ENABLED:  break;
        default: return ERRNO_INVALID_PARAM;
    }

    _imu_cfg.field_v.hp_slope_xl_en_v = value;
    err = I2C_field_write(I2C_CH_1,
                          LSM6DS3_I2C_DEV_ADDR,
                          _imu_fields.hp_slope_xl_en_f.reg_addr,
                          (uint8_t)value,
                          _imu_fields.hp_slope_xl_en_f.field_start,
                          _imu_fields.hp_slope_xl_en_f.field_len);
    return err;
}

ERRNO_e IMU_get_hp_slope_xl_en(LSM6DS3TR_C_HP_SLOPE_XL_EN_e *value)
{
    ERRNO_e err;
    uint8_t raw;

    err = I2C_field_read(I2C_CH_1,
                         LSM6DS3_I2C_DEV_ADDR,
                         _imu_fields.hp_slope_xl_en_f.reg_addr,
                         &raw,
                         _imu_fields.hp_slope_xl_en_f.field_start,
                         _imu_fields.hp_slope_xl_en_f.field_len);

    *value = (LSM6DS3TR_C_HP_SLOPE_XL_EN_e)raw;
    _imu_cfg.field_v.hp_slope_xl_en_v = *value;
    return err;
}

ERRNO_e IMU_set_hpcf_xl(LSM6DS3TR_C_HPCF_XL_e value)
{
    ERRNO_e err;

    if (!_imu_init) {
        return ERRNO_NOT_INITIALIZED;
    }

    switch (value) {
        case LSM6DS3TR_C_HPCF_XL_00: break;
        case LSM6DS3TR_C_HPCF_XL_01: break;
        case LSM6DS3TR_C_HPCF_XL_02: break;
        case LSM6DS3TR_C_HPCF_XL_03: break;
        default: return ERRNO_INVALID_PARAM;
    }

    _imu_cfg.field_v.hpcf_xl_v = value;
    err = I2C_field_write(I2C_CH_1,
                          LSM6DS3_I2C_DEV_ADDR,
                          _imu_fields.hpcf_xl_f.reg_addr,
                          (uint8_t)value,
                          _imu_fields.hpcf_xl_f.field_start,
                          _imu_fields.hpcf_xl_f.field_len);
    return err;
}

ERRNO_e IMU_get_hpcf_xl(LSM6DS3TR_C_HPCF_XL_e *value)
{
    ERRNO_e err;
    uint8_t raw;

    err = I2C_field_read(I2C_CH_1,
                         LSM6DS3_I2C_DEV_ADDR,
                         _imu_fields.hpcf_xl_f.reg_addr,
                         &raw,
                         _imu_fields.hpcf_xl_f.field_start,
                         _imu_fields.hpcf_xl_f.field_len);

    *value = (LSM6DS3TR_C_HPCF_XL_e)raw;
    _imu_cfg.field_v.hpcf_xl_v = *value;
    return err;
}

ERRNO_e IMU_set_input_composite(LSM6DS3TR_C_INPUT_COMPOSITE_e value)
{
    ERRNO_e err;

    if (!_imu_init) {
        return ERRNO_NOT_INITIALIZED;
    }

    switch (value) {
        case LSM6DS3TR_C_INPUT_COMPOSITE_LOW_LATENCY: break;
        case LSM6DS3TR_C_INPUT_COMPOSITE_LOW_NOISE:   break;
        default: return ERRNO_INVALID_PARAM;
    }

    _imu_cfg.field_v.input_composite_v = value;
    err = I2C_field_write(I2C_CH_1,
                          LSM6DS3_I2C_DEV_ADDR,
                          _imu_fields.input_composite_f.reg_addr,
                          (uint8_t)value,
                          _imu_fields.input_composite_f.field_start,
                          _imu_fields.input_composite_f.field_len);
    return err;
}

ERRNO_e IMU_get_input_composite(LSM6DS3TR_C_INPUT_COMPOSITE_e *value)
{
    ERRNO_e err;
    uint8_t raw;

    err = I2C_field_read(I2C_CH_1,
                         LSM6DS3_I2C_DEV_ADDR,
                         _imu_fields.input_composite_f.reg_addr,
                         &raw,
                         _imu_fields.input_composite_f.field_start,
                         _imu_fields.input_composite_f.field_len);

    *value = (LSM6DS3TR_C_INPUT_COMPOSITE_e)raw;
    _imu_cfg.field_v.input_composite_v = *value;
    return err;
}

ERRNO_e IMU_set_FTH(uint16_t value)
{
    ERRNO_e err;
    if (value > 2047) {
        return ERRNO_INVALID_PARAM;
    }
    _imu_cfg.field_v.fifo_threshold_v = value;
    uint16_t safe_val = value & 0x07FF;
    uint8_t low = safe_val & 0xFF;
    uint8_t high = safe_val >> 8;

    err = I2C_field_write(I2C_CH_1,
                          LSM6DS3_I2C_DEV_ADDR,
                          _imu_fields.fifo_thresh_low_f.reg_addr,
                          low,
                          _imu_fields.fifo_thresh_low_f.field_start,
                          _imu_fields.fifo_thresh_low_f.field_len);
    if (err == ERRNO_SUCCESS) {
        err = I2C_field_write(I2C_CH_1,
                              LSM6DS3_I2C_DEV_ADDR,
                              _imu_fields.fifo_thresh_high_f.reg_addr,
                              high,
                              _imu_fields.fifo_thresh_high_f.field_start,
                              _imu_fields.fifo_thresh_high_f.field_len);
    }

    return err;
}

ERRNO_e IMU_get_FTH(uint16_t *value)
{
    ERRNO_e err;
    uint8_t low, high;

    err = I2C_field_read(I2C_CH_1,
                         LSM6DS3_I2C_DEV_ADDR,
                         _imu_fields.fifo_thresh_low_f.reg_addr,
                         &low,
                         _imu_fields.fifo_thresh_low_f.field_start,
                         _imu_fields.fifo_thresh_low_f.field_len);
    if (err == ERRNO_SUCCESS) {
        err = I2C_field_read(I2C_CH_1,
                             LSM6DS3_I2C_DEV_ADDR,
                             _imu_fields.fifo_thresh_high_f.reg_addr,
                             &high,
                             _imu_fields.fifo_thresh_high_f.field_start,
                             _imu_fields.fifo_thresh_high_f.field_len);
    }
    *value = 0x07FF & ((uint16_t)low | ((uint16_t)high << 8));
    _imu_cfg.field_v.fifo_threshold_v = *value;

    return ERRNO_SUCCESS;
}

ERRNO_e IMU_set_dec_fifo_gyro(LSM6DS3TR_C_DEC_FIFO_GYRO_e value)
{
    ERRNO_e err;

    if (!_imu_init) {
        return ERRNO_NOT_INITIALIZED;
    }

    switch (value) {
        case LSM6DS3TR_C_DEC_FIFO_GYRO_DATA_NOT_IN_FIFO: break;
        case LSM6DS3TR_C_DEC_FIFO_GYRO_NO_DECIMATION:    break;
        case LSM6DS3TR_C_DEC_FIFO_GYRO_DECIMATION_BY_2:  break;
        case LSM6DS3TR_C_DEC_FIFO_GYRO_DECIMATION_BY_3:  break;
        case LSM6DS3TR_C_DEC_FIFO_GYRO_DECIMATION_BY_4:  break;
        case LSM6DS3TR_C_DEC_FIFO_GYRO_DECIMATION_BY_8:  break;
        case LSM6DS3TR_C_DEC_FIFO_GYRO_DECIMATION_BY_16: break;
        case LSM6DS3TR_C_DEC_FIFO_GYRO_DECIMATION_BY_32: break;
        default: return ERRNO_INVALID_PARAM;
    }

    _imu_cfg.field_v.dec_fifo_gyro_v = value;
    err = I2C_field_write(I2C_CH_1,
                          LSM6DS3_I2C_DEV_ADDR,
                          _imu_fields.dec_fifo_gyro_f.reg_addr,
                          (uint8_t)value,
                          _imu_fields.dec_fifo_gyro_f.field_start,
                          _imu_fields.dec_fifo_gyro_f.field_len);
    return err;
}

ERRNO_e IMU_get_dec_fifo_gyro(LSM6DS3TR_C_DEC_FIFO_GYRO_e *value)
{
    ERRNO_e err;
    uint8_t raw;

    err = I2C_field_read(I2C_CH_1,
                         LSM6DS3_I2C_DEV_ADDR,
                         _imu_fields.dec_fifo_gyro_f.reg_addr,
                         &raw,
                         _imu_fields.dec_fifo_gyro_f.field_start,
                         _imu_fields.dec_fifo_gyro_f.field_len);

    *value = (LSM6DS3TR_C_DEC_FIFO_GYRO_e)raw;
    _imu_cfg.field_v.dec_fifo_gyro_v = *value;
    return err;
}

ERRNO_e IMU_set_dec_fifo_xl(LSM6DS3TR_C_DEC_FIFO_XL_e value)
{
    ERRNO_e err;

    if (!_imu_init) {
        return ERRNO_NOT_INITIALIZED;
    }

    switch (value) {
        case LSM6DS3TR_C_DEC_FIFO_XL_DATA_NOT_IN_FIFO: break;
        case LSM6DS3TR_C_DEC_FIFO_XL_NO_DECIMATION:    break;
        case LSM6DS3TR_C_DEC_FIFO_XL_DECIMATION_BY_2:  break;
        case LSM6DS3TR_C_DEC_FIFO_XL_DECIMATION_BY_3:  break;
        case LSM6DS3TR_C_DEC_FIFO_XL_DECIMATION_BY_4:  break;
        case LSM6DS3TR_C_DEC_FIFO_XL_DECIMATION_BY_8:  break;
        case LSM6DS3TR_C_DEC_FIFO_XL_DECIMATION_BY_16: break;
        case LSM6DS3TR_C_DEC_FIFO_XL_DECIMATION_BY_32: break;
        default: return ERRNO_INVALID_PARAM;
    }

    _imu_cfg.field_v.dec_fifo_xl_v = value;
    err = I2C_field_write(I2C_CH_1,
                          LSM6DS3_I2C_DEV_ADDR,
                          _imu_fields.dec_fifo_xl_f.reg_addr,
                          (uint8_t)value,
                          _imu_fields.dec_fifo_xl_f.field_start,
                          _imu_fields.dec_fifo_xl_f.field_len);
    return err;
}

ERRNO_e IMU_get_dec_fifo_xl(LSM6DS3TR_C_DEC_FIFO_XL_e *value)
{
    ERRNO_e err;
    uint8_t raw;

    err = I2C_field_read(I2C_CH_1,
                         LSM6DS3_I2C_DEV_ADDR,
                         _imu_fields.dec_fifo_xl_f.reg_addr,
                         &raw,
                         _imu_fields.dec_fifo_xl_f.field_start,
                         _imu_fields.dec_fifo_xl_f.field_len);

    *value = (LSM6DS3TR_C_DEC_FIFO_XL_e)raw;
    _imu_cfg.field_v.dec_fifo_xl_v = *value;
    return err;
}

ERRNO_e IMU_set_odr_fifo(LSM6DS3TR_C_ODR_FIFO_e value)
{
    ERRNO_e err;

    if (!_imu_init) {
        return ERRNO_NOT_INITIALIZED;
    }

    switch (value) {
        case LSM6DS3TR_C_ODR_FIFO_DISABLE: break;
        case LSM6DS3TR_C_ODR_FIFO_12_5Hz:  break;
        case LSM6DS3TR_C_ODR_FIFO_26Hz:    break;
        case LSM6DS3TR_C_ODR_FIFO_52Hz:    break;
        case LSM6DS3TR_C_ODR_FIFO_104Hz:   break;
        case LSM6DS3TR_C_ODR_FIFO_208Hz:   break;
        case LSM6DS3TR_C_ODR_FIFO_416Hz:   break;
        case LSM6DS3TR_C_ODR_FIFO_833Hz:   break;
        case LSM6DS3TR_C_ODR_FIFO_1666Hz:  break;
        case LSM6DS3TR_C_ODR_FIFO_3332Hz:  break;
        case LSM6DS3TR_C_ODR_FIFO_6664Hz:  break;
        default: return ERRNO_INVALID_PARAM;
    }

    _imu_cfg.field_v.odr_fifo_v = value;
    err = I2C_field_write(I2C_CH_1,
                          LSM6DS3_I2C_DEV_ADDR,
                          _imu_fields.odr_fifo_f.reg_addr,
                          (uint8_t)value,
                          _imu_fields.odr_fifo_f.field_start,
                          _imu_fields.odr_fifo_f.field_len);
    return err;
}

ERRNO_e IMU_get_odr_fifo(LSM6DS3TR_C_ODR_FIFO_e *value)
{
    ERRNO_e err;
    uint8_t raw;

    err = I2C_field_read(I2C_CH_1,
                         LSM6DS3_I2C_DEV_ADDR,
                         _imu_fields.odr_fifo_f.reg_addr,
                         &raw,
                         _imu_fields.odr_fifo_f.field_start,
                         _imu_fields.odr_fifo_f.field_len);

    *value = (LSM6DS3TR_C_ODR_FIFO_e)raw;
    _imu_cfg.field_v.odr_fifo_v = *value;
    return err;
}

ERRNO_e IMU_set_fifo_mode(LSM6DS3TR_C_FIFO_MODE_e value)
{
    ERRNO_e err;

    if (!_imu_init) {
        return ERRNO_NOT_INITIALIZED;
    }

    switch (value) {
        case LSM6DS3TR_C_FIFO_MODE_BYPASS: break;
        case LSM6DS3TR_C_FIFO_MODE_FIFO_STOP_ON_FULL: break;
        case LSM6DS3TR_C_FIFO_MODE_CON_THEN_FIFO: break;
        case LSM6DS3TR_C_FIFO_MODE_BYPASS_THEN_CON: break;
        case LSM6DS3TR_C_FIFO_MODE_CONTINUOUS: break;
        default: return ERRNO_INVALID_PARAM;
    }

    _imu_cfg.field_v.fifo_mode_v = value;
    err = I2C_field_write(I2C_CH_1,
                          LSM6DS3_I2C_DEV_ADDR,
                          _imu_fields.fifo_mode_f.reg_addr,
                          (uint8_t)value,
                          _imu_fields.fifo_mode_f.field_start,
                          _imu_fields.fifo_mode_f.field_len);
    return err;
}

ERRNO_e IMU_get_fifo_mode(LSM6DS3TR_C_FIFO_MODE_e *value)
{
    ERRNO_e err;
    uint8_t raw;

    err = I2C_field_read(I2C_CH_1,
                         LSM6DS3_I2C_DEV_ADDR,
                         _imu_fields.fifo_mode_f.reg_addr,
                         &raw,
                         _imu_fields.fifo_mode_f.field_start,
                         _imu_fields.fifo_mode_f.field_len);

    *value = (LSM6DS3TR_C_FIFO_MODE_e)raw;
    _imu_cfg.field_v.fifo_mode_v = *value;
    return err;
}

ERRNO_e IMU_set_int1_fth(LSM6DS3TR_C_INT1_FTH_e value)
{
    ERRNO_e err;

    if (!_imu_init) {
        return ERRNO_NOT_INITIALIZED;
    }

    switch (value) {
        case LSM6DS3TR_C_INT1_FTH_DISABLED: break;
        case LSM6DS3TR_C_INT1_FTH_ENABLED:  break;
        default: return ERRNO_INVALID_PARAM;
    }

    _imu_cfg.field_v.int1_fth_v = value;
    err = I2C_field_write(I2C_CH_1,
                          LSM6DS3_I2C_DEV_ADDR,
                          _imu_fields.int1_fth_f.reg_addr,
                          (uint8_t)value,
                          _imu_fields.int1_fth_f.field_start,
                          _imu_fields.int1_fth_f.field_len);
    return err;
}

ERRNO_e IMU_get_int1_fth(LSM6DS3TR_C_INT1_FTH_e *value)
{
    ERRNO_e err;
    uint8_t raw;

    err = I2C_field_read(I2C_CH_1,
                         LSM6DS3_I2C_DEV_ADDR,
                         _imu_fields.int1_fth_f.reg_addr,
                         &raw,
                         _imu_fields.int1_fth_f.field_start,
                         _imu_fields.int1_fth_f.field_len);

    *value = (LSM6DS3TR_C_INT1_FTH_e)raw;
    _imu_cfg.field_v.int1_fth_v = *value;
    return err;
}

ERRNO_e IMU_set_sw_reset(LSM6DS3TR_C_SW_RESET_e value)
{
    ERRNO_e err;

    if (!_imu_init) {
        return ERRNO_NOT_INITIALIZED;
    }

    switch (value) {
        case LSM6DS3TR_C_SW_RESET_NORMAL_MODE: break;
        case LSM6DS3TR_C_SW_RESET_RESET_DEVICE:  break;
        default: return ERRNO_INVALID_PARAM;
    }

    _imu_cfg.field_v.sw_reset_v = value;
    err = I2C_field_write(I2C_CH_1,
                          LSM6DS3_I2C_DEV_ADDR,
                          _imu_fields.sw_reset_f.reg_addr,
                          (uint8_t)value,
                          _imu_fields.sw_reset_f.field_start,
                          _imu_fields.sw_reset_f.field_len);
    return err;
}

ERRNO_e IMU_get_sw_reset(LSM6DS3TR_C_SW_RESET_e *value)
{
    ERRNO_e err;
    uint8_t raw;

    err = I2C_field_read(I2C_CH_1,
                         LSM6DS3_I2C_DEV_ADDR,
                         _imu_fields.sw_reset_f.reg_addr,
                         &raw,
                         _imu_fields.sw_reset_f.field_start,
                         _imu_fields.sw_reset_f.field_len);

    *value = (LSM6DS3TR_C_SW_RESET_e)raw;
    _imu_cfg.field_v.sw_reset_v = *value;
    return err;
}

ERRNO_e IMU_set_bdu(LSM6DS3TR_C_BDU_e value)
{
    ERRNO_e err;

    if (!_imu_init) {
        return ERRNO_NOT_INITIALIZED;
    }

    switch (value) {
        case LSM6DS3TR_C_BDU_CONTINUOUS:    break;
        case LSM6DS3TR_C_BDU_BLOCK_UPDATE: break;
        default: return ERRNO_INVALID_PARAM;
    }

    _imu_cfg.field_v.bdu_v = value;
    err = I2C_field_write(I2C_CH_1,
                          LSM6DS3_I2C_DEV_ADDR,
                          _imu_fields.bdu_f.reg_addr,
                          (uint8_t)value,
                          _imu_fields.bdu_f.field_start,
                          _imu_fields.bdu_f.field_len);
    return err;
}

ERRNO_e IMU_get_bdu(LSM6DS3TR_C_BDU_e *value)
{
    ERRNO_e err;
    uint8_t raw;

    err = I2C_field_read(I2C_CH_1,
                         LSM6DS3_I2C_DEV_ADDR,
                         _imu_fields.bdu_f.reg_addr,
                         &raw,
                         _imu_fields.bdu_f.field_start,
                         _imu_fields.bdu_f.field_len);

    *value = (LSM6DS3TR_C_BDU_e)raw;
    _imu_cfg.field_v.bdu_v = *value;
    return err;
}

ERRNO_e IMU_set_interrupts_enable(LSM6DS3TR_C_INTERRUPTS_ENABLE_e value)
{
    ERRNO_e err;

    if (!_imu_init) {
        return ERRNO_NOT_INITIALIZED;
    }

    switch (value) {
        case LSM6DS3TR_C_INTERRUPTS_DISABLE: break;
        case LSM6DS3TR_C_INTERRUPTS_ENABLE:  break;
        default: return ERRNO_INVALID_PARAM;
    }

    _imu_cfg.field_v.interrupts_enable_v = value;
    err = I2C_field_write(I2C_CH_1,
                          LSM6DS3_I2C_DEV_ADDR,
                          _imu_fields.interrupts_enable_f.reg_addr,
                          (uint8_t)value,
                          _imu_fields.interrupts_enable_f.field_start,
                          _imu_fields.interrupts_enable_f.field_len);
    return err;
}

ERRNO_e IMU_get_interrupts_enable(LSM6DS3TR_C_INTERRUPTS_ENABLE_e *value)
{
    ERRNO_e err;
    uint8_t raw;

    err = I2C_field_read(I2C_CH_1,
                         LSM6DS3_I2C_DEV_ADDR,
                         _imu_fields.interrupts_enable_f.reg_addr,
                         &raw,
                         _imu_fields.interrupts_enable_f.field_start,
                         _imu_fields.interrupts_enable_f.field_len);

    *value = (LSM6DS3TR_C_INTERRUPTS_ENABLE_e)raw;
    _imu_cfg.field_v.interrupts_enable_v = *value;
    return err;
}

ERRNO_e IMU_set_slope_fds(LSM6DS3TR_C_SLOPE_FDS_e value)
{
    ERRNO_e err;

    if (!_imu_init) {
        return ERRNO_NOT_INITIALIZED;
    }

    switch (value) {
        case LSM6DS3TR_C_SLOPE_FDS_SLOPE: break;
        case LSM6DS3TR_C_SLOPE_FDS_HPF:  break;
        default: return ERRNO_INVALID_PARAM;
    }

    _imu_cfg.field_v.slope_fds_v = value;
    err = I2C_field_write(I2C_CH_1,
                          LSM6DS3_I2C_DEV_ADDR,
                          _imu_fields.slope_fds_f.reg_addr,
                          (uint8_t)value,
                          _imu_fields.slope_fds_f.field_start,
                          _imu_fields.slope_fds_f.field_len);
    return err;
}

ERRNO_e IMU_get_slope_fds(LSM6DS3TR_C_SLOPE_FDS_e *value)
{
    ERRNO_e err;
    uint8_t raw;

    err = I2C_field_read(I2C_CH_1,
                         LSM6DS3_I2C_DEV_ADDR,
                         _imu_fields.slope_fds_f.reg_addr,
                         &raw,
                         _imu_fields.slope_fds_f.field_start,
                         _imu_fields.slope_fds_f.field_len);

    *value = (LSM6DS3TR_C_SLOPE_FDS_e)raw;
    _imu_cfg.field_v.slope_fds_v = *value;
    return err;
}

ERRNO_e IMU_set_tap_x_en(LSM6DS3TR_C_TAP_X_EN_e value)
{
    ERRNO_e err;

    if (!_imu_init) {
        return ERRNO_NOT_INITIALIZED;
    }

    switch (value) {
        case LSM6DS3TR_C_TAP_X_EN_DISABLED: break;
        case LSM6DS3TR_C_TAP_X_EN_ENABLED:  break;
        default: return ERRNO_INVALID_PARAM;
    }

    _imu_cfg.field_v.tap_x_en_v = value;
    err = I2C_field_write(I2C_CH_1,
                          LSM6DS3_I2C_DEV_ADDR,
                          _imu_fields.tap_x_en_f.reg_addr,
                          (uint8_t)value,
                          _imu_fields.tap_x_en_f.field_start,
                          _imu_fields.tap_x_en_f.field_len);
    return err;
}

ERRNO_e IMU_get_tap_x_en(LSM6DS3TR_C_TAP_X_EN_e *value)
{
    ERRNO_e err;
    uint8_t raw;

    err = I2C_field_read(I2C_CH_1,
                         LSM6DS3_I2C_DEV_ADDR,
                         _imu_fields.tap_x_en_f.reg_addr,
                         &raw,
                         _imu_fields.tap_x_en_f.field_start,
                         _imu_fields.tap_x_en_f.field_len);

    *value = (LSM6DS3TR_C_TAP_X_EN_e)raw;
    _imu_cfg.field_v.tap_x_en_v = *value;
    return err;
}

ERRNO_e IMU_set_tap_y_en(LSM6DS3TR_C_TAP_Y_EN_e value)
{
    ERRNO_e err;

    if (!_imu_init) {
        return ERRNO_NOT_INITIALIZED;
    }

    switch (value) {
        case LSM6DS3TR_C_TAP_Y_EN_DISABLED: break;
        case LSM6DS3TR_C_TAP_Y_EN_ENABLED:  break;
        default: return ERRNO_INVALID_PARAM;
    }

    _imu_cfg.field_v.tap_y_en_v = value;
    err = I2C_field_write(I2C_CH_1,
                          LSM6DS3_I2C_DEV_ADDR,
                          _imu_fields.tap_y_en_f.reg_addr,
                          (uint8_t)value,
                          _imu_fields.tap_y_en_f.field_start,
                          _imu_fields.tap_y_en_f.field_len);
    return err;
}

ERRNO_e IMU_get_tap_y_en(LSM6DS3TR_C_TAP_Y_EN_e *value)
{
    ERRNO_e err;
    uint8_t raw;

    err = I2C_field_read(I2C_CH_1,
                         LSM6DS3_I2C_DEV_ADDR,
                         _imu_fields.tap_y_en_f.reg_addr,
                         &raw,
                         _imu_fields.tap_y_en_f.field_start,
                         _imu_fields.tap_y_en_f.field_len);

    *value = (LSM6DS3TR_C_TAP_Y_EN_e)raw;
    _imu_cfg.field_v.tap_y_en_v = *value;
    return err;
}

ERRNO_e IMU_set_tap_z_en(LSM6DS3TR_C_TAP_Z_EN_e value)
{
    ERRNO_e err;

    if (!_imu_init) {
        return ERRNO_NOT_INITIALIZED;
    }

    switch (value) {
        case LSM6DS3TR_C_TAP_Z_EN_DISABLED: break;
        case LSM6DS3TR_C_TAP_Z_EN_ENABLED:  break;
        default: return ERRNO_INVALID_PARAM;
    }

    _imu_cfg.field_v.tap_z_en_v = value;
    err = I2C_field_write(I2C_CH_1,
                          LSM6DS3_I2C_DEV_ADDR,
                          _imu_fields.tap_z_en_f.reg_addr,
                          (uint8_t)value,
                          _imu_fields.tap_z_en_f.field_start,
                          _imu_fields.tap_z_en_f.field_len);
    return err;
}

ERRNO_e IMU_get_tap_z_en(LSM6DS3TR_C_TAP_Z_EN_e *value)
{
    ERRNO_e err;
    uint8_t raw;

    err = I2C_field_read(I2C_CH_1,
                         LSM6DS3_I2C_DEV_ADDR,
                         _imu_fields.tap_z_en_f.reg_addr,
                         &raw,
                         _imu_fields.tap_z_en_f.field_start,
                         _imu_fields.tap_z_en_f.field_len);

    *value = (LSM6DS3TR_C_TAP_Z_EN_e)raw;
    _imu_cfg.field_v.tap_z_en_v = *value;
    return err;
}

ERRNO_e IMU_set_lir(LSM6DS3TR_C_LIR_e value)
{
    ERRNO_e err;

    if (!_imu_init) {
        return ERRNO_NOT_INITIALIZED;
    }

    switch (value) {
        case LSM6DS3TR_C_LIR_DISABLED: break;
        case LSM6DS3TR_C_LIR_ENABLED:  break;
        default: return ERRNO_INVALID_PARAM;
    }

    _imu_cfg.field_v.lir_v = value;
    err = I2C_field_write(I2C_CH_1,
                          LSM6DS3_I2C_DEV_ADDR,
                          _imu_fields.lir_f.reg_addr,
                          (uint8_t)value,
                          _imu_fields.lir_f.field_start,
                          _imu_fields.lir_f.field_len);
    return err;
}

ERRNO_e IMU_get_lir(LSM6DS3TR_C_LIR_e *value)
{
    ERRNO_e err;
    uint8_t raw;

    err = I2C_field_read(I2C_CH_1,
                         LSM6DS3_I2C_DEV_ADDR,
                         _imu_fields.lir_f.reg_addr,
                         &raw,
                         _imu_fields.lir_f.field_start,
                         _imu_fields.lir_f.field_len);

    *value = (LSM6DS3TR_C_LIR_e)raw;
    _imu_cfg.field_v.lir_v = *value;
    return err;
}

ERRNO_e IMU_set_tap_ths(uint8_t value)
{
    ERRNO_e err;

    if (!_imu_init) {
        return ERRNO_NOT_INITIALIZED;
    }

    if (value >= 32) {
        return ERRNO_INVALID_PARAM;
    }

    _imu_cfg.field_v.tap_ths_v = value;
    err = I2C_field_write(I2C_CH_1,
                          LSM6DS3_I2C_DEV_ADDR,
                          _imu_fields.tap_ths_f.reg_addr,
                          (uint8_t)value,
                          _imu_fields.tap_ths_f.field_start,
                          _imu_fields.tap_ths_f.field_len);
    return err;
}

ERRNO_e IMU_get_tap_ths(uint8_t *value)
{
    ERRNO_e err;
    uint8_t raw;

    err = I2C_field_read(I2C_CH_1,
                         LSM6DS3_I2C_DEV_ADDR,
                         _imu_fields.tap_ths_f.reg_addr,
                         &raw,
                         _imu_fields.tap_ths_f.field_start,
                         _imu_fields.tap_ths_f.field_len);

    *value = raw;
    _imu_cfg.field_v.tap_ths_v = *value;
    return err;
}

ERRNO_e IMU_set_quiet(LSM6DS3TR_C_QUIET_e value)
{
    ERRNO_e err;

    if (!_imu_init) {
        return ERRNO_NOT_INITIALIZED;
    }

    switch (value) {
        case LSM6DS3TR_C_QUIET_2_OVER_ODR_XL:  break;
        case LSM6DS3TR_C_QUIET_4_OVER_ODR_XL:  break;
        case LSM6DS3TR_C_QUIET_8_OVER_ODR_XL:  break;
        case LSM6DS3TR_C_QUIET_12_OVER_ODR_XL: break;
        default: return ERRNO_INVALID_PARAM;
    }

    _imu_cfg.field_v.quiet_v = value;
    err = I2C_field_write(I2C_CH_1,
                          LSM6DS3_I2C_DEV_ADDR,
                          _imu_fields.quiet_f.reg_addr,
                          (uint8_t)value,
                          _imu_fields.quiet_f.field_start,
                          _imu_fields.quiet_f.field_len);
    return err;
}

ERRNO_e IMU_get_quiet(LSM6DS3TR_C_QUIET_e *value)
{
    ERRNO_e err;
    uint8_t raw;

    err = I2C_field_read(I2C_CH_1,
                         LSM6DS3_I2C_DEV_ADDR,
                         _imu_fields.quiet_f.reg_addr,
                         &raw,
                         _imu_fields.quiet_f.field_start,
                         _imu_fields.quiet_f.field_len);

    *value = (LSM6DS3TR_C_QUIET_e)raw;
    _imu_cfg.field_v.quiet_v = *value;
    return err;
}

ERRNO_e IMU_set_shock(LSM6DS3TR_C_SHOCK_e value)
{
    ERRNO_e err;

    if (!_imu_init) {
        return ERRNO_NOT_INITIALIZED;
    }

    switch (value) {
        case LSM6DS3TR_C_SHOCK_4_OVER_ODR_XL:  break;
        case LSM6DS3TR_C_SHOCK_8_OVER_ODR_XL:  break;
        case LSM6DS3TR_C_SHOCK_16_OVER_ODR_XL: break;
        case LSM6DS3TR_C_SHOCK_24_OVER_ODR_XL: break;
        default: return ERRNO_INVALID_PARAM;
    }

    _imu_cfg.field_v.shock_v = value;
    err = I2C_field_write(I2C_CH_1,
                          LSM6DS3_I2C_DEV_ADDR,
                          _imu_fields.shock_f.reg_addr,
                          (uint8_t)value,
                          _imu_fields.shock_f.field_start,
                          _imu_fields.shock_f.field_len);
    return err;
}

ERRNO_e IMU_get_shock(LSM6DS3TR_C_SHOCK_e *value)
{
    ERRNO_e err;
    uint8_t raw;

    err = I2C_field_read(I2C_CH_1,
                         LSM6DS3_I2C_DEV_ADDR,
                         _imu_fields.shock_f.reg_addr,
                         &raw,
                         _imu_fields.shock_f.field_start,
                         _imu_fields.shock_f.field_len);

    *value = (LSM6DS3TR_C_SHOCK_e)raw;
    _imu_cfg.field_v.shock_v = *value;
    return err;
}

ERRNO_e IMU_set_single_double_tap(LSM6DS3TR_C_SINGLE_DOUBLE_TAP_e value)
{
    ERRNO_e err;

    if (!_imu_init) {
        return ERRNO_NOT_INITIALIZED;
    }

    switch (value) {
        case LSM6DS3TR_C_SINGLE_DOUBLE_TAP_SINGLE_ONLY:       break;
        case LSM6DS3TR_C_SINGLE_DOUBLE_TAP_SINGLE_AND_DOUBLE: break;
        default: return ERRNO_INVALID_PARAM;
    }

    _imu_cfg.field_v.single_double_tap_v = value;
    err = I2C_field_write(I2C_CH_1,
                          LSM6DS3_I2C_DEV_ADDR,
                          _imu_fields.single_double_tap_f.reg_addr,
                          (uint8_t)value,
                          _imu_fields.single_double_tap_f.field_start,
                          _imu_fields.single_double_tap_f.field_len);
    return err;
}

ERRNO_e IMU_get_single_double_tap(LSM6DS3TR_C_SINGLE_DOUBLE_TAP_e *value)
{
    ERRNO_e err;
    uint8_t raw;

    err = I2C_field_read(I2C_CH_1,
                         LSM6DS3_I2C_DEV_ADDR,
                         _imu_fields.single_double_tap_f.reg_addr,
                         &raw,
                         _imu_fields.single_double_tap_f.field_start,
                         _imu_fields.single_double_tap_f.field_len);

    *value = (LSM6DS3TR_C_SINGLE_DOUBLE_TAP_e)raw;
    _imu_cfg.field_v.single_double_tap_v = *value;
    return err;
}

ERRNO_e IMU_set_wk_ths(uint8_t value)
{
    ERRNO_e err;

    if (!_imu_init) {
        return ERRNO_NOT_INITIALIZED;
    }

    if (value > 63) {
        return ERRNO_INVALID_PARAM;
    }

    _imu_cfg.field_v.wk_ths_v = value;
    err = I2C_field_write(I2C_CH_1,
                          LSM6DS3_I2C_DEV_ADDR,
                          _imu_fields.wk_ths_f.reg_addr,
                          (uint8_t)value,
                          _imu_fields.wk_ths_f.field_start,
                          _imu_fields.wk_ths_f.field_len);
    return err;
}

ERRNO_e IMU_get_wk_ths(uint8_t *value)
{
    ERRNO_e err;
    uint8_t raw;

    err = I2C_field_read(I2C_CH_1,
                         LSM6DS3_I2C_DEV_ADDR,
                         _imu_fields.wk_ths_f.reg_addr,
                         &raw,
                         _imu_fields.wk_ths_f.field_start,
                         _imu_fields.wk_ths_f.field_len);

    *value = raw;
    _imu_cfg.field_v.wk_ths_v = *value; /* is this needed? */
    return err;
}

ERRNO_e IMU_set_wake_dur(LSM6DS3TR_C_WAKE_DUR_e value)
{
    ERRNO_e err;

    if (!_imu_init) {
        return ERRNO_NOT_INITIALIZED;
    }

    switch (value) {
        case LSM6DS3TR_C_WAKE_DUR_0_ODR_XL: break;
        case LSM6DS3TR_C_WAKE_DUR_1_ODR_XL: break;
        case LSM6DS3TR_C_WAKE_DUR_2_ODR_XL: break;
        case LSM6DS3TR_C_WAKE_DUR_3_ODR_XL: break;
        default: return ERRNO_INVALID_PARAM;
    }

    _imu_cfg.field_v.wake_dur_v = value;
    err = I2C_field_write(I2C_CH_1,
                          LSM6DS3_I2C_DEV_ADDR,
                          _imu_fields.wake_dur_f.reg_addr,
                          (uint8_t)value,
                          _imu_fields.wake_dur_f.field_start,
                          _imu_fields.wake_dur_f.field_len);
    return err;
}

ERRNO_e IMU_get_wake_dur(LSM6DS3TR_C_WAKE_DUR_e *value)
{
    ERRNO_e err;
    uint8_t raw;

    err = I2C_field_read(I2C_CH_1,
                         LSM6DS3_I2C_DEV_ADDR,
                         _imu_fields.wake_dur_f.reg_addr,
                         &raw,
                         _imu_fields.wake_dur_f.field_start,
                         _imu_fields.wake_dur_f.field_len);

    *value = (LSM6DS3TR_C_WAKE_DUR_e)raw;
    _imu_cfg.field_v.wake_dur_v = *value;
    return err;
}

ERRNO_e IMU_set_int1_single_tap(LSM6DS3TR_C_INT1_SINGLE_TAP_e value)
{
    ERRNO_e err;

    if (!_imu_init) {
        return ERRNO_NOT_INITIALIZED;
    }

    switch (value) {
        case LSM6DS3TR_C_INT1_SINGLE_TAP_DISABLED: break;
        case LSM6DS3TR_C_INT1_SINGLE_TAP_ENABLED: break;
        default: return ERRNO_INVALID_PARAM;
    }

    _imu_cfg.field_v.int1_single_tap_v = value;
    err = I2C_field_write(I2C_CH_1,
                          LSM6DS3_I2C_DEV_ADDR,
                          _imu_fields.int1_single_tap_f.reg_addr,
                          (uint8_t)value,
                          _imu_fields.int1_single_tap_f.field_start,
                          _imu_fields.int1_single_tap_f.field_len);
    return err;
}

ERRNO_e IMU_get_int1_single_tap(LSM6DS3TR_C_INT1_SINGLE_TAP_e *value)
{
    ERRNO_e err;
    uint8_t raw;

    err = I2C_field_read(I2C_CH_1,
                         LSM6DS3_I2C_DEV_ADDR,
                         _imu_fields.int1_single_tap_f.reg_addr,
                         &raw,
                         _imu_fields.int1_single_tap_f.field_start,
                         _imu_fields.int1_single_tap_f.field_len);

    *value = (LSM6DS3TR_C_INT1_SINGLE_TAP_e)raw;
    _imu_cfg.field_v.int1_single_tap_v = *value;
    return err;
}

ERRNO_e IMU_set_int1_wu(LSM6DS3TR_C_INT1_WU_e value)
{
    ERRNO_e err;

    if (!_imu_init) {
        return ERRNO_NOT_INITIALIZED;
    }

    switch (value) {
        case LSM6DS3TR_C_INT1_WU_DISABLED: break;
        case LSM6DS3TR_C_INT1_WU_ENABLED: break;
        default: return ERRNO_INVALID_PARAM;
    }

    _imu_cfg.field_v.int1_wu_v = value;
    err = I2C_field_write(I2C_CH_1,
                          LSM6DS3_I2C_DEV_ADDR,
                          _imu_fields.int1_wu_f.reg_addr,
                          (uint8_t)value,
                          _imu_fields.int1_wu_f.field_start,
                          _imu_fields.int1_wu_f.field_len);
    return err;
}

ERRNO_e IMU_get_int1_wu(LSM6DS3TR_C_INT1_WU_e *value)
{
    ERRNO_e err;
    uint8_t raw;

    err = I2C_field_read(I2C_CH_1,
                         LSM6DS3_I2C_DEV_ADDR,
                         _imu_fields.int1_wu_f.reg_addr,
                         &raw,
                         _imu_fields.int1_wu_f.field_start,
                         _imu_fields.int1_wu_f.field_len);

    *value = (LSM6DS3TR_C_INT1_WU_e)raw;
    _imu_cfg.field_v.int1_wu_v = *value;
    return err;
}

ERRNO_e IMU_set_int1_double_tap(LSM6DS3TR_C_INT1_DOUBLE_TAP_e value)
{
    ERRNO_e err;

    if (!_imu_init) {
        return ERRNO_NOT_INITIALIZED;
    }

    switch (value) {
        case LSM6DS3TR_C_INT1_DOUBLE_TAP_DISABLED: break;
        case LSM6DS3TR_C_INT1_DOUBLE_TAP_ENABLED: break;
        default: return ERRNO_INVALID_PARAM;
    }

    _imu_cfg.field_v.int1_double_tap_v = value;
    err = I2C_field_write(I2C_CH_1,
                          LSM6DS3_I2C_DEV_ADDR,
                          _imu_fields.int1_double_tap_f.reg_addr,
                          (uint8_t)value,
                          _imu_fields.int1_double_tap_f.field_start,
                          _imu_fields.int1_double_tap_f.field_len);
    return err;
}

ERRNO_e IMU_get_int1_double_tap(LSM6DS3TR_C_INT1_DOUBLE_TAP_e *value)
{
    ERRNO_e err;
    uint8_t raw;

    err = I2C_field_read(I2C_CH_1,
                         LSM6DS3_I2C_DEV_ADDR,
                         _imu_fields.int1_double_tap_f.reg_addr,
                         &raw,
                         _imu_fields.int1_double_tap_f.field_start,
                         _imu_fields.int1_double_tap_f.field_len);

    *value = (LSM6DS3TR_C_INT1_DOUBLE_TAP_e)raw;
    _imu_cfg.field_v.int1_double_tap_v = *value;
    return err;
}

ERRNO_e IMU_temp_raw_read(int16_t *p_dest)
{
    if (!_imu_init) {
        return ERRNO_NOT_INITIALIZED;
    }
    else if (p_dest == NULL) {
        return ERRNO_BAD_POINTER;
    }

    if (I2C_words_read (I2C_CH_1, LSM6DS3_I2C_DEV_ADDR, LSM6DS3TR_C_OUT_TEMP_L, (uint16_t *) p_dest, 1) != ERRNO_SUCCESS) {
        return ERRNO_I2C_FAILURE;
    }

    return ERRNO_SUCCESS;
}

ERRNO_e IMU_temp_c_read(float *p_dest)
{
    ERRNO_e err;
    int16_t reading;

    err = IMU_temp_raw_read (&reading);
    if (err != ERRNO_SUCCESS) {
        return err;
    }

    /* Need to divide by temp sensitivity to scale. */
    *p_dest = (float) reading / (float) _imu_cfg.temp_sensitivity;

    /* Add 25 degrees to remove offset. */
    // *p_dest += 25;

    return ERRNO_SUCCESS;
}

ERRNO_e IMU_temp_f_read(float *p_dest)
{
    ERRNO_e err;
    float reading;

    err = IMU_temp_c_read(&reading);
    if (err != ERRNO_SUCCESS) {
        return err;
    }

    /* Convert to F. */
    *p_dest = (reading * (float)9.0) / (float)5.0 + (float)32.0;

    return ERRNO_SUCCESS;
}

ERRNO_e IMU_reset(void)
{
    uint8_t byte = 0x01;

    if (!_imu_init) {
        return ERRNO_NOT_INITIALIZED;
    }

    if (I2C_bytes_write(I2C_CH_1, LSM6DS3_I2C_DEV_ADDR, LSM6DS3TR_C_CTRL3_C, &byte, 1) != ERRNO_SUCCESS) {
        return ERRNO_I2C_FAILURE;
    }

    /* Wait for 10 ms for the reset to complete. */
    k_busy_wait(10000);
    
    return ERRNO_SUCCESS;
}

ERRNO_e IMU_xl_raw_x_read(int16_t *p_dest)
{
    if (!_imu_init) {
        return ERRNO_NOT_INITIALIZED;
    }
    else if (p_dest == NULL) {
        return ERRNO_BAD_POINTER;
    }

    if (I2C_words_read(I2C_CH_1, LSM6DS3_I2C_DEV_ADDR, LSM6DS3TR_C_OUTX_L_XL, (uint16_t *) p_dest, 1) != ERRNO_SUCCESS) {
        return ERRNO_I2C_FAILURE;
    }

    return ERRNO_SUCCESS;
}

ERRNO_e IMU_xl_raw_y_read(int16_t *p_dest)
{
    if (!_imu_init) {
        return ERRNO_NOT_INITIALIZED;
    }
    else if (p_dest == NULL) {
        return ERRNO_BAD_POINTER;
    }

    if (I2C_words_read(I2C_CH_1, LSM6DS3_I2C_DEV_ADDR, LSM6DS3TR_C_OUTY_L_XL, (uint16_t *) p_dest, 1) != ERRNO_SUCCESS) {
        return ERRNO_I2C_FAILURE;
    }

    return ERRNO_SUCCESS;
}

ERRNO_e IMU_xl_raw_z_read(int16_t *p_dest)
{
    if (!_imu_init) {
        return ERRNO_NOT_INITIALIZED;
    }
    else if (p_dest == NULL) {
        return ERRNO_BAD_POINTER;
    }

    if (I2C_words_read(I2C_CH_1, LSM6DS3_I2C_DEV_ADDR, LSM6DS3TR_C_OUTZ_L_XL, (uint16_t *) p_dest, 1) != ERRNO_SUCCESS) {
        return ERRNO_I2C_FAILURE;
    }

    return ERRNO_SUCCESS;
}

ERRNO_e IMU_xl_x_read(float *p_dest)
{
    int16_t x;
    ERRNO_e err = IMU_xl_raw_x_read(&x);

    *p_dest = _imu_xl_raw_to_float(x);

    return err;
}

ERRNO_e IMU_xl_y_read(float *p_dest)
{
    int16_t y;
    ERRNO_e err = IMU_xl_raw_y_read(&y);

    *p_dest = _imu_xl_raw_to_float(y);

    return err;
}

ERRNO_e IMU_xl_z_read(float *p_dest)
{
    int16_t z;
    ERRNO_e err = IMU_xl_raw_z_read(&z);

    *p_dest = _imu_xl_raw_to_float(z);

    return err;
}

ERRNO_e IMU_xl_read(IMU_data_s *p_dest)
{
    ERRNO_e err;
    float   x;
    float   y;
    float   z;

    err  = IMU_xl_x_read(&x);
    err |= IMU_xl_y_read(&y);
    err |= IMU_xl_z_read(&z);

    p_dest->x = x;
    p_dest->y = y;
    p_dest->z = z;

    return err;
}

ERRNO_e IMU_gyro_raw_x_read(int16_t *p_dest)
{
    if (!_imu_init) {
        return ERRNO_NOT_INITIALIZED;
    }
    else if (p_dest == NULL) {
        return ERRNO_BAD_POINTER;
    }

    if (I2C_words_read(I2C_CH_1, LSM6DS3_I2C_DEV_ADDR, LSM6DS3TR_C_OUTX_L_G, (uint16_t *) p_dest, 1) != ERRNO_SUCCESS) {
        return ERRNO_I2C_FAILURE;
    }

    return ERRNO_SUCCESS;
}

ERRNO_e IMU_gyro_raw_y_read(int16_t *p_dest)
{
    if (!_imu_init) {
        return ERRNO_NOT_INITIALIZED;
    }
    else if (p_dest == NULL) {
        return ERRNO_BAD_POINTER;
    }

    if (I2C_words_read(I2C_CH_1, LSM6DS3_I2C_DEV_ADDR, LSM6DS3TR_C_OUTY_L_G, (uint16_t *) p_dest, 1) != ERRNO_SUCCESS) {
        return ERRNO_I2C_FAILURE;
    }

    return ERRNO_SUCCESS;
}

ERRNO_e IMU_gyro_raw_z_read(int16_t *p_dest)
{
    if (!_imu_init) {
        return ERRNO_NOT_INITIALIZED;
    }
    else if (p_dest == NULL) {
        return ERRNO_BAD_POINTER;
    }

    if (I2C_words_read(I2C_CH_1, LSM6DS3_I2C_DEV_ADDR, LSM6DS3TR_C_OUTZ_L_G, (uint16_t *) p_dest, 1) != ERRNO_SUCCESS) {
        return ERRNO_I2C_FAILURE;
    }

    return ERRNO_SUCCESS;
}

ERRNO_e IMU_gyro_x_read(float *p_dest)
{
    int16_t x;
    ERRNO_e err = IMU_gyro_raw_x_read(&x);

    *p_dest = _imu_gyro_raw_to_float(x);

    return err;
}

ERRNO_e IMU_gyro_y_read(float *p_dest)
{
    int16_t y;
    ERRNO_e err = IMU_gyro_raw_y_read (&y);

    *p_dest = _imu_gyro_raw_to_float (y);

    return err;
}

ERRNO_e IMU_gyro_z_read(float *p_dest)
{
    int16_t z;
    ERRNO_e err = IMU_gyro_raw_z_read (&z);

    *p_dest = _imu_gyro_raw_to_float (z);

    return err;
}

ERRNO_e IMU_gyro_read(IMU_data_s *p_dest)
{
    ERRNO_e err;
    float   x;
    float   y;
    float   z;

    err  = IMU_gyro_x_read (&x);
    err |= IMU_gyro_y_read (&y);
    err |= IMU_gyro_z_read (&z);

    p_dest->x = x;
    p_dest->y = y;
    p_dest->z = z;

    return err;
}

void IMU_set_shot_capture_mode(bool enable)
{
    if (enable){
        atomic_set_bit(&_imu_cfg.interrupt_mode_flags, IMU_int_shot_capture_mode);
    } else {
        atomic_clear_bit(&_imu_cfg.interrupt_mode_flags, IMU_int_shot_capture_mode);
    }

}

ERRNO_e IMU_fifo_status_1to2_read (uint16_t *unread_words, uint8_t *empty, uint8_t *smart_full, uint8_t *over_run, uint8_t *waterM)
{
    ERRNO_e err;
    uint16_t regs_val;
    //err = I2C_words_read (I2C_CH_1, LSM6DS3_I2C_DEV_ADDR, LSM6DS3TR_C_FIFO_STATUS1, &regs_val, 1);
    err = I2C_bytes_read(I2C_CH_1, LSM6DS3_I2C_DEV_ADDR, LSM6DS3TR_C_FIFO_STATUS1, (uint8_t *) &regs_val, 2);
    if (ERRNO_SUCCESS != err) {
        PRINT (ERROR, "i2c read fifo status regs failed in isr!\r\n");
    }

    if (ERRNO_SUCCESS == err) {
        *unread_words = regs_val & 0x07FF;
        *empty        = (uint8_t)(regs_val >> 8) & LSM6DS3TR_C_FIFO_EMPTY_FIFO_EMPTY;
        *smart_full   = (uint8_t)(regs_val >> 8) & LSM6DS3TR_C_FIFO_FULL_FIFO_FULL;
        *over_run     = (uint8_t)(regs_val >> 8) & LSM6DS3TR_C_OVERRUN_OVERRUN;
        *waterM       = (uint8_t)(regs_val >> 8) & LSM6DS3TR_C_WTM_ABOVE_OR_EQUAL_WTM;
    }

    return err;
}

ERRNO_e IMU_func_src1_read (uint8_t *sign_motion)
{
    ERRNO_e err;
    uint8_t reg_val;
    err = I2C_bytes_read(I2C_CH_1, LSM6DS3_I2C_DEV_ADDR, LSM6DS3TR_C_FIFO_STATUS1, &reg_val, 1);
    if (err != ERRNO_SUCCESS)
    {
        PRINT (ERROR, "i2c read func src1 reg failed in isr!\r\n");
        return err;
    }
    *sign_motion = reg_val & LSM6DS3TR_C_SIGN_MOT_EV_STATUS_DETECTED;

    return err;
}

ERRNO_e IMU_fifo_th_set(uint16_t fifo_threshold)
{
    ERRNO_e err = ERRNO_SUCCESS;
    uint8_t reset = 0;
    LSM6DS3TR_C_ODR_FIFO_e max_odr;

    if (fifo_threshold == 0 && atomic_get(&_imu_fifo_is_set)) {

        /* unroute FIFO threshold interrupt to IMU INT1 pad */
        err = IMU_set_int1_fth(LSM6DS3TR_C_INT1_FTH_DISABLED);

        /* disable fifo by setting oodr_fifo and fifo_mode to 0 */
        if (err == ERRNO_SUCCESS) {
            err = I2C_bytes_write(I2C_CH_1, LSM6DS3_I2C_DEV_ADDR, LSM6DS3TR_C_FIFO_CTRL5, &reset, 1);
        }

        /* diable accelerometer and gyro data in FIFO */
        if (err == ERRNO_SUCCESS) {
            err = I2C_bytes_write(I2C_CH_1, LSM6DS3_I2C_DEV_ADDR, LSM6DS3TR_C_FIFO_CTRL3, &reset, 1);
        }

        /* set bdu bit to continuous update */
        if (err == ERRNO_SUCCESS) {
            err = IMU_set_bdu(LSM6DS3TR_C_BDU_CONTINUOUS);
        }

        if (err == ERRNO_SUCCESS) {
            err = IMU_set_FTH(0);
        }

        atomic_set(&_imu_fifo_is_set, 0);     
    } else if (fifo_threshold && !atomic_get(&_imu_fifo_is_set)) {
        err = IMU_set_FTH(fifo_threshold);

        /* set BDU bit to enable proper reading of fifo output registers*/
        if (err == ERRNO_SUCCESS) {
            err = IMU_set_bdu(LSM6DS3TR_C_BDU_BLOCK_UPDATE);
        }

        /* enable gyroscope without decimation into fifo*/
        if (err == ERRNO_SUCCESS) {
            err = IMU_set_dec_fifo_gyro(LSM6DS3TR_C_DEC_FIFO_GYRO_NO_DECIMATION);
        }

        /* enable accelerometer data without decimation into fifo*/
        if (err == ERRNO_SUCCESS) {
            err = IMU_set_dec_fifo_xl(LSM6DS3TR_C_DEC_FIFO_XL_NO_DECIMATION);
        }

        /* set fifo odr rate equal to maximum of accelerometer and gyroscope odr */
        max_odr = (LSM6DS3TR_C_ODR_FIFO_e) ((uint8_t) _imu_cfg.field_v.odr_fifo_v > (uint8_t) _imu_cfg.field_v.odr_xl_v ? _imu_cfg.field_v.odr_fifo_v : _imu_cfg.field_v.odr_xl_v);
        if (err == ERRNO_SUCCESS) {
            err = IMU_set_odr_fifo(max_odr);
        }

        /* set fifo to continuous mode */
        if (err == ERRNO_SUCCESS) {
            err = IMU_set_fifo_mode(LSM6DS3TR_C_FIFO_MODE_CONTINUOUS);
        }

        /* route FIFO threshold interrupt to IMU INT1 pad*/
        if (err == ERRNO_SUCCESS) {
            err = IMU_set_int1_fth(LSM6DS3TR_C_INT1_FTH_ENABLED);
        }

        atomic_set(&_imu_fifo_is_set, 1); 
    }

    return err;
}

bool IMU_is_fifo_set(void)
{
    return _imu_fifo_is_set;
}

ERRNO_e IMU_motion_int_enable (bool en, uint8_t threshold, uint8_t duration)
{
    if (!_imu_init) {
        return ERRNO_NOT_INITIALIZED;
    }

    if (en && !atomic_get(&_imu_motion_int_enable)) {
        if (IMU_set_interrupts_enable(LSM6DS3TR_C_INTERRUPTS_ENABLE)) {
            return ERRNO_I2C_FAILURE;
        }

        if (IMU_set_lir(LSM6DS3TR_C_LIR_DISABLED)) {
            return ERRNO_I2C_FAILURE;
        }

        if (IMU_set_slope_fds(LSM6DS3TR_C_SLOPE_FDS_SLOPE)) {
            return ERRNO_I2C_FAILURE;
        }

        if (IMU_set_wake_dur((LSM6DS3TR_C_WAKE_DUR_e)duration)) {
            return ERRNO_I2C_FAILURE;
        }

        if (IMU_set_wk_ths(threshold)) {
            return ERRNO_I2C_FAILURE;
        }

        if (IMU_set_int1_wu(LSM6DS3TR_C_INT1_WU_ENABLED)) {
            return ERRNO_I2C_FAILURE;
        }

        atomic_set_bit(&_imu_cfg.interrupt_mode_flags, IMU_int_wake_up_mode);

        /* Set enable flag.  This is used so if the IMU is put to sleep, we keep accelerometer running in low
        power mode to detect motion and interrupt. */
        atomic_set(&_imu_motion_int_enable, 1);
    }
    else if (!en && atomic_get(&_imu_motion_int_enable)) {
        if (IMU_set_int1_wu(LSM6DS3TR_C_INT1_WU_DISABLED)) 
        {
            return ERRNO_I2C_FAILURE;
        }

        atomic_clear_bit(&_imu_cfg.interrupt_mode_flags, IMU_int_wake_up_mode);
        atomic_set(&_imu_motion_int_enable, 0);
    } else {
        goto out;
    }

out:
    return ERRNO_SUCCESS;
}

ERRNO_e IMU_sleep (void)
{
    uint8_t byte = 0x00;

    if (!_imu_init)
    {
        return ERRNO_NOT_INITIALIZED;
    }

    /* README: Not used in initial release. */
    /* Disable shot capture, if enabled. */
    // SHOT_CAPTURE_enable (false);

    if (atomic_get(&_imu_motion_int_enable))
    {
        /* Set to accelerometer low performance mode. */
        /* This lowers the output data rate of the accelerometer to 26 Hz. */
        byte = 0x10;
        if (I2C_bytes_write (I2C_CH_1, LSM6DS3_I2C_DEV_ADDR, LSM6DS3TR_C_CTRL6_C, &byte, 1) != ERRNO_SUCCESS)
        {
            return ERRNO_I2C_FAILURE;
        }

        byte = 0x20;
        if (I2C_bytes_write (I2C_CH_1, LSM6DS3_I2C_DEV_ADDR, LSM6DS3TR_C_CTRL1_XL, &byte, 1) != ERRNO_SUCCESS)
        {
            return ERRNO_I2C_FAILURE;
        }

        /* AN5130 page 38, option b. */
        k_busy_wait (100000);

        /* Route the wake-up interrupt signal to INT1 pin. */
        // byte = 0x20;
        // if (I2C_bytes_write (I2C_CH_1, LSM6DS3_I2C_DEV_ADDR, LSM6DS3TR_C_MD1_CFG, &byte, 1) != ERRNO_SUCCESS)
        // {
        //     return ERRNO_I2C_FAILURE;
        // }

        /* Configure GPIO for interrupts and register callback. */
        if (GPIO_cfg (GPIO_PIN_IMU_INT, GPIO_DIRECTION_IN, GPIO_PULLDOWN, GPIO_INIT_LOW, GPIO_INT_LL_HIGH, NULL) != ERRNO_SUCCESS)
        {
            return ERRNO_GPIO_FAILURE;
        }
    }
    else
    {
        /* Disable accelerometer. */
        byte = 0x00;
        if (I2C_bytes_write (I2C_CH_1, LSM6DS3_I2C_DEV_ADDR, LSM6DS3TR_C_CTRL1_XL, &byte, 1) != ERRNO_SUCCESS)
        {
            return ERRNO_I2C_FAILURE;
        }
    }

    /* Disable gyro. */
    byte = 0x00;
    if (I2C_bytes_write (I2C_CH_1, LSM6DS3_I2C_DEV_ADDR, LSM6DS3TR_C_CTRL2_G, &byte, 1) != ERRNO_SUCCESS)
    {
        return ERRNO_I2C_FAILURE;
    }

    return ERRNO_SUCCESS;
}

void IMU_task_install(void)
{
    static        k_tid_t  id;
    static struct k_thread data;

    id = k_thread_create(&data,
                          _imu_task_stack,
                          IMU_TASK_STACK_SIZE,
                          _imu_task,
                          NULL,
                          NULL,
                          NULL,
                          IMU_TASK_THREAD_PRIORITY,
                          0,
                          K_NO_WAIT);
    k_thread_name_set(&data, "IMU_task");
}

void IMU_interrupt_handler_task_install(void)
{
    static        k_tid_t           id;
    static struct k_thread          data;

    id = k_thread_create (&data,
                          _imu_interrrupt_handler_task_stack,
                          IMU_INTERRUPT_HANDLER_TASK_STACK_SIZE,
                          _imu_interrupt_handler_task,
                          NULL,
                          NULL,
                          NULL,
                          IMU_INTERRUPT_HANDLER_THREAD_PRIORITY,
                          0,
                          K_NO_WAIT);
    k_thread_name_set(&data, "IMU_interrupt_handler_task");
}

void IMU_cli_cmd (int32_t argc, char **argv)
{
    ERRNO_e err;

    if (argc == 1) {
        if (!strcmp (argv[0], "--reset")) {
            PRINT (INFO, "Ret code : %d\r\n", IMU_reset ());
        }
        else if (!strcmp (argv[0], "-i") || !strcmp (argv[0], "--init")) {
            PRINT (INFO, "Ret code : %d\r\n", IMU_init ());
        }
        else if (!strcmp (argv[0], "-t") || !strcmp (argv[0], "--temp")) {
            float celsius;

            err = IMU_temp_c_read (&celsius);
            PRINT (INFO, "Ret code : %d\r\n", err);

            if (err)
            {
                return;
            }

            PRINT (INFO, "Celsius    : %.3f\r\n", (double)celsius);
            PRINT (INFO, "Fahrenheit : %.3f\r\n", (((double)celsius * 9.0) / 5.0 + 32.0));
        } else if (!strcmp (argv[0], "-ds") || !strcmp (argv[0], "--dumpstatistic")) {

            _imu_statistic_dump = true;

        } else if (!strcmp (argv[0], "-r") || !strcmp (argv[0], "--read")) {
            IMU_data_s xl;
            IMU_data_s gyro;

            err = (IMU_xl_read (&xl) | IMU_gyro_read (&gyro));
            PRINT (INFO, "Ret code : %d\r\n", err);

            if (err) {
                return;
            }
            else {
                PRINT (INFO, "Accel  X : %f\r\n", (double) xl.x);
                PRINT (INFO, "       Y : %f\r\n", (double) xl.y);
                PRINT (INFO, "       Z : %f\r\n", (double) xl.z);
                PRINT (INFO, "Gyro   X : %f\r\n", (double) gyro.x);
                PRINT (INFO, "       Y : %f\r\n", (double) gyro.y);
                PRINT (INFO, "       Z : %f\r\n", (double) gyro.z);
            }
        } else if (!strcmp (argv[0], "-rr") || !strcmp (argv[0], "--readraw")) {
            int16_t xl_x, xl_y, xl_z;
            int16_t gyro_x, gyro_y, gyro_z;
            IMU_data_s xl;
            IMU_data_s gyro;
            err = (IMU_xl_raw_x_read (&xl_x) | IMU_xl_raw_y_read (&xl_y) | IMU_xl_raw_z_read (&xl_z) | IMU_gyro_raw_x_read (&gyro_x) | IMU_gyro_raw_y_read (&gyro_y) | IMU_gyro_raw_z_read (&gyro_z));
            xl.x = _imu_xl_raw_to_float(xl_x);
            xl.y = _imu_xl_raw_to_float(xl_y);
            xl.z = _imu_xl_raw_to_float(xl_z);
            gyro.x = _imu_gyro_raw_to_float(gyro_x);
            gyro.y = _imu_gyro_raw_to_float(gyro_y);
            gyro.z = _imu_gyro_raw_to_float(gyro_z);
            PRINT (INFO, "Ret code : %d\r\n", err);

            if (err) {
                return;
            }
            else {
                PRINT(INFO, "Accel  X : %d\r\n", (int) xl_x);
                PRINT(INFO, "       Y : %d\r\n", (int) xl_y);
                PRINT(INFO, "       Z : %d\r\n", (int) xl_z);
                //PRINT(INFO, "Gyro   X : %d\r\n", (int) gyro_x);
                //PRINT(INFO, "       Y : %d\r\n", (int) gyro_y);
                //PRINT(INFO, "       Z : %d\r\n", (int) gyro_z);
                //PRINT(INFO, "Accel  X : %f\r\n", (double) xl.x);
                //PRINT(INFO, "       Y : %f\r\n", (double) xl.y);
                //PRINT(INFO, "       Z : %f\r\n", (double) xl.z);
                //PRINT(INFO, "Gyro   X : %f\r\n", (double) gyro.x);
                //PRINT(INFO, "       Y : %f\r\n", (double) gyro.y);
                //PRINT(INFO, "       Z : %f\r\n", (double) gyro.z);
            }
        } else if (!strcmp (argv[0], "-rxlsr") || !strcmp (argv[0], "--readxlsamplerate")) {
            LSM6DS3TR_C_ODR_XL_e value;
            float sample_rate = 0;
            IMU_get_odr_xl(&value);
            switch (value) {
                case LSM6DS3TR_C_ODR_XL_POWER_DOWN: sample_rate = 0;    break;
                case LSM6DS3TR_C_ODR_XL_12_5Hz:     sample_rate = 12.5; break;
                case LSM6DS3TR_C_ODR_XL_26Hz:       sample_rate = 26;   break;
                case LSM6DS3TR_C_ODR_XL_52Hz:       sample_rate = 52;   break;
                case LSM6DS3TR_C_ODR_XL_104Hz:      sample_rate = 104;  break;
                case LSM6DS3TR_C_ODR_XL_208Hz:      sample_rate = 208;  break;
                case LSM6DS3TR_C_ODR_XL_416Hz:      sample_rate = 416;  break;
                case LSM6DS3TR_C_ODR_XL_833Hz:      sample_rate = 833;  break;
                case LSM6DS3TR_C_ODR_XL_1666Hz:     sample_rate = 1666; break;
                case LSM6DS3TR_C_ODR_XL_3332Hz:     sample_rate = 3332; break;
                case LSM6DS3TR_C_ODR_XL_6664Hz:     sample_rate = 6664; break;
                case LSM6DS3TR_C_ODR_XL_1_6Hz:      sample_rate = 1.6;  break;
            }
            PRINT(INFO, "Accelerometer Sample rate : %.1fHz\r\n", (double)sample_rate);
        } else if (!strcmp (argv[0], "-rxlr") || !strcmp (argv[0], "--readxlrange")) {
            LSM6DS3TR_C_FS_XL_e value;
            uint8_t range = 0;
            IMU_get_fs_xl(&value);
            switch (value) {
                case LSM6DS3TR_C_FS_XL_2g:  range = 2;  break;
                case LSM6DS3TR_C_FS_XL_4g:  range = 4;  break;
                case LSM6DS3TR_C_FS_XL_8g:  range = 8;  break; 
                case LSM6DS3TR_C_FS_XL_16g: range = 16; break;
            }
            PRINT(INFO, "Accelerometer range : %d g\r\n", range);
        } else if (!strcmp (argv[0], "-rgsr") || !strcmp (argv[0], "--readgyrosamplerate")) {
            LSM6DS3TR_C_ODR_G_e value;
            float sample_rate = 0;
            IMU_get_odr_g(&value);
            switch (value) {
                case LSM6DS3TR_C_ODR_G_POWER_DOWN: sample_rate = 0;    break;
                case LSM6DS3TR_C_ODR_G_12_5Hz:     sample_rate = 12.5; break;
                case LSM6DS3TR_C_ODR_G_26Hz:       sample_rate = 26;   break;
                case LSM6DS3TR_C_ODR_G_52Hz:       sample_rate = 52;   break;
                case LSM6DS3TR_C_ODR_G_104Hz:      sample_rate = 104;  break;
                case LSM6DS3TR_C_ODR_G_208Hz:      sample_rate = 208;  break;
                case LSM6DS3TR_C_ODR_G_416Hz:      sample_rate = 416;  break;
                case LSM6DS3TR_C_ODR_G_833Hz:      sample_rate = 833;  break;
                case LSM6DS3TR_C_ODR_G_1666Hz:     sample_rate = 1666; break;
                case LSM6DS3TR_C_ODR_G_3332Hz:     sample_rate = 3332; break;
                case LSM6DS3TR_C_ODR_G_6664Hz:     sample_rate = 6664; break;
            }
            PRINT(INFO, "Gyro Sample rate : %.1fHz\r\n", (double)sample_rate);
        } else if (!strcmp (argv[0], "-rgr") || !strcmp (argv[0], "--readgyrorange")) {
            LSM6DS3TR_C_FS_G_ABSTR_e value;
            uint16_t range = 0;
            IMU_get_fs_g(&value);
            switch (value) {
                case LSM6DS3TR_C_FS_G_ABSTR_125dps:  range = 125;  break;
                case LSM6DS3TR_C_FS_G_ABSTR_250dps:  range = 250;  break;
                case LSM6DS3TR_C_FS_G_ABSTR_500dps:  range = 500;  break;
                case LSM6DS3TR_C_FS_G_ABSTR_1000dps: range = 1000; break;
                case LSM6DS3TR_C_FS_G_ABSTR_2000dps: range = 2000; break;
            }
            PRINT(INFO, "Gyro range : %d dps\r\n", range);
        } else if (!strcmp (argv[0], "-rs") || !strcmp (argv[0], "--readshock")) {
            LSM6DS3TR_C_SHOCK_e value;
            const char *shock = "UNKNOWN";
            ERRNO_e err = IMU_get_shock(&value);
            
            if (err != ERRNO_SUCCESS) {
                PRINT(ERROR, "Failed to read shock setting: %d\r\n", err);
                return;
            }
            
            switch (value) {
                case LSM6DS3TR_C_SHOCK_4_OVER_ODR_XL:  shock = "4/ODR_XL";  break;
                case LSM6DS3TR_C_SHOCK_8_OVER_ODR_XL:  shock = "8/ODR_XL";  break;
                case LSM6DS3TR_C_SHOCK_16_OVER_ODR_XL: shock = "16/ODR_XL"; break;
                case LSM6DS3TR_C_SHOCK_24_OVER_ODR_XL: shock = "24/ODR_XL"; break;
            }
            PRINT(INFO, "Tap shock duration: %s\r\n", shock);
        } else if (!strcmp (argv[0], "-rq") || !strcmp (argv[0], "--readquiet")) {
            LSM6DS3TR_C_QUIET_e value;
            const char *quiet = "UNKNOWN";
            ERRNO_e err = IMU_get_quiet(&value);
            
            if (err != ERRNO_SUCCESS) {
                PRINT(ERROR, "Failed to read quiet setting: %d\r\n", err);
                return;
            }
            
            switch (value) {
                case LSM6DS3TR_C_QUIET_2_OVER_ODR_XL:  quiet = "2/ODR_XL";  break;
                case LSM6DS3TR_C_QUIET_4_OVER_ODR_XL:  quiet = "4/ODR_XL";  break;
                case LSM6DS3TR_C_QUIET_8_OVER_ODR_XL:  quiet = "8/ODR_XL";  break;
                case LSM6DS3TR_C_QUIET_12_OVER_ODR_XL: quiet = "16/ODR_XL"; break;
            }
            PRINT(INFO, "Tap quiet duration: %s\r\n", quiet);
        } else if (!strcmp (argv[0], "-rths") || !strcmp (argv[0], "--readtapthreshold")) {
            uint8_t value;
            LSM6DS3TR_C_FS_XL_e range;
            uint8_t fs_xl_g;
            ERRNO_e err = IMU_get_tap_ths(&value);

            if (err != ERRNO_SUCCESS) {
                PRINT(ERROR, "Failed to read tap threshold setting: %d\r\n", err);
                return;
            }

            err = IMU_get_fs_xl(&range); 
            if (err != ERRNO_SUCCESS) {
                PRINT(ERROR, "Failed to read accelerometer range setting: %d\r\n", err);
                return;
            }

            switch (range) {
                case LSM6DS3TR_C_FS_XL_2g:  fs_xl_g = 2; break;
                case LSM6DS3TR_C_FS_XL_4g:  fs_xl_g = 4; break;
                case LSM6DS3TR_C_FS_XL_8g:  fs_xl_g = 8; break;
                case LSM6DS3TR_C_FS_XL_16g: fs_xl_g = 16; break;
            }
            PRINT(INFO, "Tap threshold: %d, %f g\r\n", value, (float) (value * fs_xl_g/32.0f));
        } else if (!strcmp (argv[0], "-rws") || !strcmp (argv[0], "--readwakeupsrc")) {
            uint8_t value;

            ERRNO_e err = I2C_bytes_read(I2C_CH_1, LSM6DS3_I2C_DEV_ADDR, LSM6DS3TR_C_WAKE_UP_SRC, &value, 1);

            if (err != ERRNO_SUCCESS) {
                PRINT(ERROR, "Failed to read wakeup src reg: %d\r\n", err);
                return;
            }

            PRINT(INFO, "Wakeup src reg value: %d\r\n", value);
   
        } else if (!strcmp (argv[0], "-ri") || !strcmp (argv[0], "--readint1cfg")) {
            uint8_t value;

            ERRNO_e err = I2C_bytes_read(I2C_CH_1, LSM6DS3_I2C_DEV_ADDR, LSM6DS3TR_C_INT1_CTRL, &value, 1);

            if (err != ERRNO_SUCCESS) {
                PRINT(ERROR, "Failed to read int1 ctrl reg: %d\r\n", err);
                return;
            }

            PRINT(INFO, "Int1 ctrl reg value: 0x%02X\r\n", value);
   
        } else if (!strcmp (argv[0], "-rmd") || !strcmp (argv[0], "--readmd1cfg")) {
            uint8_t value;

            ERRNO_e err = I2C_bytes_read(I2C_CH_1, LSM6DS3_I2C_DEV_ADDR, LSM6DS3TR_C_MD1_CFG, &value, 1);

            if (err != ERRNO_SUCCESS) {
                PRINT(ERROR, "Failed to read md1 cfg reg: %d\r\n", err);
                return;
            }

            PRINT(INFO, "MD1 cfg reg value: 0x%02X\r\n", value);
   
        } else if (!strcmp (argv[0], "-stc") || !strcmp (argv[0], "--settapconfig")) {
            ERRNO_e err = IMU_set_interrupts_enable(LSM6DS3TR_C_INTERRUPTS_ENABLE);

            if (err != ERRNO_SUCCESS) {
                PRINT(ERROR, "Failed to set tap config reg: %d\r\n", err);
                return;
            }
        } else {
            goto USAGE;
        }
    }
    else if (argc == 2)
    {
        if (!strcmp (argv[0], "-ftr") || !strcmp (argv[0], "--fifothresholdread"))
        {

            _imu_cfg.field_v.fifo_threshold_v = (uint16_t)strtoul (argv[1], NULL, 0);
            if (_imu_cfg.field_v.fifo_threshold_v <= 2047 && _imu_cfg.field_v.fifo_threshold_v >= 0) {
                PRINT (INFO, "Ret code : %d\r\n", IMU_fifo_th_set(_imu_cfg.field_v.fifo_threshold_v));
            }
            else {
                goto USAGE;
            }
        }
        else if (!strcmp (argv[0], "-d") || !strcmp (argv[0], "--dump"))
        {
            _imu_data_dump_time_ms = strtoul (argv[1], NULL, 0);

            _imu_data_dump = true;

            PRINT (INFO, "//Dumping IMU data for %d milliseconds...\r\n//ACCEL.x,ACCEL.y,ACCEL.z,GYRO.x,GYRO.y,GYRO.z\r\n", _imu_data_dump_time_ms);
        }
        else if (!strcmp (argv[0], "-ge") || !strcmp (argv[0], "--gyroenable"))
        {
            if (!strcmp (argv[1], "true"))
            {
                PRINT (INFO, "Ret code : %d\r\n", IMU_gyro_enable (true));
            }
            else if (!strcmp (argv[1], "false"))
            {
                PRINT (INFO, "Ret code : %d\r\n", IMU_gyro_enable (false));
            }
            else
            {
                goto USAGE;
            }
        }
        else if (!strcmp(argv[0], "-gr") || !strcmp(argv[0], "--gyrorange"))
        {
            LSM6DS3TR_C_FS_G_ABSTR_e value;
            uint16_t range = (uint16_t) strtoul(argv[1], NULL, 0);
            switch (range) {
                case 125:  value = LSM6DS3TR_C_FS_G_ABSTR_125dps;  break;
                case 250:  value = LSM6DS3TR_C_FS_G_ABSTR_250dps;  break;
                case 500:  value = LSM6DS3TR_C_FS_G_ABSTR_500dps;  break;
                case 1000: value = LSM6DS3TR_C_FS_G_ABSTR_1000dps; break;
                case 2000: value = LSM6DS3TR_C_FS_G_ABSTR_2000dps; break;
                default: goto USAGE;
            }

            PRINT(INFO, "Ret code : %d\r\n", IMU_set_fs_g(value));
        }
        else if (!strcmp(argv[0], "-gsr") || !strcmp(argv[0], "--gyrosamplerate"))
        {
            LSM6DS3TR_C_ODR_G_e value;
            uint16_t sample_rate = (uint16_t) strtoul(argv[1], NULL, 0);
            switch (sample_rate) {
                case 13:   value = LSM6DS3TR_C_ODR_G_12_5Hz; break;
                case 26:   value = LSM6DS3TR_C_ODR_G_26Hz;   break;
                case 52:   value = LSM6DS3TR_C_ODR_G_52Hz;   break;
                case 104:  value = LSM6DS3TR_C_ODR_G_104Hz;  break;
                case 208:  value = LSM6DS3TR_C_ODR_G_208Hz;  break;
                case 416:  value = LSM6DS3TR_C_ODR_G_416Hz;  break;
                case 833:  value = LSM6DS3TR_C_ODR_G_833Hz;  break;
                case 1666: value = LSM6DS3TR_C_ODR_G_1666Hz; break;
                case 3332: value = LSM6DS3TR_C_ODR_G_3332Hz; break;
                case 6664: value = LSM6DS3TR_C_ODR_G_6664Hz; break;
                default: goto USAGE;
            }
            PRINT (INFO, "Ret code : %d\r\n", IMU_set_odr_g(value));
        }
        else if (!strcmp (argv[0], "-gle") || !strcmp (argv[0], "--gyrolowpassen"))
        {
            if (!strcmp (argv[1], "true"))
            {
                PRINT (INFO, "Ret code : %d\r\n", IMU_set_lpf1_sel_g(LSM6DS3TR_C_LPF1_SEL_G_ENABLE));
            }
            else if (!strcmp (argv[1], "false"))
            {
                PRINT (INFO, "Ret code : %d\r\n", IMU_set_lpf1_sel_g(LSM6DS3TR_C_LPF1_SEL_G_DISABLE));
            }
            else
            {
                goto USAGE;
            }
        }
        else if (!strcmp (argv[0], "-glb") || !strcmp (argv[0], "--gyrolowband"))
        {
            LSM6DS3TR_C_FTYPE_e bandwidth = (LSM6DS3TR_C_FTYPE_e) strtoul (argv[1], NULL, 0);
            switch (bandwidth) {
                case LSM6DS3TR_C_FTYPE_0: break;
                case LSM6DS3TR_C_FTYPE_1: break;
                case LSM6DS3TR_C_FTYPE_2: break;
                case LSM6DS3TR_C_FTYPE_3: break;
                default: goto USAGE;
            }
            PRINT (INFO, "Ret code : %d\r\n", IMU_set_ftype(bandwidth));
        }
        else if (!strcmp (argv[0], "-xe") || !strcmp (argv[0], "--xlenable"))
        {
            if (!strcmp (argv[1], "true"))
            {
                PRINT (INFO, "Ret code : %d\r\n", IMU_xl_enable(true));
            }
            else if (!strcmp (argv[1], "false"))
            {
                PRINT (INFO, "Ret code : %d\r\n", IMU_xl_enable(false));
            }
            else
            {
                goto USAGE;
            }
        }
        else if (!strcmp(argv[0], "-xr") || !strcmp(argv[0], "--xlrange"))
        {
            LSM6DS3TR_C_FS_G_e value;
            uint8_t range = (uint8_t) strtoul(argv[1], NULL, 0);
            switch (range) {
                case 2:  value = LSM6DS3TR_C_FS_XL_2g;  break;
                case 4:  value = LSM6DS3TR_C_FS_XL_4g;  break;
                case 8:  value = LSM6DS3TR_C_FS_XL_8g;  break;
                case 16: value = LSM6DS3TR_C_FS_XL_16g; break;
                default: goto USAGE;
            }

            PRINT(INFO, "Ret code : %d\r\n", IMU_set_fs_xl(value));
        }
        else if (!strcmp(argv[0], "-xsr") || !strcmp(argv[0], "--xlsamplerate"))
        {
            LSM6DS3TR_C_ODR_XL_e value;
            uint16_t sample_rate = (uint16_t) strtoul(argv[1], NULL, 0);
            switch (sample_rate) {
                case 13:   value = LSM6DS3TR_C_ODR_G_12_5Hz; break;
                case 26:   value = LSM6DS3TR_C_ODR_G_26Hz;   break;
                case 52:   value = LSM6DS3TR_C_ODR_G_52Hz;   break;
                case 104:  value = LSM6DS3TR_C_ODR_G_104Hz;  break;
                case 208:  value = LSM6DS3TR_C_ODR_G_208Hz;  break;
                case 416:  value = LSM6DS3TR_C_ODR_G_416Hz;  break;
                case 833:  value = LSM6DS3TR_C_ODR_G_833Hz;  break;
                case 1666: value = LSM6DS3TR_C_ODR_G_1666Hz; break;
                case 3332: value = LSM6DS3TR_C_ODR_G_3332Hz; break;
                case 6664: value = LSM6DS3TR_C_ODR_G_6664Hz; break;
                case 2:    value = LSM6DS3TR_C_ODR_XL_1_6Hz; break;
                default: goto USAGE;
            }
            PRINT(INFO, "Ret code : %d\r\n", IMU_set_odr_xl(value));
        }
        else if (!strcmp(argv[0], "-xlpf1b") || !strcmp(argv[0], "--xlow1band"))
        {
            LSM6DS3TR_C_LPF1_BW_SEL_e bandwidth = (LSM6DS3TR_C_FTYPE_e) strtoul(argv[1], NULL, 0);
            switch (bandwidth) {
                case LSM6DS3TR_C_LPF1_BW_SEL_ODR_DIV_2: break;
                case LSM6DS3TR_C_LPF1_BW_SEL_ODR_DIV_4: break;
                default: goto USAGE;
            }
            PRINT(INFO, "Ret code : %d\r\n", IMU_set_lpf1_bw_sel(bandwidth));
        }
        else if (!strcmp (argv[0], "-gfd") || !strcmp (argv[0], "--gyrofifodecimate"))
        {
            LSM6DS3TR_C_DEC_FIFO_GYRO_e value;
            uint8_t factor = (uint8_t) strtoul(argv[1], NULL, 0);
            switch (factor) {
                case 0: value = LSM6DS3TR_C_DEC_FIFO_GYRO_DATA_NOT_IN_FIFO; break;
                case 1: value = LSM6DS3TR_C_DEC_FIFO_GYRO_NO_DECIMATION;    break;
                case 2: value = LSM6DS3TR_C_DEC_FIFO_GYRO_DECIMATION_BY_2;  break;
                case 3: value = LSM6DS3TR_C_DEC_FIFO_GYRO_DECIMATION_BY_3;  break;
                case 4: value = LSM6DS3TR_C_DEC_FIFO_GYRO_DECIMATION_BY_4;  break;
                case 5: value = LSM6DS3TR_C_DEC_FIFO_GYRO_DECIMATION_BY_8;  break;
                case 6: value = LSM6DS3TR_C_DEC_FIFO_GYRO_DECIMATION_BY_16; break;
                case 7: value = LSM6DS3TR_C_DEC_FIFO_GYRO_DECIMATION_BY_32; break;
                default: goto USAGE;
            }
            PRINT (INFO, "Ret code : %d\r\n", IMU_set_dec_fifo_gyro(value));
        }
        else if (!strcmp (argv[0], "-xfd") || !strcmp (argv[0], "--xlfifodecimate"))
        {
            LSM6DS3TR_C_DEC_FIFO_XL_e value;
            uint8_t factor = (uint8_t) strtoul(argv[1], NULL, 0);
            switch (factor) {
                case 0: value = LSM6DS3TR_C_DEC_FIFO_XL_DATA_NOT_IN_FIFO; break;
                case 1: value = LSM6DS3TR_C_DEC_FIFO_XL_NO_DECIMATION;    break;
                case 2: value = LSM6DS3TR_C_DEC_FIFO_XL_DECIMATION_BY_2;  break;
                case 3: value = LSM6DS3TR_C_DEC_FIFO_XL_DECIMATION_BY_3;  break;
                case 4: value = LSM6DS3TR_C_DEC_FIFO_XL_DECIMATION_BY_4;  break;
                case 5: value = LSM6DS3TR_C_DEC_FIFO_XL_DECIMATION_BY_8;  break;
                case 6: value = LSM6DS3TR_C_DEC_FIFO_XL_DECIMATION_BY_16; break;
                case 7: value = LSM6DS3TR_C_DEC_FIFO_XL_DECIMATION_BY_32; break;
                default: goto USAGE;
            }
            PRINT (INFO, "Ret code : %d\r\n", IMU_set_dec_fifo_xl(value));
        }
        else
        {
            goto USAGE;
        }
    }
    else if (argc == 3)
    {
        if (!strcmp (argv[0], "-d") || !strcmp (argv[0], "--dump"))
        {
            _imu_data_dump_time_ms = strtoul (argv[1], NULL, 0);

            if (!strcmp (argv[2], "hex"))
            {
                _imu_data_dump_hex = true;
            }
            _imu_data_dump = true;

            PRINT (INFO, "//Dumping IMU data for %d milliseconds...\r\n//ACCEL.x,ACCEL.y,ACCEL.z,GYRO.x,GYRO.y,GYRO.z\r\n", _imu_data_dump_time_ms);
        }
        else
        {
            goto USAGE;
        }
    }
    else
    {
USAGE:
        PRINT (INFO, "Usage: imu [OPTION...]\r\n");
        PRINT (INFO, "         --reset                Resets LSM6DS3 IMU\r\n");
        PRINT (INFO, "  -i   | --init                 Initializes LSM6DS3 IMU\r\n");
        PRINT (INFO, "  -t   | --temp                 Gets the IMU temperature\r\n");
        PRINT (INFO, "  -r   | --read                 Reads IMU measurement\r\n");
        PRINT (INFO, "  -lp  | --sleep                Puts the IMU to sleep\r\n");
        PRINT (INFO, "  -ftr | ----fifothresholdread  Read fifo when threshold reached \r\n");
        PRINT (INFO, "    <%%d>                         threshold level 1 - 2023, 0 to stop fifo\r\n");
        PRINT (INFO, "  -d   | --dump                Dumps IMU raw data\r\n");
        PRINT (INFO, "    <%%d>                         number of milliseconds to dump for\r\n");
        PRINT (INFO, "      hex                      [optional] dumps data in hexadecimal format\r\n");
        PRINT (INFO, "  -ge  | --gyroenable          Enables / Disables gyroscope\r\n");
        PRINT (INFO, "    <%%s>                         true - enable , false - disable\r\n");
        PRINT (INFO, "  -gr  | --gyrorange           Max deg/s\r\n");
        PRINT (INFO, "    <%%d>                         [125, 245, 500, 1000, 2000]\r\n");
        PRINT (INFO, "  -gsr | --gyrosamplerate      Gyroscope sample rate (Hz)\r\n");
        PRINT (INFO, "    <%%d>                         [13, 26, 52, 104, 208, 416, 833, 1666, 3332, 6664]\r\n");
        PRINT (INFO, "  -gle | --gyrolowpassen       Enables / Disables gyroscope lowpass filter\r\n");
        PRINT (INFO, "    <%%s>                          true - enable , false - disable\r\n");
        PRINT (INFO, "  -glb | --gyrolowband         Gyroscope lowpass bandwidth select\r\n");
        PRINT (INFO, "    <%%d>                         [0, 1, 2, 3] (see table 68 for detailed bandwidth selecton)\r\n");
        PRINT (INFO, "  -xe  | --xlenable            Enables / Disables accelerometer\r\n");
        PRINT (INFO, "    <%%s>                         true - enable , false - disable\r\n");
        PRINT (INFO, "  -xr  | --xlrange             Max G force\r\n");
        PRINT (INFO, "    <%%d>                         [2, 4, 8, 16]\r\n");
        PRINT (INFO, "  -xsr | --xlsamplerate        Accelerometer sample rate (Hz)\r\n");
        PRINT (INFO, "    <%%d>                         [13, 26, 52, 104, 208, 416, 833, 1666, 3332, 6664, 2]\r\n");
        PRINT (INFO, "  -xlpf1b  | --xlow1band       Accelerometer lpf1 bandwidth\r\n");
        PRINT (INFO, "    <%%d>                         [0 - odr/2, 1 - odr/4]\r\n");
        PRINT (INFO, "  -gfd | --gyrofifodecimate    Fifo decimation\r\n");
        PRINT (INFO, "    <%%d>                         [0 - disable, 1, 2, 3, 4, 5 - 8, 6 - 16, 7 - 32]\r\n");
        PRINT (INFO, "  -xfd | --xlfifodecimate      Fifo decimation\r\n");
        PRINT (INFO, "    <%%d>                         [0 - disable, 1, 2, 3, 4, 5 - 8, 6 - 16, 7 - 32]\r\n");
    }
}

/*--------------------------------------------------------------------------------------------------
 * PRIVATE FUNCTION DEFINITIOPNS
 *------------------------------------------------------------------------------------------------*/
static void _imu_task(void *p1, void *p2, void *p3)
{
    while (1) {
        DBG_STAT_INC(imu_task_counter);

        DBG_STAT_INC_IF(i2c_transfer_active,imu_task_during_i2c_counter);

        if (_imu_statistic_dump)
        {
            PRINT (INFO, "//Dumping statistic data... \r\n");
            _imu_statistic_dump = false;

            /* get a copy of statistic data */
            #ifdef CONFIG_DBG_STATS
            atomic_val_t interrupt_ctr = atomic_get(&_interrupt_count);
            k_mutex_lock(&stat_mutex, K_FOREVER);
            uint32_t _fifo_read_task_counter = fifo_read_task_counter;
            uint32_t _read_shot_data_counter = read_shot_data_counter;
            uint32_t _read_into_dummy_counter = read_into_dummy_counter;
            uint32_t _fifo_read_FTH_counter = fifo_read_FTH_counter;
            uint32_t _read_shot_data_after_fifo_read_counter = read_shot_data_after_fifo_read_counter;
            uint32_t _fifo_waterM_total_hit_counter = fifo_waterM_total_hit_counter;
            uint32_t _fifo_waterM_hit_counter = fifo_waterM_hit_counter;
            uint32_t _fifo_overrun_counter = fifo_overrun_counter;
            uint32_t _unread_words_before_read = unread_words_before_read;
            uint32_t _unread_words_after_read = unread_words_after_read;
            uint32_t _max_unread_words_before_read = max_unread_words_before_read;
            uint32_t _max_unread_words_after_read = max_unread_words_after_read;
            k_mutex_unlock(&stat_mutex);
            #endif

            k_mutex_lock(&shot_data_mutex, K_FOREVER);
            bool _marked = shot_data.marked;
            bool _ready = shot_data.ready;
            bool _saved = shot_data.saved;
            k_mutex_unlock(&shot_data_mutex);

            #ifdef CONFIG_DBG_STATS
            PRINT(INFO, "fth interrupt is hit %u times !\r\n", interrupt_ctr);
            PRINT(INFO, "shot capture task has run %u times !\r\n", _fifo_read_task_counter);
            PRINT(INFO, "shot data is read into ring buffer %u times !\r\n", _read_shot_data_counter);
            PRINT(INFO, "fifo watermark full is totally hit %u times !\r\n", _fifo_waterM_total_hit_counter);
            PRINT(INFO, "fifo watermark full is truely hit %u times !\r\n", _fifo_waterM_hit_counter);
            PRINT(INFO, "fifo overrun is hit %u times !\r\n", _fifo_overrun_counter);
            PRINT(INFO, "fifo data is read into dummy buffer for %u times !\r\n", _read_into_dummy_counter);
            PRINT(INFO, "fifo FTH data is read into ring buffer %u times!\r\n", _fifo_read_FTH_counter);
            PRINT(INFO, "shot data is read after fifo FTH read %u times !\r\n", _read_shot_data_after_fifo_read_counter);
            PRINT(INFO, "last unread words before i2c read is %u !\r\n", _unread_words_before_read);
            PRINT(INFO, "last unread words after i2c read is %u !\r\n", _unread_words_after_read);
            PRINT(INFO, "maximum unread words before i2c read %u times !\r\n", _max_unread_words_before_read);
            PRINT(INFO, "maximum unread words after i2c read %u times !\r\n\r\n", _max_unread_words_after_read);
            PRINT(INFO, "_imu_task running during I2C transfer: %u times!\r\n", imu_task_during_i2c_counter);
            PRINT(INFO, "_imu_task running: %u times!\r\n\r\n", imu_task_counter);
            PRINT(INFO, "_fsm_task running during I2C transfer: %u times!\r\n", fsm_task_during_i2c_counter);
            PRINT(INFO, "_fsm_task running: %u times!\r\n\r\n", fsm_task_counter);
            PRINT(INFO, "_event_task running during I2C transfer: %u times!\r\n", event_task_during_i2c_counter);
            PRINT(INFO, "_event_task running: %u times!\r\n\r\n", event_task_counter);
            #endif

            if (_marked){
                PRINT(INFO, "shot data is marked!\r\n");
            } else {
                PRINT(INFO, "shot data is not marked!\r\n");
            }
            if (_ready) {
                PRINT(INFO, "shot data is ready!\r\n");
            } else {
                PRINT(INFO, "shot data is not ready!\r\n");
            }
            if (_saved) {
                PRINT(INFO, "shot data is saved!\r\n");
            } else {
                PRINT(INFO, "shot data is not saved!\r\n");
            }

            // PRINT (INFO, "_imu_task running: %u times !\r\n", imu_task_counter);
            // PRINT (INFO, "_imu_task running during busy wait: %u times!\r\n\r\n", imu_task_during_busy_wait_counter);
            // PRINT (INFO, "_interface_task running: %u times!\r\n", _interface_task_counter);
            // PRINT (INFO, "_interface_task running during I2C transfer: %u times!\r\n\r\n", _interface_task_during_i2c_counter);
        }
        
        switch (_imu_int_source)
        {
            case IMU_INT_SOURCE_WAKEUP:
                if (_imu_wakeup_cb != NULL)
                {
                    _imu_wakeup_cb ();
                }
                break;

            default:
        }

        /* Clear interrupt source. */
        _imu_int_source = IMU_INT_SOURCE_NONE;

        if (_imu_data_dump)
        {
            static uint32_t timer_cnt = IMU_TASK_PERIOD_MS;
                   int16_t  xlx, xly, xlz;
                   int16_t  gyx, gyy, gyz;

            (void) IMU_xl_raw_x_read (&xlx);
            (void) IMU_xl_raw_y_read (&xly);
            (void) IMU_xl_raw_z_read (&xlz);
            (void) IMU_gyro_raw_x_read (&gyx);
            (void) IMU_gyro_raw_y_read (&gyy);
            (void) IMU_gyro_raw_z_read (&gyz);

            if (_imu_data_dump_hex)
            {
                PRINT(INFO, "%X,%X,%X,%X,%X,%X\r\n", xlx, xly, xlz, gyx, gyy, gyz);
            }
            else
            {
                PRINT(INFO, "%d,%d,%d,%d,%d,%d\r\n", xlx, xly, xlz, gyx, gyy, gyz);
            }

            if (timer_cnt >= _imu_data_dump_time_ms)
            {
                /* Reset timer counter. */
                timer_cnt = 0;

                /* Clear dump flag. */
                _imu_data_dump = false;

                /* Clear hex flag. */
                _imu_data_dump_hex = false;
            }
            else
            {
                timer_cnt += IMU_TASK_PERIOD_MS;
            }
        }

        k_msleep(IMU_TASK_PERIOD_MS);
    }
}

static void _imu_interrupt_handler_task(void *p1, void *p2, void *p3)
{
    uint8_t reg;

    while (1) {
        k_sem_take(&_imu_int_sem, K_FOREVER);

        if (atomic_test_bit(&_imu_cfg.interrupt_mode_flags, IMU_int_wake_up_mode)) {
            if (I2C_bytes_read(I2C_CH_1, LSM6DS3_I2C_DEV_ADDR, LSM6DS3TR_C_WAKE_UP_SRC, &reg, 1)) {
                PRINT (ERROR, "i2c read fifo status regs failed in isr!\r\n");
            }
            if (reg & 0x08) {
                /* FSM wakes up from idle disable IMU motion wake up function until fsm goes to idle again */
                MOTION_detected_flag_set();
                IMU_motion_int_enable(false, 0x2, 0x1);
            }
        }

        if (atomic_test_bit(&_imu_cfg.interrupt_mode_flags, IMU_int_shot_capture_mode)) {
            k_sem_give(&shot_capture_sem);
        }
    }
}

static ERRNO_e _imu_who_am_i(void)
{
    ERRNO_e err;
    uint8_t result;

    err = I2C_bytes_read(I2C_CH_1, LSM6DS3_I2C_DEV_ADDR, LSM6DS3TR_C_WHO_AM_I_REG, &result, 1);
    if (err != ERRNO_SUCCESS) {
        return ERRNO_ID_FAILURE;
    }

    /* Setup the internal temperature sensor. */
    if (_imu_cfg.temp_enabled == 1) {
        /* 0x69 = LSM6DS3TR */
        if (result == LSM6DS3TR_WHO_AM_I) {
            /* Sensitivity to scale 16 */
            _imu_cfg.temp_sensitivity = 16;
        }
        /* 0x6A = LSM6DS3TR-C */
        else if (result == LSM6DS3TR_C_WHO_AM_I) {
            /* Sensitivity to scale 256 */
            _imu_cfg.temp_sensitivity = 256;
        }
    }

    return err;
}

static ERRNO_e _imu_cfg_init(void)
{
    ERRNO_e err;
    /* Gyroscope */
    err = IMU_set_fs_g(_imu_field_v_default.fs_g_v);
    if (err == ERRNO_SUCCESS) {
        err = IMU_set_odr_g(_imu_field_v_default.odr_g_v);
    }
    if (err == ERRNO_SUCCESS) {
        err = IMU_set_hp_en_g(_imu_field_v_default.hp_en_g_v);
    }
    if (err == ERRNO_SUCCESS) {
        err = IMU_set_hpm_g(_imu_field_v_default.hpm_g_v);
    }
    if (err == ERRNO_SUCCESS) {
        err = IMU_set_lpf1_sel_g(_imu_field_v_default.lpf1_sel_g_v);
    }
    if (err == ERRNO_SUCCESS) {
        err = IMU_set_ftype(_imu_field_v_default.ftype_v);
    }

    /* Accelerometer */
    if (err == ERRNO_SUCCESS) {
        err = IMU_set_fs_xl(_imu_field_v_default.fs_xl_v);
    }
    if (err == ERRNO_SUCCESS) {
        err = IMU_set_odr_xl(_imu_field_v_default.odr_xl_v);
    }
    if (err == ERRNO_SUCCESS) {
        err = IMU_set_lpf1_bw_sel(_imu_field_v_default.lpf1_bw_sel_v);
    }
    if (err == ERRNO_SUCCESS) {
        err = IMU_set_lpf2_xl_en(_imu_field_v_default.lpf2_xl_en_v);
    }
    if (err == ERRNO_SUCCESS) {
        err = IMU_set_hp_slope_xl_en(_imu_field_v_default.hp_slope_xl_en_v);
    }
    if (err == ERRNO_SUCCESS) {
        err = IMU_set_hpcf_xl(_imu_field_v_default.hpcf_xl_v);
    }
    if (err == ERRNO_SUCCESS) {
        err = IMU_set_input_composite(_imu_field_v_default.input_composite_v);
    }

    /* wakeup function */
    /* set the wake up threshold to be 2 so noise does not
     * trigger the interrupt
     */
    if (err == ERRNO_SUCCESS) {
        err = IMU_set_wk_ths(_imu_field_v_default.wk_ths_v);
    }
    

    return err;
}

static float _imu_xl_raw_to_float(int16_t raw)
{
    uint8_t factor;
    switch (_imu_cfg.field_v.fs_xl_v) {
        case LSM6DS3TR_C_FS_XL_2g: factor = 1; break;
        case LSM6DS3TR_C_FS_XL_4g: factor = 2; break;
        case LSM6DS3TR_C_FS_XL_8g: factor = 4; break;
        case LSM6DS3TR_C_FS_XL_16g: factor = 8; break;
        default: factor = 1;
    }

    return (float) raw * (float)0.061 * factor / 1000;
}

static float _imu_gyro_raw_to_float (int16_t raw)
{
    uint8_t factor;
    switch (_imu_cfg.field_v.fs_g_v) {
        case LSM6DS3TR_C_FS_G_ABSTR_125dps: factor = 1; break;
        case LSM6DS3TR_C_FS_G_ABSTR_250dps: factor = 2; break;
        case LSM6DS3TR_C_FS_G_ABSTR_500dps: factor = 4; break;
        case LSM6DS3TR_C_FS_G_ABSTR_1000dps: factor = 8; break;
        case LSM6DS3TR_C_FS_G_ABSTR_2000dps: factor = 16; break;
        default: factor = 1;
    }

    return (float) raw * (float)4.375 * (factor) / 1000;
}

static void _imu_isr(void)
{
    atomic_inc(&_interrupt_count);

    /* Signal IMU I2C fifo read thread to read IMU data via DMA */
    k_sem_give(&_imu_int_sem);
}
