unit_bmm150_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_BMM150_DATA_HPP
#define M5_UNIT_IMU_UNIT_UNIT_BMM150_DATA_HPP
 
#include <array>
#include <cstdint>
 
namespace m5 {
namespace unit {
namespace bmm150 {
 
enum class DataRate : uint8_t {
    Rate10Hz = 0x00,  
    Rate2Hz  = 0x01,  
    Rate6Hz  = 0x02,  
    Rate8Hz  = 0x03,  
    Rate15Hz = 0x04,  
    Rate20Hz = 0x05,  
    Rate25Hz = 0x06,  
    Rate30Hz = 0x07,  
};
 
enum class Preset : uint8_t {
    LowPower,         
    Regular,          
    EnhancedRegular,  
    HighAccuracy,     
};
 
constexpr uint8_t preset_rep_xy(const Preset p)
{
    return (p == Preset::LowPower)          ? 0x01
           : (p == Preset::Regular)         ? 0x04
           : (p == Preset::EnhancedRegular) ? 0x07
                                            : 0x17;
}
 
constexpr uint8_t preset_rep_z(const Preset p)
{
    return (p == Preset::LowPower)          ? 0x02
           : (p == Preset::Regular)         ? 0x0E
           : (p == Preset::EnhancedRegular) ? 0x1A
                                            : 0x52;
}
 
constexpr uint16_t repetition_xy(const uint8_t rep_xy)
{
    return static_cast<uint16_t>(1 + 2 * rep_xy);
}
 
constexpr uint16_t repetition_z(const uint8_t rep_z)
{
    return static_cast<uint16_t>(1 + rep_z);
}
 
constexpr uint32_t measurement_time_us(const uint8_t rep_xy, const uint8_t rep_z)
{
    return 145U * repetition_xy(rep_xy) + 500U * repetition_z(rep_z) + 980U;
}
 
constexpr uint32_t rate_interval_us(const DataRate rate)
{
    return (rate == DataRate::Rate10Hz)   ? 100000U
           : (rate == DataRate::Rate2Hz)  ? 500000U
           : (rate == DataRate::Rate6Hz)  ? 166667U
           : (rate == DataRate::Rate8Hz)  ? 125000U
           : (rate == DataRate::Rate15Hz) ? 66667U
           : (rate == DataRate::Rate20Hz) ? 50000U
           : (rate == DataRate::Rate25Hz) ? 40000U
                                          : 33333U;  // Rate30Hz
}
 
constexpr bool repetition_fits(const DataRate rate, const uint8_t rep_xy, const uint8_t rep_z)
{
    return measurement_time_us(rep_xy, rep_z) <= rate_interval_us(rate);
}
 
constexpr int16_t OVERFLOW_OUTPUT{-32768};
 
namespace internal {
constexpr int16_t XY_OVERFLOW_RAW{-4096};
constexpr int16_t Z_OVERFLOW_RAW{-16384};
constexpr int32_t Z_POSITIVE_SATURATION{32767};
constexpr int32_t Z_NEGATIVE_SATURATION{-32767};
}  // namespace internal
 
struct trim_t {
    int8_t dig_x1{0};      
    int8_t dig_y1{0};      
    int8_t dig_x2{0};      
    int8_t dig_y2{0};      
    uint16_t dig_z1{0};    
    int16_t dig_z2{0};     
    int16_t dig_z3{0};     
    int16_t dig_z4{0};     
    uint8_t dig_xy1{0};    
    int8_t dig_xy2{0};     
    uint16_t dig_xyz1{0};  
};
 
struct Data {
    std::array<uint8_t, 8> raw{};  
    trim_t trim{};                 
 
    inline int16_t x_raw() const
    {
        const int16_t v = static_cast<int16_t>((raw[1] << 8) | raw[0]);
        return static_cast<int16_t>(v >> 3);
    }
    inline int16_t y_raw() const
    {
        const int16_t v = static_cast<int16_t>((raw[3] << 8) | raw[2]);
        return static_cast<int16_t>(v >> 3);
    }
    inline int16_t z_raw() const
    {
        const int16_t v = static_cast<int16_t>((raw[5] << 8) | raw[4]);
        return static_cast<int16_t>(v >> 1);
    }
    inline uint16_t rhall() const
    {
        const uint16_t v = static_cast<uint16_t>((raw[7] << 8) | raw[6]);
        return static_cast<uint16_t>(v >> 2);
    }
    inline bool drdy() const
    {
        return (raw[6] & 0x01) != 0;
    }
 
