sensor/accelerometer/bma/SensorBMA4XX.hpp

Go to the documentation of this file.

 
#pragma once
 
#include "../../../SensorBuildOpt.h"
#if !SENSORLIB_EXCLUDE_BMA4XX_COMMON
 
#include "../../../platform/comm/ComplexStaticDeviceWithHal.hpp"
#include "../../../sensor/AccelerometerBase.hpp"
#include "../../../bosch/bma4xx/bma4.h"
 
static constexpr uint8_t BMA4XX_I2C_ADDR_SDO_LOW = (0x18);
static constexpr uint8_t BMA4XX_I2C_ADDR_SDO_HIGH = (0x19);
 
 
enum class AccelBandwidth : uint8_t {
    OSR4_AVG1    = 0,  
    OSR2_AVG2    = 1,  
    NORMAL_AVG4  = 2,  
    CIC_AVG8     = 3,  
    RES_AVG16    = 4,  
    RES_AVG32    = 5,  
    RES_AVG64    = 6,  
    RES_AVG128   = 7   
};
 
enum class AccelPerfMode : uint8_t {
    CIC_AVG_MODE     = 0,  
    CONTINUOUS_MODE  = 1   
};
 
enum class TapType {
    NO_TAP = 0,
    SINGLE_TAP = 1,
    DOUBLE_TAP = 2,
    TRIPLE_TAP = 3,
};
 
enum class ActivityType {
    STATIONARY = 0,
    WALKING = 1,
    RUNNING = 2,
    UNKNOWN = 3,
};
 
namespace BMA4XXCapability
{
enum class Capability : uint32_t {
    None              = 0,
    SupportDataReady  = (1 << 0),
    SupportAnyMotion  = (1 << 1),
    SupportNoMotion   = (1 << 2),
    SupportTap        = (1 << 3),
    SupportStepDetector = (1 << 4),
    SupportStepCounter  = (1 << 5),
    SupportActivity     = (1 << 6),
    SupportTilt         = (1 << 7),
    SupportAxisRemap    = (1 << 8),
};
 
constexpr Capability operator|(Capability a, Capability b)
{
    return static_cast<Capability>(static_cast<uint32_t>(a) | static_cast<uint32_t>(b));
}
 
constexpr bool operator&(Capability a, Capability b)
{
    return (static_cast<uint32_t>(a) & static_cast<uint32_t>(b)) != 0;
}
}
 
class SensorBMA4XX : public AccelerometerBase, public ComplexStaticDeviceWithHal
{
public:
 
    using bma4_dev_t = struct bma4_dev ;
 
    struct MotionAxesConfig {
        uint8_t x_axis : 1;        
        uint8_t y_axis : 1;        
        uint8_t z_axis : 1;        
 
        MotionAxesConfig(uint8_t x = 1, uint8_t y = 1, uint8_t z = 1)
            : x_axis(x), y_axis(y), z_axis(z) {}
    };
 
    enum  BMA4XXModel {
        BMA422 = 0x12,
        BMA423 = 0x13,
        BMA456 = 0x16,
        BMA456_AN_SEC = 0x1A
    };
 
protected:
    SensorBMA4XX(): dev(nullptr),  _remap_reg_offset(0),  _half_scale(0)
    {
    }
 
    ~SensorBMA4XX() = default;
 
 
public:
    bool readData(AccelerometerData &data) override
    {
        if (!dev) {
            SENSORLIB_LOG_E("Device not initialized");
            return false;
        }
        struct bma4_accel sens_data = { 0, 0, 0 };
        if (bma4_read_accel_xyz(&sens_data, dev.get()) != 0) {
            SENSORLIB_LOG_E("Failed to read acceleration data");
            return false;
        }
        data.raw.x = sens_data.x - _x_offset;
        data.raw.y = sens_data.y - _y_offset;
        data.raw.z = sens_data.z - _z_offset;
 
        data.temperature = getTemperature();
 
        data.mps2.x = (float)(sens_data.x) * _sensitivity;
        data.mps2.y = (float)(sens_data.y) * _sensitivity;
        data.mps2.z = (float)(sens_data.z) * _sensitivity;
 
        data.mps2.x = AccelerometerUtils::gToMps2(data.mps2.x);
        data.mps2.y = AccelerometerUtils::gToMps2(data.mps2.y);
        data.mps2.z = AccelerometerUtils::gToMps2(data.mps2.z);
 
        return true;
    }
 
