Added paramters for IMU Filtering Configuration (Register 85)

vectornav_driver/config/vn100_params.yaml

@@ -55,6 +55,32 @@ reference_frame: [1, 0, 0,
                   0, 1, 0,
                   0, 0, 1]
 
+# IMU Filtering Configuration (Register 85)
+# This register allows the user to configure the FIR filtering what is applied to the IMU measurements. The
+# filter is a uniformly-weighted moving window (boxcar) filter of configurable size. The filtering does not affect
+# the values used by the internal filter, but only the output values
+
+# The WindowSize parameters for each sensor define the number of samples at the IMU rate (default 800Hz)
+# which will be averaged for each output measurement
+mag_window_size: 0
+accel_window_size: 4
+gyro_window_size: 4
+temp_window_size: 4
+pres_window_size: 4
+
+# The FilterMode parameters for each sensor select which output quantities the filtering should be applied to.
+# Filtering can be applied to either the uncompensated IMU measurements, compensated (HSI and biases
+# compensated by onboard filters, if applicable), or both.
+# 0 - No filtering
+# 1 - Only filter raw uncompensated measurements
+# 2 - Only filter compensated measurements
+# 3 - Filter both raw and compensated measurements
+mag_filter_mode: 0
+accel_filter_mode: 3
+gyro_filter_mode: 3
+temp_filter_mode: 3
+pres_filter_mode: 0
+
 # Write the new settings to the onboard flash
 write_to_flash: false
 

vectornav_driver/include/vectornav_driver/vectornav_driver.hpp

@@ -70,6 +70,17 @@ class VectorNavDriver
   uint32_t sync_out_pulse_width_;
   bool publish_sync_out_stamp_on_change_;
   uint32_t sync_out_count_;
+  // IMU filtering configuration
+  uint16_t mag_window_size_;
+  uint16_t accel_window_size_;
+  uint16_t gyro_window_size_;
+  uint16_t temp_window_size_;
+  uint16_t pres_window_size_;
+  vn::protocol::uart::FilterMode mag_filter_mode_;
+  vn::protocol::uart::FilterMode accel_filter_mode_;
+  vn::protocol::uart::FilterMode gyro_filter_mode_;
+  vn::protocol::uart::FilterMode temp_filter_mode_;
+  vn::protocol::uart::FilterMode pres_filter_mode_;
   std::string frame_id_;
   double linear_acceleration_stddev_;
   double angular_velocity_stddev_;

vectornav_driver/src/vectornav_driver.cpp

@@ -103,6 +103,28 @@ void VectorNavDriver::readParams(ros::NodeHandle & pnh)
   XmlRpc::XmlRpcValue reference_frame_rpc;
   pnh.getParam("reference_frame", reference_frame_rpc);
   setMatrix(reference_frame_rpc, reference_frame_);
+  pnh.param<int>("mag_window_size", i_param, 0);
+  std::cout << "mag_window_size: " << i_param << "\n";
+  mag_window_size_ = static_cast<uint16_t>(i_param);
+  std::cout << "mag_window_size_: " << mag_window_size_ << "\n";
+  pnh.param<int>("accel_window_size", i_param, 4);
+  accel_window_size_ = static_cast<uint16_t>(i_param);
+  pnh.param<int>("gyro_window_size", i_param, 4);
+  gyro_window_size_ = static_cast<uint16_t>(i_param);
+  pnh.param<int>("temp_window_size", i_param, 4);
+  temp_window_size_ = static_cast<uint16_t>(i_param);
+  pnh.param<int>("pres_window_size", i_param, 4);
+  pres_window_size_ = static_cast<uint16_t>(i_param);
+  pnh.param<int>("mag_filter_mode", i_param, 0);
+  mag_filter_mode_ = static_cast<vn::protocol::uart::FilterMode>(i_param);
+  pnh.param<int>("accel_filter_mode", i_param, 3);
+  accel_filter_mode_ = static_cast<vn::protocol::uart::FilterMode>(i_param);
+  pnh.param<int>("gyro_filter_mode", i_param, 3);
+  gyro_filter_mode_ = static_cast<vn::protocol::uart::FilterMode>(i_param);
+  pnh.param<int>("temp_filter_mode", i_param, 3);
+  temp_filter_mode_ = static_cast<vn::protocol::uart::FilterMode>(i_param);
+  pnh.param<int>("pres_filter_mode", i_param, 0);
+  pres_filter_mode_ = static_cast<vn::protocol::uart::FilterMode>(i_param);
   pnh.param<bool>("write_to_flash", write_to_flash_, false);
   pnh.param<bool>("factory_reset_before_start", factory_reset_before_start_, false);
   pnh.param<double>("linear_acceleration_stddev", linear_acceleration_stddev_, 0.0);
@@ -224,6 +246,11 @@ void VectorNavDriver::setupSensor()
   logger_->debug("Turning off data streaming");
   sensor_.writeAsyncDataOutputType(VNOFF);
 
+  sensor_.writeImuFilteringConfiguration(mag_window_size_, accel_window_size_, gyro_window_size_,
+                                         temp_window_size_, pres_window_size_, mag_filter_mode_,
+                                         accel_filter_mode_, gyro_filter_mode_, temp_filter_mode_,
+                                         pres_filter_mode_);
+
   if (set_reference_frame_) {
     logger_->debug("Writing reference frame");
     try {