Commit 7e8740e8 authored by Pierre NARVOR's avatar Pierre NARVOR
Browse files

[driver] Basic object to receive USBL serial message (no decoding)

parent c56e2c7e
#ifndef _DEF_SEATRAC_DRIVER_SERIAL_HANDLE_H_
#define _DEF_SEATRAC_DRIVER_SERIAL_HANDLE_H_
#include <iostream>
#include <sstream>
#include <vector>
#include <memory>
#define BOOST_BIND_GLOBAL_PLACEHOLDERS
#include <boost/asio.hpp>
#include <boost/bind.hpp>
namespace narval { namespace seatrac {
class 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;
const char* Delimiter = "\r\n";
enum FlushType {
FlushReceive = TCIFLUSH,
FlushSend = TCOFLUSH,
FlushBoth = TCIOFLUSH
};
protected:
IoServicePtr ioService_;
SerialPort serial_;
ReadBuffer readBuffer_;
void reset_serial();
boost::system::error_code flush(FlushType flushType = FlushBoth);
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:
SerialHandler(const IoServicePtr& ioService,
const std::string& port = "/dev/narval_usbl");
};
}; //namespace seatrac
}; //namespace narval
#endif //_DEF_SEATRAC_DRIVER_SERIAL_HANDLE_H_
#include <seatrac_driver/SerialHandler.h>
namespace narval { namespace seatrac {
SerialHandler::SerialHandler(const IoServicePtr& ioService,
const std::string& port) :
ioService_(ioService),
serial_(*ioService, port)
{
this->initiate_read();
}
void SerialHandler::reset_serial()
{
SerialPort::baud_rate baudRate(115200);
SerialPort::character_size cSize(8);
SerialPort::parity parity(SerialPort::parity::none);
SerialPort::flow_control flowControl(SerialPort::flow_control::none);
SerialPort::stop_bits stopBits(SerialPort::stop_bits::two);
serial_.set_option(baudRate);
serial_.set_option(cSize);
serial_.set_option(parity);
serial_.set_option(flowControl);
serial_.set_option(stopBits);
this->flush();
}
boost::system::error_code SerialHandler::flush(FlushType flushType)
{
if(::tcflush(serial_.lowest_layer().native_handle(), flushType) == 0) {
return boost::system::error_code();
}
else {
return boost::system::error_code(errno,
boost::asio::error::get_system_category());
}
}
void SerialHandler::initiate_read()
{
this->reset_serial();
boost::asio::async_read_until(serial_, readBuffer_, Delimiter,
boost::bind(&SerialHandler::on_first_read, this, _1, _2));
}
/**
* Grab and discard a first (probably incomplete) chunk of data.
*/
void SerialHandler::on_first_read(const boost::system::error_code& err, size_t byteCount)
{
if(err) {
std::ostringstream oss;
oss << "Got error on first read : " << err << "\n "
<< "Is the USBL powered up and plugged-in ?";
throw std::runtime_error(oss.str());
}
// entering reading loop
boost::asio::async_read_until(serial_, readBuffer_, Delimiter,
boost::bind(&SerialHandler::read_callback, this, _1, _2));
}
void SerialHandler::read_callback(const boost::system::error_code& err, size_t byteCount)
{
if(err) {
std::ostringstream oss;
oss << "Reading error : " << err;
throw std::runtime_error(oss.str());
}
this->on_read(readBuffer_, byteCount);
readBuffer_.consume(byteCount);
// looping
boost::asio::async_read_until(serial_, readBuffer_, Delimiter,
boost::bind(&SerialHandler::read_callback, this, _1, _2));
}
void SerialHandler::on_read(ReadBuffer& buffer, size_t byteCount)
{
std::cout << "Read : " << byteCount << " bytes" << std::endl;
std::istream is(&buffer);
std::ostringstream oss;
for(int i = 0; i < byteCount; i++) {
oss << (char)is.get();
}
std::cout << oss.str() << std::flush;
}
}; //namespace seatrac
}; //namespace narval
......@@ -2,6 +2,7 @@
list(APPEND test_names
hello_test.cpp
dump_test.cpp
serial_test.cpp
)
list(APPEND test_deps
......
......@@ -15,7 +15,6 @@ void read_callback(SerialPort* serial,
const boost::system::error_code& err, size_t byteCount)
{
std::istream is(buffer);
std::ostringstream oss;
for(int i = 0; i < byteCount; i++) {
oss << (char)is.get();
......
#include <iostream>
using namespace std;
#include <seatrac_driver/SerialHandler.h>
using namespace narval::seatrac;
int main()
{
auto service = std::make_shared<SerialHandler::IoService>();
SerialHandler 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