diff --git a/ros_packages/mrs_uav_gazebo_simulation/config/spawner_params.yaml b/ros_packages/mrs_uav_gazebo_simulation/config/spawner_params.yaml
index 0f9a4d5..1cdca72 100644
--- a/ros_packages/mrs_uav_gazebo_simulation/config/spawner_params.yaml
+++ b/ros_packages/mrs_uav_gazebo_simulation/config/spawner_params.yaml
@@ -50,7 +50,9 @@ enable_dual_uv_cameras: [False, 'Add right and left UV cameras on the vehicle',
enable_back_uv_camera: [False, 'Add back UV camera on the vehicle', [f450, x500]]
uvcam_calib_file: [None, 'Specify UV camera calibration different than default one', [f450, f550, t650, x500]]
uvcam_occlusions: [False, 'Enable occlusions for UVDAR simulation (NOTE: THIS REQUIRES A BEEFY COMPUTER)', [f450, f550, t650, x500]]
+enable_qorvo_dw1000: [False, 'Add Qorvo DW1000 UWB transceiver', [x500]]
pos_file: [None, 'Load positions and ids from .csv file with format: [id, x, y, z, heading] or .yaml file with format: [uav_name: \n id: (int) \n x: (float) \n y: (float) \n z: (float) \n heading: (float)]', [f330, f450, f550, t650, x500, eaglemk2, brus, naki, big_dofec, m690]]
+uwb_id: [0, 'Set id for uwb ranging', [x500]]
pos: [None, 'Specify spawn position of the first vehicle in format [x, y, z, heading]. Additional vehicles will spawn in line with 2m x-offset', [f330, f450, f550, t650, x500, eaglemk2, brus, naki, big_dofec]]
enable_vio: [False, 'Add a forward-looking fisheye camera and a high frequency IMU', [f330, f450]]
enable_vio_down: [False, 'Add a forward-looking fisheye camera and a high frequency IMU', [f330, f450]]
diff --git a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/meshes/x500v2/qorvo_dw1000_mount.stl b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/meshes/x500v2/qorvo_dw1000_mount.stl
new file mode 100644
index 0000000..b2a123d
Binary files /dev/null and b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/meshes/x500v2/qorvo_dw1000_mount.stl differ
diff --git a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/component_snippets.sdf.jinja b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/component_snippets.sdf.jinja
index 3863472..d4a8d1a 100644
--- a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/component_snippets.sdf.jinja
+++ b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/component_snippets.sdf.jinja
@@ -1434,6 +1434,41 @@ limitations under the License.
{%- endmacro -%}
+
+{%- macro uwb_range_macro(namespace, parent_link, uwb_id, uav_name, x, y, z, roll, pitch, yaw) -%}
+
+ {{ x }} {{ y }} {{ z }} {{ roll }} {{ pitch }} {{ yaw }}
+ {{ zero_inertial_macro() }}
+
+ 10
+ 1
+
+ {{uwb_id}}
+ /{{ uav_name }}/uwb_range/range
+ {{ uav_name }}/uwb
+ -150
+ 0.1
+
+
+ 6489.6
+ 6240.0
+ 6739.2
+ 14.5
+ 2.5
+
+
+
+
+
+ {{ parent_link }}
+ uwb_range_link
+ true
+ true
+
+{%- endmacro -%}
+
+
+
diff --git a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/x500.sdf.jinja b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/x500.sdf.jinja
index 8fbe882..d7d6943 100644
--- a/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/x500.sdf.jinja
+++ b/ros_packages/mrs_uav_gazebo_simulation/models/mrs_robots_description/sdf/x500.sdf.jinja
@@ -80,6 +80,7 @@
{%- set realsense_mount_mesh_file = "model://mrs_robots_description/meshes/x500v2/realsense_mount_mesh_file.stl" -%}
{%- set bluefox_mount_mesh_file = "model://mrs_robots_description/meshes/x500v2/bluefox_mount_mesh_file.stl" -%}
{%- set uvdar_mount_mesh = "model://mrs_robots_description/meshes/dji/f450/dji_f450_uvdar_mount.dae" -%}
+ {%- set qorvo_dw1000_mount_file = "model://mrs_robots_description/meshes/x500v2/qorvo_dw1000_mount.stl" -%}
{# #}
{# Scales {--> #}
@@ -542,6 +543,24 @@
{% endif %}
+
+ {% if enable_qorvo_dw1000 %}
+
+ {{components.visual_mesh_macro(
+ name = "qorvo_dw1000_mount",
+ mesh_file = qorvo_dw1000_mount_file,
+ mesh_scale = mesh_scale_milimeters,
+ color = "DarkGrey",
+ x = 0.0,
+ y = -0.01,
+ z = -0.078,
+ roll = 0,
+ pitch = 0,
+ yaw = 0)
+ }}
+
+ {% endif %}
+
@@ -1102,5 +1121,24 @@
{% endif %}
{# #}
+ {# Qorvo UWB {--> #}
+ {% if enable_qorvo_dw1000 %}
+
+ {{ components.uwb_range_macro(
+ parent_link = root,
+ uwb_id = uwb_id,
+ uav_name = namespace,
+ x = 0.0,
+ y = 0.08,
+ z = -0.10,
+ roll = 0.0,
+ pitch = 0.0,
+ yaw = 0.0)
+ }}
+
+ {% endif %}
+ {# #}
+
+