Commit 92617edf authored by Pierre NARVOR's avatar Pierre NARVOR
Browse files

[types] Added decoding method to ACOFIX_T type (type size is NOT fixed...)

parent 9517f875
......@@ -2,6 +2,7 @@
#define _DEF_SEATRAC_DRIVER_SEATRAC_TYPES_H_
#include <sstream>
#include <cstring>
#include <seatrac_driver/SeatracEnums.h>
#include <seatrac_driver/print_utils.h>
......@@ -42,6 +43,150 @@ struct ACOMSG_T {
// PAYLOAD_ID parameter.
}__attribute__((packed));
/**
* Range fields
*
* If the message ACOFIX_T.flags parameter contains the RANGE_VALID bit, then
* the following fields are sequentially appended to the ACOFIX_T data.
*/
struct ACOFIXRANGE_T {
uint32_t count; // The number of 16kHz timer intervals that were
// counted between Request message transmission and
// Response message reception.
int32_t time; // The time in seconds derived from the RANGE_COUNT
// value, and with internal timing offsets and
// compensation applied. Values are encoded in 100
// nanosecond multiples, so divide by 10000000 to
// obtain a value in seconds.
uint16_t dist; // The resolved line-of-sight distance to the remote
// beacon, based on the RANGE_TIME and VOS values.
// Values are encoded in decimetres, so divide by 10
// for a value in metres.
unsigned int assign(unsigned int availableSize, const uint8_t* data)
{
if(availableSize < sizeof(ACOFIXRANGE_T)) {
std::ostringstream oss;
oss << "ACOFIXRANGE_T::assign : not enough data to decode binary";
throw std::runtime_error(oss.str());
}
std::memcpy(this, data, sizeof(ACOFIXRANGE_T));
return sizeof(ACOFIXRANGE_T);
}
}__attribute__((packed));
/**
* USBL fields
*
* If the message FLAGS parameter contains the USBL_VALID bit, then the
* following fields are sequentially appended to the ACOFIX_T data.
*/
struct ACOFIXUSBL_T {
uint8_t channelCount; // The number of USBL receiver channels being used
// to compute the signal angle. Typically this value
// is either 3 or 4.
int16_t rssi[4]; // An array of the received signal strengths for
// each of the USBL receiver channels, where "N" is
// the value defined by the CHANNELS field. Values
// are encoded in centi-Bels, so divide by 10 to
// obtain a value in decibels to a resolution of
// 0.1dB.
int16_t azimuth; // The incoming signal azimuth angle from 0deg to
// 360deg. Values are encoded as deci-Degrees, so
// divide by 10 for just degrees to a 0.1deg
// resolution.
int16_t elevation; // The incoming signal elevation angle from -90deg
// to +90deg. Values are encoded as deci-Degrees, so
// divide by 10 for just degrees to a 0.1deg
// resolution.
int16_t fitError; // The fit error value returns a number that
// indicates the quality of fit (or confidence) of
// the signal azimuth and elevation values from the
// timing and phase-angle data available.Values are
// dimensionless. Divide the value by 100 to obtain
// a signed floating-point value to a resolution of
// 0.01. Smaller values towards 0.00 indicate a
// better fit, while larger values (increasing above
// 2.00-3.00) indicate poorer fits and larger error
// tolerances.
unsigned int assign(unsigned int availableSize, const uint8_t* data)
{
const uint8_t* p = data;
if(availableSize < 7) {
std::ostringstream oss;
oss << "ACOFIXUSBL_T::assign : not enough data to decode binary.";
throw std::runtime_error(oss.str());
}
if(p[0] > 4) {
std::ostringstream oss;
oss << "ACOFIXUSBL_T::assing : Invalid number of USBL channels : "
<< (int)p[0];
throw std::runtime_error(oss.str());
}
channelCount = p[0];
p++;
if(availableSize < 7 + channelCount*sizeof(int16_t)) {
std::ostringstream oss;
oss << "ACOFIXUSBL_T::assign : not enough data to decode binary.";
throw std::runtime_error(oss.str());
}
std::memcpy(&rssi, p, channelCount*sizeof(int16_t));
p += channelCount*sizeof(int16_t);
std::memcpy(&azimuth, p, 3*sizeof(int16_t));
p += 3*sizeof(int16_t);
return std::distance(p, data);
}
}__attribute__((packed));
/**
* Position Fields
*
* If the message FLAGS parameter contains the POSITION_VALID bit, then the
* following fields are sequentially appended to the ACOFIX_T data.
*/
struct ACOFIXPOSITION_T {
int16_t easting; // The Easting distance component of the relative
// position of the remote beacon to the local
// beacon computed from the range, incoming
// signal angle, local beacon depth, attitude and
// magnetic heading. Values are encoded in
// decimetres, so divide by 10 for a value in
// metres.
int16_t northing; // The Northing distance component of the
// relative position of the remote beacon to the
// local beacon computed from the range, incoming
// signal angle, local beacon depth, attitude and
// magnetic heading. Values are encoded in
// decimetres, so divide by 10 for a value in
// metres.
int16_t depth; // The vertical Depth distance component of the
// remote beacon from the surface - computed from
// the range, incoming signal angle, local beacon
// depth, attitude and magnetic heading. Values
// are encoded in decimetres, so divide by 10 for
// a value in metres.
// NB: If the 'Fix' has been obtained by a
// MSG_REQU (Usbl) type request, the this value
// is computed from the beacon's attitude and
// incoming signal angle. If a MSG_REQX
// (Enhanced) type request has been used, then
// this value is the remotely transmitted beacon
// depth sensor value.
unsigned int assign(unsigned int availableSize, const uint8_t* data)
{
if(availableSize < sizeof(ACOFIXPOSITION_T)) {
std::ostringstream oss;
oss << "ACOFIXPOSITION_T::assign : not enough data to decode binary";
throw std::runtime_error(oss.str());
}
std::memcpy(this, data, sizeof(ACOFIXPOSITION_T));
return sizeof(ACOFIXPOSITION_T);
}
}__attribute__((packed));
/**
* ACOFIX_T : Acoustic Position and Range Fix Summary
*
......@@ -80,6 +225,7 @@ struct ACOMSG_T {
* required fields being appended to the end of the record as required.
*/
struct ACOFIX_T {
BID_E destId; // The ID code of the beacon that this data is sent
// to. Normally this is the local beacon ID code,
// but a value of BEACON_ALL indicates data has been
......@@ -115,102 +261,49 @@ struct ACOFIX_T {
// the received message, encoded in centibels. To
// decode, divide this value by 10 for decibels to a
// 0.1 dB resolution.
}__attribute__((packed));
/**
* Range fields
*
* If the message ACOFIX_T.flags parameter contains the RANGE_VALID bit, then
* the following fields are sequentially appended to the ACOFIX_T data.
*/
struct ACOFIXRANGE_T {
uint32_t rangeCount; // The number of 16kHz timer intervals that were
// counted between Request message transmission and
// Response message reception.
int32_t rangeTime; // The time in seconds derived from the RANGE_COUNT
// value, and with internal timing offsets and
// compensation applied. Values are encoded in 100
// nanosecond multiples, so divide by 10000000 to
// obtain a value in seconds.
uint16_t rangeDist; // The resolved line-of-sight distance to the remote
// beacon, based on the RANGE_TIME and VOS values.
// Values are encoded in decimetres, so divide by 10
// for a value in metres.
}__attribute__((packed));
ACOFIXRANGE_T range;
ACOFIXUSBL_T usbl;
ACOFIXPOSITION_T position;
/**
* USBL fields
*
* If the message FLAGS parameter contains the USBL_VALID bit, then the
* following fields are sequentially appended to the ACOFIX_T data.
*/
template <uint8_t N> // %$&^%&$&R???!!
struct ACOFIXUSBL_T {
uint8_t usblChannels; // The number of USBL receiver channels being used
// to compute the signal angle. Typically this value
// is either 3 or 4.
int16_t usblRssi[N]; // An array of the received signal strengths for
// each of the USBL receiver channels, where "N" is
// the value defined by the CHANNELS field. Values
// are encoded in centi-Bels, so divide by 10 to
// obtain a value in decibels to a resolution of
// 0.1dB.
int16_t usblAzimuth; // The incoming signal azimuth angle from 0deg to
// 360deg. Values are encoded as deci-Degrees, so
// divide by 10 for just degrees to a 0.1deg
// resolution.
int16_t usblElevation; // The incoming signal elevation angle from -90deg
// to +90deg. Values are encoded as deci-Degrees, so
// divide by 10 for just degrees to a 0.1deg
// resolution.
int16_t usblFitError; // The fit error value returns a number that
// indicates the quality of fit (or confidence) of
// the signal azimuth and elevation values from the
// timing and phase-angle data available.Values are
// dimensionless. Divide the value by 100 to obtain
// a signed floating-point value to a resolution of
// 0.01. Smaller values towards 0.00 indicate a
// better fit, while larger values (increasing above
// 2.00-3.00) indicate poorer fits and larger error
// tolerances.
}__attribute__((packed));
// This decodes data from a binary blob and returns how much data were used
// in the blob for further parsing.
unsigned int assign(unsigned int availableSize, const uint8_t* begin)
{
const uint8_t* p = begin;
const uint8_t* end = begin + availableSize;
// Size in byte to be copied to fill the first fields of the struct.
// UGLY...
unsigned int minSize = std::distance((const uint8_t*)this,
(const uint8_t*)&range);
if(availableSize < minSize) {
std::ostringstream oss;
oss << "ACOFIX_T::assign : not enough data to decode binary "
<< "(needs at least " << minSize << "bytes, got "
<< availableSize << "bytes).";
throw std::runtime_error(oss.str());
}
std::memcpy(this, p, minSize);
p += minSize;
if(flags & 0x1) {
p += range.assign(std::distance(p,end), p);
}
if(flags & 0x2) {
p += usbl.assign(std::distance(p,end), p);
}
if(flags & 0x4) {
p += position.assign(std::distance(p,end), p);
}
// returning how many byte where used in the decoding.
return std::distance(begin, p);
}
/**
* Position Fields
*
* If the message FLAGS parameter contains the POSITION_VALID bit, then the
* following fields are sequentially appended to the ACOFIX_T data.
*/
struct ACOFIXPOSITION_T {
int16_t positionEasting; // The Easting distance component of the relative
// position of the remote beacon to the local
// beacon computed from the range, incoming
// signal angle, local beacon depth, attitude and
// magnetic heading. Values are encoded in
// decimetres, so divide by 10 for a value in
// metres.
int16_t positionNorthing; // The Northing distance component of the
// relative position of the remote beacon to the
// local beacon computed from the range, incoming
// signal angle, local beacon depth, attitude and
// magnetic heading. Values are encoded in
// decimetres, so divide by 10 for a value in
// metres.
int16_t positionDepth; // The vertical Depth distance component of the
// remote beacon from the surface - computed from
// the range, incoming signal angle, local beacon
// depth, attitude and magnetic heading. Values
// are encoded in decimetres, so divide by 10 for
// a value in metres.
// NB: If the 'Fix' has been obtained by a
// MSG_REQU (Usbl) type request, the this value
// is computed from the beacon's attitude and
// incoming signal angle. If a MSG_REQX
// (Enhanced) type request has been used, then
// this value is the remotely transmitted beacon
// depth sensor value.
}__attribute__((packed));
/**
* AHRSCAL_T AHRS Calibration Coefficients
*
......
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