Commit 65891edd authored by Pierre NARVOR's avatar Pierre NARVOR
Browse files

[driver] Added skeleton of SeatracDriver, implemented and tested checksum

parent 7e8740e8
......@@ -7,10 +7,12 @@ message(STATUS "Boost location : ${Boost_INCLUDE_DIRS}")
list(APPEND seatrac_driver_headers
include/seatrac_driver/SerialHandler.h
include/seatrac_driver/SeatracDriver.h
)
add_library(seatrac_driver SHARED
src/SerialHandler.cpp
src/SeatracDriver.cpp
)
set_target_properties(seatrac_driver PROPERTIES
NARVAL_PUBLIC_HEADER "${seatrac_driver_headers}"
......
#ifndef _DEF_SEATRAC_DRIVER_SEATRAC_DRIVER_H_
#define _DEF_SEATRAC_DRIVER_SEATRAC_DRIVER_H_
#include <seatrac_driver/SerialHandler.h>
namespace narval { namespace seatrac {
class SeatracDriver : SerialHandler
{
public:
using IoService = boost::asio::io_service;
using IoServicePtr = std::shared_ptr<boost::asio::io_service>;
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);
public:
SeatracDriver(const IoServicePtr& ioService,
const std::string& port = "/dev/narval_usbl");
};
}; //namespace seatrac
}; //namespace narval
#endif //_DEF_SEATRAC_DRIVER_SEATRAC_DRIVER_H_
......@@ -29,7 +29,7 @@ class SerialHandler
FlushBoth = TCIOFLUSH
};
protected:
private:
IoServicePtr ioService_;
SerialPort serial_;
......@@ -41,6 +41,7 @@ class SerialHandler
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);
public:
......
#include <seatrac_driver/SeatracDriver.h>
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) :
SerialHandler(ioService, port)
{}
void SeatracDriver::on_read(ReadBuffer& buffer, size_t byteCount)
{
std::istream is(&buffer);
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());
}
uint16_t checksum = 0;
data_.resize(byteCount - 7);
for(auto& v : data_) {
v = is.get();
crc16_update(checksum, v);
}
//cout << "Checksum
}
}; //namespace seatrac
}; //namespace narval
......@@ -91,6 +91,5 @@ void SerialHandler::on_read(ReadBuffer& buffer, size_t byteCount)
std::cout << oss.str() << std::flush;
}
}; //namespace seatrac
}; //namespace narval
......@@ -3,6 +3,8 @@ list(APPEND test_names
hello_test.cpp
dump_test.cpp
serial_test.cpp
driver_test.cpp
checksum_test.cpp
)
list(APPEND test_deps
......
#include <iostream>
using namespace std;
#include <seatrac_driver/SeatracDriver.h>
using namespace narval::seatrac;
// This is the code found in seatrac documentation.
uint16_t CalcCRC16(uint8_t* buf, uint16_t len)
{
uint16_t poly = 0xA001;
uint16_t crc = 0;
for(uint16_t b = 0; b < len; b++) {
uint8_t v = *buf;
for(uint8_t i = 0; i < 8; i++) {
if((v & 0x01) ^ (crc & 0x01)) {
crc >>= 1;
crc ^= poly;
}
else {
crc >>= 1;
}
v >>= 1;
}
buf++;
}
return crc;
}
std::vector<uint8_t> from_string(const std::string& msg)
{
std::vector<uint8_t> data(msg.size() / 2);
auto fromHex = [](char c) { return (uint8_t)(c >= 'A') ? (c - 'A' + 10) : (c - '0'); };
for(int i = 0; i < data.size(); i++) {
data[i] = (fromHex(msg[2*i]) << 4) + fromHex(msg[2*i + 1]);
}
return data;
}
int main()
{
std::string msg1 = "02";
std::string msg2 = "15";
std::string msg3 = "1000";
std::string msg4 = "4002";
std::string msg5 = "3102010400000000";
std::string msg6 = "0282330000011B0301690E000000000000FF900301006901B7FAC5BFFF910301007A07750463A9";
std::vector<uint8_t> data;
uint16_t checksum = 0;
for(auto c : from_string(msg1)) {
SeatracDriver::crc16_update(checksum, c);
}
cout << "msg1 : " << std::hex << checksum << endl;
data = from_string(msg1);
cout << "msg1 : " << CalcCRC16(data.data(), data.size()) << endl;
checksum = 0;
for(auto c : from_string(msg2)) {
SeatracDriver::crc16_update(checksum, c);
}
cout << "msg2 : " << std::hex << checksum << endl;
data = from_string(msg2);
cout << "msg2 : " << CalcCRC16(data.data(), data.size()) << endl;
checksum = 0;
for(auto c : from_string(msg3)) {
SeatracDriver::crc16_update(checksum, c);
}
cout << "msg3 : " << std::hex << checksum << endl;
data = from_string(msg3);
cout << "msg3 : " << CalcCRC16(data.data(), data.size()) << endl;
checksum = 0;
for(auto c : from_string(msg4)) {
SeatracDriver::crc16_update(checksum, c);
}
cout << "msg4 : " << std::hex << checksum << endl;
data = from_string(msg4);
cout << "msg4 : " << CalcCRC16(data.data(), data.size()) << endl;
checksum = 0;
for(auto c : from_string(msg5)) {
SeatracDriver::crc16_update(checksum, c);
}
cout << "msg5 : " << std::hex << checksum << endl;
data = from_string(msg5);
cout << "msg5 : " << CalcCRC16(data.data(), data.size()) << endl;
checksum = 0;
for(auto c : from_string(msg6)) {
SeatracDriver::crc16_update(checksum, c);
}
cout << "msg6 : " << std::hex << checksum << endl;
data = from_string(msg6);
cout << "msg6 : " << CalcCRC16(data.data(), data.size()) << endl;
return 0;
}
#include <iostream>
using namespace std;
#include <seatrac_driver/SeatracDriver.h>
using namespace narval::seatrac;
int main()
{
auto service = std::make_shared<SerialHandler::IoService>();
SeatracDriver serial(service);
service->run();
return 0;
}
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