diff --git a/boards/px4/fmu-v2/default.cmake b/boards/px4/fmu-v2/default.cmake index fa4ae12088..9cf54ee30c 100644 --- a/boards/px4/fmu-v2/default.cmake +++ b/boards/px4/fmu-v2/default.cmake @@ -24,8 +24,8 @@ px4_add_board( #camera_capture #camera_trigger #differential_pressure # all available differential pressure drivers - differential_pressure/ms4525 - #distance_sensor # all available distance sensor drivers + # differential_pressure/ms4525 + distance_sensor # all available distance sensor drivers #distance_sensor/ll40ls #distance_sensor/lightware_laser_serial #dshot @@ -60,7 +60,7 @@ px4_add_board( tone_alarm #uavcan MODULES - airspeed_selector + # airspeed_selector #attitude_estimator_q battery_status #camera_feedback @@ -70,8 +70,8 @@ px4_add_board( #esc_battery #events flight_mode_manager - fw_att_control - fw_pos_control_l1 + # fw_att_control + # fw_pos_control_l1 #gyro_calibration #gyro_fft land_detector diff --git a/src/drivers/distance_sensor/tfmini_i2c/parameters.c b/src/drivers/distance_sensor/tfmini_i2c/parameters.c index a73bcf12e3..232c8a49e0 100644 --- a/src/drivers/distance_sensor/tfmini_i2c/parameters.c +++ b/src/drivers/distance_sensor/tfmini_i2c/parameters.c @@ -189,106 +189,24 @@ PARAM_DEFINE_INT32(SENS_TF_5_ROT, 0); PARAM_DEFINE_INT32(SENS_TF_6_ROT, 0); /** - * Benewake TFmini Sensor 7 Rotation + * Minimum distance the vehicle should keep to all obstacles * - * This parameter defines the rotation of the sensor relative to the platform. - * - * @reboot_required true - * @min 0 - * @max 7 - * @group Sensors + * Only used in Position mode. Collision avoidance is disabled by setting this parameter to a negative value * - * @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° + * @min -1 + * @max 15 + * @unit m + * @group Multicopter Position Control */ -PARAM_DEFINE_INT32(SENS_TF_7_ROT, 0); +PARAM_DEFINE_FLOAT(SENS_TF_AVD_D, 2.0f); /** - * Benewake TFmini Sensor 8 Rotation + * Manual-Position control sub-mode * - * This parameter defines the rotation of the sensor relative to the platform. * - * @reboot_required true - * @min 0 - * @max 7 - * @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° - */ -PARAM_DEFINE_INT32(SENS_TF_8_ROT, 0); - -/** - * Benewake TFmini Sensor 9 Rotation - * - * This parameter defines the rotation of the sensor relative to the platform. - * - * @reboot_required true - * @min 0 - * @max 7 - * @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° - */ -PARAM_DEFINE_INT32(SENS_TF_9_ROT, 0); - -/** - * Benewake TFmini Sensor 10 Rotation - * - * This parameter defines the rotation of the sensor relative to the platform. - * - * @reboot_required true - * @min 0 - * @max 7 - * @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° - */ -PARAM_DEFINE_INT32(SENS_TF_10_ROT, 0); - -/** - * Benewake TFmini Sensor 12 Rotation - * - * This parameter defines the rotation of the sensor relative to the platform. - * - * @reboot_required true - * @min 0 - * @max 7 - * @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 Simple position control + * @value 3 Smooth position control (Jerk optimized) + * @value 4 Acceleration based input + * @group Multicopter Position Control */ -PARAM_DEFINE_INT32(SENS_TF_11_ROT, 0); +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 7b003b30ad..e83ac3bef6 100644 --- a/src/drivers/distance_sensor/tfmini_i2c/tfmini_i2c.cpp +++ b/src/drivers/distance_sensor/tfmini_i2c/tfmini_i2c.cpp @@ -66,6 +66,7 @@ #include #include +#include using namespace time_literals; #define ARRAY_SIZE(_arr) (sizeof(_arr) / sizeof(_arr[0])) @@ -174,8 +175,16 @@ private: (ParamInt) _p_sensor3_rot, (ParamInt) _p_sensor4_rot, (ParamInt) _p_sensor5_rot, - (ParamInt) _p_sensor6_rot + (ParamInt) _p_sensor6_rot, + (ParamFloat) _p_tf_avoid_dist_m, + (ParamInt) _p_tf_avoid_mode, + (ParamFloat) _p_cp_dist, + (ParamInt) _param_mpc_pos_mode + ); + orb_advert_t _mavlink_log_pub{nullptr}; ///< mavlink log pub + float avoid_distance; + int entra_avoid_area{0}; }; TFMINI_I2C::TFMINI_I2C(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address) : @@ -235,6 +244,18 @@ TFMINI_I2C::init() 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){ + + + // _p_cp_dist.set(avoid_dist); + // _param_mpc_pos_mode.set(avoid_mode); + // printf("set avoid distance:%.2fm,mode:%d",(double)avoid_dist,avoid_mode); + avoid_distance = _p_cp_dist.get(); + printf("get avoid distance:%.2fm,mode:%d\n",(double)avoid_distance,(uint8_t)_param_mpc_pos_mode.get()); + // } // Return an error if no sensors were detected. if (_sensor_count == 0) { @@ -392,6 +413,15 @@ TFMINI_I2C::collect() // perf_end(_sample_perf); // return ret_val; // } + if(avoid_distance - distance_m > 0.1f && !entra_avoid_area){ + mavlink_log_info(&_mavlink_log_pub, "(%d)Enter obstacle avoidance range : %.2f\n",_sensor_index,(double)distance_m); + printf("(%d)Enter obstacle avoidance range : %.2f\n",_sensor_index,(double)distance_m); + entra_avoid_area = 1; + }else if(distance_m - avoid_distance > 0.1f && entra_avoid_area){ + entra_avoid_area = 0; + printf("(%d)Out obstacle avoidance range : %.2f\n",_sensor_index,(double)distance_m); + + } perf_end(_sample_perf); return PX4_OK;