sensor/imu/qmi8658/SensorQMI8658.hpp

Go to the documentation of this file.

 
#pragma once
 
#include "../../../SensorBuildOpt.h"
#if !SENSORLIB_EXCLUDE_QMI8658
 
#include <functional>
#include "../../ImuBase.hpp"
#include "../../../platform/comm/ComplexDeviceWithHal.hpp"
 
static constexpr uint8_t QMI8658_L_SLAVE_ADDRESS  = 0x6B;
static constexpr uint8_t QMI8658_H_SLAVE_ADDRESS  = 0x6A;
 
class SensorQMI8658 : public ComplexDeviceWithHal, public ImuBase
{
public:
 
    enum class AccelRange : uint8_t {
        FS_2G,   
        FS_4G,   
        FS_8G,   
        FS_16G,  
    };
 
    enum class AccelODR : uint8_t {
        ODR_7Hz,      
        ODR_14Hz,     
        ODR_28Hz,     
        ODR_56Hz,     
        ODR_112Hz,    
        ODR_224Hz,    
        ODR_448Hz,    
        ODR_896Hz,    
        ODR_1792Hz,   
        ODR_3584Hz,   
        ODR_7168Hz,   
        ODR_LP_3Hz,   
        ODR_LP_11Hz,  
        ODR_LP_21Hz,  
        ODR_LP_128Hz, 
    };
 
    enum class GyroRange : uint8_t {
        FS_16DPS,   
        FS_32DPS,   
        FS_64DPS,   
        FS_128DPS,  
        FS_256DPS,  
        FS_512DPS,  
        FS_1024DPS, 
        FS_2048DPS, 
    };
 
    enum class GyroODR : uint8_t {
        ODR_7Hz,    
        ODR_14Hz,   
        ODR_28Hz,   
        ODR_56Hz,   
        ODR_112Hz,  
        ODR_224Hz,  
        ODR_448Hz,  
        ODR_896Hz,  
        ODR_1792Hz, 
        ODR_3584Hz, 
        ODR_7168Hz, 
    };
 
    enum class LpfMode : uint8_t {
        MODE_0, 
        MODE_1, 
        MODE_2, 
        MODE_3, 
        OFF    = 0xFF,       
    };
 
    enum class FifoMode : uint8_t {
        BYPASS,  
        FIFO,    
        STREAM,  
    };
 
    enum class FifoSamples : uint8_t {
        SAMPLES_16,  
        SAMPLES_32,  
        SAMPLES_64,  
        SAMPLES_128, 
    };
 
    using IntPin = InterruptPinMap;
    using TapEvent = ::TapEvent;
 
    enum class TapPriority : uint8_t {
        X_GT_Y_GT_Z, 
        X_GT_Z_GT_Y, 
        Y_GT_X_GT_Z, 
        Y_GT_Z_GT_X, 
        Z_GT_X_GT_Y, 
        Z_GT_Y_GT_X, 
    };
 
    enum class MotionType : uint8_t {
        ANY_MOTION      = 0, 
        NO_MOTION       = 1, 
        SIGNIFICANT     = 2, 
    };
 
    enum class Layout : uint8_t {
        LAYOUT_DEFAULT     = 0, 
        LAYOUT_ROTATE_90   = 1, 
        LAYOUT_ROTATE_180  = 2, 
        LAYOUT_ROTATE_270  = 3, 
        LAYOUT_FLIP_Z      = 4, 
        LAYOUT_FLIP_Z_90   = 5, 
        LAYOUT_FLIP_Z_180  = 6, 
        LAYOUT_FLIP_Z_270  = 7, 
    };
 
    using MotionCallback = std::function<void()>;
 
    using TapCallback = std::function<void(TapEvent event)>;
 
    using DataReadyCallback = std::function<void()>;
 
    struct EventCallbacks {
        MotionCallback onAnyMotion = nullptr;
        MotionCallback onNoMotion = nullptr;
        MotionCallback onSignificantMotion = nullptr;
        MotionCallback onWakeOnMotion = nullptr;
        MotionCallback onPedometer = nullptr;
        TapCallback onTap = nullptr;
        DataReadyCallback onAccelDataReady = nullptr;
        DataReadyCallback onGyroDataReady = nullptr;
        DataReadyCallback onDataLocking = nullptr;
    };
 
    // ==================== Constructor / Destructor ====================
 
    SensorQMI8658();
 
    ~SensorQMI8658() override;
 
    // ==================== ImuBase Interface Implementation ====================
 
    bool reset() override;
 
    bool selfTest() override;
 
    bool isPresent() override;
 
    uint8_t getChipID() override;
 
    uint32_t getFirmwareVersion() override;
 
    bool setOperationMode(OperationMode mode) override;
 
    float getTemperature() override;
 
    uint32_t getTimestamp() override;
 
    bool isDataReady(uint8_t mask = static_cast<uint8_t>(ImuBase::DataReadyMask::BOTH)) override;
 
    bool readAccel(AccelerometerData &out) override;
 
    bool enableAccel() override;
 
    bool disableAccel() override;
 
    bool isAccelEnabled() override;
 
    bool setAccelFullScaleRange(AccelFullScaleRange range) override;
 
    bool setAccelOutputDataRate(float data_rate_hz) override;
 
    bool readAccelRaw(int16_t &x, int16_t &y, int16_t &z) override;
 
    bool readGyro(GyroscopeData &out) override;
 
    bool enableGyro() override;
 
    bool disableGyro() override;
 
    bool isGyroEnabled() override;
 
    bool setGyroFullScaleRange(GyroFullScaleRange range) override;
 
    bool setGyroOutputDataRate(float data_rate_hz) override;
 
    bool readGyroRaw(int16_t &x, int16_t &y, int16_t &z) override;
 
    bool enableSyncMode() override;
 
    bool disableSyncMode() override;
 
    bool configureFifo(bool enable, uint8_t watermark_samples = 16) override;
 
    uint16_t readFromFifo(AccelerometerData *accel_data, uint16_t accel_count,
                          GyroscopeData *gyro_data, uint16_t gyro_count) override;
 
    // ==================== Accelerometer Configuration ====================
 
    bool configAccel(AccelFullScaleRange range, float data_rate_hz, LpfMode lpf = LpfMode::MODE_0);
 
    // ==================== Gyroscope Configuration ====================
 
    bool configGyro(GyroFullScaleRange range, float data_rate_hz, LpfMode lpf = LpfMode::MODE_0);
 
    // ==================== Interrupt Configuration ====================
 
    bool enableInterrupt(IntPin pin);
 
    bool disableInterrupt(IntPin pin);
 
    bool enableDataReadyInterrupt(IntPin pin = IntPin::PIN2);
 
    uint8_t getInterruptStatus();
 
    void setPins(int pin);
 
    void setIntPin(int pin);
 
    // ==================== FIFO Operations ====================
 
    bool configFifo(FifoMode mode, FifoSamples samples = FifoSamples::SAMPLES_16,
                    uint8_t watermark_samples = 16);
 
    bool resetFifo();
 
    uint8_t getFifoStatus();
 
    // ==================== Synchronization ====================
 
    bool enableLockingMechanism();
 
    bool disableLockingMechanism();
 
    // ==================== Calibration ====================
 
    bool calibrate(uint16_t *gyro_x_gain = nullptr, uint16_t *gyro_y_gain = nullptr,
                   uint16_t *gyro_z_gain = nullptr);
 
    bool writeCalibration(uint16_t gyro_x_gain, uint16_t gyro_y_gain, uint16_t gyro_z_gain);
 
    void setAccelOffset(int16_t x, int16_t y, int16_t z);
 
    void setGyroOffset(int16_t x, int16_t y, int16_t z);
 
    // ==================== Self-Test ====================
 
    bool selfTestAccel();
 
    bool selfTestGyro();
 
    // ==================== Motion Detection ====================
 
    bool configMotionDetect(MotionType type, float threshold_x, float threshold_y,
                            float threshold_z, uint8_t duration);
 
    bool configMotionDetectDefault(MotionType type);
 
    bool enableMotionDetect(IntPin pin = IntPin::PIN1);
 
    bool disableMotionDetect();
 
    bool configWakeOnMotion(uint8_t threshold = 200, float odr_hz = 128.0f,
                            IntPin pin = IntPin::PIN2);
 
