diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index cee67c68f1..891b58cb85 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -700,7 +700,7 @@ MavlinkReceiver::handle_message_distance_sensor(mavlink_message_t *msg)
 	ds.q[3]             = dist_sensor.quaternion[3];
 	ds.signal_quality   = -1; // TODO: A dist_sensor.signal_quality field is missing from the mavlink message definition.
 	ds.type             = dist_sensor.type;
-	ds.id               = MAV_DISTANCE_SENSOR_LASER;
+	ds.id               = dist_sensor.id;
 	ds.orientation      = dist_sensor.orientation;
 
 	_distance_sensor_pub.publish(ds);