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

[messages] Updated Status message parsing (might be defective, to be checked)

parent 447d0974
......@@ -4,7 +4,8 @@
#include <seatrac_driver/messages/MessageBase.h>
#include <seatrac_driver/messages/SysAlive.h>
#include <seatrac_driver/messages/SysInfo.h>
// #include <seatrac_driver/messages/Status.h>
#include <seatrac_driver/messages/Status.h>
#endif //_DEF_SEATRAC_DRIVER_MESSAGES_MESSAGES_H_
......
......@@ -209,13 +209,75 @@ struct StatusCompensatedAHRS {
// calibration coefficients have been applied.
}__attribute__((packed));
struct Status : public Message
struct Status : public Message<Status>
{
STATUS_BITS_E contentType;
uint64_t timestamp;
STATUS_BITS_E contentType;
uint64_t timestamp;
StatusEnvironment environment;
StatusAttitude attitude;
StatusMagCalibration magCalibration;
StatusAccCalibration accCalibration;
StatusRawAHRS rawAHRS;
StatusCompensatedAHRS compensatedAHRS;
Status& operator=(const std::vector<uint8_t>& data) {
const uint8_t* p = data.data();
// Validating CID_E
if(p[0] != CID_STATUS) {
std::ostringstream oss;
oss << "Invalid CID_E for Status message (expected "
<< CID_STATUS << ", got " << p[0] << ").";
throw std::runtime_error(oss.str());
}
this->cmdId = CID_STATUS;
p++;
// Copying fixed status content
contentType = *reinterpret_cast<const STATUS_BITS_E*>(p);
p += sizeof(STATUS_BITS_E);
timestamp = *reinterpret_cast<const uint64_t*>(p);
p += sizeof(uint64_t);
// Now checking contentType to copy data chunks if present.
if(this->expected_size() != data.size()) {
std::ostringstream oss;
oss << "Got Status message but wrong number of bytes (expected " <<
this->expected_size() << ", got " << data.size() << ")\n";
throw std::runtime_error(oss.str());
}
if(contentType & ENVIRONMENT) {
environment = *reinterpret_cast<const messages::StatusEnvironment*>(p);
p += sizeof(messages::StatusEnvironment);
}
if(contentType & ATTITUDE) {
attitude = *reinterpret_cast<const messages::StatusAttitude*>(p);
p += sizeof(messages::StatusAttitude);
}
if(contentType & MAG_CAL) {
magCalibration = *reinterpret_cast<const messages::StatusMagCalibration*>(p);
p += sizeof(messages::StatusMagCalibration);
}
if(contentType & ACC_CAL) {
accCalibration = *reinterpret_cast<const messages::StatusAccCalibration*>(p);
p += sizeof(messages::StatusAccCalibration);
}
if(contentType & AHRS_RAW_DATA) {
rawAHRS = *reinterpret_cast<const messages::StatusRawAHRS*>(p);
p += sizeof(messages::StatusRawAHRS);
}
if(contentType & AHRS_COMP_DATA) {
compensatedAHRS = *reinterpret_cast<const messages::StatusCompensatedAHRS*>(p);
p += sizeof(messages::StatusCompensatedAHRS);
}
return *this;
}
unsigned int expected_size() const {
unsigned int expectedSize = sizeof(messages::Status);
unsigned int expectedSize = sizeof(CID_E) + sizeof(contentType) + sizeof(timestamp);
if(contentType & ENVIRONMENT) {
expectedSize += sizeof(messages::StatusEnvironment);
}
......@@ -242,17 +304,6 @@ struct Status : public Message
}; //namespace seatrac
}; //namespace narval
inline std::ostream& operator<<(std::ostream& os, const narval::seatrac::messages::Status& msg)
{
const char* prefix = "\n- ";
os << "Status : " << sizeof(msg)
<< prefix << "cmdId : " << msg.cmdId
<< prefix << "contentType : " << msg.contentType
<< prefix << "timestamp : " << msg.timestamp;
//<< prefix << "timestamp : " << msg.timestamp2;
return os;
}
inline std::ostream& operator<<(std::ostream& os,
const narval::seatrac::messages::StatusEnvironment& msg)
{
......@@ -337,4 +388,59 @@ inline std::ostream& operator<<(std::ostream& os,
return os;
};
inline std::ostream& operator<<(std::ostream& os, const narval::seatrac::messages::Status& msg)
{
const char* prefix = "\n- ";
os << "Status : " << sizeof(msg)
<< prefix << "cmdId : " << msg.cmdId
<< prefix << "contentType : " << msg.contentType
<< prefix << "timestamp : " << msg.timestamp;
//<< prefix << "timestamp : " << msg.timestamp2;
if(msg.contentType & narval::seatrac::ENVIRONMENT) {
os << prefix;
narval::seatrac::print_utils::indent(msg.environment, os);
}
else {
os << prefix << "Environment : Empty";
}
if(msg.contentType & narval::seatrac::ATTITUDE) {
os << prefix;
narval::seatrac::print_utils::indent(msg.attitude, os);
}
else {
os << prefix << "Attitude : Empty";
}
if(msg.contentType & narval::seatrac::MAG_CAL) {
os << prefix;
narval::seatrac::print_utils::indent(msg.magCalibration, os);
}
else {
os << prefix << "MagCalibration : Empty";
}
if(msg.contentType & narval::seatrac::ACC_CAL) {
os << prefix;
narval::seatrac::print_utils::indent(msg.accCalibration, os);
}
else {
os << prefix << "AccCalibration : Empty";
}
if(msg.contentType & narval::seatrac::AHRS_RAW_DATA) {
os << prefix;
narval::seatrac::print_utils::indent(msg.rawAHRS, os);
}
else {
os << prefix << "RawAHRS : Empty";
}
if(msg.contentType & narval::seatrac::AHRS_COMP_DATA) {
os << prefix;
narval::seatrac::print_utils::indent(msg.compensatedAHRS, os);
}
else {
os << prefix << "CompensatedAHRS : Empty";
}
return os;
}
#endif //_DEF_SEATRAC_DRIVER_MESSAGES_STATUS_H_
......@@ -30,6 +30,17 @@ void request_sys_info(boost::asio::steady_timer* timer,
timer->async_wait(boost::bind(&request_sys_info, timer, seatrac, _1));
}
narval::seatrac::messages::Status get_next_status(SeatracDriver& seatrac)
{
narval::seatrac::messages::Status status;
if(!seatrac.wait_for_message(CID_STATUS, &status, 1000)) {
throw TimeoutReached();
}
return status;
}
int main()
{
AsyncService service;
......@@ -41,7 +52,8 @@ int main()
service.start();
for(int i = 0; i < 10; i++) {
//for(int i = 0; i < 10; i++) {
for(int i = 0; i < 1000; i++) {
// uint8_t cid = 0x02;
// std::vector<uint8_t> respData;
// if(!seatrac.send_request(1, &cid, &respData, 1000)) {
......@@ -53,12 +65,13 @@ int main()
try {
// cout << command::sys_alive(seatrac) << endl;
cout << command::sys_info(seatrac) << endl;
// cout << command::sys_info(seatrac) << endl;
cout << get_next_status(seatrac) << endl;
}
catch(const TimeoutReached& e) {
cout << "Timeout reached" << endl;
}
getchar();
//getchar();
}
service.stop();
......
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