diff --git a/Makefile b/Makefile
index 48cb90043..219ab95ae 100644
--- a/Makefile
+++ b/Makefile
@@ -638,7 +638,7 @@ uavo-collections_clean:
 #
 ##############################
 
-ALL_UNITTESTS := logfs
+ALL_UNITTESTS := logfs lednotification
 
 # Build the directory for the unit tests
 UT_OUT_DIR := $(BUILD_DIR)/unit_tests
diff --git a/flight/tests/lednotification/FreeRTOS.h b/flight/tests/lednotification/FreeRTOS.h
new file mode 100644
index 000000000..c8b49f26d
--- /dev/null
+++ b/flight/tests/lednotification/FreeRTOS.h
@@ -0,0 +1 @@
+#include <stdlib.h>
diff --git a/flight/tests/lednotification/Makefile b/flight/tests/lednotification/Makefile
new file mode 100644
index 000000000..d013f5b83
--- /dev/null
+++ b/flight/tests/lednotification/Makefile
@@ -0,0 +1,41 @@
+###############################################################################
+# @file       Makefile
+# @author     PhoenixPilot, http://github.com/PhoenixPilot, Copyright (C) 2012
+#             Copyright (c) 2013, The OpenPilot Team, http://www.openpilot.org
+# @addtogroup 
+# @{
+# @addtogroup 
+# @{
+# @brief Makefile for unit test
+###############################################################################
+#
+# This program is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 3 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful, but
+# WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+# or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+# for more details.
+#
+# You should have received a copy of the GNU General Public License along
+# with this program; if not, write to the Free Software Foundation, Inc.,
+# 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+#
+
+ifndef OPENPILOT_IS_COOL
+    $(error Top level Makefile must be used to build this target)
+endif
+
+include $(ROOT_DIR)/make/firmware-defs.mk
+
+EXTRAINCDIRS += $(TOPDIR)
+EXTRAINCDIRS += $(PIOS)/inc
+EXTRAINCDIRS += $(FLIGHTLIB)/inc
+EXTRAINCDIRS += $(FLIGHTLIB)
+
+SRC += $(FLIGHTLIB)/optypes.c
+SRC += $(PIOS)/common/pios_notify.c
+
+include $(ROOT_DIR)/make/unittest.mk
diff --git a/flight/tests/lednotification/openpilot.h b/flight/tests/lednotification/openpilot.h
new file mode 100644
index 000000000..4c2259d19
--- /dev/null
+++ b/flight/tests/lednotification/openpilot.h
@@ -0,0 +1,11 @@
+#ifndef OPENPILOT_H
+#define OPENPILOT_H
+
+#include <stdbool.h>
+
+#define PIOS_Assert(x) \
+    if (!(x)) { while (1) {; } \
+    }
+#define PIOS_DEBUG_Assert(x) PIOS_Assert(x)
+
+#endif /* OPENPILOT_H */
diff --git a/flight/tests/lednotification/pios.h b/flight/tests/lednotification/pios.h
new file mode 100644
index 000000000..12cd5e76a
--- /dev/null
+++ b/flight/tests/lednotification/pios.h
@@ -0,0 +1,13 @@
+#ifndef PIOS_H
+#define PIOS_H
+
+/* PIOS Feature Selection */
+#include "pios_config.h"
+
+#ifdef PIOS_INCLUDE_FREERTOS
+/* FreeRTOS Includes */
+#include "FreeRTOS.h"
+#endif
+#include "pios_mem.h"
+
+#endif /* PIOS_H */
diff --git a/flight/tests/lednotification/pios_config.h b/flight/tests/lednotification/pios_config.h
new file mode 100644
index 000000000..2a645e951
--- /dev/null
+++ b/flight/tests/lednotification/pios_config.h
@@ -0,0 +1,6 @@
+#ifndef PIOS_CONFIG_H
+#define PIOS_CONFIG_H
+
+#define PIOS_INCLUDE_FREERTOS
+
+#endif /* PIOS_CONFIG_H */
diff --git a/flight/tests/lednotification/pios_mem.h b/flight/tests/lednotification/pios_mem.h
new file mode 100644
index 000000000..279ce5aef
--- /dev/null
+++ b/flight/tests/lednotification/pios_mem.h
@@ -0,0 +1,34 @@
+/**
+ ******************************************************************************
+ *
+ * @file       pios_mem.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ * @addtogroup PiOS
+ * @{
+ * @addtogroup PiOS
+ * @{
+ * @brief PiOS memory allocation API
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+#ifndef PIOS_MEM_H
+#define PIOS_MEM_H
+
+#define pios_fastheapmalloc(size) (malloc(size))
+#define pios_malloc(size)         (malloc(size))
+#define pios_free(p)              (free(p))
+
+#endif /* PIOS_MEM_H */
diff --git a/flight/tests/lednotification/unittest.cpp b/flight/tests/lednotification/unittest.cpp
new file mode 100644
index 000000000..80ef7d8a4
--- /dev/null
+++ b/flight/tests/lednotification/unittest.cpp
@@ -0,0 +1,90 @@
+#include "gtest/gtest.h"
+
+#include <stdio.h> /* printf */
+#include <stdlib.h> /* abort */
+#include <string.h> /* memset */
+
+extern "C" {
+#define xTaskGetTickCount() 1
+#define portTICK_RATE_MS 1
+
+#include "lednotification.c"
+
+void PIOS_WS2811_setColorRGB(__attribute__((unused)) Color_t c, __attribute__((unused)) uint8_t led, __attribute__((unused)) bool update){
+
+}
+void PIOS_WS2811_Update(){
+
+}
+}
+
+class LedNotificationTest : public testing::Test {};
+
+TEST_F(LedNotificationTest, TestQueueOrder1) {
+    NotifierLedStatus_t status;
+
+    for (uint8_t i = 0; i < MAX_BACKGROUND_NOTIFICATIONS; i++) {
+        status.queued_priorities[i] = NOTIFY_PRIORITY_BACKGROUND;
+    }
+
+
+    ExtLedNotification_t notification0;
+    notification0.priority = NOTIFY_PRIORITY_LOW;
+    push_queued_sequence(&notification0, &status);
+    ExtLedNotification_t notification1;
+    notification1.priority = NOTIFY_PRIORITY_CRITICAL;
+    push_queued_sequence(&notification1, &status);
+    ExtLedNotification_t notification2;
+    notification2.priority = NOTIFY_PRIORITY_LOW;
+    push_queued_sequence(&notification2, &status);
+    ExtLedNotification_t notification3;
+    notification3.priority = NOTIFY_PRIORITY_CRITICAL;
+    push_queued_sequence(&notification3, &status);
+
+    EXPECT_EQ(NOTIFY_PRIORITY_LOW, status.queued_priorities[0]);
+    EXPECT_EQ(NOTIFY_PRIORITY_LOW, status.queued_priorities[1]);
+    EXPECT_EQ(NOTIFY_PRIORITY_CRITICAL, status.queued_priorities[2]);
+    EXPECT_EQ(NOTIFY_PRIORITY_CRITICAL, status.queued_priorities[3]);
+    EXPECT_EQ(NOTIFY_PRIORITY_BACKGROUND, status.queued_priorities[4]);
+
+}
+
+TEST_F(LedNotificationTest, TestQueueOrder2) {
+    NotifierLedStatus_t status;
+
+// Fails because insert_point and first_point will both be -1. This will also cause an array-out-of bounds at:
+// 146            status->queued_priorities[insert_point] = new_notification->priority;
+// 147            status->queued_sequences[insert_point]  = new_notification->sequence;
+// 148            updated_sequence = insert_point;
+
+    for (uint8_t i = 0; i < MAX_BACKGROUND_NOTIFICATIONS; i++) {
+        status.queued_priorities[i] = NOTIFY_PRIORITY_LOW;
+    }
+
+    ExtLedNotification_t notification;
+    notification.priority = NOTIFY_PRIORITY_REGULAR;
+    push_queued_sequence(&notification, &status);
+
+    EXPECT_EQ(NOTIFY_PRIORITY_REGULAR, status.queued_priorities[4]);
+    EXPECT_EQ(NOTIFY_PRIORITY_LOW, status.queued_priorities[3]);
+    EXPECT_EQ(NOTIFY_PRIORITY_LOW, status.queued_priorities[2]);
+    EXPECT_EQ(NOTIFY_PRIORITY_LOW, status.queued_priorities[1]);
+    EXPECT_EQ(NOTIFY_PRIORITY_LOW, status.queued_priorities[0]);
+}
+
+TEST_F(LedNotificationTest, TestQueueOrder3) {
+    NotifierLedStatus_t status;
+
+    // Fails because queued_priorities[0] _LOW and not _REGULAR. I _think_ this is a bug.
+    for (uint8_t i = 0; i < MAX_BACKGROUND_NOTIFICATIONS; i++) {
+        status.queued_priorities[i] = NOTIFY_PRIORITY_REGULAR;
+    }
+
+    ExtLedNotification_t notification;
+    notification.priority = NOTIFY_PRIORITY_LOW;
+    push_queued_sequence(&notification, &status);
+
+    for (uint8_t i = 0; i < MAX_BACKGROUND_NOTIFICATIONS; i++) {
+        EXPECT_EQ(NOTIFY_PRIORITY_REGULAR, status.queued_priorities[i]);
+    }
+}
diff --git a/make/unittest.mk b/make/unittest.mk
index 5963f5567..2190a6ab4 100644
--- a/make/unittest.mk
+++ b/make/unittest.mk
@@ -52,6 +52,10 @@ CXXFLAGS += -g -Wall -Wextra
 # Flags passed to the C compiler
 CONLYFLAGS += -std=gnu99
 
+# UNIT_TEST allows to for example to have optional test fixture code enabled or private code exposed in application modules. 
+CFLAGS += -DUNIT_TEST
+CPPFLAGS += -DUNIT_TEST
+
 # Common compiler flags
 CFLAGS += -O0 -g
 CFLAGS += -Wall -Werror