    bool configWakeOnMotionAdvanced(uint8_t threshold, float odr_hz, IntPin pin,
                                    uint8_t default_pin_value, uint8_t blanking_time,
                                    AccelFullScaleRange acc_range);
 
    // ==================== Legacy Compatibility API ====================
    // Keep old API signatures here only, so they can be removed together later.
    bool configAccel(AccelRange range, AccelODR odr, LpfMode lpf = LpfMode::MODE_0);
    bool configAccel(AccelRange range, float data_rate_hz, LpfMode lpf = LpfMode::MODE_0);
    bool configGyro(GyroRange range, GyroODR odr, LpfMode lpf = LpfMode::MODE_0);
    bool configGyro(GyroRange range, float data_rate_hz, LpfMode lpf = LpfMode::MODE_0);
    bool configWakeOnMotion(uint8_t threshold, AccelODR odr, IntPin pin);
    bool configWakeOnMotion(uint8_t threshold, AccelODR odr, IntPin pin,
                            uint8_t default_pin_value, uint8_t blanking_time,
                            AccelRange acc_range);
 
    // ==================== Tap Detection ====================
 
    bool configTap(TapPriority priority, uint8_t peak_window, uint16_t tap_window,
                   uint16_t double_tap_window, float peak_mag_threshold, float quiet_threshold);
 
    bool configTapDefault(TapPriority priority = TapPriority::X_GT_Y_GT_Z);
 
    bool enableTap(IntPin pin = IntPin::PIN1);
 
    bool disableTap();
 
    TapEvent getTapStatus();
 
    // ==================== Pedometer ====================
 
    bool configPedometer(uint16_t sample_count, uint16_t peak_to_peak, uint16_t peak_threshold,
                         uint16_t time_up, uint8_t time_low = 20, uint8_t entry_count = 10,
                         uint8_t fix_precision = 0, uint8_t sig_count = 4);
 
    bool configPedometerDefault(float odr = 125.0f);
 
    bool enablePedometer(IntPin pin = IntPin::DISABLE);
 
    bool disablePedometer();
 
    uint32_t getStepCount();
 
    bool resetStepCount();
 
    // ==================== Callbacks ====================
 
    void setAnyMotionCallback(MotionCallback callback);
 
    void setNoMotionCallback(MotionCallback callback);
 
    void setSignificantMotionCallback(MotionCallback callback);
 
    void setWakeOnMotionCallback(MotionCallback callback);
 
    void setTapCallback(TapCallback callback);
 
    void setPedometerCallback(MotionCallback callback);
 
    void setAccelDataReadyCallback(DataReadyCallback callback);
 
    void setGyroDataReadyCallback(DataReadyCallback callback);
 
    void setDataLockingCallback(DataReadyCallback callback);
 
    void setCallbacks(const EventCallbacks &cbs);
 
    EventCallbacks &getCallbacks();
 
    const EventCallbacks &getCallbacks() const;
 
    // ==================== Utility ====================
 
    uint16_t update();
 
    void dumpRegisters();
 
    void getChipUsid(uint8_t *buffer, uint8_t length);
 
    float getAccelScale();
 
    float getGyroScale();
 
    // ==================== Axis Conversion ====================
 
    void setAxisLayout(Layout layout);
 
    Layout getAxisLayout() const;
 
    void axisConvert(int16_t accel_data[3], int16_t gyro_data[3]);
 
    // ==================== Static Calibration ====================
 
    void enableStaticCalibration(bool enable);
 
    bool isStaticCalibrationEnabled() const;
 
    bool isStaticCalibrationComplete() const;
 
    bool processStaticCalibration(int16_t accel_data[3], int16_t gyro_data[3]);
 
    void getStaticCalibrationOffsets(int16_t accel_offset[3], int16_t gyro_offset[3]);
 
    void resetStaticCalibration();
 
    // ==================== Dynamic Gyroscope Calibration ====================
 
    void enableDynamicGyroCalibration(bool enable);
 
    bool isDynamicGyroCalibrationEnabled() const;
 
    void getDynamicGyroCalibrationOffsets(int16_t gyro_offset[3]);
 
    void processDynamicCalibration(int16_t accel_data[3], int16_t gyro_data[3]);
 
    // ==================== Hardware Self-Test ====================
 
