sensor/accelerometer/bma/SensorBMA423.hpp

Go to the documentation of this file.

 
#pragma once
 
#include "../../../SensorBuildOpt.h"
#if !SENSORLIB_EXCLUDE_BMA423
 
#include "SensorBMA4XX.hpp"
#include "../../../bosch/bma4xx/bma423.h"
 
class BMA423Utils
{
public:
    static float threshold5_11ToMg(uint16_t threshold_5_11)
    {
        float g_value = static_cast<float>(threshold_5_11) / 2048.0f;
        return g_value * 1000.0f;  // Convert g to mg
    }
 
    static uint16_t mgToThreshold5_11(float threshold_mg)
    {
        if (threshold_mg < 0 || threshold_mg > 1000.0f) {
            SENSORLIB_LOG_E("Threshold out of range (0-1000mg): %.1f mg, will be clamped", threshold_mg);
            threshold_mg = (threshold_mg < 0) ? 0 : 1000.0f;
        }
        float g_value = threshold_mg / 1000.0f;  // Convert mg to g
        return static_cast<uint16_t>(g_value * 2048.0f + 0.5f);
    }
 
    static float samples50HzToMs(uint16_t samples)
    {
        return static_cast<float>(samples) * 20.0f;  // 20ms per sample at 50Hz
    }
 
    static uint16_t msToSamples50Hz(float ms)
    {
        return static_cast<uint16_t>(ms / 20.0f + 0.5f);
    }
 
};
 
class SensorBMA423 : public SensorBMA4XX
{
    struct MotionCfg {
        uint8_t motion_feature_enabled = 0;        // Any-motion and No-motion feature enable flag
        uint16_t any_motion_threshold = 0x00AA;     // 83mg
        uint16_t any_motion_duration = 0x0005;      // 100ms
        uint16_t no_motion_threshold = 0x00AA;      // 83mg
        uint16_t no_motion_duration = 0x0032;       // 1000ms
    };
 
    enum class BMA423_TapType {
        DOUBLE_TAP = 0,
        SINGLE_TAP,
    };
 
public:
 
    enum class Platform {
        SMARTPHONE,
        WRISTBAND
    };
 
    struct EventCallbacks {
        std::function<void(TapType)> onTap = nullptr;                     
        std::function<void()> onStepDetected = nullptr;                   
        std::function<void(uint32_t)> onStepCount = nullptr;              
        std::function<void(ActivityType)> onActivity = nullptr;           
        std::function<void()> onAnyMotion = nullptr;                      
        std::function<void()> onNoMotion = nullptr;                       
        std::function<void()> onTiltDetected = nullptr;                   
        std::function<void(AccelerometerData &)> onDataReady = nullptr;   
    };
 
    SensorBMA423() : SensorBMA4XX(), _status(0)
    {
        _remap_reg_offset = BMA423_AXES_REMAP_OFFSET;
        _mon_cfg.any_motion_threshold = 0x00AA;
        _mon_cfg.any_motion_duration = 0x0005;
        _mon_cfg.no_motion_threshold = 0x00AA;
        _mon_cfg.no_motion_duration = 0x0032;
        _tapType = BMA423_TapType::DOUBLE_TAP;
    }
 
    ~SensorBMA423() = default;
 
    using SensorBMA4XX::MotionAxesConfig;
 
    BMA4XXCapability::Capability getCapabilities() const override
    {
        return BMA4XXCapability::Capability::SupportDataReady
               | BMA4XXCapability::Capability::SupportAnyMotion
               | BMA4XXCapability::Capability::SupportNoMotion
               | BMA4XXCapability::Capability::SupportTap
               | BMA4XXCapability::Capability::SupportStepDetector
               | BMA4XXCapability::Capability::SupportStepCounter
               | BMA4XXCapability::Capability::SupportActivity
               | BMA4XXCapability::Capability::SupportTilt
               | BMA4XXCapability::Capability::SupportAxisRemap;
    }
 
    void setOnTapCallback(std::function<void(TapType)> cb)
    {
        callbacks.onTap = std::move(cb);
    }
 
    void setOnStepDetectedCallback(std::function<void()> cb)
    {
        callbacks.onStepDetected = std::move(cb);
    }
 
    void setOnStepCountCallback(std::function<void(uint32_t)> cb)
    {
        callbacks.onStepCount = std::move(cb);
    }
 
    void setOnActivityCallback(std::function<void(ActivityType)> cb)
    {
        callbacks.onActivity = std::move(cb);
    }
 
    void setOnAnyMotionCallback(std::function<void(void)> cb)
    {
        callbacks.onAnyMotion = std::move(cb);
    }
 
