diff --git a/CMakeLists.txt b/CMakeLists.txt
index f2a4d79525c4686cb91f0feda52a691468079f25..15c7ab541836426263f61e1c28f991bea2f3cf46 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -32,611 +32,270 @@ project(SkywardBoardcore)
 #                                 Entrypoints                                 #
 #-----------------------------------------------------------------------------#
 
-add_executable(config-dsgamma
-    src/entrypoints/config-dsgamma.cpp
-    ${SHARED_SOURCES}
-    ${GAMMA868_SOURCES}
-)
-target_link_libraries(config-dsgamma PRIVATE fmt::fmt-header-only)
+#src/entrypoints/anakin/anakin-accel.cpp
+#src/entrypoints/anakin/anakin-demo-board.cpp
+#src/entrypoints/anakin/anakin-test-canbus.cpp
+#src/entrypoints/anakin/anakin-test-dma.cpp
+#src/entrypoints/anakin/anakin-test-suite.cpp
+#src/entrypoints/anakin/canbus-test.cpp
+#src/entrypoints/anakin/dma-lowlevel-test.cpp
+#src/entrypoints/anakin/test-busfault.cpp
+#src/entrypoints/anakin/test-scheduler.cpp
+#src/entrypoints/anakin/test_ethernet.cpp
+#src/entrypoints/anakin/test_i2c.cpp
+
+add_executable(bmx160-calibration-entry src/entrypoints/bmx160-calibration-entry.cpp)
+set_property(TARGET bmx160-calibration-entry PROPERTY OPT_BOARD stm32f429zi_skyward_death_stack_x)
+sbs_target(bmx160-calibration-entry)
+
+add_executable(config-dsgamma src/entrypoints/config-dsgamma.cpp)
 set_property(TARGET config-dsgamma PROPERTY OPT_BOARD stm32f429zi_stm32f4discovery)
 sbs_target(config-dsgamma)
 
-#-----------------------------------------------------------------------------#
-
-add_executable(kernel-testsuite src/entrypoints/kernel-testsuite.cpp)
-set_property(TARGET kernel-testsuite PROPERTY OPT_BOARD stm32f429zi_stm32f4discovery)
-sbs_target(kernel-testsuite)
-
-#-----------------------------------------------------------------------------#
+#src/entrypoints/examples/example-fsm.cpp
 
 add_executable(mxgui-helloworld src/entrypoints/examples/mxgui-helloworld.cpp)
-target_compile_definitions(mxgui-helloworld PRIVATE DEBUG)
 set_property(TARGET mxgui-helloworld PROPERTY OPT_BOARD stm32f429zi_stm32f4discovery)
-sbs_link_mxgui(mxgui-helloworld)
 sbs_target(mxgui-helloworld)
 
-#-----------------------------------------------------------------------------#
+add_executable(kernel-testsuite src/entrypoints/kernel-testsuite.cpp)
+set_property(TARGET kernel-testsuite PROPERTY OPT_BOARD stm32f429zi_stm32f4discovery)
+sbs_target(kernel-testsuite)
 
-add_executable(sdcard-benchmark
-    src/entrypoints/sdcard-benchmark.cpp
-    ${SHARED_SOURCES}
-)
-target_link_libraries(sdcard-benchmark PRIVATE fmt::fmt-header-only)
+add_executable(sdcard-benchmark src/entrypoints/sdcard-benchmark.cpp)
 set_property(TARGET sdcard-benchmark PROPERTY OPT_BOARD stm32f429zi_stm32f4discovery)
 sbs_target(sdcard-benchmark)
 
-#-----------------------------------------------------------------------------#
-
-add_executable(bmx160-calibration-entry
-    src/entrypoints/bmx160-calibration-entry.cpp
-    ${SHARED_SOURCES}
-    ${SPI_SOURCES}
-    ${BMX160_SOURCES}
-    ${BMX160WITHCORRECTION_SOURCES}
-    ${CALIBRATION_SOURCES}
-)
-target_link_libraries(bmx160-calibration-entry PRIVATE Eigen3::Eigen fmt::fmt-header-only)
-set_property(TARGET bmx160-calibration-entry PROPERTY OPT_BOARD stm32f429zi_skyward_death_stack_x)
-sbs_target(bmx160-calibration-entry)
-
 #-----------------------------------------------------------------------------#
 #                                Tests - Catch                                #
 #-----------------------------------------------------------------------------#
 
-add_executable(tests-catch
-    src/tests/catch/catch-tests-entry.cpp
-    ${SHARED_SOURCES}
-    ${TESTS_BOARDCORE_SOURCES}
-    ${TEST_UTILS_SOURCES}
-    ${SPI_SOURCES}
-    ${SENSORMANAGER_SOURCES}
-    ${XBEE_SOURCES}
-)
-target_link_libraries(tests-catch PRIVATE Eigen3::Eigen fmt::fmt-header-only Catch2::Catch2)
-target_compile_definitions(tests-catch PRIVATE USE_MOCK_PERIPHERALS)
-set_property(TARGET tests-catch PROPERTY OPT_BOARD stm32f429zi_stm32f4discovery)
-sbs_target(tests-catch)
-
-#-----------------------------------------------------------------------------#
-
-add_executable(test-kalman-eigen
-    src/tests/catch/test-kalman-eigen.cpp
-    ${SHARED_SOURCES}
-)
-target_link_libraries(test-kalman-eigen PRIVATE Eigen3::Eigen fmt::fmt-header-only Catch2::Catch2)
-target_compile_definitions(test-kalman-eigen PRIVATE DEBUG STANDALONE_CATCH1_TEST)
-set_property(TARGET test-kalman-eigen PROPERTY OPT_BOARD stm32f429zi_stm32f4discovery)
-sbs_target(test-kalman-eigen)
-
-#-----------------------------------------------------------------------------#
-
-add_executable(test-taskscheduler
-    src/tests/test-taskscheduler.cpp
-    ${SHARED_SOURCES}
-)
-target_link_libraries(test-taskscheduler PRIVATE fmt::fmt-header-only Catch2::Catch2)
-set_property(TARGET test-taskscheduler PROPERTY OPT_BOARD stm32f429zi_skyward_death_stack_x)
-sbs_target(test-taskscheduler)
-
-#-----------------------------------------------------------------------------#
-
-add_executable(test-serial
-    src/tests/test-serial.cpp
-    ${SHARED_SOURCES}
-)
-target_link_libraries(test-serial PRIVATE fmt::fmt-header-only Catch2::Catch2)
-set_property(TARGET test-serial PROPERTY OPT_BOARD stm32f429zi_skyward_death_stack_x)
-sbs_target(test-serial)
-
-#-----------------------------------------------------------------------------#
-
-add_executable(test-trace-logger
-    src/tests/test-trace-logger.cpp
-    ${SHARED_SOURCES}
-)
-target_link_libraries(test-trace-logger PRIVATE fmt::fmt-header-only Catch2::Catch2)
-target_compile_definitions(test-trace-logger PRIVATE DEBUG)
-set_property(TARGET test-trace-logger PROPERTY OPT_BOARD stm32f429zi_stm32f4discovery)
-sbs_target(test-trace-logger)
-
-#-----------------------------------------------------------------------------#
-
-add_executable(test-hsm
-    src/tests/test-hsm.cpp
-    ${SHARED_SOURCES}
-)
-target_link_libraries(test-hsm PRIVATE fmt::fmt-header-only Catch2::Catch2)
-target_compile_definitions(test-hsm PRIVATE DEBUG)
-set_property(TARGET test-hsm PROPERTY OPT_BOARD stm32f429zi_stm32f4discovery)
-sbs_target(test-hsm)
-
-#-----------------------------------------------------------------------------#
-
-add_executable(test-interrupt
-    src/tests/test-interrupt.cpp
-    ${SHARED_SOURCES}
-)
-target_link_libraries(test-interrupt PRIVATE fmt::fmt-header-only Catch2::Catch2)
-target_compile_definitions(test-interrupt PRIVATE DEBUG)
-set_property(TARGET test-interrupt PROPERTY OPT_BOARD stm32f429zi_stm32f4discovery)
-sbs_target(test-interrupt)
+add_executable(test-calibration-benchmark src/tests/calibration/test-calibration-benchmark.cpp)
+set_property(TARGET test-calibration-benchmark PROPERTY OPT_BOARD stm32f407vg_stm32f4discovery)
+sbs_target(test-calibration-benchmark)
 
