253 lines
6.6 KiB
C++
253 lines
6.6 KiB
C++
/*
|
|
* Amc1210Analizer.cpp
|
|
*
|
|
* Created on: 16 ìàð. 2018 ã.
|
|
* Author: titov
|
|
*/
|
|
|
|
#include "Amc1210Analizer.hh"
|
|
|
|
#include "../../math/math_inc.hh"
|
|
|
|
float driver::detail::Amc1210Analizer::getSinChannel() const {
|
|
return raw.sin;
|
|
}
|
|
|
|
float driver::detail::Amc1210Analizer::getCosChannel() const {
|
|
return raw.cos;
|
|
}
|
|
|
|
void driver::detail::Amc1210Analizer::setSampleTime( float Ts ) {
|
|
|
|
zero_crossing.max_delta = max_speed * Ts;
|
|
non_update.max_delta = max_speed * Ts;
|
|
spike.max_delta = max_speed * Ts;
|
|
|
|
}
|
|
|
|
driver::detail::Amc1210Analizer::Amc1210Analizer(
|
|
driver::IResolver & scrt_amc1210) : amc_scrt(scrt_amc1210),
|
|
code(0.0f, 0.0f), prev(0.0f, 0.0f), prev_delta(0.0f, 0.0f), old(0.0f, 0.0f),
|
|
raw(0.0f, 0.0f), valid_signal(false), max_speed(0.0f), count_error(0), failure_amc1210(false),
|
|
zero_crossing(), non_update(), overflow(), spike() {}
|
|
|
|
bool driver::detail::Amc1210Analizer::configure( Setting & config ) {
|
|
|
|
max_speed = config.max_frequency * config.max_amplitude * math::constants::pi2;
|
|
|
|
zero_crossing.max_value = config.max_value;
|
|
non_update.min_delta = config.max_noise;
|
|
overflow.max_value = config.max_value;
|
|
|
|
return true;
|
|
}
|
|
|
|
void driver::detail::Amc1210Analizer::process() {
|
|
|
|
code = Quad( amc_scrt.getSinChannel(), amc_scrt.getCosChannel() );
|
|
|
|
Quad correct_1 = zero_crossing.correct( code, prev );
|
|
|
|
Quad cur_delta = correct_1 - prev;
|
|
|
|
Quad correct_2 = non_update.correct( correct_1, cur_delta, prev_delta );
|
|
|
|
bool valid = true;
|
|
|
|
valid = overflow.validate( correct_2 ) && valid;
|
|
valid = spike.validate( correct_2, cur_delta, old ) && valid;
|
|
|
|
valid_signal = valid;
|
|
|
|
if( !valid || raw == correct_2 ) {
|
|
if( count_error < max_error )
|
|
count_error++;
|
|
else
|
|
failure_amc1210 = true;
|
|
} else {
|
|
count_error = 0;
|
|
failure_amc1210 = false;
|
|
}
|
|
|
|
old = prev;
|
|
prev = correct_1;
|
|
raw = correct_2;
|
|
prev_delta = cur_delta;
|
|
|
|
}
|
|
|
|
driver::detail::Amc1210Analizer::Quad driver::detail::Amc1210Analizer::ZeroCrossingCorrection::correct(
|
|
const Quad & current, const Quad & previous ) const {
|
|
|
|
//Îáðàáîòêà íåóäà÷íîãî ïåðåõîäà ÷åðåç 0.
|
|
if( ( previous.sin <= 0 && previous.sin > - max_delta )
|
|
|| ( previous.sin >= 0 && previous.sin < max_delta ) ) {
|
|
|
|
if( current.sin < ( - max_value ) )
|
|
return Quad( current.sin + 32767, current.cos );
|
|
|
|
if( current.sin > ( max_value ) )
|
|
return Quad( current.sin - 32768, current.cos );
|
|
|
|
}
|
|
|
|
//Îáðàáîòêà íåóäà÷íîãî ïåðåõîäà ÷åðåç 0.
|
|
if( ( previous.cos <= 0 && previous.cos > - max_delta )
|
|
|| ( previous.cos >= 0 && previous.cos < max_delta ) ) {
|
|
|
|
if( current.cos < ( - max_value ) )
|
|
return Quad( current.sin, current.cos + 32767 );
|
|
|
|
if( current.cos > ( max_value ) )
|
|
return Quad( current.sin, current.cos - 32768 );
|
|
|
|
}
|
|
|
|
return Quad( current.sin, current.cos );
|
|
}
|
|
|
|
driver::detail::Amc1210Analizer::Quad driver::detail::Amc1210Analizer::NonUpdateValues::correct(
|
|
const Quad & current, const Quad & current_delta, const Quad & previous_delta ) const {
|
|
|
|
const float prev_dsin = fabsf( previous_delta.sin );
|
|
|
|
//Ïðîâåðêà íà îáíîâëåíèå äàííûõ â äâèæåíèè.
|
|
if( ( prev_dsin > min_delta )
|
|
&& ( prev_dsin < max_delta )
|
|
&& ( current_delta.sin == 0.0f ) )
|
|
|
|
return Quad( current.sin + previous_delta.sin * 0.5f, current.cos );
|
|
|
|
const float prev_dcos = fabsf( previous_delta.cos );
|
|
|
|
//Ïðîâåðêà íà îáíîâëåíèå äàííûõ â äâèæåíèè.
|
|
if( ( prev_dcos > min_delta )
|
|
&& ( prev_dcos < max_delta )
|
|
&& ( current_delta.cos == 0.0f ) )
|
|
|
|
return Quad( current.sin, current.cos + previous_delta.cos * 0.5f );
|
|
|
|
return Quad( current.sin, current.cos );
|
|
}
|
|
|
|
bool driver::detail::Amc1210Analizer::Overflow::validate( Quad & current ) {
|
|
|
|
using namespace std;
|
|
|
|
bool result( true );
|
|
|
|
//Ïðîâåðêà íà ïåðåïîëíåíèå è íåâîçìîæíûå äàííûå.
|
|
if( fabsf( current.sin ) > max_value ) {
|
|
|
|
current.sin = NAN;
|
|
|
|
result = false;
|
|
}
|
|
|
|
//Ïðîâåðêà íà ïåðåïîëíåíèå è íåâîçìîæíûå äàííûå.
|
|
if( fabsf( current.cos ) > max_value ) {
|
|
|
|
current.cos = NAN;
|
|
|
|
result = false;
|
|
}
|
|
|
|
//Ïðîâåðêà íà íîëü.
|
|
if( current.sin == 0.0f && current.cos == 0.0f ) {
|
|
|
|
current.sin = current.cos = NAN;
|
|
|
|
result = false;
|
|
}
|
|
|
|
//Ïðîâåðêà íà -1.
|
|
if( current.sin == -1.0f && current.cos == -1.0f ) {
|
|
|
|
current.sin = current.cos = NAN;
|
|
|
|
result = false;
|
|
}
|
|
|
|
|
|
return result;
|
|
}
|
|
|
|
bool driver::detail::Amc1210Analizer::Spike::validate(
|
|
Quad & current, const Quad & current_delta, const Quad & old ) {
|
|
|
|
bool result( true );
|
|
|
|
const float delta_data_0 = fabsf( current_delta.sin );
|
|
const float old_delta_data_0 = fabsf( current.sin - old.sin );
|
|
|
|
if( delta_data_0 > max_delta
|
|
&& old_delta_data_0 > max_delta ) {
|
|
|
|
current.sin = NAN;
|
|
|
|
result = false;
|
|
|
|
}
|
|
|
|
const float delta_data_1 = fabsf( current_delta.cos );
|
|
const float old_delta_data_1 = fabsf( current.cos - old.cos );
|
|
|
|
if( delta_data_1 > max_delta
|
|
&& old_delta_data_1 > max_delta ) {
|
|
|
|
current.cos = NAN;
|
|
|
|
result = false;
|
|
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
float driver::detail::Amc1210Analizer::getSquareSumChannel() const {
|
|
|
|
if( valid_signal )
|
|
return raw.sin * raw.sin + raw.cos * raw.cos;
|
|
else
|
|
return NAN;
|
|
|
|
}
|
|
|
|
bool driver::detail::Amc1210Analizer::Setting::isValid() {
|
|
|
|
using namespace std;
|
|
|
|
const Setting not_valid;
|
|
|
|
if( !isfinite( max_frequency ) || max_frequency <= 0.0f )
|
|
max_frequency = not_valid.max_frequency;
|
|
|
|
if( !isfinite( max_amplitude ) || max_amplitude <= 0.0f )
|
|
max_amplitude = not_valid.max_amplitude;
|
|
|
|
if( !isfinite( max_value ) || max_value <= 0.0f )
|
|
max_value = not_valid.max_value;
|
|
|
|
if( !isfinite( max_noise ) || max_noise <= 0.0f )
|
|
max_noise = not_valid.max_noise;
|
|
|
|
if( max_noise > max_value )
|
|
max_noise = not_valid.max_noise;
|
|
|
|
if( max_amplitude > max_value )
|
|
max_amplitude = not_valid.max_amplitude;
|
|
|
|
if( max_amplitude < max_noise )
|
|
max_noise = not_valid.max_noise;
|
|
|
|
return max_frequency != not_valid.max_frequency
|
|
and max_amplitude != not_valid.max_amplitude
|
|
and max_value != not_valid.max_value
|
|
and max_noise != not_valid.max_noise;
|
|
|
|
}
|
|
|
|
driver::detail::Amc1210Analizer::Setting::Setting() :
|
|
max_frequency(NAN), max_amplitude(NAN),
|
|
max_value(NAN), max_noise(NAN) {}
|