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 @@
+
diff --git a/src/imu.cpp b/src/imu.cpp
index 7a3b0cb..3465ef3 100644
--- a/src/imu.cpp
+++ b/src/imu.cpp
@@ -41,14 +41,16 @@ 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)
#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)
@@ -83,6 +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_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;
@@ -737,6 +743,60 @@ void Imu::getDiagnosticInfo(Imu::DiagnosticFields &fields) {
}
}
+void Imu::setOrientation(const float euler[3]) {
+
+ Imu::Packet p(COMMAND_CLASS_FILTER);
+ PacketEncoder encoder(p);
+ encoder.beginField(FIELD_FRAME_TRANSFORM);
+ encoder.append(FUNCTION_APPLY);
+ 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);
+ 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, const int axes) {
+
+ Imu::Packet p(COMMAND_CLASS_FILTER);
+ PacketEncoder encoder(p);
+
+ encoder.beginField(FIELD_RESET_ORIENTATION);
+ encoder.append(u8(action));
+ encoder.append(u8(axes));
+ encoder.endField();
+ assert(p.length == 0x04);
+ p.calcChecksum();
+ sendCommand(p);
+
+ return;
+
+}
+
void Imu::setIMUDataRate(uint16_t decimation,
const std::bitset<4>& sources) {
Imu::Packet p(COMMAND_CLASS_3DM); // was 0x04
@@ -755,6 +815,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()));
@@ -775,6 +836,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;
@@ -880,7 +942,7 @@ void Imu::enableFilterStream(bool enabled) {
}
void
-Imu::setIMUDataCallback(const std::function &cb) {
+Imu::setIMUDataCallback(const std::function &cb) {
imuDataCallback_ = cb;
}
@@ -1036,7 +1098,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 +1130,7 @@ void Imu::processPacket() {
}
if (imuDataCallback_) {
- imuDataCallback_(data);
+ imuDataCallback_(data,filterData);
}
} else if (packet_.isFilterData()) {
for (int d; (d = decoder.fieldDescriptor()) > 0; decoder.advance()) {
diff --git a/src/imu.hpp b/src/imu.hpp
index c69d40d..51d4951 100644
--- a/src/imu.hpp
+++ b/src/imu.hpp
@@ -204,6 +204,25 @@ 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,
+ };
+ };
+ struct axisOrientation {
+ enum {
+ all = 7,
+ roll = 1,
+ pitch = 2,
+ yaw = 4,
+ };
+ };
/* Exceptions */
/**
@@ -278,6 +297,30 @@ 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 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.
+ * @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, const int axis);
+
/**
* @brief ping Ping the device.
*/
@@ -381,7 +424,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 +465,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_;
};
diff --git a/src/imu_3dm_gx4.cpp b/src/imu_3dm_gx4.cpp
index 4ff5ade..d885e0a 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) {