    void setOnNoMotionCallback(std::function<void(void)> cb)
    {
        callbacks.onNoMotion = std::move(cb);
    }
 
    void setOnDataReadyCallback(std::function<void(AccelerometerData)> cb)
    {
        callbacks.onDataReady = std::move(cb);
    }
 
    void setOnTiltDetectedCallback(std::function<void(void)> cb)
    {
        callbacks.onTiltDetected = std::move(cb);
    }
 
    void setCallbacks(const EventCallbacks &cbs)
    {
        callbacks = cbs;
    }
 
    EventCallbacks &getCallbacks()
    {
        return callbacks;
    }
 
    const EventCallbacks &getCallbacks() const
    {
        return callbacks;
    }
 
    bool enableFeature(uint16_t feature, bool enable)
    {
        const uint16_t motion_mask = BMA423_ANY_MOTION | BMA423_NO_MOTION;
 
        if ((feature & (BMA423_ANY_MOTION | BMA423_NO_MOTION)) == (BMA423_ANY_MOTION | BMA423_NO_MOTION)) {
            SENSORLIB_LOG_E("Both any-motion and no-motion are specified, only any-motion will be enabled.");
            feature &= (~BMA423_NO_MOTION);
        }
 
        if (enable && (feature & motion_mask)) {
            uint16_t motion_to_enable = feature & motion_mask;
            if (_mon_cfg.motion_feature_enabled != 0 &&
                    _mon_cfg.motion_feature_enabled != motion_to_enable) {
                if (bma423_feature_enable(_mon_cfg.motion_feature_enabled, 0, dev.get()) != 0) {
                    SENSORLIB_LOG_E("Failed to disable motion feature 0x%02X",
                                    _mon_cfg.motion_feature_enabled);
                    return false;
                }
                SENSORLIB_LOG_D("Disabled previous motion feature 0x%02X",
                                _mon_cfg.motion_feature_enabled);
            }
        }
 
        if (feature & BMA423_STEP_CNTR) {
            if (bma423_step_detector_enable(enable ? BMA4_ENABLE : BMA4_DISABLE, dev.get()) != 0) {
                SENSORLIB_LOG_E("Failed to %s step detector", enable ? "enable" : "disable");
                return false;
            }
        }
 
        if (bma423_feature_enable(feature, enable, dev.get()) != 0) {
            SENSORLIB_LOG_E("Failed to %s feature 0x%04X",
                            enable ? "enable" : "disable", feature);
            return false;
        }
 
        if (feature & motion_mask) {
            if (enable) {
                _mon_cfg.motion_feature_enabled = feature & motion_mask;
            } else {
                if ((feature & motion_mask) == _mon_cfg.motion_feature_enabled) {
                    _mon_cfg.motion_feature_enabled = 0;
                }
            }
        }
 
        return true;
    }
 
    bool enableDataReady(bool enable = true, InterruptPinMap pin_map = InterruptPinMap::PIN1) override
    {
        return enableInterrupt(pin_map, BMA4_DATA_RDY_INT, enable);
    }
 
    bool enableStepCounter(bool enable, uint16_t step_counter_wm = 1, bool reset_counter = false) override
    {
        if (enable) {
            if (bma423_step_counter_set_watermark(step_counter_wm, dev.get()) != 0) {
                SENSORLIB_LOG_E("Failed to set step counter watermark");
                return false;
            }
        }
        if (reset_counter) {
            if (bma423_reset_step_counter(dev.get()) != 0) {
                SENSORLIB_LOG_E("Failed to reset step counter");
                return false;
            }
        }
        return enableFeature(BMA423_STEP_CNTR, enable);
    }
 
    bool getStepCounterWatermark(uint16_t &step_counter_wm) const
    {
        return bma423_step_counter_get_watermark(&step_counter_wm, dev.get()) == 0;
    }
 
    bool enableStepDetector(bool enable, bool interrupt_enable = false,
                            InterruptPinMap pin_map = InterruptPinMap::PIN1) override
    {
        if (!enableInterrupt(pin_map, BMA423_STEP_CNTR_INT, interrupt_enable)) {
            return false;
        }
        return enableFeature(BMA423_STEP_CNTR, enable);
    }
 
    bool enableTiltDetector(bool enable, bool interrupt_enable = false,
                            InterruptPinMap pin_map = InterruptPinMap::PIN1) override
    {
        if (!enableInterrupt(pin_map, BMA423_TILT_INT, interrupt_enable)) {
            return false;
        }
        return enableFeature(BMA423_TILT, enable);
    }
 
