192 lines
5.3 KiB
C++
192 lines
5.3 KiB
C++
/*
|
|
* BinaryEncoder.hpp
|
|
*
|
|
* Created on: 23 èþí. 2023 ã.
|
|
* 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 {};
|
|
|
|
//!Áàçîâûé êëàññ äðàéâåðà ýíêîäåðà, îáíîâëÿþùåãî ñâîè çíà÷åíèÿ â ìîìåíò îáíîâëåíèÿ.
|
|
template<typename FrameType>
|
|
class BinaryEncoderBase : public BinaryEncoderInterface {
|
|
public:
|
|
typedef communication::format::bits::bitsize bitsize;
|
|
|
|
BinaryEncoderBase( bitsize angle_size, bitsize turn_size );
|
|
|
|
//!Ôóíêöèÿ âîçðàùàåò íîìåð îáîðîòà.
|
|
Turn getTurn() const;
|
|
|
|
//!Ôóíêöèÿ âîçðàùàåò óãîë â ðàäèàíàõ îò 0 äî 2*pi èëè NaN.
|
|
Angle getAngle() const;
|
|
|
|
//!Ìåòîä ïîçâîëÿåò ïîëó÷èòü êîíñèñòåíòíûå äàííûå ïîçèöèè ýíêîäåðà â ôîðìàòå (îáîðîò, óãîë).
|
|
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 );
|
|
|
|
}
|
|
|
|
|
|
//!Äðàéâåð ýíêîäåðà â áèíàðíîì ôîðìàòå.
|
|
template<typename FrameType>
|
|
class BinaryEncoder : public BinaryEncoderBase<FrameType> {
|
|
public:
|
|
typedef communication::format::bits::bitsize bitsize;
|
|
|
|
BinaryEncoder( bitsize angle_size, bitsize turn_size );
|
|
|
|
/*!
|
|
* data - óãîë è îáîðîò â áèíàðîíîì âèäå, óãîë ñî ñìåùåíèåì 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 );
|
|
|
|
}
|
|
|
|
//!Äðàéâåð ýíêîäåðà â áèíàðíîì ôîðìàòå, âêëþ÷àåò èíâåðñèþ âõîäíûõ äàííûõ äëÿ èçìåíåíèÿ íàïðàâëåíèÿ âðàùåíèÿ.
|
|
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 );
|
|
|
|
}
|
|
|
|
//!Äðàéâåð ýíêîäåðà â ôîðìàòå êîäà ãðåé.
|
|
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;
|
|
|
|
};
|
|
|
|
//!Äðàéâåð ýíêîäåðà â ôîðìàòå êîäà ãðåé, âêëþ÷àåò èíâåðñèþ âõîäíûõ äàííûõ äëÿ èçìåíåíèÿ íàïðàâëåíèÿ âðàùåíèÿ.
|
|
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_ */
|