From dca054f85b447638378aea96de2e63f7faa7b03d Mon Sep 17 00:00:00 2001 From: zbr3550 Date: Mon, 6 Jun 2022 23:33:38 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E9=AB=98=E5=BA=A6=E9=98=B2?= =?UTF-8?q?=E6=92=9E=EF=BC=8C=E4=BB=BF=E7=9C=9F=E6=B5=8B=E8=AF=95ok?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../CollisionPrevention.cpp | 146 ++++++++++++++++++ .../CollisionPrevention.hpp | 4 + .../FlightTaskManualAltitude.cpp | 21 ++- .../FlightTaskManualAltitude.hpp | 3 + 4 files changed, 173 insertions(+), 1 deletion(-) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index 6a4cfb723c..091cd49ea7 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -531,6 +531,152 @@ CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const float max original_setpoint = new_setpoint; } +void +CollisionPrevention::modifySetpointH(float &original_setpoint, const float max_speed, const float &curr_pos, + const float &curr_vel) +{ + + static uint64_t delay_1s = hrt_absolute_time(); + bool need_prt = false; + if (hrt_absolute_time() - delay_1s > 1 * 1000000ULL) + { + need_prt = true; + delay_1s = hrt_absolute_time(); + } + + //calculate movement constraints based on range data + float setpoint = original_setpoint; + /******************************************************************/ + + // _calculateConstrainedSetpoint(new_setpoint, curr_pos, curr_vel); + // read parameters + static float min_distance; + static float max_distance; + static float up_distance; + static uint64_t _data_h_timestamps; + static uint64_t _dist_timestamps; + + // 获取测距距离值相关 + for (auto &dist_sens_sub : _distance_sensor_subs) { + distance_sensor_s distance_sensor; + + if (dist_sens_sub.update(&distance_sensor)) { + // consider only instances with valid data and orientations useful for collision prevention + // 距离值有效，并且是朝上的距离 + // if (need_prt) + // printf("0.1:ts:%lld,rot:%d\n",distance_sensor.timestamp,distance_sensor.orientation); + if ((getElapsedTime(&distance_sensor.timestamp) < RANGE_STREAM_TIMEOUT_US) && + (distance_sensor.orientation == distance_sensor_s::ROTATION_FORWARD_FACING)) { + + // update message description + _dist_timestamps = distance_sensor.timestamp; + max_distance = math::max(max_distance,(distance_sensor.max_distance )); + min_distance = math::min(min_distance,(distance_sensor.min_distance )); + + up_distance = distance_sensor.current_distance; + // if (need_prt) + // printf("1:dist up:%.2f,max:%.2f,min:%.2f\n",(double)up_distance,(double)max_distance,(double)min_distance); + + } + } + + } + // 获取一些参数设置 + const float col_prev_d = _param_cp_dist.get(); + const float col_prev_dly = _param_cp_delay.get(); + const bool move_no_data = _param_cp_go_nodata.get(); + const float xy_p = _param_mpc_xy_p.get(); + const float max_jerk = _param_mpc_jerk_max.get(); + const float max_accel = _param_mpc_acc_hor.get(); + + const float setpoint_length = -setpoint; + const hrt_abstime constrain_time = getTime(); + + + if ((constrain_time - _dist_timestamps) < RANGE_STREAM_TIMEOUT_US) { + if (setpoint_length > 0.001f) { + float vel_max = setpoint_length; + // 最小停留距离，测距最小值或者参数设置停留值 + const float min_dist_to_keep = math::max(min_distance / 100.0f, col_prev_d); + // 删除过时的值 + const hrt_abstime data_age = constrain_time - _data_h_timestamps;//TODO: _data_timestamps需要赋值 + if (data_age > RANGE_STREAM_TIMEOUT_US) { + up_distance = UINT16_MAX * 0.01f; + } + const float distance = up_distance; + const float max_range = max_distance + 0.005f; + + if (need_prt) + printf("2:age:%lld,dist:%.2f,max:%.2f\n",data_age,(double)distance,(double)max_range); + if (distance > min_distance && distance < UINT16_MAX * 0.01f) { + + // calculate max allowed velocity with a P-controller (same gain as in the position controller) + const float curr_vel_parallel = math::max(0.f, curr_vel); + float delay_distance = curr_vel_parallel * col_prev_dly; + if (distance < max_range) { + delay_distance += curr_vel_parallel * (data_age * 1e-6f); + } + const float stop_distance = math::max(0.f, distance - min_dist_to_keep - delay_distance); + const float vel_max_posctrl = xy_p * stop_distance; + + const float vel_max_smooth = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_accel, stop_distance, 0.f); + float vel_max_bin = math::min(vel_max_posctrl, vel_max_smooth) ; + // constrain the velocity + if (vel_max_bin >= 0) { + vel_max = math::min(vel_max, vel_max_bin); + } + + + if (need_prt) + printf("3:parallel:%.2f,posctrl:%.2f,stop_d:%.2f,vmax:%.2f\n",(double)curr_vel_parallel,(double)vel_max_posctrl,(double)stop_distance,(double)vel_max); + + }else if (distance >= UINT16_MAX * 0.01f ) { + if (!move_no_data || (move_no_data)) { + vel_max = 0.f; + } + } + + setpoint = -vel_max; + if (need_prt) + printf("4:vmax:%.2f\n",(double)vel_max); + } + } else { + //allow no movement + float vel_max = 0.f; + setpoint = setpoint * vel_max; + + // if distance data is stale, switch to Loiter + if (getElapsedTime(&_last_timeout_warning) > 1_s && getElapsedTime(&_time_activated) > 1_s) { + + if ((constrain_time - _dist_timestamps) > TIMEOUT_HOLD_US + && getElapsedTime(&_time_activated) > TIMEOUT_HOLD_US) { + _publishVehicleCmdDoLoiter(); + } + + _last_timeout_warning = getTime(); + } + + if (need_prt) + printf("5:setpoint:%.2f,vmax:%.2f\n", (double)setpoint,(double)vel_max); + + + } + _data_h_timestamps = getTime(); + /******************************************************************/ + + //warn user if collision prevention starts to interfere + bool currently_interfering = (setpoint < original_setpoint - 0.05f * max_speed + || setpoint > original_setpoint + 0.05f * max_speed); + + _interfering = currently_interfering; + + + original_setpoint = setpoint; + + if (need_prt) + printf("6:intf:%d,setpoint:%.2f\n", _interfering,(double)original_setpoint); +} + void CollisionPrevention::_publishVehicleCmdDoLoiter() { vehicle_command_s command{}; diff --git a/src/lib/collision_prevention/CollisionPrevention.hpp b/src/lib/collision_prevention/CollisionPrevention.hpp index aaff04e2cb..e28adbb511 100644 --- a/src/lib/collision_prevention/CollisionPrevention.hpp +++ b/src/lib/collision_prevention/CollisionPrevention.hpp @@ -82,6 +82,10 @@ public: void modifySetpoint(matrix::Vector2f &original_setpoint, const float max_speed, const matrix::Vector2f &curr_pos, const matrix::Vector2f &curr_vel); + + void modifySetpointH(float &original_setpoint, const float max_speed, + const float &curr_alt, const float &curr_vel_z); + protected: obstacle_distance_s _obstacle_map_body_frame {}; diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index c13035f363..ddaaff888c 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -43,7 +43,8 @@ using namespace matrix; FlightTaskManualAltitude::FlightTaskManualAltitude() : - _sticks(this) + _sticks(this), + _collision_prevention(this) {} bool FlightTaskManualAltitude::updateInitialize() @@ -104,6 +105,24 @@ void FlightTaskManualAltitude::_scaleSticks() // Use sticks input with deadzone and exponential curve for vertical velocity const float vel_max_z = (_sticks.getPosition()(2) > 0.0f) ? _constraints.speed_down : _constraints.speed_up; _velocity_setpoint(2) = vel_max_z * _sticks.getPositionExpo()(2); + ///////////////////////////////////////////////////////////////////// + float vel_z = vel_max_z * _sticks.getPositionExpo()(2); + // collision prevention + if (_collision_prevention.is_active()) { + _collision_prevention.modifySetpointH(vel_z, vel_max_z, _position(2), _velocity(2)); + } + + + static uint8_t cnt; + cnt+=1; + if (cnt > 50) + { + printf(" vf:%.3f,vb:%.3f\n",(double)_velocity_setpoint(2), (double)vel_z); + cnt = 0; + } + + _velocity_setpoint(2) = vel_z; + } float FlightTaskManualAltitude::_applyYawspeedFilter(float yawspeed_target) diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp index c4b14200a0..1d7089415e 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp @@ -43,6 +43,7 @@ #include "Sticks.hpp" #include #include +#include class FlightTaskManualAltitude : public FlightTask { @@ -151,4 +152,6 @@ private: float _dist_to_ground_lock = NAN; AlphaFilter _man_input_filter; + CollisionPrevention _collision_prevention; /**< collision avoidance setpoint amendment */ + };