-#-----------------------------------------------------------------------------#
+add_executable(test-calibration-stats src/tests/calibration/test-calibration-stats.cpp)
+set_property(TARGET test-calibration-stats PROPERTY OPT_BOARD stm32f407vg_stm32f4discovery)
+sbs_target(test-calibration-stats)
 
-add_executable(test-timestamp-timer
-    src/tests/test-timestamp-timer.cpp
-    ${SHARED_SOURCES}
-)
-target_link_libraries(test-timestamp-timer PRIVATE fmt::fmt-header-only Catch2::Catch2)
-target_compile_definitions(test-timestamp-timer PRIVATE DEBUG)
-set_property(TARGET test-timestamp-timer PROPERTY OPT_BOARD stm32f429zi_stm32f4discovery)
-sbs_target(test-timestamp-timer)
+add_executable(test-calibration src/tests/calibration/test-calibration.cpp)
+set_property(TARGET test-calibration PROPERTY OPT_BOARD stm32f407vg_stm32f4discovery)
+sbs_target(test-calibration)
 
-#-----------------------------------------------------------------------------#
+add_executable(tests-catch src/tests/catch/catch-tests-entry.cpp)
+target_compile_definitions(tests-catch PUBLIC USE_MOCK_PERIPHERALS)
+set_property(TARGET tests-catch PROPERTY OPT_BOARD stm32f429zi_stm32f4discovery)
+sbs_target(tests-catch)
 
-add_executable(test-logger
-    src/tests/logger/test-logger.cpp
-    ${SHARED_SOURCES}
-)
-target_link_libraries(test-logger PRIVATE fmt::fmt-header-only Catch2::Catch2)
-set_property(TARGET test-logger PROPERTY OPT_BOARD stm32f429zi_stm32f4discovery)
-sbs_target(test-logger)
+#src/tests/catch/examples/example-test-factorial.cpp
+#src/tests/catch/examples/example-test-fsm.cpp
+#src/tests/catch/spidriver/test-spidriver.cpp
+#src/tests/catch/test-aero.cpp
+#src/tests/catch/test-buttonhandler.cpp
 
-#-----------------------------------------------------------------------------#
+add_executable(test-circularbuffer src/tests/catch/test-circularbuffer.cpp)
+target_compile_definitions(test-circularbuffer PRIVATE STANDALONE_CATCH1_TEST)
+set_property(TARGET test-circularbuffer PROPERTY OPT_BOARD stm32f429zi_stm32f4discovery)
+sbs_target(test-circularbuffer)
 
-add_executable(test-kalman-eigen-benchmark
-    src/tests/kalman/test-kalman-eigen-benchmark.cpp
-    ${SHARED_SOURCES}
-)
-target_link_libraries(test-kalman-eigen-benchmark PRIVATE Eigen3::Eigen fmt::fmt-header-only Catch2::Catch2)
-set_property(TARGET test-kalman-eigen-benchmark PROPERTY OPT_BOARD stm32f429zi_stm32f4discovery)
-sbs_target(test-kalman-eigen-benchmark)
+add_executable(test-eventbroker src/tests/catch/test-eventbroker.cpp)
+target_compile_definitions(test-eventbroker PRIVATE STANDALONE_CATCH1_TEST)
+set_property(TARGET test-eventbroker PROPERTY OPT_BOARD stm32f429zi_stm32f4discovery)
+sbs_target(test-eventbroker)
 
-#-----------------------------------------------------------------------------#
+#src/tests/catch/test-hardwaretimer.cpp
 
-add_executable(test-pinobserver
-    src/tests/test-pinobserver.cpp
-    ${SHARED_SOURCES}
-)
-target_link_libraries(test-pinobserver PRIVATE fmt::fmt-header-only Catch2::Catch2)
-set_property(TARGET test-pinobserver PROPERTY OPT_BOARD stm32f429zi_stm32f4discovery)
-sbs_target(test-pinobserver)
+add_executable(test-kalman-eigen src/tests/catch/test-kalman-eigen.cpp)
+target_compile_definitions(test-kalman-eigen PRIVATE STANDALONE_CATCH1_TEST)
+set_property(TARGET test-kalman-eigen PROPERTY OPT_BOARD stm32f429zi_stm32f4discovery)
+sbs_target(test-kalman-eigen)
 
-#-----------------------------------------------------------------------------#
+#src/tests/catch/test-packetqueue.cpp
 
-add_executable(test-sensormanager-catch
-    src/tests/catch/test-sensormanager-catch.cpp
-    ${SHARED_SOURCES}
-    ${SENSORMANAGER_SOURCES}
-)
-target_link_libraries(test-sensormanager-catch PRIVATE fmt::fmt-header-only Catch2::Catch2)
+add_executable(test-sensormanager-catch src/tests/catch/test-sensormanager-catch.cpp)
 target_compile_definitions(test-sensormanager-catch PRIVATE STANDALONE_CATCH1_TEST)
 set_property(TARGET test-sensormanager-catch PROPERTY OPT_BOARD stm32f429zi_skyward_death_stack_x)
 sbs_target(test-sensormanager-catch)
 
-#-----------------------------------------------------------------------------#
-
-add_executable(test-sensormanager
-    src/tests/test-sensormanager.cpp
-    ${SHARED_SOURCES}
-    ${SENSORMANAGER_SOURCES}
-)
-target_link_libraries(test-sensormanager PRIVATE fmt::fmt-header-only Catch2::Catch2)
-target_compile_definitions(test-sensormanager PRIVATE DEBUG)
-set_property(TARGET test-sensormanager PROPERTY OPT_BOARD stm32f429zi_skyward_death_stack_x)
-sbs_target(test-sensormanager)
-
-#-----------------------------------------------------------------------------#
-
-add_executable(test-calibration
-    src/tests/calibration/test-calibration.cpp
-    ${SHARED_SOURCES}
-    ${SPI_SOURCES}
-    ${CALIBRATION_SOURCES}
-)
-target_link_libraries(test-calibration PRIVATE Eigen3::Eigen fmt::fmt-header-only Catch2::Catch2)
-target_compile_definitions(test-calibration PRIVATE DEBUG)
-set_property(TARGET test-calibration PROPERTY OPT_BOARD stm32f407vg_stm32f4discovery)
-sbs_target(test-calibration)
-
-#-----------------------------------------------------------------------------#
-
-add_executable(test-calibration-benchmark
-    src/tests/calibration/test-calibration-benchmark.cpp
-    ${SHARED_SOURCES}
-    ${SPI_SOURCES}
-    ${CALIBRATION_SOURCES}
-)
-target_link_libraries(test-calibration-benchmark PRIVATE Eigen3::Eigen fmt::fmt-header-only Catch2::Catch2)
-target_compile_definitions(test-calibration-benchmark PRIVATE DEBUG)
-set_property(TARGET test-calibration-benchmark PROPERTY OPT_BOARD stm32f407vg_stm32f4discovery)
-sbs_target(test-calibration-benchmark)
-
-#-----------------------------------------------------------------------------#
-
-add_executable(test-calibration-stats
-    src/tests/calibration/test-calibration-stats.cpp
-    ${SHARED_SOURCES}
-    ${SPI_SOURCES}
-    ${CALIBRATION_SOURCES}
-)
-target_link_libraries(test-calibration-stats PRIVATE Eigen3::Eigen fmt::fmt-header-only Catch2::Catch2)
-target_compile_definitions(test-calibration-stats PRIVATE DEBUG)
-set_property(TARGET test-calibration-stats PROPERTY OPT_BOARD stm32f407vg_stm32f4discovery)
-sbs_target(test-calibration-stats)
+#src/tests/catch/test-xbee.cpp
+#src/tests/catch/xbee/test-xbee-driver.cpp
+#src/tests/catch/xbee/test-xbee-parser.cpp
 
 #-----------------------------------------------------------------------------#
 #                               Tests - Drivers                               #
 #-----------------------------------------------------------------------------#
 
