SensorQSTMagnetic.hpp

Go to the documentation of this file.

 
#pragma once
 
#include "platform/comm/I2CDeviceWithHal.hpp"
#include "sensor/MagnetometerBase.hpp"
 
static constexpr uint8_t QMC6310U_SLAVE_ADDRESS = 0x1C;
static constexpr uint8_t QMC6310N_SLAVE_ADDRESS = 0x3C;
static constexpr uint8_t QMC5883P_SLAVE_ADDRESS = 0x2C;
 
class SensorQSTMagnetic : public MagnetometerBase, public I2CDeviceWithHal
{
public:
    enum ChipType {
        CHIP_QMC6310U,   
        CHIP_QMC6310N,   
        CHIP_QMC5883P,   
        CHIP_UNKNOWN     
    };
 
    SensorQSTMagnetic() {}
 
    ~SensorQSTMagnetic() = default;
 
    bool readData(MagnetometerData &data) override
    {
        uint8_t buffer[6] = {0};
        int16_t x = 0, y = 0, z = 0;
 
        // QMC has no data skipping bit
        data.skip_data = false;
 
        int status = readReg(REG_0x09_STAT);
        if (status < 0) {
            SENSORLIB_LOG_E("Failed to read status register");
            return false;
        }
 
        // OVL (Overflow)
        if (sensorlib::_isBitSet(status, 1)) {
            data.overflow = true;
            SENSORLIB_LOG_W("Data overflow detected");
        } else {
            data.overflow = false;
        }
 
        // DRDY (Data Ready)
        if (!sensorlib::_isBitSet(status, 0)) {
            // SENSORLIB_LOG_E("Data not ready");
            return false;
        }
 
        if (readRegBuff(REG_0x01_LSB_DX, buffer, 6) < 0) {
            SENSORLIB_LOG_E("Failed to read magnetic field data");
            return false;
        }
 
        x = (int16_t)(buffer[1] << 8) | (buffer[0]);  // Combine X LSB and MSB
        y = (int16_t)(buffer[3] << 8) | (buffer[2]);  // Combine Y LSB and MSB
        z = (int16_t)(buffer[5] << 8) | (buffer[4]);  // Combine Z LSB and MSB
 
        data.raw.x = x - _x_offset;
        data.raw.y = y - _y_offset;
        data.raw.z = z - _z_offset;
 
        // Convert raw values to Gauss using sensitivity (depends on selected magnetic range)
        data.magnetic_field.x = (float)(data.raw.x) * _sensitivity;
        data.magnetic_field.y = (float)(data.raw.y) * _sensitivity;
        data.magnetic_field.z = (float)(data.raw.z) * _sensitivity;
 
        // Calculate heading
        data.heading = MagnetometerUtils::calculateHeading(data, _declination_rad);
 
        // Convert heading to degrees
        data.heading_degrees = data.heading * (180.0 / M_PI);
 
        return true;
    }
 
    bool isDataReady() override
    {
        return getRegBit(REG_0x09_STAT, 0);
    }
 
    bool isDataOverflow()
    {
        return getRegBit(REG_0x09_STAT, 1);
    }
 
    bool reset() override
    {
        writeReg(REG_0x0B_CMD2, (uint8_t)0x80);
        hal->delay(10);
        writeReg(REG_0x0B_CMD2, (uint8_t)0x00);
        return true;
    }
 
    bool selfTest() override
    {
        int16_t x_result = 0, y_result = 0, z_result = 0;
        return selfTest(x_result, y_result, z_result);
    }
 
    bool selfTest(int16_t &x_result, int16_t &y_result, int16_t &z_result)
    {
        if (!setOperationMode(OperationMode::CONTINUOUS_MEASUREMENT)) {
            SENSORLIB_LOG_E("Failed to set CONTINUOUS_MEASUREMENT for selfTest");
            return false;
        }
        hal->delay(20);
 
        MagnetometerData data;
 
        if (!readData(data)) {
            SENSORLIB_LOG_E("Failed to read data before selfTest");
            return false;
        }
        int16_t x_pre = data.raw.x;
        int16_t y_pre = data.raw.y;
        int16_t z_pre = data.raw.z;
 
        setRegBit(REG_0x0B_CMD2, 6);
        hal->delay(5);
 
        if (!readData(data)) {
            SENSORLIB_LOG_E("Failed to read data before selfTest");
            return false;
        }
        x_result = data.raw.x - x_pre;
        y_result = data.raw.y - y_pre;
        z_result = data.raw.z - z_pre;
 
        clrRegBit(REG_0x0B_CMD2, 6);
 
        return setOperationMode(OperationMode::SUSPEND);
    }
 
