drivers: rework NXP UWB driver (#21124)

* UWB driver rework that uses 2 UWB MKBoards - 1 as Controller (Initiator), one as Controllee (Anchor)

Co-authored-by: NXPBrianna <108274268+NXPBrianna@users.noreply.github.com>

boards/nxp/fmuk66-e/default.px4board

@@ -19,6 +19,7 @@ CONFIG_DRIVERS_IMU_BOSCH_BMI088=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
 CONFIG_DRIVERS_IRLOCK=y
+CONFIG_DRIVERS_UWB_UWB_SR150=y
 CONFIG_COMMON_LIGHT=y
 CONFIG_DRIVERS_LIGHTS_RGBLED_PWM=y
 CONFIG_COMMON_MAGNETOMETER=y

boards/nxp/fmurt1062-v1/default.px4board

@@ -22,7 +22,7 @@ CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
 CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_TONE_ALARM=y
-CONFIG_COMMON_UWB=y
+CONFIG_DRIVERS_UWB_UWB_SR150=y
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y

boards/nxp/ucans32k146/default.px4board

@@ -13,6 +13,7 @@ CONFIG_COMMON_MAGNETOMETER=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_DRIVERS_UAVCANNODE=y
+CONFIG_DRIVERS_UWB_UWB_SR150=y
 CONFIG_UAVCANNODE_ARMING_STATUS=y
 CONFIG_UAVCANNODE_BEEP_COMMAND=y
 CONFIG_UAVCANNODE_ESC_RAW_COMMAND=y

msg/CMakeLists.txt

@@ -178,6 +178,7 @@ set(msg_files
 	SensorSelection.msg
 	SensorsStatus.msg
 	SensorsStatusImu.msg
+	SensorUwb.msg
 	SystemPower.msg
 	TakeoffStatus.msg
 	TaskStackInfo.msg
@@ -194,8 +195,6 @@ set(msg_files
 	UavcanParameterValue.msg
 	UlogStream.msg
 	UlogStreamAck.msg
-	UwbDistance.msg
-	UwbGrid.msg
 	VehicleAcceleration.msg
 	VehicleAirData.msg
 	VehicleAngularAccelerationSetpoint.msg

msg/SensorUwb.msg

@@ -0,0 +1,34 @@
+# UWB distance contains the distance information measured by an ultra-wideband positioning system,
+# such as Pozyx or NXP Rddrone.
+
+uint64 	timestamp		# time since system start (microseconds)
+
+uint32 	sessionid		# UWB SessionID
+uint32 	time_offset		# Time between Ranging Rounds in ms
+uint32 	counter			# Number of Ranges since last Start of Ranging
+uint16 	mac			# MAC adress of Initiator (controller)
+
+uint16 	mac_dest		# MAC adress of Responder (Controlee)
+uint16 	status			# status feedback #
+uint8 	nlos			# None line of site condition y/n
+float32 distance		# distance in m to the UWB receiver
+
+
+#Angle of arrival, Angle in Degree -60..+60; FOV in both axis is 120 degrees
+float32 aoa_azimuth_dev	# Angle of arrival of first incomming RX msg
+float32 aoa_elevation_dev	# Angle of arrival of first incomming RX msg
+float32 aoa_azimuth_resp	# Angle of arrival of first incomming RX msg at the responder
+float32 aoa_elevation_resp	# Angle of arrival of first incomming RX msg at the responder
+
+# Figure of merit for the angle measurements
+uint8 aoa_azimuth_fom		# AOA Azimuth FOM
+uint8 aoa_elevation_fom		# AOA Elevation FOM
+uint8 aoa_dest_azimuth_fom	# AOA Azimuth FOM
+uint8 aoa_dest_elevation_fom	# AOA Elevation FOM
+
+# Initiator physical configuration
+uint8 orientation		# Direction the sensor faces from MAV_SENSOR_ORIENTATION enum
+				# Standard configuration is Antennas facing down and azimuth aligened in forward direction
+float32 offset_x		# UWB initiator offset in X axis (NED drone frame)
+float32 offset_y		# UWB initiator offset in Y axis (NED drone frame)
+float32 offset_z		# UWB initiator offset in Z axis (NED drone frame)

msg/UwbDistance.msg

@@ -1,15 +0,0 @@
-# UWB distance contains the distance information measured by an ultra-wideband positioning system,
-# such as Pozyx or NXP Rddrone.
-
-uint64 	timestamp		# time since system start (microseconds)
-uint32 	time_uwb_ms		# Time of UWB device in ms
-uint32 	counter			# Number of Ranges since last Start of Ranging
-uint32 	sessionid		# UWB SessionID
-uint32 	time_offset		# Time between Ranging Rounds in ms
-uint16 	status			# status feedback #
-
-uint16[12] anchor_distance	# distance in cm to each UWB Anchor (UWB Device which takes part in Ranging)
-bool[12] nlos			# Visual line of sight yes/no
-float32[12] aoafirst		# Angle of arrival of first incoming RX msg
-
-float32[3] position		# Position of the Landing point in relation to the Drone (x,y,z in Meters NED)

msg/UwbGrid.msg

@@ -1,25 +0,0 @@
-# UWB report message contains the grid information measured by an ultra-wideband positioning system,
-# such as Pozyx or NXP Rddrone.
-
-uint64 	timestamp	# time since system start (microseconds)
-uint16 	initator_time	# time to synchronize clocks (microseconds)
-uint8 	num_anchors	# Number of anchors
-
-float64[4] target_gps	# GPS position of target of the UWB system (Lat / Lon / Alt / Yaw Offset to true North)
-
-# Grid position information
-# Position in x,y,z in  (x,y,z in centimeters NED)
-# staring with POI and Anchor 0 up to Anchor 11
-int16[3] target_pos 	# Point of Interest, mostly landing Target x,y,z
-int16[3] anchor_pos_0
-int16[3] anchor_pos_1
-int16[3] anchor_pos_2
-int16[3] anchor_pos_3
-int16[3] anchor_pos_4
-int16[3] anchor_pos_5
-int16[3] anchor_pos_6
-int16[3] anchor_pos_7
-int16[3] anchor_pos_8
-int16[3] anchor_pos_9
-int16[3] anchor_pos_10
-int16[3] anchor_pos_11

src/drivers/uwb/uwb_sr150/module.yaml

@@ -4,12 +4,11 @@ serial_config:
     port_config_param:
       name: UWB_PORT_CFG
       group: UWB
-      default: ""
+      default: "TEL2"
 
 parameters:
     - group: UWB
       definitions:
-
         UWB_INIT_OFF_X:
           description:
             short: UWB sensor X offset in body frame
@@ -32,7 +31,7 @@ parameters:
 
         UWB_INIT_OFF_Z:
           description:
-            short: UWB sensor Y offset in body frame
+            short: UWB sensor Z offset in body frame
             long: UWB sensor positioning in relation to Drone in NED. Z offset.
           type: float
           unit: m
@@ -40,14 +39,52 @@ parameters:
           increment: 0.01
           default: 0.00
 
-        UWB_INIT_OFF_YAW:
+        UWB_SENS_ROT:
           description:
-            short: UWB sensor YAW offset in body frame
+            short: UWB sensor orientation
-            long: UWB sensor positioning in relation to Drone in NED. Yaw rotation in relation to direction of FMU.
+            long: The orientation of the sensor relative to the forward direction of the body frame. Look up table in src/lib/conversion/rotation.h
-          type: float
+                  Default position is the antannaes downward facing, UWB board parallel with body frame.
-          unit: deg
+          type: enum
-          decimal: 1
+          values:
-          increment: 0.1
+            0: ROTATION_NONE
-          default: 0.00
+            1: ROTATION_YAW_45
-
+            2: ROTATION_YAW_90
-
+            3: ROTATION_YAW_135
+            4: ROTATION_YAW_180
+            5: ROTATION_YAW_225
+            6: ROTATION_YAW_270
+            7: ROTATION_YAW_315
+            8: ROTATION_ROLL_180
+            9: ROTATION_ROLL_180_YAW_45
+            10: ROTATION_ROLL_180_YAW_90
+            11: ROTATION_ROLL_180_YAW_135
+            12: ROTATION_PITCH_180
+            13: ROTATION_ROLL_180_YAW_225
+            14: ROTATION_ROLL_180_YAW_270
+            15: ROTATION_ROLL_180_YAW_315
+            16: ROTATION_ROLL_90
+            17: ROTATION_ROLL_90_YAW_45
+            18: ROTATION_ROLL_90_YAW_90
+            19: ROTATION_ROLL_90_YAW_135
+            20: ROTATION_ROLL_270
+            21: ROTATION_ROLL_270_YAW_45
+            22: ROTATION_ROLL_270_YAW_90
+            23: ROTATION_ROLL_270_YAW_135
+            24: ROTATION_PITCH_90
+            25: ROTATION_PITCH_270
+            26: ROTATION_PITCH_180_YAW_90
+            27: ROTATION_PITCH_180_YAW_270
+            28: ROTATION_ROLL_90_PITCH_90
+            29: ROTATION_ROLL_180_PITCH_90
+            30: ROTATION_ROLL_270_PITCH_90
+            31: ROTATION_ROLL_90_PITCH_180
+            32: ROTATION_ROLL_270_PITCH_180
+            33: ROTATION_ROLL_90_PITCH_270
+            34: ROTATION_ROLL_180_PITCH_270
+            35: ROTATION_ROLL_270_PITCH_270
+            36: ROTATION_ROLL_90_PITCH_180_YAW_90
+            37: ROTATION_ROLL_90_YAW_270
+            38: ROTATION_ROLL_90_PITCH_68_YAW_293
+            39: ROTATION_PITCH_315
+            40: ROTATION_ROLL_90_PITCH_315
+          default: 0

src/drivers/uwb/uwb_sr150/uwb_sr150.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2023 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -63,128 +63,130 @@
 // The default baudrate of the uwb_sr150 module before configuration
 #define DEFAULT_BAUD B115200
 
-const uint8_t CMD_RANGING_STOP[UWB_CMD_LEN ] = {UWB_CMD, 0x00, 0x02, UWB_DRONE_CTL, UWB_CMD_STOP};
-const uint8_t CMD_RANGING_START[UWB_CMD_LEN ] = {UWB_CMD, 0x00, 0x02, UWB_DRONE_CTL, UWB_CMD_START};
-const uint8_t CMD_APP_START[UWB_CMD_LEN ] = {0x01, 0x00, 0x02, UWB_APP_START, UWB_PRECNAV_APP};
-const uint8_t CMD_APP_STOP[0x04 ] = {0x01, 0x00, 0x01, UWB_APP_STOP};
-
 extern "C" __EXPORT int uwb_sr150_main(int argc, char *argv[]);
 
-UWB_SR150::UWB_SR150(const char *device_name, speed_t baudrate, bool uwb_pos_debug):
+UWB_SR150::UWB_SR150(const char *port):
 	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)),
 	_read_count_perf(perf_alloc(PC_COUNT, "uwb_sr150_count")),
 	_read_err_perf(perf_alloc(PC_COUNT, "uwb_sr150_err"))
 {
-	_uwb_pos_debug = uwb_pos_debug;
+	/* store port name */
-	// start serial port
+	strncpy(_port, port, sizeof(_port) - 1);
-	_uart = open(device_name, O_RDWR | O_NOCTTY);
+	/* enforce null termination */
-
+	_port[sizeof(_port) - 1] = '\0';
-	if (_uart < 0) { err(1, "could not open %s", device_name); }
-
-	int ret = 0;
-	struct termios uart_config {};
-	ret = tcgetattr(_uart, &uart_config);
-
-	if (ret < 0) { err(1, "failed to get attr"); }
-
-	uart_config.c_oflag &= ~ONLCR; // no CR for every LF
-	ret = cfsetispeed(&uart_config, baudrate);
-
-	if (ret < 0) { err(1, "failed to set input speed"); }
-
-	ret = cfsetospeed(&uart_config, baudrate);
-
-	if (ret < 0) { err(1, "failed to set output speed"); }
-
-	ret = tcsetattr(_uart, TCSANOW, &uart_config);
-
-	if (ret < 0) { err(1, "failed to set attr"); }
 }
 
 UWB_SR150::~UWB_SR150()
 {
 	printf("UWB: Ranging Stopped\t\n");
-	int written = write(_uart, &CMD_APP_STOP, sizeof(CMD_APP_STOP));
-
-	if (written < (int) sizeof(CMD_APP_STOP)) {
-		PX4_ERR("Only wrote %d bytes out of %d.", written, (int) sizeof(CMD_APP_STOP));
-	}
 
+	// stop{}; will be implemented when this is changed to a scheduled work task
 	perf_free(_read_err_perf);
 	perf_free(_read_count_perf);
 
 	close(_uart);
 }
 
-void UWB_SR150::run()
+bool UWB_SR150::init()
 {
-	// Subscribe to parameter_update message
+	// execute Run() on every sensor_accel publication
-	parameters_update();
+	if (!_sensor_uwb_sub.registerCallback()) {
-
+		PX4_ERR("callback registration failed");
-	//TODO replace with BLE grid configuration
+		return false;
-	grid_info_read(&_uwb_grid_info.target_pos);
-	_uwb_grid_info.num_anchors = 4;
-	_uwb_grid_info.initator_time = hrt_absolute_time();
-	_uwb_grid_info.mac_mode = 0x00;
-
-	/* Grid Info Message*/
-	_uwb_grid.timestamp = hrt_absolute_time();
-	_uwb_grid.initator_time = _uwb_grid_info.initator_time;
-	_uwb_grid.num_anchors = _uwb_grid_info.num_anchors;
-
-	memcpy(&_uwb_grid.target_pos, &_uwb_grid_info.target_pos, sizeof(position_t) * 5); //write Grid positions
-
-	_uwb_grid_pub.publish(_uwb_grid);
-
-	/* Ranging  Command */
-	int written = write(_uart, CMD_RANGING_START, UWB_CMD_LEN);
-
-	if (written <  UWB_CMD_LEN) {
-		PX4_ERR("Only wrote %d bytes out of %d.", written, (int) sizeof(uint8_t) *  UWB_CMD_LEN);
 	}
 
-	while (!should_exit()) {
+	// alternatively, Run on fixed interval
-		written = UWB_SR150::distance(); //evaluate Ranging Messages until Stop
+	// ScheduleOnInterval(5000_us); // 2000 us interval, 200 Hz rate
-	}
 
-	if (!written) { printf("ERROR: Distance Failed"); }
+	return true;
-
-	// Automatic Stop. This should not be reachable
-	written = write(_uart, &CMD_RANGING_STOP, UWB_CMD_LEN);
-
-	if (written < (int) sizeof(CMD_RANGING_STOP)) {
-		PX4_ERR("Only wrote %d bytes out of %d.", written, (int) sizeof(CMD_RANGING_STOP));
-	}
 }
 
-void UWB_SR150::grid_info_read(position_t *grid)
+void UWB_SR150::start()
 {
-	//place holder, until UWB initiator can respond with Grid info
+	/* schedule a cycle to start things */
-	/*	This Reads the position of each Anchor in the Grid.
+	ScheduleNow();
-		Right now the Grid configuration is saved on the SD card.
+}
-		In the future, we would like, that the Initiator responds with the Grid Information (Including Position). */
-	PX4_INFO("Reading UWB GRID from SD... \t\n");
-	FILE *file;
-	file = fopen(UWB_GRID_CONFIG, "r");
 
-	int  bread = 0;
+void UWB_SR150::stop()
+{
+	ScheduleClear();
+}
 
-	for (int i = 0; i < 5; i++) {
+void UWB_SR150::Run()
-		bread += fscanf(file, "%hd,%hd,%hd\n", &grid[i].x, &grid[i].y, &grid[i].z);
+{
+	if (should_exit()) {
+		ScheduleClear();
+		exit_and_cleanup();
+		return;
 	}
 
-	if (bread == 5 * 3) {
+	if (_uart < 0) {
-		PX4_INFO("GRID INFO READ! bytes read: %d \t\n", bread);
+		/* open fd */
+		_uart = ::open(_port, O_RDWR | O_NOCTTY | O_NONBLOCK);
 
-	} else { //use UUID from Grid survey
+		if (_uart < 0) {
-		PX4_INFO("GRID INFO Missing! bytes read: %d \t\n Using standrd Grid \t\n", bread);
+			PX4_ERR("open failed (%i)", errno);
-		position_t grid_setup[5] = {{0x0, 0x0, 0x0}, {(int16_t)0xff68, 0x0, 0x0a}, {0x99, 0x0, 0x0a}, {0x0, 0x96, 0x64}, {0x0, (int16_t)0xff6a, 0x63}};
+			return;
-		memcpy(grid, &grid_setup, sizeof(grid_setup));
+		}
-		PX4_INFO("Insert \"uwb_grid_config.csv\" containing gridinfo in cm at \"/fs/microsd/etc/\".\t\n");
+
-		PX4_INFO("n + 1 Anchor Positions starting with Landing Target. Int16 Format:  \"x,y,z\" \t\n");
+		struct termios uart_config;
+
+		int termios_state;
+
+		/* fill the struct for the new configuration */
+		tcgetattr(_uart, &uart_config);
+
+		/* clear ONLCR flag (which appends a CR for every LF) */
+		uart_config.c_oflag &= ~ONLCR;
+
+		//TODO: should I keep this?
+		/* no parity, one stop bit */
+		uart_config.c_cflag &= ~(CSTOPB | PARENB);
+
+		unsigned speed = DEFAULT_BAUD;
+
+		/* set baud rate */
+		if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
+			PX4_ERR("CFG: %d ISPD", termios_state);
+		}
+
+		if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
+			PX4_ERR("CFG: %d OSPD", termios_state);
+		}
+
+		if ((termios_state = tcsetattr(_uart, TCSANOW, &uart_config)) < 0) {
+			PX4_ERR("baud %d ATTR", termios_state);
+		}
 	}
 
-	fclose(file);
+	/* perform collection */
+	int collect_ret = collectData();
+
+	if (collect_ret == -EAGAIN) {
+		/* reschedule to grab the missing bits, time to transmit 8 bytes @ 9600 bps */
+		ScheduleDelayed(1042 * 8);
+
+		return;
+	}
+
+	if (OK != collect_ret) {
+
+		/* we know the sensor needs about four seconds to initialize */
+		if (hrt_absolute_time() > 5 * 1000 * 1000LL && _consecutive_fail_count < 5) {
+			PX4_ERR("collection error #%u", _consecutive_fail_count);
+		}
+
+		_consecutive_fail_count++;
+
+		/* restart the measurement state machine */
+		start();
+		return;
+
+	} else {
+		/* apparently success */
+		_consecutive_fail_count = 0;
+	}
 }
 
 int UWB_SR150::custom_command(int argc, char *argv[])
@@ -214,43 +216,20 @@ $ uwb start -d /dev/ttyS2
 	PRINT_MODULE_USAGE_COMMAND("start");
 	PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, "<file:dev>", "Name of device for serial communication with UWB", false);
 	PRINT_MODULE_USAGE_PARAM_STRING('b', nullptr, "<int>", "Baudrate for serial communication", false);
-	PRINT_MODULE_USAGE_PARAM_STRING('p', nullptr, "<int>", "Position Debug: displays errors in Multilateration", false);
 	PRINT_MODULE_USAGE_COMMAND("stop");
 	PRINT_MODULE_USAGE_COMMAND("status");
 	return 0;
 }
 
 int UWB_SR150::task_spawn(int argc, char *argv[])
-{
-	int task_id = px4_task_spawn_cmd(
-			      "uwb_driver",
-			      SCHED_DEFAULT,
-			      SCHED_PRIORITY_DEFAULT,
-			      2048,
-			      &run_trampoline,
-			      argv
-		      );
-
-	if (task_id < 0) {
-		return -errno;
-
-	} else {
-		_task_id = task_id;
-		return 0;
-	}
-}
-
-UWB_SR150 *UWB_SR150::instantiate(int argc, char *argv[])
 {
 	int ch;
 	int option_index = 1;
 	const char *option_arg;
-	const char *device_name = nullptr;
+	const char *device_name = UWB_DEFAULT_PORT;
-	bool error_flag = false;
 	int baudrate = 0;
-	bool uwb_pos_debug = false; // Display UWB position calculation debug Messages
 
-	while ((ch = px4_getopt(argc, argv, "d:b:p", &option_index, &option_arg)) != EOF) {
+	while ((ch = px4_getopt(argc, argv, "d:b", &option_index, &option_arg)) != EOF) {
 		switch (ch) {
 		case 'd':
 			device_name = option_arg;
@@ -260,47 +239,54 @@ UWB_SR150 *UWB_SR150::instantiate(int argc, char *argv[])
 			px4_get_parameter_value(option_arg, baudrate);
 			break;
 
-		case 'p':
-
-			uwb_pos_debug = true;
-			break;
-
 		default:
 			PX4_WARN("Unrecognized flag: %c", ch);
-			error_flag = true;
 			break;
 		}
 	}
 
-	if (!error_flag && device_name == nullptr) {
+	UWB_SR150 *instance = new UWB_SR150(device_name);
-		print_usage("Device name not provided. Using default Device: TEL1:/dev/ttyS4 \n");
-		device_name = "TEL2";
-		error_flag = true;
-	}
 
-	if (!error_flag && baudrate == 0) {
+	if (instance) {
-		printf("Baudrate not provided. Using default Baud: 115200 \n");
+		_object.store(instance);
-		baudrate = B115200;
+		_task_id = task_id_is_work_queue;
-	}
 
-	if (!error_flag && uwb_pos_debug == true) {
+		instance->ScheduleOnInterval(5000_us);
-		printf("UWB Position algorithm Debugging \n");
-	}
 
-	if (error_flag) {
+		if (instance->init()) {
-		PX4_WARN("Failed to start UWB driver. \n");
+			return PX4_OK;
-		return nullptr;
+		}
 
 	} else {
-		PX4_INFO("Constructing UWB_SR150. Device: %s", device_name);
+		PX4_ERR("alloc failed");
-		return new UWB_SR150(device_name, baudrate, uwb_pos_debug);
+	}
+
+	delete instance;
+	_object.store(nullptr);
+	_task_id = -1;
+
+	return PX4_ERROR;
+}
+
+int uwb_sr150_main(int argc, char *argv[])
+{
+	return UWB_SR150::main(argc, argv);
+}
+
+void UWB_SR150::parameters_update()
+{
+	if (_parameter_update_sub.updated()) {
+		parameter_update_s param_update;
+		_parameter_update_sub.copy(&param_update);
+
+		// If any parameter updated, call updateParams() to check if
+		// this class attributes need updating (and do so).
+		updateParams();
 	}
 }
 
-int UWB_SR150::distance()
+int UWB_SR150::collectData()
 {
-	_uwb_init_offset_v3f = matrix::Vector3f(_uwb_init_off_x.get(), _uwb_init_off_y.get(),
-						_uwb_init_off_z.get()); //set offset at the start
 	uint8_t *buffer = (uint8_t *) &_distance_result_msg;
 
 	FD_ZERO(&_uart_set);
@@ -350,366 +336,54 @@ int UWB_SR150::distance()
 	perf_count(_read_count_perf);
 
 	// All of the following criteria must be met for the message to be acceptable:
-	//  - Size of message == sizeof(distance_msg_t) (51 bytes)
+	//  - Size of message == sizeof(distance_msg_t) (36 bytes)
 	//  - status == 0x00
-	//  - Values of all 3 position measurements are reasonable
+	bool ok = (buffer_location == sizeof(distance_msg_t) && _distance_result_msg.stop == 0x1b);
-	//      (If one or more anchors is missed, then position might be an unreasonably large number.)
-	bool ok = (buffer_location == sizeof(distance_msg_t) && _distance_result_msg.stop == 0x1b); //||
-	//(buffer_location == sizeof(grid_msg_t) && _distance_result_msg.stop == 0x1b)
-	//);
 
 	if (ok) {
 
 		/* Ranging Message*/
-		_uwb_distance.timestamp = hrt_absolute_time();
+		_sensor_uwb.timestamp 		= hrt_absolute_time();
-		_uwb_distance.time_uwb_ms = _distance_result_msg.time_uwb_ms;
-		_uwb_distance.counter = _distance_result_msg.seq_ctr;
-		_uwb_distance.sessionid = _distance_result_msg.sessionId;
-		_uwb_distance.time_offset = _distance_result_msg.range_interval;
 
-		for (int i = 0; i < 4; i++) {
+		_sensor_uwb.sessionid 		= _distance_result_msg.sessionId;
-			_uwb_distance.anchor_distance[i] = _distance_result_msg.measurements[i].distance;
+		_sensor_uwb.time_offset 	= _distance_result_msg.range_interval;
-			_uwb_distance.nlos[i] = _distance_result_msg.measurements[i].nLos;
+		_sensor_uwb.counter 		= _distance_result_msg.seq_ctr;
-			_uwb_distance.aoafirst[i] = float(_distance_result_msg.measurements[i].aoaFirst) /
+		_sensor_uwb.mac			= _distance_result_msg.MAC;
-						    128; // Angle of Arrival has Format Q9.7; dividing by 2^7 results in the correct value
-		}
 
-		// Algorithm goes here
+		_sensor_uwb.mac_dest		= _distance_result_msg.measurements.MAC;
-		UWB_POS_ERROR_CODES UWB_POS_ERROR = UWB_SR150::localization();
+		_sensor_uwb.status		= _distance_result_msg.measurements.status;
+		_sensor_uwb.nlos 		= _distance_result_msg.measurements.nLos;
+		_sensor_uwb.distance 		= double(_distance_result_msg.measurements.distance) / 100;
 
-		_uwb_distance.status = UWB_POS_ERROR;
 
-		if (UWB_OK == UWB_POS_ERROR) {
+		/*Angle of Arrival has Format Q9.7; dividing by 2^7 and negating results in the correct value*/
+		_sensor_uwb.aoa_azimuth_dev 	= - double(_distance_result_msg.measurements.aoa_azimuth) / 128;
+		_sensor_uwb.aoa_elevation_dev 	= - double(_distance_result_msg.measurements.aoa_elevation) / 128;
+		_sensor_uwb.aoa_azimuth_resp 	= - double(_distance_result_msg.measurements.aoa_dest_azimuth) / 128;
+		_sensor_uwb.aoa_elevation_resp 	= - double(_distance_result_msg.measurements.aoa_dest_elevation) / 128;
 
-			_uwb_distance.position[0] = _rel_pos(0);
-			_uwb_distance.position[1] = _rel_pos(1);
-			_uwb_distance.position[2] = _rel_pos(2);
 
-		} else {
+		/*Angle of Arrival has Format Q9.7; dividing by 2^7 and negating results in the correct value*/
-			//only print the error if debug is enabled
+		_sensor_uwb.aoa_azimuth_fom 		= - double(_distance_result_msg.measurements.aoa_azimuth) / 128;
-			if (_uwb_pos_debug) {
+		_sensor_uwb.aoa_elevation_fom 		= - double(_distance_result_msg.measurements.aoa_elevation) / 128;
-				switch (UWB_POS_ERROR) { //UWB POSITION ALGORItHM Errors
+		_sensor_uwb.aoa_dest_azimuth_fom	= - double(_distance_result_msg.measurements.aoa_dest_azimuth) / 128;
-				case UWB_ANC_BELOW_THREE:
+		_sensor_uwb.aoa_dest_elevation_fom	= - double(_distance_result_msg.measurements.aoa_dest_elevation) / 128;
-					PX4_INFO("UWB not enough anchors for doing localization");
-					break;
 
-				case UWB_LIN_DEP_FOR_THREE:
+		/* Sensor physical offset*/ //for now we propagate the physical configuration via Uorb
-					PX4_INFO("UWB localization: linear dependent with 3 Anchors");
+		_sensor_uwb.orientation		= _sensor_rot.get();
-					break;
+		_sensor_uwb.offset_x		= _offset_x.get();
+		_sensor_uwb.offset_y		= _offset_y.get();
+		_sensor_uwb.offset_z		= _offset_z.get();
 
-				case UWB_ANC_ON_ONE_LEVEL:
+		_sensor_uwb_pub.publish(_sensor_uwb);
-					PX4_INFO("UWB localization: Anchors are on a X,Y Plane and there are not enought Anchors");
-					break;
-
-				case UWB_LIN_DEP_FOR_FOUR:
-					PX4_INFO("UWB localization: linear dependent with four or more Anchors");
-					break;
-
-				case UWB_RANK_ZERO:
-					PX4_INFO("UWB localization: rank is zero");
-					break;
-
-				default:
-					PX4_INFO("UWB localization: Unknown failure in Position Algorithm");
-					break;
-				}
-			}
-		}
-
-		_uwb_distance_pub.publish(_uwb_distance);
 
 	} else {
-		//PX4_ERR("Read %d bytes instead of %d.", (int) buffer_location, (int) sizeof(distance_msg_t));
 		perf_count(_read_err_perf);
 
 		if (buffer_location == 0) {
 			PX4_WARN("UWB module is not responding.");
-
-			//TODO add retry Ranging Start Message. Sometimes the UWB devices Crashes. (Check Power)
 		}
 	}
 
 	return 1;
 }
-
-UWB_POS_ERROR_CODES UWB_SR150::localization()
-{
-// 			WIP
-	/******************************************************
-	 ****************** 3D Localization *******************
-	 *****************************************************/
-
-	/*!@brief: This function calculates the 3D position of the initiator from the anchor distances and positions using least squared errors.
-	 *	 	   The function expects more than 4 anchors. The used equation system looks like follows:\n
-	 \verbatim
-	  		    -					-
-	  		   | M_11	M_12	M_13 |	 x	  b[0]
-	  		   | M_12	M_22	M_23 | * y	= b[1]
-	  		   | M_23	M_13	M_33 |	 z	  b[2]
-	  		    -					-
-	 \endverbatim
-	 * @param distances_cm_in_pt: 			Pointer to array that contains the distances to the anchors in cm (including invalid results)
-	 * @param no_distances: 				Number of valid distances in distance array (it's not the size of the array)
-	 * @param anchor_pos: 	Pointer to array that contains anchor positions in cm (including positions related to invalid results)
-	 * @param no_anc_positions: 			Number of valid anchor positions in the position array (it's not the size of the array)
-	 * @param position_result_pt: 			Pointer toposition. position_t variable that holds the result of this calculation
-	 * @return: The function returns a status code. */
-
-	/* 		Algorithm used:
-	 *		Linear Least Sqaures to solve Multilateration
-	 * 		with a Special case if there are only 3 Anchors.
-	 * 		Output is the Coordinates of the Initiator in relation to Anchor 0 in NEU (North-East-Up) Framing
-	 * 		In cm
-	 */
-
-	/* Resulting Position Vector*/
-	int64_t x_pos = 0;
-	int64_t y_pos = 0;
-	int64_t z_pos = 0;
-	/* Matrix components (3*3 Matrix resulting from least square error method) [cm^2] */
-	int64_t M_11 = 0;
-	int64_t M_12 = 0;																						// = M_21
-	int64_t M_13 = 0;																						// = M_31
-	int64_t M_22 = 0;
-	int64_t M_23 = 0;																						// = M_23
-	int64_t M_33 = 0;
-
-	/* Vector components (3*1 Vector resulting from least square error method) [cm^3] */
-	int64_t b[3] = {0};
-
-	/* Miscellaneous variables */
-	int64_t temp = 0;
-	int64_t temp2 = 0;
-	int64_t nominator = 0;
-	int64_t denominator = 0;
-	bool	anchors_on_x_y_plane = true;																		// Is true, if all anchors are on the same height => x-y-plane
-	bool	lin_dep = true;																						// All vectors are linear dependent, if this variable is true
-	uint8_t ind_y_indi =
-		0;	//numberr of independet vectors																					// First anchor index, for which the second row entry of the matrix [(x_1 - x_0) (x_2 - x_0) ... ; (y_1 - x_0) (y_2 - x_0) ...] is non-zero => linear independent
-
-	/* Arrays for used distances and anchor positions (without rejected ones) */
-	uint8_t 	no_distances = _uwb_grid_info.num_anchors;
-	uint32_t 	distances_cm[no_distances];
-	position_t 	anchor_pos[no_distances]; //position in CM
-	uint8_t		no_valid_distances = 0;
-
-	/* Reject invalid distances (including related anchor position) */
-	for (int i = 0; i < no_distances; i++) {
-		if (_distance_result_msg.measurements[i].distance != 0xFFFFu) {
-			//excludes any distance that is 0xFFFFU (int16 Maximum Value)
-			distances_cm[no_valid_distances] 		= _distance_result_msg.measurements[i].distance;
-			anchor_pos[no_valid_distances] 	= _uwb_grid_info.anchor_pos[i];
-			no_valid_distances++;
-
-		}
-	}
-
-	/* Check, if there are enough valid results for doing the localization at all */
-	if (no_valid_distances < 3) {
-		return UWB_ANC_BELOW_THREE;
-	}
-
-	/* Check, if anchors are on the same x-y plane */
-	for (int i = 1; i < no_valid_distances; i++) {
-		if (anchor_pos[i].z != anchor_pos[0].z) {
-			anchors_on_x_y_plane = false;
-			break;
-		}
-	}
-
-	/**** Check, if there are enough linear independent anchor positions ****/
-
-	/* Check, if the matrix |(x_1 - x_0) (x_2 - x_0) ... | has rank 2
-	 * 			|(y_1 - y_0) (y_2 - y_0) ... | 				*/
-
-	for (ind_y_indi = 2; ((ind_y_indi < no_valid_distances) && (lin_dep == true)); ind_y_indi++) {
-		temp = ((int64_t)anchor_pos[ind_y_indi].y - (int64_t)anchor_pos[0].y) * ((int64_t)anchor_pos[1].x -
-				(int64_t)anchor_pos[0].x);
-		temp2 = ((int64_t)anchor_pos[1].y - (int64_t)anchor_pos[0].y) * ((int64_t)anchor_pos[ind_y_indi].x -
-				(int64_t)anchor_pos[0].x);
-
-		if ((temp - temp2) != 0) {
-			lin_dep = false;
-			break;
-		}
-	}
-
-	/* Leave function, if rank is below 2 */
-	if (lin_dep == true) {
-		return UWB_LIN_DEP_FOR_THREE;
-	}
-
-	/* If the anchors are not on the same plane, three vectors must be independent => check */
-	if (!anchors_on_x_y_plane) {
-		/* Check, if there are enough valid results for doing the localization */
-		if (no_valid_distances < 4) {
-			return UWB_ANC_ON_ONE_LEVEL;
-		}
-
-		/* Check, if the matrix |(x_1 - x_0) (x_2 - x_0) (x_3 - x_0) ... | has rank 3 (Rank y, y already checked)
-		 * 			|(y_1 - y_0) (y_2 - y_0) (y_3 - y_0) ... |
-		 * 			|(z_1 - z_0) (z_2 - z_0) (z_3 - z_0) ... |											*/
-		lin_dep = true;
-
-		for (int i = 2; ((i < no_valid_distances) && (lin_dep == true)); i++) {
-			if (i != ind_y_indi) {
-				/* (x_1 - x_0)*[(y_2 - y_0)(z_n - z_0) - (y_n - y_0)(z_2 - z_0)] */
-				temp 	= ((int64_t)anchor_pos[ind_y_indi].y - (int64_t)anchor_pos[0].y) * ((int64_t)anchor_pos[i].z -
-						(int64_t)anchor_pos[0].z);
-				temp 	-= ((int64_t)anchor_pos[i].y - (int64_t)anchor_pos[0].y) * ((int64_t)anchor_pos[ind_y_indi].z -
-						(int64_t)anchor_pos[0].z);
-				temp2 	= ((int64_t)anchor_pos[1].x - (int64_t)anchor_pos[0].x) * temp;
-
-				/* Add (x_2 - x_0)*[(y_n - y_0)(z_1 - z_0) - (y_1 - y_0)(z_n - z_0)] */
-				temp 	= ((int64_t)anchor_pos[i].y - (int64_t)anchor_pos[0].y) * ((int64_t)anchor_pos[1].z - (int64_t)anchor_pos[0].z);
-				temp 	-= ((int64_t)anchor_pos[1].y - (int64_t)anchor_pos[0].y) * ((int64_t)anchor_pos[i].z - (int64_t)anchor_pos[0].z);
-				temp2 	+= ((int64_t)anchor_pos[ind_y_indi].x - (int64_t)anchor_pos[0].x) * temp;
-
-				/* Add (x_n - x_0)*[(y_1 - y_0)(z_2 - z_0) - (y_2 - y_0)(z_1 - z_0)] */
-				temp 	= ((int64_t)anchor_pos[1].y - (int64_t)anchor_pos[0].y) * ((int64_t)anchor_pos[ind_y_indi].z -
-						(int64_t)anchor_pos[0].z);
-				temp 	-= ((int64_t)anchor_pos[ind_y_indi].y - (int64_t)anchor_pos[0].y) * ((int64_t)anchor_pos[1].z -
-						(int64_t)anchor_pos[0].z);
-				temp2 	+= ((int64_t)anchor_pos[i].x - (int64_t)anchor_pos[0].x) * temp;
-
-				if (temp2 != 0) { lin_dep = false; }
-			}
-		}
-
-		/* Leave function, if rank is below 3 */
-		if (lin_dep == true) {
-			return UWB_LIN_DEP_FOR_FOUR;
-		}
-	}
-
-	/************************************************** Algorithm ***********************************************************************/
-
-	/* Writing values resulting from least square error method (A_trans*A*x = A_trans*r; row 0 was used to remove x^2,y^2,z^2 entries => index starts at 1) */
-	for (int i = 1; i < no_valid_distances; i++) {
-		/* Matrix (needed to be multiplied with 2, afterwards) */
-		M_11 += sq((int64_t)(anchor_pos[i].x - anchor_pos[0].x));
-		M_12 += (int64_t)((int64_t)(anchor_pos[i].x - anchor_pos[0].x) * (int64_t)(anchor_pos[i].y - anchor_pos[0].y));
-		M_13 += (int64_t)((int64_t)(anchor_pos[i].x - anchor_pos[0].x) * (int64_t)(anchor_pos[i].z - anchor_pos[0].z));
-		M_22 += sq((int64_t)(anchor_pos[i].y - anchor_pos[0].y));
-		M_23 += (int64_t)((int64_t)(anchor_pos[i].y - anchor_pos[0].y) * (int64_t)(anchor_pos[i].z - anchor_pos[0].z));
-		M_33 += sq((int64_t)(anchor_pos[i].z - anchor_pos[0].z));
-
-		/* Vector */
-		temp = sq(distances_cm[0]) - sq(distances_cm[i])
-		       + sq(anchor_pos[i].x) + sq(anchor_pos[i].y)
-		       + sq(anchor_pos[i].z) - sq(anchor_pos[0].x)
-		       - sq(anchor_pos[0].y) - sq(anchor_pos[0].z);
-
-		b[0] += (int64_t)((int64_t)(anchor_pos[i].x - anchor_pos[0].x) * temp);
-		b[1] += (int64_t)((int64_t)(anchor_pos[i].y - anchor_pos[0].y) * temp);
-		b[2] += (int64_t)((int64_t)(anchor_pos[i].z - anchor_pos[0].z) * temp);
-	}
-
-	M_11 = 2 * M_11;
-	M_12 = 2 * M_12;
-	M_13 = 2 * M_13;
-	M_22 = 2 * M_22;
-	M_23 = 2 * M_23;
-	M_33 = 2 * M_33;
-
-	/* Calculating the z-position, if calculation is possible (at least one anchor at z != 0) */
-	if (anchors_on_x_y_plane == false) {
-		nominator = b[0] * (M_12 * M_23 - M_13 * M_22) + b[1] * (M_12 * M_13 - M_11 * M_23) + b[2] *
-			    (M_11 * M_22 - M_12 * M_12);			// [cm^7]
-		denominator = M_11 * (M_33 * M_22 - M_23 * M_23) + 2 * M_12 * M_13 * M_23 - M_33 * M_12 * M_12 - M_22 * M_13 *
-			      M_13;				// [cm^6]
-
-		/* Check, if denominator is zero (Rank of matrix not high enough) */
-		if (denominator == 0) {
-			return UWB_RANK_ZERO;
-		}
-
-		z_pos = ((nominator * 10) / denominator + 5) / 10;	// [cm]
-	}
-
-	/* Else prepare for different calculation approach (after x and y were calculated) */
-	else {
-		z_pos = 0;
-	}
-
-	/* Calculating the y-position */
-	nominator = b[1] * M_11 - b[0] * M_12 - (z_pos * (M_11 * M_23 - M_12 * M_13));	// [cm^5]
-	denominator = M_11 * M_22 - M_12 * M_12;// [cm^4]
-
-	/* Check, if denominator is zero (Rank of matrix not high enough) */
-	if (denominator == 0) {
-		return UWB_RANK_ZERO;
-	}
-
-	y_pos = ((nominator * 10) / denominator + 5) / 10;	// [cm]
-
-	/* Calculating the x-position */
-	nominator = b[0] - z_pos * M_13 - y_pos * M_12;	// [cm^3]
-	denominator = M_11;	// [cm^2]
-
-	x_pos = ((nominator * 10) / denominator + 5) / 10;// [cm]
-
-	/* Calculate z-position form x and y coordinates, if z can't be determined by previous steps (All anchors at z_n = 0) */
-	if (anchors_on_x_y_plane == true) {
-		/* Calculate z-positon relative to the anchor grid's height */
-		for (int i = 0; i < no_distances; i++) {
-			/* z² = dis_meas_n² - (x - x_anc_n)² - (y - y_anc_n)² */
-			temp = (int64_t)((int64_t)pow(distances_cm[i], 2)
-					 - (int64_t)pow((x_pos - (int64_t)anchor_pos[i].x), 2)
-					 - (int64_t)pow((y_pos - (int64_t)anchor_pos[i].y), 2));
-
-			/* z² must be positive, else x and y must be wrong => calculate positive sqrt and sum up all calculated heights, if positive */
-			if (temp >= 0) {
-				z_pos += (int64_t)sqrt(temp);
-
-			} else {
-				z_pos = 0;
-			}
-		}
-
-		z_pos = z_pos / no_distances;										// Divide sum by number of distances to get the average
-
-		/* Add height of the anchor grid's height */
-		z_pos += anchor_pos[0].z;
-	}
-
-	//Output is the Coordinates of the Initiator in relation to 0,0,0 in NEU (North-East-Up) Framing
-	// The end goal of this math is to get the position relative to the landing point in the NED frame.
-	_current_position_uwb_init = matrix::Vector3f(x_pos, y_pos, z_pos);
-
-	// Construct the rotation from the UWB_R4frame to the NWU frame.
-	// The UWB_SR150 frame is just NWU, rotated by some amount about the Z (up) axis. (Parameter Yaw offset)
-	// To get back to NWU, just rotate by negative this amount about Z.
-	_uwb_init_to_nwu = matrix::Dcmf(matrix::Eulerf(0.0f, 0.0f,
-					-(_uwb_init_off_yaw.get() * M_PI_F / 180.0f))); //
-	// The actual conversion:
-	//  - Subtract _landing_point to get the position relative to the landing point, in UWB_R4 frame
-	//  - Rotate by _rddrone_to_nwu to get into the NWU frame
-	//  - Rotate by _nwu_to_ned to get into the NED frame
-	_current_position_ned = _nwu_to_ned * _uwb_init_to_nwu * _current_position_uwb_init;
-
-	// Now the position is the landing point relative to the vehicle.
-	// so the only thing left is to convert cm to Meters and to add the Initiator offset
-	_rel_pos =  _current_position_ned / 100 + _uwb_init_offset_v3f;
-
-	// The UWB report contains the position of the vehicle relative to the landing point.
-
-	return UWB_OK;
-}
-
-int uwb_sr150_main(int argc, char *argv[])
-{
-	return UWB_SR150::main(argc, argv);
-}
-
-void UWB_SR150::parameters_update()
-{
-	if (_parameter_update_sub.updated()) {
-		parameter_update_s param_update;
-		_parameter_update_sub.copy(&param_update);
-
-		// If any parameter updated, call updateParams() to check if
-		// this class attributes need updating (and do so).
-		updateParams();
-	}
-}

src/drivers/uwb/uwb_sr150/uwb_sr150.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2023 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -38,101 +38,63 @@
 #include <poll.h>
 #include <sys/select.h>
 #include <sys/time.h>
+#include <perf/perf_counter.h>
+#include <lib/conversion/rotation.h>
 
 #include <px4_platform_common/module_params.h>
 #include <px4_platform_common/module.h>
-#include <perf/perf_counter.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+
 #include <uORB/Publication.hpp>
-#include <uORB/topics/landing_target_pose.h>
-#include <uORB/topics/uwb_grid.h>
-#include <uORB/topics/uwb_distance.h>
-#include <uORB/topics/parameter_update.h>
 #include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
 #include <uORB/SubscriptionInterval.hpp>
-#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/sensor_uwb.h>
+#include <uORB/topics/parameter_update.h>
+
 #include <matrix/math.hpp>
-#include <matrix/Matrix.hpp>
+
+#define UWB_DEFAULT_PORT "/dev/ttyS1"
 
 using namespace time_literals;
 
-#define UWB_CMD  0x8e
-#define UWB_CMD_START  0x01
-#define UWB_CMD_STOP  0x00
-#define UWB_CMD_RANGING  0x0A
-#define STOP_B 0x0A
-
-#define UWB_PRECNAV_APP   0x04
-#define UWB_APP_START     0x10
-#define UWB_APP_STOP      0x11
-#define UWB_SESSION_START 0x22
-#define UWB_SESSION_STOP  0x23
-#define UWB_RANGING_START 0x01
-#define UWB_RANGING_STOP  0x00
-#define UWB_DRONE_CTL     0x0A
-
-#define UWB_CMD_LEN  0x05
-#define UWB_CMD_DISTANCE_LEN 0x21
-#define UWB_MAC_MODE 2
-#define MAX_ANCHORS 12
-#define UWB_GRID_CONFIG "/fs/microsd/etc/uwb_grid_config.csv"
-
-typedef struct {  //needs higher accuracy?
-	float lat, lon, alt, yaw; //offset to true North
-} gps_pos_t;
-
 typedef struct {
-	int16_t x, y, z; //axis in cm
+	uint16_t MAC;					// MAC address of UWB device
-} position_t; // Position of a device or target in 3D space
+	uint8_t status;					// Status of Measurement
-
+	uint16_t distance; 				// Distance in cm
-enum UWB_POS_ERROR_CODES {
+	uint8_t nLos; 					// line of sight y/n
-	UWB_OK,
+	int16_t aoa_azimuth;				// AOA of incoming msg for Azimuth antenna pairing
-	UWB_ANC_BELOW_THREE,
+	int16_t aoa_elevation;				// AOA of incoming msg for Altitude antenna pairing
-	UWB_LIN_DEP_FOR_THREE,
+	int16_t aoa_dest_azimuth;			// AOA destination Azimuth
-	UWB_ANC_ON_ONE_LEVEL,
+	int16_t aoa_dest_elevation; 			// AOA destination elevation
-	UWB_LIN_DEP_FOR_FOUR,
+	uint8_t aoa_azimuth_FOM;			// AOA Azimuth FOM
-	UWB_RANK_ZERO
+	uint8_t aoa_elevation_FOM;			// AOA Elevation FOM
-};
+	uint8_t aoa_dest_azimuth_FOM;			// AOA Azimuth FOM
-
+	uint8_t aoa_dest_elevation_FOM;			// AOA Elevation FOM
-typedef struct {
-	uint8_t MAC[2];		// MAC Adress of UWB device
-	uint8_t status;		// Status of Measurement
-	uint16_t distance; 	// Distance in cm
-	uint8_t nLos; 		// line of sight y/n
-	uint16_t aoaFirst;	// Angle of Arrival of incoming msg
 } __attribute__((packed)) UWB_range_meas_t;
 
 typedef struct {
-	uint32_t initator_time;  	//timestamp of init
+	uint8_t cmd;      				// Should be 0x8E for distance result message
-	uint32_t sessionId;	// Session ID of UWB session
+	uint16_t len; 					// Should be 0x30 for distance result message
-	uint8_t	num_anchors;	//number of anchors
+	uint32_t seq_ctr;				// Number of Ranges since last Start of Ranging
-	gps_pos_t target_gps; //GPS position of Landing Point
+	uint32_t sessionId;				// Session ID of UWB session
-	uint8_t  mac_mode;	// MAC adress mode, either 2 Byte or 8 Byte
+	uint32_t range_interval;			// Time between ranging rounds
-	uint8_t MAC[UWB_MAC_MODE][MAX_ANCHORS];
+	uint16_t MAC;					// MAC address of UWB device
-	position_t target_pos; //target position
+	UWB_range_meas_t measurements; 			//Raw anchor_distance distances in CM 2*9
-	position_t anchor_pos[MAX_ANCHORS]; // Position of each anchor
+	uint8_t stop; 					// Should be 0x1B
-	uint8_t stop; 		// Should be 27
-} grid_msg_t;
-
-typedef struct {
-	uint8_t cmd;      	// Should be 0x8E for distance result message
-	uint16_t len; 		// Should be 0x30 for distance result message
-	uint32_t time_uwb_ms;	// Timestamp of UWB device in ms
-	uint32_t seq_ctr;	// Number of Ranges since last Start of Ranging
-	uint32_t sessionId;	// Session ID of UWB session
-	uint32_t range_interval;	// Time between ranging rounds
-	uint8_t  mac_mode;	// MAC adress mode, either 2 Byte or 8 Byte
-	uint8_t  no_measurements;	// MAC adress mode, either 2 Byte or 8 Byte
-	UWB_range_meas_t measurements[4]; //Raw anchor_distance distances in CM 2*9
-	uint8_t stop; 		// Should be 0x1B
 } __attribute__((packed)) distance_msg_t;
 
-class UWB_SR150 : public ModuleBase<UWB_SR150>, public ModuleParams
+class UWB_SR150 : public ModuleBase<UWB_SR150>, public ModuleParams, public px4::ScheduledWorkItem
 {
 public:
-	UWB_SR150(const char *device_name, speed_t baudrate, bool uwb_pos_debug);
+	UWB_SR150(const char *port);
-
 	~UWB_SR150();
 
+	/**
+	 * @see ModuleBase::task_spawn
+	 */
+	static int task_spawn(int argc, char *argv[]);
+
 	/**
 	 * @see ModuleBase::custom_command
 	 */
@@ -143,67 +105,51 @@ public:
 	 */
 	static int print_usage(const char *reason = nullptr);
 
-	/**
+	bool init();
-	 * @see ModuleBase::Multilateration
-	 */
-	UWB_POS_ERROR_CODES localization();
 
-	/**
+	void start();
-	 * @see ModuleBase::Distance Result
-	 */
-	int distance();
 
-	/**
+	void stop();
-	 * @see ModuleBase::task_spawn
-	 */
-	static int task_spawn(int argc, char *argv[]);
 
-	static UWB_SR150 *instantiate(int argc, char *argv[]);
+	int collectData();
-
-	void run() override;
 
 private:
-	static constexpr int64_t sq(int64_t x) { return x * x; }
 
 	void parameters_update();
 
-	void grid_info_read(position_t *grid);
+	void Run() override;
 
-	DEFINE_PARAMETERS(
+	// Publications
-		(ParamFloat<px4::params::UWB_INIT_OFF_X>) _uwb_init_off_x,
+	uORB::Publication<sensor_uwb_s> _sensor_uwb_pub{ORB_ID(sensor_uwb)};
-		(ParamFloat<px4::params::UWB_INIT_OFF_Y>) _uwb_init_off_y,
-		(ParamFloat<px4::params::UWB_INIT_OFF_Z>) _uwb_init_off_z,
-		(ParamFloat<px4::params::UWB_INIT_OFF_YAW>) _uwb_init_off_yaw
-	)
 
+	// Subscriptions
+	uORB::SubscriptionCallbackWorkItem _sensor_uwb_sub{this, ORB_ID(sensor_uwb)};
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
-	int _uart;
+	// Parameters
-	fd_set _uart_set;
+	DEFINE_PARAMETERS(
-	struct timeval _uart_timeout {};
+		(ParamInt<px4::params::UWB_PORT_CFG>) 			_uwb_port_cfg,
-	bool _uwb_pos_debug;
+		(ParamFloat<px4::params::UWB_INIT_OFF_X>) 		_offset_x,
-
+		(ParamFloat<px4::params::UWB_INIT_OFF_Y>) 		_offset_y,
-	uORB::Publication<uwb_grid_s> _uwb_grid_pub{ORB_ID(uwb_grid)};
+		(ParamFloat<px4::params::UWB_INIT_OFF_Z>) 		_offset_z,
-	uwb_grid_s _uwb_grid{};
+		(ParamInt<px4::params::UWB_SENS_ROT>) 			_sensor_rot
-
+	)
-	uORB::Publication<uwb_distance_s> _uwb_distance_pub{ORB_ID(uwb_distance)};
+	// Performance (perf) counters
-	uwb_distance_s _uwb_distance{};
-
-	uORB::Publication<landing_target_pose_s> _landing_target_pub{ORB_ID(landing_target_pose)};
-	landing_target_pose_s _landing_target{};
-
-	grid_msg_t _uwb_grid_info{};
-	distance_msg_t _distance_result_msg{};
-	matrix::Vector3f _rel_pos;
-
-	matrix::Dcmf _uwb_init_to_nwu;
-	matrix::Dcmf _nwu_to_ned{matrix::Eulerf(M_PI_F, 0.0f, 0.0f)};
-	matrix::Vector3f _current_position_uwb_init;
-	matrix::Vector3f _current_position_ned;
-	matrix::Vector3f _uwb_init_offset_v3f;
-
 	perf_counter_t _read_count_perf;
 	perf_counter_t _read_err_perf;
-};
 
+	sensor_uwb_s _sensor_uwb{};
+
+	char _port[20] {};
+	hrt_abstime param_timestamp{0};
+
+	int _uart{-1};
+	fd_set _uart_set;
+	struct timeval _uart_timeout {};
+
+	unsigned _consecutive_fail_count;
+	int _interval{100000};
+
+	distance_msg_t 		_distance_result_msg{};
+};
 #endif //PX4_RDDRONE_H

src/modules/landing_target_estimator/LandingTargetEstimator.cpp

@@ -100,13 +100,13 @@ void LandingTargetEstimator::update()
 		}
 	}
 
