SensorQMI8658.hpp

Go to the documentation of this file.

 
#pragma once
 
#include "SensorBuildOpt.h"
#if !SENSORLIB_EXCLUDE_QMI8658
 
#pragma message("Warning: Please replace with a new driver. Please refer to the examples/QMI8658 example. SensorQMI8658.hpp will be removed in the next version.")
 
#include "platform/comm/ComplexStaticDeviceWithHal.hpp"
#include <math.h>
 
static constexpr uint8_t  QMI8658_L_SLAVE_ADDRESS = (0x6B);
static constexpr uint8_t  QMI8658_H_SLAVE_ADDRESS = (0x6A);
 
typedef struct {
    float x;
    float y;
    float z;
} IMUdata;
 
class SensorQMI8658 : public ComplexStaticDeviceWithHal
{
public:
    typedef void (*EventCallBack_t)(void);
 
    enum AccelRange {
        ACC_RANGE_2G,           // ±2g
        ACC_RANGE_4G,           // ±4g
        ACC_RANGE_8G,           // ±8g
        ACC_RANGE_16G           // ±16g
    };
 
    enum GyroRange {
        GYR_RANGE_16DPS,        // 16 degrees per second
        GYR_RANGE_32DPS,        // 32 degrees per second
        GYR_RANGE_64DPS,        // 64 degrees per second
        GYR_RANGE_128DPS,       // 128 degrees per second
        GYR_RANGE_256DPS,       // 256 degrees per second
        GYR_RANGE_512DPS,       // 512 degrees per second
        GYR_RANGE_1024DPS,      // 1024 degrees per second
    };
 
    // In 6DOF mode (accelerometer and gyroscope are both enabled),
    // the output data rate is derived from the nature frequency of gyroscope
    enum AccelODR {
        ACC_ODR_1000Hz = 3,             // 1000 Hz
        ACC_ODR_500Hz,                  // 500 Hz
        ACC_ODR_250Hz,                  // 250 Hz
        ACC_ODR_125Hz,                  // 125 Hz
        ACC_ODR_62_5Hz,                 // 62.5 Hz
        ACC_ODR_31_25Hz,                // 31.25 Hz
        ACC_ODR_LOWPOWER_128Hz  = 12,   // The accelerometer low power mode is only available when the gyroscope is disabled
        ACC_ODR_LOWPOWER_21Hz,          // The accelerometer low power mode is only available when the gyroscope is disabled
        ACC_ODR_LOWPOWER_11Hz,          // The accelerometer low power mode is only available when the gyroscope is disabled
        ACC_ODR_LOWPOWER_3Hz            // The accelerometer low power mode is only available when the gyroscope is disabled
    };
 
    enum GyroODR {
        GYR_ODR_7174_4Hz,   // 7174.4 Hz
        GYR_ODR_3587_2Hz,   // 3587.2 Hz
        GYR_ODR_1793_6Hz,   // 1793.6 Hz
        GYR_ODR_896_8Hz,    // 896.8 Hz
        GYR_ODR_448_4Hz,    // 448.4 Hz
        GYR_ODR_224_2Hz,    // 224.2 Hz
        GYR_ODR_112_1Hz,    // 112.1 Hz
        GYR_ODR_56_05Hz,    // 56.05 Hz
        GYR_ODR_28_025Hz    // 28.025 Hz
    };
 
    enum TapEvent {
        INVALID_TAP,    // Invalid tap event
        SINGLE_TAP,     // Single tap event
        DOUBLE_TAP,     // Double tap event
    };
 
    //Low-Pass Filter.
    enum LpfMode {
        LPF_MODE_0,     //2.66% of output data rate
        LPF_MODE_1,     //3.63% of output data rate
        LPF_MODE_2,     //5.39% of output data rate
        LPF_MODE_3,     //13.37% of output data rate
        LPF_OFF,        //OFF Low-Pass Filter
    };
 
    enum MotionEvent {
        MOTION_TAP,          // Tap motion detected
        MOTION_ANT_MOTION,   // Any motion detected
        MOTION_NO_MOTION,    // No motion detected
        MOTION_SIGNIFICANT,  // Significant motion detected
        MOTION_PEDOMETER,    // Pedometer motion detected
    };
 
    enum IntPin {
        INTERRUPT_PIN_1,    // Interrupt pin 1
        INTERRUPT_PIN_2,    // Interrupt pin 2
        INTERRUPT_PIN_DISABLE // Disable interrupt
    };
 
    enum FIFO_Samples {
        FIFO_SAMPLES_16,    // Configure the FIFO watermark level to 16 samples.
        FIFO_SAMPLES_32,    // Configure the FIFO watermark level to 32 samples.
        FIFO_SAMPLES_64,    // Configure the FIFO watermark level to 64 samples.
        FIFO_SAMPLES_128,   // Configure the FIFO watermark level to 128 samples.
    };
 
    enum FIFO_Mode {
        // Configure the FIFO_MODE to Bypass (0) mode, will disable the FIFO functionality.
        FIFO_MODE_BYPASS,
        // In FIFO mode, once FIFO is full,
        // the data filling will stop and new data will be discarded until host reads out the FIFO data and release the space for new data to be written to.
        FIFO_MODE_FIFO,
        // In Stream mode, once FIFO is full,
        // the data filling will continue and the oldest data will be discarded,
        // until host reads out the FIFO data and release the space for new data to be written to
        FIFO_MODE_STREAM,
    };
 
    enum SampleMode {
        SYNC_MODE,      //Synchronous sampling
        ASYNC_MODE,     //Asynchronous sampling
    };
 
    enum CommandTable {
        CTRL_CMD_ACK                            = 0x00,
        CTRL_CMD_RST_FIFO                       = 0x04,
        CTRL_CMD_REQ_FIFO                       = 0x05,
        CTRL_CMD_WRITE_WOM_SETTING              = 0x08,
        CTRL_CMD_ACCEL_HOST_DELTA_OFFSET        = 0x09,
        CTRL_CMD_GYRO_HOST_DELTA_OFFSET         = 0x0A,
        CTRL_CMD_CONFIGURE_TAP                  = 0x0C,
        CTRL_CMD_CONFIGURE_PEDOMETER            = 0x0D,
        CTRL_CMD_CONFIGURE_MOTION               = 0x0E,
        CTRL_CMD_RESET_PEDOMETER                = 0x0F,
        CTRL_CMD_COPY_USID                      = 0x10,
        CTRL_CMD_SET_RPU                        = 0x11,
        CTRL_CMD_AHB_CLOCK_GATING               = 0x12,
        CTRL_CMD_ON_DEMAND_CALIBRATION          = 0xA2,
        CTRL_CMD_APPLY_GYRO_GAINS               = 0xAA,
    };
 
    enum StatusReg {
        EVENT_SIGNIFICANT_MOTION = 128,
        EVENT_NO_MOTION = 64,
        EVENT_ANY_MOTION = 32,
        EVENT_PEDOMETER_MOTION = 16,
        EVENT_WOM_MOTION = 4,
        EVENT_TAP_MOTION = 2,
    };
 
    enum SensorStatus {
        STATUS_INT_CTRL9_CMD_DONE    = 0x0001,  // bit 0
        STATUS_INT_LOCKED            = 0x0002,  // bit 1
        STATUS_INT_AVAIL             = 0x0004,  // bit 2
        STATUS0_GYRO_DATA_READY      = 0x0008,  // bit 3
        STATUS0_ACCEL_DATA_READY     = 0x0010,  // bit 4
        STATUS1_SIGNIFICANT_MOTION   = 0x0020,  // bit 5
        STATUS1_NO_MOTION            = 0x0040,  // bit 6
        STATUS1_ANY_MOTION           = 0x0080,  // bit 7
        STATUS1_PEDOMETER_MOTION     = 0x0100,  // bit 8
        STATUS1_WOM_MOTION           = 0x0200,  // bit 9
        STATUS1_TAP_MOTION           = 0x0400,  // bit 10
    };
 
    enum TapDetectionPriority {
        PRIORITY0,      // (X > Y> Z)
        PRIORITY1,      // (X > Z > Y)
        PRIORITY2,      // (Y > X > Z)
        PRIORITY3,      // (Y > Z > X)
        PRIORITY4,      // (Z > X > Y)
        PRIORITY5,      // (Z > Y > X)
    };
 
    enum MotionCtrl {
 
        ANY_MOTION_EN_X = sensorlib::_bv(0),
        ANY_MOTION_EN_Y = sensorlib::_bv(1),
        ANY_MOTION_EN_Z = sensorlib::_bv(2),
 
        //Logic-AND between events of enabled axes for No-Motion detection, Otherwise, logical OR
        ANY_MOTION_LOGIC_AND = sensorlib::_bv(3),
 
        NO_MOTION_EN_X = sensorlib::_bv(4),
        NO_MOTION_EN_Y = sensorlib::_bv(5),
        NO_MOTION_EN_Z = sensorlib::_bv(6),
 
        //Logic-AND between events of enabled axes for No-Motion detection , Otherwise, logical OR
        NO_MOTION_LOGIC_OR = sensorlib::_bv(7),
    };
 
    SensorQMI8658()  = default;
 
    ~SensorQMI8658()
    {
        if (fifo_buffer) {
            free(fifo_buffer);
            fifo_buffer = NULL;
        }
    }
 
    void setPins(int _irq)
    {
        this->_irq = _irq;
    }
 
