From 90c707e7e23bfc0199aefde26d2a769a2e267a1a Mon Sep 17 00:00:00 2001 From: Jose Colmenares Date: Fri, 25 Sep 2015 15:39:14 +0200 Subject: [PATCH 1/8] added a private FilterData to the IMU class + small modifications to the publish imu call so that the imu message now DOES have the orientation --- src/imu.cpp | 7 ++++--- src/imu.hpp | 6 +++--- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/src/imu.cpp b/src/imu.cpp index 7a3b0cb..f664b57 100644 --- a/src/imu.cpp +++ b/src/imu.cpp @@ -880,7 +880,7 @@ void Imu::enableFilterStream(bool enabled) { } void -Imu::setIMUDataCallback(const std::function &cb) { +Imu::setIMUDataCallback(const std::function &cb) { imuDataCallback_ = cb; } @@ -1036,7 +1036,7 @@ int Imu::handleRead(size_t bytes_transferred) { void Imu::processPacket() { IMUData data; - FilterData filterData; + // FilterData filterData; PacketDecoder decoder(packet_); if (packet_.isIMUData()) { @@ -1068,7 +1068,7 @@ void Imu::processPacket() { } if (imuDataCallback_) { - imuDataCallback_(data); + imuDataCallback_(data,filterData); } } else if (packet_.isFilterData()) { for (int d; (d = decoder.fieldDescriptor()) > 0; decoder.advance()) { @@ -1077,6 +1077,7 @@ void Imu::processPacket() { decoder.extract(4, &filterData.quaternion[0]); decoder.extract(1, &filterData.quaternionStatus); filterData.fields |= FilterData::Quaternion; + // printf("%5.2f %5.2f %5.2f %5.2f \n",filterData.quaternion[0],filterData.quaternion[1],filterData.quaternion[2],filterData.quaternion[3]); break; case FIELD_GYRO_BIAS: decoder.extract(3, &filterData.bias[0]); diff --git a/src/imu.hpp b/src/imu.hpp index c69d40d..9c0ba0a 100644 --- a/src/imu.hpp +++ b/src/imu.hpp @@ -381,7 +381,7 @@ class Imu { * @brief Set the IMU data callback. * @note The IMU data callback is called every time new IMU data is read. */ - void setIMUDataCallback(const std::function &); + void setIMUDataCallback(const std::function &); /** * @brief Set the onboard filter data callback. @@ -422,11 +422,11 @@ class Imu { std::deque queue_; size_t srcIndex_, dstIndex_; - std::function + std::function imuDataCallback_; /// Called with IMU data is ready std::function filterDataCallback_; /// Called when filter data is ready - + FilterData filterData; enum { Idle = 0, Reading, } state_; Packet packet_; }; From f965d7ff05ebf77a3cf56787d04d11be0feb87bd Mon Sep 17 00:00:00 2001 From: Jose Colmenares Date: Fri, 25 Sep 2015 15:49:46 +0200 Subject: [PATCH 2/8] added a private FilterData to the IMU class + small modifications to the publish imu call so that the imu message now DOES have the orientation --- src/imu_3dm_gx4.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/src/imu_3dm_gx4.cpp b/src/imu_3dm_gx4.cpp index 4ff5ade..aab3353 100644 --- a/src/imu_3dm_gx4.cpp +++ b/src/imu_3dm_gx4.cpp @@ -30,7 +30,7 @@ std::shared_ptr updater; std::shared_ptr imuDiag; std::shared_ptr filterDiag; -void publishData(const Imu::IMUData &data) { +void publishData(const Imu::IMUData &data, const Imu::FilterData &filterData) { sensor_msgs::Imu imu; sensor_msgs::MagneticField field; sensor_msgs::FluidPressure pressure; @@ -50,8 +50,7 @@ void publishData(const Imu::IMUData &data) { pressure.header.stamp = imu.header.stamp; pressure.header.frame_id = frameId; - imu.orientation_covariance[0] = - -1; // orientation data is on a separate topic + imu.orientation_covariance[0] = -1; // orientation data is on a separate topic imu.linear_acceleration.x = data.accel[0] * kEarthGravity; imu.linear_acceleration.y = data.accel[1] * kEarthGravity; @@ -59,7 +58,10 @@ void publishData(const Imu::IMUData &data) { imu.angular_velocity.x = data.gyro[0]; imu.angular_velocity.y = data.gyro[1]; imu.angular_velocity.z = data.gyro[2]; - + imu.orientation.w = filterData.quaternion[0]; + imu.orientation.x = filterData.quaternion[1]; + imu.orientation.y = filterData.quaternion[2]; + imu.orientation.z = filterData.quaternion[3]; field.magnetic_field.x = data.mag[0]; field.magnetic_field.y = data.mag[1]; field.magnetic_field.z = data.mag[2]; @@ -173,7 +175,7 @@ int main(int argc, char **argv) { nh.param("enable_mag_update", enableMagUpdate, false); nh.param("enable_accel_update", enableAccelUpdate, true); nh.param("verbose", verbose, false); - + if (requestedFilterRate < 0 || requestedImuRate < 0) { ROS_ERROR("imu_rate and filter_rate must be > 0"); return -1; @@ -254,6 +256,7 @@ int main(int argc, char **argv) { ROS_INFO("Disabling filter data stream"); imu.enableFilterStream(false); } + imu.setIMUDataCallback(publishData); imu.setFilterDataCallback(publishFilter); @@ -268,6 +271,7 @@ int main(int argc, char **argv) { // calculate the actual rates we will get double imuRate = imuBaseRate / (1.0 * imuDecimation); + double filterRate = filterBaseRate / (1.0 * filterDecimation); imuDiag = configTopicDiagnostic("imu",&imuRate); if (enableFilter) { From 98b7d2e5f4165540752d1b8108b2a978ca2a1b11 Mon Sep 17 00:00:00 2001 From: Jose Colmenares Date: Fri, 25 Sep 2015 15:50:38 +0200 Subject: [PATCH 3/8] commiting to push to gitlab --- launch/imu.launch | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/launch/imu.launch b/launch/imu.launch index f7b6c51..8362f47 100644 --- a/launch/imu.launch +++ b/launch/imu.launch @@ -16,16 +16,16 @@ - + - + - - - + + + @@ -37,4 +37,5 @@ + From 60a41c86ba1f15d8f5e68fbd665eccc3dc13e43f Mon Sep 17 00:00:00 2001 From: Jose Colmenares Date: Wed, 30 Sep 2015 16:01:53 +0200 Subject: [PATCH 4/8] added function to set orientation --- src/imu.cpp | 22 +++++++++++++++++++++- src/imu.hpp | 11 +++++++++++ src/imu_3dm_gx4.cpp | 3 +++ 3 files changed, 35 insertions(+), 1 deletion(-) diff --git a/src/imu.cpp b/src/imu.cpp index f664b57..88d065f 100644 --- a/src/imu.cpp +++ b/src/imu.cpp @@ -41,7 +41,6 @@ extern "C" { #define COMMAND_CLASS_BASE u8(0x01) #define COMMAND_CLASS_3DM u8(0x0C) #define COMMAND_CLASS_FILTER u8(0x0D) - #define DATA_CLASS_IMU u8(0x80) #define DATA_CLASS_FILTER u8(0x82) @@ -68,6 +67,7 @@ extern "C" { #define COMMAND_SET_SOFT_IRON u8(0x3B) #define COMMAND_ENABLE_MEASUREMENTS u8(0x41) #define COMMAND_DEVICE_STATUS u8(0x64) +#define COMMAND_SET_ORIENTATION u8(0x11) // supported fields #define FIELD_QUATERNION u8(0x03) @@ -83,6 +83,11 @@ extern "C" { #define FIELD_FILTER_BASERATE u8(0x8A) #define FIELD_STATUS_REPORT u8(0x90) #define FIELD_ACK_OR_NACK u8(0xF1) +#define FIELD_SET_ORIENTATION u8(0x01) +#define FIELD_READ_ORIENTATION u8(0x02) +#define FIELD_SAVE_ORIENTATION u8(0x03) +#define FIELD_LOAD_ORIENTATION u8(0x04) +#define FIELD_RESET_ORIENTATION u8(0x05) using namespace imu_3dm_gx4; @@ -737,6 +742,21 @@ void Imu::getDiagnosticInfo(Imu::DiagnosticFields &fields) { } } +void Imu::setOrientation(const float euler[3]) { + + Imu::Packet p(COMMAND_CLASS_FILTER); + PacketEncoder encoder(p); + encoder.beginField(COMMAND_SET_ORIENTATION); + encoder.append(FUNCTION_APPLY); + for (int i=0; i < 3; i++) { + encoder.append(euler[i]); + } + encoder.endField(); + assert(p.length == 0x0F); + p.calcChecksum(); + sendCommand(p); +} + void Imu::setIMUDataRate(uint16_t decimation, const std::bitset<4>& sources) { Imu::Packet p(COMMAND_CLASS_3DM); // was 0x04 diff --git a/src/imu.hpp b/src/imu.hpp index 9c0ba0a..8c70139 100644 --- a/src/imu.hpp +++ b/src/imu.hpp @@ -278,6 +278,17 @@ class Imu { */ void selectBaudRate(unsigned int baud); + /** + * @brief Set the sensor to vehicle frame transformation matrix using Roll, + * Pitch, and Yaw Euler angles. + * @param euler Euler angles from the sensor body frame to the fixed vehicle + * frame (Roll, Pitch and Yaw). + * @note These angles define the rotation FROM the sensor body frame TO the fixed + * vehicle frame. Please reference the device Theory of Operation for more information. + * The new angles will be saved and used at the next startup + */ + void setOrientation(const float euler[3]); + /** * @brief ping Ping the device. */ diff --git a/src/imu_3dm_gx4.cpp b/src/imu_3dm_gx4.cpp index aab3353..f3a08d2 100644 --- a/src/imu_3dm_gx4.cpp +++ b/src/imu_3dm_gx4.cpp @@ -257,6 +257,9 @@ int main(int argc, char **argv) { imu.enableFilterStream(false); } + const float euler[3] = {3.14159265359,3.14159265359,0}; + imu.setOrientation(euler); + imu.setIMUDataCallback(publishData); imu.setFilterDataCallback(publishFilter); From ffcb980692a98b4cea82b86c7a0eb5711cab75c2 Mon Sep 17 00:00:00 2001 From: Jose Colmenares Date: Thu, 1 Oct 2015 19:50:54 +0200 Subject: [PATCH 5/8] added functions to tare, get and set the orientation --- src/imu.cpp | 62 ++++++++++++++++++++++++++++++++++++++------- src/imu.hpp | 22 ++++++++++++++++ src/imu_3dm_gx4.cpp | 12 +++++++-- 3 files changed, 85 insertions(+), 11 deletions(-) diff --git a/src/imu.cpp b/src/imu.cpp index 88d065f..a70f934 100644 --- a/src/imu.cpp +++ b/src/imu.cpp @@ -45,9 +45,12 @@ extern "C" { #define DATA_CLASS_FILTER u8(0x82) #define FUNCTION_APPLY u8(0x01) - +#define FUNCTION_READ u8(0x02) #define SELECTOR_IMU u8(0x01) #define SELECTOR_FILTER u8(0x03) +#define FUNCTION_SAVE u8(0x03) +#define FUNCTION_LOAD u8(0x04) +#define FUNCTION_RESET u8(0x05) // base commands #define DEVICE_PING u8(0x01) @@ -67,7 +70,6 @@ extern "C" { #define COMMAND_SET_SOFT_IRON u8(0x3B) #define COMMAND_ENABLE_MEASUREMENTS u8(0x41) #define COMMAND_DEVICE_STATUS u8(0x64) -#define COMMAND_SET_ORIENTATION u8(0x11) // supported fields #define FIELD_QUATERNION u8(0x03) @@ -83,11 +85,10 @@ extern "C" { #define FIELD_FILTER_BASERATE u8(0x8A) #define FIELD_STATUS_REPORT u8(0x90) #define FIELD_ACK_OR_NACK u8(0xF1) -#define FIELD_SET_ORIENTATION u8(0x01) -#define FIELD_READ_ORIENTATION u8(0x02) -#define FIELD_SAVE_ORIENTATION u8(0x03) -#define FIELD_LOAD_ORIENTATION u8(0x04) -#define FIELD_RESET_ORIENTATION u8(0x05) +#define FIELD_FRAME_TRANSFORM u8(0x11) +#define FIELD_GET_ORIENTATION u8(0x81) +#define FIELD_RESET_ORIENTATION u8(0x21) +#define FIELD_ALL_AXES u8(0x07) using namespace imu_3dm_gx4; @@ -746,15 +747,56 @@ void Imu::setOrientation(const float euler[3]) { Imu::Packet p(COMMAND_CLASS_FILTER); PacketEncoder encoder(p); - encoder.beginField(COMMAND_SET_ORIENTATION); + encoder.beginField(FIELD_FRAME_TRANSFORM); encoder.append(FUNCTION_APPLY); - for (int i=0; i < 3; i++) { + for (unsigned int i=0; i < 3; i++) { encoder.append(euler[i]); } encoder.endField(); assert(p.length == 0x0F); p.calcChecksum(); sendCommand(p); + + return; +} + +void Imu::getOrientation(float *euler) { + + Imu::Packet p(COMMAND_CLASS_FILTER); + PacketEncoder encoder(p); + encoder.beginField(FIELD_FRAME_TRANSFORM); + encoder.append(FUNCTION_READ); + for (unsigned int i=0; i < 3; i++) { + encoder.append(euler[i]); + } + encoder.endField(); + assert(p.length == 0x0F); + p.calcChecksum(); + sendCommand(p); + { + PacketDecoder decoder(packet_); + BOOST_VERIFY(decoder.advanceTo(FIELD_GET_ORIENTATION)); + decoder.extract(3, &euler[0]); + } + + return; +} + +void Imu::resetOrientation(const int action) { + + Imu::Packet p(COMMAND_CLASS_FILTER); + PacketEncoder encoder(p); + + encoder.beginField(FIELD_RESET_ORIENTATION); + encoder.append(u8(action)); + encoder.append(FIELD_ALL_AXES); + encoder.endField(); + assert(p.length == 0x04); + p.calcChecksum(); + sendCommand(p); + + return; + } void Imu::setIMUDataRate(uint16_t decimation, @@ -775,6 +817,7 @@ void Imu::setIMUDataRate(uint16_t decimation, fields.push_back(fieldDescs[i]); } } + encoder.beginField(COMMAND_IMU_MESSAGE_FORMAT); encoder.append(FUNCTION_APPLY, u8(fields.size())); @@ -795,6 +838,7 @@ void Imu::setFilterDataRate(uint16_t decimation, const std::bitset<4>& sources) FIELD_GYRO_BIAS, FIELD_ANGLE_UNCERTAINTY, FIELD_BIAS_UNCERTAINTY }; + assert(sizeof(fieldDescs) == sources.size()); std::vector fields; diff --git a/src/imu.hpp b/src/imu.hpp index 8c70139..8e3d845 100644 --- a/src/imu.hpp +++ b/src/imu.hpp @@ -204,6 +204,17 @@ class Imu { FilterData() : fields(0) {} }; + /** + * @brief tareOrientation Used to set the device orientation transformation + */ + struct tareOrientation { + enum { + reset = 1, + saveAsStartup = 3, + loadSaved = 4, + reset2Factory = 5, + }; + }; /* Exceptions */ /** @@ -289,6 +300,17 @@ class Imu { */ void setOrientation(const float euler[3]); + /** + * @brief Get the current Roll, Pitch, and Yaw Euler angles. + */ + void getOrientation(float *euler); + + /** + * @brief Set the current device orientation relative to the NED frame as the + * current sensor to vehicle transformation. + */ + void resetOrientation(const int action); + /** * @brief ping Ping the device. */ diff --git a/src/imu_3dm_gx4.cpp b/src/imu_3dm_gx4.cpp index f3a08d2..912f093 100644 --- a/src/imu_3dm_gx4.cpp +++ b/src/imu_3dm_gx4.cpp @@ -257,8 +257,16 @@ int main(int argc, char **argv) { imu.enableFilterStream(false); } - const float euler[3] = {3.14159265359,3.14159265359,0}; - imu.setOrientation(euler); + // float euler2[3]; + // imu.getOrientation(euler2); + // printf("%3.2f %3.2f %3.2f\n",euler2[0],euler2[1],euler2[2]); + + // const float euler[3] = {0.0f,0.0f,0.0f}; + // imu.setOrientation(euler); + + // imu.getOrientation(euler2); + // printf("%3.2f %3.2f %3.2f\n",euler2[0],euler2[1],euler2[2]); + imu.resetOrientation(Imu::tareOrientation::reset); imu.setIMUDataCallback(publishData); imu.setFilterDataCallback(publishFilter); From 84dc3fe312a38d01e44e19ce5d9dccc1d0e6e6d5 Mon Sep 17 00:00:00 2001 From: Jose Colmenares Date: Fri, 2 Oct 2015 13:13:28 +0200 Subject: [PATCH 6/8] can get angles, reset it, and put angles - commiting for testing on hyq green --- src/imu.cpp | 6 ++---- src/imu_3dm_gx4.cpp | 11 +++++------ 2 files changed, 7 insertions(+), 10 deletions(-) diff --git a/src/imu.cpp b/src/imu.cpp index a70f934..bf4b300 100644 --- a/src/imu.cpp +++ b/src/imu.cpp @@ -764,13 +764,11 @@ void Imu::getOrientation(float *euler) { Imu::Packet p(COMMAND_CLASS_FILTER); PacketEncoder encoder(p); + encoder.beginField(FIELD_FRAME_TRANSFORM); encoder.append(FUNCTION_READ); - for (unsigned int i=0; i < 3; i++) { - encoder.append(euler[i]); - } encoder.endField(); - assert(p.length == 0x0F); + // assert(p.length == 0x0F); p.calcChecksum(); sendCommand(p); { diff --git a/src/imu_3dm_gx4.cpp b/src/imu_3dm_gx4.cpp index 912f093..d907f90 100644 --- a/src/imu_3dm_gx4.cpp +++ b/src/imu_3dm_gx4.cpp @@ -256,18 +256,17 @@ int main(int argc, char **argv) { ROS_INFO("Disabling filter data stream"); imu.enableFilterStream(false); } + + // const float euler[3] = {0.0f,0.0f,0.0f}; + // imu.setOrientation(euler); - // float euler2[3]; // imu.getOrientation(euler2); // printf("%3.2f %3.2f %3.2f\n",euler2[0],euler2[1],euler2[2]); + imu.resetOrientation(Imu::tareOrientation::reset2Factory); - // const float euler[3] = {0.0f,0.0f,0.0f}; - // imu.setOrientation(euler); - + // float euler2[3] = {0.0f, 0.0f, 0.0f}; // imu.getOrientation(euler2); // printf("%3.2f %3.2f %3.2f\n",euler2[0],euler2[1],euler2[2]); - imu.resetOrientation(Imu::tareOrientation::reset); - imu.setIMUDataCallback(publishData); imu.setFilterDataCallback(publishFilter); From a827c635719874f2ec15b3697c2c64946aeae23e Mon Sep 17 00:00:00 2001 From: Michele Focchi Date: Fri, 2 Oct 2015 18:48:39 +0200 Subject: [PATCH 7/8] rotating axis is not working --- src/imu.cpp | 5 ++--- src/imu.hpp | 12 +++++++++++- src/imu_3dm_gx4.cpp | 19 +++++++++++++------ 3 files changed, 26 insertions(+), 10 deletions(-) diff --git a/src/imu.cpp b/src/imu.cpp index bf4b300..3465ef3 100644 --- a/src/imu.cpp +++ b/src/imu.cpp @@ -780,14 +780,14 @@ void Imu::getOrientation(float *euler) { return; } -void Imu::resetOrientation(const int action) { +void Imu::resetOrientation(const int action, const int axes) { Imu::Packet p(COMMAND_CLASS_FILTER); PacketEncoder encoder(p); encoder.beginField(FIELD_RESET_ORIENTATION); encoder.append(u8(action)); - encoder.append(FIELD_ALL_AXES); + encoder.append(u8(axes)); encoder.endField(); assert(p.length == 0x04); p.calcChecksum(); @@ -1139,7 +1139,6 @@ void Imu::processPacket() { decoder.extract(4, &filterData.quaternion[0]); decoder.extract(1, &filterData.quaternionStatus); filterData.fields |= FilterData::Quaternion; - // printf("%5.2f %5.2f %5.2f %5.2f \n",filterData.quaternion[0],filterData.quaternion[1],filterData.quaternion[2],filterData.quaternion[3]); break; case FIELD_GYRO_BIAS: decoder.extract(3, &filterData.bias[0]); diff --git a/src/imu.hpp b/src/imu.hpp index 8e3d845..51d4951 100644 --- a/src/imu.hpp +++ b/src/imu.hpp @@ -215,6 +215,14 @@ class Imu { reset2Factory = 5, }; }; + struct axisOrientation { + enum { + all = 7, + roll = 1, + pitch = 2, + yaw = 4, + }; + }; /* Exceptions */ /** @@ -308,8 +316,10 @@ class Imu { /** * @brief Set the current device orientation relative to the NED frame as the * current sensor to vehicle transformation. + * @param action Use the class member structure enum of type Imu::tareOrientation. + * @param axis Use the class member structure enum of type Imu::axisOrientation. */ - void resetOrientation(const int action); + void resetOrientation(const int action, const int axis); /** * @brief ping Ping the device. diff --git a/src/imu_3dm_gx4.cpp b/src/imu_3dm_gx4.cpp index d907f90..aa9fd5c 100644 --- a/src/imu_3dm_gx4.cpp +++ b/src/imu_3dm_gx4.cpp @@ -256,17 +256,24 @@ int main(int argc, char **argv) { ROS_INFO("Disabling filter data stream"); imu.enableFilterStream(false); } - - // const float euler[3] = {0.0f,0.0f,0.0f}; - // imu.setOrientation(euler); - // imu.getOrientation(euler2); - // printf("%3.2f %3.2f %3.2f\n",euler2[0],euler2[1],euler2[2]); - imu.resetOrientation(Imu::tareOrientation::reset2Factory); + + //imu.resetOrientation( Imu::tareOrientation::reset, Imu::axisOrientation::yaw ); + //const float euler[3] = {0.0f,-3.1415f,0.0f}; + //imu.setOrientation(euler); + //imu.resetOrientation( Imu::tareOrientation::reset, Imu::axisOrientation::all); + //imu.resetOrientation( Imu::tareOrientation::saveAsStartup, Imu::axisOrientation::all ); + + //float euler2[3]; + //imu.getOrientation(euler2); + //printf("%3.2f %3.2f %3.2f\n",euler2[0],euler2[1],euler2[2]); + imu.resetOrientation( Imu::tareOrientation::reset2Factory, Imu::axisOrientation::all ); + imu.resetOrientation( Imu::tareOrientation::saveAsStartup, Imu::axisOrientation::all ); // float euler2[3] = {0.0f, 0.0f, 0.0f}; // imu.getOrientation(euler2); // printf("%3.2f %3.2f %3.2f\n",euler2[0],euler2[1],euler2[2]); + imu.setIMUDataCallback(publishData); imu.setFilterDataCallback(publishFilter); From 05c21c3c606357efc211b219be93a0f1a210d510 Mon Sep 17 00:00:00 2001 From: Jose Colmenares Date: Fri, 16 Oct 2015 15:25:31 +0200 Subject: [PATCH 8/8] clean up the code a little --- src/imu_3dm_gx4.cpp | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/src/imu_3dm_gx4.cpp b/src/imu_3dm_gx4.cpp index d907f90..0e8e308 100644 --- a/src/imu_3dm_gx4.cpp +++ b/src/imu_3dm_gx4.cpp @@ -257,16 +257,6 @@ int main(int argc, char **argv) { imu.enableFilterStream(false); } - // const float euler[3] = {0.0f,0.0f,0.0f}; - // imu.setOrientation(euler); - - // imu.getOrientation(euler2); - // printf("%3.2f %3.2f %3.2f\n",euler2[0],euler2[1],euler2[2]); - imu.resetOrientation(Imu::tareOrientation::reset2Factory); - - // float euler2[3] = {0.0f, 0.0f, 0.0f}; - // imu.getOrientation(euler2); - // printf("%3.2f %3.2f %3.2f\n",euler2[0],euler2[1],euler2[2]); imu.setIMUDataCallback(publishData); imu.setFilterDataCallback(publishFilter);