123 lines
3.2 KiB
C++
123 lines
3.2 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_ */
|