    bool reset(bool waitResult = true, uint32_t timeout = 500)
    {
        int val = 0;  // initialize with some value to avoid compilation errors
        comm->writeRegister(QMI8658_REG_RESET, QMI8658_REG_RESET_DEFAULT);
        hal->delay(2);   // chip NACKs while it is resetting. https://github.com/lewisxhe/SensorLib/issues/46
        // Maximum 15ms for the Reset process to be finished
        if (waitResult) {
            uint32_t start = hal->millis();
            while (hal->millis() - start < timeout) {
                val = comm->readRegister(QMI8658_REG_RST_RESULT);
                if (val != -1 && val == QMI8658_REG_RST_RESULT_VAL) {
 
                    //EN.ADDR_AI
                    comm->setRegisterBit(QMI8658_REG_CTRL1, 6);
 
                    return true;
                }
                hal->delay(10);
            }
            SENSORLIB_LOG_E("Reset chip failed, Response val = %d - 0x%X", val, val);
            return false;
        }
 
        //EN.ADDR_AI
        comm->setRegisterBit(QMI8658_REG_CTRL1, 6);
 
        return true;
    }
 
    uint8_t getChipID()
    {
        return comm->readRegister(QMI8658_REG_REVISION);
    }
 
    int whoAmI()
    {
        return comm->readRegister(QMI8658_REG_WHOAMI);
    }
 
    uint32_t getTimestamp()
    {
        uint8_t  buffer[3];
        uint32_t timestamp;
        if (comm->readRegister(QMI8658_REG_TIMESTAMP_L, buffer, 3) != -1) {
            timestamp = (uint32_t)(((uint32_t)buffer[2] << 16) |
                                   ((uint32_t)buffer[1] << 8) | buffer[0]);
            if (timestamp > lastTimestamp) {
                lastTimestamp = timestamp;
            } else {
                lastTimestamp = (timestamp + 0x1000000 - lastTimestamp);
            }
        }
        return lastTimestamp;
    }
 
    float getTemperature_C()
    {
        uint8_t buffer[2];
        if (comm->readRegister(QMI8658_REG_TEMPERATURE_L, buffer, 2) !=  -1) {
            return (float)buffer[1] + ((float)buffer[0] / 256.0);
        }
        return NAN;
    }
 
    void enableINT(IntPin pin, bool enable = true)
    {
        switch (pin) {
        case INTERRUPT_PIN_1:
            enable ? comm->setRegisterBit(QMI8658_REG_CTRL1, 3) : comm->clrRegisterBit(QMI8658_REG_CTRL1, 3);
            _irq_enable_mask = enable ? _irq_enable_mask | 0x01 : _irq_enable_mask & 0xFE;
            break;
        case INTERRUPT_PIN_2:
            enable ? comm->setRegisterBit(QMI8658_REG_CTRL1, 4) : comm->clrRegisterBit(QMI8658_REG_CTRL1, 4);
            _irq_enable_mask = enable ? _irq_enable_mask | 0x02 : _irq_enable_mask & 0xFD;
            break;
        default:
            break;
        }
    }
 
    uint8_t getIrqStatus()
    {
        return comm->readRegister(QMI8658_REG_STATUS_INT);
    }
 
    void enableDataReadyINT(bool enable = true)
    {
        enable ? comm->clrRegisterBit(QMI8658_REG_CTRL7, 5) :
        comm->setRegisterBit(QMI8658_REG_CTRL7, 5);
    }
 
    bool configAccelerometer(AccelRange range, AccelODR odr, LpfMode lpfOdr = LPF_MODE_0)
    {
        bool en = isEnableAccelerometer();
 
        if (en) {
            disableAccelerometer();
        }
 
        //setAccelRange
        if (comm->writeRegister(QMI8658_REG_CTRL2, 0x8F, (range << 4)) != 0) {
            return false;
        }
 
        switch (range) {
        // Possible accelerometer scales (and their register bit settings) are:
        // 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs  (11).
        // Here's a bit of an algorithm to calculate DPS/(ADC tick) based on that
        // 2-bit value:
        case ACC_RANGE_2G:
            accelScales = 2.0 / 32768.0;
            break;
        case ACC_RANGE_4G:
            accelScales = 4.0 / 32768.0;
            break;
        case ACC_RANGE_8G:
            accelScales = 8.0 / 32768.0;
            break;
        case ACC_RANGE_16G:
            accelScales = 16.0 / 32768.0;
            break;
        }
 
        // setAccelOutputDataRate
        if (comm->writeRegister(QMI8658_REG_CTRL2, 0xF0, odr) != 0) {
            return false;
        }
 
        if (lpfOdr != LPF_OFF) {
            // setAccelLowPassFitterOdr
            if (comm->writeRegister(QMI8658_REG_CTRL5, QMI8658_ACCEL_LPF_MASK,  (lpfOdr << 1)) != 0) {
                return false;
            }
            // Enable Low-Pass Fitter
            comm->setRegisterBit(QMI8658_REG_CTRL5, 0);
        } else {
            // Disable Low-Pass Fitter
            comm->clrRegisterBit(QMI8658_REG_CTRL5, 0);
        }
 
        // setAccelSelfTest
        // selfTest ? comm->setRegisterBit(QMI8658_REG_CTRL2, 7) : comm->clrRegisterBit(QMI8658_REG_CTRL2, 7);
 
        if (en) {
            enableAccelerometer();
        }
 
        return true;
    }
 
 
    bool configGyroscope(GyroRange range, GyroODR odr, LpfMode lpfOdr = LPF_MODE_0)
    {
        bool en = isEnableGyroscope();
 
        if (en) {
            disableGyroscope();
        }
 
        // setGyroRange
        if (comm->writeRegister(QMI8658_REG_CTRL3, 0x8F, (range << 4)) != 0) {
            return false;
        }
 
        switch (range) {
        // Possible gyro scales (and their register bit settings) are:
        // 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS  (11).
        // Here's a bit of an algorithm to calculate DPS/(ADC tick) based on that
        // 2-bit value:
        case GYR_RANGE_16DPS:
            gyroScales = 16.0 / 32768.0;
            break;
        case GYR_RANGE_32DPS:
            gyroScales = 32.0 / 32768.0;
            break;
        case GYR_RANGE_64DPS:
            gyroScales = 64.0 / 32768.0;
            break;
        case GYR_RANGE_128DPS:
            gyroScales = 128.0 / 32768.0;
            break;
        case GYR_RANGE_256DPS:
            gyroScales = 256.0 / 32768.0;
            break;
        case GYR_RANGE_512DPS:
            gyroScales = 512.0 / 32768.0;
            break;
        case GYR_RANGE_1024DPS:
            gyroScales = 1024.0 / 32768.0;
            break;
        }
 
        // setGyroOutputDataRate
        if (comm->writeRegister(QMI8658_REG_CTRL3, 0xF0, odr) != 0) {
            return false;
        }
 
        // setGyroLowPassFitterOdr
        if (lpfOdr != LPF_OFF) {
            if (comm->writeRegister(QMI8658_REG_CTRL5, QMI8658_GYRO_LPF_MASK,  (lpfOdr << 5)) != 0) {
                return false;
            }
            // Enable Low-Pass Fitter
            comm->setRegisterBit(QMI8658_REG_CTRL5, 4);
        } else {
            // Disable Low-Pass Fitter
            comm->clrRegisterBit(QMI8658_REG_CTRL5, 4);
        }
 
        // setGyroSelfTest
        // selfTest ? comm->setRegisterBit(QMI8658_REG_CTRL3, 7) : comm->clrRegisterBit(QMI8658_REG_CTRL3, 7);
 
        if (en) {
            enableGyroscope();
        }
 
        return true;
    }
 
    bool configFIFO(FIFO_Mode mode, FIFO_Samples samples = FIFO_SAMPLES_16,
                    IntPin pin = INTERRUPT_PIN_DISABLE, uint8_t trigger_samples = 16)
    {
        bool enGyro = isEnableGyroscope();
        bool enAccel = isEnableAccelerometer();
 
        if (enGyro) {
            disableGyroscope();
        }
 
        if (enAccel) {
            disableAccelerometer();
        }
 
        // Reset FIFO configure
        if (writeCommand(CTRL_CMD_RST_FIFO) != 0) {
            SENSORLIB_LOG_E("Reset fifo failed!");
            return false;
        }
 
        _fifo_interrupt = true;
 
        switch (pin) {
        case INTERRUPT_PIN_1:
            comm->setRegisterBit(QMI8658_REG_CTRL1, 2);
            break;
        case INTERRUPT_PIN_2:
            comm->clrRegisterBit(QMI8658_REG_CTRL1, 2);
            break;
        case INTERRUPT_PIN_DISABLE:
            // Saves whether the fifo interrupt pin is enabled
            _fifo_interrupt  = false;
            break;
        default:
            break;
        }
 
        // Set fifo mode and samples len
        _fifo_mode = (samples << 2) | mode;
        if (comm->writeRegister(QMI8658_REG_FIFO_CTRL, _fifo_mode) == -1) {
            return false;
        }
 
        /*
        * The FIFO_WTM register(0x13) indicates the expected level of FIFO data that host wants to get the FIFO Watermark interrupt.
        * The unit is sample, which means 6 bytes if one of accelerometer and gyroscope is enabled, and 12 bytes if both are enabled.
        * */
        if (comm->writeRegister(QMI8658_REG_FIFO_WTM_TH, trigger_samples ) == -1) {
            return false;
        }
 
        if (enGyro) {
            enableGyroscope();
        }
 
        if (enAccel) {
            enableAccelerometer();
        }
 
        int res =  comm->readRegister(QMI8658_REG_FIFO_CTRL);
        SENSORLIB_LOG_D("QMI8658_REG_FIFO_CTRL : 0x%X", res);
        if ((res & 0x02) == 0x02) {
            SENSORLIB_LOG_D("Enabled Stream mode.");
        } else if ((res & 0x01) == 0x01) {
            SENSORLIB_LOG_D("Enabled FIFO mode.");
        } else if ((res & 0x03) == 0x00) {
            SENSORLIB_LOG_D("Disabled FIFO.");
        }
        res >>= 2;
        if ((res & 0x03) == 0x03) {
            SENSORLIB_LOG_D("128 samples.");
        } else if ((res & 0x02) == 0x02) {
            SENSORLIB_LOG_D("64 samples.");
        } else if ((res & 0x01) == 0x01) {
            SENSORLIB_LOG_D("32 samples.");
        } else if ((res & 0x03) == 0x00) {
            SENSORLIB_LOG_D("16 samples.");
        }
 
        return true;
    }
 
