ahrs_controller.hpp

Go to the documentation of this file.

/*
 * SPDX-FileCopyrightText: 2026 M5Stack Technology CO LTD
 *
 * SPDX-License-Identifier: MIT
 */
#ifndef M5_UNIT_IMU_IMU_AHRS_CONTROLLER_HPP
#define M5_UNIT_IMU_IMU_AHRS_CONTROLLER_HPP
 
#include <cstddef>
#include <cstdint>
 
#include <math/quaternion.hpp>
 
#include "axis_order.hpp"
#include "filter/filter_base.hpp"
#include "imu_data_source.hpp"
#include "imu_types.hpp"
 
namespace m5 {
namespace imu {
 
class AhrsController {
public:
    struct calibration_data_t {
        uint32_t version{1};      
        float accel_offset[3]{};  
        float gyro_offset[3]{};   
        float mag_offset[3]{};    
    };
 
    AhrsController();
    ~AhrsController();
 
    AhrsController(const AhrsController&) = delete;
    AhrsController& operator=(const AhrsController&) = delete;
 
    bool add(IMUDataSource& src);
 
    bool writeAxisOrder(const IMUDataSource& src, const AxisOrder& order);
 
    bool readAxisOrder(const IMUDataSource& src, AxisOrder& out) const;
 
    bool writeMagAxisOrder(const IMUDataSource& src, const AxisOrder& order);
 
    bool readMagAxisOrder(const IMUDataSource& src, AxisOrder& out) const;
 
    void writeUserAxisOrder(const AxisOrder& order);
 
    void readUserAxisOrder(AxisOrder& out) const;
 
    void writeAutoCalibration(const uint8_t accel, const uint8_t gyro, const uint8_t mag);
 
    void readAutoCalibration(uint8_t& accel, uint8_t& gyro, uint8_t& mag) const;
 
    void writeMagCalibrationRadius(const float radius_ut);
 
    float readMagCalibrationRadius() const;
 
    bool calibrateGyro(const uint32_t duration_ms);
 
    bool calibrateGyro(const uint32_t update_count, const float dt);
 
    bool readCalibration(calibration_data_t& out) const;
 
    bool writeCalibration(const calibration_data_t& data);
 
    bool begin();
 
    Capability update();
 
    Capability update(const float dt);
 
    bool sleep();
 
    bool wakeup();
 
    bool selectFilter(const FilterType type);
 
    void setFilter(AhrsFilterBase& filter);
 
    inline AhrsFilterBase* filter() const
    {
        return _filter;
    }
 
    void quaternion(float& w, float& x, float& y, float& z) const;
 
    void euler(float& roll_deg, float& pitch_deg, float& yaw_deg) const;
 
    void tareYaw();
 
    void clearYawTare();
 
    inline const imu_physical_data_t& latest() const
    {
        return _latest;
    }
 
    inline bool hasAccel() const
    {
        return _accel_src != nullptr;
    }
 
    inline bool hasGyro() const
    {
        return _gyro_src != nullptr;
    }
 
    inline bool hasTemp() const
    {
        return _temp_src != nullptr;
    }
 
    inline bool hasValidMag() const
    {
        return _has_valid_mag;
    }
 
    inline unsigned long lastMicros() const
    {
        return _last_micros;
    }
 
private:
    // Per-capability auto-calibration state (M5Unified IMU_Class::offset_point_t, ported to
    // physical-unit floats -- see ahrs_controller.cpp for update_stillness()/calibrate()).
    struct axis_offset_state_t {
        float offset[3]{};           // Current offset estimate, subtracted from every sample
        float prev[3]{};             // Previous physical sample, for stillness diffing
        float avg[3]{};              // Exponential moving average of recent samples
        float radius{};              // Expected magnitude of (sample - offset) at rest
        float tolerance{};           // Tolerance band around radius before nudging the offset
        float noise{};               // Per-sample movement below which stillness accumulates
        uint8_t average_shifter{1};  // avg update rate: avg += diff / (1 << average_shifter)
        uint8_t stillness{};         // 0 = moving, saturates toward 255 while stationary
        uint8_t strength{};          // 0 = auto-calibration off, 1-255 = adjustment speed
        // Radius adaptation: 0 keeps `radius` fixed (correct for accel, whose sphere is 1G
        // everywhere, and for gyro, whose sphere is the origin). Non-zero makes `radius` track the
        // observed magnitude at rate 1/(1 << radius_shift) per calibrate() call -- needed for the
        // magnetometer, whose sphere is the local field strength and therefore varies by more than
        // a factor of two across the planet (~25-65uT). Must stay much slower than the offset
        // correction itself, so that it settles on the average over orientations rather than
        // chasing each individual sample (which would cancel the correction entirely).
        uint8_t radius_shift{};
        bool radius_seeded{};  // First observed magnitude has been taken as the initial radius
 
        uint8_t update_stillness(const float sample[3]);
        void calibrate();
    };
 
    static constexpr size_t MAX_SOURCES{4};
 
    static bool is_mag_overflow(const int32_t mag[3]);
    static void apply_offset_and_calibrate(axis_offset_state_t& cal, float v[3]);
 
    bool find_source_index(const IMUDataSource& src, size_t& out_index) const;
    Capability drain_and_fuse(const float dt);
    void begin_gyro_collect();
    bool end_gyro_collect();
    void recompute_composed(const size_t index);  // Fold _user_axis_order onto one source's fixed order
    void recompute_all_composed();                // ...for every added source
 
    IMUDataSource* _sources[MAX_SOURCES]{};
    AxisOrder _axis_orders[MAX_SOURCES]{};      // Fixed board correction for Accel/Gyro/Temp
    AxisOrder _mag_axis_orders[MAX_SOURCES]{};  // Fixed board correction for Mag (writeMagAxisOrder())
    AxisOrder _user_axis_order{};               // Global user remap (writeUserAxisOrder())
 
    // Fixed order folded with the user order; these are what update() actually applies.
    AxisOrder _composed_axis_orders[MAX_SOURCES]{};
    AxisOrder _composed_mag_axis_orders[MAX_SOURCES]{};
    size_t _source_count{};
 
    IMUDataSource* _accel_src{};
    IMUDataSource* _gyro_src{};
    IMUDataSource* _mag_src{};
    IMUDataSource* _temp_src{};
 
    AhrsFilterBase* _owned_filter{};  // Non-null only when constructed by selectFilter()
    AhrsFilterBase* _filter{};        // Active filter (owned or injected); nullptr until chosen
 
    // World-frame Z rotation composed onto the filter's raw quaternion by quaternion()/euler()
    // (see tareYaw()/clearYawTare()); identity (no-op) by default.
    m5::math::Quaternion _yaw_tare{};
 
    imu_physical_data_t _latest{};
    bool _has_valid_mag{false};
 
    unsigned long _last_micros{};
    bool _has_last_micros{false};
    float _dt_ema{};
    bool _has_dt_ema{false};
    // Time from update() calls that drained no gyro sample, not yet handed to the filter
    float _pending_dt{};
 
    axis_offset_state_t _accel_cal{};
    axis_offset_state_t _gyro_cal{};
    axis_offset_state_t _mag_cal{};
 
    bool _gyro_collecting{false};
    float _gyro_collect_sum[3]{};
    uint32_t _gyro_collect_count{};
};
 
}  // namespace imu
}  // namespace m5
#endif