132 lines
3.8 KiB
C++
132 lines
3.8 KiB
C++
|
/*
|
|||
|
|
* BinaryEncoderSync.cpp
|
|||
|
|
*
|
|||
|
|
* Created on: 27 <EFBFBD><EFBFBD><EFBFBD>. 2022 <EFBFBD>.
|
|||
|
|
* Author: titov
|
|||
|
|
*/
|
|||
|
|
|
|||
|
|
#include "BinaryEncoderSync.hh"
|
|||
|
|
|
|||
|
|
#include "../math/math_inc.hh"
|
|||
|
|
#include <cstring>
|
|||
|
|
|
|||
|
|
void driver::detail::BinaryEncoderSync::read( const void * data, std::size_t size ) {
|
|||
|
|
|
|||
|
|
peripheral::nanotimer::stamp timestamp = (*peripheral::nanotimer::point)();
|
|||
|
|
|
|||
|
|
frame raw;
|
|||
|
|
std::memcpy( &raw, data, sizeof(frame) );
|
|||
|
|
|
|||
|
|
Points::Point new_point;
|
|||
|
|
new_point.turn = turn.get(raw);
|
|||
|
|
new_point.angle = angle_cost * angle.get(raw);
|
|||
|
|
new_point.timestamp = timestamp;
|
|||
|
|
|
|||
|
|
Points current = points.load();
|
|||
|
|
|
|||
|
|
current.second = current.first;
|
|||
|
|
current.first = new_point;
|
|||
|
|
|
|||
|
|
points.store( current );
|
|||
|
|
|
|||
|
|
handled.clear();
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void driver::detail::BinaryEncoderSync::process() {
|
|||
|
|
|
|||
|
|
peripheral::nanotimer::stamp timestamp = (*peripheral::nanotimer::point)();
|
|||
|
|
|
|||
|
|
if( not handled.test_and_set() )
|
|||
|
|
sample.write( per_sample( timestamp ) );
|
|||
|
|
else
|
|||
|
|
sample.write( std::pair<driver::IEncoder::Turn, driver::IEncoder::Angle>(NAN, NAN) );
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
std::pair<driver::IEncoder::Turn, driver::IEncoder::Angle> driver::detail::BinaryEncoderSync::getPosition() const {
|
|||
|
|
|
|||
|
|
return sample.read();
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
driver::detail::BinaryEncoderSync::BinaryEncoderSync( bitsize angle_offset, bitsize angle_size,
|
|||
|
|
bitsize turn_offset, bitsize turn_size ) :
|
|||
|
|
angle(angle_offset, angle_size), turn(turn_offset, turn_size),
|
|||
|
|
angle_cost( math::constants::pi2 / std::pow(2.0f, float(angle_size)) ) {
|
|||
|
|
handled.clear();
|
|||
|
|
|
|||
|
|
handled.clear();
|
|||
|
|
|
|||
|
|
sample.write( std::pair<driver::IEncoder::Turn, driver::IEncoder::Angle>(NAN, NAN) );
|
|||
|
|
sample.write( std::pair<driver::IEncoder::Turn, driver::IEncoder::Angle>(NAN, NAN) );
|
|||
|
|
|
|||
|
|
Points::Point default_point;
|
|||
|
|
default_point.turn = NAN;
|
|||
|
|
default_point.angle = NAN;
|
|||
|
|
default_point.timestamp = 0;
|
|||
|
|
|
|||
|
|
Points default_points;
|
|||
|
|
default_points.first = default_point;
|
|||
|
|
default_points.second = default_point;
|
|||
|
|
|
|||
|
|
points.store( default_points );
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void driver::detail::BinaryEncoderSync::setSampleTime( float ts_in_second ) {
|
|||
|
|
|
|||
|
|
cycle_time = ( ts_in_second * 1e9f );
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
driver::IEncoder::Turn driver::detail::BinaryEncoderSync::getTurn() const {
|
|||
|
|
|
|||
|
|
return sample.read().first;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
driver::IEncoder::Angle driver::detail::BinaryEncoderSync::getAngle() const {
|
|||
|
|
|
|||
|
|
return sample.read().second;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
std::pair<driver::IEncoder::Turn, driver::IEncoder::Angle> driver::detail::BinaryEncoderSync::per_sample( peripheral::nanotimer::stamp timestamp ) {
|
|||
|
|
|
|||
|
|
Points current = points.load();
|
|||
|
|
|
|||
|
|
Points::Point & point_1 = current.first;
|
|||
|
|
Points::Point & point_2 = current.second;
|
|||
|
|
|
|||
|
|
using namespace std;
|
|||
|
|
|
|||
|
|
if( isfinite(current.first.turn) and isfinite(current.second.turn) ) {
|
|||
|
|
|
|||
|
|
driver::IEncoder::Angle angle_increment =
|
|||
|
|
in_turn( delta( std::pair<Turn, Angle>(point_1.turn, point_1.angle), std::pair<Turn, Angle>(point_2.turn, point_2.angle) ),
|
|||
|
|
point_2.turn );
|
|||
|
|
|
|||
|
|
float reduced_time = float( (*peripheral::nanotimer::delta)( timestamp, point_2.timestamp ) ) /
|
|||
|
|
float( (*peripheral::nanotimer::delta)( point_1.timestamp, point_2.timestamp ) );
|
|||
|
|
|
|||
|
|
point_2.angle += angle_increment * reduced_time;
|
|||
|
|
|
|||
|
|
return normalize( std::pair<Turn, Angle>(point_2.turn, point_2.angle) );
|
|||
|
|
|
|||
|
|
} else
|
|||
|
|
|
|||
|
|
return std::pair<Turn, Angle>( NAN, NAN );
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
driver::detail::BinaryEncoderSync::frame driver::detail::BinaryEncoderSync::Bitfield::get( frame raw ) {
|
|||
|
|
|
|||
|
|
return ( raw >> offset ) & mask;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
driver::detail::BinaryEncoderSync::Bitfield::Bitfield( bitsize new_offset,
|
|||
|
|
bitsize new_size ) : offset( new_offset ), mask((1ul << new_size) - 1) {}
|
|||
|
|
|