132 lines
3.8 KiB
C++
132 lines
3.8 KiB
C++
/*
|
||
* BinaryEncoderSync.cpp
|
||
*
|
||
* Created on: 27 янв. 2022 г.
|
||
* 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) {}
|
||
|