    uint16_t readFromFifo(IMUdata *acc, uint16_t accLength, IMUdata *gyro, uint16_t gyrLength)
    {
        if (_fifo_mode == FIFO_MODE_BYPASS) {
            SENSORLIB_LOG_E("FIFO is not configured.");
            return 0;
        }
 
        if (!_gyro_enabled && !_accel_enabled) {
            SENSORLIB_LOG_E("Sensor not enabled.");
            return 0;
        }
 
        uint16_t data_bytes = readFromFifo();
        if (data_bytes == 0) {
            return 0;
        }
 
        if (!fifo_buffer) {
            SENSORLIB_LOG_E("FIFO buffer is NULL");
            return 0;
        }
 
        uint8_t enabled_sensor_count = (_accel_enabled && _gyro_enabled) ? 2 : 1;
        uint16_t samples_per_sensor = data_bytes / (6 * enabled_sensor_count);
        uint16_t total_samples = samples_per_sensor * enabled_sensor_count;
 
        SENSORLIB_LOG_D("Total samples: %u", total_samples);
 
        uint16_t accel_index = 0;
        uint16_t gyro_index = 0;
 
        for (uint16_t i = 0; i < total_samples; ++i) {
            auto data = reinterpret_cast < int16_t * > (&fifo_buffer[i * 6]);
            int16_t x = data[0];
            int16_t y = data[1];
            int16_t z = data[2];
 
            if (_accel_enabled && _gyro_enabled) {
                if (i % 2 == 0) {
                    // Accel
                    if (accel_index < accLength) {
                        acc[accel_index].x = x * accelScales;
                        acc[accel_index].y = y * accelScales;
                        acc[accel_index].z = z * accelScales;
                        accel_index++;
                    }
                } else {
                    // Gyro
                    if (gyro_index < gyrLength) {
                        gyro[gyro_index].x = x * gyroScales;
                        gyro[gyro_index].y = y * gyroScales;
                        gyro[gyro_index].z = z * gyroScales;
                        gyro_index++;
                    }
                }
            } else if (_accel_enabled) {
                if (accel_index < accLength) {
                    acc[accel_index].x = x * accelScales;
                    acc[accel_index].y = y * accelScales;
                    acc[accel_index].z = z * accelScales;
                    accel_index++;
                }
            } else if (_gyro_enabled) {
                if (gyro_index < gyrLength) {
                    gyro[gyro_index].x = x * gyroScales;
                    gyro[gyro_index].y = y * gyroScales;
                    gyro[gyro_index].z = z * gyroScales;
                    gyro_index++;
                }
            }
        }
        return samples_per_sensor;
    }
 
    bool enableAccelerometer()
    {
        if (comm->setRegisterBit(QMI8658_REG_CTRL7, 0)) {
            _accel_enabled = true;
        }
        return _accel_enabled;
    }
 
    bool disableAccelerometer()
    {
        if (comm->clrRegisterBit(QMI8658_REG_CTRL7, 0)) {
            _accel_enabled = false;
            return true;
        }
        return false;
    }
 
    bool isEnableAccelerometer()
    {
        return _accel_enabled;
    }
 
    bool isEnableGyroscope()
    {
        return _gyro_enabled;
    }
 
    bool enableGyroscope()
    {
        if (comm->setRegisterBit(QMI8658_REG_CTRL7, 1)) {
            _gyro_enabled = true;
        }
        return _gyro_enabled;
    }
 
    bool disableGyroscope()
    {
        if (comm->clrRegisterBit(QMI8658_REG_CTRL7, 1)) {
            _gyro_enabled = false;
            return true;
        }
        return false;
    }
 
    bool getAccelRaw(int16_t *rawBuffer)
    {
        if (!_accel_enabled) {
            return false;
        }
        uint8_t buffer[6] = {0};
        if (comm->readRegister(QMI8658_REG_AX_L, buffer, 6) != -1) {
            rawBuffer[0] = (int16_t)(buffer[1] << 8) | (buffer[0]);
            rawBuffer[1] = (int16_t)(buffer[3] << 8) | (buffer[2]);
            rawBuffer[2] = (int16_t)(buffer[5] << 8) | (buffer[4]);
        } else {
            return false;
        }
        return true;
    }
 
    bool getAccelerometer(float &x, float &y, float &z)
    {
        if (!_accel_enabled) {
            return false;
        }
        int16_t raw[3];
        if (getAccelRaw(raw)) {
            x = raw[0] * accelScales;
            y = raw[1] * accelScales;
            z = raw[2] * accelScales;
            return true;
        }
        return false;
    }
 
    float getAccelerometerScales()
    {
        return accelScales;
    }
 
    float getGyroscopeScales()
    {
        return gyroScales;
    }
 
    bool getGyroRaw(int16_t *rawBuffer)
    {
        if (!_gyro_enabled) {
            return false;
        }
        uint8_t buffer[6] = {0};
        if (comm->readRegister(QMI8658_REG_GX_L, buffer, 6) != -1) {
            rawBuffer[0] = (int16_t)(buffer[1] << 8) | (buffer[0]);
            rawBuffer[1] = (int16_t)(buffer[3] << 8) | (buffer[2]);
            rawBuffer[2] = (int16_t)(buffer[5] << 8) | (buffer[4]);
        } else {
            return false;
        }
        return true;
    }
 
    bool getGyroscope(float &x, float &y, float &z)
    {
        if (!_gyro_enabled) {
            return false;
        }
        int16_t raw[3];
        if (getGyroRaw(raw)) {
            x = raw[0] * gyroScales;
            y = raw[1] * gyroScales;
            z = raw[2] * gyroScales;
            return true;
        }
        return false;
    }
 
    bool getDataReady()
    {
        if ((_irq_enable_mask & 0x03) && (_irq != -1)) {
            if (hal->digitalRead(_irq)) {
                return false;
            }
        }
 
        switch (sampleMode) {
        case SYNC_MODE:
            return  comm->getRegisterBit(QMI8658_REG_STATUS_INT, 1);
        case ASYNC_MODE:
            //TODO: When Accel and Gyro are configured with different rates, this will always be false
            if (_accel_enabled && _gyro_enabled) {
                return comm->readRegister(QMI8658_REG_STATUS0) & 0x03;
            } else if (_gyro_enabled) {
                return comm->readRegister(QMI8658_REG_STATUS0) & 0x02;
            } else if (_accel_enabled) {
                return comm->readRegister(QMI8658_REG_STATUS0) & 0x01;
            }
            break;
        default:
            break;
        }
        return false;
    }
 
    bool enableSyncSampleMode()
    {
        sampleMode = SYNC_MODE;
        return comm->setRegisterBit(QMI8658_REG_CTRL7, 7);
    }
 
    bool disableSyncSampleMode()
    {
        sampleMode = ASYNC_MODE;
        return comm->clrRegisterBit(QMI8658_REG_CTRL7, 7);
    }
 
    bool enableLockingMechanism()
    {
        enableSyncSampleMode();
        if (comm->writeRegister(QMI8658_REG_CAL1_L, 0x01) != 0) {
            return false;
        }
        return writeCommand(CTRL_CMD_AHB_CLOCK_GATING) == 0;
    }
 
    bool disableLockingMechanism()
    {
        disableSyncSampleMode();
        if (comm->writeRegister(QMI8658_REG_CAL1_L, (uint8_t)0x00) != 0) {
            return -1;
        }
        return writeCommand(CTRL_CMD_AHB_CLOCK_GATING) == 0;
    }
 
    void dumpCtrlRegister()
    {
        uint8_t buffer[9];
        comm->readRegister(QMI8658_REG_CTRL1, buffer, 9);
        for (int i = 0; i < 9; ++i) {
            SENSORLIB_LOG_D("CTRL%d: REG:0x%02X HEX:0x%02X\n", i + 1, QMI8658_REG_CTRL1 + i, buffer[i]);
        }
 
        buffer[0] =  comm->readRegister(QMI8658_REG_FIFO_CTRL);
        SENSORLIB_LOG_D("FIFO_CTRL: REG:0x%02X HEX:0x%02X\n",  QMI8658_REG_FIFO_CTRL, buffer[0]);
    }
 
    void powerDown()
    {
        disableAccelerometer();
        disableGyroscope();
        comm->setRegisterBit(QMI8658_REG_CTRL1, 1);
    }
 
 
    void powerOn()
    {
        comm->clrRegisterBit(QMI8658_REG_CTRL1, 1);
    }
 
    int getStatusRegister()
    {
        return comm->readRegister(QMI8658_REG_STATUS1);
    }
 
    int configActivityInterruptMap(IntPin pin)
    {
        return pin == INTERRUPT_PIN_1 ? comm->setRegisterBit(QMI8658_REG_CTRL8, 6)
               : comm->clrRegisterBit(QMI8658_REG_CTRL8, 6);
    }
 
