madgwick.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_MADGWICK_HPP
#define M5_UNIT_IMU_IMU_FILTER_MADGWICK_HPP
 
#include <math/quaternion.hpp>
 
#include "filter_base.hpp"
 
namespace m5 {
namespace imu {
 
class MadgwickFilter : public AhrsFilterBase {
public:
    MadgwickFilter() = default;
 
    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 writeBeta(const float beta);
    inline float readBeta() const
    {
        return _beta;
    }
 
    bool writeQuaternion(const float w, const float x, const float y, const float z);
 
private:
    m5::math::Quaternion _q{};  // Current attitude estimate (identity by default)
    float _beta{0.1f};          // Gradient descent gain
};
 
}  // namespace imu
}  // namespace m5
#endif