[driver][WIP] Untested send command to usbl

include/seatrac_driver/SeatracDriver.h

@@ -7,7 +7,7 @@
 
 namespace narval { namespace seatrac {
 
-class SeatracDriver : SeatracSerial
+class SeatracDriver : public SeatracSerial
 {
     public:
 

include/seatrac_driver/SeatracSerial.h

@@ -40,9 +40,9 @@ class SeatracSerial
 
     private:
     
-    IoServicePtr ioService_;
-    SerialPort   serial_;
-    ReadBuffer   readBuffer_;
+    mutable IoServicePtr ioService_;
+    SerialPort           serial_;
+    ReadBuffer           readBuffer_;
     std::vector<uint8_t> decodedData_;
 
     void reset_serial();
@@ -54,14 +54,19 @@ class SeatracSerial
     void read_callback(const boost::system::error_code& err, 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.
+    // These are called after decoding/sent and can be reimplemented in a subclass to
+    // handle the received data or write error.
     virtual void on_receive(const std::vector<uint8_t>& data);
+    virtual void on_sent(const boost::system::error_code& err, size_t byteCount);
 
     public:
 
     SeatracSerial(const IoServicePtr& ioService,
                   const std::string& port = "/dev/narval_usbl");
+
+    void send(size_t size, const uint8_t* data);
+
+    IoServicePtr io_service() const;
 };
 
 }; //namespace seatrac

include/seatrac_driver/SeatracTypes.h

 #ifndef _DEF_SEATRAC_DRIVER_SEATRAC_TYPES_H_
 #define _DEF_SEATRAC_DRIVER_SEATRAC_TYPES_H_
 
+#include <sstream>
 #include <seatrac_driver/SeatracEnums.h>
 
 namespace narval { namespace seatrac {
@@ -554,4 +555,46 @@ struct SETTINGS_T {
 }; //namespace seatrac
 }; //namespace narval
 
+template <typename T>
+std::string add_indentation(const T& input, const char* indent = "  ")
+{
+    std::ostringstream tmp;
+    tmp << input;
+
+    std::ostringstream oss;
+    for(auto c : tmp.str()) {
+        oss << c;
+        if(c == '\n')
+            oss << indent;
+    }
+    return oss.str();
+}
+
+inline std::ostream& operator<<(std::ostream& os,
+                                const narval::seatrac::FIRMWARE_T& msg)
+{
+    static const char* prefix = "\n- ";
+    os << "Firmware :"
+       << prefix << "valid        : " << msg.valid
+       << prefix << "partNumber   : " << (uint32_t)msg.partNumber
+       << prefix << "versionMaj   : " << (uint32_t)msg.versionMaj
+       << prefix << "versionMin   : " << (uint32_t)msg.versionMin
+       << prefix << "versionBuild : " << (uint32_t)msg.versionBuild
+       << prefix << "checksum     : " << (uint32_t)msg.checksum;
+    return os;
+}
+
+inline std::ostream& operator<<(std::ostream& os,
+                                const narval::seatrac::HARDWARE_T& msg)
+{
+    static const char* prefix = "\n- ";
+    os << "Hardware :"
+       << prefix << "partNumber   : " << (uint32_t)msg.partNumber
+       << prefix << "partRev      : " << (uint32_t)msg.partRev
+       << prefix << "serialNumber : " << (uint32_t)msg.serialNumber
+       << prefix << "flagsSys     : " << (uint32_t)msg.flagsSys
+       << prefix << "flagsUser    : " << (uint32_t)msg.flagsUser;
+    return os;
+}
+
 #endif //_DEF_SEATRAC_DRIVER_SEATRAC_TYPES_H_

include/seatrac_driver/messages/MessageBase.h

@@ -8,7 +8,7 @@ namespace narval { namespace seatrac { namespace messages {
 struct Message
 { 
     CID_E cmdId;
-};
+}__attribute__((packed));
 
 }; //namespace messages
 }; //namespace seatrac

include/seatrac_driver/messages/Messages.h

@@ -3,6 +3,7 @@
 
 #include <seatrac_driver/messages/MessageBase.h>
 #include <seatrac_driver/messages/Status.h>
+#include <seatrac_driver/messages/SysInfo.h>
 
 #endif //_DEF_SEATRAC_DRIVER_MESSAGES_MESSAGES_H_
 

include/seatrac_driver/messages/SysInfo.h

+#ifndef _DEF_SEATRAC_DRIVER_MESSAGES_SYSINFO_H_
+#define _DEF_SEATRAC_DRIVER_MESSAGES_SYSINFO_H_
+
+#include <seatrac_driver/SeatracTypes.h>
+#include <seatrac_driver/messages/MessageBase.h>
+
+namespace narval { namespace seatrac { namespace messages {
+
+struct SysInfo : public Message
+{
+    uint32_t   alivedFor;
+    uint8_t    currentApp;
+    HARDWARE_T hardware;
+    FIRMWARE_T bootFirmware;
+    FIRMWARE_T mainFirmware;
+    uint8_t    boardRev;
+
+}__attribute__((packed));
+
+
+}; //namespace messages
+}; //namespace seatrac
+}; //namespace narval
+
+inline std::ostream& operator<<(std::ostream& os,
+                                const narval::seatrac::messages::SysInfo& msg)
+{
+    static const char* prefix = "\n- ";
+    os << "SysInfo :"
+       << prefix << "cmdId        : " << (uint32_t)msg.cmdId
+       << prefix << "alivedFor    : " << (uint32_t)msg.alivedFor
+       << prefix << "currentApp   : " << (uint32_t)msg.currentApp
+       << prefix << "hardware     : " << add_indentation(msg.hardware)
+       << prefix << "bootFirmware : " << add_indentation(msg.bootFirmware)
+       << prefix << "mainFirmware : " << add_indentation(msg.mainFirmware)
+       << prefix << "boardRev     : " << (uint32_t)msg.boardRev;
+    return os;
+}
+
+#endif //_DEF_SEATRAC_DRIVER_MESSAGES_SYSINFO_H_

include/seatrac_driver/print_utils.h

@@ -11,9 +11,10 @@ inline void print_message(std::ostream& os, const std::vector<uint8_t>& msg)
     using namespace narval::seatrac::messages;
     switch(msg[0]) {
         default: 
-            os << "Unknown message (CID : " <<  msg[0] << ")" << std::endl;
+            //os << "Unknown message (CID : " <<  (uint32_t)msg[0] << ")" << std::endl;
             break;
-        case CID_STATUS: os << *reinterpret_cast<const Status*>(msg.data()) << std::endl; break;
+        //case CID_STATUS:   os << *reinterpret_cast<const Status*>(msg.data()) << std::endl;  break;
+        case CID_SYS_INFO: os << *reinterpret_cast<const SysInfo*>(msg.data()) << std::endl; break;
     }
 }
 

src/SeatracDriver.cpp

@@ -10,8 +10,10 @@ SeatracDriver::SeatracDriver(const IoServicePtr& ioService,
 
 void SeatracDriver::on_receive(const std::vector<uint8_t>& data)
 {
-    std::cout << "Received : " << data.size() << " bytes." << std::endl;
+    //std::cout << "Received : " << data.size() << " bytes." << std::endl;
     print_message(std::cout, data);
+
+    return;
     
     messages::Status header = *reinterpret_cast<const messages::Status*>(data.data());
     if(header.cmdId != CID_STATUS) {

src/SeatracSerial.cpp

@@ -181,5 +181,44 @@ void SeatracSerial::on_receive(const std::vector<uint8_t>& data)
     std::cout << "Received " << data.size() << " bytes." << std::endl;
 }
 
+void SeatracSerial::send(size_t size, const uint8_t* data)
+{
+    std::ostringstream oss;
+    uint16_t checksum = 0;
+
+    oss << '#';
+    for(int i = 0; i < size; i++) {
+        crc16_update(checksum, data[i]);
+        oss << value_to_hexascii((data[i] & 0xf0) >> 4);
+        oss << value_to_hexascii( data[i] & 0x0f);
+    }
+    oss << value_to_hexascii((checksum & 0x00f0) >> 4);
+    oss << value_to_hexascii((checksum & 0x000f));
+    oss << value_to_hexascii((checksum & 0xf000) >> 12);
+    oss << value_to_hexascii((checksum & 0x0f00) >> 8);
+    oss << "\r\n";
+
+    boost::asio::async_write(serial_, boost::asio::buffer(oss.str()),
+        boost::bind(&SeatracSerial::on_sent, this, _1, _2));
+}
+
+void SeatracSerial::on_sent(const boost::system::error_code& err, size_t byteCount)
+{
+    if(err) {
+        std::ostringstream oss;
+        oss << "Write error : " << err;
+        //throw std::runtime_error(oss.str());
+        std::cerr << oss.str() << std::endl;
+    }
+    else {
+        std::cout << "Written " << byteCount << " bytes." << std::endl;
+    }
+}
+
+SeatracSerial::IoServicePtr SeatracSerial::io_service() const
+{
+    return ioService_;
+}
+
 }; //namespace seatrac
 }; //namespace narval

tests/CMakeLists.txt

@@ -4,7 +4,7 @@ list(APPEND test_names
     dump_test.cpp
     serial_test.cpp
     checksum_test.cpp
-    # driver_test.cpp
+    driver_test.cpp
 )
 
 list(APPEND test_deps

tests/src/driver_test.cpp

@@ -4,11 +4,31 @@ using namespace std;
 #include <seatrac_driver/SeatracDriver.h>
 using namespace narval::seatrac;
 
+void request_sys_info(boost::asio::steady_timer* timer,
+                      SeatracSerial* serial,
+                      const boost::system::error_code& err)
+{
+    if(err) {
+        std::ostringstream oss;
+        oss << "Error waiting for timer : " << err << endl;
+        throw std::runtime_error(oss.str());
+    }
+    
+    uint8_t cid = 0x02;
+    serial->send(1, &cid);
+    
+    timer->expires_from_now(boost::asio::chrono::seconds(2));
+    timer->async_wait(boost::bind(&request_sys_info, timer, serial, _1)); 
+}
+
 int main()
 {
     auto service = std::make_shared<SeatracSerial::IoService>();
     SeatracDriver serial(service);
 
+    boost::asio::steady_timer timer(*serial.io_service().get(), boost::asio::chrono::seconds(2));
+    timer.async_wait(boost::bind(&request_sys_info, &timer, &serial, _1)); 
+
     service->run();
 
     return 0;

tests/src/serial_test.cpp

@@ -4,11 +4,31 @@ using namespace std;
 #include <seatrac_driver/SeatracSerial.h>
 using namespace narval::seatrac;
 
+void request_sys_info(boost::asio::steady_timer* timer,
+                      SeatracSerial* serial,
+                      const boost::system::error_code& err)
+{
+    if(err) {
+        std::ostringstream oss;
+        oss << "Error waiting for timer : " << err << endl;
+        throw std::runtime_error(oss.str());
+    }
+    
+    uint8_t cid = 0x02;
+    serial->send(1, &cid);
+    
+    timer->expires_from_now(boost::asio::chrono::seconds(2));
+    timer->async_wait(boost::bind(&request_sys_info, timer, serial, _1)); 
+}
+
 int main()
 {
     auto service = std::make_shared<SeatracSerial::IoService>();
     SeatracSerial serial(service);
     
+    boost::asio::steady_timer timer(*serial.io_service().get(), boost::asio::chrono::seconds(2));
+    timer.async_wait(boost::bind(&request_sys_info, &timer, &serial, _1)); 
+
     service->run();
 
     return 0;