47 static constexpr
int INTERNAL_MAP_INCREMENT_DEG = 10;
48 static constexpr
int INTERNAL_MAP_USED_BINS = 360 / INTERNAL_MAP_INCREMENT_DEG;
50 static float wrap_360(
float f)
52 return wrap(f, 0.f, 360.f);
55 static int wrap_bin(
int i)
57 i = i % INTERNAL_MAP_USED_BINS;
60 i += INTERNAL_MAP_USED_BINS;
71 static_assert(INTERNAL_MAP_INCREMENT_DEG >= 5,
"INTERNAL_MAP_INCREMENT_DEG needs to be at least 5");
72 static_assert(360 % INTERNAL_MAP_INCREMENT_DEG == 0,
"INTERNAL_MAP_INCREMENT_DEG should divide 360 evenly");
81 uint64_t current_time =
getTime();
83 for (uint32_t i = 0 ; i < internal_bins; i++) {
110 bool activated = _param_cp_dist.get() > 0;
125 float increment_factor = 1.f / obstacle.
increment;
127 if (obstacle.
frame == obstacle.MAV_FRAME_GLOBAL || obstacle.
frame == obstacle.MAV_FRAME_LOCAL_NED) {
130 for (
int i = 0; i < INTERNAL_MAP_USED_BINS; i++) {
132 msg_index =
ceil(wrap_360(vehicle_orientation_deg + bin_angle_deg - obstacle.
angle_offset) * increment_factor);
135 if (obstacle.
distances[msg_index] != UINT16_MAX) {
144 }
else if (obstacle.
frame == obstacle.MAV_FRAME_BODY_FRD) {
147 for (
int i = 0; i < INTERNAL_MAP_USED_BINS; i++) {
148 float bin_angle_deg = (float)i * INTERNAL_MAP_INCREMENT_DEG +
150 msg_index =
ceil(wrap_360(bin_angle_deg - obstacle.
angle_offset) * increment_factor);
153 if (obstacle.
distances[msg_index] != UINT16_MAX) {
165 (
double)obstacle.
frame);
178 uint16_t sensor_range_cm = (int)(100 * sensor_range);
180 if (sensor_reading < sensor_range) {
216 (distance_sensor.orientation != distance_sensor_s::ROTATION_DOWNWARD_FACING) &&
217 (distance_sensor.orientation != distance_sensor_s::ROTATION_UPWARD_FACING)) {
222 (uint16_t)(distance_sensor.max_distance * 100.0f));
224 (uint16_t)(distance_sensor.min_distance * 100.0f));
258 if ((distance_reading > distance_sensor.
min_distance)) {
265 INTERNAL_MAP_INCREMENT_DEG);
267 INTERNAL_MAP_INCREMENT_DEG);
270 if (lower_bound < 0) { lower_bound++; }
272 if (upper_bound < 0) { upper_bound++; }
277 float sensor_dist_scale = cosf(
Eulerf(attitude_sensor_frame).theta());
280 distance_reading = distance_reading * sensor_dist_scale;
283 uint16_t sensor_range = (int)(100 * distance_sensor.
max_distance);
285 for (
int bin = lower_bound; bin <= upper_bound; ++bin) {
286 int wrapped_bin = wrap_bin(bin);
300 const float col_prev_d = _param_cp_dist.get();
301 const int guidance_bins =
floor(_param_cp_guide_ang.get() / INTERNAL_MAP_INCREMENT_DEG);
302 const int sp_index_original = setpoint_index;
303 float best_cost = 9999.f;
304 int new_sp_index = setpoint_index;
306 for (
int i = sp_index_original - guidance_bins; i <= sp_index_original + guidance_bins; i++) {
309 const int filter_size = 1;
312 for (
int j = i - filter_size; j <= i + filter_size; j++) {
313 int bin = wrap_bin(j);
316 mean_dist += col_prev_d * 100.f;
323 const int bin = wrap_bin(i);
324 mean_dist = mean_dist / (2.f * filter_size + 1.f);
325 const float deviation_cost = col_prev_d * 50.f *
abs(i - sp_index_original);
329 best_cost = bin_cost;
335 if (new_sp_index != setpoint_index) {
337 angle =
wrap_2pi(vehicle_yaw_angle_rad + angle);
338 setpoint_dir = {cosf(angle), sinf(angle)};
339 setpoint_index = new_sp_index;
344 CollisionPrevention::_sensorOrientationToYawOffset(
const distance_sensor_s &distance_sensor,
float angle_offset)
const 346 float offset = angle_offset > 0.0f ?
math::radians(angle_offset) : 0.0f;
349 case distance_sensor_s::ROTATION_YAW_0:
362 offset = 3.0f *
M_PI_F / 4.0f;
370 offset = -3.0f *
M_PI_F / 4.0f;
381 case distance_sensor_s::ROTATION_CUSTOM:
396 const float col_prev_d = _param_cp_dist.get();
397 const float col_prev_dly = _param_cp_delay.get();
398 const bool move_no_data = _param_cp_go_nodata.get() > 0;
399 const float xy_p = _param_mpc_xy_p.get();
400 const float max_jerk = _param_mpc_jerk_max.get();
401 const float max_accel = _param_mpc_acc_hor.get();
403 const float vehicle_yaw_angle_rad =
Eulerf(attitude).psi();
405 const float setpoint_length = setpoint.
norm();
410 if (setpoint_length > 0.001
f) {
412 Vector2f setpoint_dir = setpoint / setpoint_length;
413 float vel_max = setpoint_length;
416 const float sp_angle_body_frame = atan2f(setpoint_dir(1), setpoint_dir(0)) - vehicle_yaw_angle_rad;
417 const float sp_angle_with_offset_deg = wrap_360(
math::degrees(sp_angle_body_frame) -
419 int sp_index =
floor(sp_angle_with_offset_deg / INTERNAL_MAP_INCREMENT_DEG);
425 for (
int i = 0; i < INTERNAL_MAP_USED_BINS; i++) {
439 angle =
wrap_2pi(vehicle_yaw_angle_rad + angle);
442 const Vector2f bin_direction = {cosf(angle), sinf(angle)};
447 if (setpoint_dir.
dot(bin_direction) > 0) {
449 const float curr_vel_parallel =
math::max(0.
f, curr_vel.
dot(bin_direction));
450 float delay_distance = curr_vel_parallel * col_prev_dly;
452 if (distance < max_range) {
453 delay_distance += curr_vel_parallel * (data_age * 1e-6
f);
456 const float stop_distance =
math::max(0.
f, distance - min_dist_to_keep - delay_distance);
457 const float vel_max_posctrl = xy_p * stop_distance;
460 const float projection = bin_direction.
dot(setpoint_dir);
461 float vel_max_bin = vel_max;
463 if (projection > 0.01
f) {
464 vel_max_bin =
math::min(vel_max_posctrl, vel_max_smooth) / projection;
468 if (vel_max_bin >= 0) {
469 vel_max =
math::min(vel_max, vel_max_bin);
478 setpoint = setpoint_dir * vel_max;
484 setpoint = setpoint * vel_max;
508 Vector2f new_setpoint = original_setpoint;
512 bool currently_interfering = (new_setpoint(0) < original_setpoint(0) - 0.05f * max_speed
513 || new_setpoint(0) > original_setpoint(0) + 0.05f * max_speed
514 || new_setpoint(1) < original_setpoint(1) - 0.05f * max_speed
515 || new_setpoint(1) > original_setpoint(1) + 0.05f * max_speed);
529 original_setpoint.
copyTo(constraints.original_setpoint);
530 new_setpoint.
copyTo(constraints.adapted_setpoint);
533 original_setpoint = new_setpoint;
540 command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
#define mavlink_log_info(_pub, _text,...)
Send a mavlink info message (not printed to console).
void _publishVehicleCmdDoLoiter()
Publishes vehicle command.
#define mavlink_log_critical(_pub, _text,...)
Send a mavlink critical message and print to console.
void _updateObstacleMap()
Aggregates the sensor data into a internal obstacle map in body frame.
bool _enterData(int map_index, float sensor_range, float sensor_reading)
Determines whether a new sensor measurement is used.
Type wrap(Type x, Type low, Type high)
Wrap value to stay in range [low, high)
bool _was_active
states if the collision prevention interferes with the user input
bool is_active()
Returs true if Collision Prevention is running.
static constexpr uint64_t RANGE_STREAM_TIMEOUT_US
bool _interfering
states if the collision prevention interferes with the user input
virtual ~CollisionPrevention()
bool publish(const T &data)
Publish the struct.
Dual< Scalar, N > ceil(const Dual< Scalar, N > &a)
constexpr T degrees(T radians)
void copyTo(Type dst[M *N]) const
void _calculateConstrainedSetpoint(matrix::Vector2f &setpoint, const matrix::Vector2f &curr_pos, const matrix::Vector2f &curr_vel)
Computes collision free setpoints.
obstacle_distance_s _obstacle_map_body_frame
Type wrap_2pi(Type x)
Wrap value in range [0, 2π)
void _adaptSetpointDirection(matrix::Vector2f &setpoint_dir, int &setpoint_index, float vehicle_yaw_angle_rad)
Computes an adaption to the setpoint direction to guide towards free space.
virtual hrt_abstime getElapsedTime(const hrt_abstime *ptr)
void rotate(const AxisAngle< Type > &vec)
Rotate quaternion from rotation vector.
bool publish(const T &data)
Publish the struct.
CollisionPrevention(ModuleParams *parent)
uint64_t _data_timestamps[sizeof(_obstacle_map_body_frame.distances)/sizeof(_obstacle_map_body_frame.distances[0])]
uORB::PublicationQueued< vehicle_command_s > _vehicle_command_pub
vehicle command do publication
Type dot(const MatrixM1 &b) const
constexpr T radians(T degrees)
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
hrt_abstime _time_activated
hrt_abstime _last_timeout_warning
virtual hrt_abstime getTime()
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
constexpr _Tp min(_Tp a, _Tp b)
uORB::SubscriptionData< obstacle_distance_s > _sub_obstacle_distance
obstacle distances received form a range sensor
float computeMaxSpeedFromDistance(const float jerk, const float accel, const float braking_distance, const float final_speed)
Vector3< float > Vector3f
static constexpr uint64_t TIMEOUT_HOLD_US
uORB::Subscription _sub_distance_sensor[ORB_MULTI_MAX_INSTANCES]
distance data received from onboard rangefinders
constexpr _Tp max(_Tp a, _Tp b)
Quaternion< float > Quatf
orb_advert_t _mavlink_log_pub
Mavlink log uORB handle.
#define ORB_MULTI_MAX_INSTANCES
Maximum number of multi topic instances.
uORB::Publication< collision_constraints_s > _constraints_pub
constraints publication
int orb_unadvertise(orb_advert_t handle)
uORB::Publication< obstacle_distance_s > _obstacle_distance_pub
obstacle_distance publication
hrt_abstime _last_collision_warning
void modifySetpoint(matrix::Vector2f &original_setpoint, const float max_speed, const matrix::Vector2f &curr_pos, const matrix::Vector2f &curr_vel)
Computes collision free setpoints.
void _addObstacleSensorData(const obstacle_distance_s &obstacle, const matrix::Quatf &vehicle_attitude)
Updates obstacle distance message with measurement from offboard.
uORB::SubscriptionData< vehicle_attitude_s > _sub_vehicle_attitude
uint16_t _data_maxranges[sizeof(_obstacle_map_body_frame.distances)/sizeof(_obstacle_map_body_frame.distances[0])]
in cm
Dual< Scalar, N > abs(const Dual< Scalar, N > &a)
bool copy(void *dst)
Copy the struct.
Dual< Scalar, N > floor(const Dual< Scalar, N > &a)
void _addDistanceSensorData(distance_sensor_s &distance_sensor, const matrix::Quatf &vehicle_attitude)
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).