Commit ecaaa7be authored by Pierre NARVOR's avatar Pierre NARVOR
Browse files

[driver] Moved decoding from SeatracDriver to SeatracSerial

parent 9dbb1617
......@@ -14,13 +14,11 @@ class SeatracDriver : SeatracSerial
using SerialPort = boost::asio::serial_port;
using ReadBuffer = boost::asio::streambuf;
static void crc16_update(uint16_t& checksum, uint8_t v);
protected:
std::vector<uint8_t> data_;
virtual void on_read(ReadBuffer& buffer, size_t byteCount);
virtual void on_receive(const std::vector<uint8_t>& data);
public:
......
......@@ -11,7 +11,13 @@
#include <boost/bind.hpp>
namespace narval { namespace seatrac {
/**
* This class handler low level serial communication with the USBL device.
*
* It handles the coding and decoding of binary messages (encoding, checksum)
* as well as the serial port itself (configuration and reception).
*/
class SeatracSerial
{
public:
......@@ -21,28 +27,36 @@ class SeatracSerial
using SerialPort = boost::asio::serial_port;
using ReadBuffer = boost::asio::streambuf;
const char* Delimiter = "\r\n";
enum FlushType {
FlushReceive = TCIFLUSH,
FlushSend = TCOFLUSH,
FlushBoth = TCIOFLUSH
};
const char* Delimiter = "\r\n";
static void crc16_update(uint16_t& checksum, uint8_t v);
static uint8_t hexascii_to_value(char c);
static char value_to_hexascii(uint8_t v);
private:
IoServicePtr ioService_;
SerialPort serial_;
ReadBuffer readBuffer_;
std::vector<uint8_t> decodedData_;
void reset_serial();
boost::system::error_code flush(FlushType flushType = FlushBoth);
// These define the state machine which reads the serial port.
void initiate_read();
void on_first_read(const boost::system::error_code& err, size_t byteCount);
void read_callback(const boost::system::error_code& err, size_t byteCount);
virtual void on_read(ReadBuffer& buffer, size_t byteCount);
void decode_received(size_t byteCount);
// This is called after decoding and can be reimplemented in a subclass to
// handle the received data.
virtual void on_receive(const std::vector<uint8_t>& data);
public:
......
......@@ -2,61 +2,14 @@
namespace narval { namespace seatrac {
void SeatracDriver::crc16_update(uint16_t& checksum, uint8_t v)
{
const uint16_t poly = 0xA001;
checksum = ((v & 0x01) ^ (checksum & 0x01)) ? (checksum>>1) ^ poly : checksum>>1;
checksum = ((v>>1 & 0x01) ^ (checksum & 0x01)) ? (checksum>>1) ^ poly : checksum>>1;
checksum = ((v>>2 & 0x01) ^ (checksum & 0x01)) ? (checksum>>1) ^ poly : checksum>>1;
checksum = ((v>>3 & 0x01) ^ (checksum & 0x01)) ? (checksum>>1) ^ poly : checksum>>1;
checksum = ((v>>4 & 0x01) ^ (checksum & 0x01)) ? (checksum>>1) ^ poly : checksum>>1;
checksum = ((v>>5 & 0x01) ^ (checksum & 0x01)) ? (checksum>>1) ^ poly : checksum>>1;
checksum = ((v>>6 & 0x01) ^ (checksum & 0x01)) ? (checksum>>1) ^ poly : checksum>>1;
checksum = ((v>>7 & 0x01) ^ (checksum & 0x01)) ? (checksum>>1) ^ poly : checksum>>1;
}
SeatracDriver::SeatracDriver(const IoServicePtr& ioService,
const std::string& port) :
SeatracSerial(ioService, port)
{}
void SeatracDriver::on_read(ReadBuffer& buffer, size_t byteCount)
void SeatracDriver::on_receive(const std::vector<uint8_t>& data)
{
std::cout << "Received " << byteCount << " bytes." << std::endl;
std::istream is(&buffer);
char start = is.get();
std::cout << start << std::endl;
if(start != '$') {
std::ostringstream oss;
oss << "Invalid data line (no starting character) :\n "
<< start;
for(int i = 0; i < byteCount - 1; i++) {
oss << (char)is.get();
}
//throw std::runtime_error(oss.str());
std::cerr << oss.str() << std::endl;
}
// conversion from pair of ascii character to a 8bit value
auto fromAscii = [](char c) { return (uint8_t)(c >= 'A') ? (c - 'A' + 10) : (c - '0'); };
data_.resize((byteCount - 7) / 2);
uint16_t checksum = 0;
for(auto& v : data_) {
v = fromAscii(is.get()) << 4;
v += fromAscii(is.get());
crc16_update(checksum, v);
}
uint16_t c = 0;
c = fromAscii(is.get()) << 4;
c += fromAscii(is.get());
c += fromAscii(is.get()) << 12;
c += fromAscii(is.get()) << 8;
std::cout << std::hex << checksum << std::dec << std::endl;
std::cout << std::hex << c << std::dec << std::endl;
std::cout << (int)is.get() << " " << (int)is.get() << std::endl;
std::cout << "Received : " << data.size() << " bytes." << std::endl;
}
}; //namespace seatrac
......
......@@ -2,6 +2,53 @@
namespace narval { namespace seatrac {
/**
* This compute a single step of a CRC16 checksum.
*/
void SeatracSerial::crc16_update(uint16_t& checksum, uint8_t v)
{
const uint16_t poly = 0xA001;
checksum = ((v & 0x01) ^ (checksum & 0x01)) ? (checksum>>1) ^ poly : checksum>>1;
checksum = ((v>>1 & 0x01) ^ (checksum & 0x01)) ? (checksum>>1) ^ poly : checksum>>1;
checksum = ((v>>2 & 0x01) ^ (checksum & 0x01)) ? (checksum>>1) ^ poly : checksum>>1;
checksum = ((v>>3 & 0x01) ^ (checksum & 0x01)) ? (checksum>>1) ^ poly : checksum>>1;
checksum = ((v>>4 & 0x01) ^ (checksum & 0x01)) ? (checksum>>1) ^ poly : checksum>>1;
checksum = ((v>>5 & 0x01) ^ (checksum & 0x01)) ? (checksum>>1) ^ poly : checksum>>1;
checksum = ((v>>6 & 0x01) ^ (checksum & 0x01)) ? (checksum>>1) ^ poly : checksum>>1;
checksum = ((v>>7 & 0x01) ^ (checksum & 0x01)) ? (checksum>>1) ^ poly : checksum>>1;
}
/**
* Converts a single hexadecimal synbol in ascii into its decimal value.
*
* Only valid for ascii digit characters [0,...,9] and characters ABCDEF.
* ('0'=0, '9'=9, 'A'=10, 'B'=11, 'F'=15).
*
* Caution : only uppercase letters are valid.
*
* @param c a character representing an hexadecimal digit.
*
* @return a value between [0,15].
*/
uint8_t SeatracSerial::hexascii_to_value(char c)
{
return (c >= 'A') ? (c - 'A' + 10) : (c - '0');
}
/**
* Converts a decimal value between [0,15] into its ascii hexadecimal
* representation.
*
* @param v a value between [0,15] (only 4 bits are used)
*
* @return an hexadecimal symbol ['0',...,'9'] or ABCDEF (guaranteed
* uppercase).
*/
char SeatracSerial::value_to_hexascii(uint8_t v)
{
return (v >= 10) ? (v - 10 + 'A') : (v + '0');
}
SeatracSerial::SeatracSerial(const IoServicePtr& ioService,
const std::string& port) :
ioService_(ioService),
......@@ -68,27 +115,70 @@ void SeatracSerial::read_callback(const boost::system::error_code& err, size_t b
if(err) {
std::ostringstream oss;
oss << "Reading error : " << err;
throw std::runtime_error(oss.str());
//throw std::runtime_error(oss.str());
std::cerr << oss.str() << std::endl;
}
else {
this->decode_received(byteCount);
}
this->on_read(readBuffer_, byteCount);
readBuffer_.consume(byteCount);
readBuffer_.consume(byteCount);
// looping
boost::asio::async_read_until(serial_, readBuffer_, Delimiter,
boost::bind(&SeatracSerial::read_callback, this, _1, _2));
}
void SeatracSerial::on_read(ReadBuffer& buffer, size_t byteCount)
void SeatracSerial::decode_received(size_t byteCount)
{
std::cout << "Read : " << byteCount << " bytes" << std::endl;
//std::cout << "Received " << byteCount << " bytes." << std::endl;
std::istream is(&readBuffer_);
char start = is.get();
if(start != '$') {
std::ostringstream oss;
oss << "Invalid data line (no starting character) :\n "
<< start;
for(int i = 0; i < byteCount - 1; i++) {
oss << (char)is.get();
}
//throw std::runtime_error(oss.str());
std::cerr << oss.str() << std::endl;
return;
}
// conversion from pair of ascii characters to 8bit values.
decodedData_.resize((byteCount - 7) / 2);
uint16_t checksum = 0;
for(auto& v : decodedData_) {
v = hexascii_to_value(is.get()) << 4;
v += hexascii_to_value(is.get());
crc16_update(checksum, v);
}
// Decoding transmitted checksum
uint16_t transmittedChecksum = 0;
transmittedChecksum = hexascii_to_value(is.get()) << 4;
transmittedChecksum += hexascii_to_value(is.get());
transmittedChecksum += hexascii_to_value(is.get()) << 12;
transmittedChecksum += hexascii_to_value(is.get()) << 8;
std::istream is(&buffer);
std::ostringstream oss;
for(int i = 0; i < byteCount; i++) {
oss << (char)is.get();
if(checksum != transmittedChecksum) {
std::ostringstream oss;
oss << "Invalid data line (wrong checksum).";
//throw std::runtime_error(oss.str());
std::cerr << oss.str() << std::endl;
return;
}
std::cout << oss.str() << std::flush;
this->on_receive(decodedData_);
//std::cout << std::hex << checksum << std::dec << std::endl;
//std::cout << std::hex << receivedChecksum << std::dec << std::endl;
//std::cout << (int)is.get() << " " << (int)is.get() << std::endl;
}
void SeatracSerial::on_receive(const std::vector<uint8_t>& data)
{
std::cout << "Received " << data.size() << " bytes." << std::endl;
}
}; //namespace seatrac
......
#include <iostream>
using namespace std;
#include <seatrac_driver/SeatracDriver.h>
#include <seatrac_driver/SeatracSerial.h>
using namespace narval::seatrac;
// This is the code found in seatrac documentation.
......@@ -49,7 +49,7 @@ int main()
uint16_t checksum = 0;
for(auto c : from_string(msg1)) {
SeatracDriver::crc16_update(checksum, c);
SeatracSerial::crc16_update(checksum, c);
}
cout << "msg1 : " << std::hex << checksum << endl;
data = from_string(msg1);
......@@ -57,7 +57,7 @@ int main()
checksum = 0;
for(auto c : from_string(msg2)) {
SeatracDriver::crc16_update(checksum, c);
SeatracSerial::crc16_update(checksum, c);
}
cout << "msg2 : " << std::hex << checksum << endl;
data = from_string(msg2);
......@@ -65,7 +65,7 @@ int main()
checksum = 0;
for(auto c : from_string(msg3)) {
SeatracDriver::crc16_update(checksum, c);
SeatracSerial::crc16_update(checksum, c);
}
cout << "msg3 : " << std::hex << checksum << endl;
data = from_string(msg3);
......@@ -73,7 +73,7 @@ int main()
checksum = 0;
for(auto c : from_string(msg4)) {
SeatracDriver::crc16_update(checksum, c);
SeatracSerial::crc16_update(checksum, c);
}
cout << "msg4 : " << std::hex << checksum << endl;
data = from_string(msg4);
......@@ -82,7 +82,7 @@ int main()
checksum = 0;
for(auto c : from_string(msg5)) {
SeatracDriver::crc16_update(checksum, c);
SeatracSerial::crc16_update(checksum, c);
}
cout << "msg5 : " << std::hex << checksum << endl;
data = from_string(msg5);
......@@ -91,7 +91,7 @@ int main()
checksum = 0;
for(auto c : from_string(msg6)) {
SeatracDriver::crc16_update(checksum, c);
SeatracSerial::crc16_update(checksum, c);
}
cout << "msg6 : " << std::hex << checksum << endl;
data = from_string(msg6);
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment