96 lines
3.6 KiB
C++
96 lines
3.6 KiB
C++
/*
|
||
* PositionMixer.cpp
|
||
*
|
||
* Created on: 2 мар. 2020 г.
|
||
* Author: user
|
||
*/
|
||
|
||
#include "PositionMixer.hh"
|
||
|
||
#include "../peripheral/ICounter.hh"
|
||
|
||
driver::PositionData driver::PositionMixer::getPosition() {
|
||
|
||
const PositionCode position = increment_counter;
|
||
const PositionComponents pos_components( resolver.getSinChannel(), resolver.getCosChannel(), increment_counter.position_to_increments(position) );
|
||
|
||
float angle = NAN;
|
||
//todo можно проверять сигналы резольвера более строго (sin^2 + cos^2)
|
||
if( pos_components ) {
|
||
const float atng = std::atan2(pos_components.sin, pos_components.cos);
|
||
const float period_angle = atng > math::constants::pi ? (atng - math::constants::pi2) :
|
||
(atng < - math::constants::pi ? (atng + math::constants::pi2) : atng);
|
||
|
||
const short angle_code = position.angle_data & 0x3;
|
||
|
||
if ( ( not ( period_angle < mistrust_angle and period_angle > -mistrust_angle ) )
|
||
and ( ( period_angle < math::constants::pi_2 - mistrust_angle ) and ( period_angle > -math::constants::pi_2 + mistrust_angle ) ) ) {
|
||
switch(angle_code) {
|
||
case 0: {
|
||
phase_error_flag = ( period_angle < 0 or period_angle > math::constants::pi_2 ) ? true : false;
|
||
} break;
|
||
case 1: {
|
||
phase_error_flag = ( period_angle < math::constants::pi_2 ) ? true : false;
|
||
} break;
|
||
case 2: {
|
||
phase_error_flag = ( period_angle > -math::constants::pi_2 ) ? true : false;
|
||
} break;
|
||
case 3: {
|
||
phase_error_flag = ( period_angle < -math::constants::pi_2 or period_angle > 0 ) ? true : false;
|
||
} break;
|
||
}
|
||
}
|
||
|
||
if( period_angle < mistrust_angle and period_angle > -mistrust_angle
|
||
and ( angle_code == 0 or angle_code == 3 ) ) {
|
||
|
||
if( period_angle < 0.f )
|
||
|
||
angle = increment_counter.position_to_rad( position.angle_data )
|
||
+ ( angle_code == 0 ? 0.0f : minus_zero_point_rad );
|
||
|
||
else
|
||
|
||
angle = increment_counter.position_to_rad( position.angle_data )
|
||
+ ( angle_code == 0 ? plus_zero_point_rad : minus_zero_rad );
|
||
|
||
} else {
|
||
//приведенное к диапазону 0..2*pi значение
|
||
const float period_angle_pos = period_angle < 0.f ? (period_angle + math::constants::pi2) : period_angle;
|
||
|
||
angle = increment_counter.position_to_rad(
|
||
position.angle_data & (static_cast<unsigned long>(-1) << line_in_period_power)
|
||
) + period_angle_pos*resolver_to_rad;
|
||
|
||
}
|
||
|
||
} else {
|
||
|
||
angle = increment_counter.position_to_rad(position.angle_data);
|
||
|
||
}
|
||
|
||
pos_code = position.angle_data & 0x3;
|
||
|
||
return { .angle = angle, .turn = static_cast<unsigned short>( position.turn /*- turn*/ ) };
|
||
|
||
}
|
||
|
||
driver::PositionComponents driver::PositionMixer::getPositionComponents() {
|
||
|
||
const PositionCode position = increment_counter;
|
||
return PositionComponents( resolver.getSinChannel(), resolver.getCosChannel(), increment_counter.position_to_increments(position) );
|
||
|
||
}
|
||
|
||
void driver::PositionMixer::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
|
||
};
|
||
|
||
increment_counter.setCounter(position_code);
|
||
|
||
}
|