123 lines
3.3 KiB
C++
123 lines
3.3 KiB
C++
/*
|
||
* EncoderDataParser.hpp
|
||
*
|
||
* Created on: 21 мар. 2019 г.
|
||
* Author: krugliy
|
||
*/
|
||
|
||
#ifndef SOURCE_DRIVER_ENCODERDATAPARSER_HPP_
|
||
#define SOURCE_DRIVER_ENCODERDATAPARSER_HPP_
|
||
|
||
|
||
#include <stdint.h>
|
||
#include "../math/math_inc.hh"
|
||
|
||
namespace driver {
|
||
|
||
//!
|
||
template<class Decoder>
|
||
class EncoderDataParser {
|
||
public:
|
||
typedef EncoderDataParser<Decoder> ParserType;
|
||
typedef long CodeType;
|
||
|
||
struct Setting {
|
||
uint16_t angle_bit_len;
|
||
uint16_t turn_bit_len;
|
||
uint16_t error_bit_len;
|
||
};
|
||
|
||
struct EncoderData {
|
||
float angle;
|
||
unsigned long turn;
|
||
unsigned short error;
|
||
};
|
||
|
||
EncoderDataParser(Decoder _decoder)
|
||
: decoder(_decoder) {}
|
||
|
||
EncoderDataParser(const EncoderDataParser &) = default;
|
||
|
||
float getAngleDelta( const EncoderData & p1, const EncoderData & p2 ) {
|
||
|
||
long signed_turn1 = p1.turn << ((32ul - turn_signbit_offset)) >> turn_signbit_offset;
|
||
long signed_turn2 = p2.turn << ((32ul - turn_signbit_offset)) >> turn_signbit_offset;
|
||
|
||
return std::abs( math::constants::pi2*(signed_turn1 - signed_turn2) + (p1.angle - p2.angle) );
|
||
|
||
}
|
||
|
||
EncoderData parseEncoderData( unsigned long raw_encoder_data ) {
|
||
|
||
position_code = decoder((raw_encoder_data & position_code_mask) >> position_bit_offset, position_resolution );
|
||
|
||
return {
|
||
.angle = angle_code_to_rad * ( (position_code & angle_code_mask) >> angle_offset ),
|
||
.turn = ( position_code & turn_code_mask ) >> turn_offset,
|
||
.error = static_cast<unsigned short>(( raw_encoder_data & error_code_mask ) >> error_offset)
|
||
};
|
||
|
||
}
|
||
|
||
bool cofigure(Setting & set) {
|
||
bool result = true;
|
||
|
||
if( set.angle_bit_len == 0 ) result = false;
|
||
|
||
if( result ) {
|
||
|
||
//применять на raw_encoder_data
|
||
position_resolution = set.angle_bit_len + set.turn_bit_len;
|
||
position_bit_offset = set.error_bit_len;
|
||
position_code_mask = ((1ul << position_resolution) - 1) << position_bit_offset;
|
||
|
||
//применять на raw_encoder_data
|
||
error_offset = 0;
|
||
error_code_mask = ((1ul << set.error_bit_len) - 1) << error_offset;
|
||
|
||
//применять на position_code
|
||
angle_offset = 0;
|
||
angle_code_mask = ((1ul << set.angle_bit_len) - 1) << angle_offset;
|
||
|
||
//применять на position_code
|
||
turn_offset = set.angle_bit_len;
|
||
turn_code_mask = ((1ul << set.turn_bit_len) - 1) << turn_offset;
|
||
|
||
//TODO следить за типом!
|
||
turn_signbit_offset = set.turn_bit_len;
|
||
|
||
angle_code_to_rad = math::constants::pi2 / (1ul << set.angle_bit_len);
|
||
|
||
}
|
||
|
||
return result;
|
||
}
|
||
|
||
private:
|
||
Decoder decoder;
|
||
volatile unsigned long position_code;
|
||
|
||
unsigned long position_resolution;
|
||
unsigned long position_bit_offset;
|
||
unsigned long position_code_mask;
|
||
|
||
unsigned long angle_code_mask;
|
||
unsigned long angle_offset;
|
||
|
||
unsigned long turn_code_mask;
|
||
unsigned long turn_offset;
|
||
|
||
unsigned long error_code_mask;
|
||
unsigned long error_offset;
|
||
|
||
unsigned short turn_signbit_offset; //!<Позиция старшего бита кода позиции.
|
||
|
||
float angle_code_to_rad = NAN;
|
||
|
||
};
|
||
|
||
}
|
||
|
||
|
||
#endif /* SOURCE_DRIVER_ENCODERPOSITIONDATAPARSER_HPP_ */
|