    bool hardwareSelfTest(bool includeAccel = true, bool includeGyro = true);
 
    void getHardwareSelfTestResults(float accel_result[3], float gyro_result[3]);
 
protected:
    bool initImpl(uint8_t param) override;
    int writeCommand(uint8_t cmd, uint32_t timeout_ms = 1000);
    uint8_t mgToBytes(float mg);
 
    uint8_t getAccelRangeRegValue(AccelFullScaleRange range);
    uint8_t getGyroRangeRegValue(GyroFullScaleRange range);
    float getAccelScaleFromRange(AccelFullScaleRange range);
    float getGyroScaleFromRange(GyroFullScaleRange range);
    bool applyMotionConfig();
 
    float odrToFloat(float odr);
    uint8_t findClosestAccelOdr(float hz);
    uint8_t findClosestGyroOdr(float hz);
    uint8_t findClosestAccelLpOdr(float hz);
 
    static constexpr uint8_t STATUS_TAP_EVENT          = sensorlib::_bv(1);
    static constexpr uint8_t STATUS_WOM_EVENT          = sensorlib::_bv(2);
    static constexpr uint8_t STATUS_PEDOMETER_EVENT    = sensorlib::_bv(4);
    static constexpr uint8_t STATUS_ANY_MOTION_EVENT   = sensorlib::_bv(5);
    static constexpr uint8_t STATUS_NO_MOTION_EVENT    = sensorlib::_bv(6);
    static constexpr uint8_t STATUS_SIGNIFICANT_MOTION = sensorlib::_bv(7);
 
    AccelFullScaleRange _accel_range;
    GyroFullScaleRange _gyro_range;
    float _accel_scale;
    float _gyro_scale;
    float _accel_odr;
    float _gyro_odr;
 
    bool _motion_any_configured;
    bool _motion_no_configured;
    bool _motion_sig_configured;
    uint8_t _motion_any_thr[3];
    uint8_t _motion_no_thr[3];
    uint8_t _motion_any_window;
    uint8_t _motion_no_window;
    uint16_t _motion_sig_wait_window;
    uint16_t _motion_sig_confirm_window;
 
    bool _accel_enabled;
    bool _gyro_enabled;
    bool _sync_mode;
    bool _fifo_enabled;
    uint8_t _fifo_mode;
    uint8_t _fifo_watermark;
 
    int _int_pin;
    uint8_t _int_pin_mask;
    bool _int_enabled;
    uint32_t _last_step_count;
    bool _tap_event_active;
    TapEvent _last_tap_event;
    uint32_t _last_tap_event_ms;
 
    uint32_t _last_timestamp;
 
    uint32_t _firmware_version;
    uint8_t _usid[6];
 
    EventCallbacks callbacks;
 
    uint8_t *_fifo_buffer;
    uint16_t _fifo_buffer_size;
 
    Layout _axis_layout;
 
    static constexpr uint16_t MAX_STATIC_CALI_SAMPLES = 100;
    uint16_t _static_cali_sample_count;
    bool _static_cali_enabled;
    bool _static_cali_complete;
    int32_t _accel_cali_sum[3];
    int32_t _gyro_cali_sum[3];
    int16_t _accel_cali_offset[3];
    int16_t _gyro_cali_offset[3];
 
    static constexpr uint8_t DYN_CAL_BUFFER_SIZE = 10;
    static constexpr uint8_t DYN_CAL_STATIC_COUNT = 12;
    bool _dyn_cal_enabled;
    float _gyro_speed_buffer[DYN_CAL_BUFFER_SIZE];
    float _accel_speed_buffer[DYN_CAL_BUFFER_SIZE];
    int16_t _dyn_gyro_offset[3];
    uint32_t _dyn_static_delay;
    uint8_t _dyn_static_flag;
    uint8_t _dyn_cali_sample_count;
    int32_t _dyn_gyro_sum[3];
    float _gyro_static_deviation;
    float _accel_static_deviation;
    uint8_t _dyn_offset_applied;
 
    static constexpr float HW_ST_ACCEL_THRESHOLD_MG = 200.0f;
    static constexpr float HW_ST_GYRO_THRESHOLD_DPS = 300.0f;
    float _hw_st_accel_result[3];
    float _hw_st_gyro_result[3];
};
 
#endif