    bool setFullScaleRange(MagFullScaleRange range) override
    {
        float full_scale = 0;
        float sensitivity = 0.0f;
        uint8_t range_value = 0;
        switch (range) {
        case MagFullScaleRange::FS_30G:
            sensitivity = 0.001f;      // 1000 LSB/Gauss
            range_value = 0x00 << 2;
            full_scale = 30.0f;
            break;
        case MagFullScaleRange::FS_12G:
            sensitivity = 0.0004f;     // 2500 LSB/Gauss
            range_value = 0x01 << 2;
            full_scale = 12.0f;
            break;
        case MagFullScaleRange::FS_8G:
            sensitivity = 0.00026667f; // 3750 LSB/Gauss
            range_value = 0x02 << 2;
            full_scale = 8.0f;
            break;
        case MagFullScaleRange::FS_2G:
            sensitivity = 0.00006667f; // 15000 LSB/Gauss
            range_value = 0x03 << 2;
            full_scale = 2.0f;
            break;
        default:
            SENSORLIB_LOG_E("Invalid magnetometer range");
            return false;
        }
 
        if (updateBits(REG_0x0B_CMD2, 0x0C, range_value) < 0) {
            SENSORLIB_LOG_E("Failed to set full scale range.");
            return false;
        }
        _sensitivity = sensitivity;
        _config.range = full_scale;
        return true;
    }
 
    bool setOutputDataRate(float odr) override
    {
        int rangeInt = static_cast<int>(odr * 100 + 0.5);
        SENSORLIB_LOG_D("Input: %.2f, Integer: %d\n", odr, rangeInt);
        uint8_t regValue = 0;
        switch (rangeInt) {
        case 1000:         // 10.0
            regValue = 0x00 << 2;
            break;
        case 5000:         // 50.0
            regValue = 0x01 << 2;
            break;
        case 10000:         // 100.0
            regValue = 0x02 << 2;
            break;
        case 20000:         // 200.0
            regValue = 0x03 << 2;
            break;
        default:
            SENSORLIB_LOG_E("Invalid output data rate");
            return false;
        }
        if (updateBits(REG_0x0A_CMD1, 0x0C, regValue) < 0) {
            SENSORLIB_LOG_E("Failed to set bandwidth");
            return false;
        }
        _config.sample_rate = odr;
        return true;
    }
 
    bool setOperationMode(OperationMode mode) override
    {
        uint8_t mode_val = 0;
        switch (mode) {
        case OperationMode::SUSPEND:
            mode_val = 0x00;
            break;
        case OperationMode::NORMAL:
            mode_val = 0x01;
            break;
        case OperationMode::SINGLE_MEASUREMENT:
            mode_val = 0x02;
            break;
        case OperationMode::CONTINUOUS_MEASUREMENT:
            mode_val = 0x03;
            break;
        default:
            SENSORLIB_LOG_E("Invalid operation mode");
            return false;
        }
        if (updateBits(REG_0x0A_CMD1, 0x03, mode_val) < 0) {
            SENSORLIB_LOG_E("Failed to set operation mode");
            return false;
        }
        _config.mode = mode;
        return true;
    }
 
    bool setOversamplingRate(MagOverSampleRatio osr) override
    {
        uint8_t osr_val = 0;
        switch (osr) {
        case MagOverSampleRatio::OSR_8:
            osr_val = 0x00 << 4;
            _oversampling_rate = 8;
            break;
        case MagOverSampleRatio::OSR_4:
            osr_val = 0x01 << 4;
            _oversampling_rate = 4;
            break;
        case MagOverSampleRatio::OSR_2:
            osr_val = 0x02 << 4;
            _oversampling_rate = 2;
            break;
        case MagOverSampleRatio::OSR_1:
            osr_val = 0x03 << 4;
            _oversampling_rate = 1;
            break;
        default:
            SENSORLIB_LOG_E("Invalid oversampling rate");
            return false;
        }
        return updateBits(REG_0x0A_CMD1, 0x30, osr_val) == 0;
    }
 
    bool setDownsamplingRate(MagDownSampleRatio dsr) override
    {
        uint8_t dsr_val = 0;
        switch (dsr) {
        case MagDownSampleRatio::DSR_1:
            dsr_val = 0x00 << 6;
            break;
        case MagDownSampleRatio::DSR_2:
            dsr_val = 0x01 << 6;
            break;
        case MagDownSampleRatio::DSR_4:
            dsr_val = 0x02 << 6;
            break;
        case MagDownSampleRatio::DSR_8:
            dsr_val = 0x03 << 6;
            break;
        default:
            SENSORLIB_LOG_E("Invalid downsampling rate");
            return false;
        }
        return updateBits(REG_0x0A_CMD1, 0xC0, dsr_val) == 0;
    }
 
    uint8_t getChipID() const
    {
        return _info.uid;
    }
 
    bool configMagnetometer(OperationMode mode, MagFullScaleRange range, float odr,
                            MagOverSampleRatio osr, MagDownSampleRatio dsr) override
    {
        if (!setOperationMode(mode)) {
            return false;
        }
        if (!setFullScaleRange(range)) {
            return false;
        }
        if (!setOutputDataRate(odr)) {
            return false;
        }
        if (!setOversamplingRate(osr)) {
            return false;
        }
        if (!setDownsamplingRate(dsr)) {
            return false;
        }
        return true;
    }
 
private:
    static constexpr uint8_t REG_0x00_CHIP_ID = 0x00;
    static constexpr uint8_t REG_0x01_LSB_DX = 0x01;
    static constexpr uint8_t REG_0x02_MSB_DX = 0x02;
    static constexpr uint8_t REG_0x03_LSB_DY = 0x03;
    static constexpr uint8_t REG_0x04_MSB_DY = 0x04;
    static constexpr uint8_t REG_0x05_LSB_DZ = 0x05;
    static constexpr uint8_t REG_0x06_MSB_DZ = 0x06;
    static constexpr uint8_t REG_0x09_STAT = 0x09;
    static constexpr uint8_t REG_0x0A_CMD1 = 0x0A;
    static constexpr uint8_t REG_0x0B_CMD2 = 0x0B;
    static constexpr uint8_t REG_0x29_SIGN = 0x29;
 
    static constexpr uint8_t QMC6310_CHIP_ID = 0x80;
    static constexpr uint8_t QMC5883P_CHIP_ID = 0x80;
    static constexpr uint8_t QMC6309_CHIP_ID = 0x90;
 
    ChipType _type;
 
    bool initImpl(uint8_t param) override
    {
        uint8_t _chipId = 0;
        reset();
 
        hal->delay(20);
 
        _info.uid = readReg(REG_0x00_CHIP_ID);
        _info.manufacturer = "QSTMagnetic";
        _info.type = SensorType::MAGNETOMETER;
        _info.i2c_address = _addr;
        _info.version = 1;  // Set a default version
 
        switch (_addr) {
        case QMC6310U_SLAVE_ADDRESS:
            _type = CHIP_QMC6310U;
            _info.model = "QMC6310U";
            _chipId = QMC6310_CHIP_ID;
            break;
        case QMC6310N_SLAVE_ADDRESS:
            _type = CHIP_QMC6310N;
            _info.model = "QMC6310N";
            _chipId = QMC6310_CHIP_ID;
            break;
        case QMC5883P_SLAVE_ADDRESS:
            _type = CHIP_QMC5883P;
            _info.model = "QMC5883P";
            _chipId = QMC5883P_CHIP_ID;
            break;
        default:
            _type = CHIP_UNKNOWN;
            _info.model = "UNKNOWN";
            return false;
        }
 
        if (_info.uid == 0) {
            SENSORLIB_LOG_E("Failed to read chip ID.");
            return false;
        }
 
        if (_info.uid != _chipId) {
            SENSORLIB_LOG_E("Unexpected chip ID: 0x%02X", _chipId);
            return false;
        }
 
        SENSORLIB_LOG_D("Magnetometer initialized successfully. read chip ID: 0x%02X", _info.uid );
        // Set default configuration
        configMagnetometer(OperationMode::SUSPEND,
                           MagFullScaleRange::FS_8G,
                           50.0f,
                           MagOverSampleRatio::OSR_8,
                           MagDownSampleRatio::DSR_1);
 
        _config.mode = OperationMode::SUSPEND;
        _config.range = 8.0f;
        _config.sample_rate = 50.0f;
        _config.latency = 0;
        _config.type = SensorType::MAGNETOMETER;
 
        return true;
    }
 
};