CollisionPrevention: use FlightTasks convention for private/public methods,

add doxygen on header file

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;
 }
 

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<px4::params::MPC_COL_PREV_DLY>) _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();
 
 };