#include <CollisionPrevention.hpp>

Inheritance diagram for CollisionPrevention:

Collaboration diagram for CollisionPrevention:

Public Member Functions
	CollisionPrevention (ModuleParams *parent)

virtual	~CollisionPrevention ()

bool	is_active ()
	Returs true if Collision Prevention is running. More...

void	modifySetpoint (matrix::Vector2f &original_setpoint, const float max_speed, const matrix::Vector2f &curr_pos, const matrix::Vector2f &curr_vel)
	Computes collision free setpoints. More...

Protected Member Functions
void	_addDistanceSensorData (distance_sensor_s &distance_sensor, const matrix::Quatf &vehicle_attitude)

void	_addObstacleSensorData (const obstacle_distance_s &obstacle, const matrix::Quatf &vehicle_attitude)
	Updates obstacle distance message with measurement from offboard. More...

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. More...

bool	_enterData (int map_index, float sensor_range, float sensor_reading)
	Determines whether a new sensor measurement is used. More...

virtual hrt_abstime	getTime ()

virtual hrt_abstime	getElapsedTime (const hrt_abstime *ptr)

Protected Attributes
obstacle_distance_s	_obstacle_map_body_frame {}

uint64_t	_data_timestamps [sizeof(_obstacle_map_body_frame.distances)/sizeof(_obstacle_map_body_frame.distances[0])]

uint16_t	_data_maxranges [sizeof(_obstacle_map_body_frame.distances)/sizeof(_obstacle_map_body_frame.distances[0])]
	in cm More...