    bool selectPlatform(Platform platform)
    {
        return bma423_select_platform(static_cast<uint8_t>(platform), dev.get()) == 0;
    }
 
    bool enableTapDetector(bool enable, bool interrupt_enable = false,
                           InterruptPinMap pin_map = InterruptPinMap::PIN1) override
    {
        if (!enableInterrupt(pin_map, BMA423_WAKEUP_INT, interrupt_enable)) {
            return false;
        }
        return enableFeature(BMA423_WAKEUP, enable);
    }
 
    bool selectTapType(TapType tap)
    {
        switch (tap) {
        case TapType::DOUBLE_TAP:
            _tapType = BMA423_TapType::DOUBLE_TAP;
            break;
        case TapType::SINGLE_TAP:
            _tapType = BMA423_TapType::SINGLE_TAP;
            break;
        default:
            break;
        }
        return bma423_tap_selection(static_cast<uint8_t>(_tapType), dev.get()) == 0;
    }
 
    bool enableActivityRecognition(bool enable, bool interrupt_enable = false,
                                   InterruptPinMap pin_map = InterruptPinMap::PIN1) override
    {
        if (!enableInterrupt(pin_map, BMA423_ACTIVITY_INT, interrupt_enable)) {
            return false;
        }
        return enableFeature(BMA423_ACTIVITY, enable);
    }
 
    bool enableAnyMotionDetection(const MotionAxesConfig &cfg, bool enable,
                                  bool interrupt_enable = false,
                                  InterruptPinMap pin_map = InterruptPinMap::PIN1) override
    {
        uint16_t axis = 0;
 
        if (cfg.x_axis == 0 && cfg.y_axis == 0 && cfg.z_axis == 0 && enable) {
            SENSORLIB_LOG_E("No motion detection axis enabled");
            return false;
        }
 
        if (cfg.x_axis) axis |= BMA423_X_AXIS_EN;
        if (cfg.y_axis) axis |= BMA423_Y_AXIS_EN;
        if (cfg.z_axis) axis |= BMA423_Z_AXIS_EN;
 
        if (bma423_anymotion_enable_axis(axis, dev.get()) != 0) {
            SENSORLIB_LOG_E("Failed to configure any-motion axes");
            return false;
        }
 
        if (!enableInterrupt(pin_map, BMA423_ANY_NO_MOTION_INT, interrupt_enable)) {
            return false;
        }
 
        if (!configMotion(true, _mon_cfg.any_motion_threshold, _mon_cfg.any_motion_duration)) {
            return false;
        }
 
        return enableFeature(BMA423_ANY_MOTION, enable);
    }
 
    bool enableNoMotionDetection(const MotionAxesConfig &cfg, bool enable,
                                 bool interrupt_enable = false,
                                 InterruptPinMap pin_map = InterruptPinMap::PIN1) override
    {
        uint16_t axis = 0;
 
        if (cfg.x_axis == 0 && cfg.y_axis == 0 && cfg.z_axis == 0 && enable) {
            SENSORLIB_LOG_E("No motion detection axis enabled");
            return false;
        }
 
        if (cfg.x_axis) axis |= BMA423_X_AXIS_EN;
        if (cfg.y_axis) axis |= BMA423_Y_AXIS_EN;
        if (cfg.z_axis) axis |= BMA423_Z_AXIS_EN;
 
        if (bma423_anymotion_enable_axis(axis, dev.get()) != 0) {
            SENSORLIB_LOG_E("Failed to configure no-motion axes");
            return false;
        }
 
        if (!enableInterrupt(pin_map, BMA423_ANY_NO_MOTION_INT, interrupt_enable)) {
            return false;
        }
 
        if (!configMotion(false, _mon_cfg.no_motion_threshold, _mon_cfg.no_motion_duration)) {
            return false;
        }
 
        return enableFeature(BMA423_NO_MOTION, enable);
    }
 
    bool configNoMotion(uint16_t threshold, uint16_t duration) override
    {
        _mon_cfg.no_motion_duration = duration;
        _mon_cfg.no_motion_threshold = threshold;
        return true;
    }
 
    bool configAnyMotion(uint16_t threshold, uint16_t duration) override
    {
        _mon_cfg.any_motion_duration = duration;
        _mon_cfg.any_motion_threshold = threshold;
        return true;
    }
 
