vqf.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_VQF_HPP
#define M5_UNIT_IMU_IMU_FILTER_VQF_HPP
 
#include <memory>
 
#include <math/vector3.hpp>
 
#include "filter_base.hpp"
 
// The vendored vqf.hpp (see src/imu/filter/third_party/vqf, MIT license, unmodified from
// https://github.com/dlaidig/vqf) picks its floating-point type `vqf_real_t` (used for every
// VQF member, including its in-memory layout) based on whether VQF_SINGLE_PRECISION is
// defined *at the point vqf.hpp is included* -- defaulting to `double` otherwise. Defining it
// here, right before the only include of the vendored header in this adapter's public header,
// makes every translation unit that includes this file (the adapter's .cpp, and its tests)
// see `vqf_real_t == float`, matching this repository's float-based AhrsFilterBase. This must
// stay consistent with third_party/vqf/vqf_impl.cpp, which defines the same macro before
// compiling the vendored implementation itself (see that file for why both sides need to agree).
#ifndef VQF_SINGLE_PRECISION
#define VQF_SINGLE_PRECISION
#endif
#include "third_party/vqf/vqf.hpp"
 
namespace m5 {
namespace imu {
 
class VQFFilter : public AhrsFilterBase {
public:
    VQFFilter();
 
    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 writeTauAcc(const float tau_acc);
    inline float readTauAcc() const
    {
        return static_cast<float>(_vqf->getParams().tauAcc);
    }
 
    bool writeTauMag(const float tau_mag);
    inline float readTauMag() const
    {
        return static_cast<float>(_vqf->getParams().tauMag);
    }
 
    void writeRestBiasEstimation(const bool enabled);
    inline bool readRestBiasEstimation() const
    {
        return _vqf->getParams().restBiasEstEnabled;
    }
 
    void writeMotionBiasEstimation(const bool enabled);
    inline bool readMotionBiasEstimation() const
    {
        return _vqf->getParams().motionBiasEstEnabled;
    }
 
    void writeMagDisturbanceRejection(const bool enabled);
    inline bool readMagDisturbanceRejection() const
    {
        return _vqf->getParams().magDistRejectionEnabled;
    }
 
    bool writeBiasClip(const float bias_clip_dps);
    inline float readBiasClip() const
    {
        return static_cast<float>(_vqf->getParams().biasClip);
    }
 
    m5::math::Vector3 readGyroBias() const;
 
    inline bool restDetected() const
    {
        return _vqf->getRestDetected();
    }
 
    inline void readMagNormDip(float& norm, float& dip_deg) const
    {
        const VQFState& state = _vqf->getState();
        norm                  = static_cast<float>(state.magNormDip[0]);
        dip_deg               = static_cast<float>(state.magNormDip[1]) * (180.0f / 3.14159265358979323846f);
    }
 
    inline void readRestDeviations(float& gyro_relative, float& accel_relative) const
    {
        vqf_real_t out[2]{};
        _vqf->getRelativeRestDeviations(out);
        gyro_relative  = static_cast<float>(out[0]);
        accel_relative = static_cast<float>(out[1]);
    }
 
    inline bool magDisturbanceDetected() const
    {
        return _vqf->getMagDistDetected();
    }
 
private:
    void ensure_sample_period(const float dt);
 
    std::unique_ptr<::VQF> _vqf{};  // basicvqf's filter instance (see VQFFilter()/ensure_sample_period())
    float _built_dt{};              // Sample period _vqf was (re)built with, in seconds
    bool _mag_seen{};               // Any mag sample consumed? Gates the heading convention fix
};
 
}  // namespace imu
}  // namespace m5
#endif