mahony.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_MAHONY_HPP
#define M5_UNIT_IMU_IMU_FILTER_MAHONY_HPP
 
#include <math/quaternion.hpp>
#include <math/vector3.hpp>
 
#include "filter_base.hpp"
 
namespace m5 {
namespace imu {
 
class MahonyFilter : public AhrsFilterBase {
public:
    MahonyFilter() = 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 writeKp(const float kp);
    inline float readKp() const
    {
        return _kp;
    }
 
    bool writeKi(const float ki);
    inline float readKi() const
    {
        return _ki;
    }
 
    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)
    m5::math::Vector3 _integral_error{};  // Integral feedback accumulator (sum of e * dt)
    float _kp{1.0f};                      // Proportional feedback gain
    float _ki{0.0f};                      // Integral feedback gain
};
 
}  // namespace imu
}  // namespace m5
#endif