diff --git a/CMakeLists.txt b/CMakeLists.txt
index d92f5378..c2d9e137 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -10,9 +10,6 @@ if(NOT CMAKE_BUILD_TYPE)
     set(CMAKE_BUILD_TYPE "Release")
 endif()
 
-add_compile_options(-Wall)
-
-
 ### git ###
 
 # clone mavlink submodule if it is missing
@@ -41,7 +38,6 @@ if("${GIT_VERSION_HASH}" STREQUAL "")
   set(GIT_VERSION_HASH "0")
 endif()
 
-
 ### source files ###
 
 include_directories(
@@ -60,13 +56,17 @@ file(GLOB_RECURSE ROSFLIGHT_SOURCES
     "comms/mavlink/mavlink.cpp"
 )
 
-
 ### select boards to compile ###
 
-option(BUILD_VARMINT "Build the varmint board target" ON)
+option(BUILD_VARMINT "Build the varmint board target" OFF)
+option(BUILD_TEST "Build the test board target" OFF)
 
 if(BUILD_VARMINT)
-    message(STATUS "Selecting varmint board target")
+    message("===== Selecting varmint board target. =====")
     add_subdirectory(boards/varmint)
-endif()
-
+elseif(BUILD_TEST)
+    message("===== Selecting test board target. ======")
+#    add_subdirectory()
+else()
+    message(FATAL_ERROR "No board selected! Please select the varmint or test board with -DBUILD_VARMINT=true or -DBUILD_TEST=true.")
+endif ()
\ No newline at end of file
diff --git a/boards/varmint b/boards/varmint
index c3cbf7c8..a66cacd3 160000
--- a/boards/varmint
+++ b/boards/varmint
@@ -1 +1 @@
-Subproject commit c3cbf7c8e153861f70de7d940b5f8601c2f5678a
+Subproject commit a66cacd33d69603267120dada41252f62b618e65
diff --git a/src/main.cpp b/src/main.cpp
index e6485881..7346bec5 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -35,18 +35,22 @@
  ******************************************************************************
  **/
 
-#include <Varmint.h>
 #include <mavlink.h>
 #include <rosflight.h>
 
-extern Varmint varmint;
+// Select which board implementation to include based on cmake variable
+#ifdef BUILD_VARMINT_BOARD
+#include <Varmint.h>
+extern Varmint varmint;  // TODO: Eliminate global variable
+#endif
+#ifdef BUILD_TEST_BOARD
+#include <test.h>
+#endif
 
 #ifdef __cplusplus
 extern "C" {
 #endif
-
 int main(void);
-
 #ifdef __cplusplus
 }
 #endif
@@ -57,12 +61,18 @@ int main(void);
  */
 int main(void)
 {
-  // Rosflight board code
-  varmint.init_board();
+  // Select which board implementation to instantiate based on cmake variable
+#ifdef BUILD_VARMINT_BOARD
+  rosflight_firmware::Board * board = &varmint;
+#endif
+#ifdef BUILD_TEST_BOARD
+  return 0;
+#endif
 
   // Rosflight base code
-  rosflight_firmware::Mavlink mavlink(varmint);
-  rosflight_firmware::ROSflight firmware(varmint, mavlink);
+  board->init_board();
+  rosflight_firmware::Mavlink mavlink(* board);
+  rosflight_firmware::ROSflight firmware(* board, mavlink);
 
   firmware.init();