SENS: SF45: Scale the measured distance with pitch and roll

src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp

@@ -38,10 +38,13 @@
 #include <termios.h>
 #include <lib/crc/crc.h>
 #include <lib/mathlib/mathlib.h>
+#include <matrix/matrix/math.hpp>
 
 #include <float.h>
 
 using namespace time_literals;
+using matrix::Quatf;
+using matrix::Vector3f;
 
 /* Configuration Constants */
 
@@ -649,11 +652,22 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m)
 				PX4_DEBUG("scaled_yaw: \t %d, \t current_bin: \t %d, \t distance: \t %8.4f\n", scaled_yaw, current_bin,
 					  (double)*distance_m);
 
+				if (_vehicle_attitude_sub.updated()) {
+					vehicle_attitude_s vehicle_attitude;
+
+					if (_vehicle_attitude_sub.copy(&vehicle_attitude)) {
+						_vehicle_attitude = Quatf(vehicle_attitude.q);
+					}
+				}
+
+				_scale_dist(*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
 				}
 
 				hrt_abstime now = hrt_absolute_time();
+
 				_handle_missed_bins(current_bin, _previous_bin, _current_bin_dist, now);
 
 				_publish_obstacle_msg(now);
@@ -727,6 +741,20 @@ 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)
+{
+	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;
+}
+
 uint8_t SF45LaserSerial::sf45_convert_angle(const int16_t yaw)
 {
 	uint8_t mapped_sector = 0;

src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp

@@ -49,7 +49,9 @@
 #include <lib/perf/perf_counter.h>
 
 #include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
 #include <uORB/topics/obstacle_distance.h>
+#include <uORB/topics/vehicle_attitude.h>
 
 #include "sf45_commands.h"
 
@@ -112,7 +114,9 @@ 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 				_publish_obstacle_msg(hrt_abstime now);
+	uORB::Subscription 		_vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
 	uint64_t			_data_timestamps[BIN_COUNT];
 
 
@@ -141,6 +145,7 @@ class SF45LaserSerial : public px4::ScheduledWorkItem
 	int32_t				_orient_cfg{0};
 	uint8_t				_previous_bin{0};
 	uint16_t			_current_bin_dist{UINT16_MAX};
+	matrix::Quatf			_vehicle_attitude{};
 
 	// end of SF45/B data members