From 02bdc2c46b833228ea41acae1d33b204ec4fe23b Mon Sep 17 00:00:00 2001 From: Martina Rivizzigno Date: Tue, 25 Jun 2019 14:58:08 +0200 Subject: [PATCH] CollisionPrevention: use FlightTasks convention for private/public methods, add doxygen on header file --- .../CollisionPrevention.cpp | 24 ++++---- .../CollisionPrevention.hpp | 60 +++++++++++++++---- 2 files changed, 59 insertions(+), 25 deletions(-) diff --git a/src/lib/CollisionPrevention/CollisionPrevention.cpp b/src/lib/CollisionPrevention/CollisionPrevention.cpp index b8067231c4..13d990b14d 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.cpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.cpp @@ -55,7 +55,7 @@ CollisionPrevention::~CollisionPrevention() } } -void CollisionPrevention::publishConstrainedSetpoint(const Vector2f &original_setpoint, +void CollisionPrevention::_publishConstrainedSetpoint(const Vector2f &original_setpoint, const Vector2f &adapted_setpoint) { collision_constraints_s constraints{}; /**< collision constraints message */ @@ -77,7 +77,7 @@ void CollisionPrevention::publishConstrainedSetpoint(const Vector2f &original_se } } -void CollisionPrevention::publishObstacleDistance(obstacle_distance_s &obstacle) +void CollisionPrevention::_publishObstacleDistance(obstacle_distance_s &obstacle) { // publish fused obtacle distance message with data from offboard obstacle_distance and distance sensor if (_obstacle_distance_pub != nullptr) { @@ -88,7 +88,7 @@ void CollisionPrevention::publishObstacleDistance(obstacle_distance_s &obstacle) } } -void CollisionPrevention::updateOffboardObstacleDistance(obstacle_distance_s &obstacle) +void CollisionPrevention::_updateOffboardObstacleDistance(obstacle_distance_s &obstacle) { _sub_obstacle_distance.update(); const obstacle_distance_s &obstacle_distance = _sub_obstacle_distance.get(); @@ -99,11 +99,9 @@ void CollisionPrevention::updateOffboardObstacleDistance(obstacle_distance_s &ob } } -void CollisionPrevention::updateDistanceSensor(obstacle_distance_s &obstacle) +void CollisionPrevention::_updateDistanceSensor(obstacle_distance_s &obstacle) { for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { - // _sub_distance_sensor[i].update(); - // const distance_sensor_s &distance_sensor = _sub_distance_sensor[i].get(); distance_sensor_s distance_sensor; _sub_distance_sensor[i].copy(&distance_sensor); @@ -136,7 +134,7 @@ void CollisionPrevention::updateDistanceSensor(obstacle_distance_s &obstacle) if ((distance_sensor.current_distance > distance_sensor.min_distance) && (distance_sensor.current_distance < distance_sensor.max_distance)) { - float sensor_yaw_body_rad = sensorOrientationToYawOffset(distance_sensor, obstacle.angle_offset); + float sensor_yaw_body_rad = _sensorOrientationToYawOffset(distance_sensor, obstacle.angle_offset); matrix::Quatf attitude = Quatf(_sub_vehicle_attitude.get().q); // convert the sensor orientation from body to local frame in the range [0, 360] @@ -183,15 +181,15 @@ void CollisionPrevention::updateDistanceSensor(obstacle_distance_s &obstacle) } } - publishObstacleDistance(obstacle); + _publishObstacleDistance(obstacle); } -void CollisionPrevention::calculateConstrainedSetpoint(Vector2f &setpoint, +void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, const Vector2f &curr_pos, const Vector2f &curr_vel) { obstacle_distance_s obstacle = {}; - updateOffboardObstacleDistance(obstacle); - updateDistanceSensor(obstacle); + _updateOffboardObstacleDistance(obstacle); + _updateDistanceSensor(obstacle); //The maximum velocity formula contains a square root, therefore the whole calculation is done with squared norms. //that way the root does not have to be calculated for every range bin but once at the end. @@ -281,7 +279,7 @@ void CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const floa { //calculate movement constraints based on range data Vector2f new_setpoint = original_setpoint; - calculateConstrainedSetpoint(new_setpoint, curr_pos, curr_vel); + _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 @@ -294,7 +292,7 @@ void CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const floa } _interfering = currently_interfering; - publishConstrainedSetpoint(original_setpoint, new_setpoint); + _publishConstrainedSetpoint(original_setpoint, new_setpoint); original_setpoint = new_setpoint; } diff --git a/src/lib/CollisionPrevention/CollisionPrevention.hpp b/src/lib/CollisionPrevention/CollisionPrevention.hpp index f49d950062..bd17c45020 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.hpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.hpp @@ -62,9 +62,18 @@ public: CollisionPrevention(ModuleParams *parent); ~CollisionPrevention(); - + /** + * Returs true if Collision Prevention is running + */ bool is_active() { return _param_mpc_col_prev_d.get() > 0; } + /** + * Computes collision free setpoints + * @param original_setpoint, setpoint before collision prevention intervention + * @param max_speed, maximum xy speed + * @param curr_pos, current vehicle position + * @param curr_vel, current vehicle velocity + */ void modifySetpoint(matrix::Vector2f &original_setpoint, const float max_speed, const matrix::Vector2f &curr_pos, const matrix::Vector2f &curr_vel); @@ -89,9 +98,12 @@ private: (ParamFloat) _param_mpc_col_prev_dly /**< delay of the range measurement data*/ ) - void update(); - - inline float sensorOrientationToYawOffset(const distance_sensor_s &distance_sensor, float angle_offset) + /** + * Transforms the sensor orientation into a yaw in the local frame + * @param distance_sensor, distance sensor message + * @param angle_offset, sensor body frame offset + */ + inline float _sensorOrientationToYawOffset(const distance_sensor_s &distance_sensor, float angle_offset) { float offset = angle_offset > 0.f ? math::radians(angle_offset) : 0.0f; @@ -117,14 +129,38 @@ private: return offset; } - void calculateConstrainedSetpoint(matrix::Vector2f &setpoint, const matrix::Vector2f &curr_pos, - const matrix::Vector2f &curr_vel); - - void publishConstrainedSetpoint(const matrix::Vector2f &original_setpoint, const matrix::Vector2f &adapted_setpoint); - void publishObstacleDistance(obstacle_distance_s &obstacle); - - void updateOffboardObstacleDistance(obstacle_distance_s &obstacle); - void updateDistanceSensor(obstacle_distance_s &obstacle); + /** + * Computes collision free setpoints + * @param setpoint, setpoint before collision prevention intervention + * @param curr_pos, current vehicle position + * @param curr_vel, current vehicle velocity + */ + void _calculateConstrainedSetpoint(matrix::Vector2f &setpoint, const matrix::Vector2f &curr_pos, + const matrix::Vector2f &curr_vel); + /** + * Publishes collision_constraints message + * @param original_setpoint, setpoint before collision prevention intervention + * @param adapted_setpoint, collision prevention adaped setpoint + */ + void _publishConstrainedSetpoint(const matrix::Vector2f &original_setpoint, const matrix::Vector2f &adapted_setpoint); + /** + * Publishes obstacle_distance message with fused data from offboard and from distance sensors + * @param obstacle, obstacle_distance message to be publsihed + */ + void _publishObstacleDistance(obstacle_distance_s &obstacle); + /** + * Updates obstacle distance message with measurement from offboard + * @param obstacle, obstacle_distance message to be updated + */ + void _updateOffboardObstacleDistance(obstacle_distance_s &obstacle); + /** + * Updates obstacle distance message with measurement from distance sensors + * @param obstacle, obstacle_distance message to be updated + */ + void _updateDistanceSensor(obstacle_distance_s &obstacle); + /** + * Publishes vehicle command. + */ void _publishVehicleCmdDoLoiter(); };