You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
102 lines
3.0 KiB
C++
102 lines
3.0 KiB
C++
5 months ago
|
/*
|
||
|
|
* DRCDecomposer.cpp
|
||
|
|
*
|
||
|
|
* Author: Aleksey Gerasimenko
|
||
|
|
* gerasimenko.aleksey.n@gmail.com
|
||
|
|
*/
|
||
|
|
|
||
|
|
#include "SYSCTRL/DRCDecomposer.h"
|
||
|
|
|
||
|
|
namespace SYSCTRL
|
||
|
|
{
|
||
|
|
//CONSTRUCTOR
|
||
|
|
DRCDecomposer::DRCDecomposer():
|
||
|
|
m_time_sample(-1.0),
|
||
|
|
m_filter_direct(),
|
||
|
|
m_filter_quadrature(),
|
||
|
|
_aux_reg(FP_ZERO),
|
||
|
|
direct(FP_ZERO),
|
||
|
|
quadrature(FP_ZERO),
|
||
|
|
_execute(&SYSCTRL::DRCDecomposer::_execute_undef)
|
||
|
|
{}//CONSTRUCTOR
|
||
|
|
|
||
|
|
void DRCDecomposer::setup(float time_sample)
|
||
|
|
{
|
||
|
|
if(time_sample > FP_ZERO)
|
||
|
|
{
|
||
|
|
m_time_sample = time_sample;
|
||
|
|
m_filter_direct.setup(m_time_sample);
|
||
|
|
m_filter_quadrature.setup(m_time_sample);
|
||
|
|
}
|
||
|
|
//
|
||
|
|
}//
|
||
|
|
//
|
||
|
|
void DRCDecomposer::configure(const DRCDecomposerConfiguration& config)
|
||
|
|
{
|
||
|
|
if(m_time_sample > FP_ZERO)
|
||
|
|
{
|
||
|
|
m_filter_direct.configure(config.filter);
|
||
|
|
m_filter_quadrature.configure(config.filter);
|
||
|
|
|
||
|
|
if(m_filter_direct.compare(FLTSYSLIB::Filter::OPERATIONAL) && m_filter_quadrature.compare(FLTSYSLIB::Filter::OPERATIONAL))
|
||
|
|
//if(m_filter_direct.compare(FLTSYSLIB::FilterSecond::OPERATIONAL) && m_filter_quadrature.compare(FLTSYSLIB::FilterSecond::OPERATIONAL))
|
||
|
|
{
|
||
|
|
_execute = &SYSCTRL::DRCDecomposer::_execute_operational;
|
||
|
|
//
|
||
|
|
}//if
|
||
|
|
//
|
||
|
|
}//if
|
||
|
|
//
|
||
|
|
}//
|
||
|
|
//
|
||
|
|
void DRCDecomposer::reset()
|
||
|
|
{
|
||
|
|
m_filter_direct.reset();
|
||
|
|
m_filter_quadrature.reset();
|
||
|
|
this->direct = FP_ZERO;
|
||
|
|
this->quadrature = FP_ZERO;
|
||
|
|
//
|
||
|
|
}//
|
||
|
|
//
|
||
|
|
void DRCDecomposer::get_outputs(float& outd, float& outq)
|
||
|
|
{
|
||
|
|
this->direct = m_filter_direct.get_output();
|
||
|
|
this->quadrature = m_filter_quadrature.get_output();
|
||
|
|
outd = this->direct;
|
||
|
|
outq = this->quadrature;
|
||
|
|
//
|
||
|
|
}//
|
||
|
|
//
|
||
|
|
void DRCDecomposer::execute(float in_a, float in_b, float in_c,
|
||
|
|
float ort_direct_a, float ort_direct_b, float ort_direct_c,
|
||
|
|
float ort_orthogonal_a, float ort_orthogonal_b, float ort_orthogonal_c)
|
||
|
|
{
|
||
|
|
(this->*_execute)(in_a, in_b, in_c,
|
||
|
|
ort_direct_a, ort_direct_b, ort_direct_c,
|
||
|
|
ort_orthogonal_a, ort_orthogonal_b, ort_orthogonal_c);
|
||
|
|
//
|
||
|
|
}//
|
||
|
|
//
|
||
|
|
void DRCDecomposer::_execute_undef(float in_a, float in_b, float in_c,
|
||
|
|
float ort_direct_a, float ort_direct_b, float ort_direct_c,
|
||
|
|
float ort_orthogonal_a, float ort_orthogonal_b, float ort_orthogonal_c)
|
||
|
|
{
|
||
|
|
//
|
||
|
|
}//
|
||
|
|
//
|
||
|
|
#pragma CODE_SECTION("ramfuncs");
|
||
|
|
void DRCDecomposer::_execute_operational(float in_a, float in_b, float in_c,
|
||
|
|
float ort_direct_a, float ort_direct_b, float ort_direct_c,
|
||
|
|
float ort_orthogonal_a, float ort_orthogonal_b, float ort_orthogonal_c)
|
||
|
|
{
|
||
|
|
_aux_reg = (0.4714045)*(in_a * ort_direct_a + in_b * ort_direct_b + in_c * ort_direct_c);
|
||
|
|
direct = m_filter_direct.execute(_aux_reg);
|
||
|
|
_aux_reg = (-0.4714045)*(in_a * ort_orthogonal_a + in_b * ort_orthogonal_b + in_c * ort_orthogonal_c);
|
||
|
|
quadrature = m_filter_quadrature.execute(_aux_reg);
|
||
|
|
//
|
||
|
|
}//
|
||
|
|
//
|
||
|
|
|
||
|
|
|
||
|
|
} /* namespace SYSCTRL */
|