-	if (!_new_sensorReport) {
+	if (!_new_irlockReport) {
 		// nothing to do
 		return;
 	}
 
 	// mark this sensor measurement as consumed
-	_new_sensorReport = false;
+	_new_irlockReport = false;
 
 
 	if (!_estimator_initialized) {
@@ -254,30 +254,7 @@ void LandingTargetEstimator::_update_topics()
 		_target_position_report.rel_pos_x += _params.offset_x;
 		_target_position_report.rel_pos_y += _params.offset_y;
 
-		_new_sensorReport = true;
+		_new_irlockReport = true;
-
-	} else if (_uwbDistanceSub.update(&_uwbDistance)) {
-		if (!_vehicleAttitude_valid || !_vehicleLocalPosition_valid) {
-			// don't have the data needed for an update
-			PX4_INFO("Attitude: %d, Local pos: %d", _vehicleAttitude_valid, _vehicleLocalPosition_valid);
-			return;
-		}
-
-		if (!matrix::Vector3f(_uwbDistance.position).isAllFinite()) {
-			PX4_WARN("Marker position reading invalid!");
-			return;
-		}
-
-		_new_sensorReport = true;
-
-		// The coordinate system is NED (north-east-down)
-		// the uwb_distance msg contains the Position in NED, Vehicle relative to LP
-		// The coordinates "rel_pos_*" are the position of the landing point relative to the vehicle.
-		// To change POV we negate every Axis:
-		_target_position_report.timestamp = _uwbDistance.timestamp;
-		_target_position_report.rel_pos_x = -_uwbDistance.position[0];
-		_target_position_report.rel_pos_y = -_uwbDistance.position[1];
-		_target_position_report.rel_pos_z = -_uwbDistance.position[2];
 	}
 }
 

