diff --git a/src/MS56xx.cpp b/src/MS56xx.cpp
index 409df08..17e8599 100644
--- a/src/MS56xx.cpp
+++ b/src/MS56xx.cpp
@@ -3,9 +3,11 @@
 #include <sta/debug/assert.hpp>
 #include <sta/debug/debug.hpp>
 #include <sta/lang.hpp>
+#include <sta/config.hpp>
 
 #include <cmath>
 
+#ifndef STA_SPATZ_ENABLED
 
 namespace sta 
 {
@@ -215,3 +217,5 @@ namespace sta
         return true;
     }
 }
+
+#endif // STA_SPATZ_ENABLED
\ No newline at end of file
diff --git a/src/mock.cpp b/src/mock.cpp
new file mode 100644
index 0000000..1caa36a
--- /dev/null
+++ b/src/mock.cpp
@@ -0,0 +1,160 @@
+#include <sta/drivers/MS56xx.hpp>
+
+#include <sta/config.hpp>
+#ifdef STA_SPATZ_ENABLED
+
+#include <sta/debug/spatz.hpp>
+
+/**
+ * @brief SPATZ IDs for the pressure and temperature data.
+ * 
+ */
+#define STA_MS56XX_PRESSURE_SPATZ_ID        1
+#define STA_MS56XX_TEMPERATURE_SPATZ_ID     2
+
+
+namespace sta
+{
+    MS56xx::MS56xx(SPIDevice * device, Version version, DelayUsFunc delay, OsrLevel osr /* = OsrLevel::_1024 */)
+        : device_{device},
+          version_{version},
+          delay_{delay},
+          osr_{osr},
+          intf_{Intf::SPI},
+          C_{}
+    {
+        STA_ASSERT(device != nullptr);
+    }
+
+    MS56xx::MS56xx(I2CDevice * device, Version version, DelayUsFunc delay, OsrLevel osr /* = OsrLevel::_1024 */)
+        : device_{device},
+          version_{version},
+          delay_{delay},
+          osr_{osr},
+          intf_{Intf::I2C},
+          C_{}
+    {
+        STA_ASSERT(device != nullptr);
+    }
+
+    bool MS56xx::init()
+    {
+        return true;
+    }
+
+    void MS56xx::setOsr(OsrLevel osr)
+    {
+        osr_ = osr;
+    }
+
+    void MS56xx::reset()
+    {
+        delay_(MS56xx::RESET_DELAY);
+    }
+
+    void MS56xx::requestData(DataType type)
+    {
+        delay_(osrDelay());
+    }
+
+    float MS56xx::getPressure(Unit unit /* = Unit::hPa */)
+    {    
+        requestData(TEMPERATURE);
+        float temperature;
+        spatz::request(STA_MS56XX_TEMPERATURE_SPATZ_ID, &temperature, 1);
+        
+        requestData(PRESSURE);
+        float pressure;
+        spatz::request(STA_MS56XX_PRESSURE_SPATZ_ID, &pressure, 1);
+        
+        // Convert to desired unit.
+        pressure = convertPressure(pressure, unit);
+
+        return pressure;
+    }
+
+    float MS56xx::getTemperature()
+    {
+        requestData(TEMPERATURE);
+		float temperature;
+        spatz::request(STA_MS56XX_TEMPERATURE_SPATZ_ID, &temperature, 1);
+
+    	return temperature;
+    }
+
+    void MS56xx::setPressureReference(float pressRef, float altRef, Unit unit /* = Unit::hPa */)
+    {
+        // Taken from: https://cdn-shop.adafruit.com/datasheets/BST-BMP180-DS000-09.pdf, page 17.
+        sealevel_ = pressRef / (std::pow((1 - altRef / 44330), 5.255));
+    }
+
+    float MS56xx::getAltitudeEstimate()
+    {
+        float pressure = getPressure(Unit::hPa);
+
+        // Taken from: https://cdn-shop.adafruit.com/datasheets/BST-BMP180-DS000-09.pdf, page 16
+        float altitude = 44330 * (1 - std::pow(pressure / sealevel_, 1 / 5.255));
+
+        return altitude;
+    }
+
+    void MS56xx::readPROM() {
+        
+    }
+
+    void MS56xx::initConstants()
+    {
+        
+    }
+
+    inline float MS56xx::convertPressure(float pressure, Unit unit)
+    {
+        switch (unit)
+        {
+        case Unit::mbar:
+            return pressure / 100.0f;
+        case Unit::hPa:
+            return pressure / 100.0f;
+        case Unit::Pa:
+            return pressure;
+        default:
+            STA_UNREACHABLE();
+            break;
+        }
+    }
+
+    inline uint16_t MS56xx::uint_8BufferTouint16_t(uint8_t* buffer) {
+        return (buffer[0] << 8) | buffer[1];
+    }
+
+    bool MS56xx::busCommand(uint8_t command)
+    {
+        return true;
+    }
+
+    uint32_t MS56xx::osrDelay() {
+        // Delay times taken from:
+        // https://www.amsys-sensor.com/downloads/notes/MS5XXX-C-code-example-for-MS56xx-MS57xx-MS58xx-AMSYS-an520e.pdf
+        switch (osr_) {
+        case _256:
+            return 600;
+        case _512:
+            return 1200;
+        case _1024:
+            return 2300;
+        case _2048:
+            return 4600;
+        case _4096:
+            return 9100;
+        default:
+            STA_UNREACHABLE();
+        }
+    }
+
+    bool MS56xx::busRead(uint8_t reg, uint8_t * buffer, size_t length)
+    {
+        return true;
+    }
+} // namespace sta
+
+#endif // STA_SPATZ_ENABLED
\ No newline at end of file