SF45 improved datahandling and fixed from flighttesting with collisio…

…nprevention

src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp

@@ -47,9 +47,8 @@
 #define SF45_MAX_PAYLOAD 256
 #define SF45_SCALE_FACTOR 0.01f
 
-SF45LaserSerial::SF45LaserSerial(const char *port, uint8_t rotation) :
+SF45LaserSerial::SF45LaserSerial(const char *port) :
 	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)),
-	_px4_rangefinder(0, rotation),
 	_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
 	_comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": com_err"))
 {
@@ -69,15 +68,19 @@ SF45LaserSerial::SF45LaserSerial(const char *port, uint8_t rotation) :
 	}
 
 	_num_retries = 2;
-	_px4_rangefinder.set_device_id(device_id.devid);
-	_px4_rangefinder.set_device_type(DRV_DIST_DEVTYPE_LIGHTWARE_LASER);
 
 	// populate obstacle map members
 	_obstacle_map_msg.frame = obstacle_distance_s::MAV_FRAME_BODY_FRD;
-	_obstacle_map_msg.increment = 5;
-	_obstacle_map_msg.angle_offset = 2.5;
-	_obstacle_map_msg.min_distance = UINT16_MAX;
+	_obstacle_map_msg.sensor_type = obstacle_distance_s::MAV_DISTANCE_SENSOR_LASER;
+	_obstacle_map_msg.increment = 10;
+	_obstacle_map_msg.min_distance = 20;
 	_obstacle_map_msg.max_distance = 5000;
+	_obstacle_map_msg.angle_offset = 0;
+	const uint32_t internal_bins = sizeof(_obstacle_map_msg.distances) / sizeof(_obstacle_map_msg.distances[0]);
+
+	for (uint32_t i = 0 ; i < internal_bins; i++) {
+		_obstacle_map_msg.distances[i] = UINT16_MAX;
+	}
 
 }
 
@@ -91,16 +94,12 @@ SF45LaserSerial::~SF45LaserSerial()
 
 int SF45LaserSerial::init()
 {
-
 	param_get(param_find("SF45_UPDATE_CFG"), &_update_rate);
 	param_get(param_find("SF45_ORIENT_CFG"), &_orient_cfg);
 	param_get(param_find("SF45_YAW_CFG"), &_yaw_cfg);
 
 	/* SF45/B (50M) */
-	_px4_rangefinder.set_min_distance(0.2f);
-	_px4_rangefinder.set_max_distance(50.0f);
 	_interval = 10000;
-
 	start();
 
 	return PX4_OK;
@@ -161,7 +160,6 @@ int SF45LaserSerial::collect()
 	float distance_m = -1.0f;
 
 	/* read from the sensor (uart buffer) */
-	const hrt_abstime timestamp_sample = hrt_absolute_time();
 
 
 
@@ -214,7 +212,6 @@ int SF45LaserSerial::collect()
 		// Stream data from sensor
 
 	} else {
-
 		ret = ::read(_fd, &readbuf[0], 10);
 
 		if (ret < 0) {
@@ -262,7 +259,7 @@ int SF45LaserSerial::collect()
 	}
 
 	PX4_DEBUG("val (float): %8.4f, raw: %s, valid: %s", (double)distance_m, _linebuf, ((_crc_valid) ? "OK" : "NO"));
-	_px4_rangefinder.update(timestamp_sample, distance_m);
+
 
 	perf_end(_sample_perf);
 
@@ -687,8 +684,6 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m)
 {
 	switch (rx_field.msg_id) {
 	case SF_DISTANCE_DATA_CM: {
-
-			uint16_t obstacle_dist_cm = 0;
 			const float raw_distance = (rx_field.data[0] << 0) | (rx_field.data[1] << 8);
 			int16_t raw_yaw = ((rx_field.data[2] << 0) | (rx_field.data[3] << 8));
 			int16_t scaled_yaw = 0;
@@ -700,18 +695,18 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m)
 			}
 
 			// The sensor is facing downward, so the sensor is flipped about it's x-axis -inverse of each yaw angle
-			if (_orient_cfg == 1) {
+			if (_orient_cfg == ROTATION_DOWNWARD_FACING) {
 				raw_yaw = raw_yaw * -1;
 			}
 
 			// SF45/B product guide {Data output bit: 8 Description: "Yaw angle [1/100 deg] size: int16}"
 			scaled_yaw = raw_yaw * SF45_SCALE_FACTOR;
 
 			switch (_yaw_cfg) {
-			case 0:
+			case ROTATION_FORWARD_FACING:
 				break;
 
-			case 1:
+			case ROTATION_BACKWARD_FACING:
 				if (scaled_yaw > 180) {
 					scaled_yaw = scaled_yaw - 180;
 
@@ -721,37 +716,41 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m)
 
 				break;
 
-			case 2:
+			case ROTATION_RIGHT_FACING:
 				scaled_yaw = scaled_yaw + 90; // rotation facing right
 				break;
 
-			case 3:
+			case ROTATION_LEFT_FACING:
 				scaled_yaw = scaled_yaw - 90; // rotation facing left
 				break;
 
 			default:
 				break;
 			}
 
-			// Convert to meters for rangefinder update
+			// Convert to meters for the debug message
 			*distance_m = raw_distance * SF45_SCALE_FACTOR;
-			obstacle_dist_cm = (uint16_t)raw_distance;
+			_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);
 
 			// If we have moved to a new bin
 
 			if (current_bin != _previous_bin) {
 
+				if (_current_bin_dist > _obstacle_map_msg.max_distance) {
+					_current_bin_dist = _obstacle_map_msg.max_distance + 1; // As per ObstacleDistance.msg definition
+				}
+
 				// update the current bin to the distance sensor reading
 				// readings in cm
-				_obstacle_map_msg.distances[current_bin] = obstacle_dist_cm;
+				_obstacle_map_msg.distances[current_bin] = _current_bin_dist;
 				_obstacle_map_msg.timestamp = hrt_absolute_time();
 
+				_current_bin_dist = UINT16_MAX;
+				_previous_bin = current_bin;
 			}
 
-			_previous_bin = current_bin;
-
 			_obstacle_distance_pub.publish(_obstacle_map_msg);
 
 			break;
@@ -768,7 +767,7 @@ uint8_t SF45LaserSerial::sf45_convert_angle(const int16_t yaw)
 
 	uint8_t mapped_sector = 0;
 	float adjusted_yaw = sf45_wrap_360(yaw - _obstacle_map_msg.angle_offset);
-	mapped_sector = round(adjusted_yaw / _obstacle_map_msg.increment);
+	mapped_sector = floor(adjusted_yaw / _obstacle_map_msg.increment);
 
 	return mapped_sector;
 }