-add_executable(test-dsgamma
-    src/tests/drivers/test-dsgamma.cpp
-    ${SHARED_SOURCES}
-    ${GAMMA868_SOURCES}
-)
-target_link_libraries(test-dsgamma PRIVATE fmt::fmt-header-only)
-set_property(TARGET test-dsgamma PROPERTY OPT_BOARD stm32f429zi_stm32f4discovery)
-sbs_target(test-dsgamma)
-
-#-----------------------------------------------------------------------------#
-
-add_executable(test-canbus
-    src/tests/drivers/test-canbus.cpp
-    ${SHARED_SOURCES}
-    ${CANBUS_SOURCES}
-)
-target_link_libraries(test-canbus PRIVATE fmt::fmt-header-only)
-target_compile_definitions(test-canbus PRIVATE DEBUG)
-set_property(TARGET test-canbus PROPERTY OPT_BOARD stm32f429zi_skyward_death_stack_x)
-sbs_target(test-canbus)
-
-#-----------------------------------------------------------------------------#
-
-add_executable(test-pwm
-    src/tests/drivers/test-pwm.cpp
-    ${SHARED_SOURCES}
-    ${PWM_SOURCES}
-)
-target_link_libraries(test-pwm PRIVATE fmt::fmt-header-only)
-target_compile_definitions(test-pwm PRIVATE DEBUG)
-set_property(TARGET test-pwm PROPERTY OPT_BOARD stm32f429zi_stm32f4discovery)
-sbs_target(test-pwm)
-
-#-----------------------------------------------------------------------------#
-
-add_executable(test-lis3mdl
-    src/tests/drivers/test-lis3mdl.cpp
-    ${SHARED_SOURCES}
-    ${SPI_SOURCES}
-)
-target_link_libraries(test-lis3mdl PRIVATE fmt::fmt-header-only)
-target_compile_definitions(test-lis3mdl PRIVATE DEBUG)
-set_property(TARGET test-lis3mdl PROPERTY OPT_BOARD stm32f429zi_skyward_death_stack_x)
-sbs_target(test-lis3mdl)
+add_executable(test-analog-pressure-sensors src/tests/drivers/analog/test-analog-pressure-sensors.cpp)
+set_property(TARGET test-analog-pressure-sensors PROPERTY OPT_BOARD stm32f429zi_stm32f4discovery)
+sbs_target(test-analog-pressure-sensors)
 
-#-----------------------------------------------------------------------------#
+add_executable(test-battery-voltage src/tests/drivers/analog/test-battery-voltage.cpp)
+set_property(TARGET test-battery-voltage PROPERTY OPT_BOARD stm32f429zi_stm32f4discovery)
+sbs_target(test-battery-voltage)
 
-add_executable(test-mavlink
-    src/tests/drivers/test-mavlink.cpp
-    ${SHARED_SOURCES}
-    ${GAMMA868_SOURCES}
-)
-target_link_libraries(test-mavlink PRIVATE fmt::fmt-header-only Mavlink::Mavlink)
-target_compile_definitions(test-mavlink PRIVATE DEBUG)
-set_property(TARGET test-mavlink PROPERTY OPT_BOARD stm32f429zi_skyward_homeone)
-sbs_target(test-mavlink)
+add_executable(test-current-sensor src/tests/drivers/analog/test-current-sensor.cpp)
+set_property(TARGET test-current-sensor PROPERTY OPT_BOARD stm32f429zi_stm32f4discovery)
+sbs_target(test-current-sensor)
 
-#-----------------------------------------------------------------------------#
+#src/tests/drivers/old_examples/test-ad7994-bare.cpp
+#src/tests/drivers/old_examples/test-ad7994.cpp
+#src/tests/drivers/old_examples/test-i2c-mpu9255.cpp
+#src/tests/drivers/old_examples/test-imu-adis.cpp
+#src/tests/drivers/old_examples/test-lsm.cpp
+#src/tests/drivers/old_examples/test-spi2.cpp
+#src/tests/drivers/old_examples/test-tempSensor.cpp
+#src/tests/drivers/old_examples/test-unit-LM75B.cpp
 
-add_executable(test-circularbuffer
-    src/tests/catch/test-circularbuffer.cpp
-    ${SHARED_SOURCES}
-    ${TEST_UTILS_SOURCES}
-)
-target_link_libraries(test-circularbuffer PRIVATE fmt::fmt-header-only Catch2::Catch2)
-target_compile_definitions(test-circularbuffer PRIVATE STANDALONE_CATCH1_TEST)
-set_property(TARGET test-circularbuffer PROPERTY OPT_BOARD stm32f429zi_stm32f4discovery)
-sbs_target(test-circularbuffer)
+add_executable(test-ads1118 src/tests/drivers/test-ads1118.cpp)
+set_property(TARGET test-ads1118 PROPERTY OPT_BOARD stm32f407vg_stm32f4discovery)
+sbs_target(test-ads1118)
 
-#-----------------------------------------------------------------------------#
+add_executable(test-bme280 src/tests/drivers/test-bme280.cpp)
+set_property(TARGET test-bme280 PROPERTY OPT_BOARD stm32f407vg_stm32f4discovery)
+sbs_target(test-bme280)
 
-add_executable(test-eventbroker
-    src/tests/catch/test-eventbroker.cpp
-    ${SHARED_SOURCES}
-    ${TEST_UTILS_SOURCES}
-)
-target_link_libraries(test-eventbroker PRIVATE fmt::fmt-header-only Catch2::Catch2)
-target_compile_definitions(test-eventbroker PRIVATE STANDALONE_CATCH1_TEST)
-set_property(TARGET test-eventbroker PROPERTY OPT_BOARD stm32f429zi_stm32f4discovery)
-sbs_target(test-eventbroker)
+add_executable(test-bmx160 src/tests/drivers/test-bmx160.cpp)
+set_property(TARGET test-bmx160 PROPERTY OPT_BOARD stm32f429zi_skyward_death_stack_x)
+sbs_target(test-bmx160)
 
-#-----------------------------------------------------------------------------#
+add_executable(test-bmx160-with-correction src/tests/drivers/test-bmx160-with-correction.cpp)
+set_property(TARGET test-bmx160-with-correction PROPERTY OPT_BOARD stm32f429zi_skyward_death_stack_x)
+sbs_target(test-bmx160-with-correction)
 
