[driver] Successfull display of usbl status

include/seatrac_driver/SeatracTypes.h

@@ -38,7 +38,7 @@ struct ACOMSG_T {
                                // and definition of the PAYLOAD bytes depends
                                // on the type of payload specified in the
                                // PAYLOAD_ID parameter.
-};
+}__attribute__((packed));
 
 /**
  * ACOFIX_T : Acoustic Position and Range Fix Summary
@@ -113,7 +113,7 @@ 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
@@ -134,7 +134,7 @@ struct ACOFIXRANGE_T {
                          // 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));
 
 /**
  * USBL fields
@@ -171,7 +171,7 @@ struct ACOFIXUSBL_T {
                            // better fit, while larger values (increasing above
                            // 2.00-3.00) indicate poorer fits and larger error
                            // tolerances.
-};
+}__attribute__((packed));
 
 /**
  * Position Fields
@@ -207,7 +207,7 @@ struct ACOFIXPOSITION_T {
                               // (Enhanced) type request has been used, then
                               // this value is the remotely transmitted beacon
                               // depth sensor value.
-};
+}__attribute__((packed));
 
 /**
  * AHRSCAL_T AHRS Calibration Coefficients
@@ -281,7 +281,7 @@ struct AHRSCAL_T {
     int16_t gyroOffsetZ; // The rotational rate gyroscope Z-axis sensor offset.
                          // Valid values lie in the range -1000 to +1000.
                          // Default value of 0.
-};
+}__attribute__((packed));
 
 /**
  * FIRMWARE_T : Firmware Information
@@ -303,7 +303,7 @@ struct FIRMWARE_T {
                            // firmware (when expressed in the form Version
                            // <major>.<minor>.<build>).
     uint32_t checksum;     // The CRC32 checksum of the firmware.
-};
+}__attribute__((packed));
 
 /**
  * HARDWARE_T : Hardware Information
@@ -324,7 +324,7 @@ struct HARDWARE_T {
                            // FLAG_CMD_CSUM_DISABLE and FLAG_MAG_SENS_DISABLE
                            // are indicated in the specification but not
                            // defined...
-}; 
+}__attribute__((packed)); 
 
 /**
  * IPADDR_T : IP v4 Address
@@ -341,7 +341,7 @@ struct IPADDR_T {
         uint32_t addr;  
         uint8_t  bytes[4];
     };
-};
+}__attribute__((packed));
 
 /**
  * MACADDR_T : MAC Address
@@ -357,7 +357,7 @@ struct MACADDR_T {
         uint64_t addr;  
         uint8_t  bytes[6];
     };
-};
+}__attribute__((packed));
 
 /**
  * SETTINGS_T : Settings Record Structure
@@ -549,7 +549,7 @@ struct SETTINGS_T {
                             // limits expected by the position filter, so allow
                             // its position and restart tracking on the next
                             // fix.
-};
+}__attribute__((packed));
 
 }; //namespace seatrac
 }; //namespace narval

include/seatrac_driver/messages/Status.h

@@ -6,12 +6,6 @@
 
 namespace narval { namespace seatrac { namespace messages {
 
-struct Status : public Message
-{ 
-    STATUS_BITS_E contentType;
-    uint64_t      timestamp;
-};
-
 /**
  * Environmental Fields.
  *
@@ -44,7 +38,7 @@ struct StatusEnvironment {
                          // and salinity. Values are encoded in
                          // deci-metres-per-second, so divide by 10 to obtain a
                          // value in metres-per-second.
-};
+}__attribute__((packed));
 
 /**
  * Attitude Fields
@@ -65,7 +59,7 @@ struct StatusAttitude {
                       // magnetic north, measured by the beacons AHRS system.
                       // Values are encoded as deci-degrees, so divide the
                       // value by 10 to obtain a value in degrees.
-};
+}__attribute__((packed));
 
 /**
  * Magnetometer Calibration and Status Fields
@@ -96,7 +90,7 @@ struct StatusMagCalibration {
                         // calibration can fit the measured data to an ideal
                         // "sphere" (or perfect calibration). Values are
                         // encoded as a percentage from 0 to 100.
-};
+}__attribute__((packed));
 
 /**
  * Accelerometer Calibration Fields
@@ -125,7 +119,7 @@ struct StatusAccCalibration {
     int16_t accLimMaxZ; // Value that holds the raw accelerometer sensor value
                         // that will be used to represent +1G on the Z sensor
                         // axis.
-};
+}__attribute__((packed));
 
 /**
  * Raw AHRS Sensor Data Fields
@@ -160,7 +154,7 @@ struct StatusRawAHRS {
                           // interval (specified by the ACC_LIM_MIN_Z and
                           // ACC_LIM_MAX_Z values), gives the current
                           // gravitation acceleration seen on the sensor axis.
-    int16_t ahrsRawMagX;  // The last raw magnetometer sensor value measure on
+    int16_t ahrsRawMagX;  // The last ;raw magnetometer sensor value measure on
                           // the X-axis. This field is used during functional
                           // testing and can be used to assist with the
                           // magnetometer calibration procedure (in conjunction
@@ -184,7 +178,7 @@ struct StatusRawAHRS {
     int16_t ahrsRawGyroZ; // The last raw rate of rotation measured around the
                           // Z-axis of the gyroscope sensor. Values are encoded
                           // in degrees-per-second.
-};
+}__attribute__((packed));
 
 /**
  * Compensated AHRS Sensor Data Fields
@@ -213,7 +207,36 @@ struct StatusCompensatedAHRS {
                          // calibration coefficients have been applied.
     float ahrsCompGyroZ; // The AHRS_RAW_ GYRO _Z sensor reading after the
                          // calibration coefficients have been applied.
-};
+}__attribute__((packed));
+
+struct Status : public Message
+{ 
+    STATUS_BITS_E contentType;
+    uint64_t      timestamp;
+    
+    unsigned int expected_size() const {
+        unsigned int expectedSize = sizeof(messages::Status);
+        if(contentType & ENVIRONMENT) {
+            expectedSize += sizeof(messages::StatusEnvironment);
+        }
+        if(contentType & ATTITUDE) {
+            expectedSize += sizeof(messages::StatusAttitude);
+        }
+        if(contentType & MAG_CAL) {
+            expectedSize += sizeof(messages::StatusMagCalibration);
+        }
+        if(contentType & ACC_CAL) {
+            expectedSize += sizeof(messages::StatusAccCalibration);
+        }
+        if(contentType & AHRS_RAW_DATA) {
+            expectedSize += sizeof(messages::StatusRawAHRS);
+        }
+        if(contentType & AHRS_COMP_DATA) {
+            expectedSize += sizeof(messages::StatusCompensatedAHRS);
+        }
+        return expectedSize;
+    }
+}__attribute__((packed));
 
 }; //namespace messages
 }; //namespace seatrac
@@ -222,10 +245,11 @@ struct StatusCompensatedAHRS {
 inline std::ostream& operator<<(std::ostream& os, const narval::seatrac::messages::Status& msg)
 {
     const char* prefix = "\n- ";
-    os << "Status :"
+    os << "Status : " << sizeof(msg)
        << prefix << "cmdId       : " << msg.cmdId
        << prefix << "contentType : " << msg.contentType
        << prefix << "timestamp   : " << msg.timestamp;
+       //<< prefix << "timestamp   : " << msg.timestamp2;
     return os;
 }
 
@@ -233,7 +257,7 @@ inline std::ostream& operator<<(std::ostream& os,
                                 const narval::seatrac::messages::StatusEnvironment& msg)
 {
     const char* prefix = "\n- ";
-    os << "Environment :"
+    os << "Environment : " << sizeof(msg)
         << prefix << "envSupply   : " << msg.envSupply
         << prefix << "envTemp     : " << msg.envTemp
         << prefix << "envPressure : " << msg.envPressure

src/SeatracDriver.cpp

@@ -17,19 +17,39 @@ void SeatracDriver::on_receive(const std::vector<uint8_t>& data)
     if(header.cmdId != CID_STATUS) {
         return;
     }
+    if(header.expected_size() != data.size()) {
+        std::cerr << "Got status but wrong number of bytes (expected "
+                  << header.expected_size() << ", got " << data.size() << ")\n";
+        return;
+    }
+
     const uint8_t* p = data.data() + sizeof(messages::Status);
     if(header.contentType & ENVIRONMENT) {
+        std::cout << *reinterpret_cast<const messages::StatusEnvironment*>(p) << std::endl;
+        p += sizeof(messages::StatusEnvironment);
     }
     if(header.contentType & ATTITUDE) {
+        std::cout << *reinterpret_cast<const messages::StatusAttitude*>(p) << std::endl;
+        p += sizeof(messages::StatusAttitude);
     }
     if(header.contentType & MAG_CAL) {
+        std::cout << *reinterpret_cast<const messages::StatusMagCalibration*>(p) << std::endl;
+        p += sizeof(messages::StatusMagCalibration);
     }
     if(header.contentType & ACC_CAL) {
+        std::cout << *reinterpret_cast<const messages::StatusAccCalibration*>(p) << std::endl;
+        p += sizeof(messages::StatusAccCalibration);
     }
     if(header.contentType & AHRS_RAW_DATA) {
+        std::cout << *reinterpret_cast<const messages::StatusRawAHRS*>(p) << std::endl;
+        p += sizeof(messages::StatusRawAHRS);
     }
     if(header.contentType & AHRS_COMP_DATA) {
+        std::cout << *reinterpret_cast<const messages::StatusCompensatedAHRS*>(p) << std::endl;
+        p += sizeof(messages::StatusCompensatedAHRS);
     }
+    std::cout << "expectedSize : " << header.expected_size()
+              << ", got : " << data.size() << std::endl;
 }
 
 }; //namespace seatrac