diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc
index c7750dd47a1e525334f2e7c33b6b277f4e7e390d..7154e249663653d62d1547f3d6e74c0acd878dab 100644
--- a/src/Vehicle/Vehicle.cc
+++ b/src/Vehicle/Vehicle.cc
@@ -797,10 +797,10 @@ void Vehicle::_handleDistanceSensor(mavlink_message_t& message)
 
     if (!_distanceSensorFactGroup.idSet()) {
         _distanceSensorFactGroup.setIdSet(true);
-        _id = distanceSensor.id;
+        _distanceSensorFactGroup.setId(distanceSensor.id);
     }
 
-    if (_id != distanceSensor.id) {
+    if (_distanceSensorFactGroup.id() != distanceSensor.id) {
         // We can only handle a single sensor reporting
         return;
     }
diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h
index fb563a073b2648924fa6de97cc31d42ec4aae03e..4070ac17b5fb05e77da9f898b3a87a558b7d5cf4 100644
--- a/src/Vehicle/Vehicle.h
+++ b/src/Vehicle/Vehicle.h
@@ -71,6 +71,8 @@ public:
 
     bool idSet(void) { return _idSet; }
     void setIdSet(bool idSet) { _idSet = idSet; }
+    uint8_t id(void) { return _id; }
+    void setId(uint8_t id) { _id = id; }
 
     static const char* _rotationNoneFactName;
     static const char* _rotationYaw45FactName;