-add_executable(test-servo
-    src/tests/drivers/test-servo.cpp
-    ${SHARED_SOURCES}
-    ${SERVO_SOURCES}
-    ${PWM_SOURCES}
-)
-target_link_libraries(test-servo PRIVATE fmt::fmt-header-only)
-target_compile_definitions(test-servo PRIVATE DEBUG)
-set_property(TARGET test-servo PROPERTY OPT_BOARD stm32f429zi_skyward_death_stack_x)
-sbs_target(test-servo)
+add_executable(test-canbus src/tests/drivers/test-canbus.cpp)
+set_property(TARGET test-canbus PROPERTY OPT_BOARD stm32f429zi_skyward_death_stack_x)
+sbs_target(test-canbus)
 
-#-----------------------------------------------------------------------------#
+add_executable(test-dsgamma src/tests/drivers/test-dsgamma.cpp)
+set_property(TARGET test-dsgamma PROPERTY OPT_BOARD stm32f429zi_stm32f4discovery)
+sbs_target(test-dsgamma)
 
-add_executable(test-hbridge
-    src/tests/drivers/test-hbridge.cpp
-    ${SHARED_SOURCES}
-    ${HBRIDGE_SOURCES}
-    ${PWM_SOURCES}
-)
-target_link_libraries(test-hbridge PRIVATE fmt::fmt-header-only)
-target_compile_definitions(test-hbridge PRIVATE DEBUG)
+add_executable(test-hbridge src/tests/drivers/test-hbridge.cpp)
 set_property(TARGET test-hbridge PROPERTY OPT_BOARD stm32f429zi_stm32f4discovery)
 sbs_target(test-hbridge)
 
-#-----------------------------------------------------------------------------#
-
-add_executable(test-ms5803
-    src/tests/drivers/test-ms5803.cpp
-    ${SHARED_SOURCES}
-    ${SPI_SOURCES}
-    ${SENSORMANAGER_SOURCES}
-    ${MS5803_SOURCES}
-)
-target_link_libraries(test-ms5803 PRIVATE fmt::fmt-header-only)
-target_compile_definitions(test-ms5803 PRIVATE DEBUG)
-set_property(TARGET test-ms5803 PROPERTY OPT_BOARD stm32f429zi_skyward_death_stack_x)
-sbs_target(test-ms5803)
-
-#-----------------------------------------------------------------------------#
+add_executable(test-hwtimer-chain src/tests/drivers/test-hwtimer-chain.cpp)
+set_property(TARGET test-hwtimer-chain PROPERTY OPT_BOARD stm32f407vg_stm32f4discovery)
+sbs_target(test-hwtimer-chain)
 
-add_executable(test-ublox
-    src/tests/drivers/test-ublox.cpp
-    ${SHARED_SOURCES}
-    ${UBLOXGPS_SOURCES}
-)
-target_link_libraries(test-ublox PRIVATE fmt::fmt-header-only)
-target_compile_definitions(test-ublox PRIVATE DEBUG)
-set_property(TARGET test-ublox PROPERTY OPT_BOARD stm32f429zi_skyward_death_stack_x)
-sbs_target(test-ublox)
+add_executable(test-internal-adc src/tests/drivers/test-internal-adc.cpp)
+set_property(TARGET test-internal-adc PROPERTY OPT_BOARD stm32f429zi_skyward_death_stack_x)
+sbs_target(test-internal-adc)
 
-#-----------------------------------------------------------------------------#
+add_executable(test-internal-adc-dma src/tests/drivers/test-internal-adc-dma.cpp)
+set_property(TARGET test-internal-adc-dma PROPERTY OPT_BOARD stm32f429zi_stm32f4discovery)
+sbs_target(test-internal-adc-dma)
 
-add_executable(test-l3gd20
-    src/tests/drivers/test-l3gd20.cpp
-    ${SHARED_SOURCES}
-    ${SPI_SOURCES}
-)
-target_link_libraries(test-l3gd20 PRIVATE fmt::fmt-header-only)
+add_executable(test-l3gd20 src/tests/drivers/test-l3gd20.cpp)
 set_property(TARGET test-l3gd20 PROPERTY OPT_BOARD stm32f429zi_stm32f4discovery)
 sbs_target(test-l3gd20)
 
-#-----------------------------------------------------------------------------#
-
-add_executable(test-l3gd20-fifo
-    src/tests/drivers/test-l3gd20-fifo.cpp
-    ${SHARED_SOURCES}
-    ${SPI_SOURCES}
-)
-target_link_libraries(test-l3gd20-fifo PRIVATE fmt::fmt-header-only)
+add_executable(test-l3gd20-fifo src/tests/drivers/test-l3gd20-fifo.cpp)
 set_property(TARGET test-l3gd20-fifo PROPERTY OPT_BOARD stm32f429zi_stm32f4discovery)
 sbs_target(test-l3gd20-fifo)
 
-#-----------------------------------------------------------------------------#
-
-add_executable(test-lis3dsh
-    src/tests/drivers/test-lis3dsh.cpp
-    ${SHARED_SOURCES}
-    ${SPI_SOURCES}
-)
-target_link_libraries(test-lis3dsh PRIVATE fmt::fmt-header-only)
-target_compile_definitions(test-lis3dsh PRIVATE DEBUG)
+add_executable(test-lis3dsh src/tests/drivers/test-lis3dsh.cpp)
 set_property(TARGET test-lis3dsh PROPERTY OPT_BOARD stm32f407vg_stm32f4discovery)
 sbs_target(test-lis3dsh)
 
-#-----------------------------------------------------------------------------#
+add_executable(test-lis3mdl src/tests/drivers/test-lis3mdl.cpp)
+set_property(TARGET test-lis3mdl PROPERTY OPT_BOARD stm32f429zi_skyward_death_stack_x)
+sbs_target(test-lis3mdl)
 
-add_executable(test-bmx160
-    src/tests/drivers/test-bmx160.cpp
-    ${SHARED_SOURCES}
-    ${SPI_SOURCES}
-    ${BMX160_SOURCES}
-)
-target_link_libraries(test-bmx160 PRIVATE fmt::fmt-header-only)
-target_compile_definitions(test-bmx160 PRIVATE DEBUG)
-set_property(TARGET test-bmx160 PROPERTY OPT_BOARD stm32f429zi_skyward_death_stack_x)
-sbs_target(test-bmx160)
+#src/tests/drivers/test-mavlink-multi.cpp
 
-#-----------------------------------------------------------------------------#
+add_executable(test-mavlink src/tests/drivers/test-mavlink.cpp)
+set_property(TARGET test-mavlink PROPERTY OPT_BOARD stm32f429zi_skyward_homeone)
+sbs_target(test-mavlink)
 
-add_executable(test-bmx160-with-correction
-    src/tests/drivers/test-bmx160-with-correction.cpp
-    ${SHARED_SOURCES}
-    ${SPI_SOURCES}
-    ${BMX160_SOURCES}
-    ${BMX160WITHCORRECTION_SOURCES}
-    ${CALIBRATION_SOURCES}
-)
-target_link_libraries(test-bmx160-with-correction PRIVATE Eigen3::Eigen fmt::fmt-header-only)
-target_compile_definitions(test-bmx160-with-correction PRIVATE DEBUG)
-set_property(TARGET test-bmx160-with-correction PROPERTY OPT_BOARD stm32f429zi_skyward_death_stack_x)
-sbs_target(test-bmx160-with-correction)
+add_executable(test-mpu9250 src/tests/drivers/test-mpu9250.cpp)
+set_property(TARGET test-mpu9250 PROPERTY OPT_BOARD stm32f429zi_skyward_death_stack_x)
+sbs_target(test-mpu9250)
 
-#-----------------------------------------------------------------------------#
+add_executable(test-ms5803 src/tests/drivers/test-ms5803.cpp)
+set_property(TARGET test-ms5803 PROPERTY OPT_BOARD stm32f429zi_skyward_death_stack_x)
+sbs_target(test-ms5803)
 
-add_executable(test-ads1118
-    src/tests/drivers/test-ads1118.cpp
-    ${SHARED_SOURCES}
-    ${SPI_SOURCES}
-    ${ADS1118_SOURCES}
-)
-target_link_libraries(test-ads1118 PRIVATE fmt::fmt-header-only)
-target_compile_definitions(test-ads1118 PRIVATE DEBUG)
-set_property(TARGET test-ads1118 PROPERTY OPT_BOARD stm32f407vg_stm32f4discovery)
-sbs_target(test-ads1118)
+#src/tests/drivers/test-piksi.cpp
 
-#-----------------------------------------------------------------------------#
+add_executable(test-pwm src/tests/drivers/test-pwm.cpp)
+set_property(TARGET test-pwm PROPERTY OPT_BOARD stm32f429zi_stm32f4discovery)
+sbs_target(test-pwm)
 
-add_executable(test-xbee-snd
-    src/tests/drivers/xbee/test-xbee-snd.cpp
-    ${SHARED_SOURCES}
-    ${SPI_SOURCES}
-    ${XBEE_SOURCES}
-)
-target_link_libraries(test-xbee-snd PRIVATE fmt::fmt-header-only)
-target_compile_definitions(test-xbee-snd PRIVATE DEBUG)
-set_property(TARGET test-xbee-snd PROPERTY OPT_BOARD stm32f429zi_stm32f4discovery)
-sbs_target(test-xbee-snd)
+add_executable(test-servo src/tests/drivers/test-servo.cpp)
+set_property(TARGET test-servo PROPERTY OPT_BOARD stm32f429zi_skyward_death_stack_x)
+sbs_target(test-servo)
 
-#-----------------------------------------------------------------------------#
+add_executable(test-spi src/tests/drivers/test-spi.cpp)
+set_property(TARGET test-spi PROPERTY OPT_BOARD stm32f429zi_stm32f4discovery)
+sbs_target(test-spi)
 
-add_executable(test-xbee-rcv
-    src/tests/drivers/xbee/test-xbee-rcv.cpp
-    ${SHARED_SOURCES}
-    ${SPI_SOURCES}
-    ${XBEE_SOURCES}
-)
-target_link_libraries(test-xbee-rcv PRIVATE fmt::fmt-header-only)
-target_compile_definitions(test-xbee-rcv PRIVATE DEBUG)
-set_property(TARGET test-xbee-rcv PROPERTY OPT_BOARD stm32f429zi_stm32f4discovery)
-sbs_target(test-xbee-rcv)
+add_executable(test-ublox src/tests/drivers/test-ublox.cpp)
+set_property(TARGET test-ublox PROPERTY OPT_BOARD stm32f429zi_skyward_death_stack_x)
+sbs_target(test-ublox)
 
-#-----------------------------------------------------------------------------#
+#src/tests/drivers/xbee/logdecoder/logdecoder.cpp
 
-add_executable(test-xbee-bidir
-    src/tests/drivers/xbee/test-xbee-bidir.cpp
-    ${SHARED_SOURCES}
-    ${SPI_SOURCES}
-    ${XBEE_SOURCES}
-)
-target_link_libraries(test-xbee-bidir PRIVATE fmt::fmt-header-only)
-target_compile_definitions(test-xbee-bidir PRIVATE DEBUG)
+add_executable(test-xbee-bidir src/tests/drivers/xbee/test-xbee-bidir.cpp)
 set_property(TARGET test-xbee-bidir PROPERTY OPT_BOARD stm32f429zi_skyward_death_stack_x)
 sbs_target(test-xbee-bidir)
 
-#-----------------------------------------------------------------------------#
-
 add_executable(test-xbee-gui
     src/tests/drivers/xbee/test-xbee-gui.cpp
     src/tests/drivers/xbee/gui/res/respect.cpp
-    ${SHARED_SOURCES}
-    ${SPI_SOURCES}
-    ${XBEE_SOURCES}
 )
-target_link_libraries(test-xbee-gui PRIVATE fmt::fmt-header-only)
-target_compile_definitions(test-xbee-gui PRIVATE DEBUG)
 set_property(TARGET test-xbee-gui PROPERTY OPT_BOARD stm32f429zi_stm32f4discovery)
-sbs_link_mxgui(test-xbee-gui)
 sbs_target(test-xbee-gui)
 
-#-----------------------------------------------------------------------------#
-
-add_executable(test-bme280
-    src/tests/drivers/test-bme280.cpp
-    ${SHARED_SOURCES}
-    ${SPI_SOURCES}
-    ${BME280_SOURCES}
-)
-target_link_libraries(test-bme280 PRIVATE fmt::fmt-header-only)
-target_compile_definitions(test-bme280 PRIVATE DEBUG)
-set_property(TARGET test-bme280 PROPERTY OPT_BOARD stm32f407vg_stm32f4discovery)
-sbs_target(test-bme280)
-
-#-----------------------------------------------------------------------------#
-
-add_executable(test-internal-adc
-    src/tests/drivers/test-internal-adc.cpp
-    ${SHARED_SOURCES}
-    ${INTERNAL_ADC_SOURCES}
-)
-target_link_libraries(test-internal-adc PRIVATE fmt::fmt-header-only)
-target_compile_definitions(test-internal-adc PRIVATE DEBUG)
-set_property(TARGET test-internal-adc PROPERTY OPT_BOARD stm32f429zi_skyward_death_stack_x)
-sbs_target(test-internal-adc)
-
-#-----------------------------------------------------------------------------#
-
-add_executable(test-internal-adc-dma
-    src/tests/drivers/test-internal-adc-dma.cpp
-    ${SHARED_SOURCES}
-    ${INTERNAL_ADC_SOURCES}
-)
-target_link_libraries(test-internal-adc-dma PRIVATE fmt::fmt-header-only)
-target_compile_definitions(test-internal-adc-dma PRIVATE DEBUG)
-set_property(TARGET test-internal-adc-dma PROPERTY OPT_BOARD stm32f429zi_stm32f4discovery)
-sbs_target(test-internal-adc-dma)
+add_executable(test-xbee-rcv src/tests/drivers/xbee/test-xbee-rcv.cpp)
+set_property(TARGET test-xbee-rcv PROPERTY OPT_BOARD stm32f429zi_stm32f4discovery)
+sbs_target(test-xbee-rcv)
 
-#-----------------------------------------------------------------------------#
+add_executable(test-xbee-snd src/tests/drivers/xbee/test-xbee-snd.cpp)
+set_property(TARGET test-xbee-snd PROPERTY OPT_BOARD stm32f429zi_stm32f4discovery)
+sbs_target(test-xbee-snd)
 
