Commit 3925d5dd authored by Pierre NARVOR's avatar Pierre NARVOR
Browse files

[driver] Successful checksum handling (still work to do on parser)

parent 65891edd
......@@ -22,9 +22,11 @@ SeatracDriver::SeatracDriver(const IoServicePtr& ioService,
void SeatracDriver::on_read(ReadBuffer& buffer, size_t byteCount)
{
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 "
......@@ -32,17 +34,29 @@ void SeatracDriver::on_read(ReadBuffer& buffer, size_t byteCount)
for(int i = 0; i < byteCount - 1; i++) {
oss << (char)is.get();
}
throw std::runtime_error(oss.str());
//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;
data_.resize(byteCount - 7);
for(auto& v : data_) {
v = is.get();
crc16_update(checksum, v);
v = fromAscii(is.get()) << 4;
v += fromAscii(is.get());
crc16_update(checksum, v);
}
//cout << "Checksum
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;
}
}; //namespace seatrac
......
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