[driver] Moved decoding from SeatracDriver to SeatracSerial

include/seatrac_driver/SeatracDriver.h

@@ -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:
 

include/seatrac_driver/SeatracSerial.h

@@ -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:
 

src/SeatracDriver.cpp

@@ -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

src/SeatracSerial.cpp

@@ -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

tests/src/checksum_test.cpp

 #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);