    bool configMotion(bool any_motion, uint16_t threshold, uint16_t duration) override
    {
        struct bma423_anymotion_config any_motion_cfg;
        any_motion_cfg.threshold = threshold;
        any_motion_cfg.duration = duration;
        any_motion_cfg.nomotion_sel = any_motion ? 0 : 1;
 
        if (any_motion) {
            _mon_cfg.any_motion_threshold = threshold;
            _mon_cfg.any_motion_duration = duration;
        } else {
            _mon_cfg.no_motion_threshold = threshold;
            _mon_cfg.no_motion_duration = duration;
        }
 
        SENSORLIB_LOG_D("BMA423 motion config (raw): threshold=0x%04X, duration=%u, type: %s",
                        threshold, duration,
                        any_motion ? "Any-motion" : "No-motion");
 
        return bma423_set_any_motion_config(&any_motion_cfg, dev.get()) == 0;
    }
 
    bool getMotionConfig(uint16_t &threshold, uint16_t &duration, bool &no_motion)
    {
        struct bma423_anymotion_config any_motion;
        if (bma423_get_any_motion_config(&any_motion, dev.get()) != 0) {
            SENSORLIB_LOG_E("Failed to read BMA423 motion configuration");
            return false;
        }
        threshold = any_motion.threshold;
        duration = any_motion.duration;
        no_motion = any_motion.nomotion_sel;
        return true;
    }
 
 
    uint32_t getStepCount()
    {
        uint32_t step_count = 0;
        if (bma423_step_counter_output(&step_count, dev.get()) != 0) {
            SENSORLIB_LOG_E("Failed to read step count");
            return 0;
        }
        return step_count;
    }
 
    bool resetStepCounter()
    {
        return bma423_reset_step_counter(dev.get()) == 0;
    }
 
    ActivityType getActivityType()
    {
        uint8_t activity_type = 0;
        if (bma423_activity_output(&activity_type, dev.get()) != 0) {
            SENSORLIB_LOG_E("Failed to get activity type");
            return ActivityType::UNKNOWN;
        }
        return static_cast<ActivityType>(activity_type);
    }
 
    uint8_t getEnabledMotionFeature() const
    {
        return _mon_cfg.motion_feature_enabled;
    }
 
    bool isAnyMotionEnabled() const
    {
        return _mon_cfg.motion_feature_enabled == BMA423_ANY_MOTION;
    }
 
    bool isNoMotionEnabled() const
    {
        return _mon_cfg.motion_feature_enabled == BMA423_NO_MOTION;
    }
 
    void update() override
    {
        if (bma4_read_int_status(&_status, dev.get()) != 0) {
            SENSORLIB_LOG_E("Failed to read interrupt status");
            return;
        }
 
        if (_status == 0) {
            return;
        }
 
        if ((_status & BMA423_WAKEUP_INT) && callbacks.onTap) {
            callbacks.onTap(_tapType == BMA423_TapType::DOUBLE_TAP ? TapType::DOUBLE_TAP : TapType::SINGLE_TAP);
        }
 
        if ((_status & BMA423_STEP_CNTR_INT) && callbacks.onStepDetected) {
            callbacks.onStepDetected();
            if (callbacks.onStepCount) {
                callbacks.onStepCount(getStepCount());
            }
        }
 
        if ((_status & BMA423_ACTIVITY_INT) && callbacks.onActivity) {
            callbacks.onActivity(getActivityType());
        }
 
        if ((_status & BMA423_ANY_NO_MOTION_INT)) {
            if (_mon_cfg.motion_feature_enabled == BMA423_ANY_MOTION && callbacks.onAnyMotion) {
                callbacks.onAnyMotion();
            } else if (_mon_cfg.motion_feature_enabled == BMA423_NO_MOTION && callbacks.onNoMotion) {
                callbacks.onNoMotion();
            }
        }
 
        if ((_status & BMA4_ACCEL_DATA_RDY_INT) && callbacks.onDataReady) {
            AccelerometerData data;
            if (readData(data)) {
                callbacks.onDataReady(data);
            }
        }
 
        if ((_status & BMA423_TILT_INT) && callbacks.onTiltDetected) {
            callbacks.onTiltDetected();
        }
 
        _status = 0;
    }
 
private:
    bool boschInitImpl() override
    {
        int8_t rslt = bma423_init(dev.get());
        if ( rslt != 0 ) {
            SENSORLIB_LOG_E("bma423_init failed with code %d", rslt);
            return false;
        }
 
        dev->config_size = BMA423_CONFIG_FILE_SIZE;
 
        rslt = bma423_write_config_file(dev.get());
        if (rslt != 0) {
            SENSORLIB_LOG_E("bma423_write_config_file failed with code %d", rslt);
            return false;
        }
        return true;
    }
protected:
    EventCallbacks callbacks;
    uint16_t _status;
    MotionCfg _mon_cfg;
    BMA423_TapType _tapType;
};
 
#endif