SF45: naming suggestions for horizontal plane projection

PX4 · Jan 8, 2025 · 673d58d · 673d58d
1 parent f04210d
commit 673d58d
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 13 deletions.
diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp
@@ -660,7 +660,7 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m)
 					}
 				}
 
-				_scale_dist(*distance_m, scaled_yaw, _vehicle_attitude);
+				_project_distance_on_horizontal_plane(*distance_m, scaled_yaw, _vehicle_attitude);
 
 				if (_current_bin_dist > _obstacle_distance.max_distance) {
 					_current_bin_dist = _obstacle_distance.max_distance + 1; // As per ObstacleDistance.msg definition
@@ -741,18 +741,17 @@ void SF45LaserSerial::_handle_missed_bins(uint8_t current_bin, uint8_t previous_
 		}
 	}
 }
-void 	SF45LaserSerial::_scale_dist(float &distance, const int16_t &yaw,  const matrix::Quatf &attitude)
+void SF45LaserSerial::_project_distance_on_horizontal_plane(float &distance, const int16_t &yaw,
+		const matrix::Quatf &q_world_vehicle)
 {
-	const Quatf q_sensor(Quatf(cosf(yaw / 2.f), 0.f, 0.f, sinf(yaw / 2.f)));
-	const Vector3f forward_vector(1.0f, 0.0f, 0.0f);
-
-	const Quatf q_sensor_rotation = attitude * q_sensor;
-
-	const Vector3f rotated_sensor_vector = q_sensor_rotation.rotateVector(forward_vector);
-
-	float sensor_dist_scale = rotated_sensor_vector.xy().norm();
-	sensor_dist_scale = math::constrain(sensor_dist_scale, FLT_EPSILON, 1.0f); // limit it to the expected range.
-	distance = distance * sensor_dist_scale;
+	const Quatf q_vehicle_sensor(Quatf(cosf(yaw / 2.f), 0.f, 0.f, sinf(yaw / 2.f)));
+	const Quatf q_world_sensor = q_world_vehicle * q_vehicle_sensor;
+	const Vector3f forward(1.f, 0.f, 0.f);
+	const Vector3f sensor_direction_in_world = q_world_sensor.rotateVector(forward);
+
+	float horizontal_projection_scale = sensor_direction_in_world.xy().norm();
+	horizontal_projection_scale = math::constrain(horizontal_projection_scale, FLT_EPSILON, 1.0f);
+	distance *= horizontal_projection_scale;
 }
 
 uint8_t SF45LaserSerial::sf45_convert_angle(const int16_t yaw)

diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp
@@ -114,7 +114,7 @@ class SF45LaserSerial : public px4::ScheduledWorkItem
 	bool				_crc_valid{false};
 
 	void 				_handle_missed_bins(uint8_t current_bin, uint8_t previous_bin, uint16_t measurement, hrt_abstime now);
-	void 				_scale_dist(float &distance, const int16_t &yaw,  const matrix::Quatf &attitude);
+	void _project_distance_on_horizontal_plane(float &distance, const int16_t &yaw, const matrix::Quatf &q_world_vehicle);
 	void 				_publish_obstacle_msg(hrt_abstime now);
 	uORB::Subscription 		_vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
 	uint64_t			_data_timestamps[BIN_COUNT];