    inline int16_t comp_x() const
    {
        const int16_t data_x = x_raw();
        if (data_x == internal::XY_OVERFLOW_RAW) {
            return OVERFLOW_OUTPUT;
        }
        uint16_t base = rhall();
        if (base == 0) {
            base = trim.dig_xyz1;
        }
        if (base == 0) {
            return OVERFLOW_OUTPUT;
        }
 
        const int32_t xyz1_scaled = static_cast<int32_t>(trim.dig_xyz1) * 16384;
        const uint16_t normalized = static_cast<uint16_t>(xyz1_scaled / base) - static_cast<uint16_t>(0x4000);
        const int16_t centered    = static_cast<int16_t>(normalized);
        const int32_t squared     = static_cast<int32_t>(centered) * static_cast<int32_t>(centered);
        const int32_t cross_term  = static_cast<int32_t>(trim.dig_xy2) * (squared / 128);
        const int32_t xy1_scaled  = static_cast<int32_t>(static_cast<int16_t>(trim.dig_xy1) * 128);
        const int32_t linear_term = static_cast<int32_t>(centered) * xy1_scaled;
        const int32_t combined    = ((cross_term + linear_term) / 512) + static_cast<int32_t>(0x100000);
        const int32_t gain    = static_cast<int32_t>(static_cast<int16_t>(trim.dig_x2) + static_cast<int16_t>(0xA0));
        const int32_t scale   = (combined * gain) / 4096;
        const int32_t applied = static_cast<int32_t>(data_x) * scale;
 
        int16_t result = static_cast<int16_t>(applied / 8192);
        result         = static_cast<int16_t>((result + static_cast<int16_t>(trim.dig_x1) * 8) / 16);
        return result;
    }
    inline int16_t comp_y() const
    {
        const int16_t data_y = y_raw();
        if (data_y == internal::XY_OVERFLOW_RAW) {
            return OVERFLOW_OUTPUT;
        }
        uint16_t base = rhall();
        if (base == 0) {
            base = trim.dig_xyz1;
        }
        if (base == 0) {
            return OVERFLOW_OUTPUT;
        }
 
        const int32_t normalized_full = (static_cast<int32_t>(trim.dig_xyz1) * 16384) / base;
        const uint16_t normalized     = static_cast<uint16_t>(normalized_full) - static_cast<uint16_t>(0x4000);
        const int16_t centered        = static_cast<int16_t>(normalized);
        const int32_t squared         = static_cast<int32_t>(centered) * static_cast<int32_t>(centered);
        const int32_t cross_term      = static_cast<int32_t>(trim.dig_xy2) * (squared / 128);
        const int32_t xy1_scaled      = static_cast<int32_t>(static_cast<int16_t>(trim.dig_xy1) * 128);
        const int32_t combined_num    = cross_term + static_cast<int32_t>(centered) * xy1_scaled;
        const int32_t combined        = combined_num / 512;
        const int32_t gain    = static_cast<int32_t>(static_cast<int16_t>(trim.dig_y2) + static_cast<int16_t>(0xA0));
        const int32_t scale   = ((combined + static_cast<int32_t>(0x100000)) * gain) / 4096;
        const int32_t applied = static_cast<int32_t>(data_y) * scale;
 
        int16_t result = static_cast<int16_t>(applied / 8192);
        result         = static_cast<int16_t>((result + static_cast<int16_t>(trim.dig_y1) * 8) / 16);
        return result;
    }
    inline int16_t comp_z() const
    {
        const int16_t data_z = z_raw();
        if (data_z == internal::Z_OVERFLOW_RAW) {
            return OVERFLOW_OUTPUT;
        }
        const uint16_t rh = rhall();
        if (trim.dig_z2 == 0 || trim.dig_z1 == 0 || rh == 0 || trim.dig_xyz1 == 0) {
            return OVERFLOW_OUTPUT;
        }
 
        const int16_t hall_offset =
            static_cast<int16_t>(static_cast<int16_t>(rh) - static_cast<int16_t>(trim.dig_xyz1));
        const int32_t z3_term    = (static_cast<int32_t>(trim.dig_z3) * static_cast<int32_t>(hall_offset)) / 4;
        const int32_t z_delta    = static_cast<int32_t>(data_z - trim.dig_z4) * 32768;
        const int32_t z1_raw     = static_cast<int32_t>(trim.dig_z1) * (static_cast<int16_t>(rh) * 2);
        const int16_t z1_rounded = static_cast<int16_t>((z1_raw + 32768) / 65536);
 
        int32_t result = (z_delta - z3_term) / (trim.dig_z2 + z1_rounded);
 
        if (result > internal::Z_POSITIVE_SATURATION) {
            result = internal::Z_POSITIVE_SATURATION;
        } else if (result < internal::Z_NEGATIVE_SATURATION) {
            result = internal::Z_NEGATIVE_SATURATION;
        }
        result /= 16;
 
        return static_cast<int16_t>(result);
    }
 
    inline int32_t adj_x() const
    {
        return scale_16(comp_x());
    }
    inline int32_t adj_y() const
    {
        return scale_16(comp_y());
    }
    inline int32_t adj_z() const
    {
        return scale_16(comp_z());
    }
 
private:
    static inline int32_t scale_16(const int16_t compensated)
    {
        return (compensated == OVERFLOW_OUTPUT) ? compensated : static_cast<int32_t>(compensated) * 16;
    }
};
 
}  // namespace bmm150
}  // namespace unit
}  // namespace m5
#endif