sensor/magnetometer/qmc/SensorQMC5883L.hpp

Go to the documentation of this file.

 
#pragma once
 
#include "../../../SensorBuildOpt.h"
#if !SENSORLIB_EXCLUDE_QMC5883L
#include "platform/comm/I2CDeviceWithHal.hpp"
#include "sensor/MagnetometerBase.hpp"
 
static constexpr uint8_t QMC5883L_SLAVE_ADDRESS = 0x0D;
 
class SensorQMC5883L : public MagnetometerBase, public I2CDeviceWithHal
{
public:
 
    SensorQMC5883L() = default;
 
    ~SensorQMC5883L() = default;
 
    bool readData(MagnetometerData &data) override
    {
        uint8_t buffer[6] = {0};
        int16_t x = 0, y = 0, z = 0;
 
        int status = readReg(REG_0x06_STATUS);
        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_E("Data overflow detected");
        } else {
            data.overflow = false;
        }
 
        // DOR (Data Skip)
        if (sensorlib::_isBitSet(status, 2)) {
            SENSORLIB_LOG_E("Data skip detected");
            data.skip_data = true;
        } else {
            data.skip_data = false;
        }
 
        // DRDY (Data Ready)
        if (!sensorlib::_isBitSet(status, 0)) {
            // SENSORLIB_LOG_W("Data not ready");
            return false;
        }
 
        if (readRegBuff(REG_0x00_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_0x06_STATUS, 0);
    }
 
    bool isDataOverflow()
    {
        return getRegBit(REG_0x06_STATUS, 1);
    }
 
    bool isDataSkipped()
    {
        return getRegBit(REG_0x06_STATUS, 2);
    }
 
    bool reset() override
    {
        writeReg(REG_0x0A_CMD2, 0x80);
        hal->delay(10);
        writeReg(REG_0x0A_CMD2, 0x00);
        return true;
    }
 
    bool selfTest() override
    {
        SENSORLIB_LOG_E("QMC5883L not self-test function");
        return true;
    }
 
    bool setFullScaleRange(MagFullScaleRange range) override
    {
        float full_scale = 0;
        float sensitivity = 0.0f;
        uint8_t range_value = 0;
        switch (range) {
        case MagFullScaleRange::FS_8G:
            sensitivity = 0.00033333f; // 3000 LSB/Gauss
            range_value = 0x01 << 4;
            full_scale = 8.0f;
            break;
        case MagFullScaleRange::FS_2G:
            sensitivity = 0.00008333f; // 12000 LSB/Gauss
            range_value = 0x00 << 4;
            full_scale = 2.0f;
            break;
        default:
            SENSORLIB_LOG_E("Invalid magnetometer range");
            return false;
        }
        if (updateBits(REG_0x09_CMD1, 0x30, range_value) > 0) {
            SENSORLIB_LOG_E("Failed to set full scale range");
            return false;
        }
        _config.range = full_scale;
        _sensitivity = sensitivity;
        return true;
    }
 
    bool setOutputDataRate(float data_rate_hz) override
    {
        int rangeInt = static_cast<int>(data_rate_hz * 100 + 0.5);
        SENSORLIB_LOG_D("Input: %.2f, Integer: %d\n", data_rate_hz, 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_0x09_CMD1, 0x0C, regValue) < 0) {
            SENSORLIB_LOG_E("Failed to set bandwidth");
            return false;
        }
        _config.sample_rate = data_rate_hz;
        return true;
    }
 
    bool setOperationMode(OperationMode mode) override
    {
        uint8_t mode_val = 0;
        switch (mode) {
        case OperationMode::SUSPEND:
            mode_val = 0x00;
            break;
        case OperationMode::NORMAL:
        case OperationMode::CONTINUOUS_MEASUREMENT:
            mode_val = 0x01;
            break;
        default:
            SENSORLIB_LOG_E("Invalid operation mode");
            return false;
        }
        if (updateBits(REG_0x09_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) {
        // 512
        case MagOverSampleRatio::OSR_8:
            osr_val = 0x00 << 6;
            _oversampling_rate = 512;
            break;
        // 256
        case MagOverSampleRatio::OSR_4:
            osr_val = 0x01 << 6;
            _oversampling_rate = 256;
            break;
        // 128
        case MagOverSampleRatio::OSR_2:
            osr_val = 0x02 << 6;
            _oversampling_rate = 128;
            break;
        // 64
        case MagOverSampleRatio::OSR_1:
            osr_val = 0x03 << 6;
            _oversampling_rate = 64;
            break;
        default:
            SENSORLIB_LOG_E("Invalid oversampling rate");
            return false;
        }
        return updateBits(REG_0x09_CMD1, 0xC0, osr_val) == 0;
    }
 
    bool setDownsamplingRate(MagDownSampleRatio dsr) override
    {
        SENSORLIB_LOG_E("QSTMagnetic does not support downsampling");
        return false;
    }
 
    bool getTemperature(float &temperature, int16_t &raw_temp)
    {
        uint8_t buffer[2] = {0};
 
        if (readRegBuff(REG_0x07_TOUT_LOW, buffer, 2) < 0) {
            SENSORLIB_LOG_E("Failed to read temperature registers");
            return false;
        }
        raw_temp = (int16_t)((buffer[1] << 8) | buffer[0]);
        // 100 LSB/°C
        temperature = (float)raw_temp / 100.0f;
        //TODO: The actual test result was 0, which may be due to a chip problem.
        return true;
    }
 
    uint8_t getChipID() const
    {
        return _info.uid;
    }
 
    bool configMagnetometer(OperationMode mode, MagFullScaleRange range, float data_rate_hz,
                            MagOverSampleRatio osr, MagDownSampleRatio dsr = MagDownSampleRatio::DSR_1) override
    {
        if (!setOperationMode(mode)) {
            SENSORLIB_LOG_E("Failed to set operation mode");
            return false;
        }
        if (!setFullScaleRange(range)) {
            SENSORLIB_LOG_E("Failed to set full scale range");
            return false;
        }
        if (!setOutputDataRate(data_rate_hz)) {
            SENSORLIB_LOG_E("Failed to set bandwidth");
            return false;
        }
        if (!setOversamplingRate(osr)) {
            SENSORLIB_LOG_E("Failed to set oversampling rate");
            return false;
        }
        return true;
    }
 
private:
 
    static constexpr uint8_t REG_0x00_LSB_DX = 0x00;
    static constexpr uint8_t REG_0x01_MSB_DX = 0x01;
    static constexpr uint8_t REG_0x02_LSB_DY = 0x02;
    static constexpr uint8_t REG_0x03_MSB_DY = 0x03;
    static constexpr uint8_t REG_0x04_LSB_DZ = 0x04;
    static constexpr uint8_t REG_0x05_MSB_DZ = 0x05;
    static constexpr uint8_t REG_0x06_STATUS = 0x06;
    static constexpr uint8_t REG_0x07_TOUT_LOW = 0x07;
    static constexpr uint8_t REG_0x08_TOUT_HIGH = 0x08;
    static constexpr uint8_t REG_0x09_CMD1 = 0x09;
    static constexpr uint8_t REG_0x0A_CMD2 = 0x0A;
    static constexpr uint8_t REG_0x0B_CMD3 = 0x0B;
    static constexpr uint8_t REG_0x0D_CHIP_ID = 0x0D;
    static constexpr uint8_t QMC5883L_CHIP_ID = 0xFF;
 
    bool initImpl(uint8_t param) override
    {
        reset();
 
        hal->delay(20);
 
        // SET/RESET Period is controlled by FBR [7:0].
        // It is recommended that the register 0BH is written by 0x01.
        uint8_t set_reset_period = 0x01;
        if (writeReg(REG_0x0B_CMD3, set_reset_period) < 0) {
            SENSORLIB_LOG_E("Failed to set SET/RESET period");
            return false;
        }
        _info.uid = readReg(REG_0x0D_CHIP_ID);
        _info.manufacturer = "QSTMagnetic";
        _info.model = "QMC5883L";
        _info.type = SensorType::MAGNETOMETER;
        _info.i2c_address = _addr;
        _info.version = 1;  // Set a default version
 
 
        // Set default configuration
        configMagnetometer(OperationMode::SUSPEND,
                           MagFullScaleRange::FS_8G,
                           50.0f,
                           MagOverSampleRatio::OSR_8);
 
        _config.mode = OperationMode::SUSPEND;
        _config.range = 2.0f;
        _config.sample_rate = 50.0f;
        _config.latency = 0;
        _config.type = SensorType::MAGNETOMETER;
 
        return _info.uid == QMC5883L_CHIP_ID;
    }
 
};
 
#endif