sf45/collision-prevention: replace repeated code with ObstacleMath library functions.

src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp

@@ -620,7 +620,8 @@ void SF45LaserSerial::sf45_process_replies()
 			float distance_m = raw_distance * SF45_SCALE_FACTOR;
 			_current_bin_dist = ((uint16_t)raw_distance < _current_bin_dist) ? (uint16_t)raw_distance : _current_bin_dist;
 
-			uint8_t current_bin = sf45_convert_angle(scaled_yaw);
+			// Find bin index for the current sensor yaw angle (in sensor frame)
+			const int current_bin = ObstacleMath::get_bin_at_angle(_obstacle_distance.increment, scaled_yaw_sensor_frame);
 
 			if (current_bin != _previous_bin) {
 				PX4_DEBUG("scaled_yaw: \t %f, \t current_bin: \t %d, \t distance: \t %8.4f\n", (double)scaled_yaw, current_bin,
@@ -702,21 +703,6 @@ void SF45LaserSerial::_handle_missed_bins(uint8_t current_bin, uint8_t previous_
 	}
 }
 
-
-uint8_t SF45LaserSerial::sf45_convert_angle(const int16_t yaw)
-{
-	uint8_t mapped_sector = 0;
-	float adjusted_yaw = sf45_wrap_360(yaw - _obstacle_distance.angle_offset);
-	mapped_sector = floor(adjusted_yaw / _obstacle_distance.increment);
-
-	return mapped_sector;
-}
-
-float SF45LaserSerial::sf45_wrap_360(float f)
-{
-	return matrix::wrap(f, 0.f, 360.f);
-}
-
 uint16_t SF45LaserSerial::sf45_format_crc(uint16_t crc, uint8_t data_val)
 {
 	uint32_t i;

src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp

@@ -74,10 +74,6 @@ enum SF45_PARSED_STATE {
 };
 
 enum SensorOrientation {	  // Direction the sensor faces from MAV_SENSOR_ORIENTATION enum
-	ROTATION_FORWARD_FACING = 0,  // MAV_SENSOR_ROTATION_NONE
-	ROTATION_RIGHT_FACING = 2,    // MAV_SENSOR_ROTATION_YAW_90
-	ROTATION_BACKWARD_FACING = 4, // MAV_SENSOR_ROTATION_YAW_180
-	ROTATION_LEFT_FACING = 6,     // MAV_SENSOR_ROTATION_YAW_270
 	ROTATION_UPWARD_FACING = 24,  // MAV_SENSOR_ROTATION_PITCH_90
 	ROTATION_DOWNWARD_FACING = 25 // MAV_SENSOR_ROTATION_PITCH_270
 };
@@ -95,8 +91,6 @@ public:
 	void				sf45_send(uint8_t msg_id, bool r_w, int32_t *data, uint8_t data_len);
 	uint16_t			sf45_format_crc(uint16_t crc, uint8_t data_value);
 	void				sf45_process_replies();
-	uint8_t				sf45_convert_angle(const int16_t yaw);
-	float				sf45_wrap_360(float f);
 
 private:
 	obstacle_distance_s 			_obstacle_distance{};

src/lib/collision_prevention/CollisionPrevention.cpp

@@ -236,14 +236,14 @@ void CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obst
 		// corresponding data index (convert to world frame and shift by msg offset)
 		for (int i = 0; i < BIN_COUNT; i++) {
 			for (int j = 0; (j < 360 / obstacle.increment) && (j < BIN_COUNT); j++) {
-				float bin_lower_angle = _wrap_360((float)i * _obstacle_map_body_frame.increment + _obstacle_map_body_frame.angle_offset
+				float bin_lower_angle = ObstacleMath::get_lower_bound_angle(i, _obstacle_map_body_frame.increment,
-								  - (float)_obstacle_map_body_frame.increment / 2.f);
+							_obstacle_map_body_frame.angle_offset);
-				float bin_upper_angle = _wrap_360((float)i * _obstacle_map_body_frame.increment + _obstacle_map_body_frame.angle_offset
+				float bin_upper_angle = ObstacleMath::get_lower_bound_angle(i + 1, _obstacle_map_body_frame.increment,
-								  + (float)_obstacle_map_body_frame.increment / 2.f);
+							_obstacle_map_body_frame.angle_offset);
-				float msg_lower_angle = _wrap_360((float)j * obstacle.increment + obstacle.angle_offset - vehicle_orientation_deg -
+				float msg_lower_angle = ObstacleMath::get_lower_bound_angle(j, obstacle.increment,
-								  obstacle.increment / 2.f);
+							obstacle.angle_offset - vehicle_orientation_deg);
-				float msg_upper_angle = _wrap_360((float)j * obstacle.increment + obstacle.angle_offset - vehicle_orientation_deg +
+				float msg_upper_angle = ObstacleMath::get_lower_bound_angle(j + 1, obstacle.increment,
-								  obstacle.increment / 2.f);
+							obstacle.angle_offset - vehicle_orientation_deg);
 
 				// if a bin stretches over the 0/360 degree line, adjust the angles
 				if (bin_lower_angle > bin_upper_angle) {
@@ -277,12 +277,12 @@ void CollisionPrevention::_addObstacleSensorData(const obstacle_distance_s &obst
 		// corresponding data index (shift by msg offset)
 		for (int i = 0; i < BIN_COUNT; i++) {
 			for (int j = 0; j < 360 / obstacle.increment; j++) {
-				float bin_lower_angle = _wrap_360((float)i * _obstacle_map_body_frame.increment + _obstacle_map_body_frame.angle_offset
+				float bin_lower_angle = ObstacleMath::get_lower_bound_angle(i, _obstacle_map_body_frame.increment,
-								  - (float)_obstacle_map_body_frame.increment / 2.f);
+							_obstacle_map_body_frame.angle_offset);
-				float bin_upper_angle = _wrap_360((float)i * _obstacle_map_body_frame.increment + _obstacle_map_body_frame.angle_offset
+				float bin_upper_angle = ObstacleMath::get_lower_bound_angle(i + 1, _obstacle_map_body_frame.increment,
-								  + (float)_obstacle_map_body_frame.increment / 2.f);
+							_obstacle_map_body_frame.angle_offset);
-				float msg_lower_angle = _wrap_360((float)j * obstacle.increment + obstacle.angle_offset - obstacle.increment / 2.f);
+				float msg_lower_angle = ObstacleMath::get_lower_bound_angle(j, obstacle.increment, obstacle.angle_offset);
-				float msg_upper_angle = _wrap_360((float)j * obstacle.increment + obstacle.angle_offset + obstacle.increment / 2.f);
+				float msg_upper_angle = ObstacleMath::get_lower_bound_angle(j + 1, obstacle.increment, obstacle.angle_offset);
 
 				// if a bin stretches over the 0/360 degree line, adjust the angles
 				if (bin_lower_angle > bin_upper_angle) {
@@ -373,7 +373,7 @@ void
 CollisionPrevention::_transformSetpoint(const Vector2f &setpoint)
 {
 	const float sp_angle_body_frame = atan2f(setpoint(1), setpoint(0)) - _vehicle_yaw;
-	const float sp_angle_with_offset_deg = _wrap_360(math::degrees(sp_angle_body_frame) -
+	const float sp_angle_with_offset_deg = ObstacleMath::wrap_360(math::degrees(sp_angle_body_frame) -
 					       _obstacle_map_body_frame.angle_offset);
 	_setpoint_index = floor(sp_angle_with_offset_deg / BIN_SIZE);
 	// change setpoint direction slightly (max by _param_cp_guide_ang degrees) to help guide through narrow gaps
@@ -394,7 +394,8 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor,
 
 	// discard values below min range
 	if (distance_reading > distance_sensor.min_distance) {
-		float sensor_yaw_body_rad = _sensorOrientationToYawOffset(distance_sensor, _obstacle_map_body_frame.angle_offset);
+		float sensor_yaw_body_rad = ObstacleMath::sensor_orientation_to_yaw_offset(static_cast<ObstacleMath::SensorOrientation>
+					    (distance_sensor.orientation), distance_sensor.q);
 		float sensor_yaw_body_deg = math::degrees(wrap_2pi(sensor_yaw_body_rad));
 
 		// calculate the field of view boundary bin indices
@@ -408,7 +409,7 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor,
 		uint16_t sensor_range = static_cast<uint16_t>(100.0f * distance_sensor.max_distance + 0.5f); // convert to cm
 
 		for (int bin = lower_bound; bin <= upper_bound; ++bin) {
-			int wrapped_bin = _wrap_bin(bin);
+			int wrapped_bin = ObstacleMath::wrap_bin(bin, BIN_COUNT);
 
 			if (_enterData(wrapped_bin, distance_sensor.max_distance, distance_reading)) {
 				_obstacle_map_body_frame.distances[wrapped_bin] = static_cast<uint16_t>(100.0f * distance_reading + 0.5f);
@@ -435,7 +436,7 @@ CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoi
 		float mean_dist = 0;
 
 		for (int j = i - filter_size; j <= i + filter_size; j++) {
-			int bin = _wrap_bin(j);
+			int bin = ObstacleMath::wrap_bin(j, BIN_COUNT);
 
 			if (_obstacle_map_body_frame.distances[bin] == UINT16_MAX) {
 				mean_dist += _param_cp_dist.get() * 100.f;
@@ -445,7 +446,7 @@ CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoi
 			}
 		}
 
-		const int bin = _wrap_bin(i);
+		const int bin = ObstacleMath::wrap_bin(i, BIN_COUNT);
 		mean_dist = mean_dist / (2.f * filter_size + 1.f);
 		const float deviation_cost = _param_cp_dist.get() * 50.f * abs(i - sp_index_original);
 		const float bin_cost = deviation_cost - mean_dist - _obstacle_map_body_frame.distances[bin];
@@ -465,52 +466,6 @@ CollisionPrevention::_adaptSetpointDirection(Vector2f &setpoint_dir, int &setpoi
 	}
 }
 
-float
-CollisionPrevention::_sensorOrientationToYawOffset(const distance_sensor_s &distance_sensor, float angle_offset) const
-{
-	float offset = math::max(math::radians(angle_offset), 0.f);
-
-	switch (distance_sensor.orientation) {
-	case distance_sensor_s::ROTATION_YAW_0:
-		offset = 0.0f;
-		break;
-
-	case distance_sensor_s::ROTATION_YAW_45:
-		offset = M_PI_F / 4.0f;
-		break;
-
-	case distance_sensor_s::ROTATION_YAW_90:
-		offset = M_PI_F / 2.0f;
-		break;
-
-	case distance_sensor_s::ROTATION_YAW_135:
-		offset = 3.0f * M_PI_F / 4.0f;
-		break;
-
-	case distance_sensor_s::ROTATION_YAW_180:
-		offset = M_PI_F;
-		break;
-
-	case distance_sensor_s::ROTATION_YAW_225:
-		offset = -3.0f * M_PI_F / 4.0f;
-		break;
-
-	case distance_sensor_s::ROTATION_YAW_270:
-		offset = -M_PI_F / 2.0f;
-		break;
-
-	case distance_sensor_s::ROTATION_YAW_315:
-		offset = -M_PI_F / 4.0f;
-		break;
-
-	case distance_sensor_s::ROTATION_CUSTOM:
-		offset = Eulerf(Quatf(distance_sensor.q)).psi();
-		break;
-	}
-
-	return offset;
-}
-
 float CollisionPrevention::_getObstacleDistance(const Vector2f &direction)
 {
 	float obstacle_distance = 0.f;
@@ -520,9 +475,9 @@ float CollisionPrevention::_getObstacleDistance(const Vector2f &direction)
 		Vector2f dir = direction / direction_norm;
 		const float sp_angle_body_frame = atan2f(dir(1), dir(0)) - _vehicle_yaw;
 		const float sp_angle_with_offset_deg =
-			_wrap_360(math::degrees(sp_angle_body_frame) - _obstacle_map_body_frame.angle_offset);
+			ObstacleMath::wrap_360(math::degrees(sp_angle_body_frame) - _obstacle_map_body_frame.angle_offset);
-		int dir_index = floor(sp_angle_with_offset_deg / BIN_SIZE);
+
-		dir_index = math::constrain(dir_index, 0, BIN_COUNT - 1);
+		const int dir_index = ObstacleMath::get_bin_at_angle(BIN_SIZE, sp_angle_with_offset_deg);
 		obstacle_distance   = _obstacle_map_body_frame.distances[dir_index] * 0.01f;
 	}
 
@@ -632,19 +587,3 @@ void CollisionPrevention::_publishVehicleCmdDoLoiter()
 	command.timestamp = getTime();
 	_vehicle_command_pub.publish(command);
 }
-
-float CollisionPrevention::_wrap_360(const float f)
-{
-	return wrap(f, 0.f, 360.f);
-}
-
-int CollisionPrevention::_wrap_bin(int i)
-{
-	i = i % BIN_COUNT;
-
-	while (i < 0) {
-		i += BIN_COUNT;
-	}
-
-	return i;
-}

src/lib/collision_prevention/CollisionPrevention.hpp

@@ -204,13 +204,6 @@ private:
 		(ParamFloat<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual   /**< maximum velocity in manual flight mode*/
 	)
 
-	/**
-	 * Transforms the sensor orientation into a yaw in the local frame
-	 * @param distance_sensor, distance sensor message
-	 * @param angle_offset, sensor body frame offset
-	 */
-	float _sensorOrientationToYawOffset(const distance_sensor_s &distance_sensor, float angle_offset) const;
-
 	/**
 	 * Computes collision free setpoints
 	 * @param setpoint, setpoint before collision prevention intervention
@@ -238,6 +231,4 @@ private:
 	 */
 	void _publishVehicleCmdDoLoiter();
 
-	static float _wrap_360(const float f);
-	static int _wrap_bin(int i);
 };

src/lib/collision_prevention/ObstacleMath.cpp

@@ -57,13 +57,19 @@ int get_bin_at_angle(float bin_width, float angle)
 	return wrap_bin(bin_at_angle, 360 / bin_width);
 }
 
+float get_lower_bound_angle(int bin, float bin_width, float angle_offset)
+{
+	bin = wrap_bin(bin, 360 / bin_width);
+	return wrap_360(bin * bin_width + angle_offset - bin_width / 2.f);
+}
+
 int get_offset_bin_index(int bin, float bin_width, float angle_offset)
 {
 	int offset = get_bin_at_angle(bin_width, angle_offset);
 	return wrap_bin(bin - offset, 360 / bin_width);
 }
 
-float sensor_orientation_to_yaw_offset(const SensorOrientation orientation)
+float sensor_orientation_to_yaw_offset(const SensorOrientation orientation, const float q[4])
 {
 	float offset = 0.0f;
 
@@ -99,6 +105,13 @@ float sensor_orientation_to_yaw_offset(const SensorOrientation orientation)
 	case SensorOrientation::ROTATION_YAW_315:
 		offset = -M_PI_F / 4.0f;
 		break;
+
+	case SensorOrientation::ROTATION_CUSTOM:
+		if (q != nullptr) {
+			offset = Eulerf(Quatf(q)).psi();
+		}
+
+		break;
 	}
 
 	return offset;
@@ -109,4 +122,9 @@ int wrap_bin(int bin, int bin_count)
 	return (bin + bin_count) % bin_count;
 }
 
+float wrap_360(const float angle)
+{
+	return matrix::wrap(angle, 0.0f, 360.0f);
+}
+
 } // ObstacleMath

src/lib/collision_prevention/ObstacleMath.hpp

@@ -45,6 +45,7 @@ enum SensorOrientation {
 	ROTATION_YAW_225 = 5,	  // MAV_SENSOR_ROTATION_YAW_225
 	ROTATION_YAW_270 = 6,	  // MAV_SENSOR_ROTATION_YAW_270
 	ROTATION_YAW_315 = 7,	  // MAV_SENSOR_ROTATION_YAW_315
+	ROTATION_CUSTOM  = 100,	  // MAV_SENSOR_ROTATION_CUSTOM
 
 	ROTATION_FORWARD_FACING  = 0, // MAV_SENSOR_ROTATION_NONE
 	ROTATION_RIGHT_FACING    = 2, // MAV_SENSOR_ROTATION_YAW_90
@@ -56,7 +57,7 @@ enum SensorOrientation {
  * Converts a sensor orientation to a yaw offset
  * @param orientation sensor orientation
  */
-float sensor_orientation_to_yaw_offset(const SensorOrientation orientation);
+float sensor_orientation_to_yaw_offset(const SensorOrientation orientation, const float q[4] = nullptr);
 
 /**
  * Scales a distance measurement taken in the vehicle body horizontal plane onto the world horizontal plane
@@ -73,6 +74,14 @@ void project_distance_on_horizontal_plane(float &distance, const float yaw, cons
  */
 int get_bin_at_angle(float bin_width, float angle);
 
+/**
+ * Returns lower bound angle of a bin
+ * @param bin bin index
+ * @param bin_width width of a bin in degrees
+ * @param angle_offset clockwise angle offset in degrees
+ */
+float get_lower_bound_angle(int bin, float bin_width, float angle_offset);
+
 /**
  * Returns bin index for the current bin after an angle offset
  * @param bin current bin index
@@ -88,4 +97,11 @@ int get_offset_bin_index(int bin, float bin_width, float angle_offset);
  */
 int wrap_bin(int bin, int bin_count);
 
+/**
+ * Wraps an angle to the range [0, 360)
+ * @param angle angle in degrees
+ */
+float wrap_360(const float angle);
+
+
 } // ObstacleMath

src/lib/collision_prevention/ObstacleMathTest.cpp

@@ -133,6 +133,22 @@ TEST(ObstacleMathTest, GetBinAtAngle)
 	EXPECT_EQ(bin_index, 18);
 }
 
+TEST(ObstacleMathTest, GetLowerBound)
+{
+	// GIVEN: an invalid bin index, non-integer bin width, and a negative non-integer angle offset
+	int      bin          = -1;
+	float    bin_width    =  7.5f;
+	float    angle_offset = -4.3f;
+
+	// WHEN: we calculate the lower bound angle of the bin
+	float lower_bound = ObstacleMath::get_lower_bound_angle(bin, bin_width, angle_offset);
+
+	// THEN: the lower bound angle should be correct. The bin index is wrapped to the end and
+	// the angle offset is applied in the counter-clockwise direction.
+	EXPECT_FLOAT_EQ(lower_bound, 344.45);
+
+}
+
 
 TEST(ObstacleMathTest, OffsetBinIndex)
 {