diff --git a/NucuCar.Sensors/EnvironmentSensor/Bme680Sensor.cs b/NucuCar.Sensors/EnvironmentSensor/Bme680Sensor.cs
index aedcba5..516f201 100644
--- a/NucuCar.Sensors/EnvironmentSensor/Bme680Sensor.cs
+++ b/NucuCar.Sensors/EnvironmentSensor/Bme680Sensor.cs
@@ -15,7 +15,7 @@ namespace NucuCar.Sensors.EnvironmentSensor
     /// Abstraction for the BME680 sensor.
     /// See: https://www.bosch-sensortec.com/bst/products/all_products/bme680
     /// </summary>
-    public class Bme680Sensor : IDisposable, ITelemeter
+    public class Bme680Sensor : IDisposable, ITelemeter, ISensor<Bme680Sensor>
     {
         private readonly ILogger _logger;
         private I2cConnectionSettings _i2CSettings;
@@ -42,15 +42,16 @@ namespace NucuCar.Sensors.EnvironmentSensor
                 _logger?.LogInformation("BME680 Sensor is disabled!");
                 _sensorStateEnum = SensorStateEnum.Disabled;
             }
+
+            Object = this;
         }
 
-        // TODO Make more generic, Add interface and remove virtual
+        // TODO Make more generic
         public virtual EnvironmentSensorMeasurement GetMeasurement()
         {
             return _lastMeasurement;
         }
 
-        // TODO: Add interface and remove virtual
         public virtual SensorStateEnum GetState()
         {
             return _sensorStateEnum;
@@ -135,5 +136,7 @@ namespace NucuCar.Sensors.EnvironmentSensor
 
             return returnValue;
         }
+
+        public Bme680Sensor Object { get; }
     }
 }