-add_executable(test-analog-pressure-sensors
-    src/tests/drivers/analog/test-analog-pressure-sensors.cpp
-    ${SHARED_SOURCES}
-    ${SPI_SOURCES}
-    ${ADS1118_SOURCES}
-)
-target_link_libraries(test-analog-pressure-sensors PRIVATE fmt::fmt-header-only)
-target_compile_definitions(test-analog-pressure-sensors PRIVATE DEBUG)
-set_property(TARGET test-analog-pressure-sensors PROPERTY OPT_BOARD stm32f429zi_stm32f4discovery)
-sbs_target(test-analog-pressure-sensors)
+add_executable(test-kalman-eigen-benchmark src/tests/kalman/test-kalman-eigen-benchmark.cpp)
+set_property(TARGET test-kalman-eigen-benchmark PROPERTY OPT_BOARD stm32f429zi_stm32f4discovery)
+sbs_target(test-kalman-eigen-benchmark)
 
-#-----------------------------------------------------------------------------#
+add_executable(test-logger src/tests/logger/test-logger.cpp)
+set_property(TARGET test-logger PROPERTY OPT_BOARD stm32f429zi_stm32f4discovery)
+sbs_target(test-logger)
 
-add_executable(test-battery-voltage
-    src/tests/drivers/analog/test-battery-voltage.cpp
-    ${SHARED_SOURCES}
-    ${INTERNAL_ADC_SOURCES}
-)
-target_link_libraries(test-battery-voltage PRIVATE fmt::fmt-header-only)
-target_compile_definitions(test-battery-voltage PRIVATE DEBUG)
-set_property(TARGET test-battery-voltage PROPERTY OPT_BOARD stm32f429zi_stm32f4discovery)
-sbs_target(test-battery-voltage)
+#src/tests/misc/xbee-bitrate.cpp
+#src/tests/misc/xbee-send-rcv.cpp
+#src/tests/misc/xbee-time-to-send.cpp
+#src/tests/test-eventinjector.cpp
 
-#-----------------------------------------------------------------------------#
+add_executable(test-hsm src/tests/test-hsm.cpp)
+set_property(TARGET test-hsm PROPERTY OPT_BOARD stm32f429zi_stm32f4discovery)
+sbs_target(test-hsm)
 
-add_executable(test-current-sensor
-    src/tests/drivers/analog/test-current-sensor.cpp
-    ${SHARED_SOURCES}
-    ${INTERNAL_ADC_SOURCES}
-)
-target_link_libraries(test-current-sensor PRIVATE fmt::fmt-header-only)
-target_compile_definitions(test-current-sensor PRIVATE DEBUG)
-set_property(TARGET test-current-sensor PROPERTY OPT_BOARD stm32f429zi_stm32f4discovery)
-sbs_target(test-current-sensor)
+add_executable(test-interrupt src/tests/test-interrupt.cpp)
+set_property(TARGET test-interrupt PROPERTY OPT_BOARD stm32f429zi_stm32f4discovery)
+sbs_target(test-interrupt)
 
-#-----------------------------------------------------------------------------#
+add_executable(test-pinobserver src/tests/test-pinobserver.cpp)
+set_property(TARGET test-pinobserver PROPERTY OPT_BOARD stm32f429zi_stm32f4discovery)
+sbs_target(test-pinobserver)
 
-add_executable(test-spi
-    src/tests/drivers/test-spi.cpp
-    ${SHARED_SOURCES}
-    ${SPI_SOURCES}
-)
-target_link_libraries(test-spi PRIVATE fmt::fmt-header-only)
-target_compile_definitions(test-spi PRIVATE DEBUG)
-set_property(TARGET test-spi PROPERTY OPT_BOARD stm32f429zi_stm32f4discovery)
-sbs_target(test-spi)
+add_executable(test-sensormanager src/tests/test-sensormanager.cpp)
+set_property(TARGET test-sensormanager PROPERTY OPT_BOARD stm32f429zi_skyward_death_stack_x)
+sbs_target(test-sensormanager)
 
-#-----------------------------------------------------------------------------#
+add_executable(test-serial src/tests/test-serial.cpp)
+set_property(TARGET test-serial PROPERTY OPT_BOARD stm32f429zi_skyward_death_stack_x)
+sbs_target(test-serial)
 
-add_executable(test-hwtimer-chain
-    src/tests/drivers/test-hwtimer-chain.cpp
-    ${SHARED_SOURCES}
-)
-target_link_libraries(test-hwtimer-chain PRIVATE fmt::fmt-header-only)
-target_compile_definitions(test-hwtimer-chain PRIVATE DEBUG)
-set_property(TARGET test-hwtimer-chain PROPERTY OPT_BOARD stm32f407vg_stm32f4discovery)
-sbs_target(test-hwtimer-chain)
+add_executable(test-taskscheduler src/tests/test-taskscheduler.cpp)
+set_property(TARGET test-taskscheduler PROPERTY OPT_BOARD stm32f429zi_skyward_death_stack_x)
+sbs_target(test-taskscheduler)
 
-#-----------------------------------------------------------------------------#
+add_executable(test-timestamp-timer src/tests/test-timestamp-timer.cpp)
+set_property(TARGET test-timestamp-timer PROPERTY OPT_BOARD stm32f429zi_stm32f4discovery)
+sbs_target(test-timestamp-timer)
 
