From d87dfb3520fea7c8a6023cd7fd72aaf1edcb4eb3 Mon Sep 17 00:00:00 2001 From: Naoki Mizuno Date: Tue, 8 Aug 2017 22:02:25 +0900 Subject: [PATCH 1/3] Move scaling factor constants to class variables This change allows other methods to use the scaling factor constants. --- include/xbow440/xbow440.h | 8 ++++++++ src/xbow440.cpp | 14 +++++++------- 2 files changed, 15 insertions(+), 7 deletions(-) diff --git a/include/xbow440/xbow440.h b/include/xbow440/xbow440.h index 72615bd..b0e91b8 100644 --- a/include/xbow440/xbow440.h +++ b/include/xbow440/xbow440.h @@ -247,6 +247,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 diff --git a/src/xbow440.cpp b/src/xbow440.cpp index a5aed16..e26b3b4 100644 --- a/src/xbow440.cpp +++ b/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; @@ -186,13 +193,6 @@ void XBOW440::Resync() { void XBOW440::Parse(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) { From fa621f87c9bf2152bc0ceada2f530d7d69f3a850 Mon Sep 17 00:00:00 2001 From: Naoki Mizuno Date: Tue, 8 Aug 2017 22:03:24 +0900 Subject: [PATCH 2/3] Add parsing function for A2 packet types A2 packets are published for VG and higher models of the 440 series. --- include/xbow440/xbow440.h | 14 +++++- src/xbow440.cpp | 102 ++++++++++++++++++++++++++++++++++++-- 2 files changed, 111 insertions(+), 5 deletions(-) diff --git a/include/xbow440/xbow440.h b/include/xbow440/xbow440.h index b0e91b8..3f1366c 100644 --- a/include/xbow440/xbow440.h +++ b/include/xbow440/xbow440.h @@ -88,6 +88,10 @@ struct ImuData { 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 +210,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 +224,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. diff --git a/src/xbow440.cpp b/src/xbow440.cpp index e26b3b4..81a894f 100644 --- a/src/xbow440.cpp +++ b/src/xbow440.cpp @@ -125,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; + } } } @@ -191,7 +201,7 @@ 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) { // TODO: check CRC @@ -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_.ax = s1contents.ssdata*kGyroscopeScaleFactorA2; + + // pitch angle + s1contents.cdata[0] = data[3]; + s1contents.cdata[1] = data[2]; + imu_data_.ay = s1contents.ssdata*kGyroscopeScaleFactorA2; + + // yaw angle true + s1contents.cdata[0] = data[5]; + s1contents.cdata[1] = data[4]; + imu_data_.az = 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) From 576726d07ff771c6544a9265a0000d339fb68752 Mon Sep 17 00:00:00 2001 From: Naoki Mizuno Date: Mon, 4 Sep 2017 11:17:11 +0900 Subject: [PATCH 3/3] Add roll, pitch, yaw for A2 packets --- include/xbow440/xbow440.h | 3 +++ src/xbow440.cpp | 6 +++--- src/xbow440_node.cc | 5 +++++ 3 files changed, 11 insertions(+), 3 deletions(-) diff --git a/include/xbow440/xbow440.h b/include/xbow440/xbow440.h index 3f1366c..224c7e3 100644 --- a/include/xbow440/xbow440.h +++ b/include/xbow440/xbow440.h @@ -82,6 +82,9 @@ 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 diff --git a/src/xbow440.cpp b/src/xbow440.cpp index 81a894f..53d65c9 100644 --- a/src/xbow440.cpp +++ b/src/xbow440.cpp @@ -328,17 +328,17 @@ void XBOW440::ParseA(unsigned char *data, unsigned short packet_type) { // roll angle s1contents.cdata[0] = data[1]; s1contents.cdata[1] = data[0]; - imu_data_.ax = s1contents.ssdata*kGyroscopeScaleFactorA2; + imu_data_.roll = s1contents.ssdata*kGyroscopeScaleFactorA2; // pitch angle s1contents.cdata[0] = data[3]; s1contents.cdata[1] = data[2]; - imu_data_.ay = s1contents.ssdata*kGyroscopeScaleFactorA2; + imu_data_.pitch = s1contents.ssdata*kGyroscopeScaleFactorA2; // yaw angle true s1contents.cdata[0] = data[5]; s1contents.cdata[1] = data[4]; - imu_data_.az = s1contents.ssdata*kGyroscopeScaleFactorA2; + imu_data_.yaw = s1contents.ssdata*kGyroscopeScaleFactorA2; // x rate corrected (rad) s1contents.cdata[0] = data[7]; diff --git a/src/xbow440_node.cc b/src/xbow440_node.cc index 6a8b013..b4e17b4 100644 --- a/src/xbow440_node.cc +++ b/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); }