fusion.hpp

Go to the documentation of this file.

/*
 * SPDX-FileCopyrightText: 2026 M5Stack Technology CO LTD
 *
 * SPDX-License-Identifier: MIT
 */
#ifndef M5_UNIT_IMU_IMU_FILTER_FUSION_HPP
#define M5_UNIT_IMU_IMU_FILTER_FUSION_HPP
 
#include "filter_base.hpp"
 
// The vendored sources are plain C (see src/imu/filter/third_party/fusion, MIT license,
// unmodified from https://github.com/xioTechnologies/Fusion) and FusionAhrs.h itself has
// no extern "C" guard of its own, so this adapter supplies one at the include site to
// give FusionAhrs.c's symbols C linkage when called from this C++ translation unit.
extern "C" {
#include "third_party/fusion/FusionAhrs.h"
}
 
namespace m5 {
namespace imu {
 
class FusionFilter : public AhrsFilterBase {
public:
    FusionFilter();
 
    virtual void reset() override;
    virtual void update(const float dt, const float accel[3], const float gyro[3], const float* mag) override;
    virtual void quaternion(float& w, float& x, float& y, float& z) const override;
 
    bool writeGain(const float gain);
    inline float readGain() const
    {
        return _settings.gain;
    }
 
    bool startupInProgress() const;
 
    bool writeQuaternion(const float w, const float x, const float y, const float z);
 
private:
    ::FusionAhrsSettings _settings{};  // Cached settings (Fusion keeps its own copy internally too)
    ::FusionAhrs _ahrs{};              // Fusion's AHRS state (quaternion + internal feedback state)
};
 
}  // namespace imu
}  // namespace m5
#endif