-add_executable(test-mpu9250
-    src/tests/drivers/test-mpu9250.cpp
-    ${SHARED_SOURCES}
-    ${SPI_SOURCES}
-    ${MPU9250_SOURCES}
-)
-target_link_libraries(test-mpu9250 PRIVATE fmt::fmt-header-only)
-target_compile_definitions(test-mpu9250 PRIVATE DEBUG)
-set_property(TARGET test-mpu9250 PROPERTY OPT_BOARD stm32f429zi_skyward_death_stack_x)
-sbs_target(test-mpu9250)
+add_executable(test-trace-logger src/tests/test-trace-logger.cpp)
+set_property(TARGET test-trace-logger PROPERTY OPT_BOARD stm32f429zi_stm32f4discovery)
+sbs_target(test-trace-logger)
diff --git a/cmake/boardcore.cmake b/cmake/boardcore.cmake
new file mode 100644
index 0000000000000000000000000000000000000000..430bb10275039ea4ae8a28df577959e1a30f25f0
--- /dev/null
+++ b/cmake/boardcore.cmake
@@ -0,0 +1,113 @@
+# Copyright (c) 2021 Skyward Experimental Rocketry
+# Authors: Damiano Amatruda
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in
+# all copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+# THE SOFTWARE.
+
+include(dependencies)
+
+add_custom_target(boardcore)
+
+foreach(OPT_BOARD ${BOARDS})
+    set(BOARDCORE_LIBRARY boardcore-${OPT_BOARD})
+    add_library(${BOARDCORE_LIBRARY} STATIC EXCLUDE_FROM_ALL
+        # Debug
+        ${SBS_BASE}/src/shared/Debug.cpp
+        ${SBS_BASE}/src/shared/diagnostic/CpuMeter.cpp
+        ${SBS_BASE}/src/shared/diagnostic/PrintLogger.cpp
+
+        # Drivers
+        ${SBS_BASE}/src/shared/drivers/adc/ADS1118/ADS1118.cpp
+        ${SBS_BASE}/src/shared/drivers/adc/InternalADC/InternalADC.cpp
+        ${SBS_BASE}/src/shared/drivers/canbus/CanBus.cpp
+        ${SBS_BASE}/src/shared/drivers/canbus/CanInterrupt.cpp
+        ${SBS_BASE}/src/shared/drivers/canbus/CanManager.cpp
+        #${SBS_BASE}/src/shared/drivers/ethernet/PacketBuffer.cpp
+        #${SBS_BASE}/src/shared/drivers/ethernet/UdpManager.cpp
+        #${SBS_BASE}/src/shared/drivers/ethernet/W5200/spi_impl.cpp
+        #${SBS_BASE}/src/shared/drivers/ethernet/W5200/w5200.cpp
+        #${SBS_BASE}/src/shared/drivers/ethernet/WatchdogTimer.cpp
+        ${SBS_BASE}/src/shared/drivers/gamma868/Gamma868.cpp
+        ${SBS_BASE}/src/shared/drivers/gps/ublox/UbloxGPS.cpp
+        ${SBS_BASE}/src/shared/drivers/hbridge/HBridge.cpp
+        ${SBS_BASE}/src/shared/drivers/i2c/stm32f2_f4_i2c.cpp
+        #${SBS_BASE}/src/shared/drivers/ISB_protocol/IsbProtocol_serial2.cpp
+        #${SBS_BASE}/src/shared/drivers/ISB_protocol/IsbProtocol_serial3.cpp
+        ${SBS_BASE}/src/shared/drivers/interrupt/external_interrupts.cpp
+        #${SBS_BASE}/src/shared/drivers/interrupt/InterruptManager.cpp
+        #${SBS_BASE}/src/shared/drivers/Leds.cpp
+        #${SBS_BASE}/src/shared/drivers/memory/MultiFlashController.cpp
+        #${SBS_BASE}/src/shared/drivers/modbus/PDU.cpp
+        #${SBS_BASE}/src/shared/drivers/modbus/slave/RtuSlave.cpp
+        #${SBS_BASE}/src/shared/drivers/modbus/slave/SlaveEngine.cpp
+        #${SBS_BASE}/src/shared/drivers/modbus/slave/old/SlaveInterface.cpp
+        #${SBS_BASE}/src/shared/drivers/modbus/slave/old/Timer.cpp
+        #${SBS_BASE}/src/shared/drivers/old_examples/piksi/piksi.cpp
+        ${SBS_BASE}/src/shared/drivers/pwm/pwm.cpp
+        ${SBS_BASE}/src/shared/drivers/servo/servo.cpp
+        ${SBS_BASE}/src/shared/drivers/spi/SPITransaction.cpp
+        #${SBS_BASE}/src/shared/drivers/spi/SensorSpi.cpp
+        ${SBS_BASE}/src/shared/drivers/Xbee/APIFrameParser.cpp
+        ${SBS_BASE}/src/shared/drivers/Xbee/Xbee.cpp
+
+        # Events
+        ${SBS_BASE}/src/shared/events/EventBroker.cpp
+
+        # Logger
+        ${SBS_BASE}/src/shared/logger/Logger.cpp
+        #${SBS_BASE}/src/shared/logger/decoder/logdecoder.cpp
+
+        # Math
+        ${SBS_BASE}/src/shared/math/Matrix.cpp
+        ${SBS_BASE}/src/shared/math/SkyQuaternion.cpp
+        ${SBS_BASE}/src/shared/math/Stats.cpp
+
+        # Scheduler
+        ${SBS_BASE}/src/shared/scheduler/TaskScheduler.cpp
+
+        # Sensors
+        ${SBS_BASE}/src/shared/sensors/BME280/BME280.cpp
+        ${SBS_BASE}/src/shared/sensors/BMX160/BMX160.cpp
+        ${SBS_BASE}/src/shared/sensors/BMX160/BMX160WithCorrection.cpp
+        ${SBS_BASE}/src/shared/sensors/calibration/SensorDataExtra.cpp
+        ${SBS_BASE}/src/shared/sensors/MPU9250/MPU9250.cpp
+        ${SBS_BASE}/src/shared/sensors/MS5803/MS5803.cpp
+        ${SBS_BASE}/src/shared/sensors/SensorManager.cpp
+        ${SBS_BASE}/src/shared/sensors/SensorSampler.cpp
+
+        # Timer
+        ${SBS_BASE}/src/shared/TimestampTimer.cpp
+
+        # AeroUtils
+        ${SBS_BASE}/src/shared/utils/aero/AeroUtils.cpp
+
+        # TestUtils
+        ${SBS_BASE}/src/shared/utils/testutils/TestHelper.cpp
+    )
+    add_library(SkywardBoardcore::Boardcore-${OPT_BOARD} ALIAS ${BOARDCORE_LIBRARY})
+    target_include_directories(${BOARDCORE_LIBRARY} PUBLIC ${SBS_BASE}/src/shared)
+    target_link_libraries(${BOARDCORE_LIBRARY} PUBLIC
+        Miosix::Miosix-${OPT_BOARD}
+        TSCPP::TSCPP
+        Eigen3::Eigen
+        fmt::fmt-header-only
+        Catch2::Catch2
+        Mavlink::Mavlink
+    )
+    add_dependencies(boardcore boardcore-${OPT_BOARD})
+endforeach()
diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake
index fd4aa8e4a0be4713f54a96e3556211fd671df015..877b2d9c0d5936418218ea02de4cab2b7e979665 100644
--- a/cmake/dependencies.cmake
+++ b/cmake/dependencies.cmake
@@ -19,7 +19,12 @@
 # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 # THE SOFTWARE.
 
+file(GLOB KPATH ${SBS_BASE}/libs/miosix-kernel/miosix)
+if(NOT KPATH)
+    message(FATAL_ERROR "Kernel directory not found")
+endif()
 add_subdirectory(${KPATH} EXCLUDE_FROM_ALL)
+include(${KPATH}/config/boards.cmake)
 
 add_subdirectory(${SBS_BASE}/libs/mxgui EXCLUDE_FROM_ALL)
 
@@ -41,115 +46,3 @@ add_library(Catch2::Catch2 ALIAS Catch2)
 target_include_directories(Catch2 INTERFACE ${SBS_BASE}/libs/Catch2/single_include)
 
 add_subdirectory(${SBS_BASE}/libs/mavlink_skyward_lib EXCLUDE_FROM_ALL)
-
-set(SHARED_SOURCES
-    ${SBS_BASE}/src/shared/scheduler/TaskScheduler.cpp
-    ${SBS_BASE}/src/shared/diagnostic/CpuMeter.cpp
-    ${SBS_BASE}/src/shared/events/EventBroker.cpp
-    ${SBS_BASE}/src/shared/math/Stats.cpp
-    ${SBS_BASE}/src/shared/drivers/interrupt/external_interrupts.cpp
-    ${SBS_BASE}/src/shared/utils/aero/AeroUtils.cpp
-    ${SBS_BASE}/src/shared/Debug.cpp
-    ${SBS_BASE}/src/shared/TimestampTimer.cpp
-    ${SBS_BASE}/src/shared/diagnostic/PrintLogger.cpp
-    ${SBS_BASE}/src/shared/logger/Logger.cpp
-    ${SBS_BASE}/libs/tscpp/buffer.cpp
-    ${SBS_BASE}/libs/tscpp/stream.cpp
-)
-set(UBLOXGPS_SOURCES
-    ${SBS_BASE}/src/shared/drivers/gps/ublox/UbloxGPS.cpp
-)
-set(CANBUS_SOURCES
-    ${SBS_BASE}/src/shared/drivers/canbus/CanManager.cpp
-    ${SBS_BASE}/src/shared/drivers/canbus/CanBus.cpp
-    ${SBS_BASE}/src/shared/drivers/canbus/CanInterrupt.cpp
-)
-set(CALIBRATION_SOURCES
-    ${SBS_BASE}/src/shared/sensors/calibration/SensorDataExtra.cpp
-)
-set(PIKSI_SOURCES
-    ${SBS_BASE}/src/shared/drivers/old_examples/piksi/piksi.cpp
-)
-set(PWM_SOURCES
-    ${SBS_BASE}/src/shared/drivers/pwm/pwm.cpp
-)
-set(SPI_SOURCES
-    ${SBS_BASE}/src/shared/drivers/spi/SPITransaction.cpp
-)
-set(I2C_SOURCES
-    ${SBS_BASE}/src/shared/drivers/i2c/stm32f2_f4_i2c.cpp
-)
-set(ETHERNET_SOURCES
-    ${SBS_BASE}/src/shared/drivers/ethernet/UdpManager.cpp
-    ${SBS_BASE}/src/shared/drivers/ethernet/W5200/w5200.cpp
-    ${SBS_BASE}/src/shared/drivers/ethernet/W5200/spi_impl.cpp
-    ${SBS_BASE}/src/shared/drivers/ethernet/PacketBuffer.cpp
-    ${SBS_BASE}/src/shared/drivers/ethernet/WatchdogTimer.cpp
-)
-set(ANAKIN_DATA_SOURCES
-    ${SBS_BASE}/src/shared/boards/AnakinBoard.cpp
-    ${SBS_BASE}/src/shared/drivers/Leds.cpp
-)
-set(SENSORMANAGER_SOURCES
-    ${SBS_BASE}/src/shared/sensors/SensorManager.cpp
-    ${SBS_BASE}/src/shared/sensors/SensorSampler.cpp
-)
-set(MATH_SOURCES
-    ${SBS_BASE}/src/shared/math/SkyQuaternion.cpp
-    ${SBS_BASE}/src/shared/math/Matrix.cpp
-)
-set(GAMMA868_SOURCES
-    ${SBS_BASE}/src/shared/drivers/gamma868/Gamma868.cpp
-)
-set(XBEE_SOURCES
-    ${SBS_BASE}/src/shared/drivers/Xbee/APIFrameParser.cpp
-    ${SBS_BASE}/src/shared/drivers/Xbee/Xbee.cpp
-)
-set(EVENTS_SOURCES
-    ${SBS_BASE}/src/shared/events/EventBroker.cpp
-)
-set(SERVO_SOURCES
-    ${SBS_BASE}/src/shared/drivers/servo/servo.cpp
-)
-set(HBRIDGE_SOURCES
-    ${SBS_BASE}/src/shared/drivers/hbridge/HBridge.cpp
-)
-set(TEST_UTILS_SOURCES
-    ${SBS_BASE}/src/shared/utils/testutils/TestHelper.cpp
-)
-set(TESTS_BOARDCORE_SOURCES
-    ${SBS_BASE}/src/tests/catch/test-aero.cpp
-    ${SBS_BASE}/src/tests/catch/test-buttonhandler.cpp
-    ${SBS_BASE}/src/tests/catch/test-circularbuffer.cpp
-    ${SBS_BASE}/src/tests/catch/test-eventbroker.cpp
-    ${SBS_BASE}/src/tests/catch/test-sensormanager-catch.cpp
-    ${SBS_BASE}/src/tests/catch/test-hardwaretimer.cpp
-    #${SBS_BASE}/src/tests/catch/test-kalman.cpp
-    #${SBS_BASE}/src/tests/catch/test-kalman-eigen.cpp
-    #${SBS_BASE}/src/tests/catch/test-matrix.cpp
-    ${SBS_BASE}/src/tests/catch/test-packetqueue.cpp
-    ${SBS_BASE}/src/tests/catch/spidriver/test-spidriver.cpp
-    ${SBS_BASE}/src/tests/catch/xbee/test-xbee-parser.cpp
-    ${SBS_BASE}/src/tests/catch/xbee/test-xbee-driver.cpp
-)
-set(INTERNAL_ADC_SOURCES
-    ${SBS_BASE}/src/shared/drivers/adc/InternalADC/InternalADC.cpp
-)
-set(ADS1118_SOURCES
-    ${SBS_BASE}/src/shared/drivers/adc/ADS1118/ADS1118.cpp
-)
-set(MPU9250_SOURCES
-    ${SBS_BASE}/src/shared/sensors/MPU9250/MPU9250.cpp
-)
-set(BME280_SOURCES
-    ${SBS_BASE}/src/shared/sensors/BME280/BME280.cpp
-)
-set(BMX160_SOURCES
-    ${SBS_BASE}/src/shared/sensors/BMX160/BMX160.cpp
-)
-set(BMX160WITHCORRECTION_SOURCES
-    ${SBS_BASE}/src/shared/sensors/BMX160/BMX160WithCorrection.cpp
-)
-set(MS5803_SOURCES
-    ${SBS_BASE}/src/shared/sensors/MS5803/MS5803.cpp
-)
diff --git a/cmake/sbs.cmake b/cmake/sbs.cmake
index 813aa1361a2011a45b275320c0bf5da0775d4e1a..3a3c5b1a47e44c62b7b21b2ed6c2fd118d575fa5 100644
--- a/cmake/sbs.cmake
+++ b/cmake/sbs.cmake
@@ -23,18 +23,15 @@ enable_language(C CXX ASM)
 
 list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR})
 get_filename_component(SBS_BASE ${CMAKE_CURRENT_LIST_DIR} DIRECTORY)
-file(GLOB KPATH ${SBS_BASE}/libs/miosix-kernel/miosix)
-if(NOT KPATH)
-    message(FATAL_ERROR "Kernel directory not found")
-endif()
-include(dependencies)
 
-if(NOT CMAKE_SOURCE_DIR STREQUAL SBS_BASE)
+if(NOT CMAKE_CURRENT_SOURCE_DIR STREQUAL SBS_BASE)
+    add_subdirectory(${SBS_BASE} EXCLUDE_FROM_ALL)
     list(APPEND CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/cmake)
     include(${CMAKE_SOURCE_DIR}/cmake/dependencies.cmake OPTIONAL)
+    return()
 endif()
 
-include(${KPATH}/config/boards.cmake)
+include(boardcore)
 
 string(REPLACE ";" "\\n" BOARDS_STR "${BOARDS}")
 add_custom_target(
@@ -44,26 +41,13 @@ add_custom_target(
     VERBATIM
 )
 
-function(sbs_get_board TARGET)
+function(sbs_target TARGET)
     get_target_property(OPT_BOARD ${TARGET} OPT_BOARD)
     if(NOT OPT_BOARD)
         message(FATAL_ERROR "No board selected")
     endif()
-    set(OPT_BOARD ${OPT_BOARD} PARENT_SCOPE)
-endfunction()
-
-function(sbs_link_mxgui TARGET)
-    sbs_get_board(${TARGET})
-    target_link_libraries(${TARGET} PRIVATE Mxgui::Mxgui-${OPT_BOARD})
-endfunction()
-
-function(sbs_target TARGET)
-    sbs_get_board(${TARGET})
-    target_include_directories(${TARGET} PRIVATE
-        ${SBS_BASE}/src/shared
-        src/shared
-    )
-    target_link_libraries(${TARGET} PRIVATE miosix::miosix-${OPT_BOARD})
+    target_include_directories(${TARGET} PRIVATE src/shared)
+    target_link_libraries(${TARGET} PRIVATE SkywardBoardcore::Boardcore-${OPT_BOARD})
     add_custom_command(
         TARGET ${TARGET} POST_BUILD
         COMMAND ${CMAKE_OBJCOPY} -O ihex ${TARGET} ${TARGET}.hex