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

[driver] Added rx/tx dumping in SeatracSerial

parent f1ca283b
......@@ -3,6 +3,7 @@
#include <iostream>
#include <sstream>
#include <fstream>
#include <vector>
#include <memory>
......@@ -45,6 +46,10 @@ class SeatracSerial
ReadBuffer readBuffer_;
std::vector<uint8_t> decodedData_;
// dump related attributes
mutable std::ofstream rxDump_;
mutable std::ofstream txDump_;
void reset_serial();
boost::system::error_code flush(FlushType flushType = FlushBoth);
......@@ -67,9 +72,15 @@ class SeatracSerial
void send(size_t size, const uint8_t* data);
IoServicePtr io_service() const;
void enable_io_dump(const std::string& rxFile="serial_rx.dump",
const std::string& txFile="serial_tx.dump") const;
void disable_io_dump() const;
};
}; //namespace seatrac
}; //namespace narval
#endif //_DEF_SEATRAC_DRIVER_SEATRAC_SERIAL_H_
......@@ -13,7 +13,7 @@ inline void print_message(std::ostream& os, const std::vector<uint8_t>& msg)
default:
//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;
}
}
......
......@@ -13,7 +13,7 @@ void SeatracDriver::on_receive(const std::vector<uint8_t>& data)
//std::cout << "Received : " << data.size() << " bytes." << std::endl;
print_message(std::cout, data);
return;
//return;
messages::Status header = *reinterpret_cast<const messages::Status*>(data.data());
if(header.cmdId != CID_STATUS) {
......
......@@ -131,12 +131,33 @@ void SeatracSerial::read_callback(const boost::system::error_code& err, size_t b
void SeatracSerial::decode_received(size_t byteCount)
{
//std::cout << "Received " << byteCount << " bytes." << std::endl;
std::istream is(&readBuffer_);
std::istream is_raw(&readBuffer_);
std::string rawData;
{
std::ostringstream oss;
char c = is_raw.get();
while(c != EOF) {
oss << c;
c = is_raw.get();
}
rawData = oss.str();
}
//std::cout << rawData << std::flush;
if(rxDump_.is_open()) {
rxDump_ << rawData << std::flush;
}
std::istringstream is(rawData);
char start = is.get();
while(start == 0) { // WHY ????
byteCount--;
start = is.get();
}
if(start != '$') {
std::ostringstream oss;
oss << "Invalid data line (no starting character) :\n "
oss << "Invalid data line (no starting character) "
<< (uint32_t)start << " " << (uint32_t)'$'
<< " :\n "
<< start;
for(int i = 0; i < byteCount - 1; i++) {
oss << (char)is.get();
......@@ -198,6 +219,10 @@ void SeatracSerial::send(size_t size, const uint8_t* data)
oss << value_to_hexascii((checksum & 0x0f00) >> 8);
oss << "\r\n";
if(txDump_.is_open()) {
txDump_ << oss.str() << std::flush;
}
boost::asio::async_write(serial_, boost::asio::buffer(oss.str()),
boost::bind(&SeatracSerial::on_sent, this, _1, _2));
}
......@@ -220,5 +245,34 @@ SeatracSerial::IoServicePtr SeatracSerial::io_service() const
return ioService_;
}
void SeatracSerial::enable_io_dump(const std::string& rxFile,
const std::string& txFile) const
{
if(rxDump_.is_open() || txDump_.is_open()) {
std::cerr << "SeatracSerial::io_dump already enabled." << std::endl;
return;
}
rxDump_.open(rxFile, std::ios_base::out);
if(!rxDump_.is_open()) {
std::cerr << "SeatracSerial : could not open \""
<< rxFile << "\" for writing.";
}
txDump_.open(txFile, std::ios_base::out);
if(!txDump_.is_open()) {
std::cerr << "SeatracSerial : could not open \""
<< txFile << "\" for writing.";
}
}
void SeatracSerial::disable_io_dump() const
{
if(rxDump_.is_open())
rxDump_.close();
if(txDump_.is_open())
txDump_.close();
}
}; //namespace seatrac
}; //namespace narval
......@@ -25,6 +25,7 @@ int main()
{
auto service = std::make_shared<SeatracSerial::IoService>();
SeatracDriver serial(service);
serial.enable_io_dump();
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));
......
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