\ No newline at end of file
diff --git a/NucuCar.Sensors/EnvironmentSensor/Bme680Worker.cs b/NucuCar.Sensors/EnvironmentSensor/Bme680Worker.cs
index bf65523..499ac71 100644
--- a/NucuCar.Sensors/EnvironmentSensor/Bme680Worker.cs
+++ b/NucuCar.Sensors/EnvironmentSensor/Bme680Worker.cs
@@ -36,7 +36,7 @@ namespace NucuCar.Sensors.EnvironmentSensor
 
         protected override async Task ExecuteAsync(CancellationToken stoppingToken)
         {
-            if (_serviceEnabled)
+            if (!_serviceEnabled)
             {
                 return;
             }
diff --git a/NucuCar.Sensors/ISensor.cs b/NucuCar.Sensors/ISensor.cs
index 207c79b..db1ac21 100644
--- a/NucuCar.Sensors/ISensor.cs
+++ b/NucuCar.Sensors/ISensor.cs
@@ -2,7 +2,7 @@ using NucuCar.Domain.Telemetry;
 
 namespace NucuCar.Sensors
 {
-    public interface ISensor<out TSensor> where TSensor : class, new()
+    public interface ISensor<out TSensor> where TSensor : class, ITelemeter
     {
         TSensor Object { get; }
     }
diff --git a/NucuCar.Sensors/Program.cs b/NucuCar.Sensors/Program.cs
index 5322e4f..a1b2916 100644
--- a/NucuCar.Sensors/Program.cs
+++ b/NucuCar.Sensors/Program.cs
@@ -1,5 +1,4 @@
 using Microsoft.AspNetCore.Hosting;
-using Microsoft.Extensions.Configuration;
 using Microsoft.Extensions.DependencyInjection;
 using Microsoft.Extensions.Hosting;
 using NucuCar.Sensors.EnvironmentSensor;
@@ -23,9 +22,8 @@ namespace NucuCar.Sensors
 
                     // Singletons
                     services.AddSingleton<SensorTelemetry>();
-                    services.AddSingleton<Bme680Sensor>();
-                    services.AddSingleton<ISensor<Bme680Sensor>>();
-
+                    services.AddSingleton<ISensor<Bme680Sensor>, Bme680Sensor>();
+                    
                     // Workers
                     services.AddHostedService<TelemetryWorker>();
                     services.AddHostedService<Bme680Worker>();
diff --git a/NucuCar.Sensors/Sensor.cs b/NucuCar.Sensors/Sensor.cs
new file mode 100644
index 0000000..ab61aaf
--- /dev/null
+++ b/NucuCar.Sensors/Sensor.cs
@@ -0,0 +1,9 @@
+using NucuCar.Domain.Telemetry;
+
+namespace NucuCar.Sensors
+{
+    public class Sensor<T> : ISensor<T> where T : class, ITelemeter
+    {
+        public T Object { get; }
+    }
+}
\ No newline at end of file
diff --git a/NucuCar.UnitTests/NucuCar.Sensors.Tests/EnvironmentSensor.Tests/Bme680GrpcServiceTest.cs b/NucuCar.UnitTests/NucuCar.Sensors.Tests/EnvironmentSensor.Tests/Bme680GrpcServiceTest.cs
index 5d0237e..c3b7f2f 100644
--- a/NucuCar.UnitTests/NucuCar.Sensors.Tests/EnvironmentSensor.Tests/Bme680GrpcServiceTest.cs
+++ b/NucuCar.UnitTests/NucuCar.Sensors.Tests/EnvironmentSensor.Tests/Bme680GrpcServiceTest.cs
@@ -1,7 +1,6 @@
-using System;
-using Grpc.Core;
 using Microsoft.Extensions.Logging;
 using Moq;
+using NucuCar.Sensors;
 using NucuCar.Sensors.EnvironmentSensor;
 using NucuCarSensorsProto;
 using Xunit;
@@ -9,47 +8,47 @@ using Xunit.Abstractions;
 
 namespace NucuCar.UnitTests.NucuCar.Sensors.Tests.EnvironmentSensor.Tests
 {
-    public class Bme680GrpcServiceTest
+    public partial class Bme680GrpcServiceTest
     {
         private readonly ITestOutputHelper _testOutputHelper;
         private readonly Mock<ILogger<Bme680GrpcService>> _mockLogger;
-        private readonly Mock<Bme680Sensor> _mockSensor;
+        private readonly Mock<ISensor<Bme680Sensor>> _mockSensor;
 
-        
         public Bme680GrpcServiceTest(ITestOutputHelper testOutputHelper)
         {
             _testOutputHelper = testOutputHelper;
             _mockLogger = new Mock<ILogger<Bme680GrpcService>>();
-            _mockSensor = new Mock<Bme680Sensor>();
+            _mockSensor = new Mock<ISensor<Bme680Sensor>>();
+            _mockSensor.Setup(ms => ms.Object).Returns(new Mock<TestBme680Sensor>().Object);
         }
 
-//
-//        [Fact]
-//        public void Test_GetSensorState()
-//        {
-//            var service = new Bme680GrpcService(_mockLogger.Object, _mockSensor.Object);
-//            var result = service.GetSensorState(null, null).Result;
-//
-//            // Default sensor state is error
-//            Assert.Equal(SensorStateEnum.Error, result.State);
-//
-//            // Verify that the sensor get state method is called.
-//            _mockSensor.Verify(s => s.GetState(), Times.AtLeastOnce());
-//            
-//            _mockSensor.Setup(s => s.GetState()).Returns(SensorStateEnum.Initialized);
-//            result = service.GetSensorState(null, null).Result;
-//            Assert.Equal(SensorStateEnum.Initialized, result.State);
-//        }
-//        
-//        [Fact]
-//        public void Test_GetSensorMeasurement()
-//        {
-//            var service = new Bme680GrpcService(_mockLogger.Object, _mockSensor.Object);
-//            service.GetSensorMeasurement(null, null);
-//            
-//            // Verify that the sensor get measurement method is called.
-//            _mockSensor.Verify(s => s.GetMeasurement(), Times.AtLeastOnce());
-//
-//        }
+
+        [Fact]
+        public void Test_GetSensorState()
+        {
+            var service = new Bme680GrpcService(_mockLogger.Object, _mockSensor.Object);
+            var result = service.GetSensorState(null, null).Result;
+
+            // Default sensor state is error
+            Assert.Equal(SensorStateEnum.Error, result.State);
+
+            // Verify that the sensor get state method is called.
+            _mockSensor.Verify(s => s.Object.GetState(), Times.AtLeastOnce());
+            
+            _mockSensor.Setup(s => s.Object.GetState()).Returns(SensorStateEnum.Initialized);
+            result = service.GetSensorState(null, null).Result;
+            Assert.Equal(SensorStateEnum.Initialized, result.State);
+        }
+        
+        [Fact]
+        public void Test_GetSensorMeasurement()
+        {
+            var service = new Bme680GrpcService(_mockLogger.Object, _mockSensor.Object);
+            service.GetSensorMeasurement(null, null);
+            
+            // Verify that the sensor get measurement method is called.
+            _mockSensor.Verify(s => s.Object.GetMeasurement(), Times.AtLeastOnce());
+
+        }
     }
 }
\ No newline at end of file
diff --git a/NucuCar.UnitTests/NucuCar.Sensors.Tests/EnvironmentSensor.Tests/TestBme680Sensor.cs b/NucuCar.UnitTests/NucuCar.Sensors.Tests/EnvironmentSensor.Tests/TestBme680Sensor.cs
new file mode 100644
index 0000000..1c7c60a
--- /dev/null
+++ b/NucuCar.UnitTests/NucuCar.Sensors.Tests/EnvironmentSensor.Tests/TestBme680Sensor.cs
@@ -0,0 +1,22 @@
+using NucuCar.Sensors.EnvironmentSensor;
+using NucuCarSensorsProto;
+
+namespace NucuCar.UnitTests.NucuCar.Sensors.Tests.EnvironmentSensor.Tests
+{
+    public partial class Bme680GrpcServiceTest
+    {
+        public class TestBme680Sensor : Bme680Sensor
+        {
+            // TODO Make more generic
+            public override EnvironmentSensorMeasurement GetMeasurement()
+            {
+                return new EnvironmentSensorMeasurement();
+            }
+            
+            public override SensorStateEnum GetState()
+            {
+                return SensorStateEnum.Error;
+            }
+        }
+    }
+}
\ No newline at end of file