Add support for NXP UWB position sensor

msg/uwb_report.msg

@@ -1,3 +1,6 @@
+# UWB report message contains the coordinates of the vehicle measured by an ultra-wideband positioning system,
+# such as Pozyx or NXP Rddrone.
+
 uint64 timestamp					# time since system start (microseconds)
 # The coordinate frame of the UWB grid should be a Z-down, right-handed coordinate system.
 # The yaw alignment is arbitrary, and stored in the yaw_offset field.
@@ -6,7 +9,21 @@ float32 pos_x      # X position in the UWB grid frame, in meters
 float32 pos_y      # Y position in the UWB grid frame, in meters
 float32 pos_z      # Z position in the UWB grid frame, in meters
 
-#float32 yaw_offset  # Angle between the X axis of the UWB grid frame and true north. I.e., your compass heading if you face along the positive X axis
+# The reason these values are kept in the uwb_report message is that we do not know the transform between the
+# UWB coordinate system and the world coordinate system. Different UWB systems might have different mechanisms for
+# this, so the UWB driver should handle it. Unless you
+float32 target_pos_x  # X position of the vehicle relative to the UWB origin (usually the landing point), in NED frame, in meters
+float32 target_pos_y  # Y position of the vehicle relative to the UWB origin (usually the landing point), in NED frame, in meters
+float32 target_pos_z  # Z position of the vehicle relative to the UWB origin (usually the landing point), in NED frame, in meters
+
+# TODO: We could add the following fields. They would allow us to calculate the vehicle's global position by simply
+#  adding the offset (measured by the UWB system) to the origin lat/lon/alt.
+#  However, this functionality is not yet needed or used anywhere, so it is omitted for now to save space.
+
+# bool global_pos_valid  # True iff the values origin_lat, origin_lon, and origin_alt are all valid
+# float32 origin_lat  # Latitude of the point (0, 0, 0) of the UWB system
+# float32 origin_lon  # Longitude of the point (0, 0, 0) of the UWB system
+# float32 origin_alt  # Altitude of the point (0, 0, 0) of the UWB system
 
 int16 var_x
 int16 var_y

src/drivers/uwb/rddrone/module.yaml

@@ -4,4 +4,4 @@ serial_config:
     port_config_param:
       name: UWB_CONFIG
       group: UWB
-      default: TEL2
+      default: ""

src/drivers/uwb/rddrone/rddrone.cpp

@@ -166,7 +166,7 @@ void RDDrone::run()
 			_uwb_report.pos_y = _message.pos_y ;// / 100.0f;
 			_uwb_report.pos_z = _message.pos_z ;// / 100.0f;
 			_uwb_report.timestamp = hrt_absolute_time();
-			_uwb_pub.publish(_uwb_report);
+
 			_attitude_sub.update(&_vehicle_attitude);
 
 			// The end goal of this math is to get the position relative to the landing point in the NED frame.
@@ -182,15 +182,13 @@ void RDDrone::run()
 			//  - Rotate by _nwu_to_ned to get into the NED frame
 			_current_position_ned = _nwu_to_ned * _rddrone_to_nwu * _current_position_rddrone;
 
-			_landing_target.timestamp = hrt_absolute_time();
-			_landing_target.rel_pos_valid = true;
-			_landing_target.rel_vel_valid = false;
-			_landing_target.is_static = true;
-			_landing_target.x_rel = -_current_position_ned(0);
-			_landing_target.y_rel = -_current_position_ned(1);
-			_landing_target.z_rel = -_current_position_ned(2);
+			// Now the position is the vehicle relative to the landing point. We need the landing point relative to
+			// the vehicle. So just negate everything.
+			_uwb_report.target_pos_x = _current_position_ned(0);
+			_uwb_report.target_pos_y = _current_position_ned(1);
+			_uwb_report.target_pos_z = _current_position_ned(2);
 
-			_landing_target_pub.publish(_landing_target);
+			_uwb_pub.publish(_uwb_report);
 
 		} else {
 			//PX4_ERR("Read %d bytes instead of %d.", (int) buffer_location, (int) sizeof(position_msg_t));

src/modules/landing_target_estimator/LandingTargetEstimator.cpp

@@ -279,22 +279,24 @@ void LandingTargetEstimator::_update_topics()
 			return;
 		}
 
-		if (!PX4_ISFINITE(_uwbReport.pos_y) || !PX4_ISFINITE(_uwbReport.pos_x) ||
-		    !PX4_ISFINITE(_uwbReport.pos_z)) {
+		if (!PX4_ISFINITE(_uwbReport.target_pos_y) || !PX4_ISFINITE(_uwbReport.target_pos_x) ||
+		    !PX4_ISFINITE(_uwbReport.target_pos_z)) {
 			return;
 		}
 
 		_new_sensorReport = true;
 		_uncertainty_scale = 1.0f;
 
-		// we're assuming the coordinate system is NEU
-		// to get the position relative to use we just need to negate x and y
+		// The coordinate system is NED (north-east-down).
+		// The coordinates "rel_pos_*" are the position of the landing point relative to the vehicle.
+		// The UWB report contains the position of the vehicle relative to the landing point.
+		// So, just negate all 3 components.
 		_sensor_report.timestamp = _uwbReport.timestamp;
-		_sensor_report.rel_pos_x = -_uwbReport.pos_x;
-		_sensor_report.rel_pos_y = -_uwbReport.pos_y;
-		_sensor_report.rel_pos_z = _uwbReport.pos_z;
+		_sensor_report.rel_pos_x = -_uwbReport.target_pos_x;
+		_sensor_report.rel_pos_y = -_uwbReport.target_pos_y;
+		_sensor_report.rel_pos_z = -_uwbReport.target_pos_z;
 
-		//PX4_WARN("data from uwb x: %.2f, y: %.2f", (double)_sensor_report.rel_pos_x,
+		//PX4_INFO("data from uwb x: %.2f, y: %.2f", (double)_sensor_report.rel_pos_x,
 		//	 (double)_sensor_report.rel_pos_y);
 	}
 }