    bool configPedometer(uint16_t ped_sample_cnt, uint16_t ped_fix_peak2peak, uint16_t ped_fix_peak,
                         uint16_t ped_time_up, uint8_t ped_time_low = 0x14, uint8_t ped_time_cnt_entry = 0x0A, uint8_t ped_fix_precision = 0x00,
                         uint8_t ped_sig_count = 0x04)
    {
        // The Pedometer can only work in Non-SyncSample mode
        disableSyncSampleMode();
 
        bool enGyro = isEnableGyroscope();
        bool enAccel = isEnableAccelerometer();
 
        if (enGyro) {
            disableGyroscope();
        }
 
        if (enAccel) {
            disableAccelerometer();
        }
 
        comm->writeRegister(QMI8658_REG_CAL1_L, ped_sample_cnt & 0xFF);
        comm->writeRegister(QMI8658_REG_CAL1_H, (ped_sample_cnt >> 8) & 0xFF);
        comm->writeRegister(QMI8658_REG_CAL2_L, ped_fix_peak2peak & 0xFF);
        comm->writeRegister(QMI8658_REG_CAL2_H, (ped_fix_peak2peak >> 8) & 0xFF);
        comm->writeRegister(QMI8658_REG_CAL3_L, ped_fix_peak & 0xFF);
        comm->writeRegister(QMI8658_REG_CAL3_H, (ped_fix_peak >> 8) & 0xFF);
        comm->writeRegister(QMI8658_REG_CAL4_H, 0x01);
        comm->writeRegister(QMI8658_REG_CAL4_L, 0x02);
 
        writeCommand(CTRL_CMD_CONFIGURE_PEDOMETER);
 
        comm->writeRegister(QMI8658_REG_CAL1_L, ped_time_up & 0xFF);
        comm->writeRegister(QMI8658_REG_CAL1_H, (ped_time_up >> 8) & 0xFF);
        comm->writeRegister(QMI8658_REG_CAL2_L, ped_time_low);
        comm->writeRegister(QMI8658_REG_CAL2_H, ped_time_cnt_entry);
        comm->writeRegister(QMI8658_REG_CAL3_L, ped_fix_precision);
        comm->writeRegister(QMI8658_REG_CAL3_H, ped_sig_count);
        comm->writeRegister(QMI8658_REG_CAL4_H, 0x02);
        comm->writeRegister(QMI8658_REG_CAL4_L, 0x02);
 
        writeCommand(CTRL_CMD_CONFIGURE_PEDOMETER);
 
        if (enGyro) {
            enableGyroscope();
        }
 
        if (enAccel) {
            enableAccelerometer();
        }
        return true;
    }
 
    uint32_t getPedometerCounter()
    {
        uint8_t buffer[3];
        if (comm->readRegister(QMI8658_REG_STEP_CNT_LOW, buffer, 3) != -1) {
            return (uint32_t)(((uint32_t)buffer[2] << 16) | ((uint32_t)buffer[1] << 8) | buffer[0]);
        }
        return 0;
    }
 
    bool clearPedometerCounter()
    {
        return writeCommand(CTRL_CMD_RESET_PEDOMETER) == 0;
    }
 
    bool enablePedometer(IntPin pin = INTERRUPT_PIN_DISABLE)
    {
        if (!_accel_enabled)return false;
 
        switch (pin) {
        case INTERRUPT_PIN_1:
        case INTERRUPT_PIN_2:
            configActivityInterruptMap(pin);
            enableINT(pin);
            break;
        default:
            break;
        }
        return comm->setRegisterBit(QMI8658_REG_CTRL8, 4);
    }
 
    bool disablePedometer()
    {
        if (!_accel_enabled)return false;
        return comm->clrRegisterBit(QMI8658_REG_CTRL8, 4);
    }
 
 
 
    bool configTap(uint8_t priority, uint8_t peakWindow, uint16_t tapWindow, uint16_t dTapWindow,
                   float alpha, float gamma, float peakMagThr, float UDMThr)
    {
 
        // The Tap detection can only work in Non-SyncSample mode
        if (!disableSyncSampleMode()) {
            return false;
        }
 
        bool enGyro = isEnableGyroscope();
        bool enAccel = isEnableAccelerometer();
 
        if (enGyro) {
            disableGyroscope();
        }
 
        if (enAccel) {
            disableAccelerometer();
        }
        comm->writeRegister(QMI8658_REG_CAL1_L, peakWindow);
        comm->writeRegister(QMI8658_REG_CAL1_H, priority);
        comm->writeRegister(QMI8658_REG_CAL2_L, tapWindow & 0xFF);
        comm->writeRegister(QMI8658_REG_CAL2_H, (tapWindow >> 8) & 0xFF);
        comm->writeRegister(QMI8658_REG_CAL3_L, dTapWindow & 0xFF);
        comm->writeRegister(QMI8658_REG_CAL3_H, (dTapWindow >> 8) & 0xFF);
        // comm->writeRegister(QMI8658_REG_CAL4_L, 0x02);
        comm->writeRegister(QMI8658_REG_CAL4_H, 0x01);
 
        writeCommand(CTRL_CMD_CONFIGURE_TAP);
 
        // 1-byte unsigned,7-bits fraction
        uint8_t alphaHex = (uint8_t)(alpha * 128);
        comm->writeRegister(QMI8658_REG_CAL1_L, alphaHex);
 
        // 1-byte unsigned,7-bits fraction
        uint8_t gammaHex = (uint8_t)(gamma * 128);
        comm->writeRegister(QMI8658_REG_CAL1_H, gammaHex);
 
        const double g = 9.81; // Earth's gravitational acceleration m/s^2
        double resolution = 0.001 * g * g; // Calculation resolution  0.001g^2
 
        double acceleration_square = peakMagThr * g * g;     // Calculate the square of the acceleration
        uint16_t value = (uint16_t)(acceleration_square / resolution); // Calculates the value of a 2-byte unsigned integer
 
        comm->writeRegister(QMI8658_REG_CAL2_L, sensorlib::_lowByte(value));
        comm->writeRegister(QMI8658_REG_CAL2_H, sensorlib::_highByte(value));
 
        acceleration_square = UDMThr * g * g;     // Calculate the square of the acceleration
        value = (uint16_t)(acceleration_square / resolution); // Calculates the value of a 2-byte unsigned integer
 
        comm->writeRegister(QMI8658_REG_CAL3_L, sensorlib::_lowByte(value));
        comm->writeRegister(QMI8658_REG_CAL3_H, sensorlib::_highByte(value));
        // comm->writeRegister(QMI8658_REG_CAL4_L, 0x02);
        comm->writeRegister(QMI8658_REG_CAL4_H, 0x02);
 
        writeCommand(CTRL_CMD_CONFIGURE_TAP);
 
        if (enGyro) {
            enableGyroscope();
        }
 
        if (enAccel) {
            enableAccelerometer();
        }
 
        return true;
    }
 
    bool enableTap(IntPin pin = INTERRUPT_PIN_1)
    {
        if (!_accel_enabled)return false;
        switch (pin) {
        case INTERRUPT_PIN_1:
        case INTERRUPT_PIN_2:
            configActivityInterruptMap(pin);
            enableINT(pin);
            break;
        default:
            break;
        }
        return comm->setRegisterBit(QMI8658_REG_CTRL8, 0);
    }
 
    bool disableTap()
    {
        return comm->clrRegisterBit(QMI8658_REG_CTRL8, 0);
    }
 
    TapEvent getTapStatus()
    {
        int val = comm->readRegister(QMI8658_REG_TAP_STATUS);
        if (val & sensorlib::_bv(7)) {
            SENSORLIB_LOG_I("Tap was detected on the negative direction of the Tap axis");
        } else {
            SENSORLIB_LOG_I("Tap was detected on the positive direction of the Tap axis");
        }
        uint8_t t = (val >> 4) & 0x03;
        switch (t) {
        case 0:
            SENSORLIB_LOG_I("No Tap was detected");
            break;
        case 1:
            SENSORLIB_LOG_I("Tap was detected on X axis");
            break;
        case 2:
            SENSORLIB_LOG_I("Tap was detected on Y axis");
            break;
        case 3:
            SENSORLIB_LOG_I("Tap was detected on Z axis");
            break;
        default:
            break;
        }
        t = val & 0x03;
        switch (t) {
        case 0:
            SENSORLIB_LOG_I("No Tap was detected");
            return INVALID_TAP;
        case 1:
            SENSORLIB_LOG_I("Single-Tap was detected");
            return SINGLE_TAP;
        case 2:
            SENSORLIB_LOG_I("Double-Tap was detected");
            return DOUBLE_TAP;
        default:
            break;
        }
        return INVALID_TAP;
    }
 
 
    //TODO:Need Test
    bool configMotion(
        //* See enum MotionCtrl
        uint8_t modeCtrl,
        //* Define the slope threshold of the x-axis for arbitrary motion detection
        float AnyMotionXThr,
        //* Define the slope threshold of the y-axis for arbitrary motion detection
        float AnyMotionYThr,
        //* Define the slope threshold of the z-axis for arbitrary motion detection
        float AnyMotionZThr,
        //* Defines the minimum number of consecutive samples (duration) that the absolute
        //* of the slope of the enabled axis/axes data should keep higher than the threshold
        uint8_t AnyMotionWindow,
        //* Defines the slope threshold of the x-axis for no motion detection
        float NoMotionXThr,
        //* Defines the slope threshold of the y-axis for no motion detection
        float NoMotionYThr,
        //* Defines the slope threshold of the z-axis for no motion detection
        float NoMotionZThr,
        //* Defines the minimum number of consecutive samples (duration) that the absolute
        //* of the slope of the enabled axis/axes data should keep lower than the threshold
        uint8_t NoMotionWindow,
        //* Defines the wait window (idle time) starts from the first Any-Motion event until
        //* starting to detecting another Any-Motion event form confirmation
        uint16_t SigMotionWaitWindow,
        //* Defines the maximum duration for detecting the other Any-Motion
        //* event to confirm Significant-Motion, starts from the first Any -Motion event
        uint16_t SigMotionConfirmWindow)
    {
        // Only work in Non-SyncSample mode
        if (!disableSyncSampleMode()) {
            return false;
        }
 
        bool enGyro = isEnableGyroscope();
        bool enAccel = isEnableAccelerometer();
 
        if (enGyro) {
            disableGyroscope();
        }
 
        if (enAccel) {
            disableAccelerometer();
        }
 
        comm->writeRegister(QMI8658_REG_CAL1_L, mgToBytes(AnyMotionXThr));
        comm->writeRegister(QMI8658_REG_CAL1_H, mgToBytes(AnyMotionYThr));
        comm->writeRegister(QMI8658_REG_CAL2_L, mgToBytes(AnyMotionZThr));
        comm->writeRegister(QMI8658_REG_CAL2_H, mgToBytes(NoMotionXThr));
        comm->writeRegister(QMI8658_REG_CAL3_L, mgToBytes(NoMotionYThr));
        comm->writeRegister(QMI8658_REG_CAL3_H, mgToBytes(NoMotionZThr));
        comm->writeRegister(QMI8658_REG_CAL4_L, modeCtrl);
        comm->writeRegister(QMI8658_REG_CAL4_H, 0x01);
 
        writeCommand(CTRL_CMD_CONFIGURE_MOTION);
 
        comm->writeRegister(QMI8658_REG_CAL1_L, AnyMotionWindow);
        comm->writeRegister(QMI8658_REG_CAL1_H, NoMotionWindow);
        comm->writeRegister(QMI8658_REG_CAL2_L, sensorlib::_lowByte(SigMotionWaitWindow));
        comm->writeRegister(QMI8658_REG_CAL2_H, sensorlib::_highByte(SigMotionWaitWindow));
        comm->writeRegister(QMI8658_REG_CAL3_L, sensorlib::_lowByte(SigMotionConfirmWindow));
        comm->writeRegister(QMI8658_REG_CAL3_H, sensorlib::_highByte(SigMotionConfirmWindow));
        // comm->writeRegister(QMI8658_REG_CAL4_L, 0x02);
        comm->writeRegister(QMI8658_REG_CAL4_H, 0x02);
 
        writeCommand(CTRL_CMD_CONFIGURE_MOTION);
 
        if (enGyro) {
            enableGyroscope();
        }
 
        if (enAccel) {
            enableAccelerometer();
        }
        return true;
    }
 
    bool enableMotionDetect(IntPin pin = INTERRUPT_PIN_1)
    {
        if (!_accel_enabled)return false;
        switch (pin) {
        case INTERRUPT_PIN_1:
        case INTERRUPT_PIN_2:
            configActivityInterruptMap(pin);
            enableINT(pin);
            break;
        default:
            break;
        }
        comm->setRegisterBit(QMI8658_REG_CTRL8, 1);
        comm->setRegisterBit(QMI8658_REG_CTRL8, 2);
        comm->setRegisterBit(QMI8658_REG_CTRL8, 3);
        return true;
    }
 
    bool disableMotionDetect()
    {
        comm->clrRegisterBit(QMI8658_REG_CTRL8, 1);
        comm->clrRegisterBit(QMI8658_REG_CTRL8, 2);
        comm->clrRegisterBit(QMI8658_REG_CTRL8, 3);
        return false;
    }
 
 
    bool configWakeOnMotion(uint8_t WoMThreshold = 200,
                            AccelODR odr = ACC_ODR_LOWPOWER_128Hz,
                            IntPin pin = INTERRUPT_PIN_2,
                            uint8_t defaultPinValue = 1,
                            uint8_t blankingTime = 0x20,
                            AccelRange accRange = ACC_RANGE_8G)
    {
 
        uint8_t val = 0;
 
        // Reset default value
        if (!reset()) {
            return false;
        }
 
        // Disable sensors
        comm->clrRegisterBit(QMI8658_REG_CTRL7, 0);
 
        // SetAccelRange
        if (comm->writeRegister(QMI8658_REG_CTRL2, 0x8F, (accRange << 4)) != 0) {
            return false;
        }
 
        // setAccelOutputDataRate
        if (comm->writeRegister(QMI8658_REG_CTRL2, 0xF0, odr) != 0) {
            return false;
        }
 
        // Set wom
        if (comm->writeRegister(QMI8658_REG_CAL1_L, WoMThreshold) != 0) {
            return false;
        }
 
        if ( pin == INTERRUPT_PIN_1) {
            val = defaultPinValue ? 0x02 : 0x00;
        } else if (pin == INTERRUPT_PIN_2) {
            val = defaultPinValue ? 0x03 : 0x01;
        }
 
        val <<= 6;
        val |= (blankingTime & 0x3F);
        if (comm->writeRegister(QMI8658_REG_CAL1_H, val) != 0) {
            return false;
        }
 
        if (writeCommand(CTRL_CMD_WRITE_WOM_SETTING) != 0) {
            return false;
        }
 
        enableAccelerometer();
 
        enableINT(pin);
 
        return true;
    }
 
 
    void getChipUsid(uint8_t *buffer, uint8_t length)
    {
        if (length > 6) {
            length = 6;
        }
        memcpy(buffer, usid, length);
    }
 
 
    uint32_t getChipFirmwareVersion()
    {
        return revisionID;
    }
 
    uint16_t update()
    {
        uint16_t result = 0;
        // STATUSINT 0x2D
        // STATUS0 0x2E
        // STATUS1 0x2F
        uint8_t status[3];
        if (comm->readRegister(QMI8658_REG_STATUS_INT, status, 3) != 0) {
            return 0;
        }
 
        // SENSORLIB_LOG_I("STATUSINT:0x%X BIN:", status[0]);
        // SENSORLIB_LOG_I("STATUS0:0x%X BIN:", status[1]);
        // SENSORLIB_LOG_I("STATUS1:0x%X BIN:", status[2]);
        // SENSORLIB_LOG_I("------------------\n");
 
        // Ctrl9 CmdDone
        // Indicates CTRL9 Command was done, as part of CTRL9 protocol
        // 0: Not Completed
        // 1: Done
        if (status[0] & 0x80) {
            result |= STATUS_INT_CTRL9_CMD_DONE;
        }
        // If syncSample (CTRL7.bit7) = 1:
        //      0: Sensor Data is not locked.
        //      1: Sensor Data is locked.
        // If syncSample = 0, this bit shows the same value of INT1 level
        if (status[0] & 0x02) {
            result |= STATUS_INT_LOCKED;
        }
        // If syncSample (CTRL7.bit7) = 1:
        //      0: Sensor Data is not available
        //      1: Sensor Data is available for reading
        // If syncSample = 0, this bit shows the same value of INT2 level
        if (status[0] & 0x01) {
            result |= STATUS_INT_AVAIL;
            // if (eventGyroDataReady)eventGyroDataReady();
            // if (eventAccelDataReady)eventAccelDataReady();
        }
 
        //Locking Mechanism Can reading..
        if ((status[0] & 0x03) == 0x03) {
            if (eventDataLocking)eventDataLocking();
        }
 
        //=======================================
        // Valid only in asynchronous mode
        if (sampleMode == ASYNC_MODE) {
            // Gyroscope new data available
            // 0: No updates since last read.
            // 1: New data available
            if (status[1] & 0x02) {
                result |= STATUS0_GYRO_DATA_READY;
                if (eventGyroDataReady)eventGyroDataReady();
                _gDataReady = true;
            }
            // Accelerometer new data available
            // 0: No updates since last read.
            // 1: New data available.
            if (status[1] & 0x01) {
                result |= STATUS0_ACCEL_DATA_READY;
                if (eventAccelDataReady)eventAccelDataReady();
                _aDataReady = true;
            }
        }
 
        //=======================================
        // Significant Motion
        // 0: No Significant-Motion was detected
        // 1: Significant-Motion was detected
        if (status[2] & 0x80) {
            result |= STATUS1_SIGNIFICANT_MOTION;
            if (eventSignificantMotion)eventSignificantMotion();
        }
        // No Motion
        // 0: No No-Motion was detected
        // 1: No-Motion was detected
        if (status[2] & 0x40) {
            result |= STATUS1_NO_MOTION;
            if (eventNoMotionEvent)eventNoMotionEvent();
        }
        // Any Motion
        // 0: No Any-Motion was detected
        // 1: Any-Motion was detected
        if (status[2] & 0x20) {
            result |= STATUS1_ANY_MOTION;
            if (eventAnyMotionEvent)eventAnyMotionEvent();
        }
        // Pedometer
        // 0: No step was detected
        // 1: step was detected
        if (status[2] & 0x10) {
            result |= STATUS1_PEDOMETER_MOTION;
            if (eventPedometerEvent)eventPedometerEvent();
        }
        // WoM
        // 0: No WoM was detected
        // 1: WoM was detected
        if (status[2] & 0x04) {
            result |= STATUS1_WOM_MOTION;
            if (eventWomEvent)eventWomEvent();
        }
        // TAP
        // 0: No Tap was detected
        // 1: Tap was detected
        if (status[2] & 0x02) {
            result |= STATUS1_TAP_MOTION;
            if (eventTagEvent)eventTagEvent();
        }
        return result;
    }
 
    void setWakeupMotionEventCallBack(EventCallBack_t cb)
    {
        eventWomEvent = cb;
    }
 
    void setTapEventCallBack(EventCallBack_t cb)
    {
        eventTagEvent = cb;
    }
 
    void setPedometerEventCallBack(EventCallBack_t cb)
    {
        eventPedometerEvent = cb;
    }
 
    void setNoMotionEventCallBack(EventCallBack_t cb)
    {
        eventNoMotionEvent = cb;
    }
 
    void setAnyMotionEventCallBack(EventCallBack_t cb)
    {
        eventAnyMotionEvent = cb;
    }
 
    void setSignificantMotionEventCallBack(EventCallBack_t cb)
    {
        eventSignificantMotion = cb;
    }
 
    void setGyroDataReadyCallBack(EventCallBack_t cb)
    {
        eventGyroDataReady = cb;
    }
 
    void setAccelDataReadyEventCallBack(EventCallBack_t cb)
    {
        eventAccelDataReady = cb;
    }
 
