Merge pull request #194 from Sandruz/add-efs-alg-message-topic

Add EfsALG msg and topic

README.md

@@ -185,6 +185,7 @@ To publish a given u-blox message to a ROS topic, set the parameter shown below
 
 ### ESF messages
 * `publish/esf/all`: This is the default value for the `publish/esf/<message>` parameters below. It defaults to `publish/all` for **ADR/UDR devices**. Individual messages can be enabled or disabled by setting the parameters below.
+* `publish/esf/alg`: Topic `~esfalg`
 * `publish/esf/ins`: Topic `~esfins`
 * `publish/esf/meas`: Topic `~esfmeas`
 * `publish/esf/raw`: Topic `~esfraw`

ublox_gps/src/node.cpp

@@ -1338,6 +1338,12 @@ void AdrUdrProduct::subscribe() {
     gps.subscribe<ublox_msgs::NavATT>(boost::bind(
         publish<ublox_msgs::NavATT>, _1, "navatt"), kSubscribeRate);
 
+  // Subscribe to ESF ALG messages
+  nh->param("publish/esf/alg", enabled["esf_alg"], enabled["esf"]);
+  if (enabled["esf_alg"])
+    gps.subscribe<ublox_msgs::EsfALG>(boost::bind(
+        publish<ublox_msgs::EsfALG>, _1, "esfalg"), kSubscribeRate);
+
   // Subscribe to ESF INS messages
   nh->param("publish/esf/ins", enabled["esf_ins"], enabled["esf"]);
   if (enabled["esf_ins"])

ublox_msgs/include/ublox_msgs/ublox_msgs.h

@@ -99,6 +99,7 @@
 #include <ublox_msgs/AidEPH.h>
 #include <ublox_msgs/AidHUI.h>
 
+#include <ublox_msgs/EsfALG.h>
 #include <ublox_msgs/EsfINS.h>
 #include <ublox_msgs/EsfMEAS.h>
 #include <ublox_msgs/EsfRAW.h>
@@ -239,6 +240,7 @@ namespace Message {
   }
 
   namespace ESF {
+    static const uint8_t ALG = EsfALG::MESSAGE_ID;
     static const uint8_t INS = EsfINS::MESSAGE_ID;
     static const uint8_t MEAS = EsfMEAS::MESSAGE_ID;
     static const uint8_t RAW = EsfRAW::MESSAGE_ID;

ublox_msgs/msg/EsfALG.msg

@@ -0,0 +1,31 @@
+# ESF-ALG (0x10 0x14)
+# IMU alignment information
+#
+# This message outputs the IMU alignment angles which define the rotation from the installation-frame to the
+# IMU-frame. In addition, it indicates the automatic IMU-mount alignment status.
+#
+
+uint8 CLASS_ID = 16
+uint8 MESSAGE_ID = 20
+
+uint8 FLAGS_AUTO_MNT_ALG_ON = 0
+uint32 FLAGS_STATUS = 14
+
+uint8 FLAGS_STATUS_USER_FIXED_ANGLES_USED = 0
+uint8 FLAGS_STATUS_ROLL_PITCH_ANGLES_ALIGNEMENT_ONGOING = 1
+uint8 FLAGS_STATUS_ROLL_PITCH_YAW_ANGLES_ALIGNEMENT_ONGOING = 2
+uint8 FLAGS_STATUS_COARSE_ALIGNMENT_USED = 3
+uint8 FLAGS_STATUS_FINE_ALIGNEMENT_USED = 4
+
+uint8 ERROR_TILT_ARG_ERROR = 1
+uint8 ERROR_YAW_ARG_ERROR = 2
+uint8 ERROR_ANGLE_ERROR = 3
+
+uint32 iTOW
+uint8 version
+uint8 flags
+uint8 errors
+uint8 reserved0
+uint32 yaw # IMU-mount yaw angle [0, 360]
+int16 pitch # IMU-mount pitch angle [-90, 90]
+int16 roll # IMU-mount roll angle [-180, 180]

ublox_msgs/src/ublox_msgs.cpp

@@ -182,6 +182,8 @@ DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::AID, ublox_msgs::Message::AID::EPH,
 DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::AID, ublox_msgs::Message::AID::HUI, 
                       ublox_msgs, AidHUI);
 
+DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ESF, ublox_msgs::Message::ESF::ALG, 
+                      ublox_msgs, EsfALG);
 DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ESF, ublox_msgs::Message::ESF::INS,
                       ublox_msgs, EsfINS);
 DECLARE_UBLOX_MESSAGE(ublox_msgs::Class::ESF, ublox_msgs::Message::ESF::MEAS,