231 lines
7.0 KiB
C++
231 lines
7.0 KiB
C++
|
/*
|
|||
|
|
* PositionMixerv2.cpp
|
|||
|
|
*
|
|||
|
|
* Created on: 20 <EFBFBD><EFBFBD><EFBFBD>. 2020 <EFBFBD>.
|
|||
|
|
* Author: user
|
|||
|
|
*/
|
|||
|
|
|
|||
|
|
#include "PositionMixerv2.hh"
|
|||
|
|
|
|||
|
|
driver::PositionCode driver::PositionMixer_v2::cross_correction(const PositionCode & position, const float period_angle ) {
|
|||
|
|
//<2F><><EFBFBD><EFBFBD> <20> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
const bool in_pos_sector1 = (position.angle_data & 0x3) == 0;
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD><EFBFBD> <20> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
const bool in_pos_sector4 = (position.angle_data & 0x3) == 3;
|
|||
|
|
|
|||
|
|
const bool period_angle_sector_34 = period_angle > math::constants::pi;
|
|||
|
|
|
|||
|
|
const bool period_angle_sector_12 = period_angle <= math::constants::pi;
|
|||
|
|
|
|||
|
|
if( period_angle_sector_34 && in_pos_sector1 )
|
|||
|
|
return decSinCosPeriod(position);
|
|||
|
|
|
|||
|
|
if( period_angle_sector_12 && in_pos_sector4 )
|
|||
|
|
return incSinCosPeriod(position);
|
|||
|
|
|
|||
|
|
return position;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void driver::PositionMixer_v2::sector_synchronization(
|
|||
|
|
const PositionCode & position, float period_angle ) {
|
|||
|
|
|
|||
|
|
if( not is_sector_synchronized ) {
|
|||
|
|
|
|||
|
|
unsigned long counter_on_synchronization = increment_counter.position_to_increments(position);
|
|||
|
|
|
|||
|
|
if( counter_before_synchronized and counter_before_synchronized != counter_on_synchronization ) {
|
|||
|
|
|
|||
|
|
short code_sector = static_cast<short>(position.angle_data & 0x3);
|
|||
|
|
|
|||
|
|
if( counter_on_synchronization > counter_before_synchronized )
|
|||
|
|
period_angle += math::constants::pi_2;
|
|||
|
|
|
|||
|
|
short angle_sector = -1;
|
|||
|
|
|
|||
|
|
for( int i = 0; i < 4; ++i ) {
|
|||
|
|
|
|||
|
|
float base_point = math::constants::pi_2 * i + math::constants::pi_2;
|
|||
|
|
|
|||
|
|
if( period_angle >= base_point - sector_deviation and
|
|||
|
|
period_angle < base_point + sector_deviation )
|
|||
|
|
angle_sector = i;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if( angle_sector != -1 ) {
|
|||
|
|
|
|||
|
|
sector_offset = angle_sector
|
|||
|
|
- code_sector;
|
|||
|
|
|
|||
|
|
is_sector_synchronized = true;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
} else
|
|||
|
|
|
|||
|
|
counter_before_synchronized = counter_on_synchronization;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
driver::PositionData driver::PositionMixer_v2::calculatePosition() {
|
|||
|
|
|
|||
|
|
PositionCode position = increment_counter;
|
|||
|
|
const PositionComponents pos_components( resolver.getSinChannel(), resolver.getCosChannel(), increment_counter.position_to_increments(position) );
|
|||
|
|
|
|||
|
|
last_position = pos_components;
|
|||
|
|
|
|||
|
|
const PositionCode offset = { sector_offset, max_turn_value / 2 };
|
|||
|
|
position = PositionCode::applyOffset(position, offset, line_in_turn, max_turn_value);
|
|||
|
|
|
|||
|
|
if( pos_components ) {
|
|||
|
|
|
|||
|
|
if( resolver_valid_value_count < resolver_hyst )
|
|||
|
|
++resolver_valid_value_count;
|
|||
|
|
else {
|
|||
|
|
scrt_valid = true;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
resolver_invalid_value_count = 0;
|
|||
|
|
|
|||
|
|
} else {
|
|||
|
|
|
|||
|
|
if( resolver_invalid_value_count < counter_hyst )
|
|||
|
|
++resolver_invalid_value_count;
|
|||
|
|
else {
|
|||
|
|
scrt_valid = false;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
resolver_valid_value_count = 0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
float angle = increment_counter.position_to_rad(position.angle_data);
|
|||
|
|
unsigned short turn = position.turn;
|
|||
|
|
|
|||
|
|
if( scrt_valid ) {
|
|||
|
|
|
|||
|
|
if( pos_components ) {
|
|||
|
|
|
|||
|
|
//atan2 <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> [-pi, +pi]
|
|||
|
|
const float atng = std::atan2(pos_components.sin, pos_components.cos);
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> [0, 2*pi]
|
|||
|
|
const float period_angle = atng < 0 ? (atng + math::constants::pi2) : atng;
|
|||
|
|
|
|||
|
|
sector_synchronization( position, period_angle );
|
|||
|
|
|
|||
|
|
if( is_sector_synchronized ) {
|
|||
|
|
|
|||
|
|
short code_sector = static_cast<short>(position.angle_data & 0x3);
|
|||
|
|
if( code_sector == 0 or code_sector == 3 and pos_components.cos < 0.0f )
|
|||
|
|
phase_error_flag = true;
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
|
|||
|
|
position = cross_correction( position, period_angle );
|
|||
|
|
|
|||
|
|
angle = increment_counter.position_to_rad(
|
|||
|
|
position.angle_data & (static_cast<unsigned long>(-1) << line_in_period_power)
|
|||
|
|
)
|
|||
|
|
+ period_angle * resolver_to_rad
|
|||
|
|
- half_increment;
|
|||
|
|
turn = position.turn;
|
|||
|
|
|
|||
|
|
if( angle < 0.0f ) {
|
|||
|
|
angle += math::constants::pi2;
|
|||
|
|
turn -= 1;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
} else {
|
|||
|
|
|
|||
|
|
angle = NAN;
|
|||
|
|
turn = position.turn;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return { .angle = angle, .turn = turn };
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
driver::PositionComponents driver::PositionMixer_v2::getPositionComponents() const {
|
|||
|
|
|
|||
|
|
return last_position;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void driver::PositionMixer_v2::setAbsolutePosition(
|
|||
|
|
unsigned long absolute_position) {
|
|||
|
|
|
|||
|
|
const driver::PositionCode position_code = {
|
|||
|
|
.angle_data = (absolute_position & ((1 << step_per_turn) - 1)) >> (periods_in_turn_power - line_in_period_power),
|
|||
|
|
.turn = ( absolute_position >> step_per_turn ) % max_turn_value
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
increment_counter.setCounter(position_code);
|
|||
|
|
sector_resynchronizarion();
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
driver::PositionMixer_v2::PositionMixer_v2(peripheral::ICounter &counter,
|
|||
|
|
unsigned long sin_period_resolution,
|
|||
|
|
unsigned long max_turn_value,
|
|||
|
|
driver::IResolver &resolver,
|
|||
|
|
unsigned short resolver_hyst, unsigned short counter_hyst, float analog_sector_deviation )
|
|||
|
|
: increment_counter(counter, 1 << (sin_period_resolution + line_in_period_power), max_turn_value), resolver(resolver),
|
|||
|
|
resolver_to_rad(1.f/(1 << sin_period_resolution)), step_per_turn(sin_period_resolution + periods_in_turn_power),
|
|||
|
|
max_angle_code((1 << (sin_period_resolution + line_in_period_power)) - 1),
|
|||
|
|
line_in_turn(1 << (sin_period_resolution + line_in_period_power)), max_turn_value(max_turn_value), resolver_hyst(resolver_hyst), counter_hyst(counter_hyst),
|
|||
|
|
half_increment( increment_counter.position_to_rad(1) * 0.5f ), sector_deviation(analog_sector_deviation) {}
|
|||
|
|
|
|||
|
|
driver::PositionCode driver::PositionMixer_v2::incSinCosPeriod(PositionCode code) {
|
|||
|
|
|
|||
|
|
if(code.angle_data > max_angle_code - 1) {
|
|||
|
|
|
|||
|
|
code.angle_data = 0;
|
|||
|
|
code.turn++;
|
|||
|
|
code.turn = increment_counter.toCorrectPositionCode(code).turn;
|
|||
|
|
|
|||
|
|
} else {
|
|||
|
|
|
|||
|
|
code.angle_data+= 1;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return code;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
driver::PositionCode driver::PositionMixer_v2::decSinCosPeriod(PositionCode code) {
|
|||
|
|
|
|||
|
|
if(code.angle_data < 1) {
|
|||
|
|
|
|||
|
|
code.angle_data = max_angle_code;
|
|||
|
|
code.turn--;
|
|||
|
|
code.turn = increment_counter.toCorrectPositionCode(code).turn;
|
|||
|
|
|
|||
|
|
} else {
|
|||
|
|
|
|||
|
|
code.angle_data-= 1;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return code;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void driver::PositionMixer_v2::sector_resynchronizarion() {
|
|||
|
|
is_sector_synchronized = false;
|
|||
|
|
counter_before_synchronized = 0;
|
|||
|
|
sector_offset = 0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void driver::PositionMixer_v2::reset() {
|
|||
|
|
increment_counter.resetCounter();
|
|||
|
|
sector_resynchronizarion();
|
|||
|
|
resolver_valid_value_count = 0;
|
|||
|
|
}
|