96 lines
3.5 KiB
C++
96 lines
3.5 KiB
C++
|
/*
|
|||
|
|
* PositionMixer.cpp
|
|||
|
|
*
|
|||
|
|
* Created on: 2 <EFBFBD><EFBFBD><EFBFBD>. 2020 <EFBFBD>.
|
|||
|
|
* 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 <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> (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 {
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 0..2*pi <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
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);
|
|||
|
|
|
|||
|
|
}
|