sygsp-complementary_mimu_fusion.hpp

#pragma once
/*
Copyright 2023 Travis J. West, Input Devices and Music Interaction Laboratory
(IDMIL), Centre for Interdisciplinary Research in Music Media and Technology
(CIRMMT), McGill University, Montréal, Canada, and Univ. Lille, Inria, CNRS,
Centrale Lille, UMR 9189 CRIStAL, F-59000 Lille, France
 
SPDX-License-Identifier: MIT
*/
 
#include <array>
#include "sygac-mimu.hpp"
#include "sygsp-complementary_mimu_fusion_endpoints.hpp"
 
namespace sygaldry { namespace sygsp {
 
 
 
void complementary_mimu_fusion_init( ComplementaryMimuFusionInputs& in
                                   , ComplementaryMimuFusionOutputs& out
                                   );
 
 
void complementary_mimu_fusion( const std::array<float, 3>& gyro
                              , const std::array<float, 3>& accl, bool accl_updated
                              , const std::array<float, 3>& magn, bool magn_updated
                              , const unsigned long elapsed
                              , const ComplementaryMimuFusionInputs& in
                              , ComplementaryMimuFusionOutputs& out
                              );
 
template<MimuComponent Mimu>
struct ComplementaryMimuFusion
: name_<"Complementary MIMU Fusion Filter">
, description_<"Runtime calibration and sensor fusion using a complementary filter approach">
, author_<"Travis J. West">
, copyright_<"Copyright 2023 Sygaldry Contributors">
, license_<"SPDX-License-Identifier: MIT">
, version_<"0.0.0">
{
    ComplementaryMimuFusionInputs inputs;
 
    ComplementaryMimuFusionOutputs outputs;
 
    void init()
    {
        complementary_mimu_fusion_init(inputs, outputs);
    }
 
    void main(const Mimu& mimu)
    {
        if (gyro_of(mimu).updated)
            complementary_mimu_fusion( gyro_of(mimu)
                                     , accl_of(mimu), accl_of(mimu).updated
                                     , magn_of(mimu), magn_of(mimu).updated
                                     , mimu.outputs.elapsed
                                     , inputs, outputs);
    }
};
 
 
} }