175 lines
5.4 KiB
C++
175 lines
5.4 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_ */
|