Skip to content
Snippets Groups Projects
Commit 7628a073 authored by Paul D'Angio's avatar Paul D'Angio
Browse files

added max data rate specification and actual data rate logging

parent b770beee
No related branches found
No related tags found
No related merge requests found
...@@ -121,6 +121,10 @@ A Raspberry Pi driver for MPU9250. Ensure that the pigpio daemon is running bef ...@@ -121,6 +121,10 @@ A Raspberry Pi driver for MPU9250. Ensure that the pigpio daemon is running bef
+/- 8g = 2 +/- 8g = 2
+/- 16g = 3 +/- 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 ## Bugs & Feature Requests
......
...@@ -91,7 +91,7 @@ void driver::deinitialize() ...@@ -91,7 +91,7 @@ void driver::deinitialize()
deinitialize_i2c(); 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. // Read the current configuration for gyro/temp.
unsigned char gyro_configuration = read_mpu9250_register(register_mpu9250_type::CONFIG); 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 ...@@ -214,11 +214,17 @@ void driver::p_dlpf_frequencies(gyro_dlpf_frequency_type gyro_frequency, accel_d
} }
// Calculate frequency divider. // 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. // 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) // 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) void driver::p_gyro_fsr(gyro_fsr_type fsr)
{ {
......
...@@ -101,9 +101,11 @@ public: ...@@ -101,9 +101,11 @@ public:
/// \brief p_dlpf_frequencies Sets the digital low-pass filter (DLPF) cutoff frequencies for the accelerometers and gyroscopes. /// \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 gyro_frequency The cut-off frequency for the gyroscopes and temperature sensor.
/// \param accel_frequency The cut-off frequency for the accelerometers. /// \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. /// \brief p_gyro_fsr Sets the full scale range (FSR) of the gyroscopes.
/// \param fsr The FSR to set. /// \param fsr The FSR to set.
......
...@@ -20,20 +20,15 @@ ros_node::ros_node(driver *driver, int argc, char **argv) ...@@ -20,20 +20,15 @@ ros_node::ros_node(driver *driver, int argc, char **argv)
// Read parameters. // Read parameters.
ros::NodeHandle private_node("~"); ros::NodeHandle private_node("~");
int param_i2c_bus; int param_i2c_bus = private_node.param<int>("i2c_bus", 1);
private_node.param<int>("i2c_bus", param_i2c_bus, 1); int param_i2c_address = private_node.param<int>("i2c_address", 0x68);
int param_i2c_address; int param_interrupt_pin = private_node.param<int>("interrupt_gpio_pin", 0);
private_node.param<int>("i2c_address", param_i2c_address, 0x68); int param_gyro_dlpf_frequency = private_node.param<int>("gyro_dlpf_frequency", 0);
int param_interrupt_pin; int param_accel_dlpf_frequency = private_node.param<int>("accel_dlpf_frequency", 0);
private_node.param<int>("interrupt_gpio_pin", param_interrupt_pin, 0); int param_gyro_fsr = private_node.param<int>("gyro_fsr", 0);
int param_gyro_dlpf_frequency; int param_accel_fsr = private_node.param<int>("accel_fsr", 0);
private_node.param<int>("gyro_dlpf_frequency", param_gyro_dlpf_frequency, 0); float param_max_data_rate = private_node.param<float>("max_data_rate", 8000.0F);
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);
// Set up publishers. // Set up publishers.
ros_node::m_publisher_accelerometer = ros_node::m_node->advertise<sensor_msgs_ext::accelerometer>("imu/accelerometer", 1); 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) ...@@ -49,11 +44,12 @@ ros_node::ros_node(driver *driver, int argc, char **argv)
// Initialize driver. // 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)); 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. // 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_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_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) catch (std::exception& e)
{ {
...@@ -85,7 +81,7 @@ void ros_node::deinitialize_driver() ...@@ -85,7 +81,7 @@ void ros_node::deinitialize_driver()
try try
{ {
ros_node::m_driver->deinitialize(); ros_node::m_driver->deinitialize();
ROS_INFO_STREAM("Driver successfully deinitialized."); ROS_INFO_STREAM("driver successfully deinitialized");
} }
catch (std::exception& e) catch (std::exception& e)
{ {
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment