Commit 26bf0112 authored by Pierre NARVOR's avatar Pierre NARVOR
Browse files

[types] Fixed decoding of ACOFIX messages and added proper print functions

parent e6789c18
......@@ -138,7 +138,7 @@ struct ACOFIXUSBL_T {
std::memcpy(&azimuth, p, 3*sizeof(int16_t));
p += 3*sizeof(int16_t);
return std::distance(p, data);
return std::distance(data, p);
}
}__attribute__((packed));
......@@ -651,6 +651,81 @@ struct SETTINGS_T {
}; //namespace seatrac
}; //namespace narval
inline std::ostream& operator<<(std::ostream& os,
const narval::seatrac::ACOFIXRANGE_T& msg)
{
static const char* prefix = "\n- ";
os << "AcofixRange :"
<< prefix << "count : " << msg.count
<< prefix << "time : " << msg.time
<< prefix << "dist : " << msg.dist;
return os;
}
inline std::ostream& operator<<(std::ostream& os,
const narval::seatrac::ACOFIXUSBL_T& msg)
{
static const char* prefix = "\n- ";
os << "AcoFixUSBL :"
<< prefix << "channelCount : " << (int)msg.channelCount;
for(int i = 0; i < msg.channelCount; i++) {
os << prefix << " rssi[" << i << "] : " << msg.rssi[i];
}
os << prefix << "azimuth : " << msg.azimuth
<< prefix << "elevation : " << msg.elevation
<< prefix << "fitError : " << msg.fitError;
return os;
}
inline std::ostream& operator<<(std::ostream& os,
const narval::seatrac::ACOFIXPOSITION_T& msg)
{
static const char* prefix = "\n- ";
os << "AcoFixPosition :"
<< prefix << "easting : " << msg.easting
<< prefix << "northing : " << msg.northing
<< prefix << "depth : " << msg.depth;
return os;
}
inline std::ostream& operator<<(std::ostream& os,
const narval::seatrac::ACOFIX_T& msg)
{
using namespace narval::seatrac;
static const char* prefix = "\n- ";
os << "AcousticFix :"
<< prefix << "destId : " << msg.destId
<< prefix << "srcId : " << msg.srcId
<< prefix << "flags : " << (int)msg.flags
<< prefix << "msgType : " << msg.msgType
<< prefix << "attitudeYaw : " << msg.attitudeYaw
<< prefix << "attitudePitch : " << msg.attitudePitch
<< prefix << "attitudeRoll : " << msg.attitudeRoll
<< prefix << "depthLocal : " << msg.depthLocal
<< prefix << "vos : " << msg.vos
<< prefix << "rssi : " << msg.rssi;
if(msg.flags & 0x1) {
os << prefix << "range : " << print_utils::indent(msg.range);
}
else {
os << prefix << "range : No data.";
}
if(msg.flags & 0x2) {
os << prefix << "usbl : " << print_utils::indent(msg.usbl);
}
else {
os << prefix << "usbl : No data";
}
if(msg.flags & 0x4) {
os << prefix << "position : " << print_utils::indent(msg.position);
}
else {
os << prefix << "position : No data.";
}
return os;
}
inline std::ostream& operator<<(std::ostream& os,
const narval::seatrac::FIRMWARE_T& msg)
{
......
......@@ -29,9 +29,7 @@ struct PingResp : public Message<PingResp>
inline std::ostream& operator<<(std::ostream& os,
const narval::seatrac::messages::PingResp& msg)
{
const char* prefix = "\n- ";
//os << "PingResp : " << msg.acoFix;
os << "PingResp";
os << "PingResp : " << msg.acoFix;
return os;
}
......
#include <seatrac_driver/SeatracDriver.h>
#include <seatrac_driver/print_utils.h>
#include <seatrac_driver/messages/Messages.h>
namespace narval { namespace seatrac {
SeatracDriver::SeatracDriver(const IoServicePtr& ioService,
......@@ -37,7 +39,12 @@ void SeatracDriver::on_receive(const std::vector<uint8_t>& data)
std::cout << "Got a Ping Request" << std::endl << std::flush;
break;
case CID_PING_RESP:
std::cout << "Got a Ping Repsonse" << std::endl << std::flush;
// std::cout << "Got a Ping Response" << std::endl << std::flush;
{
messages::PingResp response;
response = data;
std::cout << response << std::endl;
}
break;
case CID_STATUS:
break;
......
......@@ -74,8 +74,9 @@ 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::settings_get(seatrac) << endl;
//cout << command::ping_send(seatrac, BEACON_ID_15) << endl;
cout << command::ping_send(seatrac, BEACON_ID_15, MSG_REQU) << 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