192 lines
5.3 KiB
C++
192 lines
5.3 KiB
C++
|
/*
|
|||
|
|
* BinaryEncoder.hpp
|
|||
|
|
*
|
|||
|
|
* Created on: 23 <EFBFBD><EFBFBD><EFBFBD>. 2023 <EFBFBD>.
|
|||
|
|
* Author: titov
|
|||
|
|
*/
|
|||
|
|
|
|||
|
|
#ifndef UMLIBRARY_DRIVER_BINARYENCODER_HPP_
|
|||
|
|
#define UMLIBRARY_DRIVER_BINARYENCODER_HPP_
|
|||
|
|
|
|||
|
|
|
|||
|
|
#include <cmath>
|
|||
|
|
#include <stdint.h>
|
|||
|
|
|
|||
|
|
#include "IEncoder.hh"
|
|||
|
|
#include "../communication/format/BinaryDataPublisher.hh"
|
|||
|
|
|
|||
|
|
#include "../common/DoubleBuffer.hpp"
|
|||
|
|
#include "../communication/format/BinaryHelpers.hh"
|
|||
|
|
|
|||
|
|
#include <atomic>
|
|||
|
|
|
|||
|
|
#include "../math/math_inc.hh"
|
|||
|
|
#include <cstring>
|
|||
|
|
|
|||
|
|
namespace driver { namespace detail {
|
|||
|
|
|
|||
|
|
struct BinaryEncoderInterface : public IEncoder, public communication::IBinaryDataSubscriber {};
|
|||
|
|
|
|||
|
|
//!<21><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>, <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
|
|||
|
|
template<typename FrameType>
|
|||
|
|
class BinaryEncoderBase : public BinaryEncoderInterface {
|
|||
|
|
public:
|
|||
|
|
typedef communication::format::bits::bitsize bitsize;
|
|||
|
|
|
|||
|
|
BinaryEncoderBase( bitsize angle_size, bitsize turn_size );
|
|||
|
|
|
|||
|
|
//!<21><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
|
|||
|
|
Turn getTurn() const;
|
|||
|
|
|
|||
|
|
//!<21><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> <20> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><> 0 <20><> 2*pi <20><><EFBFBD> NaN.
|
|||
|
|
Angle getAngle() const;
|
|||
|
|
|
|||
|
|
//!<21><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> (<28><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>, <20><><EFBFBD><EFBFBD>).
|
|||
|
|
std::pair<Turn, Angle> getPosition() const;
|
|||
|
|
|
|||
|
|
protected:
|
|||
|
|
typedef FrameType frame;
|
|||
|
|
|
|||
|
|
mutable std::atomic<FrameType> point;
|
|||
|
|
|
|||
|
|
private:
|
|||
|
|
const bitsize turn_offset;
|
|||
|
|
const frame turn_mask;
|
|||
|
|
const frame angle_mask;
|
|||
|
|
|
|||
|
|
const Angle angle_cost;
|
|||
|
|
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
template<typename FrameType>
|
|||
|
|
inline driver::detail::BinaryEncoderBase<FrameType>::BinaryEncoderBase( bitsize angle_size, bitsize turn_size ) :
|
|||
|
|
turn_offset(angle_size),
|
|||
|
|
turn_mask( ( 1ull << turn_size ) - 1ull ),
|
|||
|
|
angle_mask( ( 1ull << angle_size ) - 1ull),
|
|||
|
|
angle_cost( math::constants::pi2 / std::pow(2.0f, float(angle_size) ) ), point() {
|
|||
|
|
|
|||
|
|
point.store(0);
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
template<typename FrameType>
|
|||
|
|
inline driver::IEncoder::Turn driver::detail::BinaryEncoderBase<FrameType>::getTurn() const {
|
|||
|
|
|
|||
|
|
frame current_frame = point.load();
|
|||
|
|
|
|||
|
|
Turn turn = ( current_frame >> turn_offset ) & turn_mask;
|
|||
|
|
|
|||
|
|
return turn;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
template<typename FrameType>
|
|||
|
|
inline driver::IEncoder::Angle driver::detail::BinaryEncoderBase<FrameType>::getAngle() const {
|
|||
|
|
|
|||
|
|
frame current_frame = point.load();
|
|||
|
|
|
|||
|
|
Angle angle = ( current_frame & angle_mask ) * angle_cost;
|
|||
|
|
|
|||
|
|
return angle;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
template<typename FrameType>
|
|||
|
|
std::pair<
|
|||
|
|
driver::IEncoder::Turn,
|
|||
|
|
driver::IEncoder::Angle
|
|||
|
|
> driver::detail::BinaryEncoderBase<FrameType>::getPosition() const {
|
|||
|
|
|
|||
|
|
frame current_frame = point.load();
|
|||
|
|
|
|||
|
|
Turn turn = ( current_frame >> turn_offset ) & turn_mask;
|
|||
|
|
Angle angle = ( current_frame & angle_mask ) * angle_cost;
|
|||
|
|
|
|||
|
|
return std::pair<Turn, Angle>( turn, angle );
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
//!<21><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
|
|||
|
|
template<typename FrameType>
|
|||
|
|
class BinaryEncoder : public BinaryEncoderBase<FrameType> {
|
|||
|
|
public:
|
|||
|
|
typedef communication::format::bits::bitsize bitsize;
|
|||
|
|
|
|||
|
|
BinaryEncoder( bitsize angle_size, bitsize turn_size );
|
|||
|
|
|
|||
|
|
/*!
|
|||
|
|
* data - <EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD>, <EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 0.
|
|||
|
|
*/
|
|||
|
|
void read( const void * data, std::size_t size ) override;
|
|||
|
|
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
template<typename FrameType>
|
|||
|
|
BinaryEncoder<FrameType>::BinaryEncoder( bitsize angle_size, bitsize turn_size ) :
|
|||
|
|
BinaryEncoderBase<FrameType>(angle_size, turn_size) {}
|
|||
|
|
|
|||
|
|
template<typename FrameType>
|
|||
|
|
void driver::detail::BinaryEncoder<FrameType>::read( const void * data,
|
|||
|
|
std::size_t size ) {
|
|||
|
|
|
|||
|
|
typename BinaryEncoderBase<FrameType>::frame new_frame;
|
|||
|
|
std::memcpy( &new_frame, data, sizeof(new_frame) );
|
|||
|
|
BinaryEncoderBase<FrameType>::point.store( new_frame );
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//!<21><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>, <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
|
|||
|
|
template<typename FrameType>
|
|||
|
|
class BinaryEncoderInverted : public BinaryEncoderBase<FrameType> {
|
|||
|
|
public:
|
|||
|
|
typedef communication::format::bits::bitsize bitsize;
|
|||
|
|
|
|||
|
|
BinaryEncoderInverted( bitsize angle_size, bitsize turn_size );
|
|||
|
|
|
|||
|
|
void read( const void * data, std::size_t size ) override;
|
|||
|
|
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
template<typename FrameType>
|
|||
|
|
BinaryEncoderInverted<FrameType>::BinaryEncoderInverted( bitsize angle_size, bitsize turn_size ) :
|
|||
|
|
BinaryEncoderBase<FrameType>(angle_size, turn_size) {}
|
|||
|
|
|
|||
|
|
template<typename FrameType>
|
|||
|
|
void driver::detail::BinaryEncoderInverted<FrameType>::read( const void * data,
|
|||
|
|
std::size_t size ) {
|
|||
|
|
|
|||
|
|
typename BinaryEncoderBase<FrameType>::frame new_frame;
|
|||
|
|
std::memcpy( &new_frame, data, sizeof(new_frame) );
|
|||
|
|
BinaryEncoderBase<FrameType>::point.store( ~new_frame );
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//!<21><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD>.
|
|||
|
|
template<typename FrameType>
|
|||
|
|
class BinaryEncoderGray : public BinaryEncoderBase<FrameType> {
|
|||
|
|
public:
|
|||
|
|
typedef communication::format::bits::bitsize bitsize;
|
|||
|
|
|
|||
|
|
BinaryEncoderGray( bitsize angle_size, bitsize turn_size );
|
|||
|
|
|
|||
|
|
void read( const void * data, std::size_t size ) override;
|
|||
|
|
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
//!<21><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD>, <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
|
|||
|
|
template<typename FrameType>
|
|||
|
|
class BinaryEncoderGrayInverted : BinaryEncoderBase<FrameType> {
|
|||
|
|
public:
|
|||
|
|
typedef communication::format::bits::bitsize bitsize;
|
|||
|
|
|
|||
|
|
BinaryEncoderGrayInverted( bitsize angle_size, bitsize turn_size );
|
|||
|
|
|
|||
|
|
void read( const void * data, std::size_t size ) override;
|
|||
|
|
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
}}
|
|||
|
|
|
|||
|
|
#endif /* UMLIBRARY_DRIVER_BINARYENCODER_HPP_ */
|