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>
PX4 · Jul 12, 2023 · f8c9be0 · f8c9be0
1 parent 715a1ff
commit f8c9be0
Show file tree

Hide file tree

Showing 12 changed files with 310 additions and 687 deletions.
diff --git a/boards/nxp/fmuk66-e/default.px4board b/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

diff --git a/boards/nxp/fmurt1062-v1/default.px4board b/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

diff --git a/boards/nxp/ucans32k146/default.px4board b/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

diff --git a/msg/CMakeLists.txt b/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

diff --git a/msg/SensorUwb.msg b/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)
diff --git a/msg/UwbDistance.msg b/msg/UwbDistance.msg
diff --git a/msg/UwbGrid.msg b/msg/UwbGrid.msg
diff --git a/src/drivers/uwb/uwb_sr150/module.yaml b/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,22 +31,60 @@ 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
           decimal: 2
           increment: 0.01
           default: 0.00
 
-        UWB_INIT_OFF_YAW:
+        UWB_SENS_ROT:
           description:
-            short: UWB sensor YAW offset in body frame
-            long: UWB sensor positioning in relation to Drone in NED. Yaw rotation in relation to direction of FMU.
-          type: float
-          unit: deg
-          decimal: 1
-          increment: 0.1
-          default: 0.00
-
-
+            short: UWB sensor orientation
+            long: The orientation of the sensor relative to the forward direction of the body frame. Look up table in src/lib/conversion/rotation.h
+                  Default position is the antannaes downward facing, UWB board parallel with body frame.
+          type: enum
+          values:
+            0: ROTATION_NONE
+            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