192 lines
5.7 KiB
C++
192 lines
5.7 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_ */
|