diff --git a/cfg/Bno055Imu.cfg b/cfg/Bno055Imu.cfg
index d8fb52ac510864cde06106250cd50d7e5684ad15..c52f99dabb131035e90d76e7381654f1eec7a840 100755
--- a/cfg/Bno055Imu.cfg
+++ b/cfg/Bno055Imu.cfg
@@ -53,6 +53,83 @@ gen.const("ndof_off", int_t, 11, "ndof off"),
gen.const("ndof", int_t, 12, "ndof")
], "Possible operation modes.")
+accel = gen.add_group("Accelerometer")
+accel_range = gen.enum([
+gen.const("2G", int_t, 0, "+/- 2G"),
+gen.const("4G", int_t, 1, "+/- 4G"),
+gen.const("8G", int_t, 2, "+/- 8G"),
+gen.const("16G", int_t, 3, "+/- 16G")
+], "Accelerometer ranges.")
+accel_bw = gen.enum([
+gen.const("7_81Hz", int_t, 0, "7.81Hz"),
+gen.const("15_63Hz", int_t, 4, "15.63Hz"),
+gen.const("32_25Hz", int_t, 8, "32.25Hz"),
+gen.const("62_5Hz", int_t, 12, "62.5Hz"),
+gen.const("125Hz", int_t, 16, "125Hz"),
+gen.const("250Hz", int_t, 20, "250Hz"),
+gen.const("500Hz", int_t, 24, "500Hz"),
+gen.const("1000Hz", int_t, 28, "1000Hz")
+], "Accelerometer bandwidth.")
+accel_pwr_mode = gen.enum([
+gen.const("accel_normal", int_t, 0, "Normal"),
+gen.const("accel_suspend", int_t, 32, "Suspend"),
+gen.const("accel_low_pwr1", int_t, 64, "Low power 1"),
+gen.const("accel_standby", int_t, 96, "Stand by"),
+gen.const("accel_low_pwr2", int_t, 128, "Low power 2"),
+gen.const("accel_deep_suspend", int_t, 160, "Seep suspend")
+], "Accelerometer power modes.")
+
+gyro = gen.add_group("Gyroscope")
+gyro_range = gen.enum([
+gen.const("2000dps", int_t, 0, "+/- 2000 dps"),
+gen.const("1000dps", int_t, 1, "+/- 1000 dps"),
+gen.const("500dps", int_t, 2, "+/- 500 dps"),
+gen.const("250dps", int_t, 3, "+/- 250 dps"),
+gen.const("124dps", int_t, 4, "+/- 125 dps")
+], "Gyroscope ranges.")
+gyro_bw = gen.enum([
+gen.const("523Hz", int_t, 0, "523Hz"),
+gen.const("230Hz", int_t, 8, "230Hz"),
+gen.const("116Hz", int_t, 16, "116Hz"),
+gen.const("47Hz", int_t, 24, "47Hz"),
+gen.const("23Hz", int_t, 32, "23Hz"),
+gen.const("12Hz", int_t, 40, "12Hz"),
+gen.const("64Hz", int_t, 48, "64Hz"),
+gen.const("32Hz", int_t, 56, "32Hz")
+], "Gyroscope bandwidth.")
+gyro_pwr_mode = gen.enum([
+gen.const("gyro_normal", int_t, 0, "Normal"),
+gen.const("gyro_fast_pwr_up", int_t, 1, "Fast power up"),
+gen.const("gyro_deep_suspend", int_t, 2, "Deep suspend"),
+gen.const("gyro_suspend", int_t, 3, "Suspend"),
+gen.const("gyro_adv_pwr_save", int_t, 4, "Advanced power save"),
+], "Gyroscope power modes.")
+
+mag = gen.add_group("Magnetometer")
+mag_rate = gen.enum([
+gen.const("2Hz", int_t, 0, "2 Hz"),
+gen.const("6Hz", int_t, 1, "6 Hz"),
+gen.const("8Hz", int_t, 2, "8 Hz"),
+gen.const("10Hz", int_t, 3, "10 Hz"),
+gen.const("15Hz", int_t, 4, "15 Hz"),
+gen.const("20Hz", int_t, 5, "20 Hz"),
+gen.const("25Hz", int_t, 6, "25 Hz"),
+gen.const("30Hz", int_t, 7, "30 Hz")
+], "Magnetometer rates.")
+mag_op_mode = gen.enum([
+gen.const("low_pwr", int_t, 0, "Low power"),
+gen.const("regular", int_t, 8, "Regular"),
+gen.const("en_regular", int_t, 16, "Enhanced regular"),
+gen.const("high_acc", int_t, 24, "High accuracy")
+], "Magnetometer operation moes.")
+mag_pwr_mode = gen.enum([
+gen.const("mag_normal", int_t, 0, "Normal"),
+gen.const("mag_sleep", int_t, 32, "Sleep"),
+gen.const("mag_suspend", int_t, 64, "Suspend"),
+gen.const("mag_force_mode", int_t, 96, "Force mode"),
+], "Magnetometer power modes.")
+
+
# Name Type Reconfiguration level Description Default Min Max
gen.add("update_rate", double_t, SensorLevels.RECONFIGURE_STOP, "Data update rate in Hz", 20.0, 1.0, 100.0)
gen.add("serial_device", str_t, SensorLevels.RECONFIGURE_CLOSE, "Device serial port", "/dev/ttyUSB1")
@@ -60,6 +137,15 @@ gen.add("cal_filename", str_t, SensorLevels.RECONFIGURE_STOP, "
gen.add("tf_prefix", str_t, SensorLevels.RECONFIGURE_STOP, "TF prefix", "")
gen.add("frame_id", str_t, SensorLevels.RECONFIGURE_STOP, "IMU frame_id in the urdf file", "")
gen.add("mode", int_t, SensorLevels.RECONFIGURE_STOP, "Operation mode", 12, 1, 12, edit_method=enum_operation_mode)
+accel.add("accel_range", int_t, SensorLevels.RECONFIGURE_STOP, "Accelerometer operation range", 1, 0, 3, edit_method=accel_range)
+accel.add("accel_bandwidth", int_t, SensorLevels.RECONFIGURE_STOP, "Accelerometer bandwidth", 12, 0, 28, edit_method=accel_bw)
+accel.add("accel_pwr_mode", int_t, SensorLevels.RECONFIGURE_STOP, "Accelerometer power mode", 0, 0, 160, edit_method=accel_pwr_mode)
+gyro.add("gyro_range", int_t, SensorLevels.RECONFIGURE_STOP, "Gyroscope operation range", 0, 0, 4, edit_method=gyro_range)
+gyro.add("gyro_bandwidth", int_t, SensorLevels.RECONFIGURE_STOP, "Gyroscope bandwidth", 56, 0, 56, edit_method=gyro_bw)
+gyro.add("gyro_pwr_mode", int_t, SensorLevels.RECONFIGURE_STOP, "Gyroscope power mode", 0, 0, 4, edit_method=gyro_pwr_mode)
+gyro.add("mag_rate", int_t, SensorLevels.RECONFIGURE_STOP, "Magnetometer rate", 3, 0, 7, edit_method=mag_rate)
+gyro.add("mag_op_mode", int_t, SensorLevels.RECONFIGURE_STOP, "Magnetometer operation mode", 1, 0, 24, edit_method=mag_op_mode)
+gyro.add("mag_pwr_mode", int_t, SensorLevels.RECONFIGURE_STOP, "Magnetometer power mode", 0, 0, 96, edit_method=mag_pwr_mode)
#gen.add("velocity_scale_factor", double_t, SensorLevels.RECONFIGURE_STOP, "Maximum velocity scale factor", 0.5, 0.0, 1.0)
exit(gen.generate(PACKAGE, "Bno055ImuDriver", "Bno055Imu"))
diff --git a/src/bno055_imu_ros_driver.cpp b/src/bno055_imu_ros_driver.cpp
index e5a3b3fbdb0d06f5aa711938e6d66b6e8a68075a..1b03935cb095e0ef26bdd2ac6071998b2982e6f9 100644
--- a/src/bno055_imu_ros_driver.cpp
+++ b/src/bno055_imu_ros_driver.cpp
@@ -108,6 +108,10 @@ void Bno055ImuDriver::config_update(Config& new_cfg, uint32_t level)
case iri_base_driver::OPENED:
this->imu_device->set_data_rate(new_cfg.update_rate);
this->frame_id=new_cfg.tf_prefix+"/"+new_cfg.frame_id;
+ // change configuration
+ this->imu_device->set_accel_config(new_cfg.accel_range,new_cfg.accel_bandwidth,new_cfg.accel_pwr_mode);
+ this->imu_device->set_gyro_config(new_cfg.gyro_range,new_cfg.gyro_bandwidth,new_cfg.gyro_pwr_mode);
+ this->imu_device->set_mag_config(new_cfg.mag_rate,new_cfg.mag_op_mode,new_cfg.mag_pwr_mode);
break;
case iri_base_driver::RUNNING: