236 lines
6.5 KiB
C++
236 lines
6.5 KiB
C++
/*
|
||
* TrackingPositionEncoder.cpp
|
||
*
|
||
* Created on: 22 мая 2018 г.
|
||
* Author: krugliy
|
||
*/
|
||
|
||
#include "TrackingPositionEncoder.hh"
|
||
|
||
processing::TrackingPositionEncoder::TrackingPositionEncoder(
|
||
driver::IEncoder & enc, uint32_t max_turn, bool tuning_enable ) :
|
||
encoder(enc), prev_n1_encoder_angle(NAN), prev_n2_encoder_angle(NAN),
|
||
_Ts(1.0f), _1_Ts(1.0f), _Tun(0.0f), _DTs(0.0f),
|
||
_L1Ts(0.0f), _L2Ts(0.0f),
|
||
_Qe(0.0f), _1_Qe(0.0f),
|
||
position_error(0.0f),
|
||
est_position(NAN), est_angle(NAN), est_speed(NAN), est_turn(NAN),
|
||
count_error(0), maxcount_error(1),
|
||
max_turn(max_turn), half_turn(max_turn/2), repeat_counter(0), max_repeat(8),
|
||
prev_angle(NAN), prev_turn(NAN), tuning(tuning_enable) {}
|
||
|
||
void processing::TrackingPositionEncoder::configure( const Setting & setting ) {
|
||
|
||
_L1Ts = setting.L1 * _Ts,
|
||
_L2Ts = setting.L2 * _Ts;
|
||
|
||
_Tun = math::constants::pi_2 / setting.max_speed;
|
||
_DTs = setting.max_speed * _Ts;
|
||
|
||
_Qe = setting.quant_error;
|
||
_1_Qe = setting.quant_error > 0.0f ? 1.0f / setting.quant_error : 0.0f;
|
||
|
||
_demping = setting.error_demping;
|
||
|
||
turn_offset = set_turn_offset = setting.turn_offset;
|
||
|
||
}
|
||
|
||
void processing::TrackingPositionEncoder::setSampleTime( float Ts ) {
|
||
|
||
_L1Ts = _L1Ts * Ts / _Ts,
|
||
_L2Ts = _L2Ts * Ts / _Ts;
|
||
_DTs = _DTs * Ts / _Ts;
|
||
maxcount_error = static_cast<unsigned int>( _Tun / Ts );
|
||
_Ts = Ts;
|
||
_1_Ts = 1.0f / Ts;
|
||
|
||
}
|
||
|
||
float processing::TrackingPositionEncoder::turnover(float raw_turn) {
|
||
|
||
float encoder_turn;
|
||
|
||
using namespace std;
|
||
|
||
if( isfinite(raw_turn) ) {
|
||
uint32_t turn = raw_turn + turn_offset;
|
||
int32_t turnover = turn % max_turn;
|
||
int32_t turnoff = turnover > half_turn ? turnover - max_turn : turnover;
|
||
encoder_turn = turnoff;
|
||
} else
|
||
encoder_turn = NAN;
|
||
|
||
return encoder_turn;
|
||
|
||
}
|
||
|
||
void processing::TrackingPositionEncoder::miss( const float demping ) {
|
||
|
||
if (maxcount_error > count_error) {
|
||
|
||
position_error = position_error * demping;
|
||
count_error++;
|
||
|
||
} else
|
||
position_error = NAN;
|
||
|
||
}
|
||
|
||
void processing::TrackingPositionEncoder::angle_tuning(float raw_angle,
|
||
float raw_turn,
|
||
float & encoder_angle,
|
||
float & encoder_turn) {
|
||
|
||
using namespace std;
|
||
|
||
const unsigned int on_speed_repeat =
|
||
isfinite(est_speed) ? ( (_Qe / fabsf(est_speed) ) * _1_Ts) : max_repeat;
|
||
|
||
if( repeat_counter > on_speed_repeat)
|
||
repeat_counter = 0;
|
||
|
||
bool data_actual = not repeat_counter;
|
||
|
||
if( data_actual and isfinite(raw_angle) and isfinite(raw_turn)
|
||
and not prev_sample_actual ) {
|
||
|
||
float turn_mismatch = prev_turn - raw_turn;
|
||
float prev_angle_in_same_turn = prev_angle
|
||
+ turn_mismatch * math::constants::pi2;
|
||
|
||
float average_angle = (raw_angle + prev_angle_in_same_turn) * 0.5f;
|
||
|
||
if( average_angle > math::constants::pi2 ) {
|
||
|
||
encoder_angle = average_angle - math::constants::pi2;
|
||
encoder_turn = raw_turn + 1;
|
||
|
||
} else if (average_angle < 0.0f) {
|
||
|
||
encoder_angle = average_angle + math::constants::pi2;
|
||
encoder_turn = raw_turn - 1;
|
||
|
||
} else {
|
||
|
||
encoder_angle = average_angle;
|
||
encoder_turn = raw_turn;
|
||
|
||
}
|
||
|
||
prev_angle = raw_angle;
|
||
prev_turn = raw_turn;
|
||
|
||
}
|
||
|
||
prev_sample_actual = data_actual;
|
||
|
||
}
|
||
|
||
void processing::TrackingPositionEncoder::process() {
|
||
|
||
using namespace std;
|
||
|
||
if( turn_offset != set_turn_offset ) {
|
||
prev_turn = prev_turn + ( set_turn_offset - turn_offset );
|
||
est_turn = est_turn + ( set_turn_offset - turn_offset );
|
||
turn_offset = set_turn_offset;
|
||
}
|
||
|
||
driver::IEncoder::Position raw = encoder.getPosition();
|
||
|
||
float raw_turn = turnover( raw.first );
|
||
float raw_angle = raw.second;
|
||
|
||
float encoder_angle = raw_angle;
|
||
float encoder_turn = raw_turn;
|
||
|
||
if( raw_angle == prev_angle and repeat_counter < max_repeat )
|
||
++repeat_counter;
|
||
else
|
||
repeat_counter = 0;
|
||
|
||
if( tuning )
|
||
angle_tuning(raw_angle, raw_turn, encoder_angle, encoder_turn);
|
||
|
||
bool data_actual = not repeat_counter;
|
||
|
||
if( isfinite(encoder_angle) ) {
|
||
if( isfinite( est_position ) ) {
|
||
|
||
float delta_n1 = prev_n1_encoder_angle - encoder_angle;
|
||
float delta_n2 = prev_n2_encoder_angle - encoder_angle;
|
||
|
||
if(delta_n1 > math::constants::pi)
|
||
delta_n1 = delta_n1 - math::constants::pi2;
|
||
else if (delta_n1 <= -math::constants::pi)
|
||
delta_n1 = delta_n1 + math::constants::pi2;
|
||
|
||
if(delta_n2 > math::constants::pi)
|
||
delta_n2 = delta_n2 - math::constants::pi2;
|
||
else if (delta_n2 <= -math::constants::pi)
|
||
delta_n2 = delta_n2 + math::constants::pi2;
|
||
|
||
prev_n2_encoder_angle = prev_n1_encoder_angle;
|
||
prev_n1_encoder_angle = encoder_angle;
|
||
|
||
if( data_actual ) {
|
||
|
||
if( not( fabsf( delta_n1 ) > _DTs && fabsf( delta_n2 ) > _DTs ) ) {
|
||
|
||
position_error = encoder_angle - est_angle;
|
||
count_error = 0;
|
||
|
||
if(position_error > math::constants::pi)
|
||
position_error = position_error - math::constants::pi2;
|
||
else if (position_error <= -math::constants::pi)
|
||
position_error = position_error + math::constants::pi2;
|
||
|
||
} else miss( _demping );
|
||
|
||
} else {
|
||
|
||
position_error = position_error;
|
||
count_error = 0;
|
||
|
||
}
|
||
|
||
} else if( isfinite(encoder_turn) ) {
|
||
|
||
prev_n2_encoder_angle = prev_n1_encoder_angle = encoder_angle;
|
||
|
||
est_turn = encoder_turn;
|
||
est_angle = encoder_angle;
|
||
est_position = est_angle + est_turn * math::constants::pi2;
|
||
|
||
position_error = est_speed = 0.0f;
|
||
|
||
}
|
||
|
||
} else miss( _demping );
|
||
|
||
est_speed = est_speed + position_error * _L2Ts;
|
||
est_angle = est_angle + est_speed * _Ts + position_error * _L1Ts;
|
||
|
||
if( est_angle < 0.0f ) {
|
||
|
||
est_turn = est_turn - 1.0f;
|
||
est_angle += math::constants::pi2;
|
||
|
||
} else if( est_angle >= math::constants::pi2 ) {
|
||
|
||
est_turn = est_turn + 1.0f;
|
||
est_angle -= math::constants::pi2;
|
||
|
||
}
|
||
|
||
est_position = est_angle + est_turn * math::constants::pi2;
|
||
|
||
}
|
||
|
||
void processing::TrackingPositionEncoder::setTurnOffset(float turn) {
|
||
|
||
set_turn_offset = turn;
|
||
|
||
}
|