src/modules/landing_target_estimator/LandingTargetEstimator.h

@@ -54,9 +54,6 @@
 #include <uORB/topics/irlock_report.h>
 #include <uORB/topics/landing_target_pose.h>
 #include <uORB/topics/landing_target_innovations.h>
-#include <uORB/topics/uwb_distance.h>
-#include <uORB/topics/uwb_grid.h>
-#include <uORB/topics/estimator_sensor_bias.h>
 #include <uORB/topics/parameter_update.h>
 #include <matrix/math.hpp>
 #include <mathlib/mathlib.h>
@@ -153,14 +150,11 @@ private:
 	uORB::Subscription _attitudeSub{ORB_ID(vehicle_attitude)};
 	uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
 	uORB::Subscription _irlockReportSub{ORB_ID(irlock_report)};
-	uORB::Subscription _uwbDistanceSub{ORB_ID(uwb_distance)};
 
 	vehicle_local_position_s	_vehicleLocalPosition{};
 	vehicle_attitude_s		_vehicleAttitude{};
 	vehicle_acceleration_s		_vehicle_acceleration{};
 	irlock_report_s			_irlockReport{};
-	uwb_grid_s		_uwbGrid{};
-	uwb_distance_s		_uwbDistance{};
 
 	// keep track of which topics we have received
 	bool _vehicleLocalPosition_valid{false};