src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp

@@ -63,10 +63,19 @@ enum SF_SERIAL_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
+};
 class SF45LaserSerial : public px4::ScheduledWorkItem
 {
 public:
-	SF45LaserSerial(const char *port, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
+	SF45LaserSerial(const char *port);
 	~SF45LaserSerial() override;
 
 	int 			        init();
@@ -77,27 +86,25 @@ class SF45LaserSerial : public px4::ScheduledWorkItem
 	void                            sf45_process_replies(float *data);
 	uint8_t                         sf45_convert_angle(const int16_t yaw);
 	float                           sf45_wrap_360(float f);
-protected:
-	obstacle_distance_s                       _obstacle_map_msg{};
-	uORB::Publication<obstacle_distance_s>		_obstacle_distance_pub{ORB_ID(obstacle_distance)};	/**< obstacle_distance publication */
 
 private:
+	obstacle_distance_s 			_obstacle_map_msg{};
+	uORB::Publication<obstacle_distance_s>	_obstacle_distance_pub{ORB_ID(obstacle_distance)};	/**< obstacle_distance publication */
 
 	void				start();
 	void				stop();
 	void				Run() override;
 	int				measure();
 	int				collect();
 	bool                            _crc_valid{false};
-	PX4Rangefinder                  _px4_rangefinder;
 
 	char 				_port[20] {};
-	int	        _interval{10000};
+	int	        		_interval{10000};
 	bool				_collect_phase{false};
 	int 				_fd{-1};
-	int         _linebuf[256] {};
-	unsigned		_linebuf_index{0};
-	hrt_abstime _last_read{0};
+	int         			_linebuf[256] {};
+	unsigned			_linebuf_index{0};
+	hrt_abstime 			_last_read{0};
 
 	// SF45/B uses a binary protocol to include header,flags
 	// message ID, payload, and checksum
@@ -120,12 +127,13 @@ class SF45LaserSerial : public px4::ScheduledWorkItem
 	int32_t                         _orient_cfg{0};
 	int32_t                         _collision_constraint{0};
 	uint16_t                        _previous_bin{0};
+	uint16_t			_current_bin_dist{UINT16_MAX};
 
 	// end of SF45/B data members
 
-	unsigned			                  _consecutive_fail_count;
+	unsigned			_consecutive_fail_count;
 
-	perf_counter_t			            _sample_perf;
-	perf_counter_t			            _comms_errors;
+	perf_counter_t			_sample_perf;
+	perf_counter_t			_comms_errors;
 
 };

src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial_main.cpp

@@ -41,7 +41,7 @@ namespace lightware_sf45
 
 SF45LaserSerial *g_dev{nullptr};
 
-static int start(const char *port, uint8_t rotation)
+static int start(const char *port)
 {
 	if (g_dev != nullptr) {
 		PX4_WARN("already started");
@@ -54,7 +54,7 @@ static int start(const char *port, uint8_t rotation)
 	}
 
 	/* create the driver */
-	g_dev = new SF45LaserSerial(port, rotation);
+	g_dev = new SF45LaserSerial(port);
 
 	if (g_dev == nullptr) {
 		return -1;
@@ -102,7 +102,7 @@ static int usage()
 
 Serial bus driver for the Lightware SF45/b Laser rangefinder.
 
-Setup/usage information: https://docs.px4.io/master/en/sensor/sfxx_lidar.html
+Setup/usage information: https://docs.px4.io/main/en/sensor/rangefinders.html
 
 ### Examples
 
@@ -116,7 +116,6 @@ Stop driver
 	PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor");
 	PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start driver");
 	PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, nullptr, "Serial device", false);
-	PRINT_MODULE_USAGE_PARAM_INT('R', 25, 0, 25, "Sensor rotation - downward facing by default", false);
 	PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stop driver");
 	return PX4_OK;
 }
@@ -125,18 +124,13 @@ Stop driver
 
 extern "C" __EXPORT int lightware_sf45_serial_main(int argc, char *argv[])
 {
-	uint8_t rotation = distance_sensor_s::ROTATION_FORWARD_FACING;
 	const char *device_path = nullptr;
 	int ch;
 	int myoptind = 1;
 	const char *myoptarg = nullptr;
 
 	while ((ch = px4_getopt(argc, argv, "R:d:", &myoptind, &myoptarg)) != EOF) {
 		switch (ch) {
-		case 'R':
-			rotation = (uint8_t)atoi(myoptarg);
-			break;
-
 		case 'd':
 			device_path = myoptarg;
 			break;
@@ -153,7 +147,7 @@ extern "C" __EXPORT int lightware_sf45_serial_main(int argc, char *argv[])
 	}
 
 	if (!strcmp(argv[myoptind], "start")) {
-		return lightware_sf45::start(device_path, rotation);
+		return lightware_sf45::start(device_path);
 
 	} else if (!strcmp(argv[myoptind], "stop")) {
 		return lightware_sf45::stop();

src/drivers/distance_sensor/lightware_sf45_serial/module.yaml

@@ -1,6 +1,6 @@
 module_name: Lightware SF45 Rangefinder (serial)
 serial_config:
-    - command: lightware_sf45_serial start -R 0 -d ${SERIAL_DEV}
+    - command: lightware_sf45_serial start -d ${SERIAL_DEV}
       port_config_param:
         name: SENS_EN_SF45_CFG
         group: Sensors
@@ -41,11 +41,11 @@ parameters:
                       The SF45 mounted facing upward or downward on the frame
               type: enum
               values:
-                0: Rotation upward
-                1: Rotation downward
+                24: Rotation upward
+                25: Rotation downward
               reboot_required: true
               num_instances: 1
-              default: 0
+              default: 24
 
           SF45_YAW_CFG:
               description:
@@ -55,9 +55,9 @@ parameters:
               type: enum
               values:
                 0: Rotation forward
-                1: Rotation backward
                 2: Rotation right
-                3: Rotation left
+                4: Rotation backward
+                6: Rotation left
               reboot_required: true
               num_instances: 1
               default: 0

src/modules/logger/logged_topics.cpp

@@ -323,7 +323,9 @@ void LoggedTopics::add_sensor_comparison_topics()
 void LoggedTopics::add_vision_and_avoidance_topics()
 {
 	add_topic("collision_constraints");
+	add_topic_multi("distance_sensor");
 	add_topic("obstacle_distance_fused");
+	add_topic("obstacle_distance");
 	add_topic("vehicle_mocap_odometry", 30);
 	add_topic("vehicle_trajectory_waypoint", 200);
 	add_topic("vehicle_trajectory_waypoint_desired", 200);