187 lines
4.6 KiB
C++
187 lines
4.6 KiB
C++
//
|
|
// Created by sozonov on 10.11.2021.
|
|
//
|
|
|
|
#include "SignalIntegrator.hh"
|
|
|
|
|
|
schematic::SignalIntegrator::SignalIntegrator( Value & _inp ) :
|
|
i_input( _inp )
|
|
, input ( 0.0f )
|
|
, output( 0.0f )
|
|
, dt( 0.0f )
|
|
, init_state ( 0.0f )
|
|
, wrap_value( 0.0f , 0.0f )
|
|
, saturation( HUGE_VALF, -HUGE_VALF )
|
|
, saturation_upper_reached ( false )
|
|
, saturation_down_reached ( false )
|
|
, delta_wrap ( 0.0f )
|
|
, reset_to(output)
|
|
{
|
|
|
|
}
|
|
|
|
schematic::SignalIntegrator::SignalIntegrator( Value & _inp, float _wrap_up, float _wrap_down, float _pos_sat, float _neg_sat, float _init_state ) :
|
|
i_input( _inp )
|
|
, input ( 0.0f )
|
|
, output( _init_state )
|
|
, dt( 0.0f )
|
|
, init_state ( _init_state )
|
|
, wrap_value( _wrap_up > init_state ? ( _wrap_up >= _wrap_down ? _wrap_up : _wrap_down ) : 0,
|
|
_wrap_down < init_state ? ( _wrap_up >= _wrap_down ? _wrap_down : _wrap_up ) : 0 )
|
|
|
|
, saturation( _pos_sat, _neg_sat )
|
|
, saturation_upper_reached ( false )
|
|
, saturation_down_reached ( false )
|
|
, delta_wrap ( wrap_value.first - wrap_value.second )
|
|
, reset_to(output)
|
|
{
|
|
|
|
}
|
|
|
|
schematic::SignalIntegrator::Setting::Setting() :
|
|
init_state ( HUGE_VALF ),
|
|
wrap_up ( HUGE_VALF ),
|
|
wrap_down ( -HUGE_VALF ),
|
|
positive_saturation ( -HUGE_VALF ),
|
|
negative_saturation ( HUGE_VALF ) {}
|
|
|
|
|
|
bool schematic::SignalIntegrator::Setting::isValid() {
|
|
|
|
using namespace std;
|
|
|
|
const Setting non_valid;
|
|
|
|
if( !isfinite( init_state ) ) {
|
|
init_state = non_valid.init_state;
|
|
}
|
|
|
|
if( !isfinite( wrap_up ) ) {
|
|
wrap_up = non_valid.wrap_up;
|
|
}
|
|
|
|
if( !isfinite( wrap_down ) ) {
|
|
wrap_down = non_valid.wrap_down;
|
|
}
|
|
|
|
|
|
if( positive_saturation < 0 ) {
|
|
positive_saturation = non_valid.positive_saturation;
|
|
}
|
|
|
|
if( negative_saturation > 0 ) {
|
|
negative_saturation = non_valid.negative_saturation;
|
|
}
|
|
|
|
|
|
return init_state != non_valid.init_state and wrap_up != non_valid.wrap_up
|
|
and wrap_down != non_valid.wrap_down
|
|
and positive_saturation != non_valid.positive_saturation
|
|
and negative_saturation != non_valid.negative_saturation;
|
|
}
|
|
|
|
bool schematic::SignalIntegrator::configure( Setting & config ) {
|
|
|
|
init_state = config.init_state;
|
|
|
|
output = init_state;
|
|
|
|
set_wrap_value( config.wrap_up, config.wrap_down);
|
|
set_saturation( config.positive_saturation, config.negative_saturation );
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
void schematic::SignalIntegrator::set_input(float _in_value) {
|
|
input = _in_value;
|
|
}
|
|
|
|
float schematic::SignalIntegrator::get_output_value() const {
|
|
return output;
|
|
}
|
|
|
|
void schematic::SignalIntegrator::process() {
|
|
|
|
value_update();
|
|
|
|
integration();
|
|
check_saturation();
|
|
wrap_state();
|
|
|
|
}
|
|
|
|
void schematic::SignalIntegrator::integration() {
|
|
output += input * dt;
|
|
}
|
|
|
|
void schematic::SignalIntegrator::set_wrap_value( float _wr_val_up, float _wr_val_down ) {
|
|
wrap_value.first = _wr_val_up > init_state ?
|
|
( _wr_val_up >= _wr_val_down ? _wr_val_up : _wr_val_down ) : 0;
|
|
wrap_value.second = _wr_val_down < init_state ?
|
|
( _wr_val_up >= _wr_val_down ? _wr_val_down : _wr_val_up ) : 0 ;
|
|
|
|
delta_wrap = wrap_value.first - wrap_value.second;
|
|
}
|
|
|
|
void schematic::SignalIntegrator::wrap_state() {
|
|
|
|
if( output > wrap_value.first ){
|
|
output -= delta_wrap;
|
|
}
|
|
if( output < wrap_value.second ){
|
|
output += delta_wrap;
|
|
}
|
|
|
|
|
|
}
|
|
|
|
void schematic::SignalIntegrator::set_saturation( float _positive_saturation, float _negative_saturation ) {
|
|
saturation.first = _positive_saturation > 0 ? _positive_saturation : 0;
|
|
saturation.second = _negative_saturation < 0 ? _negative_saturation : 0;
|
|
}
|
|
|
|
void schematic::SignalIntegrator::check_saturation() {
|
|
|
|
saturation_down_reached = false;
|
|
saturation_upper_reached = false;
|
|
|
|
if( output >= saturation.first ) {
|
|
saturation_upper_reached = true;
|
|
output = saturation.first;
|
|
}
|
|
|
|
if( output <= saturation.second ) {
|
|
saturation_down_reached = true;
|
|
output = saturation.second;
|
|
}
|
|
|
|
}
|
|
|
|
void schematic::SignalIntegrator::set_dt(float _dt) {
|
|
if( _dt > 0){
|
|
dt = _dt;
|
|
}
|
|
}
|
|
|
|
const bool &schematic::SignalIntegrator::get_saturation_upper_reached() {
|
|
return saturation_upper_reached;
|
|
}
|
|
|
|
const bool &schematic::SignalIntegrator::get_saturation_down_reached() {
|
|
return saturation_down_reached;
|
|
}
|
|
|
|
void schematic::SignalIntegrator::value_update() {
|
|
|
|
input = i_input;
|
|
|
|
}
|
|
|
|
void schematic::SignalIntegrator::reset() {
|
|
output = init_state;
|
|
}
|
|
|
|
|