204 lines
6.4 KiB
C++
204 lines
6.4 KiB
C++
/*
|
||
* TrackingPositionScrt.cpp
|
||
*
|
||
* Created on: 26 янв. 2018 г.
|
||
* Author: titov
|
||
*/
|
||
|
||
#include "TrackingPositionScrt.hh"
|
||
|
||
using processing::TrackingPositionScrt;
|
||
|
||
void TrackingPositionScrt::configure( const Setting & setting ) {
|
||
|
||
using namespace std;
|
||
|
||
_L1Base = setting.L1;
|
||
_L2Base = setting.L2;
|
||
|
||
_L1Ts = _L1Base * _Ts,
|
||
_L2Ts = _L2Base * _Ts;
|
||
|
||
_ThetaZero = setting.ThetaNull,
|
||
_Np_Nr = setting.Np / setting.Nr;
|
||
|
||
_power_max = setting.scrt_power_max * setting.scrt_power_max;
|
||
_power_min = setting.scrt_power_min * setting.scrt_power_min;
|
||
|
||
_Tun = math::constants::pi_2 / setting.scrt_speed_max;
|
||
_Nerr = static_cast<unsigned int>( _Tun / _Ts );
|
||
|
||
_1_Nr = 1.f / static_cast<float>( setting.Nr );
|
||
_pi2_Nr = math::constants::pi2 / static_cast<float>( setting.Nr );
|
||
|
||
}
|
||
|
||
void TrackingPositionScrt::setSampleTime( float Ts ) {
|
||
|
||
_L1Ts = _L1Base * Ts;
|
||
_L2Ts = _L2Base * Ts;
|
||
_Nerr = static_cast<unsigned int>( _Tun / Ts );
|
||
_Ts = Ts;
|
||
|
||
}
|
||
|
||
processing::TrackingPositionScrt::TrackingPositionScrt(
|
||
driver::IResolver & resolver) : scrt(resolver),
|
||
_Ts(0.0f),
|
||
_power_max(0.0f), _power_min(0.0f), _Tun(0.0f), _Nerr(0.0f),
|
||
_L1Base(0.0f), _L2Base(0.0f),
|
||
_L1Ts(0.0f), _L2Ts(0.0f),
|
||
_ThetaZero(0.0f), _Np_Nr(0.0f),
|
||
_1_Nr(0.0f), _pi2_Nr(0.0f),
|
||
|
||
prev_error(0.0f), prev_scrt_sin(0.0f), prev_scrt_cos(0.0f),
|
||
|
||
est_sens_speed(0.0f), est_sens_position_pi2(0.0f), est_sens_turn(0.0f), est_sens_error(0.0f),
|
||
est_theta(0.0f), est_omega(0.0f),
|
||
est_position(0.0f), est_speed(0.0f),
|
||
error_counter(0) {}
|
||
|
||
|
||
|
||
void TrackingPositionScrt::bad_data_handler() {
|
||
|
||
const float demping_k = 0.5f;
|
||
|
||
if (error_counter < _Nerr)
|
||
error_counter++;
|
||
else
|
||
est_sens_speed = est_sens_error = 0.0f;
|
||
|
||
est_sens_error = est_sens_error * demping_k;
|
||
prev_error = est_sens_error;
|
||
}
|
||
|
||
void TrackingPositionScrt::process() {
|
||
|
||
using namespace std;
|
||
|
||
//Обновление ошибки системы.
|
||
{
|
||
float scrt_sin = scrt.getSinChannel();
|
||
float scrt_cos = scrt.getCosChannel();
|
||
|
||
//В случае не валидных данных принимаем ошибку равной нулю.
|
||
if( isfinite(scrt_sin) && isfinite(scrt_cos) ) {
|
||
|
||
float sin_d2 = scrt_sin * scrt_sin;
|
||
float cos_d2 = scrt_cos * scrt_cos;
|
||
|
||
float amp_data = ( sin_d2 + cos_d2 );
|
||
|
||
if( amp_data < _power_max && amp_data > _power_min ) {
|
||
|
||
float est_sin, est_cos;
|
||
math::function::sincos(est_sens_position_pi2, &est_sin, &est_cos);
|
||
|
||
float _1_amp_data = math::function::q_rsqrt( amp_data );
|
||
|
||
float error_0 = ( scrt_sin * est_cos - scrt_cos * est_sin ) * _1_amp_data;
|
||
float error_1 = ( prev_scrt_sin * est_cos - scrt_cos * est_sin ) * _1_amp_data;
|
||
float error_2 = ( scrt_sin * est_cos - prev_scrt_cos * est_sin ) * _1_amp_data;
|
||
|
||
float derror_0 = fabsf( prev_error - error_0 );
|
||
float derror_1 = fabsf( prev_error - error_1 );
|
||
float derror_2 = fabsf( prev_error - error_2 );
|
||
|
||
est_sens_error =
|
||
derror_0 < derror_1 and derror_0 < derror_2 ? error_0 :
|
||
derror_1 < derror_2 ? error_1 :
|
||
error_2;
|
||
|
||
if( error_counter && error_counter != _Nerr )
|
||
error_counter--;
|
||
|
||
prev_error = est_sens_error;
|
||
prev_scrt_sin = scrt_sin;
|
||
prev_scrt_cos = scrt_cos;
|
||
|
||
} else bad_data_handler();
|
||
} else bad_data_handler();
|
||
}
|
||
|
||
//Сам наблюдатель:
|
||
{
|
||
est_sens_speed = est_sens_speed + est_sens_error * _L2Ts;
|
||
est_sens_position_pi2 = est_sens_position_pi2 + est_sens_speed * _Ts + est_sens_error * _L1Ts;
|
||
|
||
if( est_sens_position_pi2 < 0.0f ) {
|
||
|
||
est_sens_turn = est_sens_turn - 1.0f;
|
||
est_sens_position_pi2 += math::constants::pi2;
|
||
|
||
} else if( est_sens_position_pi2 >= math::constants::pi2 ) {
|
||
|
||
est_sens_turn = est_sens_turn + 1.0f;
|
||
est_sens_position_pi2 -= math::constants::pi2;
|
||
|
||
}
|
||
}
|
||
|
||
//Представление в электрических координатах.
|
||
{
|
||
est_theta = fmodf( est_sens_position_pi2 * _Np_Nr + _ThetaZero, math::constants::pi2 );
|
||
est_omega = est_sens_speed * _Np_Nr;
|
||
|
||
if( est_theta < 0.0f )
|
||
est_theta = est_theta + math::constants::pi2;
|
||
|
||
else if( est_theta >= math::constants::pi2 )
|
||
est_theta = est_theta - math::constants::pi2;
|
||
}
|
||
|
||
//Представление в физических координатах.
|
||
{
|
||
est_speed = _1_Nr * est_sens_speed;
|
||
est_position = _1_Nr * est_sens_position_pi2 + _pi2_Nr * est_sens_turn;
|
||
}
|
||
}
|
||
|
||
bool processing::TrackingPositionScrt::Setting::isValid() {
|
||
|
||
|
||
const Setting notValidSetting;
|
||
|
||
if( L1 > L2 ) {
|
||
L1 = notValidSetting.L1;
|
||
L2 = notValidSetting.L2;
|
||
}
|
||
|
||
if( not Np )
|
||
Np = notValidSetting.Np;
|
||
|
||
if( not( ( - math::constants::pi2) < ThetaNull && ThetaNull < math::constants::pi2 ) )
|
||
ThetaNull = notValidSetting.ThetaNull;
|
||
|
||
if( scrt_power_max < 0.0f )
|
||
scrt_power_max = notValidSetting.scrt_power_max;
|
||
|
||
if( scrt_power_min < 0.0f )
|
||
scrt_power_min = notValidSetting.scrt_power_min;
|
||
|
||
if( scrt_power_min >= scrt_power_max ) {
|
||
scrt_power_min = notValidSetting.scrt_power_min;
|
||
scrt_power_max = notValidSetting.scrt_power_max;
|
||
}
|
||
|
||
if( scrt_speed_max <= 0.0f )
|
||
scrt_speed_max = notValidSetting.scrt_speed_max;
|
||
|
||
if( not Nr )
|
||
Nr = notValidSetting.Nr;
|
||
|
||
return L1 != notValidSetting.L1
|
||
and L2 != notValidSetting.L2
|
||
and ThetaNull != notValidSetting.ThetaNull
|
||
and Np != notValidSetting.Np
|
||
and scrt_power_max != notValidSetting.scrt_power_max
|
||
and scrt_power_min != notValidSetting.scrt_power_min
|
||
and scrt_speed_max != notValidSetting.scrt_speed_max
|
||
and Nr != notValidSetting.Nr;
|
||
|
||
}
|