Skip to content

Commit

Permalink
removed old implementation
Browse files Browse the repository at this point in the history
  • Loading branch information
@Claudio-Chies
Claudio-Chies committed Oct 1, 2024
1 parent 0967652 commit 045c131
Showing 1 changed file with 0 additions and 125 deletions.
125 changes: 0 additions & 125 deletions src/lib/collision_prevention/CollisionPrevention.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -383,131 +383,6 @@ CollisionPrevention::_sensorOrientationToYawOffset(const distance_sensor_s &dist
return offset;
}

void
CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vector2f &curr_pos,
const Vector2f &curr_vel)
{
_updateObstacleMap();

// read parameters
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 matrix::Quatf attitude = Quatf(_sub_vehicle_attitude.get().q);
const float vehicle_yaw_angle_rad = Eulerf(attitude).psi();

const float setpoint_length = setpoint.norm();

const hrt_abstime constrain_time = getTime();
int num_fov_bins = 0;

if ((constrain_time - _obstacle_map_body_frame.timestamp) < RANGE_STREAM_TIMEOUT_US) {
if (setpoint_length > 0.001f) {

Vector2f setpoint_dir = setpoint / setpoint_length;
float vel_max = setpoint_length;
const float min_dist_to_keep = math::max(_obstacle_map_body_frame.min_distance / 100.0f, col_prev_d);

const float sp_angle_body_frame = atan2f(setpoint_dir(1), setpoint_dir(0)) - vehicle_yaw_angle_rad;
const float sp_angle_with_offset_deg = wrap_360(math::degrees(sp_angle_body_frame) -
_obstacle_map_body_frame.angle_offset);
int sp_index = floor(sp_angle_with_offset_deg / INTERNAL_MAP_INCREMENT_DEG);

// 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);

// limit speed for safe flight
for (int i = 0; i < INTERNAL_MAP_USED_BINS; i++) { // disregard unused bins at the end of the message

// delete stale values
const hrt_abstime data_age = constrain_time - _data_timestamps[i];

if (data_age > RANGE_STREAM_TIMEOUT_US) {
_obstacle_map_body_frame.distances[i] = UINT16_MAX;
}

const float distance = _obstacle_map_body_frame.distances[i] * 0.01f; // convert to meters
const float max_range = _data_maxranges[i] * 0.01f; // convert to meters
float angle = math::radians((float)i * INTERNAL_MAP_INCREMENT_DEG + _obstacle_map_body_frame.angle_offset);

// convert from body to local frame in the range [0, 2*pi]
angle = wrap_2pi(vehicle_yaw_angle_rad + angle);

// get direction of current bin
const Vector2f bin_direction = {cosf(angle), sinf(angle)};

//count number of bins in the field of valid_new
if (_obstacle_map_body_frame.distances[i] < UINT16_MAX) {
num_fov_bins ++;
}

if (_obstacle_map_body_frame.distances[i] > _obstacle_map_body_frame.min_distance
&& _obstacle_map_body_frame.distances[i] < UINT16_MAX) {

if (setpoint_dir.dot(bin_direction) > 0) {
// 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.dot(bin_direction));
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);
const float projection = bin_direction.dot(setpoint_dir);
float vel_max_bin = vel_max;

if (projection > 0.01f) {
vel_max_bin = math::min(vel_max_posctrl, vel_max_smooth) / projection;
}

// constrain the velocity
if (vel_max_bin >= 0) {
vel_max = math::min(vel_max, vel_max_bin);
}
}

} else if (_obstacle_map_body_frame.distances[i] == UINT16_MAX && i == sp_index) {
if (!move_no_data || (move_no_data && _data_fov[i])) {
vel_max = 0.f;
}
}
}

//if the sensor field of view is zero, never allow to move (even if move_no_data=1)
if (num_fov_bins == 0) {
vel_max = 0.f;
}

setpoint = setpoint_dir * 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 - _obstacle_map_body_frame.timestamp) > TIMEOUT_HOLD_US
&& getElapsedTime(&_time_activated) > TIMEOUT_HOLD_US) {
_publishVehicleCmdDoLoiter();
}

_last_timeout_warning = getTime();
}


}
}

void CollisionPrevention::_constrainAccelerationSetpoint(matrix::Vector2f &setpoint_accel,
const matrix::Vector2f &setpoint_vel)
{
Expand Down

0 comments on commit 045c131

Please sign in to comment.