From 5a73c704b498dd59cc86dee01d7a972c753d6850 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E9=82=A3=E4=B8=AAZeng?= Date: Wed, 18 May 2022 00:11:28 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0tf=20iic=E5=9C=B0=E5=9D=80?= =?UTF-8?q?=E8=AE=BE=E7=BD=AE?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../distance_sensor/tfmini_i2c/parameters.c | 186 +++++++++--------- .../distance_sensor/tfmini_i2c/tfmini_i2c.cpp | 36 ++-- .../FlightTaskManualAltitude.cpp | 2 +- v2_fmu.sh | 2 +- 4 files changed, 119 insertions(+), 107 deletions(-) diff --git a/src/drivers/distance_sensor/tfmini_i2c/parameters.c b/src/drivers/distance_sensor/tfmini_i2c/parameters.c index 8f3574e968..d11b3035c1 100644 --- a/src/drivers/distance_sensor/tfmini_i2c/parameters.c +++ b/src/drivers/distance_sensor/tfmini_i2c/parameters.c @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * Benewake TFmini Rangefinder Sonar (tfmini i2c) + * Benewake TF Rangefinder Sonar (tf i2c) * * @reboot_required true * @@ -42,7 +42,21 @@ PARAM_DEFINE_INT32(SENS_EN_TFI2C, 0); /** - * Benewake TFmini Sensor 0 Rotation + * Benewake TF Sensor 0 Address + * + * This parameter defines the address of the TFi2c sensor. + * + * @reboot_required true + * @min 1 + * @max 255 + * @group Sensors + * + */ + +PARAM_DEFINE_INT32(SENS_TF_0_ADDR, 16); + +/** + * Benewake TF Sensor 0 Rotation * * This parameter defines the rotation of the sensor relative to the platform. * @@ -69,7 +83,7 @@ PARAM_DEFINE_INT32(SENS_EN_TFI2C, 0); PARAM_DEFINE_INT32(SENS_TF_0_ROT, 0); /** - * Benewake TFmini Sensor 0 Max Distance + * Benewake TF Sensor 0 Max Distance * * This parameter defines maximum measuring distance. * @@ -82,7 +96,7 @@ PARAM_DEFINE_INT32(SENS_TF_0_ROT, 0); PARAM_DEFINE_INT32(SENS_TF_0_MAXD, 1200); /** - * Benewake TFmini Sensor 0 Min Distance + * Benewake TF Sensor 0 Min Distance * * This parameter defines min measuring distance. * @@ -95,7 +109,21 @@ PARAM_DEFINE_INT32(SENS_TF_0_MAXD, 1200); PARAM_DEFINE_INT32(SENS_TF_0_MIND, 10); /** - * Benewake TFmini Sensor 1 Rotation + * Benewake TF Sensor 1 Address + * + * This parameter defines the address of the TFi2c sensor. + * + * @reboot_required true + * @min 1 + * @max 255 + * @group Sensors + * + */ + +PARAM_DEFINE_INT32(SENS_TF_1_ADDR, 17); + +/** + * Benewake TF Sensor 1 Rotation * * This parameter defines the rotation of the sensor relative to the platform. * @@ -122,7 +150,7 @@ PARAM_DEFINE_INT32(SENS_TF_0_MIND, 10); PARAM_DEFINE_INT32(SENS_TF_1_ROT, 0); /** - * Benewake TFmini Sensor 1 Max Distance + * Benewake TF Sensor 1 Max Distance * * This parameter defines maximum measuring distance. * @@ -135,7 +163,7 @@ PARAM_DEFINE_INT32(SENS_TF_1_ROT, 0); PARAM_DEFINE_INT32(SENS_TF_1_MAXD, 1200); /** - * Benewake TFmini Sensor 1 Min Distance + * Benewake TF Sensor 1 Min Distance * * This parameter defines min measuring distance. * @@ -148,7 +176,21 @@ PARAM_DEFINE_INT32(SENS_TF_1_MAXD, 1200); PARAM_DEFINE_INT32(SENS_TF_1_MIND, 10); /** - * Benewake TFmini Sensor 2 Rotation + * Benewake TF Sensor 2 Address + * + * This parameter defines the address of the TFi2c sensor. + * + * @reboot_required true + * @min 1 + * @max 255 + * @group Sensors + * + */ + +PARAM_DEFINE_INT32(SENS_TF_2_ADDR, 18); + +/** + * Benewake TF Sensor 2 Rotation * * This parameter defines the rotation of the sensor relative to the platform. * @@ -175,7 +217,7 @@ PARAM_DEFINE_INT32(SENS_TF_1_MIND, 10); PARAM_DEFINE_INT32(SENS_TF_2_ROT, 0); /** - * Benewake TFmini Sensor 2 Max Distance + * Benewake TF Sensor 2 Max Distance * * This parameter defines maximum measuring distance. * @@ -188,7 +230,7 @@ PARAM_DEFINE_INT32(SENS_TF_2_ROT, 0); PARAM_DEFINE_INT32(SENS_TF_2_MAXD, 1200); /** - * Benewake TFmini Sensor 2 Min Distance + * Benewake TF Sensor 2 Min Distance * * This parameter defines min measuring distance. * @@ -201,7 +243,21 @@ PARAM_DEFINE_INT32(SENS_TF_2_MAXD, 1200); PARAM_DEFINE_INT32(SENS_TF_2_MIND, 10); /** - * Benewake TFmini Sensor 3 Rotation + * Benewake TF Sensor 3 Address + * + * This parameter defines the address of the TFi2c sensor. + * + * @reboot_required true + * @min 1 + * @max 255 + * @group Sensors + * + */ + +PARAM_DEFINE_INT32(SENS_TF_3_ADDR, 19); + +/** + * Benewake TF Sensor 3 Rotation * * This parameter defines the rotation of the sensor relative to the platform. * @@ -228,7 +284,7 @@ PARAM_DEFINE_INT32(SENS_TF_2_MIND, 10); PARAM_DEFINE_INT32(SENS_TF_3_ROT, 0); /** - * Benewake TFmini Sensor 3 Max Distance + * Benewake TF Sensor 3 Max Distance * * This parameter defines maximum measuring distance. * @@ -241,7 +297,7 @@ PARAM_DEFINE_INT32(SENS_TF_3_ROT, 0); PARAM_DEFINE_INT32(SENS_TF_3_MAXD, 1200); /** - * Benewake TFmini Sensor 3 Min Distance + * Benewake TF Sensor 3 Min Distance * * This parameter defines min measuring distance. * @@ -254,7 +310,21 @@ PARAM_DEFINE_INT32(SENS_TF_3_MAXD, 1200); PARAM_DEFINE_INT32(SENS_TF_3_MIND, 10); /** - * Benewake TFmini Sensor 4 Rotation + * Benewake TF Sensor 4 Address + * + * This parameter defines the address of the TFi2c sensor. + * + * @reboot_required true + * @min 1 + * @max 255 + * @group Sensors + * + */ + +PARAM_DEFINE_INT32(SENS_TF_4_ADDR, 20); + +/** + * Benewake TF Sensor 4 Rotation * * This parameter defines the rotation of the sensor relative to the platform. * @@ -281,7 +351,7 @@ PARAM_DEFINE_INT32(SENS_TF_3_MIND, 10); PARAM_DEFINE_INT32(SENS_TF_4_ROT, 0); /** - * Benewake TFmini Sensor 4 Max Distance + * Benewake TF Sensor 4 Max Distance * * This parameter defines maximum measuring distance. * @@ -294,7 +364,7 @@ PARAM_DEFINE_INT32(SENS_TF_4_ROT, 0); PARAM_DEFINE_INT32(SENS_TF_4_MAXD, 1200); /** - * Benewake TFmini Sensor 4 Min Distance + * Benewake TF Sensor 4 Min Distance * * This parameter defines min measuring distance. * @@ -307,66 +377,26 @@ PARAM_DEFINE_INT32(SENS_TF_4_MAXD, 1200); PARAM_DEFINE_INT32(SENS_TF_4_MIND, 10); /** - * Benewake TFmini Sensor 5 Rotation + * Benewake TF Sensor 5 Address * - * This parameter defines the rotation of the sensor relative to the platform. + * This parameter defines the address of the TFi2c sensor. * * @reboot_required true - * @min 0 - * @max 25 - * @group Sensors - * - * @value 0 No rotation - * @value 1 Yaw 45° - * @value 2 Yaw 90° - * @value 3 Yaw 135° - * @value 4 Yaw 180° - * @value 5 Yaw 225° - * @value 6 Yaw 270° - * @value 7 Yaw 315° - * @value 0 ROTATION_FORWARD_FACING - * @value 2 ROTATION_RIGHT_FACING - * @value 4 ROTATION_BACKWARD_FACING - * @value 6 ROTATION_LEFT_FACING - * @value 24 ROTATION_UPWARD_FACING - * @value 25 ROTATION_DOWNWARD_FACING - */ -PARAM_DEFINE_INT32(SENS_TF_5_ROT, 0); - -/** - * Benewake TFmini Sensor 5 Max Distance - * - * This parameter defines maximum measuring distance. - * - * @reboot_required false - * @min 5 - * @max 20000 + * @min 1 + * @max 255 * @group Sensors * */ -PARAM_DEFINE_INT32(SENS_TF_5_MAXD, 1200); -/** - * Benewake TFmini Sensor 5 Min Distance - * - * This parameter defines min measuring distance. - * - * @reboot_required false - * @min 5 - * @max 20000 - * @group Sensors - * - */ -PARAM_DEFINE_INT32(SENS_TF_5_MIND, 10); +PARAM_DEFINE_INT32(SENS_TF_5_ADDR, 21); /** - * Benewake TFmini Sensor 6 Rotation + * Benewake TF Sensor 5 Rotation * * This parameter defines the rotation of the sensor relative to the platform. * * @reboot_required true * @min 0 - * @max 7 * @max 25 * @group Sensors * @@ -385,10 +415,10 @@ PARAM_DEFINE_INT32(SENS_TF_5_MIND, 10); * @value 24 ROTATION_UPWARD_FACING * @value 25 ROTATION_DOWNWARD_FACING */ -PARAM_DEFINE_INT32(SENS_TF_6_ROT, 0); +PARAM_DEFINE_INT32(SENS_TF_5_ROT, 0); /** - * Benewake TFmini Sensor 6 Max Distance + * Benewake TF Sensor 5 Max Distance * * This parameter defines maximum measuring distance. * @@ -398,10 +428,10 @@ PARAM_DEFINE_INT32(SENS_TF_6_ROT, 0); * @group Sensors * */ -PARAM_DEFINE_INT32(SENS_TF_6_MAXD, 1200); +PARAM_DEFINE_INT32(SENS_TF_5_MAXD, 1200); /** - * Benewake TFmini Sensor 6 Min Distance + * Benewake TF Sensor 5 Min Distance * * This parameter defines min measuring distance. * @@ -411,27 +441,5 @@ PARAM_DEFINE_INT32(SENS_TF_6_MAXD, 1200); * @group Sensors * */ -PARAM_DEFINE_INT32(SENS_TF_6_MIND, 10); - -/** - * Minimum distance the vehicle should keep to all obstacles - * - * Only used in Position mode. Collision avoidance is disabled by setting this parameter to a negative value - * - * @min -1 - * @max 15 - * @unit m - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(SENS_TF_AVD_D, 2.0f); +PARAM_DEFINE_INT32(SENS_TF_5_MIND, 10); -/** - * Manual-Position control sub-mode - * - * - * @value 0 Simple position control - * @value 3 Smooth position control (Jerk optimized) - * @value 4 Acceleration based input - * @group Multicopter Position Control - */ -PARAM_DEFINE_INT32(SENS_TF_AVD_M, 0); diff --git a/src/drivers/distance_sensor/tfmini_i2c/tfmini_i2c.cpp b/src/drivers/distance_sensor/tfmini_i2c/tfmini_i2c.cpp index 113dafe4a3..e0e4cc785a 100644 --- a/src/drivers/distance_sensor/tfmini_i2c/tfmini_i2c.cpp +++ b/src/drivers/distance_sensor/tfmini_i2c/tfmini_i2c.cpp @@ -174,31 +174,32 @@ private: max_min_dist dist_range[6]; DEFINE_PARAMETERS( (ParamInt) _p_sensor_enabled, + (ParamInt) _p_sensor0_addr, + (ParamInt) _p_sensor1_addr, + (ParamInt) _p_sensor2_addr, + (ParamInt) _p_sensor3_addr, + (ParamInt) _p_sensor4_addr, + (ParamInt) _p_sensor5_addr, (ParamInt) _p_sensor0_rot, (ParamInt) _p_sensor1_rot, (ParamInt) _p_sensor2_rot, (ParamInt) _p_sensor3_rot, (ParamInt) _p_sensor4_rot, (ParamInt) _p_sensor5_rot, - (ParamInt) _p_sensor6_rot, (ParamInt) _p_tf_0_max_dist, (ParamInt) _p_tf_1_max_dist, (ParamInt) _p_tf_2_max_dist, (ParamInt) _p_tf_3_max_dist, (ParamInt) _p_tf_4_max_dist, (ParamInt) _p_tf_5_max_dist, - (ParamInt) _p_tf_6_max_dist, (ParamInt) _p_tf_0_min_dist, (ParamInt) _p_tf_1_min_dist, (ParamInt) _p_tf_2_min_dist, (ParamInt) _p_tf_3_min_dist, (ParamInt) _p_tf_4_min_dist, (ParamInt) _p_tf_5_min_dist, - (ParamInt) _p_tf_6_min_dist, - (ParamFloat) _p_tf_avoid_dist_m, - (ParamInt) _p_tf_avoid_mode, (ParamFloat) _p_cp_dist, - (ParamInt) _param_mpc_pos_mode + (ParamInt) _param_mpc_pos_mode ); orb_advert_t _mavlink_log_pub{nullptr}; ///< mavlink log pub @@ -240,32 +241,36 @@ TFMINI_I2C::init() return PX4_ERROR; } printf("\n--------------------\n"); - + const uint8_t param_total_addr[6] = { (uint8_t)_p_sensor0_addr.get(), (uint8_t)_p_sensor1_addr.get(), (uint8_t)_p_sensor2_addr.get(), \ + (uint8_t)_p_sensor3_addr.get(),(uint8_t)_p_sensor4_addr.get(),(uint8_t)_p_sensor5_addr.get() }; + const uint8_t param_total_rot[6] = { (uint8_t)_p_sensor0_rot.get(), (uint8_t)_p_sensor1_rot.get(), (uint8_t)_p_sensor2_rot.get(), \ + (uint8_t)_p_sensor3_rot.get(),(uint8_t)_p_sensor4_rot.get(),(uint8_t)_p_sensor5_rot.get() }; // Check for connected rangefinders on each i2c port by decrementing from the base address, // (TFMINI_I2C_BASE_ADDR = 112, TFMINI_I2C_MAX_ADDR = 90). - for (uint8_t address = TFMINI_I2C_BASE_ADDR; address < TFMINI_I2C_MAX_ADDR ; address++) { + uint8_t address; + for (uint8_t index = 0; index < RANGE_FINDER_MAX_SENSORS ; index++) { + address = param_total_addr[index]; set_device_address(address); px4_usleep(_measure_interval); if (measure() == PX4_OK) { + px4_usleep(_measure_interval); // Store I2C address _sensor_addresses[_sensor_count] = address; - _sensor_rotations[_sensor_count] = get_sensor_rotation(_sensor_count); + _sensor_rotations[_sensor_count] = param_total_rot[index]; _sensor_count++; int set_ret = set_param(); - PX4_INFO("sensor %i at address 0x%02X ,set:%d", _sensor_count, get_device_address(),set_ret); - + printf("tf[%i]: address 0x%02X ,rototion %d, set param:%d\n", _sensor_count, get_device_address(),_sensor_rotations[_sensor_count],set_ret); + mavlink_log_info(&_mavlink_log_pub, "sensor %i at address 0x%02X ,rotation:%d", _sensor_count, get_device_address(),_sensor_rotations[_sensor_count]); if (_sensor_count >= RANGE_FINDER_MAX_SENSORS) { break; } }else{ - printf("\nerr tf[%d]:add%02x",_sensor_count,address); + printf("\nerr tf[%d]:address:%02x",_sensor_count,address); } px4_usleep(_measure_interval); } printf("\n--------------------\n"); - // float avoid_dist = _p_tf_avoid_dist_m.get(); - // int32_t avoid_mode = _p_tf_avoid_mode.get(); // if(avoid_dist > 0.5f){ @@ -432,7 +437,7 @@ TFMINI_I2C::collect() uint16_t distance_cm = distance; if (distance_cm > dist_range[_sensor_index].max || distance_cm < dist_range[_sensor_index].min ) // 超出距离范围 { - distance_cm = dist_range[_sensor_index].max + 50; + distance_cm = dist_range[_sensor_index].max ; } float distance_m = static_cast(distance_cm) * 1e-2f; @@ -509,7 +514,6 @@ TFMINI_I2C::get_sensor_rotation(const size_t index) case 5: return _p_sensor5_rot.get(); - case 6: return _p_sensor6_rot.get(); default: return PX4_ERROR; } diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index 56ce303648..5b33965b75 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -103,7 +103,7 @@ void FlightTaskManualAltitude::_scaleSticks() bool need_prt = false; if (hrt_absolute_time() - delay_1s > 1 * 1000000ULL) { - need_prt = true; + // need_prt = true; delay_1s = hrt_absolute_time(); } diff --git a/v2_fmu.sh b/v2_fmu.sh index 3c367984b6..0280de1169 100755 --- a/v2_fmu.sh +++ b/v2_fmu.sh @@ -1,6 +1,6 @@ date -R starttime=`date +'%Y-%m-%d %H:%M:%S'` -make px4_fmu-v2_default upload +make px4_fmu-v2_default endtime=`date +'%Y-%m-%d %H:%M:%S'` date -R start_seconds=$(date --date="$starttime" +%s);