unit_bmm350_data.hpp

Go to the documentation of this file.

/*
 * SPDX-FileCopyrightText: 2026 M5Stack Technology CO LTD
 *
 * SPDX-License-Identifier: MIT
 */
#ifndef M5_UNIT_IMU_UNIT_UNIT_BMM350_DATA_HPP
#define M5_UNIT_IMU_UNIT_UNIT_BMM350_DATA_HPP
 
#include <array>
#include <cstddef>
#include <cstdint>
 
namespace m5 {
namespace unit {
namespace bmm350 {
 
enum class DataRate : uint8_t {
    Rate400Hz    = 0x02,  
    Rate200Hz    = 0x03,  
    Rate100Hz    = 0x04,  
    Rate50Hz     = 0x05,  
    Rate25Hz     = 0x06,  
    Rate12_5Hz   = 0x07,  
    Rate6_25Hz   = 0x08,  
    Rate3_125Hz  = 0x09,  
    Rate1_5625Hz = 0x0A,  
};
 
enum class Averaging : uint8_t {
    None,  
    X2,    
    X4,    
    X8,    
};
 
constexpr Averaging clamp_averaging(const DataRate rate, const Averaging avg)
{
    return (rate == DataRate::Rate400Hz) ? Averaging::None
           : (rate == DataRate::Rate200Hz)
               ? (static_cast<uint8_t>(avg) > static_cast<uint8_t>(Averaging::X2) ? Averaging::X2 : avg)
           : (rate == DataRate::Rate100Hz)
               ? (static_cast<uint8_t>(avg) > static_cast<uint8_t>(Averaging::X4) ? Averaging::X4 : avg)
               : avg;
}
 
constexpr int32_t fix_sign24(const uint32_t raw)
{
    return (raw >= 0x800000u) ? static_cast<int32_t>(raw - 0x1000000u) : static_cast<int32_t>(raw);
}
 
// The following conversion coefficients are the values published in the
// BMM350 datasheet (BST-BMM350-DS001), and match the ones the Bosch
// BMM350_SensorAPI (BSD-3-Clause) derives at runtime from the sensor's
// analog front-end gains in update_default_coefiecents(): e.g. for X/Y,
// (1e6/2^20) / (14.55 * 19.46 * (1/1.5) * 0.714607238769531) = 0.0070700 uT/LSB.
 
constexpr float lsb_to_ut_xy()
{
    return 0.00707f;
}
constexpr float lsb_to_ut_z()
{
    return 0.0071750f;
}
constexpr float temp_res_raw24()
{
    return 0.00098128f;
}
constexpr float TEMP_OFFSET{-25.49f};
 
struct otp_compensation_t {
    float t_sens{};     
    float t_offs{};     
    float t0{23.0f};    
    float offset[3]{};  
    float sens[3]{};    
    float tco[3]{};     
    float tcs[3]{};     
    float cross[4]{};   
 
    inline float compensate_temp(const float t_raw) const
    {
        return (1.0f + t_sens) * t_raw + t_offs;
    }
 
    void compensate_mag(float (&m)[3], const float t_comp) const
    {
        for (size_t i = 0; i < 3; ++i) {
            m[i] *= 1.0f + sens[i];
            m[i] += offset[i];
            m[i] += tco[i] * (t_comp - t0);
            m[i] /= 1.0f + tcs[i] * (t_comp - t0);
        }
 
        const float cross_xy = cross[0];
        const float cross_yx = cross[1];
        const float cross_zx = cross[2];
        const float cross_zy = cross[3];
        const float denom    = 1.0f - cross_yx * cross_xy;
 
        const float x       = (m[0] - cross_xy * m[1]) / denom;
        const float y       = (m[1] - cross_yx * m[0]) / denom;
        const float z_delta = m[0] * (cross_yx * cross_zy - cross_zx) - m[1] * (cross_zy - cross_xy * cross_zx);
        const float z       = m[2] + z_delta / denom;
 
        m[0] = x;
        m[1] = y;
        m[2] = z;
    }
};
 
struct Data {
    std::array<uint8_t, 12> raw{};  
    otp_compensation_t comp{};      
 
    inline int32_t x_raw() const
    {
        return fix_sign24(assemble24(0));
    }
    inline int32_t y_raw() const
    {
        return fix_sign24(assemble24(3));
    }
    inline int32_t z_raw() const
    {
        return fix_sign24(assemble24(6));
    }
    inline int32_t temp_raw24() const
    {
        return fix_sign24(assemble24(9));
    }
    inline int16_t temp_adc() const
    {
        return static_cast<int16_t>(temp_raw24() >> 8);
    }
 
    inline float x_ut() const
    {
        return compensated_mag()[0];
    }
    inline float y_ut() const
    {
        return compensated_mag()[1];
    }
    inline float z_ut() const
    {
        return compensated_mag()[2];
    }
 
private:
    // Assembles a little-endian 24bit unsigned value from raw[idx..idx+2]
    inline uint32_t assemble24(const size_t idx) const
    {
        return static_cast<uint32_t>(raw[idx]) | (static_cast<uint32_t>(raw[idx + 1]) << 8) |
               (static_cast<uint32_t>(raw[idx + 2]) << 16);
    }
    // Primary (pre-OTP) temperature conversion (degC)
    inline float primary_temp() const
    {
        return static_cast<float>(temp_raw24()) * temp_res_raw24() + TEMP_OFFSET;
    }
    // Primary conversion (uT) followed by full OTP compensation for all 3 axes
    inline std::array<float, 3> compensated_mag() const
    {
        std::array<float, 3> m{static_cast<float>(x_raw()) * lsb_to_ut_xy(),
                               static_cast<float>(y_raw()) * lsb_to_ut_xy(),
                               static_cast<float>(z_raw()) * lsb_to_ut_z()};
        float raw_arr[3] = {m[0], m[1], m[2]};
        comp.compensate_mag(raw_arr, comp.compensate_temp(primary_temp()));
        m[0] = raw_arr[0];
        m[1] = raw_arr[1];
        m[2] = raw_arr[2];
        return m;
    }
};
 
}  // namespace bmm350
}  // namespace unit
}  // namespace m5
#endif