Private Member Functions
	DEFINE_PARAMETERS ((ParamFloat< px4::params::CP_DIST >) _param_cp_dist,(ParamFloat< px4::params::CP_DELAY >) _param_cp_delay,(ParamFloat< px4::params::CP_GUIDE_ANG >) _param_cp_guide_ang,(ParamFloat< px4::params::CP_GO_NO_DATA >) _param_cp_go_nodata,(ParamFloat< px4::params::MPC_XY_P >) _param_mpc_xy_p,(ParamFloat< px4::params::MPC_JERK_MAX >) _param_mpc_jerk_max,(ParamFloat< px4::params::MPC_ACC_HOR >) _param_mpc_acc_hor) float _sensorOrientationToYawOffset(const distance_sensor_s &distance_sensor
	Transforms the sensor orientation into a yaw in the local frame. More...

void	_calculateConstrainedSetpoint (matrix::Vector2f &setpoint, const matrix::Vector2f &curr_pos, const matrix::Vector2f &curr_vel)
	Computes collision free setpoints. More...

void	_publishConstrainedSetpoint (const matrix::Vector2f &original_setpoint, const matrix::Vector2f &adapted_setpoint)
	Publishes collision_constraints message. More...

void	_publishObstacleDistance (obstacle_distance_s &obstacle)
	Publishes obstacle_distance message with fused data from offboard and from distance sensors. More...

void	_updateObstacleMap ()
	Aggregates the sensor data into a internal obstacle map in body frame. More...

void	_publishVehicleCmdDoLoiter ()
	Publishes vehicle command. More...

Private Attributes
bool	_interfering {false}
	states if the collision prevention interferes with the user input More...

bool	_was_active {false}
	states if the collision prevention interferes with the user input More...

orb_advert_t	_mavlink_log_pub {nullptr}
	Mavlink log uORB handle. More...

uORB::Publication< collision_constraints_s >	_constraints_pub {ORB_ID(collision_constraints)}
	constraints publication More...

uORB::Publication< obstacle_distance_s >	_obstacle_distance_pub {ORB_ID(obstacle_distance_fused)}
	obstacle_distance publication More...

uORB::PublicationQueued< vehicle_command_s >	_vehicle_command_pub {ORB_ID(vehicle_command)}
	vehicle command do publication More...

uORB::SubscriptionData< obstacle_distance_s >	_sub_obstacle_distance {ORB_ID(obstacle_distance)}
	obstacle distances received form a range sensor More...

uORB::Subscription	_sub_distance_sensor [ORB_MULTI_MAX_INSTANCES] {{ORB_ID(distance_sensor), 0}, {ORB_ID(distance_sensor), 1}, {ORB_ID(distance_sensor), 2}, {ORB_ID(distance_sensor), 3}}
	distance data received from onboard rangefinders More...

uORB::SubscriptionData< vehicle_attitude_s >	_sub_vehicle_attitude {ORB_ID(vehicle_attitude)}

hrt_abstime	_last_collision_warning {0}

hrt_abstime	_last_timeout_warning {0}

hrt_abstime	_time_activated {0}

float angle_offset	const

Static Private Attributes
static constexpr uint64_t	RANGE_STREAM_TIMEOUT_US {500_ms}

static constexpr uint64_t	TIMEOUT_HOLD_US {5_s}

Detailed Description

Definition at line 64 of file CollisionPrevention.hpp.

Constructor & Destructor Documentation

◆ CollisionPrevention()

CollisionPrevention::CollisionPrevention ( ModuleParams * parent )

Definition at line 68 of file CollisionPrevention.cpp.

References _data_maxranges, _data_timestamps, _obstacle_map_body_frame, obstacle_distance_s::angle_offset, obstacle_distance_s::distances, getTime(), obstacle_distance_s::increment, obstacle_distance_s::max_distance, obstacle_distance_s::min_distance, and obstacle_distance_s::timestamp.

Here is the call graph for this function:

◆ ~CollisionPrevention()

CollisionPrevention::~CollisionPrevention ( )

virtual

Definition at line 90 of file CollisionPrevention.cpp.

References _mavlink_log_pub, and orb_unadvertise().

Here is the call graph for this function:

Member Function Documentation

◆ _adaptSetpointDirection()

void CollisionPrevention::_adaptSetpointDirection	(	matrix::Vector2f &	setpoint_dir,
		int &	setpoint_index,
		float	vehicle_yaw_angle_rad
	)

protected

Computes an adaption to the setpoint direction to guide towards free space.

Parameters

setpoint_dir,setpoint	direction before collision prevention intervention
setpoint_index,index	of the setpoint in the internal obstacle map
vehicle_yaw_angle_rad,vehicle	orientation

Definition at line 298 of file CollisionPrevention.cpp.

References _obstacle_map_body_frame, matrix::abs(), obstacle_distance_s::angle_offset, obstacle_distance_s::distances, matrix::floor(), M_PI_F, distance_sensor_s::orientation, distance_sensor_s::q, math::radians(), ROTATION_YAW_135, ROTATION_YAW_180, ROTATION_YAW_225, ROTATION_YAW_270, ROTATION_YAW_315, ROTATION_YAW_45, ROTATION_YAW_90, and matrix::wrap_2pi().

Referenced by _calculateConstrainedSetpoint().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ _addDistanceSensorData()

void CollisionPrevention::_addDistanceSensorData	(	distance_sensor_s &	distance_sensor,
		const matrix::Quatf &	vehicle_attitude
	)

protected

Definition at line 252 of file CollisionPrevention.cpp.

References _data_maxranges, _data_timestamps, _enterData(), _obstacle_map_body_frame, obstacle_distance_s::angle_offset, distance_sensor_s::current_distance, math::degrees(), obstacle_distance_s::distances, f(), matrix::floor(), distance_sensor_s::h_fov, distance_sensor_s::max_distance, math::min(), distance_sensor_s::min_distance, matrix::Quaternion< Type >::rotate(), obstacle_distance_s::timestamp, and matrix::wrap_2pi().

Referenced by _updateObstacleMap().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ _addObstacleSensorData()

void CollisionPrevention::_addObstacleSensorData	(	const obstacle_distance_s &	obstacle,
		const matrix::Quatf &	vehicle_attitude
	)

protected

Updates obstacle distance message with measurement from offboard.

Parameters

obstacle,obstacle_distance message to be updated

Definition at line 121 of file CollisionPrevention.cpp.

References _data_maxranges, _data_timestamps, _enterData(), _mavlink_log_pub, _obstacle_map_body_frame, obstacle_distance_s::angle_offset, matrix::ceil(), math::degrees(), obstacle_distance_s::distances, obstacle_distance_s::frame, obstacle_distance_s::increment, mavlink_log_critical, obstacle_distance_s::max_distance, and obstacle_distance_s::timestamp.

Referenced by _updateObstacleMap().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ _calculateConstrainedSetpoint()

void CollisionPrevention::_calculateConstrainedSetpoint	(	matrix::Vector2f &	setpoint,
		const matrix::Vector2f &	curr_pos,
		const matrix::Vector2f &	curr_vel
	)

private

Computes collision free setpoints.

Parameters

setpoint,setpoint	before collision prevention intervention
curr_pos,current	vehicle position
curr_vel,current	vehicle velocity

Definition at line 390 of file CollisionPrevention.cpp.

References _adaptSetpointDirection(), _data_maxranges, _data_timestamps, _last_timeout_warning, _mavlink_log_pub, _obstacle_map_body_frame, _publishVehicleCmdDoLoiter(), _sub_vehicle_attitude, _time_activated, _updateObstacleMap(), obstacle_distance_s::angle_offset, attitude, math::trajectory::computeMaxSpeedFromDistance(), math::degrees(), obstacle_distance_s::distances, matrix::Vector< Type, M >::dot(), f(), matrix::floor(), uORB::SubscriptionData< T >::get(), getElapsedTime(), getTime(), hrt_abstime, mavlink_log_critical, math::max(), math::min(), obstacle_distance_s::min_distance, matrix::Vector< Type, M >::norm(), vehicle_attitude_s::q, math::radians(), RANGE_STREAM_TIMEOUT_US, TIMEOUT_HOLD_US, obstacle_distance_s::timestamp, and matrix::wrap_2pi().

Referenced by modifySetpoint().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ _enterData()

bool CollisionPrevention::_enterData	(	int	map_index,
		float	sensor_range,
		float	sensor_reading
	)

protected

Determines whether a new sensor measurement is used.

Parameters

map_index,index	of the bin in the internal map the measurement belongs in
sensor_range,max	range of the sensor in meters
sensor_reading,distance	measurement in meters

Definition at line 170 of file CollisionPrevention.cpp.

References _data_maxranges, _obstacle_map_body_frame, and obstacle_distance_s::distances.

Referenced by _addDistanceSensorData(), and _addObstacleSensorData().

Here is the caller graph for this function:

◆ _publishConstrainedSetpoint()

void CollisionPrevention::_publishConstrainedSetpoint	(	const matrix::Vector2f &	original_setpoint,
		const matrix::Vector2f &	adapted_setpoint
	)

private

Publishes collision_constraints message.

Parameters

original_setpoint,setpoint	before collision prevention intervention
adapted_setpoint,collision	prevention adaped setpoint

◆ _publishObstacleDistance()

void CollisionPrevention::_publishObstacleDistance ( obstacle_distance_s & obstacle )

private

Publishes obstacle_distance message with fused data from offboard and from distance sensors.

Parameters

obstacle,obstacle_distance message to be publsihed

◆ _publishVehicleCmdDoLoiter()

void CollisionPrevention::_publishVehicleCmdDoLoiter ( )

private

Publishes vehicle command.

Definition at line 536 of file CollisionPrevention.cpp.

References _vehicle_command_pub, command, getTime(), uORB::PublicationQueued< T >::publish(), PX4_CUSTOM_MAIN_MODE_AUTO, and PX4_CUSTOM_SUB_MODE_AUTO_LOITER.

Referenced by _calculateConstrainedSetpoint().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ _updateObstacleMap()

void CollisionPrevention::_updateObstacleMap ( )

private

Aggregates the sensor data into a internal obstacle map in body frame.

Definition at line 202 of file CollisionPrevention.cpp.

References _addDistanceSensorData(), _addObstacleSensorData(), _obstacle_distance_pub, _obstacle_map_body_frame, _sub_distance_sensor, _sub_obstacle_distance, _sub_vehicle_attitude, uORB::Subscription::copy(), uORB::SubscriptionData< T >::get(), getElapsedTime(), obstacle_distance_s::increment, math::max(), obstacle_distance_s::max_distance, math::min(), obstacle_distance_s::min_distance, ORB_MULTI_MAX_INSTANCES, uORB::Publication< T >::publish(), vehicle_attitude_s::q, RANGE_STREAM_TIMEOUT_US, obstacle_distance_s::timestamp, and uORB::SubscriptionData< T >::update().

Referenced by _calculateConstrainedSetpoint().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ DEFINE_PARAMETERS()

CollisionPrevention::DEFINE_PARAMETERS	(	(ParamFloat< px4::params::CP_DIST >)	_param_cp_dist,
		(ParamFloat< px4::params::CP_DELAY >)	_param_cp_delay,
		(ParamFloat< px4::params::CP_GUIDE_ANG >)	_param_cp_guide_ang,
		(ParamFloat< px4::params::CP_GO_NO_DATA >)	_param_cp_go_nodata,
		(ParamFloat< px4::params::MPC_XY_P >)	_param_mpc_xy_p,
		(ParamFloat< px4::params::MPC_JERK_MAX >)	_param_mpc_jerk_max,
		(ParamFloat< px4::params::MPC_ACC_HOR >)	_param_mpc_acc_hor
	)		const &

private

Transforms the sensor orientation into a yaw in the local frame.

Parameters

distance_sensor,distance	sensor message
angle_offset,sensor	body frame offset

Parameters

_param_cp_dist	collision prevention keep minimum distance
_param_cp_delay	delay of the range measurement data
_param_cp_guide_ang	collision prevention change setpoint angle
_param_cp_go_nodata	movement allowed where no data
_param_mpc_xy_p	p gain from position controller
_param_mpc_jerk_max	vehicle maximum jerk
_param_mpc_acc_hor	vehicle maximum horizontal acceleration

◆ getElapsedTime()

hrt_abstime CollisionPrevention::getElapsedTime ( const hrt_abstime * ptr )

protectedvirtual

Reimplemented in TestTimingCollisionPrevention.

Definition at line 103 of file CollisionPrevention.cpp.

References hrt_absolute_time().

Referenced by _calculateConstrainedSetpoint(), _updateObstacleMap(), and modifySetpoint().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getTime()

hrt_abstime CollisionPrevention::getTime ( )

protectedvirtual

Reimplemented in TestTimingCollisionPrevention.

Definition at line 98 of file CollisionPrevention.cpp.

References hrt_absolute_time().

Referenced by _calculateConstrainedSetpoint(), _publishVehicleCmdDoLoiter(), CollisionPrevention(), is_active(), and modifySetpoint().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ is_active()

bool CollisionPrevention::is_active ( )

Returs true if Collision Prevention is running.

Definition at line 108 of file CollisionPrevention.cpp.

References _time_activated, _was_active, and getTime().

Referenced by FlightTaskManualPosition::_scaleSticks(), and TEST_F().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ modifySetpoint()

void CollisionPrevention::modifySetpoint	(	matrix::Vector2f &	original_setpoint,
		const float	max_speed,
		const matrix::Vector2f &	curr_pos,
		const matrix::Vector2f &	curr_vel
	)

Computes collision free setpoints.

Parameters

original_setpoint,setpoint	before collision prevention intervention
max_speed,maximum	xy speed
curr_pos,current	vehicle position
curr_vel,current	vehicle velocity

Definition at line 504 of file CollisionPrevention.cpp.

References _calculateConstrainedSetpoint(), _constraints_pub, _interfering, _last_collision_warning, _mavlink_log_pub, matrix::Matrix< Type, M, N >::copyTo(), getElapsedTime(), getTime(), mavlink_log_info, uORB::Publication< T >::publish(), and collision_constraints_s::timestamp.

Referenced by FlightTaskManualPosition::_scaleSticks(), and TEST_F().

Here is the call graph for this function:

Here is the caller graph for this function:

Member Data Documentation

◆ _constraints_pub

uORB::Publication<collision_constraints_s> CollisionPrevention::_constraints_pub {ORB_ID(collision_constraints)}

private

constraints publication

Definition at line 130 of file CollisionPrevention.hpp.

Referenced by modifySetpoint().

◆ _data_maxranges

uint16_t CollisionPrevention::_data_maxranges[sizeof(_obstacle_map_body_frame.distances)/sizeof( _obstacle_map_body_frame.distances[0])]

protected

in cm

Definition at line 91 of file CollisionPrevention.hpp.

Referenced by _addDistanceSensorData(), _addObstacleSensorData(), _calculateConstrainedSetpoint(), _enterData(), and CollisionPrevention().

◆ _data_timestamps

uint64_t CollisionPrevention::_data_timestamps[sizeof(_obstacle_map_body_frame.distances)/sizeof(_obstacle_map_body_frame.distances[0])]

protected

Definition at line 89 of file CollisionPrevention.hpp.

Referenced by _addDistanceSensorData(), _addObstacleSensorData(), _calculateConstrainedSetpoint(), and CollisionPrevention().

◆ _interfering

bool CollisionPrevention::_interfering {false}

private

states if the collision prevention interferes with the user input

Definition at line 125 of file CollisionPrevention.hpp.

Referenced by modifySetpoint().

◆ _last_collision_warning

hrt_abstime CollisionPrevention::_last_collision_warning {0}

private

Definition at line 141 of file CollisionPrevention.hpp.

Referenced by modifySetpoint().

◆ _last_timeout_warning

hrt_abstime CollisionPrevention::_last_timeout_warning {0}

private

Definition at line 142 of file CollisionPrevention.hpp.

Referenced by _calculateConstrainedSetpoint().

◆ _mavlink_log_pub

orb_advert_t CollisionPrevention::_mavlink_log_pub {nullptr}

private

Mavlink log uORB handle.

Definition at line 128 of file CollisionPrevention.hpp.

Referenced by _addObstacleSensorData(), _calculateConstrainedSetpoint(), modifySetpoint(), and ~CollisionPrevention().

◆ _obstacle_distance_pub

uORB::Publication<obstacle_distance_s> CollisionPrevention::_obstacle_distance_pub {ORB_ID(obstacle_distance_fused)}

private

obstacle_distance publication

Definition at line 131 of file CollisionPrevention.hpp.

Referenced by _updateObstacleMap().

◆ _obstacle_map_body_frame

obstacle_distance_s CollisionPrevention::_obstacle_map_body_frame {}

protected

Definition at line 88 of file CollisionPrevention.hpp.

Referenced by _adaptSetpointDirection(), _addDistanceSensorData(), _addObstacleSensorData(), _calculateConstrainedSetpoint(), _enterData(), _updateObstacleMap(), and CollisionPrevention().

◆ _sub_distance_sensor

uORB::Subscription CollisionPrevention::_sub_distance_sensor[ORB_MULTI_MAX_INSTANCES] {{ORB_ID(distance_sensor), 0}, {ORB_ID(distance_sensor), 1}, {ORB_ID(distance_sensor), 2}, {ORB_ID(distance_sensor), 3}}

private

distance data received from onboard rangefinders

Definition at line 135 of file CollisionPrevention.hpp.

Referenced by _updateObstacleMap().

◆ _sub_obstacle_distance

uORB::SubscriptionData<obstacle_distance_s> CollisionPrevention::_sub_obstacle_distance {ORB_ID(obstacle_distance)}

private

obstacle distances received form a range sensor

Definition at line 134 of file CollisionPrevention.hpp.

Referenced by _updateObstacleMap().

◆ _sub_vehicle_attitude

uORB::SubscriptionData<vehicle_attitude_s> CollisionPrevention::_sub_vehicle_attitude {ORB_ID(vehicle_attitude)}

private

Definition at line 136 of file CollisionPrevention.hpp.

Referenced by _calculateConstrainedSetpoint(), and _updateObstacleMap().

◆ _time_activated

hrt_abstime CollisionPrevention::_time_activated {0}

private

Definition at line 143 of file CollisionPrevention.hpp.

Referenced by _calculateConstrainedSetpoint(), and is_active().

◆ _vehicle_command_pub

uORB::PublicationQueued<vehicle_command_s> CollisionPrevention::_vehicle_command_pub {ORB_ID(vehicle_command)}

private

vehicle command do publication

Definition at line 132 of file CollisionPrevention.hpp.

Referenced by _publishVehicleCmdDoLoiter().

◆ _was_active

bool CollisionPrevention::_was_active {false}

private

states if the collision prevention interferes with the user input

Definition at line 126 of file CollisionPrevention.hpp.

Referenced by is_active().

◆ const

float angle_offset CollisionPrevention::const

private

Definition at line 160 of file CollisionPrevention.hpp.

◆ RANGE_STREAM_TIMEOUT_US

constexpr uint64_t CollisionPrevention::RANGE_STREAM_TIMEOUT_US {500_ms}

staticprivate

Definition at line 138 of file CollisionPrevention.hpp.

Referenced by _calculateConstrainedSetpoint(), and _updateObstacleMap().

◆ TIMEOUT_HOLD_US

constexpr uint64_t CollisionPrevention::TIMEOUT_HOLD_US {5_s}

staticprivate

Definition at line 139 of file CollisionPrevention.hpp.

Referenced by _calculateConstrainedSetpoint().

The documentation for this class was generated from the following files:

src/lib/collision_prevention/CollisionPrevention.hpp
src/lib/collision_prevention/CollisionPrevention.cpp

Public Member Functions

Protected Member Functions

Protected Attributes

Private Member Functions

Private Attributes

Static Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ CollisionPrevention()

◆ ~CollisionPrevention()

Member Function Documentation

◆ _adaptSetpointDirection()

◆ _addDistanceSensorData()

◆ _addObstacleSensorData()

◆ _calculateConstrainedSetpoint()

◆ _enterData()

◆ _publishConstrainedSetpoint()

◆ _publishObstacleDistance()

◆ _publishVehicleCmdDoLoiter()

◆ _updateObstacleMap()

◆ DEFINE_PARAMETERS()

◆ getElapsedTime()

◆ getTime()

◆ is_active()

◆ modifySetpoint()

Member Data Documentation

◆ _constraints_pub

◆ _data_maxranges

◆ _data_timestamps

◆ _interfering

◆ _last_collision_warning

◆ _last_timeout_warning

◆ _mavlink_log_pub

◆ _obstacle_distance_pub

◆ _obstacle_map_body_frame

◆ _sub_distance_sensor

◆ _sub_obstacle_distance

◆ _sub_vehicle_attitude

◆ _time_activated

◆ _vehicle_command_pub

◆ _was_active

◆ const

◆ RANGE_STREAM_TIMEOUT_US

◆ TIMEOUT_HOLD_US