    bool isDataReady() override
    {
        uint16_t int_status = 0;
        bma4_read_int_status(&int_status, dev.get());
        return (int_status & BMA4_ACCEL_DATA_RDY_INT) != 0;
    }
 
    bool reset() override
    {
        if (!dev) {
            SENSORLIB_LOG_E("Device not initialized");
            return false;
        }
        return bma4_soft_reset(dev.get()) == 0;
    }
 
    bool selfTest() override
    {
        if (!dev) {
            SENSORLIB_LOG_E("Device not initialized");
            return false;
        }
        int8_t selftest_rslt = 0;
        if (bma4_perform_accel_selftest(&selftest_rslt, dev.get()) != 0) {
            SENSORLIB_LOG_I("Self test failed");
            return false;
        }
        return selftest_rslt == 0;
    }
 
 
    bool setFullScaleRange(AccelFullScaleRange range) override
    {
        uint8_t regValue = 0;
        switch (range) {
        case AccelFullScaleRange::FS_2G:  // ±2g
            regValue = BMA4_ACCEL_RANGE_2G;
            break;
        case AccelFullScaleRange::FS_4G:  // ±4g
            regValue = BMA4_ACCEL_RANGE_4G;
            break;
        case AccelFullScaleRange::FS_8G:  // ±8g
            regValue = BMA4_ACCEL_RANGE_8G;
            break;
        case AccelFullScaleRange::FS_16G: // ±16g
            regValue = BMA4_ACCEL_RANGE_16G;
            break;
        default:
            SENSORLIB_LOG_E("Error: Invalid range %d\n", static_cast<int>(range));
            return false;
        }
        _accel_conf.range = regValue;
        if (bma4_set_accel_config(&_accel_conf, dev.get()) == 0) {
            float tmp_range = AccelerometerUtils::rangeToG(range);
            _config.range = (int)tmp_range;
            _sensitivity = tmp_range / _half_scale;
            return true;
        }
        return false;
    }
 
    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 78: // 0.78HZ
            regValue = BMA4_OUTPUT_DATA_RATE_0_78HZ;
            break;
        case 156: // 1.56HZ
            regValue = BMA4_OUTPUT_DATA_RATE_1_56HZ;
            break;
        case 312: // 3.12HZ
            regValue = BMA4_OUTPUT_DATA_RATE_3_12HZ;
            break;
        case 625: // 6.25HZ
            regValue = BMA4_OUTPUT_DATA_RATE_6_25HZ;
            break;
        case 1250: // 12.5HZ
            regValue = BMA4_OUTPUT_DATA_RATE_12_5HZ;
            break;
        case 2500: // 25.0HZ
            regValue = BMA4_OUTPUT_DATA_RATE_25HZ;
            break;
        case 5000: // 50.0HZ
            regValue = BMA4_OUTPUT_DATA_RATE_50HZ;
            break;
        case 10000: // 100.0HZ
            regValue = BMA4_OUTPUT_DATA_RATE_100HZ;
            break;
        case 20000: // 200.0HZ
            regValue = BMA4_OUTPUT_DATA_RATE_200HZ;
            break;
        case 40000: // 400.0HZ
            regValue = BMA4_OUTPUT_DATA_RATE_400HZ;
            break;
        case 80000: // 800.0HZ
            regValue = BMA4_OUTPUT_DATA_RATE_800HZ;
            break;
        case 160000: // 1600.0HZ
            regValue = BMA4_OUTPUT_DATA_RATE_1600HZ;
            break;
        default:
            SENSORLIB_LOG_E("Error: Invalid range %.2f\n", data_rate_hz);
            break;
        }
        _accel_conf.odr = regValue;
        if (bma4_set_accel_config(&_accel_conf, dev.get()) == 0) {
            _config.sample_rate = data_rate_hz;
            return true;
        }
        return false;
    }
 
    bool setBandwidth(AccelBandwidth bandwidth)
    {
        uint8_t regValue = 0;
        switch (bandwidth) {
        case AccelBandwidth::OSR4_AVG1:
            regValue = BMA4_ACCEL_OSR4_AVG1;
            break;
        case AccelBandwidth::OSR2_AVG2:
            regValue = BMA4_ACCEL_OSR2_AVG2;
            break;
        case AccelBandwidth::NORMAL_AVG4:
            regValue = BMA4_ACCEL_NORMAL_AVG4;
            break;
        case AccelBandwidth::CIC_AVG8:
            regValue = BMA4_ACCEL_CIC_AVG8;
            break;
        case AccelBandwidth::RES_AVG16:
            regValue = BMA4_ACCEL_RES_AVG16;
            break;
        case AccelBandwidth::RES_AVG32:
            regValue = BMA4_ACCEL_RES_AVG32;
            break;
        case AccelBandwidth::RES_AVG64:
            regValue = BMA4_ACCEL_RES_AVG64;
            break;
        case AccelBandwidth::RES_AVG128:
            regValue = BMA4_ACCEL_RES_AVG128;
            break;
        default:
            SENSORLIB_LOG_E("Error: Invalid bandwidth %d\n", static_cast<int>(bandwidth));
            return false;
        }
        _accel_conf.bandwidth = regValue;
        if (bma4_set_accel_config(&_accel_conf, dev.get()) == 0) {
            return true;
        }
        return false;
    }
 
    bool setPerformanceMode(AccelPerfMode mode)
    {
        uint8_t regValue = 0;
        switch (mode) {
        case AccelPerfMode::CIC_AVG_MODE:
            regValue = BMA4_CIC_AVG_MODE;
            break;
        case AccelPerfMode::CONTINUOUS_MODE:
            regValue = BMA4_CONTINUOUS_MODE;
            break;
        default:
            SENSORLIB_LOG_E("Error: Invalid performance mode %d\n", static_cast<int>(mode));
            return false;
        }
        _accel_conf.perf_mode = regValue;
        if (bma4_set_accel_config(&_accel_conf, dev.get()) == 0) {
            return true;
        }
        return false;
    }
 
    bool setOperationMode(OperationMode mode) override
    {
        bool enable = false;
        switch (mode) {
        case OperationMode::SUSPEND:
            enable = false;
            break;
        case OperationMode::NORMAL:
            enable = true;
            break;
        default:
            SENSORLIB_LOG_E("Error: Invalid operation mode %d\n", static_cast<int>(mode));
            break;
        }
        if (bma4_set_accel_enable(enable, dev.get()) == 0) {
            _config.mode = mode;
            return true;
        }
        return false;
    }
 
    bool configAccelerometer(OperationMode mode, AccelFullScaleRange range, float data_rate_hz,
                             AccelBandwidth bandwidth, AccelPerfMode perf_mode)
 
    {
        bool rslt;
 
        rslt = setFullScaleRange(range);
        if (!rslt) {
            return false;
        }
 
        rslt = setOutputDataRate(data_rate_hz);
        if (!rslt) {
            return false;
        }
 
        rslt = setBandwidth(bandwidth);
        if (!rslt) {
            return false;
        }
 
        rslt = setPerformanceMode(perf_mode);
        if (!rslt) {
            return false;
        }
 
        rslt = setOperationMode(mode);
        if (!rslt) {
            return false;
        }
        return true;
    }
 
    float getTemperature() const
    {
        float temperature = NAN;
        int32_t raw_temperature = 0;
        if (bma4_get_temperature(&raw_temperature, BMA4_DEG, dev.get()) == 0) {
            if (((raw_temperature - 23) / BMA4_SCALE_TEMP) != 0x80) {
                temperature = static_cast<float>(raw_temperature) / BMA4_SCALE_TEMP;
            }
        }
        return temperature;
    }
 
    uint16_t getInterruptStatus()
    {
        uint16_t int_status = 0;
        if (bma4_read_int_status(&int_status, dev.get()) != 0) {
            return 0;
        }
        return int_status;
    }
 
    bool isInterruptActive(uint16_t interrupt_mask)
    {
        uint16_t int_status = getInterruptStatus();
        return (int_status & interrupt_mask) != 0;
    }
 
    bool setInterruptPinConfig(InterruptPinMap pin_map,
                               bool edge_trigger = false,
                               bool active_low = true,
                               bool output_en = true,
                               bool input_en = false)
    {
        struct bma4_int_pin_config config ;
        config.edge_ctrl = edge_trigger ? BMA4_EDGE_TRIGGER : BMA4_LEVEL_TRIGGER;
        config.lvl = active_low ? BMA4_ACTIVE_LOW : BMA4_ACTIVE_HIGH;
        config.od = active_low ? BMA4_OPEN_DRAIN : BMA4_PUSH_PULL;
        config.output_en = output_en ? BMA4_OUTPUT_ENABLE : BMA4_OUTPUT_DISABLE;
        config.input_en = input_en ? BMA4_INPUT_ENABLE : BMA4_INPUT_DISABLE;
        uint8_t int_line = pin_map == InterruptPinMap::PIN1 ? BMA4_INTR1_MAP : BMA4_INTR2_MAP;
        return bma4_set_int_pin_config(&config, int_line, dev.get()) == 0;
    }
 
    bool enableInterrupt(InterruptPinMap pin_map, uint16_t interrupt_sources, bool enable)
    {
        uint8_t int_line = 0;
        switch (pin_map) {
        case InterruptPinMap::PIN1:
            int_line = 0x00;
            break;
        case InterruptPinMap::PIN2:
            int_line = 0x01;
            break;
        default:
            SENSORLIB_LOG_E("Invalid interrupt pin specified");
            return false;
        }
 
        int8_t result = bma4_map_interrupt(int_line, interrupt_sources, enable, dev.get());
        if (result != 0) {
            SENSORLIB_LOG_E("Failed to %s interrupt 0x%04X on pin %d: %d",
                            enable ? "enable" : "disable",
                            interrupt_sources,
                            (pin_map == InterruptPinMap::PIN1) ? 1 : 2,
                            result);
            return false;
        }
 
        return true;
    }
 
    virtual bool setRemapAxes(SensorRemap remap)
    {
        if (_remap_reg_offset == 0) {
            SENSORLIB_LOG_E("Remap feature not supported for this model");
            return false;
        }
        struct bma4_remap remap_data;
        switch (remap) {
        case SensorRemap::TOP_LAYER_LEFT_CORNER:
            remap_data.x = BMA4_NEG_Y;
            remap_data.y = BMA4_X;
            remap_data.z = BMA4_Z;
            break;
        case SensorRemap::TOP_LAYER_RIGHT_CORNER:
            remap_data.x = BMA4_X;
            remap_data.y = BMA4_Y;
            remap_data.z = BMA4_Z;
            break;
        case SensorRemap::TOP_LAYER_BOTTOM_RIGHT_CORNER:
            remap_data.x = BMA4_Y;
            remap_data.y = BMA4_NEG_Y;
            remap_data.z = BMA4_Z;
            break;
        case SensorRemap::TOP_LAYER_BOTTOM_LEFT_CORNER:
            remap_data.x = BMA4_NEG_X;
            remap_data.y = BMA4_NEG_Y;
            remap_data.z = BMA4_Z;
            break;
        case SensorRemap::BOTTOM_LAYER_TOP_LEFT_CORNER:
            remap_data.x = BMA4_NEG_X;
            remap_data.y = BMA4_Y;
            remap_data.z = BMA4_NEG_Z;
            break;
        case SensorRemap::BOTTOM_LAYER_TOP_RIGHT_CORNER:
            remap_data.x = BMA4_Y;
            remap_data.y = BMA4_X;
            remap_data.z = BMA4_NEG_Z;
            break;
        case SensorRemap::BOTTOM_LAYER_BOTTOM_RIGHT_CORNER:
            remap_data.x = BMA4_X;
            remap_data.y = BMA4_NEG_Y;
            remap_data.z = BMA4_NEG_Z;
            break;
        case SensorRemap::BOTTOM_LAYER_BOTTOM_LEFT_CORNER:
            remap_data.x = BMA4_NEG_Y;
            remap_data.y = BMA4_NEG_X;
            remap_data.z = BMA4_NEG_Z;
            break;
        default:
            SENSORLIB_LOG_E("Invalid remap option");
            return false;
        }
        SENSORLIB_LOG_D("Remap register offset: 0x%X", _remap_reg_offset);
        auto feature_config = std::unique_ptr<uint8_t[]>(new uint8_t[dev->feature_len]());
        return bma4_set_remap_axes(&remap_data, feature_config.get(),
                                   _remap_reg_offset, dev->feature_len, dev.get()) == 0;
    }
 
    const char *getModelName() const
    {
        if (!dev) {
            SENSORLIB_LOG_E("Device not initialized");
            return "Unknown";
        }
        switch (dev->chip_id) {
        case BMA4XXModel::BMA422:
            return "BMA422";
        case BMA4XXModel::BMA423:
            return "BMA423";
        case BMA4XXModel::BMA456:
            return "BMA456";
        case BMA4XXModel::BMA456_AN_SEC:
            return "BMA456_AN_SEC";
        default:
            return "Unknown";
        }
    }
 
    bma4_dev_t *getDev()
    {
        return dev.get();
    }
 
    uint32_t getTimeSampleMs() const
    {
        uint32_t sensor_time_ticks = 0;
        bma4_get_sensor_time(&sensor_time_ticks, dev.get());
        // Convert ticks to milliseconds (1 tick = 39.0625 μs = 0.0390625 ms)
        float time_ms = sensor_time_ticks * 0.0390625f;
        return static_cast<uint32_t>(time_ms);
    }
 
    virtual BMA4XXCapability::Capability getCapabilities() const = 0;
 
    virtual bool enableDataReady(bool enable = true, InterruptPinMap pin_map = InterruptPinMap::PIN1)
    {
        (void)enable;
        (void)pin_map;
        return false;
    }
 
    virtual bool enableAnyMotionDetection(const MotionAxesConfig &cfg, bool enable,
                                          bool interrupt_enable = false,
                                          InterruptPinMap pin_map = InterruptPinMap::PIN1)
    {
        (void)cfg;
        (void)enable;
        (void)interrupt_enable;
        (void)pin_map;
        return false;
    }
 
    virtual bool enableNoMotionDetection(const MotionAxesConfig &cfg, bool enable,
                                         bool interrupt_enable = false,
                                         InterruptPinMap pin_map = InterruptPinMap::PIN1)
    {
        (void)cfg;
        (void)enable;
        (void)interrupt_enable;
        (void)pin_map;
        return false;
    }
 
    virtual bool configAnyMotion(uint16_t threshold, uint16_t duration)
    {
        (void)threshold;
        (void)duration;
        return false;
    }
 
    virtual bool configNoMotion(uint16_t threshold, uint16_t duration)
    {
        (void)threshold;
        (void)duration;
        return false;
    }
 
    virtual bool configMotion(bool any_motion, uint16_t threshold, uint16_t duration)
    {
        (void)any_motion;
        (void)threshold;
        (void)duration;
        return false;
    }
 
    virtual bool configAnyMotionInPhysicalUnits(float threshold_mg, float duration_ms)
    {
        (void)threshold_mg;
        (void)duration_ms;
        return false;
    }
 
    virtual bool configNoMotionInPhysicalUnits(float threshold_mg, float duration_ms)
    {
        (void)threshold_mg;
        (void)duration_ms;
        return false;
    }
 
    virtual bool enableTapDetector(bool enable, bool interrupt_enable = false,
                                   InterruptPinMap pin_map = InterruptPinMap::PIN1)
    {
        (void)enable;
        (void)interrupt_enable;
        (void)pin_map;
        return false;
    }
 
    virtual bool enableStepCounter(bool enable, uint16_t step_counter_wm = 1,
                                   bool reset_counter = false)
    {
        (void)enable;
        (void)step_counter_wm;
        (void)reset_counter;
        return false;
    }
 
    virtual bool enableStepDetector(bool enable, bool interrupt_enable = false,
                                    InterruptPinMap pin_map = InterruptPinMap::PIN1)
    {
        (void)enable;
        (void)interrupt_enable;
        (void)pin_map;
        return false;
    }
 
    virtual bool enableActivityRecognition(bool enable, bool interrupt_enable = false,
                                           InterruptPinMap pin_map = InterruptPinMap::PIN1)
    {
        (void)enable;
        (void)interrupt_enable;
        (void)pin_map;
        return false;
    }
 
    virtual bool enableTiltDetector(bool enable, bool interrupt_enable = false,
                                    InterruptPinMap pin_map = InterruptPinMap::PIN1)
    {
        (void)enable;
        (void)interrupt_enable;
        (void)pin_map;
        return false;
    }
 
    virtual void update() = 0;
 
private:
    virtual bool boschInitImpl() = 0;
 
    bool initImpl(uint8_t param) override
    {
        dev = std::make_unique<struct bma4_dev>();
        if (!dev) {
            SENSORLIB_LOG_E(" Device handler malloc failed!");
            return false;
        }
        switch (_iface) {
        case COMM_I2C:
            dev->intf = BMA4_I2C_INTF;
            break;
        case COMM_SPI:
            dev->intf = BMA4_SPI_INTF;
            break;
        default:
            return false;
        }
        dev->bus_read = SensorCommStatic::sensor_static_read_data;
        dev->bus_write = SensorCommStatic::sensor_static_write_data;
        dev->intf_ptr = staticComm.get();
        dev->delay_us = SensorCommStatic::sensor_static_delay_us;
        dev->read_write_len = 32;
        dev->perf_mode_status = BMA4_DISABLE;
 
        if (bma4_soft_reset(dev.get()) != 0) {
            SENSORLIB_LOG_E("BMA4xx soft reset failed");
            return false;
        }
        hal->delay(20);
 
 
        if (!boschInitImpl()) {
            SENSORLIB_LOG_E("BMA4xx sensor initialization failed");
            return false;
        }
 
        _info.uid = dev->chip_id;
        _info.manufacturer = "Bosch-Sensortec";
        _info.model = getModelName();
        _info.type = SensorType::ACCELEROMETER;
        _info.i2c_address = _addr;
        _info.version = 1;  // Set a default version
 
        // Set default axis remapping
        struct bma4_axes_remap axes_remap;
        axes_remap.x_axis = BMA4_MAP_X_AXIS;
        axes_remap.x_axis_sign = BMA4_MAP_POSITIVE;
        axes_remap.y_axis = BMA4_MAP_Y_AXIS;
        axes_remap.y_axis_sign = BMA4_MAP_POSITIVE;
        axes_remap.z_axis = BMA4_MAP_Z_AXIS;
        axes_remap.z_axis_sign = BMA4_MAP_POSITIVE;
        dev->remap = axes_remap;
 
        // Set accelerometer configuration values
        _accel_conf.bandwidth = BMA4_ACCEL_NORMAL_AVG4;
        _accel_conf.odr = BMA4_OUTPUT_DATA_RATE_50HZ;
        _accel_conf.perf_mode = BMA4_CIC_AVG_MODE;
        _accel_conf.range = BMA4_ACCEL_RANGE_2G;
 
        _config.mode = OperationMode::SUSPEND;
        _config.range = 2.0f;
        _config.sample_rate = 50.0f;
        _config.latency = 0;
        _config.type = SensorType::ACCELEROMETER;
 
        // Calculate half scale
        _half_scale = powf(2.0f, (float)dev->resolution) / 2.0f;
 
        SENSORLIB_LOG_D("Half scale calculated: %.0f (for %d-bit sensor)",
                        _half_scale, dev->resolution);
 
        // Set default full scale range to 2g
        if (!setFullScaleRange(AccelFullScaleRange::FS_2G)) {
            SENSORLIB_LOG_E("Failed to set default 2g range");
            return false;
        }
 
        // Set default output data rate to 50Hz
        if (!setOutputDataRate(50.0f)) {
            SENSORLIB_LOG_E("Failed to set default ODR 50Hz");
            return false;
        }
 
        return true;
    }
 
protected:
    std::unique_ptr<struct bma4_dev> dev;
    struct bma4_accel_config _accel_conf;
    uint8_t  _remap_reg_offset;
    float    _half_scale;
};
 
#endif