diff --git a/README.md b/README.md
index 1b74e27492ed7c2b44d2e8f98713a0c657fd1737..5b42f5d97c7e986fb7cc11e71e24453478cd9dc9 100644
--- a/README.md
+++ b/README.md
@@ -121,6 +121,10 @@ A Raspberry Pi driver for MPU9250. Ensure that the pigpio daemon is running bef
+/- 8g = 2
+/- 16g = 3
+* **`~/max_data_rate`** (float, default: 8000)
+
+ The maximum allowable sensor data rate, in Hz. Data rate is normally calculated as half of the accel/gyro DLPF frequency (nyquist criterion). This parameter allows maximum cap on the data rate, regardless of the DLPF frequency.
+
## Bugs & Feature Requests
diff --git a/src/driver.cpp b/src/driver.cpp
index e4a43b8169a1ba1fe9d55adb8eb27819ca7803eb..6889f085a71cb5725aee5b7f9ad9444985f32ec0 100644
--- a/src/driver.cpp
+++ b/src/driver.cpp
@@ -91,7 +91,7 @@ void driver::deinitialize()
deinitialize_i2c();
}
-void driver::p_dlpf_frequencies(gyro_dlpf_frequency_type gyro_frequency, accel_dlpf_frequency_type accel_frequency)
+float driver::p_dlpf_frequencies(gyro_dlpf_frequency_type gyro_frequency, accel_dlpf_frequency_type accel_frequency, float max_sample_rate)
{
// Read the current configuration for gyro/temp.
unsigned char gyro_configuration = read_mpu9250_register(register_mpu9250_type::CONFIG);
@@ -214,11 +214,17 @@ void driver::p_dlpf_frequencies(gyro_dlpf_frequency_type gyro_frequency, accel_d
}
// Calculate frequency divider.
+ // First, determine the desired approximate measurement frequency.
// NOTE: Temp DLPF bandwidth is always a few hertz higher than gyro, but use of 0.5 on top of 2x multiplier (nyquist) gives enough headroom.
- unsigned int frequency_divider = static_cast<unsigned int>(std::round(static_cast<float>(internal_frequency) / (static_cast<float>(dlpf_frequency) * 2.5f)));
+ float desired_frequency = std::min(static_cast<float>(dlpf_frequency) * 2.5F, max_sample_rate);
+ // Calculate a frequency divider to obtain an actual frequency nearest to the desired frequency without going over.
+ unsigned char frequency_divider = static_cast<unsigned char>(std::max(1.0F, std::ceil(static_cast<float>(internal_frequency) / desired_frequency)));
// Set the sample rate divider (formula is INTERNAL_SAMPLE_RATE / (1 + DIVIDER)
- write_mpu9250_register(register_mpu9250_type::SAMPLE_RATE_DIVIDER, static_cast<unsigned char>(frequency_divider - 1));
+ write_mpu9250_register(register_mpu9250_type::SAMPLE_RATE_DIVIDER, frequency_divider - 1);
+
+ // Return the actual sample frequency.
+ return internal_frequency / static_cast<float>(frequency_divider);
}
void driver::p_gyro_fsr(gyro_fsr_type fsr)
{
diff --git a/src/driver.h b/src/driver.h
index 1625cc9908cab8cdebc1e0c50e497e032f724eb9..11d2ac2f3c7fb4d26fba006c9021840e2bfe6e88 100644
--- a/src/driver.h
+++ b/src/driver.h
@@ -101,9 +101,11 @@ public:
/// \brief p_dlpf_frequencies Sets the digital low-pass filter (DLPF) cutoff frequencies for the accelerometers and gyroscopes.
/// \param gyro_frequency The cut-off frequency for the gyroscopes and temperature sensor.
/// \param accel_frequency The cut-off frequency for the accelerometers.
- /// \note This also sets an appropriate sample rate of approximately 2.5x the highest cutoff frequency.
+ /// \param max_sample_rate The maximum sample rate to use. Defaults to unlimited.
+ /// \returns The configured data sample rate (Hz)
+ /// \note The data rate is set to the nearest minimum value of lpf/2.5 or max_sample_rate.
///
- void p_dlpf_frequencies(gyro_dlpf_frequency_type gyro_frequency, accel_dlpf_frequency_type accel_frequency);
+ float p_dlpf_frequencies(gyro_dlpf_frequency_type gyro_frequency, accel_dlpf_frequency_type accel_frequency, float max_sample_rate = 8000.0F);
///
/// \brief p_gyro_fsr Sets the full scale range (FSR) of the gyroscopes.
/// \param fsr The FSR to set.
diff --git a/src/ros_node.cpp b/src/ros_node.cpp
index a216c43a5db21bda5876c698abbbffa440894d80..670c2c9edb318e703124190c36483d266f6b8114 100644
--- a/src/ros_node.cpp
+++ b/src/ros_node.cpp
@@ -20,20 +20,15 @@ ros_node::ros_node(driver *driver, int argc, char **argv)
// Read parameters.
ros::NodeHandle private_node("~");
- int param_i2c_bus;
- private_node.param<int>("i2c_bus", param_i2c_bus, 1);
- int param_i2c_address;
- private_node.param<int>("i2c_address", param_i2c_address, 0x68);
- int param_interrupt_pin;
- private_node.param<int>("interrupt_gpio_pin", param_interrupt_pin, 0);
- int param_gyro_dlpf_frequency;
- private_node.param<int>("gyro_dlpf_frequency", param_gyro_dlpf_frequency, 0);
- int param_accel_dlpf_frequency;
- private_node.param<int>("accel_dlpf_frequency", param_accel_dlpf_frequency, 0);
- int param_gyro_fsr;
- private_node.param<int>("gyro_fsr", param_gyro_fsr, 0);
- int param_accel_fsr;
- private_node.param<int>("accel_fsr", param_accel_fsr, 0);
+ int param_i2c_bus = private_node.param<int>("i2c_bus", 1);
+ int param_i2c_address = private_node.param<int>("i2c_address", 0x68);
+ int param_interrupt_pin = private_node.param<int>("interrupt_gpio_pin", 0);
+ int param_gyro_dlpf_frequency = private_node.param<int>("gyro_dlpf_frequency", 0);
+ int param_accel_dlpf_frequency = private_node.param<int>("accel_dlpf_frequency", 0);
+ int param_gyro_fsr = private_node.param<int>("gyro_fsr", 0);
+ int param_accel_fsr = private_node.param<int>("accel_fsr", 0);
+ float param_max_data_rate = private_node.param<float>("max_data_rate", 8000.0F);
+
// Set up publishers.
ros_node::m_publisher_accelerometer = ros_node::m_node->advertise<sensor_msgs_ext::accelerometer>("imu/accelerometer", 1);
@@ -49,11 +44,12 @@ ros_node::ros_node(driver *driver, int argc, char **argv)
// Initialize driver.
ros_node::m_driver->initialize(static_cast<unsigned int>(param_i2c_bus), static_cast<unsigned int>(param_i2c_address), static_cast<unsigned int>(param_interrupt_pin));
// Set parameters.
- ros_node::m_driver->p_dlpf_frequencies(static_cast<driver::gyro_dlpf_frequency_type>(param_gyro_dlpf_frequency), static_cast<driver::accel_dlpf_frequency_type>(param_accel_dlpf_frequency));
+ float data_rate = ros_node::m_driver->p_dlpf_frequencies(static_cast<driver::gyro_dlpf_frequency_type>(param_gyro_dlpf_frequency), static_cast<driver::accel_dlpf_frequency_type>(param_accel_dlpf_frequency), param_max_data_rate);
ros_node::m_driver->p_gyro_fsr(static_cast<driver::gyro_fsr_type>(param_gyro_fsr));
ros_node::m_driver->p_accel_fsr(static_cast<driver::accel_fsr_type>(param_accel_fsr));
- ROS_INFO_STREAM("MPU9250 driver successfully initialized on I2C bus " << param_i2c_bus << " at address 0x" << std::hex << param_i2c_address << ".");
+ ROS_INFO_STREAM("mpu9250 driver successfully initialized on i2c bus " << param_i2c_bus << " at address 0x" << std::hex << param_i2c_address);
+ ROS_INFO_STREAM("sensor data rate is " << data_rate << " hz");
}
catch (std::exception& e)
{
@@ -85,7 +81,7 @@ void ros_node::deinitialize_driver()
try
{
ros_node::m_driver->deinitialize();
- ROS_INFO_STREAM("Driver successfully deinitialized.");
+ ROS_INFO_STREAM("driver successfully deinitialized");
}
catch (std::exception& e)
{