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

[driver] Handle partial data reception on serial port

parent 7eab4afb
......@@ -34,8 +34,11 @@ class SeatracSerial
FlushBoth = TCIOFLUSH
};
const char* Delimiter = "\r\n";
static void crc16_update(uint16_t& checksum, uint8_t v);
static constexpr const char* Delimiter = "\r\n";
static bool read_until(std::istream& is, const char* delimiter,
std::ostream& os);
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);
......@@ -44,6 +47,7 @@ class SeatracSerial
mutable IoServicePtr ioService_;
SerialPort serial_;
ReadBuffer readBuffer_;
std::ostringstream partialData_;
std::vector<uint8_t> decodedData_;
// dump related attributes
......@@ -57,7 +61,7 @@ class SeatracSerial
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);
void decode_received(size_t byteCount);
void decode_received(const std::string& msg);
// These are called after decoding/sent and can be reimplemented in a subclass to
// handle the received data or write error.
......
......@@ -49,6 +49,39 @@ char SeatracSerial::value_to_hexascii(uint8_t v)
return (v >= 10) ? (v - 10 + 'A') : (v + '0');
}
/**
* Consumes characters from an istream until a delimiter is reached.
*
* If the istream contains the delimiter, the output string ends with the
* delimiter and the remaining data stays in the istream. Otherwise the istream
* is emptied and the output string contains all of the stream data.
*/
bool SeatracSerial::read_until(std::istream& is, const char* delimiter,
std::ostream& os)
{
const char* currentChar = delimiter;
if(*currentChar == '\0')
return true;
char c = is.get();
while(c != EOF) {
os << c;
// If c matches current char of delimiter, checking next delimiter
// char. If not, the match should restart from the begin.
if(c == *currentChar)
currentChar++;
else
currentChar = delimiter;
if(*currentChar == '\0') {
// end of delimiter reached -> delimiter found.
return true;
}
c = is.get();
}
// end of data without finding a delimiter.
return false;
}
SeatracSerial::SeatracSerial(const IoServicePtr& ioService,
const std::string& port) :
ioService_(ioService),
......@@ -89,23 +122,6 @@ void SeatracSerial::initiate_read()
{
this->reset_serial();
boost::asio::async_read_until(serial_, readBuffer_, Delimiter,
boost::bind(&SeatracSerial::on_first_read, this, _1, _2));
}
/**
* Grab and discard a first (probably incomplete) chunk of data.
*/
void SeatracSerial::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(&SeatracSerial::read_callback, this, _1, _2));
}
......@@ -119,56 +135,41 @@ void SeatracSerial::read_callback(const boost::system::error_code& err, size_t b
std::cerr << oss.str() << std::endl;
}
else {
this->decode_received(byteCount);
std::istream is(&readBuffer_);
while(SeatracSerial::read_until(is, Delimiter, partialData_)) {
auto msg = partialData_.str();
if(msg[0] == '$') {
this->decode_received(msg);
}
else {
std::cerr << "Start of message missing. Ignoring." << std::endl;
}
// clearing partialData
partialData_.str("");
partialData_.clear();
}
}
readBuffer_.consume(byteCount);
// looping
boost::asio::async_read_until(serial_, readBuffer_, Delimiter,
boost::bind(&SeatracSerial::read_callback, this, _1, _2));
}
void SeatracSerial::decode_received(size_t byteCount)
// void SeatracSerial::decode_received(size_t byteCount)
void SeatracSerial::decode_received(const std::string& msg)
{
//std::cout << "Received " << byteCount << " bytes." << std::endl;
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;
// msg should be a complete message (starting with '$' and ending with delimiter)
if(rxDump_.is_open()) {
rxDump_ << rawData << std::flush;
rxDump_ << msg << 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) "
<< (uint32_t)start << " " << (uint32_t)'$'
<< " :\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;
}
std::istringstream is(msg);
is.get(); // discarding first character (is '$').
// conversion from pair of ascii characters to 8bit values.
decodedData_.resize((byteCount - 7) / 2);
decodedData_.resize((msg.size() - 7) / 2);
uint16_t checksum = 0;
for(auto& v : decodedData_) {
v = hexascii_to_value(is.get()) << 4;
......@@ -192,9 +193,6 @@ void SeatracSerial::decode_received(size_t byteCount)
}
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)
......@@ -235,9 +233,6 @@ void SeatracSerial::on_sent(const boost::system::error_code& err, size_t byteCou
//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
......
......@@ -2,6 +2,7 @@
list(APPEND test_names
hello_test.cpp
dump_test.cpp
decoding_test.cpp
serial_test.cpp
checksum_test.cpp
driver_test.cpp
......
#include <iostream>
using namespace std;
#include <seatrac_driver/SeatracSerial.h>
using namespace narval::seatrac;
int main()
{
std::istringstream iss("this is a test sentence");
std::ostringstream oss;
SeatracSerial::read_until(iss, "est ", oss);
//SeatracSerial::read_until(iss, "\0", oss);
//SeatracSerial::read_until(iss, "ence", oss);
cout << "Output : \"" << oss.str() << "\"" << endl;
cout << "Remaining data : \"";
char c = iss.get();
while(c != EOF) {
cout << c;
c = iss.get();
}
cout << "\"" << endl;
return 0;
}
......@@ -55,8 +55,8 @@ int main(int argc, char** argv)
//cout << command::sys_alive(seatrac) << endl;
//cout << command::sys_info(seatrac) << endl;
//cout << command::status_config_get(seatrac) << endl;
//cout << command::settings_get(seatrac) << endl;
cout << command::ping_send(seatrac, BEACON_ID_15) << endl;
cout << command::settings_get(seatrac) << endl;
//cout << command::ping_send(seatrac, BEACON_ID_15) << endl;
}
catch(const TimeoutReached& e) {
cout << "Timeout reached" << endl;
......
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