Added configuration parameters to the dynamic reconfigure interface.

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"))

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: