diff --git a/Drivers/Tests/CircularBufferTest.cpp b/Drivers/Tests/CircularBufferTest.cpp
new file mode 100644
index 00000000..75fa5e9a
--- /dev/null
+++ b/Drivers/Tests/CircularBufferTest.cpp
@@ -0,0 +1,80 @@
+#include <gtest/gtest.h>
+#include "circular_buffer.hpp"
+
+TEST(CircularBuffer, BasicTest) {
+    const size_t bufSize = 8;
+    uint8_t buf[bufSize];
+
+    // Has space initially
+    CircularBuffer circularBuf(buf, bufSize);
+    EXPECT_EQ(circularBuf.isFull(), false) << "The circular buffer does not have space initially!";
+    EXPECT_EQ(circularBuf.getNumAvailBytes(), 0) << "The circular buffer has data!";
+
+    const size_t data1Size = 4;
+    const uint8_t data1[data1Size] = {'A', 'B', 'C', 'D'};
+    for (size_t i = 0; i < data1Size; ++i) {
+        circularBuf.write(data1[i]);
+    }
+
+    EXPECT_EQ(circularBuf.isFull(), false) << "The circular buffer is full despite at half capacity!";
+    EXPECT_EQ(circularBuf.getNumAvailBytes(), data1Size) << "The circular buffer doesn't have the right amount of data!";
+
+    // Peek all elements
+    uint8_t peekVal ,peekRes;
+    for (size_t i = 0; i < data1Size; ++i) {
+        peekRes = circularBuf.peek(peekVal, i);
+        EXPECT_EQ(peekRes, 1) << "Couldn't peek byte #" << i;
+        EXPECT_EQ(peekVal, data1[i]) << "Expected byte #" << i << " to be " << data1[i] << ". Got " << peekVal << ".";
+    }
+
+    // Write and read to end of buf
+    for (size_t i = 0; i < data1Size; ++i) {
+        circularBuf.write(data1[i]);
+    }
+
+    uint8_t readBuf1[data1Size];
+    uint8_t readRes = 0;
+
+    // Read first half
+    readRes = circularBuf.read(readBuf1, data1Size);
+    uint8_t cmpRes = memcmp(readBuf1, data1, data1Size);
+    EXPECT_EQ(readRes, 1) << "Read failed (half buf read #1)";
+    EXPECT_EQ(cmpRes, 0) << "Read data not equal to write data! (half buf read #1)";
+
+    // Read second half
+    readRes = circularBuf.read(readBuf1, data1Size);
+    cmpRes = memcmp(readBuf1, data1, data1Size);
+    EXPECT_EQ(readRes, 1) << "Read failed (half buf read #2)";
+    EXPECT_EQ(cmpRes, 0) << "Read data not equal to write data! (half buf read #2)";
+
+    // Write and read to half of buf for next text
+    for (size_t i = 0; i < data1Size; ++i) {
+        circularBuf.write(data1[i]);
+    }
+    circularBuf.read(readBuf1, data1Size);
+
+    // Write and read past end
+    const size_t data2Size = bufSize;
+    uint8_t data2[data2Size] = {'a', 'b', 'c', 'd', 'e', 'f', 'g', 'h'};
+    for (size_t i = 0; i < data2Size; ++i) {
+        circularBuf.write(data2[i]);
+    }
+
+    EXPECT_EQ(circularBuf.isFull(), true) << "The circular buffer has space!";
+    EXPECT_EQ(circularBuf.getNumAvailBytes(), data2Size) << "The circular buffer doesn't have the right amount of data!";
+
+    uint8_t readBuf2[data2Size];
+    readRes = circularBuf.read(readBuf2, data2Size);
+    cmpRes = memcmp(readBuf2, data2, data2Size);
+    EXPECT_EQ(readRes, 1) << "Read failed (full buf read)";
+    EXPECT_EQ(cmpRes, 0) << "Read data not equal to write data! (full buf read)";
+
+    // Read past write pointer
+    uint8_t readByte = 'x';
+    readRes = circularBuf.read(&readByte, 1);
+    EXPECT_EQ(readRes, 0) << "Read succeeded when reading past wptr!";
+    EXPECT_EQ(readByte, 'x') << "Read byte is changed!";
+
+    EXPECT_EQ(circularBuf.isFull(), false) << "The circular buffer doesn't have space!";
+    EXPECT_EQ(circularBuf.getNumAvailBytes(), 0) << "The circular buffer has data!";
+}
\ No newline at end of file
diff --git a/Tools/Testing/CMakeLists.txt b/Tools/Testing/CMakeLists.txt
index 2fc8c832..52c74eff 100644
--- a/Tools/Testing/CMakeLists.txt
+++ b/Tools/Testing/CMakeLists.txt
@@ -75,7 +75,7 @@ file(GLOB TM_CODE
 
 # Drivers
 set(DRIVERS_FOLDER "${ROOT_DIR}/Drivers")
-set(DRIVERS_INC "{$DRIVERS_FOLDER}/Inc")
+set(DRIVERS_INC "${DRIVERS_FOLDER}/Common/Inc")
 set(DRIVERS_SRC "{$DRIVERS_FOLDER}/Src")
 
 include_directories(${DRIVERS_INC})
@@ -84,6 +84,7 @@ file(GLOB DRIVERS_CODE
 
     # add necessary Drivers source files below
     # "${DRIVERS_SRC}/foo.cpp"
+    "${DRIVERS_FOLDER}/Common/Src/circular_buffer.cpp"
 )
 
 # --- ZeroPilot File Indexing End ---