diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index 091cd49ea7..5c4ce20553 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -384,6 +384,15 @@ void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vector2f &curr_pos, const Vector2f &curr_vel) { + bool need_prt = false; + static uint8_t cnt; + cnt+=1; + if (cnt > 100) + { + // need_prt = true; + cnt = 0; + } + ////////////////////////////// _updateObstacleMap(); // read parameters @@ -401,6 +410,10 @@ CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vec const hrt_abstime constrain_time = getTime(); int num_fov_bins = 0; + if (need_prt) + { + printf("[clc]setpoint_length:%.2f,contime:%d,obtime:%d,delt:%d\n",(double)setpoint_length,constrain_time , _obstacle_map_body_frame.timestamp,constrain_time - _obstacle_map_body_frame.timestamp); + } if ((constrain_time - _obstacle_map_body_frame.timestamp) < RANGE_STREAM_TIMEOUT_US) { if (setpoint_length > 0.001f) { @@ -416,6 +429,11 @@ CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vec // change setpoint direction slightly (max by _param_cp_guide_ang degrees) to help guide through narrow gaps _adaptSetpointDirection(setpoint_dir, sp_index, vehicle_yaw_angle_rad); + if (need_prt) + { + printf("[clc]setpoint_dir = %.2f,%.2f\n",(double)setpoint_dir(0),(double)setpoint_dir(1)); + } + // limit speed for safe flight for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) { // disregard unused bins at the end of the message @@ -483,6 +501,10 @@ CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vec } setpoint = setpoint_dir * vel_max; + if (need_prt) + { + printf("[clc]setpoint = %.2f,%.2f\n",(double)setpoint(0),(double)setpoint(1)); + } } } else { @@ -509,10 +531,21 @@ void CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const float max_speed, const Vector2f &curr_pos, const Vector2f &curr_vel) { + + bool need_prt = false; + static uint8_t cnt; + cnt+=1; + if (cnt > 100) + { + // need_prt = true; + cnt = 0; + } + ///////////////////////////////////// //calculate movement constraints based on range data Vector2f new_setpoint = original_setpoint; _calculateConstrainedSetpoint(new_setpoint, curr_pos, curr_vel); + //warn user if collision prevention starts to interfere bool currently_interfering = (new_setpoint(0) < original_setpoint(0) - 0.05f * max_speed || new_setpoint(0) > original_setpoint(0) + 0.05f * max_speed @@ -521,6 +554,10 @@ CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const float max _interfering = currently_interfering; + if (need_prt) + { + printf("setpoint:%.2f,%.2f,%d\n",(double)original_setpoint(0),(double)new_setpoint(0),_interfering); + } // publish constraints collision_constraints_s constraints{}; constraints.timestamp = getTime(); @@ -535,51 +572,41 @@ 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 min_distance = 0.2f; + static float max_distance = 12.0f; static float up_distance; static uint64_t _data_h_timestamps; static uint64_t _dist_timestamps; + bool need_prt = false; + static uint8_t cnt; + cnt+=1; + if (cnt > 100) + { + // need_prt = true; + cnt = 0; + } + // 获取测距距离值相关 for (auto &dist_sens_sub : _distance_sensor_subs) { - distance_sensor_s distance_sensor; - + 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)) { - + // (distance_sensor.orientation == distance_sensor_s::ROTATION_UPWARD_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 )); - + max_distance = math::min(max_distance,(distance_sensor.max_distance )); + min_distance = math::max(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(); @@ -588,17 +615,21 @@ CollisionPrevention::modifySetpointH(float &original_setpoint, const float max_s 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 (need_prt) + { + printf("dist:%.2f,dt:%d,%.2f,%.2f\n",(double)up_distance,constrain_time-_dist_timestamps,(double)min_distance,(double)max_distance); + } + + if ((constrain_time - _dist_timestamps) < RANGE_STREAM_TIMEOUT_US && up_distance > min_distance && up_distance < max_distance) { 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; @@ -606,75 +637,59 @@ CollisionPrevention::modifySetpointH(float &original_setpoint, const float max_s 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); + 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); + } + + }else if (distance >= UINT16_MAX * 0.01f ) { + if (!move_no_data) { + vel_max = 0.f; + } + } + setpoint = -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); - - + // 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(); + // } } _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); + // if(_interfering){ + // static uint8_t not_cnt; + // not_cnt += 1; + // if(not_cnt > 10){ + // not_cnt = 0; + // printf("notic:%.2f\n",(double)original_setpoint); + // } + // } } void CollisionPrevention::_publishVehicleCmdDoLoiter() diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index ddaaff888c..fd147fe0f7 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -106,23 +106,23 @@ void FlightTaskManualAltitude::_scaleSticks() 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 + float vel_z ; if (_collision_prevention.is_active()) { + vel_z = vel_max_z * _sticks.getPositionExpo()(2); _collision_prevention.modifySetpointH(vel_z, vel_max_z, _position(2), _velocity(2)); + _velocity_setpoint(2) = vel_z; + // static uint8_t cnt; + // cnt+=1; + // if (cnt > 100) + // { + // printf(" vx:%.3f,vy:%.3f,vz:%.3f,vz2:%.3f\n",(double)_velocity_setpoint(0),(double)_velocity_setpoint(1),(double)_velocity_setpoint(2),(double)vel_z); + // printf(" px:%.3f,py:%.3f,pz:%.3f\n",(double)_position(0),(double)_position(1),(double)_position(2)); + // cnt = 0; + // } } - 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) @@ -377,6 +377,20 @@ void FlightTaskManualAltitude::_updateSetpoints() _acceleration_setpoint.xy() = sp * tanf(math::radians(_param_mpc_man_tilt_max.get())) * CONSTANTS_ONE_G; + Vector2f setpoint = _acceleration_setpoint.xy(); + if (_collision_prevention.is_active()) { + _collision_prevention.modifySetpoint(setpoint, 1.0f, _position.xy(), _velocity.xy()); + + // static uint8_t cnt; + // cnt+=1; + // if (cnt > 100) + // { + // printf(" ax:%.3f,ay:%.3f,ax2:%.3f,ay2:%.3f\n",(double)_acceleration_setpoint(0),(double)_acceleration_setpoint(1),(double)setpoint(0),(double)setpoint(1)); + // // printf(" px:%.3f,py:%.3f,pz:%.3f\n",(double)_position(0),(double)_position(1),(double)_position(2)); + // cnt = 0; + // } + } + _updateAltitudeLock(); _respectGroundSlowdown(); }