    void setDataLockingEventCallBack(EventCallBack_t cb)
    {
        eventDataLocking = cb;
    }
 
 
    bool calibration(uint16_t *gX_gain = NULL, uint16_t *gY_gain = NULL, uint16_t *gZ_gain = NULL)
    {
        // 1.Set CTRL7.aEN = 0 and CTRL7.gEN = 0, to disable the accelerometer and gyroscope.
        if (comm->writeRegister(QMI8658_REG_CTRL7, (uint8_t)0x00) != 0) {
            return false;
        }
 
        // 2.Issue the CTRL_CMD_ON_DEMAND_CALIBRATION (0xA2) by CTRL9 command.
        if (writeCommand(CTRL_CMD_ON_DEMAND_CALIBRATION, 3000) != 0) {
            return false;
        }
 
        // 3.And wait about 1.5 seconds for QMI8658A to finish the CTRL9 command.
        hal->delay(1600);
 
        // 4.Read the COD_STATUS register (0x46) to check the result/status of the COD implementation.
        int result =  comm->readRegister(QMI8658_REG_COD_STATUS);
 
        if (result == -1)return false;
 
        // During the process, it is recommended to place the device in quiet, otherwise, the COD might fail and report error.
 
        if (result & sensorlib::_bv(7)) {
            SENSORLIB_LOG_E("COD failed for checking low sensitivity limit of X axis of gyroscope");
            return false;
        }
        if (result & sensorlib::_bv(6)) {
            SENSORLIB_LOG_E("COD failed for checking high sensitivity limit of X axis of gyroscope");
            return false;
        }
        if (result & sensorlib::_bv(5)) {
            SENSORLIB_LOG_E("COD failed for checking low sensitivity limit of Y axis of gyroscope");
            return false;
        }
        if (result & sensorlib::_bv(4)) {
            SENSORLIB_LOG_E("COD failed for checking high sensitivity limit of Y axis of gyroscope");
            return false;
        }
        if (result & sensorlib::_bv(3)) {
            SENSORLIB_LOG_E("Accelerometer checked failed (significant vibration happened during COD)");
            return false;
        }
        if (result & sensorlib::_bv(2)) {
            SENSORLIB_LOG_E("Gyroscope startup failure happened when COD was called");
            return false;
        }
        if (result & sensorlib::_bv(1)) {
            SENSORLIB_LOG_E("COD was called while gyroscope was enabled, COD return failure");
            return false;
        }
        if (result & sensorlib::_bv(0)) {
            SENSORLIB_LOG_E("COD failed; no COD correction applied");
            return false;
        }
        SENSORLIB_LOG_D("All calibrations are completed");
 
        if (gX_gain && gY_gain && gZ_gain) {
            uint8_t rawBuffer[6] = {0};
            if (comm->readRegister(QMI8658_REG_DVX_L, rawBuffer, 6) != 0) {
                return false;
            }
            *gX_gain = ((uint16_t)rawBuffer[0]) | (uint16_t)(rawBuffer[1] << 8);
            *gY_gain = ((uint16_t)rawBuffer[2]) | (uint16_t)(rawBuffer[3] << 8);
            *gZ_gain = ((uint16_t)rawBuffer[4]) | (uint16_t)(rawBuffer[5] << 8);
        }
 
        return true;
    }
 
 
    bool writeCalibration(uint16_t gX_gain, uint16_t gY_gain, uint16_t gZ_gain)
    {
        // 1. Disable Accelerometer and Gyroscope by setting CTRL7.aEN = 0 and CTRL7.gEN = 0
        if (comm->writeRegister(QMI8658_REG_CTRL7, (uint8_t)0x00) != 0) {
            return false;
        }
 
        uint8_t buffer[] = {
            // 2. write Gyro-X gain (16 bits) to registers CAL1_L and CAL1_H registers (0x0B, 0x0C)
            sensorlib::_lowByte(gX_gain),
            sensorlib::_highByte(gX_gain),
            // 3. write Gyro-Y gain (16 bits) to registers CAL2_L and CAL2_H registers (0x0D, 0x0E)
            sensorlib::_lowByte(gY_gain),
            sensorlib::_highByte(gY_gain),
            // 4. write Gyro-Z gain (16 bits) to registers CAL3_L and CAL3_H registers (0x0F, 0x10)
            sensorlib::_lowByte(gZ_gain),
            sensorlib::_highByte(gZ_gain),
        };
 
        comm->writeRegister(QMI8658_REG_CAL1_L, buffer, sizeof(buffer));
 
        // 5. Write 0xAA to CTRL9 and follow CTRL9 protocol
        if (writeCommand(CTRL_CMD_APPLY_GYRO_GAINS, 3000) != 0) {
            return false;
        }
 
        return true;
    }
 
    bool selfTestAccel()
    {
        // 1- Disable the sensors (CTRL7 = 0x00).
        if (comm->writeRegister(QMI8658_REG_CTRL7, (uint8_t)0x00) != 0) {
            return false;
        }
 
        // 2- Set proper accelerometer ODR (CTRL2.aODR) and bit CTRL2.aST (bit7) to 1 to trigger the Self-Test.
        if (comm->writeRegister(QMI8658_REG_CTRL2, 0xF0, ACC_ODR_1000Hz | 0x80) != 0) {
            return false;
        }
 
        // 3- Wait for QMI8658A to drive INT2 to High, if INT2 is enabled (CTRL1.bit4 = 1), or STATUSINT.bit0 is set to 1.
        int retry = 50;
        int dataReady = 0x00;
        while (dataReady != 0x01) {
            uint8_t reg_var = comm->readRegister(QMI8658_REG_STATUS_INT);
            SENSORLIB_LOG_D("reg_var : %x", reg_var);
            dataReady = reg_var & 0x01;
            // dataReady = comm->readRegister(QMI8658_REG_STATUS_INT) & 0x01;
            hal->delay(20);
            if (--retry <= 0) {
                SENSORLIB_LOG_E("No response.");
                return false;
            }
        }
 
        SENSORLIB_LOG_I("Data is ready for reading....");
 
        //4- Set CTRL2.aST(bit7) to 0, to clear STATUSINT1.bit0 and/or INT2.
        comm->clrRegisterBit(QMI8658_REG_CTRL2, 7);
 
        // 5- Check for QMI8658A drives INT2 back to Low, and sets STATUSINT1.bit0 to 0.
        retry = 50;
        while (dataReady == 0x01) {
            uint8_t reg_var = comm->readRegister(QMI8658_REG_STATUS_INT);
            SENSORLIB_LOG_D("reg_var : %x", reg_var);
            dataReady = (reg_var & 0x01);
            // dataReady = !(comm->readRegister(QMI8658_REG_STATUS_INT) & 0x01);
            hal->delay(20);
            if (--retry <= 0) {
                SENSORLIB_LOG_E("No response.");
                return false;
            }
        }
 
        /*
            6- Read the Accel Self-Test result:
                X channel: dVX_L and dVX_H (registers 0x51 and 0x52)
                Y channel: dVY_L and dVY_H (registers 0x53 and 0x54)
                Z channel: dVZ_L and dVZ_H (registers 0x55 and 0x56)
                The results are 16-bits in format signed U5.11, resolution 0.5mg (1 / 2^11 g).
        */
        uint8_t rawBuffer[6];
 
        if (comm->readRegister(QMI8658_REG_DVX_L, rawBuffer, 6) != 0) {
            return false;
        }
 
        int16_t dVX = (int16_t)(rawBuffer[0]) | (int16_t)(((int16_t)rawBuffer[1]) << 8);
        int16_t dVY = (int16_t)(rawBuffer[2]) | (int16_t)(((int16_t)rawBuffer[3]) << 8);
        int16_t dVZ = (int16_t)(rawBuffer[4]) | (int16_t)(((int16_t)rawBuffer[5]) << 8);
 
        // To convert to mg, considering the U5.11 format, we need to divide by (2^11) to get the actual mg value
        float dVX_mg = dVX * 0.5;   // 0.5mg is the smallest unit of this format
        float dVY_mg = dVY * 0.5;
        float dVZ_mg = dVZ * 0.5;
 
        SENSORLIB_LOG_D("\n\tdVX_mg:%05.11f \n\tdVY_mg:%05.11f \n\tdVZ_mg:%05.11f", dVX_mg, dVY_mg, dVZ_mg);
        // If the absolute results of all three axes are higher than 200mg, the accelerometer can be considered functional.
        // Otherwise, the accelerometer cannot be considered functional.
        if (abs(dVX_mg) > 200 && abs(dVY_mg) > 200 && abs(dVZ_mg) > 200) {
            SENSORLIB_LOG_D("Accelerometer is working properly.");
        } else {
            SENSORLIB_LOG_D("Accelerometer is not working properly.");
            return false;
        }
        return true;
    }
 
 
    bool selfTestGyro()
    {
        // 1- Disable the sensors (CTRL7 = 0x00).
        if (comm->writeRegister(QMI8658_REG_CTRL7, (uint8_t)0x00) != 0) {
            return false;
        }
 
        // 2- Set the bit gST to 1. (CTRL3.bit7 = 1"b1).
        comm->setRegisterBit(QMI8658_REG_CTRL3, 7);
 
        // 3- Wait for QMI8658A to drive INT2 to High, if INT2 is enabled, or STATUS_INT.bit0 is set to 1.
        int retry = 50;
        int dataReady = 0x00;
        while (dataReady != 0x01) {
            dataReady = comm->readRegister(QMI8658_REG_STATUS_INT) & 0x01;
            hal->delay(20);
            if (--retry <= 0) {
                SENSORLIB_LOG_E("No response.");
                return false;
            }
        }
 
        SENSORLIB_LOG_I("Data is ready for reading....");
 
        //4- Set CTRL3.aST(bit7) to 0, to clear STATUS_INT1.bit0 and/or INT2.
        comm->clrRegisterBit(QMI8658_REG_CTRL3, 7);
 
        // 5- Check for QMI8658A drives INT2 back to Low, or sets STATUSINT1.bit0 to 0.
        retry = 50;
        while (dataReady != 0x00) {
            dataReady = !(comm->readRegister(QMI8658_REG_STATUS_INT) & 0x01);
            hal->delay(20);
            if (--retry <= 0) {
                SENSORLIB_LOG_E("No response.");
                return false;
            }
        }
 
        /*
            6- Read the Gyro Self-Test result:
                X channel: dVX_L and dVX_H (registers 0x51 and 0x52)
                Y channel: dVY_L and dVY_H (registers 0x53 and 0x54)
                Z channel: dVZ_L and dVZ_H (registers 0x55 and 0x56)
                Read the 16 bits result in format signed U12.4, resolution is 62.5mdps (1 / 2^4 dps).
        */
        uint8_t rawBuffer[6];
        if (comm->readRegister(QMI8658_REG_DVX_L, rawBuffer, 6) != 0) {
            return false;
        }
 
        // int16_t x = (int16_t)(rawBuffer[0]) | ((int16_t)(rawBuffer[1] << 8));
        // int16_t y = (int16_t)(rawBuffer[2]) | ((int16_t)(rawBuffer[3] << 8));
        // int16_t z = (int16_t)(rawBuffer[4]) | ((int16_t)(rawBuffer[5] << 8));
 
        float dVX = (((int16_t)rawBuffer[0]) << 12) | ((int16_t)(rawBuffer[1]) >> 4);
        float dVY = (((int16_t)rawBuffer[2]) << 12) | ((int16_t)(rawBuffer[3]) >> 4);
        float dVZ = (((int16_t)rawBuffer[4]) << 12) | ((int16_t)(rawBuffer[5]) >> 4);
 
        dVX *= (1.0 / (1 << 4)); // 62.5 mdps
        dVY *= (1.0 / (1 << 4)); // 62.5 mdps
        dVZ *= (1.0 / (1 << 4)); // 62.5 mdps
 
        SENSORLIB_LOG_D("\n\tdVX:%12.4f \n\tdVY:%12.4f \n\tdVZ:%12.4f", dVX, dVY, dVZ);
 
        //  If the absolute results of all three axes are higher than 300dps, the gyroscope can be considered functional.
        // Otherwise, the gyroscope cannot be considered functional.
        if (abs(dVX) > 300 && abs(dVY) > 300 && abs(dVZ) > 300) {
            SENSORLIB_LOG_D("Gyro is working properly.");
        } else {
            SENSORLIB_LOG_D("Gyro is not working properly.");
            return false;
        }
 
        return true;
    }
 
    void setAccelOffset(int16_t offset_x, int16_t offset_y, int16_t offset_z)
    {
        uint8_t data[6];
        data[0] = sensorlib::_lowByte(offset_x);
        data[1] = sensorlib::_highByte(offset_x);
        data[2] = sensorlib::_lowByte(offset_y);
        data[3] = sensorlib::_highByte(offset_y);
        data[4] = sensorlib::_lowByte(offset_z);
        data[5] = sensorlib::_highByte(offset_z);
        comm->writeRegister(QMI8658_REG_CAL1_L, data, 2);
        comm->writeRegister(QMI8658_REG_CAL2_L, data + 2, 2);
        comm->writeRegister(QMI8658_REG_CAL3_L, data + 4, 2);
        writeCommand(CTRL_CMD_ACCEL_HOST_DELTA_OFFSET);
    }
 
    void setGyroOffset(int16_t offset_x, int16_t offset_y, int16_t offset_z)
    {
        uint8_t data[6];
        data[0] = sensorlib::_lowByte(offset_x);
        data[1] = sensorlib::_highByte(offset_x);
        data[2] = sensorlib::_lowByte(offset_y);
        data[3] = sensorlib::_highByte(offset_y);
        data[4] = sensorlib::_lowByte(offset_z);
        data[5] = sensorlib::_highByte(offset_z);
        comm->writeRegister(QMI8658_REG_CAL1_L, data, 2);
        comm->writeRegister(QMI8658_REG_CAL2_L, data + 2, 2);
        comm->writeRegister(QMI8658_REG_CAL3_L, data + 4, 2);
        writeCommand(CTRL_CMD_GYRO_HOST_DELTA_OFFSET);
    }
 
private:
 
    uint16_t getFifoNeedBytes()
    {
        uint8_t sam[] = {16, 32, 64, 128};
        uint8_t sensors  = 0;
        if (_gyro_enabled && _accel_enabled) {
            sensors = 2;
        } else if (_gyro_enabled || _accel_enabled) {
            sensors = 1;
        }
        uint8_t samples =  ((_fifo_mode >> 2) & 0x03) ;
        return sam[samples] * 6 * sensors;
    }
 
 
    uint16_t readFromFifo()
    {
        uint8_t  status[2];
        uint16_t fifo_bytes   = 0;
 
        if ((_irq != -1) && _fifo_interrupt) {
            /*
             * Once the corresponds INT pin is configured to the push-pull mode, the FIFO watermark interrupt can be seen on the
             * corresponds INT pin. It will keep high level as long as the FIFO filled level is equal to or higher than the watermark, will
             * drop to low level as long as the FIFO filled level is lower than the configured FIFO watermark after reading out by host
             * and FIFO_RD_MODE is cleared.
            */
            if (hal->digitalRead(_irq) == LOW) {
                return false;
            }
        }
 
        size_t alloc_size = getFifoNeedBytes();
        if (!fifo_buffer) {
            fifo_buffer = (uint8_t *)calloc(alloc_size, sizeof(uint8_t));
            if (!fifo_buffer) {
                SENSORLIB_LOG_E("Calloc buffer size %u bytes failed!", alloc_size);
                return 0;
            }
            _fifo_size = alloc_size;
 
        } else if (alloc_size > _fifo_size) {
            fifo_buffer = (uint8_t *)realloc(fifo_buffer, alloc_size);
            if (!fifo_buffer) {
                SENSORLIB_LOG_E("Realloc buffer size %u bytes failed!", alloc_size);
                return 0;
            }
        }
 
        // Got FIFO watermark interrupt by INT pin or polling the FIFO_STATUS register (FIFO_WTM and/or FIFO_FULL).
        int val = comm->readRegister(QMI8658_REG_FIFO_STATUS);
        if (val == -1) {
            return 0;
        }
        SENSORLIB_LOG_D("FIFO status:0x%x", val);
 
        if (!(val & sensorlib::_bv(4))) {
            SENSORLIB_LOG_D("FIFO is Empty");
            return 0;
        }
        if (val & sensorlib::_bv(5)) {
            SENSORLIB_LOG_D("FIFO Overflow condition has happened (data dropping happened)");
            // return 0;
        }
        if (val & sensorlib::_bv(6)) {
            SENSORLIB_LOG_D("FIFO Water Mark Level Hit");
        }
        if (val & sensorlib::_bv(7)) {
            SENSORLIB_LOG_D("FIFO is Full");
        }
 
        // Read the FIFO_SMPL_CNT and FIFO_STATUS registers, to calculate the level of FIFO content data, refer to 8.4 FIFO Sample Count.
        if (comm->readRegister(QMI8658_REG_FIFO_COUNT, status, 2) == -1) {
            SENSORLIB_LOG_E("Bus communication failed!");
            return 0;
        }
 
        // FIFO_Sample_Count (in byte) = 2 * (fifo_smpl_cnt_msb[1:0] * 256 + fifo_smpl_cnt_lsb[7:0])
        fifo_bytes = 2 * (((status[1] & 0x03)) << 8 | status[0]);
 
        SENSORLIB_LOG_D("reg fifo_bytes:%d ", fifo_bytes);
 
        //Samples 16  * 6 * 2  = 192
        //Samples 32  * 6 * 2  = 384
        //Samples 64  * 6 * 2  = 768
        //Samples 128 * 6 * 2  = 1536
 
        // Send CTRL_CMD_REQ_FIFO (0x05) by CTRL9 command, to enable FIFO read mode. Refer to CTRL_CMD_REQ_FIFO for details.
        if (writeCommand(CTRL_CMD_REQ_FIFO) != 0) {
            SENSORLIB_LOG_E("Request FIFO failed!");
            return 0;
        }
 
        // Read from the FIFO_DATA register per FIFO_Sample_Count.
        if (comm->readRegister(QMI8658_REG_FIFO_DATA, fifo_buffer, fifo_bytes) == -1) {
            SENSORLIB_LOG_E("Request FIFO data failed !");
            return 0;
        }
 
        // Disable the FIFO Read Mode by setting FIFO_CTRL.FIFO_rd_mode to 0. New data will be filled into FIFO afterwards.
        if (comm->writeRegister(QMI8658_REG_FIFO_CTRL, _fifo_mode) == -1) {
            SENSORLIB_LOG_E("Clear FIFO flag failed!");
            return 0;
        }
 
        return fifo_bytes;
    }
 
 
    int writeCommand(CommandTable cmd, uint32_t wait_ms = 1000)
    {
        int      val;
        uint32_t startMillis;
        if (comm->writeRegister(QMI8658_REG_CTRL9, cmd) == -1) {
            return -1;
        }
        startMillis = hal->millis();
        do {
            val = comm->readRegister(QMI8658_REG_STATUS_INT);
            hal->delay(1);
            if (hal->millis() - startMillis > wait_ms) {
                SENSORLIB_LOG_E("wait for ctrl9 command done time out : %d val:%d", cmd, val);
                return -1;
            }
        } while (val != -1 && !(val & 0x80));
 
        if (comm->writeRegister(QMI8658_REG_CTRL9, CTRL_CMD_ACK) == -1) {
            return -1;
        }
 
        startMillis = hal->millis();
        do {
            val = comm->readRegister(QMI8658_REG_STATUS_INT);
            hal->delay(1);
            if (hal->millis() - startMillis > wait_ms) {
                SENSORLIB_LOG_E("Clear ctrl9 command done flag timeout : %d val:%d", cmd, val);
                return -1;
            }
        } while (val != -1 && (val & 0x80));
 
        return 0;
    }
 
 
    uint8_t mgToBytes(float mg)
    {
        float g = mg / 1000.0;      // Convert to grams
        int units = (int)round(g / 0.03125); //Convert grams to units of specified(1/32) resolution
        return (units & 0x1F) << 3; // Shift the 5 decimal places to the left by 3 places, because there are only 3 integer places
    }
 
 
    int _irq = -1;
    float accelScales = 0.0f, gyroScales = 0.0f;
    bool _accel_enabled = false;
    bool _gyro_enabled = false;
    bool _gDataReady = false;
    bool _aDataReady = false;
    bool _fifo_interrupt = false;
 
    uint8_t  usid[6];
    uint8_t sampleMode = ASYNC_MODE;
    uint8_t _irq_enable_mask = false;
    uint8_t _fifo_mode = 0x00;
    uint8_t *fifo_buffer = NULL;
    uint16_t _fifo_size = 0;
 
    uint32_t lastTimestamp = 0;
    uint32_t revisionID = 0x00;
 
    EventCallBack_t eventWomEvent = NULL;
    EventCallBack_t eventTagEvent = NULL;
    EventCallBack_t eventPedometerEvent = NULL;
    EventCallBack_t eventNoMotionEvent = NULL;
    EventCallBack_t eventAnyMotionEvent = NULL;
    EventCallBack_t eventSignificantMotion = NULL;
    EventCallBack_t eventGyroDataReady = NULL;
    EventCallBack_t eventAccelDataReady = NULL;
    EventCallBack_t eventDataLocking = NULL;
 
 
protected:
 
    bool initImpl(uint8_t param) override
    {
        uint8_t buffer[6] = {0};
 
 
        if (_irq != -1) {
            hal->pinMode(_irq, INPUT);
        }
 
        if (!reset()) {
            return false;
        }
 
        uint8_t id = whoAmI();
        if (id != QMI8658_REG_WHOAMI_DEFAULT) {
            SENSORLIB_LOG_E("ERROR! ID NOT MATCH QMI8658 , Response id is 0x%x", id);
            return false;
        }
        // Enable address auto increment, Big-Endian format
        // comm->writeRegister(QMI8658_REG_CTRL1, 0x60);
 
        // Little-Endian / address auto increment
        // comm->writeRegister(QMI8658_REG_CTRL1, 0x40);
 
        // no need . reset function has set
        //EN.ADDR_AI
        // comm->setRegisterBit(QMI8658_REG_CTRL1, 6);
 
        // Use STATUS_INT.bit7 as CTRL9 handshake
        comm->writeRegister(QMI8658_REG_CTRL8, 0x80);
 
        // Get firmware version and usid
        writeCommand(CTRL_CMD_COPY_USID);
 
        if (comm->readRegister(QMI8658_REG_DQW_L, buffer, 3) != -1) {
            revisionID = buffer[0] | (uint32_t)(buffer[1] << 8) | (uint32_t)(buffer[2] << 16);
            SENSORLIB_LOG_D("FW Version :0x%02X%02X%02X", buffer[0], buffer[1], buffer[2]);
        }
 
        if (comm->readRegister(QMI8658_REG_DVX_L, usid, 6) != -1) {
            SENSORLIB_LOG_D("USID :%02X%02X%02X%02X%02X%02X",
                            usid[0], usid[1], usid[2],
                            usid[3], usid[4], usid[5]);
        }
 
        return true;
    }
 
    // @brief  registers default value
    static constexpr uint8_t QMI8658_REG_WHOAMI_DEFAULT = 0x05;
    static constexpr uint8_t QMI8658_REG_STATUS_DEFAULT = 0x03;
    static constexpr uint8_t QMI8658_REG_RESET_DEFAULT = 0xB0;
 
 
    //* General Purpose Registers
    static constexpr uint8_t QMI8658_REG_WHOAMI = 0x00;
    static constexpr uint8_t QMI8658_REG_REVISION = 0x01;
 
 
    //* Setup and Control Registers
    static constexpr uint8_t QMI8658_REG_CTRL1 = 0x02;
    static constexpr uint8_t QMI8658_REG_CTRL2 = 0x03;
    static constexpr uint8_t QMI8658_REG_CTRL3 = 0x04;
    // Reserved
    static constexpr uint8_t QMI8658_REG_CTRL5 = 0x06;
    // Reserved
    static constexpr uint8_t QMI8658_REG_CTRL7 = 0x08;
    static constexpr uint8_t QMI8658_REG_CTRL8 = 0x09;
    static constexpr uint8_t QMI8658_REG_CTRL9 = 0x0A;
 
    //* Host Controlled Calibration Registers (See CTRL9, Usage is Optional)
    static constexpr uint8_t QMI8658_REG_CAL1_L = 0x0B;
    static constexpr uint8_t QMI8658_REG_CAL1_H = 0x0C;
    static constexpr uint8_t QMI8658_REG_CAL2_L = 0x0D;
    static constexpr uint8_t QMI8658_REG_CAL2_H = 0x0E;
    static constexpr uint8_t QMI8658_REG_CAL3_L = 0x0F;
    static constexpr uint8_t QMI8658_REG_CAL3_H = 0x10;
    static constexpr uint8_t QMI8658_REG_CAL4_L = 0x11;
    static constexpr uint8_t QMI8658_REG_CAL4_H = 0x12;
 
    //* FIFO Registers
    static constexpr uint8_t QMI8658_REG_FIFO_WTM_TH = 0x13;
    static constexpr uint8_t QMI8658_REG_FIFO_CTRL = 0x14;
    static constexpr uint8_t QMI8658_REG_FIFO_COUNT = 0x15;
    static constexpr uint8_t QMI8658_REG_FIFO_STATUS = 0x16;
    static constexpr uint8_t QMI8658_REG_FIFO_DATA = 0x17;
 
    //* Status Registers
    static constexpr uint8_t QMI8658_REG_STATUS_INT = 0x2D;
    static constexpr uint8_t QMI8658_REG_STATUS0 = 0x2E;
    static constexpr uint8_t QMI8658_REG_STATUS1 = 0x2F;
 
    //* Timestamp Register
    static constexpr uint8_t QMI8658_REG_TIMESTAMP_L = 0x30;
    static constexpr uint8_t QMI8658_REG_TIMESTAMP_M = 0x31;
    static constexpr uint8_t QMI8658_REG_TIMESTAMP_H = 0x32;
 
    //* Data Output Registers (16 bits 2’s Complement Except COD Sensor Data)
    static constexpr uint8_t QMI8658_REG_TEMPERATURE_L = 0x33;
    static constexpr uint8_t QMI8658_REG_TEMPERATURE_H = 0x34;
    static constexpr uint8_t QMI8658_REG_AX_L = 0x35;
    static constexpr uint8_t QMI8658_REG_AX_H = 0x36;
    static constexpr uint8_t QMI8658_REG_AY_L = 0x37;
    static constexpr uint8_t QMI8658_REG_AY_H = 0x38;
    static constexpr uint8_t QMI8658_REG_AZ_L = 0x39;
    static constexpr uint8_t QMI8658_REG_AZ_H = 0x3A;
    static constexpr uint8_t QMI8658_REG_GX_L = 0x3B;
    static constexpr uint8_t QMI8658_REG_GX_H = 0x3C;
    static constexpr uint8_t QMI8658_REG_GY_L = 0x3D;
    static constexpr uint8_t QMI8658_REG_GY_H = 0x3E;
    static constexpr uint8_t QMI8658_REG_GZ_L = 0x3F;
    static constexpr uint8_t QMI8658_REG_GZ_H = 0x40;
 
    //* COD Indication and General Purpose Registers
 
    // Calibration-On-Demand status register
    static constexpr uint8_t QMI8658_REG_COD_STATUS = 0x46;
    static constexpr uint8_t QMI8658_REG_DQW_L = 0x49;
    static constexpr uint8_t QMI8658_REG_DQW_H = 0x4A;
    static constexpr uint8_t QMI8658_REG_DQX_L = 0x4B;
    static constexpr uint8_t QMI8658_REG_DQX_H = 0x4C;
 
    static constexpr uint8_t QMI8658_REG_DQY_L = 0x4D;
    static constexpr uint8_t QMI8658_REG_DQY_H = 0x4E;
    static constexpr uint8_t QMI8658_REG_DQZ_L = 0x4F;
    static constexpr uint8_t QMI8658_REG_DQZ_H = 0x50;
 
    static constexpr uint8_t QMI8658_REG_DVX_L = 0x51;
    static constexpr uint8_t QMI8658_REG_DVX_H = 0x52;
    static constexpr uint8_t QMI8658_REG_DVY_L = 0x53;
    static constexpr uint8_t QMI8658_REG_DVY_H = 0x54;
    static constexpr uint8_t QMI8658_REG_DVZ_L = 0x55;
    static constexpr uint8_t QMI8658_REG_DVZ_H = 0x56;
 
    //* Activity Detection Output Registers
    static constexpr uint8_t QMI8658_REG_TAP_STATUS = 0x59;
    static constexpr uint8_t QMI8658_REG_STEP_CNT_LOW = 0x5A;
    static constexpr uint8_t QMI8658_REG_STEP_CNT_MID = 0x5B;
    static constexpr uint8_t QMI8658_REG_STEP_CNT_HIGH = 0x5C;
    static constexpr uint8_t QMI8658_REG_RESET = 0x60;
 
    //* Reset Register
    static constexpr uint8_t QMI8658_REG_RST_RESULT = 0x4D;
    static constexpr uint8_t QMI8658_REG_RST_RESULT_VAL = 0x80;
 
    static constexpr uint8_t STATUS0_ACCEL_AVAIL = 0x01;
    static constexpr uint8_t STATUS0_GYRO_AVAIL = 0x02;
    static constexpr uint8_t QMI8658_ACCEL_LPF_MASK = 0xF9;
    static constexpr uint8_t QMI8658_GYRO_LPF_MASK = 0x9F;
 
    static constexpr uint8_t QMI8658_ACCEL_EN_MASK = 0x01;
    static constexpr uint8_t QMI8658_GYRO_EN_MASK = 0x02;
    static constexpr uint8_t QMI8658_ACCEL_GYRO_EN_MASK = 0x03;
 
 
    static constexpr uint8_t QMI8658_FIFO_MAP_INT1 = 0x04;
};
 
#endif