Rotate frame

launch/imu.launch

@@ -16,16 +16,16 @@
     <arg name="baudrate" default="115200"/>
 
     <!-- Data rate in Hz -->
-    <arg name="imu_rate" default="100"/>
+    <arg name="imu_rate" default="50"/>
     <arg name="filter_rate" default="100"/>
 
     <!-- Enable/Disable the filter -->
-    <arg name="enable_filter" default="false"/>
+    <arg name="enable_filter" default="true"/>
 
     <!-- Enable/Disable filter updates -->
-    <arg name="enable_accel_update" default="false"/>
-    <arg name="enable_mag_update" default="false"/>
-
+    <arg name="enable_accel_update" default="true"/>
+    <arg name="enable_mag_update" default="true"/>
+    
     <node pkg="imu_3dm_gx4" name="$(arg imu)" type="imu_3dm_gx4" output="$(arg output)">
         <param name="device" type="string" value="$(arg device)" />
         <param name="verbose" type="bool" value="$(arg verbose)"/>
@@ -37,4 +37,5 @@
         <param name="enable_accel_update" type="bool" value="$(arg enable_accel_update)"/>
         <param name="enable_mag_update" type="bool" value="$(arg enable_mag_update)"/>
     </node>
+
 </launch>

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<uint8_t> fields;
 
@@ -880,7 +942,7 @@ void Imu::enableFilterStream(bool enabled) {
 }
 
 void
-Imu::setIMUDataCallback(const std::function<void(const Imu::IMUData &)> &cb) {
+Imu::setIMUDataCallback(const std::function<void(const Imu::IMUData &, const Imu::FilterData &)> &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()) {

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(const Imu::IMUData &)> &);
+  void setIMUDataCallback(const std::function<void(const Imu::IMUData &, const Imu::FilterData &)> &);
 
   /**
    * @brief Set the onboard filter data callback.
@@ -422,11 +465,11 @@ class Imu {
   std::deque<uint8_t> queue_;
   size_t srcIndex_, dstIndex_;
 
-  std::function<void(const Imu::IMUData &)>
+  std::function<void(const Imu::IMUData &, const Imu::FilterData &)>
   imuDataCallback_; /// Called with IMU data is ready
   std::function<void(const Imu::FilterData &)>
   filterDataCallback_; /// Called when filter data is ready
-
+  FilterData filterData;
   enum { Idle = 0, Reading, } state_;
   Packet packet_;
 };

src/imu_3dm_gx4.cpp

@@ -30,7 +30,7 @@ std::shared_ptr<diagnostic_updater::Updater> updater;
 std::shared_ptr<diagnostic_updater::TopicDiagnostic> imuDiag;
 std::shared_ptr<diagnostic_updater::TopicDiagnostic> 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,16 +50,18 @@ 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;
   imu.linear_acceleration.z = data.accel[2] * kEarthGravity;
   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<bool>("enable_mag_update", enableMagUpdate, false);
   nh.param<bool>("enable_accel_update", enableAccelUpdate, true);
   nh.param<bool>("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) {