diff --git a/src/drivers/distance_sensor/rds02uf/module.yaml b/src/drivers/distance_sensor/rds02uf/module.yaml index ebab98d0d3..4a8f1f225a 100644 --- a/src/drivers/distance_sensor/rds02uf/module.yaml +++ b/src/drivers/distance_sensor/rds02uf/module.yaml @@ -2,13 +2,13 @@ module_name: Benewake RDS02UF Rangefinder serial_config: - command: rds02uf start -d ${SERIAL_DEV} -R SENS_RDS02UF_ROT port_config_param: - name: SENS_RDS02UF_CFG + name: SENS_RDS02_CFG group: Sensors parameters: - group: Sensors definitions: - SENS_RDS02UF_ROT: + SENS_RDS02_ROT: description: short: Distance Sensor Rotation long: | @@ -24,3 +24,23 @@ parameters: 2: ROTATION_RIGHT_FACING reboot_required: true default: 25 + + SENS_RDS02_MAXD: + description: + short: Benewake RDS02UF Sensor Max Distance + long: | + This parameter defines maximum measuring distance in cm. + type: int32 + min: 5 + max: 20000 + default: 1500 + + SENS_RDS02_MIND: + description: + short: Benewake RDS02UF Sensor Min Distance + long: | + This plsarameter defines min measuring distance in cm. + type: int32 + min: 5 + max: 20000 + default: 10 diff --git a/src/drivers/distance_sensor/rds02uf/rds02uf.cpp b/src/drivers/distance_sensor/rds02uf/rds02uf.cpp index 8a02a3506b..971b832580 100644 --- a/src/drivers/distance_sensor/rds02uf/rds02uf.cpp +++ b/src/drivers/distance_sensor/rds02uf/rds02uf.cpp @@ -73,15 +73,27 @@ Rds02uf::~Rds02uf() int Rds02uf::init() { - param_t _param_rds02uf_rot = param_find("SENS_RDS02UF_ROT"); - int32_t rotation; + param_t _param_rds02uf_rot = param_find("SENS_RDS02_ROT"); + param_t _param_rds02uf_mind = param_find("SENS_RDS02_MIND"); + param_t _param_rds02uf_maxd = param_find("SENS_RDS02_MAXD"); + int32_t rotation,max_dist,min_dist; if (param_get(_param_rds02uf_rot, &rotation) == PX4_OK) { _px4_rangefinder.set_orientation(rotation); } + if (param_get(_param_rds02uf_mind, &min_dist) == PX4_OK) + { + _px4_rangefinder.set_min_distance(min_dist / 100.0f); + } else { + _px4_rangefinder.set_min_distance(0.4f); + } + if (param_get(_param_rds02uf_maxd, &max_dist) == PX4_OK) + { + _px4_rangefinder.set_max_distance(max_dist / 100.0f); + } else { + _px4_rangefinder.set_max_distance(20.0f); + } - _px4_rangefinder.set_min_distance(0.4f); - _px4_rangefinder.set_max_distance(20.0f); // _px4_rangefinder.set_fov(math::radians(1.15f)); // status diff --git a/src/drivers/distance_sensor/tfmini/TFMINI.cpp b/src/drivers/distance_sensor/tfmini/TFMINI.cpp index 63853c6cf2..76925e0c79 100644 --- a/src/drivers/distance_sensor/tfmini/TFMINI.cpp +++ b/src/drivers/distance_sensor/tfmini/TFMINI.cpp @@ -73,10 +73,26 @@ int TFMINI::init() { int32_t hw_model = 1; // only one model so far... - param_t _param_tf_rot = param_find("SENS_TFMINI_ROT"); - int32_t tf_rotation; - if (_param_tf_rot != PARAM_INVALID) { - param_get(_param_tf_rot, &tf_rotation); + + param_t _param_tfmini_rot = param_find("SENS_TFMINI_ROT"); + param_t _param_tfmini_mind = param_find("SENS_TFMINI_MIND"); + param_t _param_tfmini_maxd = param_find("SENS_TFMINI_MAXD"); + int32_t rotation,max_dist,min_dist; + if (param_get(_param_tfmini_rot, &rotation) == PX4_OK) + { + _px4_rangefinder.set_orientation(rotation); + } + if (param_get(_param_tfmini_mind, &min_dist) == PX4_OK) + { + _px4_rangefinder.set_min_distance(min_dist / 100.0f); + } else { + _px4_rangefinder.set_min_distance(0.4f); + } + if (param_get(_param_tfmini_maxd, &max_dist) == PX4_OK) + { + _px4_rangefinder.set_max_distance(max_dist / 100.0f); + } else { + _px4_rangefinder.set_max_distance(12.0f); } switch (hw_model) { @@ -86,10 +102,10 @@ TFMINI::init() // 0.3 is too close to minimum so chattering of invalid sensor decision // is happening sometimes. this cause EKF to believe inconsistent range readings. // So we set 0.4 as valid minimum. - _px4_rangefinder.set_min_distance(0.4f); - _px4_rangefinder.set_max_distance(12.0f); + // _px4_rangefinder.set_min_distance(0.4f); // use parameter settings + // _px4_rangefinder.set_max_distance(12.0f); _px4_rangefinder.set_fov(math::radians(1.15f)); - _px4_rangefinder.set_orientation(tf_rotation); + // _px4_rangefinder.set_orientation(tf_rotation); break; diff --git a/src/drivers/distance_sensor/tfmini/module.yaml b/src/drivers/distance_sensor/tfmini/module.yaml index 56f1d0c2d3..b061610f92 100644 --- a/src/drivers/distance_sensor/tfmini/module.yaml +++ b/src/drivers/distance_sensor/tfmini/module.yaml @@ -24,3 +24,23 @@ parameters: 2: ROTATION_RIGHT_FACING reboot_required: true default: 25 + + SENS_TFMINI_MAXD: + description: + short: Benewake TF Sensor Max Distance + long: | + This parameter defines maximum measuring distance in cm. + type: int32 + min: 5 + max: 20000 + default: 1200 + + SENS_TFMINI_MIND: + description: + short: Benewake TF Sensor Min Distance + long: | + This parameter defines min measuring distance in cm. + type: int32 + min: 5 + max: 20000 + default: 10 diff --git a/src/drivers/distance_sensor/tfmini_i2c/module.yaml b/src/drivers/distance_sensor/tfmini_i2c/module.yaml index 7d677bbcb8..86839da340 100644 --- a/src/drivers/distance_sensor/tfmini_i2c/module.yaml +++ b/src/drivers/distance_sensor/tfmini_i2c/module.yaml @@ -52,7 +52,7 @@ parameters: description: short: Benewake TF Sensor ${i} Max Distance long: | - This parameter defines maximum measuring distance. + This parameter defines maximum measuring distance in cm. type: int32 min: 5 max: 20000 @@ -64,7 +64,7 @@ parameters: description: short: Benewake TF Sensor ${i} Min Distance long: | - This parameter defines min measuring distance. + This parameter defines min measuring distance in cm. type: int32 min: 5 max: 20000