253 lines
6.8 KiB
C++
253 lines
6.8 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) {}
|