diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc
index c7750dd..7154e24 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 fb563a0..4070ac1 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;