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

[bin] Added a simple pinger with serial and tcp-nmea dump

parent 3df1d153
......@@ -73,6 +73,7 @@ void NmeaTcpReceiver::on_connect(const boost::system::error_code& err)
void NmeaTcpReceiver::read_callback(const boost::system::error_code& err, size_t byteCount)
{
currentTime_ = TimePoint(); // timing functions called in constructor
if(err) {
if(!reconnect_) {
std::ostringstream oss;
......@@ -88,6 +89,9 @@ void NmeaTcpReceiver::read_callback(const boost::system::error_code& err, size_t
while(NmeaTcpReceiver::read_until(is, NmeaEnd, partialData_)) {
auto msg = partialData_.str();
if(msg[0] == '$') {
if(rxDump_.is_open()) {
rxDump_ << currentTime_ << " " << msg << std::flush;
}
if(messageCallback_)
messageCallback_(msg);
}
......@@ -121,6 +125,27 @@ void NmeaTcpReceiver::unset_callback()
messageCallback_ = Callback();
}
void NmeaTcpReceiver::enable_io_dump(const std::string& rxFile) const
{
if(rxDump_.is_open()) {
std::cerr << "NmeaTcpReceiver::io_dump already enabled." << std::endl;
return;
}
rxDump_.open(rxFile, std::ios_base::out);
if(!rxDump_.is_open()) {
std::ostringstream oss;
oss << "NmeaTcpReceiver : could not open \""
<< rxFile << "\" for writing.";
throw std::runtime_error(oss.str());
}
}
void NmeaTcpReceiver::disable_io_dump() const
{
if(rxDump_.is_open())
rxDump_.close();
}
}; //namespace seatrac
}; //namespace narval
......@@ -8,6 +8,8 @@
#include <boost/bind.hpp>
#include <boost/function.hpp>
#include <seatrac_driver/SeatracSerial.h>
namespace narval { namespace seatrac {
class NmeaTcpReceiver
......@@ -21,6 +23,7 @@ class NmeaTcpReceiver
using EndPoint = boost::asio::ip::tcp::endpoint;
using ReadBuffer = boost::asio::streambuf;
using Callback = boost::function<void(const std::string&)>;
using TimePoint = SeatracSerial::TimePoint;
static constexpr const char NmeaStart = '$';
static constexpr const char* NmeaEnd = "\r\n";
......@@ -38,6 +41,9 @@ class NmeaTcpReceiver
std::ostringstream partialData_;
std::vector<uint8_t> decodedData_;
Callback messageCallback_;
TimePoint currentTime_;
mutable std::ofstream rxDump_;
public:
......@@ -54,6 +60,9 @@ class NmeaTcpReceiver
void set_callback(const Callback& callback);
void unset_callback();
void enable_io_dump(const std::string& rxFile="nmea.dump") const;
void disable_io_dump() const;
};
}; //namespace seatrac
......
#include <iostream>
#include <thread>
#include <csignal>
using namespace std;
#include <seatrac_driver/AsyncService.h>
#include <seatrac_driver/SeatracDriver.h>
#include <seatrac_driver/commands.h>
#include <seatrac_driver/messages/Messages.h>
#include "NmeaTcpReceiver.h"
using namespace narval::seatrac;
int main()
bool running;
void signal_handler(int sig)
{
running = false;
}
int main(int argc, char** argv)
{
auto service = std::make_shared<NmeaTcpReceiver::IoService>();
NmeaTcpReceiver receiver(service, "127.0.0.1", 19008);
service->run();
const char* device = "/dev/narval_usbl";
if(argc > 1) device = argv[1];
uint8_t targetId = 14;
AsyncService nmeaService;
NmeaTcpReceiver receiver(nmeaService.io_service(), "127.0.0.1", 19008);
receiver.enable_io_dump();
AsyncService seatracService;
SeatracDriver seatrac(seatracService.io_service(), device);
seatrac.enable_io_dump();
nmeaService.start();
seatracService.start();
getchar();
running = true;
signal(SIGINT, signal_handler);
while(running) {
try {
if(command::xcvr_status(seatrac).statusCode != CST_XCVR_STATE_IDLE) {
// acoustic channel not idle
std::this_thread::sleep_for(100ms);
continue;
}
auto result = command::ping_send(seatrac, (BID_E)targetId, MSG_REQX);
if(result.statusCode != CST_OK) {
std::cerr << "Ping not sent :\n" << result << std::endl;
continue;
}
std::this_thread::sleep_for(100ms);
}
catch(const TimeoutReached&) {
std::cerr << "Timeout reached while waiting for seatrac feedback." << std::endl;
}
std::this_thread::sleep_for(100ms);
}
service->stop();
nmeaService.stop();
seatracService.stop();
return 0;
}
......@@ -98,11 +98,23 @@ class SeatracSerial
void enable_io_dump(const std::string& rxFile="serial_rx.dump",
const std::string& txFile="serial_tx.dump") const;
void disable_io_dump() const;
TimePoint current_time() const;
};
}; //namespace seatrac
}; //namespace narval
inline std::ostream& operator<<(std::ostream& os, narval::seatrac::SeatracSerial::TimePoint& t)
{
os << std::chrono::duration_cast<std::chrono::milliseconds>(
t.systemTime.time_since_epoch()).count()
<< " "
<< std::chrono::duration_cast<std::chrono::milliseconds>(
t.steadyTime.time_since_epoch()).count();
return os;
}
#endif //_DEF_SEATRAC_DRIVER_SEATRAC_SERIAL_H_
......@@ -51,8 +51,8 @@ void SeatracDriver::on_receive(const std::vector<uint8_t>& data)
std::cout << response << std::endl;
}
break;
case CID_STATUS:
break;
//case CID_STATUS:
// break;
}
}
......
......@@ -163,7 +163,7 @@ void SeatracSerial::decode_received(const std::string& msg)
{
// msg should be a complete message (starting with '$' and ending with delimiter)
if(rxDump_.is_open()) {
rxDump_ << msg << std::flush;
rxDump_ << currentTime_ << " " << msg << std::flush;
}
std::istringstream is(msg);
......@@ -219,7 +219,7 @@ void SeatracSerial::send(size_t size, const uint8_t* data)
oss << "\r\n";
if(txDump_.is_open()) {
txDump_ << oss.str() << std::flush;
txDump_ << currentTime_ << " " << oss.str() << std::flush;
}
boost::asio::async_write(serial_, boost::asio::buffer(oss.str()),
......@@ -270,5 +270,10 @@ void SeatracSerial::disable_io_dump() const
txDump_.close();
}
SeatracSerial::TimePoint SeatracSerial::current_time() const
{
return currentTime_;
}
}; //namespace seatrac
}; //namespace narval
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