175 lines
6.2 KiB
C++
175 lines
6.2 KiB
C++
/*
|
||
* EncoderSsi.h
|
||
*
|
||
* Created on: 20 мар. 2019 г.
|
||
* Author: krugliy
|
||
*/
|
||
|
||
#ifndef SOURCE_DRIVER_ENCODERSSI_H_
|
||
#define SOURCE_DRIVER_ENCODERSSI_H_
|
||
|
||
#include <cmath>
|
||
#include <stdint.h>
|
||
|
||
#include "IEncoder.hh"
|
||
|
||
#include "../common/DoubleBuffer.hpp"
|
||
|
||
namespace driver { namespace detail {
|
||
|
||
struct EncoderSsiConfig {
|
||
struct Config {
|
||
|
||
uint16_t num_error; //!<Количество ошибок, при котором формируется отказ отказа.
|
||
float max_angle_delta; //!<Изменение между двумя значениями угла больше данного порога считаются ошибкой.
|
||
|
||
uint16_t ssi_data_bit_len; //!<Количество бит, которые передает энкодер.
|
||
uint16_t max_turn_bit; //!<Количество бит, используемых для кода оборотов.
|
||
|
||
//аппаратные настройки:
|
||
uint16_t frame_size; //!<Размер фрейма в настройках spi,
|
||
//чтобы не потерять младшие биты от энкодера, должен быть не меньше чем ssi_data_bit_len
|
||
|
||
uint16_t ssi_extra_bit; //!Старшие незначащие биты принятые из-за настроек spi
|
||
uint16_t shift_count; //!<Сдвиг данных в принятом кадре. Учитывает задержки в линии данных относительно clk.
|
||
bool inverse; //!<Признак инверсии принятых данных.
|
||
|
||
bool swap_bytes; //!< Признак необходимости менять местами байты в принятых данных.
|
||
};
|
||
|
||
EncoderSsiConfig( Config config ) : error_counter(config.num_error), max_angle_delta(config.max_angle_delta),
|
||
turn_bit_mask( ( 1 << config.max_turn_bit ) - 1 ),
|
||
shift_count(config.frame_size - config.ssi_data_bit_len - ( config.ssi_extra_bit + config.shift_count )),
|
||
inverse(config.inverse), swap_bytes( config.swap_bytes ) {}
|
||
|
||
struct ErrorCounter {
|
||
|
||
typedef unsigned short ErrorCount;
|
||
|
||
ErrorCount counter;
|
||
ErrorCount max_error;
|
||
|
||
ErrorCounter( ErrorCount num_error ) : counter(1), max_error(num_error) {}
|
||
|
||
} error_counter;
|
||
|
||
float max_angle_delta;
|
||
uint32_t turn_bit_mask;
|
||
|
||
uint16_t shift_count;
|
||
bool inverse;
|
||
|
||
bool swap_bytes; //!< Признак необходимости менять местами байты в принятых данных.
|
||
};
|
||
|
||
template<class Interface, class DataParser>
|
||
class EncoderSsi : public IEncoder, public EncoderSsiConfig {
|
||
public:
|
||
|
||
EncoderSsi( DataParser _parser, Interface & _interface, EncoderSsiConfig::Config config )
|
||
: EncoderSsiConfig(config), interface(_interface), parser(_parser) {}
|
||
|
||
virtual ~EncoderSsi() = default;
|
||
|
||
float getAngle() const override { return error_counter.counter ? NAN : encoder_angle; }
|
||
float getTurn() const override { return error_counter.counter ? NAN : encoder_turn & turn_bit_mask; }
|
||
std::pair<float, float> getPosition() const override;
|
||
|
||
// unsigned short getErrorCode() const { return encoder_error_code; }
|
||
float getErrorCode() const { return encoder_error_code; }
|
||
|
||
bool isValidData() const { return not error_counter.counter; }
|
||
bool isFailure() const { return error_counter.counter >= error_counter.max_error; }
|
||
bool isDirectionError() const { return direction_error; }
|
||
|
||
void process();
|
||
|
||
uint32_t getRawData();
|
||
|
||
private:
|
||
DataParser parser;
|
||
Interface & interface;
|
||
|
||
unsigned short encoder_turn = 0;
|
||
float encoder_angle = NAN; //только угол
|
||
|
||
typename DataParser::EncoderData encoder_position; //угол + обороты + ошибка
|
||
|
||
unsigned short encoder_error_code = 0;
|
||
bool direction_error = false; //!<Программно определяемая ошибка в данных энкодера, связанная со скачкообразным изменением показаний.
|
||
|
||
DoubleBuffer< std::pair<float, float> > data;
|
||
|
||
|
||
};
|
||
|
||
|
||
} /* namespace detail */
|
||
} /* namespace driver */
|
||
|
||
template<class Interface, class DataParser>
|
||
inline void driver::detail::EncoderSsi<Interface, DataParser>::process() {
|
||
|
||
//TODO: сперва нужно проверить есть ли у connection новые данные для нас, иначе он выдаст старые..
|
||
uint32_t encoder_data = getRawData();
|
||
|
||
typename DataParser::EncoderData next_position_data = parser.parseEncoderData( encoder_data );
|
||
|
||
if( encoder_error_code = next_position_data.error ) {
|
||
|
||
if( error_counter.counter < error_counter.max_error )
|
||
error_counter.counter++;
|
||
|
||
} else {
|
||
|
||
encoder_angle = next_position_data.angle;
|
||
encoder_turn = next_position_data.turn;
|
||
|
||
//обработчки скачков показаний энкодера в оборотах
|
||
direction_error = parser.getAngleDelta( next_position_data, encoder_position ) > max_angle_delta;
|
||
|
||
encoder_position = next_position_data;
|
||
|
||
if( error_counter.counter )
|
||
--error_counter.counter;
|
||
|
||
data.write( std::pair<float, float>( encoder_turn, encoder_angle ) );
|
||
}
|
||
|
||
}
|
||
|
||
template<class Interface, class DataParser>
|
||
inline std::pair<float, float> driver::detail::EncoderSsi<Interface, DataParser>::getPosition() const {
|
||
|
||
return error_counter.counter ? std::pair<float, float>( 0, NAN ) : data.read();
|
||
|
||
}
|
||
|
||
template<class Interface, class DataParser>
|
||
inline uint32_t driver::detail::EncoderSsi<Interface, DataParser>::getRawData() {
|
||
|
||
if( swap_bytes ) {
|
||
|
||
uint32_t raw_encoder_data = interface.get();
|
||
|
||
uint32_t encoder_data = ( raw_encoder_data << 16 ) | ( (raw_encoder_data >> 16) & 0xFFFF );
|
||
|
||
if( inverse ) encoder_data = ~encoder_data;
|
||
|
||
encoder_data >>= shift_count;
|
||
|
||
return encoder_data;
|
||
|
||
} else {
|
||
|
||
uint32_t encoder_data = interface.get();
|
||
|
||
if( inverse ) encoder_data = ~encoder_data;
|
||
|
||
return encoder_data;
|
||
}
|
||
|
||
}
|
||
|
||
#endif /* SOURCE_DRIVER_ENCODERSSI_HPP_ */
|