Add A2 packets support

include/xbow440/xbow440.h

@@ -82,12 +82,19 @@ struct ImuData {
     double rollrate; //1:rad/s 2:rad
     double pitchrate; //1:rad/s 2:rad
     double yawrate; //1:rad/s 2:rad
+    double roll; //rad
+    double pitch; //rad
+    double yaw; //rad
     double xtemp; //degC
     double ytemp; //degC
     double ztemp; //degC
     double boardtemp; //degC
     unsigned short counter; //packets
     unsigned short bitstatus;
+
+    double rollangle; // rad (only VG and higher)
+    double pitchangle; // rad (only VG and higher)
+    double yawangle; // rad (only VG and higher)
 };
 
 
@@ -206,7 +213,7 @@ class XBOW440{
     * serial port.  When a complete packet is received, the parse 
     * method is called to process the data
     * 
-    * @see xbow440::XBOW440::Parse, xbow440::XBOW440::StartReading, xbow440::XBOW440::StopReading
+    * @see xbow440::XBOW440::ParseS, xbow440::XBOW440::StartReading, xbow440::XBOW440::StopReading
     */    
     void ReadSerialPort();
 
@@ -220,7 +227,13 @@ class XBOW440{
     * Parses a packet of data from the IMU.  Scale factors are 
     * also applied to the data to convert into engineering units.
     */
-    void Parse(unsigned char *data, unsigned short packet_type);
+    void ParseS(unsigned char *data, unsigned short packet_type);
+   /*!
+    * Parses an 'A' packet of data from the IMU.  Scale factors are 
+    * also applied to the data to convert into engineering units.
+    * 'A2' are only sent by the VG, AHRS, NAV series.
+    */
+    void ParseA(unsigned char *data, unsigned short packet_type);
    /*!
     * Calculated a cyclic redundancy check (CRC) on the received
     * data to check for errors in data transmission.
@@ -247,6 +260,14 @@ class XBOW440{
     bool reading_status_;  //!< True if the read thread is running, false otherwise.
     DataCallback data_handler_; //!< Function pointer to callback function for parsed data
     GetTimeCallback time_handler_; //!< Function pointer to callback function for timestamping
+
+    // scale factors for converting raw data
+    static const double kAccelerometerScaleFactorS1; // scale for m/s^2
+    static const double kGyroscopeScaleFactorS1; // scale for rad/s
+    static const double kTemperatureScaleFactorS1;
+    static const double kAccelerometerScaleFactorS2;
+    static const double kGyroscopeScaleFactorS2;
+    static const double kGyroscopeScaleFactorA2;
 };
 
 }; // end namespace

src/xbow440.cpp

@@ -5,6 +5,13 @@ using namespace xbow440;
 #define WIN32_LEAN_AND_MEAN 
 #include "boost/date_time/posix_time/posix_time.hpp"
 
+const double XBOW440::kAccelerometerScaleFactorS1 = 0.002992752075195; // scale for m/s^2
+const double XBOW440::kGyroscopeScaleFactorS1 = 0.000335558297349984; // scale for rad/s
+const double XBOW440::kTemperatureScaleFactorS1 = 0.0030517578125;
+const double XBOW440::kAccelerometerScaleFactorS2 = 0.00000004656612873077393;
+const double XBOW440::kGyroscopeScaleFactorS2 = 0.000000005120213277435059;
+const double XBOW440::kGyroscopeScaleFactorA2 = 9.58737992428526e-5;
+
 void DefaultProcessData(const ImuData& data) {
     std::cout << "IMU440 Packet:" << std::endl;
     std::cout << " Timestamp: " << data.receive_time;
@@ -118,8 +125,18 @@ void XBOW440::ReadSerialPort() {
 			continue;
 		}
 
-		// parse packet
-		Parse(buffer+5, buffer[3]);
+		switch (buffer[2]) {
+			case 'S':
+				// parse packet
+				ParseS(buffer+5, buffer[3]);
+				break;
+			case 'A':
+				ParseA(buffer+5, buffer[3]);
+				break;
+			default:
+				std::cout << "Unsupported packet type." << std::endl;
+				break;
+		}
 	}
 
 }
@@ -184,15 +201,8 @@ void XBOW440::Resync() {
 	serial_port_->write(msgToSend,7);
 } */
 
-void XBOW440::Parse(unsigned char *data, unsigned short packet_type) {
+void XBOW440::ParseS(unsigned char *data, unsigned short packet_type) {
 
-	// scale factors for converting raw data
-	static const double kAccelerometerScaleFactorS1 = 0.002992752075195; // scale for m/s^2
-	static const double kGyroscopeScaleFactorS1 = 0.000335558297349984; // scale for rad/s
-	static const double kTemperatureScaleFactorS1 = 0.0030517578125;
-	static const double kAccelerometerScaleFactorS2 = 0.00000004656612873077393;
-	static const double kGyroscopeScaleFactorS2 = 0.000000005120213277435059;
-
 	// TODO: check CRC
 
     switch (packet_type) {
@@ -312,6 +322,92 @@ void XBOW440::Parse(unsigned char *data, unsigned short packet_type) {
 
 }
 
+void XBOW440::ParseA(unsigned char *data, unsigned short packet_type) {
+	switch (packet_type) {
+	case 0x32: //A2 Payload
+		// roll angle
+		s1contents.cdata[0] = data[1];
+		s1contents.cdata[1] = data[0];
+		imu_data_.roll = s1contents.ssdata*kGyroscopeScaleFactorA2;
+
+		// pitch angle
+		s1contents.cdata[0] = data[3];
+		s1contents.cdata[1] = data[2];
+		imu_data_.pitch = s1contents.ssdata*kGyroscopeScaleFactorA2;
+
+		// yaw angle true
+		s1contents.cdata[0] = data[5];
+		s1contents.cdata[1] = data[4];
+		imu_data_.yaw = s1contents.ssdata*kGyroscopeScaleFactorA2;
+
+		// x rate corrected (rad)
+		s1contents.cdata[0] = data[7];
+		s1contents.cdata[1] = data[6];
+		imu_data_.rollrate = s1contents.ssdata*kGyroscopeScaleFactorS1;
+
+		// y rate corrected (rad)
+		s1contents.cdata[0] = data[9];
+		s1contents.cdata[1] = data[8];
+		imu_data_.pitchrate = s1contents.ssdata*kGyroscopeScaleFactorS1;
+
+		// z rate corrected (rad)
+		s1contents.cdata[0] = data[11];
+		s1contents.cdata[1] = data[10];
+		imu_data_.yawrate = s1contents.ssdata*kGyroscopeScaleFactorS1;
+
+		// x accel (m/s)
+		s1contents.cdata[0] = data[13];
+		s1contents.cdata[1] = data[12];
+		imu_data_.ax = s1contents.ssdata*kAccelerometerScaleFactorS1;
+
+		// y accel (m/s)
+		s1contents.cdata[0] = data[15];
+		s1contents.cdata[1] = data[14];
+		imu_data_.ay = s1contents.ssdata*kAccelerometerScaleFactorS1;
+
+		// z accel (m/s)
+		s1contents.cdata[0] = data[17];
+		s1contents.cdata[1] = data[16];
+		imu_data_.az = s1contents.ssdata*kAccelerometerScaleFactorS1;
+
+		// x rate temp (degC)
+		s1contents.cdata[0] = data[19];
+		s1contents.cdata[1] = data[18];
+		imu_data_.xtemp = s1contents.ssdata*kTemperatureScaleFactorS1;
+
+		// y rate temp (degC)
+		s1contents.cdata[0] = data[21];
+		s1contents.cdata[1] = data[20];
+		imu_data_.ytemp = s1contents.ssdata*kTemperatureScaleFactorS1;
+
+		// z rate temp (degC)
+		s1contents.cdata[0] = data[23];
+		s1contents.cdata[1] = data[22];
+		imu_data_.ztemp = s1contents.ssdata*kTemperatureScaleFactorS1;
+
+		// time ITOW (ms)
+		s2contents.cdata[0] = data[27];
+		s2contents.cdata[1] = data[26];
+		s2contents.cdata[2] = data[25];
+		s2contents.cdata[3] = data[24];
+		imu_data_.counter = s2contents.usdata;
+
+		// bit status
+		s1contents.cdata[0] = data[29];
+		s1contents.cdata[1] = data[28];
+		imu_data_.bitstatus = s1contents.usdata;
+
+		break;
+	default:
+		std::cout << "Unsupported packet type." << std::endl;
+		break;
+	}
+
+	// call callback with data
+	if (data_handler_!=NULL)
+		data_handler_(imu_data_);
+}
+
 /*bool XBOW440::SetOutputRate(unsigned short rate) {
 
 	if (serialPort==NULL)

src/xbow440_node.cc

@@ -1,5 +1,7 @@
 #include "ros/ros.h"
 #include "sensor_msgs/Imu.h"
+#include "tf2/LinearMath/Quaternion.h"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
 
 #include "xbow440/xbow440.h"
 using namespace xbow440;
@@ -18,6 +20,9 @@ void PublishImuData(const ImuData& data) {
     msg.linear_acceleration.x = data.ax;
     msg.linear_acceleration.y = data.ay;
     msg.linear_acceleration.z = data.az;
+    tf2::Quaternion q;
+    q.setRPY(data.roll, data.pitch, data.yaw);
+    msg.orientation = tf2::toMsg(q);
     imu_pub.publish(msg);
 }