diff --git a/cmake/boardcore.cmake b/cmake/boardcore.cmake
index 2dfdeaaa41869b533c0fdf885b0e35db3ca317b7..ec3d4bae26cb2926a2375491b63b80c1c4e8cd4b 100644
--- a/cmake/boardcore.cmake
+++ b/cmake/boardcore.cmake
@@ -45,8 +45,8 @@ set(BOARDCORE_SRC
     ${BOARDCORE_PATH}/src/shared/actuators/stepper/StepperPWM.cpp
 
     # Algorithms
-    ${BOARDCORE_PATH}/src/shared/algorithms/ADA/ADA.cpp #
-    ${BOARDCORE_PATH}/src/shared/algorithms/MEA/MEA.cpp #
+    # ${BOARDCORE_PATH}/src/shared/algorithms/ADA/ADA.cpp
+    # ${BOARDCORE_PATH}/src/shared/algorithms/MEA/MEA.cpp
     ${BOARDCORE_PATH}/src/shared/algorithms/AirBrakes/AirBrakesPI.cpp
     ${BOARDCORE_PATH}/src/shared/algorithms/AirBrakes/AirBrakesInterp.cpp
     ${BOARDCORE_PATH}/src/shared/algorithms/NAS/NAS.cpp
@@ -155,8 +155,8 @@ set(BOARDCORE_SRC
 
 set(PROTECT_SRC
     ${BOARDCORE_PATH}/src/shared/aspis/aspis.cpp
-    # ${BOARDCORE_PATH}/src/shared/algorithms/MEA/MEA.cpp
-    # ${BOARDCORE_PATH}/src/shared/algorithms/ADA/ADA.cpp
+    ${BOARDCORE_PATH}/src/shared/algorithms/MEA/MEA.cpp
+    ${BOARDCORE_PATH}/src/shared/algorithms/ADA/ADA.cpp
 )
 
 function(link_boardcore_submodules TARGET TYPE)
@@ -191,6 +191,9 @@ function(link_boardcore_libraries TARGET TYPE)
     endif()
 endfunction()
 
+
+set(ASPIS_PASSES_PATH /home/streben/Documenti/ASPIS/build/passes)
+
 # Given two libs, it creates a static library
 function(apply_aspis_passes TO_PROTECT_LIB EXTERNAL_LIB OUTLIBRARY CXXFLAGS_RET)
     # Getting the sources of the two libraries
@@ -208,28 +211,20 @@ function(apply_aspis_passes TO_PROTECT_LIB EXTERNAL_LIB OUTLIBRARY CXXFLAGS_RET)
     list(TRANSFORM EXTERNAL_SRC APPEND ".o" OUTPUT_VARIABLE EXTERNAL_COMPILED_SOURCES)
     list(TRANSFORM EXTERNAL_COMPILED_SOURCES REPLACE ${CMAKE_SOURCE_DIR} "")
     list(TRANSFORM EXTERNAL_COMPILED_SOURCES PREPEND ${CMAKE_CURRENT_BINARY_DIR}/CMakeFiles/${EXTERNAL_LIB}.dir)
-    
-    set(ASPIS_PASSES_PATH /home/streben/Documenti/POLIMI/TESI/ASPIS/build/passes)
 
     add_custom_target(
         ${OUTLIBRARY}-target
         BYPRODUCTS lib${OUTLIBRARY}.a
-        COMMAND echo "${PROTECT_COMPILED_SOURCES}"
-        COMMAND ${MIOSIX_PREFIX}-link -v ${PROTECT_COMPILED_SOURCES} -o duplication.bc 
-        COMMAND ${CMAKE_OPT} -load-pass-plugin=${ASPIS_PASSES_PATH}/libEDDI.so --passes="func-ret-to-ref" duplication.bc -o funcToRef.bc
-        # COMMAND ${CMAKE_OPT} --bugpoint-enable-legacy-pm -load ${ASPIS_PASSES_PATH}/libEDDI.so -func_ret_to_ref duplication.bc -o funcToRef.bc
-        # COMMAND ${CMAKE_OPT} --bugpoint-enable-legacy-pm -load  -eddi_verify funcToRef.bc -o dataProtection.bc
-        COMMAND ${CMAKE_OPT} -load-pass-plugin=${ASPIS_PASSES_PATH}/libSEDDI.so --passes="eddi-verify" funcToRef.bc -o dataProtection.bc
-        COMMAND ${CMAKE_OPT} --passes="dce,simplifycfg" dataProtection.bc -o simplifycfg.bc
-        COMMAND ${CMAKE_OPT} -load-pass-plugin=${ASPIS_PASSES_PATH}/libRASM.so --passes="rasm-verify" simplifycfg.bc -o protected.bc
-        COMMAND echo "${EXTERNAL_COMPILED_SOURCES}"
-        COMMAND ${MIOSIX_PREFIX}-link ${EXTERNAL_COMPILED_SOURCES} protected.bc -o linked.bc
-        COMMAND ${CMAKE_OPT} -load-pass-plugin=${ASPIS_PASSES_PATH}/libEDDI.so --passes="duplicate-globals" linked.bc -o out.bc
-        COMMAND echo "cxxflags_ret : ${CXXFLAGS_RET} ${${CXXFLAGS_RET}}"
-        COMMAND echo "CXXFLAGS_BASE: ${CXXFLAGS_BASE}"
-        # COMMAND clang++ -c --target=arm-none-eabi ${${CXXFLAGS_RET}} -o compiled.o out.bc 
-        COMMAND clang++ -c --target=arm-none-eabi ${CXXFLAGS_RET} -o compiled.o out.bc 
-        COMMAND ${CMAKE_AR} ru lib${OUTLIBRARY}.a compiled.o
+        COMMAND export LLVM_SYMBOLIZER_PATH=${LLVM_PREFIX}/llvm-symbolizer
+        COMMAND ${MIOSIX_PREFIX}-link ${EXTERNAL_COMPILED_SOURCES} -o externals.bc
+        COMMAND ${CMAKE_OPT} -load-pass-plugin=${ASPIS_PASSES_PATH}/libMarkToExclude.so --passes="mark-to-exclude" externals.bc -o externals.bc
+        COMMAND ${MIOSIX_PREFIX}-link -v ${PROTECT_COMPILED_SOURCES} externals.bc -o out.bc
+        # COMMAND ${CMAKE_OPT} -load-pass-plugin=${ASPIS_PASSES_PATH}/libEDDI.so --passes="func-ret-to-ref" out.bc -o ret_to_ref.bc
+        # COMMAND ${CMAKE_OPT} -load-pass-plugin=${ASPIS_PASSES_PATH}/libSEDDI.so --passes="eddi-verify" ret_to_ref.bc -o out.bc -debug
+        # COMMAND ${CMAKE_OPT} --passes="dce,simplifycfg" out.bc -o out.bc
+        # COMMAND ${CMAKE_OPT} -load-pass-plugin=${ASPIS_PASSES_PATH}/libRASM.so --passes="rasm-verify" out.bc -o out.bc
+        # COMMAND clang++ -c -v --target=arm-none-eabi ${CXXFLAGS_RET} -o compiled.o out.bc 
+        COMMAND ${CMAKE_AR} ru lib${OUTLIBRARY}.a out.bc
         COMMAND_EXPAND_LISTS
         COMMENT "Applying ASPIS passes and creating archive lib${OUTLIBRARY}.a"
     )
@@ -243,6 +238,53 @@ function(apply_aspis_passes TO_PROTECT_LIB EXTERNAL_LIB OUTLIBRARY CXXFLAGS_RET)
 endfunction()
 
 
+# Given two libs, it creates an executable
+function(apply_aspis_passes_exe TO_PROTECT_EXE EXTERNAL_EXE OUTEXECUTABLE CXXFLAGS_RET)
+    # Getting the sources of the two libraries
+    get_property(TO_PROTECT_SRC TARGET ${TO_PROTECT_EXE} PROPERTY SOURCES)
+    get_property(EXTERNAL_SRC TARGET ${EXTERNAL_EXE} PROPERTY SOURCES)
+
+    # Create list of compiled protected sources for each board
+    set(PROTECT_COMPILED_SOURCES TO_PROTECT_COMPILED)
+    list(TRANSFORM TO_PROTECT_SRC APPEND ".o" OUTPUT_VARIABLE PROTECT_COMPILED_SOURCES)
+    list(TRANSFORM PROTECT_COMPILED_SOURCES REPLACE ${CMAKE_SOURCE_DIR} "")
+    list(TRANSFORM PROTECT_COMPILED_SOURCES PREPEND ${CMAKE_CURRENT_BINARY_DIR}/CMakeFiles/${TO_PROTECT_EXE}.dir/)
+    
+    # Create list of compiled boardcore sources for each board
+    set(EXTERNAL_COMPILED_SOURCES EXTERNAL_COMPILED)
+    list(TRANSFORM EXTERNAL_SRC APPEND ".o" OUTPUT_VARIABLE EXTERNAL_COMPILED_SOURCES)
+    list(TRANSFORM EXTERNAL_COMPILED_SOURCES REPLACE ${CMAKE_SOURCE_DIR} "")
+    list(TRANSFORM EXTERNAL_COMPILED_SOURCES PREPEND ${CMAKE_CURRENT_BINARY_DIR}/CMakeFiles/${EXTERNAL_EXE}.dir/)
+    
+    add_custom_target(
+        ${OUTEXECUTABLE}-target
+        BYPRODUCTS ${OUTEXECUTABLE}.elf
+        COMMAND export LLVM_SYMBOLIZER_PATH=${LLVM_PREFIX}/llvm-symbolizer
+        COMMAND ${MIOSIX_PREFIX}-link ${EXTERNAL_COMPILED_SOURCES} -o externals.bc
+        COMMAND ${CMAKE_OPT} -load-pass-plugin=${ASPIS_PASSES_PATH}/libMarkToExclude.so --passes="mark-to-exclude" externals.bc -o externals.bc
+        COMMAND ${MIOSIX_PREFIX}-link -v ${CMAKE_BINARY_DIR}/skyward_boardcore.bc ${PROTECT_COMPILED_SOURCES} externals.bc -o out.bc
+        COMMAND ${CMAKE_OPT} -load-pass-plugin=${ASPIS_PASSES_PATH}/libEDDI.so --passes="func-ret-to-ref" out.bc -o ret_to_ref.bc
+        COMMAND ${CMAKE_OPT} -load-pass-plugin=${ASPIS_PASSES_PATH}/libSEDDI.so --passes="eddi-verify" ret_to_ref.bc -o out.bc -debug
+        COMMAND ${CMAKE_OPT} --passes="dce,simplifycfg" out.bc -o out.bc
+        # COMMAND ${CMAKE_OPT} -load-pass-plugin=${ASPIS_PASSES_PATH}/libRASM.so --passes="rasm-verify" out.bc -o out.bc
+        # COMMAND clang++ -v --target=arm-none-eabi ${CXXFLAGS_RET} -o ${OUTEXECUTABLE}.elf out.bc
+        COMMAND clang++ --target=arm-none-eabi --sysroot=/opt/arm-miosix-eabi/arm-miosix-eabi/lib -DDEBUG -DEIGEN_MAX_ALIGN_BYTES=0 -DFMT_HEADER_ONLY=1 -DFMT_STATIC_THOUSANDS_SEPARATOR=0 -DFMT_UNICODE=0 -D_GLIBCXX_USE_WCHAR_T -D_MIOSIX -D_MIOSIX_GCC_PATCH_MAJOR=3 -D_MIOSIX_GCC_PATCH_MINOR=1 -D__GXX_TYPEINFO_EQUALITY_INLINE=0 -I/home/streben/Documenti/POLIMI/TESI/obsw/src -I/home/streben/Documenti/POLIMI/TESI/obsw/src/boards -I/home/streben/Documenti/POLIMI/TESI/obsw/skyward-boardcore/src/shared -I/home/streben/Documenti/POLIMI/TESI/obsw/skyward-boardcore/libs/miosix-kernel/miosix -I/home/streben/Documenti/POLIMI/TESI/obsw/skyward-boardcore/libs/miosix-kernel/miosix/arch/common -I/home/streben/Documenti/POLIMI/TESI/obsw/skyward-boardcore/libs/miosix-kernel/miosix/arch/cortexM7_stm32f7/common -I/home/streben/Documenti/POLIMI/TESI/obsw/skyward-boardcore/src/bsps/stm32f767zi_gemini_motor -I/home/streben/Documenti/POLIMI/TESI/obsw/skyward-boardcore/src/bsps/stm32f767zi_gemini_motor/config -I/home/streben/Documenti/POLIMI/TESI/obsw/skyward-boardcore/libs/miosix-kernel/miosix/default -I/opt/arm-miosix-eabi/arm-miosix-eabi/include/c++/9.2.0/arm-miosix-eabi -I/opt/arm-miosix-eabi/arm-miosix-eabi/include/c++/9.2.0 -I/opt/arm-miosix-eabi/arm-miosix-eabi/include -I/home/streben/Documenti/POLIMI/TESI/obsw/skyward-boardcore/libs/tscpp -I/home/streben/Documenti/POLIMI/TESI/obsw/skyward-boardcore/libs/eigen -I/home/streben/Documenti/POLIMI/TESI/obsw/skyward-boardcore/libs/fmt/include -I/home/streben/Documenti/POLIMI/TESI/obsw/skyward-boardcore/libs/Catch2/single_include -I/home/streben/Documenti/POLIMI/TESI/obsw/skyward-boardcore/libs/mavlink-skyward-lib -g -O0 -std=c++11 -D_MIOSIX_BOARDNAME=\"stm32f767zi_gemini_motor\" -D_DEFAULT_SOURCE=1 -std=c++14 -ffunction-sections -Wall -Werror=return-type -g -D_BOARD_STM32F767ZI_GEMINI_MOTOR -D_ARCH_CORTEXM7_STM32F7 -DHSE_VALUE=25000000 -DSYSCLK_FREQ_216MHz=216000000 -D__ENABLE_XRAM -mcpu=cortex-m7 -mthumb -mfloat-abi=hard -mfpu=fpv5-d16 -Wl,--gc-sections -Wl,-T/home/streben/Documenti/POLIMI/TESI/obsw/skyward-boardcore/src/bsps/stm32f767zi_gemini_motor/stm32_2m+16m_xram.ld -L/opt/arm-miosix-eabi/arm-miosix-eabi/lib/thumb/cm7/hardfp/fpv5 -L/opt/arm-miosix-eabi/lib/gcc/arm-miosix-eabi/9.2.0/thumb/cm7/hardfp/fpv5  -Wl,--start-group libmiosix.a -lstdc++ -lc -lm -lgcc -latomic -Wl,--end-group -nostdlib -fno-math-errno -MD -MT out.o -MF out.o.d  -o ${OUTEXECUTABLE}.elf out.bc /home/streben/Documenti/POLIMI/TESI/obsw/skyward-boardcore/libs/tscpp/tscpp/buffer.cpp /home/streben/Documenti/POLIMI/TESI/obsw/skyward-boardcore/libs/tscpp/tscpp/stream.cpp
+        COMMAND_EXPAND_LISTS
+        COMMENT "Applying ASPIS passes and creating ${OUTEXECUTABLE}.elf"
+    )
+    add_dependencies(${OUTEXECUTABLE}-target ${TO_PROTECT_EXE})
+    add_dependencies(${OUTEXECUTABLE}-target ${EXTERNAL_EXE})
+
+# /usr/lib64/ccache/clang++ --target=arm-none-eabi --sysroot=/opt/arm-miosix-eabi/arm-miosix-eabi/lib -g -O0 -Wl,-Map,/home/streben/Documenti/POLIMI/TESI/obsw/src/entrypoints/Main/build/main-entry.map -Wl,-Map,/home/streben/Documenti/POLIMI/TESI/obsw/src/entrypoints/Main/build/skyward-boardcore.map -mcpu=cortex-m7 -mthumb -mfloat-abi=hard -mfpu=fpv5-d16 -Wl,--gc-sections -Wl,-T/home/streben/Documenti/POLIMI/TESI/obsw/skyward-boardcore/src/bsps/stm32f767zi_gemini_motor/stm32_2m+16m_xram.ld -nostdlib -o main-entry.elf   -L/opt/arm-miosix-eabi/arm-miosix-eabi/lib/thumb/cm7/hardfp/fpv5  -L/opt/arm-miosix-eabi/lib/gcc/arm-miosix-eabi/9.2.0/thumb/cm7/hardfp/fpv5  libskyward-boardcore.a -Wl,--start-group libmiosix.a -lstdc++ -lc -lm -lgcc -latomic -Wl,--end-group
+
+
+    # Importing the just built library
+    add_executable(${OUTEXECUTABLE} IMPORTED)
+    add_dependencies(${OUTEXECUTABLE} ${OUTEXECUTABLE}-target)
+    set_target_properties(${OUTEXECUTABLE} PROPERTIES IMPORTED_LOCATION ${CMAKE_CURRENT_BINARY_DIR}/${OUTEXECUTABLE}.elf)
+endfunction()
+
+# COMMAND clang++ -v --target=arm-none-eabi --sysroot=/opt/arm-miosix-eabi/arm-miosix-eabi/lib -mcpu=cortex-m7 -mthumb -mfloat-abi=hard -mfpu=fpv5-d16 -Wl,--gc-sections -Wl,-T/home/streben/Documenti/POLIMI/TESI/obsw/skyward-boardcore/src/bsps/stm32f767zi_gemini_motor/stm32_2m+16m_xram.ld -nostdlib  -o ${OUTEXECUTABLE}.elf out.bc -L/opt/arm-miosix-eabi/arm-miosix-eabi/lib/thumb/cm7/hardfp/fpv5  -L/opt/arm-miosix-eabi/lib/gcc/arm-miosix-eabi/9.2.0/thumb/cm7/hardfp/fpv5
 
 # Function to link the Miosix libraries to a target and register the build command
 #
@@ -253,33 +295,33 @@ endfunction()
 # - Tells the linker to generate the map file
 # - Registers custom targets to create the hex and bin files (${TARGET}_bin and ${TARGET}_hex)
 # - Registers a custom target to flash the program to the board (${TARGET}_program)
-function(boardcore_link_target TARGET)
+function(boardcore_link_target TARGET TYPE)
 
     # Linker script and linking options are eredited from miosix libraries
 
     # Linking skyward-boardcore library
-    target_link_libraries(${TARGET} PUBLIC skyward-boardcore)
+    target_link_libraries(${TARGET} ${TYPE} skyward-boardcore)
 
     # Tell the linker to produce the map file
-    target_link_options(${TARGET} PRIVATE -Wl,-Map,$<TARGET_FILE_DIR:${TARGET}>/$<TARGET_FILE_BASE_NAME:${TARGET}>.map)
-
-    # Add a post build command to create the hex file to flash on the board
-    add_custom_command(
-        OUTPUT ${TARGET}.hex
-        COMMAND ${CMAKE_OBJCOPY} -O ihex $<TARGET_FILE:${TARGET}> ${TARGET}.hex
-        COMMENT "Creating ${TARGET}.hex"
-        VERBATIM
-    )
-    add_custom_target(${TARGET}_hex ALL DEPENDS ${TARGET}.hex)
-    add_custom_command(
-        OUTPUT ${TARGET}.bin
-        COMMAND ${CMAKE_OBJCOPY} -O binary $<TARGET_FILE:${TARGET}> ${TARGET}.bin
-        COMMENT "Creating ${TARGET}.bin"
-        VERBATIM
-    )
-    add_custom_target(${TARGET}_bin ALL DEPENDS ${TARGET}.bin)
-
-    # Generate custom build command to flash the target
-    miosix_add_program_target(${TARGET})
+    target_link_options(${TARGET} ${TYPE} -Wl,-Map,$<TARGET_FILE_DIR:${TARGET}>/$<TARGET_FILE_BASE_NAME:${TARGET}>.map)  
+    
+    # # Add a post build command to create the hex file to flash on the board
+    # add_custom_command(
+    #     OUTPUT ${TARGET}.hex
+    #     COMMAND ${CMAKE_OBJCOPY} -O ihex $<TARGET_FILE:${TARGET}> ${TARGET}.hex
+    #     COMMENT "Creating ${TARGET}.hex"
+    #     VERBATIM
+    # )
+    # add_custom_target(${TARGET}_hex ALL DEPENDS ${TARGET}.hex)
+    # add_custom_command(
+    #     OUTPUT ${TARGET}.bin
+    #     COMMAND ${CMAKE_OBJCOPY} -O binary $<TARGET_FILE:${TARGET}> ${TARGET}.bin
+    #     COMMENT "Creating ${TARGET}.bin"
+    #     VERBATIM
+    # )
+    # add_custom_target(${TARGET}_bin ALL DEPENDS ${TARGET}.bin)
+
+    # # Generate custom build command to flash the target
+    # miosix_add_program_target(${TARGET})
 endfunction()
 
diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake
index 317289e2c84855e76a43d3fbbdc1369f6048e96f..421f3dd3739ad1ba7c4f00af060fcae71c17d446 100644
--- a/cmake/dependencies.cmake
+++ b/cmake/dependencies.cmake
@@ -26,7 +26,7 @@ include(${BOARDCORE_PATH}/libs/miosix-kernel/miosix/CMakeLists.txt)
 add_subdirectory(${BOARDCORE_PATH}/libs/miosix-host miosix-host EXCLUDE_FROM_ALL)
 
 # MxGui graphical library
-include(${BOARDCORE_PATH}/libs/mxgui/cmake/mxgui.cmake)
+# include(${BOARDCORE_PATH}/libs/mxgui/cmake/mxgui.cmake)
 
 # Serialization library
 add_subdirectory(${BOARDCORE_PATH}/libs/tscpp tscpp EXCLUDE_FROM_ALL)
diff --git a/src/bsps/stm32f767zi_gemini_motor/interfaces-impl/bsp.cpp b/src/bsps/stm32f767zi_gemini_motor/interfaces-impl/bsp.cpp
index 3c0257fc683b61c4d0520da468fcd1887f7c0e1a..ccb478f86aea734737407aa9fffdfcac4d1de942 100644
--- a/src/bsps/stm32f767zi_gemini_motor/interfaces-impl/bsp.cpp
+++ b/src/bsps/stm32f767zi_gemini_motor/interfaces-impl/bsp.cpp
@@ -246,43 +246,43 @@ void IRQbspInit()
     // Enable USART1 pins port
     RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN;
 
-    interfaces::spi1::sck::mode(Mode::ALTERNATE);
     interfaces::spi1::sck::alternateFunction(5);
-    interfaces::spi1::miso::mode(Mode::ALTERNATE);
+    interfaces::spi1::sck::mode(Mode::ALTERNATE);
     interfaces::spi1::miso::alternateFunction(5);
-    interfaces::spi1::mosi::mode(Mode::ALTERNATE);
+    interfaces::spi1::miso::mode(Mode::ALTERNATE);
     interfaces::spi1::mosi::alternateFunction(5);
+    interfaces::spi1::mosi::mode(Mode::ALTERNATE);
 
-    interfaces::spi3::sck::mode(Mode::ALTERNATE);
     interfaces::spi3::sck::alternateFunction(6);
-    interfaces::spi3::miso::mode(Mode::ALTERNATE);
+    interfaces::spi3::sck::mode(Mode::ALTERNATE);
     interfaces::spi3::miso::alternateFunction(6);
-    interfaces::spi3::mosi::mode(Mode::ALTERNATE);
+    interfaces::spi3::miso::mode(Mode::ALTERNATE);
     interfaces::spi3::mosi::alternateFunction(5);
+    interfaces::spi3::mosi::mode(Mode::ALTERNATE);
 
-    interfaces::spi4::sck::mode(Mode::ALTERNATE);
     interfaces::spi4::sck::alternateFunction(5);
-    interfaces::spi4::miso::mode(Mode::ALTERNATE);
+    interfaces::spi4::sck::mode(Mode::ALTERNATE);
     interfaces::spi4::miso::alternateFunction(5);
-    interfaces::spi4::mosi::mode(Mode::ALTERNATE);
+    interfaces::spi4::miso::mode(Mode::ALTERNATE);
     interfaces::spi4::mosi::alternateFunction(5);
+    interfaces::spi4::mosi::mode(Mode::ALTERNATE);
 
-    interfaces::i2c1::sda::mode(Mode::ALTERNATE);
     interfaces::i2c1::sda::alternateFunction(4);
-    interfaces::i2c1::scl::mode(Mode::ALTERNATE);
+    interfaces::i2c1::sda::mode(Mode::ALTERNATE);
     interfaces::i2c1::scl::alternateFunction(4);
+    interfaces::i2c1::scl::mode(Mode::ALTERNATE);
 
     // USART1 configured by the miosix serial driver
 
-    interfaces::usart2::tx::mode(Mode::ALTERNATE);
     interfaces::usart2::tx::alternateFunction(7);
-    interfaces::usart2::rx::mode(Mode::ALTERNATE);
+    interfaces::usart2::tx::mode(Mode::ALTERNATE);
     interfaces::usart2::rx::alternateFunction(7);
+    interfaces::usart2::rx::mode(Mode::ALTERNATE);
 
-    interfaces::can2::tx::mode(Mode::ALTERNATE);
     interfaces::can2::tx::alternateFunction(9);
-    interfaces::can2::rx::mode(Mode::ALTERNATE);
+    interfaces::can2::tx::mode(Mode::ALTERNATE);
     interfaces::can2::rx::alternateFunction(9);
+    interfaces::can2::rx::mode(Mode::ALTERNATE);
 
     peripherals::leds::userLed1::mode(Mode::OUTPUT);
     peripherals::leds::userLed2::mode(Mode::OUTPUT);
@@ -314,10 +314,10 @@ void IRQbspInit()
     // sensors::max31856::cs::mode(Mode::OUTPUT);
     // sensors::max31856::cs::high();
 
-    peripherals::servos::servo1::mode(Mode::ALTERNATE);
     peripherals::servos::servo1::alternateFunction(3);
-    peripherals::servos::servo2::mode(Mode::ALTERNATE);
+    peripherals::servos::servo1::mode(Mode::ALTERNATE);
     peripherals::servos::servo2::alternateFunction(3);
+    peripherals::servos::servo2::mode(Mode::ALTERNATE);
 
     sensors::tank_level::lvl1::mode(Mode::INPUT);
     sensors::tank_level::lvl2::mode(Mode::INPUT);
diff --git a/src/shared/aspis/aspis.cpp b/src/shared/aspis/aspis.cpp
index 9e61a7e49076a888dd199512ec9884e767bc6e42..aa47eed8f2646522da3b8749177b5ed8d4ca1848 100644
--- a/src/shared/aspis/aspis.cpp
+++ b/src/shared/aspis/aspis.cpp
@@ -1,12 +1,15 @@
 #include "aspis.h"
 
+#include <stdio.h>
+
 void __attribute__((weak)) DataCorruption_Handler()
 {
+    printf("\t****************** DataCorruption_Handler\n");
     while(1);
 }
 
-
 void __attribute__((weak)) SigMismatch_Handler()
 {
+    printf("\t****************** SigMismatch_Handler\n");
     while(1);
 }
\ No newline at end of file