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

[driver][WIP] Untested send command to usbl

parent 9a0d51cf
......@@ -7,7 +7,7 @@
namespace narval { namespace seatrac {
class SeatracDriver : SeatracSerial
class SeatracDriver : public SeatracSerial
{
public:
......
......@@ -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
......
#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_
......@@ -8,7 +8,7 @@ namespace narval { namespace seatrac { namespace messages {
struct Message
{
CID_E cmdId;
};
}__attribute__((packed));
}; //namespace messages
}; //namespace seatrac
......
......@@ -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_
......
#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_
......@@ -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;
}
}
......
......@@ -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) {
......
......@@ -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
......@@ -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
......
......@@ -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;
......
......@@ -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;
......
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