diff --git a/quad/src/queue/test/Unity_test_queue.c b/quad/src/queue/test/Unity_test_queue.c
new file mode 100644
index 0000000000000000000000000000000000000000..c4718e5c31355513e59886c946bdd091d50c28a4
--- /dev/null
+++ b/quad/src/queue/test/Unity_test_queue.c
@@ -0,0 +1,158 @@
+#include "test.h"
+#include "queue.h"
+#include "unity.h"
+
+void test_free() {
+  struct Queue *q = queue_malloc(4);
+  queue_free(q);
+  TEST_ASSERT_NULL(q);
+}
+
+void test_add() {
+  struct Queue *q = queue_malloc(4);
+  unsigned char c;
+  TEST_ASSERT_EQUAL(0, queue_add(q, 'a'));
+  TEST_ASSERT_EQUAL(0, queue_add(q, 'b'));
+  TEST_ASSERT_EQUAL(0, queue_add(q, 'c'));
+  TEST_ASSERT_EQUAL(0, queue_add(q, 'd'));
+  TEST_ASSERT_EQUAL(-1, queue_add(q, 'x'));
+  queue_print(q);
+
+  queue_remove(q, &c);
+  queue_remove(q, &c);
+  TEST_ASSERT_EQUAL(0, queue_add(q, 'e'));
+  TEST_ASSERT_EQUAL(0, queue_add(q, 'f'));
+  TEST_ASSERT_EQUAL(-1, queue_add(q, 'x'));
+  queue_print(q);
+}
+
+void test_remove() {
+  struct Queue *q = queue_malloc(4);
+  unsigned char c;
+  queue_add(q, 'x');
+  queue_add(q, 'x');
+  TEST_ASSERT_EQUAL(0, queue_remove(q, &c));
+  TEST_ASSERT_EQUAL('x', c);
+  TEST_ASSERT_EQUAL(0, queue_remove(q, &c));
+  TEST_ASSERT_EQUAL('x', c);
+  TEST_ASSERT_EQUAL(-1, queue_remove(q, &c));
+  queue_print(q);
+
+  queue_add(q, 'a');
+  queue_add(q, 'b');
+  queue_add(q, 'c');
+  queue_add(q, 'd');
+  queue_add(q, 'x');
+  TEST_ASSERT_EQUAL(0, queue_remove(q, &c));
+  TEST_ASSERT_EQUAL('a', c);
+  TEST_ASSERT_EQUAL(0, queue_remove(q, &c));
+  TEST_ASSERT_EQUAL('b', c);
+  TEST_ASSERT_EQUAL(0, queue_remove(q, &c));
+  TEST_ASSERT_EQUAL('c', c);
+  TEST_ASSERT_EQUAL(0, queue_remove(q, &c));
+  TEST_ASSERT_EQUAL('d', c);
+  TEST_ASSERT_EQUAL(-1, queue_remove(q, &c));
+}
+
+
+void test_size() {
+  struct Queue *q = queue_malloc(4);
+  unsigned char c;
+
+  TEST_ASSERT_EQUAL(0, queue_size(q));
+  queue_print(q);
+
+  queue_add(q, 'a');
+  TEST_ASSERT_EQUAL(1, queue_size(q));
+  queue_print(q);
+
+  queue_add(q, 'b');
+  TEST_ASSERT_EQUAL(2, queue_size(q));
+  queue_print(q);
+
+  queue_add(q, 'c');
+  TEST_ASSERT_EQUAL(3, queue_size(q));
+  queue_print(q);
+
+  queue_add(q, 'd');
+  TEST_ASSERT_EQUAL(4, queue_size(q));
+  queue_print(q);
+
+  queue_remove(q, &c);
+  queue_remove(q, &c);
+  TEST_ASSERT_EQUAL(2, queue_size(q));
+  queue_print(q);
+
+  queue_add(q, 'e');
+  TEST_ASSERT_EQUAL(3, queue_size(q));
+  queue_print(q);
+}
+
+void test_full() {
+  struct Queue *q = queue_malloc(4);
+  unsigned char c;
+
+  TEST_ASSERT_FALSE(queue_full(q));
+  queue_print(q);
+
+  queue_add(q, 'a');
+  queue_add(q, 'b');
+  queue_add(q, 'c');
+  TEST_ASSERT_FALSE(queue_full(q));
+  queue_print(q);
+
+  queue_add(q, 'd');
+  TEST_ASSERT_TRUE(queue_full(q));
+  queue_print(q);
+
+  queue_remove(q, &c);
+  TEST_ASSERT_FALSE(queue_full(q));
+  queue_print(q);
+
+  queue_remove(q, &c);
+  queue_add(q, 'x');
+  queue_add(q, 'x');
+  TEST_ASSERT_TRUE(queue_full(q));
+  queue_print(q);
+}
+
+void test_empty() {
+  struct Queue *q = queue_malloc(4);
+  unsigned char c;
+
+  TEST_ASSERT_TRUE(queue_empty(q));
+  queue_print(q);
+
+  queue_add(q, 'a');
+  queue_add(q, 'b');
+  queue_add(q, 'c');
+  TEST_ASSERT_FALSE(queue_empty(q));
+  queue_print(q);
+
+  queue_add(q, 'd');
+  TEST_ASSERT_FALSE(queue_empty(q));
+  queue_print(q);
+
+  queue_remove(q, &c);
+  TEST_ASSERT_FALSE(queue_empty(q));
+  queue_print(q);
+
+  queue_remove(q, &c);
+  queue_remove(q, &c);
+  TEST_ASSERT_FALSE(queue_empty(q));
+
+  queue_remove(q, &c);
+  TEST_ASSERT_TRUE(queue_empty(q));
+}
+
+
+int main() {
+  UNITY_BEGIN();
+  RUN_TEST(test_free);
+  RUN_TEST(test_add);
+  RUN_TEST(test_remove);
+  RUN_TEST(test_size);
+  RUN_TEST(test_full);
+  RUN_TEST(test_empty);
+  return UNITY_END();
+}