From e8ea7257eae96e7b41755d617f8b33b3200aaa58 Mon Sep 17 00:00:00 2001
From: Alberto Nidasio <alberto.nidasio@skywarder.eu>
Date: Wed, 1 Nov 2023 18:52:14 +0100
Subject: [PATCH] [Miosix][sbs] Upgrade to Miosix 2.7 and move BSPs to
 Boardcore

This commit upgrades Miosix to version 2.7, and moves BSPs definitions
outside of the Miosix tree and into the Boardcore repository. Link
groups have also been fixed. Miosix Host and MxGui dependencies have
also been updated in order to support BSPs outside the kernel tree.
---
 .gitignore                                    |   1 -
 .vscode/c_cpp_properties.json                 | 301 +++++------
 CMakeLists.txt                                | 106 ++--
 cmake/boardcore-host.cmake                    |  16 +-
 cmake/boardcore.cmake                         | 273 +++++-----
 cmake/boards.cmake                            |  37 ++
 cmake/dependencies.cmake                      |  19 +-
 cmake/sbs.cmake                               |  29 +-
 libs/miosix-host                              |   2 +-
 libs/miosix-kernel                            |   2 +-
 libs/mxgui                                    |   2 +-
 sbs                                           |   2 +-
 .../config/board_options.cmake                |  99 ++++
 .../stm32f205rc_ciuti/config/board_settings.h |  68 +++
 .../config/miosix_settings.h                  | 240 +++++++++
 .../stm32f205rc_ciuti/core/stage_1_boot.cpp   | 399 ++++++++++++++
 .../interfaces-impl/arch_registers_impl.h     |  33 ++
 .../stm32f205rc_ciuti/interfaces-impl/bsp.cpp | 173 ++++++
 .../interfaces-impl/bsp_impl.h                |  72 +++
 .../interfaces-impl/delays.cpp                |  76 +++
 .../interfaces-impl/hwmapping.h               | 125 +++++
 .../stm32f205rc_ciuti/stm32_512k+128k_rom.ld  | 170 ++++++
 .../config/board_options.cmake                | 122 +++++
 .../config/board_settings.h                   |  74 +++
 .../config/miosix_settings.h                  | 240 +++++++++
 .../core/stage_1_boot.cpp                     | 372 +++++++++++++
 .../interfaces-impl/arch_registers_impl.h     |  34 ++
 .../interfaces-impl/bsp.cpp                   | 483 +++++++++++++++++
 .../interfaces-impl/bsp_impl.h                |  66 +++
 .../interfaces-impl/hwmapping.h               | 192 +++++++
 .../stm32_2m+256k_rom.ld                      | 181 +++++++
 .../stm32_2m+8m_xram.ld                       | 178 ++++++
 .../config/board_options.cmake                | 120 +++++
 .../config/board_settings.h                   |  74 +++
 .../config/miosix_settings.h                  | 240 +++++++++
 .../core/stage_1_boot.cpp                     | 372 +++++++++++++
 .../interfaces-impl/arch_registers_impl.h     |  34 ++
 .../interfaces-impl/bsp.cpp                   | 486 +++++++++++++++++
 .../interfaces-impl/bsp_impl.h                |  74 +++
 .../interfaces-impl/hwmapping.h               | 193 +++++++
 .../stm32_2m+256k_rom.ld                      | 181 +++++++
 .../stm32_2m+8m_xram.ld                       | 178 ++++++
 .../config/board_options.cmake                | 120 +++++
 .../config/board_settings.h                   |  74 +++
 .../config/miosix_settings.h                  | 240 +++++++++
 .../core/stage_1_boot.cpp                     | 436 +++++++++++++++
 .../interfaces-impl/arch_registers_impl.h     |  34 ++
 .../interfaces-impl/bsp.cpp                   | 505 ++++++++++++++++++
 .../interfaces-impl/bsp_impl.h                |  76 +++
 .../interfaces-impl/hwmapping.h               | 209 ++++++++
 .../stm32_2m+256k_rom.ld                      | 181 +++++++
 .../stm32_2m+8m_xram.ld                       | 178 ++++++
 .../config/board_options.cmake                | 124 +++++
 .../stm32f429zi_nokia/config/board_settings.h |  70 +++
 .../config/miosix_settings.h                  | 240 +++++++++
 .../stm32f429zi_nokia/core/stage_1_boot.cpp   | 372 +++++++++++++
 .../interfaces-impl/arch_registers_impl.h     |  34 ++
 .../stm32f429zi_nokia/interfaces-impl/bsp.cpp | 308 +++++++++++
 .../interfaces-impl/bsp_impl.h                |  66 +++
 .../interfaces-impl/hwmapping.h               |  88 +++
 .../stm32f429zi_nokia/stm32_2m+256k_rom.ld    | 180 +++++++
 .../stm32f429zi_nokia/stm32_2m+6m_xram.ld     | 183 +++++++
 .../stm32f429zi_nokia/stm32_2m+8m_xram.ld     | 177 ++++++
 .../config/board_options.cmake                | 120 +++++
 .../config/board_settings.h                   |  59 ++
 .../config/miosix_settings.h                  | 240 +++++++++
 .../core/stage_1_boot.cpp                     | 436 +++++++++++++++
 .../interfaces-impl/arch_registers_impl.h     |  34 ++
 .../interfaces-impl/bsp.cpp                   | 313 +++++++++++
 .../interfaces-impl/bsp_impl.h                |  48 ++
 .../interfaces-impl/hwmapping.h               | 123 +++++
 .../stm32f429zi_parafoil/stm32_2m+256k_rom.ld | 181 +++++++
 .../stm32f429zi_parafoil/stm32_2m+8m_xram.ld  | 178 ++++++
 .../config/board_options.cmake                | 114 ++++
 .../config/board_settings.h                   |  64 +++
 .../config/miosix_settings.h                  | 240 +++++++++
 .../core/stage_1_boot.cpp                     | 430 +++++++++++++++
 .../interfaces-impl/arch_registers_impl.h     |  34 ++
 .../interfaces-impl/bsp.cpp                   | 127 +++++
 .../interfaces-impl/bsp_impl.h                |  73 +++
 .../interfaces-impl/hwmapping.h               |  82 +++
 .../stm32_2m+256k_rom.ld                      | 181 +++++++
 .../config/board_options.cmake                | 121 +++++
 .../stm32f429zi_rig/config/board_settings.h   |  59 ++
 .../stm32f429zi_rig/config/miosix_settings.h  | 240 +++++++++
 .../stm32f429zi_rig/core/stage_1_boot.cpp     | 372 +++++++++++++
 .../interfaces-impl/arch_registers_impl.h     |  34 ++
 .../stm32f429zi_rig/interfaces-impl/bsp.cpp   | 364 +++++++++++++
 .../interfaces-impl/bsp_impl.h                |  48 ++
 .../interfaces-impl/hwmapping.h               | 173 ++++++
 src/bsps/stm32f429zi_rig/stm32_2m+256k_rom.ld | 181 +++++++
 src/bsps/stm32f429zi_rig/stm32_2m+8m_xram.ld  | 178 ++++++
 .../config/board_options.cmake                | 102 ++++
 .../config/board_settings.h                   |  61 +++
 .../config/miosix_settings.h                  | 240 +++++++++
 .../stm32f756zg_nucleo/core/stage_1_boot.cpp  | 454 ++++++++++++++++
 .../interfaces-impl/arch_registers_impl.h     |  39 ++
 .../interfaces-impl/bsp.cpp                   | 124 +++++
 .../interfaces-impl/bsp_impl.h                |  95 ++++
 .../stm32f756zg_nucleo/stm32_1m+256k_rom.ld   | 178 ++++++
 .../config/board_options.cmake                | 106 ++++
 .../config/board_settings.h                   |  62 +++
 .../config/miosix_settings.h                  | 240 +++++++++
 .../core/stage_1_boot.cpp                     | 505 ++++++++++++++++++
 .../interfaces-impl/arch_registers_impl.h     |  39 ++
 .../interfaces-impl/bsp.cpp                   | 367 +++++++++++++
 .../interfaces-impl/bsp_impl.h                | 116 ++++
 .../interfaces-impl/hwmapping.h               | 131 +++++
 .../stm32_2m+16m_xram.ld                      | 190 +++++++
 .../stm32_2m+384k_ram.ld                      | 189 +++++++
 .../config/board_options.cmake                | 106 ++++
 .../config/board_settings.h                   |  62 +++
 .../config/miosix_settings.h                  | 240 +++++++++
 .../core/stage_1_boot.cpp                     | 505 ++++++++++++++++++
 .../interfaces-impl/arch_registers_impl.h     |  39 ++
 .../interfaces-impl/bsp.cpp                   | 302 +++++++++++
 .../interfaces-impl/bsp_impl.h                | 116 ++++
 .../stm32_2m+16m_xram.ld                      | 190 +++++++
 .../stm32_2m+384k_ram.ld                      | 189 +++++++
 .../config/board_options.cmake                | 106 ++++
 .../config/board_settings.h                   |  62 +++
 .../config/miosix_settings.h                  | 240 +++++++++
 .../core/stage_1_boot.cpp                     | 505 ++++++++++++++++++
 .../interfaces-impl/arch_registers_impl.h     |  39 ++
 .../interfaces-impl/bsp.cpp                   | 419 +++++++++++++++
 .../interfaces-impl/bsp_impl.h                | 116 ++++
 .../interfaces-impl/hwmapping.h               | 221 ++++++++
 .../stm32_2m+16m_xram.ld                      | 190 +++++++
 .../stm32_2m+384k_ram.ld                      | 189 +++++++
 .../config/board_options.cmake                | 106 ++++
 .../config/board_settings.h                   |  62 +++
 .../config/miosix_settings.h                  | 240 +++++++++
 .../core/stage_1_boot.cpp                     | 505 ++++++++++++++++++
 .../interfaces-impl/arch_registers_impl.h     |  39 ++
 .../interfaces-impl/bsp.cpp                   | 355 ++++++++++++
 .../interfaces-impl/bsp_impl.h                | 116 ++++
 .../interfaces-impl/hwmapping.h               | 130 +++++
 .../stm32_2m+16m_xram.ld                      | 190 +++++++
 .../stm32_2m+384k_ram.ld                      | 189 +++++++
 .../config/board_options.cmake                | 106 ++++
 .../config/board_settings.h                   |  62 +++
 .../config/miosix_settings.h                  | 240 +++++++++
 .../core/stage_1_boot.cpp                     | 505 ++++++++++++++++++
 .../interfaces-impl/arch_registers_impl.h     |  39 ++
 .../interfaces-impl/bsp.cpp                   | 370 +++++++++++++
 .../interfaces-impl/bsp_impl.h                | 106 ++++
 .../interfaces-impl/hwmapping.h               | 145 +++++
 .../stm32_2m+16m_xram.ld                      | 190 +++++++
 .../stm32_2m+384k_ram.ld                      | 189 +++++++
 src/entrypoints/bmx160-calibration-entry.cpp  |   8 +-
 src/entrypoints/sx1278-serial.cpp             |   4 +-
 src/tests/drivers/xbee/test-xbee-bidir.cpp    |  10 +-
 .../radio/sx1278/fsk/test-sx1278-bidir.cpp    |   2 +-
 .../radio/sx1278/fsk/test-sx1278-mavlink.cpp  |   4 +-
 .../radio/sx1278/lora/test-sx1278-bidir.cpp   |   2 +-
 .../radio/sx1278/lora/test-sx1278-mavlink.cpp |   2 +-
 .../radio/sx1278/lora/test-sx1278-simple.cpp  |   2 +-
 src/tests/radio/sx1278/sx1278-init.h          |  20 +-
 src/tests/sensors/test-ubxgps-spi.cpp         |   2 +-
 159 files changed, 25516 insertions(+), 379 deletions(-)
 create mode 100644 cmake/boards.cmake
 create mode 100644 src/bsps/stm32f205rc_ciuti/config/board_options.cmake
 create mode 100644 src/bsps/stm32f205rc_ciuti/config/board_settings.h
 create mode 100644 src/bsps/stm32f205rc_ciuti/config/miosix_settings.h
 create mode 100644 src/bsps/stm32f205rc_ciuti/core/stage_1_boot.cpp
 create mode 100644 src/bsps/stm32f205rc_ciuti/interfaces-impl/arch_registers_impl.h
 create mode 100644 src/bsps/stm32f205rc_ciuti/interfaces-impl/bsp.cpp
 create mode 100644 src/bsps/stm32f205rc_ciuti/interfaces-impl/bsp_impl.h
 create mode 100644 src/bsps/stm32f205rc_ciuti/interfaces-impl/delays.cpp
 create mode 100644 src/bsps/stm32f205rc_ciuti/interfaces-impl/hwmapping.h
 create mode 100644 src/bsps/stm32f205rc_ciuti/stm32_512k+128k_rom.ld
 create mode 100644 src/bsps/stm32f429zi_death_stack_v1/config/board_options.cmake
 create mode 100644 src/bsps/stm32f429zi_death_stack_v1/config/board_settings.h
 create mode 100644 src/bsps/stm32f429zi_death_stack_v1/config/miosix_settings.h
 create mode 100644 src/bsps/stm32f429zi_death_stack_v1/core/stage_1_boot.cpp
 create mode 100644 src/bsps/stm32f429zi_death_stack_v1/interfaces-impl/arch_registers_impl.h
 create mode 100644 src/bsps/stm32f429zi_death_stack_v1/interfaces-impl/bsp.cpp
 create mode 100644 src/bsps/stm32f429zi_death_stack_v1/interfaces-impl/bsp_impl.h
 create mode 100644 src/bsps/stm32f429zi_death_stack_v1/interfaces-impl/hwmapping.h
 create mode 100644 src/bsps/stm32f429zi_death_stack_v1/stm32_2m+256k_rom.ld
 create mode 100644 src/bsps/stm32f429zi_death_stack_v1/stm32_2m+8m_xram.ld
 create mode 100644 src/bsps/stm32f429zi_death_stack_v2/config/board_options.cmake
 create mode 100644 src/bsps/stm32f429zi_death_stack_v2/config/board_settings.h
 create mode 100644 src/bsps/stm32f429zi_death_stack_v2/config/miosix_settings.h
 create mode 100644 src/bsps/stm32f429zi_death_stack_v2/core/stage_1_boot.cpp
 create mode 100644 src/bsps/stm32f429zi_death_stack_v2/interfaces-impl/arch_registers_impl.h
 create mode 100644 src/bsps/stm32f429zi_death_stack_v2/interfaces-impl/bsp.cpp
 create mode 100644 src/bsps/stm32f429zi_death_stack_v2/interfaces-impl/bsp_impl.h
 create mode 100644 src/bsps/stm32f429zi_death_stack_v2/interfaces-impl/hwmapping.h
 create mode 100644 src/bsps/stm32f429zi_death_stack_v2/stm32_2m+256k_rom.ld
 create mode 100644 src/bsps/stm32f429zi_death_stack_v2/stm32_2m+8m_xram.ld
 create mode 100644 src/bsps/stm32f429zi_death_stack_v3/config/board_options.cmake
 create mode 100644 src/bsps/stm32f429zi_death_stack_v3/config/board_settings.h
 create mode 100644 src/bsps/stm32f429zi_death_stack_v3/config/miosix_settings.h
 create mode 100644 src/bsps/stm32f429zi_death_stack_v3/core/stage_1_boot.cpp
 create mode 100644 src/bsps/stm32f429zi_death_stack_v3/interfaces-impl/arch_registers_impl.h
 create mode 100644 src/bsps/stm32f429zi_death_stack_v3/interfaces-impl/bsp.cpp
 create mode 100644 src/bsps/stm32f429zi_death_stack_v3/interfaces-impl/bsp_impl.h
 create mode 100644 src/bsps/stm32f429zi_death_stack_v3/interfaces-impl/hwmapping.h
 create mode 100644 src/bsps/stm32f429zi_death_stack_v3/stm32_2m+256k_rom.ld
 create mode 100644 src/bsps/stm32f429zi_death_stack_v3/stm32_2m+8m_xram.ld
 create mode 100644 src/bsps/stm32f429zi_nokia/config/board_options.cmake
 create mode 100644 src/bsps/stm32f429zi_nokia/config/board_settings.h
 create mode 100644 src/bsps/stm32f429zi_nokia/config/miosix_settings.h
 create mode 100644 src/bsps/stm32f429zi_nokia/core/stage_1_boot.cpp
 create mode 100644 src/bsps/stm32f429zi_nokia/interfaces-impl/arch_registers_impl.h
 create mode 100644 src/bsps/stm32f429zi_nokia/interfaces-impl/bsp.cpp
 create mode 100644 src/bsps/stm32f429zi_nokia/interfaces-impl/bsp_impl.h
 create mode 100644 src/bsps/stm32f429zi_nokia/interfaces-impl/hwmapping.h
 create mode 100644 src/bsps/stm32f429zi_nokia/stm32_2m+256k_rom.ld
 create mode 100644 src/bsps/stm32f429zi_nokia/stm32_2m+6m_xram.ld
 create mode 100644 src/bsps/stm32f429zi_nokia/stm32_2m+8m_xram.ld
 create mode 100644 src/bsps/stm32f429zi_parafoil/config/board_options.cmake
 create mode 100644 src/bsps/stm32f429zi_parafoil/config/board_settings.h
 create mode 100644 src/bsps/stm32f429zi_parafoil/config/miosix_settings.h
 create mode 100644 src/bsps/stm32f429zi_parafoil/core/stage_1_boot.cpp
 create mode 100644 src/bsps/stm32f429zi_parafoil/interfaces-impl/arch_registers_impl.h
 create mode 100644 src/bsps/stm32f429zi_parafoil/interfaces-impl/bsp.cpp
 create mode 100644 src/bsps/stm32f429zi_parafoil/interfaces-impl/bsp_impl.h
 create mode 100644 src/bsps/stm32f429zi_parafoil/interfaces-impl/hwmapping.h
 create mode 100644 src/bsps/stm32f429zi_parafoil/stm32_2m+256k_rom.ld
 create mode 100644 src/bsps/stm32f429zi_parafoil/stm32_2m+8m_xram.ld
 create mode 100644 src/bsps/stm32f429zi_pyxis_auxiliary/config/board_options.cmake
 create mode 100644 src/bsps/stm32f429zi_pyxis_auxiliary/config/board_settings.h
 create mode 100644 src/bsps/stm32f429zi_pyxis_auxiliary/config/miosix_settings.h
 create mode 100644 src/bsps/stm32f429zi_pyxis_auxiliary/core/stage_1_boot.cpp
 create mode 100644 src/bsps/stm32f429zi_pyxis_auxiliary/interfaces-impl/arch_registers_impl.h
 create mode 100644 src/bsps/stm32f429zi_pyxis_auxiliary/interfaces-impl/bsp.cpp
 create mode 100644 src/bsps/stm32f429zi_pyxis_auxiliary/interfaces-impl/bsp_impl.h
 create mode 100644 src/bsps/stm32f429zi_pyxis_auxiliary/interfaces-impl/hwmapping.h
 create mode 100644 src/bsps/stm32f429zi_pyxis_auxiliary/stm32_2m+256k_rom.ld
 create mode 100644 src/bsps/stm32f429zi_rig/config/board_options.cmake
 create mode 100644 src/bsps/stm32f429zi_rig/config/board_settings.h
 create mode 100644 src/bsps/stm32f429zi_rig/config/miosix_settings.h
 create mode 100644 src/bsps/stm32f429zi_rig/core/stage_1_boot.cpp
 create mode 100644 src/bsps/stm32f429zi_rig/interfaces-impl/arch_registers_impl.h
 create mode 100644 src/bsps/stm32f429zi_rig/interfaces-impl/bsp.cpp
 create mode 100644 src/bsps/stm32f429zi_rig/interfaces-impl/bsp_impl.h
 create mode 100644 src/bsps/stm32f429zi_rig/interfaces-impl/hwmapping.h
 create mode 100644 src/bsps/stm32f429zi_rig/stm32_2m+256k_rom.ld
 create mode 100644 src/bsps/stm32f429zi_rig/stm32_2m+8m_xram.ld
 create mode 100644 src/bsps/stm32f756zg_nucleo/config/board_options.cmake
 create mode 100644 src/bsps/stm32f756zg_nucleo/config/board_settings.h
 create mode 100644 src/bsps/stm32f756zg_nucleo/config/miosix_settings.h
 create mode 100644 src/bsps/stm32f756zg_nucleo/core/stage_1_boot.cpp
 create mode 100644 src/bsps/stm32f756zg_nucleo/interfaces-impl/arch_registers_impl.h
 create mode 100644 src/bsps/stm32f756zg_nucleo/interfaces-impl/bsp.cpp
 create mode 100644 src/bsps/stm32f756zg_nucleo/interfaces-impl/bsp_impl.h
 create mode 100644 src/bsps/stm32f756zg_nucleo/stm32_1m+256k_rom.ld
 create mode 100644 src/bsps/stm32f767zi_automated_antennas/config/board_options.cmake
 create mode 100644 src/bsps/stm32f767zi_automated_antennas/config/board_settings.h
 create mode 100644 src/bsps/stm32f767zi_automated_antennas/config/miosix_settings.h
 create mode 100644 src/bsps/stm32f767zi_automated_antennas/core/stage_1_boot.cpp
 create mode 100644 src/bsps/stm32f767zi_automated_antennas/interfaces-impl/arch_registers_impl.h
 create mode 100644 src/bsps/stm32f767zi_automated_antennas/interfaces-impl/bsp.cpp
 create mode 100644 src/bsps/stm32f767zi_automated_antennas/interfaces-impl/bsp_impl.h
 create mode 100644 src/bsps/stm32f767zi_automated_antennas/interfaces-impl/hwmapping.h
 create mode 100644 src/bsps/stm32f767zi_automated_antennas/stm32_2m+16m_xram.ld
 create mode 100644 src/bsps/stm32f767zi_automated_antennas/stm32_2m+384k_ram.ld
 create mode 100644 src/bsps/stm32f767zi_compute_unit/config/board_options.cmake
 create mode 100644 src/bsps/stm32f767zi_compute_unit/config/board_settings.h
 create mode 100644 src/bsps/stm32f767zi_compute_unit/config/miosix_settings.h
 create mode 100644 src/bsps/stm32f767zi_compute_unit/core/stage_1_boot.cpp
 create mode 100644 src/bsps/stm32f767zi_compute_unit/interfaces-impl/arch_registers_impl.h
 create mode 100644 src/bsps/stm32f767zi_compute_unit/interfaces-impl/bsp.cpp
 create mode 100644 src/bsps/stm32f767zi_compute_unit/interfaces-impl/bsp_impl.h
 create mode 100644 src/bsps/stm32f767zi_compute_unit/stm32_2m+16m_xram.ld
 create mode 100644 src/bsps/stm32f767zi_compute_unit/stm32_2m+384k_ram.ld
 create mode 100644 src/bsps/stm32f767zi_death_stack_v4/config/board_options.cmake
 create mode 100644 src/bsps/stm32f767zi_death_stack_v4/config/board_settings.h
 create mode 100644 src/bsps/stm32f767zi_death_stack_v4/config/miosix_settings.h
 create mode 100644 src/bsps/stm32f767zi_death_stack_v4/core/stage_1_boot.cpp
 create mode 100644 src/bsps/stm32f767zi_death_stack_v4/interfaces-impl/arch_registers_impl.h
 create mode 100644 src/bsps/stm32f767zi_death_stack_v4/interfaces-impl/bsp.cpp
 create mode 100644 src/bsps/stm32f767zi_death_stack_v4/interfaces-impl/bsp_impl.h
 create mode 100644 src/bsps/stm32f767zi_death_stack_v4/interfaces-impl/hwmapping.h
 create mode 100644 src/bsps/stm32f767zi_death_stack_v4/stm32_2m+16m_xram.ld
 create mode 100644 src/bsps/stm32f767zi_death_stack_v4/stm32_2m+384k_ram.ld
 create mode 100644 src/bsps/stm32f767zi_gemini_gs/config/board_options.cmake
 create mode 100644 src/bsps/stm32f767zi_gemini_gs/config/board_settings.h
 create mode 100644 src/bsps/stm32f767zi_gemini_gs/config/miosix_settings.h
 create mode 100644 src/bsps/stm32f767zi_gemini_gs/core/stage_1_boot.cpp
 create mode 100644 src/bsps/stm32f767zi_gemini_gs/interfaces-impl/arch_registers_impl.h
 create mode 100644 src/bsps/stm32f767zi_gemini_gs/interfaces-impl/bsp.cpp
 create mode 100644 src/bsps/stm32f767zi_gemini_gs/interfaces-impl/bsp_impl.h
 create mode 100644 src/bsps/stm32f767zi_gemini_gs/interfaces-impl/hwmapping.h
 create mode 100644 src/bsps/stm32f767zi_gemini_gs/stm32_2m+16m_xram.ld
 create mode 100644 src/bsps/stm32f767zi_gemini_gs/stm32_2m+384k_ram.ld
 create mode 100644 src/bsps/stm32f767zi_gemini_motor/config/board_options.cmake
 create mode 100644 src/bsps/stm32f767zi_gemini_motor/config/board_settings.h
 create mode 100644 src/bsps/stm32f767zi_gemini_motor/config/miosix_settings.h
 create mode 100644 src/bsps/stm32f767zi_gemini_motor/core/stage_1_boot.cpp
 create mode 100644 src/bsps/stm32f767zi_gemini_motor/interfaces-impl/arch_registers_impl.h
 create mode 100644 src/bsps/stm32f767zi_gemini_motor/interfaces-impl/bsp.cpp
 create mode 100644 src/bsps/stm32f767zi_gemini_motor/interfaces-impl/bsp_impl.h
 create mode 100644 src/bsps/stm32f767zi_gemini_motor/interfaces-impl/hwmapping.h
 create mode 100644 src/bsps/stm32f767zi_gemini_motor/stm32_2m+16m_xram.ld
 create mode 100644 src/bsps/stm32f767zi_gemini_motor/stm32_2m+384k_ram.ld

diff --git a/.gitignore b/.gitignore
index 68d5f6e5f..b343c6b27 100644
--- a/.gitignore
+++ b/.gitignore
@@ -27,7 +27,6 @@ cmake-build-*
 
 store.json
 **/generated/
-core
 
 __pycache__
 /scripts/generators/generated
diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
index 895903d93..b16913f5d 100644
--- a/.vscode/c_cpp_properties.json
+++ b/.vscode/c_cpp_properties.json
@@ -20,15 +20,16 @@
         ]
     },
     "configurations": [
+        // Skyward boards
         {
-            "name": "stm32f205rc_skyward_ciuti",
+            "name": "stm32f205rc_ciuti",
             "cStandard": "c11",
             "cppStandard": "c++14",
             "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
             "defines": [
-                "{defaultDefines}",
-                "_MIOSIX_BOARDNAME=stm32f205RC_skyward_ciuti",
-                "_BOARD_STM32F205RC_SKYWARD_CIUTI",
+                "${defaultDefines}",
+                "_MIOSIX_BOARDNAME=stm32f205rc_ciuti",
+                "_BOARD_STM32F205RC_CIUTI",
                 "_ARCH_CORTEXM3_STM32F2",
                 "STM32F205xx",
                 "HSE_VALUE=25000000",
@@ -38,64 +39,43 @@
             "includePath": [
                 "${defaultIncludePaths}",
                 "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM3_stm32f2/common",
-                "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM3_stm32f2/stm32f205rc_skyward_ciuti",
-                "${workspaceFolder}/libs/miosix-kernel/miosix/config/arch/cortexM3_stm32f2/stm32f205rc_skyward_ciuti"
+                "${workspaceFolder}/src/bsps/stm32f205rc_ciuti/config",
+                "${workspaceFolder}/src/bsps/stm32f205rc_ciuti"
             ]
         },
         {
-            "name": "stm32f407vg_stm32f4discovery",
+            "name": "stm32f429zi_death_stack_v1",
             "cStandard": "c11",
             "cppStandard": "c++14",
             "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
             "defines": [
-                "{defaultDefines}",
-                "_MIOSIX_BOARDNAME=stm32f407vg_stm32f4discovery",
-                "_BOARD_STM32F4DISCOVERY",
-                "_ARCH_CORTEXM4_STM32F4",
-                "STM32F407xx",
-                "HSE_VALUE=8000000",
-                "SYSCLK_FREQ_168MHz=168000000",
-                "V_DDA_VOLTAGE=3.0f"
-            ],
-            "includePath": [
-                "${defaultIncludePaths}",
-                "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common",
-                "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f407vg_stm32f4discovery",
-                "${workspaceFolder}/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f407vg_stm32f4discovery"
-            ]
-        },
-        {
-            "name": "stm32f429zi_hre_test_stand",
-            "cStandard": "c11",
-            "cppStandard": "c++14",
-            "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
-            "defines": [
-                "{defaultDefines}",
-                "_MIOSIX_BOARDNAME=stm32f429zi_hre_test_stand",
-                "_BOARD_STM32F429ZI_HRE_TEST_STAND",
+                "${defaultDefines}",
+                "_MIOSIX_BOARDNAME=stm32f429zi_death_stack_v1",
+                "_BOARD_STM32F429ZI_DEATH_STACK_V1",
                 "_ARCH_CORTEXM4_STM32F4",
                 "STM32F429xx",
                 "HSE_VALUE=8000000",
                 "SYSCLK_FREQ_168MHz=168000000",
                 "__ENABLE_XRAM",
-                "V_DDA_VOLTAGE=3.0f"
+                "V_DDA_VOLTAGE=3.3f"
             ],
             "includePath": [
                 "${defaultIncludePaths}",
                 "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common",
-                "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_hre_test_stand",
-                "${workspaceFolder}/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_hre_test_stand"
-            ]
+                "${workspaceFolder}/src/bsps/stm32f429zi_death_stack_v1/config",
+                "${workspaceFolder}/src/bsps/stm32f429zi_death_stack_v1"
+            ],
+            "configurationProvider": "ms-vscode.cmake-tools"
         },
         {
-            "name": "stm32f429zi_skyward_death_stack_v3",
+            "name": "stm32f429zi_death_stack_v2",
             "cStandard": "c11",
             "cppStandard": "c++14",
             "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
             "defines": [
-                "{defaultDefines}",
-                "_MIOSIX_BOARDNAME=stm32f429zi_skyward_death_stack_v3",
-                "_BOARD_STM32F429ZI_SKYWARD_DEATHST_X",
+                "${defaultDefines}",
+                "_MIOSIX_BOARDNAME=stm32f429zi_death_stack_v2",
+                "_BOARD_STM32F429ZI_DEATH_STACK_V2",
                 "_ARCH_CORTEXM4_STM32F4",
                 "STM32F429xx",
                 "HSE_VALUE=8000000",
@@ -106,19 +86,19 @@
             "includePath": [
                 "${defaultIncludePaths}",
                 "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common",
-                "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_v3",
-                "${workspaceFolder}/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_v3"
+                "${workspaceFolder}/src/bsps/stm32f429zi_death_stack_v2/config",
+                "${workspaceFolder}/src/bsps/stm32f429zi_death_stack_v2"
             ]
         },
         {
-            "name": "stm32f429zi_skyward_death_stack_x",
+            "name": "stm32f429zi_death_stack_v3",
             "cStandard": "c11",
             "cppStandard": "c++14",
             "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
             "defines": [
-                "{defaultDefines}",
-                "_MIOSIX_BOARDNAME=stm32f429zi_skyward_death_stack_x",
-                "_BOARD_STM32F429ZI_SKYWARD_DEATHST_X",
+                "${defaultDefines}",
+                "_MIOSIX_BOARDNAME=stm32f429zi_death_stack_v3",
+                "_BOARD_STM32F429ZI_DEATH_STACK_V3",
                 "_ARCH_CORTEXM4_STM32F4",
                 "STM32F429xx",
                 "HSE_VALUE=8000000",
@@ -129,42 +109,42 @@
             "includePath": [
                 "${defaultIncludePaths}",
                 "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common",
-                "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_x",
-                "${workspaceFolder}/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_x"
+                "${workspaceFolder}/src/bsps/stm32f429zi_death_stack_v3/config",
+                "${workspaceFolder}/src/bsps/stm32f429zi_death_stack_v3"
             ]
         },
         {
-            "name": "stm32f429zi_skyward_death_stack_x_maker_faire",
+            "name": "stm32f429zi_nokia",
             "cStandard": "c11",
             "cppStandard": "c++14",
             "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
             "defines": [
-                "{defaultDefines}",
-                "_MIOSIX_BOARDNAME=stm32f429zi_skyward_death_stack_x_maker_faire",
-                "_BOARD_STM32F429ZI_SKYWARD_DEATHST_X_MAKER_FAIRE",
+                "${defaultDefines}",
+                "_MIOSIX_BOARDNAME=stm32f429zi_nokia",
+                "_BOARD_STM32F429ZI_NOKIA",
                 "_ARCH_CORTEXM4_STM32F4",
                 "STM32F429xx",
                 "HSE_VALUE=8000000",
                 "SYSCLK_FREQ_168MHz=168000000",
                 "__ENABLE_XRAM",
-                "V_DDA_VOLTAGE=3.3f"
+                "V_DDA_VOLTAGE=3.0f"
             ],
             "includePath": [
                 "${defaultIncludePaths}",
                 "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common",
-                "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_x_maker_faire",
-                "${workspaceFolder}/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_death_stack_x_maker_faire"
+                "${workspaceFolder}/src/bsps/stm32f429zi_nokia/config",
+                "${workspaceFolder}/src/bsps/stm32f429zi_nokia"
             ]
         },
         {
-            "name": "stm32f429zi_skyward_groundstation",
+            "name": "stm32f429zi_parafoil",
             "cStandard": "c11",
             "cppStandard": "c++14",
             "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
             "defines": [
-                "{defaultDefines}",
-                "_MIOSIX_BOARDNAME=stm32f429zi_skyward_groundstation",
-                "_BOARD_STM32F429ZI_SKYWARD_GS",
+                "${defaultDefines}",
+                "_MIOSIX_BOARDNAME=stm32f429zi_parafoil",
+                "_BOARD_STM32F429ZI_PARAFOIL",
                 "_ARCH_CORTEXM4_STM32F4",
                 "STM32F429xx",
                 "HSE_VALUE=8000000",
@@ -175,42 +155,41 @@
             "includePath": [
                 "${defaultIncludePaths}",
                 "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common",
-                "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_groundstation",
-                "${workspaceFolder}/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_groundstation"
+                "${workspaceFolder}/src/bsps/stm32f429zi_parafoil/config",
+                "${workspaceFolder}/src/bsps/stm32f429zi_parafoil"
             ]
         },
         {
-            "name": "stm32f429zi_skyward_groundstation_parafoil",
+            "name": "stm32f429zi_pyxis_auxiliary",
             "cStandard": "c11",
             "cppStandard": "c++14",
             "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
             "defines": [
-                "{defaultDefines}",
-                "_MIOSIX_BOARDNAME=stm32f429zi_skyward_groundstation_parafoil",
-                "D_BOARD_STM32F429ZI_SKYWARD_GS_PARAFOIL",
+                "${defaultDefines}",
+                "_MIOSIX_BOARDNAME=stm32f429zi_pyxis_auxiliary",
+                "_BOARD_STM32F429ZI_PYXIS_AUXILIARY",
                 "_ARCH_CORTEXM4_STM32F4",
                 "STM32F429xx",
                 "HSE_VALUE=8000000",
                 "SYSCLK_FREQ_168MHz=168000000",
-                "__ENABLE_XRAM",
-                "V_DDA_VOLTAGE=3.0f"
+                "V_DDA_VOLTAGE=3.3f"
             ],
             "includePath": [
                 "${defaultIncludePaths}",
                 "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common",
-                "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_groundstation_parafoil",
-                "${workspaceFolder}/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_groundstation_parafoil"
+                "${workspaceFolder}/src/bsps/stm32f429zi_pyxis_auxiliary/config",
+                "${workspaceFolder}/src/bsps/stm32f429zi_pyxis_auxiliary"
             ]
         },
         {
-            "name": "stm32f429zi_skyward_groundstation_v2",
+            "name": "stm32f429zi_rig",
             "cStandard": "c11",
             "cppStandard": "c++14",
             "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
             "defines": [
-                "{defaultDefines}",
-                "_MIOSIX_BOARDNAME=stm32f429zi_skyward_groundstation_v2",
-                "_BOARD_STM32F429ZI_SKYWARD_GS_V2",
+                "${defaultDefines}",
+                "_MIOSIX_BOARDNAME=stm32f429zi_rig",
+                "_BOARD_STM32F429ZI_RIG",
                 "_ARCH_CORTEXM4_STM32F4",
                 "STM32F429xx",
                 "HSE_VALUE=8000000",
@@ -221,110 +200,111 @@
             "includePath": [
                 "${defaultIncludePaths}",
                 "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common",
-                "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_groundstation_v2",
-                "${workspaceFolder}/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_groundstation_v2"
+                "${workspaceFolder}/src/bsps/stm32f429zi_rig/config",
+                "${workspaceFolder}/src/bsps/stm32f429zi_rig"
             ]
         },
         {
-            "name": "stm32f429zi_skyward_parafoil",
+            "name": "stm32f756zg_nucleo",
             "cStandard": "c11",
             "cppStandard": "c++14",
             "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
             "defines": [
-                "{defaultDefines}",
-                "_MIOSIX_BOARDNAME=stm32f429zi_skyward_parafoil",
-                "_BOARD_STM32F429ZI_SKYWARD_PARAFOIL",
-                "_ARCH_CORTEXM4_STM32F4",
-                "STM32F429xx",
-                "HSE_VALUE=8000000",
-                "SYSCLK_FREQ_168MHz=168000000",
+                "${defaultDefines}",
+                "_MIOSIX_BOARDNAME=stm32f756zg_nucleo",
+                "D_BOARD_STM32F756ZG_NUCLEO",
+                "_ARCH_CORTEXM7_STM32F7",
+                "STM32F756xx",
+                "HSE_VALUE=25000000",
+                "SYSCLK_FREQ_216MHz=216000000",
                 "__ENABLE_XRAM",
-                "V_DDA_VOLTAGE=3.0f"
+                "V_DDA_VOLTAGE=3.3f"
             ],
             "includePath": [
                 "${defaultIncludePaths}",
-                "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common",
-                "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_parafoil",
-                "${workspaceFolder}/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_parafoil"
+                "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM7_stm32f7/common",
+                "${workspaceFolder}/src/bsps/stm32f756zg_nucleo/config",
+                "${workspaceFolder}/src/bsps/stm32f756zg_nucleo"
             ]
         },
         {
-            "name": "stm32f429zi_skyward_pyxis_auxiliary",
+            "name": "stm32f767zi_automated_antennas",
             "cStandard": "c11",
             "cppStandard": "c++14",
             "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
             "defines": [
-                "{defaultDefines}",
-                "_MIOSIX_BOARDNAME=stm32f429zi_skyward_pyxis_auxiliary",
-                "_BOARD_STM32F429ZI_SKYWARD_PYXIS_AUXILIARY",
-                "_ARCH_CORTEXM4_STM32F4",
-                "STM32F429xx",
-                "HSE_VALUE=8000000",
-                "SYSCLK_FREQ_168MHz=168000000",
+                "${defaultDefines}",
+                "_MIOSIX_BOARDNAME=stm32f767zi_automated_antennas",
+                "_BOARD_STM32F767ZI_AUTOMATED_ANTENNAS",
+                "_ARCH_CORTEXM7_STM32F7",
+                "STM32F769xx",
+                "HSE_VALUE=25000000",
+                "SYSCLK_FREQ_216MHz=216000000",
+                "__ENABLE_XRAM",
                 "V_DDA_VOLTAGE=3.3f"
             ],
             "includePath": [
                 "${defaultIncludePaths}",
-                "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common",
-                "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_pyxis_auxiliary",
-                "${workspaceFolder}/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_pyxis_auxiliary"
+                "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM7_stm32f7/common",
+                "${workspaceFolder}/src/bsps/stm32f767zi_automated_antennas/config",
+                "${workspaceFolder}/src/bsps/stm32f767zi_automated_antennas"
             ]
         },
         {
-            "name": "stm32f429zi_skyward_rig",
+            "name": "stm32f767zi_compute_unit",
             "cStandard": "c11",
             "cppStandard": "c++14",
             "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
             "defines": [
-                "{defaultDefines}",
-                "_MIOSIX_BOARDNAME=stm32f429zi_skyward_rig",
-                "_BOARD_STM32F429ZI_SKYWARD_RIG",
-                "_ARCH_CORTEXM4_STM32F4",
-                "STM32F429xx",
-                "HSE_VALUE=8000000",
-                "SYSCLK_FREQ_168MHz=168000000",
+                "${defaultDefines}",
+                "_MIOSIX_BOARDNAME=stm32f767zi_compute_unit",
+                "_BOARD_STM32F767ZI_COMPUTE_UNIT",
+                "_ARCH_CORTEXM7_STM32F7",
+                "STM32F769xx",
+                "HSE_VALUE=25000000",
+                "SYSCLK_FREQ_216MHz=216000000",
                 "__ENABLE_XRAM",
-                "V_DDA_VOLTAGE=3.0f"
+                "V_DDA_VOLTAGE=3.3f"
             ],
             "includePath": [
                 "${defaultIncludePaths}",
-                "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common",
-                "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_skyward_rig",
-                "${workspaceFolder}/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_skyward_rig"
+                "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM7_stm32f7/common",
+                "${workspaceFolder}/src/bsps/stm32f767zi_compute_unit/config",
+                "${workspaceFolder}/src/bsps/stm32f767zi_compute_unit"
             ]
         },
         {
-            "name": "stm32f429zi_stm32f4discovery",
+            "name": "stm32f767zi_death_stack_v4",
             "cStandard": "c11",
             "cppStandard": "c++14",
             "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
             "defines": [
-                "{defaultDefines}",
-                "_MIOSIX_BOARDNAME=stm32f429zi_stm32f4discovery",
-                "_BOARD_STM32F429ZI_STM32F4DISCOVERY",
-                "_ARCH_CORTEXM4_STM32F4",
-                "STM32F429xx",
-                "HSE_VALUE=8000000",
-                "SYSCLK_FREQ_168MHz=168000000",
+                "${defaultDefines}",
+                "_MIOSIX_BOARDNAME=stm32f767zi_death_stack_v4",
+                "_BOARD_STM32F767ZI_DEATH_STACK_V4",
+                "_ARCH_CORTEXM7_STM32F7",
+                "STM32F769xx",
+                "HSE_VALUE=25000000",
+                "SYSCLK_FREQ_216MHz=216000000",
                 "__ENABLE_XRAM",
-                "V_DDA_VOLTAGE=3.0f"
+                "V_DDA_VOLTAGE=3.3f"
             ],
             "includePath": [
                 "${defaultIncludePaths}",
-                "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common",
-                "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_stm32f4discovery",
-                "${workspaceFolder}/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_stm32f4discovery"
+                "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM7_stm32f7/common",
+                "${workspaceFolder}/src/bsps/stm32f767zi_death_stack_v4/config",
+                "${workspaceFolder}/src/bsps/stm32f767zi_death_stack_v4"
             ]
         },
         {
-            "name": "stm32f767zi_compute_unit",
+            "name": "stm32f767zi_gemini_gs",
             "cStandard": "c11",
             "cppStandard": "c++14",
             "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
             "defines": [
-                "{defaultDefines}",
-                "_MIOSIX_BOARDNAME=stm32f767zi_compute_unit",
-                "_BOARD_STM32F767ZI_COMPUTE_UNIT",
+                "${defaultDefines}",
+                "_MIOSIX_BOARDNAME=stm32f767zi_gemini_gs",
+                "_BOARD_STM32F767ZI_GEMINI_GS",
                 "_ARCH_CORTEXM7_STM32F7",
                 "STM32F769xx",
                 "HSE_VALUE=25000000",
@@ -335,19 +315,19 @@
             "includePath": [
                 "${defaultIncludePaths}",
                 "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM7_stm32f7/common",
-                "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM7_stm32f7/stm32f767zi_compute_unit",
-                "${workspaceFolder}/libs/miosix-kernel/miosix/config/arch/cortexM7_stm32f7/stm32f767zi_compute_unit"
+                "${workspaceFolder}/src/bsps/stm32f767zi_gemini_gs/config",
+                "${workspaceFolder}/src/bsps/stm32f767zi_gemini_gs"
             ]
         },
         {
-            "name": "stm32f767zi_gemini_gs",
+            "name": "stm32f767zi_gemini_motor",
             "cStandard": "c11",
             "cppStandard": "c++14",
             "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
             "defines": [
-                "{defaultDefines}",
-                "_MIOSIX_BOARDNAME=stm32f767zi_gemini_gs",
-                "_BOARD_STM32F767ZI_GEMINI_GS",
+                "${defaultDefines}",
+                "_MIOSIX_BOARDNAME=stm32f767zi_gemini_motor",
+                "_BOARD_STM32F767ZI_GEMINI_MOTOR",
                 "_ARCH_CORTEXM7_STM32F7",
                 "STM32F769xx",
                 "HSE_VALUE=25000000",
@@ -358,31 +338,54 @@
             "includePath": [
                 "${defaultIncludePaths}",
                 "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM7_stm32f7/common",
-                "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM7_stm32f7/stm32f767zi_gemini_gs",
-                "${workspaceFolder}/libs/miosix-kernel/miosix/config/arch/cortexM7_stm32f7/stm32f767zi_gemini_gs"
+                "${workspaceFolder}/src/bsps/stm32f767zi_gemini_motor/config",
+                "${workspaceFolder}/src/bsps/stm32f767zi_gemini_motor"
             ]
         },
+        // Miosix boards
         {
-            "name": "stm32f756zg_nucleo",
+            "name": "stm32f407vg_stm32f4discovery",
             "cStandard": "c11",
             "cppStandard": "c++14",
             "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
             "defines": [
-                "{defaultDefines}",
-                "_MIOSIX_BOARDNAME=stm32f756zg_nucleo",
-                "D_BOARD_STM32F756ZG_NUCLEO",
-                "_ARCH_CORTEXM7_STM32F7",
-                "STM32F756xx",
-                "HSE_VALUE=25000000",
-                "SYSCLK_FREQ_216MHz=216000000",
+                "${defaultDefines}",
+                "_MIOSIX_BOARDNAME=stm32f407vg_stm32f4discovery",
+                "_BOARD_STM32F4DISCOVERY",
+                "_ARCH_CORTEXM4_STM32F4",
+                "STM32F407xx",
+                "HSE_VALUE=8000000",
+                "SYSCLK_FREQ_168MHz=168000000",
+                "V_DDA_VOLTAGE=3.0f"
+            ],
+            "includePath": [
+                "${defaultIncludePaths}",
+                "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common",
+                "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f407vg_stm32f4discovery",
+                "${workspaceFolder}/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f407vg_stm32f4discovery"
+            ]
+        },
+        {
+            "name": "stm32f429zi_stm32f4discovery",
+            "cStandard": "c11",
+            "cppStandard": "c++14",
+            "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
+            "defines": [
+                "${defaultDefines}",
+                "_MIOSIX_BOARDNAME=stm32f429zi_stm32f4discovery",
+                "_BOARD_STM32F429ZI_STM32F4DISCOVERY",
+                "_ARCH_CORTEXM4_STM32F4",
+                "STM32F429xx",
+                "HSE_VALUE=8000000",
+                "SYSCLK_FREQ_168MHz=168000000",
                 "__ENABLE_XRAM",
-                "V_DDA_VOLTAGE=3.3f"
+                "V_DDA_VOLTAGE=3.0f"
             ],
             "includePath": [
                 "${defaultIncludePaths}",
-                "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM7_stm32f7/common",
-                "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM7_stm32f7/stm32f756zg_nucleo",
-                "${workspaceFolder}/libs/miosix-kernel/miosix/config/arch/cortexM7_stm32f7/stm32f756zg_nucleo"
+                "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/common",
+                "${workspaceFolder}/libs/miosix-kernel/miosix/arch/cortexM4_stm32f4/stm32f429zi_stm32f4discovery",
+                "${workspaceFolder}/libs/miosix-kernel/miosix/config/arch/cortexM4_stm32f4/stm32f429zi_stm32f4discovery"
             ]
         },
         {
@@ -391,7 +394,7 @@
             "cppStandard": "c++14",
             "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
             "defines": [
-                "{defaultDefines}",
+                "${defaultDefines}",
                 "_MIOSIX_BOARDNAME=stm32f767zi_nucleo",
                 "D_BOARD_STM32F767ZI_NUCLEO",
                 "_ARCH_CORTEXM7_STM32F7",
@@ -414,7 +417,7 @@
             "cppStandard": "c++14",
             "compilerPath": "/opt/arm-miosix-eabi/bin/arm-miosix-eabi-g++",
             "defines": [
-                "{defaultDefines}",
+                "${defaultDefines}",
                 "_MIOSIX_BOARDNAME=stm32f769ni_discovery",
                 "_BOARD_STM32F769NI_DISCO",
                 "_ARCH_CORTEXM7_STM32F7",
@@ -433,4 +436,4 @@
         }
     ],
     "version": 4
-}
+}
\ No newline at end of file
diff --git a/CMakeLists.txt b/CMakeLists.txt
index f50a6b1ba..2671df6f6 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -19,7 +19,7 @@
 # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 # THE SOFTWARE.
 
-cmake_minimum_required(VERSION 3.16)
+cmake_minimum_required(VERSION 3.25)
 enable_testing()
 include(cmake/sbs.cmake)
 
@@ -27,43 +27,43 @@ include(cmake/sbs.cmake)
 #                                   Project                                   #
 #-----------------------------------------------------------------------------#
 
-project(SkywardBoardcore)
+project(Boardcore)
 
 #-----------------------------------------------------------------------------#
 #                                 Entrypoints                                 #
 #-----------------------------------------------------------------------------#
 
 add_executable(bmx160-calibration-v2 src/entrypoints/bmx160-calibration-entry.cpp)
-sbs_target(bmx160-calibration-v2 stm32f429zi_skyward_death_stack_x)
+sbs_target(bmx160-calibration-v2 stm32f429zi_death_stack_v2)
 
 add_executable(bmx160-calibration-v3 src/entrypoints/bmx160-calibration-entry.cpp)
-sbs_target(bmx160-calibration-v3 stm32f429zi_skyward_death_stack_v3)
+sbs_target(bmx160-calibration-v3 stm32f429zi_death_stack_v3)
 
 add_executable(config-dsgamma src/entrypoints/config-dsgamma.cpp)
 sbs_target(config-dsgamma stm32f429zi_stm32f4discovery)
 
 add_executable(imu-calibration src/entrypoints/imu-calibration.cpp)
-sbs_target(imu-calibration stm32f429zi_skyward_parafoil)
+sbs_target(imu-calibration stm32f429zi_parafoil)
 
 add_executable(mxgui-helloworld src/entrypoints/examples/mxgui-helloworld.cpp)
 sbs_target(mxgui-helloworld stm32f429zi_stm32f4discovery)
 
-add_executable(kernel-testsuite src/entrypoints/kernel-testsuite.cpp)
-sbs_target(kernel-testsuite stm32f767zi_compute_unit)
+# add_executable(kernel-testsuite src/entrypoints/kernel-testsuite.cpp)
+# sbs_target(kernel-testsuite stm32f767zi_compute_unit)
 
 add_executable(runcam-settings src/entrypoints/runcam-settings.cpp)
 sbs_target(runcam-settings stm32f407vg_stm32f4discovery)
 
 add_executable(sdcard-benchmark src/entrypoints/sdcard-benchmark.cpp)
-sbs_target(sdcard-benchmark stm32f429zi_skyward_death_stack_x)
+sbs_target(sdcard-benchmark stm32f429zi_death_stack_v2)
 
 add_executable(sx1278fsk-ra01-serial src/entrypoints/sx1278-serial.cpp)
 target_compile_definitions(sx1278fsk-ra01-serial PRIVATE SX1278_IS_FSK)
-sbs_target(sx1278fsk-ra01-serial stm32f429zi_skyward_groundstation_v2)
+sbs_target(sx1278fsk-ra01-serial stm32f429zi_nokia)
 
 add_executable(sx1278lora-ra01-serial src/entrypoints/sx1278-serial.cpp)
 target_compile_definitions(sx1278lora-ra01-serial PRIVATE SX1278_IS_LORA)
-sbs_target(sx1278lora-ra01-serial stm32f429zi_skyward_groundstation_v2)
+sbs_target(sx1278lora-ra01-serial stm32f429zi_nokia)
 
 add_executable(sx1278fsk-skyward433-serial src/entrypoints/sx1278-serial.cpp)
 target_compile_definitions(sx1278fsk-skyward433-serial PRIVATE SX1278_IS_FSK SX1278_IS_SKYWARD433)
@@ -71,11 +71,11 @@ sbs_target(sx1278fsk-skyward433-serial stm32f767zi_gemini_gs)
 
 add_executable(sx1278lora-skyward433-serial src/entrypoints/sx1278-serial.cpp)
 target_compile_definitions(sx1278lora-skyward433-serial PRIVATE SX1278_IS_LORA SX1278_IS_SKYWARD433)
-sbs_target(sx1278lora-skyward433-serial stm32f429zi_skyward_groundstation_v2)
+sbs_target(sx1278lora-skyward433-serial stm32f429zi_nokia)
 
 add_executable(sx1278fsk-ebyterig-serial src/entrypoints/sx1278-serial.cpp)
 target_compile_definitions(sx1278fsk-ebyterig-serial PRIVATE SX1278_IS_FSK)
-sbs_target(sx1278fsk-ebyterig-serial stm32f429zi_skyward_rig)
+sbs_target(sx1278fsk-ebyterig-serial stm32f429zi_rig)
 
 #-----------------------------------------------------------------------------#
 #                                    Tests                                    #
@@ -103,10 +103,10 @@ add_executable(test-rtc src/tests/test-rtc.cpp)
 sbs_target(test-rtc stm32f767zi_compute_unit)
 
 add_executable(test-sensormanager src/tests/test-sensormanager.cpp)
-sbs_target(test-sensormanager stm32f429zi_skyward_death_stack_x)
+sbs_target(test-sensormanager stm32f429zi_death_stack_v2)
 
 add_executable(test-serial src/tests/test-serial.cpp)
-sbs_target(test-serial stm32f756zg_nucleo)
+sbs_target(test-serial stm32f767zi_nucleo)
 
 add_executable(test-taskscheduler src/tests/scheduler/test-taskscheduler.cpp)
 sbs_target(test-taskscheduler stm32f407vg_stm32f4discovery)
@@ -150,8 +150,8 @@ sbs_target(test-hbridge stm32f429zi_stm32f4discovery)
 add_executable(test-servo src/tests/actuators/test-servo.cpp)
 sbs_target(test-servo stm32f429zi_stm32f4discovery)
 
-add_executable(test-buzzer src/tests/actuators/test-buzzer.cpp)
-sbs_target(test-buzzer stm32f429zi_hre_test_stand)
+# add_executable(test-buzzer src/tests/actuators/test-buzzer.cpp)
+# sbs_target(test-buzzer stm32f429zi_hre_test_stand)
 
 add_executable(test-stepper src/tests/actuators/test-stepper.cpp)
 sbs_target(test-stepper stm32f767zi_nucleo)
@@ -167,28 +167,28 @@ add_executable(test-kalman-benchmark src/tests/algorithms/Kalman/test-kalman-ben
 sbs_target(test-kalman-benchmark stm32f429zi_stm32f4discovery)
 
 add_executable(test-attitude-parafoil src/tests/algorithms/NAS/test-attitude-parafoil.cpp)
-sbs_target(test-attitude-parafoil stm32f429zi_skyward_parafoil)
+sbs_target(test-attitude-parafoil stm32f429zi_parafoil)
 
 add_executable(test-attitude-stack src/tests/algorithms/NAS/test-attitude-stack.cpp)
-sbs_target(test-attitude-stack stm32f429zi_skyward_death_stack_x)
+sbs_target(test-attitude-stack stm32f429zi_death_stack_v2)
 
 add_executable(test-nas-parafoil src/tests/algorithms/NAS/test-nas-parafoil.cpp)
-sbs_target(test-nas-parafoil stm32f429zi_skyward_parafoil)
+sbs_target(test-nas-parafoil stm32f429zi_parafoil)
 
 add_executable(test-nas src/tests/algorithms/NAS/test-nas.cpp)
-sbs_target(test-nas stm32f429zi_skyward_death_stack_v3)
+sbs_target(test-nas stm32f429zi_death_stack_v3)
 
 add_executable(test-nas-with-triad src/tests/algorithms/NAS/test-nas-with-triad.cpp)
-sbs_target(test-nas-with-triad stm32f429zi_skyward_death_stack_x)
+sbs_target(test-nas-with-triad stm32f429zi_death_stack_v2)
 
 add_executable(test-triad src/tests/algorithms/NAS/test-triad.cpp)
-sbs_target(test-triad stm32f429zi_skyward_death_stack_v3)
+sbs_target(test-triad stm32f429zi_death_stack_v3)
 
 add_executable(test-triad-parafoil src/tests/algorithms/NAS/test-triad-parafoil.cpp)
-sbs_target(test-triad-parafoil stm32f429zi_skyward_parafoil)
+sbs_target(test-triad-parafoil stm32f429zi_parafoil)
 
 add_executable(test-ada src/tests/algorithms/ADA/test-ada.cpp)
-sbs_target(test-ada stm32f429zi_skyward_death_stack_v3)
+sbs_target(test-ada stm32f429zi_death_stack_v3)
 
 #-----------------------------------------------------------------------------#
 #                               Tests - Boards                                #
@@ -205,19 +205,19 @@ sbs_target(test-qspi-flash stm32f767zi_compute_unit)
 #-----------------------------------------------------------------------------#
 
 add_executable(test-ad5204 src/tests/drivers/test-ad5204.cpp)
-sbs_target(test-ad5204 stm32f205rc_skyward_ciuti)
+sbs_target(test-ad5204 stm32f205rc_ciuti)
 
 add_executable(test-can-2way src/tests/drivers/canbus/CanDriver/test-can-2way.cpp)
-sbs_target(test-can-2way stm32f429zi_skyward_pyxis_auxiliary)
+sbs_target(test-can-2way stm32f429zi_pyxis_auxiliary)
 
 add_executable(test-can-filters src/tests/drivers/canbus/CanDriver/test-can-filters.cpp)
-sbs_target(test-can-filters stm32f429zi_skyward_pyxis_auxiliary)
+sbs_target(test-can-filters stm32f429zi_pyxis_auxiliary)
 
 add_executable(test-can-loopback src/tests/drivers/canbus/CanDriver/test-can-loopback.cpp)
-sbs_target(test-can-loopback stm32f429zi_skyward_death_stack_x)
+sbs_target(test-can-loopback stm32f429zi_death_stack_v2)
 
 add_executable(test-can-protocol src/tests/drivers/canbus/CanProtocol/test-can-protocol.cpp)
-sbs_target(test-can-protocol stm32f429zi_skyward_death_stack_x)
+sbs_target(test-can-protocol stm32f429zi_death_stack_v2)
 
 add_executable(test-dsgamma src/tests/drivers/test-dsgamma.cpp)
 sbs_target(test-dsgamma stm32f429zi_stm32f4discovery)
@@ -250,7 +250,7 @@ add_executable(test-timestamptimer src/tests/drivers/timer/test-timestamptimer.c
 sbs_target(test-timestamptimer stm32f429zi_stm32f4discovery)
 
 add_executable(test-xbee-bidir src/tests/drivers/xbee/test-xbee-bidir.cpp)
-sbs_target(test-xbee-bidir stm32f429zi_skyward_death_stack_x)
+sbs_target(test-xbee-bidir stm32f429zi_death_stack_v2)
 
 add_executable(test-xbee-gui
     src/tests/drivers/xbee/test-xbee-gui.cpp
@@ -297,59 +297,59 @@ sbs_target(test-fsm stm32f429zi_stm32f4discovery)
 #-----------------------------------------------------------------------------#
 
 add_executable(test-sx1278fsk-bidir src/tests/radio/sx1278/fsk/test-sx1278-bidir.cpp)
-sbs_target(test-sx1278fsk-bidir stm32f429zi_skyward_groundstation_v2)
+sbs_target(test-sx1278fsk-bidir stm32f429zi_nokia)
 
 add_executable(test-sx1278fsk-tx src/tests/radio/sx1278/test-sx1278-bench-serial.cpp)
 target_compile_definitions(test-sx1278fsk-tx PRIVATE DISABLE_RX)
-sbs_target(test-sx1278fsk-tx stm32f429zi_skyward_groundstation_v2)
+sbs_target(test-sx1278fsk-tx stm32f429zi_nokia)
 
 add_executable(test-sx1278fsk-rx src/tests/radio/sx1278/test-sx1278-bench-serial.cpp)
 target_compile_definitions(test-sx1278fsk-rx PRIVATE DISABLE_TX)
-sbs_target(test-sx1278fsk-rx stm32f429zi_skyward_groundstation_v2)
+sbs_target(test-sx1278fsk-rx stm32f429zi_nokia)
 
 add_executable(test-sx1278fsk-gui-tx src/tests/radio/sx1278/test-sx1278-bench-gui.cpp)
 target_compile_definitions(test-sx1278fsk-gui-tx PRIVATE DISABLE_RX)
-sbs_target(test-sx1278fsk-gui-tx stm32f429zi_skyward_groundstation_v2)
+sbs_target(test-sx1278fsk-gui-tx stm32f429zi_nokia)
 
 add_executable(test-sx1278fsk-gui-rx src/tests/radio/sx1278/test-sx1278-bench-gui.cpp)
 target_compile_definitions(test-sx1278fsk-gui-rx PRIVATE DISABLE_TX)
-sbs_target(test-sx1278fsk-gui-rx stm32f429zi_skyward_groundstation_v2)
+sbs_target(test-sx1278fsk-gui-rx stm32f429zi_nokia)
 
 add_executable(test-sx1278fsk-mavlink src/tests/radio/sx1278/fsk/test-sx1278-mavlink.cpp)
-sbs_target(test-sx1278fsk-mavlink stm32f429zi_skyward_groundstation_v2)
+sbs_target(test-sx1278fsk-mavlink stm32f429zi_nokia)
 
 # add_executable(test-mavlinkdriver src/tests/radio/test-mavlinkdriver.cpp)
 # sbs_target(test-mavlinkdriver stm32f407vg_stm32f4discovery)
 
 add_executable(test-sx1278lora-bidir src/tests/radio/sx1278/lora/test-sx1278-bidir.cpp)
-sbs_target(test-sx1278lora-bidir stm32f429zi_skyward_groundstation_v2)
+sbs_target(test-sx1278lora-bidir stm32f429zi_nokia)
 
 add_executable(test-sx1278lora-mavlink src/tests/radio/sx1278/lora/test-sx1278-mavlink.cpp)
-sbs_target(test-sx1278lora-mavlink stm32f429zi_skyward_groundstation_v2)
+sbs_target(test-sx1278lora-mavlink stm32f429zi_nokia)
 
 add_executable(test-sx1278lora-simple-rx src/tests/radio/sx1278/lora/test-sx1278-simple.cpp)
 target_compile_definitions(test-sx1278lora-simple-rx PRIVATE ENABLE_RX)
-sbs_target(test-sx1278lora-simple-rx stm32f429zi_skyward_groundstation_v2)
+sbs_target(test-sx1278lora-simple-rx stm32f429zi_nokia)
 
 add_executable(test-sx1278lora-simple-tx src/tests/radio/sx1278/lora/test-sx1278-simple.cpp)
 target_compile_definitions(test-sx1278lora-simple-tx PRIVATE ENABLE_TX)
-sbs_target(test-sx1278lora-simple-tx stm32f429zi_skyward_groundstation_v2)
+sbs_target(test-sx1278lora-simple-tx stm32f429zi_nokia)
 
 add_executable(test-sx1278lora-tx src/tests/radio/sx1278/test-sx1278-bench-serial.cpp)
 target_compile_definitions(test-sx1278lora-tx PRIVATE DISABLE_RX SX1278_IS_LORA)
-sbs_target(test-sx1278lora-tx stm32f429zi_skyward_groundstation_v2)
+sbs_target(test-sx1278lora-tx stm32f429zi_nokia)
 
 add_executable(test-sx1278lora-rx src/tests/radio/sx1278/test-sx1278-bench-serial.cpp)
 target_compile_definitions(test-sx1278lora-rx PRIVATE DISABLE_TX SX1278_IS_LORA)
-sbs_target(test-sx1278lora-rx stm32f429zi_skyward_groundstation_v2)
+sbs_target(test-sx1278lora-rx stm32f429zi_nokia)
 
 add_executable(test-sx1278lora-gui-rx src/tests/radio/sx1278/test-sx1278-bench-gui.cpp)
 target_compile_definitions(test-sx1278lora-gui-rx PRIVATE DISABLE_TX SX1278_IS_LORA)
-sbs_target(test-sx1278lora-gui-rx stm32f429zi_skyward_groundstation_v2)
+sbs_target(test-sx1278lora-gui-rx stm32f429zi_nokia)
 
 add_executable(test-sx1278lora-gui-tx src/tests/radio/sx1278/test-sx1278-bench-gui.cpp)
 target_compile_definitions(test-sx1278lora-gui-tx PRIVATE DISABLE_RX SX1278_IS_LORA)
-sbs_target(test-sx1278lora-gui-tx stm32f429zi_skyward_groundstation_v2)
+sbs_target(test-sx1278lora-gui-tx stm32f429zi_nokia)
 
 #-----------------------------------------------------------------------------#
 #                               Tests - Sensors                               #
@@ -359,7 +359,7 @@ add_executable(test-ads1118 src/tests/sensors/test-ads1118.cpp)
 sbs_target(test-ads1118 stm32f407vg_stm32f4discovery)
 
 add_executable(test-ads131m04 src/tests/sensors/test-ads131m04.cpp)
-sbs_target(test-ads131m04 stm32f429zi_skyward_death_stack_v3)
+sbs_target(test-ads131m04 stm32f429zi_death_stack_v3)
 
 add_executable(test-ads131m08 src/tests/sensors/test-ads131m08.cpp)
 sbs_target(test-ads131m08 stm32f767zi_compute_unit)
@@ -392,10 +392,10 @@ add_executable(test-bmp280-i2c src/tests/sensors/test-bmp280-i2c.cpp)
 sbs_target(test-bmp280-i2c stm32f429zi_stm32f4discovery)
 
 add_executable(test-bmx160 src/tests/sensors/test-bmx160.cpp)
-sbs_target(test-bmx160 stm32f429zi_skyward_death_stack_x)
+sbs_target(test-bmx160 stm32f429zi_death_stack_v2)
 
 add_executable(test-bmx160-with-correction src/tests/sensors/test-bmx160-with-correction.cpp)
-sbs_target(test-bmx160-with-correction stm32f429zi_skyward_death_stack_x)
+sbs_target(test-bmx160-with-correction stm32f429zi_death_stack_v2)
 
 add_executable(test-hx711 src/tests/sensors/test-hx711.cpp)
 sbs_target(test-hx711 stm32f429zi_stm32f4discovery)
@@ -410,10 +410,10 @@ add_executable(test-lis3dsh src/tests/sensors/test-lis3dsh.cpp)
 sbs_target(test-lis3dsh stm32f407vg_stm32f4discovery)
 
 add_executable(test-lis3mdl src/tests/sensors/test-lis3mdl.cpp)
-sbs_target(test-lis3mdl stm32f429zi_skyward_death_stack_x)
+sbs_target(test-lis3mdl stm32f429zi_death_stack_v2)
 
 add_executable(test-lis331hh src/tests/sensors/test-lis331hh.cpp)
-sbs_target(test-lis331hh stm32f205rc_skyward_ciuti)
+sbs_target(test-lis331hh stm32f205rc_ciuti)
 
 add_executable(test-lps331ap src/tests/sensors/test-lps331ap.cpp)
 sbs_target(test-lps331ap stm32f429zi_stm32f4discovery)
@@ -428,19 +428,19 @@ add_executable(test-max31856 src/tests/sensors/test-max31856.cpp)
 sbs_target(test-max31856 stm32f767zi_compute_unit)
 
 add_executable(test-mpu9250 src/tests/sensors/test-mpu9250.cpp)
-sbs_target(test-mpu9250 stm32f429zi_skyward_parafoil)
+sbs_target(test-mpu9250 stm32f429zi_parafoil)
 
 add_executable(test-ms5803-spi src/tests/sensors/test-ms5803-spi.cpp)
-sbs_target(test-ms5803-spi stm32f429zi_skyward_death_stack_x)
+sbs_target(test-ms5803-spi stm32f429zi_death_stack_v2)
 
 add_executable(test-ms5803-i2c src/tests/sensors/test-ms5803-i2c.cpp)
 sbs_target(test-ms5803-i2c stm32f429zi_stm32f4discovery)
 
 add_executable(test-ubxgps-serial src/tests/sensors/test-ubxgps-serial.cpp)
-sbs_target(test-ubxgps-serial stm32f429zi_skyward_death_stack_x)
+sbs_target(test-ubxgps-serial stm32f429zi_death_stack_v2)
 
 add_executable(test-ubxgps-spi src/tests/sensors/test-ubxgps-spi.cpp)
-sbs_target(test-ubxgps-spi stm32f429zi_skyward_death_stack_x)
+sbs_target(test-ubxgps-spi stm32f429zi_death_stack_v2)
 
 add_executable(test-vn100 src/tests/sensors/test-vn100.cpp)
 sbs_target(test-vn100 stm32f407vg_stm32f4discovery)
diff --git a/cmake/boardcore-host.cmake b/cmake/boardcore-host.cmake
index 977102ebb..f99e104f0 100644
--- a/cmake/boardcore-host.cmake
+++ b/cmake/boardcore-host.cmake
@@ -19,7 +19,8 @@
 # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 # THE SOFTWARE.
 
-add_library(boardcore-host STATIC EXCLUDE_FROM_ALL
+# Boardcore source files used when compiling for host
+set(BOARDCORE_HOST_SRC
     # Debug
     ${SBS_BASE}/src/shared/utils/Debug.cpp
     ${SBS_BASE}/src/shared/diagnostic/CpuMeter/CpuMeter.cpp
@@ -58,8 +59,14 @@ add_library(boardcore-host STATIC EXCLUDE_FROM_ALL
     ${SBS_BASE}/src/shared/utils/Stats/Stats.cpp
     ${SBS_BASE}/src/shared/utils/TestUtils/TestHelper.cpp
 )
-add_library(SkywardBoardcore::Boardcore::host ALIAS boardcore-host)
-target_include_directories(boardcore-host PUBLIC ${SBS_BASE}/src/shared)
+
+# Create a library specific for host builds
+add_library(boardcore-host STATIC EXCLUDE_FROM_ALL ${BOARDCORE_HOST_SRC})
+
+# Only one include directory for Boardcore!
+target_include_directories(boardcore-host PUBLIC ${BOARDCORE_PATH}/src/shared)
+
+# Link libraries
 target_link_libraries(boardcore-host PUBLIC
     Miosix::Miosix::host
     TSCPP::TSCPP
@@ -68,3 +75,6 @@ target_link_libraries(boardcore-host PUBLIC
     Catch2::Catch2
     Mavlink::Mavlink
 )
+
+# Create a nice alias for the library
+add_library(Skyward::Boardcore::host ALIAS boardcore-host)
\ No newline at end of file
diff --git a/cmake/boardcore.cmake b/cmake/boardcore.cmake
index 49c749382..b45ff0b53 100644
--- a/cmake/boardcore.cmake
+++ b/cmake/boardcore.cmake
@@ -19,123 +19,141 @@
 # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 # THE SOFTWARE.
 
-include(dependencies)
-
-include(boardcore-host)
-
-foreach(OPT_BOARD ${BOARDS})
-    set(BOARDCORE_LIBRARY boardcore-${OPT_BOARD})
-    add_library(${BOARDCORE_LIBRARY} STATIC EXCLUDE_FROM_ALL
-
-        # Actuators
-        ${SBS_BASE}/src/shared/actuators/HBridge/HBridge.cpp
-        ${SBS_BASE}/src/shared/actuators/Servo/Servo.cpp
-        ${SBS_BASE}/src/shared/actuators/stepper/Stepper.cpp
-        ${SBS_BASE}/src/shared/actuators/stepper/StepperPWM.cpp
-
-        # Algorithms
-        ${SBS_BASE}/src/shared/algorithms/ADA/ADA.cpp
-        ${SBS_BASE}/src/shared/algorithms/MEA/MEA.cpp
-        ${SBS_BASE}/src/shared/algorithms/AirBrakes/AirBrakes.cpp
-        ${SBS_BASE}/src/shared/algorithms/AirBrakes/AirBrakesPI.cpp
-        ${SBS_BASE}/src/shared/algorithms/AirBrakes/AirBrakesInterp.cpp
-        ${SBS_BASE}/src/shared/algorithms/NAS/NAS.cpp
-        ${SBS_BASE}/src/shared/algorithms/NAS/StateInitializer.cpp
-
-        # Debug
-        ${SBS_BASE}/src/shared/utils/Debug.cpp
-        ${SBS_BASE}/src/shared/diagnostic/CpuMeter/CpuMeter.cpp
-        ${SBS_BASE}/src/shared/diagnostic/PrintLogger.cpp
-
-        # Drivers
-        ${SBS_BASE}/src/shared/drivers/AD5204/AD5204.cpp
-        ${SBS_BASE}/src/shared/drivers/adc/InternalADC.cpp
-        ${SBS_BASE}/src/shared/drivers/canbus/CanDriver/CanDriver.cpp
-        ${SBS_BASE}/src/shared/drivers/canbus/CanDriver/CanInterrupt.cpp
-        ${SBS_BASE}/src/shared/drivers/canbus/CanProtocol/CanProtocol.cpp
-        ${SBS_BASE}/src/shared/drivers/interrupt/external_interrupts.cpp
-        ${SBS_BASE}/src/shared/drivers/timer/PWM.cpp
-        ${SBS_BASE}/src/shared/drivers/timer/CountedPWM.cpp
-        ${SBS_BASE}/src/shared/drivers/timer/TimestampTimer.cpp
-        ${SBS_BASE}/src/shared/drivers/runcam/Runcam.cpp
-        ${SBS_BASE}/src/shared/drivers/spi/SPITransaction.cpp
-        ${SBS_BASE}/src/shared/drivers/usart/USART.cpp
-        ${SBS_BASE}/src/shared/drivers/i2c/I2CDriver-f4.cpp
-        ${SBS_BASE}/src/shared/drivers/i2c/I2CDriver-f7.cpp
-        ${SBS_BASE}/src/shared/drivers/i2c/I2C.cpp
-        ${SBS_BASE}/src/shared/drivers/WIZ5500/WIZ5500.cpp
-
-        # Events
-        ${SBS_BASE}/src/shared/events/EventBroker.cpp
-
-        # Logger
-        ${SBS_BASE}/src/shared/logger/Logger.cpp
-
-        # Radio
-        ${SBS_BASE}/src/shared/radio/gamma868/Gamma868.cpp
-        ${SBS_BASE}/src/shared/radio/Xbee/APIFrameParser.cpp
-        ${SBS_BASE}/src/shared/radio/Xbee/Xbee.cpp
-        ${SBS_BASE}/src/shared/radio/SX1278/SX1278Fsk.cpp
-        ${SBS_BASE}/src/shared/radio/SX1278/SX1278Lora.cpp
-        ${SBS_BASE}/src/shared/radio/SX1278/SX1278Common.cpp
-
-        # Scheduler
-        ${SBS_BASE}/src/shared/scheduler/TaskScheduler.cpp
-
-        # Sensors
-        ${SBS_BASE}/src/shared/sensors/ADS1118/ADS1118.cpp
-        ${SBS_BASE}/src/shared/sensors/ADS131M04/ADS131M04.cpp
-        ${SBS_BASE}/src/shared/sensors/ADS131M08/ADS131M08.cpp
-        ${SBS_BASE}/src/shared/sensors/BME280/BME280.cpp
-        ${SBS_BASE}/src/shared/sensors/BME280/BME280I2C.cpp
-        ${SBS_BASE}/src/shared/sensors/BMP280/BMP280.cpp
-        ${SBS_BASE}/src/shared/sensors/BMP280/BMP280I2C.cpp
-        ${SBS_BASE}/src/shared/sensors/BMX160/BMX160.cpp
-        ${SBS_BASE}/src/shared/sensors/BMX160/BMX160WithCorrection.cpp
-        ${SBS_BASE}/src/shared/sensors/H3LIS331DL/H3LIS331DL.cpp
-        ${SBS_BASE}/src/shared/sensors/HX711/HX711.cpp
-        ${SBS_BASE}/src/shared/sensors/LIS3MDL/LIS3MDL.cpp
-        ${SBS_BASE}/src/shared/sensors/LIS331HH/LIS331HH.cpp
-        ${SBS_BASE}/src/shared/sensors/LPS331AP/LPS331AP.cpp
-        ${SBS_BASE}/src/shared/sensors/MAX6675/MAX6675.cpp
-        ${SBS_BASE}/src/shared/sensors/MAX31855/MAX31855.cpp
-        ${SBS_BASE}/src/shared/sensors/MAX31856/MAX31856.cpp
-        ${SBS_BASE}/src/shared/sensors/MBLoadCell/MBLoadCell.cpp
-        ${SBS_BASE}/src/shared/sensors/MPU9250/MPU9250.cpp
-        ${SBS_BASE}/src/shared/sensors/MS5803/MS5803.cpp
-        ${SBS_BASE}/src/shared/sensors/MS5803/MS5803I2C.cpp
-        ${SBS_BASE}/src/shared/sensors/SensorManager.cpp
-        ${SBS_BASE}/src/shared/sensors/SensorSampler.cpp
-        ${SBS_BASE}/src/shared/sensors/UBXGPS/UBXGPSSerial.cpp
-        ${SBS_BASE}/src/shared/sensors/UBXGPS/UBXGPSSpi.cpp
-        ${SBS_BASE}/src/shared/sensors/VN100/VN100.cpp
-        ${SBS_BASE}/src/shared/sensors/LIS2MDL/LIS2MDL.cpp
-        ${SBS_BASE}/src/shared/sensors/LPS28DFW/LPS28DFW.cpp
-        ${SBS_BASE}/src/shared/sensors/LPS22DF/LPS22DF.cpp
-        ${SBS_BASE}/src/shared/sensors/LSM6DSRX/LSM6DSRX.cpp
-
-        # Calibration
-        ${SBS_BASE}/src/shared/sensors/calibration/BiasCalibration/BiasCalibration.cpp
-        ${SBS_BASE}/src/shared/sensors/calibration/SensorDataExtra/SensorDataExtra.cpp
-        ${SBS_BASE}/src/shared/sensors/calibration/SixParameterCalibration/SixParameterCalibration.cpp
-        ${SBS_BASE}/src/shared/sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.cpp
-
-        # Correction
-        ${SBS_BASE}/src/shared/sensors/correction/BiasCorrector/BiasCorrector.cpp
-        ${SBS_BASE}/src/shared/sensors/correction/SixParametersCorrector/SixParametersCorrector.cpp
-
-        # Utils
-        ${SBS_BASE}/src/shared/utils/AeroUtils/AeroUtils.cpp
-        ${SBS_BASE}/src/shared/utils/ButtonHandler/ButtonHandler.cpp
-        ${SBS_BASE}/src/shared/utils/PinObserver/PinObserver.cpp
-        ${SBS_BASE}/src/shared/utils/SkyQuaternion/SkyQuaternion.cpp
-        ${SBS_BASE}/src/shared/utils/Stats/Stats.cpp
-        ${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}
+# Load in BOARDCORE_PATH the project path
+cmake_path(GET CMAKE_CURRENT_LIST_DIR PARENT_PATH BOARDCORE_PATH)
+
+# Include dependencies and board list
+include(${BOARDCORE_PATH}/cmake/dependencies.cmake)
+include(${BOARDCORE_PATH}/cmake/boardcore-host.cmake)
+include(${BOARDCORE_PATH}/cmake/boards.cmake)
+
+# Boardcore source files
+set(BOARDCORE_SRC
+    # Actuators
+    ${BOARDCORE_PATH}/src/shared/actuators/HBridge/HBridge.cpp
+    ${BOARDCORE_PATH}/src/shared/actuators/Servo/Servo.cpp
+    ${BOARDCORE_PATH}/src/shared/actuators/stepper/Stepper.cpp
+    ${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/AirBrakes/AirBrakes.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
+    ${BOARDCORE_PATH}/src/shared/algorithms/NAS/StateInitializer.cpp
+
+    # Debug
+    ${BOARDCORE_PATH}/src/shared/utils/Debug.cpp
+    ${BOARDCORE_PATH}/src/shared/diagnostic/CpuMeter/CpuMeter.cpp
+    ${BOARDCORE_PATH}/src/shared/diagnostic/PrintLogger.cpp
+
+    # Drivers
+    ${BOARDCORE_PATH}/src/shared/drivers/AD5204/AD5204.cpp
+    ${BOARDCORE_PATH}/src/shared/drivers/adc/InternalADC.cpp
+    ${BOARDCORE_PATH}/src/shared/drivers/canbus/CanDriver/CanDriver.cpp
+    ${BOARDCORE_PATH}/src/shared/drivers/canbus/CanDriver/CanInterrupt.cpp
+    ${BOARDCORE_PATH}/src/shared/drivers/canbus/CanProtocol/CanProtocol.cpp
+    ${BOARDCORE_PATH}/src/shared/drivers/interrupt/external_interrupts.cpp
+    ${BOARDCORE_PATH}/src/shared/drivers/timer/PWM.cpp
+    ${BOARDCORE_PATH}/src/shared/drivers/timer/CountedPWM.cpp
+    ${BOARDCORE_PATH}/src/shared/drivers/timer/TimestampTimer.cpp
+    ${BOARDCORE_PATH}/src/shared/drivers/runcam/Runcam.cpp
+    ${BOARDCORE_PATH}/src/shared/drivers/spi/SPITransaction.cpp
+    ${BOARDCORE_PATH}/src/shared/drivers/usart/USART.cpp
+    ${BOARDCORE_PATH}/src/shared/drivers/i2c/I2CDriver-f4.cpp
+    ${BOARDCORE_PATH}/src/shared/drivers/i2c/I2CDriver-f7.cpp
+    ${BOARDCORE_PATH}/src/shared/drivers/i2c/I2C.cpp
+    ${BOARDCORE_PATH}/src/shared/drivers/WIZ5500/WIZ5500.cpp
+
+    # Events
+    ${BOARDCORE_PATH}/src/shared/events/EventBroker.cpp
+
+    # Logger
+    ${BOARDCORE_PATH}/src/shared/logger/Logger.cpp
+
+    # Radio
+    ${BOARDCORE_PATH}/src/shared/radio/gamma868/Gamma868.cpp
+    ${BOARDCORE_PATH}/src/shared/radio/Xbee/APIFrameParser.cpp
+    ${BOARDCORE_PATH}/src/shared/radio/Xbee/Xbee.cpp
+    ${BOARDCORE_PATH}/src/shared/radio/SX1278/SX1278Fsk.cpp
+    ${BOARDCORE_PATH}/src/shared/radio/SX1278/SX1278Lora.cpp
+    ${BOARDCORE_PATH}/src/shared/radio/SX1278/SX1278Common.cpp
+
+    # Scheduler
+    ${BOARDCORE_PATH}/src/shared/scheduler/TaskScheduler.cpp
+
+    # Sensors
+    ${BOARDCORE_PATH}/src/shared/sensors/ADS1118/ADS1118.cpp
+    ${BOARDCORE_PATH}/src/shared/sensors/ADS131M04/ADS131M04.cpp
+    ${BOARDCORE_PATH}/src/shared/sensors/ADS131M08/ADS131M08.cpp
+    ${BOARDCORE_PATH}/src/shared/sensors/BME280/BME280.cpp
+    ${BOARDCORE_PATH}/src/shared/sensors/BME280/BME280I2C.cpp
+    ${BOARDCORE_PATH}/src/shared/sensors/BMP280/BMP280.cpp
+    ${BOARDCORE_PATH}/src/shared/sensors/BMP280/BMP280I2C.cpp
+    ${BOARDCORE_PATH}/src/shared/sensors/BMX160/BMX160.cpp
+    ${BOARDCORE_PATH}/src/shared/sensors/BMX160/BMX160WithCorrection.cpp
+    ${BOARDCORE_PATH}/src/shared/sensors/H3LIS331DL/H3LIS331DL.cpp
+    ${BOARDCORE_PATH}/src/shared/sensors/HX711/HX711.cpp
+    ${BOARDCORE_PATH}/src/shared/sensors/LIS3MDL/LIS3MDL.cpp
+    ${BOARDCORE_PATH}/src/shared/sensors/LIS331HH/LIS331HH.cpp
+    ${BOARDCORE_PATH}/src/shared/sensors/LPS331AP/LPS331AP.cpp
+    ${BOARDCORE_PATH}/src/shared/sensors/MAX6675/MAX6675.cpp
+    ${BOARDCORE_PATH}/src/shared/sensors/MAX31855/MAX31855.cpp
+    ${BOARDCORE_PATH}/src/shared/sensors/MAX31856/MAX31856.cpp
+    ${BOARDCORE_PATH}/src/shared/sensors/MBLoadCell/MBLoadCell.cpp
+    ${BOARDCORE_PATH}/src/shared/sensors/MPU9250/MPU9250.cpp
+    ${BOARDCORE_PATH}/src/shared/sensors/MS5803/MS5803.cpp
+    ${BOARDCORE_PATH}/src/shared/sensors/MS5803/MS5803I2C.cpp
+    ${BOARDCORE_PATH}/src/shared/sensors/SensorManager.cpp
+    ${BOARDCORE_PATH}/src/shared/sensors/SensorSampler.cpp
+    ${BOARDCORE_PATH}/src/shared/sensors/UBXGPS/UBXGPSSerial.cpp
+    ${BOARDCORE_PATH}/src/shared/sensors/UBXGPS/UBXGPSSpi.cpp
+    ${BOARDCORE_PATH}/src/shared/sensors/VN100/VN100.cpp
+    ${BOARDCORE_PATH}/src/shared/sensors/LIS2MDL/LIS2MDL.cpp
+    ${BOARDCORE_PATH}/src/shared/sensors/LPS28DFW/LPS28DFW.cpp
+    ${BOARDCORE_PATH}/src/shared/sensors/LPS22DF/LPS22DF.cpp
+    ${BOARDCORE_PATH}/src/shared/sensors/LSM6DSRX/LSM6DSRX.cpp
+
+    # Calibration
+    ${BOARDCORE_PATH}/src/shared/sensors/calibration/BiasCalibration/BiasCalibration.cpp
+    ${BOARDCORE_PATH}/src/shared/sensors/calibration/SensorDataExtra/SensorDataExtra.cpp
+    ${BOARDCORE_PATH}/src/shared/sensors/calibration/SixParameterCalibration/SixParameterCalibration.cpp
+    ${BOARDCORE_PATH}/src/shared/sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.cpp
+
+    # Correction
+    ${BOARDCORE_PATH}/src/shared/sensors/correction/BiasCorrector/BiasCorrector.cpp
+    ${BOARDCORE_PATH}/src/shared/sensors/correction/SixParametersCorrector/SixParametersCorrector.cpp
+
+    # Utils
+    ${BOARDCORE_PATH}/src/shared/utils/AeroUtils/AeroUtils.cpp
+    ${BOARDCORE_PATH}/src/shared/utils/ButtonHandler/ButtonHandler.cpp
+    ${BOARDCORE_PATH}/src/shared/utils/PinObserver/PinObserver.cpp
+    ${BOARDCORE_PATH}/src/shared/utils/SkyQuaternion/SkyQuaternion.cpp
+    ${BOARDCORE_PATH}/src/shared/utils/Stats/Stats.cpp
+    ${BOARDCORE_PATH}/src/shared/utils/TestUtils/TestHelper.cpp
+)
+
+# Creates the Skyward::Boardcore::${BOARD_NAME} library
+function(add_boardcore_library BOARD_OPTIONS_FILE)
+    # Get board options
+    include(${BOARD_OPTIONS_FILE})
+
+    # Create a library for the board
+    set(BOARDCORE_LIB boardcore-${BOARD_NAME})
+    add_library(${BOARDCORE_LIB} STATIC EXCLUDE_FROM_ALL ${BOARDCORE_SRC})
+
+    # Only one include directory for Boardcore!
+    target_include_directories(${BOARDCORE_LIB} PUBLIC ${BOARDCORE_PATH}/src/shared)
+
+    # Define DEBUG when in Debug mode
+    target_compile_definitions(${BOARDCORE_LIB} PUBLIC $<$<CONFIG:Debug>:DEBUG>) 
+
+    # Link libraries
+    target_link_libraries(${BOARDCORE_LIB} PUBLIC
+        $<TARGET_OBJECTS:Miosix::Boot::${BOARD_NAME}>
+        $<LINK_GROUP:RESCAN,Miosix::Kernel::${BOARD_NAME},stdc++,c,m,gcc,atomic>
         TSCPP::TSCPP
         Eigen3::Eigen
         fmt::fmt-header-only
@@ -143,8 +161,23 @@ foreach(OPT_BOARD ${BOARDS})
         Mavlink::Mavlink
     )
 
-    # Link MxGui only if supported by the target
-    if(${OPT_BOARD} IN_LIST MXGUI_BOARDS)
-        target_link_libraries(${BOARDCORE_LIBRARY} PUBLIC Mxgui::Mxgui::${OPT_BOARD})
+    # Link MxGui if supported by the target
+    if(DEFINED MXGUI_BASE_BOARD_NAME)
+        target_link_libraries(${BOARDCORE_LIB} PUBLIC MxGui::${MXGUI_BASE_BOARD_NAME})
+    elseif(TARGET MxGui::${BOARD_NAME})
+        target_link_libraries(${BOARDCORE_LIB} PUBLIC MxGui::${BOARD_NAME})
     endif()
+
+    # Create a nice alias for the library
+    add_library(Skyward::Boardcore::${BOARD_NAME} ALIAS ${BOARDCORE_LIB})
+endfunction()
+
+# Create the Miosix libraries for Boardcore custom boards
+foreach(BOARD_OPTIONS_FILE ${BOARDCORE_BOARDS_OPTIONS_FILES})
+    add_miosix_libraries(${BOARD_OPTIONS_FILE})
+endforeach()
+
+# Create Boardcore library for each board
+foreach(BOARD_OPTIONS_FILE ${MIOSIX_BOARDS_OPTIONS_FILES} ${BOARDCORE_BOARDS_OPTIONS_FILES})
+    add_boardcore_library(${BOARD_OPTIONS_FILE})
 endforeach()
diff --git a/cmake/boards.cmake b/cmake/boards.cmake
new file mode 100644
index 000000000..aaf6074cb
--- /dev/null
+++ b/cmake/boards.cmake
@@ -0,0 +1,37 @@
+# Copyright (c) 2023 Skyward Experimental Rocketry
+# Authors: Alberto Nidasio
+#
+# 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.
+
+set(BOARDCORE_BOARDS_OPTIONS_FILES
+    ${BOARDCORE_PATH}/src/bsps/stm32f205rc_ciuti/config/board_options.cmake
+    ${BOARDCORE_PATH}/src/bsps/stm32f429zi_nokia/config/board_options.cmake
+    ${BOARDCORE_PATH}/src/bsps/stm32f429zi_parafoil/config/board_options.cmake
+    ${BOARDCORE_PATH}/src/bsps/stm32f429zi_death_stack_v1/config/board_options.cmake
+    ${BOARDCORE_PATH}/src/bsps/stm32f429zi_death_stack_v2/config/board_options.cmake
+    ${BOARDCORE_PATH}/src/bsps/stm32f429zi_death_stack_v3/config/board_options.cmake
+    ${BOARDCORE_PATH}/src/bsps/stm32f429zi_pyxis_auxiliary/config/board_options.cmake
+    ${BOARDCORE_PATH}/src/bsps/stm32f429zi_rig/config/board_options.cmake
+    ${BOARDCORE_PATH}/src/bsps/stm32f756zg_nucleo/config/board_options.cmake
+    ${BOARDCORE_PATH}/src/bsps/stm32f767zi_automated_antennas/config/board_options.cmake
+    ${BOARDCORE_PATH}/src/bsps/stm32f767zi_compute_unit/config/board_options.cmake
+    ${BOARDCORE_PATH}/src/bsps/stm32f767zi_gemini_gs/config/board_options.cmake
+    ${BOARDCORE_PATH}/src/bsps/stm32f767zi_gemini_motor/config/board_options.cmake
+    ${BOARDCORE_PATH}/src/bsps/stm32f767zi_death_stack_v4/config/board_options.cmake
+)
diff --git a/cmake/dependencies.cmake b/cmake/dependencies.cmake
index 7a4f691a0..f0a5db058 100644
--- a/cmake/dependencies.cmake
+++ b/cmake/dependencies.cmake
@@ -19,33 +19,34 @@
 # 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)
+# Miosix Kernel
+include(${SBS_BASE}/libs/miosix-kernel/miosix/cmake/miosix.cmake EXCLUDE_FROM_ALL)
 
+# Miosix Host
 add_subdirectory(${SBS_BASE}/libs/miosix-host EXCLUDE_FROM_ALL)
 
-set(KPATH ${KPATH} CACHE PATH "Path to kernel directory")
-add_subdirectory(${SBS_BASE}/libs/mxgui EXCLUDE_FROM_ALL)
-include(${SBS_BASE}/libs/mxgui/cmake/boards.cmake)
+# MxGui graphical library
+include(${SBS_BASE}/libs/mxgui/cmake/mxgui.cmake)
 
+# Serialization library
 add_subdirectory(${SBS_BASE}/libs/tscpp EXCLUDE_FROM_ALL)
 
+# Eigen library
 set(CMAKE_BUILD_WITH_INSTALL_RPATH ON)
 set(EIGEN_TEST_NOQT ON CACHE BOOL "Disable Qt support in unit tests")
 set(CMAKE_Fortran_COMPILER NOTFOUND)
 add_subdirectory(${SBS_BASE}/libs/eigen EXCLUDE_FROM_ALL)
 target_compile_definitions(eigen INTERFACE EIGEN_MAX_ALIGN_BYTES=0)
 
+# Format library
 add_subdirectory(${SBS_BASE}/libs/fmt EXCLUDE_FROM_ALL)
 target_compile_definitions(fmt-header-only INTERFACE _GLIBCXX_USE_WCHAR_T FMT_UNICODE=0 FMT_STATIC_THOUSANDS_SEPARATOR=0)
 target_compile_options(fmt-header-only INTERFACE -fno-math-errno)
 
+# Catch2 library
 add_subdirectory(${SBS_BASE}/libs/Catch2 EXCLUDE_FROM_ALL)
 list(APPEND CMAKE_MODULE_PATH ${SBS_BASE}/libs/Catch2/contrib)
 include(Catch)
 
+# MavLink library
 add_subdirectory(${SBS_BASE}/libs/mavlink-skyward-lib EXCLUDE_FROM_ALL)
diff --git a/cmake/sbs.cmake b/cmake/sbs.cmake
index e3d206573..7cd5a66c2 100644
--- a/cmake/sbs.cmake
+++ b/cmake/sbs.cmake
@@ -21,19 +21,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)
-
-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()
+# Load in SBS_BASE the project path
+cmake_path(GET CMAKE_CURRENT_LIST_DIR PARENT_PATH SBS_BASE)
 
+# Include the Boardcore libraries
+list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR})
 include(boardcore)
 
-string(REPLACE ";" "\\n" BOARDS_STR "${BOARDS}")
+# Command to print all the available boards used by the sbs script
+string(REPLACE ";" "\\n" BOARDS_STR "${MIOSIX_BOARDS};${BOARDCORE_BOARDS}")
 add_custom_target(
     help-boards
     COMMAND printf ${BOARDS_STR}
@@ -41,13 +37,22 @@ add_custom_target(
     VERBATIM
 )
 
+# Function to link the Boardcore library to the target
 function(sbs_target TARGET OPT_BOARD)
     if(NOT OPT_BOARD)
         message(FATAL_ERROR "No board selected")
     endif()
+
+    # The only include directory of Boardcore is shared!
     target_include_directories(${TARGET} PRIVATE src/shared)
+
     if(CMAKE_CROSSCOMPILING)
-        target_link_libraries(${TARGET} PRIVATE SkywardBoardcore::Boardcore::${OPT_BOARD})
+        # Link the embedded Boardcore library
+        target_link_libraries(${TARGET} PRIVATE Skyward::Boardcore::${OPT_BOARD})
+
+        # Linker script and linking options are eredited from the kernel library
+
+        # Add a post build command to create the hex file to flash on the board
         add_custom_command(
             TARGET ${TARGET} POST_BUILD
             COMMAND ${CMAKE_OBJCOPY} -O ihex ${TARGET} ${TARGET}.hex
@@ -56,7 +61,7 @@ function(sbs_target TARGET OPT_BOARD)
             VERBATIM
         )
     else()
-        target_link_libraries(${TARGET} PRIVATE SkywardBoardcore::Boardcore::host)
+        target_link_libraries(${TARGET} PRIVATE Skyward::Boardcore::host)
     endif()
 endfunction()
 
diff --git a/libs/miosix-host b/libs/miosix-host
index ed8a4d8c2..5b09ee538 160000
--- a/libs/miosix-host
+++ b/libs/miosix-host
@@ -1 +1 @@
-Subproject commit ed8a4d8c24e59ce7513b1cebc9b6427fde55c7ce
+Subproject commit 5b09ee5386e53b9c0e350603a9f0994934e4deea
diff --git a/libs/miosix-kernel b/libs/miosix-kernel
index fb3e5b7c8..331b5d5c0 160000
--- a/libs/miosix-kernel
+++ b/libs/miosix-kernel
@@ -1 +1 @@
-Subproject commit fb3e5b7c8047ea08b111c9beab94745556e0f8d3
+Subproject commit 331b5d5c0b54e05ed2fdd6b59eeb6037123f567c
diff --git a/libs/mxgui b/libs/mxgui
index 85c4cfb0c..e300a6c25 160000
--- a/libs/mxgui
+++ b/libs/mxgui
@@ -1 +1 @@
-Subproject commit 85c4cfb0cddab47ceb14ed158559ffd4fa7af288
+Subproject commit e300a6c2587d29f4795b67ee0eaebf18aeebb5cb
diff --git a/sbs b/sbs
index 5cecc767b..f867e9960 100755
--- a/sbs
+++ b/sbs
@@ -47,7 +47,7 @@ init_dirs() {
     source_dir="$PWD"
     build_default_dir="$source_dir/$BUILD_DEFAULT_DIRNAME"
     build_host_dir="$source_dir/$BUILD_HOST_DIRNAME"
-    toolchain_file="$sbs_base/libs/miosix-kernel/miosix/_tools/toolchain.cmake"
+    toolchain_file="$sbs_base/libs/miosix-kernel/miosix/cmake/toolchain.cmake"
 }
 
 find_deps() {
diff --git a/src/bsps/stm32f205rc_ciuti/config/board_options.cmake b/src/bsps/stm32f205rc_ciuti/config/board_options.cmake
new file mode 100644
index 000000000..2c0856354
--- /dev/null
+++ b/src/bsps/stm32f205rc_ciuti/config/board_options.cmake
@@ -0,0 +1,99 @@
+# Copyright (C) 2023 by Skyward
+#
+# This program is free software; you can redistribute it and/or 
+# it under the terms of the GNU General Public License as published 
+# the Free Software Foundation; either version 2 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.
+#
+# As a special exception, if other files instantiate templates or use
+# macros or inline functions from this file, or you compile this file
+# and link it with other works to produce a work based on this file,
+# this file does not by itself cause the resulting work to be covered
+# by the GNU General Public License. However the source code for this
+# file must still be made available in accordance with the GNU 
+# Public License. This exception does not invalidate any other 
+# why a work based on this file might be covered by the GNU General
+# Public License.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, see <http://www.gnu.org/licenses/>
+
+set(BOARD_NAME stm32f205rc_ciuti)
+set(ARCH_NAME cortexM3_stm32f2)
+
+# Base directories with header files for this board
+set(ARCH_PATH ${KPATH}/arch/${ARCH_NAME}/common)
+set(BOARD_PATH ${BOARDCORE_PATH}/src/bsps/${BOARD_NAME})
+set(BOARD_CONFIG_PATH ${BOARDCORE_PATH}/src/bsps/${BOARD_NAME}/config)
+
+# Specify where to find the board specific config/miosix_settings.h
+set(BOARD_MIOSIX_SETTINGS_PATH ${BOARD_PATH})
+
+# Specify where to find the board specific config/mxgui_settings.h
+set(BOARD_MXGUI_SETTINGS_PATH ${BOARD_PATH})
+
+# Optimization flags:
+# -O0 do no optimization, the default if no optimization level is specified
+# -O or -O1 optimize minimally
+# -O2 optimize more
+# -O3 optimize even more
+# -Ofast optimize very aggressively to the point of breaking the standard
+# -Og Optimize debugging experience, enables optimizations that do not
+# interfere with debugging
+# -Os Optimize for size with -O2 optimizations that do not increase code size
+set(OPT_OPTIMIZATION -O2)
+
+# Boot file and linker script
+set(BOOT_FILE ${BOARD_PATH}/core/stage_1_boot.cpp)
+set(LINKER_SCRIPT ${BOARD_PATH}/stm32_512k+128k_rom.ld)
+
+# Clock frequency
+set(CLOCK_FREQ -DHSE_VALUE=25000000 -DSYSCLK_FREQ_120MHz=120000000)
+
+# C++ Exception/rtti support disable flags.
+# To save code size if not using C++ exceptions (nor some STL code which
+# implicitly uses it) uncomment this option.
+# -D__NO_EXCEPTIONS is used by Miosix to know if exceptions are used.
+# set(OPT_EXCEPT -fno-exceptions -fno-rtti -D__NO_EXCEPTIONS)
+
+# Specify a custom flash command
+# This is the program that is invoked when the flash flag (-f or --flash) is
+# used with the Miosix Build System. Use $binary or $hex as placeolders, they
+# will be replaced by the build systems with the binary or hex file repectively.
+# If a command is not specified, the build system will use st-flash if found
+# set(PROGRAM_CMDLINE "here your custom flash command")
+
+# Basic flags
+set(FLAGS_BASE -mcpu=cortex-m3 -mthumb)
+
+# Flags for ASM and linker
+set(AFLAGS_BASE ${FLAGS_BASE})
+set(LFLAGS_BASE ${FLAGS_BASE} -Wl,--gc-sections,-Map,main.map -Wl,-T${LINKER_SCRIPT} ${OPT_EXCEPT} ${OPT_OPTIMIZATION} -nostdlib)
+
+# Flags for C/C++
+set(CFLAGS_BASE
+    -D_BOARD_STM32F205_GENERIC "-D_MIOSIX_BOARDNAME=\"${BOARD_NAME}\""
+    -D_DEFAULT_SOURCE=1 -ffunction-sections -Wall -Werror=return-type -g
+    -D_ARCH_CORTEXM3_STM32F2
+    ${CLOCK_FREQ} ${XRAM} ${FLAGS_BASE} ${OPT_OPTIMIZATION} -c
+)
+set(CXXFLAGS_BASE ${CFLAGS_BASE} ${OPT_EXCEPT})
+
+# Select architecture specific files
+set(ARCH_SRC
+    ${ARCH_PATH}/interfaces-impl/gpio_impl.cpp
+    ${ARCH_PATH}/interfaces-impl/portability.cpp
+    ${BOARD_PATH}/interfaces-impl/bsp.cpp
+    ${BOARD_PATH}/interfaces-impl/delays.cpp
+    ${KPATH}/arch/common/CMSIS/Device/ST/STM32F2xx/Source/Templates/system_stm32f2xx.c
+    ${KPATH}/arch/common/core/interrupts_cortexMx.cpp
+    ${KPATH}/arch/common/core/mpu_cortexMx.cpp
+    ${KPATH}/arch/common/core/stm32f2_f4_l4_f7_h7_os_timer.cpp
+    ${KPATH}/arch/common/drivers/serial_stm32.cpp
+    ${KPATH}/arch/common/drivers/sd_stm32f2_f4_f7.cpp
+)
diff --git a/src/bsps/stm32f205rc_ciuti/config/board_settings.h b/src/bsps/stm32f205rc_ciuti/config/board_settings.h
new file mode 100644
index 000000000..d53b41328
--- /dev/null
+++ b/src/bsps/stm32f205rc_ciuti/config/board_settings.h
@@ -0,0 +1,68 @@
+/* Copyright (c) 2021 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * 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.
+ */
+
+#ifndef BOARD_SETTINGS_H
+#define BOARD_SETTINGS_H
+
+#include "util/version.h"
+
+/**
+ * \internal
+ * Versioning for board_settings.h for out of git tree projects
+ */
+#define BOARD_SETTINGS_VERSION 300
+
+namespace miosix
+{
+
+/**
+ * \addtogroup Settings
+ * \{
+ */
+
+/// Size of stack for main().
+/// The C standard library is stack-heavy (iprintf requires 1.5KB) and the
+/// STM32F205RC has 128KB of RAM so there is room for a big 4K stack.
+const unsigned int MAIN_STACK_SIZE = 4 * 1024;
+
+/// Serial port
+const unsigned int defaultSerial      = 1;
+const unsigned int defaultSerialSpeed = 115200;
+const bool defaultSerialFlowctrl      = false;
+#define SERIAL_1_DMA
+// #define SERIAL_2_DMA //Serial 2 is used by the pogo pins
+// #define SERIAL_3_DMA  //Serial 3 is used by the pogo pins
+
+// SD card driver
+static const unsigned char sdVoltage = 33;  // Board powered @ 3.3V
+// #define SD_ONE_BIT_DATABUS // This board supports 4 bit data bus to SD card
+
+/// Analog supply voltage for ADC, DAC, Reset blocks, RCs and PLL
+#define V_DDA_VOLTAGE 3.3f
+
+/**
+ * \}
+ */
+
+}  // namespace miosix
+
+#endif /* BOARD_SETTINGS_H */
diff --git a/src/bsps/stm32f205rc_ciuti/config/miosix_settings.h b/src/bsps/stm32f205rc_ciuti/config/miosix_settings.h
new file mode 100644
index 000000000..22c33aa66
--- /dev/null
+++ b/src/bsps/stm32f205rc_ciuti/config/miosix_settings.h
@@ -0,0 +1,240 @@
+/* Copyright (c) 2021 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * 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.
+ */
+
+#pragma once
+
+// Before you can compile the kernel you have to configure it by editing this
+// file. After that, comment out this line to disable the reminder error.
+// The PARSING_FROM_IDE is because Netbeans gets confused by this, it is never
+// defined when compiling the code.
+#ifndef PARSING_FROM_IDE
+//#error This error is a reminder that you have not edited miosix_settings.h
+// yet.
+#endif  // PARSING_FROM_IDE
+
+/**
+ * \file miosix_settings.h
+ * NOTE: this file contains ONLY configuration options that are not dependent
+ * on architecture specific details. The other options are in the following
+ * files which are included here:
+ * miosix/arch/architecture name/common/arch_settings.h
+ * miosix/config/arch/architecture name/board name/board_settings.h
+ */
+#include "arch_settings.h"
+#include "board_settings.h"
+#include "util/version.h"
+
+/**
+ * \internal
+ * Versioning for miosix_settings.h for out of git tree projects
+ */
+#define MIOSIX_SETTINGS_VERSION 300
+
+namespace miosix
+{
+
+/**
+ * \addtogroup Settings
+ * \{
+ */
+
+//
+// Scheduler options
+//
+
+/// \def SCHED_TYPE_PRIORITY
+/// If uncommented selects the priority scheduler
+/// \def SCHED_TYPE_CONTROL_BASED
+/// If uncommented selects the control based scheduler
+/// \def SCHED_TYPE_EDF
+/// If uncommented selects the EDF scheduler
+// Uncomment only *one* of those
+
+#define SCHED_TYPE_PRIORITY
+// #define SCHED_TYPE_CONTROL_BASED
+// #define SCHED_TYPE_EDF
+
+/// \def WITH_CPU_TIME_COUNTER
+/// Allows to enable/disable CPUTimeCounter to save code size and remove its
+/// overhead from the scheduling process. By default it is not defined
+/// (CPUTimeCounter is disabled).
+// #define WITH_CPU_TIME_COUNTER
+
+//
+// Filesystem options
+//
+
+/// \def WITH_FILESYSTEM
+/// Allows to enable/disable filesystem support to save code size
+/// By default it is defined (filesystem support is enabled)
+#define WITH_FILESYSTEM
+
+/// \def WITH_DEVFS
+/// Allows to enable/disable DevFs support to save code size
+/// By default it is defined (DevFs is enabled)
+#define WITH_DEVFS
+
+/// \def SYNC_AFTER_WRITE
+/// Increases filesystem write robustness. After each write operation the
+/// filesystem is synced so that a power failure happens data is not lost
+/// (unless power failure happens exactly between the write and the sync)
+/// Unfortunately write latency and throughput becomes twice as worse
+/// By default it is defined (slow but safe)
+#define SYNC_AFTER_WRITE
+
+/// Maximum number of open files. Trying to open more will fail.
+/// Cannot be lower than 3, as the first three are stdin, stdout, stderr
+const unsigned char MAX_OPEN_FILES = 8;
+
+/// \def WITH_PROCESSES
+/// If uncommented enables support for processes as well as threads.
+/// This enables the dynamic loader to load elf programs, the extended system
+/// call service and, if the hardware supports it, the MPU to provide memory
+/// isolation of processes
+// #define WITH_PROCESSES
+
+#if defined(WITH_PROCESSES) && defined(__NO_EXCEPTIONS)
+#error Processes require C++ exception support
+#endif  // defined(WITH_PROCESSES) && defined(__NO_EXCEPTIONS)
+
+#if defined(WITH_PROCESSES) && !defined(WITH_FILESYSTEM)
+#error Processes require filesystem support
+#endif  // defined(WITH_PROCESSES) && !defined(WITH_FILESYSTEM)
+
+#if defined(WITH_PROCESSES) && !defined(WITH_DEVFS)
+#error Processes require devfs support
+#endif  // defined(WITH_PROCESSES) && !defined(WITH_DEVFS)
+
+//
+// C/C++ standard library I/O (stdin, stdout and stderr related)
+//
+
+/// \def WITH_BOOTLOG
+/// Uncomment to print bootlogs on stdout.
+/// By default it is defined (bootlogs are printed)
+#define WITH_BOOTLOG
+
+/// \def WITH_ERRLOG
+/// Uncomment for debug information on stdout.
+/// By default it is defined (error information is printed)
+#define WITH_ERRLOG
+
+//
+// Kernel related options (stack sizes, priorities)
+//
+
+/// \def WITH_DEEP_SLEEP
+/// Adds interfaces and required variables to support deep sleep state switch
+/// automatically when peripherals are not required
+// #define WITH_DEEP_SLEEP
+
+/**
+ * \def JTAG_DISABLE_SLEEP
+ * JTAG debuggers lose communication with the device if it enters sleep
+ * mode, so to use debugging it is necessary to disable sleep in the idle
+ * thread. By default it is not defined (idle thread calls sleep).
+ */
+// #define JTAG_DISABLE_SLEEP
+
+#if defined(WITH_DEEP_SLEEP) && defined(JTAG_DISABLE_SLEEP)
+#error Deep sleep cannot work together with jtag
+#endif  // defined(WITH_PROCESSES) && !defined(WITH_DEVFS)
+
+/// Minimum stack size (MUST be divisible by 4)
+const unsigned int STACK_MIN = 256;
+
+/// \internal Size of idle thread stack.
+/// Should be >=STACK_MIN (MUST be divisible by 4)
+const unsigned int STACK_IDLE = 256;
+
+/// Default stack size for pthread_create.
+/// The chosen value is enough to call C standard library functions
+/// such as printf/fopen which are stack-heavy
+const unsigned int STACK_DEFAULT_FOR_PTHREAD = 2048;
+
+/// Maximum size of the RAM image of a process. If a program requires more
+/// the kernel will not run it (MUST be divisible by 4)
+const unsigned int MAX_PROCESS_IMAGE_SIZE = 64 * 1024;
+
+/// Minimum size of the stack for a process. If a program specifies a lower
+/// size the kernel will not run it (MUST be divisible by 4)
+const unsigned int MIN_PROCESS_STACK_SIZE = STACK_MIN;
+
+/// Every userspace thread has two stacks, one for when it is running in
+/// userspace and one for when it is running in kernelspace (that is, while it
+/// is executing system calls). This is the size of the stack for when the
+/// thread is running in kernelspace (MUST be divisible by 4)
+const unsigned int SYSTEM_MODE_PROCESS_STACK_SIZE = 2 * 1024;
+
+/// Number of priorities (MUST be >1)
+/// PRIORITY_MAX-1 is the highest priority, 0 is the lowest. -1 is reserved as
+/// the priority of the idle thread.
+/// The meaning of a thread's priority depends on the chosen scheduler.
+#ifdef SCHED_TYPE_PRIORITY
+// Can be modified, but a high value makes context switches more expensive
+const short int PRIORITY_MAX = 4;
+#elif defined(SCHED_TYPE_CONTROL_BASED)
+// Don't touch, the limit is due to the fixed point implementation
+// It's not needed for if floating point is selected, but kept for consistency
+const short int PRIORITY_MAX = 64;
+#else  // SCHED_TYPE_EDF
+// Doesn't exist for this kind of scheduler
+#endif
+
+/// Priority of main()
+/// The meaning of a thread's priority depends on the chosen scheduler.
+const unsigned char MAIN_PRIORITY = 1;
+
+#ifdef SCHED_TYPE_PRIORITY
+/// Maximum thread time slice in nanoseconds, after which preemption occurs
+const unsigned int MAX_TIME_SLICE = 1000000;
+#endif  // SCHED_TYPE_PRIORITY
+
+//
+// Other low level kernel options. There is usually no need to modify these.
+//
+
+/// \internal Length of wartermark (in bytes) to check stack overflow.
+/// MUST be divisible by 4 and can also be zero.
+/// A high value increases context switch time.
+const unsigned int WATERMARK_LEN = 16;
+
+/// \internal Used to fill watermark
+const unsigned int WATERMARK_FILL = 0xaaaaaaaa;
+
+/// \internal Used to fill stack (for checking stack usage)
+const unsigned int STACK_FILL = 0xbbbbbbbb;
+
+// Compiler version checks
+#if !defined(_MIOSIX_GCC_PATCH_MAJOR) || _MIOSIX_GCC_PATCH_MAJOR < 3
+#error \
+    "You are using a too old or unsupported compiler. Get the latest one from https://miosix.org/wiki/index.php?title=Miosix_Toolchain"
+#endif
+#if _MIOSIX_GCC_PATCH_MAJOR > 3
+#warning "You are using a too new compiler, which may not be supported"
+#endif
+
+/**
+ * \}
+ */
+
+}  // namespace miosix
diff --git a/src/bsps/stm32f205rc_ciuti/core/stage_1_boot.cpp b/src/bsps/stm32f205rc_ciuti/core/stage_1_boot.cpp
new file mode 100644
index 000000000..c3f27e6bb
--- /dev/null
+++ b/src/bsps/stm32f205rc_ciuti/core/stage_1_boot.cpp
@@ -0,0 +1,399 @@
+/* Copyright (c) 2021 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * 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 <string.h>
+
+#include "core/interrupts.h"  //For the unexpected interrupt call
+#include "interfaces/arch_registers.h"
+#include "kernel/stage_2_boot.h"
+
+/*
+ * startup.cpp
+ * STM32 C++ startup.
+ * NOTE: for stm32f2 devices ONLY.
+ * Supports interrupt handlers in C++ without extern "C"
+ * Developed by Terraneo Federico, based on ST startup code.
+ * Additionally modified to boot Miosix.
+ */
+
+/**
+ * Called by Reset_Handler, performs initialization and calls main.
+ * Never returns.
+ */
+void program_startup() __attribute__((noreturn));
+void program_startup()
+{
+    // Cortex M3 core appears to get out of reset with interrupts already
+    // enabled
+    __disable_irq();
+
+    // These are defined in the linker script
+    extern unsigned char _etext asm("_etext");
+    extern unsigned char _data asm("_data");
+    extern unsigned char _edata asm("_edata");
+    extern unsigned char _bss_start asm("_bss_start");
+    extern unsigned char _bss_end asm("_bss_end");
+
+    // Initialize .data section, clear .bss section
+    unsigned char *etext     = &_etext;
+    unsigned char *data      = &_data;
+    unsigned char *edata     = &_edata;
+    unsigned char *bss_start = &_bss_start;
+    unsigned char *bss_end   = &_bss_end;
+    // The memcpy is usually enclosed in an #ifndef __ENABLE_XRAM, in other
+    // boards but in this case it is not, since the *_code_in_xram.ld linker
+    // script puts code in XRAM, but data in the internal one, so there's still
+    // the need to copy it in its final place
+    memcpy(data, etext, edata - data);
+    memset(bss_start, 0, bss_end - bss_start);
+
+    // Move on to stage 2
+    _init();
+
+    // If main returns, reboot
+    NVIC_SystemReset();
+    for (;;)
+        ;
+}
+
+/**
+ * Reset handler, called by hardware immediately after reset
+ */
+void Reset_Handler() __attribute__((__interrupt__, noreturn));
+void Reset_Handler()
+{
+    /*
+     * SystemInit() is called *before* initializing .data and zeroing .bss
+     * Despite all startup files provided by ST do the opposite, there are three
+     * good reasons to do so:
+     * First, the CMSIS specifications say that SystemInit() must not access
+     * global variables, so it is actually possible to call it before
+     * Second, when running Miosix with the xram linker scripts .data and .bss
+     * are placed in the external RAM, so we *must* call SystemInit(), which
+     * enables xram, before touching .data and .bss
+     * Third, this is a performance improvement since the loops that initialize
+     * .data and zeros .bss now run with the CPU at full speed instead of 16MHz
+     * Note that it is called before switching stacks because the memory
+     * at _heap_end can be unavailable until the external RAM is initialized.
+     */
+    SystemInit();
+
+    /*
+     * Initialize process stack and switch to it.
+     * This is required for booting Miosix, a small portion of the top of the
+     * heap area will be used as stack until the first thread starts. After,
+     * this stack will be abandoned and the process stack will point to the
+     * current thread's stack.
+     */
+    asm volatile(
+        "ldr r0,  =_heap_end          \n\t"
+        "msr psp, r0                  \n\t"
+        "movw r0, #2                  \n\n"  // Privileged, process stack
+        "msr control, r0              \n\t"
+        "isb                          \n\t" ::
+            : "r0");
+
+    program_startup();
+}
+
+/**
+ * All unused interrupts call this function.
+ */
+extern "C" void Default_Handler() { unexpectedInterrupt(); }
+
+// System handlers
+void /*__attribute__((weak))*/ Reset_Handler();      // These interrupts are not
+void /*__attribute__((weak))*/ NMI_Handler();        // weak because they are
+void /*__attribute__((weak))*/ HardFault_Handler();  // surely defined by Miosix
+void /*__attribute__((weak))*/ MemManage_Handler();
+void /*__attribute__((weak))*/ BusFault_Handler();
+void /*__attribute__((weak))*/ UsageFault_Handler();
+void /*__attribute__((weak))*/ SVC_Handler();
+void /*__attribute__((weak))*/ DebugMon_Handler();
+void /*__attribute__((weak))*/ PendSV_Handler();
+void __attribute__((weak)) SysTick_Handler();
+
+// Interrupt handlers
+void __attribute__((weak)) WWDG_IRQHandler();
+void __attribute__((weak)) PVD_IRQHandler();
+void __attribute__((weak)) TAMP_STAMP_IRQHandler();
+void __attribute__((weak)) RTC_WKUP_IRQHandler();
+void __attribute__((weak)) FLASH_IRQHandler();
+void __attribute__((weak)) RCC_IRQHandler();
+void __attribute__((weak)) EXTI0_IRQHandler();
+void __attribute__((weak)) EXTI1_IRQHandler();
+void __attribute__((weak)) EXTI2_IRQHandler();
+void __attribute__((weak)) EXTI3_IRQHandler();
+void __attribute__((weak)) EXTI4_IRQHandler();
+void __attribute__((weak)) DMA1_Stream0_IRQHandler();
+void __attribute__((weak)) DMA1_Stream1_IRQHandler();
+void __attribute__((weak)) DMA1_Stream2_IRQHandler();
+void __attribute__((weak)) DMA1_Stream3_IRQHandler();
+void __attribute__((weak)) DMA1_Stream4_IRQHandler();
+void __attribute__((weak)) DMA1_Stream5_IRQHandler();
+void __attribute__((weak)) DMA1_Stream6_IRQHandler();
+void __attribute__((weak)) ADC_IRQHandler();
+void __attribute__((weak)) CAN1_TX_IRQHandler();
+void __attribute__((weak)) CAN1_RX0_IRQHandler();
+void __attribute__((weak)) CAN1_RX1_IRQHandler();
+void __attribute__((weak)) CAN1_SCE_IRQHandler();
+void __attribute__((weak)) EXTI9_5_IRQHandler();
+void __attribute__((weak)) TIM1_BRK_TIM9_IRQHandler();
+void __attribute__((weak)) TIM1_UP_TIM10_IRQHandler();
+void __attribute__((weak)) TIM1_TRG_COM_TIM11_IRQHandler();
+void __attribute__((weak)) TIM1_CC_IRQHandler();
+void __attribute__((weak)) TIM2_IRQHandler();
+void __attribute__((weak)) TIM3_IRQHandler();
+void __attribute__((weak)) TIM4_IRQHandler();
+void __attribute__((weak)) I2C1_EV_IRQHandler();
+void __attribute__((weak)) I2C1_ER_IRQHandler();
+void __attribute__((weak)) I2C2_EV_IRQHandler();
+void __attribute__((weak)) I2C2_ER_IRQHandler();
+void __attribute__((weak)) SPI1_IRQHandler();
+void __attribute__((weak)) SPI2_IRQHandler();
+void __attribute__((weak)) USART1_IRQHandler();
+void __attribute__((weak)) USART2_IRQHandler();
+void __attribute__((weak)) USART3_IRQHandler();
+void __attribute__((weak)) EXTI15_10_IRQHandler();
+void __attribute__((weak)) RTC_Alarm_IRQHandler();
+void __attribute__((weak)) OTG_FS_WKUP_IRQHandler();
+void __attribute__((weak)) TIM8_BRK_TIM12_IRQHandler();
+void __attribute__((weak)) TIM8_UP_TIM13_IRQHandler();
+void __attribute__((weak)) TIM8_TRG_COM_TIM14_IRQHandler();
+void __attribute__((weak)) TIM8_CC_IRQHandler();
+void __attribute__((weak)) DMA1_Stream7_IRQHandler();
+void __attribute__((weak)) FSMC_IRQHandler();
+void __attribute__((weak)) SDIO_IRQHandler();
+void __attribute__((weak)) TIM5_IRQHandler();
+void __attribute__((weak)) SPI3_IRQHandler();
+void __attribute__((weak)) UART4_IRQHandler();
+void __attribute__((weak)) UART5_IRQHandler();
+void __attribute__((weak)) TIM6_DAC_IRQHandler();
+void __attribute__((weak)) TIM7_IRQHandler();
+void __attribute__((weak)) DMA2_Stream0_IRQHandler();
+void __attribute__((weak)) DMA2_Stream1_IRQHandler();
+void __attribute__((weak)) DMA2_Stream2_IRQHandler();
+void __attribute__((weak)) DMA2_Stream3_IRQHandler();
+void __attribute__((weak)) DMA2_Stream4_IRQHandler();
+void __attribute__((weak)) CAN2_TX_IRQHandler();
+void __attribute__((weak)) CAN2_RX0_IRQHandler();
+void __attribute__((weak)) CAN2_RX1_IRQHandler();
+void __attribute__((weak)) CAN2_SCE_IRQHandler();
+void __attribute__((weak)) OTG_FS_IRQHandler();
+void __attribute__((weak)) DMA2_Stream5_IRQHandler();
+void __attribute__((weak)) DMA2_Stream6_IRQHandler();
+void __attribute__((weak)) DMA2_Stream7_IRQHandler();
+void __attribute__((weak)) USART6_IRQHandler();
+void __attribute__((weak)) I2C3_EV_IRQHandler();
+void __attribute__((weak)) I2C3_ER_IRQHandler();
+void __attribute__((weak)) OTG_HS_EP1_OUT_IRQHandler();
+void __attribute__((weak)) OTG_HS_EP1_IN_IRQHandler();
+void __attribute__((weak)) OTG_HS_WKUP_IRQHandler();
+void __attribute__((weak)) OTG_HS_IRQHandler();
+void __attribute__((weak)) HASH_RNG_IRQHandler();
+
+// Stack top, defined in the linker script
+extern char _main_stack_top asm("_main_stack_top");
+
+// Interrupt vectors, must be placed @ address 0x00000000
+// The extern declaration is required otherwise g++ optimizes it out
+extern void (*const __Vectors[])();
+void (*const __Vectors[])() __attribute__((section(".isr_vector"))) = {
+    reinterpret_cast<void (*)()>(&_main_stack_top), /* Stack pointer*/
+    Reset_Handler,                                  /* Reset Handler */
+    NMI_Handler,                                    /* NMI Handler */
+    HardFault_Handler,                              /* Hard Fault Handler */
+    MemManage_Handler,                              /* MPU Fault Handler */
+    BusFault_Handler,                               /* Bus Fault Handler */
+    UsageFault_Handler,                             /* Usage Fault Handler */
+    0,                                              /* Reserved */
+    0,                                              /* Reserved */
+    0,                                              /* Reserved */
+    0,                                              /* Reserved */
+    SVC_Handler,                                    /* SVCall Handler */
+    DebugMon_Handler,                               /* Debug Monitor Handler */
+    0,                                              /* Reserved */
+    PendSV_Handler,                                 /* PendSV Handler */
+    SysTick_Handler,                                /* SysTick Handler */
+
+    /* External Interrupts */
+    WWDG_IRQHandler,
+    PVD_IRQHandler,
+    TAMP_STAMP_IRQHandler,
+    RTC_WKUP_IRQHandler,
+    FLASH_IRQHandler,
+    RCC_IRQHandler,
+    EXTI0_IRQHandler,
+    EXTI1_IRQHandler,
+    EXTI2_IRQHandler,
+    EXTI3_IRQHandler,
+    EXTI4_IRQHandler,
+    DMA1_Stream0_IRQHandler,
+    DMA1_Stream1_IRQHandler,
+    DMA1_Stream2_IRQHandler,
+    DMA1_Stream3_IRQHandler,
+    DMA1_Stream4_IRQHandler,
+    DMA1_Stream5_IRQHandler,
+    DMA1_Stream6_IRQHandler,
+    ADC_IRQHandler,
+    CAN1_TX_IRQHandler,
+    CAN1_RX0_IRQHandler,
+    CAN1_RX1_IRQHandler,
+    CAN1_SCE_IRQHandler,
+    EXTI9_5_IRQHandler,
+    TIM1_BRK_TIM9_IRQHandler,
+    TIM1_UP_TIM10_IRQHandler,
+    TIM1_TRG_COM_TIM11_IRQHandler,
+    TIM1_CC_IRQHandler,
+    TIM2_IRQHandler,
+    TIM3_IRQHandler,
+    TIM4_IRQHandler,
+    I2C1_EV_IRQHandler,
+    I2C1_ER_IRQHandler,
+    I2C2_EV_IRQHandler,
+    I2C2_ER_IRQHandler,
+    SPI1_IRQHandler,
+    SPI2_IRQHandler,
+    USART1_IRQHandler,
+    USART2_IRQHandler,
+    USART3_IRQHandler,
+    EXTI15_10_IRQHandler,
+    RTC_Alarm_IRQHandler,
+    OTG_FS_WKUP_IRQHandler,
+    TIM8_BRK_TIM12_IRQHandler,
+    TIM8_UP_TIM13_IRQHandler,
+    TIM8_TRG_COM_TIM14_IRQHandler,
+    TIM8_CC_IRQHandler,
+    DMA1_Stream7_IRQHandler,
+    FSMC_IRQHandler,
+    SDIO_IRQHandler,
+    TIM5_IRQHandler,
+    SPI3_IRQHandler,
+    UART4_IRQHandler,
+    UART5_IRQHandler,
+    TIM6_DAC_IRQHandler,
+    TIM7_IRQHandler,
+    DMA2_Stream0_IRQHandler,
+    DMA2_Stream1_IRQHandler,
+    DMA2_Stream2_IRQHandler,
+    DMA2_Stream3_IRQHandler,
+    DMA2_Stream4_IRQHandler,
+    0,
+    0,
+    CAN2_TX_IRQHandler,
+    CAN2_RX0_IRQHandler,
+    CAN2_RX1_IRQHandler,
+    CAN2_SCE_IRQHandler,
+    OTG_FS_IRQHandler,
+    DMA2_Stream5_IRQHandler,
+    DMA2_Stream6_IRQHandler,
+    DMA2_Stream7_IRQHandler,
+    USART6_IRQHandler,
+    I2C3_EV_IRQHandler,
+    I2C3_ER_IRQHandler,
+    OTG_HS_EP1_OUT_IRQHandler,
+    OTG_HS_EP1_IN_IRQHandler,
+    OTG_HS_WKUP_IRQHandler,
+    OTG_HS_IRQHandler,
+    0,
+    0,
+    HASH_RNG_IRQHandler,
+};
+
+#pragma weak SysTick_IRQHandler            = Default_Handler
+#pragma weak WWDG_IRQHandler               = Default_Handler
+#pragma weak PVD_IRQHandler                = Default_Handler
+#pragma weak TAMP_STAMP_IRQHandler         = Default_Handler
+#pragma weak RTC_WKUP_IRQHandler           = Default_Handler
+#pragma weak FLASH_IRQHandler              = Default_Handler
+#pragma weak RCC_IRQHandler                = Default_Handler
+#pragma weak EXTI0_IRQHandler              = Default_Handler
+#pragma weak EXTI1_IRQHandler              = Default_Handler
+#pragma weak EXTI2_IRQHandler              = Default_Handler
+#pragma weak EXTI3_IRQHandler              = Default_Handler
+#pragma weak EXTI4_IRQHandler              = Default_Handler
+#pragma weak DMA1_Stream0_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream1_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream2_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream3_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream4_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream5_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream6_IRQHandler       = Default_Handler
+#pragma weak ADC_IRQHandler                = Default_Handler
+#pragma weak CAN1_TX_IRQHandler            = Default_Handler
+#pragma weak CAN1_RX0_IRQHandler           = Default_Handler
+#pragma weak CAN1_RX1_IRQHandler           = Default_Handler
+#pragma weak CAN1_SCE_IRQHandler           = Default_Handler
+#pragma weak EXTI9_5_IRQHandler            = Default_Handler
+#pragma weak TIM1_BRK_TIM9_IRQHandler      = Default_Handler
+#pragma weak TIM1_UP_TIM10_IRQHandler      = Default_Handler
+#pragma weak TIM1_TRG_COM_TIM11_IRQHandler = Default_Handler
+#pragma weak TIM1_CC_IRQHandler            = Default_Handler
+#pragma weak TIM2_IRQHandler               = Default_Handler
+#pragma weak TIM3_IRQHandler               = Default_Handler
+#pragma weak TIM4_IRQHandler               = Default_Handler
+#pragma weak I2C1_EV_IRQHandler            = Default_Handler
+#pragma weak I2C1_ER_IRQHandler            = Default_Handler
+#pragma weak I2C2_EV_IRQHandler            = Default_Handler
+#pragma weak I2C2_ER_IRQHandler            = Default_Handler
+#pragma weak SPI1_IRQHandler               = Default_Handler
+#pragma weak SPI2_IRQHandler               = Default_Handler
+// #pragma weak USART1_IRQHandler             = Default_Handler
+// #pragma weak USART2_IRQHandler             = Default_Handler
+// #pragma weak USART3_IRQHandler             = Default_Handler
+#pragma weak EXTI15_10_IRQHandler          = Default_Handler
+#pragma weak RTC_Alarm_IRQHandler          = Default_Handler
+#pragma weak OTG_FS_WKUP_IRQHandler        = Default_Handler
+#pragma weak TIM8_BRK_TIM12_IRQHandler     = Default_Handler
+#pragma weak TIM8_UP_TIM13_IRQHandler      = Default_Handler
+#pragma weak TIM8_TRG_COM_TIM14_IRQHandler = Default_Handler
+#pragma weak TIM8_CC_IRQHandler            = Default_Handler
+#pragma weak DMA1_Stream7_IRQHandler       = Default_Handler
+#pragma weak FSMC_IRQHandler               = Default_Handler
+#pragma weak SDIO_IRQHandler               = Default_Handler
+#pragma weak TIM5_IRQHandler               = Default_Handler
+#pragma weak SPI3_IRQHandler               = Default_Handler
+// #pragma weak UART4_IRQHandler              = Default_Handler
+// #pragma weak UART5_IRQHandler              = Default_Handler
+#pragma weak TIM6_DAC_IRQHandler     = Default_Handler
+#pragma weak TIM7_IRQHandler         = Default_Handler
+#pragma weak DMA2_Stream0_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream1_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream2_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream3_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream4_IRQHandler = Default_Handler
+#pragma weak CAN2_TX_IRQHandler      = Default_Handler
+#pragma weak CAN2_RX0_IRQHandler     = Default_Handler
+#pragma weak CAN2_RX1_IRQHandler     = Default_Handler
+#pragma weak CAN2_SCE_IRQHandler     = Default_Handler
+#pragma weak OTG_FS_IRQHandler       = Default_Handler
+#pragma weak DMA2_Stream5_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream6_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream7_IRQHandler = Default_Handler
+// #pragma weak USART6_IRQHandler             = Default_Handler
+#pragma weak I2C3_EV_IRQHandler        = Default_Handler
+#pragma weak I2C3_ER_IRQHandler        = Default_Handler
+#pragma weak OTG_HS_EP1_OUT_IRQHandler = Default_Handler
+#pragma weak OTG_HS_EP1_IN_IRQHandler  = Default_Handler
+#pragma weak OTG_HS_WKUP_IRQHandler    = Default_Handler
+#pragma weak OTG_HS_IRQHandler         = Default_Handler
+#pragma weak HASH_RNG_IRQHandler       = Default_Handler
diff --git a/src/bsps/stm32f205rc_ciuti/interfaces-impl/arch_registers_impl.h b/src/bsps/stm32f205rc_ciuti/interfaces-impl/arch_registers_impl.h
new file mode 100644
index 000000000..3c336decc
--- /dev/null
+++ b/src/bsps/stm32f205rc_ciuti/interfaces-impl/arch_registers_impl.h
@@ -0,0 +1,33 @@
+/* Copyright (c) 2021 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * 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.
+ */
+
+#ifndef ARCH_REGISTERS_IMPL_H
+#define ARCH_REGISTERS_IMPL_H
+
+#define STM32F205xx
+#include "CMSIS/Device/ST/STM32F2xx/Include/stm32f2xx.h"
+#include "CMSIS/Device/ST/STM32F2xx/Include/system_stm32f2xx.h"
+#include "CMSIS/Include/core_cm3.h"
+
+#define RCC_SYNC()  // Workaround for a bug in stm32f42x
+
+#endif  // ARCH_REGISTERS_IMPL_H
diff --git a/src/bsps/stm32f205rc_ciuti/interfaces-impl/bsp.cpp b/src/bsps/stm32f205rc_ciuti/interfaces-impl/bsp.cpp
new file mode 100644
index 000000000..39c47a138
--- /dev/null
+++ b/src/bsps/stm32f205rc_ciuti/interfaces-impl/bsp.cpp
@@ -0,0 +1,173 @@
+/* Copyright (c) 2021 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * 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.
+ */
+
+/***********************************************************************
+ * bsp.cpp Part of the Miosix Embedded OS.
+ * Board support package, this file initializes hardware.
+ ************************************************************************/
+
+#include "interfaces/bsp.h"
+
+#include <inttypes.h>
+#include <sys/ioctl.h>
+
+#include <cstdlib>
+
+#include "board_settings.h"
+#include "config/miosix_settings.h"
+#include "drivers/dcc.h"
+#include "drivers/sd_stm32f2_f4_f7.h"
+#include "drivers/serial.h"
+#include "filesystem/console/console_device.h"
+#include "filesystem/file_access.h"
+#include "interfaces/arch_registers.h"
+#include "interfaces/delays.h"
+#include "interfaces/portability.h"
+#include "kernel/kernel.h"
+#include "kernel/logging.h"
+#include "kernel/sync.h"
+
+using namespace std;
+
+namespace miosix
+{
+
+//
+// Initialization
+//
+
+void IRQbspInit()
+{
+    // Enable all gpios
+    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN | RCC_AHB1ENR_GPIOBEN |
+                    RCC_AHB1ENR_GPIOCEN | RCC_AHB1ENR_GPIODEN;
+    RCC_SYNC();
+    GPIOA->OSPEEDR = 0xaaaaaaaa;  // Default to 50MHz speed for all GPIOS
+    GPIOB->OSPEEDR = 0xaaaaaaaa;
+    GPIOC->OSPEEDR = 0xaaaaaaaa;
+    GPIOD->OSPEEDR = 0xaaaaaaaa;
+
+    using namespace interfaces;
+
+    spi1::cs::mode(Mode::OUTPUT);
+    spi1::cs::high();
+    spi1::sck::mode(Mode::ALTERNATE);
+    spi1::sck::alternateFunction(5);
+    spi1::miso::mode(Mode::ALTERNATE);
+    spi1::miso::alternateFunction(5);
+    spi1::mosi::mode(Mode::ALTERNATE);
+    spi1::mosi::alternateFunction(5);
+
+    spi2::sck::mode(Mode::ALTERNATE);
+    spi2::sck::alternateFunction(5);
+    spi2::miso::mode(Mode::ALTERNATE);
+    spi2::miso::alternateFunction(5);
+    spi2::mosi::mode(Mode::ALTERNATE);
+    spi2::mosi::alternateFunction(5);
+
+    usart1::rx::mode(Mode::ALTERNATE);
+    usart1::rx::alternateFunction(7);
+    usart1::tx::mode(Mode::ALTERNATE);
+    usart1::tx::alternateFunction(7);
+
+    usart2::rx::mode(Mode::ALTERNATE);
+    usart2::rx::alternateFunction(7);
+    usart2::tx::mode(Mode::ALTERNATE);
+    usart2::tx::alternateFunction(7);
+
+    usart3::rx::mode(Mode::ALTERNATE);
+    usart3::rx::alternateFunction(7);
+    usart3::tx::mode(Mode::ALTERNATE);
+    usart3::tx::alternateFunction(7);
+
+    can1::rx::mode(Mode::ALTERNATE);
+    can1::rx::alternateFunction(9);
+    can1::tx::mode(Mode::ALTERNATE);
+    can1::tx::alternateFunction(9);
+
+    using namespace devices;
+
+    lis331hh::cs::mode(Mode::OUTPUT);
+    lis331hh::cs::high();
+
+    ad5204::cs::mode(Mode::OUTPUT);
+    ad5204::cs::high();
+
+    ina188::vsense1::mode(Mode::INPUT_ANALOG);
+    ina188::vsense2::mode(Mode::INPUT_ANALOG);
+    ina188::mosfet1::mode(Mode::OUTPUT);
+    ina188::mosfet1::high();
+    ina188::mosfet2::mode(Mode::OUTPUT);
+    ina188::mosfet2::high();
+
+    vbat::mode(Mode::INPUT_ANALOG);
+
+    buttons::bypass::mode(Mode::INPUT);
+    buttons::record::mode(Mode::INPUT);
+
+    buzzer::drive::mode(Mode::ALTERNATE);
+    buzzer::drive::alternateFunction(3);
+
+    leds::led1::mode(Mode::OUTPUT);
+    leds::led1::low();
+    leds::led2::mode(Mode::OUTPUT);
+    leds::led2::low();
+    leds::led3::mode(Mode::OUTPUT);
+    leds::led3::low();
+    leds::led4::mode(Mode::OUTPUT);
+    leds::led4::low();
+
+    DefaultConsole::instance().IRQset(intrusive_ref_ptr<Device>(
+        new STM32Serial(defaultSerial, defaultSerialSpeed,
+                        defaultSerialFlowctrl ? STM32Serial::RTSCTS
+                                              : STM32Serial::NOFLOWCTRL)));
+}
+
+void bspInit2()
+{
+#ifdef WITH_FILESYSTEM
+    basicFilesystemSetup(SDIODriver::instance());
+#endif  // WITH_FILESYSTEM
+}
+
+//
+// Shutdown and reboot
+//
+
+/**
+ * For safety reasons, we never want to shutdown. When requested to shutdown, we
+ * reboot instead.
+ */
+void shutdown() { reboot(); }
+
+void reboot()
+{
+    ioctl(STDOUT_FILENO, IOCTL_SYNC, 0);
+
+#ifdef WITH_FILESYSTEM
+    FilesystemManager::instance().umountAll();
+#endif  // WITH_FILESYSTEM
+
+    disableInterrupts();
+    miosix_private::IRQsystemReboot();
+}
+}  // namespace miosix
diff --git a/src/bsps/stm32f205rc_ciuti/interfaces-impl/bsp_impl.h b/src/bsps/stm32f205rc_ciuti/interfaces-impl/bsp_impl.h
new file mode 100644
index 000000000..b2158cabc
--- /dev/null
+++ b/src/bsps/stm32f205rc_ciuti/interfaces-impl/bsp_impl.h
@@ -0,0 +1,72 @@
+/* Copyright (c) 2021 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * 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.
+ */
+
+/***********************************************************************
+ * bsp_impl.h Part of the Miosix Embedded OS.
+ * Board support package, this file initializes hardware.
+ ************************************************************************/
+
+#ifndef BSP_IMPL_H
+#define BSP_IMPL_H
+
+#include "config/miosix_settings.h"
+#include "drivers/stm32_hardware_rng.h"
+#include "hwmapping.h"
+#include "interfaces/gpio.h"
+
+namespace miosix
+{
+
+/**
+\addtogroup Hardware
+\{
+*/
+
+inline void ledOn()
+{
+    devices::leds::led1::high();
+    devices::leds::led2::high();
+    devices::leds::led3::high();
+    devices::leds::led4::high();
+}
+
+inline void ledOff()
+{
+    devices::leds::led1::low();
+    devices::leds::led2::low();
+    devices::leds::led3::low();
+    devices::leds::led4::low();
+}
+
+/**
+ * Polls the SD card sense GPIO
+ * \return true if there is an uSD card in the socket.
+ */
+inline bool sdCardSense() { return true; }
+
+/**
+\}
+*/
+
+}  // namespace miosix
+
+#endif  // BSP_IMPL_H
diff --git a/src/bsps/stm32f205rc_ciuti/interfaces-impl/delays.cpp b/src/bsps/stm32f205rc_ciuti/interfaces-impl/delays.cpp
new file mode 100644
index 000000000..65ff9f9c4
--- /dev/null
+++ b/src/bsps/stm32f205rc_ciuti/interfaces-impl/delays.cpp
@@ -0,0 +1,76 @@
+/* Copyright (c) 2021 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * 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 "interfaces/delays.h"
+
+namespace miosix
+{
+
+void delayMs(unsigned int mseconds)
+{
+#ifndef __CODE_IN_XRAM
+
+#ifndef SYSCLK_FREQ_120MHz
+#error "Delays are uncalibrated for this clock frequency"
+#endif
+    const unsigned int count = 29999;
+
+    for (unsigned int i = 0; i < mseconds; i++)
+    {
+        // This delay has been calibrated to take 1 millisecond
+        // It is written in assembler to be independent on compiler optimization
+        asm volatile(
+            "           mov   r1, #0     \n"
+            "___loop_m: cmp   r1, %0     \n"
+            "           itt   lo         \n"
+            "           addlo r1, r1, #1 \n"
+            "           blo   ___loop_m  \n" ::"r"(count)
+            : "r1");
+    }
+
+#else  //__CODE_IN_XRAM
+#error "No delays"
+#endif  //__CODE_IN_XRAM
+}
+
+void delayUs(unsigned int useconds)
+{
+#ifndef __CODE_IN_XRAM
+
+    // This delay has been calibrated to take x microseconds
+    // It is written in assembler to be independent on compiler optimization
+    asm volatile(
+        "           mov   r1, #30    \n"
+        "           mul   r2, %0, r1 \n"
+        "           mov   r1, #0     \n"
+        "___loop_u: cmp   r1, r2     \n"
+        "           itt   lo         \n"
+        "           addlo r1, r1, #1 \n"
+        "           blo   ___loop_u  \n" ::"r"(useconds)
+        : "r1", "r2");
+
+#else  //__CODE_IN_XRAM
+#error "No delays"
+#endif  //__CODE_IN_XRAM
+}
+
+}  // namespace miosix
diff --git a/src/bsps/stm32f205rc_ciuti/interfaces-impl/hwmapping.h b/src/bsps/stm32f205rc_ciuti/interfaces-impl/hwmapping.h
new file mode 100644
index 000000000..7edc4d4b3
--- /dev/null
+++ b/src/bsps/stm32f205rc_ciuti/interfaces-impl/hwmapping.h
@@ -0,0 +1,125 @@
+/* Copyright (c) 2021 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * 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.
+ */
+
+#ifndef HWMAPPING_H
+#define HWMAPPING_H
+
+#include "interfaces/gpio.h"
+
+namespace miosix
+{
+
+namespace interfaces
+{
+
+namespace spi1
+{
+using cs   = Gpio<GPIOA_BASE, 4>;
+using sck  = Gpio<GPIOA_BASE, 5>;
+using miso = Gpio<GPIOA_BASE, 6>;
+using mosi = Gpio<GPIOA_BASE, 7>;
+}  // namespace spi1
+
+// LIS331HH and AD8403
+namespace spi2
+{
+using sck  = Gpio<GPIOB_BASE, 13>;
+using miso = Gpio<GPIOB_BASE, 14>;
+using mosi = Gpio<GPIOB_BASE, 15>;
+}  // namespace spi2
+
+// Debug
+namespace usart1
+{
+using tx = Gpio<GPIOA_BASE, 9>;
+using rx = Gpio<GPIOA_BASE, 10>;
+}  // namespace usart1
+
+// Pogo pin
+namespace usart2
+{
+using tx = Gpio<GPIOA_BASE, 2>;
+using rx = Gpio<GPIOA_BASE, 3>;
+}  // namespace usart2
+
+// Pogo pin
+namespace usart3
+{
+using tx = Gpio<GPIOB_BASE, 10>;
+using rx = Gpio<GPIOB_BASE, 11>;
+}  // namespace usart3
+
+namespace can1
+{
+using rx = Gpio<GPIOA_BASE, 11>;
+using tx = Gpio<GPIOA_BASE, 12>;
+}  // namespace can1
+
+}  // namespace interfaces
+
+namespace devices
+{
+
+namespace lis331hh
+{
+using cs = Gpio<GPIOC_BASE, 4>;
+}
+
+namespace ad5204
+{
+using cs = Gpio<GPIOC_BASE, 0>;
+}
+
+namespace ina188
+{
+using vsense1 = Gpio<GPIOA_BASE, 0>;
+using vsense2 = Gpio<GPIOA_BASE, 1>;
+using mosfet1 = Gpio<GPIOC_BASE, 3>;
+using mosfet2 = Gpio<GPIOC_BASE, 2>;
+}  // namespace ina188
+
+using vbat = Gpio<GPIOC_BASE, 5>;
+
+namespace buttons
+{
+using bypass = Gpio<GPIOC_BASE, 7>;
+using record = Gpio<GPIOA_BASE, 8>;
+}  // namespace buttons
+
+namespace buzzer
+{
+using drive = Gpio<GPIOC_BASE, 6>;  // PWM TIM8_CH1
+}
+
+namespace leds
+{
+using led1 = Gpio<GPIOB_BASE, 0>;  // TIM3_CH3
+using led2 = Gpio<GPIOB_BASE, 5>;
+using led3 = Gpio<GPIOB_BASE, 6>;
+using led4 = Gpio<GPIOB_BASE, 7>;
+}  // namespace leds
+
+}  // namespace devices
+
+}  // namespace miosix
+
+#endif  // HWMAPPING_H
diff --git a/src/bsps/stm32f205rc_ciuti/stm32_512k+128k_rom.ld b/src/bsps/stm32f205rc_ciuti/stm32_512k+128k_rom.ld
new file mode 100644
index 000000000..62406d42b
--- /dev/null
+++ b/src/bsps/stm32f205rc_ciuti/stm32_512k+128k_rom.ld
@@ -0,0 +1,170 @@
+/*
+ * C++ enabled linker script for Skyward stormtrooper board
+ * Developed by Terraneo Federico, adapted by Silvano Seva
+ * STM32F205RC has 512KB of flash and 128KB of RAM
+ */
+
+/*
+ * This linker script puts:
+ * - read only data and code (.text, .rodata, .eh_*) in flash
+ * - stacks, heap and sections .data and .bss in the internal ram
+ */
+
+/*
+ * The main stack is used for interrupt handling by the kernel.
+ *
+ * *** Readme ***
+ * This linker script places the main stack (used by the kernel for interrupts)
+ * at the bottom of the ram, instead of the top. This is done for two reasons:
+ *
+ * - as an optimization for microcontrollers with little ram memory. In fact
+ *   the implementation of malloc from newlib requests memory to the OS in 4KB
+ *   block (except the first block that can be smaller). This is probably done
+ *   for compatibility with OSes with an MMU and paged memory. To see why this
+ *   is bad, consider a microcontroller with 8KB of ram: when malloc finishes
+ *   up the first 4KB it will call _sbrk_r asking for a 4KB block, but this will
+ *   fail because the top part of the ram is used by the main stack. As a
+ *   result, the top part of the memory will not be used by malloc, even if
+ *   available (and it is nearly *half* the ram on an 8KB mcu). By placing the
+ *   main stack at the bottom of the ram, the upper 4KB block will be entirely
+ *   free and available as heap space.
+ *
+ * - In case of main stack overflow the cpu will fault because access to memory
+ *   before the beginning of the ram faults. Instead with the default stack
+ *   placement the main stack will silently collide with the heap.
+ * Note: if increasing the main stack size also increase the ORIGIN value in
+ * the MEMORY definitions below accordingly.
+ */
+
+_main_stack_size = 0x00000200;                     /* main stack = 512Bytes */
+_main_stack_top  = 0x20000000 + _main_stack_size;
+ASSERT(_main_stack_size % 8 == 0, "MAIN stack size error");
+
+/* end of the heap on 128KB microcontrollers */
+_heap_end = 0x20020000;                            /* end of available ram  */
+
+/* identify the Entry Point  */
+ENTRY(_Z13Reset_Handlerv)
+
+/* specify the memory areas  */
+MEMORY
+{
+    flash(rx)   : ORIGIN = 0,          LENGTH = 512K
+
+    /*
+     * Note, the ram starts at 0x20000000 but it is necessary to add the size
+     * of the main stack, so it is 0x20000200.
+     */
+    ram(wx)     : ORIGIN = 0x20000200, LENGTH =  128K-0x200
+}
+
+/* now define the output sections  */
+SECTIONS
+{
+    . = 0;
+    
+    /* .text section: code goes to flash */
+    .text :
+    {
+        /* Startup code must go at address 0 */
+        KEEP(*(.isr_vector))
+        
+        *(.text)
+        *(.text.*)
+        *(.gnu.linkonce.t.*)
+        /* these sections for thumb interwork? */
+        *(.glue_7)
+        *(.glue_7t)
+        /* these sections for C++? */
+        *(.gcc_except_table)
+        *(.gcc_except_table.*)
+        *(.ARM.extab*)
+        *(.gnu.linkonce.armextab.*)
+
+        . = ALIGN(4);
+        /* .rodata: constant data */
+        *(.rodata)
+        *(.rodata.*)
+        *(.gnu.linkonce.r.*)
+
+        /* C++ Static constructors/destructors (eabi) */
+        . = ALIGN(4);
+        KEEP(*(.init))
+        
+        . = ALIGN(4);
+        __miosix_init_array_start = .;
+        KEEP (*(SORT(.miosix_init_array.*)))
+        KEEP (*(.miosix_init_array))
+        __miosix_init_array_end = .;
+
+        . = ALIGN(4);
+        __preinit_array_start = .;
+        KEEP (*(.preinit_array))
+        __preinit_array_end = .;
+
+        . = ALIGN(4);
+        __init_array_start = .;
+        KEEP (*(SORT(.init_array.*)))
+        KEEP (*(.init_array))
+        __init_array_end = .;
+
+        . = ALIGN(4);
+        KEEP(*(.fini))
+
+        . = ALIGN(4);
+        __fini_array_start = .;
+        KEEP (*(.fini_array))
+        KEEP (*(SORT(.fini_array.*)))
+        __fini_array_end = .;
+
+        /* C++ Static constructors/destructors (elf)  */
+        . = ALIGN(4);
+        _ctor_start = .;
+        KEEP (*crtbegin.o(.ctors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .ctors))
+        KEEP (*(SORT(.ctors.*)))
+        KEEP (*crtend.o(.ctors))
+       _ctor_end = .;
+
+        . = ALIGN(4);
+        KEEP (*crtbegin.o(.dtors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .dtors))
+        KEEP (*(SORT(.dtors.*)))
+        KEEP (*crtend.o(.dtors))
+    } > flash
+
+    /* .ARM.exidx is sorted, so has to go in its own output section.  */
+    __exidx_start = .;
+    .ARM.exidx :
+    {
+        *(.ARM.exidx* .gnu.linkonce.armexidx.*)
+    } > flash
+    __exidx_end = .;
+
+	/* .data section: global variables go to ram, but also store a copy to
+       flash to initialize them */
+    .data : ALIGN(8)
+    {
+        _data = .;
+        *(.data)
+        *(.data.*)
+        *(.gnu.linkonce.d.*)
+        . = ALIGN(8);
+        _edata = .;
+    } > ram AT > flash
+    _etext = LOADADDR(.data);
+
+    /* .bss section: uninitialized global variables go to ram */
+    _bss_start = .;
+    .bss :
+    {
+        *(.bss)
+        *(.bss.*)
+        *(.gnu.linkonce.b.*)
+        . = ALIGN(8);
+    } > ram
+    _bss_end = .;
+
+    _end = .;
+    PROVIDE(end = .);
+}
diff --git a/src/bsps/stm32f429zi_death_stack_v1/config/board_options.cmake b/src/bsps/stm32f429zi_death_stack_v1/config/board_options.cmake
new file mode 100644
index 000000000..f9da510a8
--- /dev/null
+++ b/src/bsps/stm32f429zi_death_stack_v1/config/board_options.cmake
@@ -0,0 +1,122 @@
+# Copyright (C) 2023 by Skyward
+#
+# This program is free software; you can redistribute it and/or 
+# it under the terms of the GNU General Public License as published 
+# the Free Software Foundation; either version 2 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.
+#
+# As a special exception, if other files instantiate templates or use
+# macros or inline functions from this file, or you compile this file
+# and link it with other works to produce a work based on this file,
+# this file does not by itself cause the resulting work to be covered
+# by the GNU General Public License. However the source code for this
+# file must still be made available in accordance with the GNU 
+# Public License. This exception does not invalidate any other 
+# why a work based on this file might be covered by the GNU General
+# Public License.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, see <http://www.gnu.org/licenses/>
+
+set(BOARD_NAME stm32f429zi_death_stack_v1)
+set(ARCH_NAME cortexM4_stm32f4)
+
+# Base directories with header files for this board
+set(ARCH_PATH ${KPATH}/arch/${ARCH_NAME}/common)
+set(BOARD_PATH ${BOARDCORE_PATH}/src/bsps/${BOARD_NAME})
+set(BOARD_CONFIG_PATH ${BOARDCORE_PATH}/src/bsps/${BOARD_NAME}/config)
+
+# Specify where to find the board specific config/miosix_settings.h
+set(BOARD_MIOSIX_SETTINGS_PATH ${BOARD_PATH})
+
+# Specify where to find the board specific config/mxgui_settings.h
+set(BOARD_MXGUI_SETTINGS_PATH ${BOARD_PATH})
+
+# Optimization flags:
+# -O0 do no optimization, the default if no optimization level is specified
+# -O or -O1 optimize minimally
+# -O2 optimize more
+# -O3 optimize even more
+# -Ofast optimize very aggressively to the point of breaking the standard
+# -Og Optimize debugging experience, enables optimizations that do not
+# interfere with debugging
+# -Os Optimize for size with -O2 optimizations that do not increase code size
+set(OPT_OPTIMIZATION -O2)
+
+# Boot and linker files
+set(BOOT_FILE ${BOARD_PATH}/core/stage_1_boot.cpp)
+
+# Linker script type, there are three options
+# 1) Code in FLASH, stack + heap in internal RAM (file *_rom.ld)
+#    the most common choice, available for all microcontrollers
+# 2) Code in FLASH, stack + heap in external RAM (file *8m_xram.ld)
+#    You must uncomment -D__ENABLE_XRAM below in this case.
+# set(LINKER_SCRIPT ${BOARD_PATH}/stm32_2m+256k_rom.ld)
+set(LINKER_SCRIPT ${BOARD_PATH}/stm32_2m+8m_xram.ld)
+
+# Uncommenting __ENABLE_XRAM enables the initialization of the external
+# 8MB SDRAM memory.
+set(XRAM -D__ENABLE_XRAM)
+
+# Select clock frequency. Warning: the default clock frequency for
+# this board is 168MHz and not 180MHz because, due to a limitation in
+# the PLL, it is not possible to generate a precise 48MHz output when
+# running the core at 180MHz. If 180MHz is chosen the USB peripheral will
+# NOT WORK and the SDIO and RNG will run ~6% slower (45MHz insteand of 48)
+# Define USE_INTERNAL_CLOCK to bypass the external oscillator
+# set(CLOCK_FREQ -DHSE_VALUE=8000000 -DSYSCLK_FREQ_180MHz=180000000)
+set(CLOCK_FREQ -DHSE_VALUE=8000000 -DSYSCLK_FREQ_168MHz=168000000 -DUSE_INTERNAL_CLOCK)
+# set(CLOCK_FREQ -DHSE_VALUE=8000000 -DSYSCLK_FREQ_100MHz=100000000)
+
+# C++ Exception/rtti support disable flags.
+# To save code size if not using C++ exceptions (nor some STL code which
+# implicitly uses it) uncomment this option.
+# -D__NO_EXCEPTIONS is used by Miosix to know if exceptions are used.
+# set(OPT_EXCEPT -fno-exceptions -fno-rtti -D__NO_EXCEPTIONS)
+
+# Specify a custom flash command
+# This is the program that is invoked when the flash flag (-f or --flash) is
+# used with the Miosix Build System. Use $binary or $hex as placeolders, they
+# will be replaced by the build systems with the binary or hex file repectively.
+# If a command is not specified, the build system will use st-flash if found
+# set(PROGRAM_CMDLINE "here your custom flash command")
+
+# Basic flags
+set(FLAGS_BASE -mcpu=cortex-m4 -mthumb -mfloat-abi=hard -mfpu=fpv4-sp-d16)
+
+# Flags for ASM and linker
+set(AFLAGS_BASE ${FLAGS_BASE})
+set(LFLAGS_BASE ${FLAGS_BASE} -Wl,--gc-sections,-Map,main.map -Wl,-T${LINKER_SCRIPT} ${OPT_EXCEPT} ${OPT_OPTIMIZATION} -nostdlib)
+
+# Flags for C/C++
+string(TOUPPER ${BOARD_NAME} BOARD_UPPER)
+set(CFLAGS_BASE
+    -D_BOARD_${BOARD_UPPER} -D_MIOSIX_BOARDNAME=\"${BOARD_NAME}\"
+    -D_DEFAULT_SOURCE=1 -ffunction-sections -Wall -Werror=return-type -g
+    -D_ARCH_CORTEXM4_STM32F4
+    ${CLOCK_FREQ} ${XRAM} ${SRAM_BOOT} ${FLAGS_BASE} ${OPT_OPTIMIZATION} -c
+)
+set(CXXFLAGS_BASE ${CFLAGS_BASE} ${OPT_EXCEPT})
+
+# Select architecture specific files
+set(ARCH_SRC
+    ${ARCH_PATH}/interfaces-impl/delays.cpp
+    ${ARCH_PATH}/interfaces-impl/gpio_impl.cpp
+    ${ARCH_PATH}/interfaces-impl/portability.cpp
+    ${BOARD_PATH}/interfaces-impl/bsp.cpp
+    ${KPATH}/arch/common/CMSIS/Device/ST/STM32F4xx/Source/Templates/system_stm32f4xx.c
+    ${KPATH}/arch/common/core/interrupts_cortexMx.cpp
+    ${KPATH}/arch/common/core/mpu_cortexMx.cpp
+    ${KPATH}/arch/common/core/stm32f2_f4_l4_f7_h7_os_timer.cpp
+    ${KPATH}/arch/common/drivers/sd_stm32f2_f4_f7.cpp
+    ${KPATH}/arch/common/drivers/serial_stm32.cpp
+    ${KPATH}/arch/common/drivers/stm32_hardware_rng.cpp
+    ${KPATH}/arch/common/drivers/servo_stm32.cpp
+    ${KPATH}/arch/common/drivers/stm32_wd.cpp
+    ${KPATH}/arch/common/drivers/stm32f2_f4_i2c.cpp
+)
diff --git a/src/bsps/stm32f429zi_death_stack_v1/config/board_settings.h b/src/bsps/stm32f429zi_death_stack_v1/config/board_settings.h
new file mode 100644
index 000000000..9dfffe6c7
--- /dev/null
+++ b/src/bsps/stm32f429zi_death_stack_v1/config/board_settings.h
@@ -0,0 +1,74 @@
+/* Copyright (c) 2019 Skyward Experimental Rocketry
+ * Authors: Silvano Seva, Alberto Nidasio
+ *
+ * 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.
+ */
+
+#ifndef BOARD_SETTINGS_H
+#define BOARD_SETTINGS_H
+
+#include "util/version.h"
+
+/**
+ * \internal
+ * Versioning for board_settings.h for out of git tree projects
+ */
+#define BOARD_SETTINGS_VERSION 300
+
+namespace miosix
+{
+
+/**
+ * \addtogroup Settings
+ * \{
+ */
+
+/// Size of stack for main().
+/// The C standard library is stack-heavy (iprintf requires 1KB) but the
+/// STM32F407VG only has 192KB of RAM so there is room for a big 4K stack.
+const unsigned int MAIN_STACK_SIZE = 16 * 1024;
+
+/// Serial port
+const unsigned int defaultSerial      = 1;
+const unsigned int defaultSerialSpeed = 115200;
+const bool defaultSerialFlowctrl      = false;
+#define SERIAL_1_DMA
+// #define SERIAL_2_DMA //Serial 2 can't be used (GPIO conflict), so no DMA
+// #define SERIAL_3_DMA //Serial 3 can't be used (GPIO conflict), so no DMA
+
+// #define I2C_WITH_DMA
+
+// SD card driver
+static const unsigned char sdVoltage = 33;  // Board powered @ 3.3V
+#ifdef __ENABLE_XRAM
+// Reduce SD clock to ~4.8MHz
+#define OVERRIDE_SD_CLOCK_DIVIDER_MAX 8
+#endif  //__ENABLE_XRAM
+// #define SD_ONE_BIT_DATABUS //This board supports 4 bit databus to SD card
+
+/// Analog supply voltage for ADC, DAC, Reset blocks, RCs and PLL
+#define V_DDA_VOLTAGE 3.3f
+
+/**
+ * \}
+ */
+
+}  // namespace miosix
+
+#endif /* BOARD_SETTINGS_H */
diff --git a/src/bsps/stm32f429zi_death_stack_v1/config/miosix_settings.h b/src/bsps/stm32f429zi_death_stack_v1/config/miosix_settings.h
new file mode 100644
index 000000000..dcffa1b92
--- /dev/null
+++ b/src/bsps/stm32f429zi_death_stack_v1/config/miosix_settings.h
@@ -0,0 +1,240 @@
+/* Copyright (c) 2019 Skyward Experimental Rocketry
+ * Authors: Silvano Seva, Alberto Nidasio
+ *
+ * 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.
+ */
+
+#pragma once
+
+// Before you can compile the kernel you have to configure it by editing this
+// file. After that, comment out this line to disable the reminder error.
+// The PARSING_FROM_IDE is because Netbeans gets confused by this, it is never
+// defined when compiling the code.
+#ifndef PARSING_FROM_IDE
+//#error This error is a reminder that you have not edited miosix_settings.h
+// yet.
+#endif  // PARSING_FROM_IDE
+
+/**
+ * \file miosix_settings.h
+ * NOTE: this file contains ONLY configuration options that are not dependent
+ * on architecture specific details. The other options are in the following
+ * files which are included here:
+ * miosix/arch/architecture name/common/arch_settings.h
+ * miosix/config/arch/architecture name/board name/board_settings.h
+ */
+#include "arch_settings.h"
+#include "board_settings.h"
+#include "util/version.h"
+
+/**
+ * \internal
+ * Versioning for miosix_settings.h for out of git tree projects
+ */
+#define MIOSIX_SETTINGS_VERSION 300
+
+namespace miosix
+{
+
+/**
+ * \addtogroup Settings
+ * \{
+ */
+
+//
+// Scheduler options
+//
+
+/// \def SCHED_TYPE_PRIORITY
+/// If uncommented selects the priority scheduler
+/// \def SCHED_TYPE_CONTROL_BASED
+/// If uncommented selects the control based scheduler
+/// \def SCHED_TYPE_EDF
+/// If uncommented selects the EDF scheduler
+// Uncomment only *one* of those
+
+#define SCHED_TYPE_PRIORITY
+//#define SCHED_TYPE_CONTROL_BASED
+//#define SCHED_TYPE_EDF
+
+/// \def WITH_CPU_TIME_COUNTER
+/// Allows to enable/disable CPUTimeCounter to save code size and remove its
+/// overhead from the scheduling process. By default it is not defined
+/// (CPUTimeCounter is disabled).
+//#define WITH_CPU_TIME_COUNTER
+
+//
+// Filesystem options
+//
+
+/// \def WITH_FILESYSTEM
+/// Allows to enable/disable filesystem support to save code size
+/// By default it is defined (filesystem support is enabled)
+#define WITH_FILESYSTEM
+
+/// \def WITH_DEVFS
+/// Allows to enable/disable DevFs support to save code size
+/// By default it is defined (DevFs is enabled)
+#define WITH_DEVFS
+
+/// \def SYNC_AFTER_WRITE
+/// Increases filesystem write robustness. After each write operation the
+/// filesystem is synced so that a power failure happens data is not lost
+/// (unless power failure happens exactly between the write and the sync)
+/// Unfortunately write latency and throughput becomes twice as worse
+/// By default it is defined (slow but safe)
+#define SYNC_AFTER_WRITE
+
+/// Maximum number of open files. Trying to open more will fail.
+/// Cannot be lower than 3, as the first three are stdin, stdout, stderr
+const unsigned char MAX_OPEN_FILES = 8;
+
+/// \def WITH_PROCESSES
+/// If uncommented enables support for processes as well as threads.
+/// This enables the dynamic loader to load elf programs, the extended system
+/// call service and, if the hardware supports it, the MPU to provide memory
+/// isolation of processes
+//#define WITH_PROCESSES
+
+#if defined(WITH_PROCESSES) && defined(__NO_EXCEPTIONS)
+#error Processes require C++ exception support
+#endif  // defined(WITH_PROCESSES) && defined(__NO_EXCEPTIONS)
+
+#if defined(WITH_PROCESSES) && !defined(WITH_FILESYSTEM)
+#error Processes require filesystem support
+#endif  // defined(WITH_PROCESSES) && !defined(WITH_FILESYSTEM)
+
+#if defined(WITH_PROCESSES) && !defined(WITH_DEVFS)
+#error Processes require devfs support
+#endif  // defined(WITH_PROCESSES) && !defined(WITH_DEVFS)
+
+//
+// C/C++ standard library I/O (stdin, stdout and stderr related)
+//
+
+/// \def WITH_BOOTLOG
+/// Uncomment to print bootlogs on stdout.
+/// By default it is defined (bootlogs are printed)
+#define WITH_BOOTLOG
+
+/// \def WITH_ERRLOG
+/// Uncomment for debug information on stdout.
+/// By default it is defined (error information is printed)
+#define WITH_ERRLOG
+
+//
+// Kernel related options (stack sizes, priorities)
+//
+
+/// \def WITH_DEEP_SLEEP
+/// Adds interfaces and required variables to support deep sleep state switch
+/// automatically when peripherals are not required
+//#define WITH_DEEP_SLEEP
+
+/**
+ * \def JTAG_DISABLE_SLEEP
+ * JTAG debuggers lose communication with the device if it enters sleep
+ * mode, so to use debugging it is necessary to disable sleep in the idle
+ * thread. By default it is not defined (idle thread calls sleep).
+ */
+//#define JTAG_DISABLE_SLEEP
+
+#if defined(WITH_DEEP_SLEEP) && defined(JTAG_DISABLE_SLEEP)
+#error Deep sleep cannot work together with jtag
+#endif  // defined(WITH_PROCESSES) && !defined(WITH_DEVFS)
+
+/// Minimum stack size (MUST be divisible by 4)
+const unsigned int STACK_MIN = 256;
+
+/// \internal Size of idle thread stack.
+/// Should be >=STACK_MIN (MUST be divisible by 4)
+const unsigned int STACK_IDLE = 256;
+
+/// Default stack size for pthread_create.
+/// The chosen value is enough to call C standard library functions
+/// such as printf/fopen which are stack-heavy
+const unsigned int STACK_DEFAULT_FOR_PTHREAD = 2048;
+
+/// Maximum size of the RAM image of a process. If a program requires more
+/// the kernel will not run it (MUST be divisible by 4)
+const unsigned int MAX_PROCESS_IMAGE_SIZE = 64 * 1024;
+
+/// Minimum size of the stack for a process. If a program specifies a lower
+/// size the kernel will not run it (MUST be divisible by 4)
+const unsigned int MIN_PROCESS_STACK_SIZE = STACK_MIN;
+
+/// Every userspace thread has two stacks, one for when it is running in
+/// userspace and one for when it is running in kernelspace (that is, while it
+/// is executing system calls). This is the size of the stack for when the
+/// thread is running in kernelspace (MUST be divisible by 4)
+const unsigned int SYSTEM_MODE_PROCESS_STACK_SIZE = 2 * 1024;
+
+/// Number of priorities (MUST be >1)
+/// PRIORITY_MAX-1 is the highest priority, 0 is the lowest. -1 is reserved as
+/// the priority of the idle thread.
+/// The meaning of a thread's priority depends on the chosen scheduler.
+#ifdef SCHED_TYPE_PRIORITY
+// Can be modified, but a high value makes context switches more expensive
+const short int PRIORITY_MAX = 4;
+#elif defined(SCHED_TYPE_CONTROL_BASED)
+// Don't touch, the limit is due to the fixed point implementation
+// It's not needed for if floating point is selected, but kept for consistency
+const short int PRIORITY_MAX = 64;
+#else  // SCHED_TYPE_EDF
+// Doesn't exist for this kind of scheduler
+#endif
+
+/// Priority of main()
+/// The meaning of a thread's priority depends on the chosen scheduler.
+const unsigned char MAIN_PRIORITY = 1;
+
+#ifdef SCHED_TYPE_PRIORITY
+/// Maximum thread time slice in nanoseconds, after which preemption occurs
+const unsigned int MAX_TIME_SLICE = 1000000;
+#endif  // SCHED_TYPE_PRIORITY
+
+//
+// Other low level kernel options. There is usually no need to modify these.
+//
+
+/// \internal Length of wartermark (in bytes) to check stack overflow.
+/// MUST be divisible by 4 and can also be zero.
+/// A high value increases context switch time.
+const unsigned int WATERMARK_LEN = 16;
+
+/// \internal Used to fill watermark
+const unsigned int WATERMARK_FILL = 0xaaaaaaaa;
+
+/// \internal Used to fill stack (for checking stack usage)
+const unsigned int STACK_FILL = 0xbbbbbbbb;
+
+// Compiler version checks
+#if !defined(_MIOSIX_GCC_PATCH_MAJOR) || _MIOSIX_GCC_PATCH_MAJOR < 3
+#error \
+    "You are using a too old or unsupported compiler. Get the latest one from https://miosix.org/wiki/index.php?title=Miosix_Toolchain"
+#endif
+#if _MIOSIX_GCC_PATCH_MAJOR > 3
+#warning "You are using a too new compiler, which may not be supported"
+#endif
+
+/**
+ * \}
+ */
+
+}  // namespace miosix
diff --git a/src/bsps/stm32f429zi_death_stack_v1/core/stage_1_boot.cpp b/src/bsps/stm32f429zi_death_stack_v1/core/stage_1_boot.cpp
new file mode 100644
index 000000000..0abb640f9
--- /dev/null
+++ b/src/bsps/stm32f429zi_death_stack_v1/core/stage_1_boot.cpp
@@ -0,0 +1,372 @@
+/* Copyright (c) 2019 Skyward Experimental Rocketry
+ * Authors: Silvano Seva, Alberto Nidasio
+ *
+ * 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 <string.h>
+
+#include "core/interrupts.h"  //For the unexpected interrupt call
+#include "interfaces/arch_registers.h"
+#include "interfaces/bsp.h"
+#include "kernel/stage_2_boot.h"
+
+/*
+ * startup.cpp
+ * STM32 C++ startup.
+ * NOTE: for stm32f42x devices ONLY.
+ * Supports interrupt handlers in C++ without extern "C"
+ * Developed by Terraneo Federico, based on ST startup code.
+ * Additionally modified to boot Miosix.
+ */
+
+/**
+ * Called by Reset_Handler, performs initialization and calls main.
+ * Never returns.
+ */
+void program_startup() __attribute__((noreturn));
+void program_startup()
+{
+    // Cortex M3 core appears to get out of reset with interrupts already
+    // enabled
+    __disable_irq();
+
+    // These are defined in the linker script
+    extern unsigned char _etext asm("_etext");
+    extern unsigned char _data asm("_data");
+    extern unsigned char _edata asm("_edata");
+    extern unsigned char _bss_start asm("_bss_start");
+    extern unsigned char _bss_end asm("_bss_end");
+
+    // Initialize .data section, clear .bss section
+    unsigned char *etext     = &_etext;
+    unsigned char *data      = &_data;
+    unsigned char *edata     = &_edata;
+    unsigned char *bss_start = &_bss_start;
+    unsigned char *bss_end   = &_bss_end;
+    memcpy(data, etext, edata - data);
+    memset(bss_start, 0, bss_end - bss_start);
+
+    // Move on to stage 2
+    _init();
+
+    // If main returns, reboot
+    NVIC_SystemReset();
+    for (;;)
+        ;
+}
+
+/**
+ * Reset handler, called by hardware immediately after reset
+ */
+void Reset_Handler() __attribute__((__interrupt__, noreturn));
+void Reset_Handler()
+{
+    // SystemInit() is called *before* initializing .data and zeroing .bss
+    // Despite all startup files provided by ST do the opposite, there are three
+    // good reasons to do so:
+    // First, the CMSIS specifications say that SystemInit() must not access
+    // global variables, so it is actually possible to call it before
+    // Second, when running Miosix with the xram linker scripts .data and .bss
+    // are placed in the external RAM, so we *must* call SystemInit(), which
+    // enables xram, before touching .data and .bss
+    // Third, this is a performance improvement since the loops that initialize
+    //.data and zeros .bss now run with the CPU at full speed instead of 8MHz
+    SystemInit();
+// ST does not provide code to initialize the stm32f429-disco SDRAM at boot.
+// Put after SystemInit() as SDRAM is timing-sensitive and needs the full
+// clock speed.
+#ifdef __ENABLE_XRAM
+    miosix::configureSdram();
+#endif  //__ENABLE_XRAM
+
+    /*
+     * Initialize process stack and switch to it.
+     * This is required for booting Miosix, a small portion of the top of the
+     * heap area will be used as stack until the first thread starts. After,
+     * this stack will be abandoned and the process stack will point to the
+     * current thread's stack.
+     */
+    asm volatile(
+        "ldr r0,  =_heap_end          \n\t"
+        "msr psp, r0                  \n\t"
+        "movw r0, #2                  \n\n"  // Privileged, process stack
+        "msr control, r0              \n\t"
+        "isb                          \n\t" ::
+            : "r0");
+
+    program_startup();
+}
+
+/**
+ * All unused interrupts call this function.
+ */
+extern "C" void Default_Handler() { unexpectedInterrupt(); }
+
+// System handlers
+void /*__attribute__((weak))*/ Reset_Handler();      // These interrupts are not
+void /*__attribute__((weak))*/ NMI_Handler();        // weak because they are
+void /*__attribute__((weak))*/ HardFault_Handler();  // surely defined by Miosix
+void /*__attribute__((weak))*/ MemManage_Handler();
+void /*__attribute__((weak))*/ BusFault_Handler();
+void /*__attribute__((weak))*/ UsageFault_Handler();
+void /*__attribute__((weak))*/ SVC_Handler();
+void /*__attribute__((weak))*/ DebugMon_Handler();
+void /*__attribute__((weak))*/ PendSV_Handler();
+void __attribute__((weak)) SysTick_Handler();
+
+// Interrupt handlers
+void __attribute__((weak)) WWDG_IRQHandler();
+void __attribute__((weak)) PVD_IRQHandler();
+void __attribute__((weak)) TAMP_STAMP_IRQHandler();
+void __attribute__((weak)) RTC_WKUP_IRQHandler();
+void __attribute__((weak)) FLASH_IRQHandler();
+void __attribute__((weak)) RCC_IRQHandler();
+void __attribute__((weak)) EXTI0_IRQHandler();
+void __attribute__((weak)) EXTI1_IRQHandler();
+void __attribute__((weak)) EXTI2_IRQHandler();
+void __attribute__((weak)) EXTI3_IRQHandler();
+void __attribute__((weak)) EXTI4_IRQHandler();
+void __attribute__((weak)) DMA1_Stream0_IRQHandler();
+void __attribute__((weak)) DMA1_Stream1_IRQHandler();
+void __attribute__((weak)) DMA1_Stream2_IRQHandler();
+void __attribute__((weak)) DMA1_Stream3_IRQHandler();
+void __attribute__((weak)) DMA1_Stream4_IRQHandler();
+void __attribute__((weak)) DMA1_Stream5_IRQHandler();
+void __attribute__((weak)) DMA1_Stream6_IRQHandler();
+void __attribute__((weak)) ADC_IRQHandler();
+void __attribute__((weak)) CAN1_TX_IRQHandler();
+void __attribute__((weak)) CAN1_RX0_IRQHandler();
+void __attribute__((weak)) CAN1_RX1_IRQHandler();
+void __attribute__((weak)) CAN1_SCE_IRQHandler();
+void __attribute__((weak)) EXTI9_5_IRQHandler();
+void __attribute__((weak)) TIM1_BRK_TIM9_IRQHandler();
+void __attribute__((weak)) TIM1_UP_TIM10_IRQHandler();
+void __attribute__((weak)) TIM1_TRG_COM_TIM11_IRQHandler();
+void __attribute__((weak)) TIM1_CC_IRQHandler();
+void __attribute__((weak)) TIM2_IRQHandler();
+void __attribute__((weak)) TIM3_IRQHandler();
+void __attribute__((weak)) TIM4_IRQHandler();
+void __attribute__((weak)) I2C1_EV_IRQHandler();
+void __attribute__((weak)) I2C1_ER_IRQHandler();
+void __attribute__((weak)) I2C2_EV_IRQHandler();
+void __attribute__((weak)) I2C2_ER_IRQHandler();
+void __attribute__((weak)) SPI1_IRQHandler();
+void __attribute__((weak)) SPI2_IRQHandler();
+void __attribute__((weak)) USART1_IRQHandler();
+void __attribute__((weak)) USART2_IRQHandler();
+void __attribute__((weak)) USART3_IRQHandler();
+void __attribute__((weak)) EXTI15_10_IRQHandler();
+void __attribute__((weak)) RTC_Alarm_IRQHandler();
+void __attribute__((weak)) OTG_FS_WKUP_IRQHandler();
+void __attribute__((weak)) TIM8_BRK_TIM12_IRQHandler();
+void __attribute__((weak)) TIM8_UP_TIM13_IRQHandler();
+void __attribute__((weak)) TIM8_TRG_COM_TIM14_IRQHandler();
+void __attribute__((weak)) TIM8_CC_IRQHandler();
+void __attribute__((weak)) DMA1_Stream7_IRQHandler();
+void __attribute__((weak)) FMC_IRQHandler();
+void __attribute__((weak)) SDIO_IRQHandler();
+void __attribute__((weak)) TIM5_IRQHandler();
+void __attribute__((weak)) SPI3_IRQHandler();
+void __attribute__((weak)) UART4_IRQHandler();
+void __attribute__((weak)) UART5_IRQHandler();
+void __attribute__((weak)) TIM6_DAC_IRQHandler();
+void __attribute__((weak)) TIM7_IRQHandler();
+void __attribute__((weak)) DMA2_Stream0_IRQHandler();
+void __attribute__((weak)) DMA2_Stream1_IRQHandler();
+void __attribute__((weak)) DMA2_Stream2_IRQHandler();
+void __attribute__((weak)) DMA2_Stream3_IRQHandler();
+void __attribute__((weak)) DMA2_Stream4_IRQHandler();
+void __attribute__((weak)) ETH_IRQHandler();
+void __attribute__((weak)) ETH_WKUP_IRQHandler();
+void __attribute__((weak)) CAN2_TX_IRQHandler();
+void __attribute__((weak)) CAN2_RX0_IRQHandler();
+void __attribute__((weak)) CAN2_RX1_IRQHandler();
+void __attribute__((weak)) CAN2_SCE_IRQHandler();
+void __attribute__((weak)) OTG_FS_IRQHandler();
+void __attribute__((weak)) DMA2_Stream5_IRQHandler();
+void __attribute__((weak)) DMA2_Stream6_IRQHandler();
+void __attribute__((weak)) DMA2_Stream7_IRQHandler();
+void __attribute__((weak)) USART6_IRQHandler();
+void __attribute__((weak)) I2C3_EV_IRQHandler();
+void __attribute__((weak)) I2C3_ER_IRQHandler();
+void __attribute__((weak)) OTG_HS_EP1_OUT_IRQHandler();
+void __attribute__((weak)) OTG_HS_EP1_IN_IRQHandler();
+void __attribute__((weak)) OTG_HS_WKUP_IRQHandler();
+void __attribute__((weak)) OTG_HS_IRQHandler();
+void __attribute__((weak)) DCMI_IRQHandler();
+void __attribute__((weak)) CRYP_IRQHandler();
+void __attribute__((weak)) HASH_RNG_IRQHandler();
+void __attribute__((weak)) FPU_IRQHandler();
+void __attribute__((weak)) UART7_IRQHandler();
+void __attribute__((weak)) UART8_IRQHandler();
+void __attribute__((weak)) SPI4_IRQHandler();
+void __attribute__((weak)) SPI5_IRQHandler();
+void __attribute__((weak)) SPI6_IRQHandler();
+void __attribute__((weak)) SAI1_IRQHandler();
+void __attribute__((weak)) LTDC_IRQHandler();
+void __attribute__((weak)) LTDC_ER_IRQHandler();
+void __attribute__((weak)) DMA2D_IRQHandler();
+
+// Stack top, defined in the linker script
+extern char _main_stack_top asm("_main_stack_top");
+
+// Interrupt vectors, must be placed @ address 0x00000000
+// The extern declaration is required otherwise g++ optimizes it out
+extern void (*const __Vectors[])();
+void (*const __Vectors[])() __attribute__((section(".isr_vector"))) = {
+    reinterpret_cast<void (*)()>(&_main_stack_top), /* Stack pointer*/
+    Reset_Handler,                                  /* Reset Handler */
+    NMI_Handler,                                    /* NMI Handler */
+    HardFault_Handler,                              /* Hard Fault Handler */
+    MemManage_Handler,                              /* MPU Fault Handler */
+    BusFault_Handler,                               /* Bus Fault Handler */
+    UsageFault_Handler,                             /* Usage Fault Handler */
+    0,                                              /* Reserved */
+    0,                                              /* Reserved */
+    0,                                              /* Reserved */
+    0,                                              /* Reserved */
+    SVC_Handler,                                    /* SVCall Handler */
+    DebugMon_Handler,                               /* Debug Monitor Handler */
+    0,                                              /* Reserved */
+    PendSV_Handler,                                 /* PendSV Handler */
+    SysTick_Handler,                                /* SysTick Handler */
+
+    /* External Interrupts */
+    WWDG_IRQHandler, PVD_IRQHandler, TAMP_STAMP_IRQHandler, RTC_WKUP_IRQHandler,
+    FLASH_IRQHandler, RCC_IRQHandler, EXTI0_IRQHandler, EXTI1_IRQHandler,
+    EXTI2_IRQHandler, EXTI3_IRQHandler, EXTI4_IRQHandler,
+    DMA1_Stream0_IRQHandler, DMA1_Stream1_IRQHandler, DMA1_Stream2_IRQHandler,
+    DMA1_Stream3_IRQHandler, DMA1_Stream4_IRQHandler, DMA1_Stream5_IRQHandler,
+    DMA1_Stream6_IRQHandler, ADC_IRQHandler, CAN1_TX_IRQHandler,
+    CAN1_RX0_IRQHandler, CAN1_RX1_IRQHandler, CAN1_SCE_IRQHandler,
+    EXTI9_5_IRQHandler, TIM1_BRK_TIM9_IRQHandler, TIM1_UP_TIM10_IRQHandler,
+    TIM1_TRG_COM_TIM11_IRQHandler, TIM1_CC_IRQHandler, TIM2_IRQHandler,
+    TIM3_IRQHandler, TIM4_IRQHandler, I2C1_EV_IRQHandler, I2C1_ER_IRQHandler,
+    I2C2_EV_IRQHandler, I2C2_ER_IRQHandler, SPI1_IRQHandler, SPI2_IRQHandler,
+    USART1_IRQHandler, USART2_IRQHandler, USART3_IRQHandler,
+    EXTI15_10_IRQHandler, RTC_Alarm_IRQHandler, OTG_FS_WKUP_IRQHandler,
+    TIM8_BRK_TIM12_IRQHandler, TIM8_UP_TIM13_IRQHandler,
+    TIM8_TRG_COM_TIM14_IRQHandler, TIM8_CC_IRQHandler, DMA1_Stream7_IRQHandler,
+    FMC_IRQHandler, SDIO_IRQHandler, TIM5_IRQHandler, SPI3_IRQHandler,
+    UART4_IRQHandler, UART5_IRQHandler, TIM6_DAC_IRQHandler, TIM7_IRQHandler,
+    DMA2_Stream0_IRQHandler, DMA2_Stream1_IRQHandler, DMA2_Stream2_IRQHandler,
+    DMA2_Stream3_IRQHandler, DMA2_Stream4_IRQHandler, ETH_IRQHandler,
+    ETH_WKUP_IRQHandler, CAN2_TX_IRQHandler, CAN2_RX0_IRQHandler,
+    CAN2_RX1_IRQHandler, CAN2_SCE_IRQHandler, OTG_FS_IRQHandler,
+    DMA2_Stream5_IRQHandler, DMA2_Stream6_IRQHandler, DMA2_Stream7_IRQHandler,
+    USART6_IRQHandler, I2C3_EV_IRQHandler, I2C3_ER_IRQHandler,
+    OTG_HS_EP1_OUT_IRQHandler, OTG_HS_EP1_IN_IRQHandler, OTG_HS_WKUP_IRQHandler,
+    OTG_HS_IRQHandler, DCMI_IRQHandler, CRYP_IRQHandler, HASH_RNG_IRQHandler,
+    FPU_IRQHandler, UART7_IRQHandler, UART8_IRQHandler, SPI4_IRQHandler,
+    SPI5_IRQHandler, SPI6_IRQHandler, SAI1_IRQHandler, LTDC_IRQHandler,
+    LTDC_ER_IRQHandler, DMA2D_IRQHandler};
+
+#pragma weak SysTick_IRQHandler            = Default_Handler
+#pragma weak WWDG_IRQHandler               = Default_Handler
+#pragma weak PVD_IRQHandler                = Default_Handler
+#pragma weak TAMP_STAMP_IRQHandler         = Default_Handler
+#pragma weak RTC_WKUP_IRQHandler           = Default_Handler
+#pragma weak FLASH_IRQHandler              = Default_Handler
+#pragma weak RCC_IRQHandler                = Default_Handler
+#pragma weak EXTI0_IRQHandler              = Default_Handler
+#pragma weak EXTI1_IRQHandler              = Default_Handler
+#pragma weak EXTI2_IRQHandler              = Default_Handler
+#pragma weak EXTI3_IRQHandler              = Default_Handler
+#pragma weak EXTI4_IRQHandler              = Default_Handler
+#pragma weak DMA1_Stream0_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream1_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream2_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream3_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream4_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream5_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream6_IRQHandler       = Default_Handler
+#pragma weak ADC_IRQHandler                = Default_Handler
+#pragma weak CAN1_TX_IRQHandler            = Default_Handler
+#pragma weak CAN1_RX0_IRQHandler           = Default_Handler
+#pragma weak CAN1_RX1_IRQHandler           = Default_Handler
+#pragma weak CAN1_SCE_IRQHandler           = Default_Handler
+#pragma weak EXTI9_5_IRQHandler            = Default_Handler
+#pragma weak TIM1_BRK_TIM9_IRQHandler      = Default_Handler
+#pragma weak TIM1_UP_TIM10_IRQHandler      = Default_Handler
+#pragma weak TIM1_TRG_COM_TIM11_IRQHandler = Default_Handler
+#pragma weak TIM1_CC_IRQHandler            = Default_Handler
+#pragma weak TIM2_IRQHandler               = Default_Handler
+#pragma weak TIM3_IRQHandler               = Default_Handler
+#pragma weak TIM4_IRQHandler               = Default_Handler
+#pragma weak I2C1_EV_IRQHandler            = Default_Handler
+#pragma weak I2C1_ER_IRQHandler            = Default_Handler
+#pragma weak I2C2_EV_IRQHandler            = Default_Handler
+#pragma weak I2C2_ER_IRQHandler            = Default_Handler
+#pragma weak SPI1_IRQHandler               = Default_Handler
+#pragma weak SPI2_IRQHandler               = Default_Handler
+// #pragma weak USART1_IRQHandler             = Default_Handler
+// #pragma weak USART2_IRQHandler             = Default_Handler
+// #pragma weak USART3_IRQHandler             = Default_Handler
+#pragma weak EXTI15_10_IRQHandler          = Default_Handler
+#pragma weak RTC_Alarm_IRQHandler          = Default_Handler
+#pragma weak OTG_FS_WKUP_IRQHandler        = Default_Handler
+#pragma weak TIM8_BRK_TIM12_IRQHandler     = Default_Handler
+#pragma weak TIM8_UP_TIM13_IRQHandler      = Default_Handler
+#pragma weak TIM8_TRG_COM_TIM14_IRQHandler = Default_Handler
+#pragma weak TIM8_CC_IRQHandler            = Default_Handler
+#pragma weak DMA1_Stream7_IRQHandler       = Default_Handler
+#pragma weak FMC_IRQHandler                = Default_Handler
+#pragma weak SDIO_IRQHandler               = Default_Handler
+#pragma weak TIM5_IRQHandler               = Default_Handler
+#pragma weak SPI3_IRQHandler               = Default_Handler
+// #pragma weak UART4_IRQHandler              = Default_Handler
+// #pragma weak UART5_IRQHandler              = Default_Handler
+#pragma weak TIM6_DAC_IRQHandler     = Default_Handler
+#pragma weak TIM7_IRQHandler         = Default_Handler
+#pragma weak DMA2_Stream0_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream1_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream2_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream3_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream4_IRQHandler = Default_Handler
+#pragma weak ETH_IRQHandler          = Default_Handler
+#pragma weak ETH_WKUP_IRQHandler     = Default_Handler
+#pragma weak CAN2_TX_IRQHandler      = Default_Handler
+#pragma weak CAN2_RX0_IRQHandler     = Default_Handler
+#pragma weak CAN2_RX1_IRQHandler     = Default_Handler
+#pragma weak CAN2_SCE_IRQHandler     = Default_Handler
+#pragma weak OTG_FS_IRQHandler       = Default_Handler
+#pragma weak DMA2_Stream5_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream6_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream7_IRQHandler = Default_Handler
+// #pragma weak USART6_IRQHandler             = Default_Handler
+#pragma weak I2C3_EV_IRQHandler        = Default_Handler
+#pragma weak I2C3_ER_IRQHandler        = Default_Handler
+#pragma weak OTG_HS_EP1_OUT_IRQHandler = Default_Handler
+#pragma weak OTG_HS_EP1_IN_IRQHandler  = Default_Handler
+#pragma weak OTG_HS_WKUP_IRQHandler    = Default_Handler
+#pragma weak OTG_HS_IRQHandler         = Default_Handler
+#pragma weak DCMI_IRQHandler           = Default_Handler
+#pragma weak CRYP_IRQHandler           = Default_Handler
+#pragma weak HASH_RNG_IRQHandler       = Default_Handler
+#pragma weak FPU_IRQHandler            = Default_Handler
+// #pragma weak UART7_IRQHandler              = Default_Handler
+// #pragma weak UART8_IRQHandler              = Default_Handler
+#pragma weak SPI4_IRQHandler    = Default_Handler
+#pragma weak SPI5_IRQHandler    = Default_Handler
+#pragma weak SPI6_IRQHandler    = Default_Handler
+#pragma weak SAI1_IRQHandler    = Default_Handler
+#pragma weak LTDC_IRQHandler    = Default_Handler
+#pragma weak LTDC_ER_IRQHandler = Default_Handler
+#pragma weak DMA2D_IRQHandler   = Default_Handler
diff --git a/src/bsps/stm32f429zi_death_stack_v1/interfaces-impl/arch_registers_impl.h b/src/bsps/stm32f429zi_death_stack_v1/interfaces-impl/arch_registers_impl.h
new file mode 100644
index 000000000..0ff1e6919
--- /dev/null
+++ b/src/bsps/stm32f429zi_death_stack_v1/interfaces-impl/arch_registers_impl.h
@@ -0,0 +1,34 @@
+/* Copyright (c) 2019 Skyward Experimental Rocketry
+ * Authors: Silvano Seva, Alberto Nidasio
+ *
+ * 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.
+ */
+
+#ifndef ARCH_REGISTERS_IMPL_H
+#define ARCH_REGISTERS_IMPL_H
+
+// Always include stm32f4xx.h before core_cm4.h, there's some nasty dependency
+#define STM32F429xx
+#include "CMSIS/Device/ST/STM32F4xx/Include/stm32f4xx.h"
+#include "CMSIS/Device/ST/STM32F4xx/Include/system_stm32f4xx.h"
+#include "CMSIS/Include/core_cm4.h"
+
+#define RCC_SYNC() __DSB()  // Workaround for a bug in stm32f42x
+
+#endif  // ARCH_REGISTERS_IMPL_H
diff --git a/src/bsps/stm32f429zi_death_stack_v1/interfaces-impl/bsp.cpp b/src/bsps/stm32f429zi_death_stack_v1/interfaces-impl/bsp.cpp
new file mode 100644
index 000000000..646ee99b2
--- /dev/null
+++ b/src/bsps/stm32f429zi_death_stack_v1/interfaces-impl/bsp.cpp
@@ -0,0 +1,483 @@
+/* Copyright (c) 2019 Skyward Experimental Rocketry
+ * Authors: Silvano Seva, Alberto Nidasio
+ *
+ * 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.
+ */
+
+/***********************************************************************
+ * bsp.cpp Part of the Miosix Embedded OS.
+ * Board support package, this file initializes hardware.
+ ************************************************************************/
+
+#include "interfaces/bsp.h"
+
+#include <inttypes.h>
+#include <sys/ioctl.h>
+
+#include <cstdlib>
+
+#include "board_settings.h"
+#include "config/miosix_settings.h"
+#include "drivers/sd_stm32f2_f4_f7.h"
+#include "drivers/serial.h"
+#include "filesystem/console/console_device.h"
+#include "filesystem/file_access.h"
+#include "hwmapping.h"
+#include "interfaces/arch_registers.h"
+#include "interfaces/delays.h"
+#include "interfaces/portability.h"
+#include "kernel/kernel.h"
+#include "kernel/logging.h"
+#include "kernel/sync.h"
+
+namespace miosix
+{
+
+//
+// Initialization
+//
+
+/**
+ * The example code from ST checks for the busy flag after each command.
+ * Interestingly I couldn't find any mention of this in the datsheet.
+ */
+static void sdramCommandWait()
+{
+    for (int i = 0; i < 0xffff; i++)
+        if ((FMC_Bank5_6->SDSR & FMC_SDSR_BUSY) == 0)
+            return;
+}
+
+#ifdef SDRAM_ISSI
+
+void configureSdram()
+{
+    // Enable all gpios, passing clock
+    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN | RCC_AHB1ENR_GPIOBEN |
+                    RCC_AHB1ENR_GPIOCEN | RCC_AHB1ENR_GPIODEN |
+                    RCC_AHB1ENR_GPIOEEN | RCC_AHB1ENR_GPIOFEN |
+                    RCC_AHB1ENR_GPIOGEN | RCC_AHB1ENR_GPIOHEN;
+    RCC_SYNC();
+
+    // First, configure SDRAM GPIOs
+    GPIOB->AFR[0] = 0x0cc00000;
+    GPIOC->AFR[0] = 0x0000000c;
+    GPIOD->AFR[0] = 0x000000cc;
+    GPIOD->AFR[1] = 0xcc000ccc;
+    GPIOE->AFR[0] = 0xc00000cc;
+    GPIOE->AFR[1] = 0xcccccccc;
+    GPIOF->AFR[0] = 0x00cccccc;
+    GPIOF->AFR[1] = 0xccccc000;
+    GPIOG->AFR[0] = 0x00cc00cc;
+    GPIOG->AFR[1] = 0xc000000c;
+
+    GPIOB->MODER = 0x00002800;
+    GPIOC->MODER = 0x00000002;
+    GPIOD->MODER = 0xa02a000a;
+    GPIOE->MODER = 0xaaaa800a;
+    GPIOF->MODER = 0xaa800aaa;
+    GPIOG->MODER = 0x80020a0a;
+
+    GPIOA->OSPEEDR = 0xaaaaaaaa;  // Default to 50MHz speed for all GPIOs...
+    GPIOB->OSPEEDR =
+        0xaaaaaaaa | 0x00003c00;  //...but 100MHz for the SDRAM pins
+    GPIOC->OSPEEDR = 0xaaaaaaaa | 0x00000003;
+    GPIOD->OSPEEDR = 0xaaaaaaaa | 0xf03f000f;
+    GPIOE->OSPEEDR = 0xaaaaaaaa | 0xffffc00f;
+    GPIOF->OSPEEDR = 0xaaaaaaaa | 0xffc00fff;
+    GPIOG->OSPEEDR = 0xaaaaaaaa | 0xc0030f0f;
+    GPIOH->OSPEEDR = 0xaaaaaaaa;
+
+    // Since we'we un-configured PB3/PB4 from the default at boot TDO,NTRST,
+    // finish the job and remove the default pull-up
+    GPIOB->PUPDR = 0;
+
+    // Second, actually start the SDRAM controller
+    RCC->AHB3ENR |= RCC_AHB3ENR_FMCEN;
+    RCC_SYNC();
+
+    // SDRAM is a IS42S16400J -7 speed grade, connected to bank 2 (0xd0000000)
+    // Some bits in SDCR[1] are don't care, and the have to be set in SDCR[0],
+    // they aren't just don't care, the controller will fail if they aren't at 0
+    FMC_Bank5_6->SDCR[0] = FMC_SDCR1_SDCLK_1  // SDRAM runs @ half CPU frequency
+                           | FMC_SDCR1_RBURST  // Enable read burst
+                           | 0;              //  0 delay between reads after CAS
+    FMC_Bank5_6->SDCR[1] = 0                 //  8 bit column address
+                           | FMC_SDCR1_NR_0  // 12 bit row address
+                           | FMC_SDCR1_MWID_0  // 16 bit data bus
+                           | FMC_SDCR1_NB      //  4 banks
+                           |
+                           FMC_SDCR1_CAS_1;  //  2 cycle CAS latency (F<133MHz)
+
+#ifdef SYSCLK_FREQ_180MHz
+    // One SDRAM clock cycle is 11.1ns
+    // Some bits in SDTR[1] are don't care, and the have to be set in SDTR[0],
+    // they aren't just don't care, the controller will fail if they aren't at 0
+    FMC_Bank5_6->SDTR[0] = (6 - 1) << 12     // 6 cycle TRC  (66.6ns>63ns)
+                           | (2 - 1) << 20;  // 2 cycle TRP  (22.2ns>15ns)
+    FMC_Bank5_6->SDTR[1] = (2 - 1) << 0      // 2 cycle TMRD
+                           | (7 - 1) << 4    // 7 cycle TXSR (77.7ns>70ns)
+                           | (4 - 1) << 8    // 4 cycle TRAS (44.4ns>42ns)
+                           | (2 - 1) << 16   // 2 cycle TWR
+                           | (2 - 1) << 24;  // 2 cycle TRCD (22.2ns>15ns)
+#elif defined(SYSCLK_FREQ_168MHz)
+    // One SDRAM clock cycle is 11.9ns
+    // Some bits in SDTR[1] are don't care, and the have to be set in SDTR[0],
+    // they aren't just don't care, the controller will fail if they aren't at 0
+    FMC_Bank5_6->SDTR[0] = (6 - 1) << 12     // 6 cycle TRC  (71.4ns>63ns)
+                           | (2 - 1) << 20;  // 2 cycle TRP  (23.8ns>15ns)
+    FMC_Bank5_6->SDTR[1] = (2 - 1) << 0      // 2 cycle TMRD
+                           | (6 - 1) << 4    // 6 cycle TXSR (71.4ns>70ns)
+                           | (4 - 1) << 8    // 4 cycle TRAS (47.6ns>42ns)
+                           | (2 - 1) << 16   // 2 cycle TWR
+                           | (2 - 1) << 24;  // 2 cycle TRCD (23.8ns>15ns)
+#else
+#error No SDRAM timings for this clock
+#endif
+
+    FMC_Bank5_6->SDCMR = FMC_SDCMR_CTB2  // Enable bank 2
+                         | 1;            // MODE=001 clock enabled
+    sdramCommandWait();
+
+    // ST and SDRAM datasheet agree a 100us delay is required here.
+    delayUs(100);
+
+    FMC_Bank5_6->SDCMR = FMC_SDCMR_CTB2  // Enable bank 2
+                         | 2;            // MODE=010 precharge all command
+    sdramCommandWait();
+
+    FMC_Bank5_6->SDCMR = (8 - 1) << 5      // NRFS=8 SDRAM datasheet says
+                                           // "at least two AUTO REFRESH cycles"
+                         | FMC_SDCMR_CTB2  // Enable bank 2
+                         | 3;              // MODE=011 auto refresh
+    sdramCommandWait();
+
+    FMC_Bank5_6->SDCMR = 0x220 << 9  // MRD=0x220:CAS latency=2 burst len=1
+                         | FMC_SDCMR_CTB2  // Enable bank 2
+                         | 4;              // MODE=100 load mode register
+    sdramCommandWait();
+
+// 64ms/4096=15.625us
+#ifdef SYSCLK_FREQ_180MHz
+    // 15.625us*90MHz=1406-20=1386
+    FMC_Bank5_6->SDRTR = 1386 << 1;
+#elif defined(SYSCLK_FREQ_168MHz)
+    // 15.625us*84MHz=1312-20=1292
+    FMC_Bank5_6->SDRTR = 1292 << 1;
+#else
+#error No refresh timings for this clock
+#endif
+}
+
+#else
+
+void configureSdram()
+{
+    // Enable all gpios
+    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN | RCC_AHB1ENR_GPIOBEN |
+                    RCC_AHB1ENR_GPIOCEN | RCC_AHB1ENR_GPIODEN |
+                    RCC_AHB1ENR_GPIOEEN | RCC_AHB1ENR_GPIOFEN |
+                    RCC_AHB1ENR_GPIOGEN | RCC_AHB1ENR_GPIOHEN;
+    RCC_SYNC();
+
+    // First, configure SDRAM GPIOs
+    GPIOB->AFR[0] = 0x0cc00000;
+    GPIOC->AFR[0] = 0x0000000c;
+    GPIOD->AFR[0] = 0x000000cc;
+    GPIOD->AFR[1] = 0xcc000ccc;
+    GPIOE->AFR[0] = 0xc00000cc;
+    GPIOE->AFR[1] = 0xcccccccc;
+    GPIOF->AFR[0] = 0x00cccccc;
+    GPIOF->AFR[1] = 0xccccc000;
+    GPIOG->AFR[0] = 0x00cc00cc;
+    GPIOG->AFR[1] = 0xc000000c;
+
+    GPIOB->MODER = 0x00002800;
+    GPIOC->MODER = 0x00000002;
+    GPIOD->MODER = 0xa02a000a;
+    GPIOE->MODER = 0xaaaa800a;
+    GPIOF->MODER = 0xaa800aaa;
+    GPIOG->MODER = 0x80020a0a;
+
+    GPIOA->OSPEEDR = 0xaaaaaaaa;  // Default to 50MHz speed for all GPIOs...
+    GPIOB->OSPEEDR =
+        0xaaaaaaaa | 0x00003c00;  //...but 100MHz for the SDRAM pins
+    GPIOC->OSPEEDR = 0xaaaaaaaa | 0x00000003;
+    GPIOD->OSPEEDR = 0xaaaaaaaa | 0xf03f000f;
+    GPIOE->OSPEEDR = 0xaaaaaaaa | 0xffffc00f;
+    GPIOF->OSPEEDR = 0xaaaaaaaa | 0xffc00fff;
+    GPIOG->OSPEEDR = 0xaaaaaaaa | 0xc0030f0f;
+    GPIOH->OSPEEDR = 0xaaaaaaaa;
+
+    // Since we'we un-configured PB3/PB4 from the default at boot TDO,NTRST,
+    // finish the job and remove the default pull-up
+    GPIOB->PUPDR = 0;
+
+    // Second, actually start the SDRAM controller
+    RCC->AHB3ENR |= RCC_AHB3ENR_FMCEN;
+    RCC_SYNC();
+
+    // SDRAM is a AS4C4M16SA-6TAN, connected to bank 2 (0xd0000000)
+    // Some bits in SDCR[1] are don't care, and the have to be set in SDCR[0],
+    // they aren't just don't care, the controller will fail if they aren't at 0
+    FMC_Bank5_6->SDCR[0] = FMC_SDCR1_SDCLK_1  // SDRAM runs @ half CPU frequency
+                           | FMC_SDCR1_RBURST  // Enable read burst
+                           | 0;  //  0 delay between reads after CAS
+    FMC_Bank5_6->SDCR[1] =
+        0                   //  8 bit column address
+        | FMC_SDCR1_NR_0    // 12 bit row address
+        | FMC_SDCR1_MWID_0  // 16 bit data bus
+        | FMC_SDCR1_NB      //  4 banks
+        | FMC_SDCR1_CAS_1;  //  2 cycle CAS latency (TCK>9+0.5ns [1])
+
+#ifdef SYSCLK_FREQ_180MHz
+    // One SDRAM clock cycle is 11.1ns
+    // Some bits in SDTR[1] are don't care, and the have to be set in SDTR[0],
+    // they aren't just don't care, the controller will fail if they aren't at 0
+    FMC_Bank5_6->SDTR[0] = (6 - 1) << 12     // 6 cycle TRC  (66.6ns>60ns)
+                           | (2 - 1) << 20;  // 2 cycle TRP  (22.2ns>18ns)
+    FMC_Bank5_6->SDTR[1] = (2 - 1) << 0      // 2 cycle TMRD
+                           |
+                           (6 - 1) << 4  // 6 cycle TXSR (66.6ns>61.5+0.5ns [1])
+                           | (4 - 1) << 8    // 4 cycle TRAS (44.4ns>42ns)
+                           | (2 - 1) << 16   // 2 cycle TWR
+                           | (2 - 1) << 24;  // 2 cycle TRCD (22.2ns>18ns)
+#elif defined(SYSCLK_FREQ_168MHz)
+    // One SDRAM clock cycle is 11.9ns
+    // Some bits in SDTR[1] are don't care, and the have to be set in SDTR[0],
+    // they aren't just don't care, the controller will fail if they aren't at 0
+    FMC_Bank5_6->SDTR[0] = (6 - 1) << 12     // 6 cycle TRC  (71.4ns>60ns)
+                           | (2 - 1) << 20;  // 2 cycle TRP  (23.8ns>18ns)
+    FMC_Bank5_6->SDTR[1] = (2 - 1) << 0      // 2 cycle TMRD
+                           |
+                           (6 - 1) << 4  // 6 cycle TXSR (71.4ns>61.5+0.5ns [1])
+                           | (4 - 1) << 8    // 4 cycle TRAS (47.6ns>42ns)
+                           | (2 - 1) << 16   // 2 cycle TWR
+                           | (2 - 1) << 24;  // 2 cycle TRCD (23.8ns>18ns)
+#else
+#error No SDRAM timings for this clock
+#endif
+    // NOTE [1]: the timings for TCK and TIS depend on rise and fall times
+    //(see note 9 and 10 on datasheet). Timings are adjusted accordingly to
+    // the measured 2ns rise and fall time
+
+    FMC_Bank5_6->SDCMR = FMC_SDCMR_CTB2  // Enable bank 2
+                         | 1;            // MODE=001 clock enabled
+    sdramCommandWait();
+
+    // SDRAM datasheet requires 200us delay here (note 11), here we use 10% more
+    delayUs(220);
+
+    FMC_Bank5_6->SDCMR = FMC_SDCMR_CTB2  // Enable bank 2
+                         | 2;            // MODE=010 precharge all command
+    sdramCommandWait();
+
+    // FIXME: note 11 on SDRAM datasheet says extended mode register must be
+    // set, but the ST datasheet does not seem to explain how
+    FMC_Bank5_6->SDCMR = 0x220 << 9  // MRD=0x220:CAS latency=2 burst len=1
+                         | FMC_SDCMR_CTB2  // Enable bank 2
+                         | 4;              // MODE=100 load mode register
+    sdramCommandWait();
+
+    FMC_Bank5_6->SDCMR = (4 - 1) << 5  // NRFS=8 SDRAM datasheet requires
+                                       // a minimum of 2 cycles, here we use 4
+                         | FMC_SDCMR_CTB2  // Enable bank 2
+                         | 3;              // MODE=011 auto refresh
+    sdramCommandWait();
+
+// 32ms/4096=7.8125us, but datasheet says to round that to 7.8us
+#ifdef SYSCLK_FREQ_180MHz
+    // 7.8us*90MHz=702-20=682
+    FMC_Bank5_6->SDRTR = 682 << 1;
+#elif defined(SYSCLK_FREQ_168MHz)
+    // 7.8us*84MHz=655-20=635
+    FMC_Bank5_6->SDRTR = 635 << 1;
+#else
+#error No refresh timings for this clock
+#endif
+}
+
+#endif
+
+void IRQbspInit()
+{
+// If using SDRAM GPIOs are enabled by configureSdram(), else enable them here
+#ifndef __ENABLE_XRAM
+    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN | RCC_AHB1ENR_GPIOBEN |
+                    RCC_AHB1ENR_GPIOCEN | RCC_AHB1ENR_GPIODEN |
+                    RCC_AHB1ENR_GPIOEEN | RCC_AHB1ENR_GPIOFEN |
+                    RCC_AHB1ENR_GPIOGEN | RCC_AHB1ENR_GPIOHEN;
+    RCC_SYNC();
+#endif  //__ENABLE_XRAM
+
+    using namespace interfaces;
+    spi1::sck::mode(Mode::ALTERNATE);
+    spi1::sck::alternateFunction(5);
+    spi1::miso::mode(Mode::ALTERNATE);
+    spi1::miso::alternateFunction(5);
+    spi1::mosi::mode(Mode::ALTERNATE);
+    spi1::mosi::alternateFunction(5);
+
+    spi2::sck::mode(Mode::ALTERNATE);
+    spi2::sck::alternateFunction(5);
+    spi2::miso::mode(Mode::ALTERNATE);
+    spi2::miso::alternateFunction(5);
+    spi2::mosi::mode(Mode::ALTERNATE);
+    spi2::mosi::alternateFunction(5);
+
+    // Software I2C
+    i2c::scl::high();
+    i2c::scl::mode(Mode::OPEN_DRAIN);
+    i2c::sda::high();
+    i2c::sda::mode(Mode::OPEN_DRAIN);
+
+    uart4::rx::mode(Mode::ALTERNATE);
+    uart4::rx::alternateFunction(8);
+    uart4::tx::mode(Mode::ALTERNATE);
+    uart4::tx::alternateFunction(8);
+
+    can::rx::mode(Mode::ALTERNATE);
+    can::rx::alternateFunction(9);
+    can::tx::mode(Mode::ALTERNATE);
+    can::tx::alternateFunction(9);
+
+    using namespace sensors;
+    adis16405::cs::mode(Mode::OUTPUT);
+    adis16405::cs::high();
+    adis16405::ckIn::mode(Mode::ALTERNATE);
+    adis16405::ckIn::alternateFunction(2);
+    adis16405::dio1::mode(Mode::INPUT);
+    adis16405::rst::mode(Mode::OUTPUT);
+    adis16405::rst::high();
+
+    ad7994::ab::mode(Mode::INPUT);
+    ad7994::nconvst::mode(Mode::OUTPUT);
+
+    mpu9250::cs::mode(Mode::OUTPUT);
+    mpu9250::cs::high();
+    mpu9250::intr::mode(Mode::INPUT);
+
+    ms5803::cs::mode(Mode::OUTPUT);
+    ms5803::cs::high();
+
+    lsm6ds3h::cs::mode(Mode::OUTPUT);
+    lsm6ds3h::cs::high();
+    lsm6ds3h::int1::mode(Mode::INPUT);
+    lsm6ds3h::int2::mode(Mode::INPUT);
+
+    using namespace inputs;
+    vbat::mode(Mode::INPUT_ANALOG);
+    lp_dtch::mode(Mode::INPUT);
+    btn_open::mode(Mode::INPUT);
+    btn_close::mode(Mode::INPUT);
+
+    using namespace nosecone;
+    motEn::mode(Mode::OUTPUT);
+    motEn::low();
+
+    motP1::mode(Mode::OUTPUT);
+    motP1::low();
+    motP2::mode(Mode::OUTPUT);
+    motP2::low();
+
+    rogP1::mode(Mode::ALTERNATE);
+    rogP1::alternateFunction(2);
+
+    rogP2::mode(Mode::ALTERNATE);
+    rogP2::alternateFunction(2);
+
+    nc_dtch::mode(Mode::INPUT);
+    motor_act::mode(Mode::INPUT);
+
+    motor_3v::mode(Mode::OUTPUT);
+    motor_3v::high();
+
+    motor_gnd::mode(Mode::OUTPUT);
+    motor_gnd::low();
+
+    using namespace actuators;
+    tcPwm::mode(Mode::ALTERNATE);
+    tcPwm::alternateFunction(3);
+
+    thCut1::ena::mode(Mode::OUTPUT);
+    thCut1::ena::low();
+    thCut1::csens::mode(Mode::INPUT_ANALOG);
+
+    thCut2::ena::mode(Mode::OUTPUT);
+    thCut2::ena::low();
+    thCut2::csens::mode(Mode::INPUT_ANALOG);
+
+    misc::buzz::mode(Mode::OUTPUT);
+    misc::buzz::low();
+
+    xbee::cs::mode(Mode::OUTPUT);
+    xbee::cs::high();
+    xbee::attn::mode(Mode::INPUT_PULL_UP);
+    xbee::sleep_status::mode(Mode::INPUT);
+
+    xbee::reset::mode(Mode::OPEN_DRAIN);
+    xbee::reset::high();
+
+    // Led blink
+    led1::mode(Mode::OUTPUT);
+
+    ledOn();
+    delayMs(100);
+    ledOff();
+
+    DefaultConsole::instance().IRQset(intrusive_ref_ptr<Device>(
+        new STM32Serial(defaultSerial, defaultSerialSpeed,
+                        defaultSerialFlowctrl ? STM32Serial::RTSCTS
+                                              : STM32Serial::NOFLOWCTRL)));
+}
+
+void bspInit2()
+{
+#ifdef WITH_FILESYSTEM
+    // PA2,PA3
+    intrusive_ref_ptr<DevFs> devFs =
+        basicFilesystemSetup(SDIODriver::instance());
+    devFs->addDevice("gps",
+                     intrusive_ref_ptr<Device>(new STM32Serial(2, 115200)));
+#endif  // WITH_FILESYSTEM
+}
+
+//
+// Shutdown and reboot
+//
+
+/**
+ * For safety reasons, we never want the board to shutdown.
+ * When requested to shutdown, we reboot instead.
+ */
+void shutdown() { reboot(); }
+
+void reboot()
+{
+    ioctl(STDOUT_FILENO, IOCTL_SYNC, 0);
+
+#ifdef WITH_FILESYSTEM
+    FilesystemManager::instance().umountAll();
+#endif  // WITH_FILESYSTEM
+
+    disableInterrupts();
+    miosix_private::IRQsystemReboot();
+}
+
+}  // namespace miosix
diff --git a/src/bsps/stm32f429zi_death_stack_v1/interfaces-impl/bsp_impl.h b/src/bsps/stm32f429zi_death_stack_v1/interfaces-impl/bsp_impl.h
new file mode 100644
index 000000000..b3fa83223
--- /dev/null
+++ b/src/bsps/stm32f429zi_death_stack_v1/interfaces-impl/bsp_impl.h
@@ -0,0 +1,66 @@
+/* Copyright (c) 2019 Skyward Experimental Rocketry
+ * Authors: Silvano Seva, Alberto Nidasio
+ *
+ * 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.
+ */
+
+/***********************************************************************
+ * bsp_impl.h Part of the Miosix Embedded OS.
+ * Board support package, this file initializes hardware.
+ ************************************************************************/
+
+#ifndef BSP_IMPL_H
+#define BSP_IMPL_H
+
+#include "config/miosix_settings.h"
+#include "drivers/stm32_hardware_rng.h"
+#include "hwmapping.h"
+#include "interfaces/gpio.h"
+
+namespace miosix
+{
+
+/**
+\addtogroup Hardware
+\{
+*/
+
+/**
+ * \internal
+ * Called by stage_1_boot.cpp to enable the SDRAM before initializing .data/.bss
+ * Requires the CPU clock to be already configured (running from the PLL)
+ */
+void configureSdram();
+
+/**
+ * \internal
+ * used by the ledOn() and ledOff() implementation
+ */
+
+inline void ledOn() { led1::high(); }
+
+inline void ledOff() { led1::low(); }
+
+/**
+\}
+*/
+
+}  // namespace miosix
+
+#endif  // BSP_IMPL_H
diff --git a/src/bsps/stm32f429zi_death_stack_v1/interfaces-impl/hwmapping.h b/src/bsps/stm32f429zi_death_stack_v1/interfaces-impl/hwmapping.h
new file mode 100644
index 000000000..f3e2accb6
--- /dev/null
+++ b/src/bsps/stm32f429zi_death_stack_v1/interfaces-impl/hwmapping.h
@@ -0,0 +1,192 @@
+/* Copyright (c) 2019 Skyward Experimental Rocketry
+ * Authors: Silvano Seva, Alberto Nidasio
+ *
+ * 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.
+ */
+
+#ifndef HWMAPPING_H
+#define HWMAPPING_H
+
+#include "interfaces/gpio.h"
+
+namespace miosix
+{
+
+namespace interfaces
+{
+
+namespace spi1
+{
+using sck  = Gpio<GPIOA_BASE, 5>;
+using miso = Gpio<GPIOA_BASE, 6>;
+using mosi = Gpio<GPIOA_BASE, 7>;
+}  // namespace spi1
+
+namespace spi2
+{
+using sck  = Gpio<GPIOB_BASE, 13>;
+using miso = Gpio<GPIOB_BASE, 14>;
+using mosi = Gpio<GPIOB_BASE, 15>;
+}  // namespace spi2
+
+namespace i2c
+{
+using scl = Gpio<GPIOB_BASE, 8>;
+using sda = Gpio<GPIOB_BASE, 9>;
+}  // namespace i2c
+
+namespace uart4
+{
+using tx = Gpio<GPIOA_BASE, 0>;
+using rx = Gpio<GPIOA_BASE, 1>;
+}  // namespace uart4
+
+namespace can
+{
+using rx = Gpio<GPIOA_BASE, 11>;
+using tx = Gpio<GPIOA_BASE, 12>;
+}  // namespace can
+}  // namespace interfaces
+
+namespace sensors
+{
+
+namespace adis16405
+{
+using cs   = Gpio<GPIOA_BASE, 8>;
+using dio1 = Gpio<GPIOB_BASE, 4>;
+using dio2 = Gpio<GPIOD_BASE, 6>;
+using dio3 = Gpio<GPIOD_BASE, 4>;
+using rst  = Gpio<GPIOD_BASE, 5>;
+using ckIn = Gpio<GPIOA_BASE, 15>;
+}  // namespace adis16405
+
+namespace ad7994
+{
+using ab                      = Gpio<GPIOB_BASE, 1>;
+using nconvst                 = Gpio<GPIOG_BASE, 9>;
+static constexpr uint8_t addr = 0x22 << 1;
+}  // namespace ad7994
+
+namespace lm75b_analog
+{
+static constexpr uint8_t addr = 0x48 << 1;
+}  // namespace lm75b_analog
+
+namespace lm75b_imu
+{
+static constexpr uint8_t addr = 0x49 << 1;
+}  // namespace lm75b_imu
+
+namespace mpu9250
+{
+using cs   = Gpio<GPIOC_BASE, 3>;
+using intr = Gpio<GPIOE_BASE, 5>;
+}  // namespace mpu9250
+
+namespace ms5803
+{
+using cs = Gpio<GPIOD_BASE, 7>;
+}  // namespace ms5803
+
+namespace lsm6ds3h
+{
+using cs   = Gpio<GPIOC_BASE, 1>;
+using int1 = Gpio<GPIOB_BASE, 12>;
+using int2 = Gpio<GPIOG_BASE, 3>;
+}  // namespace lsm6ds3h
+
+}  // namespace sensors
+
+namespace inputs
+{
+using vbat      = Gpio<GPIOF_BASE, 7>;
+using lp_dtch   = Gpio<GPIOC_BASE, 6>;  // launchpad detach
+using btn_open  = Gpio<GPIOG_BASE, 11>;
+using btn_close = Gpio<GPIOG_BASE, 13>;
+}  // namespace inputs
+
+namespace nosecone
+{
+using motEn   = Gpio<GPIOG_BASE, 14>;
+using motP1   = Gpio<GPIOC_BASE, 7>;   // Pwm motore 1
+using motP2   = Gpio<GPIOB_BASE, 0>;   // Pwm motore 2
+using rogP1   = Gpio<GPIOD_BASE, 12>;  // Pwm rogallina 1
+using rogP2   = Gpio<GPIOD_BASE, 13>;  // Pwm rogallina 2
+using nc_dtch = Gpio<GPIOB_BASE, 7>;   // Nosecone detach
+
+using motor_act = Gpio<GPIOE_BASE, 3>;   // Motor actuation
+using motor_3v  = Gpio<GPIOC_BASE, 15>;  // Motor actuation 3v
+using motor_gnd = Gpio<GPIOE_BASE, 2>;   // Motor actuation gnd
+}  // namespace nosecone
+
+namespace actuators
+{
+
+using tcPwm = Gpio<GPIOE_BASE, 6>;  // Pwm thermal cutters
+
+namespace thCut2
+{
+using ena   = Gpio<GPIOD_BASE, 11>;
+using csens = Gpio<GPIOF_BASE, 8>;
+}  // namespace thCut2
+
+namespace thCut1
+{
+using ena   = Gpio<GPIOG_BASE, 2>;
+using csens = Gpio<GPIOF_BASE, 6>;
+}  // namespace thCut1
+}  // namespace actuators
+
+namespace misc
+{
+using buzz = Gpio<GPIOD_BASE, 3>;
+
+using aux1 = Gpio<GPIOE_BASE, 2>;
+using aux2 = Gpio<GPIOE_BASE, 3>;
+using aux3 = Gpio<GPIOE_BASE, 4>;
+using aux4 = Gpio<GPIOC_BASE, 14>;
+using aux5 = Gpio<GPIOC_BASE, 15>;
+
+}  // namespace misc
+
+using led1 = Gpio<GPIOC_BASE, 4>;
+
+/* NOTE: Direct access to these leds is possible
+ * only when the STM board is detached from
+ * the rest of the stack
+ */
+/*
+using led2  = Gpio<GPIOG_BASE, 2>;
+using led3  = Gpio<GPIOD_BASE, 11>;
+using led4  = Gpio<GPIOB_BASE, 1>;
+using led1  = Gpio<GPIOG_BASE, 3>;
+*/
+
+namespace xbee
+{
+using cs           = Gpio<GPIOF_BASE, 9>;
+using attn         = Gpio<GPIOF_BASE, 10>;
+using reset        = Gpio<GPIOC_BASE, 13>;
+using sleep_req    = Gpio<GPIOC_BASE, 2>;
+using sleep_status = Gpio<GPIOA_BASE, 4>;
+}  // namespace xbee
+}  // namespace miosix
+
+#endif  // HWMAPPING_H
diff --git a/src/bsps/stm32f429zi_death_stack_v1/stm32_2m+256k_rom.ld b/src/bsps/stm32f429zi_death_stack_v1/stm32_2m+256k_rom.ld
new file mode 100644
index 000000000..cfcc2ce53
--- /dev/null
+++ b/src/bsps/stm32f429zi_death_stack_v1/stm32_2m+256k_rom.ld
@@ -0,0 +1,181 @@
+/*
+ * C++ enabled linker script for stm32 (2M FLASH, 256K RAM)
+ * Developed by TFT: Terraneo Federico Technologies
+ * Optimized for use with the Miosix kernel
+ */
+
+/*
+ * This chip has an unusual quirk that the RAM is divided in two block mapped
+ * at two non contiguous memory addresses. I don't know why they've done that,
+ * probably doing the obvious thing would have made writing code too easy...
+ * Anyway, since hardware can't be changed, we've got to live with that and
+ * try to make use of both RAMs.
+ * 
+ * Given the constraints above, this linker script puts:
+ * - read only data and code (.text, .rodata, .eh_*) in FLASH
+ * - the 512Byte main (IRQ) stack, .data and .bss in the "small" 64KB RAM
+ * - stacks and heap in the "large" 192KB RAM.
+ * 
+ * Unfortunately thread stacks can't be put in the small RAM as Miosix
+ * allocates them inside the heap.
+ */
+
+/*
+ * The main stack is used for interrupt handling by the kernel.
+ *
+ * *** Readme ***
+ * This linker script places the main stack (used by the kernel for interrupts)
+ * at the bottom of the ram, instead of the top. This is done for two reasons:
+ *
+ * - as an optimization for microcontrollers with little ram memory. In fact
+ *   the implementation of malloc from newlib requests memory to the OS in 4KB
+ *   block (except the first block that can be smaller). This is probably done
+ *   for compatibility with OSes with an MMU and paged memory. To see why this
+ *   is bad, consider a microcontroller with 8KB of ram: when malloc finishes
+ *   up the first 4KB it will call _sbrk_r asking for a 4KB block, but this will
+ *   fail because the top part of the ram is used by the main stack. As a
+ *   result, the top part of the memory will not be used by malloc, even if
+ *   available (and it is nearly *half* the ram on an 8KB mcu). By placing the
+ *   main stack at the bottom of the ram, the upper 4KB block will be entirely
+ *   free and available as heap space.
+ *
+ * - In case of main stack overflow the cpu will fault because access to memory
+ *   before the beginning of the ram faults. Instead with the default stack
+ *   placement the main stack will silently collide with the heap.
+ * Note: if increasing the main stack size also increase the ORIGIN value in
+ * the MEMORY definitions below accordingly.
+ */
+_main_stack_size = 0x00000200;                     /* main stack = 512Bytes */
+_main_stack_top  = 0x10000000 + _main_stack_size;
+ASSERT(_main_stack_size   % 8 == 0, "MAIN stack size error");
+
+/* Mapping the heap into the large 192KB RAM */
+_end =      0x20000000;
+_heap_end = 0x20030000;                            /* end of available ram  */
+
+/* identify the Entry Point  */
+ENTRY(_Z13Reset_Handlerv)
+
+/* specify the memory areas  */
+MEMORY
+{
+    flash(rx)    : ORIGIN = 0x08000000, LENGTH =   2M
+    /*
+     * Note, the small ram starts at 0x10000000 but it is necessary to add the
+     * size of the main stack, so it is 0x10000200.
+     */
+    smallram(wx) : ORIGIN = 0x10000200, LENGTH =  64K-0x200 
+    largeram(wx) : ORIGIN = 0x20000000, LENGTH = 192K
+    backupram(rw): ORIGIN = 0x40024000, LENGTH =  4K
+}
+
+/* now define the output sections  */
+SECTIONS
+{
+    . = 0;
+    
+    /* .text section: code goes to flash */
+    .text :
+    {
+        /* Startup code must go at address 0 */
+        KEEP(*(.isr_vector))
+        
+        *(.text)
+        *(.text.*)
+        *(.gnu.linkonce.t.*)
+        /* these sections for thumb interwork? */
+        *(.glue_7)
+        *(.glue_7t)
+        /* these sections for C++? */
+        *(.gcc_except_table)
+        *(.gcc_except_table.*)
+        *(.ARM.extab*)
+        *(.gnu.linkonce.armextab.*)
+
+        . = ALIGN(4);
+        /* .rodata: constant data */
+        *(.rodata)
+        *(.rodata.*)
+        *(.gnu.linkonce.r.*)
+
+        /* C++ Static constructors/destructors (eabi) */
+        . = ALIGN(4);
+        KEEP(*(.init))
+        
+        . = ALIGN(4);
+        __miosix_init_array_start = .;
+        KEEP (*(SORT(.miosix_init_array.*)))
+        KEEP (*(.miosix_init_array))
+        __miosix_init_array_end = .;
+
+        . = ALIGN(4);
+        __preinit_array_start = .;
+        KEEP (*(.preinit_array))
+        __preinit_array_end = .;
+
+        . = ALIGN(4);
+        __init_array_start = .;
+        KEEP (*(SORT(.init_array.*)))
+        KEEP (*(.init_array))
+        __init_array_end = .;
+
+        . = ALIGN(4);
+        KEEP(*(.fini))
+
+        . = ALIGN(4);
+        __fini_array_start = .;
+        KEEP (*(.fini_array))
+        KEEP (*(SORT(.fini_array.*)))
+        __fini_array_end = .;
+
+        /* C++ Static constructors/destructors (elf)  */
+        . = ALIGN(4);
+        _ctor_start = .;
+        KEEP (*crtbegin.o(.ctors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .ctors))
+        KEEP (*(SORT(.ctors.*)))
+        KEEP (*crtend.o(.ctors))
+       _ctor_end = .;
+
+        . = ALIGN(4);
+        KEEP (*crtbegin.o(.dtors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .dtors))
+        KEEP (*(SORT(.dtors.*)))
+        KEEP (*crtend.o(.dtors))
+    } > flash
+
+    /* .ARM.exidx is sorted, so has to go in its own output section.  */
+    __exidx_start = .;
+    .ARM.exidx :
+    {
+        *(.ARM.exidx* .gnu.linkonce.armexidx.*)
+    } > flash
+    __exidx_end = .;
+
+	/* .data section: global variables go to ram, but also store a copy to
+       flash to initialize them */
+    .data : ALIGN(8)
+    {
+        _data = .;
+        *(.data)
+        *(.data.*)
+        *(.gnu.linkonce.d.*)
+        . = ALIGN(8);
+        _edata = .;
+    } > smallram AT > flash
+    _etext = LOADADDR(.data);
+
+    /* .bss section: uninitialized global variables go to ram */
+    _bss_start = .;
+    .bss :
+    {
+        *(.bss)
+        *(.bss.*)
+        *(.gnu.linkonce.b.*)
+        . = ALIGN(8);
+    } > smallram
+    _bss_end = .;
+
+    /*_end = .;*/
+    /*PROVIDE(end = .);*/
+}
diff --git a/src/bsps/stm32f429zi_death_stack_v1/stm32_2m+8m_xram.ld b/src/bsps/stm32f429zi_death_stack_v1/stm32_2m+8m_xram.ld
new file mode 100644
index 000000000..b6a0d2668
--- /dev/null
+++ b/src/bsps/stm32f429zi_death_stack_v1/stm32_2m+8m_xram.ld
@@ -0,0 +1,178 @@
+/*
+ * C++ enabled linker script for stm32 (2M FLASH, 256K RAM, 8M XRAM)
+ * Developed by TFT: Terraneo Federico Technologies
+ * Optimized for use with the Miosix kernel
+ */
+
+/*
+ * This chip has an unusual quirk that the RAM is divided in two block mapped
+ * at two non contiguous memory addresses. I don't know why they've done that,
+ * probably doing the obvious thing would have made writing code too easy...
+ * Anyway, since hardware can't be changed, we've got to live with that and
+ * try to make use of both RAMs.
+ * 
+ * Given the constraints above, this linker script puts:
+ * - read only data and code (.text, .rodata, .eh_*) in FLASH
+ * - the 512Byte main (IRQ) stack, .data and .bss in the "small" 64KB RAM
+ * - .data, .bss, stacks and heap in the external 8MB SDRAM.
+ */
+
+/*
+ * The main stack is used for interrupt handling by the kernel.
+ *
+ * *** Readme ***
+ * This linker script places the main stack (used by the kernel for interrupts)
+ * at the bottom of the ram, instead of the top. This is done for two reasons:
+ *
+ * - as an optimization for microcontrollers with little ram memory. In fact
+ *   the implementation of malloc from newlib requests memory to the OS in 4KB
+ *   block (except the first block that can be smaller). This is probably done
+ *   for compatibility with OSes with an MMU and paged memory. To see why this
+ *   is bad, consider a microcontroller with 8KB of ram: when malloc finishes
+ *   up the first 4KB it will call _sbrk_r asking for a 4KB block, but this will
+ *   fail because the top part of the ram is used by the main stack. As a
+ *   result, the top part of the memory will not be used by malloc, even if
+ *   available (and it is nearly *half* the ram on an 8KB mcu). By placing the
+ *   main stack at the bottom of the ram, the upper 4KB block will be entirely
+ *   free and available as heap space.
+ *
+ * - In case of main stack overflow the cpu will fault because access to memory
+ *   before the beginning of the ram faults. Instead with the default stack
+ *   placement the main stack will silently collide with the heap.
+ * Note: if increasing the main stack size also increase the ORIGIN value in
+ * the MEMORY definitions below accordingly.
+ */
+_main_stack_size = 0x00000200;                     /* main stack = 512Bytes */
+_main_stack_top  = 0x10000000 + _main_stack_size;
+ASSERT(_main_stack_size   % 8 == 0, "MAIN stack size error");
+
+/* Mapping the heap into XRAM */
+_heap_end = 0xd0800000;                            /* end of available ram  */
+
+/* identify the Entry Point  */
+ENTRY(_Z13Reset_Handlerv)
+
+/* specify the memory areas  */
+MEMORY
+{
+    flash(rx)    : ORIGIN = 0x08000000, LENGTH =   2M
+    /*
+     * Note, the small ram starts at 0x10000000 but it is necessary to add the
+     * size of the main stack, so it is 0x10000200.
+     */
+    smallram(wx) : ORIGIN = 0x10000200, LENGTH =  64K-0x200 
+    largeram(wx) : ORIGIN = 0x20000000, LENGTH = 192K
+    backupram(rw): ORIGIN = 0x40024000, LENGTH =   4K
+    xram(wx)     : ORIGIN = 0xd0000000, LENGTH =   8M
+}
+
+/* now define the output sections  */
+SECTIONS
+{
+    . = 0;
+    
+    /* .text section: code goes to flash */
+    .text :
+    {
+        /* Startup code must go at address 0 */
+        KEEP(*(.isr_vector))
+        
+        *(.text)
+        *(.text.*)
+        *(.gnu.linkonce.t.*)
+        /* these sections for thumb interwork? */
+        *(.glue_7)
+        *(.glue_7t)
+        /* these sections for C++? */
+        *(.gcc_except_table)
+        *(.gcc_except_table.*)
+        *(.ARM.extab*)
+        *(.gnu.linkonce.armextab.*)
+
+        . = ALIGN(4);
+        /* .rodata: constant data */
+        *(.rodata)
+        *(.rodata.*)
+        *(.gnu.linkonce.r.*)
+
+        /* C++ Static constructors/destructors (eabi) */
+        . = ALIGN(4);
+        KEEP(*(.init))
+        
+        . = ALIGN(4);
+        __miosix_init_array_start = .;
+        KEEP (*(SORT(.miosix_init_array.*)))
+        KEEP (*(.miosix_init_array))
+        __miosix_init_array_end = .;
+
+        . = ALIGN(4);
+        __preinit_array_start = .;
+        KEEP (*(.preinit_array))
+        __preinit_array_end = .;
+
+        . = ALIGN(4);
+        __init_array_start = .;
+        KEEP (*(SORT(.init_array.*)))
+        KEEP (*(.init_array))
+        __init_array_end = .;
+
+        . = ALIGN(4);
+        KEEP(*(.fini))
+
+        . = ALIGN(4);
+        __fini_array_start = .;
+        KEEP (*(.fini_array))
+        KEEP (*(SORT(.fini_array.*)))
+        __fini_array_end = .;
+
+        /* C++ Static constructors/destructors (elf)  */
+        . = ALIGN(4);
+        _ctor_start = .;
+        KEEP (*crtbegin.o(.ctors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .ctors))
+        KEEP (*(SORT(.ctors.*)))
+        KEEP (*crtend.o(.ctors))
+       _ctor_end = .;
+
+        . = ALIGN(4);
+        KEEP (*crtbegin.o(.dtors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .dtors))
+        KEEP (*(SORT(.dtors.*)))
+        KEEP (*crtend.o(.dtors))
+    } > flash
+
+    /* .ARM.exidx is sorted, so has to go in its own output section.  */
+    __exidx_start = .;
+    .ARM.exidx :
+    {
+        *(.ARM.exidx* .gnu.linkonce.armexidx.*)
+    } > flash
+    __exidx_end = .;
+
+	/* .data section: global variables go to ram, but also store a copy to
+       flash to initialize them */
+    .data : ALIGN(8)
+    {
+        _data = .;
+        *(.data)
+        *(.data.*)
+        *(.gnu.linkonce.d.*)
+        . = ALIGN(8);
+        _edata = .;
+    } > xram AT > flash
+    _etext = LOADADDR(.data);
+
+    /* .bss section: uninitialized global variables go to ram */
+    _bss_start = .;
+    .bss :
+    {
+        *(.bss)
+        *(.bss.*)
+        *(.gnu.linkonce.b.*)
+        . = ALIGN(8);
+    } > xram
+    _bss_end = .;
+
+    _end = .;
+    PROVIDE(end = .);
+}
diff --git a/src/bsps/stm32f429zi_death_stack_v2/config/board_options.cmake b/src/bsps/stm32f429zi_death_stack_v2/config/board_options.cmake
new file mode 100644
index 000000000..9b4741c60
--- /dev/null
+++ b/src/bsps/stm32f429zi_death_stack_v2/config/board_options.cmake
@@ -0,0 +1,120 @@
+# Copyright (C) 2023 by Skyward
+#
+# This program is free software; you can redistribute it and/or 
+# it under the terms of the GNU General Public License as published 
+# the Free Software Foundation; either version 2 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.
+#
+# As a special exception, if other files instantiate templates or use
+# macros or inline functions from this file, or you compile this file
+# and link it with other works to produce a work based on this file,
+# this file does not by itself cause the resulting work to be covered
+# by the GNU General Public License. However the source code for this
+# file must still be made available in accordance with the GNU 
+# Public License. This exception does not invalidate any other 
+# why a work based on this file might be covered by the GNU General
+# Public License.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, see <http://www.gnu.org/licenses/>
+
+set(BOARD_NAME stm32f429zi_death_stack_v2)
+set(ARCH_NAME cortexM4_stm32f4)
+
+# Base directories with header files for this board
+set(ARCH_PATH ${KPATH}/arch/${ARCH_NAME}/common)
+set(BOARD_PATH ${BOARDCORE_PATH}/src/bsps/${BOARD_NAME})
+set(BOARD_CONFIG_PATH ${BOARDCORE_PATH}/src/bsps/${BOARD_NAME}/config)
+
+# Specify where to find the board specific config/miosix_settings.h
+set(BOARD_MIOSIX_SETTINGS_PATH ${BOARD_PATH})
+
+# Specify where to find the board specific config/mxgui_settings.h
+set(BOARD_MXGUI_SETTINGS_PATH ${BOARD_PATH})
+
+# Optimization flags:
+# -O0 do no optimization, the default if no optimization level is specified
+# -O or -O1 optimize minimally
+# -O2 optimize more
+# -O3 optimize even more
+# -Ofast optimize very aggressively to the point of breaking the standard
+# -Og Optimize debugging experience, enables optimizations that do not
+# interfere with debugging
+# -Os Optimize for size with -O2 optimizations that do not increase code size
+set(OPT_OPTIMIZATION -O2)
+
+# Boot and linker files
+set(BOOT_FILE ${BOARD_PATH}/core/stage_1_boot.cpp)
+
+# Linker script type, there are three options
+# 1) Code in FLASH, stack + heap in internal RAM (file *_rom.ld)
+#    the most common choice, available for all microcontrollers
+# 2) Code in FLASH, stack + heap in external RAM (file *8m_xram.ld)
+#    You must uncomment -D__ENABLE_XRAM below in this case.
+# set(LINKER_SCRIPT ${BOARD_PATH}/stm32_2m+256k_rom.ld)
+set(LINKER_SCRIPT ${BOARD_PATH}/stm32_2m+8m_xram.ld)
+
+# Uncommenting __ENABLE_XRAM enables the initialization of the external
+# 8MB SDRAM memory. Do not uncomment this even if you don't use a linker
+# script that requires it, as it is used for the LCD framebuffer.
+set(XRAM -D__ENABLE_XRAM)
+
+# Select clock frequency. Warning: the default clock frequency for
+# this board is 168MHz and not 180MHz because, due to a limitation in
+# the PLL, it is not possible to generate a precise 48MHz output when
+# running the core at 180MHz. If 180MHz is chosen the USB peripheral will
+# NOT WORK and the SDIO and RNG will run ~6% slower (45MHz insteand of 48)
+# Define USE_INTERNAL_CLOCK to bypass the external oscillator
+# set(CLOCK_FREQ -DHSE_VALUE=8000000 -DSYSCLK_FREQ_180MHz=180000000)
+set(CLOCK_FREQ -DHSE_VALUE=8000000 -DSYSCLK_FREQ_168MHz=168000000)
+# set(CLOCK_FREQ -DHSE_VALUE=8000000 -DSYSCLK_FREQ_100MHz=100000000)
+
+# C++ Exception/rtti support disable flags.
+# To save code size if not using C++ exceptions (nor some STL code which
+# implicitly uses it) uncomment this option.
+# -D__NO_EXCEPTIONS is used by Miosix to know if exceptions are used.
+# set(OPT_EXCEPT -fno-exceptions -fno-rtti -D__NO_EXCEPTIONS)
+
+# Specify a custom flash command
+# This is the program that is invoked when the flash flag (-f or --flash) is
+# used with the Miosix Build System. Use $binary or $hex as placeolders, they
+# will be replaced by the build systems with the binary or hex file repectively.
+# If a command is not specified, the build system will use st-flash if found
+# set(PROGRAM_CMDLINE "here your custom flash command")
+
+# Basic flags
+set(FLAGS_BASE -mcpu=cortex-m4 -mthumb -mfloat-abi=hard -mfpu=fpv4-sp-d16)
+
+# Flags for ASM and linker
+set(AFLAGS_BASE ${FLAGS_BASE})
+set(LFLAGS_BASE ${FLAGS_BASE} -Wl,--gc-sections,-Map,main.map -Wl,-T${LINKER_SCRIPT} ${OPT_EXCEPT} ${OPT_OPTIMIZATION} -nostdlib)
+
+# Flags for C/C++
+string(TOUPPER ${BOARD_NAME} BOARD_UPPER)
+set(CFLAGS_BASE
+    -D_BOARD_${BOARD_UPPER} -D_MIOSIX_BOARDNAME=\"${BOARD_NAME}\"
+    -D_DEFAULT_SOURCE=1 -ffunction-sections -Wall -Werror=return-type -g
+    -D_ARCH_CORTEXM4_STM32F4
+    ${CLOCK_FREQ} ${XRAM} ${SRAM_BOOT} ${FLAGS_BASE} ${OPT_OPTIMIZATION} -c
+)
+set(CXXFLAGS_BASE ${CFLAGS_BASE} ${OPT_EXCEPT})
+
+# Select architecture specific files
+set(ARCH_SRC
+    ${ARCH_PATH}/interfaces-impl/delays.cpp
+    ${ARCH_PATH}/interfaces-impl/gpio_impl.cpp
+    ${ARCH_PATH}/interfaces-impl/portability.cpp
+    ${BOARD_PATH}/interfaces-impl/bsp.cpp
+    ${KPATH}/arch/common/CMSIS/Device/ST/STM32F4xx/Source/Templates/system_stm32f4xx.c
+    ${KPATH}/arch/common/core/interrupts_cortexMx.cpp
+    ${KPATH}/arch/common/core/mpu_cortexMx.cpp
+    ${KPATH}/arch/common/core/stm32f2_f4_l4_f7_h7_os_timer.cpp
+    ${KPATH}/arch/common/drivers/sd_stm32f2_f4_f7.cpp
+    ${KPATH}/arch/common/drivers/serial_stm32.cpp
+    ${KPATH}/arch/common/drivers/stm32_hardware_rng.cpp
+)
diff --git a/src/bsps/stm32f429zi_death_stack_v2/config/board_settings.h b/src/bsps/stm32f429zi_death_stack_v2/config/board_settings.h
new file mode 100644
index 000000000..85e971472
--- /dev/null
+++ b/src/bsps/stm32f429zi_death_stack_v2/config/board_settings.h
@@ -0,0 +1,74 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * 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.
+ */
+
+#ifndef BOARD_SETTINGS_H
+#define BOARD_SETTINGS_H
+
+#include "util/version.h"
+
+/**
+ * \internal
+ * Versioning for board_settings.h for out of git tree projects
+ */
+#define BOARD_SETTINGS_VERSION 300
+
+namespace miosix
+{
+
+/**
+ * \addtogroup Settings
+ * \{
+ */
+
+/// Size of stack for main().
+/// The C standard library is stack-heavy (iprintf requires 1KB) but the
+/// STM32F407VG only has 192KB of RAM so there is room for a big 4K stack.
+const unsigned int MAIN_STACK_SIZE = 16 * 1024;
+
+/// Serial port
+const unsigned int defaultSerial      = 1;
+const unsigned int defaultSerialSpeed = 115200;
+const bool defaultSerialFlowctrl      = false;
+#define SERIAL_1_DMA
+// #define SERIAL_2_DMA //Serial 2 can't be used (GPIO conflict), so no DMA
+// #define SERIAL_3_DMA //Serial 3 can't be used (GPIO conflict), so no DMA
+
+// #define I2C_WITH_DMA
+
+// SD card driver
+static const unsigned char sdVoltage = 33;  // Board powered @ 3.3V
+#ifdef __ENABLE_XRAM
+// Reduce SD clock to ~4.8MHz
+#define OVERRIDE_SD_CLOCK_DIVIDER_MAX 8
+#endif  //__ENABLE_XRAM
+// #define SD_ONE_BIT_DATABUS //This board supports 4 bit databus to SD card
+
+/// Analog supply voltage for ADC, DAC, Reset blocks, RCs and PLL
+#define V_DDA_VOLTAGE 3.3f
+
+/**
+ * \}
+ */
+
+}  // namespace miosix
+
+#endif /* BOARD_SETTINGS_H */
diff --git a/src/bsps/stm32f429zi_death_stack_v2/config/miosix_settings.h b/src/bsps/stm32f429zi_death_stack_v2/config/miosix_settings.h
new file mode 100644
index 000000000..5006e0bca
--- /dev/null
+++ b/src/bsps/stm32f429zi_death_stack_v2/config/miosix_settings.h
@@ -0,0 +1,240 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * 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.
+ */
+
+#pragma once
+
+// Before you can compile the kernel you have to configure it by editing this
+// file. After that, comment out this line to disable the reminder error.
+// The PARSING_FROM_IDE is because Netbeans gets confused by this, it is never
+// defined when compiling the code.
+#ifndef PARSING_FROM_IDE
+//#error This error is a reminder that you have not edited miosix_settings.h
+// yet.
+#endif  // PARSING_FROM_IDE
+
+/**
+ * \file miosix_settings.h
+ * NOTE: this file contains ONLY configuration options that are not dependent
+ * on architecture specific details. The other options are in the following
+ * files which are included here:
+ * miosix/arch/architecture name/common/arch_settings.h
+ * miosix/config/arch/architecture name/board name/board_settings.h
+ */
+#include "arch_settings.h"
+#include "board_settings.h"
+#include "util/version.h"
+
+/**
+ * \internal
+ * Versioning for miosix_settings.h for out of git tree projects
+ */
+#define MIOSIX_SETTINGS_VERSION 300
+
+namespace miosix
+{
+
+/**
+ * \addtogroup Settings
+ * \{
+ */
+
+//
+// Scheduler options
+//
+
+/// \def SCHED_TYPE_PRIORITY
+/// If uncommented selects the priority scheduler
+/// \def SCHED_TYPE_CONTROL_BASED
+/// If uncommented selects the control based scheduler
+/// \def SCHED_TYPE_EDF
+/// If uncommented selects the EDF scheduler
+// Uncomment only *one* of those
+
+#define SCHED_TYPE_PRIORITY
+//#define SCHED_TYPE_CONTROL_BASED
+//#define SCHED_TYPE_EDF
+
+/// \def WITH_CPU_TIME_COUNTER
+/// Allows to enable/disable CPUTimeCounter to save code size and remove its
+/// overhead from the scheduling process. By default it is not defined
+/// (CPUTimeCounter is disabled).
+//#define WITH_CPU_TIME_COUNTER
+
+//
+// Filesystem options
+//
+
+/// \def WITH_FILESYSTEM
+/// Allows to enable/disable filesystem support to save code size
+/// By default it is defined (filesystem support is enabled)
+#define WITH_FILESYSTEM
+
+/// \def WITH_DEVFS
+/// Allows to enable/disable DevFs support to save code size
+/// By default it is defined (DevFs is enabled)
+#define WITH_DEVFS
+
+/// \def SYNC_AFTER_WRITE
+/// Increases filesystem write robustness. After each write operation the
+/// filesystem is synced so that a power failure happens data is not lost
+/// (unless power failure happens exactly between the write and the sync)
+/// Unfortunately write latency and throughput becomes twice as worse
+/// By default it is defined (slow but safe)
+#define SYNC_AFTER_WRITE
+
+/// Maximum number of open files. Trying to open more will fail.
+/// Cannot be lower than 3, as the first three are stdin, stdout, stderr
+const unsigned char MAX_OPEN_FILES = 8;
+
+/// \def WITH_PROCESSES
+/// If uncommented enables support for processes as well as threads.
+/// This enables the dynamic loader to load elf programs, the extended system
+/// call service and, if the hardware supports it, the MPU to provide memory
+/// isolation of processes
+//#define WITH_PROCESSES
+
+#if defined(WITH_PROCESSES) && defined(__NO_EXCEPTIONS)
+#error Processes require C++ exception support
+#endif  // defined(WITH_PROCESSES) && defined(__NO_EXCEPTIONS)
+
+#if defined(WITH_PROCESSES) && !defined(WITH_FILESYSTEM)
+#error Processes require filesystem support
+#endif  // defined(WITH_PROCESSES) && !defined(WITH_FILESYSTEM)
+
+#if defined(WITH_PROCESSES) && !defined(WITH_DEVFS)
+#error Processes require devfs support
+#endif  // defined(WITH_PROCESSES) && !defined(WITH_DEVFS)
+
+//
+// C/C++ standard library I/O (stdin, stdout and stderr related)
+//
+
+/// \def WITH_BOOTLOG
+/// Uncomment to print bootlogs on stdout.
+/// By default it is defined (bootlogs are printed)
+#define WITH_BOOTLOG
+
+/// \def WITH_ERRLOG
+/// Uncomment for debug information on stdout.
+/// By default it is defined (error information is printed)
+#define WITH_ERRLOG
+
+//
+// Kernel related options (stack sizes, priorities)
+//
+
+/// \def WITH_DEEP_SLEEP
+/// Adds interfaces and required variables to support deep sleep state switch
+/// automatically when peripherals are not required
+//#define WITH_DEEP_SLEEP
+
+/**
+ * \def JTAG_DISABLE_SLEEP
+ * JTAG debuggers lose communication with the device if it enters sleep
+ * mode, so to use debugging it is necessary to disable sleep in the idle
+ * thread. By default it is not defined (idle thread calls sleep).
+ */
+//#define JTAG_DISABLE_SLEEP
+
+#if defined(WITH_DEEP_SLEEP) && defined(JTAG_DISABLE_SLEEP)
+#error Deep sleep cannot work together with jtag
+#endif  // defined(WITH_PROCESSES) && !defined(WITH_DEVFS)
+
+/// Minimum stack size (MUST be divisible by 4)
+const unsigned int STACK_MIN = 256;
+
+/// \internal Size of idle thread stack.
+/// Should be >=STACK_MIN (MUST be divisible by 4)
+const unsigned int STACK_IDLE = 256;
+
+/// Default stack size for pthread_create.
+/// The chosen value is enough to call C standard library functions
+/// such as printf/fopen which are stack-heavy
+const unsigned int STACK_DEFAULT_FOR_PTHREAD = 2048;
+
+/// Maximum size of the RAM image of a process. If a program requires more
+/// the kernel will not run it (MUST be divisible by 4)
+const unsigned int MAX_PROCESS_IMAGE_SIZE = 64 * 1024;
+
+/// Minimum size of the stack for a process. If a program specifies a lower
+/// size the kernel will not run it (MUST be divisible by 4)
+const unsigned int MIN_PROCESS_STACK_SIZE = STACK_MIN;
+
+/// Every userspace thread has two stacks, one for when it is running in
+/// userspace and one for when it is running in kernelspace (that is, while it
+/// is executing system calls). This is the size of the stack for when the
+/// thread is running in kernelspace (MUST be divisible by 4)
+const unsigned int SYSTEM_MODE_PROCESS_STACK_SIZE = 2 * 1024;
+
+/// Number of priorities (MUST be >1)
+/// PRIORITY_MAX-1 is the highest priority, 0 is the lowest. -1 is reserved as
+/// the priority of the idle thread.
+/// The meaning of a thread's priority depends on the chosen scheduler.
+#ifdef SCHED_TYPE_PRIORITY
+// Can be modified, but a high value makes context switches more expensive
+const short int PRIORITY_MAX = 4;
+#elif defined(SCHED_TYPE_CONTROL_BASED)
+// Don't touch, the limit is due to the fixed point implementation
+// It's not needed for if floating point is selected, but kept for consistency
+const short int PRIORITY_MAX = 64;
+#else  // SCHED_TYPE_EDF
+// Doesn't exist for this kind of scheduler
+#endif
+
+/// Priority of main()
+/// The meaning of a thread's priority depends on the chosen scheduler.
+const unsigned char MAIN_PRIORITY = 1;
+
+#ifdef SCHED_TYPE_PRIORITY
+/// Maximum thread time slice in nanoseconds, after which preemption occurs
+const unsigned int MAX_TIME_SLICE = 1000000;
+#endif  // SCHED_TYPE_PRIORITY
+
+//
+// Other low level kernel options. There is usually no need to modify these.
+//
+
+/// \internal Length of wartermark (in bytes) to check stack overflow.
+/// MUST be divisible by 4 and can also be zero.
+/// A high value increases context switch time.
+const unsigned int WATERMARK_LEN = 16;
+
+/// \internal Used to fill watermark
+const unsigned int WATERMARK_FILL = 0xaaaaaaaa;
+
+/// \internal Used to fill stack (for checking stack usage)
+const unsigned int STACK_FILL = 0xbbbbbbbb;
+
+// Compiler version checks
+#if !defined(_MIOSIX_GCC_PATCH_MAJOR) || _MIOSIX_GCC_PATCH_MAJOR < 3
+#error \
+    "You are using a too old or unsupported compiler. Get the latest one from https://miosix.org/wiki/index.php?title=Miosix_Toolchain"
+#endif
+#if _MIOSIX_GCC_PATCH_MAJOR > 3
+#warning "You are using a too new compiler, which may not be supported"
+#endif
+
+/**
+ * \}
+ */
+
+}  // namespace miosix
diff --git a/src/bsps/stm32f429zi_death_stack_v2/core/stage_1_boot.cpp b/src/bsps/stm32f429zi_death_stack_v2/core/stage_1_boot.cpp
new file mode 100644
index 000000000..e40004e97
--- /dev/null
+++ b/src/bsps/stm32f429zi_death_stack_v2/core/stage_1_boot.cpp
@@ -0,0 +1,372 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * 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 <string.h>
+
+#include "core/interrupts.h"  //For the unexpected interrupt call
+#include "interfaces/arch_registers.h"
+#include "interfaces/bsp.h"
+#include "kernel/stage_2_boot.h"
+
+/*
+ * startup.cpp
+ * STM32 C++ startup.
+ * NOTE: for stm32f42x devices ONLY.
+ * Supports interrupt handlers in C++ without extern "C"
+ * Developed by Terraneo Federico, based on ST startup code.
+ * Additionally modified to boot Miosix.
+ */
+
+/**
+ * Called by Reset_Handler, performs initialization and calls main.
+ * Never returns.
+ */
+void program_startup() __attribute__((noreturn));
+void program_startup()
+{
+    // Cortex M3 core appears to get out of reset with interrupts already
+    // enabled
+    __disable_irq();
+
+    // These are defined in the linker script
+    extern unsigned char _etext asm("_etext");
+    extern unsigned char _data asm("_data");
+    extern unsigned char _edata asm("_edata");
+    extern unsigned char _bss_start asm("_bss_start");
+    extern unsigned char _bss_end asm("_bss_end");
+
+    // Initialize .data section, clear .bss section
+    unsigned char *etext     = &_etext;
+    unsigned char *data      = &_data;
+    unsigned char *edata     = &_edata;
+    unsigned char *bss_start = &_bss_start;
+    unsigned char *bss_end   = &_bss_end;
+    memcpy(data, etext, edata - data);
+    memset(bss_start, 0, bss_end - bss_start);
+
+    // Move on to stage 2
+    _init();
+
+    // If main returns, reboot
+    NVIC_SystemReset();
+    for (;;)
+        ;
+}
+
+/**
+ * Reset handler, called by hardware immediately after reset
+ */
+void Reset_Handler() __attribute__((__interrupt__, noreturn));
+void Reset_Handler()
+{
+    // SystemInit() is called *before* initializing .data and zeroing .bss
+    // Despite all startup files provided by ST do the opposite, there are three
+    // good reasons to do so:
+    // First, the CMSIS specifications say that SystemInit() must not access
+    // global variables, so it is actually possible to call it before
+    // Second, when running Miosix with the xram linker scripts .data and .bss
+    // are placed in the external RAM, so we *must* call SystemInit(), which
+    // enables xram, before touching .data and .bss
+    // Third, this is a performance improvement since the loops that initialize
+    //.data and zeros .bss now run with the CPU at full speed instead of 8MHz
+    SystemInit();
+// ST does not provide code to initialize the stm32f429-disco SDRAM at boot.
+// Put after SystemInit() as SDRAM is timing-sensitive and needs the full
+// clock speed.
+#ifdef __ENABLE_XRAM
+    miosix::configureSdram();
+#endif  //__ENABLE_XRAM
+
+    /*
+     * Initialize process stack and switch to it.
+     * This is required for booting Miosix, a small portion of the top of the
+     * heap area will be used as stack until the first thread starts. After,
+     * this stack will be abandoned and the process stack will point to the
+     * current thread's stack.
+     */
+    asm volatile(
+        "ldr r0,  =_heap_end          \n\t"
+        "msr psp, r0                  \n\t"
+        "movw r0, #2                  \n\n"  // Privileged, process stack
+        "msr control, r0              \n\t"
+        "isb                          \n\t" ::
+            : "r0");
+
+    program_startup();
+}
+
+/**
+ * All unused interrupts call this function.
+ */
+extern "C" void Default_Handler() { unexpectedInterrupt(); }
+
+// System handlers
+void /*__attribute__((weak))*/ Reset_Handler();      // These interrupts are not
+void /*__attribute__((weak))*/ NMI_Handler();        // weak because they are
+void /*__attribute__((weak))*/ HardFault_Handler();  // surely defined by Miosix
+void /*__attribute__((weak))*/ MemManage_Handler();
+void /*__attribute__((weak))*/ BusFault_Handler();
+void /*__attribute__((weak))*/ UsageFault_Handler();
+void /*__attribute__((weak))*/ SVC_Handler();
+void /*__attribute__((weak))*/ DebugMon_Handler();
+void /*__attribute__((weak))*/ PendSV_Handler();
+void __attribute__((weak)) SysTick_Handler();
+
+// Interrupt handlers
+void __attribute__((weak)) WWDG_IRQHandler();
+void __attribute__((weak)) PVD_IRQHandler();
+void __attribute__((weak)) TAMP_STAMP_IRQHandler();
+void __attribute__((weak)) RTC_WKUP_IRQHandler();
+void __attribute__((weak)) FLASH_IRQHandler();
+void __attribute__((weak)) RCC_IRQHandler();
+void __attribute__((weak)) EXTI0_IRQHandler();
+void __attribute__((weak)) EXTI1_IRQHandler();
+void __attribute__((weak)) EXTI2_IRQHandler();
+void __attribute__((weak)) EXTI3_IRQHandler();
+void __attribute__((weak)) EXTI4_IRQHandler();
+void __attribute__((weak)) DMA1_Stream0_IRQHandler();
+void __attribute__((weak)) DMA1_Stream1_IRQHandler();
+void __attribute__((weak)) DMA1_Stream2_IRQHandler();
+void __attribute__((weak)) DMA1_Stream3_IRQHandler();
+void __attribute__((weak)) DMA1_Stream4_IRQHandler();
+void __attribute__((weak)) DMA1_Stream5_IRQHandler();
+void __attribute__((weak)) DMA1_Stream6_IRQHandler();
+void __attribute__((weak)) ADC_IRQHandler();
+void __attribute__((weak)) CAN1_TX_IRQHandler();
+void __attribute__((weak)) CAN1_RX0_IRQHandler();
+void __attribute__((weak)) CAN1_RX1_IRQHandler();
+void __attribute__((weak)) CAN1_SCE_IRQHandler();
+void __attribute__((weak)) EXTI9_5_IRQHandler();
+void __attribute__((weak)) TIM1_BRK_TIM9_IRQHandler();
+void __attribute__((weak)) TIM1_UP_TIM10_IRQHandler();
+void __attribute__((weak)) TIM1_TRG_COM_TIM11_IRQHandler();
+void __attribute__((weak)) TIM1_CC_IRQHandler();
+void __attribute__((weak)) TIM2_IRQHandler();
+void __attribute__((weak)) TIM3_IRQHandler();
+void __attribute__((weak)) TIM4_IRQHandler();
+void __attribute__((weak)) I2C1_EV_IRQHandler();
+void __attribute__((weak)) I2C1_ER_IRQHandler();
+void __attribute__((weak)) I2C2_EV_IRQHandler();
+void __attribute__((weak)) I2C2_ER_IRQHandler();
+void __attribute__((weak)) SPI1_IRQHandler();
+void __attribute__((weak)) SPI2_IRQHandler();
+void __attribute__((weak)) USART1_IRQHandler();
+void __attribute__((weak)) USART2_IRQHandler();
+void __attribute__((weak)) USART3_IRQHandler();
+void __attribute__((weak)) EXTI15_10_IRQHandler();
+void __attribute__((weak)) RTC_Alarm_IRQHandler();
+void __attribute__((weak)) OTG_FS_WKUP_IRQHandler();
+void __attribute__((weak)) TIM8_BRK_TIM12_IRQHandler();
+void __attribute__((weak)) TIM8_UP_TIM13_IRQHandler();
+void __attribute__((weak)) TIM8_TRG_COM_TIM14_IRQHandler();
+void __attribute__((weak)) TIM8_CC_IRQHandler();
+void __attribute__((weak)) DMA1_Stream7_IRQHandler();
+void __attribute__((weak)) FMC_IRQHandler();
+void __attribute__((weak)) SDIO_IRQHandler();
+void __attribute__((weak)) TIM5_IRQHandler();
+void __attribute__((weak)) SPI3_IRQHandler();
+void __attribute__((weak)) UART4_IRQHandler();
+void __attribute__((weak)) UART5_IRQHandler();
+void __attribute__((weak)) TIM6_DAC_IRQHandler();
+void __attribute__((weak)) TIM7_IRQHandler();
+void __attribute__((weak)) DMA2_Stream0_IRQHandler();
+void __attribute__((weak)) DMA2_Stream1_IRQHandler();
+void __attribute__((weak)) DMA2_Stream2_IRQHandler();
+void __attribute__((weak)) DMA2_Stream3_IRQHandler();
+void __attribute__((weak)) DMA2_Stream4_IRQHandler();
+void __attribute__((weak)) ETH_IRQHandler();
+void __attribute__((weak)) ETH_WKUP_IRQHandler();
+void __attribute__((weak)) CAN2_TX_IRQHandler();
+void __attribute__((weak)) CAN2_RX0_IRQHandler();
+void __attribute__((weak)) CAN2_RX1_IRQHandler();
+void __attribute__((weak)) CAN2_SCE_IRQHandler();
+void __attribute__((weak)) OTG_FS_IRQHandler();
+void __attribute__((weak)) DMA2_Stream5_IRQHandler();
+void __attribute__((weak)) DMA2_Stream6_IRQHandler();
+void __attribute__((weak)) DMA2_Stream7_IRQHandler();
+void __attribute__((weak)) USART6_IRQHandler();
+void __attribute__((weak)) I2C3_EV_IRQHandler();
+void __attribute__((weak)) I2C3_ER_IRQHandler();
+void __attribute__((weak)) OTG_HS_EP1_OUT_IRQHandler();
+void __attribute__((weak)) OTG_HS_EP1_IN_IRQHandler();
+void __attribute__((weak)) OTG_HS_WKUP_IRQHandler();
+void __attribute__((weak)) OTG_HS_IRQHandler();
+void __attribute__((weak)) DCMI_IRQHandler();
+void __attribute__((weak)) CRYP_IRQHandler();
+void __attribute__((weak)) HASH_RNG_IRQHandler();
+void __attribute__((weak)) FPU_IRQHandler();
+void __attribute__((weak)) UART7_IRQHandler();
+void __attribute__((weak)) UART8_IRQHandler();
+void __attribute__((weak)) SPI4_IRQHandler();
+void __attribute__((weak)) SPI5_IRQHandler();
+void __attribute__((weak)) SPI6_IRQHandler();
+void __attribute__((weak)) SAI1_IRQHandler();
+void __attribute__((weak)) LTDC_IRQHandler();
+void __attribute__((weak)) LTDC_ER_IRQHandler();
+void __attribute__((weak)) DMA2D_IRQHandler();
+
+// Stack top, defined in the linker script
+extern char _main_stack_top asm("_main_stack_top");
+
+// Interrupt vectors, must be placed @ address 0x00000000
+// The extern declaration is required otherwise g++ optimizes it out
+extern void (*const __Vectors[])();
+void (*const __Vectors[])() __attribute__((section(".isr_vector"))) = {
+    reinterpret_cast<void (*)()>(&_main_stack_top), /* Stack pointer*/
+    Reset_Handler,                                  /* Reset Handler */
+    NMI_Handler,                                    /* NMI Handler */
+    HardFault_Handler,                              /* Hard Fault Handler */
+    MemManage_Handler,                              /* MPU Fault Handler */
+    BusFault_Handler,                               /* Bus Fault Handler */
+    UsageFault_Handler,                             /* Usage Fault Handler */
+    0,                                              /* Reserved */
+    0,                                              /* Reserved */
+    0,                                              /* Reserved */
+    0,                                              /* Reserved */
+    SVC_Handler,                                    /* SVCall Handler */
+    DebugMon_Handler,                               /* Debug Monitor Handler */
+    0,                                              /* Reserved */
+    PendSV_Handler,                                 /* PendSV Handler */
+    SysTick_Handler,                                /* SysTick Handler */
+
+    /* External Interrupts */
+    WWDG_IRQHandler, PVD_IRQHandler, TAMP_STAMP_IRQHandler, RTC_WKUP_IRQHandler,
+    FLASH_IRQHandler, RCC_IRQHandler, EXTI0_IRQHandler, EXTI1_IRQHandler,
+    EXTI2_IRQHandler, EXTI3_IRQHandler, EXTI4_IRQHandler,
+    DMA1_Stream0_IRQHandler, DMA1_Stream1_IRQHandler, DMA1_Stream2_IRQHandler,
+    DMA1_Stream3_IRQHandler, DMA1_Stream4_IRQHandler, DMA1_Stream5_IRQHandler,
+    DMA1_Stream6_IRQHandler, ADC_IRQHandler, CAN1_TX_IRQHandler,
+    CAN1_RX0_IRQHandler, CAN1_RX1_IRQHandler, CAN1_SCE_IRQHandler,
+    EXTI9_5_IRQHandler, TIM1_BRK_TIM9_IRQHandler, TIM1_UP_TIM10_IRQHandler,
+    TIM1_TRG_COM_TIM11_IRQHandler, TIM1_CC_IRQHandler, TIM2_IRQHandler,
+    TIM3_IRQHandler, TIM4_IRQHandler, I2C1_EV_IRQHandler, I2C1_ER_IRQHandler,
+    I2C2_EV_IRQHandler, I2C2_ER_IRQHandler, SPI1_IRQHandler, SPI2_IRQHandler,
+    USART1_IRQHandler, USART2_IRQHandler, USART3_IRQHandler,
+    EXTI15_10_IRQHandler, RTC_Alarm_IRQHandler, OTG_FS_WKUP_IRQHandler,
+    TIM8_BRK_TIM12_IRQHandler, TIM8_UP_TIM13_IRQHandler,
+    TIM8_TRG_COM_TIM14_IRQHandler, TIM8_CC_IRQHandler, DMA1_Stream7_IRQHandler,
+    FMC_IRQHandler, SDIO_IRQHandler, TIM5_IRQHandler, SPI3_IRQHandler,
+    UART4_IRQHandler, UART5_IRQHandler, TIM6_DAC_IRQHandler, TIM7_IRQHandler,
+    DMA2_Stream0_IRQHandler, DMA2_Stream1_IRQHandler, DMA2_Stream2_IRQHandler,
+    DMA2_Stream3_IRQHandler, DMA2_Stream4_IRQHandler, ETH_IRQHandler,
+    ETH_WKUP_IRQHandler, CAN2_TX_IRQHandler, CAN2_RX0_IRQHandler,
+    CAN2_RX1_IRQHandler, CAN2_SCE_IRQHandler, OTG_FS_IRQHandler,
+    DMA2_Stream5_IRQHandler, DMA2_Stream6_IRQHandler, DMA2_Stream7_IRQHandler,
+    USART6_IRQHandler, I2C3_EV_IRQHandler, I2C3_ER_IRQHandler,
+    OTG_HS_EP1_OUT_IRQHandler, OTG_HS_EP1_IN_IRQHandler, OTG_HS_WKUP_IRQHandler,
+    OTG_HS_IRQHandler, DCMI_IRQHandler, CRYP_IRQHandler, HASH_RNG_IRQHandler,
+    FPU_IRQHandler, UART7_IRQHandler, UART8_IRQHandler, SPI4_IRQHandler,
+    SPI5_IRQHandler, SPI6_IRQHandler, SAI1_IRQHandler, LTDC_IRQHandler,
+    LTDC_ER_IRQHandler, DMA2D_IRQHandler};
+
+#pragma weak SysTick_IRQHandler            = Default_Handler
+#pragma weak WWDG_IRQHandler               = Default_Handler
+#pragma weak PVD_IRQHandler                = Default_Handler
+#pragma weak TAMP_STAMP_IRQHandler         = Default_Handler
+#pragma weak RTC_WKUP_IRQHandler           = Default_Handler
+#pragma weak FLASH_IRQHandler              = Default_Handler
+#pragma weak RCC_IRQHandler                = Default_Handler
+#pragma weak EXTI0_IRQHandler              = Default_Handler
+#pragma weak EXTI1_IRQHandler              = Default_Handler
+#pragma weak EXTI2_IRQHandler              = Default_Handler
+#pragma weak EXTI3_IRQHandler              = Default_Handler
+#pragma weak EXTI4_IRQHandler              = Default_Handler
+#pragma weak DMA1_Stream0_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream1_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream2_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream3_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream4_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream5_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream6_IRQHandler       = Default_Handler
+#pragma weak ADC_IRQHandler                = Default_Handler
+#pragma weak CAN1_TX_IRQHandler            = Default_Handler
+#pragma weak CAN1_RX0_IRQHandler           = Default_Handler
+#pragma weak CAN1_RX1_IRQHandler           = Default_Handler
+#pragma weak CAN1_SCE_IRQHandler           = Default_Handler
+#pragma weak EXTI9_5_IRQHandler            = Default_Handler
+#pragma weak TIM1_BRK_TIM9_IRQHandler      = Default_Handler
+#pragma weak TIM1_UP_TIM10_IRQHandler      = Default_Handler
+#pragma weak TIM1_TRG_COM_TIM11_IRQHandler = Default_Handler
+#pragma weak TIM1_CC_IRQHandler            = Default_Handler
+#pragma weak TIM2_IRQHandler               = Default_Handler
+#pragma weak TIM3_IRQHandler               = Default_Handler
+#pragma weak TIM4_IRQHandler               = Default_Handler
+#pragma weak I2C1_EV_IRQHandler            = Default_Handler
+#pragma weak I2C1_ER_IRQHandler            = Default_Handler
+#pragma weak I2C2_EV_IRQHandler            = Default_Handler
+#pragma weak I2C2_ER_IRQHandler            = Default_Handler
+#pragma weak SPI1_IRQHandler               = Default_Handler
+#pragma weak SPI2_IRQHandler               = Default_Handler
+// #pragma weak USART1_IRQHandler             = Default_Handler
+// #pragma weak USART2_IRQHandler             = Default_Handler
+// #pragma weak USART3_IRQHandler             = Default_Handler
+#pragma weak EXTI15_10_IRQHandler          = Default_Handler
+#pragma weak RTC_Alarm_IRQHandler          = Default_Handler
+#pragma weak OTG_FS_WKUP_IRQHandler        = Default_Handler
+#pragma weak TIM8_BRK_TIM12_IRQHandler     = Default_Handler
+#pragma weak TIM8_UP_TIM13_IRQHandler      = Default_Handler
+#pragma weak TIM8_TRG_COM_TIM14_IRQHandler = Default_Handler
+#pragma weak TIM8_CC_IRQHandler            = Default_Handler
+#pragma weak DMA1_Stream7_IRQHandler       = Default_Handler
+#pragma weak FMC_IRQHandler                = Default_Handler
+#pragma weak SDIO_IRQHandler               = Default_Handler
+#pragma weak TIM5_IRQHandler               = Default_Handler
+#pragma weak SPI3_IRQHandler               = Default_Handler
+// #pragma weak UART4_IRQHandler              = Default_Handler
+// #pragma weak UART5_IRQHandler              = Default_Handler
+#pragma weak TIM6_DAC_IRQHandler     = Default_Handler
+#pragma weak TIM7_IRQHandler         = Default_Handler
+#pragma weak DMA2_Stream0_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream1_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream2_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream3_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream4_IRQHandler = Default_Handler
+#pragma weak ETH_IRQHandler          = Default_Handler
+#pragma weak ETH_WKUP_IRQHandler     = Default_Handler
+#pragma weak CAN2_TX_IRQHandler      = Default_Handler
+#pragma weak CAN2_RX0_IRQHandler     = Default_Handler
+#pragma weak CAN2_RX1_IRQHandler     = Default_Handler
+#pragma weak CAN2_SCE_IRQHandler     = Default_Handler
+#pragma weak OTG_FS_IRQHandler       = Default_Handler
+#pragma weak DMA2_Stream5_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream6_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream7_IRQHandler = Default_Handler
+// #pragma weak USART6_IRQHandler             = Default_Handler
+#pragma weak I2C3_EV_IRQHandler        = Default_Handler
+#pragma weak I2C3_ER_IRQHandler        = Default_Handler
+#pragma weak OTG_HS_EP1_OUT_IRQHandler = Default_Handler
+#pragma weak OTG_HS_EP1_IN_IRQHandler  = Default_Handler
+#pragma weak OTG_HS_WKUP_IRQHandler    = Default_Handler
+#pragma weak OTG_HS_IRQHandler         = Default_Handler
+#pragma weak DCMI_IRQHandler           = Default_Handler
+#pragma weak CRYP_IRQHandler           = Default_Handler
+#pragma weak HASH_RNG_IRQHandler       = Default_Handler
+#pragma weak FPU_IRQHandler            = Default_Handler
+// #pragma weak UART7_IRQHandler              = Default_Handler
+// #pragma weak UART8_IRQHandler              = Default_Handler
+#pragma weak SPI4_IRQHandler    = Default_Handler
+#pragma weak SPI5_IRQHandler    = Default_Handler
+#pragma weak SPI6_IRQHandler    = Default_Handler
+#pragma weak SAI1_IRQHandler    = Default_Handler
+#pragma weak LTDC_IRQHandler    = Default_Handler
+#pragma weak LTDC_ER_IRQHandler = Default_Handler
+#pragma weak DMA2D_IRQHandler   = Default_Handler
diff --git a/src/bsps/stm32f429zi_death_stack_v2/interfaces-impl/arch_registers_impl.h b/src/bsps/stm32f429zi_death_stack_v2/interfaces-impl/arch_registers_impl.h
new file mode 100644
index 000000000..aa9f5579f
--- /dev/null
+++ b/src/bsps/stm32f429zi_death_stack_v2/interfaces-impl/arch_registers_impl.h
@@ -0,0 +1,34 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * 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.
+ */
+
+#ifndef ARCH_REGISTERS_IMPL_H
+#define ARCH_REGISTERS_IMPL_H
+
+// Always include stm32f4xx.h before core_cm4.h, there's some nasty dependency
+#define STM32F429xx
+#include "CMSIS/Device/ST/STM32F4xx/Include/stm32f4xx.h"
+#include "CMSIS/Device/ST/STM32F4xx/Include/system_stm32f4xx.h"
+#include "CMSIS/Include/core_cm4.h"
+
+#define RCC_SYNC() __DSB()  // Workaround for a bug in stm32f42x
+
+#endif  // ARCH_REGISTERS_IMPL_H
diff --git a/src/bsps/stm32f429zi_death_stack_v2/interfaces-impl/bsp.cpp b/src/bsps/stm32f429zi_death_stack_v2/interfaces-impl/bsp.cpp
new file mode 100644
index 000000000..b47e68191
--- /dev/null
+++ b/src/bsps/stm32f429zi_death_stack_v2/interfaces-impl/bsp.cpp
@@ -0,0 +1,486 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * 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.
+ */
+
+/***********************************************************************
+ * bsp.cpp Part of the Miosix Embedded OS.
+ * Board support package, this file initializes hardware.
+ ************************************************************************/
+
+#include "interfaces/bsp.h"
+
+#include <inttypes.h>
+#include <sys/ioctl.h>
+
+#include <cstdlib>
+
+#include "board_settings.h"
+#include "config/miosix_settings.h"
+#include "drivers/sd_stm32f2_f4_f7.h"
+#include "drivers/serial.h"
+#include "filesystem/console/console_device.h"
+#include "filesystem/file_access.h"
+#include "hwmapping.h"
+#include "interfaces/arch_registers.h"
+#include "interfaces/delays.h"
+#include "interfaces/portability.h"
+#include "kernel/kernel.h"
+#include "kernel/logging.h"
+#include "kernel/sync.h"
+
+namespace miosix
+{
+
+//
+// Initialization
+//
+
+/**
+ * The example code from ST checks for the busy flag after each command.
+ * Interestingly I couldn't find any mention of this in the datsheet.
+ */
+static void sdramCommandWait()
+{
+    for (int i = 0; i < 0xffff; i++)
+        if ((FMC_Bank5_6->SDSR & FMC_SDSR_BUSY) == 0)
+            return;
+}
+
+#ifdef SDRAM_ISSI
+
+void configureSdram()
+{
+    // Enable all gpios, passing clock
+    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN | RCC_AHB1ENR_GPIOBEN |
+                    RCC_AHB1ENR_GPIOCEN | RCC_AHB1ENR_GPIODEN |
+                    RCC_AHB1ENR_GPIOEEN | RCC_AHB1ENR_GPIOFEN |
+                    RCC_AHB1ENR_GPIOGEN | RCC_AHB1ENR_GPIOHEN;
+    RCC_SYNC();
+
+    // First, configure SDRAM GPIOs
+    GPIOB->AFR[0] = 0x0cc00000;
+    GPIOC->AFR[0] = 0x0000000c;
+    GPIOD->AFR[0] = 0x000000cc;
+    GPIOD->AFR[1] = 0xcc000ccc;
+    GPIOE->AFR[0] = 0xc00000cc;
+    GPIOE->AFR[1] = 0xcccccccc;
+    GPIOF->AFR[0] = 0x00cccccc;
+    GPIOF->AFR[1] = 0xccccc000;
+    GPIOG->AFR[0] = 0x00cc00cc;
+    GPIOG->AFR[1] = 0xc000000c;
+
+    GPIOB->MODER = 0x00002800;
+    GPIOC->MODER = 0x00000002;
+    GPIOD->MODER = 0xa02a000a;
+    GPIOE->MODER = 0xaaaa800a;
+    GPIOF->MODER = 0xaa800aaa;
+    GPIOG->MODER = 0x80020a0a;
+
+    GPIOA->OSPEEDR = 0xaaaaaaaa;  // Default to 50MHz speed for all GPIOs...
+    GPIOB->OSPEEDR =
+        0xaaaaaaaa | 0x00003c00;  //...but 100MHz for the SDRAM pins
+    GPIOC->OSPEEDR = 0xaaaaaaaa | 0x00000003;
+    GPIOD->OSPEEDR = 0xaaaaaaaa | 0xf03f000f;
+    GPIOE->OSPEEDR = 0xaaaaaaaa | 0xffffc00f;
+    GPIOF->OSPEEDR = 0xaaaaaaaa | 0xffc00fff;
+    GPIOG->OSPEEDR = 0xaaaaaaaa | 0xc0030f0f;
+    GPIOH->OSPEEDR = 0xaaaaaaaa;
+
+    // Since we'we un-configured PB3/PB4 from the default at boot TDO,NTRST,
+    // finish the job and remove the default pull-up
+    GPIOB->PUPDR = 0;
+
+    // Second, actually start the SDRAM controller
+    RCC->AHB3ENR |= RCC_AHB3ENR_FMCEN;
+    RCC_SYNC();
+
+    // SDRAM is a IS42S16400J -7 speed grade, connected to bank 2 (0xd0000000)
+    // Some bits in SDCR[1] are don't care, and the have to be set in SDCR[0],
+    // they aren't just don't care, the controller will fail if they aren't at 0
+    FMC_Bank5_6->SDCR[0] = FMC_SDCR1_SDCLK_1  // SDRAM runs @ half CPU frequency
+                           | FMC_SDCR1_RBURST  // Enable read burst
+                           | 0;              //  0 delay between reads after CAS
+    FMC_Bank5_6->SDCR[1] = 0                 //  8 bit column address
+                           | FMC_SDCR1_NR_0  // 12 bit row address
+                           | FMC_SDCR1_MWID_0  // 16 bit data bus
+                           | FMC_SDCR1_NB      //  4 banks
+                           |
+                           FMC_SDCR1_CAS_1;  //  2 cycle CAS latency (F<133MHz)
+
+#ifdef SYSCLK_FREQ_180MHz
+    // One SDRAM clock cycle is 11.1ns
+    // Some bits in SDTR[1] are don't care, and the have to be set in SDTR[0],
+    // they aren't just don't care, the controller will fail if they aren't at 0
+    FMC_Bank5_6->SDTR[0] = (6 - 1) << 12     // 6 cycle TRC  (66.6ns>63ns)
+                           | (2 - 1) << 20;  // 2 cycle TRP  (22.2ns>15ns)
+    FMC_Bank5_6->SDTR[1] = (2 - 1) << 0      // 2 cycle TMRD
+                           | (7 - 1) << 4    // 7 cycle TXSR (77.7ns>70ns)
+                           | (4 - 1) << 8    // 4 cycle TRAS (44.4ns>42ns)
+                           | (2 - 1) << 16   // 2 cycle TWR
+                           | (2 - 1) << 24;  // 2 cycle TRCD (22.2ns>15ns)
+#elif defined(SYSCLK_FREQ_168MHz)
+    // One SDRAM clock cycle is 11.9ns
+    // Some bits in SDTR[1] are don't care, and the have to be set in SDTR[0],
+    // they aren't just don't care, the controller will fail if they aren't at 0
+    FMC_Bank5_6->SDTR[0] = (6 - 1) << 12     // 6 cycle TRC  (71.4ns>63ns)
+                           | (2 - 1) << 20;  // 2 cycle TRP  (23.8ns>15ns)
+    FMC_Bank5_6->SDTR[1] = (2 - 1) << 0      // 2 cycle TMRD
+                           | (6 - 1) << 4    // 6 cycle TXSR (71.4ns>70ns)
+                           | (4 - 1) << 8    // 4 cycle TRAS (47.6ns>42ns)
+                           | (2 - 1) << 16   // 2 cycle TWR
+                           | (2 - 1) << 24;  // 2 cycle TRCD (23.8ns>15ns)
+#else
+#error No SDRAM timings for this clock
+#endif
+
+    FMC_Bank5_6->SDCMR = FMC_SDCMR_CTB2  // Enable bank 2
+                         | 1;            // MODE=001 clock enabled
+    sdramCommandWait();
+
+    // ST and SDRAM datasheet agree a 100us delay is required here.
+    delayUs(100);
+
+    FMC_Bank5_6->SDCMR = FMC_SDCMR_CTB2  // Enable bank 2
+                         | 2;            // MODE=010 precharge all command
+    sdramCommandWait();
+
+    FMC_Bank5_6->SDCMR = (8 - 1) << 5      // NRFS=8 SDRAM datasheet says
+                                           // "at least two AUTO REFRESH cycles"
+                         | FMC_SDCMR_CTB2  // Enable bank 2
+                         | 3;              // MODE=011 auto refresh
+    sdramCommandWait();
+
+    FMC_Bank5_6->SDCMR = 0x220 << 9  // MRD=0x220:CAS latency=2 burst len=1
+                         | FMC_SDCMR_CTB2  // Enable bank 2
+                         | 4;              // MODE=100 load mode register
+    sdramCommandWait();
+
+// 64ms/4096=15.625us
+#ifdef SYSCLK_FREQ_180MHz
+    // 15.625us*90MHz=1406-20=1386
+    FMC_Bank5_6->SDRTR = 1386 << 1;
+#elif defined(SYSCLK_FREQ_168MHz)
+    // 15.625us*84MHz=1312-20=1292
+    FMC_Bank5_6->SDRTR = 1292 << 1;
+#else
+#error No refresh timings for this clock
+#endif
+}
+
+#else
+
+void configureSdram()
+{
+    // Enable all gpios
+    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN | RCC_AHB1ENR_GPIOBEN |
+                    RCC_AHB1ENR_GPIOCEN | RCC_AHB1ENR_GPIODEN |
+                    RCC_AHB1ENR_GPIOEEN | RCC_AHB1ENR_GPIOFEN |
+                    RCC_AHB1ENR_GPIOGEN | RCC_AHB1ENR_GPIOHEN;
+    RCC_SYNC();
+
+    // First, configure SDRAM GPIOs
+    GPIOB->AFR[0] = 0x0cc00000;
+    GPIOC->AFR[0] = 0x0000000c;
+    GPIOD->AFR[0] = 0x000000cc;
+    GPIOD->AFR[1] = 0xcc000ccc;
+    GPIOE->AFR[0] = 0xc00000cc;
+    GPIOE->AFR[1] = 0xcccccccc;
+    GPIOF->AFR[0] = 0x00cccccc;
+    GPIOF->AFR[1] = 0xccccc000;
+    GPIOG->AFR[0] = 0x00cc00cc;
+    GPIOG->AFR[1] = 0xc000000c;
+
+    GPIOB->MODER = 0x00002800;
+    GPIOC->MODER = 0x00000002;
+    GPIOD->MODER = 0xa02a000a;
+    GPIOE->MODER = 0xaaaa800a;
+    GPIOF->MODER = 0xaa800aaa;
+    GPIOG->MODER = 0x80020a0a;
+
+    GPIOA->OSPEEDR = 0xaaaaaaaa;  // Default to 50MHz speed for all GPIOs...
+    GPIOB->OSPEEDR =
+        0xaaaaaaaa | 0x00003c00;  //...but 100MHz for the SDRAM pins
+    GPIOC->OSPEEDR = 0xaaaaaaaa | 0x00000003;
+    GPIOD->OSPEEDR = 0xaaaaaaaa | 0xf03f000f;
+    GPIOE->OSPEEDR = 0xaaaaaaaa | 0xffffc00f;
+    GPIOF->OSPEEDR = 0xaaaaaaaa | 0xffc00fff;
+    GPIOG->OSPEEDR = 0xaaaaaaaa | 0xc0030f0f;
+    GPIOH->OSPEEDR = 0xaaaaaaaa;
+
+    // Since we'we un-configured PB3/PB4 from the default at boot TDO,NTRST,
+    // finish the job and remove the default pull-up
+    GPIOB->PUPDR = 0;
+
+    // Second, actually start the SDRAM controller
+    RCC->AHB3ENR |= RCC_AHB3ENR_FMCEN;
+    RCC_SYNC();
+
+    // SDRAM is a AS4C4M16SA-6TAN, connected to bank 2 (0xd0000000)
+    // Some bits in SDCR[1] are don't care, and the have to be set in SDCR[0],
+    // they aren't just don't care, the controller will fail if they aren't at 0
+    FMC_Bank5_6->SDCR[0] = FMC_SDCR1_SDCLK_1  // SDRAM runs @ half CPU frequency
+                           | FMC_SDCR1_RBURST  // Enable read burst
+                           | 0;  //  0 delay between reads after CAS
+    FMC_Bank5_6->SDCR[1] =
+        0                   //  8 bit column address
+        | FMC_SDCR1_NR_0    // 12 bit row address
+        | FMC_SDCR1_MWID_0  // 16 bit data bus
+        | FMC_SDCR1_NB      //  4 banks
+        | FMC_SDCR1_CAS_1;  //  2 cycle CAS latency (TCK>9+0.5ns [1])
+
+#ifdef SYSCLK_FREQ_180MHz
+    // One SDRAM clock cycle is 11.1ns
+    // Some bits in SDTR[1] are don't care, and the have to be set in SDTR[0],
+    // they aren't just don't care, the controller will fail if they aren't at 0
+    FMC_Bank5_6->SDTR[0] = (6 - 1) << 12     // 6 cycle TRC  (66.6ns>60ns)
+                           | (2 - 1) << 20;  // 2 cycle TRP  (22.2ns>18ns)
+    FMC_Bank5_6->SDTR[1] = (2 - 1) << 0      // 2 cycle TMRD
+                           |
+                           (6 - 1) << 4  // 6 cycle TXSR (66.6ns>61.5+0.5ns [1])
+                           | (4 - 1) << 8    // 4 cycle TRAS (44.4ns>42ns)
+                           | (2 - 1) << 16   // 2 cycle TWR
+                           | (2 - 1) << 24;  // 2 cycle TRCD (22.2ns>18ns)
+#elif defined(SYSCLK_FREQ_168MHz)
+    // One SDRAM clock cycle is 11.9ns
+    // Some bits in SDTR[1] are don't care, and the have to be set in SDTR[0],
+    // they aren't just don't care, the controller will fail if they aren't at 0
+    FMC_Bank5_6->SDTR[0] = (6 - 1) << 12     // 6 cycle TRC  (71.4ns>60ns)
+                           | (2 - 1) << 20;  // 2 cycle TRP  (23.8ns>18ns)
+    FMC_Bank5_6->SDTR[1] = (2 - 1) << 0      // 2 cycle TMRD
+                           |
+                           (6 - 1) << 4  // 6 cycle TXSR (71.4ns>61.5+0.5ns [1])
+                           | (4 - 1) << 8    // 4 cycle TRAS (47.6ns>42ns)
+                           | (2 - 1) << 16   // 2 cycle TWR
+                           | (2 - 1) << 24;  // 2 cycle TRCD (23.8ns>18ns)
+#else
+#error No SDRAM timings for this clock
+#endif
+    // NOTE [1]: the timings for TCK and TIS depend on rise and fall times
+    //(see note 9 and 10 on datasheet). Timings are adjusted accordingly to
+    // the measured 2ns rise and fall time
+
+    FMC_Bank5_6->SDCMR = FMC_SDCMR_CTB2  // Enable bank 2
+                         | 1;            // MODE=001 clock enabled
+    sdramCommandWait();
+
+    // SDRAM datasheet requires 200us delay here (note 11), here we use 10% more
+    delayUs(220);
+
+    FMC_Bank5_6->SDCMR = FMC_SDCMR_CTB2  // Enable bank 2
+                         | 2;            // MODE=010 precharge all command
+    sdramCommandWait();
+
+    // FIXME: note 11 on SDRAM datasheet says extended mode register must be
+    // set, but the ST datasheet does not seem to explain how
+    FMC_Bank5_6->SDCMR = 0x220 << 9  // MRD=0x220:CAS latency=2 burst len=1
+                         | FMC_SDCMR_CTB2  // Enable bank 2
+                         | 4;              // MODE=100 load mode register
+    sdramCommandWait();
+
+    FMC_Bank5_6->SDCMR = (4 - 1) << 5  // NRFS=8 SDRAM datasheet requires
+                                       // a minimum of 2 cycles, here we use 4
+                         | FMC_SDCMR_CTB2  // Enable bank 2
+                         | 3;              // MODE=011 auto refresh
+    sdramCommandWait();
+
+// 32ms/4096=7.8125us, but datasheet says to round that to 7.8us
+#ifdef SYSCLK_FREQ_180MHz
+    // 7.8us*90MHz=702-20=682
+    FMC_Bank5_6->SDRTR = 682 << 1;
+#elif defined(SYSCLK_FREQ_168MHz)
+    // 7.8us*84MHz=655-20=635
+    FMC_Bank5_6->SDRTR = 635 << 1;
+#else
+#error No refresh timings for this clock
+#endif
+}
+
+#endif
+
+void IRQbspInit()
+{
+// If using SDRAM GPIOs are enabled by configureSdram(), else enable them here
+#ifndef __ENABLE_XRAM
+    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN | RCC_AHB1ENR_GPIOBEN |
+                    RCC_AHB1ENR_GPIOCEN | RCC_AHB1ENR_GPIODEN |
+                    RCC_AHB1ENR_GPIOEEN | RCC_AHB1ENR_GPIOFEN |
+                    RCC_AHB1ENR_GPIOGEN | RCC_AHB1ENR_GPIOHEN;
+    RCC_SYNC();
+#endif  //__ENABLE_XRAM
+
+    // enable spi1, spi2
+    RCC->APB2ENR |= RCC_APB2ENR_SPI1EN;
+    RCC->APB1ENR |= RCC_APB1ENR_SPI2EN;
+
+    // enable can1
+    RCC->APB1ENR |= RCC_APB1ENR_CAN1EN;
+
+    RCC_SYNC();
+
+    using namespace interfaces;
+
+    spi1::sck::mode(Mode::ALTERNATE);
+    spi1::sck::alternateFunction(5);
+    spi1::miso::mode(Mode::ALTERNATE);
+    spi1::miso::alternateFunction(5);
+    spi1::mosi::mode(Mode::ALTERNATE);
+    spi1::mosi::alternateFunction(5);
+
+    spi2::sck::mode(Mode::ALTERNATE);
+    spi2::sck::alternateFunction(5);
+    spi2::miso::mode(Mode::ALTERNATE);
+    spi2::miso::alternateFunction(5);
+    spi2::mosi::mode(Mode::ALTERNATE);
+    spi2::mosi::alternateFunction(5);
+
+    uart2::rx::mode(Mode::ALTERNATE);
+    uart2::rx::alternateFunction(7);
+    uart2::tx::mode(Mode::ALTERNATE);
+    uart2::tx::alternateFunction(7);
+
+    uart3::rx::mode(Mode::ALTERNATE);
+    uart3::rx::alternateFunction(7);
+    uart3::tx::mode(Mode::ALTERNATE);
+    uart3::tx::alternateFunction(7);
+
+    uart4::rx::mode(Mode::ALTERNATE);
+    uart4::rx::alternateFunction(8);
+    uart4::tx::mode(Mode::ALTERNATE);
+    uart4::tx::alternateFunction(8);
+
+    can::rx::mode(Mode::ALTERNATE);
+    can::rx::alternateFunction(9);
+    can::tx::mode(Mode::ALTERNATE);
+    can::tx::alternateFunction(9);
+
+    timers::tim4ch1::mode(Mode::ALTERNATE);
+    timers::tim4ch1::alternateFunction(2);
+    timers::tim8ch2::mode(Mode::ALTERNATE);
+    timers::tim8ch2::alternateFunction(3);
+
+    camMosfet::mode(Mode::OUTPUT);
+    camMosfet::low();
+
+    using namespace sensors;
+
+    ads1118::cs::mode(Mode::OUTPUT);
+    ads1118::cs::high();
+
+    bmx160::cs::mode(Mode::OUTPUT);
+    bmx160::cs::high();
+    bmx160::intr::mode(Mode::INPUT_PULL_UP);
+
+    lsm9ds1::cs_a_g::mode(Mode::OUTPUT);
+    lsm9ds1::cs_a_g::high();
+    lsm9ds1::cs_m::mode(Mode::OUTPUT);
+    lsm9ds1::cs_m::high();
+    lsm9ds1::intr_a_g::mode(Mode::INPUT);
+
+    lis3mdl::cs::mode(Mode::OUTPUT);
+    lis3mdl::cs::high();
+
+    ms5803::cs::mode(Mode::OUTPUT);
+    ms5803::cs::high();
+
+    using namespace inputs;
+
+    vbat::mode(Mode::INPUT_ANALOG);
+    expulsion::mode(Mode::INPUT);
+
+    using namespace actuators;
+
+    nosecone::th_cut_input::mode(Mode::OUTPUT);
+
+    nosecone::thermal_cutter_1::enable::mode(Mode::OUTPUT);
+    nosecone::thermal_cutter_1::enable::low();
+    nosecone::thermal_cutter_1::cutter_sens::mode(Mode::INPUT_ANALOG);
+
+    nosecone::thermal_cutter_2::enable::mode(Mode::OUTPUT);
+    nosecone::thermal_cutter_2::enable::low();
+    nosecone::thermal_cutter_2::cutter_sens::mode(Mode::INPUT_ANALOG);
+
+    using namespace aux;
+
+    sense_aux_1::mode(Mode::INPUT);
+    aux_pd_pu::mode(Mode::OUTPUT);
+    aux_spi1_cs::mode(Mode::OUTPUT);
+
+    using namespace leds;
+
+    led_red1::mode(Mode::OUTPUT);
+    led_red2::mode(Mode::OUTPUT);
+    led_blue1::mode(Mode::OUTPUT);
+    led_ring::mode(Mode::OUTPUT);
+
+    using namespace xbee;
+
+    xbee::cs::mode(Mode::OUTPUT);
+    xbee::cs::high();
+    xbee::attn::mode(Mode::INPUT_PULL_UP);
+    xbee::reset::mode(Mode::OPEN_DRAIN);
+    xbee::reset::high();
+
+    DefaultConsole::instance().IRQset(intrusive_ref_ptr<Device>(
+        new STM32Serial(defaultSerial, defaultSerialSpeed,
+                        defaultSerialFlowctrl ? STM32Serial::RTSCTS
+                                              : STM32Serial::NOFLOWCTRL)));
+
+    for (uint8_t i = 0; i < 3; i++)
+    {
+        ledOn();
+        delayMs(10);
+        ledOff();
+        delayMs(10);
+    }
+}
+
+void bspInit2()
+{
+#ifdef WITH_FILESYSTEM
+    // PA2,PA3
+    intrusive_ref_ptr<DevFs> devFs =
+        basicFilesystemSetup(SDIODriver::instance());
+#endif  // WITH_FILESYSTEM
+}
+
+//
+// Shutdown and reboot
+//
+
+/**
+ * For safety reasons, we never want the board to shutdown.
+ * When requested to shutdown, we reboot instead.
+ */
+void shutdown() { reboot(); }
+
+void reboot()
+{
+    ioctl(STDOUT_FILENO, IOCTL_SYNC, 0);
+
+#ifdef WITH_FILESYSTEM
+    FilesystemManager::instance().umountAll();
+#endif  // WITH_FILESYSTEM
+
+    disableInterrupts();
+    miosix_private::IRQsystemReboot();
+}
+
+}  // namespace miosix
diff --git a/src/bsps/stm32f429zi_death_stack_v2/interfaces-impl/bsp_impl.h b/src/bsps/stm32f429zi_death_stack_v2/interfaces-impl/bsp_impl.h
new file mode 100644
index 000000000..91279b1f3
--- /dev/null
+++ b/src/bsps/stm32f429zi_death_stack_v2/interfaces-impl/bsp_impl.h
@@ -0,0 +1,74 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * 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.
+ */
+
+/***********************************************************************
+ * bsp_impl.h Part of the Miosix Embedded OS.
+ * Board support package, this file initializes hardware.
+ ************************************************************************/
+
+#ifndef BSP_IMPL_H
+#define BSP_IMPL_H
+
+#include "config/miosix_settings.h"
+#include "drivers/stm32_hardware_rng.h"
+#include "hwmapping.h"
+#include "interfaces/gpio.h"
+
+namespace miosix
+{
+
+/**
+\addtogroup Hardware
+\{
+*/
+
+/**
+ * \internal
+ * Called by stage_1_boot.cpp to enable the SDRAM before initializing .data/.bss
+ * Requires the CPU clock to be already configured (running from the PLL)
+ */
+void configureSdram();
+
+/**
+ * \internal
+ * used by the ledOn() and ledOff() implementation
+ */
+
+inline void ledOn()
+{
+    leds::led_blue1::high();
+    leds::led_ring::high();
+}
+
+inline void ledOff()
+{
+    leds::led_blue1::low();
+    leds::led_ring::low();
+}
+
+/**
+\}
+*/
+
+}  // namespace miosix
+
+#endif  // BSP_IMPL_H
diff --git a/src/bsps/stm32f429zi_death_stack_v2/interfaces-impl/hwmapping.h b/src/bsps/stm32f429zi_death_stack_v2/interfaces-impl/hwmapping.h
new file mode 100644
index 000000000..272509d07
--- /dev/null
+++ b/src/bsps/stm32f429zi_death_stack_v2/interfaces-impl/hwmapping.h
@@ -0,0 +1,193 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * 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.
+ */
+
+#ifndef HWMAPPING_H
+#define HWMAPPING_H
+
+#include "interfaces/gpio.h"
+
+namespace miosix
+{
+
+namespace interfaces
+{
+
+namespace spi1
+{
+using sck  = Gpio<GPIOA_BASE, 5>;
+using miso = Gpio<GPIOA_BASE, 6>;
+using mosi = Gpio<GPIOA_BASE, 7>;
+}  // namespace spi1
+
+namespace spi2
+{
+using sck  = Gpio<GPIOB_BASE, 13>;
+using miso = Gpio<GPIOB_BASE, 14>;
+using mosi = Gpio<GPIOB_BASE, 15>;
+}  // namespace spi2
+
+// USB UART
+namespace uart1
+{
+using tx = Gpio<GPIOA_BASE, 9>;
+using rx = Gpio<GPIOA_BASE, 10>;
+}  // namespace uart1
+
+// GPS UART
+namespace uart2
+{
+using tx = Gpio<GPIOA_BASE, 2>;
+using rx = Gpio<GPIOA_BASE, 3>;
+}  // namespace uart2
+
+namespace uart3
+{
+using tx = Gpio<GPIOB_BASE, 10>;
+using rx = Gpio<GPIOB_BASE, 11>;
+}  // namespace uart3
+
+namespace uart4
+{
+using tx = Gpio<GPIOA_BASE, 0>;
+using rx = Gpio<GPIOA_BASE, 1>;
+}  // namespace uart4
+
+// CAN1
+namespace can
+{
+using rx = Gpio<GPIOA_BASE, 11>;
+using tx = Gpio<GPIOA_BASE, 12>;
+}  // namespace can
+
+// Servo motors timers
+namespace timers
+{
+using tim4ch1 = Gpio<GPIOD_BASE, 12>;  // Servo 1
+using tim8ch2 = Gpio<GPIOC_BASE, 7>;   // Servo 2
+}  // namespace timers
+
+using camMosfet = Gpio<GPIOC_BASE, 14>;
+
+}  // namespace interfaces
+
+namespace sensors
+{
+
+namespace ads1118
+{
+using cs = Gpio<GPIOB_BASE, 1>;
+}  // namespace ads1118
+
+namespace bmx160
+{
+using cs   = Gpio<GPIOA_BASE, 8>;
+using intr = Gpio<GPIOE_BASE, 5>;
+}  // namespace bmx160
+
+namespace lsm9ds1
+{
+using cs_a_g   = Gpio<GPIOC_BASE, 1>;
+using cs_m     = Gpio<GPIOC_BASE, 3>;
+using intr_a_g = Gpio<GPIOB_BASE, 12>;
+}  // namespace lsm9ds1
+
+namespace lis3mdl
+{
+using cs = Gpio<GPIOG_BASE, 6>;
+}  // namespace lis3mdl
+
+namespace ms5803
+{
+using cs = Gpio<GPIOD_BASE, 7>;
+}  // namespace ms5803
+
+}  // namespace sensors
+
+namespace inputs
+{
+using vbat      = Gpio<GPIOF_BASE, 7>;
+using expulsion = Gpio<GPIOE_BASE, 4>;
+}  // namespace inputs
+
+namespace actuators
+{
+
+namespace servos
+{
+using servo1 = interfaces::timers::tim4ch1;
+using servo2 = interfaces::timers::tim8ch2;
+}  // namespace servos
+
+namespace nosecone
+{
+
+using th_cut_input = Gpio<GPIOE_BASE, 6>;  // Input thermal cutters
+
+namespace thermal_cutter_1
+{
+using enable      = Gpio<GPIOG_BASE, 2>;
+using cutter_sens = Gpio<GPIOF_BASE, 6>;  // ADC3 CH4
+}  // namespace thermal_cutter_1
+
+namespace thermal_cutter_2
+{
+using enable      = Gpio<GPIOD_BASE, 11>;
+using cutter_sens = Gpio<GPIOF_BASE, 8>;  // ADC3 CH6
+}  // namespace thermal_cutter_2
+
+}  // namespace nosecone
+
+}  // namespace actuators
+
+namespace aux
+{
+using sense_aux_1 = Gpio<GPIOE_BASE, 2>;
+using aux_pd_pu   = Gpio<GPIOC_BASE, 5>;
+using aux_spi1_cs = Gpio<GPIOG_BASE, 7>;
+}  // namespace aux
+
+namespace leds
+{
+using led_red1  = Gpio<GPIOG_BASE, 7>;
+using led_red2  = Gpio<GPIOG_BASE, 10>;
+using led_blue1 = Gpio<GPIOG_BASE, 14>;
+using led_ring  = Gpio<GPIOE_BASE, 3>;
+
+/**
+ * These are connected to the enable pin of the thermal cutters and the cs of
+ * the lis3mdl magnetometer.
+ */
+// using led_blue2  = Gpio<GPIOG_BASE, 2>;
+// using led_green1 = Gpio<GPIOG_BASE, 6>;
+// using led_green2 = Gpio<GPIOD_BASE, 11>;
+}  // namespace leds
+
+namespace xbee
+{
+using cs    = Gpio<GPIOF_BASE, 9>;
+using attn  = Gpio<GPIOF_BASE, 10>;
+using reset = Gpio<GPIOC_BASE, 13>;
+}  // namespace xbee
+
+}  // namespace miosix
+
+#endif  // HWMAPPING_H
diff --git a/src/bsps/stm32f429zi_death_stack_v2/stm32_2m+256k_rom.ld b/src/bsps/stm32f429zi_death_stack_v2/stm32_2m+256k_rom.ld
new file mode 100644
index 000000000..cfcc2ce53
--- /dev/null
+++ b/src/bsps/stm32f429zi_death_stack_v2/stm32_2m+256k_rom.ld
@@ -0,0 +1,181 @@
+/*
+ * C++ enabled linker script for stm32 (2M FLASH, 256K RAM)
+ * Developed by TFT: Terraneo Federico Technologies
+ * Optimized for use with the Miosix kernel
+ */
+
+/*
+ * This chip has an unusual quirk that the RAM is divided in two block mapped
+ * at two non contiguous memory addresses. I don't know why they've done that,
+ * probably doing the obvious thing would have made writing code too easy...
+ * Anyway, since hardware can't be changed, we've got to live with that and
+ * try to make use of both RAMs.
+ * 
+ * Given the constraints above, this linker script puts:
+ * - read only data and code (.text, .rodata, .eh_*) in FLASH
+ * - the 512Byte main (IRQ) stack, .data and .bss in the "small" 64KB RAM
+ * - stacks and heap in the "large" 192KB RAM.
+ * 
+ * Unfortunately thread stacks can't be put in the small RAM as Miosix
+ * allocates them inside the heap.
+ */
+
+/*
+ * The main stack is used for interrupt handling by the kernel.
+ *
+ * *** Readme ***
+ * This linker script places the main stack (used by the kernel for interrupts)
+ * at the bottom of the ram, instead of the top. This is done for two reasons:
+ *
+ * - as an optimization for microcontrollers with little ram memory. In fact
+ *   the implementation of malloc from newlib requests memory to the OS in 4KB
+ *   block (except the first block that can be smaller). This is probably done
+ *   for compatibility with OSes with an MMU and paged memory. To see why this
+ *   is bad, consider a microcontroller with 8KB of ram: when malloc finishes
+ *   up the first 4KB it will call _sbrk_r asking for a 4KB block, but this will
+ *   fail because the top part of the ram is used by the main stack. As a
+ *   result, the top part of the memory will not be used by malloc, even if
+ *   available (and it is nearly *half* the ram on an 8KB mcu). By placing the
+ *   main stack at the bottom of the ram, the upper 4KB block will be entirely
+ *   free and available as heap space.
+ *
+ * - In case of main stack overflow the cpu will fault because access to memory
+ *   before the beginning of the ram faults. Instead with the default stack
+ *   placement the main stack will silently collide with the heap.
+ * Note: if increasing the main stack size also increase the ORIGIN value in
+ * the MEMORY definitions below accordingly.
+ */
+_main_stack_size = 0x00000200;                     /* main stack = 512Bytes */
+_main_stack_top  = 0x10000000 + _main_stack_size;
+ASSERT(_main_stack_size   % 8 == 0, "MAIN stack size error");
+
+/* Mapping the heap into the large 192KB RAM */
+_end =      0x20000000;
+_heap_end = 0x20030000;                            /* end of available ram  */
+
+/* identify the Entry Point  */
+ENTRY(_Z13Reset_Handlerv)
+
+/* specify the memory areas  */
+MEMORY
+{
+    flash(rx)    : ORIGIN = 0x08000000, LENGTH =   2M
+    /*
+     * Note, the small ram starts at 0x10000000 but it is necessary to add the
+     * size of the main stack, so it is 0x10000200.
+     */
+    smallram(wx) : ORIGIN = 0x10000200, LENGTH =  64K-0x200 
+    largeram(wx) : ORIGIN = 0x20000000, LENGTH = 192K
+    backupram(rw): ORIGIN = 0x40024000, LENGTH =  4K
+}
+
+/* now define the output sections  */
+SECTIONS
+{
+    . = 0;
+    
+    /* .text section: code goes to flash */
+    .text :
+    {
+        /* Startup code must go at address 0 */
+        KEEP(*(.isr_vector))
+        
+        *(.text)
+        *(.text.*)
+        *(.gnu.linkonce.t.*)
+        /* these sections for thumb interwork? */
+        *(.glue_7)
+        *(.glue_7t)
+        /* these sections for C++? */
+        *(.gcc_except_table)
+        *(.gcc_except_table.*)
+        *(.ARM.extab*)
+        *(.gnu.linkonce.armextab.*)
+
+        . = ALIGN(4);
+        /* .rodata: constant data */
+        *(.rodata)
+        *(.rodata.*)
+        *(.gnu.linkonce.r.*)
+
+        /* C++ Static constructors/destructors (eabi) */
+        . = ALIGN(4);
+        KEEP(*(.init))
+        
+        . = ALIGN(4);
+        __miosix_init_array_start = .;
+        KEEP (*(SORT(.miosix_init_array.*)))
+        KEEP (*(.miosix_init_array))
+        __miosix_init_array_end = .;
+
+        . = ALIGN(4);
+        __preinit_array_start = .;
+        KEEP (*(.preinit_array))
+        __preinit_array_end = .;
+
+        . = ALIGN(4);
+        __init_array_start = .;
+        KEEP (*(SORT(.init_array.*)))
+        KEEP (*(.init_array))
+        __init_array_end = .;
+
+        . = ALIGN(4);
+        KEEP(*(.fini))
+
+        . = ALIGN(4);
+        __fini_array_start = .;
+        KEEP (*(.fini_array))
+        KEEP (*(SORT(.fini_array.*)))
+        __fini_array_end = .;
+
+        /* C++ Static constructors/destructors (elf)  */
+        . = ALIGN(4);
+        _ctor_start = .;
+        KEEP (*crtbegin.o(.ctors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .ctors))
+        KEEP (*(SORT(.ctors.*)))
+        KEEP (*crtend.o(.ctors))
+       _ctor_end = .;
+
+        . = ALIGN(4);
+        KEEP (*crtbegin.o(.dtors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .dtors))
+        KEEP (*(SORT(.dtors.*)))
+        KEEP (*crtend.o(.dtors))
+    } > flash
+
+    /* .ARM.exidx is sorted, so has to go in its own output section.  */
+    __exidx_start = .;
+    .ARM.exidx :
+    {
+        *(.ARM.exidx* .gnu.linkonce.armexidx.*)
+    } > flash
+    __exidx_end = .;
+
+	/* .data section: global variables go to ram, but also store a copy to
+       flash to initialize them */
+    .data : ALIGN(8)
+    {
+        _data = .;
+        *(.data)
+        *(.data.*)
+        *(.gnu.linkonce.d.*)
+        . = ALIGN(8);
+        _edata = .;
+    } > smallram AT > flash
+    _etext = LOADADDR(.data);
+
+    /* .bss section: uninitialized global variables go to ram */
+    _bss_start = .;
+    .bss :
+    {
+        *(.bss)
+        *(.bss.*)
+        *(.gnu.linkonce.b.*)
+        . = ALIGN(8);
+    } > smallram
+    _bss_end = .;
+
+    /*_end = .;*/
+    /*PROVIDE(end = .);*/
+}
diff --git a/src/bsps/stm32f429zi_death_stack_v2/stm32_2m+8m_xram.ld b/src/bsps/stm32f429zi_death_stack_v2/stm32_2m+8m_xram.ld
new file mode 100644
index 000000000..b6a0d2668
--- /dev/null
+++ b/src/bsps/stm32f429zi_death_stack_v2/stm32_2m+8m_xram.ld
@@ -0,0 +1,178 @@
+/*
+ * C++ enabled linker script for stm32 (2M FLASH, 256K RAM, 8M XRAM)
+ * Developed by TFT: Terraneo Federico Technologies
+ * Optimized for use with the Miosix kernel
+ */
+
+/*
+ * This chip has an unusual quirk that the RAM is divided in two block mapped
+ * at two non contiguous memory addresses. I don't know why they've done that,
+ * probably doing the obvious thing would have made writing code too easy...
+ * Anyway, since hardware can't be changed, we've got to live with that and
+ * try to make use of both RAMs.
+ * 
+ * Given the constraints above, this linker script puts:
+ * - read only data and code (.text, .rodata, .eh_*) in FLASH
+ * - the 512Byte main (IRQ) stack, .data and .bss in the "small" 64KB RAM
+ * - .data, .bss, stacks and heap in the external 8MB SDRAM.
+ */
+
+/*
+ * The main stack is used for interrupt handling by the kernel.
+ *
+ * *** Readme ***
+ * This linker script places the main stack (used by the kernel for interrupts)
+ * at the bottom of the ram, instead of the top. This is done for two reasons:
+ *
+ * - as an optimization for microcontrollers with little ram memory. In fact
+ *   the implementation of malloc from newlib requests memory to the OS in 4KB
+ *   block (except the first block that can be smaller). This is probably done
+ *   for compatibility with OSes with an MMU and paged memory. To see why this
+ *   is bad, consider a microcontroller with 8KB of ram: when malloc finishes
+ *   up the first 4KB it will call _sbrk_r asking for a 4KB block, but this will
+ *   fail because the top part of the ram is used by the main stack. As a
+ *   result, the top part of the memory will not be used by malloc, even if
+ *   available (and it is nearly *half* the ram on an 8KB mcu). By placing the
+ *   main stack at the bottom of the ram, the upper 4KB block will be entirely
+ *   free and available as heap space.
+ *
+ * - In case of main stack overflow the cpu will fault because access to memory
+ *   before the beginning of the ram faults. Instead with the default stack
+ *   placement the main stack will silently collide with the heap.
+ * Note: if increasing the main stack size also increase the ORIGIN value in
+ * the MEMORY definitions below accordingly.
+ */
+_main_stack_size = 0x00000200;                     /* main stack = 512Bytes */
+_main_stack_top  = 0x10000000 + _main_stack_size;
+ASSERT(_main_stack_size   % 8 == 0, "MAIN stack size error");
+
+/* Mapping the heap into XRAM */
+_heap_end = 0xd0800000;                            /* end of available ram  */
+
+/* identify the Entry Point  */
+ENTRY(_Z13Reset_Handlerv)
+
+/* specify the memory areas  */
+MEMORY
+{
+    flash(rx)    : ORIGIN = 0x08000000, LENGTH =   2M
+    /*
+     * Note, the small ram starts at 0x10000000 but it is necessary to add the
+     * size of the main stack, so it is 0x10000200.
+     */
+    smallram(wx) : ORIGIN = 0x10000200, LENGTH =  64K-0x200 
+    largeram(wx) : ORIGIN = 0x20000000, LENGTH = 192K
+    backupram(rw): ORIGIN = 0x40024000, LENGTH =   4K
+    xram(wx)     : ORIGIN = 0xd0000000, LENGTH =   8M
+}
+
+/* now define the output sections  */
+SECTIONS
+{
+    . = 0;
+    
+    /* .text section: code goes to flash */
+    .text :
+    {
+        /* Startup code must go at address 0 */
+        KEEP(*(.isr_vector))
+        
+        *(.text)
+        *(.text.*)
+        *(.gnu.linkonce.t.*)
+        /* these sections for thumb interwork? */
+        *(.glue_7)
+        *(.glue_7t)
+        /* these sections for C++? */
+        *(.gcc_except_table)
+        *(.gcc_except_table.*)
+        *(.ARM.extab*)
+        *(.gnu.linkonce.armextab.*)
+
+        . = ALIGN(4);
+        /* .rodata: constant data */
+        *(.rodata)
+        *(.rodata.*)
+        *(.gnu.linkonce.r.*)
+
+        /* C++ Static constructors/destructors (eabi) */
+        . = ALIGN(4);
+        KEEP(*(.init))
+        
+        . = ALIGN(4);
+        __miosix_init_array_start = .;
+        KEEP (*(SORT(.miosix_init_array.*)))
+        KEEP (*(.miosix_init_array))
+        __miosix_init_array_end = .;
+
+        . = ALIGN(4);
+        __preinit_array_start = .;
+        KEEP (*(.preinit_array))
+        __preinit_array_end = .;
+
+        . = ALIGN(4);
+        __init_array_start = .;
+        KEEP (*(SORT(.init_array.*)))
+        KEEP (*(.init_array))
+        __init_array_end = .;
+
+        . = ALIGN(4);
+        KEEP(*(.fini))
+
+        . = ALIGN(4);
+        __fini_array_start = .;
+        KEEP (*(.fini_array))
+        KEEP (*(SORT(.fini_array.*)))
+        __fini_array_end = .;
+
+        /* C++ Static constructors/destructors (elf)  */
+        . = ALIGN(4);
+        _ctor_start = .;
+        KEEP (*crtbegin.o(.ctors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .ctors))
+        KEEP (*(SORT(.ctors.*)))
+        KEEP (*crtend.o(.ctors))
+       _ctor_end = .;
+
+        . = ALIGN(4);
+        KEEP (*crtbegin.o(.dtors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .dtors))
+        KEEP (*(SORT(.dtors.*)))
+        KEEP (*crtend.o(.dtors))
+    } > flash
+
+    /* .ARM.exidx is sorted, so has to go in its own output section.  */
+    __exidx_start = .;
+    .ARM.exidx :
+    {
+        *(.ARM.exidx* .gnu.linkonce.armexidx.*)
+    } > flash
+    __exidx_end = .;
+
+	/* .data section: global variables go to ram, but also store a copy to
+       flash to initialize them */
+    .data : ALIGN(8)
+    {
+        _data = .;
+        *(.data)
+        *(.data.*)
+        *(.gnu.linkonce.d.*)
+        . = ALIGN(8);
+        _edata = .;
+    } > xram AT > flash
+    _etext = LOADADDR(.data);
+
+    /* .bss section: uninitialized global variables go to ram */
+    _bss_start = .;
+    .bss :
+    {
+        *(.bss)
+        *(.bss.*)
+        *(.gnu.linkonce.b.*)
+        . = ALIGN(8);
+    } > xram
+    _bss_end = .;
+
+    _end = .;
+    PROVIDE(end = .);
+}
diff --git a/src/bsps/stm32f429zi_death_stack_v3/config/board_options.cmake b/src/bsps/stm32f429zi_death_stack_v3/config/board_options.cmake
new file mode 100644
index 000000000..21174c223
--- /dev/null
+++ b/src/bsps/stm32f429zi_death_stack_v3/config/board_options.cmake
@@ -0,0 +1,120 @@
+# Copyright (C) 2023 by Skyward
+#
+# This program is free software; you can redistribute it and/or 
+# it under the terms of the GNU General Public License as published 
+# the Free Software Foundation; either version 2 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.
+#
+# As a special exception, if other files instantiate templates or use
+# macros or inline functions from this file, or you compile this file
+# and link it with other works to produce a work based on this file,
+# this file does not by itself cause the resulting work to be covered
+# by the GNU General Public License. However the source code for this
+# file must still be made available in accordance with the GNU 
+# Public License. This exception does not invalidate any other 
+# why a work based on this file might be covered by the GNU General
+# Public License.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, see <http://www.gnu.org/licenses/>
+
+set(BOARD_NAME stm32f429zi_death_stack_v3)
+set(ARCH_NAME cortexM4_stm32f4)
+
+# Base directories with header files for this board
+set(ARCH_PATH ${KPATH}/arch/${ARCH_NAME}/common)
+set(BOARD_PATH ${BOARDCORE_PATH}/src/bsps/${BOARD_NAME})
+set(BOARD_CONFIG_PATH ${BOARDCORE_PATH}/src/bsps/${BOARD_NAME}/config)
+
+# Specify where to find the board specific config/miosix_settings.h
+set(BOARD_MIOSIX_SETTINGS_PATH ${BOARD_PATH})
+
+# Specify where to find the board specific config/mxgui_settings.h
+set(BOARD_MXGUI_SETTINGS_PATH ${BOARD_PATH})
+
+# Optimization flags:
+# -O0 do no optimization, the default if no optimization level is specified
+# -O or -O1 optimize minimally
+# -O2 optimize more
+# -O3 optimize even more
+# -Ofast optimize very aggressively to the point of breaking the standard
+# -Og Optimize debugging experience, enables optimizations that do not
+# interfere with debugging
+# -Os Optimize for size with -O2 optimizations that do not increase code size
+set(OPT_OPTIMIZATION -O2)
+
+# Boot and linker files
+set(BOOT_FILE ${BOARD_PATH}/core/stage_1_boot.cpp)
+
+# Linker script type, there are three options
+# 1) Code in FLASH, stack + heap in internal RAM (file *_rom.ld)
+#    the most common choice, available for all microcontrollers
+# 2) Code in FLASH, stack + heap in external RAM (file *8m_xram.ld)
+#    You must uncomment -D__ENABLE_XRAM below in this case.
+# set(LINKER_SCRIPT ${BOARD_PATH}/stm32_2m+256k_rom.ld)
+set(LINKER_SCRIPT ${BOARD_PATH}/stm32_2m+8m_xram.ld)
+
+# Uncommenting __ENABLE_XRAM enables the initialization of the external
+# 8MB SDRAM memory. Do not uncomment this even if you don't use a linker
+# script that requires it, as it is used for the LCD framebuffer.
+set(XRAM -D__ENABLE_XRAM)
+
+# Select clock frequency. Warning: the default clock frequency for
+# this board is 168MHz and not 180MHz because, due to a limitation in
+# the PLL, it is not possible to generate a precise 48MHz output when
+# running the core at 180MHz. If 180MHz is chosen the USB peripheral will
+# NOT WORK and the SDIO and RNG will run ~6% slower (45MHz insteand of 48)
+# Define USE_INTERNAL_CLOCK to bypass the external oscillator
+# set(CLOCK_FREQ -DHSE_VALUE=8000000 -DSYSCLK_FREQ_180MHz=180000000)
+set(CLOCK_FREQ -DHSE_VALUE=8000000 -DSYSCLK_FREQ_168MHz=168000000)
+# set(CLOCK_FREQ -DHSE_VALUE=8000000 -DSYSCLK_FREQ_100MHz=100000000)
+
+# C++ Exception/rtti support disable flags.
+# To save code size if not using C++ exceptions (nor some STL code which
+# implicitly uses it) uncomment this option.
+# -D__NO_EXCEPTIONS is used by Miosix to know if exceptions are used.
+# set(OPT_EXCEPT -fno-exceptions -fno-rtti -D__NO_EXCEPTIONS)
+
+# Specify a custom flash command
+# This is the program that is invoked when the flash flag (-f or --flash) is
+# used with the Miosix Build System. Use $binary or $hex as placeolders, they
+# will be replaced by the build systems with the binary or hex file repectively.
+# If a command is not specified, the build system will use st-flash if found
+# set(PROGRAM_CMDLINE "here your custom flash command")
+
+# Basic flags
+set(FLAGS_BASE -mcpu=cortex-m4 -mthumb -mfloat-abi=hard -mfpu=fpv4-sp-d16)
+
+# Flags for ASM and linker
+set(AFLAGS_BASE ${FLAGS_BASE})
+set(LFLAGS_BASE ${FLAGS_BASE} -Wl,--gc-sections,-Map,main.map -Wl,-T${LINKER_SCRIPT} ${OPT_EXCEPT} ${OPT_OPTIMIZATION} -nostdlib)
+
+# Flags for C/C++
+string(TOUPPER ${BOARD_NAME} BOARD_UPPER)
+set(CFLAGS_BASE
+    -D_BOARD_${BOARD_UPPER} -D_MIOSIX_BOARDNAME=\"${BOARD_NAME}\"
+    -D_DEFAULT_SOURCE=1 -ffunction-sections -Wall -Werror=return-type -g
+    -D_ARCH_CORTEXM4_STM32F4
+    ${CLOCK_FREQ} ${XRAM} ${SRAM_BOOT} ${FLAGS_BASE} ${OPT_OPTIMIZATION} -c
+)
+set(CXXFLAGS_BASE ${CFLAGS_BASE} ${OPT_EXCEPT})
+
+# Select architecture specific files
+set(ARCH_SRC
+    ${ARCH_PATH}/interfaces-impl/delays.cpp
+    ${ARCH_PATH}/interfaces-impl/gpio_impl.cpp
+    ${ARCH_PATH}/interfaces-impl/portability.cpp
+    ${BOARD_PATH}/interfaces-impl/bsp.cpp
+    ${KPATH}/arch/common/CMSIS/Device/ST/STM32F4xx/Source/Templates/system_stm32f4xx.c
+    ${KPATH}/arch/common/core/interrupts_cortexMx.cpp
+    ${KPATH}/arch/common/core/mpu_cortexMx.cpp
+    ${KPATH}/arch/common/core/stm32f2_f4_l4_f7_h7_os_timer.cpp
+    ${KPATH}/arch/common/drivers/sd_stm32f2_f4_f7.cpp
+    ${KPATH}/arch/common/drivers/serial_stm32.cpp
+    ${KPATH}/arch/common/drivers/stm32_hardware_rng.cpp
+)
diff --git a/src/bsps/stm32f429zi_death_stack_v3/config/board_settings.h b/src/bsps/stm32f429zi_death_stack_v3/config/board_settings.h
new file mode 100644
index 000000000..226e0edad
--- /dev/null
+++ b/src/bsps/stm32f429zi_death_stack_v3/config/board_settings.h
@@ -0,0 +1,74 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Authors: Emilio Corigliano, Alberto Nidasio
+ *
+ * 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.
+ */
+
+#ifndef BOARD_SETTINGS_H
+#define BOARD_SETTINGS_H
+
+#include "util/version.h"
+
+/**
+ * \internal
+ * Versioning for board_settings.h for out of git tree projects
+ */
+#define BOARD_SETTINGS_VERSION 300
+
+namespace miosix
+{
+
+/**
+ * \addtogroup Settings
+ * \{
+ */
+
+/// Size of stack for main().
+/// The C standard library is stack-heavy (iprintf requires 1KB) but the
+/// STM32F407VG only has 192KB of RAM so there is room for a big 4K stack.
+const unsigned int MAIN_STACK_SIZE = 16 * 1024;
+
+/// Serial port
+const unsigned int defaultSerial      = 1;
+const unsigned int defaultSerialSpeed = 115200;
+const bool defaultSerialFlowctrl      = false;
+// #define SERIAL_1_DMA
+// #define SERIAL_2_DMA //Serial 2 can't be used (GPIO conflict), so no DMA
+// #define SERIAL_3_DMA //Serial 3 can't be used (GPIO conflict), so no DMA
+
+// #define I2C_WITH_DMA
+
+// SD card driver
+static const unsigned char sdVoltage = 33;  // Board powered @ 3.3V
+#ifdef __ENABLE_XRAM
+// Reduce SD clock to ~4.8MHz
+#define OVERRIDE_SD_CLOCK_DIVIDER_MAX 8
+#endif  //__ENABLE_XRAM
+// #define SD_ONE_BIT_DATABUS //This board supports 4 bit databus to SD card
+
+/// Analog supply voltage for ADC, DAC, Reset blocks, RCs and PLL
+#define V_DDA_VOLTAGE 3.3f
+
+/**
+ * \}
+ */
+
+}  // namespace miosix
+
+#endif /* BOARD_SETTINGS_H */
diff --git a/src/bsps/stm32f429zi_death_stack_v3/config/miosix_settings.h b/src/bsps/stm32f429zi_death_stack_v3/config/miosix_settings.h
new file mode 100644
index 000000000..cdef9ddab
--- /dev/null
+++ b/src/bsps/stm32f429zi_death_stack_v3/config/miosix_settings.h
@@ -0,0 +1,240 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Authors: Emilio Corigliano, Alberto Nidasio
+ *
+ * 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.
+ */
+
+#pragma once
+
+// Before you can compile the kernel you have to configure it by editing this
+// file. After that, comment out this line to disable the reminder error.
+// The PARSING_FROM_IDE is because Netbeans gets confused by this, it is never
+// defined when compiling the code.
+#ifndef PARSING_FROM_IDE
+//#error This error is a reminder that you have not edited miosix_settings.h
+// yet.
+#endif  // PARSING_FROM_IDE
+
+/**
+ * \file miosix_settings.h
+ * NOTE: this file contains ONLY configuration options that are not dependent
+ * on architecture specific details. The other options are in the following
+ * files which are included here:
+ * miosix/arch/architecture name/common/arch_settings.h
+ * miosix/config/arch/architecture name/board name/board_settings.h
+ */
+#include "arch_settings.h"
+#include "board_settings.h"
+#include "util/version.h"
+
+/**
+ * \internal
+ * Versioning for miosix_settings.h for out of git tree projects
+ */
+#define MIOSIX_SETTINGS_VERSION 300
+
+namespace miosix
+{
+
+/**
+ * \addtogroup Settings
+ * \{
+ */
+
+//
+// Scheduler options
+//
+
+/// \def SCHED_TYPE_PRIORITY
+/// If uncommented selects the priority scheduler
+/// \def SCHED_TYPE_CONTROL_BASED
+/// If uncommented selects the control based scheduler
+/// \def SCHED_TYPE_EDF
+/// If uncommented selects the EDF scheduler
+// Uncomment only *one* of those
+
+#define SCHED_TYPE_PRIORITY
+//#define SCHED_TYPE_CONTROL_BASED
+//#define SCHED_TYPE_EDF
+
+/// \def WITH_CPU_TIME_COUNTER
+/// Allows to enable/disable CPUTimeCounter to save code size and remove its
+/// overhead from the scheduling process. By default it is not defined
+/// (CPUTimeCounter is disabled).
+//#define WITH_CPU_TIME_COUNTER
+
+//
+// Filesystem options
+//
+
+/// \def WITH_FILESYSTEM
+/// Allows to enable/disable filesystem support to save code size
+/// By default it is defined (filesystem support is enabled)
+#define WITH_FILESYSTEM
+
+/// \def WITH_DEVFS
+/// Allows to enable/disable DevFs support to save code size
+/// By default it is defined (DevFs is enabled)
+#define WITH_DEVFS
+
+/// \def SYNC_AFTER_WRITE
+/// Increases filesystem write robustness. After each write operation the
+/// filesystem is synced so that a power failure happens data is not lost
+/// (unless power failure happens exactly between the write and the sync)
+/// Unfortunately write latency and throughput becomes twice as worse
+/// By default it is defined (slow but safe)
+#define SYNC_AFTER_WRITE
+
+/// Maximum number of open files. Trying to open more will fail.
+/// Cannot be lower than 3, as the first three are stdin, stdout, stderr
+const unsigned char MAX_OPEN_FILES = 8;
+
+/// \def WITH_PROCESSES
+/// If uncommented enables support for processes as well as threads.
+/// This enables the dynamic loader to load elf programs, the extended system
+/// call service and, if the hardware supports it, the MPU to provide memory
+/// isolation of processes
+//#define WITH_PROCESSES
+
+#if defined(WITH_PROCESSES) && defined(__NO_EXCEPTIONS)
+#error Processes require C++ exception support
+#endif  // defined(WITH_PROCESSES) && defined(__NO_EXCEPTIONS)
+
+#if defined(WITH_PROCESSES) && !defined(WITH_FILESYSTEM)
+#error Processes require filesystem support
+#endif  // defined(WITH_PROCESSES) && !defined(WITH_FILESYSTEM)
+
+#if defined(WITH_PROCESSES) && !defined(WITH_DEVFS)
+#error Processes require devfs support
+#endif  // defined(WITH_PROCESSES) && !defined(WITH_DEVFS)
+
+//
+// C/C++ standard library I/O (stdin, stdout and stderr related)
+//
+
+/// \def WITH_BOOTLOG
+/// Uncomment to print bootlogs on stdout.
+/// By default it is defined (bootlogs are printed)
+#define WITH_BOOTLOG
+
+/// \def WITH_ERRLOG
+/// Uncomment for debug information on stdout.
+/// By default it is defined (error information is printed)
+#define WITH_ERRLOG
+
+//
+// Kernel related options (stack sizes, priorities)
+//
+
+/// \def WITH_DEEP_SLEEP
+/// Adds interfaces and required variables to support deep sleep state switch
+/// automatically when peripherals are not required
+//#define WITH_DEEP_SLEEP
+
+/**
+ * \def JTAG_DISABLE_SLEEP
+ * JTAG debuggers lose communication with the device if it enters sleep
+ * mode, so to use debugging it is necessary to disable sleep in the idle
+ * thread. By default it is not defined (idle thread calls sleep).
+ */
+//#define JTAG_DISABLE_SLEEP
+
+#if defined(WITH_DEEP_SLEEP) && defined(JTAG_DISABLE_SLEEP)
+#error Deep sleep cannot work together with jtag
+#endif  // defined(WITH_PROCESSES) && !defined(WITH_DEVFS)
+
+/// Minimum stack size (MUST be divisible by 4)
+const unsigned int STACK_MIN = 256;
+
+/// \internal Size of idle thread stack.
+/// Should be >=STACK_MIN (MUST be divisible by 4)
+const unsigned int STACK_IDLE = 256;
+
+/// Default stack size for pthread_create.
+/// The chosen value is enough to call C standard library functions
+/// such as printf/fopen which are stack-heavy
+const unsigned int STACK_DEFAULT_FOR_PTHREAD = 2048;
+
+/// Maximum size of the RAM image of a process. If a program requires more
+/// the kernel will not run it (MUST be divisible by 4)
+const unsigned int MAX_PROCESS_IMAGE_SIZE = 64 * 1024;
+
+/// Minimum size of the stack for a process. If a program specifies a lower
+/// size the kernel will not run it (MUST be divisible by 4)
+const unsigned int MIN_PROCESS_STACK_SIZE = STACK_MIN;
+
+/// Every userspace thread has two stacks, one for when it is running in
+/// userspace and one for when it is running in kernelspace (that is, while it
+/// is executing system calls). This is the size of the stack for when the
+/// thread is running in kernelspace (MUST be divisible by 4)
+const unsigned int SYSTEM_MODE_PROCESS_STACK_SIZE = 2 * 1024;
+
+/// Number of priorities (MUST be >1)
+/// PRIORITY_MAX-1 is the highest priority, 0 is the lowest. -1 is reserved as
+/// the priority of the idle thread.
+/// The meaning of a thread's priority depends on the chosen scheduler.
+#ifdef SCHED_TYPE_PRIORITY
+// Can be modified, but a high value makes context switches more expensive
+const short int PRIORITY_MAX = 4;
+#elif defined(SCHED_TYPE_CONTROL_BASED)
+// Don't touch, the limit is due to the fixed point implementation
+// It's not needed for if floating point is selected, but kept for consistency
+const short int PRIORITY_MAX = 64;
+#else  // SCHED_TYPE_EDF
+// Doesn't exist for this kind of scheduler
+#endif
+
+/// Priority of main()
+/// The meaning of a thread's priority depends on the chosen scheduler.
+const unsigned char MAIN_PRIORITY = 1;
+
+#ifdef SCHED_TYPE_PRIORITY
+/// Maximum thread time slice in nanoseconds, after which preemption occurs
+const unsigned int MAX_TIME_SLICE = 1000000;
+#endif  // SCHED_TYPE_PRIORITY
+
+//
+// Other low level kernel options. There is usually no need to modify these.
+//
+
+/// \internal Length of wartermark (in bytes) to check stack overflow.
+/// MUST be divisible by 4 and can also be zero.
+/// A high value increases context switch time.
+const unsigned int WATERMARK_LEN = 16;
+
+/// \internal Used to fill watermark
+const unsigned int WATERMARK_FILL = 0xaaaaaaaa;
+
+/// \internal Used to fill stack (for checking stack usage)
+const unsigned int STACK_FILL = 0xbbbbbbbb;
+
+// Compiler version checks
+#if !defined(_MIOSIX_GCC_PATCH_MAJOR) || _MIOSIX_GCC_PATCH_MAJOR < 3
+#error \
+    "You are using a too old or unsupported compiler. Get the latest one from https://miosix.org/wiki/index.php?title=Miosix_Toolchain"
+#endif
+#if _MIOSIX_GCC_PATCH_MAJOR > 3
+#warning "You are using a too new compiler, which may not be supported"
+#endif
+
+/**
+ * \}
+ */
+
+}  // namespace miosix
diff --git a/src/bsps/stm32f429zi_death_stack_v3/core/stage_1_boot.cpp b/src/bsps/stm32f429zi_death_stack_v3/core/stage_1_boot.cpp
new file mode 100644
index 000000000..47b5354ae
--- /dev/null
+++ b/src/bsps/stm32f429zi_death_stack_v3/core/stage_1_boot.cpp
@@ -0,0 +1,436 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Authors: Emilio Corigliano, Alberto Nidasio
+ *
+ * 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 <string.h>
+
+#include "core/interrupts.h"  //For the unexpected interrupt call
+#include "interfaces/arch_registers.h"
+#include "interfaces/bsp.h"
+#include "kernel/stage_2_boot.h"
+
+/*
+ * startup.cpp
+ * STM32 C++ startup.
+ * NOTE: for stm32f42x devices ONLY.
+ * Supports interrupt handlers in C++ without extern "C"
+ * Developed by Terraneo Federico, based on ST startup code.
+ * Additionally modified to boot Miosix.
+ */
+
+/**
+ * Called by Reset_Handler, performs initialization and calls main.
+ * Never returns.
+ */
+void program_startup() __attribute__((noreturn));
+void program_startup()
+{
+    // Cortex M3 core appears to get out of reset with interrupts already
+    // enabled
+    __disable_irq();
+
+    // These are defined in the linker script
+    extern unsigned char _etext asm("_etext");
+    extern unsigned char _data asm("_data");
+    extern unsigned char _edata asm("_edata");
+    extern unsigned char _bss_start asm("_bss_start");
+    extern unsigned char _bss_end asm("_bss_end");
+
+    // Initialize .data section, clear .bss section
+    unsigned char *etext     = &_etext;
+    unsigned char *data      = &_data;
+    unsigned char *edata     = &_edata;
+    unsigned char *bss_start = &_bss_start;
+    unsigned char *bss_end   = &_bss_end;
+    memcpy(data, etext, edata - data);
+    memset(bss_start, 0, bss_end - bss_start);
+
+    // Move on to stage 2
+    _init();
+
+    // If main returns, reboot
+    NVIC_SystemReset();
+    for (;;)
+        ;
+}
+
+/**
+ * Reset handler, called by hardware immediately after reset
+ */
+void Reset_Handler() __attribute__((__interrupt__, noreturn));
+void Reset_Handler()
+{
+    // SystemInit() is called *before* initializing .data and zeroing .bss
+    // Despite all startup files provided by ST do the opposite, there are three
+    // good reasons to do so:
+    // First, the CMSIS specifications say that SystemInit() must not access
+    // global variables, so it is actually possible to call it before
+    // Second, when running Miosix with the xram linker scripts .data and .bss
+    // are placed in the external RAM, so we *must* call SystemInit(), which
+    // enables xram, before touching .data and .bss
+    // Third, this is a performance improvement since the loops that initialize
+    //.data and zeros .bss now run with the CPU at full speed instead of 8MHz
+    SystemInit();
+// ST does not provide code to initialize the stm32f429-disco SDRAM at boot.
+// Put after SystemInit() as SDRAM is timing-sensitive and needs the full
+// clock speed.
+#ifdef __ENABLE_XRAM
+    miosix::configureSdram();
+#endif  //__ENABLE_XRAM
+
+    /*
+     * Initialize process stack and switch to it.
+     * This is required for booting Miosix, a small portion of the top of the
+     * heap area will be used as stack until the first thread starts. After,
+     * this stack will be abandoned and the process stack will point to the
+     * current thread's stack.
+     */
+    asm volatile(
+        "ldr r0,  =_heap_end          \n\t"
+        "msr psp, r0                  \n\t"
+        "movw r0, #2                  \n\n"  // Privileged, process stack
+        "msr control, r0              \n\t"
+        "isb                          \n\t" ::
+            : "r0");
+
+    program_startup();
+}
+
+/**
+ * All unused interrupts call this function.
+ */
+extern "C" void Default_Handler() { unexpectedInterrupt(); }
+
+// System handlers
+void /*__attribute__((weak))*/ Reset_Handler();      // These interrupts are not
+void /*__attribute__((weak))*/ NMI_Handler();        // weak because they are
+void /*__attribute__((weak))*/ HardFault_Handler();  // surely defined by Miosix
+void /*__attribute__((weak))*/ MemManage_Handler();
+void /*__attribute__((weak))*/ BusFault_Handler();
+void /*__attribute__((weak))*/ UsageFault_Handler();
+void /*__attribute__((weak))*/ SVC_Handler();
+void /*__attribute__((weak))*/ DebugMon_Handler();
+void /*__attribute__((weak))*/ PendSV_Handler();
+void __attribute__((weak)) SysTick_Handler();
+
+// Interrupt handlers
+void __attribute__((weak)) WWDG_IRQHandler();
+void __attribute__((weak)) PVD_IRQHandler();
+void __attribute__((weak)) TAMP_STAMP_IRQHandler();
+void __attribute__((weak)) RTC_WKUP_IRQHandler();
+void __attribute__((weak)) FLASH_IRQHandler();
+void __attribute__((weak)) RCC_IRQHandler();
+void __attribute__((weak)) EXTI0_IRQHandler();
+void __attribute__((weak)) EXTI1_IRQHandler();
+void __attribute__((weak)) EXTI2_IRQHandler();
+void __attribute__((weak)) EXTI3_IRQHandler();
+void __attribute__((weak)) EXTI4_IRQHandler();
+void __attribute__((weak)) DMA1_Stream0_IRQHandler();
+void __attribute__((weak)) DMA1_Stream1_IRQHandler();
+void __attribute__((weak)) DMA1_Stream2_IRQHandler();
+void __attribute__((weak)) DMA1_Stream3_IRQHandler();
+void __attribute__((weak)) DMA1_Stream4_IRQHandler();
+void __attribute__((weak)) DMA1_Stream5_IRQHandler();
+void __attribute__((weak)) DMA1_Stream6_IRQHandler();
+void __attribute__((weak)) ADC_IRQHandler();
+void __attribute__((weak)) CAN1_TX_IRQHandler();
+void __attribute__((weak)) CAN1_RX0_IRQHandler();
+void __attribute__((weak)) CAN1_RX1_IRQHandler();
+void __attribute__((weak)) CAN1_SCE_IRQHandler();
+void __attribute__((weak)) EXTI9_5_IRQHandler();
+void __attribute__((weak)) TIM1_BRK_TIM9_IRQHandler();
+void __attribute__((weak)) TIM1_UP_TIM10_IRQHandler();
+void __attribute__((weak)) TIM1_TRG_COM_TIM11_IRQHandler();
+void __attribute__((weak)) TIM1_CC_IRQHandler();
+void __attribute__((weak)) TIM2_IRQHandler();
+void __attribute__((weak)) TIM3_IRQHandler();
+void __attribute__((weak)) TIM4_IRQHandler();
+void __attribute__((weak)) I2C1_EV_IRQHandler();
+void __attribute__((weak)) I2C1_ER_IRQHandler();
+void __attribute__((weak)) I2C2_EV_IRQHandler();
+void __attribute__((weak)) I2C2_ER_IRQHandler();
+void __attribute__((weak)) SPI1_IRQHandler();
+void __attribute__((weak)) SPI2_IRQHandler();
+void __attribute__((weak)) USART1_IRQHandler();
+void __attribute__((weak)) USART2_IRQHandler();
+void __attribute__((weak)) USART3_IRQHandler();
+void __attribute__((weak)) EXTI15_10_IRQHandler();
+void __attribute__((weak)) RTC_Alarm_IRQHandler();
+void __attribute__((weak)) OTG_FS_WKUP_IRQHandler();
+void __attribute__((weak)) TIM8_BRK_TIM12_IRQHandler();
+void __attribute__((weak)) TIM8_UP_TIM13_IRQHandler();
+void __attribute__((weak)) TIM8_TRG_COM_TIM14_IRQHandler();
+void __attribute__((weak)) TIM8_CC_IRQHandler();
+void __attribute__((weak)) DMA1_Stream7_IRQHandler();
+void __attribute__((weak)) FMC_IRQHandler();
+void __attribute__((weak)) SDIO_IRQHandler();
+void __attribute__((weak)) TIM5_IRQHandler();
+void __attribute__((weak)) SPI3_IRQHandler();
+void __attribute__((weak)) UART4_IRQHandler();
+void __attribute__((weak)) UART5_IRQHandler();
+void __attribute__((weak)) TIM6_DAC_IRQHandler();
+void __attribute__((weak)) TIM7_IRQHandler();
+void __attribute__((weak)) DMA2_Stream0_IRQHandler();
+void __attribute__((weak)) DMA2_Stream1_IRQHandler();
+void __attribute__((weak)) DMA2_Stream2_IRQHandler();
+void __attribute__((weak)) DMA2_Stream3_IRQHandler();
+void __attribute__((weak)) DMA2_Stream4_IRQHandler();
+void __attribute__((weak)) ETH_IRQHandler();
+void __attribute__((weak)) ETH_WKUP_IRQHandler();
+void __attribute__((weak)) CAN2_TX_IRQHandler();
+void __attribute__((weak)) CAN2_RX0_IRQHandler();
+void __attribute__((weak)) CAN2_RX1_IRQHandler();
+void __attribute__((weak)) CAN2_SCE_IRQHandler();
+void __attribute__((weak)) OTG_FS_IRQHandler();
+void __attribute__((weak)) DMA2_Stream5_IRQHandler();
+void __attribute__((weak)) DMA2_Stream6_IRQHandler();
+void __attribute__((weak)) DMA2_Stream7_IRQHandler();
+void __attribute__((weak)) USART6_IRQHandler();
+void __attribute__((weak)) I2C3_EV_IRQHandler();
+void __attribute__((weak)) I2C3_ER_IRQHandler();
+void __attribute__((weak)) OTG_HS_EP1_OUT_IRQHandler();
+void __attribute__((weak)) OTG_HS_EP1_IN_IRQHandler();
+void __attribute__((weak)) OTG_HS_WKUP_IRQHandler();
+void __attribute__((weak)) OTG_HS_IRQHandler();
+void __attribute__((weak)) DCMI_IRQHandler();
+void __attribute__((weak)) CRYP_IRQHandler();
+void __attribute__((weak)) HASH_RNG_IRQHandler();
+void __attribute__((weak)) FPU_IRQHandler();
+void __attribute__((weak)) UART7_IRQHandler();
+void __attribute__((weak)) UART8_IRQHandler();
+void __attribute__((weak)) SPI4_IRQHandler();
+void __attribute__((weak)) SPI5_IRQHandler();
+void __attribute__((weak)) SPI6_IRQHandler();
+void __attribute__((weak)) SAI1_IRQHandler();
+void __attribute__((weak)) LTDC_IRQHandler();
+void __attribute__((weak)) LTDC_ER_IRQHandler();
+void __attribute__((weak)) DMA2D_IRQHandler();
+
+// Stack top, defined in the linker script
+extern char _main_stack_top asm("_main_stack_top");
+
+// Interrupt vectors, must be placed @ address 0x00000000
+// The extern declaration is required otherwise g++ optimizes it out
+extern void (*const __Vectors[])();
+void (*const __Vectors[])() __attribute__((section(".isr_vector"))) = {
+    reinterpret_cast<void (*)()>(&_main_stack_top), /* Stack pointer*/
+    Reset_Handler,                                  /* Reset Handler */
+    NMI_Handler,                                    /* NMI Handler */
+    HardFault_Handler,                              /* Hard Fault Handler */
+    MemManage_Handler,                              /* MPU Fault Handler */
+    BusFault_Handler,                               /* Bus Fault Handler */
+    UsageFault_Handler,                             /* Usage Fault Handler */
+    0,                                              /* Reserved */
+    0,                                              /* Reserved */
+    0,                                              /* Reserved */
+    0,                                              /* Reserved */
+    SVC_Handler,                                    /* SVCall Handler */
+    DebugMon_Handler,                               /* Debug Monitor Handler */
+    0,                                              /* Reserved */
+    PendSV_Handler,                                 /* PendSV Handler */
+    SysTick_Handler,                                /* SysTick Handler */
+
+    /* External Interrupts */
+    WWDG_IRQHandler,
+    PVD_IRQHandler,
+    TAMP_STAMP_IRQHandler,
+    RTC_WKUP_IRQHandler,
+    FLASH_IRQHandler,
+    RCC_IRQHandler,
+    EXTI0_IRQHandler,
+    EXTI1_IRQHandler,
+    EXTI2_IRQHandler,
+    EXTI3_IRQHandler,
+    EXTI4_IRQHandler,
+    DMA1_Stream0_IRQHandler,
+    DMA1_Stream1_IRQHandler,
+    DMA1_Stream2_IRQHandler,
+    DMA1_Stream3_IRQHandler,
+    DMA1_Stream4_IRQHandler,
+    DMA1_Stream5_IRQHandler,
+    DMA1_Stream6_IRQHandler,
+    ADC_IRQHandler,
+    CAN1_TX_IRQHandler,
+    CAN1_RX0_IRQHandler,
+    CAN1_RX1_IRQHandler,
+    CAN1_SCE_IRQHandler,
+    EXTI9_5_IRQHandler,
+    TIM1_BRK_TIM9_IRQHandler,
+    TIM1_UP_TIM10_IRQHandler,
+    TIM1_TRG_COM_TIM11_IRQHandler,
+    TIM1_CC_IRQHandler,
+    TIM2_IRQHandler,
+    TIM3_IRQHandler,
+    TIM4_IRQHandler,
+    I2C1_EV_IRQHandler,
+    I2C1_ER_IRQHandler,
+    I2C2_EV_IRQHandler,
+    I2C2_ER_IRQHandler,
+    SPI1_IRQHandler,
+    SPI2_IRQHandler,
+    USART1_IRQHandler,
+    USART2_IRQHandler,
+    USART3_IRQHandler,
+    EXTI15_10_IRQHandler,
+    RTC_Alarm_IRQHandler,
+    OTG_FS_WKUP_IRQHandler,
+    TIM8_BRK_TIM12_IRQHandler,
+    TIM8_UP_TIM13_IRQHandler,
+    TIM8_TRG_COM_TIM14_IRQHandler,
+    TIM8_CC_IRQHandler,
+    DMA1_Stream7_IRQHandler,
+    FMC_IRQHandler,
+    SDIO_IRQHandler,
+    TIM5_IRQHandler,
+    SPI3_IRQHandler,
+    UART4_IRQHandler,
+    UART5_IRQHandler,
+    TIM6_DAC_IRQHandler,
+    TIM7_IRQHandler,
+    DMA2_Stream0_IRQHandler,
+    DMA2_Stream1_IRQHandler,
+    DMA2_Stream2_IRQHandler,
+    DMA2_Stream3_IRQHandler,
+    DMA2_Stream4_IRQHandler,
+    ETH_IRQHandler,
+    ETH_WKUP_IRQHandler,
+    CAN2_TX_IRQHandler,
+    CAN2_RX0_IRQHandler,
+    CAN2_RX1_IRQHandler,
+    CAN2_SCE_IRQHandler,
+    OTG_FS_IRQHandler,
+    DMA2_Stream5_IRQHandler,
+    DMA2_Stream6_IRQHandler,
+    DMA2_Stream7_IRQHandler,
+    USART6_IRQHandler,
+    I2C3_EV_IRQHandler,
+    I2C3_ER_IRQHandler,
+    OTG_HS_EP1_OUT_IRQHandler,
+    OTG_HS_EP1_IN_IRQHandler,
+    OTG_HS_WKUP_IRQHandler,
+    OTG_HS_IRQHandler,
+    DCMI_IRQHandler,
+    CRYP_IRQHandler,
+    HASH_RNG_IRQHandler,
+    FPU_IRQHandler,
+    UART7_IRQHandler,
+    UART8_IRQHandler,
+    SPI4_IRQHandler,
+    SPI5_IRQHandler,
+    SPI6_IRQHandler,
+    SAI1_IRQHandler,
+    LTDC_IRQHandler,
+    LTDC_ER_IRQHandler,
+    DMA2D_IRQHandler,
+};
+
+#pragma weak SysTick_IRQHandler            = Default_Handler
+#pragma weak WWDG_IRQHandler               = Default_Handler
+#pragma weak PVD_IRQHandler                = Default_Handler
+#pragma weak TAMP_STAMP_IRQHandler         = Default_Handler
+#pragma weak RTC_WKUP_IRQHandler           = Default_Handler
+#pragma weak FLASH_IRQHandler              = Default_Handler
+#pragma weak RCC_IRQHandler                = Default_Handler
+#pragma weak EXTI0_IRQHandler              = Default_Handler
+#pragma weak EXTI1_IRQHandler              = Default_Handler
+#pragma weak EXTI2_IRQHandler              = Default_Handler
+#pragma weak EXTI3_IRQHandler              = Default_Handler
+#pragma weak EXTI4_IRQHandler              = Default_Handler
+#pragma weak DMA1_Stream0_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream1_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream2_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream3_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream4_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream5_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream6_IRQHandler       = Default_Handler
+#pragma weak ADC_IRQHandler                = Default_Handler
+#pragma weak CAN1_TX_IRQHandler            = Default_Handler
+#pragma weak CAN1_RX0_IRQHandler           = Default_Handler
+#pragma weak CAN1_RX1_IRQHandler           = Default_Handler
+#pragma weak CAN1_SCE_IRQHandler           = Default_Handler
+#pragma weak EXTI9_5_IRQHandler            = Default_Handler
+#pragma weak TIM1_BRK_TIM9_IRQHandler      = Default_Handler
+#pragma weak TIM1_UP_TIM10_IRQHandler      = Default_Handler
+#pragma weak TIM1_TRG_COM_TIM11_IRQHandler = Default_Handler
+#pragma weak TIM1_CC_IRQHandler            = Default_Handler
+#pragma weak TIM2_IRQHandler               = Default_Handler
+#pragma weak TIM3_IRQHandler               = Default_Handler
+#pragma weak TIM4_IRQHandler               = Default_Handler
+#pragma weak I2C1_EV_IRQHandler            = Default_Handler
+#pragma weak I2C1_ER_IRQHandler            = Default_Handler
+#pragma weak I2C2_EV_IRQHandler            = Default_Handler
+#pragma weak I2C2_ER_IRQHandler            = Default_Handler
+#pragma weak SPI1_IRQHandler               = Default_Handler
+#pragma weak SPI2_IRQHandler               = Default_Handler
+// #pragma weak USART1_IRQHandler             = Default_Handler
+// #pragma weak USART2_IRQHandler             = Default_Handler
+// #pragma weak USART3_IRQHandler             = Default_Handler
+#pragma weak EXTI15_10_IRQHandler          = Default_Handler
+#pragma weak RTC_Alarm_IRQHandler          = Default_Handler
+#pragma weak OTG_FS_WKUP_IRQHandler        = Default_Handler
+#pragma weak TIM8_BRK_TIM12_IRQHandler     = Default_Handler
+#pragma weak TIM8_UP_TIM13_IRQHandler      = Default_Handler
+#pragma weak TIM8_TRG_COM_TIM14_IRQHandler = Default_Handler
+#pragma weak TIM8_CC_IRQHandler            = Default_Handler
+#pragma weak DMA1_Stream7_IRQHandler       = Default_Handler
+#pragma weak FMC_IRQHandler                = Default_Handler
+#pragma weak SDIO_IRQHandler               = Default_Handler
+#pragma weak TIM5_IRQHandler               = Default_Handler
+#pragma weak SPI3_IRQHandler               = Default_Handler
+// #pragma weak UART4_IRQHandler              = Default_Handler
+// #pragma weak UART5_IRQHandler              = Default_Handler
+#pragma weak TIM6_DAC_IRQHandler     = Default_Handler
+#pragma weak TIM7_IRQHandler         = Default_Handler
+#pragma weak DMA2_Stream0_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream1_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream2_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream3_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream4_IRQHandler = Default_Handler
+#pragma weak ETH_IRQHandler          = Default_Handler
+#pragma weak ETH_WKUP_IRQHandler     = Default_Handler
+#pragma weak CAN2_TX_IRQHandler      = Default_Handler
+#pragma weak CAN2_RX0_IRQHandler     = Default_Handler
+#pragma weak CAN2_RX1_IRQHandler     = Default_Handler
+#pragma weak CAN2_SCE_IRQHandler     = Default_Handler
+#pragma weak OTG_FS_IRQHandler       = Default_Handler
+#pragma weak DMA2_Stream5_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream6_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream7_IRQHandler = Default_Handler
+// #pragma weak USART6_IRQHandler             = Default_Handler
+#pragma weak I2C3_EV_IRQHandler        = Default_Handler
+#pragma weak I2C3_ER_IRQHandler        = Default_Handler
+#pragma weak OTG_HS_EP1_OUT_IRQHandler = Default_Handler
+#pragma weak OTG_HS_EP1_IN_IRQHandler  = Default_Handler
+#pragma weak OTG_HS_WKUP_IRQHandler    = Default_Handler
+#pragma weak OTG_HS_IRQHandler         = Default_Handler
+#pragma weak DCMI_IRQHandler           = Default_Handler
+#pragma weak CRYP_IRQHandler           = Default_Handler
+#pragma weak HASH_RNG_IRQHandler       = Default_Handler
+#pragma weak FPU_IRQHandler            = Default_Handler
+// #pragma weak UART7_IRQHandler              = Default_Handler
+// #pragma weak UART8_IRQHandler              = Default_Handler
+#pragma weak SPI4_IRQHandler    = Default_Handler
+#pragma weak SPI5_IRQHandler    = Default_Handler
+#pragma weak SPI6_IRQHandler    = Default_Handler
+#pragma weak SAI1_IRQHandler    = Default_Handler
+#pragma weak LTDC_IRQHandler    = Default_Handler
+#pragma weak LTDC_ER_IRQHandler = Default_Handler
+#pragma weak DMA2D_IRQHandler   = Default_Handler
diff --git a/src/bsps/stm32f429zi_death_stack_v3/interfaces-impl/arch_registers_impl.h b/src/bsps/stm32f429zi_death_stack_v3/interfaces-impl/arch_registers_impl.h
new file mode 100644
index 000000000..5a2bfe8f0
--- /dev/null
+++ b/src/bsps/stm32f429zi_death_stack_v3/interfaces-impl/arch_registers_impl.h
@@ -0,0 +1,34 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Authors: Emilio Corigliano, Alberto Nidasio
+ *
+ * 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.
+ */
+
+#ifndef ARCH_REGISTERS_IMPL_H
+#define ARCH_REGISTERS_IMPL_H
+
+// Always include stm32f4xx.h before core_cm4.h, there's some nasty dependency
+#define STM32F429xx
+#include "CMSIS/Device/ST/STM32F4xx/Include/stm32f4xx.h"
+#include "CMSIS/Device/ST/STM32F4xx/Include/system_stm32f4xx.h"
+#include "CMSIS/Include/core_cm4.h"
+
+#define RCC_SYNC() __DSB()  // Workaround for a bug in stm32f42x
+
+#endif  // ARCH_REGISTERS_IMPL_H
diff --git a/src/bsps/stm32f429zi_death_stack_v3/interfaces-impl/bsp.cpp b/src/bsps/stm32f429zi_death_stack_v3/interfaces-impl/bsp.cpp
new file mode 100644
index 000000000..94f8eee9b
--- /dev/null
+++ b/src/bsps/stm32f429zi_death_stack_v3/interfaces-impl/bsp.cpp
@@ -0,0 +1,505 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Authors: Emilio Corigliano, Alberto Nidasio
+ *
+ * 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.
+ */
+
+/***********************************************************************
+ * bsp.cpp Part of the Miosix Embedded OS.
+ * Board support package, this file initializes hardware.
+ ************************************************************************/
+
+#include "interfaces/bsp.h"
+
+#include <inttypes.h>
+#include <sys/ioctl.h>
+
+#include <cstdlib>
+
+#include "board_settings.h"
+#include "config/miosix_settings.h"
+#include "drivers/sd_stm32f2_f4_f7.h"
+#include "drivers/serial.h"
+#include "filesystem/console/console_device.h"
+#include "filesystem/file_access.h"
+#include "hwmapping.h"
+#include "interfaces/arch_registers.h"
+#include "interfaces/delays.h"
+#include "interfaces/portability.h"
+#include "kernel/kernel.h"
+#include "kernel/logging.h"
+#include "kernel/sync.h"
+
+namespace miosix
+{
+
+//
+// Initialization
+//
+
+/**
+ * The example code from ST checks for the busy flag after each command.
+ * Interestingly I couldn't find any mention of this in the datsheet.
+ */
+static void sdramCommandWait()
+{
+    for (int i = 0; i < 0xffff; i++)
+        if ((FMC_Bank5_6->SDSR & FMC_SDSR_BUSY) == 0)
+            return;
+}
+
+#ifdef SDRAM_ISSI
+
+void configureSdram()
+{
+    // Enable all gpios, passing clock
+    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN | RCC_AHB1ENR_GPIOBEN |
+                    RCC_AHB1ENR_GPIOCEN | RCC_AHB1ENR_GPIODEN |
+                    RCC_AHB1ENR_GPIOEEN | RCC_AHB1ENR_GPIOFEN |
+                    RCC_AHB1ENR_GPIOGEN | RCC_AHB1ENR_GPIOHEN;
+    RCC_SYNC();
+
+    // First, configure SDRAM GPIOs
+    GPIOB->AFR[0] = 0x0cc00000;
+    GPIOC->AFR[0] = 0x0000000c;
+    GPIOD->AFR[0] = 0x000000cc;
+    GPIOD->AFR[1] = 0xcc000ccc;
+    GPIOE->AFR[0] = 0xc00000cc;
+    GPIOE->AFR[1] = 0xcccccccc;
+    GPIOF->AFR[0] = 0x00cccccc;
+    GPIOF->AFR[1] = 0xccccc000;
+    GPIOG->AFR[0] = 0x00cc00cc;
+    GPIOG->AFR[1] = 0xc000000c;
+
+    GPIOB->MODER = 0x00002800;
+    GPIOC->MODER = 0x00000002;
+    GPIOD->MODER = 0xa02a000a;
+    GPIOE->MODER = 0xaaaa800a;
+    GPIOF->MODER = 0xaa800aaa;
+    GPIOG->MODER = 0x80020a0a;
+
+    GPIOA->OSPEEDR = 0xaaaaaaaa;  // Default to 50MHz speed for all GPIOs...
+    GPIOB->OSPEEDR =
+        0xaaaaaaaa | 0x00003c00;  //...but 100MHz for the SDRAM pins
+    GPIOC->OSPEEDR = 0xaaaaaaaa | 0x00000003;
+    GPIOD->OSPEEDR = 0xaaaaaaaa | 0xf03f000f;
+    GPIOE->OSPEEDR = 0xaaaaaaaa | 0xffffc00f;
+    GPIOF->OSPEEDR = 0xaaaaaaaa | 0xffc00fff;
+    GPIOG->OSPEEDR = 0xaaaaaaaa | 0xc0030f0f;
+    GPIOH->OSPEEDR = 0xaaaaaaaa;
+
+    // Since we'we un-configured PB3/PB4 from the default at boot TDO,NTRST,
+    // finish the job and remove the default pull-up
+    GPIOB->PUPDR = 0;
+
+    // Second, actually start the SDRAM controller
+    RCC->AHB3ENR |= RCC_AHB3ENR_FMCEN;
+    RCC_SYNC();
+
+    // SDRAM is a IS42S16400J -7 speed grade, connected to bank 2 (0xd0000000)
+    // Some bits in SDCR[1] are don't care, and the have to be set in SDCR[0],
+    // they aren't just don't care, the controller will fail if they aren't at 0
+    FMC_Bank5_6->SDCR[0] = FMC_SDCR1_SDCLK_1  // SDRAM runs @ half CPU frequency
+                           | FMC_SDCR1_RBURST  // Enable read burst
+                           | 0;              //  0 delay between reads after CAS
+    FMC_Bank5_6->SDCR[1] = 0                 //  8 bit column address
+                           | FMC_SDCR1_NR_0  // 12 bit row address
+                           | FMC_SDCR1_MWID_0  // 16 bit data bus
+                           | FMC_SDCR1_NB      //  4 banks
+                           |
+                           FMC_SDCR1_CAS_1;  //  2 cycle CAS latency (F<133MHz)
+
+#ifdef SYSCLK_FREQ_180MHz
+    // One SDRAM clock cycle is 11.1ns
+    // Some bits in SDTR[1] are don't care, and the have to be set in SDTR[0],
+    // they aren't just don't care, the controller will fail if they aren't at 0
+    FMC_Bank5_6->SDTR[0] = (6 - 1) << 12     // 6 cycle TRC  (66.6ns>63ns)
+                           | (2 - 1) << 20;  // 2 cycle TRP  (22.2ns>15ns)
+    FMC_Bank5_6->SDTR[1] = (2 - 1) << 0      // 2 cycle TMRD
+                           | (7 - 1) << 4    // 7 cycle TXSR (77.7ns>70ns)
+                           | (4 - 1) << 8    // 4 cycle TRAS (44.4ns>42ns)
+                           | (2 - 1) << 16   // 2 cycle TWR
+                           | (2 - 1) << 24;  // 2 cycle TRCD (22.2ns>15ns)
+#elif defined(SYSCLK_FREQ_168MHz)
+    // One SDRAM clock cycle is 11.9ns
+    // Some bits in SDTR[1] are don't care, and the have to be set in SDTR[0],
+    // they aren't just don't care, the controller will fail if they aren't at 0
+    FMC_Bank5_6->SDTR[0] = (6 - 1) << 12     // 6 cycle TRC  (71.4ns>63ns)
+                           | (2 - 1) << 20;  // 2 cycle TRP  (23.8ns>15ns)
+    FMC_Bank5_6->SDTR[1] = (2 - 1) << 0      // 2 cycle TMRD
+                           | (6 - 1) << 4    // 6 cycle TXSR (71.4ns>70ns)
+                           | (4 - 1) << 8    // 4 cycle TRAS (47.6ns>42ns)
+                           | (2 - 1) << 16   // 2 cycle TWR
+                           | (2 - 1) << 24;  // 2 cycle TRCD (23.8ns>15ns)
+#else
+#error No SDRAM timings for this clock
+#endif
+
+    FMC_Bank5_6->SDCMR = FMC_SDCMR_CTB2  // Enable bank 2
+                         | 1;            // MODE=001 clock enabled
+    sdramCommandWait();
+
+    // ST and SDRAM datasheet agree a 100us delay is required here.
+    delayUs(100);
+
+    FMC_Bank5_6->SDCMR = FMC_SDCMR_CTB2  // Enable bank 2
+                         | 2;            // MODE=010 precharge all command
+    sdramCommandWait();
+
+    FMC_Bank5_6->SDCMR = (8 - 1) << 5      // NRFS=8 SDRAM datasheet says
+                                           // "at least two AUTO REFRESH cycles"
+                         | FMC_SDCMR_CTB2  // Enable bank 2
+                         | 3;              // MODE=011 auto refresh
+    sdramCommandWait();
+
+    FMC_Bank5_6->SDCMR = 0x220 << 9  // MRD=0x220:CAS latency=2 burst len=1
+                         | FMC_SDCMR_CTB2  // Enable bank 2
+                         | 4;              // MODE=100 load mode register
+    sdramCommandWait();
+
+// 64ms/4096=15.625us
+#ifdef SYSCLK_FREQ_180MHz
+    // 15.625us*90MHz=1406-20=1386
+    FMC_Bank5_6->SDRTR = 1386 << 1;
+#elif defined(SYSCLK_FREQ_168MHz)
+    // 15.625us*84MHz=1312-20=1292
+    FMC_Bank5_6->SDRTR = 1292 << 1;
+#else
+#error No refresh timings for this clock
+#endif
+}
+
+#else
+
+void configureSdram()
+{
+    // Enable all gpios
+    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN | RCC_AHB1ENR_GPIOBEN |
+                    RCC_AHB1ENR_GPIOCEN | RCC_AHB1ENR_GPIODEN |
+                    RCC_AHB1ENR_GPIOEEN | RCC_AHB1ENR_GPIOFEN |
+                    RCC_AHB1ENR_GPIOGEN | RCC_AHB1ENR_GPIOHEN;
+    RCC_SYNC();
+
+    // First, configure SDRAM GPIOs
+    GPIOB->AFR[0] = 0x0cc00000;
+    GPIOC->AFR[0] = 0x0000000c;
+    GPIOD->AFR[0] = 0x000000cc;
+    GPIOD->AFR[1] = 0xcc000ccc;
+    GPIOE->AFR[0] = 0xc00000cc;
+    GPIOE->AFR[1] = 0xcccccccc;
+    GPIOF->AFR[0] = 0x00cccccc;
+    GPIOF->AFR[1] = 0xccccc000;
+    GPIOG->AFR[0] = 0x00cc00cc;
+    GPIOG->AFR[1] = 0xc000000c;
+
+    GPIOB->MODER = 0x00002800;
+    GPIOC->MODER = 0x00000002;
+    GPIOD->MODER = 0xa02a000a;
+    GPIOE->MODER = 0xaaaa800a;
+    GPIOF->MODER = 0xaa800aaa;
+    GPIOG->MODER = 0x80020a0a;
+
+    GPIOA->OSPEEDR = 0xaaaaaaaa;  // Default to 50MHz speed for all GPIOs...
+    GPIOB->OSPEEDR =
+        0xaaaaaaaa | 0x00003c00;  //...but 100MHz for the SDRAM pins
+    GPIOC->OSPEEDR = 0xaaaaaaaa | 0x00000003;
+    GPIOD->OSPEEDR = 0xaaaaaaaa | 0xf03f000f;
+    GPIOE->OSPEEDR = 0xaaaaaaaa | 0xffffc00f;
+    GPIOF->OSPEEDR = 0xaaaaaaaa | 0xffc00fff;
+    GPIOG->OSPEEDR = 0xaaaaaaaa | 0xc0030f0f;
+    GPIOH->OSPEEDR = 0xaaaaaaaa;
+
+    // Since we'we un-configured PB3/PB4 from the default at boot TDO,NTRST,
+    // finish the job and remove the default pull-up
+    GPIOB->PUPDR = 0;
+
+    // Second, actually start the SDRAM controller
+    RCC->AHB3ENR |= RCC_AHB3ENR_FMCEN;
+    RCC_SYNC();
+
+    // SDRAM is a AS4C4M16SA-6TAN, connected to bank 2 (0xd0000000)
+    // Some bits in SDCR[1] are don't care, and the have to be set in SDCR[0],
+    // they aren't just don't care, the controller will fail if they aren't at 0
+    FMC_Bank5_6->SDCR[0] = FMC_SDCR1_SDCLK_1  // SDRAM runs @ half CPU frequency
+                           | FMC_SDCR1_RBURST  // Enable read burst
+                           | 0;  //  0 delay between reads after CAS
+    FMC_Bank5_6->SDCR[1] =
+        0                   //  8 bit column address
+        | FMC_SDCR1_NR_0    // 12 bit row address
+        | FMC_SDCR1_MWID_0  // 16 bit data bus
+        | FMC_SDCR1_NB      //  4 banks
+        | FMC_SDCR1_CAS_1;  //  2 cycle CAS latency (TCK>9+0.5ns [1])
+
+#ifdef SYSCLK_FREQ_180MHz
+    // One SDRAM clock cycle is 11.1ns
+    // Some bits in SDTR[1] are don't care, and the have to be set in SDTR[0],
+    // they aren't just don't care, the controller will fail if they aren't at 0
+    FMC_Bank5_6->SDTR[0] = (6 - 1) << 12     // 6 cycle TRC  (66.6ns>60ns)
+                           | (2 - 1) << 20;  // 2 cycle TRP  (22.2ns>18ns)
+    FMC_Bank5_6->SDTR[1] = (2 - 1) << 0      // 2 cycle TMRD
+                           |
+                           (6 - 1) << 4  // 6 cycle TXSR (66.6ns>61.5+0.5ns [1])
+                           | (4 - 1) << 8    // 4 cycle TRAS (44.4ns>42ns)
+                           | (2 - 1) << 16   // 2 cycle TWR
+                           | (2 - 1) << 24;  // 2 cycle TRCD (22.2ns>18ns)
+#elif defined(SYSCLK_FREQ_168MHz)
+    // One SDRAM clock cycle is 11.9ns
+    // Some bits in SDTR[1] are don't care, and the have to be set in SDTR[0],
+    // they aren't just don't care, the controller will fail if they aren't at 0
+    FMC_Bank5_6->SDTR[0] = (6 - 1) << 12     // 6 cycle TRC  (71.4ns>60ns)
+                           | (2 - 1) << 20;  // 2 cycle TRP  (23.8ns>18ns)
+    FMC_Bank5_6->SDTR[1] = (2 - 1) << 0      // 2 cycle TMRD
+                           |
+                           (6 - 1) << 4  // 6 cycle TXSR (71.4ns>61.5+0.5ns [1])
+                           | (4 - 1) << 8    // 4 cycle TRAS (47.6ns>42ns)
+                           | (2 - 1) << 16   // 2 cycle TWR
+                           | (2 - 1) << 24;  // 2 cycle TRCD (23.8ns>18ns)
+#else
+#error No SDRAM timings for this clock
+#endif
+    // NOTE [1]: the timings for TCK and TIS depend on rise and fall times
+    //(see note 9 and 10 on datasheet). Timings are adjusted accordingly to
+    // the measured 2ns rise and fall time
+
+    FMC_Bank5_6->SDCMR = FMC_SDCMR_CTB2  // Enable bank 2
+                         | 1;            // MODE=001 clock enabled
+    sdramCommandWait();
+
+    // SDRAM datasheet requires 200us delay here (note 11), here we use 10% more
+    delayUs(220);
+
+    FMC_Bank5_6->SDCMR = FMC_SDCMR_CTB2  // Enable bank 2
+                         | 2;            // MODE=010 precharge all command
+    sdramCommandWait();
+
+    // FIXME: note 11 on SDRAM datasheet says extended mode register must be
+    // set, but the ST datasheet does not seem to explain how
+    FMC_Bank5_6->SDCMR = 0x220 << 9  // MRD=0x220:CAS latency=2 burst len=1
+                         | FMC_SDCMR_CTB2  // Enable bank 2
+                         | 4;              // MODE=100 load mode register
+    sdramCommandWait();
+
+    FMC_Bank5_6->SDCMR = (4 - 1) << 5  // NRFS=8 SDRAM datasheet requires
+                                       // a minimum of 2 cycles, here we use 4
+                         | FMC_SDCMR_CTB2  // Enable bank 2
+                         | 3;              // MODE=011 auto refresh
+    sdramCommandWait();
+
+// 32ms/4096=7.8125us, but datasheet says to round that to 7.8us
+#ifdef SYSCLK_FREQ_180MHz
+    // 7.8us*90MHz=702-20=682
+    FMC_Bank5_6->SDRTR = 682 << 1;
+#elif defined(SYSCLK_FREQ_168MHz)
+    // 7.8us*84MHz=655-20=635
+    FMC_Bank5_6->SDRTR = 635 << 1;
+#else
+#error No refresh timings for this clock
+#endif
+}
+
+#endif
+
+void IRQbspInit()
+{
+// If using SDRAM GPIOs are enabled by configureSdram(), else enable them here
+#ifndef __ENABLE_XRAM
+    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN | RCC_AHB1ENR_GPIOBEN |
+                    RCC_AHB1ENR_GPIOCEN | RCC_AHB1ENR_GPIODEN |
+                    RCC_AHB1ENR_GPIOEEN | RCC_AHB1ENR_GPIOFEN |
+                    RCC_AHB1ENR_GPIOGEN | RCC_AHB1ENR_GPIOHEN;
+    RCC_SYNC();
+#endif  //__ENABLE_XRAM
+
+    // enable spi1, spi2
+    RCC->APB2ENR |= RCC_APB2ENR_SPI1EN;
+    RCC->APB1ENR |= RCC_APB1ENR_SPI2EN;
+
+    // enable can1
+    RCC->APB1ENR |= RCC_APB1ENR_CAN1EN;
+
+    // Set the clock divider for the analog circuitry (/8)
+    ADC->CCR |= ADC_CCR_ADCPRE_0 | ADC_CCR_ADCPRE_1;
+
+    RCC_SYNC();
+
+    interfaces::spi1::sck::mode(Mode::ALTERNATE);
+    interfaces::spi1::sck::alternateFunction(5);
+    interfaces::spi1::miso::mode(Mode::ALTERNATE);
+    interfaces::spi1::miso::alternateFunction(5);
+    interfaces::spi1::mosi::mode(Mode::ALTERNATE);
+    interfaces::spi1::mosi::alternateFunction(5);
+
+    interfaces::spi2::sck::mode(Mode::ALTERNATE);
+    interfaces::spi2::sck::alternateFunction(5);
+    interfaces::spi2::miso::mode(Mode::ALTERNATE);
+    interfaces::spi2::miso::alternateFunction(5);
+    interfaces::spi2::mosi::mode(Mode::ALTERNATE);
+    interfaces::spi2::mosi::alternateFunction(5);
+
+    interfaces::spi4::sck::mode(Mode::ALTERNATE);
+    interfaces::spi4::sck::alternateFunction(5);
+    interfaces::spi4::miso::mode(Mode::ALTERNATE);
+    interfaces::spi4::miso::alternateFunction(5);
+    interfaces::spi4::mosi::mode(Mode::ALTERNATE);
+    interfaces::spi4::mosi::alternateFunction(5);
+
+    interfaces::spi5::sck::mode(Mode::ALTERNATE);
+    interfaces::spi5::sck::alternateFunction(5);
+    interfaces::spi5::miso::mode(Mode::ALTERNATE);
+    interfaces::spi5::miso::alternateFunction(5);
+    interfaces::spi5::mosi::mode(Mode::ALTERNATE);
+    interfaces::spi5::mosi::alternateFunction(5);
+
+    interfaces::spi6::sck::mode(Mode::ALTERNATE);
+    interfaces::spi6::sck::alternateFunction(5);
+    interfaces::spi6::miso::mode(Mode::ALTERNATE);
+    interfaces::spi6::miso::alternateFunction(5);
+    interfaces::spi6::mosi::mode(Mode::ALTERNATE);
+    interfaces::spi6::mosi::alternateFunction(5);
+
+    interfaces::usart1::rx::mode(Mode::ALTERNATE);
+    interfaces::usart1::rx::alternateFunction(7);
+    interfaces::usart1::tx::mode(Mode::ALTERNATE);
+    interfaces::usart1::tx::alternateFunction(7);
+
+    interfaces::usart2::rx::mode(Mode::ALTERNATE);
+    interfaces::usart2::rx::alternateFunction(7);
+    interfaces::usart2::tx::mode(Mode::ALTERNATE);
+    interfaces::usart2::tx::alternateFunction(7);
+
+    interfaces::usart3::rx::mode(Mode::ALTERNATE);
+    interfaces::usart3::rx::alternateFunction(7);
+    interfaces::usart3::tx::mode(Mode::ALTERNATE);
+    interfaces::usart3::tx::alternateFunction(7);
+
+    interfaces::uart4::rx::mode(Mode::ALTERNATE);
+    interfaces::uart4::rx::alternateFunction(8);
+    interfaces::uart4::tx::mode(Mode::ALTERNATE);
+    interfaces::uart4::tx::alternateFunction(8);
+
+    interfaces::can1::rx::mode(Mode::ALTERNATE);
+    interfaces::can1::rx::alternateFunction(9);
+    interfaces::can1::tx::mode(Mode::ALTERNATE);
+    interfaces::can1::tx::alternateFunction(9);
+
+    sensors::ads131m04::cs1::mode(Mode::OUTPUT);
+    sensors::ads131m04::cs1::high();
+
+    sensors::bmx160::cs::mode(Mode::OUTPUT);
+    sensors::bmx160::cs::high();
+    sensors::bmx160::intr::mode(Mode::INPUT_PULL_UP);
+
+    sensors::mpu9250::cs::mode(Mode::OUTPUT);
+    sensors::mpu9250::cs::high();
+
+    sensors::cc3135::cs::mode(Mode::OUTPUT);
+    sensors::cc3135::cs::high();
+    sensors::cc3135::intr::mode(Mode::INPUT_PULL_UP);
+
+    sensors::sx127x::cs::mode(Mode::OUTPUT);
+    sensors::sx127x::cs::high();
+    sensors::sx127x::dio0::mode(Mode::INPUT);
+
+    sensors::gps::cs::mode(Mode::OUTPUT);
+    sensors::gps::cs::high();
+
+    sensors::ms5803::cs::mode(Mode::OUTPUT);
+    sensors::ms5803::cs::high();
+
+    sensors::mlx91221_1::vout::mode(Mode::INPUT_ANALOG);
+    sensors::mlx91221_1::vout::high();
+
+    sensors::mlx91221_2::vout::mode(Mode::INPUT_ANALOG);
+    sensors::mlx91221_2::vout::high();
+
+    sensors::launchpad_detach::mode(Mode::INPUT);
+
+    expulsion::servo::mode(Mode::ALTERNATE);
+    expulsion::servo::alternateFunction(2);
+    expulsion::sense::mode(Mode::INPUT_PULL_UP);
+    expulsion::nosecone_detach::mode(Mode::INPUT);
+
+    cutter::enable::mode(Mode::OUTPUT);
+    cutter::enable::low();
+    cutter::enable_backup::mode(Mode::OUTPUT);
+    cutter::enable_backup::low();
+    cutter::sense::mode(Mode::INPUT_ANALOG);
+
+    airbrakes::servo::mode(Mode::ALTERNATE);
+    airbrakes::servo::alternateFunction(3);
+    airbrakes::sensor::mode(Mode::INPUT_ANALOG);
+
+    leds::red::mode(Mode::OUTPUT);
+    leds::green::mode(Mode::OUTPUT);
+    leds::blue::mode(Mode::OUTPUT);
+
+    buzzer::drive::mode(Mode::ALTERNATE);
+    buzzer::drive::alternateFunction(3);
+
+    aux::servo::mode(Mode::ALTERNATE);
+    aux::servo::alternateFunction(3);
+    aux::sense_1::mode(Mode::INPUT);
+    aux::sense_2::mode(Mode::INPUT);
+
+    DefaultConsole::instance().IRQset(intrusive_ref_ptr<Device>(
+        new STM32Serial(defaultSerial, defaultSerialSpeed,
+                        defaultSerialFlowctrl ? STM32Serial::RTSCTS
+                                              : STM32Serial::NOFLOWCTRL)));
+
+    for (uint8_t i = 0; i < 3; i++)
+    {
+        ledOn();
+        delayMs(10);
+        ledOff();
+        delayMs(10);
+    }
+}
+
+void bspInit2()
+{
+#ifdef WITH_FILESYSTEM
+    // PA2,PA3
+    intrusive_ref_ptr<DevFs> devFs =
+        basicFilesystemSetup(SDIODriver::instance());
+#endif  // WITH_FILESYSTEM
+}
+
+//
+// Shutdown and reboot
+//
+
+/**
+ * For safety reasons, we never want the board to shutdown.
+ * When requested to shutdown, we reboot instead.
+ */
+void shutdown() { reboot(); }
+
+void reboot()
+{
+    ioctl(STDOUT_FILENO, IOCTL_SYNC, 0);
+
+#ifdef WITH_FILESYSTEM
+    FilesystemManager::instance().umountAll();
+#endif  // WITH_FILESYSTEM
+
+    disableInterrupts();
+    miosix_private::IRQsystemReboot();
+}
+
+}  // namespace miosix
diff --git a/src/bsps/stm32f429zi_death_stack_v3/interfaces-impl/bsp_impl.h b/src/bsps/stm32f429zi_death_stack_v3/interfaces-impl/bsp_impl.h
new file mode 100644
index 000000000..687864e16
--- /dev/null
+++ b/src/bsps/stm32f429zi_death_stack_v3/interfaces-impl/bsp_impl.h
@@ -0,0 +1,76 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Authors: Emilio Corigliano, Alberto Nidasio
+ *
+ * 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.
+ */
+
+/***********************************************************************
+ * bsp_impl.h Part of the Miosix Embedded OS.
+ * Board support package, this file initializes hardware.
+ ************************************************************************/
+
+#ifndef BSP_IMPL_H
+#define BSP_IMPL_H
+
+#include "config/miosix_settings.h"
+#include "drivers/stm32_hardware_rng.h"
+#include "hwmapping.h"
+#include "interfaces/gpio.h"
+
+namespace miosix
+{
+
+/**
+\addtogroup Hardware
+\{
+*/
+
+/**
+ * \internal
+ * Called by stage_1_boot.cpp to enable the SDRAM before initializing .data/.bss
+ * Requires the CPU clock to be already configured (running from the PLL)
+ */
+void configureSdram();
+
+/**
+ * \internal
+ * used by the ledOn() and ledOff() implementation
+ */
+
+inline void ledOn()
+{
+    leds::red::high();
+    leds::green::high();
+    leds::blue::high();
+}
+
+inline void ledOff()
+{
+    leds::red::low();
+    leds::green::low();
+    leds::blue::low();
+}
+
+/**
+\}
+*/
+
+}  // namespace miosix
+
+#endif  // BSP_IMPL_H
diff --git a/src/bsps/stm32f429zi_death_stack_v3/interfaces-impl/hwmapping.h b/src/bsps/stm32f429zi_death_stack_v3/interfaces-impl/hwmapping.h
new file mode 100644
index 000000000..1357f586d
--- /dev/null
+++ b/src/bsps/stm32f429zi_death_stack_v3/interfaces-impl/hwmapping.h
@@ -0,0 +1,209 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Authors: Emilio Corigliano, Alberto Nidasio
+ *
+ * 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.
+ */
+
+#ifndef HWMAPPING_H
+#define HWMAPPING_H
+
+#include "interfaces/gpio.h"
+
+// Remember to modify pins of leds
+namespace miosix
+{
+
+namespace interfaces
+{
+
+// ADS131M04
+namespace spi1
+{
+using sck  = Gpio<GPIOA_BASE, 5>;
+using miso = Gpio<GPIOA_BASE, 6>;
+using mosi = Gpio<GPIOA_BASE, 7>;
+}  // namespace spi1
+
+// MS5803 - GPS
+namespace spi2
+{
+using sck  = Gpio<GPIOB_BASE, 13>;
+using miso = Gpio<GPIOB_BASE, 14>;
+using mosi = Gpio<GPIOB_BASE, 15>;
+}  // namespace spi2
+
+// BMX160 - MPU9250
+namespace spi4
+{
+using sck  = Gpio<GPIOE_BASE, 2>;
+using miso = Gpio<GPIOE_BASE, 5>;
+using mosi = Gpio<GPIOE_BASE, 6>;
+}  // namespace spi4
+
+// SX127x
+namespace spi5
+{
+using sck  = Gpio<GPIOF_BASE, 7>;
+using miso = Gpio<GPIOF_BASE, 8>;
+using mosi = Gpio<GPIOF_BASE, 9>;
+}  // namespace spi5
+
+// CC3135
+namespace spi6
+{
+using sck  = Gpio<GPIOG_BASE, 13>;
+using miso = Gpio<GPIOG_BASE, 12>;
+using mosi = Gpio<GPIOG_BASE, 14>;
+}  // namespace spi6
+
+// Debug
+namespace usart1
+{
+using tx = Gpio<GPIOA_BASE, 9>;
+using rx = Gpio<GPIOA_BASE, 10>;
+}  // namespace usart1
+
+// VN100
+namespace usart2
+{
+using tx = Gpio<GPIOA_BASE, 2>;
+using rx = Gpio<GPIOA_BASE, 3>;
+}  // namespace usart2
+
+// HIL
+namespace usart3
+{
+using tx = Gpio<GPIOB_BASE, 10>;
+using rx = Gpio<GPIOB_BASE, 11>;
+}  // namespace usart3
+
+// RunCam
+namespace uart4
+{
+using tx = Gpio<GPIOA_BASE, 0>;
+using rx = Gpio<GPIOA_BASE, 1>;
+}  // namespace uart4
+
+namespace can1
+{
+using rx = Gpio<GPIOA_BASE, 11>;
+using tx = Gpio<GPIOA_BASE, 12>;
+}  // namespace can1
+
+}  // namespace interfaces
+
+namespace sensors
+{
+
+namespace ads131m04
+{
+using cs1  = Gpio<GPIOA_BASE, 4>;
+using cs2  = Gpio<GPIOA_BASE, 8>;  // TIM1_CH1
+using sck2 = Gpio<GPIOB_BASE, 1>;  // TIM3_CH4
+}  // namespace ads131m04
+
+namespace bmx160
+{
+using cs   = Gpio<GPIOE_BASE, 4>;
+using intr = Gpio<GPIOE_BASE, 3>;
+}  // namespace bmx160
+
+namespace mpu9250
+{
+using cs = Gpio<GPIOD_BASE, 7>;
+}  // namespace mpu9250
+
+namespace cc3135
+{
+using cs   = Gpio<GPIOG_BASE, 11>;
+using intr = Gpio<GPIOG_BASE, 10>;
+}  // namespace cc3135
+
+namespace sx127x
+{
+using cs   = Gpio<GPIOF_BASE, 6>;
+using dio0 = Gpio<GPIOF_BASE, 10>;
+}  // namespace sx127x
+
+namespace gps
+{
+using cs = Gpio<GPIOB_BASE, 12>;
+}  // namespace gps
+
+namespace ms5803
+{
+using cs = Gpio<GPIOD_BASE, 11>;
+}  // namespace ms5803
+
+namespace mlx91221_1
+{
+using vout = Gpio<GPIOC_BASE, 1>;  // ADC
+}  // namespace mlx91221_1
+
+namespace mlx91221_2
+{
+using vout = Gpio<GPIOC_BASE, 2>;  // ADC
+}  // namespace mlx91221_2
+
+using launchpad_detach = Gpio<GPIOD_BASE, 5>;  // launchpad detach
+
+}  // namespace sensors
+
+namespace expulsion
+{
+using servo           = Gpio<GPIOB_BASE, 7>;  // Pwm expulsion servo, TIM4_CH2
+using sense           = Gpio<GPIOD_BASE, 3>;  // Expulsion sensor
+using nosecone_detach = Gpio<GPIOD_BASE, 4>;  // Nosecone detach
+}  // namespace expulsion
+
+namespace cutter
+{
+using enable        = Gpio<GPIOG_BASE, 2>;
+using enable_backup = Gpio<GPIOG_BASE, 3>;
+using sense         = Gpio<GPIOC_BASE, 5>;
+}  // namespace cutter
+
+namespace airbrakes
+{
+using servo  = Gpio<GPIOB_BASE, 8>;  // Airbrakes PWM, TIM10_CH1
+using sensor = Gpio<GPIOC_BASE, 3>;  // Airbrakes ADC
+}  // namespace airbrakes
+
+namespace leds
+{
+using red   = Gpio<GPIOC_BASE, 4>;
+using green = Gpio<GPIOD_BASE, 12>;
+using blue  = Gpio<GPIOD_BASE, 13>;
+}  // namespace leds
+
+namespace buzzer
+{
+using drive = Gpio<GPIOC_BASE, 6>;  // PWM TIM8_CH1
+}  // namespace buzzer
+
+namespace aux
+{
+using servo   = Gpio<GPIOB_BASE, 9>;  // TIM11_CH1
+using sense_1 = Gpio<GPIOG_BASE, 6>;
+using sense_2 = Gpio<GPIOG_BASE, 7>;
+}  // namespace aux
+
+}  // namespace miosix
+
+#endif  // HWMAPPING_H
diff --git a/src/bsps/stm32f429zi_death_stack_v3/stm32_2m+256k_rom.ld b/src/bsps/stm32f429zi_death_stack_v3/stm32_2m+256k_rom.ld
new file mode 100644
index 000000000..cfcc2ce53
--- /dev/null
+++ b/src/bsps/stm32f429zi_death_stack_v3/stm32_2m+256k_rom.ld
@@ -0,0 +1,181 @@
+/*
+ * C++ enabled linker script for stm32 (2M FLASH, 256K RAM)
+ * Developed by TFT: Terraneo Federico Technologies
+ * Optimized for use with the Miosix kernel
+ */
+
+/*
+ * This chip has an unusual quirk that the RAM is divided in two block mapped
+ * at two non contiguous memory addresses. I don't know why they've done that,
+ * probably doing the obvious thing would have made writing code too easy...
+ * Anyway, since hardware can't be changed, we've got to live with that and
+ * try to make use of both RAMs.
+ * 
+ * Given the constraints above, this linker script puts:
+ * - read only data and code (.text, .rodata, .eh_*) in FLASH
+ * - the 512Byte main (IRQ) stack, .data and .bss in the "small" 64KB RAM
+ * - stacks and heap in the "large" 192KB RAM.
+ * 
+ * Unfortunately thread stacks can't be put in the small RAM as Miosix
+ * allocates them inside the heap.
+ */
+
+/*
+ * The main stack is used for interrupt handling by the kernel.
+ *
+ * *** Readme ***
+ * This linker script places the main stack (used by the kernel for interrupts)
+ * at the bottom of the ram, instead of the top. This is done for two reasons:
+ *
+ * - as an optimization for microcontrollers with little ram memory. In fact
+ *   the implementation of malloc from newlib requests memory to the OS in 4KB
+ *   block (except the first block that can be smaller). This is probably done
+ *   for compatibility with OSes with an MMU and paged memory. To see why this
+ *   is bad, consider a microcontroller with 8KB of ram: when malloc finishes
+ *   up the first 4KB it will call _sbrk_r asking for a 4KB block, but this will
+ *   fail because the top part of the ram is used by the main stack. As a
+ *   result, the top part of the memory will not be used by malloc, even if
+ *   available (and it is nearly *half* the ram on an 8KB mcu). By placing the
+ *   main stack at the bottom of the ram, the upper 4KB block will be entirely
+ *   free and available as heap space.
+ *
+ * - In case of main stack overflow the cpu will fault because access to memory
+ *   before the beginning of the ram faults. Instead with the default stack
+ *   placement the main stack will silently collide with the heap.
+ * Note: if increasing the main stack size also increase the ORIGIN value in
+ * the MEMORY definitions below accordingly.
+ */
+_main_stack_size = 0x00000200;                     /* main stack = 512Bytes */
+_main_stack_top  = 0x10000000 + _main_stack_size;
+ASSERT(_main_stack_size   % 8 == 0, "MAIN stack size error");
+
+/* Mapping the heap into the large 192KB RAM */
+_end =      0x20000000;
+_heap_end = 0x20030000;                            /* end of available ram  */
+
+/* identify the Entry Point  */
+ENTRY(_Z13Reset_Handlerv)
+
+/* specify the memory areas  */
+MEMORY
+{
+    flash(rx)    : ORIGIN = 0x08000000, LENGTH =   2M
+    /*
+     * Note, the small ram starts at 0x10000000 but it is necessary to add the
+     * size of the main stack, so it is 0x10000200.
+     */
+    smallram(wx) : ORIGIN = 0x10000200, LENGTH =  64K-0x200 
+    largeram(wx) : ORIGIN = 0x20000000, LENGTH = 192K
+    backupram(rw): ORIGIN = 0x40024000, LENGTH =  4K
+}
+
+/* now define the output sections  */
+SECTIONS
+{
+    . = 0;
+    
+    /* .text section: code goes to flash */
+    .text :
+    {
+        /* Startup code must go at address 0 */
+        KEEP(*(.isr_vector))
+        
+        *(.text)
+        *(.text.*)
+        *(.gnu.linkonce.t.*)
+        /* these sections for thumb interwork? */
+        *(.glue_7)
+        *(.glue_7t)
+        /* these sections for C++? */
+        *(.gcc_except_table)
+        *(.gcc_except_table.*)
+        *(.ARM.extab*)
+        *(.gnu.linkonce.armextab.*)
+
+        . = ALIGN(4);
+        /* .rodata: constant data */
+        *(.rodata)
+        *(.rodata.*)
+        *(.gnu.linkonce.r.*)
+
+        /* C++ Static constructors/destructors (eabi) */
+        . = ALIGN(4);
+        KEEP(*(.init))
+        
+        . = ALIGN(4);
+        __miosix_init_array_start = .;
+        KEEP (*(SORT(.miosix_init_array.*)))
+        KEEP (*(.miosix_init_array))
+        __miosix_init_array_end = .;
+
+        . = ALIGN(4);
+        __preinit_array_start = .;
+        KEEP (*(.preinit_array))
+        __preinit_array_end = .;
+
+        . = ALIGN(4);
+        __init_array_start = .;
+        KEEP (*(SORT(.init_array.*)))
+        KEEP (*(.init_array))
+        __init_array_end = .;
+
+        . = ALIGN(4);
+        KEEP(*(.fini))
+
+        . = ALIGN(4);
+        __fini_array_start = .;
+        KEEP (*(.fini_array))
+        KEEP (*(SORT(.fini_array.*)))
+        __fini_array_end = .;
+
+        /* C++ Static constructors/destructors (elf)  */
+        . = ALIGN(4);
+        _ctor_start = .;
+        KEEP (*crtbegin.o(.ctors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .ctors))
+        KEEP (*(SORT(.ctors.*)))
+        KEEP (*crtend.o(.ctors))
+       _ctor_end = .;
+
+        . = ALIGN(4);
+        KEEP (*crtbegin.o(.dtors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .dtors))
+        KEEP (*(SORT(.dtors.*)))
+        KEEP (*crtend.o(.dtors))
+    } > flash
+
+    /* .ARM.exidx is sorted, so has to go in its own output section.  */
+    __exidx_start = .;
+    .ARM.exidx :
+    {
+        *(.ARM.exidx* .gnu.linkonce.armexidx.*)
+    } > flash
+    __exidx_end = .;
+
+	/* .data section: global variables go to ram, but also store a copy to
+       flash to initialize them */
+    .data : ALIGN(8)
+    {
+        _data = .;
+        *(.data)
+        *(.data.*)
+        *(.gnu.linkonce.d.*)
+        . = ALIGN(8);
+        _edata = .;
+    } > smallram AT > flash
+    _etext = LOADADDR(.data);
+
+    /* .bss section: uninitialized global variables go to ram */
+    _bss_start = .;
+    .bss :
+    {
+        *(.bss)
+        *(.bss.*)
+        *(.gnu.linkonce.b.*)
+        . = ALIGN(8);
+    } > smallram
+    _bss_end = .;
+
+    /*_end = .;*/
+    /*PROVIDE(end = .);*/
+}
diff --git a/src/bsps/stm32f429zi_death_stack_v3/stm32_2m+8m_xram.ld b/src/bsps/stm32f429zi_death_stack_v3/stm32_2m+8m_xram.ld
new file mode 100644
index 000000000..b6a0d2668
--- /dev/null
+++ b/src/bsps/stm32f429zi_death_stack_v3/stm32_2m+8m_xram.ld
@@ -0,0 +1,178 @@
+/*
+ * C++ enabled linker script for stm32 (2M FLASH, 256K RAM, 8M XRAM)
+ * Developed by TFT: Terraneo Federico Technologies
+ * Optimized for use with the Miosix kernel
+ */
+
+/*
+ * This chip has an unusual quirk that the RAM is divided in two block mapped
+ * at two non contiguous memory addresses. I don't know why they've done that,
+ * probably doing the obvious thing would have made writing code too easy...
+ * Anyway, since hardware can't be changed, we've got to live with that and
+ * try to make use of both RAMs.
+ * 
+ * Given the constraints above, this linker script puts:
+ * - read only data and code (.text, .rodata, .eh_*) in FLASH
+ * - the 512Byte main (IRQ) stack, .data and .bss in the "small" 64KB RAM
+ * - .data, .bss, stacks and heap in the external 8MB SDRAM.
+ */
+
+/*
+ * The main stack is used for interrupt handling by the kernel.
+ *
+ * *** Readme ***
+ * This linker script places the main stack (used by the kernel for interrupts)
+ * at the bottom of the ram, instead of the top. This is done for two reasons:
+ *
+ * - as an optimization for microcontrollers with little ram memory. In fact
+ *   the implementation of malloc from newlib requests memory to the OS in 4KB
+ *   block (except the first block that can be smaller). This is probably done
+ *   for compatibility with OSes with an MMU and paged memory. To see why this
+ *   is bad, consider a microcontroller with 8KB of ram: when malloc finishes
+ *   up the first 4KB it will call _sbrk_r asking for a 4KB block, but this will
+ *   fail because the top part of the ram is used by the main stack. As a
+ *   result, the top part of the memory will not be used by malloc, even if
+ *   available (and it is nearly *half* the ram on an 8KB mcu). By placing the
+ *   main stack at the bottom of the ram, the upper 4KB block will be entirely
+ *   free and available as heap space.
+ *
+ * - In case of main stack overflow the cpu will fault because access to memory
+ *   before the beginning of the ram faults. Instead with the default stack
+ *   placement the main stack will silently collide with the heap.
+ * Note: if increasing the main stack size also increase the ORIGIN value in
+ * the MEMORY definitions below accordingly.
+ */
+_main_stack_size = 0x00000200;                     /* main stack = 512Bytes */
+_main_stack_top  = 0x10000000 + _main_stack_size;
+ASSERT(_main_stack_size   % 8 == 0, "MAIN stack size error");
+
+/* Mapping the heap into XRAM */
+_heap_end = 0xd0800000;                            /* end of available ram  */
+
+/* identify the Entry Point  */
+ENTRY(_Z13Reset_Handlerv)
+
+/* specify the memory areas  */
+MEMORY
+{
+    flash(rx)    : ORIGIN = 0x08000000, LENGTH =   2M
+    /*
+     * Note, the small ram starts at 0x10000000 but it is necessary to add the
+     * size of the main stack, so it is 0x10000200.
+     */
+    smallram(wx) : ORIGIN = 0x10000200, LENGTH =  64K-0x200 
+    largeram(wx) : ORIGIN = 0x20000000, LENGTH = 192K
+    backupram(rw): ORIGIN = 0x40024000, LENGTH =   4K
+    xram(wx)     : ORIGIN = 0xd0000000, LENGTH =   8M
+}
+
+/* now define the output sections  */
+SECTIONS
+{
+    . = 0;
+    
+    /* .text section: code goes to flash */
+    .text :
+    {
+        /* Startup code must go at address 0 */
+        KEEP(*(.isr_vector))
+        
+        *(.text)
+        *(.text.*)
+        *(.gnu.linkonce.t.*)
+        /* these sections for thumb interwork? */
+        *(.glue_7)
+        *(.glue_7t)
+        /* these sections for C++? */
+        *(.gcc_except_table)
+        *(.gcc_except_table.*)
+        *(.ARM.extab*)
+        *(.gnu.linkonce.armextab.*)
+
+        . = ALIGN(4);
+        /* .rodata: constant data */
+        *(.rodata)
+        *(.rodata.*)
+        *(.gnu.linkonce.r.*)
+
+        /* C++ Static constructors/destructors (eabi) */
+        . = ALIGN(4);
+        KEEP(*(.init))
+        
+        . = ALIGN(4);
+        __miosix_init_array_start = .;
+        KEEP (*(SORT(.miosix_init_array.*)))
+        KEEP (*(.miosix_init_array))
+        __miosix_init_array_end = .;
+
+        . = ALIGN(4);
+        __preinit_array_start = .;
+        KEEP (*(.preinit_array))
+        __preinit_array_end = .;
+
+        . = ALIGN(4);
+        __init_array_start = .;
+        KEEP (*(SORT(.init_array.*)))
+        KEEP (*(.init_array))
+        __init_array_end = .;
+
+        . = ALIGN(4);
+        KEEP(*(.fini))
+
+        . = ALIGN(4);
+        __fini_array_start = .;
+        KEEP (*(.fini_array))
+        KEEP (*(SORT(.fini_array.*)))
+        __fini_array_end = .;
+
+        /* C++ Static constructors/destructors (elf)  */
+        . = ALIGN(4);
+        _ctor_start = .;
+        KEEP (*crtbegin.o(.ctors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .ctors))
+        KEEP (*(SORT(.ctors.*)))
+        KEEP (*crtend.o(.ctors))
+       _ctor_end = .;
+
+        . = ALIGN(4);
+        KEEP (*crtbegin.o(.dtors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .dtors))
+        KEEP (*(SORT(.dtors.*)))
+        KEEP (*crtend.o(.dtors))
+    } > flash
+
+    /* .ARM.exidx is sorted, so has to go in its own output section.  */
+    __exidx_start = .;
+    .ARM.exidx :
+    {
+        *(.ARM.exidx* .gnu.linkonce.armexidx.*)
+    } > flash
+    __exidx_end = .;
+
+	/* .data section: global variables go to ram, but also store a copy to
+       flash to initialize them */
+    .data : ALIGN(8)
+    {
+        _data = .;
+        *(.data)
+        *(.data.*)
+        *(.gnu.linkonce.d.*)
+        . = ALIGN(8);
+        _edata = .;
+    } > xram AT > flash
+    _etext = LOADADDR(.data);
+
+    /* .bss section: uninitialized global variables go to ram */
+    _bss_start = .;
+    .bss :
+    {
+        *(.bss)
+        *(.bss.*)
+        *(.gnu.linkonce.b.*)
+        . = ALIGN(8);
+    } > xram
+    _bss_end = .;
+
+    _end = .;
+    PROVIDE(end = .);
+}
diff --git a/src/bsps/stm32f429zi_nokia/config/board_options.cmake b/src/bsps/stm32f429zi_nokia/config/board_options.cmake
new file mode 100644
index 000000000..c531b5c9a
--- /dev/null
+++ b/src/bsps/stm32f429zi_nokia/config/board_options.cmake
@@ -0,0 +1,124 @@
+# Copyright (C) 2023 by Skyward
+#
+# This program is free software; you can redistribute it and/or 
+# it under the terms of the GNU General Public License as published 
+# the Free Software Foundation; either version 2 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.
+#
+# As a special exception, if other files instantiate templates or use
+# macros or inline functions from this file, or you compile this file
+# and link it with other works to produce a work based on this file,
+# this file does not by itself cause the resulting work to be covered
+# by the GNU General Public License. However the source code for this
+# file must still be made available in accordance with the GNU 
+# Public License. This exception does not invalidate any other 
+# why a work based on this file might be covered by the GNU General
+# Public License.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, see <http://www.gnu.org/licenses/>
+
+set(BOARD_NAME stm32f429zi_nokia)
+set(ARCH_NAME cortexM4_stm32f4)
+
+# Specify which MxGui base library to use
+set(MXGUI_BASE_BOARD_NAME stm32f429zi_stm32f4discovery)
+
+# Base directories with header files for this board
+set(ARCH_PATH ${KPATH}/arch/${ARCH_NAME}/common)
+set(BOARD_PATH ${BOARDCORE_PATH}/src/bsps/${BOARD_NAME})
+set(BOARD_CONFIG_PATH ${BOARDCORE_PATH}/src/bsps/${BOARD_NAME}/config)
+
+# Specify where to find the board specific config/miosix_settings.h
+set(BOARD_MIOSIX_SETTINGS_PATH ${BOARD_PATH})
+
+# Specify where to find the board specific config/mxgui_settings.h
+set(BOARD_MXGUI_SETTINGS_PATH ${BOARD_PATH})
+
+# Optimization flags:
+# -O0 do no optimization, the default if no optimization level is specified
+# -O or -O1 optimize minimally
+# -O2 optimize more
+# -O3 optimize even more
+# -Ofast optimize very aggressively to the point of breaking the standard
+# -Og Optimize debugging experience, enables optimizations that do not
+# interfere with debugging
+# -Os Optimize for size with -O2 optimizations that do not increase code size
+set(OPT_OPTIMIZATION -O2)
+
+# Boot file
+set(BOOT_FILE ${BOARD_PATH}/core/stage_1_boot.cpp)
+
+# Linker script type, there are three options
+# 1) Code in FLASH, stack + heap in internal RAM (file *_rom.ld)
+#    the most common choice, available for all microcontrollers
+# 2) Code in FLASH, stack + heap in external RAM (file *8m_xram.ld)
+#    You must uncomment -D__ENABLE_XRAM below in this case.
+# set(LINKER_SCRIPT ${BOARD_PATH}/stm32_2m+256k_rom.ld)
+set(LINKER_SCRIPT ${BOARD_PATH}/stm32_2m+8m_xram.ld)
+
+# Uncommenting __ENABLE_XRAM enables the initialization of the external
+# 8MB SDRAM memory. Do not uncomment this even if you don't use a linker
+# script that requires it, as it is used for the LCD framebuffer.
+set(XRAM -D__ENABLE_XRAM)
+
+# Select clock frequency. 
+# Warning: the default clock frequency for
+# this board is 168MHz and not 180MHz because, due to a limitation in
+# the PLL, it is not possible to generate a precise 48MHz output when
+# running the core at 180MHz. If 180MHz is chosen the USB peripheral will
+# NOT WORK and the SDIO and RNG will run ~6% slower (45MHz insteand of 48)
+# Define USE_INTERNAL_CLOCK to bypass the external oscillator
+# set(CLOCK_FREQ -DHSE_VALUE=8000000 -DSYSCLK_FREQ_180MHz=180000000)
+set(CLOCK_FREQ -DHSE_VALUE=8000000 -DSYSCLK_FREQ_168MHz=168000000)
+# set(CLOCK_FREQ -DHSE_VALUE=8000000 -DSYSCLK_FREQ_100MHz=100000000)
+
+# C++ Exception/rtti support disable flags.
+# To save code size if not using C++ exceptions (nor some STL code which
+# implicitly uses it) uncomment this option.
+# -D__NO_EXCEPTIONS is used by Miosix to know if exceptions are used.
+# set(OPT_EXCEPT -fno-exceptions -fno-rtti -D__NO_EXCEPTIONS)
+
+# Specify a custom flash command
+# This is the program that is invoked when the flash flag (-f or --flash) is
+# used with the Miosix Build System. Use $binary or $hex as placeolders, they
+# will be replaced by the build systems with the binary or hex file repectively.
+# If a command is not specified, the build system will use st-flash if found
+# set(PROGRAM_CMDLINE "here your custom flash command")
+
+# Basic flags
+set(FLAGS_BASE -mcpu=cortex-m4 -mthumb -mfloat-abi=hard -mfpu=fpv4-sp-d16)
+
+# Flags for ASM and linker
+set(AFLAGS_BASE ${FLAGS_BASE})
+set(LFLAGS_BASE ${FLAGS_BASE} -Wl,--gc-sections,-Map,main.map -Wl,-T${LINKER_SCRIPT} ${OPT_EXCEPT} ${OPT_OPTIMIZATION} -nostdlib)
+
+# Flags for C/C++
+string(TOUPPER ${BOARD_NAME} BOARD_UPPER)
+set(CFLAGS_BASE
+    -D_BOARD_STM32F429ZI_NOKIA -D_MIOSIX_BOARDNAME=\"${BOARD_NAME}\"
+    -D_DEFAULT_SOURCE=1 -ffunction-sections -Wall -Werror=return-type -g
+    -D_ARCH_CORTEXM4_STM32F4
+    ${CLOCK_FREQ} ${XRAM} ${SRAM_BOOT} ${FLAGS_BASE} ${OPT_OPTIMIZATION} -c
+)
+set(CXXFLAGS_BASE ${CFLAGS_BASE} ${OPT_EXCEPT})
+
+# Select architecture specific files
+set(ARCH_SRC
+    ${ARCH_PATH}/interfaces-impl/delays.cpp
+    ${ARCH_PATH}/interfaces-impl/gpio_impl.cpp
+    ${ARCH_PATH}/interfaces-impl/portability.cpp
+    ${BOARD_PATH}/interfaces-impl/bsp.cpp
+    ${KPATH}/arch/common/CMSIS/Device/ST/STM32F4xx/Source/Templates/system_stm32f4xx.c
+    ${KPATH}/arch/common/core/interrupts_cortexMx.cpp
+    ${KPATH}/arch/common/core/mpu_cortexMx.cpp
+    ${KPATH}/arch/common/core/stm32f2_f4_l4_f7_h7_os_timer.cpp
+    ${KPATH}/arch/common/drivers/sd_stm32f2_f4_f7.cpp
+    ${KPATH}/arch/common/drivers/serial_stm32.cpp
+    ${KPATH}/arch/common/drivers/stm32_hardware_rng.cpp
+)
diff --git a/src/bsps/stm32f429zi_nokia/config/board_settings.h b/src/bsps/stm32f429zi_nokia/config/board_settings.h
new file mode 100644
index 000000000..b817b53d4
--- /dev/null
+++ b/src/bsps/stm32f429zi_nokia/config/board_settings.h
@@ -0,0 +1,70 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * 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.
+ */
+
+#ifndef BOARD_SETTINGS_H
+#define BOARD_SETTINGS_H
+
+#include "util/version.h"
+
+/**
+ * \internal
+ * Versioning for board_settings.h for out of git tree projects
+ */
+#define BOARD_SETTINGS_VERSION 300
+
+namespace miosix
+{
+
+/**
+ * \addtogroup Settings
+ * \{
+ */
+
+/// Size of stack for main().
+/// The C standard library is stack-heavy (iprintf requires 1KB) but the
+/// STM32F407VG only has 192KB of RAM so there is room for a big 4K stack.
+const unsigned int MAIN_STACK_SIZE = 4 * 1024;
+
+/// Serial port
+const unsigned int defaultSerial      = 1;
+const unsigned int defaultSerialSpeed = 115200;
+const bool defaultSerialFlowctrl      = false;
+// #define SERIAL_1_DMA
+// #define SERIAL_2_DMA //Serial 2 can't be used (GPIO conflict), so no DMA
+// #define SERIAL_3_DMA //Serial 3 can't be used (GPIO conflict), so no DMA
+
+// #define I2C_WITH_DMA
+
+// SD card driver
+static const unsigned char sdVoltage = 30;  // Board powered @ 3.0V
+#define SD_ONE_BIT_DATABUS  // Can't use 4 bit databus due to pin conflicts
+
+/// Analog supply voltage for ADC, DAC, Reset blocks, RCs and PLL
+#define V_DDA_VOLTAGE 3.0f
+
+/**
+ * \}
+ */
+
+}  // namespace miosix
+
+#endif /* BOARD_SETTINGS_H */
diff --git a/src/bsps/stm32f429zi_nokia/config/miosix_settings.h b/src/bsps/stm32f429zi_nokia/config/miosix_settings.h
new file mode 100644
index 000000000..2b82675b7
--- /dev/null
+++ b/src/bsps/stm32f429zi_nokia/config/miosix_settings.h
@@ -0,0 +1,240 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * 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.
+ */
+
+#pragma once
+
+// Before you can compile the kernel you have to configure it by editing this
+// file. After that, comment out this line to disable the reminder error.
+// The PARSING_FROM_IDE is because Netbeans gets confused by this, it is never
+// defined when compiling the code.
+#ifndef PARSING_FROM_IDE
+//#error This error is a reminder that you have not edited miosix_settings.h
+// yet.
+#endif  // PARSING_FROM_IDE
+
+/**
+ * \file miosix_settings.h
+ * NOTE: this file contains ONLY configuration options that are not dependent
+ * on architecture specific details. The other options are in the following
+ * files which are included here:
+ * miosix/arch/architecture name/common/arch_settings.h
+ * miosix/config/arch/architecture name/board name/board_settings.h
+ */
+#include "arch_settings.h"
+#include "board_settings.h"
+#include "util/version.h"
+
+/**
+ * \internal
+ * Versioning for miosix_settings.h for out of git tree projects
+ */
+#define MIOSIX_SETTINGS_VERSION 300
+
+namespace miosix
+{
+
+/**
+ * \addtogroup Settings
+ * \{
+ */
+
+//
+// Scheduler options
+//
+
+/// \def SCHED_TYPE_PRIORITY
+/// If uncommented selects the priority scheduler
+/// \def SCHED_TYPE_CONTROL_BASED
+/// If uncommented selects the control based scheduler
+/// \def SCHED_TYPE_EDF
+/// If uncommented selects the EDF scheduler
+// Uncomment only *one* of those
+
+#define SCHED_TYPE_PRIORITY
+//#define SCHED_TYPE_CONTROL_BASED
+//#define SCHED_TYPE_EDF
+
+/// \def WITH_CPU_TIME_COUNTER
+/// Allows to enable/disable CPUTimeCounter to save code size and remove its
+/// overhead from the scheduling process. By default it is not defined
+/// (CPUTimeCounter is disabled).
+//#define WITH_CPU_TIME_COUNTER
+
+//
+// Filesystem options
+//
+
+/// \def WITH_FILESYSTEM
+/// Allows to enable/disable filesystem support to save code size
+/// By default it is defined (filesystem support is enabled)
+#define WITH_FILESYSTEM
+
+/// \def WITH_DEVFS
+/// Allows to enable/disable DevFs support to save code size
+/// By default it is defined (DevFs is enabled)
+#define WITH_DEVFS
+
+/// \def SYNC_AFTER_WRITE
+/// Increases filesystem write robustness. After each write operation the
+/// filesystem is synced so that a power failure happens data is not lost
+/// (unless power failure happens exactly between the write and the sync)
+/// Unfortunately write latency and throughput becomes twice as worse
+/// By default it is defined (slow but safe)
+#define SYNC_AFTER_WRITE
+
+/// Maximum number of open files. Trying to open more will fail.
+/// Cannot be lower than 3, as the first three are stdin, stdout, stderr
+const unsigned char MAX_OPEN_FILES = 8;
+
+/// \def WITH_PROCESSES
+/// If uncommented enables support for processes as well as threads.
+/// This enables the dynamic loader to load elf programs, the extended system
+/// call service and, if the hardware supports it, the MPU to provide memory
+/// isolation of processes
+//#define WITH_PROCESSES
+
+#if defined(WITH_PROCESSES) && defined(__NO_EXCEPTIONS)
+#error Processes require C++ exception support
+#endif  // defined(WITH_PROCESSES) && defined(__NO_EXCEPTIONS)
+
+#if defined(WITH_PROCESSES) && !defined(WITH_FILESYSTEM)
+#error Processes require filesystem support
+#endif  // defined(WITH_PROCESSES) && !defined(WITH_FILESYSTEM)
+
+#if defined(WITH_PROCESSES) && !defined(WITH_DEVFS)
+#error Processes require devfs support
+#endif  // defined(WITH_PROCESSES) && !defined(WITH_DEVFS)
+
+//
+// C/C++ standard library I/O (stdin, stdout and stderr related)
+//
+
+/// \def WITH_BOOTLOG
+/// Uncomment to print bootlogs on stdout.
+/// By default it is defined (bootlogs are printed)
+#define WITH_BOOTLOG
+
+/// \def WITH_ERRLOG
+/// Uncomment for debug information on stdout.
+/// By default it is defined (error information is printed)
+#define WITH_ERRLOG
+
+//
+// Kernel related options (stack sizes, priorities)
+//
+
+/// \def WITH_DEEP_SLEEP
+/// Adds interfaces and required variables to support deep sleep state switch
+/// automatically when peripherals are not required
+//#define WITH_DEEP_SLEEP
+
+/**
+ * \def JTAG_DISABLE_SLEEP
+ * JTAG debuggers lose communication with the device if it enters sleep
+ * mode, so to use debugging it is necessary to disable sleep in the idle
+ * thread. By default it is not defined (idle thread calls sleep).
+ */
+//#define JTAG_DISABLE_SLEEP
+
+#if defined(WITH_DEEP_SLEEP) && defined(JTAG_DISABLE_SLEEP)
+#error Deep sleep cannot work together with jtag
+#endif  // defined(WITH_PROCESSES) && !defined(WITH_DEVFS)
+
+/// Minimum stack size (MUST be divisible by 4)
+const unsigned int STACK_MIN = 256;
+
+/// \internal Size of idle thread stack.
+/// Should be >=STACK_MIN (MUST be divisible by 4)
+const unsigned int STACK_IDLE = 256;
+
+/// Default stack size for pthread_create.
+/// The chosen value is enough to call C standard library functions
+/// such as printf/fopen which are stack-heavy
+const unsigned int STACK_DEFAULT_FOR_PTHREAD = 2048;
+
+/// Maximum size of the RAM image of a process. If a program requires more
+/// the kernel will not run it (MUST be divisible by 4)
+const unsigned int MAX_PROCESS_IMAGE_SIZE = 64 * 1024;
+
+/// Minimum size of the stack for a process. If a program specifies a lower
+/// size the kernel will not run it (MUST be divisible by 4)
+const unsigned int MIN_PROCESS_STACK_SIZE = STACK_MIN;
+
+/// Every userspace thread has two stacks, one for when it is running in
+/// userspace and one for when it is running in kernelspace (that is, while it
+/// is executing system calls). This is the size of the stack for when the
+/// thread is running in kernelspace (MUST be divisible by 4)
+const unsigned int SYSTEM_MODE_PROCESS_STACK_SIZE = 2 * 1024;
+
+/// Number of priorities (MUST be >1)
+/// PRIORITY_MAX-1 is the highest priority, 0 is the lowest. -1 is reserved as
+/// the priority of the idle thread.
+/// The meaning of a thread's priority depends on the chosen scheduler.
+#ifdef SCHED_TYPE_PRIORITY
+// Can be modified, but a high value makes context switches more expensive
+const short int PRIORITY_MAX = 4;
+#elif defined(SCHED_TYPE_CONTROL_BASED)
+// Don't touch, the limit is due to the fixed point implementation
+// It's not needed for if floating point is selected, but kept for consistency
+const short int PRIORITY_MAX = 64;
+#else  // SCHED_TYPE_EDF
+// Doesn't exist for this kind of scheduler
+#endif
+
+/// Priority of main()
+/// The meaning of a thread's priority depends on the chosen scheduler.
+const unsigned char MAIN_PRIORITY = 1;
+
+#ifdef SCHED_TYPE_PRIORITY
+/// Maximum thread time slice in nanoseconds, after which preemption occurs
+const unsigned int MAX_TIME_SLICE = 1000000;
+#endif  // SCHED_TYPE_PRIORITY
+
+//
+// Other low level kernel options. There is usually no need to modify these.
+//
+
+/// \internal Length of wartermark (in bytes) to check stack overflow.
+/// MUST be divisible by 4 and can also be zero.
+/// A high value increases context switch time.
+const unsigned int WATERMARK_LEN = 16;
+
+/// \internal Used to fill watermark
+const unsigned int WATERMARK_FILL = 0xaaaaaaaa;
+
+/// \internal Used to fill stack (for checking stack usage)
+const unsigned int STACK_FILL = 0xbbbbbbbb;
+
+// Compiler version checks
+#if !defined(_MIOSIX_GCC_PATCH_MAJOR) || _MIOSIX_GCC_PATCH_MAJOR < 3
+#error \
+    "You are using a too old or unsupported compiler. Get the latest one from https://miosix.org/wiki/index.php?title=Miosix_Toolchain"
+#endif
+#if _MIOSIX_GCC_PATCH_MAJOR > 3
+#warning "You are using a too new compiler, which may not be supported"
+#endif
+
+/**
+ * \}
+ */
+
+}  // namespace miosix
diff --git a/src/bsps/stm32f429zi_nokia/core/stage_1_boot.cpp b/src/bsps/stm32f429zi_nokia/core/stage_1_boot.cpp
new file mode 100644
index 000000000..6e86bb25f
--- /dev/null
+++ b/src/bsps/stm32f429zi_nokia/core/stage_1_boot.cpp
@@ -0,0 +1,372 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * 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 <string.h>
+
+#include "core/interrupts.h"  //For the unexpected interrupt call
+#include "interfaces/arch_registers.h"
+#include "interfaces/bsp.h"
+#include "kernel/stage_2_boot.h"
+
+/*
+ * startup.cpp
+ * STM32 C++ startup.
+ * NOTE: for stm32f42x devices ONLY.
+ * Supports interrupt handlers in C++ without extern "C"
+ * Developed by Terraneo Federico, based on ST startup code.
+ * Additionally modified to boot Miosix.
+ */
+
+/**
+ * Called by Reset_Handler, performs initialization and calls main.
+ * Never returns.
+ */
+void program_startup() __attribute__((noreturn));
+void program_startup()
+{
+    // Cortex M3 core appears to get out of reset with interrupts already
+    // enabled
+    __disable_irq();
+
+    // These are defined in the linker script
+    extern unsigned char _etext asm("_etext");
+    extern unsigned char _data asm("_data");
+    extern unsigned char _edata asm("_edata");
+    extern unsigned char _bss_start asm("_bss_start");
+    extern unsigned char _bss_end asm("_bss_end");
+
+    // Initialize .data section, clear .bss section
+    unsigned char *etext     = &_etext;
+    unsigned char *data      = &_data;
+    unsigned char *edata     = &_edata;
+    unsigned char *bss_start = &_bss_start;
+    unsigned char *bss_end   = &_bss_end;
+    memcpy(data, etext, edata - data);
+    memset(bss_start, 0, bss_end - bss_start);
+
+    // Move on to stage 2
+    _init();
+
+    // If main returns, reboot
+    NVIC_SystemReset();
+    for (;;)
+        ;
+}
+
+/**
+ * Reset handler, called by hardware immediately after reset
+ */
+void Reset_Handler() __attribute__((__interrupt__, noreturn));
+void Reset_Handler()
+{
+    // SystemInit() is called *before* initializing .data and zeroing .bss
+    // Despite all startup files provided by ST do the opposite, there are three
+    // good reasons to do so:
+    // First, the CMSIS specifications say that SystemInit() must not access
+    // global variables, so it is actually possible to call it before
+    // Second, when running Miosix with the xram linker scripts .data and .bss
+    // are placed in the external RAM, so we *must* call SystemInit(), which
+    // enables xram, before touching .data and .bss
+    // Third, this is a performance improvement since the loops that initialize
+    //.data and zeros .bss now run with the CPU at full speed instead of 8MHz
+    SystemInit();
+// ST does not provide code to initialize the stm32f429-disco SDRAM at boot.
+// Put after SystemInit() as SDRAM is timing-sensitive and needs the full
+// clock speed.
+#ifdef __ENABLE_XRAM
+    miosix::configureSdram();
+#endif  //__ENABLE_XRAM
+
+    /*
+     * Initialize process stack and switch to it.
+     * This is required for booting Miosix, a small portion of the top of the
+     * heap area will be used as stack until the first thread starts. After,
+     * this stack will be abandoned and the process stack will point to the
+     * current thread's stack.
+     */
+    asm volatile(
+        "ldr r0,  =_heap_end          \n\t"
+        "msr psp, r0                  \n\t"
+        "movw r0, #2                  \n\n"  // Privileged, process stack
+        "msr control, r0              \n\t"
+        "isb                          \n\t" ::
+            : "r0");
+
+    program_startup();
+}
+
+/**
+ * All unused interrupts call this function.
+ */
+extern "C" void Default_Handler() { unexpectedInterrupt(); }
+
+// System handlers
+void /*__attribute__((weak))*/ Reset_Handler();      // These interrupts are not
+void /*__attribute__((weak))*/ NMI_Handler();        // weak because they are
+void /*__attribute__((weak))*/ HardFault_Handler();  // surely defined by Miosix
+void /*__attribute__((weak))*/ MemManage_Handler();
+void /*__attribute__((weak))*/ BusFault_Handler();
+void /*__attribute__((weak))*/ UsageFault_Handler();
+void /*__attribute__((weak))*/ SVC_Handler();
+void /*__attribute__((weak))*/ DebugMon_Handler();
+void /*__attribute__((weak))*/ PendSV_Handler();
+void __attribute__((weak)) SysTick_Handler();
+
+// Interrupt handlers
+void __attribute__((weak)) WWDG_IRQHandler();
+void __attribute__((weak)) PVD_IRQHandler();
+void __attribute__((weak)) TAMP_STAMP_IRQHandler();
+void __attribute__((weak)) RTC_WKUP_IRQHandler();
+void __attribute__((weak)) FLASH_IRQHandler();
+void __attribute__((weak)) RCC_IRQHandler();
+void __attribute__((weak)) EXTI0_IRQHandler();
+void __attribute__((weak)) EXTI1_IRQHandler();
+void __attribute__((weak)) EXTI2_IRQHandler();
+void __attribute__((weak)) EXTI3_IRQHandler();
+void __attribute__((weak)) EXTI4_IRQHandler();
+void __attribute__((weak)) DMA1_Stream0_IRQHandler();
+void __attribute__((weak)) DMA1_Stream1_IRQHandler();
+void __attribute__((weak)) DMA1_Stream2_IRQHandler();
+void __attribute__((weak)) DMA1_Stream3_IRQHandler();
+void __attribute__((weak)) DMA1_Stream4_IRQHandler();
+void __attribute__((weak)) DMA1_Stream5_IRQHandler();
+void __attribute__((weak)) DMA1_Stream6_IRQHandler();
+void __attribute__((weak)) ADC_IRQHandler();
+void __attribute__((weak)) CAN1_TX_IRQHandler();
+void __attribute__((weak)) CAN1_RX0_IRQHandler();
+void __attribute__((weak)) CAN1_RX1_IRQHandler();
+void __attribute__((weak)) CAN1_SCE_IRQHandler();
+void __attribute__((weak)) EXTI9_5_IRQHandler();
+void __attribute__((weak)) TIM1_BRK_TIM9_IRQHandler();
+void __attribute__((weak)) TIM1_UP_TIM10_IRQHandler();
+void __attribute__((weak)) TIM1_TRG_COM_TIM11_IRQHandler();
+void __attribute__((weak)) TIM1_CC_IRQHandler();
+void __attribute__((weak)) TIM2_IRQHandler();
+void __attribute__((weak)) TIM3_IRQHandler();
+void __attribute__((weak)) TIM4_IRQHandler();
+void __attribute__((weak)) I2C1_EV_IRQHandler();
+void __attribute__((weak)) I2C1_ER_IRQHandler();
+void __attribute__((weak)) I2C2_EV_IRQHandler();
+void __attribute__((weak)) I2C2_ER_IRQHandler();
+void __attribute__((weak)) SPI1_IRQHandler();
+void __attribute__((weak)) SPI2_IRQHandler();
+void __attribute__((weak)) USART1_IRQHandler();
+void __attribute__((weak)) USART2_IRQHandler();
+void __attribute__((weak)) USART3_IRQHandler();
+void __attribute__((weak)) EXTI15_10_IRQHandler();
+void __attribute__((weak)) RTC_Alarm_IRQHandler();
+void __attribute__((weak)) OTG_FS_WKUP_IRQHandler();
+void __attribute__((weak)) TIM8_BRK_TIM12_IRQHandler();
+void __attribute__((weak)) TIM8_UP_TIM13_IRQHandler();
+void __attribute__((weak)) TIM8_TRG_COM_TIM14_IRQHandler();
+void __attribute__((weak)) TIM8_CC_IRQHandler();
+void __attribute__((weak)) DMA1_Stream7_IRQHandler();
+void __attribute__((weak)) FMC_IRQHandler();
+void __attribute__((weak)) SDIO_IRQHandler();
+void __attribute__((weak)) TIM5_IRQHandler();
+void __attribute__((weak)) SPI3_IRQHandler();
+void __attribute__((weak)) UART4_IRQHandler();
+void __attribute__((weak)) UART5_IRQHandler();
+void __attribute__((weak)) TIM6_DAC_IRQHandler();
+void __attribute__((weak)) TIM7_IRQHandler();
+void __attribute__((weak)) DMA2_Stream0_IRQHandler();
+void __attribute__((weak)) DMA2_Stream1_IRQHandler();
+void __attribute__((weak)) DMA2_Stream2_IRQHandler();
+void __attribute__((weak)) DMA2_Stream3_IRQHandler();
+void __attribute__((weak)) DMA2_Stream4_IRQHandler();
+void __attribute__((weak)) ETH_IRQHandler();
+void __attribute__((weak)) ETH_WKUP_IRQHandler();
+void __attribute__((weak)) CAN2_TX_IRQHandler();
+void __attribute__((weak)) CAN2_RX0_IRQHandler();
+void __attribute__((weak)) CAN2_RX1_IRQHandler();
+void __attribute__((weak)) CAN2_SCE_IRQHandler();
+void __attribute__((weak)) OTG_FS_IRQHandler();
+void __attribute__((weak)) DMA2_Stream5_IRQHandler();
+void __attribute__((weak)) DMA2_Stream6_IRQHandler();
+void __attribute__((weak)) DMA2_Stream7_IRQHandler();
+void __attribute__((weak)) USART6_IRQHandler();
+void __attribute__((weak)) I2C3_EV_IRQHandler();
+void __attribute__((weak)) I2C3_ER_IRQHandler();
+void __attribute__((weak)) OTG_HS_EP1_OUT_IRQHandler();
+void __attribute__((weak)) OTG_HS_EP1_IN_IRQHandler();
+void __attribute__((weak)) OTG_HS_WKUP_IRQHandler();
+void __attribute__((weak)) OTG_HS_IRQHandler();
+void __attribute__((weak)) DCMI_IRQHandler();
+void __attribute__((weak)) CRYP_IRQHandler();
+void __attribute__((weak)) HASH_RNG_IRQHandler();
+void __attribute__((weak)) FPU_IRQHandler();
+void __attribute__((weak)) UART7_IRQHandler();
+void __attribute__((weak)) UART8_IRQHandler();
+void __attribute__((weak)) SPI4_IRQHandler();
+void __attribute__((weak)) SPI5_IRQHandler();
+void __attribute__((weak)) SPI6_IRQHandler();
+void __attribute__((weak)) SAI1_IRQHandler();
+void __attribute__((weak)) LTDC_IRQHandler();
+void __attribute__((weak)) LTDC_ER_IRQHandler();
+void __attribute__((weak)) DMA2D_IRQHandler();
+
+// Stack top, defined in the linker script
+extern char _main_stack_top asm("_main_stack_top");
+
+// Interrupt vectors, must be placed @ address 0x00000000
+// The extern declaration is required otherwise g++ optimizes it out
+extern void (*const __Vectors[])();
+void (*const __Vectors[])() __attribute__((section(".isr_vector"))) = {
+    reinterpret_cast<void (*)()>(&_main_stack_top), /* Stack pointer*/
+    Reset_Handler,                                  /* Reset Handler */
+    NMI_Handler,                                    /* NMI Handler */
+    HardFault_Handler,                              /* Hard Fault Handler */
+    MemManage_Handler,                              /* MPU Fault Handler */
+    BusFault_Handler,                               /* Bus Fault Handler */
+    UsageFault_Handler,                             /* Usage Fault Handler */
+    0,                                              /* Reserved */
+    0,                                              /* Reserved */
+    0,                                              /* Reserved */
+    0,                                              /* Reserved */
+    SVC_Handler,                                    /* SVCall Handler */
+    DebugMon_Handler,                               /* Debug Monitor Handler */
+    0,                                              /* Reserved */
+    PendSV_Handler,                                 /* PendSV Handler */
+    SysTick_Handler,                                /* SysTick Handler */
+
+    /* External Interrupts */
+    WWDG_IRQHandler, PVD_IRQHandler, TAMP_STAMP_IRQHandler, RTC_WKUP_IRQHandler,
+    FLASH_IRQHandler, RCC_IRQHandler, EXTI0_IRQHandler, EXTI1_IRQHandler,
+    EXTI2_IRQHandler, EXTI3_IRQHandler, EXTI4_IRQHandler,
+    DMA1_Stream0_IRQHandler, DMA1_Stream1_IRQHandler, DMA1_Stream2_IRQHandler,
+    DMA1_Stream3_IRQHandler, DMA1_Stream4_IRQHandler, DMA1_Stream5_IRQHandler,
+    DMA1_Stream6_IRQHandler, ADC_IRQHandler, CAN1_TX_IRQHandler,
+    CAN1_RX0_IRQHandler, CAN1_RX1_IRQHandler, CAN1_SCE_IRQHandler,
+    EXTI9_5_IRQHandler, TIM1_BRK_TIM9_IRQHandler, TIM1_UP_TIM10_IRQHandler,
+    TIM1_TRG_COM_TIM11_IRQHandler, TIM1_CC_IRQHandler, TIM2_IRQHandler,
+    TIM3_IRQHandler, TIM4_IRQHandler, I2C1_EV_IRQHandler, I2C1_ER_IRQHandler,
+    I2C2_EV_IRQHandler, I2C2_ER_IRQHandler, SPI1_IRQHandler, SPI2_IRQHandler,
+    USART1_IRQHandler, USART2_IRQHandler, USART3_IRQHandler,
+    EXTI15_10_IRQHandler, RTC_Alarm_IRQHandler, OTG_FS_WKUP_IRQHandler,
+    TIM8_BRK_TIM12_IRQHandler, TIM8_UP_TIM13_IRQHandler,
+    TIM8_TRG_COM_TIM14_IRQHandler, TIM8_CC_IRQHandler, DMA1_Stream7_IRQHandler,
+    FMC_IRQHandler, SDIO_IRQHandler, TIM5_IRQHandler, SPI3_IRQHandler,
+    UART4_IRQHandler, UART5_IRQHandler, TIM6_DAC_IRQHandler, TIM7_IRQHandler,
+    DMA2_Stream0_IRQHandler, DMA2_Stream1_IRQHandler, DMA2_Stream2_IRQHandler,
+    DMA2_Stream3_IRQHandler, DMA2_Stream4_IRQHandler, ETH_IRQHandler,
+    ETH_WKUP_IRQHandler, CAN2_TX_IRQHandler, CAN2_RX0_IRQHandler,
+    CAN2_RX1_IRQHandler, CAN2_SCE_IRQHandler, OTG_FS_IRQHandler,
+    DMA2_Stream5_IRQHandler, DMA2_Stream6_IRQHandler, DMA2_Stream7_IRQHandler,
+    USART6_IRQHandler, I2C3_EV_IRQHandler, I2C3_ER_IRQHandler,
+    OTG_HS_EP1_OUT_IRQHandler, OTG_HS_EP1_IN_IRQHandler, OTG_HS_WKUP_IRQHandler,
+    OTG_HS_IRQHandler, DCMI_IRQHandler, CRYP_IRQHandler, HASH_RNG_IRQHandler,
+    FPU_IRQHandler, UART7_IRQHandler, UART8_IRQHandler, SPI4_IRQHandler,
+    SPI5_IRQHandler, SPI6_IRQHandler, SAI1_IRQHandler, LTDC_IRQHandler,
+    LTDC_ER_IRQHandler, DMA2D_IRQHandler};
+
+#pragma weak SysTick_IRQHandler            = Default_Handler
+#pragma weak WWDG_IRQHandler               = Default_Handler
+#pragma weak PVD_IRQHandler                = Default_Handler
+#pragma weak TAMP_STAMP_IRQHandler         = Default_Handler
+#pragma weak RTC_WKUP_IRQHandler           = Default_Handler
+#pragma weak FLASH_IRQHandler              = Default_Handler
+#pragma weak RCC_IRQHandler                = Default_Handler
+#pragma weak EXTI0_IRQHandler              = Default_Handler
+#pragma weak EXTI1_IRQHandler              = Default_Handler
+#pragma weak EXTI2_IRQHandler              = Default_Handler
+#pragma weak EXTI3_IRQHandler              = Default_Handler
+#pragma weak EXTI4_IRQHandler              = Default_Handler
+#pragma weak DMA1_Stream0_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream1_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream2_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream3_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream4_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream5_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream6_IRQHandler       = Default_Handler
+#pragma weak ADC_IRQHandler                = Default_Handler
+#pragma weak CAN1_TX_IRQHandler            = Default_Handler
+#pragma weak CAN1_RX0_IRQHandler           = Default_Handler
+#pragma weak CAN1_RX1_IRQHandler           = Default_Handler
+#pragma weak CAN1_SCE_IRQHandler           = Default_Handler
+#pragma weak EXTI9_5_IRQHandler            = Default_Handler
+#pragma weak TIM1_BRK_TIM9_IRQHandler      = Default_Handler
+#pragma weak TIM1_UP_TIM10_IRQHandler      = Default_Handler
+#pragma weak TIM1_TRG_COM_TIM11_IRQHandler = Default_Handler
+#pragma weak TIM1_CC_IRQHandler            = Default_Handler
+#pragma weak TIM2_IRQHandler               = Default_Handler
+#pragma weak TIM3_IRQHandler               = Default_Handler
+#pragma weak TIM4_IRQHandler               = Default_Handler
+#pragma weak I2C1_EV_IRQHandler            = Default_Handler
+#pragma weak I2C1_ER_IRQHandler            = Default_Handler
+#pragma weak I2C2_EV_IRQHandler            = Default_Handler
+#pragma weak I2C2_ER_IRQHandler            = Default_Handler
+#pragma weak SPI1_IRQHandler               = Default_Handler
+#pragma weak SPI2_IRQHandler               = Default_Handler
+// #pragma weak USART1_IRQHandler             = Default_Handler
+// #pragma weak USART2_IRQHandler             = Default_Handler
+// #pragma weak USART3_IRQHandler             = Default_Handler
+#pragma weak EXTI15_10_IRQHandler          = Default_Handler
+#pragma weak RTC_Alarm_IRQHandler          = Default_Handler
+#pragma weak OTG_FS_WKUP_IRQHandler        = Default_Handler
+#pragma weak TIM8_BRK_TIM12_IRQHandler     = Default_Handler
+#pragma weak TIM8_UP_TIM13_IRQHandler      = Default_Handler
+#pragma weak TIM8_TRG_COM_TIM14_IRQHandler = Default_Handler
+#pragma weak TIM8_CC_IRQHandler            = Default_Handler
+#pragma weak DMA1_Stream7_IRQHandler       = Default_Handler
+#pragma weak FMC_IRQHandler                = Default_Handler
+#pragma weak SDIO_IRQHandler               = Default_Handler
+#pragma weak TIM5_IRQHandler               = Default_Handler
+#pragma weak SPI3_IRQHandler               = Default_Handler
+// #pragma weak UART4_IRQHandler              = Default_Handler
+// #pragma weak UART5_IRQHandler              = Default_Handler
+#pragma weak TIM6_DAC_IRQHandler     = Default_Handler
+#pragma weak TIM7_IRQHandler         = Default_Handler
+#pragma weak DMA2_Stream0_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream1_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream2_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream3_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream4_IRQHandler = Default_Handler
+#pragma weak ETH_IRQHandler          = Default_Handler
+#pragma weak ETH_WKUP_IRQHandler     = Default_Handler
+#pragma weak CAN2_TX_IRQHandler      = Default_Handler
+#pragma weak CAN2_RX0_IRQHandler     = Default_Handler
+#pragma weak CAN2_RX1_IRQHandler     = Default_Handler
+#pragma weak CAN2_SCE_IRQHandler     = Default_Handler
+#pragma weak OTG_FS_IRQHandler       = Default_Handler
+#pragma weak DMA2_Stream5_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream6_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream7_IRQHandler = Default_Handler
+// #pragma weak USART6_IRQHandler             = Default_Handler
+#pragma weak I2C3_EV_IRQHandler        = Default_Handler
+#pragma weak I2C3_ER_IRQHandler        = Default_Handler
+#pragma weak OTG_HS_EP1_OUT_IRQHandler = Default_Handler
+#pragma weak OTG_HS_EP1_IN_IRQHandler  = Default_Handler
+#pragma weak OTG_HS_WKUP_IRQHandler    = Default_Handler
+#pragma weak OTG_HS_IRQHandler         = Default_Handler
+#pragma weak DCMI_IRQHandler           = Default_Handler
+#pragma weak CRYP_IRQHandler           = Default_Handler
+#pragma weak HASH_RNG_IRQHandler       = Default_Handler
+#pragma weak FPU_IRQHandler            = Default_Handler
+// #pragma weak UART7_IRQHandler              = Default_Handler
+// #pragma weak UART8_IRQHandler              = Default_Handler
+#pragma weak SPI4_IRQHandler    = Default_Handler
+#pragma weak SPI5_IRQHandler    = Default_Handler
+#pragma weak SPI6_IRQHandler    = Default_Handler
+#pragma weak SAI1_IRQHandler    = Default_Handler
+#pragma weak LTDC_IRQHandler    = Default_Handler
+#pragma weak LTDC_ER_IRQHandler = Default_Handler
+#pragma weak DMA2D_IRQHandler   = Default_Handler
diff --git a/src/bsps/stm32f429zi_nokia/interfaces-impl/arch_registers_impl.h b/src/bsps/stm32f429zi_nokia/interfaces-impl/arch_registers_impl.h
new file mode 100644
index 000000000..87f3cf661
--- /dev/null
+++ b/src/bsps/stm32f429zi_nokia/interfaces-impl/arch_registers_impl.h
@@ -0,0 +1,34 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * 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.
+ */
+
+#ifndef ARCH_REGISTERS_IMPL_H
+#define ARCH_REGISTERS_IMPL_H
+
+// Always include stm32f4xx.h before core_cm4.h, there's some nasty dependency
+#define STM32F429xx
+#include "CMSIS/Device/ST/STM32F4xx/Include/stm32f4xx.h"
+#include "CMSIS/Device/ST/STM32F4xx/Include/system_stm32f4xx.h"
+#include "CMSIS/Include/core_cm4.h"
+
+#define RCC_SYNC() __DSB()  // Workaround for a bug in stm32f42x
+
+#endif  // ARCH_REGISTERS_IMPL_H
diff --git a/src/bsps/stm32f429zi_nokia/interfaces-impl/bsp.cpp b/src/bsps/stm32f429zi_nokia/interfaces-impl/bsp.cpp
new file mode 100644
index 000000000..f79a64ec5
--- /dev/null
+++ b/src/bsps/stm32f429zi_nokia/interfaces-impl/bsp.cpp
@@ -0,0 +1,308 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * 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.
+ */
+
+/***********************************************************************
+ * bsp.cpp Part of the Miosix Embedded OS.
+ * Board support package, this file initializes hardware.
+ ************************************************************************/
+
+#include "interfaces/bsp.h"
+
+#include <inttypes.h>
+#include <sys/ioctl.h>
+
+#include <cstdlib>
+
+#include "board_settings.h"
+#include "config/miosix_settings.h"
+#include "drivers/sd_stm32f2_f4_f7.h"
+#include "drivers/serial.h"
+#include "filesystem/console/console_device.h"
+#include "filesystem/file_access.h"
+#include "hwmapping.h"
+#include "interfaces/arch_registers.h"
+#include "interfaces/delays.h"
+#include "interfaces/portability.h"
+#include "kernel/kernel.h"
+#include "kernel/logging.h"
+#include "kernel/sync.h"
+// #include "kernel/IRQDisplayPrint.h"
+
+namespace miosix
+{
+
+//
+// Initialization
+//
+
+/**
+ * The example code from ST checks for the busy flag after each command.
+ * Interestingly I couldn't find any mention of this in the datsheet.
+ */
+static void sdramCommandWait()
+{
+    for (int i = 0; i < 0xffff; i++)
+        if ((FMC_Bank5_6->SDSR & FMC_SDSR_BUSY) == 0)
+            return;
+}
+
+void configureSdram()
+{
+    // Enable all gpios, passing clock
+    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN | RCC_AHB1ENR_GPIOBEN |
+                    RCC_AHB1ENR_GPIOCEN | RCC_AHB1ENR_GPIODEN |
+                    RCC_AHB1ENR_GPIOEEN | RCC_AHB1ENR_GPIOFEN |
+                    RCC_AHB1ENR_GPIOGEN | RCC_AHB1ENR_GPIOHEN;
+    RCC_SYNC();
+
+    // First, configure SDRAM GPIOs
+    GPIOB->AFR[0] = 0x0cc00000;
+    GPIOC->AFR[0] = 0x0000000c;
+    GPIOD->AFR[0] = 0x000000cc;
+    GPIOD->AFR[1] = 0xcc000ccc;
+    GPIOE->AFR[0] = 0xc00000cc;
+    GPIOE->AFR[1] = 0xcccccccc;
+    GPIOF->AFR[0] = 0x00cccccc;
+    GPIOF->AFR[1] = 0xccccc000;
+    GPIOG->AFR[0] = 0x00cc00cc;
+    GPIOG->AFR[1] = 0xc000000c;
+
+    GPIOB->MODER = 0x00002800;
+    GPIOC->MODER = 0x00000002;
+    GPIOD->MODER = 0xa02a000a;
+    GPIOE->MODER = 0xaaaa800a;
+    GPIOF->MODER = 0xaa800aaa;
+    GPIOG->MODER = 0x80020a0a;
+
+    GPIOA->OSPEEDR = 0xaaaaaaaa;  // Default to 50MHz speed for all GPIOs...
+    GPIOB->OSPEEDR =
+        0xaaaaaaaa | 0x00003c00;  //...but 100MHz for the SDRAM pins
+    GPIOC->OSPEEDR = 0xaaaaaaaa | 0x00000003;
+    GPIOD->OSPEEDR = 0xaaaaaaaa | 0xf03f000f;
+    GPIOE->OSPEEDR = 0xaaaaaaaa | 0xffffc00f;
+    GPIOF->OSPEEDR = 0xaaaaaaaa | 0xffc00fff;
+    GPIOG->OSPEEDR = 0xaaaaaaaa | 0xc0030f0f;
+    GPIOH->OSPEEDR = 0xaaaaaaaa;
+
+    // Since we'we un-configured PB3/PB4 from the default at boot TDO,NTRST,
+    // finish the job and remove the default pull-up
+    GPIOB->PUPDR = 0;
+
+    // Second, actually start the SDRAM controller
+    RCC->AHB3ENR |= RCC_AHB3ENR_FMCEN;
+    RCC_SYNC();
+
+    // SDRAM is a IS42S16400J -7 speed grade, connected to bank 2 (0xd0000000)
+    // Some bits in SDCR[1] are don't care, and the have to be set in SDCR[0],
+    // they aren't just don't care, the controller will fail if they aren't at 0
+    FMC_Bank5_6->SDCR[0] = FMC_SDCR1_SDCLK_1  // SDRAM runs @ half CPU frequency
+                           | FMC_SDCR1_RBURST  // Enable read burst
+                           | 0;              //  0 delay between reads after CAS
+    FMC_Bank5_6->SDCR[1] = 0                 //  8 bit column address
+                           | FMC_SDCR1_NR_0  // 12 bit row address
+                           | FMC_SDCR1_MWID_0  // 16 bit data bus
+                           | FMC_SDCR1_NB      //  4 banks
+                           |
+                           FMC_SDCR1_CAS_1;  //  2 cycle CAS latency (F<133MHz)
+
+#ifdef SYSCLK_FREQ_180MHz
+    // One SDRAM clock cycle is 11.1ns
+    // Some bits in SDTR[1] are don't care, and the have to be set in SDTR[0],
+    // they aren't just don't care, the controller will fail if they aren't at 0
+    FMC_Bank5_6->SDTR[0] = (6 - 1) << 12     // 6 cycle TRC  (66.6ns>63ns)
+                           | (2 - 1) << 20;  // 2 cycle TRP  (22.2ns>15ns)
+    FMC_Bank5_6->SDTR[1] = (2 - 1) << 0      // 2 cycle TMRD
+                           | (7 - 1) << 4    // 7 cycle TXSR (77.7ns>70ns)
+                           | (4 - 1) << 8    // 4 cycle TRAS (44.4ns>42ns)
+                           | (2 - 1) << 16   // 2 cycle TWR
+                           | (2 - 1) << 24;  // 2 cycle TRCD (22.2ns>15ns)
+#elif defined(SYSCLK_FREQ_168MHz)
+    // One SDRAM clock cycle is 11.9ns
+    // Some bits in SDTR[1] are don't care, and the have to be set in SDTR[0],
+    // they aren't just don't care, the controller will fail if they aren't at 0
+    FMC_Bank5_6->SDTR[0] = (6 - 1) << 12     // 6 cycle TRC  (71.4ns>63ns)
+                           | (2 - 1) << 20;  // 2 cycle TRP  (23.8ns>15ns)
+    FMC_Bank5_6->SDTR[1] = (2 - 1) << 0      // 2 cycle TMRD
+                           | (6 - 1) << 4    // 6 cycle TXSR (71.4ns>70ns)
+                           | (4 - 1) << 8    // 4 cycle TRAS (47.6ns>42ns)
+                           | (2 - 1) << 16   // 2 cycle TWR
+                           | (2 - 1) << 24;  // 2 cycle TRCD (23.8ns>15ns)
+#else
+#error No SDRAM timings for this clock
+#endif
+
+    FMC_Bank5_6->SDCMR = FMC_SDCMR_CTB2  // Enable bank 2
+                         | 1;            // MODE=001 clock enabled
+    sdramCommandWait();
+
+    // ST and SDRAM datasheet agree a 100us delay is required here.
+    delayUs(100);
+
+    FMC_Bank5_6->SDCMR = FMC_SDCMR_CTB2  // Enable bank 2
+                         | 2;            // MODE=010 precharge all command
+    sdramCommandWait();
+
+    FMC_Bank5_6->SDCMR = (8 - 1) << 5      // NRFS=8 SDRAM datasheet says
+                                           // "at least two AUTO REFRESH cycles"
+                         | FMC_SDCMR_CTB2  // Enable bank 2
+                         | 3;              // MODE=011 auto refresh
+    sdramCommandWait();
+
+    FMC_Bank5_6->SDCMR = 0x220 << 9  // MRD=0x220:CAS latency=2 burst len=1
+                         | FMC_SDCMR_CTB2  // Enable bank 2
+                         | 4;              // MODE=100 load mode register
+    sdramCommandWait();
+
+// 64ms/4096=15.625us
+#ifdef SYSCLK_FREQ_180MHz
+    // 15.625us*90MHz=1406-20=1386
+    FMC_Bank5_6->SDRTR = 1386 << 1;
+#elif defined(SYSCLK_FREQ_168MHz)
+    // 15.625us*84MHz=1312-20=1292
+    FMC_Bank5_6->SDRTR = 1292 << 1;
+#else
+#error No refresh timings for this clock
+#endif
+}
+
+// static IRQDisplayPrint *irq_display;
+void IRQbspInit()
+{
+// If using SDRAM GPIOs are enabled by configureSdram(), else enable them here
+#ifndef __ENABLE_XRAM
+    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN | RCC_AHB1ENR_GPIOBEN |
+                    RCC_AHB1ENR_GPIOCEN | RCC_AHB1ENR_GPIODEN |
+                    RCC_AHB1ENR_GPIOEEN | RCC_AHB1ENR_GPIOFEN |
+                    RCC_AHB1ENR_GPIOGEN | RCC_AHB1ENR_GPIOHEN;
+    RCC_SYNC();
+#endif  //__ENABLE_XRAM
+
+    // enable spi1, spi4
+    RCC->APB2ENR |= RCC_APB2ENR_SPI1EN | RCC_APB2ENR_SPI4EN;
+
+    RCC_SYNC();
+
+    interfaces::spi1::sck::mode(Mode::ALTERNATE);
+    interfaces::spi1::sck::alternateFunction(5);
+    interfaces::spi1::miso::mode(Mode::ALTERNATE);
+    interfaces::spi1::miso::alternateFunction(5);
+    interfaces::spi1::mosi::mode(Mode::ALTERNATE);
+    interfaces::spi1::mosi::alternateFunction(5);
+
+    interfaces::spi4::sck::mode(Mode::ALTERNATE);
+    interfaces::spi4::sck::alternateFunction(5);
+    interfaces::spi4::miso::mode(Mode::ALTERNATE);
+    interfaces::spi4::miso::alternateFunction(5);
+    interfaces::spi4::mosi::mode(Mode::ALTERNATE);
+    interfaces::spi4::mosi::alternateFunction(5);
+
+    peripherals::ra01::pc13::cs::mode(Mode::OUTPUT);
+    peripherals::ra01::pc13::dio0::mode(Mode::INPUT);
+    peripherals::ra01::pc13::nrst::mode(Mode::OUTPUT);
+    peripherals::ra01::pc13::cs::high();
+    peripherals::ra01::pc13::nrst::high();
+
+    peripherals::ra01::pe4::cs::mode(Mode::OUTPUT);
+    peripherals::ra01::pe4::dio0::mode(Mode::INPUT);
+    peripherals::ra01::pe4::nrst::mode(Mode::OUTPUT);
+    peripherals::ra01::pe4::cs::high();
+    peripherals::ra01::pe4::nrst::high();
+
+    peripherals::cc3135::cs::mode(Mode::OUTPUT);
+    peripherals::cc3135::hib::mode(Mode::OUTPUT);
+    peripherals::cc3135::intr::mode(Mode::INPUT);
+    peripherals::cc3135::nrst::mode(Mode::OUTPUT);
+    peripherals::cc3135::cs::high();
+    peripherals::cc3135::hib::high();
+    peripherals::cc3135::nrst::high();
+
+    _led::mode(Mode::OUTPUT);
+    ledOn();
+    delayMs(100);
+    ledOff();
+    DefaultConsole::instance().IRQset(intrusive_ref_ptr<Device>(
+        new STM32Serial(defaultSerial, defaultSerialSpeed,
+                        defaultSerialFlowctrl ? STM32Serial::RTSCTS
+                                              : STM32Serial::NOFLOWCTRL)));
+    //     irq_display = new IRQDisplayPrint();
+    //     DefaultConsole::instance().IRQset(intrusive_ref_ptr<Device>(irq_display));
+}
+
+// void* printIRQ(void *argv)
+// {
+// 	intrusive_ref_ptr<IRQDisplayPrint> irqq(irq_display);
+// 	irqq.get()->printIRQ();
+// 	return NULL;
+// }
+
+void bspInit2()
+{
+#ifdef WITH_FILESYSTEM
+    basicFilesystemSetup(SDIODriver::instance());
+#endif  // WITH_FILESYSTEM
+    //     Thread::create(printIRQ, 2048);
+}
+
+//
+// Shutdown and reboot
+//
+
+/**
+This function disables filesystem (if enabled), serial port (if enabled) and
+puts the processor in deep sleep mode.<br>
+Wakeup occurs when PA.0 goes high, but instead of sleep(), a new boot happens.
+<br>This function does not return.<br>
+WARNING: close all files before using this function, since it unmounts the
+filesystem.<br>
+When in shutdown mode, power consumption of the miosix board is reduced to ~
+5uA??, however, true power consumption depends on what is connected to the GPIO
+pins. The user is responsible to put the devices connected to the GPIO pin in
+the minimal power consumption mode before calling shutdown(). Please note that
+to minimize power consumption all unused GPIO must not be left floating.
+*/
+void shutdown()
+{
+    ioctl(STDOUT_FILENO, IOCTL_SYNC, 0);
+
+#ifdef WITH_FILESYSTEM
+    FilesystemManager::instance().umountAll();
+#endif  // WITH_FILESYSTEM
+
+    disableInterrupts();
+
+    for (;;)
+        ;
+}
+
+void reboot()
+{
+    ioctl(STDOUT_FILENO, IOCTL_SYNC, 0);
+
+#ifdef WITH_FILESYSTEM
+    FilesystemManager::instance().umountAll();
+#endif  // WITH_FILESYSTEM
+
+    disableInterrupts();
+    miosix_private::IRQsystemReboot();
+}
+
+}  // namespace miosix
diff --git a/src/bsps/stm32f429zi_nokia/interfaces-impl/bsp_impl.h b/src/bsps/stm32f429zi_nokia/interfaces-impl/bsp_impl.h
new file mode 100644
index 000000000..6b5cc0c6a
--- /dev/null
+++ b/src/bsps/stm32f429zi_nokia/interfaces-impl/bsp_impl.h
@@ -0,0 +1,66 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * 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.
+ */
+
+/***********************************************************************
+ * bsp_impl.h Part of the Miosix Embedded OS.
+ * Board support package, this file initializes hardware.
+ ************************************************************************/
+
+#ifndef BSP_IMPL_H
+#define BSP_IMPL_H
+
+#include "config/miosix_settings.h"
+#include "drivers/stm32_hardware_rng.h"
+#include "interfaces/gpio.h"
+
+namespace miosix
+{
+
+/**
+\addtogroup Hardware
+\{
+*/
+
+/**
+ * \internal
+ * Called by stage_1_boot.cpp to enable the SDRAM before initializing .data/.bss
+ * Requires the CPU clock to be already configured (running from the PLL)
+ */
+void configureSdram();
+
+/**
+ * \internal
+ * used by the ledOn() and ledOff() implementation
+ */
+typedef Gpio<GPIOG_BASE, 14> _led;
+
+inline void ledOn() { _led::high(); }
+
+inline void ledOff() { _led::low(); }
+
+/**
+\}
+*/
+
+}  // namespace miosix
+
+#endif  // BSP_IMPL_H
diff --git a/src/bsps/stm32f429zi_nokia/interfaces-impl/hwmapping.h b/src/bsps/stm32f429zi_nokia/interfaces-impl/hwmapping.h
new file mode 100644
index 000000000..0e79ea7f1
--- /dev/null
+++ b/src/bsps/stm32f429zi_nokia/interfaces-impl/hwmapping.h
@@ -0,0 +1,88 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * 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.
+ */
+
+#ifndef HWMAPPING_H
+#define HWMAPPING_H
+
+#include "interfaces/gpio.h"
+
+namespace miosix
+{
+
+namespace interfaces
+{
+
+// CC3135
+namespace spi1
+{
+using sck  = Gpio<GPIOA_BASE, 5>;
+using miso = Gpio<GPIOB_BASE, 4>;
+using mosi = Gpio<GPIOA_BASE, 7>;
+}  // namespace spi1
+
+// RA-01
+namespace spi4
+{
+using sck  = Gpio<GPIOE_BASE, 2>;
+using miso = Gpio<GPIOE_BASE, 5>;
+using mosi = Gpio<GPIOE_BASE, 6>;
+}  // namespace spi4
+
+}  // namespace interfaces
+
+namespace peripherals
+{
+
+namespace ra01
+{
+
+namespace pc13
+{
+using cs   = Gpio<GPIOC_BASE, 13>;
+using dio0 = Gpio<GPIOF_BASE, 6>;
+using dio1 = Gpio<GPIOA_BASE, 4>;
+using dio3 = Gpio<GPIOC_BASE, 11>;
+using nrst = Gpio<GPIOC_BASE, 14>;
+}  // namespace pc13
+
+namespace pe4
+{
+using cs   = Gpio<GPIOE_BASE, 4>;
+using dio0 = Gpio<GPIOE_BASE, 3>;
+using nrst = Gpio<GPIOG_BASE, 2>;
+}  // namespace pe4
+
+}  // namespace ra01
+
+namespace cc3135
+{
+using cs   = Gpio<GPIOD_BASE, 4>;
+using hib  = Gpio<GPIOG_BASE, 3>;
+using intr = Gpio<GPIOD_BASE, 5>;
+using nrst = Gpio<GPIOB_BASE, 7>;
+}  // namespace cc3135
+
+}  // namespace peripherals
+
+}  // namespace miosix
+
+#endif  // HWMAPPING_H
diff --git a/src/bsps/stm32f429zi_nokia/stm32_2m+256k_rom.ld b/src/bsps/stm32f429zi_nokia/stm32_2m+256k_rom.ld
new file mode 100644
index 000000000..e2425d265
--- /dev/null
+++ b/src/bsps/stm32f429zi_nokia/stm32_2m+256k_rom.ld
@@ -0,0 +1,180 @@
+/*
+ * C++ enabled linker script for stm32 (2M FLASH, 256K RAM)
+ * Developed by TFT: Terraneo Federico Technologies
+ * Optimized for use with the Miosix kernel
+ */
+
+/*
+ * This chip has an unusual quirk that the RAM is divided in two block mapped
+ * at two non contiguous memory addresses. I don't know why they've done that,
+ * probably doing the obvious thing would have made writing code too easy...
+ * Anyway, since hardware can't be changed, we've got to live with that and
+ * try to make use of both RAMs.
+ * 
+ * Given the constraints above, this linker script puts:
+ * - read only data and code (.text, .rodata, .eh_*) in FLASH
+ * - the 512Byte main (IRQ) stack, .data and .bss in the "small" 64KB RAM
+ * - stacks and heap in the "large" 192KB RAM.
+ * 
+ * Unfortunately thread stacks can't be put in the small RAM as Miosix
+ * allocates them inside the heap.
+ */
+
+/*
+ * The main stack is used for interrupt handling by the kernel.
+ *
+ * *** Readme ***
+ * This linker script places the main stack (used by the kernel for interrupts)
+ * at the bottom of the ram, instead of the top. This is done for two reasons:
+ *
+ * - as an optimization for microcontrollers with little ram memory. In fact
+ *   the implementation of malloc from newlib requests memory to the OS in 4KB
+ *   block (except the first block that can be smaller). This is probably done
+ *   for compatibility with OSes with an MMU and paged memory. To see why this
+ *   is bad, consider a microcontroller with 8KB of ram: when malloc finishes
+ *   up the first 4KB it will call _sbrk_r asking for a 4KB block, but this will
+ *   fail because the top part of the ram is used by the main stack. As a
+ *   result, the top part of the memory will not be used by malloc, even if
+ *   available (and it is nearly *half* the ram on an 8KB mcu). By placing the
+ *   main stack at the bottom of the ram, the upper 4KB block will be entirely
+ *   free and available as heap space.
+ *
+ * - In case of main stack overflow the cpu will fault because access to memory
+ *   before the beginning of the ram faults. Instead with the default stack
+ *   placement the main stack will silently collide with the heap.
+ * Note: if increasing the main stack size also increase the ORIGIN value in
+ * the MEMORY definitions below accordingly.
+ */
+_main_stack_size = 0x00000200;                     /* main stack = 512Bytes */
+_main_stack_top  = 0x10000000 + _main_stack_size;
+ASSERT(_main_stack_size   % 8 == 0, "MAIN stack size error");
+
+/* Mapping the heap into the large 192KB RAM */
+_end =      0x20000000;
+_heap_end = 0x20030000;                            /* end of available ram  */
+
+/* identify the Entry Point  */
+ENTRY(_Z13Reset_Handlerv)
+
+/* specify the memory areas  */
+MEMORY
+{
+    flash(rx)    : ORIGIN = 0x08000000, LENGTH =   2M
+    /*
+     * Note, the small ram starts at 0x10000000 but it is necessary to add the
+     * size of the main stack, so it is 0x10000200.
+     */
+    smallram(wx) : ORIGIN = 0x10000200, LENGTH =  64K-0x200 
+    largeram(wx) : ORIGIN = 0x20000000, LENGTH = 192K
+}
+
+/* now define the output sections  */
+SECTIONS
+{
+    . = 0;
+    
+    /* .text section: code goes to flash */
+    .text :
+    {
+        /* Startup code must go at address 0 */
+        KEEP(*(.isr_vector))
+        
+        *(.text)
+        *(.text.*)
+        *(.gnu.linkonce.t.*)
+        /* these sections for thumb interwork? */
+        *(.glue_7)
+        *(.glue_7t)
+        /* these sections for C++? */
+        *(.gcc_except_table)
+        *(.gcc_except_table.*)
+        *(.ARM.extab*)
+        *(.gnu.linkonce.armextab.*)
+
+        . = ALIGN(4);
+        /* .rodata: constant data */
+        *(.rodata)
+        *(.rodata.*)
+        *(.gnu.linkonce.r.*)
+
+        /* C++ Static constructors/destructors (eabi) */
+        . = ALIGN(4);
+        KEEP(*(.init))
+        
+        . = ALIGN(4);
+        __miosix_init_array_start = .;
+        KEEP (*(SORT(.miosix_init_array.*)))
+        KEEP (*(.miosix_init_array))
+        __miosix_init_array_end = .;
+
+        . = ALIGN(4);
+        __preinit_array_start = .;
+        KEEP (*(.preinit_array))
+        __preinit_array_end = .;
+
+        . = ALIGN(4);
+        __init_array_start = .;
+        KEEP (*(SORT(.init_array.*)))
+        KEEP (*(.init_array))
+        __init_array_end = .;
+
+        . = ALIGN(4);
+        KEEP(*(.fini))
+
+        . = ALIGN(4);
+        __fini_array_start = .;
+        KEEP (*(.fini_array))
+        KEEP (*(SORT(.fini_array.*)))
+        __fini_array_end = .;
+
+        /* C++ Static constructors/destructors (elf)  */
+        . = ALIGN(4);
+        _ctor_start = .;
+        KEEP (*crtbegin.o(.ctors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .ctors))
+        KEEP (*(SORT(.ctors.*)))
+        KEEP (*crtend.o(.ctors))
+       _ctor_end = .;
+
+        . = ALIGN(4);
+        KEEP (*crtbegin.o(.dtors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .dtors))
+        KEEP (*(SORT(.dtors.*)))
+        KEEP (*crtend.o(.dtors))
+    } > flash
+
+    /* .ARM.exidx is sorted, so has to go in its own output section.  */
+    __exidx_start = .;
+    .ARM.exidx :
+    {
+        *(.ARM.exidx* .gnu.linkonce.armexidx.*)
+    } > flash
+    __exidx_end = .;
+
+	/* .data section: global variables go to ram, but also store a copy to
+       flash to initialize them */
+    .data : ALIGN(8)
+    {
+        _data = .;
+        *(.data)
+        *(.data.*)
+        *(.gnu.linkonce.d.*)
+        . = ALIGN(8);
+        _edata = .;
+    } > smallram AT > flash
+    _etext = LOADADDR(.data);
+
+    /* .bss section: uninitialized global variables go to ram */
+    _bss_start = .;
+    .bss :
+    {
+        *(.bss)
+        *(.bss.*)
+        *(.gnu.linkonce.b.*)
+        . = ALIGN(8);
+    } > smallram
+    _bss_end = .;
+
+    /*_end = .;*/
+    /*PROVIDE(end = .);*/
+}
diff --git a/src/bsps/stm32f429zi_nokia/stm32_2m+6m_xram.ld b/src/bsps/stm32f429zi_nokia/stm32_2m+6m_xram.ld
new file mode 100644
index 000000000..b61f04cea
--- /dev/null
+++ b/src/bsps/stm32f429zi_nokia/stm32_2m+6m_xram.ld
@@ -0,0 +1,183 @@
+/*
+ * C++ enabled linker script for stm32 (2M FLASH, 256K RAM, 8M XRAM)
+ * Developed by TFT: Terraneo Federico Technologies
+ * Optimized for use with the Miosix kernel
+ */
+
+/*
+ * This chip has an unusual quirk that the RAM is divided in two block mapped
+ * at two non contiguous memory addresses. I don't know why they've done that,
+ * probably doing the obvious thing would have made writing code too easy...
+ * Anyway, since hardware can't be changed, we've got to live with that and
+ * try to make use of both RAMs.
+ * 
+ * Given the constraints above, this linker script puts:
+ * - read only data and code (.text, .rodata, .eh_*) in FLASH
+ * - the 512Byte main (IRQ) stack, .data and .bss in the "small" 64KB RAM
+ * - .data, .bss, stacks and heap in the external 6MB SDRAM.
+ * Note that the SDRAM is 8MB, but this linker script only uses the lower 6.
+ * The upper 2MB are reserved as framebuffers for the LCD. Notice that the
+ * choice to allocate this space is due to the SDRAM architecture, composed of
+ * 4 banks of 2MB each. Reserving a bank for the LCD allows the software
+ * running on the board and the LCD to operate (almost) independently.
+ */
+
+/*
+ * The main stack is used for interrupt handling by the kernel.
+ *
+ * *** Readme ***
+ * This linker script places the main stack (used by the kernel for interrupts)
+ * at the bottom of the ram, instead of the top. This is done for two reasons:
+ *
+ * - as an optimization for microcontrollers with little ram memory. In fact
+ *   the implementation of malloc from newlib requests memory to the OS in 4KB
+ *   block (except the first block that can be smaller). This is probably done
+ *   for compatibility with OSes with an MMU and paged memory. To see why this
+ *   is bad, consider a microcontroller with 8KB of ram: when malloc finishes
+ *   up the first 4KB it will call _sbrk_r asking for a 4KB block, but this will
+ *   fail because the top part of the ram is used by the main stack. As a
+ *   result, the top part of the memory will not be used by malloc, even if
+ *   available (and it is nearly *half* the ram on an 8KB mcu). By placing the
+ *   main stack at the bottom of the ram, the upper 4KB block will be entirely
+ *   free and available as heap space.
+ *
+ * - In case of main stack overflow the cpu will fault because access to memory
+ *   before the beginning of the ram faults. Instead with the default stack
+ *   placement the main stack will silently collide with the heap.
+ * Note: if increasing the main stack size also increase the ORIGIN value in
+ * the MEMORY definitions below accordingly.
+ */
+_main_stack_size = 0x00000200;                     /* main stack = 512Bytes */
+_main_stack_top  = 0x10000000 + _main_stack_size;
+ASSERT(_main_stack_size   % 8 == 0, "MAIN stack size error");
+
+/* Mapping the heap into XRAM */
+_heap_end = 0xd0600000;                            /* end of available ram  */
+
+/* identify the Entry Point  */
+ENTRY(_Z13Reset_Handlerv)
+
+/* specify the memory areas  */
+MEMORY
+{
+    flash(rx)    : ORIGIN = 0x08000000, LENGTH =   2M
+    /*
+     * Note, the small ram starts at 0x10000000 but it is necessary to add the
+     * size of the main stack, so it is 0x10000200.
+     */
+    smallram(wx) : ORIGIN = 0x10000200, LENGTH =  64K-0x200 
+    largeram(wx) : ORIGIN = 0x20000000, LENGTH = 192K
+    xram(wx)     : ORIGIN = 0xd0000000, LENGTH =   6M
+    lcdram(wx)   : ORIGIN = 0xd0600000, LENGTH =   2M
+}
+
+/* now define the output sections  */
+SECTIONS
+{
+    . = 0;
+    
+    /* .text section: code goes to flash */
+    .text :
+    {
+        /* Startup code must go at address 0 */
+        KEEP(*(.isr_vector))
+        
+        *(.text)
+        *(.text.*)
+        *(.gnu.linkonce.t.*)
+        /* these sections for thumb interwork? */
+        *(.glue_7)
+        *(.glue_7t)
+        /* these sections for C++? */
+        *(.gcc_except_table)
+        *(.gcc_except_table.*)
+        *(.ARM.extab*)
+        *(.gnu.linkonce.armextab.*)
+
+        . = ALIGN(4);
+        /* .rodata: constant data */
+        *(.rodata)
+        *(.rodata.*)
+        *(.gnu.linkonce.r.*)
+
+        /* C++ Static constructors/destructors (eabi) */
+        . = ALIGN(4);
+        KEEP(*(.init))
+        
+        . = ALIGN(4);
+        __miosix_init_array_start = .;
+        KEEP (*(SORT(.miosix_init_array.*)))
+        KEEP (*(.miosix_init_array))
+        __miosix_init_array_end = .;
+
+        . = ALIGN(4);
+        __preinit_array_start = .;
+        KEEP (*(.preinit_array))
+        __preinit_array_end = .;
+
+        . = ALIGN(4);
+        __init_array_start = .;
+        KEEP (*(SORT(.init_array.*)))
+        KEEP (*(.init_array))
+        __init_array_end = .;
+
+        . = ALIGN(4);
+        KEEP(*(.fini))
+
+        . = ALIGN(4);
+        __fini_array_start = .;
+        KEEP (*(.fini_array))
+        KEEP (*(SORT(.fini_array.*)))
+        __fini_array_end = .;
+
+        /* C++ Static constructors/destructors (elf)  */
+        . = ALIGN(4);
+        _ctor_start = .;
+        KEEP (*crtbegin.o(.ctors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .ctors))
+        KEEP (*(SORT(.ctors.*)))
+        KEEP (*crtend.o(.ctors))
+       _ctor_end = .;
+
+        . = ALIGN(4);
+        KEEP (*crtbegin.o(.dtors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .dtors))
+        KEEP (*(SORT(.dtors.*)))
+        KEEP (*crtend.o(.dtors))
+    } > flash
+
+    /* .ARM.exidx is sorted, so has to go in its own output section.  */
+    __exidx_start = .;
+    .ARM.exidx :
+    {
+        *(.ARM.exidx* .gnu.linkonce.armexidx.*)
+    } > flash
+    __exidx_end = .;
+
+	/* .data section: global variables go to ram, but also store a copy to
+       flash to initialize them */
+    .data : ALIGN(8)
+    {
+        _data = .;
+        *(.data)
+        *(.data.*)
+        *(.gnu.linkonce.d.*)
+        . = ALIGN(8);
+        _edata = .;
+    } > xram AT > flash
+    _etext = LOADADDR(.data);
+
+    /* .bss section: uninitialized global variables go to ram */
+    _bss_start = .;
+    .bss :
+    {
+        *(.bss)
+        *(.bss.*)
+        *(.gnu.linkonce.b.*)
+        . = ALIGN(8);
+    } > xram
+    _bss_end = .;
+
+    _end = .;
+    PROVIDE(end = .);
+}
diff --git a/src/bsps/stm32f429zi_nokia/stm32_2m+8m_xram.ld b/src/bsps/stm32f429zi_nokia/stm32_2m+8m_xram.ld
new file mode 100644
index 000000000..78fe53104
--- /dev/null
+++ b/src/bsps/stm32f429zi_nokia/stm32_2m+8m_xram.ld
@@ -0,0 +1,177 @@
+/*
+ * C++ enabled linker script for stm32 (2M FLASH, 256K RAM, 8M XRAM)
+ * Developed by TFT: Terraneo Federico Technologies
+ * Optimized for use with the Miosix kernel
+ */
+
+/*
+ * This chip has an unusual quirk that the RAM is divided in two block mapped
+ * at two non contiguous memory addresses. I don't know why they've done that,
+ * probably doing the obvious thing would have made writing code too easy...
+ * Anyway, since hardware can't be changed, we've got to live with that and
+ * try to make use of both RAMs.
+ * 
+ * Given the constraints above, this linker script puts:
+ * - read only data and code (.text, .rodata, .eh_*) in FLASH
+ * - the 512Byte main (IRQ) stack, .data and .bss in the "small" 64KB RAM
+ * - .data, .bss, stacks and heap in the external 8MB SDRAM.
+ */
+
+/*
+ * The main stack is used for interrupt handling by the kernel.
+ *
+ * *** Readme ***
+ * This linker script places the main stack (used by the kernel for interrupts)
+ * at the bottom of the ram, instead of the top. This is done for two reasons:
+ *
+ * - as an optimization for microcontrollers with little ram memory. In fact
+ *   the implementation of malloc from newlib requests memory to the OS in 4KB
+ *   block (except the first block that can be smaller). This is probably done
+ *   for compatibility with OSes with an MMU and paged memory. To see why this
+ *   is bad, consider a microcontroller with 8KB of ram: when malloc finishes
+ *   up the first 4KB it will call _sbrk_r asking for a 4KB block, but this will
+ *   fail because the top part of the ram is used by the main stack. As a
+ *   result, the top part of the memory will not be used by malloc, even if
+ *   available (and it is nearly *half* the ram on an 8KB mcu). By placing the
+ *   main stack at the bottom of the ram, the upper 4KB block will be entirely
+ *   free and available as heap space.
+ *
+ * - In case of main stack overflow the cpu will fault because access to memory
+ *   before the beginning of the ram faults. Instead with the default stack
+ *   placement the main stack will silently collide with the heap.
+ * Note: if increasing the main stack size also increase the ORIGIN value in
+ * the MEMORY definitions below accordingly.
+ */
+_main_stack_size = 0x00000200;                     /* main stack = 512Bytes */
+_main_stack_top  = 0x10000000 + _main_stack_size;
+ASSERT(_main_stack_size   % 8 == 0, "MAIN stack size error");
+
+/* Mapping the heap into XRAM */
+_heap_end = 0xd0800000;                            /* end of available ram  */
+
+/* identify the Entry Point  */
+ENTRY(_Z13Reset_Handlerv)
+
+/* specify the memory areas  */
+MEMORY
+{
+    flash(rx)    : ORIGIN = 0x08000000, LENGTH =   2M
+    /*
+     * Note, the small ram starts at 0x10000000 but it is necessary to add the
+     * size of the main stack, so it is 0x10000200.
+     */
+    smallram(wx) : ORIGIN = 0x10000200, LENGTH =  64K-0x200 
+    largeram(wx) : ORIGIN = 0x20000000, LENGTH = 192K
+    xram(wx)     : ORIGIN = 0xd0000000, LENGTH =   8M
+}
+
+/* now define the output sections  */
+SECTIONS
+{
+    . = 0;
+    
+    /* .text section: code goes to flash */
+    .text :
+    {
+        /* Startup code must go at address 0 */
+        KEEP(*(.isr_vector))
+        
+        *(.text)
+        *(.text.*)
+        *(.gnu.linkonce.t.*)
+        /* these sections for thumb interwork? */
+        *(.glue_7)
+        *(.glue_7t)
+        /* these sections for C++? */
+        *(.gcc_except_table)
+        *(.gcc_except_table.*)
+        *(.ARM.extab*)
+        *(.gnu.linkonce.armextab.*)
+
+        . = ALIGN(4);
+        /* .rodata: constant data */
+        *(.rodata)
+        *(.rodata.*)
+        *(.gnu.linkonce.r.*)
+
+        /* C++ Static constructors/destructors (eabi) */
+        . = ALIGN(4);
+        KEEP(*(.init))
+        
+        . = ALIGN(4);
+        __miosix_init_array_start = .;
+        KEEP (*(SORT(.miosix_init_array.*)))
+        KEEP (*(.miosix_init_array))
+        __miosix_init_array_end = .;
+
+        . = ALIGN(4);
+        __preinit_array_start = .;
+        KEEP (*(.preinit_array))
+        __preinit_array_end = .;
+
+        . = ALIGN(4);
+        __init_array_start = .;
+        KEEP (*(SORT(.init_array.*)))
+        KEEP (*(.init_array))
+        __init_array_end = .;
+
+        . = ALIGN(4);
+        KEEP(*(.fini))
+
+        . = ALIGN(4);
+        __fini_array_start = .;
+        KEEP (*(.fini_array))
+        KEEP (*(SORT(.fini_array.*)))
+        __fini_array_end = .;
+
+        /* C++ Static constructors/destructors (elf)  */
+        . = ALIGN(4);
+        _ctor_start = .;
+        KEEP (*crtbegin.o(.ctors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .ctors))
+        KEEP (*(SORT(.ctors.*)))
+        KEEP (*crtend.o(.ctors))
+       _ctor_end = .;
+
+        . = ALIGN(4);
+        KEEP (*crtbegin.o(.dtors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .dtors))
+        KEEP (*(SORT(.dtors.*)))
+        KEEP (*crtend.o(.dtors))
+    } > flash
+
+    /* .ARM.exidx is sorted, so has to go in its own output section.  */
+    __exidx_start = .;
+    .ARM.exidx :
+    {
+        *(.ARM.exidx* .gnu.linkonce.armexidx.*)
+    } > flash
+    __exidx_end = .;
+
+	/* .data section: global variables go to ram, but also store a copy to
+       flash to initialize them */
+    .data : ALIGN(8)
+    {
+        _data = .;
+        *(.data)
+        *(.data.*)
+        *(.gnu.linkonce.d.*)
+        . = ALIGN(8);
+        _edata = .;
+    } > xram AT > flash
+    _etext = LOADADDR(.data);
+
+    /* .bss section: uninitialized global variables go to ram */
+    _bss_start = .;
+    .bss :
+    {
+        *(.bss)
+        *(.bss.*)
+        *(.gnu.linkonce.b.*)
+        . = ALIGN(8);
+    } > xram
+    _bss_end = .;
+
+    _end = .;
+    PROVIDE(end = .);
+}
diff --git a/src/bsps/stm32f429zi_parafoil/config/board_options.cmake b/src/bsps/stm32f429zi_parafoil/config/board_options.cmake
new file mode 100644
index 000000000..9e545b1cf
--- /dev/null
+++ b/src/bsps/stm32f429zi_parafoil/config/board_options.cmake
@@ -0,0 +1,120 @@
+# Copyright (C) 2023 by Skyward
+#
+# This program is free software; you can redistribute it and/or 
+# it under the terms of the GNU General Public License as published 
+# the Free Software Foundation; either version 2 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.
+#
+# As a special exception, if other files instantiate templates or use
+# macros or inline functions from this file, or you compile this file
+# and link it with other works to produce a work based on this file,
+# this file does not by itself cause the resulting work to be covered
+# by the GNU General Public License. However the source code for this
+# file must still be made available in accordance with the GNU 
+# Public License. This exception does not invalidate any other 
+# why a work based on this file might be covered by the GNU General
+# Public License.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, see <http://www.gnu.org/licenses/>
+
+set(BOARD_NAME stm32f429zi_parafoil)
+set(ARCH_NAME cortexM4_stm32f4)
+
+# Base directories with header files for this board
+set(ARCH_PATH ${KPATH}/arch/${ARCH_NAME}/common)
+set(BOARD_PATH ${BOARDCORE_PATH}/src/bsps/${BOARD_NAME})
+set(BOARD_CONFIG_PATH ${BOARDCORE_PATH}/src/bsps/${BOARD_NAME}/config)
+
+# Specify where to find the board specific config/miosix_settings.h
+set(BOARD_MIOSIX_SETTINGS_PATH ${BOARD_PATH})
+
+# Specify where to find the board specific config/mxgui_settings.h
+set(BOARD_MXGUI_SETTINGS_PATH ${BOARD_PATH})
+
+# Optimization flags:
+# -O0 do no optimization, the default if no optimization level is specified
+# -O or -O1 optimize minimally
+# -O2 optimize more
+# -O3 optimize even more
+# -Ofast optimize very aggressively to the point of breaking the standard
+# -Og Optimize debugging experience, enables optimizations that do not
+# interfere with debugging
+# -Os Optimize for size with -O2 optimizations that do not increase code size
+set(OPT_OPTIMIZATION -O2)
+
+# Boot and linker files
+set(BOOT_FILE ${BOARD_PATH}/core/stage_1_boot.cpp)
+
+# Linker script type, there are three options
+# 1) Code in FLASH, stack + heap in internal RAM (file *_rom.ld)
+#    the most common choice, available for all microcontrollers
+# 2) Code in FLASH, stack + heap in external RAM (file *8m_xram.ld)
+#    You must uncomment -D__ENABLE_XRAM below in this case.
+# set(LINKER_SCRIPT ${BOARD_PATH}/stm32_2m+256k_rom.ld)
+set(LINKER_SCRIPT ${BOARD_PATH}/stm32_2m+8m_xram.ld)
+
+# Uncommenting __ENABLE_XRAM enables the initialization of the external
+# 8MB SDRAM memory. Do not uncomment this even if you don't use a linker
+# script that requires it, as it is used for the LCD framebuffer.
+set(XRAM -D__ENABLE_XRAM)
+
+# Select clock frequency. Warning: the default clock frequency for
+# this board is 168MHz and not 180MHz because, due to a limitation in
+# the PLL, it is not possible to generate a precise 48MHz output when
+# running the core at 180MHz. If 180MHz is chosen the USB peripheral will
+# NOT WORK and the SDIO and RNG will run ~6% slower (45MHz insteand of 48)
+# Define USE_INTERNAL_CLOCK to bypass the external oscillator
+# set(CLOCK_FREQ -DHSE_VALUE=8000000 -DSYSCLK_FREQ_180MHz=180000000)
+set(CLOCK_FREQ -DHSE_VALUE=8000000 -DSYSCLK_FREQ_168MHz=168000000)
+# set(CLOCK_FREQ -DHSE_VALUE=8000000 -DSYSCLK_FREQ_100MHz=100000000)
+
+# C++ Exception/rtti support disable flags.
+# To save code size if not using C++ exceptions (nor some STL code which
+# implicitly uses it) uncomment this option.
+# -D__NO_EXCEPTIONS is used by Miosix to know if exceptions are used.
+# set(OPT_EXCEPT -fno-exceptions -fno-rtti -D__NO_EXCEPTIONS)
+
+# Specify a custom flash command
+# This is the program that is invoked when the flash flag (-f or --flash) is
+# used with the Miosix Build System. Use $binary or $hex as placeolders, they
+# will be replaced by the build systems with the binary or hex file repectively.
+# If a command is not specified, the build system will use st-flash if found
+# set(PROGRAM_CMDLINE "here your custom flash command")
+
+# Basic flags
+set(FLAGS_BASE -mcpu=cortex-m4 -mthumb -mfloat-abi=hard -mfpu=fpv4-sp-d16)
+
+# Flags for ASM and linker
+set(AFLAGS_BASE ${FLAGS_BASE})
+set(LFLAGS_BASE ${FLAGS_BASE} -Wl,--gc-sections,-Map,main.map -Wl,-T${LINKER_SCRIPT} ${OPT_EXCEPT} ${OPT_OPTIMIZATION} -nostdlib)
+
+# Flags for C/C++
+string(TOUPPER ${BOARD_NAME} BOARD_UPPER)
+set(CFLAGS_BASE
+    -D_BOARD_${BOARD_UPPER} -D_MIOSIX_BOARDNAME=\"${BOARD_NAME}\"
+    -D_DEFAULT_SOURCE=1 -ffunction-sections -Wall -Werror=return-type -g
+    -D_ARCH_CORTEXM4_STM32F4
+    ${CLOCK_FREQ} ${XRAM} ${SRAM_BOOT} ${FLAGS_BASE} ${OPT_OPTIMIZATION} -c
+)
+set(CXXFLAGS_BASE ${CFLAGS_BASE} ${OPT_EXCEPT})
+
+# Select architecture specific files
+set(ARCH_SRC
+    ${ARCH_PATH}/interfaces-impl/delays.cpp
+    ${ARCH_PATH}/interfaces-impl/gpio_impl.cpp
+    ${ARCH_PATH}/interfaces-impl/portability.cpp
+    ${BOARD_PATH}/interfaces-impl/bsp.cpp
+    ${KPATH}/arch/common/CMSIS/Device/ST/STM32F4xx/Source/Templates/system_stm32f4xx.c
+    ${KPATH}/arch/common/core/interrupts_cortexMx.cpp
+    ${KPATH}/arch/common/core/mpu_cortexMx.cpp
+    ${KPATH}/arch/common/core/stm32f2_f4_l4_f7_h7_os_timer.cpp
+    ${KPATH}/arch/common/drivers/sd_stm32f2_f4_f7.cpp
+    ${KPATH}/arch/common/drivers/serial_stm32.cpp
+    ${KPATH}/arch/common/drivers/stm32_hardware_rng.cpp
+)
diff --git a/src/bsps/stm32f429zi_parafoil/config/board_settings.h b/src/bsps/stm32f429zi_parafoil/config/board_settings.h
new file mode 100644
index 000000000..d016c08f9
--- /dev/null
+++ b/src/bsps/stm32f429zi_parafoil/config/board_settings.h
@@ -0,0 +1,59 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * 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.
+ */
+
+#ifndef BOARD_SETTINGS_H
+#define BOARD_SETTINGS_H
+
+#include "util/version.h"
+
+/**
+ * \internal
+ * Versioning for board_settings.h for out of git tree projects
+ */
+#define BOARD_SETTINGS_VERSION 300
+
+namespace miosix
+{
+
+/// Size of stack for main().
+/// The C standard library is stack-heavy (iprintf requires 1KB) but the
+/// STM32F407VG only has 192KB of RAM so there is room for a big 4K stack.
+const unsigned int MAIN_STACK_SIZE = 4 * 1024;
+
+/// Serial port
+const unsigned int defaultSerial      = 1;
+const unsigned int defaultSerialSpeed = 115200;
+const bool defaultSerialFlowctrl      = false;
+#define SERIAL_1_DMA
+// #define SERIAL_2_DMA
+// #define SERIAL_3_DMA
+
+// SD card driver
+static const unsigned char sdVoltage = 30;  // Board powered @ 3.0V
+#define SD_ONE_BIT_DATABUS  // Can't use 4 bit databus due to pin conflicts
+
+/// Analog supply voltage for ADC, DAC, Reset blocks, RCs and PLL
+#define V_DDA_VOLTAGE 3.0f
+
+}  // namespace miosix
+
+#endif /* BOARD_SETTINGS_H */
diff --git a/src/bsps/stm32f429zi_parafoil/config/miosix_settings.h b/src/bsps/stm32f429zi_parafoil/config/miosix_settings.h
new file mode 100644
index 000000000..5006e0bca
--- /dev/null
+++ b/src/bsps/stm32f429zi_parafoil/config/miosix_settings.h
@@ -0,0 +1,240 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * 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.
+ */
+
+#pragma once
+
+// Before you can compile the kernel you have to configure it by editing this
+// file. After that, comment out this line to disable the reminder error.
+// The PARSING_FROM_IDE is because Netbeans gets confused by this, it is never
+// defined when compiling the code.
+#ifndef PARSING_FROM_IDE
+//#error This error is a reminder that you have not edited miosix_settings.h
+// yet.
+#endif  // PARSING_FROM_IDE
+
+/**
+ * \file miosix_settings.h
+ * NOTE: this file contains ONLY configuration options that are not dependent
+ * on architecture specific details. The other options are in the following
+ * files which are included here:
+ * miosix/arch/architecture name/common/arch_settings.h
+ * miosix/config/arch/architecture name/board name/board_settings.h
+ */
+#include "arch_settings.h"
+#include "board_settings.h"
+#include "util/version.h"
+
+/**
+ * \internal
+ * Versioning for miosix_settings.h for out of git tree projects
+ */
+#define MIOSIX_SETTINGS_VERSION 300
+
+namespace miosix
+{
+
+/**
+ * \addtogroup Settings
+ * \{
+ */
+
+//
+// Scheduler options
+//
+
+/// \def SCHED_TYPE_PRIORITY
+/// If uncommented selects the priority scheduler
+/// \def SCHED_TYPE_CONTROL_BASED
+/// If uncommented selects the control based scheduler
+/// \def SCHED_TYPE_EDF
+/// If uncommented selects the EDF scheduler
+// Uncomment only *one* of those
+
+#define SCHED_TYPE_PRIORITY
+//#define SCHED_TYPE_CONTROL_BASED
+//#define SCHED_TYPE_EDF
+
+/// \def WITH_CPU_TIME_COUNTER
+/// Allows to enable/disable CPUTimeCounter to save code size and remove its
+/// overhead from the scheduling process. By default it is not defined
+/// (CPUTimeCounter is disabled).
+//#define WITH_CPU_TIME_COUNTER
+
+//
+// Filesystem options
+//
+
+/// \def WITH_FILESYSTEM
+/// Allows to enable/disable filesystem support to save code size
+/// By default it is defined (filesystem support is enabled)
+#define WITH_FILESYSTEM
+
+/// \def WITH_DEVFS
+/// Allows to enable/disable DevFs support to save code size
+/// By default it is defined (DevFs is enabled)
+#define WITH_DEVFS
+
+/// \def SYNC_AFTER_WRITE
+/// Increases filesystem write robustness. After each write operation the
+/// filesystem is synced so that a power failure happens data is not lost
+/// (unless power failure happens exactly between the write and the sync)
+/// Unfortunately write latency and throughput becomes twice as worse
+/// By default it is defined (slow but safe)
+#define SYNC_AFTER_WRITE
+
+/// Maximum number of open files. Trying to open more will fail.
+/// Cannot be lower than 3, as the first three are stdin, stdout, stderr
+const unsigned char MAX_OPEN_FILES = 8;
+
+/// \def WITH_PROCESSES
+/// If uncommented enables support for processes as well as threads.
+/// This enables the dynamic loader to load elf programs, the extended system
+/// call service and, if the hardware supports it, the MPU to provide memory
+/// isolation of processes
+//#define WITH_PROCESSES
+
+#if defined(WITH_PROCESSES) && defined(__NO_EXCEPTIONS)
+#error Processes require C++ exception support
+#endif  // defined(WITH_PROCESSES) && defined(__NO_EXCEPTIONS)
+
+#if defined(WITH_PROCESSES) && !defined(WITH_FILESYSTEM)
+#error Processes require filesystem support
+#endif  // defined(WITH_PROCESSES) && !defined(WITH_FILESYSTEM)
+
+#if defined(WITH_PROCESSES) && !defined(WITH_DEVFS)
+#error Processes require devfs support
+#endif  // defined(WITH_PROCESSES) && !defined(WITH_DEVFS)
+
+//
+// C/C++ standard library I/O (stdin, stdout and stderr related)
+//
+
+/// \def WITH_BOOTLOG
+/// Uncomment to print bootlogs on stdout.
+/// By default it is defined (bootlogs are printed)
+#define WITH_BOOTLOG
+
+/// \def WITH_ERRLOG
+/// Uncomment for debug information on stdout.
+/// By default it is defined (error information is printed)
+#define WITH_ERRLOG
+
+//
+// Kernel related options (stack sizes, priorities)
+//
+
+/// \def WITH_DEEP_SLEEP
+/// Adds interfaces and required variables to support deep sleep state switch
+/// automatically when peripherals are not required
+//#define WITH_DEEP_SLEEP
+
+/**
+ * \def JTAG_DISABLE_SLEEP
+ * JTAG debuggers lose communication with the device if it enters sleep
+ * mode, so to use debugging it is necessary to disable sleep in the idle
+ * thread. By default it is not defined (idle thread calls sleep).
+ */
+//#define JTAG_DISABLE_SLEEP
+
+#if defined(WITH_DEEP_SLEEP) && defined(JTAG_DISABLE_SLEEP)
+#error Deep sleep cannot work together with jtag
+#endif  // defined(WITH_PROCESSES) && !defined(WITH_DEVFS)
+
+/// Minimum stack size (MUST be divisible by 4)
+const unsigned int STACK_MIN = 256;
+
+/// \internal Size of idle thread stack.
+/// Should be >=STACK_MIN (MUST be divisible by 4)
+const unsigned int STACK_IDLE = 256;
+
+/// Default stack size for pthread_create.
+/// The chosen value is enough to call C standard library functions
+/// such as printf/fopen which are stack-heavy
+const unsigned int STACK_DEFAULT_FOR_PTHREAD = 2048;
+
+/// Maximum size of the RAM image of a process. If a program requires more
+/// the kernel will not run it (MUST be divisible by 4)
+const unsigned int MAX_PROCESS_IMAGE_SIZE = 64 * 1024;
+
+/// Minimum size of the stack for a process. If a program specifies a lower
+/// size the kernel will not run it (MUST be divisible by 4)
+const unsigned int MIN_PROCESS_STACK_SIZE = STACK_MIN;
+
+/// Every userspace thread has two stacks, one for when it is running in
+/// userspace and one for when it is running in kernelspace (that is, while it
+/// is executing system calls). This is the size of the stack for when the
+/// thread is running in kernelspace (MUST be divisible by 4)
+const unsigned int SYSTEM_MODE_PROCESS_STACK_SIZE = 2 * 1024;
+
+/// Number of priorities (MUST be >1)
+/// PRIORITY_MAX-1 is the highest priority, 0 is the lowest. -1 is reserved as
+/// the priority of the idle thread.
+/// The meaning of a thread's priority depends on the chosen scheduler.
+#ifdef SCHED_TYPE_PRIORITY
+// Can be modified, but a high value makes context switches more expensive
+const short int PRIORITY_MAX = 4;
+#elif defined(SCHED_TYPE_CONTROL_BASED)
+// Don't touch, the limit is due to the fixed point implementation
+// It's not needed for if floating point is selected, but kept for consistency
+const short int PRIORITY_MAX = 64;
+#else  // SCHED_TYPE_EDF
+// Doesn't exist for this kind of scheduler
+#endif
+
+/// Priority of main()
+/// The meaning of a thread's priority depends on the chosen scheduler.
+const unsigned char MAIN_PRIORITY = 1;
+
+#ifdef SCHED_TYPE_PRIORITY
+/// Maximum thread time slice in nanoseconds, after which preemption occurs
+const unsigned int MAX_TIME_SLICE = 1000000;
+#endif  // SCHED_TYPE_PRIORITY
+
+//
+// Other low level kernel options. There is usually no need to modify these.
+//
+
+/// \internal Length of wartermark (in bytes) to check stack overflow.
+/// MUST be divisible by 4 and can also be zero.
+/// A high value increases context switch time.
+const unsigned int WATERMARK_LEN = 16;
+
+/// \internal Used to fill watermark
+const unsigned int WATERMARK_FILL = 0xaaaaaaaa;
+
+/// \internal Used to fill stack (for checking stack usage)
+const unsigned int STACK_FILL = 0xbbbbbbbb;
+
+// Compiler version checks
+#if !defined(_MIOSIX_GCC_PATCH_MAJOR) || _MIOSIX_GCC_PATCH_MAJOR < 3
+#error \
+    "You are using a too old or unsupported compiler. Get the latest one from https://miosix.org/wiki/index.php?title=Miosix_Toolchain"
+#endif
+#if _MIOSIX_GCC_PATCH_MAJOR > 3
+#warning "You are using a too new compiler, which may not be supported"
+#endif
+
+/**
+ * \}
+ */
+
+}  // namespace miosix
diff --git a/src/bsps/stm32f429zi_parafoil/core/stage_1_boot.cpp b/src/bsps/stm32f429zi_parafoil/core/stage_1_boot.cpp
new file mode 100644
index 000000000..10804a0b7
--- /dev/null
+++ b/src/bsps/stm32f429zi_parafoil/core/stage_1_boot.cpp
@@ -0,0 +1,436 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * 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 <string.h>
+
+#include "core/interrupts.h"  //For the unexpected interrupt call
+#include "interfaces/arch_registers.h"
+#include "interfaces/bsp.h"
+#include "kernel/stage_2_boot.h"
+
+/*
+ * startup.cpp
+ * STM32 C++ startup.
+ * NOTE: for stm32f42x devices ONLY.
+ * Supports interrupt handlers in C++ without extern "C"
+ * Developed by Terraneo Federico, based on ST startup code.
+ * Additionally modified to boot Miosix.
+ */
+
+/**
+ * Called by Reset_Handler, performs initialization and calls main.
+ * Never returns.
+ */
+void program_startup() __attribute__((noreturn));
+void program_startup()
+{
+    // Cortex M3 core appears to get out of reset with interrupts already
+    // enabled
+    __disable_irq();
+
+    // These are defined in the linker script
+    extern unsigned char _etext asm("_etext");
+    extern unsigned char _data asm("_data");
+    extern unsigned char _edata asm("_edata");
+    extern unsigned char _bss_start asm("_bss_start");
+    extern unsigned char _bss_end asm("_bss_end");
+
+    // Initialize .data section, clear .bss section
+    unsigned char *etext     = &_etext;
+    unsigned char *data      = &_data;
+    unsigned char *edata     = &_edata;
+    unsigned char *bss_start = &_bss_start;
+    unsigned char *bss_end   = &_bss_end;
+    memcpy(data, etext, edata - data);
+    memset(bss_start, 0, bss_end - bss_start);
+
+    // Move on to stage 2
+    _init();
+
+    // If main returns, reboot
+    NVIC_SystemReset();
+    for (;;)
+        ;
+}
+
+/**
+ * Reset handler, called by hardware immediately after reset
+ */
+void Reset_Handler() __attribute__((__interrupt__, noreturn));
+void Reset_Handler()
+{
+    // SystemInit() is called *before* initializing .data and zeroing .bss
+    // Despite all startup files provided by ST do the opposite, there are three
+    // good reasons to do so:
+    // First, the CMSIS specifications say that SystemInit() must not access
+    // global variables, so it is actually possible to call it before
+    // Second, when running Miosix with the xram linker scripts .data and .bss
+    // are placed in the external RAM, so we *must* call SystemInit(), which
+    // enables xram, before touching .data and .bss
+    // Third, this is a performance improvement since the loops that initialize
+    //.data and zeros .bss now run with the CPU at full speed instead of 8MHz
+    SystemInit();
+// ST does not provide code to initialize the stm32f429-disco SDRAM at boot.
+// Put after SystemInit() as SDRAM is timing-sensitive and needs the full
+// clock speed.
+#ifdef __ENABLE_XRAM
+    miosix::configureSdram();
+#endif  //__ENABLE_XRAM
+
+    /*
+     * Initialize process stack and switch to it.
+     * This is required for booting Miosix, a small portion of the top of the
+     * heap area will be used as stack until the first thread starts. After,
+     * this stack will be abandoned and the process stack will point to the
+     * current thread's stack.
+     */
+    asm volatile(
+        "ldr r0,  =_heap_end          \n\t"
+        "msr psp, r0                  \n\t"
+        "movw r0, #2                  \n\n"  // Privileged, process stack
+        "msr control, r0              \n\t"
+        "isb                          \n\t" ::
+            : "r0");
+
+    program_startup();
+}
+
+/**
+ * All unused interrupts call this function.
+ */
+extern "C" void Default_Handler() { unexpectedInterrupt(); }
+
+// System handlers
+void /*__attribute__((weak))*/ Reset_Handler();      // These interrupts are not
+void /*__attribute__((weak))*/ NMI_Handler();        // weak because they are
+void /*__attribute__((weak))*/ HardFault_Handler();  // surely defined by Miosix
+void /*__attribute__((weak))*/ MemManage_Handler();
+void /*__attribute__((weak))*/ BusFault_Handler();
+void /*__attribute__((weak))*/ UsageFault_Handler();
+void /*__attribute__((weak))*/ SVC_Handler();
+void /*__attribute__((weak))*/ DebugMon_Handler();
+void /*__attribute__((weak))*/ PendSV_Handler();
+void __attribute__((weak)) SysTick_Handler();
+
+// Interrupt handlers
+void __attribute__((weak)) WWDG_IRQHandler();
+void __attribute__((weak)) PVD_IRQHandler();
+void __attribute__((weak)) TAMP_STAMP_IRQHandler();
+void __attribute__((weak)) RTC_WKUP_IRQHandler();
+void __attribute__((weak)) FLASH_IRQHandler();
+void __attribute__((weak)) RCC_IRQHandler();
+void __attribute__((weak)) EXTI0_IRQHandler();
+void __attribute__((weak)) EXTI1_IRQHandler();
+void __attribute__((weak)) EXTI2_IRQHandler();
+void __attribute__((weak)) EXTI3_IRQHandler();
+void __attribute__((weak)) EXTI4_IRQHandler();
+void __attribute__((weak)) DMA1_Stream0_IRQHandler();
+void __attribute__((weak)) DMA1_Stream1_IRQHandler();
+void __attribute__((weak)) DMA1_Stream2_IRQHandler();
+void __attribute__((weak)) DMA1_Stream3_IRQHandler();
+void __attribute__((weak)) DMA1_Stream4_IRQHandler();
+void __attribute__((weak)) DMA1_Stream5_IRQHandler();
+void __attribute__((weak)) DMA1_Stream6_IRQHandler();
+void __attribute__((weak)) ADC_IRQHandler();
+void __attribute__((weak)) CAN1_TX_IRQHandler();
+void __attribute__((weak)) CAN1_RX0_IRQHandler();
+void __attribute__((weak)) CAN1_RX1_IRQHandler();
+void __attribute__((weak)) CAN1_SCE_IRQHandler();
+void __attribute__((weak)) EXTI9_5_IRQHandler();
+void __attribute__((weak)) TIM1_BRK_TIM9_IRQHandler();
+void __attribute__((weak)) TIM1_UP_TIM10_IRQHandler();
+void __attribute__((weak)) TIM1_TRG_COM_TIM11_IRQHandler();
+void __attribute__((weak)) TIM1_CC_IRQHandler();
+void __attribute__((weak)) TIM2_IRQHandler();
+void __attribute__((weak)) TIM3_IRQHandler();
+void __attribute__((weak)) TIM4_IRQHandler();
+void __attribute__((weak)) I2C1_EV_IRQHandler();
+void __attribute__((weak)) I2C1_ER_IRQHandler();
+void __attribute__((weak)) I2C2_EV_IRQHandler();
+void __attribute__((weak)) I2C2_ER_IRQHandler();
+void __attribute__((weak)) SPI1_IRQHandler();
+void __attribute__((weak)) SPI2_IRQHandler();
+void __attribute__((weak)) USART1_IRQHandler();
+void __attribute__((weak)) USART2_IRQHandler();
+void __attribute__((weak)) USART3_IRQHandler();
+void __attribute__((weak)) EXTI15_10_IRQHandler();
+void __attribute__((weak)) RTC_Alarm_IRQHandler();
+void __attribute__((weak)) OTG_FS_WKUP_IRQHandler();
+void __attribute__((weak)) TIM8_BRK_TIM12_IRQHandler();
+void __attribute__((weak)) TIM8_UP_TIM13_IRQHandler();
+void __attribute__((weak)) TIM8_TRG_COM_TIM14_IRQHandler();
+void __attribute__((weak)) TIM8_CC_IRQHandler();
+void __attribute__((weak)) DMA1_Stream7_IRQHandler();
+void __attribute__((weak)) FMC_IRQHandler();
+void __attribute__((weak)) SDIO_IRQHandler();
+void __attribute__((weak)) TIM5_IRQHandler();
+void __attribute__((weak)) SPI3_IRQHandler();
+void __attribute__((weak)) UART4_IRQHandler();
+void __attribute__((weak)) UART5_IRQHandler();
+void __attribute__((weak)) TIM6_DAC_IRQHandler();
+void __attribute__((weak)) TIM7_IRQHandler();
+void __attribute__((weak)) DMA2_Stream0_IRQHandler();
+void __attribute__((weak)) DMA2_Stream1_IRQHandler();
+void __attribute__((weak)) DMA2_Stream2_IRQHandler();
+void __attribute__((weak)) DMA2_Stream3_IRQHandler();
+void __attribute__((weak)) DMA2_Stream4_IRQHandler();
+void __attribute__((weak)) ETH_IRQHandler();
+void __attribute__((weak)) ETH_WKUP_IRQHandler();
+void __attribute__((weak)) CAN2_TX_IRQHandler();
+void __attribute__((weak)) CAN2_RX0_IRQHandler();
+void __attribute__((weak)) CAN2_RX1_IRQHandler();
+void __attribute__((weak)) CAN2_SCE_IRQHandler();
+void __attribute__((weak)) OTG_FS_IRQHandler();
+void __attribute__((weak)) DMA2_Stream5_IRQHandler();
+void __attribute__((weak)) DMA2_Stream6_IRQHandler();
+void __attribute__((weak)) DMA2_Stream7_IRQHandler();
+void __attribute__((weak)) USART6_IRQHandler();
+void __attribute__((weak)) I2C3_EV_IRQHandler();
+void __attribute__((weak)) I2C3_ER_IRQHandler();
+void __attribute__((weak)) OTG_HS_EP1_OUT_IRQHandler();
+void __attribute__((weak)) OTG_HS_EP1_IN_IRQHandler();
+void __attribute__((weak)) OTG_HS_WKUP_IRQHandler();
+void __attribute__((weak)) OTG_HS_IRQHandler();
+void __attribute__((weak)) DCMI_IRQHandler();
+void __attribute__((weak)) CRYP_IRQHandler();
+void __attribute__((weak)) HASH_RNG_IRQHandler();
+void __attribute__((weak)) FPU_IRQHandler();
+void __attribute__((weak)) UART7_IRQHandler();
+void __attribute__((weak)) UART8_IRQHandler();
+void __attribute__((weak)) SPI4_IRQHandler();
+void __attribute__((weak)) SPI5_IRQHandler();
+void __attribute__((weak)) SPI6_IRQHandler();
+void __attribute__((weak)) SAI1_IRQHandler();
+void __attribute__((weak)) LTDC_IRQHandler();
+void __attribute__((weak)) LTDC_ER_IRQHandler();
+void __attribute__((weak)) DMA2D_IRQHandler();
+
+// Stack top, defined in the linker script
+extern char _main_stack_top asm("_main_stack_top");
+
+// Interrupt vectors, must be placed @ address 0x00000000
+// The extern declaration is required otherwise g++ optimizes it out
+extern void (*const __Vectors[])();
+void (*const __Vectors[])() __attribute__((section(".isr_vector"))) = {
+    reinterpret_cast<void (*)()>(&_main_stack_top), /* Stack pointer*/
+    Reset_Handler,                                  /* Reset Handler */
+    NMI_Handler,                                    /* NMI Handler */
+    HardFault_Handler,                              /* Hard Fault Handler */
+    MemManage_Handler,                              /* MPU Fault Handler */
+    BusFault_Handler,                               /* Bus Fault Handler */
+    UsageFault_Handler,                             /* Usage Fault Handler */
+    0,                                              /* Reserved */
+    0,                                              /* Reserved */
+    0,                                              /* Reserved */
+    0,                                              /* Reserved */
+    SVC_Handler,                                    /* SVCall Handler */
+    DebugMon_Handler,                               /* Debug Monitor Handler */
+    0,                                              /* Reserved */
+    PendSV_Handler,                                 /* PendSV Handler */
+    SysTick_Handler,                                /* SysTick Handler */
+
+    /* External Interrupts */
+    WWDG_IRQHandler,
+    PVD_IRQHandler,
+    TAMP_STAMP_IRQHandler,
+    RTC_WKUP_IRQHandler,
+    FLASH_IRQHandler,
+    RCC_IRQHandler,
+    EXTI0_IRQHandler,
+    EXTI1_IRQHandler,
+    EXTI2_IRQHandler,
+    EXTI3_IRQHandler,
+    EXTI4_IRQHandler,
+    DMA1_Stream0_IRQHandler,
+    DMA1_Stream1_IRQHandler,
+    DMA1_Stream2_IRQHandler,
+    DMA1_Stream3_IRQHandler,
+    DMA1_Stream4_IRQHandler,
+    DMA1_Stream5_IRQHandler,
+    DMA1_Stream6_IRQHandler,
+    ADC_IRQHandler,
+    CAN1_TX_IRQHandler,
+    CAN1_RX0_IRQHandler,
+    CAN1_RX1_IRQHandler,
+    CAN1_SCE_IRQHandler,
+    EXTI9_5_IRQHandler,
+    TIM1_BRK_TIM9_IRQHandler,
+    TIM1_UP_TIM10_IRQHandler,
+    TIM1_TRG_COM_TIM11_IRQHandler,
+    TIM1_CC_IRQHandler,
+    TIM2_IRQHandler,
+    TIM3_IRQHandler,
+    TIM4_IRQHandler,
+    I2C1_EV_IRQHandler,
+    I2C1_ER_IRQHandler,
+    I2C2_EV_IRQHandler,
+    I2C2_ER_IRQHandler,
+    SPI1_IRQHandler,
+    SPI2_IRQHandler,
+    USART1_IRQHandler,
+    USART2_IRQHandler,
+    USART3_IRQHandler,
+    EXTI15_10_IRQHandler,
+    RTC_Alarm_IRQHandler,
+    OTG_FS_WKUP_IRQHandler,
+    TIM8_BRK_TIM12_IRQHandler,
+    TIM8_UP_TIM13_IRQHandler,
+    TIM8_TRG_COM_TIM14_IRQHandler,
+    TIM8_CC_IRQHandler,
+    DMA1_Stream7_IRQHandler,
+    FMC_IRQHandler,
+    SDIO_IRQHandler,
+    TIM5_IRQHandler,
+    SPI3_IRQHandler,
+    UART4_IRQHandler,
+    UART5_IRQHandler,
+    TIM6_DAC_IRQHandler,
+    TIM7_IRQHandler,
+    DMA2_Stream0_IRQHandler,
+    DMA2_Stream1_IRQHandler,
+    DMA2_Stream2_IRQHandler,
+    DMA2_Stream3_IRQHandler,
+    DMA2_Stream4_IRQHandler,
+    ETH_IRQHandler,
+    ETH_WKUP_IRQHandler,
+    CAN2_TX_IRQHandler,
+    CAN2_RX0_IRQHandler,
+    CAN2_RX1_IRQHandler,
+    CAN2_SCE_IRQHandler,
+    OTG_FS_IRQHandler,
+    DMA2_Stream5_IRQHandler,
+    DMA2_Stream6_IRQHandler,
+    DMA2_Stream7_IRQHandler,
+    USART6_IRQHandler,
+    I2C3_EV_IRQHandler,
+    I2C3_ER_IRQHandler,
+    OTG_HS_EP1_OUT_IRQHandler,
+    OTG_HS_EP1_IN_IRQHandler,
+    OTG_HS_WKUP_IRQHandler,
+    OTG_HS_IRQHandler,
+    DCMI_IRQHandler,
+    CRYP_IRQHandler,
+    HASH_RNG_IRQHandler,
+    FPU_IRQHandler,
+    UART7_IRQHandler,
+    UART8_IRQHandler,
+    SPI4_IRQHandler,
+    SPI5_IRQHandler,
+    SPI6_IRQHandler,
+    SAI1_IRQHandler,
+    LTDC_IRQHandler,
+    LTDC_ER_IRQHandler,
+    DMA2D_IRQHandler,
+};
+
+#pragma weak SysTick_IRQHandler            = Default_Handler
+#pragma weak WWDG_IRQHandler               = Default_Handler
+#pragma weak PVD_IRQHandler                = Default_Handler
+#pragma weak TAMP_STAMP_IRQHandler         = Default_Handler
+#pragma weak RTC_WKUP_IRQHandler           = Default_Handler
+#pragma weak FLASH_IRQHandler              = Default_Handler
+#pragma weak RCC_IRQHandler                = Default_Handler
+#pragma weak EXTI0_IRQHandler              = Default_Handler
+#pragma weak EXTI1_IRQHandler              = Default_Handler
+#pragma weak EXTI2_IRQHandler              = Default_Handler
+#pragma weak EXTI3_IRQHandler              = Default_Handler
+#pragma weak EXTI4_IRQHandler              = Default_Handler
+#pragma weak DMA1_Stream0_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream1_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream2_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream3_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream4_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream5_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream6_IRQHandler       = Default_Handler
+#pragma weak ADC_IRQHandler                = Default_Handler
+#pragma weak CAN1_TX_IRQHandler            = Default_Handler
+#pragma weak CAN1_RX0_IRQHandler           = Default_Handler
+#pragma weak CAN1_RX1_IRQHandler           = Default_Handler
+#pragma weak CAN1_SCE_IRQHandler           = Default_Handler
+#pragma weak EXTI9_5_IRQHandler            = Default_Handler
+#pragma weak TIM1_BRK_TIM9_IRQHandler      = Default_Handler
+#pragma weak TIM1_UP_TIM10_IRQHandler      = Default_Handler
+#pragma weak TIM1_TRG_COM_TIM11_IRQHandler = Default_Handler
+#pragma weak TIM1_CC_IRQHandler            = Default_Handler
+#pragma weak TIM2_IRQHandler               = Default_Handler
+#pragma weak TIM3_IRQHandler               = Default_Handler
+#pragma weak TIM4_IRQHandler               = Default_Handler
+#pragma weak I2C1_EV_IRQHandler            = Default_Handler
+#pragma weak I2C1_ER_IRQHandler            = Default_Handler
+#pragma weak I2C2_EV_IRQHandler            = Default_Handler
+#pragma weak I2C2_ER_IRQHandler            = Default_Handler
+#pragma weak SPI1_IRQHandler               = Default_Handler
+#pragma weak SPI2_IRQHandler               = Default_Handler
+// #pragma weak USART1_IRQHandler             = Default_Handler
+// #pragma weak USART2_IRQHandler             = Default_Handler
+// #pragma weak USART3_IRQHandler             = Default_Handler
+#pragma weak EXTI15_10_IRQHandler          = Default_Handler
+#pragma weak RTC_Alarm_IRQHandler          = Default_Handler
+#pragma weak OTG_FS_WKUP_IRQHandler        = Default_Handler
+#pragma weak TIM8_BRK_TIM12_IRQHandler     = Default_Handler
+#pragma weak TIM8_UP_TIM13_IRQHandler      = Default_Handler
+#pragma weak TIM8_TRG_COM_TIM14_IRQHandler = Default_Handler
+#pragma weak TIM8_CC_IRQHandler            = Default_Handler
+#pragma weak DMA1_Stream7_IRQHandler       = Default_Handler
+#pragma weak FMC_IRQHandler                = Default_Handler
+#pragma weak SDIO_IRQHandler               = Default_Handler
+#pragma weak TIM5_IRQHandler               = Default_Handler
+#pragma weak SPI3_IRQHandler               = Default_Handler
+// #pragma weak UART4_IRQHandler              = Default_Handler
+// #pragma weak UART5_IRQHandler              = Default_Handler
+#pragma weak TIM6_DAC_IRQHandler     = Default_Handler
+#pragma weak TIM7_IRQHandler         = Default_Handler
+#pragma weak DMA2_Stream0_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream1_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream2_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream3_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream4_IRQHandler = Default_Handler
+#pragma weak ETH_IRQHandler          = Default_Handler
+#pragma weak ETH_WKUP_IRQHandler     = Default_Handler
+#pragma weak CAN2_TX_IRQHandler      = Default_Handler
+#pragma weak CAN2_RX0_IRQHandler     = Default_Handler
+#pragma weak CAN2_RX1_IRQHandler     = Default_Handler
+#pragma weak CAN2_SCE_IRQHandler     = Default_Handler
+#pragma weak OTG_FS_IRQHandler       = Default_Handler
+#pragma weak DMA2_Stream5_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream6_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream7_IRQHandler = Default_Handler
+// #pragma weak USART6_IRQHandler             = Default_Handler
+#pragma weak I2C3_EV_IRQHandler        = Default_Handler
+#pragma weak I2C3_ER_IRQHandler        = Default_Handler
+#pragma weak OTG_HS_EP1_OUT_IRQHandler = Default_Handler
+#pragma weak OTG_HS_EP1_IN_IRQHandler  = Default_Handler
+#pragma weak OTG_HS_WKUP_IRQHandler    = Default_Handler
+#pragma weak OTG_HS_IRQHandler         = Default_Handler
+#pragma weak DCMI_IRQHandler           = Default_Handler
+#pragma weak CRYP_IRQHandler           = Default_Handler
+#pragma weak HASH_RNG_IRQHandler       = Default_Handler
+#pragma weak FPU_IRQHandler            = Default_Handler
+// #pragma weak UART7_IRQHandler              = Default_Handler
+// #pragma weak UART8_IRQHandler              = Default_Handler
+#pragma weak SPI4_IRQHandler    = Default_Handler
+#pragma weak SPI5_IRQHandler    = Default_Handler
+#pragma weak SPI6_IRQHandler    = Default_Handler
+#pragma weak SAI1_IRQHandler    = Default_Handler
+#pragma weak LTDC_IRQHandler    = Default_Handler
+#pragma weak LTDC_ER_IRQHandler = Default_Handler
+#pragma weak DMA2D_IRQHandler   = Default_Handler
diff --git a/src/bsps/stm32f429zi_parafoil/interfaces-impl/arch_registers_impl.h b/src/bsps/stm32f429zi_parafoil/interfaces-impl/arch_registers_impl.h
new file mode 100644
index 000000000..aa9f5579f
--- /dev/null
+++ b/src/bsps/stm32f429zi_parafoil/interfaces-impl/arch_registers_impl.h
@@ -0,0 +1,34 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * 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.
+ */
+
+#ifndef ARCH_REGISTERS_IMPL_H
+#define ARCH_REGISTERS_IMPL_H
+
+// Always include stm32f4xx.h before core_cm4.h, there's some nasty dependency
+#define STM32F429xx
+#include "CMSIS/Device/ST/STM32F4xx/Include/stm32f4xx.h"
+#include "CMSIS/Device/ST/STM32F4xx/Include/system_stm32f4xx.h"
+#include "CMSIS/Include/core_cm4.h"
+
+#define RCC_SYNC() __DSB()  // Workaround for a bug in stm32f42x
+
+#endif  // ARCH_REGISTERS_IMPL_H
diff --git a/src/bsps/stm32f429zi_parafoil/interfaces-impl/bsp.cpp b/src/bsps/stm32f429zi_parafoil/interfaces-impl/bsp.cpp
new file mode 100644
index 000000000..12077a4c4
--- /dev/null
+++ b/src/bsps/stm32f429zi_parafoil/interfaces-impl/bsp.cpp
@@ -0,0 +1,313 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * 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.
+ */
+
+/***********************************************************************
+ * bsp.cpp Part of the Miosix Embedded OS.
+ * Board support package, this file initializes hardware.
+ ************************************************************************/
+
+#include "interfaces/bsp.h"
+
+#include <inttypes.h>
+#include <sys/ioctl.h>
+
+#include <cstdlib>
+
+#include "board_settings.h"
+#include "config/miosix_settings.h"
+#include "drivers/sd_stm32f2_f4_f7.h"
+#include "drivers/serial.h"
+#include "filesystem/console/console_device.h"
+#include "filesystem/file_access.h"
+#include "hwmapping.h"
+#include "interfaces/arch_registers.h"
+#include "interfaces/delays.h"
+#include "interfaces/portability.h"
+#include "kernel/kernel.h"
+#include "kernel/logging.h"
+#include "kernel/sync.h"
+
+namespace miosix
+{
+
+//
+// Initialization
+//
+
+/**
+ * The example code from ST checks for the busy flag after each command.
+ * Interestingly I couldn't find any mention of this in the datsheet.
+ */
+static void sdramCommandWait()
+{
+    for (int i = 0; i < 0xffff; i++)
+        if ((FMC_Bank5_6->SDSR & FMC_SDSR_BUSY) == 0)
+            return;
+}
+
+void configureSdram()
+{
+    // Enable all gpios, passing clock
+    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN | RCC_AHB1ENR_GPIOBEN |
+                    RCC_AHB1ENR_GPIOCEN | RCC_AHB1ENR_GPIODEN |
+                    RCC_AHB1ENR_GPIOEEN | RCC_AHB1ENR_GPIOFEN |
+                    RCC_AHB1ENR_GPIOGEN | RCC_AHB1ENR_GPIOHEN;
+    RCC_SYNC();
+
+    // First, configure SDRAM GPIOs
+    GPIOB->AFR[0] = 0x0cc00000;
+    GPIOC->AFR[0] = 0x0000000c;
+    GPIOD->AFR[0] = 0x000000cc;
+    GPIOD->AFR[1] = 0xcc000ccc;
+    GPIOE->AFR[0] = 0xc00000cc;
+    GPIOE->AFR[1] = 0xcccccccc;
+    GPIOF->AFR[0] = 0x00cccccc;
+    GPIOF->AFR[1] = 0xccccc000;
+    GPIOG->AFR[0] = 0x00cc00cc;
+    GPIOG->AFR[1] = 0xc000000c;
+
+    GPIOB->MODER = 0x00002800;
+    GPIOC->MODER = 0x00000002;
+    GPIOD->MODER = 0xa02a000a;
+    GPIOE->MODER = 0xaaaa800a;
+    GPIOF->MODER = 0xaa800aaa;
+    GPIOG->MODER = 0x80020a0a;
+
+    GPIOA->OSPEEDR = 0xaaaaaaaa;  // Default to 50MHz speed for all GPIOs...
+    GPIOB->OSPEEDR =
+        0xaaaaaaaa | 0x00003c00;  //...but 100MHz for the SDRAM pins
+    GPIOC->OSPEEDR = 0xaaaaaaaa | 0x00000003;
+    GPIOD->OSPEEDR = 0xaaaaaaaa | 0xf03f000f;
+    GPIOE->OSPEEDR = 0xaaaaaaaa | 0xffffc00f;
+    GPIOF->OSPEEDR = 0xaaaaaaaa | 0xffc00fff;
+    GPIOG->OSPEEDR = 0xaaaaaaaa | 0xc0030f0f;
+    GPIOH->OSPEEDR = 0xaaaaaaaa;
+
+    // Since we'we un-configured PB3/PB4 from the default at boot TDO,NTRST,
+    // finish the job and remove the default pull-up
+    GPIOB->PUPDR = 0;
+
+    // Second, actually start the SDRAM controller
+    RCC->AHB3ENR |= RCC_AHB3ENR_FMCEN;
+    RCC_SYNC();
+
+    // SDRAM is a IS42S16400J -7 speed grade, connected to bank 2 (0xd0000000)
+    // Some bits in SDCR[1] are don't care, and the have to be set in SDCR[0],
+    // they aren't just don't care, the controller will fail if they aren't at 0
+    FMC_Bank5_6->SDCR[0] = FMC_SDCR1_SDCLK_1  // SDRAM runs @ half CPU frequency
+                           | FMC_SDCR1_RBURST  // Enable read burst
+                           | 0;              //  0 delay between reads after CAS
+    FMC_Bank5_6->SDCR[1] = 0                 //  8 bit column address
+                           | FMC_SDCR1_NR_0  // 12 bit row address
+                           | FMC_SDCR1_MWID_0  // 16 bit data bus
+                           | FMC_SDCR1_NB      //  4 banks
+                           |
+                           FMC_SDCR1_CAS_1;  //  2 cycle CAS latency (F<133MHz)
+
+#ifdef SYSCLK_FREQ_180MHz
+    // One SDRAM clock cycle is 11.1ns
+    // Some bits in SDTR[1] are don't care, and the have to be set in SDTR[0],
+    // they aren't just don't care, the controller will fail if they aren't at 0
+    FMC_Bank5_6->SDTR[0] = (6 - 1) << 12     // 6 cycle TRC  (66.6ns>63ns)
+                           | (2 - 1) << 20;  // 2 cycle TRP  (22.2ns>15ns)
+    FMC_Bank5_6->SDTR[1] = (2 - 1) << 0      // 2 cycle TMRD
+                           | (7 - 1) << 4    // 7 cycle TXSR (77.7ns>70ns)
+                           | (4 - 1) << 8    // 4 cycle TRAS (44.4ns>42ns)
+                           | (2 - 1) << 16   // 2 cycle TWR
+                           | (2 - 1) << 24;  // 2 cycle TRCD (22.2ns>15ns)
+#elif defined(SYSCLK_FREQ_168MHz)
+    // One SDRAM clock cycle is 11.9ns
+    // Some bits in SDTR[1] are don't care, and the have to be set in SDTR[0],
+    // they aren't just don't care, the controller will fail if they aren't at 0
+    FMC_Bank5_6->SDTR[0] = (6 - 1) << 12     // 6 cycle TRC  (71.4ns>63ns)
+                           | (2 - 1) << 20;  // 2 cycle TRP  (23.8ns>15ns)
+    FMC_Bank5_6->SDTR[1] = (2 - 1) << 0      // 2 cycle TMRD
+                           | (6 - 1) << 4    // 6 cycle TXSR (71.4ns>70ns)
+                           | (4 - 1) << 8    // 4 cycle TRAS (47.6ns>42ns)
+                           | (2 - 1) << 16   // 2 cycle TWR
+                           | (2 - 1) << 24;  // 2 cycle TRCD (23.8ns>15ns)
+#else
+#error No SDRAM timings for this clock
+#endif
+
+    FMC_Bank5_6->SDCMR = FMC_SDCMR_CTB2  // Enable bank 2
+                         | 1;            // MODE=001 clock enabled
+    sdramCommandWait();
+
+    // ST and SDRAM datasheet agree a 100us delay is required here.
+    delayUs(100);
+
+    FMC_Bank5_6->SDCMR = FMC_SDCMR_CTB2  // Enable bank 2
+                         | 2;            // MODE=010 precharge all command
+    sdramCommandWait();
+
+    FMC_Bank5_6->SDCMR = (8 - 1) << 5      // NRFS=8 SDRAM datasheet says
+                                           // "at least two AUTO REFRESH cycles"
+                         | FMC_SDCMR_CTB2  // Enable bank 2
+                         | 3;              // MODE=011 auto refresh
+    sdramCommandWait();
+
+    FMC_Bank5_6->SDCMR = 0x220 << 9  // MRD=0x220:CAS latency=2 burst len=1
+                         | FMC_SDCMR_CTB2  // Enable bank 2
+                         | 4;              // MODE=100 load mode register
+    sdramCommandWait();
+
+// 64ms/4096=15.625us
+#ifdef SYSCLK_FREQ_180MHz
+    // 15.625us*90MHz=1406-20=1386
+    FMC_Bank5_6->SDRTR = 1386 << 1;
+#elif defined(SYSCLK_FREQ_168MHz)
+    // 15.625us*84MHz=1312-20=1292
+    FMC_Bank5_6->SDRTR = 1292 << 1;
+#else
+#error No refresh timings for this clock
+#endif
+}
+
+void IRQbspInit()
+{
+// If using SDRAM GPIOs are enabled by configureSdram(), else enable them here
+#ifndef __ENABLE_XRAM
+    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN | RCC_AHB1ENR_GPIOBEN |
+                    RCC_AHB1ENR_GPIOCEN | RCC_AHB1ENR_GPIODEN |
+                    RCC_AHB1ENR_GPIOEEN | RCC_AHB1ENR_GPIOFEN |
+                    RCC_AHB1ENR_GPIOGEN | RCC_AHB1ENR_GPIOHEN;
+    RCC_SYNC();
+#endif  //__ENABLE_XRAM
+
+    RCC->APB2ENR |= RCC_APB2ENR_SPI1EN;
+    RCC->APB2ENR |= RCC_APB2ENR_SPI4EN;
+    RCC->APB2ENR |= RCC_APB2ENR_SPI5EN;
+
+    RCC->AHB1ENR |= RCC_AHB1ENR_DMA2EN;
+
+    RCC->APB2ENR |= RCC_APB2ENR_TIM1EN;
+    RCC->APB1ENR |= RCC_APB1ENR_TIM3EN;
+    RCC->APB2ENR |= RCC_APB2ENR_TIM8EN;
+    RCC->APB2ENR |= RCC_APB2ENR_TIM9EN;
+    RCC->APB2ENR |= RCC_APB2ENR_TIM10EN;
+    RCC->APB2ENR |= RCC_APB2ENR_TIM11EN;
+
+    RCC_SYNC();
+
+    using namespace interfaces;
+
+    spi1::sck::mode(Mode::ALTERNATE);
+    spi1::sck::alternateFunction(5);
+    spi1::miso::mode(Mode::ALTERNATE);
+    spi1::miso::alternateFunction(5);
+    spi1::mosi::mode(Mode::ALTERNATE);
+    spi1::mosi::alternateFunction(5);
+
+    spi4::sck::mode(Mode::ALTERNATE);
+    spi4::sck::alternateFunction(5);
+    spi4::miso::mode(Mode::ALTERNATE);
+    spi4::miso::alternateFunction(5);
+    spi4::mosi::mode(Mode::ALTERNATE);
+    spi4::mosi::alternateFunction(5);
+
+    uart1::rx::mode(Mode::ALTERNATE);
+    uart1::rx::alternateFunction(7);
+    uart1::tx::mode(Mode::ALTERNATE);
+    uart1::tx::alternateFunction(7);
+
+    uart2::rx::mode(Mode::ALTERNATE);
+    uart2::rx::alternateFunction(7);
+    uart2::tx::mode(Mode::ALTERNATE);
+    uart2::tx::alternateFunction(7);
+
+    timers::tim4ch2::mode(Mode::ALTERNATE);
+    timers::tim4ch2::alternateFunction(2);
+    timers::tim10ch1::mode(Mode::ALTERNATE);
+    timers::tim10ch1::alternateFunction(3);
+
+    using namespace sensors;
+
+    mpu9250::cs::mode(Mode::OUTPUT);
+    mpu9250::cs::high();
+
+    bme280::cs::mode(Mode::OUTPUT);
+    bme280::cs::high();
+
+    adc::battery::mode(Mode::INPUT_ANALOG);
+
+    sx1278::cs::mode(Mode::OUTPUT);
+    sx1278::cs::high();
+    sx1278::interrupt::mode(Mode::OUTPUT);
+    sx1278::interrupt::high();
+
+    using namespace ui;
+
+    button::mode(Mode::INPUT);
+
+    greenLed::mode(Mode::OUTPUT);
+    greenLed::low();
+    redLed::mode(Mode::OUTPUT);
+    redLed::low();
+
+    DefaultConsole::instance().IRQset(intrusive_ref_ptr<Device>(
+        new STM32Serial(defaultSerial, defaultSerialSpeed,
+                        defaultSerialFlowctrl ? STM32Serial::RTSCTS
+                                              : STM32Serial::NOFLOWCTRL)));
+}
+
+void bspInit2()
+{
+#ifdef WITH_FILESYSTEM
+    basicFilesystemSetup(SDIODriver::instance());
+#endif  // WITH_FILESYSTEM
+}
+
+//
+// Shutdown and reboot
+//
+
+/**
+ * @brief This function disables filesystem and serial port.
+ */
+void shutdown()
+{
+    ioctl(STDOUT_FILENO, IOCTL_SYNC, 0);
+
+#ifdef WITH_FILESYSTEM
+    FilesystemManager::instance().umountAll();
+#endif  // WITH_FILESYSTEM
+
+    disableInterrupts();
+
+    while (true)
+        ;
+}
+
+void reboot()
+{
+    ioctl(STDOUT_FILENO, IOCTL_SYNC, 0);
+
+#ifdef WITH_FILESYSTEM
+    FilesystemManager::instance().umountAll();
+#endif  // WITH_FILESYSTEM
+
+    disableInterrupts();
+    miosix_private::IRQsystemReboot();
+}
+
+}  // namespace miosix
diff --git a/src/bsps/stm32f429zi_parafoil/interfaces-impl/bsp_impl.h b/src/bsps/stm32f429zi_parafoil/interfaces-impl/bsp_impl.h
new file mode 100644
index 000000000..15913a0da
--- /dev/null
+++ b/src/bsps/stm32f429zi_parafoil/interfaces-impl/bsp_impl.h
@@ -0,0 +1,48 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * 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.
+ */
+
+/***********************************************************************
+ * bsp_impl.h Part of the Miosix Embedded OS.
+ * Board support package, this file initializes hardware.
+ ************************************************************************/
+
+#ifndef BSP_IMPL_H
+#define BSP_IMPL_H
+
+#include "config/miosix_settings.h"
+#include "drivers/stm32_hardware_rng.h"
+#include "hwmapping.h"
+#include "interfaces/gpio.h"
+
+namespace miosix
+{
+
+/**
+ * \internal
+ * Called by stage_1_boot.cpp to enable the SDRAM before initializing .data/.bss
+ * Requires the CPU clock to be already configured (running from the PLL)
+ */
+void configureSdram();
+
+}  // namespace miosix
+
+#endif  // BSP_IMPL_H
diff --git a/src/bsps/stm32f429zi_parafoil/interfaces-impl/hwmapping.h b/src/bsps/stm32f429zi_parafoil/interfaces-impl/hwmapping.h
new file mode 100644
index 000000000..4002b6c91
--- /dev/null
+++ b/src/bsps/stm32f429zi_parafoil/interfaces-impl/hwmapping.h
@@ -0,0 +1,123 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include "interfaces/gpio.h"
+
+namespace miosix
+{
+
+namespace interfaces
+{
+
+namespace spi1
+{
+using sck  = Gpio<GPIOA_BASE, 5>;
+using miso = Gpio<GPIOB_BASE, 4>;
+using mosi = Gpio<GPIOA_BASE, 7>;
+}  // namespace spi1
+
+namespace spi4
+{
+using sck  = Gpio<GPIOE_BASE, 2>;
+using miso = Gpio<GPIOE_BASE, 5>;
+using mosi = Gpio<GPIOE_BASE, 6>;
+}  // namespace spi4
+
+// USB UART
+namespace uart1
+{
+using tx = Gpio<GPIOA_BASE, 9>;
+using rx = Gpio<GPIOA_BASE, 10>;
+}  // namespace uart1
+
+// GPS UART
+namespace uart2
+{
+using tx = Gpio<GPIOA_BASE, 2>;
+using rx = Gpio<GPIOA_BASE, 3>;
+}  // namespace uart2
+
+namespace timers
+{
+using tim4ch2  = Gpio<GPIOB_BASE, 7>;  // Servo 1
+using tim10ch1 = Gpio<GPIOF_BASE, 6>;  // Servo 2
+}  // namespace timers
+
+}  // namespace interfaces
+
+namespace sensors
+{
+
+namespace mpu9250
+{
+using cs   = Gpio<GPIOB_BASE, 2>;
+using sck  = interfaces::spi1::sck;
+using miso = interfaces::spi1::miso;
+using mosi = interfaces::spi1::mosi;
+}  // namespace mpu9250
+
+namespace bme280
+{
+using cs   = Gpio<GPIOC_BASE, 11>;
+using sck  = interfaces::spi1::sck;
+using miso = interfaces::spi1::miso;
+using mosi = interfaces::spi1::mosi;
+}  // namespace bme280
+
+namespace gps
+{
+using tx = interfaces::uart2::tx;
+using rx = interfaces::uart2::rx;
+}  // namespace gps
+
+namespace adc
+{
+using battery = Gpio<GPIOC_BASE, 3>;
+}
+
+}  // namespace sensors
+
+namespace sx1278
+{
+using cs        = Gpio<GPIOC_BASE, 1>;
+using interrupt = Gpio<GPIOF_BASE, 10>;
+using sck       = interfaces::spi4::sck;
+using miso      = interfaces::spi4::miso;
+using mosi      = interfaces::spi4::mosi;
+}  // namespace sx1278
+
+namespace servos
+{
+using servo1 = interfaces::timers::tim4ch2;
+using servo2 = interfaces::timers::tim10ch1;
+}  // namespace servos
+
+namespace ui
+{
+using button   = Gpio<GPIOA_BASE, 0>;   // User button
+using greenLed = Gpio<GPIOG_BASE, 13>;  // Green LED
+using redLed   = Gpio<GPIOG_BASE, 14>;  // Red LED
+}  // namespace ui
+
+}  // namespace miosix
diff --git a/src/bsps/stm32f429zi_parafoil/stm32_2m+256k_rom.ld b/src/bsps/stm32f429zi_parafoil/stm32_2m+256k_rom.ld
new file mode 100644
index 000000000..cfcc2ce53
--- /dev/null
+++ b/src/bsps/stm32f429zi_parafoil/stm32_2m+256k_rom.ld
@@ -0,0 +1,181 @@
+/*
+ * C++ enabled linker script for stm32 (2M FLASH, 256K RAM)
+ * Developed by TFT: Terraneo Federico Technologies
+ * Optimized for use with the Miosix kernel
+ */
+
+/*
+ * This chip has an unusual quirk that the RAM is divided in two block mapped
+ * at two non contiguous memory addresses. I don't know why they've done that,
+ * probably doing the obvious thing would have made writing code too easy...
+ * Anyway, since hardware can't be changed, we've got to live with that and
+ * try to make use of both RAMs.
+ * 
+ * Given the constraints above, this linker script puts:
+ * - read only data and code (.text, .rodata, .eh_*) in FLASH
+ * - the 512Byte main (IRQ) stack, .data and .bss in the "small" 64KB RAM
+ * - stacks and heap in the "large" 192KB RAM.
+ * 
+ * Unfortunately thread stacks can't be put in the small RAM as Miosix
+ * allocates them inside the heap.
+ */
+
+/*
+ * The main stack is used for interrupt handling by the kernel.
+ *
+ * *** Readme ***
+ * This linker script places the main stack (used by the kernel for interrupts)
+ * at the bottom of the ram, instead of the top. This is done for two reasons:
+ *
+ * - as an optimization for microcontrollers with little ram memory. In fact
+ *   the implementation of malloc from newlib requests memory to the OS in 4KB
+ *   block (except the first block that can be smaller). This is probably done
+ *   for compatibility with OSes with an MMU and paged memory. To see why this
+ *   is bad, consider a microcontroller with 8KB of ram: when malloc finishes
+ *   up the first 4KB it will call _sbrk_r asking for a 4KB block, but this will
+ *   fail because the top part of the ram is used by the main stack. As a
+ *   result, the top part of the memory will not be used by malloc, even if
+ *   available (and it is nearly *half* the ram on an 8KB mcu). By placing the
+ *   main stack at the bottom of the ram, the upper 4KB block will be entirely
+ *   free and available as heap space.
+ *
+ * - In case of main stack overflow the cpu will fault because access to memory
+ *   before the beginning of the ram faults. Instead with the default stack
+ *   placement the main stack will silently collide with the heap.
+ * Note: if increasing the main stack size also increase the ORIGIN value in
+ * the MEMORY definitions below accordingly.
+ */
+_main_stack_size = 0x00000200;                     /* main stack = 512Bytes */
+_main_stack_top  = 0x10000000 + _main_stack_size;
+ASSERT(_main_stack_size   % 8 == 0, "MAIN stack size error");
+
+/* Mapping the heap into the large 192KB RAM */
+_end =      0x20000000;
+_heap_end = 0x20030000;                            /* end of available ram  */
+
+/* identify the Entry Point  */
+ENTRY(_Z13Reset_Handlerv)
+
+/* specify the memory areas  */
+MEMORY
+{
+    flash(rx)    : ORIGIN = 0x08000000, LENGTH =   2M
+    /*
+     * Note, the small ram starts at 0x10000000 but it is necessary to add the
+     * size of the main stack, so it is 0x10000200.
+     */
+    smallram(wx) : ORIGIN = 0x10000200, LENGTH =  64K-0x200 
+    largeram(wx) : ORIGIN = 0x20000000, LENGTH = 192K
+    backupram(rw): ORIGIN = 0x40024000, LENGTH =  4K
+}
+
+/* now define the output sections  */
+SECTIONS
+{
+    . = 0;
+    
+    /* .text section: code goes to flash */
+    .text :
+    {
+        /* Startup code must go at address 0 */
+        KEEP(*(.isr_vector))
+        
+        *(.text)
+        *(.text.*)
+        *(.gnu.linkonce.t.*)
+        /* these sections for thumb interwork? */
+        *(.glue_7)
+        *(.glue_7t)
+        /* these sections for C++? */
+        *(.gcc_except_table)
+        *(.gcc_except_table.*)
+        *(.ARM.extab*)
+        *(.gnu.linkonce.armextab.*)
+
+        . = ALIGN(4);
+        /* .rodata: constant data */
+        *(.rodata)
+        *(.rodata.*)
+        *(.gnu.linkonce.r.*)
+
+        /* C++ Static constructors/destructors (eabi) */
+        . = ALIGN(4);
+        KEEP(*(.init))
+        
+        . = ALIGN(4);
+        __miosix_init_array_start = .;
+        KEEP (*(SORT(.miosix_init_array.*)))
+        KEEP (*(.miosix_init_array))
+        __miosix_init_array_end = .;
+
+        . = ALIGN(4);
+        __preinit_array_start = .;
+        KEEP (*(.preinit_array))
+        __preinit_array_end = .;
+
+        . = ALIGN(4);
+        __init_array_start = .;
+        KEEP (*(SORT(.init_array.*)))
+        KEEP (*(.init_array))
+        __init_array_end = .;
+
+        . = ALIGN(4);
+        KEEP(*(.fini))
+
+        . = ALIGN(4);
+        __fini_array_start = .;
+        KEEP (*(.fini_array))
+        KEEP (*(SORT(.fini_array.*)))
+        __fini_array_end = .;
+
+        /* C++ Static constructors/destructors (elf)  */
+        . = ALIGN(4);
+        _ctor_start = .;
+        KEEP (*crtbegin.o(.ctors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .ctors))
+        KEEP (*(SORT(.ctors.*)))
+        KEEP (*crtend.o(.ctors))
+       _ctor_end = .;
+
+        . = ALIGN(4);
+        KEEP (*crtbegin.o(.dtors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .dtors))
+        KEEP (*(SORT(.dtors.*)))
+        KEEP (*crtend.o(.dtors))
+    } > flash
+
+    /* .ARM.exidx is sorted, so has to go in its own output section.  */
+    __exidx_start = .;
+    .ARM.exidx :
+    {
+        *(.ARM.exidx* .gnu.linkonce.armexidx.*)
+    } > flash
+    __exidx_end = .;
+
+	/* .data section: global variables go to ram, but also store a copy to
+       flash to initialize them */
+    .data : ALIGN(8)
+    {
+        _data = .;
+        *(.data)
+        *(.data.*)
+        *(.gnu.linkonce.d.*)
+        . = ALIGN(8);
+        _edata = .;
+    } > smallram AT > flash
+    _etext = LOADADDR(.data);
+
+    /* .bss section: uninitialized global variables go to ram */
+    _bss_start = .;
+    .bss :
+    {
+        *(.bss)
+        *(.bss.*)
+        *(.gnu.linkonce.b.*)
+        . = ALIGN(8);
+    } > smallram
+    _bss_end = .;
+
+    /*_end = .;*/
+    /*PROVIDE(end = .);*/
+}
diff --git a/src/bsps/stm32f429zi_parafoil/stm32_2m+8m_xram.ld b/src/bsps/stm32f429zi_parafoil/stm32_2m+8m_xram.ld
new file mode 100644
index 000000000..b6a0d2668
--- /dev/null
+++ b/src/bsps/stm32f429zi_parafoil/stm32_2m+8m_xram.ld
@@ -0,0 +1,178 @@
+/*
+ * C++ enabled linker script for stm32 (2M FLASH, 256K RAM, 8M XRAM)
+ * Developed by TFT: Terraneo Federico Technologies
+ * Optimized for use with the Miosix kernel
+ */
+
+/*
+ * This chip has an unusual quirk that the RAM is divided in two block mapped
+ * at two non contiguous memory addresses. I don't know why they've done that,
+ * probably doing the obvious thing would have made writing code too easy...
+ * Anyway, since hardware can't be changed, we've got to live with that and
+ * try to make use of both RAMs.
+ * 
+ * Given the constraints above, this linker script puts:
+ * - read only data and code (.text, .rodata, .eh_*) in FLASH
+ * - the 512Byte main (IRQ) stack, .data and .bss in the "small" 64KB RAM
+ * - .data, .bss, stacks and heap in the external 8MB SDRAM.
+ */
+
+/*
+ * The main stack is used for interrupt handling by the kernel.
+ *
+ * *** Readme ***
+ * This linker script places the main stack (used by the kernel for interrupts)
+ * at the bottom of the ram, instead of the top. This is done for two reasons:
+ *
+ * - as an optimization for microcontrollers with little ram memory. In fact
+ *   the implementation of malloc from newlib requests memory to the OS in 4KB
+ *   block (except the first block that can be smaller). This is probably done
+ *   for compatibility with OSes with an MMU and paged memory. To see why this
+ *   is bad, consider a microcontroller with 8KB of ram: when malloc finishes
+ *   up the first 4KB it will call _sbrk_r asking for a 4KB block, but this will
+ *   fail because the top part of the ram is used by the main stack. As a
+ *   result, the top part of the memory will not be used by malloc, even if
+ *   available (and it is nearly *half* the ram on an 8KB mcu). By placing the
+ *   main stack at the bottom of the ram, the upper 4KB block will be entirely
+ *   free and available as heap space.
+ *
+ * - In case of main stack overflow the cpu will fault because access to memory
+ *   before the beginning of the ram faults. Instead with the default stack
+ *   placement the main stack will silently collide with the heap.
+ * Note: if increasing the main stack size also increase the ORIGIN value in
+ * the MEMORY definitions below accordingly.
+ */
+_main_stack_size = 0x00000200;                     /* main stack = 512Bytes */
+_main_stack_top  = 0x10000000 + _main_stack_size;
+ASSERT(_main_stack_size   % 8 == 0, "MAIN stack size error");
+
+/* Mapping the heap into XRAM */
+_heap_end = 0xd0800000;                            /* end of available ram  */
+
+/* identify the Entry Point  */
+ENTRY(_Z13Reset_Handlerv)
+
+/* specify the memory areas  */
+MEMORY
+{
+    flash(rx)    : ORIGIN = 0x08000000, LENGTH =   2M
+    /*
+     * Note, the small ram starts at 0x10000000 but it is necessary to add the
+     * size of the main stack, so it is 0x10000200.
+     */
+    smallram(wx) : ORIGIN = 0x10000200, LENGTH =  64K-0x200 
+    largeram(wx) : ORIGIN = 0x20000000, LENGTH = 192K
+    backupram(rw): ORIGIN = 0x40024000, LENGTH =   4K
+    xram(wx)     : ORIGIN = 0xd0000000, LENGTH =   8M
+}
+
+/* now define the output sections  */
+SECTIONS
+{
+    . = 0;
+    
+    /* .text section: code goes to flash */
+    .text :
+    {
+        /* Startup code must go at address 0 */
+        KEEP(*(.isr_vector))
+        
+        *(.text)
+        *(.text.*)
+        *(.gnu.linkonce.t.*)
+        /* these sections for thumb interwork? */
+        *(.glue_7)
+        *(.glue_7t)
+        /* these sections for C++? */
+        *(.gcc_except_table)
+        *(.gcc_except_table.*)
+        *(.ARM.extab*)
+        *(.gnu.linkonce.armextab.*)
+
+        . = ALIGN(4);
+        /* .rodata: constant data */
+        *(.rodata)
+        *(.rodata.*)
+        *(.gnu.linkonce.r.*)
+
+        /* C++ Static constructors/destructors (eabi) */
+        . = ALIGN(4);
+        KEEP(*(.init))
+        
+        . = ALIGN(4);
+        __miosix_init_array_start = .;
+        KEEP (*(SORT(.miosix_init_array.*)))
+        KEEP (*(.miosix_init_array))
+        __miosix_init_array_end = .;
+
+        . = ALIGN(4);
+        __preinit_array_start = .;
+        KEEP (*(.preinit_array))
+        __preinit_array_end = .;
+
+        . = ALIGN(4);
+        __init_array_start = .;
+        KEEP (*(SORT(.init_array.*)))
+        KEEP (*(.init_array))
+        __init_array_end = .;
+
+        . = ALIGN(4);
+        KEEP(*(.fini))
+
+        . = ALIGN(4);
+        __fini_array_start = .;
+        KEEP (*(.fini_array))
+        KEEP (*(SORT(.fini_array.*)))
+        __fini_array_end = .;
+
+        /* C++ Static constructors/destructors (elf)  */
+        . = ALIGN(4);
+        _ctor_start = .;
+        KEEP (*crtbegin.o(.ctors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .ctors))
+        KEEP (*(SORT(.ctors.*)))
+        KEEP (*crtend.o(.ctors))
+       _ctor_end = .;
+
+        . = ALIGN(4);
+        KEEP (*crtbegin.o(.dtors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .dtors))
+        KEEP (*(SORT(.dtors.*)))
+        KEEP (*crtend.o(.dtors))
+    } > flash
+
+    /* .ARM.exidx is sorted, so has to go in its own output section.  */
+    __exidx_start = .;
+    .ARM.exidx :
+    {
+        *(.ARM.exidx* .gnu.linkonce.armexidx.*)
+    } > flash
+    __exidx_end = .;
+
+	/* .data section: global variables go to ram, but also store a copy to
+       flash to initialize them */
+    .data : ALIGN(8)
+    {
+        _data = .;
+        *(.data)
+        *(.data.*)
+        *(.gnu.linkonce.d.*)
+        . = ALIGN(8);
+        _edata = .;
+    } > xram AT > flash
+    _etext = LOADADDR(.data);
+
+    /* .bss section: uninitialized global variables go to ram */
+    _bss_start = .;
+    .bss :
+    {
+        *(.bss)
+        *(.bss.*)
+        *(.gnu.linkonce.b.*)
+        . = ALIGN(8);
+    } > xram
+    _bss_end = .;
+
+    _end = .;
+    PROVIDE(end = .);
+}
diff --git a/src/bsps/stm32f429zi_pyxis_auxiliary/config/board_options.cmake b/src/bsps/stm32f429zi_pyxis_auxiliary/config/board_options.cmake
new file mode 100644
index 000000000..cb77b98dd
--- /dev/null
+++ b/src/bsps/stm32f429zi_pyxis_auxiliary/config/board_options.cmake
@@ -0,0 +1,114 @@
+# Copyright (C) 2023 by Skyward
+#
+# This program is free software; you can redistribute it and/or 
+# it under the terms of the GNU General Public License as published 
+# the Free Software Foundation; either version 2 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.
+#
+# As a special exception, if other files instantiate templates or use
+# macros or inline functions from this file, or you compile this file
+# and link it with other works to produce a work based on this file,
+# this file does not by itself cause the resulting work to be covered
+# by the GNU General Public License. However the source code for this
+# file must still be made available in accordance with the GNU 
+# Public License. This exception does not invalidate any other 
+# why a work based on this file might be covered by the GNU General
+# Public License.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, see <http://www.gnu.org/licenses/>
+
+set(BOARD_NAME stm32f429zi_pyxis_auxiliary)
+set(ARCH_NAME cortexM4_stm32f4)
+
+# Base directories with header files for this board
+set(ARCH_PATH ${KPATH}/arch/${ARCH_NAME}/common)
+set(BOARD_PATH ${BOARDCORE_PATH}/src/bsps/${BOARD_NAME})
+set(BOARD_CONFIG_PATH ${BOARDCORE_PATH}/src/bsps/${BOARD_NAME}/config)
+
+# Specify where to find the board specific config/miosix_settings.h
+set(BOARD_MIOSIX_SETTINGS_PATH ${BOARD_PATH})
+
+# Specify where to find the board specific config/mxgui_settings.h
+set(BOARD_MXGUI_SETTINGS_PATH ${BOARD_PATH})
+
+# Optimization flags:
+# -O0 do no optimization, the default if no optimization level is specified
+# -O or -O1 optimize minimally
+# -O2 optimize more
+# -O3 optimize even more
+# -Ofast optimize very aggressively to the point of breaking the standard
+# -Og Optimize debugging experience, enables optimizations that do not
+# interfere with debugging
+# -Os Optimize for size with -O2 optimizations that do not increase code size
+set(OPT_OPTIMIZATION -O2)
+
+# Boot and linker files
+set(BOOT_FILE ${BOARD_PATH}/core/stage_1_boot.cpp)
+set(LINKER_SCRIPT ${BOARD_PATH}/stm32_2m+256k_rom.ld)
+
+# Uncommenting __ENABLE_XRAM enables the initialization of the external
+# 8MB SDRAM memory. Do not uncomment this even if you don't use a linker
+# script that requires it, as it is used for the LCD framebuffer.
+set(XRAM -D__ENABLE_XRAM)
+
+# Select clock frequency. 
+# Warning: the default clock frequency for
+# this board is 168MHz and not 180MHz because, due to a limitation in
+# the PLL, it is not possible to generate a precise 48MHz output when
+# running the core at 180MHz. If 180MHz is chosen the USB peripheral will
+# NOT WORK and the SDIO and RNG will run ~6% slower (45MHz insteand of 48)
+# Define USE_INTERNAL_CLOCK to bypass the external oscillator
+# set(CLOCK_FREQ -DHSE_VALUE=8000000 -DSYSCLK_FREQ_180MHz=180000000)
+set(CLOCK_FREQ -DHSE_VALUE=8000000 -DSYSCLK_FREQ_168MHz=168000000)
+# set(CLOCK_FREQ -DHSE_VALUE=8000000 -DSYSCLK_FREQ_100MHz=100000000)
+
+# C++ Exception/rtti support disable flags.
+# To save code size if not using C++ exceptions (nor some STL code which
+# implicitly uses it) uncomment this option.
+# -D__NO_EXCEPTIONS is used by Miosix to know if exceptions are used.
+# set(OPT_EXCEPT -fno-exceptions -fno-rtti -D__NO_EXCEPTIONS)
+
+# Specify a custom flash command
+# This is the program that is invoked when the flash flag (-f or --flash) is
+# used with the Miosix Build System. Use $binary or $hex as placeolders, they
+# will be replaced by the build systems with the binary or hex file repectively.
+# If a command is not specified, the build system will use st-flash if found
+# set(PROGRAM_CMDLINE "here your custom flash command")
+
+# Basic flags
+set(FLAGS_BASE -mcpu=cortex-m4 -mthumb -mfloat-abi=hard -mfpu=fpv4-sp-d16)
+
+# Flags for ASM and linker
+set(AFLAGS_BASE ${FLAGS_BASE})
+set(LFLAGS_BASE ${FLAGS_BASE} -Wl,--gc-sections,-Map,main.map -Wl,-T${LINKER_SCRIPT} ${OPT_EXCEPT} ${OPT_OPTIMIZATION} -nostdlib)
+
+# Flags for C/C++
+string(TOUPPER ${BOARD_NAME} BOARD_UPPER)
+set(CFLAGS_BASE
+    -D_BOARD_${BOARD_UPPER} -D_MIOSIX_BOARDNAME=\"${BOARD_NAME}\"
+    -D_DEFAULT_SOURCE=1 -ffunction-sections -Wall -Werror=return-type -g
+    -D_ARCH_CORTEXM4_STM32F4
+    ${CLOCK_FREQ} ${XRAM} ${SRAM_BOOT} ${FLAGS_BASE} ${OPT_OPTIMIZATION} -c
+)
+set(CXXFLAGS_BASE ${CFLAGS_BASE} ${OPT_EXCEPT})
+
+# Select architecture specific files
+set(ARCH_SRC
+    ${ARCH_PATH}/interfaces-impl/delays.cpp
+    ${ARCH_PATH}/interfaces-impl/gpio_impl.cpp
+    ${ARCH_PATH}/interfaces-impl/portability.cpp
+    ${BOARD_PATH}/interfaces-impl/bsp.cpp
+    ${KPATH}/arch/common/CMSIS/Device/ST/STM32F4xx/Source/Templates/system_stm32f4xx.c
+    ${KPATH}/arch/common/core/interrupts_cortexMx.cpp
+    ${KPATH}/arch/common/core/mpu_cortexMx.cpp
+    ${KPATH}/arch/common/core/stm32f2_f4_l4_f7_h7_os_timer.cpp
+    ${KPATH}/arch/common/drivers/sd_stm32f2_f4_f7.cpp
+    ${KPATH}/arch/common/drivers/serial_stm32.cpp
+    ${KPATH}/arch/common/drivers/stm32_hardware_rng.cpp
+)
diff --git a/src/bsps/stm32f429zi_pyxis_auxiliary/config/board_settings.h b/src/bsps/stm32f429zi_pyxis_auxiliary/config/board_settings.h
new file mode 100644
index 000000000..961c15809
--- /dev/null
+++ b/src/bsps/stm32f429zi_pyxis_auxiliary/config/board_settings.h
@@ -0,0 +1,64 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * 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.
+ */
+
+#ifndef BOARD_SETTINGS_H
+#define BOARD_SETTINGS_H
+
+#include "util/version.h"
+
+/**
+ * \internal
+ * Versioning for board_settings.h for out of git tree projects
+ */
+#define BOARD_SETTINGS_VERSION 300
+
+namespace miosix
+{
+
+/**
+ * \addtogroup Settings
+ * \{
+ */
+
+/// Size of stack for main().
+/// The C standard library is stack-heavy (iprintf requires 1KB) but the
+/// STM32F407VG only has 192KB of RAM so there is room for a big 4K stack.
+const unsigned int MAIN_STACK_SIZE = 16 * 1024;
+
+/// Serial port
+const unsigned int defaultSerial      = 4;
+const unsigned int defaultSerialSpeed = 115200;
+const bool defaultSerialFlowctrl      = false;
+
+// SD card driver
+static const unsigned char sdVoltage = 33;  // Board powered @ 3.3V
+
+/// Analog supply voltage for ADC, DAC, Reset blocks, RCs and PLL
+#define V_DDA_VOLTAGE 3.3f
+
+/**
+ * \}
+ */
+
+}  // namespace miosix
+
+#endif /* BOARD_SETTINGS_H */
diff --git a/src/bsps/stm32f429zi_pyxis_auxiliary/config/miosix_settings.h b/src/bsps/stm32f429zi_pyxis_auxiliary/config/miosix_settings.h
new file mode 100644
index 000000000..5006e0bca
--- /dev/null
+++ b/src/bsps/stm32f429zi_pyxis_auxiliary/config/miosix_settings.h
@@ -0,0 +1,240 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * 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.
+ */
+
+#pragma once
+
+// Before you can compile the kernel you have to configure it by editing this
+// file. After that, comment out this line to disable the reminder error.
+// The PARSING_FROM_IDE is because Netbeans gets confused by this, it is never
+// defined when compiling the code.
+#ifndef PARSING_FROM_IDE
+//#error This error is a reminder that you have not edited miosix_settings.h
+// yet.
+#endif  // PARSING_FROM_IDE
+
+/**
+ * \file miosix_settings.h
+ * NOTE: this file contains ONLY configuration options that are not dependent
+ * on architecture specific details. The other options are in the following
+ * files which are included here:
+ * miosix/arch/architecture name/common/arch_settings.h
+ * miosix/config/arch/architecture name/board name/board_settings.h
+ */
+#include "arch_settings.h"
+#include "board_settings.h"
+#include "util/version.h"
+
+/**
+ * \internal
+ * Versioning for miosix_settings.h for out of git tree projects
+ */
+#define MIOSIX_SETTINGS_VERSION 300
+
+namespace miosix
+{
+
+/**
+ * \addtogroup Settings
+ * \{
+ */
+
+//
+// Scheduler options
+//
+
+/// \def SCHED_TYPE_PRIORITY
+/// If uncommented selects the priority scheduler
+/// \def SCHED_TYPE_CONTROL_BASED
+/// If uncommented selects the control based scheduler
+/// \def SCHED_TYPE_EDF
+/// If uncommented selects the EDF scheduler
+// Uncomment only *one* of those
+
+#define SCHED_TYPE_PRIORITY
+//#define SCHED_TYPE_CONTROL_BASED
+//#define SCHED_TYPE_EDF
+
+/// \def WITH_CPU_TIME_COUNTER
+/// Allows to enable/disable CPUTimeCounter to save code size and remove its
+/// overhead from the scheduling process. By default it is not defined
+/// (CPUTimeCounter is disabled).
+//#define WITH_CPU_TIME_COUNTER
+
+//
+// Filesystem options
+//
+
+/// \def WITH_FILESYSTEM
+/// Allows to enable/disable filesystem support to save code size
+/// By default it is defined (filesystem support is enabled)
+#define WITH_FILESYSTEM
+
+/// \def WITH_DEVFS
+/// Allows to enable/disable DevFs support to save code size
+/// By default it is defined (DevFs is enabled)
+#define WITH_DEVFS
+
+/// \def SYNC_AFTER_WRITE
+/// Increases filesystem write robustness. After each write operation the
+/// filesystem is synced so that a power failure happens data is not lost
+/// (unless power failure happens exactly between the write and the sync)
+/// Unfortunately write latency and throughput becomes twice as worse
+/// By default it is defined (slow but safe)
+#define SYNC_AFTER_WRITE
+
+/// Maximum number of open files. Trying to open more will fail.
+/// Cannot be lower than 3, as the first three are stdin, stdout, stderr
+const unsigned char MAX_OPEN_FILES = 8;
+
+/// \def WITH_PROCESSES
+/// If uncommented enables support for processes as well as threads.
+/// This enables the dynamic loader to load elf programs, the extended system
+/// call service and, if the hardware supports it, the MPU to provide memory
+/// isolation of processes
+//#define WITH_PROCESSES
+
+#if defined(WITH_PROCESSES) && defined(__NO_EXCEPTIONS)
+#error Processes require C++ exception support
+#endif  // defined(WITH_PROCESSES) && defined(__NO_EXCEPTIONS)
+
+#if defined(WITH_PROCESSES) && !defined(WITH_FILESYSTEM)
+#error Processes require filesystem support
+#endif  // defined(WITH_PROCESSES) && !defined(WITH_FILESYSTEM)
+
+#if defined(WITH_PROCESSES) && !defined(WITH_DEVFS)
+#error Processes require devfs support
+#endif  // defined(WITH_PROCESSES) && !defined(WITH_DEVFS)
+
+//
+// C/C++ standard library I/O (stdin, stdout and stderr related)
+//
+
+/// \def WITH_BOOTLOG
+/// Uncomment to print bootlogs on stdout.
+/// By default it is defined (bootlogs are printed)
+#define WITH_BOOTLOG
+
+/// \def WITH_ERRLOG
+/// Uncomment for debug information on stdout.
+/// By default it is defined (error information is printed)
+#define WITH_ERRLOG
+
+//
+// Kernel related options (stack sizes, priorities)
+//
+
+/// \def WITH_DEEP_SLEEP
+/// Adds interfaces and required variables to support deep sleep state switch
+/// automatically when peripherals are not required
+//#define WITH_DEEP_SLEEP
+
+/**
+ * \def JTAG_DISABLE_SLEEP
+ * JTAG debuggers lose communication with the device if it enters sleep
+ * mode, so to use debugging it is necessary to disable sleep in the idle
+ * thread. By default it is not defined (idle thread calls sleep).
+ */
+//#define JTAG_DISABLE_SLEEP
+
+#if defined(WITH_DEEP_SLEEP) && defined(JTAG_DISABLE_SLEEP)
+#error Deep sleep cannot work together with jtag
+#endif  // defined(WITH_PROCESSES) && !defined(WITH_DEVFS)
+
+/// Minimum stack size (MUST be divisible by 4)
+const unsigned int STACK_MIN = 256;
+
+/// \internal Size of idle thread stack.
+/// Should be >=STACK_MIN (MUST be divisible by 4)
+const unsigned int STACK_IDLE = 256;
+
+/// Default stack size for pthread_create.
+/// The chosen value is enough to call C standard library functions
+/// such as printf/fopen which are stack-heavy
+const unsigned int STACK_DEFAULT_FOR_PTHREAD = 2048;
+
+/// Maximum size of the RAM image of a process. If a program requires more
+/// the kernel will not run it (MUST be divisible by 4)
+const unsigned int MAX_PROCESS_IMAGE_SIZE = 64 * 1024;
+
+/// Minimum size of the stack for a process. If a program specifies a lower
+/// size the kernel will not run it (MUST be divisible by 4)
+const unsigned int MIN_PROCESS_STACK_SIZE = STACK_MIN;
+
+/// Every userspace thread has two stacks, one for when it is running in
+/// userspace and one for when it is running in kernelspace (that is, while it
+/// is executing system calls). This is the size of the stack for when the
+/// thread is running in kernelspace (MUST be divisible by 4)
+const unsigned int SYSTEM_MODE_PROCESS_STACK_SIZE = 2 * 1024;
+
+/// Number of priorities (MUST be >1)
+/// PRIORITY_MAX-1 is the highest priority, 0 is the lowest. -1 is reserved as
+/// the priority of the idle thread.
+/// The meaning of a thread's priority depends on the chosen scheduler.
+#ifdef SCHED_TYPE_PRIORITY
+// Can be modified, but a high value makes context switches more expensive
+const short int PRIORITY_MAX = 4;
+#elif defined(SCHED_TYPE_CONTROL_BASED)
+// Don't touch, the limit is due to the fixed point implementation
+// It's not needed for if floating point is selected, but kept for consistency
+const short int PRIORITY_MAX = 64;
+#else  // SCHED_TYPE_EDF
+// Doesn't exist for this kind of scheduler
+#endif
+
+/// Priority of main()
+/// The meaning of a thread's priority depends on the chosen scheduler.
+const unsigned char MAIN_PRIORITY = 1;
+
+#ifdef SCHED_TYPE_PRIORITY
+/// Maximum thread time slice in nanoseconds, after which preemption occurs
+const unsigned int MAX_TIME_SLICE = 1000000;
+#endif  // SCHED_TYPE_PRIORITY
+
+//
+// Other low level kernel options. There is usually no need to modify these.
+//
+
+/// \internal Length of wartermark (in bytes) to check stack overflow.
+/// MUST be divisible by 4 and can also be zero.
+/// A high value increases context switch time.
+const unsigned int WATERMARK_LEN = 16;
+
+/// \internal Used to fill watermark
+const unsigned int WATERMARK_FILL = 0xaaaaaaaa;
+
+/// \internal Used to fill stack (for checking stack usage)
+const unsigned int STACK_FILL = 0xbbbbbbbb;
+
+// Compiler version checks
+#if !defined(_MIOSIX_GCC_PATCH_MAJOR) || _MIOSIX_GCC_PATCH_MAJOR < 3
+#error \
+    "You are using a too old or unsupported compiler. Get the latest one from https://miosix.org/wiki/index.php?title=Miosix_Toolchain"
+#endif
+#if _MIOSIX_GCC_PATCH_MAJOR > 3
+#warning "You are using a too new compiler, which may not be supported"
+#endif
+
+/**
+ * \}
+ */
+
+}  // namespace miosix
diff --git a/src/bsps/stm32f429zi_pyxis_auxiliary/core/stage_1_boot.cpp b/src/bsps/stm32f429zi_pyxis_auxiliary/core/stage_1_boot.cpp
new file mode 100644
index 000000000..c5b8d22f2
--- /dev/null
+++ b/src/bsps/stm32f429zi_pyxis_auxiliary/core/stage_1_boot.cpp
@@ -0,0 +1,430 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * 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 <string.h>
+
+#include "core/interrupts.h"  //For the unexpected interrupt call
+#include "interfaces/arch_registers.h"
+#include "interfaces/bsp.h"
+#include "kernel/stage_2_boot.h"
+
+/*
+ * startup.cpp
+ * STM32 C++ startup.
+ * NOTE: for stm32f42x devices ONLY.
+ * Supports interrupt handlers in C++ without extern "C"
+ * Developed by Terraneo Federico, based on ST startup code.
+ * Additionally modified to boot Miosix.
+ */
+
+/**
+ * Called by Reset_Handler, performs initialization and calls main.
+ * Never returns.
+ */
+void program_startup() __attribute__((noreturn));
+void program_startup()
+{
+    // Cortex M3 core appears to get out of reset with interrupts already
+    // enabled
+    __disable_irq();
+
+    // These are defined in the linker script
+    extern unsigned char _etext asm("_etext");
+    extern unsigned char _data asm("_data");
+    extern unsigned char _edata asm("_edata");
+    extern unsigned char _bss_start asm("_bss_start");
+    extern unsigned char _bss_end asm("_bss_end");
+
+    // Initialize .data section, clear .bss section
+    unsigned char *etext     = &_etext;
+    unsigned char *data      = &_data;
+    unsigned char *edata     = &_edata;
+    unsigned char *bss_start = &_bss_start;
+    unsigned char *bss_end   = &_bss_end;
+    memcpy(data, etext, edata - data);
+    memset(bss_start, 0, bss_end - bss_start);
+
+    // Move on to stage 2
+    _init();
+
+    // If main returns, reboot
+    NVIC_SystemReset();
+    for (;;)
+        ;
+}
+
+/**
+ * Reset handler, called by hardware immediately after reset
+ */
+void Reset_Handler() __attribute__((__interrupt__, noreturn));
+void Reset_Handler()
+{
+    // SystemInit() is called *before* initializing .data and zeroing .bss
+    // Despite all startup files provided by ST do the opposite, there are three
+    // good reasons to do so:
+    // First, the CMSIS specifications say that SystemInit() must not access
+    // global variables, so it is actually possible to call it before
+    // Second, when running Miosix with the xram linker scripts .data and .bss
+    // are placed in the external RAM, so we *must* call SystemInit(), which
+    // enables xram, before touching .data and .bss
+    // Third, this is a performance improvement since the loops that initialize
+    //.data and zeros .bss now run with the CPU at full speed instead of 8MHz
+    SystemInit();
+
+    /*
+     * Initialize process stack and switch to it.
+     * This is required for booting Miosix, a small portion of the top of the
+     * heap area will be used as stack until the first thread starts. After,
+     * this stack will be abandoned and the process stack will point to the
+     * current thread's stack.
+     */
+    asm volatile(
+        "ldr r0,  =_heap_end          \n\t"
+        "msr psp, r0                  \n\t"
+        "movw r0, #2                  \n\n"  // Privileged, process stack
+        "msr control, r0              \n\t"
+        "isb                          \n\t" ::
+            : "r0");
+
+    program_startup();
+}
+
+/**
+ * All unused interrupts call this function.
+ */
+extern "C" void Default_Handler() { unexpectedInterrupt(); }
+
+// System handlers
+void /*__attribute__((weak))*/ Reset_Handler();      // These interrupts are not
+void /*__attribute__((weak))*/ NMI_Handler();        // weak because they are
+void /*__attribute__((weak))*/ HardFault_Handler();  // surely defined by Miosix
+void /*__attribute__((weak))*/ MemManage_Handler();
+void /*__attribute__((weak))*/ BusFault_Handler();
+void /*__attribute__((weak))*/ UsageFault_Handler();
+void /*__attribute__((weak))*/ SVC_Handler();
+void /*__attribute__((weak))*/ DebugMon_Handler();
+void /*__attribute__((weak))*/ PendSV_Handler();
+void __attribute__((weak)) SysTick_Handler();
+
+// Interrupt handlers
+void __attribute__((weak)) WWDG_IRQHandler();
+void __attribute__((weak)) PVD_IRQHandler();
+void __attribute__((weak)) TAMP_STAMP_IRQHandler();
+void __attribute__((weak)) RTC_WKUP_IRQHandler();
+void __attribute__((weak)) FLASH_IRQHandler();
+void __attribute__((weak)) RCC_IRQHandler();
+void __attribute__((weak)) EXTI0_IRQHandler();
+void __attribute__((weak)) EXTI1_IRQHandler();
+void __attribute__((weak)) EXTI2_IRQHandler();
+void __attribute__((weak)) EXTI3_IRQHandler();
+void __attribute__((weak)) EXTI4_IRQHandler();
+void __attribute__((weak)) DMA1_Stream0_IRQHandler();
+void __attribute__((weak)) DMA1_Stream1_IRQHandler();
+void __attribute__((weak)) DMA1_Stream2_IRQHandler();
+void __attribute__((weak)) DMA1_Stream3_IRQHandler();
+void __attribute__((weak)) DMA1_Stream4_IRQHandler();
+void __attribute__((weak)) DMA1_Stream5_IRQHandler();
+void __attribute__((weak)) DMA1_Stream6_IRQHandler();
+void __attribute__((weak)) ADC_IRQHandler();
+void __attribute__((weak)) CAN1_TX_IRQHandler();
+void __attribute__((weak)) CAN1_RX0_IRQHandler();
+void __attribute__((weak)) CAN1_RX1_IRQHandler();
+void __attribute__((weak)) CAN1_SCE_IRQHandler();
+void __attribute__((weak)) EXTI9_5_IRQHandler();
+void __attribute__((weak)) TIM1_BRK_TIM9_IRQHandler();
+void __attribute__((weak)) TIM1_UP_TIM10_IRQHandler();
+void __attribute__((weak)) TIM1_TRG_COM_TIM11_IRQHandler();
+void __attribute__((weak)) TIM1_CC_IRQHandler();
+void __attribute__((weak)) TIM2_IRQHandler();
+void __attribute__((weak)) TIM3_IRQHandler();
+void __attribute__((weak)) TIM4_IRQHandler();
+void __attribute__((weak)) I2C1_EV_IRQHandler();
+void __attribute__((weak)) I2C1_ER_IRQHandler();
+void __attribute__((weak)) I2C2_EV_IRQHandler();
+void __attribute__((weak)) I2C2_ER_IRQHandler();
+void __attribute__((weak)) SPI1_IRQHandler();
+void __attribute__((weak)) SPI2_IRQHandler();
+void __attribute__((weak)) USART1_IRQHandler();
+void __attribute__((weak)) USART2_IRQHandler();
+void __attribute__((weak)) USART3_IRQHandler();
+void __attribute__((weak)) EXTI15_10_IRQHandler();
+void __attribute__((weak)) RTC_Alarm_IRQHandler();
+void __attribute__((weak)) OTG_FS_WKUP_IRQHandler();
+void __attribute__((weak)) TIM8_BRK_TIM12_IRQHandler();
+void __attribute__((weak)) TIM8_UP_TIM13_IRQHandler();
+void __attribute__((weak)) TIM8_TRG_COM_TIM14_IRQHandler();
+void __attribute__((weak)) TIM8_CC_IRQHandler();
+void __attribute__((weak)) DMA1_Stream7_IRQHandler();
+void __attribute__((weak)) FMC_IRQHandler();
+void __attribute__((weak)) SDIO_IRQHandler();
+void __attribute__((weak)) TIM5_IRQHandler();
+void __attribute__((weak)) SPI3_IRQHandler();
+void __attribute__((weak)) UART4_IRQHandler();
+void __attribute__((weak)) UART5_IRQHandler();
+void __attribute__((weak)) TIM6_DAC_IRQHandler();
+void __attribute__((weak)) TIM7_IRQHandler();
+void __attribute__((weak)) DMA2_Stream0_IRQHandler();
+void __attribute__((weak)) DMA2_Stream1_IRQHandler();
+void __attribute__((weak)) DMA2_Stream2_IRQHandler();
+void __attribute__((weak)) DMA2_Stream3_IRQHandler();
+void __attribute__((weak)) DMA2_Stream4_IRQHandler();
+void __attribute__((weak)) ETH_IRQHandler();
+void __attribute__((weak)) ETH_WKUP_IRQHandler();
+void __attribute__((weak)) CAN2_TX_IRQHandler();
+void __attribute__((weak)) CAN2_RX0_IRQHandler();
+void __attribute__((weak)) CAN2_RX1_IRQHandler();
+void __attribute__((weak)) CAN2_SCE_IRQHandler();
+void __attribute__((weak)) OTG_FS_IRQHandler();
+void __attribute__((weak)) DMA2_Stream5_IRQHandler();
+void __attribute__((weak)) DMA2_Stream6_IRQHandler();
+void __attribute__((weak)) DMA2_Stream7_IRQHandler();
+void __attribute__((weak)) USART6_IRQHandler();
+void __attribute__((weak)) I2C3_EV_IRQHandler();
+void __attribute__((weak)) I2C3_ER_IRQHandler();
+void __attribute__((weak)) OTG_HS_EP1_OUT_IRQHandler();
+void __attribute__((weak)) OTG_HS_EP1_IN_IRQHandler();
+void __attribute__((weak)) OTG_HS_WKUP_IRQHandler();
+void __attribute__((weak)) OTG_HS_IRQHandler();
+void __attribute__((weak)) DCMI_IRQHandler();
+void __attribute__((weak)) CRYP_IRQHandler();
+void __attribute__((weak)) HASH_RNG_IRQHandler();
+void __attribute__((weak)) FPU_IRQHandler();
+void __attribute__((weak)) UART7_IRQHandler();
+void __attribute__((weak)) UART8_IRQHandler();
+void __attribute__((weak)) SPI4_IRQHandler();
+void __attribute__((weak)) SPI5_IRQHandler();
+void __attribute__((weak)) SPI6_IRQHandler();
+void __attribute__((weak)) SAI1_IRQHandler();
+void __attribute__((weak)) LTDC_IRQHandler();
+void __attribute__((weak)) LTDC_ER_IRQHandler();
+void __attribute__((weak)) DMA2D_IRQHandler();
+
+// Stack top, defined in the linker script
+extern char _main_stack_top asm("_main_stack_top");
+
+// Interrupt vectors, must be placed @ address 0x00000000
+// The extern declaration is required otherwise g++ optimizes it out
+extern void (*const __Vectors[])();
+void (*const __Vectors[])() __attribute__((section(".isr_vector"))) = {
+    reinterpret_cast<void (*)()>(&_main_stack_top), /* Stack pointer*/
+    Reset_Handler,                                  /* Reset Handler */
+    NMI_Handler,                                    /* NMI Handler */
+    HardFault_Handler,                              /* Hard Fault Handler */
+    MemManage_Handler,                              /* MPU Fault Handler */
+    BusFault_Handler,                               /* Bus Fault Handler */
+    UsageFault_Handler,                             /* Usage Fault Handler */
+    0,                                              /* Reserved */
+    0,                                              /* Reserved */
+    0,                                              /* Reserved */
+    0,                                              /* Reserved */
+    SVC_Handler,                                    /* SVCall Handler */
+    DebugMon_Handler,                               /* Debug Monitor Handler */
+    0,                                              /* Reserved */
+    PendSV_Handler,                                 /* PendSV Handler */
+    SysTick_Handler,                                /* SysTick Handler */
+
+    /* External Interrupts */
+    WWDG_IRQHandler,
+    PVD_IRQHandler,
+    TAMP_STAMP_IRQHandler,
+    RTC_WKUP_IRQHandler,
+    FLASH_IRQHandler,
+    RCC_IRQHandler,
+    EXTI0_IRQHandler,
+    EXTI1_IRQHandler,
+    EXTI2_IRQHandler,
+    EXTI3_IRQHandler,
+    EXTI4_IRQHandler,
+    DMA1_Stream0_IRQHandler,
+    DMA1_Stream1_IRQHandler,
+    DMA1_Stream2_IRQHandler,
+    DMA1_Stream3_IRQHandler,
+    DMA1_Stream4_IRQHandler,
+    DMA1_Stream5_IRQHandler,
+    DMA1_Stream6_IRQHandler,
+    ADC_IRQHandler,
+    CAN1_TX_IRQHandler,
+    CAN1_RX0_IRQHandler,
+    CAN1_RX1_IRQHandler,
+    CAN1_SCE_IRQHandler,
+    EXTI9_5_IRQHandler,
+    TIM1_BRK_TIM9_IRQHandler,
+    TIM1_UP_TIM10_IRQHandler,
+    TIM1_TRG_COM_TIM11_IRQHandler,
+    TIM1_CC_IRQHandler,
+    TIM2_IRQHandler,
+    TIM3_IRQHandler,
+    TIM4_IRQHandler,
+    I2C1_EV_IRQHandler,
+    I2C1_ER_IRQHandler,
+    I2C2_EV_IRQHandler,
+    I2C2_ER_IRQHandler,
+    SPI1_IRQHandler,
+    SPI2_IRQHandler,
+    USART1_IRQHandler,
+    USART2_IRQHandler,
+    USART3_IRQHandler,
+    EXTI15_10_IRQHandler,
+    RTC_Alarm_IRQHandler,
+    OTG_FS_WKUP_IRQHandler,
+    TIM8_BRK_TIM12_IRQHandler,
+    TIM8_UP_TIM13_IRQHandler,
+    TIM8_TRG_COM_TIM14_IRQHandler,
+    TIM8_CC_IRQHandler,
+    DMA1_Stream7_IRQHandler,
+    FMC_IRQHandler,
+    SDIO_IRQHandler,
+    TIM5_IRQHandler,
+    SPI3_IRQHandler,
+    UART4_IRQHandler,
+    UART5_IRQHandler,
+    TIM6_DAC_IRQHandler,
+    TIM7_IRQHandler,
+    DMA2_Stream0_IRQHandler,
+    DMA2_Stream1_IRQHandler,
+    DMA2_Stream2_IRQHandler,
+    DMA2_Stream3_IRQHandler,
+    DMA2_Stream4_IRQHandler,
+    ETH_IRQHandler,
+    ETH_WKUP_IRQHandler,
+    CAN2_TX_IRQHandler,
+    CAN2_RX0_IRQHandler,
+    CAN2_RX1_IRQHandler,
+    CAN2_SCE_IRQHandler,
+    OTG_FS_IRQHandler,
+    DMA2_Stream5_IRQHandler,
+    DMA2_Stream6_IRQHandler,
+    DMA2_Stream7_IRQHandler,
+    USART6_IRQHandler,
+    I2C3_EV_IRQHandler,
+    I2C3_ER_IRQHandler,
+    OTG_HS_EP1_OUT_IRQHandler,
+    OTG_HS_EP1_IN_IRQHandler,
+    OTG_HS_WKUP_IRQHandler,
+    OTG_HS_IRQHandler,
+    DCMI_IRQHandler,
+    CRYP_IRQHandler,
+    HASH_RNG_IRQHandler,
+    FPU_IRQHandler,
+    UART7_IRQHandler,
+    UART8_IRQHandler,
+    SPI4_IRQHandler,
+    SPI5_IRQHandler,
+    SPI6_IRQHandler,
+    SAI1_IRQHandler,
+    LTDC_IRQHandler,
+    LTDC_ER_IRQHandler,
+    DMA2D_IRQHandler,
+};
+
+#pragma weak SysTick_IRQHandler            = Default_Handler
+#pragma weak WWDG_IRQHandler               = Default_Handler
+#pragma weak PVD_IRQHandler                = Default_Handler
+#pragma weak TAMP_STAMP_IRQHandler         = Default_Handler
+#pragma weak RTC_WKUP_IRQHandler           = Default_Handler
+#pragma weak FLASH_IRQHandler              = Default_Handler
+#pragma weak RCC_IRQHandler                = Default_Handler
+#pragma weak EXTI0_IRQHandler              = Default_Handler
+#pragma weak EXTI1_IRQHandler              = Default_Handler
+#pragma weak EXTI2_IRQHandler              = Default_Handler
+#pragma weak EXTI3_IRQHandler              = Default_Handler
+#pragma weak EXTI4_IRQHandler              = Default_Handler
+#pragma weak DMA1_Stream0_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream1_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream2_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream3_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream4_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream5_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream6_IRQHandler       = Default_Handler
+#pragma weak ADC_IRQHandler                = Default_Handler
+#pragma weak CAN1_TX_IRQHandler            = Default_Handler
+#pragma weak CAN1_RX0_IRQHandler           = Default_Handler
+#pragma weak CAN1_RX1_IRQHandler           = Default_Handler
+#pragma weak CAN1_SCE_IRQHandler           = Default_Handler
+#pragma weak EXTI9_5_IRQHandler            = Default_Handler
+#pragma weak TIM1_BRK_TIM9_IRQHandler      = Default_Handler
+#pragma weak TIM1_UP_TIM10_IRQHandler      = Default_Handler
+#pragma weak TIM1_TRG_COM_TIM11_IRQHandler = Default_Handler
+#pragma weak TIM1_CC_IRQHandler            = Default_Handler
+#pragma weak TIM2_IRQHandler               = Default_Handler
+#pragma weak TIM3_IRQHandler               = Default_Handler
+#pragma weak TIM4_IRQHandler               = Default_Handler
+#pragma weak I2C1_EV_IRQHandler            = Default_Handler
+#pragma weak I2C1_ER_IRQHandler            = Default_Handler
+#pragma weak I2C2_EV_IRQHandler            = Default_Handler
+#pragma weak I2C2_ER_IRQHandler            = Default_Handler
+#pragma weak SPI1_IRQHandler               = Default_Handler
+#pragma weak SPI2_IRQHandler               = Default_Handler
+// #pragma weak USART1_IRQHandler             = Default_Handler
+// #pragma weak USART2_IRQHandler             = Default_Handler
+// #pragma weak USART3_IRQHandler             = Default_Handler
+#pragma weak EXTI15_10_IRQHandler          = Default_Handler
+#pragma weak RTC_Alarm_IRQHandler          = Default_Handler
+#pragma weak OTG_FS_WKUP_IRQHandler        = Default_Handler
+#pragma weak TIM8_BRK_TIM12_IRQHandler     = Default_Handler
+#pragma weak TIM8_UP_TIM13_IRQHandler      = Default_Handler
+#pragma weak TIM8_TRG_COM_TIM14_IRQHandler = Default_Handler
+#pragma weak TIM8_CC_IRQHandler            = Default_Handler
+#pragma weak DMA1_Stream7_IRQHandler       = Default_Handler
+#pragma weak FMC_IRQHandler                = Default_Handler
+#pragma weak SDIO_IRQHandler               = Default_Handler
+#pragma weak TIM5_IRQHandler               = Default_Handler
+#pragma weak SPI3_IRQHandler               = Default_Handler
+// #pragma weak UART4_IRQHandler              = Default_Handler
+// #pragma weak UART5_IRQHandler              = Default_Handler
+#pragma weak TIM6_DAC_IRQHandler     = Default_Handler
+#pragma weak TIM7_IRQHandler         = Default_Handler
+#pragma weak DMA2_Stream0_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream1_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream2_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream3_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream4_IRQHandler = Default_Handler
+#pragma weak ETH_IRQHandler          = Default_Handler
+#pragma weak ETH_WKUP_IRQHandler     = Default_Handler
+#pragma weak CAN2_TX_IRQHandler      = Default_Handler
+#pragma weak CAN2_RX0_IRQHandler     = Default_Handler
+#pragma weak CAN2_RX1_IRQHandler     = Default_Handler
+#pragma weak CAN2_SCE_IRQHandler     = Default_Handler
+#pragma weak OTG_FS_IRQHandler       = Default_Handler
+#pragma weak DMA2_Stream5_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream6_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream7_IRQHandler = Default_Handler
+// #pragma weak USART6_IRQHandler             = Default_Handler
+#pragma weak I2C3_EV_IRQHandler        = Default_Handler
+#pragma weak I2C3_ER_IRQHandler        = Default_Handler
+#pragma weak OTG_HS_EP1_OUT_IRQHandler = Default_Handler
+#pragma weak OTG_HS_EP1_IN_IRQHandler  = Default_Handler
+#pragma weak OTG_HS_WKUP_IRQHandler    = Default_Handler
+#pragma weak OTG_HS_IRQHandler         = Default_Handler
+#pragma weak DCMI_IRQHandler           = Default_Handler
+#pragma weak CRYP_IRQHandler           = Default_Handler
+#pragma weak HASH_RNG_IRQHandler       = Default_Handler
+#pragma weak FPU_IRQHandler            = Default_Handler
+// #pragma weak UART7_IRQHandler              = Default_Handler
+// #pragma weak UART8_IRQHandler              = Default_Handler
+#pragma weak SPI4_IRQHandler    = Default_Handler
+#pragma weak SPI5_IRQHandler    = Default_Handler
+#pragma weak SPI6_IRQHandler    = Default_Handler
+#pragma weak SAI1_IRQHandler    = Default_Handler
+#pragma weak LTDC_IRQHandler    = Default_Handler
+#pragma weak LTDC_ER_IRQHandler = Default_Handler
+#pragma weak DMA2D_IRQHandler   = Default_Handler
diff --git a/src/bsps/stm32f429zi_pyxis_auxiliary/interfaces-impl/arch_registers_impl.h b/src/bsps/stm32f429zi_pyxis_auxiliary/interfaces-impl/arch_registers_impl.h
new file mode 100644
index 000000000..787b322d1
--- /dev/null
+++ b/src/bsps/stm32f429zi_pyxis_auxiliary/interfaces-impl/arch_registers_impl.h
@@ -0,0 +1,34 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * 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.
+ */
+
+#ifndef ARCH_REGISTERS_IMPL_H
+#define ARCH_REGISTERS_IMPL_H
+
+// Always include stm32f4xx.h before core_cm4.h, there's some nasty dependency
+#define STM32F429xx
+#include "CMSIS/Device/ST/STM32F4xx/Include/stm32f4xx.h"
+#include "CMSIS/Device/ST/STM32F4xx/Include/system_stm32f4xx.h"
+#include "CMSIS/Include/core_cm4.h"
+
+#define RCC_SYNC() __DSB()  // Workaround for a bug in stm32f42x
+
+#endif  // ARCH_REGISTERS_IMPL_H
\ No newline at end of file
diff --git a/src/bsps/stm32f429zi_pyxis_auxiliary/interfaces-impl/bsp.cpp b/src/bsps/stm32f429zi_pyxis_auxiliary/interfaces-impl/bsp.cpp
new file mode 100644
index 000000000..025840ce8
--- /dev/null
+++ b/src/bsps/stm32f429zi_pyxis_auxiliary/interfaces-impl/bsp.cpp
@@ -0,0 +1,127 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * 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.
+ */
+
+/***********************************************************************
+ * bsp.cpp Part of the Miosix Embedded OS.
+ * Board support package, this file initializes hardware.
+ ************************************************************************/
+
+#include "interfaces/bsp.h"
+
+#include <inttypes.h>
+#include <sys/ioctl.h>
+
+#include <cstdlib>
+
+#include "board_settings.h"
+#include "config/miosix_settings.h"
+#include "drivers/sd_stm32f2_f4_f7.h"
+#include "drivers/serial.h"
+#include "filesystem/console/console_device.h"
+#include "filesystem/file_access.h"
+#include "hwmapping.h"
+#include "interfaces/arch_registers.h"
+#include "interfaces/delays.h"
+#include "interfaces/portability.h"
+#include "kernel/kernel.h"
+#include "kernel/logging.h"
+#include "kernel/sync.h"
+
+namespace miosix
+{
+
+void IRQbspInit()
+{
+    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN | RCC_AHB1ENR_GPIOBEN |
+                    RCC_AHB1ENR_GPIOCEN | RCC_AHB1ENR_GPIOEEN |
+                    RCC_AHB1ENR_GPIOFEN;
+
+    // Enable can1
+    RCC->APB1ENR |= RCC_APB1ENR_CAN1EN;
+
+    RCC_SYNC();
+
+    using namespace interfaces;
+
+    debug::rx::mode(Mode::ALTERNATE);
+    debug::rx::alternateFunction(8);
+    debug::tx::mode(Mode::ALTERNATE);
+    debug::tx::alternateFunction(8);
+
+    cam1::rx::mode(Mode::ALTERNATE);
+    cam1::rx::alternateFunction(7);
+    cam1::tx::mode(Mode::ALTERNATE);
+    cam1::tx::alternateFunction(7);
+
+    cam2::tx::mode(Mode::ALTERNATE);
+    cam2::tx::alternateFunction(8);
+
+    cam3::tx::mode(Mode::ALTERNATE);
+    cam3::tx::alternateFunction(8);
+
+    camMosfet::mode(Mode::OUTPUT);
+    camMosfet::low();
+
+    can1::rx::mode(Mode::ALTERNATE);
+    can1::rx::alternateFunction(9);
+    can1::tx::mode(Mode::ALTERNATE);
+    can1::tx::alternateFunction(9);
+
+    using namespace leds;
+
+    led1::mode(Mode::OUTPUT);
+    led2::mode(Mode::OUTPUT);
+    led3::mode(Mode::OUTPUT);
+    led4::mode(Mode::OUTPUT);
+    led5::mode(Mode::OUTPUT);
+
+    for (uint8_t i = 0; i < 3; i++)
+    {
+        ledOn();
+        delayMs(10);
+        ledOff();
+        delayMs(10);
+    }
+
+    DefaultConsole::instance().IRQset(intrusive_ref_ptr<Device>(
+        new STM32Serial(defaultSerial, defaultSerialSpeed,
+                        defaultSerialFlowctrl ? STM32Serial::RTSCTS
+                                              : STM32Serial::NOFLOWCTRL)));
+}
+
+void bspInit2() {}
+
+/**
+ * For safety reasons, we never want the board to shutdown.
+ * When requested to shutdown, we reboot instead.
+ */
+void shutdown() { reboot(); }
+
+void reboot()
+{
+    ioctl(STDOUT_FILENO, IOCTL_SYNC, 0);
+
+    disableInterrupts();
+    miosix_private::IRQsystemReboot();
+}
+
+}  // namespace miosix
diff --git a/src/bsps/stm32f429zi_pyxis_auxiliary/interfaces-impl/bsp_impl.h b/src/bsps/stm32f429zi_pyxis_auxiliary/interfaces-impl/bsp_impl.h
new file mode 100644
index 000000000..9bfc06f02
--- /dev/null
+++ b/src/bsps/stm32f429zi_pyxis_auxiliary/interfaces-impl/bsp_impl.h
@@ -0,0 +1,73 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * 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.
+ */
+
+/***********************************************************************
+ * bsp_impl.h Part of the Miosix Embedded OS.
+ * Board support package, this file initializes hardware.
+ ************************************************************************/
+
+#ifndef BSP_IMPL_H
+#define BSP_IMPL_H
+
+#include "config/miosix_settings.h"
+#include "drivers/stm32_hardware_rng.h"
+#include "hwmapping.h"
+#include "interfaces/gpio.h"
+
+namespace miosix
+{
+
+/**
+\addtogroup Hardware
+\{
+*/
+
+/**
+ * \internal
+ * used by the ledOn() and ledOff() implementation
+ */
+
+inline void ledOn()
+{
+    leds::led1::high();
+    leds::led2::high();
+    leds::led3::high();
+    leds::led4::high();
+    leds::led5::high();
+}
+
+inline void ledOff()
+{
+    leds::led1::low();
+    leds::led2::low();
+    leds::led3::low();
+    leds::led4::low();
+    leds::led5::low();
+}
+
+/**
+\}
+*/
+
+}  // namespace miosix
+
+#endif  // BSP_IMPL_H
diff --git a/src/bsps/stm32f429zi_pyxis_auxiliary/interfaces-impl/hwmapping.h b/src/bsps/stm32f429zi_pyxis_auxiliary/interfaces-impl/hwmapping.h
new file mode 100644
index 000000000..a637939d4
--- /dev/null
+++ b/src/bsps/stm32f429zi_pyxis_auxiliary/interfaces-impl/hwmapping.h
@@ -0,0 +1,82 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * 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.
+ */
+
+#ifndef HWMAPPING_H
+#define HWMAPPING_H
+
+#include "interfaces/gpio.h"
+
+// Remember to modify pins of leds
+namespace miosix
+{
+
+namespace interfaces
+{
+
+// Debug - USART4
+namespace debug
+{
+using tx = Gpio<GPIOA_BASE, 0>;
+using rx = Gpio<GPIOA_BASE, 1>;
+}  // namespace debug
+
+// Cam 1 - USART2
+namespace cam1
+{
+using tx = Gpio<GPIOA_BASE, 2>;
+using rx = Gpio<GPIOA_BASE, 3>;
+}  // namespace cam1
+
+// Cam 2 - UART7
+namespace cam2
+{
+using tx = Gpio<GPIOF_BASE, 7>;
+}  // namespace cam2
+
+// Cam 3 - UART7
+namespace cam3
+{
+using tx = Gpio<GPIOE_BASE, 8>;
+}  // namespace cam3
+
+using camMosfet = Gpio<GPIOE_BASE, 7>;
+
+namespace can1
+{
+using rx = Gpio<GPIOA_BASE, 11>;
+using tx = Gpio<GPIOA_BASE, 12>;
+}  // namespace can1
+
+}  // namespace interfaces
+
+namespace leds
+{
+using led1 = Gpio<GPIOC_BASE, 15>;
+using led2 = Gpio<GPIOB_BASE, 4>;
+using led3 = Gpio<GPIOB_BASE, 5>;
+using led4 = Gpio<GPIOB_BASE, 6>;
+using led5 = Gpio<GPIOB_BASE, 7>;
+}  // namespace leds
+
+}  // namespace miosix
+
+#endif  // HWMAPPING_H
diff --git a/src/bsps/stm32f429zi_pyxis_auxiliary/stm32_2m+256k_rom.ld b/src/bsps/stm32f429zi_pyxis_auxiliary/stm32_2m+256k_rom.ld
new file mode 100644
index 000000000..cfcc2ce53
--- /dev/null
+++ b/src/bsps/stm32f429zi_pyxis_auxiliary/stm32_2m+256k_rom.ld
@@ -0,0 +1,181 @@
+/*
+ * C++ enabled linker script for stm32 (2M FLASH, 256K RAM)
+ * Developed by TFT: Terraneo Federico Technologies
+ * Optimized for use with the Miosix kernel
+ */
+
+/*
+ * This chip has an unusual quirk that the RAM is divided in two block mapped
+ * at two non contiguous memory addresses. I don't know why they've done that,
+ * probably doing the obvious thing would have made writing code too easy...
+ * Anyway, since hardware can't be changed, we've got to live with that and
+ * try to make use of both RAMs.
+ * 
+ * Given the constraints above, this linker script puts:
+ * - read only data and code (.text, .rodata, .eh_*) in FLASH
+ * - the 512Byte main (IRQ) stack, .data and .bss in the "small" 64KB RAM
+ * - stacks and heap in the "large" 192KB RAM.
+ * 
+ * Unfortunately thread stacks can't be put in the small RAM as Miosix
+ * allocates them inside the heap.
+ */
+
+/*
+ * The main stack is used for interrupt handling by the kernel.
+ *
+ * *** Readme ***
+ * This linker script places the main stack (used by the kernel for interrupts)
+ * at the bottom of the ram, instead of the top. This is done for two reasons:
+ *
+ * - as an optimization for microcontrollers with little ram memory. In fact
+ *   the implementation of malloc from newlib requests memory to the OS in 4KB
+ *   block (except the first block that can be smaller). This is probably done
+ *   for compatibility with OSes with an MMU and paged memory. To see why this
+ *   is bad, consider a microcontroller with 8KB of ram: when malloc finishes
+ *   up the first 4KB it will call _sbrk_r asking for a 4KB block, but this will
+ *   fail because the top part of the ram is used by the main stack. As a
+ *   result, the top part of the memory will not be used by malloc, even if
+ *   available (and it is nearly *half* the ram on an 8KB mcu). By placing the
+ *   main stack at the bottom of the ram, the upper 4KB block will be entirely
+ *   free and available as heap space.
+ *
+ * - In case of main stack overflow the cpu will fault because access to memory
+ *   before the beginning of the ram faults. Instead with the default stack
+ *   placement the main stack will silently collide with the heap.
+ * Note: if increasing the main stack size also increase the ORIGIN value in
+ * the MEMORY definitions below accordingly.
+ */
+_main_stack_size = 0x00000200;                     /* main stack = 512Bytes */
+_main_stack_top  = 0x10000000 + _main_stack_size;
+ASSERT(_main_stack_size   % 8 == 0, "MAIN stack size error");
+
+/* Mapping the heap into the large 192KB RAM */
+_end =      0x20000000;
+_heap_end = 0x20030000;                            /* end of available ram  */
+
+/* identify the Entry Point  */
+ENTRY(_Z13Reset_Handlerv)
+
+/* specify the memory areas  */
+MEMORY
+{
+    flash(rx)    : ORIGIN = 0x08000000, LENGTH =   2M
+    /*
+     * Note, the small ram starts at 0x10000000 but it is necessary to add the
+     * size of the main stack, so it is 0x10000200.
+     */
+    smallram(wx) : ORIGIN = 0x10000200, LENGTH =  64K-0x200 
+    largeram(wx) : ORIGIN = 0x20000000, LENGTH = 192K
+    backupram(rw): ORIGIN = 0x40024000, LENGTH =  4K
+}
+
+/* now define the output sections  */
+SECTIONS
+{
+    . = 0;
+    
+    /* .text section: code goes to flash */
+    .text :
+    {
+        /* Startup code must go at address 0 */
+        KEEP(*(.isr_vector))
+        
+        *(.text)
+        *(.text.*)
+        *(.gnu.linkonce.t.*)
+        /* these sections for thumb interwork? */
+        *(.glue_7)
+        *(.glue_7t)
+        /* these sections for C++? */
+        *(.gcc_except_table)
+        *(.gcc_except_table.*)
+        *(.ARM.extab*)
+        *(.gnu.linkonce.armextab.*)
+
+        . = ALIGN(4);
+        /* .rodata: constant data */
+        *(.rodata)
+        *(.rodata.*)
+        *(.gnu.linkonce.r.*)
+
+        /* C++ Static constructors/destructors (eabi) */
+        . = ALIGN(4);
+        KEEP(*(.init))
+        
+        . = ALIGN(4);
+        __miosix_init_array_start = .;
+        KEEP (*(SORT(.miosix_init_array.*)))
+        KEEP (*(.miosix_init_array))
+        __miosix_init_array_end = .;
+
+        . = ALIGN(4);
+        __preinit_array_start = .;
+        KEEP (*(.preinit_array))
+        __preinit_array_end = .;
+
+        . = ALIGN(4);
+        __init_array_start = .;
+        KEEP (*(SORT(.init_array.*)))
+        KEEP (*(.init_array))
+        __init_array_end = .;
+
+        . = ALIGN(4);
+        KEEP(*(.fini))
+
+        . = ALIGN(4);
+        __fini_array_start = .;
+        KEEP (*(.fini_array))
+        KEEP (*(SORT(.fini_array.*)))
+        __fini_array_end = .;
+
+        /* C++ Static constructors/destructors (elf)  */
+        . = ALIGN(4);
+        _ctor_start = .;
+        KEEP (*crtbegin.o(.ctors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .ctors))
+        KEEP (*(SORT(.ctors.*)))
+        KEEP (*crtend.o(.ctors))
+       _ctor_end = .;
+
+        . = ALIGN(4);
+        KEEP (*crtbegin.o(.dtors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .dtors))
+        KEEP (*(SORT(.dtors.*)))
+        KEEP (*crtend.o(.dtors))
+    } > flash
+
+    /* .ARM.exidx is sorted, so has to go in its own output section.  */
+    __exidx_start = .;
+    .ARM.exidx :
+    {
+        *(.ARM.exidx* .gnu.linkonce.armexidx.*)
+    } > flash
+    __exidx_end = .;
+
+	/* .data section: global variables go to ram, but also store a copy to
+       flash to initialize them */
+    .data : ALIGN(8)
+    {
+        _data = .;
+        *(.data)
+        *(.data.*)
+        *(.gnu.linkonce.d.*)
+        . = ALIGN(8);
+        _edata = .;
+    } > smallram AT > flash
+    _etext = LOADADDR(.data);
+
+    /* .bss section: uninitialized global variables go to ram */
+    _bss_start = .;
+    .bss :
+    {
+        *(.bss)
+        *(.bss.*)
+        *(.gnu.linkonce.b.*)
+        . = ALIGN(8);
+    } > smallram
+    _bss_end = .;
+
+    /*_end = .;*/
+    /*PROVIDE(end = .);*/
+}
diff --git a/src/bsps/stm32f429zi_rig/config/board_options.cmake b/src/bsps/stm32f429zi_rig/config/board_options.cmake
new file mode 100644
index 000000000..7b87f4a0e
--- /dev/null
+++ b/src/bsps/stm32f429zi_rig/config/board_options.cmake
@@ -0,0 +1,121 @@
+# Copyright (C) 2023 by Skyward
+#
+# This program is free software; you can redistribute it and/or 
+# it under the terms of the GNU General Public License as published 
+# the Free Software Foundation; either version 2 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.
+#
+# As a special exception, if other files instantiate templates or use
+# macros or inline functions from this file, or you compile this file
+# and link it with other works to produce a work based on this file,
+# this file does not by itself cause the resulting work to be covered
+# by the GNU General Public License. However the source code for this
+# file must still be made available in accordance with the GNU 
+# Public License. This exception does not invalidate any other 
+# why a work based on this file might be covered by the GNU General
+# Public License.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, see <http://www.gnu.org/licenses/>
+
+set(BOARD_NAME stm32f429zi_rig)
+set(ARCH_NAME cortexM4_stm32f4)
+
+# Base directories with header files for this board
+set(ARCH_PATH ${KPATH}/arch/${ARCH_NAME}/common)
+set(BOARD_PATH ${BOARDCORE_PATH}/src/bsps/${BOARD_NAME})
+set(BOARD_CONFIG_PATH ${BOARDCORE_PATH}/src/bsps/${BOARD_NAME}/config)
+
+# Specify where to find the board specific config/miosix_settings.h
+set(BOARD_MIOSIX_SETTINGS_PATH ${BOARD_PATH})
+
+# Specify where to find the board specific config/mxgui_settings.h
+set(BOARD_MXGUI_SETTINGS_PATH ${BOARD_PATH})
+
+# Optimization flags:
+# -O0 do no optimization, the default if no optimization level is specified
+# -O or -O1 optimize minimally
+# -O2 optimize more
+# -O3 optimize even more
+# -Ofast optimize very aggressively to the point of breaking the standard
+# -Og Optimize debugging experience, enables optimizations that do not
+# interfere with debugging
+# -Os Optimize for size with -O2 optimizations that do not increase code size
+set(OPT_OPTIMIZATION -O2)
+
+# Boot file
+set(BOOT_FILE ${BOARD_PATH}/core/stage_1_boot.cpp)
+
+# Linker script type, there are three options
+# 1) Code in FLASH, stack + heap in internal RAM (file *_rom.ld)
+#    the most common choice, available for all microcontrollers
+# 2) Code in FLASH, stack + heap in external RAM (file *8m_xram.ld)
+#    You must uncomment -D__ENABLE_XRAM below in this case.
+# set(LINKER_SCRIPT ${BOARD_PATH}/stm32_2m+256k_rom.ld)
+set(LINKER_SCRIPT ${BOARD_PATH}/stm32_2m+8m_xram.ld)
+
+# Uncommenting __ENABLE_XRAM enables the initialization of the external
+# 8MB SDRAM memory. Do not uncomment this even if you don't use a linker
+# script that requires it, as it is used for the LCD framebuffer.
+set(XRAM -D__ENABLE_XRAM)
+
+# Select clock frequency. 
+# Warning: the default clock frequency for
+# this board is 168MHz and not 180MHz because, due to a limitation in
+# the PLL, it is not possible to generate a precise 48MHz output when
+# running the core at 180MHz. If 180MHz is chosen the USB peripheral will
+# NOT WORK and the SDIO and RNG will run ~6% slower (45MHz insteand of 48)
+# Define USE_INTERNAL_CLOCK to bypass the external oscillator
+# set(CLOCK_FREQ -DHSE_VALUE=8000000 -DSYSCLK_FREQ_180MHz=180000000)
+set(CLOCK_FREQ -DHSE_VALUE=8000000 -DSYSCLK_FREQ_168MHz=168000000)
+# set(CLOCK_FREQ -DHSE_VALUE=8000000 -DSYSCLK_FREQ_100MHz=100000000)
+
+# C++ Exception/rtti support disable flags.
+# To save code size if not using C++ exceptions (nor some STL code which
+# implicitly uses it) uncomment this option.
+# -D__NO_EXCEPTIONS is used by Miosix to know if exceptions are used.
+# set(OPT_EXCEPT -fno-exceptions -fno-rtti -D__NO_EXCEPTIONS)
+
+# Specify a custom flash command
+# This is the program that is invoked when the flash flag (-f or --flash) is
+# used with the Miosix Build System. Use $binary or $hex as placeolders, they
+# will be replaced by the build systems with the binary or hex file repectively.
+# If a command is not specified, the build system will use st-flash if found
+# set(PROGRAM_CMDLINE "here your custom flash command")
+
+# Basic flags
+set(FLAGS_BASE -mcpu=cortex-m4 -mthumb -mfloat-abi=hard -mfpu=fpv4-sp-d16)
+
+# Flags for ASM and linker
+set(AFLAGS_BASE ${FLAGS_BASE})
+set(LFLAGS_BASE ${FLAGS_BASE} -Wl,--gc-sections,-Map,main.map -Wl,-T${LINKER_SCRIPT} ${OPT_EXCEPT} ${OPT_OPTIMIZATION} -nostdlib)
+
+# Flags for C/C++
+string(TOUPPER ${BOARD_NAME} BOARD_UPPER)
+set(CFLAGS_BASE
+    -D_BOARD_${BOARD_UPPER} -D_MIOSIX_BOARDNAME=\"${BOARD_NAME}\"
+    -D_DEFAULT_SOURCE=1 -ffunction-sections -Wall -Werror=return-type -g
+    -D_ARCH_CORTEXM4_STM32F4
+    ${CLOCK_FREQ} ${XRAM} ${SRAM_BOOT} ${FLAGS_BASE} ${OPT_OPTIMIZATION} -c
+)
+set(CXXFLAGS_BASE ${CFLAGS_BASE} ${OPT_EXCEPT})
+
+# Select architecture specific files
+set(ARCH_SRC
+    ${ARCH_PATH}/interfaces-impl/delays.cpp
+    ${ARCH_PATH}/interfaces-impl/gpio_impl.cpp
+    ${ARCH_PATH}/interfaces-impl/portability.cpp
+    ${BOARD_PATH}/interfaces-impl/bsp.cpp
+    ${KPATH}/arch/common/CMSIS/Device/ST/STM32F4xx/Source/Templates/system_stm32f4xx.c
+    ${KPATH}/arch/common/core/interrupts_cortexMx.cpp
+    ${KPATH}/arch/common/core/mpu_cortexMx.cpp
+    ${KPATH}/arch/common/core/stm32f2_f4_l4_f7_h7_os_timer.cpp
+    ${KPATH}/arch/common/drivers/sd_stm32f2_f4_f7.cpp
+    ${KPATH}/arch/common/drivers/serial_stm32.cpp
+    ${KPATH}/arch/common/drivers/stm32_hardware_rng.cpp
+)
diff --git a/src/bsps/stm32f429zi_rig/config/board_settings.h b/src/bsps/stm32f429zi_rig/config/board_settings.h
new file mode 100644
index 000000000..3946b4d3c
--- /dev/null
+++ b/src/bsps/stm32f429zi_rig/config/board_settings.h
@@ -0,0 +1,59 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * 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.
+ */
+
+#ifndef BOARD_SETTINGS_H
+#define BOARD_SETTINGS_H
+
+#include "util/version.h"
+
+/**
+ * \internal
+ * Versioning for board_settings.h for out of git tree projects
+ */
+#define BOARD_SETTINGS_VERSION 300
+
+namespace miosix
+{
+
+/// Size of stack for main().
+/// The C standard library is stack-heavy (iprintf requires 1KB) but the
+/// STM32F407VG only has 192KB of RAM so there is room for a big 4K stack.
+const unsigned int MAIN_STACK_SIZE = 4 * 1024;
+
+/// Serial port
+const unsigned int defaultSerial      = 1;
+const unsigned int defaultSerialSpeed = 115200;
+const bool defaultSerialFlowctrl      = false;
+#define SERIAL_1_DMA
+// #define SERIAL_2_DMA
+// #define SERIAL_3_DMA
+
+// SD card driver
+static const unsigned char sdVoltage = 30;  // Board powered @ 3.0V
+#define SD_ONE_BIT_DATABUS  // Can't use 4 bit databus due to pin conflicts
+
+/// Analog supply voltage for ADC, DAC, Reset blocks, RCs and PLL
+#define V_DDA_VOLTAGE 3.0f
+
+}  // namespace miosix
+
+#endif /* BOARD_SETTINGS_H */
diff --git a/src/bsps/stm32f429zi_rig/config/miosix_settings.h b/src/bsps/stm32f429zi_rig/config/miosix_settings.h
new file mode 100644
index 000000000..5fe66bf6c
--- /dev/null
+++ b/src/bsps/stm32f429zi_rig/config/miosix_settings.h
@@ -0,0 +1,240 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * 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.
+ */
+
+#pragma once
+
+// Before you can compile the kernel you have to configure it by editing this
+// file. After that, comment out this line to disable the reminder error.
+// The PARSING_FROM_IDE is because Netbeans gets confused by this, it is never
+// defined when compiling the code.
+#ifndef PARSING_FROM_IDE
+//#error This error is a reminder that you have not edited miosix_settings.h
+// yet.
+#endif  // PARSING_FROM_IDE
+
+/**
+ * \file miosix_settings.h
+ * NOTE: this file contains ONLY configuration options that are not dependent
+ * on architecture specific details. The other options are in the following
+ * files which are included here:
+ * miosix/arch/architecture name/common/arch_settings.h
+ * miosix/config/arch/architecture name/board name/board_settings.h
+ */
+#include "arch_settings.h"
+#include "board_settings.h"
+#include "util/version.h"
+
+/**
+ * \internal
+ * Versioning for miosix_settings.h for out of git tree projects
+ */
+#define MIOSIX_SETTINGS_VERSION 300
+
+namespace miosix
+{
+
+/**
+ * \addtogroup Settings
+ * \{
+ */
+
+//
+// Scheduler options
+//
+
+/// \def SCHED_TYPE_PRIORITY
+/// If uncommented selects the priority scheduler
+/// \def SCHED_TYPE_CONTROL_BASED
+/// If uncommented selects the control based scheduler
+/// \def SCHED_TYPE_EDF
+/// If uncommented selects the EDF scheduler
+// Uncomment only *one* of those
+
+#define SCHED_TYPE_PRIORITY
+//#define SCHED_TYPE_CONTROL_BASED
+//#define SCHED_TYPE_EDF
+
+/// \def WITH_CPU_TIME_COUNTER
+/// Allows to enable/disable CPUTimeCounter to save code size and remove its
+/// overhead from the scheduling process. By default it is not defined
+/// (CPUTimeCounter is disabled).
+//#define WITH_CPU_TIME_COUNTER
+
+//
+// Filesystem options
+//
+
+/// \def WITH_FILESYSTEM
+/// Allows to enable/disable filesystem support to save code size
+/// By default it is defined (filesystem support is enabled)
+#define WITH_FILESYSTEM
+
+/// \def WITH_DEVFS
+/// Allows to enable/disable DevFs support to save code size
+/// By default it is defined (DevFs is enabled)
+#define WITH_DEVFS
+
+/// \def SYNC_AFTER_WRITE
+/// Increases filesystem write robustness. After each write operation the
+/// filesystem is synced so that a power failure happens data is not lost
+/// (unless power failure happens exactly between the write and the sync)
+/// Unfortunately write latency and throughput becomes twice as worse
+/// By default it is defined (slow but safe)
+#define SYNC_AFTER_WRITE
+
+/// Maximum number of open files. Trying to open more will fail.
+/// Cannot be lower than 3, as the first three are stdin, stdout, stderr
+const unsigned char MAX_OPEN_FILES = 8;
+
+/// \def WITH_PROCESSES
+/// If uncommented enables support for processes as well as threads.
+/// This enables the dynamic loader to load elf programs, the extended system
+/// call service and, if the hardware supports it, the MPU to provide memory
+/// isolation of processes
+//#define WITH_PROCESSES
+
+#if defined(WITH_PROCESSES) && defined(__NO_EXCEPTIONS)
+#error Processes require C++ exception support
+#endif  // defined(WITH_PROCESSES) && defined(__NO_EXCEPTIONS)
+
+#if defined(WITH_PROCESSES) && !defined(WITH_FILESYSTEM)
+#error Processes require filesystem support
+#endif  // defined(WITH_PROCESSES) && !defined(WITH_FILESYSTEM)
+
+#if defined(WITH_PROCESSES) && !defined(WITH_DEVFS)
+#error Processes require devfs support
+#endif  // defined(WITH_PROCESSES) && !defined(WITH_DEVFS)
+
+//
+// C/C++ standard library I/O (stdin, stdout and stderr related)
+//
+
+/// \def WITH_BOOTLOG
+/// Uncomment to print bootlogs on stdout.
+/// By default it is defined (bootlogs are printed)
+#define WITH_BOOTLOG
+
+/// \def WITH_ERRLOG
+/// Uncomment for debug information on stdout.
+/// By default it is defined (error information is printed)
+#define WITH_ERRLOG
+
+//
+// Kernel related options (stack sizes, priorities)
+//
+
+/// \def WITH_DEEP_SLEEP
+/// Adds interfaces and required variables to support deep sleep state switch
+/// automatically when peripherals are not required
+//#define WITH_DEEP_SLEEP
+
+/**
+ * \def JTAG_DISABLE_SLEEP
+ * JTAG debuggers lose communication with the device if it enters sleep
+ * mode, so to use debugging it is necessary to disable sleep in the idle
+ * thread. By default it is not defined (idle thread calls sleep).
+ */
+//#define JTAG_DISABLE_SLEEP
+
+#if defined(WITH_DEEP_SLEEP) && defined(JTAG_DISABLE_SLEEP)
+#error Deep sleep cannot work together with jtag
+#endif  // defined(WITH_PROCESSES) && !defined(WITH_DEVFS)
+
+/// Minimum stack size (MUST be divisible by 4)
+const unsigned int STACK_MIN = 256;
+
+/// \internal Size of idle thread stack.
+/// Should be >=STACK_MIN (MUST be divisible by 4)
+const unsigned int STACK_IDLE = 256;
+
+/// Default stack size for pthread_create.
+/// The chosen value is enough to call C standard library functions
+/// such as printf/fopen which are stack-heavy
+const unsigned int STACK_DEFAULT_FOR_PTHREAD = 2048;
+
+/// Maximum size of the RAM image of a process. If a program requires more
+/// the kernel will not run it (MUST be divisible by 4)
+const unsigned int MAX_PROCESS_IMAGE_SIZE = 64 * 1024;
+
+/// Minimum size of the stack for a process. If a program specifies a lower
+/// size the kernel will not run it (MUST be divisible by 4)
+const unsigned int MIN_PROCESS_STACK_SIZE = STACK_MIN;
+
+/// Every userspace thread has two stacks, one for when it is running in
+/// userspace and one for when it is running in kernelspace (that is, while it
+/// is executing system calls). This is the size of the stack for when the
+/// thread is running in kernelspace (MUST be divisible by 4)
+const unsigned int SYSTEM_MODE_PROCESS_STACK_SIZE = 2 * 1024;
+
+/// Number of priorities (MUST be >1)
+/// PRIORITY_MAX-1 is the highest priority, 0 is the lowest. -1 is reserved as
+/// the priority of the idle thread.
+/// The meaning of a thread's priority depends on the chosen scheduler.
+#ifdef SCHED_TYPE_PRIORITY
+// Can be modified, but a high value makes context switches more expensive
+const short int PRIORITY_MAX = 4;
+#elif defined(SCHED_TYPE_CONTROL_BASED)
+// Don't touch, the limit is due to the fixed point implementation
+// It's not needed for if floating point is selected, but kept for consistency
+const short int PRIORITY_MAX = 64;
+#else  // SCHED_TYPE_EDF
+// Doesn't exist for this kind of scheduler
+#endif
+
+/// Priority of main()
+/// The meaning of a thread's priority depends on the chosen scheduler.
+const unsigned char MAIN_PRIORITY = 1;
+
+#ifdef SCHED_TYPE_PRIORITY
+/// Maximum thread time slice in nanoseconds, after which preemption occurs
+const unsigned int MAX_TIME_SLICE = 1000000;
+#endif  // SCHED_TYPE_PRIORITY
+
+//
+// Other low level kernel options. There is usually no need to modify these.
+//
+
+/// \internal Length of wartermark (in bytes) to check stack overflow.
+/// MUST be divisible by 4 and can also be zero.
+/// A high value increases context switch time.
+const unsigned int WATERMARK_LEN = 16;
+
+/// \internal Used to fill watermark
+const unsigned int WATERMARK_FILL = 0xaaaaaaaa;
+
+/// \internal Used to fill stack (for checking stack usage)
+const unsigned int STACK_FILL = 0xbbbbbbbb;
+
+// Compiler version checks
+#if !defined(_MIOSIX_GCC_PATCH_MAJOR) || _MIOSIX_GCC_PATCH_MAJOR < 3
+#error \
+    "You are using a too old or unsupported compiler. Get the latest one from https://miosix.org/wiki/index.php?title=Miosix_Toolchain"
+#endif
+#if _MIOSIX_GCC_PATCH_MAJOR > 3
+#warning "You are using a too new compiler, which may not be supported"
+#endif
+
+/**
+ * \}
+ */
+
+}  // namespace miosix
diff --git a/src/bsps/stm32f429zi_rig/core/stage_1_boot.cpp b/src/bsps/stm32f429zi_rig/core/stage_1_boot.cpp
new file mode 100644
index 000000000..d62f81003
--- /dev/null
+++ b/src/bsps/stm32f429zi_rig/core/stage_1_boot.cpp
@@ -0,0 +1,372 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * 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 <string.h>
+
+#include "core/interrupts.h"  //For the unexpected interrupt call
+#include "interfaces/arch_registers.h"
+#include "interfaces/bsp.h"
+#include "kernel/stage_2_boot.h"
+
+/*
+ * startup.cpp
+ * STM32 C++ startup.
+ * NOTE: for stm32f42x devices ONLY.
+ * Supports interrupt handlers in C++ without extern "C"
+ * Developed by Terraneo Federico, based on ST startup code.
+ * Additionally modified to boot Miosix.
+ */
+
+/**
+ * Called by Reset_Handler, performs initialization and calls main.
+ * Never returns.
+ */
+void program_startup() __attribute__((noreturn));
+void program_startup()
+{
+    // Cortex M3 core appears to get out of reset with interrupts already
+    // enabled
+    __disable_irq();
+
+    // These are defined in the linker script
+    extern unsigned char _etext asm("_etext");
+    extern unsigned char _data asm("_data");
+    extern unsigned char _edata asm("_edata");
+    extern unsigned char _bss_start asm("_bss_start");
+    extern unsigned char _bss_end asm("_bss_end");
+
+    // Initialize .data section, clear .bss section
+    unsigned char *etext     = &_etext;
+    unsigned char *data      = &_data;
+    unsigned char *edata     = &_edata;
+    unsigned char *bss_start = &_bss_start;
+    unsigned char *bss_end   = &_bss_end;
+    memcpy(data, etext, edata - data);
+    memset(bss_start, 0, bss_end - bss_start);
+
+    // Move on to stage 2
+    _init();
+
+    // If main returns, reboot
+    NVIC_SystemReset();
+    for (;;)
+        ;
+}
+
+/**
+ * Reset handler, called by hardware immediately after reset
+ */
+void Reset_Handler() __attribute__((__interrupt__, noreturn));
+void Reset_Handler()
+{
+    // SystemInit() is called *before* initializing .data and zeroing .bss
+    // Despite all startup files provided by ST do the opposite, there are three
+    // good reasons to do so:
+    // First, the CMSIS specifications say that SystemInit() must not access
+    // global variables, so it is actually possible to call it before
+    // Second, when running Miosix with the xram linker scripts .data and .bss
+    // are placed in the external RAM, so we *must* call SystemInit(), which
+    // enables xram, before touching .data and .bss
+    // Third, this is a performance improvement since the loops that initialize
+    //.data and zeros .bss now run with the CPU at full speed instead of 8MHz
+    SystemInit();
+// ST does not provide code to initialize the stm32f429-disco SDRAM at boot.
+// Put after SystemInit() as SDRAM is timing-sensitive and needs the full
+// clock speed.
+#ifdef __ENABLE_XRAM
+    miosix::configureSdram();
+#endif  //__ENABLE_XRAM
+
+    /*
+     * Initialize process stack and switch to it.
+     * This is required for booting Miosix, a small portion of the top of the
+     * heap area will be used as stack until the first thread starts. After,
+     * this stack will be abandoned and the process stack will point to the
+     * current thread's stack.
+     */
+    asm volatile(
+        "ldr r0,  =_heap_end          \n\t"
+        "msr psp, r0                  \n\t"
+        "movw r0, #2                  \n\n"  // Privileged, process stack
+        "msr control, r0              \n\t"
+        "isb                          \n\t" ::
+            : "r0");
+
+    program_startup();
+}
+
+/**
+ * All unused interrupts call this function.
+ */
+extern "C" void Default_Handler() { unexpectedInterrupt(); }
+
+// System handlers
+void /*__attribute__((weak))*/ Reset_Handler();      // These interrupts are not
+void /*__attribute__((weak))*/ NMI_Handler();        // weak because they are
+void /*__attribute__((weak))*/ HardFault_Handler();  // surely defined by Miosix
+void /*__attribute__((weak))*/ MemManage_Handler();
+void /*__attribute__((weak))*/ BusFault_Handler();
+void /*__attribute__((weak))*/ UsageFault_Handler();
+void /*__attribute__((weak))*/ SVC_Handler();
+void /*__attribute__((weak))*/ DebugMon_Handler();
+void /*__attribute__((weak))*/ PendSV_Handler();
+void __attribute__((weak)) SysTick_Handler();
+
+// Interrupt handlers
+void __attribute__((weak)) WWDG_IRQHandler();
+void __attribute__((weak)) PVD_IRQHandler();
+void __attribute__((weak)) TAMP_STAMP_IRQHandler();
+void __attribute__((weak)) RTC_WKUP_IRQHandler();
+void __attribute__((weak)) FLASH_IRQHandler();
+void __attribute__((weak)) RCC_IRQHandler();
+void __attribute__((weak)) EXTI0_IRQHandler();
+void __attribute__((weak)) EXTI1_IRQHandler();
+void __attribute__((weak)) EXTI2_IRQHandler();
+void __attribute__((weak)) EXTI3_IRQHandler();
+void __attribute__((weak)) EXTI4_IRQHandler();
+void __attribute__((weak)) DMA1_Stream0_IRQHandler();
+void __attribute__((weak)) DMA1_Stream1_IRQHandler();
+void __attribute__((weak)) DMA1_Stream2_IRQHandler();
+void __attribute__((weak)) DMA1_Stream3_IRQHandler();
+void __attribute__((weak)) DMA1_Stream4_IRQHandler();
+void __attribute__((weak)) DMA1_Stream5_IRQHandler();
+void __attribute__((weak)) DMA1_Stream6_IRQHandler();
+void __attribute__((weak)) ADC_IRQHandler();
+void __attribute__((weak)) CAN1_TX_IRQHandler();
+void __attribute__((weak)) CAN1_RX0_IRQHandler();
+void __attribute__((weak)) CAN1_RX1_IRQHandler();
+void __attribute__((weak)) CAN1_SCE_IRQHandler();
+void __attribute__((weak)) EXTI9_5_IRQHandler();
+void __attribute__((weak)) TIM1_BRK_TIM9_IRQHandler();
+void __attribute__((weak)) TIM1_UP_TIM10_IRQHandler();
+void __attribute__((weak)) TIM1_TRG_COM_TIM11_IRQHandler();
+void __attribute__((weak)) TIM1_CC_IRQHandler();
+void __attribute__((weak)) TIM2_IRQHandler();
+void __attribute__((weak)) TIM3_IRQHandler();
+void __attribute__((weak)) TIM4_IRQHandler();
+void __attribute__((weak)) I2C1_EV_IRQHandler();
+void __attribute__((weak)) I2C1_ER_IRQHandler();
+void __attribute__((weak)) I2C2_EV_IRQHandler();
+void __attribute__((weak)) I2C2_ER_IRQHandler();
+void __attribute__((weak)) SPI1_IRQHandler();
+void __attribute__((weak)) SPI2_IRQHandler();
+void __attribute__((weak)) USART1_IRQHandler();
+void __attribute__((weak)) USART2_IRQHandler();
+void __attribute__((weak)) USART3_IRQHandler();
+void __attribute__((weak)) EXTI15_10_IRQHandler();
+void __attribute__((weak)) RTC_Alarm_IRQHandler();
+void __attribute__((weak)) OTG_FS_WKUP_IRQHandler();
+void __attribute__((weak)) TIM8_BRK_TIM12_IRQHandler();
+void __attribute__((weak)) TIM8_UP_TIM13_IRQHandler();
+void __attribute__((weak)) TIM8_TRG_COM_TIM14_IRQHandler();
+void __attribute__((weak)) TIM8_CC_IRQHandler();
+void __attribute__((weak)) DMA1_Stream7_IRQHandler();
+void __attribute__((weak)) FMC_IRQHandler();
+void __attribute__((weak)) SDIO_IRQHandler();
+void __attribute__((weak)) TIM5_IRQHandler();
+void __attribute__((weak)) SPI3_IRQHandler();
+void __attribute__((weak)) UART4_IRQHandler();
+void __attribute__((weak)) UART5_IRQHandler();
+void __attribute__((weak)) TIM6_DAC_IRQHandler();
+void __attribute__((weak)) TIM7_IRQHandler();
+void __attribute__((weak)) DMA2_Stream0_IRQHandler();
+void __attribute__((weak)) DMA2_Stream1_IRQHandler();
+void __attribute__((weak)) DMA2_Stream2_IRQHandler();
+void __attribute__((weak)) DMA2_Stream3_IRQHandler();
+void __attribute__((weak)) DMA2_Stream4_IRQHandler();
+void __attribute__((weak)) ETH_IRQHandler();
+void __attribute__((weak)) ETH_WKUP_IRQHandler();
+void __attribute__((weak)) CAN2_TX_IRQHandler();
+void __attribute__((weak)) CAN2_RX0_IRQHandler();
+void __attribute__((weak)) CAN2_RX1_IRQHandler();
+void __attribute__((weak)) CAN2_SCE_IRQHandler();
+void __attribute__((weak)) OTG_FS_IRQHandler();
+void __attribute__((weak)) DMA2_Stream5_IRQHandler();
+void __attribute__((weak)) DMA2_Stream6_IRQHandler();
+void __attribute__((weak)) DMA2_Stream7_IRQHandler();
+void __attribute__((weak)) USART6_IRQHandler();
+void __attribute__((weak)) I2C3_EV_IRQHandler();
+void __attribute__((weak)) I2C3_ER_IRQHandler();
+void __attribute__((weak)) OTG_HS_EP1_OUT_IRQHandler();
+void __attribute__((weak)) OTG_HS_EP1_IN_IRQHandler();
+void __attribute__((weak)) OTG_HS_WKUP_IRQHandler();
+void __attribute__((weak)) OTG_HS_IRQHandler();
+void __attribute__((weak)) DCMI_IRQHandler();
+void __attribute__((weak)) CRYP_IRQHandler();
+void __attribute__((weak)) HASH_RNG_IRQHandler();
+void __attribute__((weak)) FPU_IRQHandler();
+void __attribute__((weak)) UART7_IRQHandler();
+void __attribute__((weak)) UART8_IRQHandler();
+void __attribute__((weak)) SPI4_IRQHandler();
+void __attribute__((weak)) SPI5_IRQHandler();
+void __attribute__((weak)) SPI6_IRQHandler();
+void __attribute__((weak)) SAI1_IRQHandler();
+void __attribute__((weak)) LTDC_IRQHandler();
+void __attribute__((weak)) LTDC_ER_IRQHandler();
+void __attribute__((weak)) DMA2D_IRQHandler();
+
+// Stack top, defined in the linker script
+extern char _main_stack_top asm("_main_stack_top");
+
+// Interrupt vectors, must be placed @ address 0x00000000
+// The extern declaration is required otherwise g++ optimizes it out
+extern void (*const __Vectors[])();
+void (*const __Vectors[])() __attribute__((section(".isr_vector"))) = {
+    reinterpret_cast<void (*)()>(&_main_stack_top), /* Stack pointer*/
+    Reset_Handler,                                  /* Reset Handler */
+    NMI_Handler,                                    /* NMI Handler */
+    HardFault_Handler,                              /* Hard Fault Handler */
+    MemManage_Handler,                              /* MPU Fault Handler */
+    BusFault_Handler,                               /* Bus Fault Handler */
+    UsageFault_Handler,                             /* Usage Fault Handler */
+    0,                                              /* Reserved */
+    0,                                              /* Reserved */
+    0,                                              /* Reserved */
+    0,                                              /* Reserved */
+    SVC_Handler,                                    /* SVCall Handler */
+    DebugMon_Handler,                               /* Debug Monitor Handler */
+    0,                                              /* Reserved */
+    PendSV_Handler,                                 /* PendSV Handler */
+    SysTick_Handler,                                /* SysTick Handler */
+
+    /* External Interrupts */
+    WWDG_IRQHandler, PVD_IRQHandler, TAMP_STAMP_IRQHandler, RTC_WKUP_IRQHandler,
+    FLASH_IRQHandler, RCC_IRQHandler, EXTI0_IRQHandler, EXTI1_IRQHandler,
+    EXTI2_IRQHandler, EXTI3_IRQHandler, EXTI4_IRQHandler,
+    DMA1_Stream0_IRQHandler, DMA1_Stream1_IRQHandler, DMA1_Stream2_IRQHandler,
+    DMA1_Stream3_IRQHandler, DMA1_Stream4_IRQHandler, DMA1_Stream5_IRQHandler,
+    DMA1_Stream6_IRQHandler, ADC_IRQHandler, CAN1_TX_IRQHandler,
+    CAN1_RX0_IRQHandler, CAN1_RX1_IRQHandler, CAN1_SCE_IRQHandler,
+    EXTI9_5_IRQHandler, TIM1_BRK_TIM9_IRQHandler, TIM1_UP_TIM10_IRQHandler,
+    TIM1_TRG_COM_TIM11_IRQHandler, TIM1_CC_IRQHandler, TIM2_IRQHandler,
+    TIM3_IRQHandler, TIM4_IRQHandler, I2C1_EV_IRQHandler, I2C1_ER_IRQHandler,
+    I2C2_EV_IRQHandler, I2C2_ER_IRQHandler, SPI1_IRQHandler, SPI2_IRQHandler,
+    USART1_IRQHandler, USART2_IRQHandler, USART3_IRQHandler,
+    EXTI15_10_IRQHandler, RTC_Alarm_IRQHandler, OTG_FS_WKUP_IRQHandler,
+    TIM8_BRK_TIM12_IRQHandler, TIM8_UP_TIM13_IRQHandler,
+    TIM8_TRG_COM_TIM14_IRQHandler, TIM8_CC_IRQHandler, DMA1_Stream7_IRQHandler,
+    FMC_IRQHandler, SDIO_IRQHandler, TIM5_IRQHandler, SPI3_IRQHandler,
+    UART4_IRQHandler, UART5_IRQHandler, TIM6_DAC_IRQHandler, TIM7_IRQHandler,
+    DMA2_Stream0_IRQHandler, DMA2_Stream1_IRQHandler, DMA2_Stream2_IRQHandler,
+    DMA2_Stream3_IRQHandler, DMA2_Stream4_IRQHandler, ETH_IRQHandler,
+    ETH_WKUP_IRQHandler, CAN2_TX_IRQHandler, CAN2_RX0_IRQHandler,
+    CAN2_RX1_IRQHandler, CAN2_SCE_IRQHandler, OTG_FS_IRQHandler,
+    DMA2_Stream5_IRQHandler, DMA2_Stream6_IRQHandler, DMA2_Stream7_IRQHandler,
+    USART6_IRQHandler, I2C3_EV_IRQHandler, I2C3_ER_IRQHandler,
+    OTG_HS_EP1_OUT_IRQHandler, OTG_HS_EP1_IN_IRQHandler, OTG_HS_WKUP_IRQHandler,
+    OTG_HS_IRQHandler, DCMI_IRQHandler, CRYP_IRQHandler, HASH_RNG_IRQHandler,
+    FPU_IRQHandler, UART7_IRQHandler, UART8_IRQHandler, SPI4_IRQHandler,
+    SPI5_IRQHandler, SPI6_IRQHandler, SAI1_IRQHandler, LTDC_IRQHandler,
+    LTDC_ER_IRQHandler, DMA2D_IRQHandler};
+
+#pragma weak SysTick_IRQHandler            = Default_Handler
+#pragma weak WWDG_IRQHandler               = Default_Handler
+#pragma weak PVD_IRQHandler                = Default_Handler
+#pragma weak TAMP_STAMP_IRQHandler         = Default_Handler
+#pragma weak RTC_WKUP_IRQHandler           = Default_Handler
+#pragma weak FLASH_IRQHandler              = Default_Handler
+#pragma weak RCC_IRQHandler                = Default_Handler
+#pragma weak EXTI0_IRQHandler              = Default_Handler
+#pragma weak EXTI1_IRQHandler              = Default_Handler
+#pragma weak EXTI2_IRQHandler              = Default_Handler
+#pragma weak EXTI3_IRQHandler              = Default_Handler
+#pragma weak EXTI4_IRQHandler              = Default_Handler
+#pragma weak DMA1_Stream0_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream1_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream2_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream3_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream4_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream5_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream6_IRQHandler       = Default_Handler
+#pragma weak ADC_IRQHandler                = Default_Handler
+#pragma weak CAN1_TX_IRQHandler            = Default_Handler
+#pragma weak CAN1_RX0_IRQHandler           = Default_Handler
+#pragma weak CAN1_RX1_IRQHandler           = Default_Handler
+#pragma weak CAN1_SCE_IRQHandler           = Default_Handler
+#pragma weak EXTI9_5_IRQHandler            = Default_Handler
+#pragma weak TIM1_BRK_TIM9_IRQHandler      = Default_Handler
+#pragma weak TIM1_UP_TIM10_IRQHandler      = Default_Handler
+#pragma weak TIM1_TRG_COM_TIM11_IRQHandler = Default_Handler
+#pragma weak TIM1_CC_IRQHandler            = Default_Handler
+#pragma weak TIM2_IRQHandler               = Default_Handler
+#pragma weak TIM3_IRQHandler               = Default_Handler
+#pragma weak TIM4_IRQHandler               = Default_Handler
+#pragma weak I2C1_EV_IRQHandler            = Default_Handler
+#pragma weak I2C1_ER_IRQHandler            = Default_Handler
+#pragma weak I2C2_EV_IRQHandler            = Default_Handler
+#pragma weak I2C2_ER_IRQHandler            = Default_Handler
+#pragma weak SPI1_IRQHandler               = Default_Handler
+#pragma weak SPI2_IRQHandler               = Default_Handler
+// #pragma weak USART1_IRQHandler             = Default_Handler
+// #pragma weak USART2_IRQHandler             = Default_Handler
+// #pragma weak USART3_IRQHandler             = Default_Handler
+#pragma weak EXTI15_10_IRQHandler          = Default_Handler
+#pragma weak RTC_Alarm_IRQHandler          = Default_Handler
+#pragma weak OTG_FS_WKUP_IRQHandler        = Default_Handler
+#pragma weak TIM8_BRK_TIM12_IRQHandler     = Default_Handler
+#pragma weak TIM8_UP_TIM13_IRQHandler      = Default_Handler
+#pragma weak TIM8_TRG_COM_TIM14_IRQHandler = Default_Handler
+#pragma weak TIM8_CC_IRQHandler            = Default_Handler
+#pragma weak DMA1_Stream7_IRQHandler       = Default_Handler
+#pragma weak FMC_IRQHandler                = Default_Handler
+#pragma weak SDIO_IRQHandler               = Default_Handler
+#pragma weak TIM5_IRQHandler               = Default_Handler
+#pragma weak SPI3_IRQHandler               = Default_Handler
+// #pragma weak UART4_IRQHandler              = Default_Handler
+// #pragma weak UART5_IRQHandler              = Default_Handler
+#pragma weak TIM6_DAC_IRQHandler     = Default_Handler
+#pragma weak TIM7_IRQHandler         = Default_Handler
+#pragma weak DMA2_Stream0_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream1_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream2_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream3_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream4_IRQHandler = Default_Handler
+#pragma weak ETH_IRQHandler          = Default_Handler
+#pragma weak ETH_WKUP_IRQHandler     = Default_Handler
+#pragma weak CAN2_TX_IRQHandler      = Default_Handler
+#pragma weak CAN2_RX0_IRQHandler     = Default_Handler
+#pragma weak CAN2_RX1_IRQHandler     = Default_Handler
+#pragma weak CAN2_SCE_IRQHandler     = Default_Handler
+#pragma weak OTG_FS_IRQHandler       = Default_Handler
+#pragma weak DMA2_Stream5_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream6_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream7_IRQHandler = Default_Handler
+// #pragma weak USART6_IRQHandler             = Default_Handler
+#pragma weak I2C3_EV_IRQHandler        = Default_Handler
+#pragma weak I2C3_ER_IRQHandler        = Default_Handler
+#pragma weak OTG_HS_EP1_OUT_IRQHandler = Default_Handler
+#pragma weak OTG_HS_EP1_IN_IRQHandler  = Default_Handler
+#pragma weak OTG_HS_WKUP_IRQHandler    = Default_Handler
+#pragma weak OTG_HS_IRQHandler         = Default_Handler
+#pragma weak DCMI_IRQHandler           = Default_Handler
+#pragma weak CRYP_IRQHandler           = Default_Handler
+#pragma weak HASH_RNG_IRQHandler       = Default_Handler
+#pragma weak FPU_IRQHandler            = Default_Handler
+// #pragma weak UART7_IRQHandler              = Default_Handler
+// #pragma weak UART8_IRQHandler              = Default_Handler
+#pragma weak SPI4_IRQHandler    = Default_Handler
+#pragma weak SPI5_IRQHandler    = Default_Handler
+#pragma weak SPI6_IRQHandler    = Default_Handler
+#pragma weak SAI1_IRQHandler    = Default_Handler
+#pragma weak LTDC_IRQHandler    = Default_Handler
+#pragma weak LTDC_ER_IRQHandler = Default_Handler
+#pragma weak DMA2D_IRQHandler   = Default_Handler
diff --git a/src/bsps/stm32f429zi_rig/interfaces-impl/arch_registers_impl.h b/src/bsps/stm32f429zi_rig/interfaces-impl/arch_registers_impl.h
new file mode 100644
index 000000000..06956a183
--- /dev/null
+++ b/src/bsps/stm32f429zi_rig/interfaces-impl/arch_registers_impl.h
@@ -0,0 +1,34 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * 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.
+ */
+
+#ifndef ARCH_REGISTERS_IMPL_H
+#define ARCH_REGISTERS_IMPL_H
+
+// Always include stm32f4xx.h before core_cm4.h, there's some nasty dependency
+#define STM32F429xx
+#include "CMSIS/Device/ST/STM32F4xx/Include/stm32f4xx.h"
+#include "CMSIS/Device/ST/STM32F4xx/Include/system_stm32f4xx.h"
+#include "CMSIS/Include/core_cm4.h"
+
+#define RCC_SYNC() __DSB()  // Workaround for a bug in stm32f42x
+
+#endif  // ARCH_REGISTERS_IMPL_H
diff --git a/src/bsps/stm32f429zi_rig/interfaces-impl/bsp.cpp b/src/bsps/stm32f429zi_rig/interfaces-impl/bsp.cpp
new file mode 100644
index 000000000..2caef6f01
--- /dev/null
+++ b/src/bsps/stm32f429zi_rig/interfaces-impl/bsp.cpp
@@ -0,0 +1,364 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * 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.
+ */
+
+/***********************************************************************
+ * bsp.cpp Part of the Miosix Embedded OS.
+ * Board support package, this file initializes hardware.
+ ************************************************************************/
+
+#include "interfaces/bsp.h"
+
+#include <inttypes.h>
+#include <sys/ioctl.h>
+
+#include <cstdlib>
+
+#include "board_settings.h"
+#include "config/miosix_settings.h"
+#include "drivers/sd_stm32f2_f4_f7.h"
+#include "drivers/serial.h"
+#include "filesystem/console/console_device.h"
+#include "filesystem/file_access.h"
+#include "hwmapping.h"
+#include "interfaces/arch_registers.h"
+#include "interfaces/delays.h"
+#include "interfaces/portability.h"
+#include "kernel/kernel.h"
+#include "kernel/logging.h"
+#include "kernel/sync.h"
+
+namespace miosix
+{
+
+//
+// Initialization
+//
+
+/**
+ * The example code from ST checks for the busy flag after each command.
+ * Interestingly I couldn't find any mention of this in the datsheet.
+ */
+static void sdramCommandWait()
+{
+    for (int i = 0; i < 0xffff; i++)
+        if ((FMC_Bank5_6->SDSR & FMC_SDSR_BUSY) == 0)
+            return;
+}
+
+void configureSdram()
+{
+    // Enable all gpios, passing clock
+    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN | RCC_AHB1ENR_GPIOBEN |
+                    RCC_AHB1ENR_GPIOCEN | RCC_AHB1ENR_GPIODEN |
+                    RCC_AHB1ENR_GPIOEEN | RCC_AHB1ENR_GPIOFEN |
+                    RCC_AHB1ENR_GPIOGEN | RCC_AHB1ENR_GPIOHEN;
+    RCC_SYNC();
+
+    // First, configure SDRAM GPIOs
+    GPIOB->AFR[0] = 0x0cc00000;
+    GPIOC->AFR[0] = 0x0000000c;
+    GPIOD->AFR[0] = 0x000000cc;
+    GPIOD->AFR[1] = 0xcc000ccc;
+    GPIOE->AFR[0] = 0xc00000cc;
+    GPIOE->AFR[1] = 0xcccccccc;
+    GPIOF->AFR[0] = 0x00cccccc;
+    GPIOF->AFR[1] = 0xccccc000;
+    GPIOG->AFR[0] = 0x00cc00cc;
+    GPIOG->AFR[1] = 0xc000000c;
+
+    GPIOB->MODER = 0x00002800;
+    GPIOC->MODER = 0x00000002;
+    GPIOD->MODER = 0xa02a000a;
+    GPIOE->MODER = 0xaaaa800a;
+    GPIOF->MODER = 0xaa800aaa;
+    GPIOG->MODER = 0x80020a0a;
+
+    GPIOA->OSPEEDR = 0xaaaaaaaa;  // Default to 50MHz speed for all GPIOs...
+    GPIOB->OSPEEDR =
+        0xaaaaaaaa | 0x00003c00;  //...but 100MHz for the SDRAM pins
+    GPIOC->OSPEEDR = 0xaaaaaaaa | 0x00000003;
+    GPIOD->OSPEEDR = 0xaaaaaaaa | 0xf03f000f;
+    GPIOE->OSPEEDR = 0xaaaaaaaa | 0xffffc00f;
+    GPIOF->OSPEEDR = 0xaaaaaaaa | 0xffc00fff;
+    GPIOG->OSPEEDR = 0xaaaaaaaa | 0xc0030f0f;
+    GPIOH->OSPEEDR = 0xaaaaaaaa;
+
+    // Since we'we un-configured PB3/PB4 from the default at boot TDO,NTRST,
+    // finish the job and remove the default pull-up
+    GPIOB->PUPDR = 0;
+
+    // Second, actually start the SDRAM controller
+    RCC->AHB3ENR |= RCC_AHB3ENR_FMCEN;
+    RCC_SYNC();
+
+    // SDRAM is a IS42S16400J -7 speed grade, connected to bank 2 (0xd0000000)
+    // Some bits in SDCR[1] are don't care, and the have to be set in SDCR[0],
+    // they aren't just don't care, the controller will fail if they aren't at 0
+    FMC_Bank5_6->SDCR[0] = FMC_SDCR1_SDCLK_1  // SDRAM runs @ half CPU frequency
+                           | FMC_SDCR1_RBURST  // Enable read burst
+                           | 0;              //  0 delay between reads after CAS
+    FMC_Bank5_6->SDCR[1] = 0                 //  8 bit column address
+                           | FMC_SDCR1_NR_0  // 12 bit row address
+                           | FMC_SDCR1_MWID_0  // 16 bit data bus
+                           | FMC_SDCR1_NB      //  4 banks
+                           |
+                           FMC_SDCR1_CAS_1;  //  2 cycle CAS latency (F<133MHz)
+
+#ifdef SYSCLK_FREQ_180MHz
+    // One SDRAM clock cycle is 11.1ns
+    // Some bits in SDTR[1] are don't care, and the have to be set in SDTR[0],
+    // they aren't just don't care, the controller will fail if they aren't at 0
+    FMC_Bank5_6->SDTR[0] = (6 - 1) << 12     // 6 cycle TRC  (66.6ns>63ns)
+                           | (2 - 1) << 20;  // 2 cycle TRP  (22.2ns>15ns)
+    FMC_Bank5_6->SDTR[1] = (2 - 1) << 0      // 2 cycle TMRD
+                           | (7 - 1) << 4    // 7 cycle TXSR (77.7ns>70ns)
+                           | (4 - 1) << 8    // 4 cycle TRAS (44.4ns>42ns)
+                           | (2 - 1) << 16   // 2 cycle TWR
+                           | (2 - 1) << 24;  // 2 cycle TRCD (22.2ns>15ns)
+#elif defined(SYSCLK_FREQ_168MHz)
+    // One SDRAM clock cycle is 11.9ns
+    // Some bits in SDTR[1] are don't care, and the have to be set in SDTR[0],
+    // they aren't just don't care, the controller will fail if they aren't at 0
+    FMC_Bank5_6->SDTR[0] = (6 - 1) << 12     // 6 cycle TRC  (71.4ns>63ns)
+                           | (2 - 1) << 20;  // 2 cycle TRP  (23.8ns>15ns)
+    FMC_Bank5_6->SDTR[1] = (2 - 1) << 0      // 2 cycle TMRD
+                           | (6 - 1) << 4    // 6 cycle TXSR (71.4ns>70ns)
+                           | (4 - 1) << 8    // 4 cycle TRAS (47.6ns>42ns)
+                           | (2 - 1) << 16   // 2 cycle TWR
+                           | (2 - 1) << 24;  // 2 cycle TRCD (23.8ns>15ns)
+#else
+#error No SDRAM timings for this clock
+#endif
+
+    FMC_Bank5_6->SDCMR = FMC_SDCMR_CTB2  // Enable bank 2
+                         | 1;            // MODE=001 clock enabled
+    sdramCommandWait();
+
+    // ST and SDRAM datasheet agree a 100us delay is required here.
+    delayUs(100);
+
+    FMC_Bank5_6->SDCMR = FMC_SDCMR_CTB2  // Enable bank 2
+                         | 2;            // MODE=010 precharge all command
+    sdramCommandWait();
+
+    FMC_Bank5_6->SDCMR = (8 - 1) << 5      // NRFS=8 SDRAM datasheet says
+                                           // "at least two AUTO REFRESH cycles"
+                         | FMC_SDCMR_CTB2  // Enable bank 2
+                         | 3;              // MODE=011 auto refresh
+    sdramCommandWait();
+
+    FMC_Bank5_6->SDCMR = 0x220 << 9  // MRD=0x220:CAS latency=2 burst len=1
+                         | FMC_SDCMR_CTB2  // Enable bank 2
+                         | 4;              // MODE=100 load mode register
+    sdramCommandWait();
+
+// 64ms/4096=15.625us
+#ifdef SYSCLK_FREQ_180MHz
+    // 15.625us*90MHz=1406-20=1386
+    FMC_Bank5_6->SDRTR = 1386 << 1;
+#elif defined(SYSCLK_FREQ_168MHz)
+    // 15.625us*84MHz=1312-20=1292
+    FMC_Bank5_6->SDRTR = 1292 << 1;
+#else
+#error No refresh timings for this clock
+#endif
+}
+
+void IRQbspInit()
+{
+// If using SDRAM GPIOs are enabled by configureSdram(), else enable them here
+#ifndef __ENABLE_XRAM
+    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN | RCC_AHB1ENR_GPIOBEN |
+                    RCC_AHB1ENR_GPIOCEN | RCC_AHB1ENR_GPIODEN |
+                    RCC_AHB1ENR_GPIOEEN | RCC_AHB1ENR_GPIOFEN |
+                    RCC_AHB1ENR_GPIOGEN | RCC_AHB1ENR_GPIOHEN;
+    RCC_SYNC();
+#endif  //__ENABLE_XRAM
+
+    RCC->APB2ENR |= RCC_APB2ENR_SPI1EN;
+    RCC->APB1ENR |= RCC_APB1ENR_SPI2EN;
+    RCC->APB2ENR |= RCC_APB2ENR_SPI4EN;
+    RCC->APB2ENR |= RCC_APB2ENR_SPI5EN;
+    RCC->APB2ENR |= RCC_APB2ENR_SPI6EN;
+
+    RCC->AHB1ENR |= RCC_AHB1ENR_DMA2EN;
+
+    RCC->APB2ENR |= RCC_APB2ENR_TIM1EN;
+    RCC->APB1ENR |= RCC_APB1ENR_TIM3EN;
+    RCC->APB1ENR |= RCC_APB1ENR_TIM4EN;
+    RCC->APB2ENR |= RCC_APB2ENR_TIM8EN;
+    RCC->APB1ENR |= RCC_APB1ENR_TIM2EN;
+    RCC->APB2ENR |= RCC_APB2ENR_TIM10EN;
+    RCC->APB2ENR |= RCC_APB2ENR_TIM11EN;
+
+    RCC_SYNC();
+
+    using namespace interfaces;
+
+    spi1::sck::mode(Mode::ALTERNATE);
+    spi1::sck::alternateFunction(5);
+    spi1::miso::mode(Mode::ALTERNATE);
+    spi1::miso::alternateFunction(5);
+    spi1::mosi::mode(Mode::ALTERNATE);
+    spi1::mosi::alternateFunction(5);
+
+    spi2::sck::mode(Mode::ALTERNATE);
+    spi2::sck::alternateFunction(5);
+    spi2::miso::mode(Mode::ALTERNATE);
+    spi2::miso::alternateFunction(5);
+    spi2::mosi::mode(Mode::ALTERNATE);
+    spi2::mosi::alternateFunction(5);
+
+    spi4::sck::mode(Mode::ALTERNATE);
+    spi4::sck::alternateFunction(5);
+    spi4::miso::mode(Mode::ALTERNATE);
+    spi4::miso::alternateFunction(5);
+    spi4::mosi::mode(Mode::ALTERNATE);
+    spi4::mosi::alternateFunction(5);
+
+    spi5::sck::mode(Mode::ALTERNATE);
+    spi5::sck::alternateFunction(5);
+    spi5::miso::mode(Mode::ALTERNATE);
+    spi5::miso::alternateFunction(5);
+    spi5::mosi::mode(Mode::ALTERNATE);
+    spi5::mosi::alternateFunction(5);
+
+    spi6::sck::mode(Mode::ALTERNATE);
+    spi6::sck::alternateFunction(5);
+    spi6::miso::mode(Mode::ALTERNATE);
+    spi6::miso::alternateFunction(5);
+    spi6::mosi::mode(Mode::ALTERNATE);
+    spi6::mosi::alternateFunction(5);
+
+    uart1::rx::mode(Mode::ALTERNATE);
+    uart1::rx::alternateFunction(7);
+    uart1::tx::mode(Mode::ALTERNATE);
+    uart1::tx::alternateFunction(7);
+
+    can1::rx::mode(Mode::ALTERNATE);
+    can1::rx::alternateFunction(9);
+    can1::tx::mode(Mode::ALTERNATE);
+    can1::tx::alternateFunction(9);
+
+    timers::tim4ch2::mode(Mode::ALTERNATE);
+    timers::tim11ch1::mode(Mode::ALTERNATE);
+    timers::tim3ch1::mode(Mode::ALTERNATE);
+    timers::tim10ch1::mode(Mode::ALTERNATE);
+    timers::tim8ch1::mode(Mode::ALTERNATE);
+
+    timers::tim4ch2::alternateFunction(2);
+    timers::tim11ch1::alternateFunction(3);
+    timers::tim3ch1::alternateFunction(2);
+    timers::tim10ch1::alternateFunction(3);
+    timers::tim8ch1::alternateFunction(3);
+
+    using namespace sensors;
+    ADS131_1::cs::mode(Mode::OUTPUT);
+    ADS131_1::cs::high();
+
+    ADS131_2::cs::mode(Mode::OUTPUT);
+    ADS131_2::cs::high();
+
+    MAX31855::cs::mode(Mode::OUTPUT);
+    MAX31855::cs::high();
+
+    using namespace relays;
+    ignition::mode(Mode::OUTPUT);
+    ledLamp::mode(Mode::OUTPUT);
+    nitrogen::mode(Mode::OUTPUT);
+    generalPurpose::mode(Mode::OUTPUT);
+
+    ignition::high();
+    ledLamp::high();
+    nitrogen::high();
+    generalPurpose::high();
+
+    using namespace radio;
+    cs::mode(Mode::OUTPUT);
+    dio0::mode(Mode::INPUT_PULL_UP);
+    dio1::mode(Mode::INPUT_PULL_UP);
+    dio3::mode(Mode::INPUT_PULL_UP);
+    txEn::mode(Mode::OUTPUT);
+    rxEn::mode(Mode::OUTPUT);
+    nrst::mode(Mode::OUTPUT);
+
+    cs::high();
+    nrst::high();
+
+    // TODO define default configs
+
+    using namespace ui;
+
+    button::mode(Mode::INPUT);
+
+    greenLed::mode(Mode::OUTPUT);
+    greenLed::low();
+    redLed::mode(Mode::OUTPUT);
+    redLed::low();
+
+    DefaultConsole::instance().IRQset(intrusive_ref_ptr<Device>(
+        new STM32Serial(defaultSerial, defaultSerialSpeed,
+                        defaultSerialFlowctrl ? STM32Serial::RTSCTS
+                                              : STM32Serial::NOFLOWCTRL)));
+}
+
+void bspInit2()
+{
+#ifdef WITH_FILESYSTEM
+    basicFilesystemSetup(SDIODriver::instance());
+#endif  // WITH_FILESYSTEM
+}
+
+//
+// Shutdown and reboot
+//
+
+/**
+ * @brief This function disables filesystem and serial port.
+ */
+void shutdown()
+{
+    ioctl(STDOUT_FILENO, IOCTL_SYNC, 0);
+
+#ifdef WITH_FILESYSTEM
+    FilesystemManager::instance().umountAll();
+#endif  // WITH_FILESYSTEM
+
+    disableInterrupts();
+
+    while (true)
+        ;
+}
+
+void reboot()
+{
+    ioctl(STDOUT_FILENO, IOCTL_SYNC, 0);
+
+#ifdef WITH_FILESYSTEM
+    FilesystemManager::instance().umountAll();
+#endif  // WITH_FILESYSTEM
+
+    disableInterrupts();
+    miosix_private::IRQsystemReboot();
+}
+
+}  // namespace miosix
diff --git a/src/bsps/stm32f429zi_rig/interfaces-impl/bsp_impl.h b/src/bsps/stm32f429zi_rig/interfaces-impl/bsp_impl.h
new file mode 100644
index 000000000..5317e8ae0
--- /dev/null
+++ b/src/bsps/stm32f429zi_rig/interfaces-impl/bsp_impl.h
@@ -0,0 +1,48 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * 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.
+ */
+
+/***********************************************************************
+ * bsp_impl.h Part of the Miosix Embedded OS.
+ * Board support package, this file initializes hardware.
+ ************************************************************************/
+
+#ifndef BSP_IMPL_H
+#define BSP_IMPL_H
+
+#include "config/miosix_settings.h"
+#include "drivers/stm32_hardware_rng.h"
+#include "hwmapping.h"
+#include "interfaces/gpio.h"
+
+namespace miosix
+{
+
+/**
+ * \internal
+ * Called by stage_1_boot.cpp to enable the SDRAM before initializing .data/.bss
+ * Requires the CPU clock to be already configured (running from the PLL)
+ */
+void configureSdram();
+
+}  // namespace miosix
+
+#endif  // BSP_IMPL_H
diff --git a/src/bsps/stm32f429zi_rig/interfaces-impl/hwmapping.h b/src/bsps/stm32f429zi_rig/interfaces-impl/hwmapping.h
new file mode 100644
index 000000000..6fe1b142a
--- /dev/null
+++ b/src/bsps/stm32f429zi_rig/interfaces-impl/hwmapping.h
@@ -0,0 +1,173 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include "interfaces/gpio.h"
+
+namespace miosix
+{
+
+namespace interfaces
+{
+
+namespace spi1
+{
+using sck  = Gpio<GPIOA_BASE, 5>;
+using miso = Gpio<GPIOA_BASE, 6>;
+using mosi = Gpio<GPIOA_BASE, 7>;
+}  // namespace spi1
+
+namespace spi2
+{
+using sck  = Gpio<GPIOD_BASE, 3>;
+using miso = Gpio<GPIOC_BASE, 2>;
+using mosi = Gpio<GPIOC_BASE, 3>;
+}  // namespace spi2
+
+namespace spi4
+{
+using sck  = Gpio<GPIOE_BASE, 2>;
+using miso = Gpio<GPIOE_BASE, 5>;
+using mosi = Gpio<GPIOE_BASE, 6>;
+}  // namespace spi4
+
+namespace spi5
+{
+using sck  = Gpio<GPIOF_BASE, 7>;
+using miso = Gpio<GPIOF_BASE, 8>;
+using mosi = Gpio<GPIOF_BASE, 9>;
+}  // namespace spi5
+
+namespace spi6
+{
+using sck  = Gpio<GPIOG_BASE, 13>;
+using miso = Gpio<GPIOG_BASE, 12>;
+using mosi = Gpio<GPIOG_BASE, 14>;
+}  // namespace spi6
+
+// USB UART
+namespace uart1
+{
+using tx = Gpio<GPIOA_BASE, 9>;
+using rx = Gpio<GPIOA_BASE, 10>;
+}  // namespace uart1
+
+namespace can1
+{
+using tx = Gpio<GPIOA_BASE, 12>;
+using rx = Gpio<GPIOA_BASE, 11>;
+}  // namespace can1
+
+namespace timers
+{
+using tim4ch2  = Gpio<GPIOB_BASE, 7>;  // Servo 1
+using tim11ch1 = Gpio<GPIOB_BASE, 9>;  // Servo 2
+using tim3ch1  = Gpio<GPIOB_BASE, 4>;  // Servo 3
+using tim10ch1 = Gpio<GPIOB_BASE, 8>;  // Servo 4
+using tim8ch1  = Gpio<GPIOC_BASE, 6>;  // Servo 5
+}  // namespace timers
+
+}  // namespace interfaces
+
+namespace sensors
+{
+
+namespace ADS131_1
+{
+using cs   = Gpio<GPIOB_BASE, 1>;
+using sck  = interfaces::spi1::sck;
+using miso = interfaces::spi1::miso;
+using mosi = interfaces::spi1::mosi;
+}  // namespace ADS131_1
+
+namespace ADS131_2
+{
+using cs   = Gpio<GPIOE_BASE, 4>;
+using sck  = interfaces::spi1::sck;
+using miso = interfaces::spi1::miso;
+using mosi = interfaces::spi1::mosi;
+}  // namespace ADS131_2
+
+namespace MAX31855
+{
+using cs   = Gpio<GPIOF_BASE, 9>;
+using sck  = interfaces::spi1::sck;
+using miso = interfaces::spi1::miso;
+using mosi = interfaces::spi1::mosi;
+}  // namespace MAX31855
+
+namespace HX711_1
+{
+using sck = interfaces::spi2::sck;
+}
+namespace HX711_2
+{
+using sck = interfaces::spi6::sck;
+}
+
+namespace HX711_3
+{
+using sck = interfaces::spi4::sck;
+}
+
+}  // namespace sensors
+
+namespace servos
+{
+using servo1 = interfaces::timers::tim4ch2;
+using servo2 = interfaces::timers::tim11ch1;
+using servo3 = interfaces::timers::tim3ch1;
+using servo4 = interfaces::timers::tim10ch1;
+using servo5 = interfaces::timers::tim8ch1;
+}  // namespace servos
+
+namespace relays
+{
+using ignition       = Gpio<GPIOC_BASE, 14>;
+using ledLamp        = Gpio<GPIOE_BASE, 3>;
+using nitrogen       = Gpio<GPIOC_BASE, 13>;
+using generalPurpose = Gpio<GPIOA_BASE, 15>;
+}  // namespace relays
+
+namespace radio
+{
+using cs   = Gpio<GPIOF_BASE, 6>;
+using sck  = interfaces::spi4::sck;
+using miso = interfaces::spi4::miso;
+using mosi = interfaces::spi4::mosi;
+using dio0 = Gpio<GPIOD_BASE, 5>;
+using dio1 = Gpio<GPIOD_BASE, 12>;
+using dio3 = Gpio<GPIOD_BASE, 13>;
+using txEn = Gpio<GPIOG_BASE, 2>;
+using rxEn = Gpio<GPIOG_BASE, 3>;
+using nrst = Gpio<GPIOB_BASE, 0>;
+}  // namespace radio
+
+namespace ui
+{
+using button   = Gpio<GPIOA_BASE, 0>;   // User button
+using greenLed = Gpio<GPIOG_BASE, 13>;  // Green LED
+using redLed   = Gpio<GPIOG_BASE, 14>;  // Red LED
+}  // namespace ui
+
+}  // namespace miosix
diff --git a/src/bsps/stm32f429zi_rig/stm32_2m+256k_rom.ld b/src/bsps/stm32f429zi_rig/stm32_2m+256k_rom.ld
new file mode 100644
index 000000000..cfcc2ce53
--- /dev/null
+++ b/src/bsps/stm32f429zi_rig/stm32_2m+256k_rom.ld
@@ -0,0 +1,181 @@
+/*
+ * C++ enabled linker script for stm32 (2M FLASH, 256K RAM)
+ * Developed by TFT: Terraneo Federico Technologies
+ * Optimized for use with the Miosix kernel
+ */
+
+/*
+ * This chip has an unusual quirk that the RAM is divided in two block mapped
+ * at two non contiguous memory addresses. I don't know why they've done that,
+ * probably doing the obvious thing would have made writing code too easy...
+ * Anyway, since hardware can't be changed, we've got to live with that and
+ * try to make use of both RAMs.
+ * 
+ * Given the constraints above, this linker script puts:
+ * - read only data and code (.text, .rodata, .eh_*) in FLASH
+ * - the 512Byte main (IRQ) stack, .data and .bss in the "small" 64KB RAM
+ * - stacks and heap in the "large" 192KB RAM.
+ * 
+ * Unfortunately thread stacks can't be put in the small RAM as Miosix
+ * allocates them inside the heap.
+ */
+
+/*
+ * The main stack is used for interrupt handling by the kernel.
+ *
+ * *** Readme ***
+ * This linker script places the main stack (used by the kernel for interrupts)
+ * at the bottom of the ram, instead of the top. This is done for two reasons:
+ *
+ * - as an optimization for microcontrollers with little ram memory. In fact
+ *   the implementation of malloc from newlib requests memory to the OS in 4KB
+ *   block (except the first block that can be smaller). This is probably done
+ *   for compatibility with OSes with an MMU and paged memory. To see why this
+ *   is bad, consider a microcontroller with 8KB of ram: when malloc finishes
+ *   up the first 4KB it will call _sbrk_r asking for a 4KB block, but this will
+ *   fail because the top part of the ram is used by the main stack. As a
+ *   result, the top part of the memory will not be used by malloc, even if
+ *   available (and it is nearly *half* the ram on an 8KB mcu). By placing the
+ *   main stack at the bottom of the ram, the upper 4KB block will be entirely
+ *   free and available as heap space.
+ *
+ * - In case of main stack overflow the cpu will fault because access to memory
+ *   before the beginning of the ram faults. Instead with the default stack
+ *   placement the main stack will silently collide with the heap.
+ * Note: if increasing the main stack size also increase the ORIGIN value in
+ * the MEMORY definitions below accordingly.
+ */
+_main_stack_size = 0x00000200;                     /* main stack = 512Bytes */
+_main_stack_top  = 0x10000000 + _main_stack_size;
+ASSERT(_main_stack_size   % 8 == 0, "MAIN stack size error");
+
+/* Mapping the heap into the large 192KB RAM */
+_end =      0x20000000;
+_heap_end = 0x20030000;                            /* end of available ram  */
+
+/* identify the Entry Point  */
+ENTRY(_Z13Reset_Handlerv)
+
+/* specify the memory areas  */
+MEMORY
+{
+    flash(rx)    : ORIGIN = 0x08000000, LENGTH =   2M
+    /*
+     * Note, the small ram starts at 0x10000000 but it is necessary to add the
+     * size of the main stack, so it is 0x10000200.
+     */
+    smallram(wx) : ORIGIN = 0x10000200, LENGTH =  64K-0x200 
+    largeram(wx) : ORIGIN = 0x20000000, LENGTH = 192K
+    backupram(rw): ORIGIN = 0x40024000, LENGTH =  4K
+}
+
+/* now define the output sections  */
+SECTIONS
+{
+    . = 0;
+    
+    /* .text section: code goes to flash */
+    .text :
+    {
+        /* Startup code must go at address 0 */
+        KEEP(*(.isr_vector))
+        
+        *(.text)
+        *(.text.*)
+        *(.gnu.linkonce.t.*)
+        /* these sections for thumb interwork? */
+        *(.glue_7)
+        *(.glue_7t)
+        /* these sections for C++? */
+        *(.gcc_except_table)
+        *(.gcc_except_table.*)
+        *(.ARM.extab*)
+        *(.gnu.linkonce.armextab.*)
+
+        . = ALIGN(4);
+        /* .rodata: constant data */
+        *(.rodata)
+        *(.rodata.*)
+        *(.gnu.linkonce.r.*)
+
+        /* C++ Static constructors/destructors (eabi) */
+        . = ALIGN(4);
+        KEEP(*(.init))
+        
+        . = ALIGN(4);
+        __miosix_init_array_start = .;
+        KEEP (*(SORT(.miosix_init_array.*)))
+        KEEP (*(.miosix_init_array))
+        __miosix_init_array_end = .;
+
+        . = ALIGN(4);
+        __preinit_array_start = .;
+        KEEP (*(.preinit_array))
+        __preinit_array_end = .;
+
+        . = ALIGN(4);
+        __init_array_start = .;
+        KEEP (*(SORT(.init_array.*)))
+        KEEP (*(.init_array))
+        __init_array_end = .;
+
+        . = ALIGN(4);
+        KEEP(*(.fini))
+
+        . = ALIGN(4);
+        __fini_array_start = .;
+        KEEP (*(.fini_array))
+        KEEP (*(SORT(.fini_array.*)))
+        __fini_array_end = .;
+
+        /* C++ Static constructors/destructors (elf)  */
+        . = ALIGN(4);
+        _ctor_start = .;
+        KEEP (*crtbegin.o(.ctors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .ctors))
+        KEEP (*(SORT(.ctors.*)))
+        KEEP (*crtend.o(.ctors))
+       _ctor_end = .;
+
+        . = ALIGN(4);
+        KEEP (*crtbegin.o(.dtors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .dtors))
+        KEEP (*(SORT(.dtors.*)))
+        KEEP (*crtend.o(.dtors))
+    } > flash
+
+    /* .ARM.exidx is sorted, so has to go in its own output section.  */
+    __exidx_start = .;
+    .ARM.exidx :
+    {
+        *(.ARM.exidx* .gnu.linkonce.armexidx.*)
+    } > flash
+    __exidx_end = .;
+
+	/* .data section: global variables go to ram, but also store a copy to
+       flash to initialize them */
+    .data : ALIGN(8)
+    {
+        _data = .;
+        *(.data)
+        *(.data.*)
+        *(.gnu.linkonce.d.*)
+        . = ALIGN(8);
+        _edata = .;
+    } > smallram AT > flash
+    _etext = LOADADDR(.data);
+
+    /* .bss section: uninitialized global variables go to ram */
+    _bss_start = .;
+    .bss :
+    {
+        *(.bss)
+        *(.bss.*)
+        *(.gnu.linkonce.b.*)
+        . = ALIGN(8);
+    } > smallram
+    _bss_end = .;
+
+    /*_end = .;*/
+    /*PROVIDE(end = .);*/
+}
diff --git a/src/bsps/stm32f429zi_rig/stm32_2m+8m_xram.ld b/src/bsps/stm32f429zi_rig/stm32_2m+8m_xram.ld
new file mode 100644
index 000000000..b6a0d2668
--- /dev/null
+++ b/src/bsps/stm32f429zi_rig/stm32_2m+8m_xram.ld
@@ -0,0 +1,178 @@
+/*
+ * C++ enabled linker script for stm32 (2M FLASH, 256K RAM, 8M XRAM)
+ * Developed by TFT: Terraneo Federico Technologies
+ * Optimized for use with the Miosix kernel
+ */
+
+/*
+ * This chip has an unusual quirk that the RAM is divided in two block mapped
+ * at two non contiguous memory addresses. I don't know why they've done that,
+ * probably doing the obvious thing would have made writing code too easy...
+ * Anyway, since hardware can't be changed, we've got to live with that and
+ * try to make use of both RAMs.
+ * 
+ * Given the constraints above, this linker script puts:
+ * - read only data and code (.text, .rodata, .eh_*) in FLASH
+ * - the 512Byte main (IRQ) stack, .data and .bss in the "small" 64KB RAM
+ * - .data, .bss, stacks and heap in the external 8MB SDRAM.
+ */
+
+/*
+ * The main stack is used for interrupt handling by the kernel.
+ *
+ * *** Readme ***
+ * This linker script places the main stack (used by the kernel for interrupts)
+ * at the bottom of the ram, instead of the top. This is done for two reasons:
+ *
+ * - as an optimization for microcontrollers with little ram memory. In fact
+ *   the implementation of malloc from newlib requests memory to the OS in 4KB
+ *   block (except the first block that can be smaller). This is probably done
+ *   for compatibility with OSes with an MMU and paged memory. To see why this
+ *   is bad, consider a microcontroller with 8KB of ram: when malloc finishes
+ *   up the first 4KB it will call _sbrk_r asking for a 4KB block, but this will
+ *   fail because the top part of the ram is used by the main stack. As a
+ *   result, the top part of the memory will not be used by malloc, even if
+ *   available (and it is nearly *half* the ram on an 8KB mcu). By placing the
+ *   main stack at the bottom of the ram, the upper 4KB block will be entirely
+ *   free and available as heap space.
+ *
+ * - In case of main stack overflow the cpu will fault because access to memory
+ *   before the beginning of the ram faults. Instead with the default stack
+ *   placement the main stack will silently collide with the heap.
+ * Note: if increasing the main stack size also increase the ORIGIN value in
+ * the MEMORY definitions below accordingly.
+ */
+_main_stack_size = 0x00000200;                     /* main stack = 512Bytes */
+_main_stack_top  = 0x10000000 + _main_stack_size;
+ASSERT(_main_stack_size   % 8 == 0, "MAIN stack size error");
+
+/* Mapping the heap into XRAM */
+_heap_end = 0xd0800000;                            /* end of available ram  */
+
+/* identify the Entry Point  */
+ENTRY(_Z13Reset_Handlerv)
+
+/* specify the memory areas  */
+MEMORY
+{
+    flash(rx)    : ORIGIN = 0x08000000, LENGTH =   2M
+    /*
+     * Note, the small ram starts at 0x10000000 but it is necessary to add the
+     * size of the main stack, so it is 0x10000200.
+     */
+    smallram(wx) : ORIGIN = 0x10000200, LENGTH =  64K-0x200 
+    largeram(wx) : ORIGIN = 0x20000000, LENGTH = 192K
+    backupram(rw): ORIGIN = 0x40024000, LENGTH =   4K
+    xram(wx)     : ORIGIN = 0xd0000000, LENGTH =   8M
+}
+
+/* now define the output sections  */
+SECTIONS
+{
+    . = 0;
+    
+    /* .text section: code goes to flash */
+    .text :
+    {
+        /* Startup code must go at address 0 */
+        KEEP(*(.isr_vector))
+        
+        *(.text)
+        *(.text.*)
+        *(.gnu.linkonce.t.*)
+        /* these sections for thumb interwork? */
+        *(.glue_7)
+        *(.glue_7t)
+        /* these sections for C++? */
+        *(.gcc_except_table)
+        *(.gcc_except_table.*)
+        *(.ARM.extab*)
+        *(.gnu.linkonce.armextab.*)
+
+        . = ALIGN(4);
+        /* .rodata: constant data */
+        *(.rodata)
+        *(.rodata.*)
+        *(.gnu.linkonce.r.*)
+
+        /* C++ Static constructors/destructors (eabi) */
+        . = ALIGN(4);
+        KEEP(*(.init))
+        
+        . = ALIGN(4);
+        __miosix_init_array_start = .;
+        KEEP (*(SORT(.miosix_init_array.*)))
+        KEEP (*(.miosix_init_array))
+        __miosix_init_array_end = .;
+
+        . = ALIGN(4);
+        __preinit_array_start = .;
+        KEEP (*(.preinit_array))
+        __preinit_array_end = .;
+
+        . = ALIGN(4);
+        __init_array_start = .;
+        KEEP (*(SORT(.init_array.*)))
+        KEEP (*(.init_array))
+        __init_array_end = .;
+
+        . = ALIGN(4);
+        KEEP(*(.fini))
+
+        . = ALIGN(4);
+        __fini_array_start = .;
+        KEEP (*(.fini_array))
+        KEEP (*(SORT(.fini_array.*)))
+        __fini_array_end = .;
+
+        /* C++ Static constructors/destructors (elf)  */
+        . = ALIGN(4);
+        _ctor_start = .;
+        KEEP (*crtbegin.o(.ctors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .ctors))
+        KEEP (*(SORT(.ctors.*)))
+        KEEP (*crtend.o(.ctors))
+       _ctor_end = .;
+
+        . = ALIGN(4);
+        KEEP (*crtbegin.o(.dtors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .dtors))
+        KEEP (*(SORT(.dtors.*)))
+        KEEP (*crtend.o(.dtors))
+    } > flash
+
+    /* .ARM.exidx is sorted, so has to go in its own output section.  */
+    __exidx_start = .;
+    .ARM.exidx :
+    {
+        *(.ARM.exidx* .gnu.linkonce.armexidx.*)
+    } > flash
+    __exidx_end = .;
+
+	/* .data section: global variables go to ram, but also store a copy to
+       flash to initialize them */
+    .data : ALIGN(8)
+    {
+        _data = .;
+        *(.data)
+        *(.data.*)
+        *(.gnu.linkonce.d.*)
+        . = ALIGN(8);
+        _edata = .;
+    } > xram AT > flash
+    _etext = LOADADDR(.data);
+
+    /* .bss section: uninitialized global variables go to ram */
+    _bss_start = .;
+    .bss :
+    {
+        *(.bss)
+        *(.bss.*)
+        *(.gnu.linkonce.b.*)
+        . = ALIGN(8);
+    } > xram
+    _bss_end = .;
+
+    _end = .;
+    PROVIDE(end = .);
+}
diff --git a/src/bsps/stm32f756zg_nucleo/config/board_options.cmake b/src/bsps/stm32f756zg_nucleo/config/board_options.cmake
new file mode 100644
index 000000000..079c7af5d
--- /dev/null
+++ b/src/bsps/stm32f756zg_nucleo/config/board_options.cmake
@@ -0,0 +1,102 @@
+# Copyright (C) 2023 by Skyward
+#
+# This program is free software; you can redistribute it and/or 
+# it under the terms of the GNU General Public License as published 
+# the Free Software Foundation; either version 2 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.
+#
+# As a special exception, if other files instantiate templates or use
+# macros or inline functions from this file, or you compile this file
+# and link it with other works to produce a work based on this file,
+# this file does not by itself cause the resulting work to be covered
+# by the GNU General Public License. However the source code for this
+# file must still be made available in accordance with the GNU 
+# Public License. This exception does not invalidate any other 
+# why a work based on this file might be covered by the GNU General
+# Public License.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, see <http://www.gnu.org/licenses/>
+
+set(BOARD_NAME stm32f756zg_nucleo)
+set(ARCH_NAME cortexM7_stm32f7)
+
+# Base directories with header files for this board
+set(ARCH_PATH ${KPATH}/arch/${ARCH_NAME}/common)
+set(BOARD_PATH ${BOARDCORE_PATH}/src/bsps/${BOARD_NAME})
+set(BOARD_CONFIG_PATH ${BOARDCORE_PATH}/src/bsps/${BOARD_NAME}/config)
+
+# Specify where to find the board specific config/miosix_settings.h
+set(BOARD_MIOSIX_SETTINGS_PATH ${BOARD_PATH})
+
+# Specify where to find the board specific config/mxgui_settings.h
+set(BOARD_MXGUI_SETTINGS_PATH ${BOARD_PATH})
+
+# Optimization flags:
+# -O0 do no optimization, the default if no optimization level is specified
+# -O or -O1 optimize minimally
+# -O2 optimize more
+# -O3 optimize even more
+# -Ofast optimize very aggressively to the point of breaking the standard
+# -Og Optimize debugging experience, enables optimizations that do not
+# interfere with debugging
+# -Os Optimize for size with -O2 optimizations that do not increase code size
+set(OPT_OPTIMIZATION -O2)
+
+# Boot file and linker script
+set(BOOT_FILE ${BOARD_PATH}/core/stage_1_boot.cpp)
+set(LINKER_SCRIPT ${BOARD_PATH}/stm32_1m+256k_rom.ld)
+
+# Select clock frequency (HSE_VALUE is the xtal on board, fixed)
+set(CLOCK_FREQ -DHSE_VALUE=8000000 -DSYSCLK_FREQ_216MHz=216000000)
+
+# C++ Exception/rtti support disable flags.
+# To save code size if not using C++ exceptions (nor some STL code which
+# implicitly uses it) uncomment this option.
+# -D__NO_EXCEPTIONS is used by Miosix to know if exceptions are used.
+# set(OPT_EXCEPT -fno-exceptions -fno-rtti -D__NO_EXCEPTIONS)
+
+# Specify a custom flash command
+# This is the program that is invoked when the flash flag (-f or --flash) is
+# used with the Miosix Build System. Use $binary or $hex as placeolders, they
+# will be replaced by the build systems with the binary or hex file repectively.
+# If a command is not specified, the build system will use st-flash if found
+# set(PROGRAM_CMDLINE "here your custom flash command")
+
+# Basic flags
+set(FLAGS_BASE -mcpu=cortex-m4 -mthumb -mfloat-abi=hard -mfpu=fpv4-sp-d16)
+
+# Flags for ASM and linker
+set(AFLAGS_BASE ${FLAGS_BASE})
+set(LFLAGS_BASE ${FLAGS_BASE} -Wl,--gc-sections,-Map,main.map -Wl,-T${LINKER_SCRIPT} ${OPT_EXCEPT} ${OPT_OPTIMIZATION} -nostdlib)
+
+# Flags for C/C++
+string(TOUPPER ${BOARD_NAME} BOARD_UPPER)
+set(CFLAGS_BASE
+    -D_BOARD_${BOARD_UPPER} -D_MIOSIX_BOARDNAME=\"${BOARD_NAME}\"
+    -D_DEFAULT_SOURCE=1 -ffunction-sections -Wall -Werror=return-type -g
+    -D_ARCH_CORTEXM7_STM32F7
+    ${CLOCK_FREQ} ${XRAM} ${SRAM_BOOT} ${FLAGS_BASE} ${OPT_OPTIMIZATION} -c
+)
+set(CXXFLAGS_BASE ${CFLAGS_BASE} ${OPT_EXCEPT})
+
+# Select architecture specific files
+set(ARCH_SRC
+    ${ARCH_PATH}/interfaces-impl/delays.cpp
+    ${ARCH_PATH}/interfaces-impl/gpio_impl.cpp
+    ${ARCH_PATH}/interfaces-impl/portability.cpp
+    ${BOARD_PATH}/interfaces-impl/bsp.cpp
+    ${KPATH}/arch/common/CMSIS/Device/ST/STM32F7xx/Source/Templates/system_stm32f7xx.c
+    ${KPATH}/arch/common/core/cache_cortexMx.cpp
+    ${KPATH}/arch/common/core/interrupts_cortexMx.cpp
+    ${KPATH}/arch/common/core/mpu_cortexMx.cpp
+    ${KPATH}/arch/common/core/stm32f2_f4_l4_f7_h7_os_timer.cpp
+    ${KPATH}/arch/common/drivers/sd_stm32f2_f4_f7.cpp
+    ${KPATH}/arch/common/drivers/serial_stm32.cpp
+    ${KPATH}/arch/common/drivers/stm32_hardware_rng.cpp
+)
diff --git a/src/bsps/stm32f756zg_nucleo/config/board_settings.h b/src/bsps/stm32f756zg_nucleo/config/board_settings.h
new file mode 100644
index 000000000..bacc222e3
--- /dev/null
+++ b/src/bsps/stm32f756zg_nucleo/config/board_settings.h
@@ -0,0 +1,61 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Lorenzo Cucchi
+ *
+ * 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.
+ */
+
+#pragma once
+
+/**
+ * \internal
+ * Versioning for board_settings.h for out of git tree projects
+ */
+#define BOARD_SETTINGS_VERSION 300
+
+namespace miosix
+{
+
+/**
+ * \addtogroup Settings
+ * \{
+ */
+
+/// Size of stack for main().
+/// The C standard library is stack-heavy (iprintf requires 1KB)
+const unsigned int MAIN_STACK_SIZE = 4 * 1024;
+
+/// Serial port
+const unsigned int defaultSerialSpeed = 115200;
+// #define SERIAL_1_DMA
+// #define SERIAL_2_DMA
+#define SERIAL_3_DMA
+
+// SD card driver
+static const unsigned char sdVoltage = 33;  // Board powered @ 3.3V
+#define SD_ONE_BIT_DATABUS                  // For now we'll use 1 bit bus
+#define SD_SDMMC 1                          // Select either SDMMC1 or SDMMC2
+
+/// Analog supply voltage for ADC, DAC, Reset blocks, RCs and PLL
+#define V_DDA_VOLTAGE 3.3f
+
+/**
+ * \}
+ */
+
+}  // namespace miosix
diff --git a/src/bsps/stm32f756zg_nucleo/config/miosix_settings.h b/src/bsps/stm32f756zg_nucleo/config/miosix_settings.h
new file mode 100644
index 000000000..25ff28235
--- /dev/null
+++ b/src/bsps/stm32f756zg_nucleo/config/miosix_settings.h
@@ -0,0 +1,240 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Lorenzo Cucchi
+ *
+ * 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.
+ */
+
+#pragma once
+
+// Before you can compile the kernel you have to configure it by editing this
+// file. After that, comment out this line to disable the reminder error.
+// The PARSING_FROM_IDE is because Netbeans gets confused by this, it is never
+// defined when compiling the code.
+#ifndef PARSING_FROM_IDE
+//#error This error is a reminder that you have not edited miosix_settings.h
+// yet.
+#endif  // PARSING_FROM_IDE
+
+/**
+ * \file miosix_settings.h
+ * NOTE: this file contains ONLY configuration options that are not dependent
+ * on architecture specific details. The other options are in the following
+ * files which are included here:
+ * miosix/arch/architecture name/common/arch_settings.h
+ * miosix/config/arch/architecture name/board name/board_settings.h
+ */
+#include "arch_settings.h"
+#include "board_settings.h"
+#include "util/version.h"
+
+/**
+ * \internal
+ * Versioning for miosix_settings.h for out of git tree projects
+ */
+#define MIOSIX_SETTINGS_VERSION 300
+
+namespace miosix
+{
+
+/**
+ * \addtogroup Settings
+ * \{
+ */
+
+//
+// Scheduler options
+//
+
+/// \def SCHED_TYPE_PRIORITY
+/// If uncommented selects the priority scheduler
+/// \def SCHED_TYPE_CONTROL_BASED
+/// If uncommented selects the control based scheduler
+/// \def SCHED_TYPE_EDF
+/// If uncommented selects the EDF scheduler
+// Uncomment only *one* of those
+
+#define SCHED_TYPE_PRIORITY
+//#define SCHED_TYPE_CONTROL_BASED
+//#define SCHED_TYPE_EDF
+
+/// \def WITH_CPU_TIME_COUNTER
+/// Allows to enable/disable CPUTimeCounter to save code size and remove its
+/// overhead from the scheduling process. By default it is not defined
+/// (CPUTimeCounter is disabled).
+//#define WITH_CPU_TIME_COUNTER
+
+//
+// Filesystem options
+//
+
+/// \def WITH_FILESYSTEM
+/// Allows to enable/disable filesystem support to save code size
+/// By default it is defined (filesystem support is enabled)
+#define WITH_FILESYSTEM
+
+/// \def WITH_DEVFS
+/// Allows to enable/disable DevFs support to save code size
+/// By default it is defined (DevFs is enabled)
+#define WITH_DEVFS
+
+/// \def SYNC_AFTER_WRITE
+/// Increases filesystem write robustness. After each write operation the
+/// filesystem is synced so that a power failure happens data is not lost
+/// (unless power failure happens exactly between the write and the sync)
+/// Unfortunately write latency and throughput becomes twice as worse
+/// By default it is defined (slow but safe)
+#define SYNC_AFTER_WRITE
+
+/// Maximum number of open files. Trying to open more will fail.
+/// Cannot be lower than 3, as the first three are stdin, stdout, stderr
+const unsigned char MAX_OPEN_FILES = 8;
+
+/// \def WITH_PROCESSES
+/// If uncommented enables support for processes as well as threads.
+/// This enables the dynamic loader to load elf programs, the extended system
+/// call service and, if the hardware supports it, the MPU to provide memory
+/// isolation of processes
+//#define WITH_PROCESSES
+
+#if defined(WITH_PROCESSES) && defined(__NO_EXCEPTIONS)
+#error Processes require C++ exception support
+#endif  // defined(WITH_PROCESSES) && defined(__NO_EXCEPTIONS)
+
+#if defined(WITH_PROCESSES) && !defined(WITH_FILESYSTEM)
+#error Processes require filesystem support
+#endif  // defined(WITH_PROCESSES) && !defined(WITH_FILESYSTEM)
+
+#if defined(WITH_PROCESSES) && !defined(WITH_DEVFS)
+#error Processes require devfs support
+#endif  // defined(WITH_PROCESSES) && !defined(WITH_DEVFS)
+
+//
+// C/C++ standard library I/O (stdin, stdout and stderr related)
+//
+
+/// \def WITH_BOOTLOG
+/// Uncomment to print bootlogs on stdout.
+/// By default it is defined (bootlogs are printed)
+#define WITH_BOOTLOG
+
+/// \def WITH_ERRLOG
+/// Uncomment for debug information on stdout.
+/// By default it is defined (error information is printed)
+#define WITH_ERRLOG
+
+//
+// Kernel related options (stack sizes, priorities)
+//
+
+/// \def WITH_DEEP_SLEEP
+/// Adds interfaces and required variables to support deep sleep state switch
+/// automatically when peripherals are not required
+//#define WITH_DEEP_SLEEP
+
+/**
+ * \def JTAG_DISABLE_SLEEP
+ * JTAG debuggers lose communication with the device if it enters sleep
+ * mode, so to use debugging it is necessary to disable sleep in the idle
+ * thread. By default it is not defined (idle thread calls sleep).
+ */
+//#define JTAG_DISABLE_SLEEP
+
+#if defined(WITH_DEEP_SLEEP) && defined(JTAG_DISABLE_SLEEP)
+#error Deep sleep cannot work together with jtag
+#endif  // defined(WITH_PROCESSES) && !defined(WITH_DEVFS)
+
+/// Minimum stack size (MUST be divisible by 4)
+const unsigned int STACK_MIN = 256;
+
+/// \internal Size of idle thread stack.
+/// Should be >=STACK_MIN (MUST be divisible by 4)
+const unsigned int STACK_IDLE = 256;
+
+/// Default stack size for pthread_create.
+/// The chosen value is enough to call C standard library functions
+/// such as printf/fopen which are stack-heavy
+const unsigned int STACK_DEFAULT_FOR_PTHREAD = 2048;
+
+/// Maximum size of the RAM image of a process. If a program requires more
+/// the kernel will not run it (MUST be divisible by 4)
+const unsigned int MAX_PROCESS_IMAGE_SIZE = 64 * 1024;
+
+/// Minimum size of the stack for a process. If a program specifies a lower
+/// size the kernel will not run it (MUST be divisible by 4)
+const unsigned int MIN_PROCESS_STACK_SIZE = STACK_MIN;
+
+/// Every userspace thread has two stacks, one for when it is running in
+/// userspace and one for when it is running in kernelspace (that is, while it
+/// is executing system calls). This is the size of the stack for when the
+/// thread is running in kernelspace (MUST be divisible by 4)
+const unsigned int SYSTEM_MODE_PROCESS_STACK_SIZE = 2 * 1024;
+
+/// Number of priorities (MUST be >1)
+/// PRIORITY_MAX-1 is the highest priority, 0 is the lowest. -1 is reserved as
+/// the priority of the idle thread.
+/// The meaning of a thread's priority depends on the chosen scheduler.
+#ifdef SCHED_TYPE_PRIORITY
+// Can be modified, but a high value makes context switches more expensive
+const short int PRIORITY_MAX = 4;
+#elif defined(SCHED_TYPE_CONTROL_BASED)
+// Don't touch, the limit is due to the fixed point implementation
+// It's not needed for if floating point is selected, but kept for consistency
+const short int PRIORITY_MAX = 64;
+#else  // SCHED_TYPE_EDF
+// Doesn't exist for this kind of scheduler
+#endif
+
+/// Priority of main()
+/// The meaning of a thread's priority depends on the chosen scheduler.
+const unsigned char MAIN_PRIORITY = 1;
+
+#ifdef SCHED_TYPE_PRIORITY
+/// Maximum thread time slice in nanoseconds, after which preemption occurs
+const unsigned int MAX_TIME_SLICE = 1000000;
+#endif  // SCHED_TYPE_PRIORITY
+
+//
+// Other low level kernel options. There is usually no need to modify these.
+//
+
+/// \internal Length of wartermark (in bytes) to check stack overflow.
+/// MUST be divisible by 4 and can also be zero.
+/// A high value increases context switch time.
+const unsigned int WATERMARK_LEN = 16;
+
+/// \internal Used to fill watermark
+const unsigned int WATERMARK_FILL = 0xaaaaaaaa;
+
+/// \internal Used to fill stack (for checking stack usage)
+const unsigned int STACK_FILL = 0xbbbbbbbb;
+
+// Compiler version checks
+#if !defined(_MIOSIX_GCC_PATCH_MAJOR) || _MIOSIX_GCC_PATCH_MAJOR < 3
+#error \
+    "You are using a too old or unsupported compiler. Get the latest one from https://miosix.org/wiki/index.php?title=Miosix_Toolchain"
+#endif
+#if _MIOSIX_GCC_PATCH_MAJOR > 3
+#warning "You are using a too new compiler, which may not be supported"
+#endif
+
+/**
+ * \}
+ */
+
+}  // namespace miosix
diff --git a/src/bsps/stm32f756zg_nucleo/core/stage_1_boot.cpp b/src/bsps/stm32f756zg_nucleo/core/stage_1_boot.cpp
new file mode 100644
index 000000000..f0bb48965
--- /dev/null
+++ b/src/bsps/stm32f756zg_nucleo/core/stage_1_boot.cpp
@@ -0,0 +1,454 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Lorenzo Cucchi
+ *
+ * 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 <string.h>
+
+#include "core/cache_cortexMx.h"
+#include "core/interrupts.h"  //For the unexpected interrupt call
+#include "core/interrupts_cortexMx.h"
+#include "interfaces/arch_registers.h"
+#include "interfaces/bsp.h"
+#include "kernel/stage_2_boot.h"
+
+/*
+ * startup.cpp
+ * STM32 C++ startup.
+ * NOTE: for stm32f756 devices ONLY.
+ * Supports interrupt handlers in C++ without extern "C"
+ * Developed by Terraneo Federico, based on ST startup code.
+ * Additionally modified to boot Miosix.
+ */
+
+/**
+ * Called by Reset_Handler, performs initialization and calls main.
+ * Never returns.
+ */
+void program_startup() __attribute__((noreturn));
+void program_startup()
+{
+    // Cortex M7 core appears to get out of reset with interrupts already
+    // enabled
+    __disable_irq();
+
+    // SystemInit() is called *before* initializing .data and zeroing .bss
+    // Despite all startup files provided by ST do the opposite, there are three
+    // good reasons to do so:
+    // First, the CMSIS specifications say that SystemInit() must not access
+    // global variables, so it is actually possible to call it before
+    // Second, when running Miosix with the xram linker scripts .data and .bss
+    // are placed in the external RAM, so we *must* call SystemInit(), which
+    // enables xram, before touching .data and .bss
+    // Third, this is a performance improvement since the loops that initialize
+    //.data and zeros .bss now run with the CPU at full speed instead of 8MHz
+    SystemInit();
+
+    miosix::IRQconfigureCache();
+
+    // These are defined in the linker script
+    extern unsigned char _etext asm("_etext");
+    extern unsigned char _data asm("_data");
+    extern unsigned char _edata asm("_edata");
+    extern unsigned char _bss_start asm("_bss_start");
+    extern unsigned char _bss_end asm("_bss_end");
+
+    // Initialize .data section, clear .bss section
+    unsigned char *etext     = &_etext;
+    unsigned char *data      = &_data;
+    unsigned char *edata     = &_edata;
+    unsigned char *bss_start = &_bss_start;
+    unsigned char *bss_end   = &_bss_end;
+    memcpy(data, etext, edata - data);
+    memset(bss_start, 0, bss_end - bss_start);
+
+    // Move on to stage 2
+    _init();
+
+    // If main returns, reboot
+    NVIC_SystemReset();
+    for (;;)
+        ;
+}
+
+/**
+ * Reset handler, called by hardware immediately after reset
+ */
+void Reset_Handler() __attribute__((__interrupt__, noreturn));
+void Reset_Handler()
+{
+    /*
+     * Load into the program stack pointer the heap end address and switch from
+     * the msp to sps.
+     * This is required for booting Miosix, a small portion of the top of the
+     * heap area will be used as stack until the first thread starts. After,
+     * this stack will be abandoned and the process stack will point to the
+     * current thread's stack.
+     */
+    asm volatile(
+        "ldr r0,  =_heap_end          \n\t"
+        "msr psp, r0                  \n\t"
+        "movw r0, #2                  \n\n"  // Set the control register to use
+        "msr control, r0              \n\t"  // the process stack
+        "isb                          \n\t" ::
+            : "r0");
+
+    program_startup();
+}
+
+/**
+ * All unused interrupts call this function.
+ */
+extern "C" void Default_Handler() { unexpectedInterrupt(); }
+
+// System handlers
+void /*__attribute__((weak))*/ Reset_Handler();      // These interrupts are not
+void /*__attribute__((weak))*/ NMI_Handler();        // weak because they are
+void /*__attribute__((weak))*/ HardFault_Handler();  // surely defined by Miosix
+void /*__attribute__((weak))*/ MemManage_Handler();
+void /*__attribute__((weak))*/ BusFault_Handler();
+void /*__attribute__((weak))*/ UsageFault_Handler();
+void /*__attribute__((weak))*/ SVC_Handler();
+void /*__attribute__((weak))*/ DebugMon_Handler();
+void /*__attribute__((weak))*/ PendSV_Handler();
+void __attribute__((weak)) SysTick_Handler();
+
+// Interrupt handlers
+void __attribute__((weak)) WWDG_IRQHandler();
+void __attribute__((weak)) PVD_IRQHandler();
+void __attribute__((weak)) TAMP_STAMP_IRQHandler();
+void __attribute__((weak)) RTC_WKUP_IRQHandler();
+void __attribute__((weak)) FLASH_IRQHandler();
+void __attribute__((weak)) RCC_IRQHandler();
+void __attribute__((weak)) EXTI0_IRQHandler();
+void __attribute__((weak)) EXTI1_IRQHandler();
+void __attribute__((weak)) EXTI2_IRQHandler();
+void __attribute__((weak)) EXTI3_IRQHandler();
+void __attribute__((weak)) EXTI4_IRQHandler();
+void __attribute__((weak)) DMA1_Stream0_IRQHandler();
+void __attribute__((weak)) DMA1_Stream1_IRQHandler();
+void __attribute__((weak)) DMA1_Stream2_IRQHandler();
+void __attribute__((weak)) DMA1_Stream3_IRQHandler();
+void __attribute__((weak)) DMA1_Stream4_IRQHandler();
+void __attribute__((weak)) DMA1_Stream5_IRQHandler();
+void __attribute__((weak)) DMA1_Stream6_IRQHandler();
+void __attribute__((weak)) ADC_IRQHandler();
+void __attribute__((weak)) CAN1_TX_IRQHandler();
+void __attribute__((weak)) CAN1_RX0_IRQHandler();
+void __attribute__((weak)) CAN1_RX1_IRQHandler();
+void __attribute__((weak)) CAN1_SCE_IRQHandler();
+void __attribute__((weak)) EXTI9_5_IRQHandler();
+void __attribute__((weak)) TIM1_BRK_TIM9_IRQHandler();
+void __attribute__((weak)) TIM1_UP_TIM10_IRQHandler();
+void __attribute__((weak)) TIM1_TRG_COM_TIM11_IRQHandler();
+void __attribute__((weak)) TIM1_CC_IRQHandler();
+void __attribute__((weak)) TIM2_IRQHandler();
+void __attribute__((weak)) TIM3_IRQHandler();
+void __attribute__((weak)) TIM4_IRQHandler();
+void __attribute__((weak)) I2C1_EV_IRQHandler();
+void __attribute__((weak)) I2C1_ER_IRQHandler();
+void __attribute__((weak)) I2C2_EV_IRQHandler();
+void __attribute__((weak)) I2C2_ER_IRQHandler();
+void __attribute__((weak)) SPI1_IRQHandler();
+void __attribute__((weak)) SPI2_IRQHandler();
+void __attribute__((weak)) USART1_IRQHandler();
+void __attribute__((weak)) USART2_IRQHandler();
+void __attribute__((weak)) USART3_IRQHandler();
+void __attribute__((weak)) EXTI15_10_IRQHandler();
+void __attribute__((weak)) RTC_Alarm_IRQHandler();
+void __attribute__((weak)) OTG_FS_WKUP_IRQHandler();
+void __attribute__((weak)) TIM8_BRK_TIM12_IRQHandler();
+void __attribute__((weak)) TIM8_UP_TIM13_IRQHandler();
+void __attribute__((weak)) TIM8_TRG_COM_TIM14_IRQHandler();
+void __attribute__((weak)) TIM8_CC_IRQHandler();
+void __attribute__((weak)) DMA1_Stream7_IRQHandler();
+void __attribute__((weak)) FMC_IRQHandler();
+void __attribute__((weak)) SDMMC1_IRQHandler();
+void __attribute__((weak)) TIM5_IRQHandler();
+void __attribute__((weak)) SPI3_IRQHandler();
+void __attribute__((weak)) UART4_IRQHandler();
+void __attribute__((weak)) UART5_IRQHandler();
+void __attribute__((weak)) TIM6_DAC_IRQHandler();
+void __attribute__((weak)) TIM7_IRQHandler();
+void __attribute__((weak)) DMA2_Stream0_IRQHandler();
+void __attribute__((weak)) DMA2_Stream1_IRQHandler();
+void __attribute__((weak)) DMA2_Stream2_IRQHandler();
+void __attribute__((weak)) DMA2_Stream3_IRQHandler();
+void __attribute__((weak)) DMA2_Stream4_IRQHandler();
+void __attribute__((weak)) ETH_IRQHandler();
+void __attribute__((weak)) ETH_WKUP_IRQHandler();
+void __attribute__((weak)) CAN2_TX_IRQHandler();
+void __attribute__((weak)) CAN2_RX0_IRQHandler();
+void __attribute__((weak)) CAN2_RX1_IRQHandler();
+void __attribute__((weak)) CAN2_SCE_IRQHandler();
+void __attribute__((weak)) OTG_FS_IRQHandler();
+void __attribute__((weak)) DMA2_Stream5_IRQHandler();
+void __attribute__((weak)) DMA2_Stream6_IRQHandler();
+void __attribute__((weak)) DMA2_Stream7_IRQHandler();
+void __attribute__((weak)) USART6_IRQHandler();
+void __attribute__((weak)) I2C3_EV_IRQHandler();
+void __attribute__((weak)) I2C3_ER_IRQHandler();
+void __attribute__((weak)) OTG_HS_EP1_OUT_IRQHandler();
+void __attribute__((weak)) OTG_HS_EP1_IN_IRQHandler();
+void __attribute__((weak)) OTG_HS_WKUP_IRQHandler();
+void __attribute__((weak)) OTG_HS_IRQHandler();
+void __attribute__((weak)) DCMI_IRQHandler();
+void __attribute__((weak)) RNG_IRQHandler();
+void __attribute__((weak)) FPU_IRQHandler();
+void __attribute__((weak)) UART7_IRQHandler();
+void __attribute__((weak)) UART8_IRQHandler();
+void __attribute__((weak)) SPI4_IRQHandler();
+void __attribute__((weak)) SPI5_IRQHandler();
+void __attribute__((weak)) SPI6_IRQHandler();
+void __attribute__((weak)) SAI1_IRQHandler();
+void __attribute__((weak)) LTDC_IRQHandler();
+void __attribute__((weak)) LTDC_ER_IRQHandler();
+void __attribute__((weak)) DMA2D_IRQHandler();
+void __attribute__((weak)) SAI2_IRQHandler();
+void __attribute__((weak)) QUADSPI_IRQHandler();
+void __attribute__((weak)) LPTIM1_IRQHandler();
+void __attribute__((weak)) CEC_IRQHandler();
+void __attribute__((weak)) I2C4_EV_IRQHandler();
+void __attribute__((weak)) I2C4_ER_IRQHandler();
+void __attribute__((weak)) SPDIF_RX_IRQHandler();
+
+// Stack top, defined in the linker script
+extern char _main_stack_top asm("_main_stack_top");
+
+// Interrupt vectors, must be placed @ address 0x00000000
+// The extern declaration is required otherwise g++ optimizes it out
+extern void (*const __Vectors[])();
+void (*const __Vectors[])() __attribute__((section(".isr_vector"))) = {
+    reinterpret_cast<void (*)()>(&_main_stack_top), /* Stack pointer*/
+    Reset_Handler,                                  /* Reset Handler */
+    NMI_Handler,                                    /* NMI Handler */
+    HardFault_Handler,                              /* Hard Fault Handler */
+    MemManage_Handler,                              /* MPU Fault Handler */
+    BusFault_Handler,                               /* Bus Fault Handler */
+    UsageFault_Handler,                             /* Usage Fault Handler */
+    0,                                              /* Reserved */
+    0,                                              /* Reserved */
+    0,                                              /* Reserved */
+    0,                                              /* Reserved */
+    SVC_Handler,                                    /* SVCall Handler */
+    DebugMon_Handler,                               /* Debug Monitor Handler */
+    0,                                              /* Reserved */
+    PendSV_Handler,                                 /* PendSV Handler */
+    SysTick_Handler,                                /* SysTick Handler */
+
+    /* External Interrupts */
+    WWDG_IRQHandler,
+    PVD_IRQHandler,
+    TAMP_STAMP_IRQHandler,
+    RTC_WKUP_IRQHandler,
+    FLASH_IRQHandler,
+    RCC_IRQHandler,
+    EXTI0_IRQHandler,
+    EXTI1_IRQHandler,
+    EXTI2_IRQHandler,
+    EXTI3_IRQHandler,
+    EXTI4_IRQHandler,
+    DMA1_Stream0_IRQHandler,
+    DMA1_Stream1_IRQHandler,
+    DMA1_Stream2_IRQHandler,
+    DMA1_Stream3_IRQHandler,
+    DMA1_Stream4_IRQHandler,
+    DMA1_Stream5_IRQHandler,
+    DMA1_Stream6_IRQHandler,
+    ADC_IRQHandler,
+    CAN1_TX_IRQHandler,
+    CAN1_RX0_IRQHandler,
+    CAN1_RX1_IRQHandler,
+    CAN1_SCE_IRQHandler,
+    EXTI9_5_IRQHandler,
+    TIM1_BRK_TIM9_IRQHandler,
+    TIM1_UP_TIM10_IRQHandler,
+    TIM1_TRG_COM_TIM11_IRQHandler,
+    TIM1_CC_IRQHandler,
+    TIM2_IRQHandler,
+    TIM3_IRQHandler,
+    TIM4_IRQHandler,
+    I2C1_EV_IRQHandler,
+    I2C1_ER_IRQHandler,
+    I2C2_EV_IRQHandler,
+    I2C2_ER_IRQHandler,
+    SPI1_IRQHandler,
+    SPI2_IRQHandler,
+    USART1_IRQHandler,
+    USART2_IRQHandler,
+    USART3_IRQHandler,
+    EXTI15_10_IRQHandler,
+    RTC_Alarm_IRQHandler,
+    OTG_FS_WKUP_IRQHandler,
+    TIM8_BRK_TIM12_IRQHandler,
+    TIM8_UP_TIM13_IRQHandler,
+    TIM8_TRG_COM_TIM14_IRQHandler,
+    TIM8_CC_IRQHandler,
+    DMA1_Stream7_IRQHandler,
+    FMC_IRQHandler,
+    SDMMC1_IRQHandler,
+    TIM5_IRQHandler,
+    SPI3_IRQHandler,
+    UART4_IRQHandler,
+    UART5_IRQHandler,
+    TIM6_DAC_IRQHandler,
+    TIM7_IRQHandler,
+    DMA2_Stream0_IRQHandler,
+    DMA2_Stream1_IRQHandler,
+    DMA2_Stream2_IRQHandler,
+    DMA2_Stream3_IRQHandler,
+    DMA2_Stream4_IRQHandler,
+    ETH_IRQHandler,
+    ETH_WKUP_IRQHandler,
+    CAN2_TX_IRQHandler,
+    CAN2_RX0_IRQHandler,
+    CAN2_RX1_IRQHandler,
+    CAN2_SCE_IRQHandler,
+    OTG_FS_IRQHandler,
+    DMA2_Stream5_IRQHandler,
+    DMA2_Stream6_IRQHandler,
+    DMA2_Stream7_IRQHandler,
+    USART6_IRQHandler,
+    I2C3_EV_IRQHandler,
+    I2C3_ER_IRQHandler,
+    OTG_HS_EP1_OUT_IRQHandler,
+    OTG_HS_EP1_IN_IRQHandler,
+    OTG_HS_WKUP_IRQHandler,
+    OTG_HS_IRQHandler,
+    DCMI_IRQHandler,
+    0,
+    RNG_IRQHandler,
+    FPU_IRQHandler,
+    UART7_IRQHandler,
+    UART8_IRQHandler,
+    SPI4_IRQHandler,
+    SPI5_IRQHandler,
+    SPI6_IRQHandler,
+    SAI1_IRQHandler,
+    LTDC_IRQHandler,
+    LTDC_ER_IRQHandler,
+    DMA2D_IRQHandler,
+    SAI2_IRQHandler,
+    QUADSPI_IRQHandler,
+    LPTIM1_IRQHandler,
+    CEC_IRQHandler,
+    I2C4_EV_IRQHandler,
+    I2C4_ER_IRQHandler,
+    SPDIF_RX_IRQHandler,
+};
+
+#pragma weak SysTick_IRQHandler            = Default_Handler
+#pragma weak WWDG_IRQHandler               = Default_Handler
+#pragma weak PVD_IRQHandler                = Default_Handler
+#pragma weak TAMP_STAMP_IRQHandler         = Default_Handler
+#pragma weak RTC_WKUP_IRQHandler           = Default_Handler
+#pragma weak FLASH_IRQHandler              = Default_Handler
+#pragma weak RCC_IRQHandler                = Default_Handler
+#pragma weak EXTI0_IRQHandler              = Default_Handler
+#pragma weak EXTI1_IRQHandler              = Default_Handler
+#pragma weak EXTI2_IRQHandler              = Default_Handler
+#pragma weak EXTI3_IRQHandler              = Default_Handler
+#pragma weak EXTI4_IRQHandler              = Default_Handler
+#pragma weak DMA1_Stream0_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream1_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream2_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream3_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream4_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream5_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream6_IRQHandler       = Default_Handler
+#pragma weak ADC_IRQHandler                = Default_Handler
+#pragma weak CAN1_TX_IRQHandler            = Default_Handler
+#pragma weak CAN1_RX0_IRQHandler           = Default_Handler
+#pragma weak CAN1_RX1_IRQHandler           = Default_Handler
+#pragma weak CAN1_SCE_IRQHandler           = Default_Handler
+#pragma weak EXTI9_5_IRQHandler            = Default_Handler
+#pragma weak TIM1_BRK_TIM9_IRQHandler      = Default_Handler
+#pragma weak TIM1_UP_TIM10_IRQHandler      = Default_Handler
+#pragma weak TIM1_TRG_COM_TIM11_IRQHandler = Default_Handler
+#pragma weak TIM1_CC_IRQHandler            = Default_Handler
+#pragma weak TIM2_IRQHandler               = Default_Handler
+#pragma weak TIM3_IRQHandler               = Default_Handler
+#pragma weak TIM4_IRQHandler               = Default_Handler
+#pragma weak I2C1_EV_IRQHandler            = Default_Handler
+#pragma weak I2C1_ER_IRQHandler            = Default_Handler
+#pragma weak I2C2_EV_IRQHandler            = Default_Handler
+#pragma weak I2C2_ER_IRQHandler            = Default_Handler
+#pragma weak SPI1_IRQHandler               = Default_Handler
+#pragma weak SPI2_IRQHandler               = Default_Handler
+// #pragma weak USART1_IRQHandler             = Default_Handler
+// #pragma weak USART2_IRQHandler             = Default_Handler
+// #pragma weak USART3_IRQHandler             = Default_Handler
+#pragma weak EXTI15_10_IRQHandler          = Default_Handler
+#pragma weak RTC_Alarm_IRQHandler          = Default_Handler
+#pragma weak OTG_FS_WKUP_IRQHandler        = Default_Handler
+#pragma weak TIM8_BRK_TIM12_IRQHandler     = Default_Handler
+#pragma weak TIM8_UP_TIM13_IRQHandler      = Default_Handler
+#pragma weak TIM8_TRG_COM_TIM14_IRQHandler = Default_Handler
+#pragma weak TIM8_CC_IRQHandler            = Default_Handler
+#pragma weak DMA1_Stream7_IRQHandler       = Default_Handler
+#pragma weak FMC_IRQHandler                = Default_Handler
+#pragma weak SDMMC1_IRQHandler             = Default_Handler
+#pragma weak TIM5_IRQHandler               = Default_Handler
+#pragma weak SPI3_IRQHandler               = Default_Handler
+// #pragma weak UART4_IRQHandler              = Default_Handler
+// #pragma weak UART5_IRQHandler              = Default_Handler
+#pragma weak TIM6_DAC_IRQHandler     = Default_Handler
+#pragma weak TIM7_IRQHandler         = Default_Handler
+#pragma weak DMA2_Stream0_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream1_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream2_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream3_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream4_IRQHandler = Default_Handler
+#pragma weak ETH_IRQHandler          = Default_Handler
+#pragma weak ETH_WKUP_IRQHandler     = Default_Handler
+#pragma weak CAN2_TX_IRQHandler      = Default_Handler
+#pragma weak CAN2_RX0_IRQHandler     = Default_Handler
+#pragma weak CAN2_RX1_IRQHandler     = Default_Handler
+#pragma weak CAN2_SCE_IRQHandler     = Default_Handler
+#pragma weak OTG_FS_IRQHandler       = Default_Handler
+#pragma weak DMA2_Stream5_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream6_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream7_IRQHandler = Default_Handler
+// #pragma weak USART6_IRQHandler             = Default_Handler
+#pragma weak I2C3_EV_IRQHandler        = Default_Handler
+#pragma weak I2C3_ER_IRQHandler        = Default_Handler
+#pragma weak OTG_HS_EP1_OUT_IRQHandler = Default_Handler
+#pragma weak OTG_HS_EP1_IN_IRQHandler  = Default_Handler
+#pragma weak OTG_HS_WKUP_IRQHandler    = Default_Handler
+#pragma weak OTG_HS_IRQHandler         = Default_Handler
+#pragma weak DCMI_IRQHandler           = Default_Handler
+#pragma weak RNG_IRQHandler            = Default_Handler
+#pragma weak FPU_IRQHandler            = Default_Handler
+// #pragma weak UART7_IRQHandler              = Default_Handler
+// #pragma weak UART8_IRQHandler              = Default_Handler
+#pragma weak SPI4_IRQHandler     = Default_Handler
+#pragma weak SPI5_IRQHandler     = Default_Handler
+#pragma weak SPI6_IRQHandler     = Default_Handler
+#pragma weak SAI1_IRQHandler     = Default_Handler
+#pragma weak LTDC_IRQHandler     = Default_Handler
+#pragma weak LTDC_ER_IRQHandler  = Default_Handler
+#pragma weak DMA2D_IRQHandler    = Default_Handler
+#pragma weak SAI2_IRQHandler     = Default_Handler
+#pragma weak QUADSPI_IRQHandler  = Default_Handler
+#pragma weak LPTIM1_IRQHandler   = Default_Handler
+#pragma weak CEC_IRQHandler      = Default_Handler
+#pragma weak I2C4_EV_IRQHandler  = Default_Handler
+#pragma weak I2C4_ER_IRQHandler  = Default_Handler
+#pragma weak SPDIF_RX_IRQHandler = Default_Handler
diff --git a/src/bsps/stm32f756zg_nucleo/interfaces-impl/arch_registers_impl.h b/src/bsps/stm32f756zg_nucleo/interfaces-impl/arch_registers_impl.h
new file mode 100644
index 000000000..ab9a54543
--- /dev/null
+++ b/src/bsps/stm32f756zg_nucleo/interfaces-impl/arch_registers_impl.h
@@ -0,0 +1,39 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Lorenzo Cucchi
+ *
+ * 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.
+ */
+
+#ifndef ARCH_REGISTERS_IMPL_H
+#define ARCH_REGISTERS_IMPL_H
+
+// stm32f7xx.h defines a few macros like __ICACHE_PRESENT, __DCACHE_PRESENT and
+// includes core_cm7.h. Do not include core_cm7.h before.
+#define STM32F756xx
+#include "CMSIS/Device/ST/STM32F7xx/Include/stm32f7xx.h"
+
+#if (__ICACHE_PRESENT != 1) || (__DCACHE_PRESENT != 1)
+#error "Wrong include order"
+#endif
+
+#include "CMSIS/Device/ST/STM32F7xx/Include/system_stm32f7xx.h"
+
+#define RCC_SYNC() __DSB()  // TODO: can this dsb be removed?
+
+#endif  // ARCH_REGISTERS_IMPL_H
diff --git a/src/bsps/stm32f756zg_nucleo/interfaces-impl/bsp.cpp b/src/bsps/stm32f756zg_nucleo/interfaces-impl/bsp.cpp
new file mode 100644
index 000000000..2cc3d8230
--- /dev/null
+++ b/src/bsps/stm32f756zg_nucleo/interfaces-impl/bsp.cpp
@@ -0,0 +1,124 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Lorenzo Cucchi
+ *
+ * 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.
+ */
+
+/***********************************************************************
+ * bsp.cpp Part of the Miosix Embedded OS.
+ * Board support package, this file initializes hardware.
+ ************************************************************************/
+
+#include "interfaces/bsp.h"
+
+#include <inttypes.h>
+#include <sys/ioctl.h>
+
+#include <cstdlib>
+
+#include "board_settings.h"
+#include "config/miosix_settings.h"
+#include "drivers/sd_stm32f2_f4_f7.h"
+#include "drivers/serial.h"
+#include "filesystem/console/console_device.h"
+#include "filesystem/file_access.h"
+#include "interfaces/arch_registers.h"
+#include "interfaces/delays.h"
+#include "interfaces/portability.h"
+#include "kernel/kernel.h"
+#include "kernel/logging.h"
+#include "kernel/sync.h"
+
+namespace miosix
+{
+
+//
+// Initialization
+//
+
+void IRQbspInit()
+{
+    // Enable all gpios
+    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN | RCC_AHB1ENR_GPIOBEN |
+                    RCC_AHB1ENR_GPIOCEN | RCC_AHB1ENR_GPIODEN |
+                    RCC_AHB1ENR_GPIOEEN | RCC_AHB1ENR_GPIOFEN |
+                    RCC_AHB1ENR_GPIOGEN | RCC_AHB1ENR_GPIOHEN;
+    RCC_SYNC();
+    GPIOA->OSPEEDR = 0xaaaaaaaa;  // Default to 50MHz speed for all GPIOS
+    GPIOB->OSPEEDR = 0xaaaaaaaa;
+    GPIOC->OSPEEDR = 0xaaaaaaaa;
+    GPIOD->OSPEEDR = 0xaaaaaaaa;
+    GPIOE->OSPEEDR = 0xaaaaaaaa;
+    GPIOF->OSPEEDR = 0xaaaaaaaa;
+    GPIOG->OSPEEDR = 0xaaaaaaaa;
+    GPIOH->OSPEEDR = 0xaaaaaaaa;
+
+    userLed1::mode(Mode::OUTPUT);
+    userLed2::mode(Mode::OUTPUT);
+    userLed3::mode(Mode::OUTPUT);
+    userBtn::mode(Mode::INPUT);
+
+    ledOn();
+    delayMs(100);
+    ledOff();
+    auto tx = Gpio<GPIOD_BASE, 8>::getPin();
+    tx.alternateFunction(7);
+    auto rx = Gpio<GPIOD_BASE, 9>::getPin();
+    rx.alternateFunction(7);
+    DefaultConsole::instance().IRQset(intrusive_ref_ptr<Device>(
+        new STM32Serial(3, defaultSerialSpeed, tx, rx)));
+}
+
+void bspInit2()
+{
+#ifdef WITH_FILESYSTEM
+    basicFilesystemSetup(SDIODriver::instance());
+#endif  // WITH_FILESYSTEM
+}
+
+//
+// Shutdown and reboot
+//
+
+void shutdown()
+{
+    ioctl(STDOUT_FILENO, IOCTL_SYNC, 0);
+
+#ifdef WITH_FILESYSTEM
+    FilesystemManager::instance().umountAll();
+#endif  // WITH_FILESYSTEM
+
+    disableInterrupts();
+    for (;;)
+        ;
+}
+
+void reboot()
+{
+    ioctl(STDOUT_FILENO, IOCTL_SYNC, 0);
+
+#ifdef WITH_FILESYSTEM
+    FilesystemManager::instance().umountAll();
+#endif  // WITH_FILESYSTEM
+
+    disableInterrupts();
+    miosix_private::IRQsystemReboot();
+}
+
+}  // namespace miosix
diff --git a/src/bsps/stm32f756zg_nucleo/interfaces-impl/bsp_impl.h b/src/bsps/stm32f756zg_nucleo/interfaces-impl/bsp_impl.h
new file mode 100644
index 000000000..c4a613fd1
--- /dev/null
+++ b/src/bsps/stm32f756zg_nucleo/interfaces-impl/bsp_impl.h
@@ -0,0 +1,95 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Lorenzo Cucchi
+ *
+ * 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.
+ */
+
+/***********************************************************************
+ * bsp_impl.h Part of the Miosix Embedded OS.
+ * Board support package, this file initializes hardware.
+ ************************************************************************/
+
+#ifndef BSP_IMPL_H
+#define BSP_IMPL_H
+
+#include "config/miosix_settings.h"
+#include "interfaces/gpio.h"
+
+namespace miosix
+{
+
+/**
+\addtogroup Hardware
+\{
+*/
+
+/**
+ * \internal
+ * Board pin definition
+ */
+typedef Gpio<GPIOB_BASE, 0> userLed1;
+typedef Gpio<GPIOB_BASE, 7> userLed2;
+typedef Gpio<GPIOB_BASE, 14> userLed3;
+typedef Gpio<GPIOC_BASE, 13> userBtn;
+
+inline void ledOn()
+{
+    userLed1::high();
+    userLed2::high();
+    userLed3::high();
+}
+
+inline void ledOff()
+{
+    userLed1::low();
+    userLed2::low();
+    userLed3::low();
+}
+
+inline void led1On() { userLed1::high(); }
+
+inline void led1Off() { userLed1::low(); }
+
+inline void led2On() { userLed2::high(); }
+
+inline void led2Off() { userLed2::low(); }
+
+inline void led3On() { userLed3::high(); }
+
+inline void led3Off() { userLed3::low(); }
+
+/**
+ * Polls the SD card sense GPIO.
+ *
+ * This board has no SD card whatsoever, but a card can be connected to the
+ * following GPIOs:
+ * TODO: never tested
+ *
+ * \return true. As there's no SD card sense switch, let's pretend that
+ * the card is present.
+ */
+inline bool sdCardSense() { return true; }
+
+/**
+\}
+*/
+
+}  // namespace miosix
+
+#endif  // BSP_IMPL_H
diff --git a/src/bsps/stm32f756zg_nucleo/stm32_1m+256k_rom.ld b/src/bsps/stm32f756zg_nucleo/stm32_1m+256k_rom.ld
new file mode 100644
index 000000000..1f81bdcc8
--- /dev/null
+++ b/src/bsps/stm32f756zg_nucleo/stm32_1m+256k_rom.ld
@@ -0,0 +1,178 @@
+/*
+ * C++ enabled linker script for stm32f746zg (1M FLASH, 256K RAM)
+ * Developed by TFT: Terraneo Federico Technologies
+ * Optimized for use with the Miosix kernel
+ */
+
+/*
+ * This linker script puts:
+ * - read only data and code (.text, .rodata, .eh_*) in FLASH
+ * - the 512Byte main (IRQ) stack, .data and .bss in the DTCM 64KB RAM
+ * - .data, .bss, stacks and heap in the internal RAM.
+ */
+
+/*
+ * The main stack is used for interrupt handling by the kernel.
+ *
+ * *** Readme ***
+ * This linker script places the main stack (used by the kernel for interrupts)
+ * at the bottom of the ram, instead of the top. This is done for two reasons:
+ *
+ * - as an optimization for microcontrollers with little ram memory. In fact
+ *   the implementation of malloc from newlib requests memory to the OS in 4KB
+ *   block (except the first block that can be smaller). This is probably done
+ *   for compatibility with OSes with an MMU and paged memory. To see why this
+ *   is bad, consider a microcontroller with 8KB of ram: when malloc finishes
+ *   up the first 4KB it will call _sbrk_r asking for a 4KB block, but this will
+ *   fail because the top part of the ram is used by the main stack. As a
+ *   result, the top part of the memory will not be used by malloc, even if
+ *   available (and it is nearly *half* the ram on an 8KB mcu). By placing the
+ *   main stack at the bottom of the ram, the upper 4KB block will be entirely
+ *   free and available as heap space.
+ *
+ * - In case of main stack overflow the cpu will fault because access to memory
+ *   before the beginning of the ram faults. Instead with the default stack
+ *   placement the main stack will silently collide with the heap.
+ * Note: if increasing the main stack size also increase the ORIGIN value in
+ * the MEMORY definitions below accordingly.
+ */
+
+_main_stack_size = 512;                             /* main stack = 512Bytes */
+_main_stack_top  = 0x20000000 + _main_stack_size;
+ASSERT(_main_stack_size   % 8 == 0, "MAIN stack size error");
+
+/* Mapping the heap to the end of SRAM2 */
+_heap_end = 0x20000000 + 320K;                       /* end of available ram */
+
+/* Identify the Entry Point  */
+ENTRY(_Z13Reset_Handlerv)
+
+/*
+ * Specify the memory areas
+ *
+ * NOTE: starting at 0x20000000 there's 64KB of DTCM (Data Tightly Coupled
+ * Memory). Technically, we could use this as normal RAM as there's a way for
+ * the DMA to access it, but the datasheet is unclear about performance
+ * penalties for doing so. To avoid nonuniform DMA memory access latencies,
+ * we leave this 64KB DTCM unused except for the first 512Bytes which are for
+ * the interrupt stack. This leaves us with 384KB of RAM
+ */
+MEMORY
+{
+    sram(wx)  : ORIGIN = 0x20010000, LENGTH = 256K
+    dtcm(wx)  : ORIGIN = 0x20000000, LENGTH = 64K     /* Used for main stack */
+    flash(rx) : ORIGIN = 0x08000000, LENGTH = 1M
+}
+
+/* Now define the output sections  */
+SECTIONS
+{
+    . = 0;
+    
+    /* .text section: code goes to flash */
+    .text :
+    {
+        /* Startup code must go at address 0 */
+        KEEP(*(.isr_vector))
+        
+        *(.text)
+        *(.text.*)
+        *(.gnu.linkonce.t.*)
+        /* These sections for thumb interwork? */
+        *(.glue_7)
+        *(.glue_7t)
+        /* These sections for C++? */
+        *(.gcc_except_table)
+        *(.gcc_except_table.*)
+        *(.ARM.extab*)
+        *(.gnu.linkonce.armextab.*)
+
+        . = ALIGN(4);
+        /* .rodata: constant data */
+        *(.rodata)
+        *(.rodata.*)
+        *(.gnu.linkonce.r.*)
+
+        /* C++ Static constructors/destructors (eabi) */
+        . = ALIGN(4);
+        KEEP(*(.init))
+        
+        . = ALIGN(4);
+        __miosix_init_array_start = .;
+        KEEP (*(SORT(.miosix_init_array.*)))
+        KEEP (*(.miosix_init_array))
+        __miosix_init_array_end = .;
+
+        . = ALIGN(4);
+        __preinit_array_start = .;
+        KEEP (*(.preinit_array))
+        __preinit_array_end = .;
+
+        . = ALIGN(4);
+        __init_array_start = .;
+        KEEP (*(SORT(.init_array.*)))
+        KEEP (*(.init_array))
+        __init_array_end = .;
+
+        . = ALIGN(4);
+        KEEP(*(.fini))
+
+        . = ALIGN(4);
+        __fini_array_start = .;
+        KEEP (*(.fini_array))
+        KEEP (*(SORT(.fini_array.*)))
+        __fini_array_end = .;
+
+        /* C++ Static constructors/destructors (elf)  */
+        . = ALIGN(4);
+        _ctor_start = .;
+        KEEP (*crtbegin.o(.ctors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .ctors))
+        KEEP (*(SORT(.ctors.*)))
+        KEEP (*crtend.o(.ctors))
+       _ctor_end = .;
+
+        . = ALIGN(4);
+        KEEP (*crtbegin.o(.dtors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .dtors))
+        KEEP (*(SORT(.dtors.*)))
+        KEEP (*crtend.o(.dtors))
+    } > flash
+
+    /* .ARM.exidx is sorted, so has to go in its own output section.  */
+    __exidx_start = .;
+    .ARM.exidx :
+    {
+        *(.ARM.exidx* .gnu.linkonce.armexidx.*)
+    } > flash
+    __exidx_end = .;
+
+	/*
+     * .data section: global variables go to sram, but also store a copy to
+     * flash to initialize them
+     */
+    .data : ALIGN(8)
+    {
+        _data = .;
+        *(.data)
+        *(.data.*)
+        *(.gnu.linkonce.d.*)
+        . = ALIGN(8);
+        _edata = .;
+    } > sram AT > flash
+    _etext = LOADADDR(.data);
+
+    /* .bss section: uninitialized global variables go to sram */
+    _bss_start = .;
+    .bss :
+    {
+        *(.bss)
+        *(.bss.*)
+        *(.gnu.linkonce.b.*)
+        . = ALIGN(8);
+    } > sram
+    _bss_end = .;
+
+    _end = .;
+    PROVIDE(end = .);
+}
diff --git a/src/bsps/stm32f767zi_automated_antennas/config/board_options.cmake b/src/bsps/stm32f767zi_automated_antennas/config/board_options.cmake
new file mode 100644
index 000000000..276c30af5
--- /dev/null
+++ b/src/bsps/stm32f767zi_automated_antennas/config/board_options.cmake
@@ -0,0 +1,106 @@
+# Copyright (C) 2023 by Skyward
+#
+# This program is free software; you can redistribute it and/or 
+# it under the terms of the GNU General Public License as published 
+# the Free Software Foundation; either version 2 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.
+#
+# As a special exception, if other files instantiate templates or use
+# macros or inline functions from this file, or you compile this file
+# and link it with other works to produce a work based on this file,
+# this file does not by itself cause the resulting work to be covered
+# by the GNU General Public License. However the source code for this
+# file must still be made available in accordance with the GNU 
+# Public License. This exception does not invalidate any other 
+# why a work based on this file might be covered by the GNU General
+# Public License.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, see <http://www.gnu.org/licenses/>
+
+set(BOARD_NAME stm32f767zi_automated_antennas)
+set(ARCH_NAME cortexM7_stm32f7)
+
+# Base directories with header files for this board
+set(ARCH_PATH ${KPATH}/arch/${ARCH_NAME}/common)
+set(BOARD_PATH ${BOARDCORE_PATH}/src/bsps/${BOARD_NAME})
+set(BOARD_CONFIG_PATH ${BOARDCORE_PATH}/src/bsps/${BOARD_NAME}/config)
+
+# Specify where to find the board specific config/miosix_settings.h
+set(BOARD_MIOSIX_SETTINGS_PATH ${BOARD_PATH})
+
+# Specify where to find the board specific config/mxgui_settings.h
+set(BOARD_MXGUI_SETTINGS_PATH ${BOARD_PATH})
+
+# Optimization flags:
+# -O0 do no optimization, the default if no optimization level is specified
+# -O or -O1 optimize minimally
+# -O2 optimize more
+# -O3 optimize even more
+# -Ofast optimize very aggressively to the point of breaking the standard
+# -Og Optimize debugging experience, enables optimizations that do not
+# interfere with debugging
+# -Os Optimize for size with -O2 optimizations that do not increase code size
+set(OPT_OPTIMIZATION -O2)
+
+# Boot file and linker script
+set(BOOT_FILE ${BOARD_PATH}/core/stage_1_boot.cpp)
+# set(LINKER_SCRIPT ${BOARD_PATH}/stm32_2m+384k_ram.ld)
+set(LINKER_SCRIPT ${BOARD_PATH}/stm32_2m+16m_xram.ld)
+
+# Enables the initialization of the external 16MB SDRAM memory
+set(XRAM -D__ENABLE_XRAM)
+
+# Select clock frequency (HSE_VALUE is the xtal on board, fixed)
+set(CLOCK_FREQ -DHSE_VALUE=25000000 -DSYSCLK_FREQ_216MHz=216000000)
+
+# C++ Exception/rtti support disable flags.
+# To save code size if not using C++ exceptions (nor some STL code which
+# implicitly uses it) uncomment this option.
+# -D__NO_EXCEPTIONS is used by Miosix to know if exceptions are used.
+# set(OPT_EXCEPT -fno-exceptions -fno-rtti -D__NO_EXCEPTIONS)
+
+# Specify a custom flash command
+# This is the program that is invoked when the flash flag (-f or --flash) is
+# used with the Miosix Build System. Use $binary or $hex as placeolders, they
+# will be replaced by the build systems with the binary or hex file repectively.
+# If a command is not specified, the build system will use st-flash if found
+# set(PROGRAM_CMDLINE "here your custom flash command")
+
+# Basic flags
+set(FLAGS_BASE -mcpu=cortex-m7 -mthumb -mfloat-abi=hard -mfpu=fpv5-d16)
+
+# Flags for ASM and linker
+set(AFLAGS_BASE ${FLAGS_BASE})
+set(LFLAGS_BASE ${FLAGS_BASE} -Wl,--gc-sections,-Map,main.map -Wl,-T${LINKER_SCRIPT} ${OPT_EXCEPT} ${OPT_OPTIMIZATION} -nostdlib)
+
+# Flags for C/C++
+string(TOUPPER ${BOARD_NAME} BOARD_UPPER)
+set(CFLAGS_BASE
+    -D_BOARD_${BOARD_UPPER} -D_MIOSIX_BOARDNAME=\"${BOARD_NAME}\"
+    -D_DEFAULT_SOURCE=1 -ffunction-sections -Wall -Werror=return-type -g
+    -D_ARCH_CORTEXM7_STM32F7
+    ${CLOCK_FREQ} ${XRAM} ${SRAM_BOOT} ${FLAGS_BASE} ${OPT_OPTIMIZATION} -c
+)
+set(CXXFLAGS_BASE ${CFLAGS_BASE} ${OPT_EXCEPT})
+
+# Select architecture specific files
+set(ARCH_SRC
+    ${ARCH_PATH}/interfaces-impl/delays.cpp
+    ${ARCH_PATH}/interfaces-impl/gpio_impl.cpp
+    ${ARCH_PATH}/interfaces-impl/portability.cpp
+    ${BOARD_PATH}/interfaces-impl/bsp.cpp
+    ${KPATH}/arch/common/CMSIS/Device/ST/STM32F7xx/Source/Templates/system_stm32f7xx.c
+    ${KPATH}/arch/common/core/cache_cortexMx.cpp
+    ${KPATH}/arch/common/core/interrupts_cortexMx.cpp
+    ${KPATH}/arch/common/core/mpu_cortexMx.cpp
+    ${KPATH}/arch/common/core/stm32f2_f4_l4_f7_h7_os_timer.cpp
+    ${KPATH}/arch/common/drivers/sd_stm32f2_f4_f7.cpp
+    ${KPATH}/arch/common/drivers/serial_stm32.cpp
+    ${KPATH}/arch/common/drivers/stm32_hardware_rng.cpp
+)
diff --git a/src/bsps/stm32f767zi_automated_antennas/config/board_settings.h b/src/bsps/stm32f767zi_automated_antennas/config/board_settings.h
new file mode 100644
index 000000000..1fc053248
--- /dev/null
+++ b/src/bsps/stm32f767zi_automated_antennas/config/board_settings.h
@@ -0,0 +1,62 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Emilio Corigliano
+ *
+ * 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.
+ */
+
+#pragma once
+
+/**
+ * \internal
+ * Versioning for board_settings.h for out of git tree projects
+ */
+#define BOARD_SETTINGS_VERSION 300
+
+namespace miosix
+{
+
+/**
+ * \addtogroup Settings
+ * \{
+ */
+
+/// Size of stack for main().
+/// The C standard library is stack-heavy (iprintf requires 1KB)
+const unsigned int MAIN_STACK_SIZE = 16 * 1024;
+
+/// Serial port
+const unsigned int defaultSerial      = 1;
+const unsigned int defaultSerialSpeed = 115200;
+#define SERIAL_1_DMA
+// #define SERIAL_2_DMA
+// #define SERIAL_3_DMA
+
+// SD card driver
+static const unsigned char sdVoltage = 33;  // Board powered @ 3.3V
+#define SD_ONE_BIT_DATABUS
+#define SD_SDMMC 1  // Select either SDMMC1 or SDMMC2
+
+/// Analog supply voltage for ADC, DAC, Reset blocks, RCs and PLL
+#define V_DDA_VOLTAGE 3.3f
+
+/**
+ * \}
+ */
+
+}  // namespace miosix
diff --git a/src/bsps/stm32f767zi_automated_antennas/config/miosix_settings.h b/src/bsps/stm32f767zi_automated_antennas/config/miosix_settings.h
new file mode 100644
index 000000000..5a19de638
--- /dev/null
+++ b/src/bsps/stm32f767zi_automated_antennas/config/miosix_settings.h
@@ -0,0 +1,240 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Emilio Corigliano
+ *
+ * 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.
+ */
+
+#pragma once
+
+// Before you can compile the kernel you have to configure it by editing this
+// file. After that, comment out this line to disable the reminder error.
+// The PARSING_FROM_IDE is because Netbeans gets confused by this, it is never
+// defined when compiling the code.
+#ifndef PARSING_FROM_IDE
+//#error This error is a reminder that you have not edited miosix_settings.h
+// yet.
+#endif  // PARSING_FROM_IDE
+
+/**
+ * \file miosix_settings.h
+ * NOTE: this file contains ONLY configuration options that are not dependent
+ * on architecture specific details. The other options are in the following
+ * files which are included here:
+ * miosix/arch/architecture name/common/arch_settings.h
+ * miosix/config/arch/architecture name/board name/board_settings.h
+ */
+#include "arch_settings.h"
+#include "board_settings.h"
+#include "util/version.h"
+
+/**
+ * \internal
+ * Versioning for miosix_settings.h for out of git tree projects
+ */
+#define MIOSIX_SETTINGS_VERSION 300
+
+namespace miosix
+{
+
+/**
+ * \addtogroup Settings
+ * \{
+ */
+
+//
+// Scheduler options
+//
+
+/// \def SCHED_TYPE_PRIORITY
+/// If uncommented selects the priority scheduler
+/// \def SCHED_TYPE_CONTROL_BASED
+/// If uncommented selects the control based scheduler
+/// \def SCHED_TYPE_EDF
+/// If uncommented selects the EDF scheduler
+// Uncomment only *one* of those
+
+#define SCHED_TYPE_PRIORITY
+//#define SCHED_TYPE_CONTROL_BASED
+//#define SCHED_TYPE_EDF
+
+/// \def WITH_CPU_TIME_COUNTER
+/// Allows to enable/disable CPUTimeCounter to save code size and remove its
+/// overhead from the scheduling process. By default it is not defined
+/// (CPUTimeCounter is disabled).
+//#define WITH_CPU_TIME_COUNTER
+
+//
+// Filesystem options
+//
+
+/// \def WITH_FILESYSTEM
+/// Allows to enable/disable filesystem support to save code size
+/// By default it is defined (filesystem support is enabled)
+#define WITH_FILESYSTEM
+
+/// \def WITH_DEVFS
+/// Allows to enable/disable DevFs support to save code size
+/// By default it is defined (DevFs is enabled)
+#define WITH_DEVFS
+
+/// \def SYNC_AFTER_WRITE
+/// Increases filesystem write robustness. After each write operation the
+/// filesystem is synced so that a power failure happens data is not lost
+/// (unless power failure happens exactly between the write and the sync)
+/// Unfortunately write latency and throughput becomes twice as worse
+/// By default it is defined (slow but safe)
+#define SYNC_AFTER_WRITE
+
+/// Maximum number of open files. Trying to open more will fail.
+/// Cannot be lower than 3, as the first three are stdin, stdout, stderr
+const unsigned char MAX_OPEN_FILES = 8;
+
+/// \def WITH_PROCESSES
+/// If uncommented enables support for processes as well as threads.
+/// This enables the dynamic loader to load elf programs, the extended system
+/// call service and, if the hardware supports it, the MPU to provide memory
+/// isolation of processes
+//#define WITH_PROCESSES
+
+#if defined(WITH_PROCESSES) && defined(__NO_EXCEPTIONS)
+#error Processes require C++ exception support
+#endif  // defined(WITH_PROCESSES) && defined(__NO_EXCEPTIONS)
+
+#if defined(WITH_PROCESSES) && !defined(WITH_FILESYSTEM)
+#error Processes require filesystem support
+#endif  // defined(WITH_PROCESSES) && !defined(WITH_FILESYSTEM)
+
+#if defined(WITH_PROCESSES) && !defined(WITH_DEVFS)
+#error Processes require devfs support
+#endif  // defined(WITH_PROCESSES) && !defined(WITH_DEVFS)
+
+//
+// C/C++ standard library I/O (stdin, stdout and stderr related)
+//
+
+/// \def WITH_BOOTLOG
+/// Uncomment to print bootlogs on stdout.
+/// By default it is defined (bootlogs are printed)
+#define WITH_BOOTLOG
+
+/// \def WITH_ERRLOG
+/// Uncomment for debug information on stdout.
+/// By default it is defined (error information is printed)
+#define WITH_ERRLOG
+
+//
+// Kernel related options (stack sizes, priorities)
+//
+
+/// \def WITH_DEEP_SLEEP
+/// Adds interfaces and required variables to support deep sleep state switch
+/// automatically when peripherals are not required
+//#define WITH_DEEP_SLEEP
+
+/**
+ * \def JTAG_DISABLE_SLEEP
+ * JTAG debuggers lose communication with the device if it enters sleep
+ * mode, so to use debugging it is necessary to disable sleep in the idle
+ * thread. By default it is not defined (idle thread calls sleep).
+ */
+//#define JTAG_DISABLE_SLEEP
+
+#if defined(WITH_DEEP_SLEEP) && defined(JTAG_DISABLE_SLEEP)
+#error Deep sleep cannot work together with jtag
+#endif  // defined(WITH_PROCESSES) && !defined(WITH_DEVFS)
+
+/// Minimum stack size (MUST be divisible by 4)
+const unsigned int STACK_MIN = 256;
+
+/// \internal Size of idle thread stack.
+/// Should be >=STACK_MIN (MUST be divisible by 4)
+const unsigned int STACK_IDLE = 256;
+
+/// Default stack size for pthread_create.
+/// The chosen value is enough to call C standard library functions
+/// such as printf/fopen which are stack-heavy
+const unsigned int STACK_DEFAULT_FOR_PTHREAD = 2048;
+
+/// Maximum size of the RAM image of a process. If a program requires more
+/// the kernel will not run it (MUST be divisible by 4)
+const unsigned int MAX_PROCESS_IMAGE_SIZE = 64 * 1024;
+
+/// Minimum size of the stack for a process. If a program specifies a lower
+/// size the kernel will not run it (MUST be divisible by 4)
+const unsigned int MIN_PROCESS_STACK_SIZE = STACK_MIN;
+
+/// Every userspace thread has two stacks, one for when it is running in
+/// userspace and one for when it is running in kernelspace (that is, while it
+/// is executing system calls). This is the size of the stack for when the
+/// thread is running in kernelspace (MUST be divisible by 4)
+const unsigned int SYSTEM_MODE_PROCESS_STACK_SIZE = 2 * 1024;
+
+/// Number of priorities (MUST be >1)
+/// PRIORITY_MAX-1 is the highest priority, 0 is the lowest. -1 is reserved as
+/// the priority of the idle thread.
+/// The meaning of a thread's priority depends on the chosen scheduler.
+#ifdef SCHED_TYPE_PRIORITY
+// Can be modified, but a high value makes context switches more expensive
+const short int PRIORITY_MAX = 4;
+#elif defined(SCHED_TYPE_CONTROL_BASED)
+// Don't touch, the limit is due to the fixed point implementation
+// It's not needed for if floating point is selected, but kept for consistency
+const short int PRIORITY_MAX = 64;
+#else  // SCHED_TYPE_EDF
+// Doesn't exist for this kind of scheduler
+#endif
+
+/// Priority of main()
+/// The meaning of a thread's priority depends on the chosen scheduler.
+const unsigned char MAIN_PRIORITY = 1;
+
+#ifdef SCHED_TYPE_PRIORITY
+/// Maximum thread time slice in nanoseconds, after which preemption occurs
+const unsigned int MAX_TIME_SLICE = 1000000;
+#endif  // SCHED_TYPE_PRIORITY
+
+//
+// Other low level kernel options. There is usually no need to modify these.
+//
+
+/// \internal Length of wartermark (in bytes) to check stack overflow.
+/// MUST be divisible by 4 and can also be zero.
+/// A high value increases context switch time.
+const unsigned int WATERMARK_LEN = 16;
+
+/// \internal Used to fill watermark
+const unsigned int WATERMARK_FILL = 0xaaaaaaaa;
+
+/// \internal Used to fill stack (for checking stack usage)
+const unsigned int STACK_FILL = 0xbbbbbbbb;
+
+// Compiler version checks
+#if !defined(_MIOSIX_GCC_PATCH_MAJOR) || _MIOSIX_GCC_PATCH_MAJOR < 3
+#error \
+    "You are using a too old or unsupported compiler. Get the latest one from https://miosix.org/wiki/index.php?title=Miosix_Toolchain"
+#endif
+#if _MIOSIX_GCC_PATCH_MAJOR > 3
+#warning "You are using a too new compiler, which may not be supported"
+#endif
+
+/**
+ * \}
+ */
+
+}  // namespace miosix
diff --git a/src/bsps/stm32f767zi_automated_antennas/core/stage_1_boot.cpp b/src/bsps/stm32f767zi_automated_antennas/core/stage_1_boot.cpp
new file mode 100644
index 000000000..d696c2eae
--- /dev/null
+++ b/src/bsps/stm32f767zi_automated_antennas/core/stage_1_boot.cpp
@@ -0,0 +1,505 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Emilio Corigliano
+ *
+ * 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 <string.h>
+
+#include "core/cache_cortexMx.h"
+#include "core/interrupts.h"  //For the unexpected interrupt call
+#include "core/interrupts_cortexMx.h"
+#include "interfaces/arch_registers.h"
+#include "interfaces/bsp.h"
+#include "kernel/stage_2_boot.h"
+
+/*
+ * startup.cpp
+ * STM32 C++ startup.
+ * NOTE: for stm32f767 devices ONLY.
+ * Supports interrupt handlers in C++ without extern "C"
+ * Developed by Terraneo Federico, based on ST startup code.
+ * Additionally modified to boot Miosix.
+ */
+
+/**
+ * Called by Reset_Handler, performs initialization and calls main.
+ * Never returns.
+ */
+void program_startup() __attribute__((noreturn));
+void program_startup()
+{
+    // Cortex M7 core appears to get out of reset with interrupts already
+    // enabled
+    __disable_irq();
+
+    miosix::IRQconfigureCache((const unsigned int *)0xd0000000,
+                              8 * 1024 * 1024);
+
+    // These are defined in the linker script
+    extern unsigned char _etext asm("_etext");
+    extern unsigned char _data asm("_data");
+    extern unsigned char _edata asm("_edata");
+    extern unsigned char _bss_start asm("_bss_start");
+    extern unsigned char _bss_end asm("_bss_end");
+
+    // Initialize .data section, clear .bss section
+    unsigned char *etext     = &_etext;
+    unsigned char *data      = &_data;
+    unsigned char *edata     = &_edata;
+    unsigned char *bss_start = &_bss_start;
+    unsigned char *bss_end   = &_bss_end;
+    memcpy(data, etext, edata - data);
+    memset(bss_start, 0, bss_end - bss_start);
+
+    // Move on to stage 2
+    _init();
+
+    // If main returns, reboot
+    NVIC_SystemReset();
+    for (;;)
+        ;
+}
+
+/**
+ * Reset handler, called by hardware immediately after reset
+ */
+void Reset_Handler() __attribute__((__interrupt__, noreturn));
+void Reset_Handler()
+{
+    /**
+     * SystemInit() is called *before* initializing .data and zeroing .bss
+     * Despite all startup files provided by ST do the opposite, there are three
+     * good reasons to do so:
+     * 1. First, the CMSIS specifications say that SystemInit() must not access
+     *    global variables, so it is actually possible to call it before
+     * 2. Second, when running Miosix with the xram linker scripts .data and
+     *    .bss are placed in the external RAM, so we *must* call SystemInit(),
+     *    which enables xram, before touching .data and .bss
+     * 3. Third, this is a performance improvement since the loops that
+     *    initialize .data and zeros .bss now run with the CPU at full speed
+     *    instead of 8MHz
+     */
+    SystemInit();
+
+/**
+ * ST does not provide code to initialize the SDRAM at boot.
+ * Put after SystemInit() as SDRAM is timing-sensitive and needs the full
+ * clock speed.
+ */
+#ifdef __ENABLE_XRAM
+    miosix::configureSdram();
+#endif  //__ENABLE_XRAM
+
+    /*
+     * Load into the program stack pointer the heap end address and switch from
+     * the msp to sps.
+     * This is required for booting Miosix, a small portion of the top of the
+     * heap area will be used as stack until the first thread starts. After,
+     * this stack will be abandoned and the process stack will point to the
+     * current thread's stack.
+     */
+    asm volatile(
+        "ldr r0,  =_heap_end          \n\t"
+        "msr psp, r0                  \n\t"
+        "movw r0, #2                  \n\n"  // Set the control register to use
+        "msr control, r0              \n\t"  // the process stack
+        "isb                          \n\t" ::
+            : "r0");
+
+    program_startup();
+}
+
+/**
+ * All unused interrupts call this function.
+ */
+extern "C" void Default_Handler() { unexpectedInterrupt(); }
+
+// System handlers
+void /*__attribute__((weak))*/ Reset_Handler();      // These interrupts are not
+void /*__attribute__((weak))*/ NMI_Handler();        // weak because they are
+void /*__attribute__((weak))*/ HardFault_Handler();  // surely defined by Miosix
+void /*__attribute__((weak))*/ MemManage_Handler();
+void /*__attribute__((weak))*/ BusFault_Handler();
+void /*__attribute__((weak))*/ UsageFault_Handler();
+void /*__attribute__((weak))*/ SVC_Handler();
+void /*__attribute__((weak))*/ DebugMon_Handler();
+void /*__attribute__((weak))*/ PendSV_Handler();
+void __attribute__((weak)) SysTick_Handler();
+
+// Interrupt handlers
+void __attribute__((weak)) WWDG_IRQHandler();
+void __attribute__((weak)) PVD_IRQHandler();
+void __attribute__((weak)) TAMP_STAMP_IRQHandler();
+void __attribute__((weak)) RTC_WKUP_IRQHandler();
+void __attribute__((weak)) FLASH_IRQHandler();
+void __attribute__((weak)) RCC_IRQHandler();
+void __attribute__((weak)) EXTI0_IRQHandler();
+void __attribute__((weak)) EXTI1_IRQHandler();
+void __attribute__((weak)) EXTI2_IRQHandler();
+void __attribute__((weak)) EXTI3_IRQHandler();
+void __attribute__((weak)) EXTI4_IRQHandler();
+void __attribute__((weak)) DMA1_Stream0_IRQHandler();
+void __attribute__((weak)) DMA1_Stream1_IRQHandler();
+void __attribute__((weak)) DMA1_Stream2_IRQHandler();
+void __attribute__((weak)) DMA1_Stream3_IRQHandler();
+void __attribute__((weak)) DMA1_Stream4_IRQHandler();
+void __attribute__((weak)) DMA1_Stream5_IRQHandler();
+void __attribute__((weak)) DMA1_Stream6_IRQHandler();
+void __attribute__((weak)) ADC_IRQHandler();
+void __attribute__((weak)) CAN1_TX_IRQHandler();
+void __attribute__((weak)) CAN1_RX0_IRQHandler();
+void __attribute__((weak)) CAN1_RX1_IRQHandler();
+void __attribute__((weak)) CAN1_SCE_IRQHandler();
+void __attribute__((weak)) EXTI9_5_IRQHandler();
+void __attribute__((weak)) TIM1_BRK_TIM9_IRQHandler();
+void __attribute__((weak)) TIM1_UP_TIM10_IRQHandler();
+void __attribute__((weak)) TIM1_TRG_COM_TIM11_IRQHandler();
+void __attribute__((weak)) TIM1_CC_IRQHandler();
+void __attribute__((weak)) TIM2_IRQHandler();
+void __attribute__((weak)) TIM3_IRQHandler();
+void __attribute__((weak)) TIM4_IRQHandler();
+void __attribute__((weak)) I2C1_EV_IRQHandler();
+void __attribute__((weak)) I2C1_ER_IRQHandler();
+void __attribute__((weak)) I2C2_EV_IRQHandler();
+void __attribute__((weak)) I2C2_ER_IRQHandler();
+void __attribute__((weak)) SPI1_IRQHandler();
+void __attribute__((weak)) SPI2_IRQHandler();
+void __attribute__((weak)) USART1_IRQHandler();
+void __attribute__((weak)) USART2_IRQHandler();
+void __attribute__((weak)) USART3_IRQHandler();
+void __attribute__((weak)) EXTI15_10_IRQHandler();
+void __attribute__((weak)) RTC_Alarm_IRQHandler();
+void __attribute__((weak)) OTG_FS_WKUP_IRQHandler();
+void __attribute__((weak)) TIM8_BRK_TIM12_IRQHandler();
+void __attribute__((weak)) TIM8_UP_TIM13_IRQHandler();
+void __attribute__((weak)) TIM8_TRG_COM_TIM14_IRQHandler();
+void __attribute__((weak)) TIM8_CC_IRQHandler();
+void __attribute__((weak)) DMA1_Stream7_IRQHandler();
+void __attribute__((weak)) FMC_IRQHandler();
+void __attribute__((weak)) SDMMC1_IRQHandler();
+void __attribute__((weak)) TIM5_IRQHandler();
+void __attribute__((weak)) SPI3_IRQHandler();
+void __attribute__((weak)) UART4_IRQHandler();
+void __attribute__((weak)) UART5_IRQHandler();
+void __attribute__((weak)) TIM6_DAC_IRQHandler();
+void __attribute__((weak)) TIM7_IRQHandler();
+void __attribute__((weak)) DMA2_Stream0_IRQHandler();
+void __attribute__((weak)) DMA2_Stream1_IRQHandler();
+void __attribute__((weak)) DMA2_Stream2_IRQHandler();
+void __attribute__((weak)) DMA2_Stream3_IRQHandler();
+void __attribute__((weak)) DMA2_Stream4_IRQHandler();
+void __attribute__((weak)) ETH_IRQHandler();
+void __attribute__((weak)) ETH_WKUP_IRQHandler();
+void __attribute__((weak)) CAN2_TX_IRQHandler();
+void __attribute__((weak)) CAN2_RX0_IRQHandler();
+void __attribute__((weak)) CAN2_RX1_IRQHandler();
+void __attribute__((weak)) CAN2_SCE_IRQHandler();
+void __attribute__((weak)) OTG_FS_IRQHandler();
+void __attribute__((weak)) DMA2_Stream5_IRQHandler();
+void __attribute__((weak)) DMA2_Stream6_IRQHandler();
+void __attribute__((weak)) DMA2_Stream7_IRQHandler();
+void __attribute__((weak)) USART6_IRQHandler();
+void __attribute__((weak)) I2C3_EV_IRQHandler();
+void __attribute__((weak)) I2C3_ER_IRQHandler();
+void __attribute__((weak)) OTG_HS_EP1_OUT_IRQHandler();
+void __attribute__((weak)) OTG_HS_EP1_IN_IRQHandler();
+void __attribute__((weak)) OTG_HS_WKUP_IRQHandler();
+void __attribute__((weak)) OTG_HS_IRQHandler();
+void __attribute__((weak)) DCMI_IRQHandler();
+void __attribute__((weak)) CRYP_IRQHandler();
+void __attribute__((weak)) RNG_IRQHandler();
+void __attribute__((weak)) FPU_IRQHandler();
+void __attribute__((weak)) UART7_IRQHandler();
+void __attribute__((weak)) UART8_IRQHandler();
+void __attribute__((weak)) SPI4_IRQHandler();
+void __attribute__((weak)) SPI5_IRQHandler();
+void __attribute__((weak)) SPI6_IRQHandler();
+void __attribute__((weak)) SAI1_IRQHandler();
+void __attribute__((weak)) LTDC_IRQHandler();
+void __attribute__((weak)) LTDC_ER_IRQHandler();
+void __attribute__((weak)) DMA2D_IRQHandler();
+void __attribute__((weak)) SAI2_IRQHandler();
+void __attribute__((weak)) QUADSPI_IRQHandler();
+void __attribute__((weak)) LPTIM1_IRQHandler();
+void __attribute__((weak)) CEC_IRQHandler();
+void __attribute__((weak)) I2C4_EV_IRQHandler();
+void __attribute__((weak)) I2C4_ER_IRQHandler();
+void __attribute__((weak)) SPDIF_RX_IRQHandler();
+void __attribute__((weak)) DSIHOST_IRQHandler();
+void __attribute__((weak)) DFSDM1_FLT0_IRQHandler();
+void __attribute__((weak)) DFSDM1_FLT1_IRQHandler();
+void __attribute__((weak)) DFSDM1_FLT2_IRQHandler();
+void __attribute__((weak)) DFSDM1_FLT3_IRQHandler();
+void __attribute__((weak)) SDMMC2_IRQHandler();
+void __attribute__((weak)) CAN3_TX_IRQHandler();
+void __attribute__((weak)) CAN3_RX0_IRQHandler();
+void __attribute__((weak)) CAN3_RX1_IRQHandler();
+void __attribute__((weak)) CAN3_SCE_IRQHandler();
+void __attribute__((weak)) JPEG_IRQHandler();
+void __attribute__((weak)) MDIOS_IRQHandler();
+
+// Stack top, defined in the linker script
+extern char _main_stack_top asm("_main_stack_top");
+
+// Interrupt vectors, must be placed @ address 0x00000000
+// The extern declaration is required otherwise g++ optimizes it out
+extern void (*const __Vectors[])();
+void (*const __Vectors[])() __attribute__((section(".isr_vector"))) = {
+    reinterpret_cast<void (*)()>(&_main_stack_top), /* Stack pointer*/
+    Reset_Handler,                                  /* Reset Handler */
+    NMI_Handler,                                    /* NMI Handler */
+    HardFault_Handler,                              /* Hard Fault Handler */
+    MemManage_Handler,                              /* MPU Fault Handler */
+    BusFault_Handler,                               /* Bus Fault Handler */
+    UsageFault_Handler,                             /* Usage Fault Handler */
+    0,                                              /* Reserved */
+    0,                                              /* Reserved */
+    0,                                              /* Reserved */
+    0,                                              /* Reserved */
+    SVC_Handler,                                    /* SVCall Handler */
+    DebugMon_Handler,                               /* Debug Monitor Handler */
+    0,                                              /* Reserved */
+    PendSV_Handler,                                 /* PendSV Handler */
+    SysTick_Handler,                                /* SysTick Handler */
+
+    /* External Interrupts */
+    WWDG_IRQHandler,
+    PVD_IRQHandler,
+    TAMP_STAMP_IRQHandler,
+    RTC_WKUP_IRQHandler,
+    FLASH_IRQHandler,
+    RCC_IRQHandler,
+    EXTI0_IRQHandler,
+    EXTI1_IRQHandler,
+    EXTI2_IRQHandler,
+    EXTI3_IRQHandler,
+    EXTI4_IRQHandler,
+    DMA1_Stream0_IRQHandler,
+    DMA1_Stream1_IRQHandler,
+    DMA1_Stream2_IRQHandler,
+    DMA1_Stream3_IRQHandler,
+    DMA1_Stream4_IRQHandler,
+    DMA1_Stream5_IRQHandler,
+    DMA1_Stream6_IRQHandler,
+    ADC_IRQHandler,
+    CAN1_TX_IRQHandler,
+    CAN1_RX0_IRQHandler,
+    CAN1_RX1_IRQHandler,
+    CAN1_SCE_IRQHandler,
+    EXTI9_5_IRQHandler,
+    TIM1_BRK_TIM9_IRQHandler,
+    TIM1_UP_TIM10_IRQHandler,
+    TIM1_TRG_COM_TIM11_IRQHandler,
+    TIM1_CC_IRQHandler,
+    TIM2_IRQHandler,
+    TIM3_IRQHandler,
+    TIM4_IRQHandler,
+    I2C1_EV_IRQHandler,
+    I2C1_ER_IRQHandler,
+    I2C2_EV_IRQHandler,
+    I2C2_ER_IRQHandler,
+    SPI1_IRQHandler,
+    SPI2_IRQHandler,
+    USART1_IRQHandler,
+    USART2_IRQHandler,
+    USART3_IRQHandler,
+    EXTI15_10_IRQHandler,
+    RTC_Alarm_IRQHandler,
+    OTG_FS_WKUP_IRQHandler,
+    TIM8_BRK_TIM12_IRQHandler,
+    TIM8_UP_TIM13_IRQHandler,
+    TIM8_TRG_COM_TIM14_IRQHandler,
+    TIM8_CC_IRQHandler,
+    DMA1_Stream7_IRQHandler,
+    FMC_IRQHandler,
+    SDMMC1_IRQHandler,
+    TIM5_IRQHandler,
+    SPI3_IRQHandler,
+    UART4_IRQHandler,
+    UART5_IRQHandler,
+    TIM6_DAC_IRQHandler,
+    TIM7_IRQHandler,
+    DMA2_Stream0_IRQHandler,
+    DMA2_Stream1_IRQHandler,
+    DMA2_Stream2_IRQHandler,
+    DMA2_Stream3_IRQHandler,
+    DMA2_Stream4_IRQHandler,
+    ETH_IRQHandler,
+    ETH_WKUP_IRQHandler,
+    CAN2_TX_IRQHandler,
+    CAN2_RX0_IRQHandler,
+    CAN2_RX1_IRQHandler,
+    CAN2_SCE_IRQHandler,
+    OTG_FS_IRQHandler,
+    DMA2_Stream5_IRQHandler,
+    DMA2_Stream6_IRQHandler,
+    DMA2_Stream7_IRQHandler,
+    USART6_IRQHandler,
+    I2C3_EV_IRQHandler,
+    I2C3_ER_IRQHandler,
+    OTG_HS_EP1_OUT_IRQHandler,
+    OTG_HS_EP1_IN_IRQHandler,
+    OTG_HS_WKUP_IRQHandler,
+    OTG_HS_IRQHandler,
+    DCMI_IRQHandler,
+    CRYP_IRQHandler,
+    RNG_IRQHandler,
+    FPU_IRQHandler,
+    UART7_IRQHandler,
+    UART8_IRQHandler,
+    SPI4_IRQHandler,
+    SPI5_IRQHandler,
+    SPI6_IRQHandler,
+    SAI1_IRQHandler,
+    LTDC_IRQHandler,
+    LTDC_ER_IRQHandler,
+    DMA2D_IRQHandler,
+    SAI2_IRQHandler,
+    QUADSPI_IRQHandler,
+    LPTIM1_IRQHandler,
+    CEC_IRQHandler,
+    I2C4_EV_IRQHandler,
+    I2C4_ER_IRQHandler,
+    SPDIF_RX_IRQHandler,
+    DSIHOST_IRQHandler,
+    DFSDM1_FLT0_IRQHandler,
+    DFSDM1_FLT1_IRQHandler,
+    DFSDM1_FLT2_IRQHandler,
+    DFSDM1_FLT3_IRQHandler,
+    SDMMC2_IRQHandler,
+    CAN3_TX_IRQHandler,
+    CAN3_RX0_IRQHandler,
+    CAN3_RX1_IRQHandler,
+    CAN3_SCE_IRQHandler,
+    JPEG_IRQHandler,
+    MDIOS_IRQHandler,
+};
+
+#pragma weak SysTick_IRQHandler            = Default_Handler
+#pragma weak WWDG_IRQHandler               = Default_Handler
+#pragma weak PVD_IRQHandler                = Default_Handler
+#pragma weak TAMP_STAMP_IRQHandler         = Default_Handler
+#pragma weak RTC_WKUP_IRQHandler           = Default_Handler
+#pragma weak FLASH_IRQHandler              = Default_Handler
+#pragma weak RCC_IRQHandler                = Default_Handler
+#pragma weak EXTI0_IRQHandler              = Default_Handler
+#pragma weak EXTI1_IRQHandler              = Default_Handler
+#pragma weak EXTI2_IRQHandler              = Default_Handler
+#pragma weak EXTI3_IRQHandler              = Default_Handler
+#pragma weak EXTI4_IRQHandler              = Default_Handler
+#pragma weak DMA1_Stream0_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream1_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream2_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream3_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream4_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream5_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream6_IRQHandler       = Default_Handler
+#pragma weak ADC_IRQHandler                = Default_Handler
+#pragma weak CAN1_TX_IRQHandler            = Default_Handler
+#pragma weak CAN1_RX0_IRQHandler           = Default_Handler
+#pragma weak CAN1_RX1_IRQHandler           = Default_Handler
+#pragma weak CAN1_SCE_IRQHandler           = Default_Handler
+#pragma weak EXTI9_5_IRQHandler            = Default_Handler
+#pragma weak TIM1_BRK_TIM9_IRQHandler      = Default_Handler
+#pragma weak TIM1_UP_TIM10_IRQHandler      = Default_Handler
+#pragma weak TIM1_TRG_COM_TIM11_IRQHandler = Default_Handler
+#pragma weak TIM1_CC_IRQHandler            = Default_Handler
+#pragma weak TIM2_IRQHandler               = Default_Handler
+#pragma weak TIM3_IRQHandler               = Default_Handler
+#pragma weak TIM4_IRQHandler               = Default_Handler
+#pragma weak I2C1_EV_IRQHandler            = Default_Handler
+#pragma weak I2C1_ER_IRQHandler            = Default_Handler
+#pragma weak I2C2_EV_IRQHandler            = Default_Handler
+#pragma weak I2C2_ER_IRQHandler            = Default_Handler
+#pragma weak SPI1_IRQHandler               = Default_Handler
+#pragma weak SPI2_IRQHandler               = Default_Handler
+// #pragma weak USART1_IRQHandler             = Default_Handler
+// #pragma weak USART2_IRQHandler             = Default_Handler
+// #pragma weak USART3_IRQHandler             = Default_Handler
+#pragma weak EXTI15_10_IRQHandler          = Default_Handler
+#pragma weak RTC_Alarm_IRQHandler          = Default_Handler
+#pragma weak OTG_FS_WKUP_IRQHandler        = Default_Handler
+#pragma weak TIM8_BRK_TIM12_IRQHandler     = Default_Handler
+#pragma weak TIM8_UP_TIM13_IRQHandler      = Default_Handler
+#pragma weak TIM8_TRG_COM_TIM14_IRQHandler = Default_Handler
+#pragma weak TIM8_CC_IRQHandler            = Default_Handler
+#pragma weak DMA1_Stream7_IRQHandler       = Default_Handler
+#pragma weak FMC_IRQHandler                = Default_Handler
+#pragma weak SDMMC1_IRQHandler             = Default_Handler
+#pragma weak TIM5_IRQHandler               = Default_Handler
+#pragma weak SPI3_IRQHandler               = Default_Handler
+// #pragma weak UART4_IRQHandler              = Default_Handler
+// #pragma weak UART5_IRQHandler              = Default_Handler
+#pragma weak TIM6_DAC_IRQHandler     = Default_Handler
+#pragma weak TIM7_IRQHandler         = Default_Handler
+#pragma weak DMA2_Stream0_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream1_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream2_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream3_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream4_IRQHandler = Default_Handler
+#pragma weak ETH_IRQHandler          = Default_Handler
+#pragma weak ETH_WKUP_IRQHandler     = Default_Handler
+#pragma weak CAN2_TX_IRQHandler      = Default_Handler
+#pragma weak CAN2_RX0_IRQHandler     = Default_Handler
+#pragma weak CAN2_RX1_IRQHandler     = Default_Handler
+#pragma weak CAN2_SCE_IRQHandler     = Default_Handler
+#pragma weak OTG_FS_IRQHandler       = Default_Handler
+#pragma weak DMA2_Stream5_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream6_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream7_IRQHandler = Default_Handler
+// #pragma weak USART6_IRQHandler             = Default_Handler
+#pragma weak I2C3_EV_IRQHandler        = Default_Handler
+#pragma weak I2C3_ER_IRQHandler        = Default_Handler
+#pragma weak OTG_HS_EP1_OUT_IRQHandler = Default_Handler
+#pragma weak OTG_HS_EP1_IN_IRQHandler  = Default_Handler
+#pragma weak OTG_HS_WKUP_IRQHandler    = Default_Handler
+#pragma weak OTG_HS_IRQHandler         = Default_Handler
+#pragma weak DCMI_IRQHandler           = Default_Handler
+#pragma weak CRYP_IRQHandler           = Default_Handler
+#pragma weak RNG_IRQHandler            = Default_Handler
+#pragma weak FPU_IRQHandler            = Default_Handler
+// #pragma weak UART7_IRQHandler              = Default_Handler
+// #pragma weak UART8_IRQHandler              = Default_Handler
+#pragma weak SPI4_IRQHandler        = Default_Handler
+#pragma weak SPI5_IRQHandler        = Default_Handler
+#pragma weak SPI6_IRQHandler        = Default_Handler
+#pragma weak SAI1_IRQHandler        = Default_Handler
+#pragma weak LTDC_IRQHandler        = Default_Handler
+#pragma weak LTDC_ER_IRQHandler     = Default_Handler
+#pragma weak DMA2D_IRQHandler       = Default_Handler
+#pragma weak SAI2_IRQHandler        = Default_Handler
+#pragma weak QUADSPI_IRQHandler     = Default_Handler
+#pragma weak LPTIM1_IRQHandler      = Default_Handler
+#pragma weak CEC_IRQHandler         = Default_Handler
+#pragma weak I2C4_EV_IRQHandler     = Default_Handler
+#pragma weak I2C4_ER_IRQHandler     = Default_Handler
+#pragma weak SPDIF_RX_IRQHandler    = Default_Handler
+#pragma weak DSIHOST_IRQHandler     = Default_Handler
+#pragma weak DFSDM1_FLT0_IRQHandler = Default_Handler
+#pragma weak DFSDM1_FLT1_IRQHandler = Default_Handler
+#pragma weak DFSDM1_FLT2_IRQHandler = Default_Handler
+#pragma weak DFSDM1_FLT3_IRQHandler = Default_Handler
+#pragma weak SDMMC2_IRQHandler      = Default_Handler
+#pragma weak CAN3_TX_IRQHandler     = Default_Handler
+#pragma weak CAN3_RX0_IRQHandler    = Default_Handler
+#pragma weak CAN3_RX1_IRQHandler    = Default_Handler
+#pragma weak CAN3_SCE_IRQHandler    = Default_Handler
+#pragma weak JPEG_IRQHandler        = Default_Handler
+#pragma weak MDIOS_IRQHandler       = Default_Handler
diff --git a/src/bsps/stm32f767zi_automated_antennas/interfaces-impl/arch_registers_impl.h b/src/bsps/stm32f767zi_automated_antennas/interfaces-impl/arch_registers_impl.h
new file mode 100644
index 000000000..a2f830b21
--- /dev/null
+++ b/src/bsps/stm32f767zi_automated_antennas/interfaces-impl/arch_registers_impl.h
@@ -0,0 +1,39 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Emilio Corigliano
+ *
+ * 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.
+ */
+
+#ifndef ARCH_REGISTERS_IMPL_H
+#define ARCH_REGISTERS_IMPL_H
+
+// stm32f7xx.h defines a few macros like __ICACHE_PRESENT, __DCACHE_PRESENT and
+// includes core_cm7.h. Do not include core_cm7.h before.
+#define STM32F767xx
+#include "CMSIS/Device/ST/STM32F7xx/Include/stm32f7xx.h"
+
+#if (__ICACHE_PRESENT != 1) || (__DCACHE_PRESENT != 1)
+#error "Wrong include order"
+#endif
+
+#include "CMSIS/Device/ST/STM32F7xx/Include/system_stm32f7xx.h"
+
+#define RCC_SYNC() __DSB()  // TODO: can this dsb be removed?
+
+#endif  // ARCH_REGISTERS_IMPL_H
diff --git a/src/bsps/stm32f767zi_automated_antennas/interfaces-impl/bsp.cpp b/src/bsps/stm32f767zi_automated_antennas/interfaces-impl/bsp.cpp
new file mode 100644
index 000000000..0853b1f8b
--- /dev/null
+++ b/src/bsps/stm32f767zi_automated_antennas/interfaces-impl/bsp.cpp
@@ -0,0 +1,367 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Emilio Corigliano
+ *
+ * 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.
+ */
+
+/***********************************************************************
+ * bsp.cpp Part of the Miosix Embedded OS.
+ * Board support package, this file initializes hardware.
+ ************************************************************************/
+
+#include "interfaces/bsp.h"
+
+#include <inttypes.h>
+#include <sys/ioctl.h>
+
+#include <cstdlib>
+
+#include "board_settings.h"
+#include "config/miosix_settings.h"
+#include "drivers/sd_stm32f2_f4_f7.h"
+#include "drivers/serial.h"
+#include "drivers/serial_stm32.h"
+#include "drivers/stm32_sgm.h"
+#include "filesystem/console/console_device.h"
+#include "filesystem/file_access.h"
+#include "hwmapping.h"
+#include "interfaces/arch_registers.h"
+#include "interfaces/delays.h"
+#include "interfaces/portability.h"
+#include "kernel/kernel.h"
+#include "kernel/logging.h"
+#include "kernel/sync.h"
+
+namespace miosix
+{
+
+//
+// Initialization
+//
+
+static void sdramCommandWait()
+{
+    for (int i = 0; i < 0xffff; i++)
+        if ((FMC_Bank5_6->SDSR & FMC_SDSR_BUSY) == 0)
+            return;
+}
+
+void configureSdram()
+{
+    // Enable gpios used by the ram
+    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN | RCC_AHB1ENR_GPIOCEN |
+                    RCC_AHB1ENR_GPIODEN | RCC_AHB1ENR_GPIOEEN |
+                    RCC_AHB1ENR_GPIOFEN | RCC_AHB1ENR_GPIOGEN;
+    RCC_SYNC();
+
+    // On the compute unit with F767ZI, the SDRAM pins are:
+    // - PG8:  FMC_SDCLK  (sdram clock)
+    // - PB5:  FMC_SDCKE1 (sdram bank 2 clock enable)
+    // - PB6:  FMC_SDNE1  (sdram bank 2 chip enable)
+    // - PF0:  FMC_A0
+    // - PF1:  FMC_A1
+    // - PF2:  FMC_A2
+    // - PF3:  FMC_A3
+    // - PF4:  FMC_A4
+    // - PF5:  FMC_A5
+    // - PF12: FMC_A6
+    // - PF13: FMC_A7
+    // - PF14: FMC_A8
+    // - PF15: FMC_A9
+    // - PG0:  FMC_A10
+    // - PG1:  FMC_A11
+    // - PG2:  FMC_A12    (used only by the 32MB ram, not by the 8MB one)
+    // - PD14: FMC_D0
+    // - PD15: FMC_D1
+    // - PD0:  FMC_D2
+    // - PD1:  FMC_D3
+    // - PE7:  FMC_D4
+    // - PE8:  FMC_D5
+    // - PE9:  FMC_D6
+    // - PE10: FMC_D7
+    // - PE11: FMC_D8
+    // - PE12: FMC_D9
+    // - PE13: FMC_D10
+    // - PE14: FMC_D11
+    // - PE15: FMC_D12
+    // - PD8:  FMC_D13
+    // - PD9:  FMC_D14
+    // - PD10: FMC_D15
+
+    // - PG4:  FMC_BA0
+    // - PG5:  FMC_BA1
+    // - PF11: FMC_SDNRAS
+    // - PG15: FMC_SDNCAS
+    // - PC0:  FMC_SDNWE
+    // - PE0:  FMC_NBL0
+    // - PE1:  FMC_NBL1
+
+    // All SDRAM GPIOs needs to be configured with alternate function 12 and
+    // maximum speed
+
+    // WARNING: The current configuration is for the 8MB ram
+
+    // Alternate functions
+    GPIOB->AFR[0] = 0x0cc00000;
+    GPIOC->AFR[0] = 0x0000000c;
+    GPIOD->AFR[0] = 0x000000cc;
+    GPIOD->AFR[1] = 0xcc000ccc;
+    GPIOE->AFR[0] = 0xc00000cc;
+    GPIOE->AFR[1] = 0xcccccccc;
+    GPIOF->AFR[0] = 0x00cccccc;
+    GPIOF->AFR[1] = 0xccccc000;
+    GPIOG->AFR[0] = 0x00cc0ccc;
+    GPIOG->AFR[1] = 0xc000000c;
+
+    // Mode
+    GPIOB->MODER = 0x00002800;
+    GPIOC->MODER = 0x00000002;
+    GPIOD->MODER = 0xa02a000a;
+    GPIOE->MODER = 0xaaaa800a;
+    GPIOF->MODER = 0xaa800aaa;
+    GPIOG->MODER = 0x80020a2a;
+
+    // Speed (high speed for all, very high speed for SDRAM pins)
+    GPIOB->OSPEEDR = 0x00003c00;
+    GPIOC->OSPEEDR = 0x00000003;
+    GPIOD->OSPEEDR = 0xf03f000f;
+    GPIOE->OSPEEDR = 0xffffc00f;
+    GPIOF->OSPEEDR = 0xffc00fff;
+    GPIOG->OSPEEDR = 0xc0030f3f;
+
+    // Since we'we un-configured PB3 and PB4 (by default they are SWO and NJRST)
+    // finish the job and remove the default pull-up
+    GPIOB->PUPDR = 0;
+
+    // Enable the SDRAM controller clock
+    RCC->AHB3ENR |= RCC_AHB3ENR_FMCEN;
+    RCC_SYNC();
+
+    // The SDRAM is a AS4C4M16SA-6TAN
+    // HCLK = 216MHz -> SDRAM clock = HCLK/2 = 133MHz
+
+    // 1. Memory device features
+    FMC_Bank5_6->SDCR[0] = 0                     // 0 delay after CAS latency
+                           | FMC_SDCR1_RBURST    // Enable read bursts
+                           | FMC_SDCR1_SDCLK_1;  // SDCLK = HCLK / 2
+    FMC_Bank5_6->SDCR[1] = 0                     // Write accesses allowed
+                           | FMC_SDCR2_CAS_1     // 2 cycles CAS latency
+                           | FMC_SDCR2_NB        // 4 internal banks
+                           | FMC_SDCR2_MWID_0    // 16 bit data bus
+                           | FMC_SDCR2_NR_1      // 13 bit row address
+                           // cppcheck-suppress duplicateExpression
+                           | 0;  // 8 bit column address
+
+// 2. Memory device timings
+#ifdef SYSCLK_FREQ_216MHz
+    // SDRAM timings. One clock cycle is 9.26ns
+    FMC_Bank5_6->SDTR[0] =
+        (2 - 1) << FMC_SDTR1_TRP_Pos     // 2 cycles TRP  (18.52ns > 18ns)
+        | (7 - 1) << FMC_SDTR1_TRC_Pos;  // 7 cycles TRC  (64.82ns > 60ns)
+    FMC_Bank5_6->SDTR[1] =
+        (2 - 1) << FMC_SDTR1_TRCD_Pos     // 2 cycles TRCD (18.52ns > 18ns)
+        | (2 - 1) << FMC_SDTR1_TWR_Pos    // 2 cycles TWR  (min 2cc > 12ns)
+        | (5 - 1) << FMC_SDTR1_TRAS_Pos   // 5 cycles TRAS (46.3ns  > 42ns)
+        | (7 - 1) << FMC_SDTR1_TXSR_Pos   // 7 cycles TXSR (74.08ns > 61.5ns)
+        | (2 - 1) << FMC_SDTR1_TMRD_Pos;  // 2 cycles TMRD (min 2cc > 12ns)
+#else
+#error No SDRAM timings for this clock
+#endif
+
+    // 3. Enable the bank 2 clock
+    FMC_Bank5_6->SDCMR = FMC_SDCMR_MODE_0   // Clock Configuration Enable
+                         | FMC_SDCMR_CTB2;  // Bank 2
+    sdramCommandWait();
+
+    // 4. Wait during command execution
+    delayUs(100);
+
+    // 5. Issue a "Precharge All" command
+    FMC_Bank5_6->SDCMR = FMC_SDCMR_MODE_1   // Precharge all
+                         | FMC_SDCMR_CTB2;  // Bank 2
+    sdramCommandWait();
+
+    // 6. Issue Auto-Refresh commands
+    FMC_Bank5_6->SDCMR = FMC_SDCMR_MODE_1 | FMC_SDCMR_MODE_0  // Auto-Refresh
+                         | FMC_SDCMR_CTB2                     // Bank 2
+                         | (8 - 1) << FMC_SDCMR_NRFS_Pos;     // 2 Auto-Refresh
+    sdramCommandWait();
+
+    // 7. Issue a Load Mode Register command
+    FMC_Bank5_6->SDCMR = FMC_SDCMR_MODE_2               /// Load mode register
+                         | FMC_SDCMR_CTB2               // Bank 2
+                         | 0x220 << FMC_SDCMR_MRD_Pos;  // CAS = 2, burst = 1
+    sdramCommandWait();
+
+// 8. Program the refresh rate (4K / 32ms)
+// 64ms / 8192 = 7.8125us
+#ifdef SYSCLK_FREQ_216MHz
+    // 7.8125us * 133MHz = 1039 - 20 = 1019
+    FMC_Bank5_6->SDRTR = 1019 << FMC_SDRTR_COUNT_Pos;
+#else
+#error No SDRAM refresh timings for this clock
+#endif
+}
+
+void IRQbspInit()
+{
+    // Enable USART1 pins port
+    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN;
+
+    userLed1::mode(Mode::OUTPUT);
+    userLed2::mode(Mode::OUTPUT);
+    userLed3_1::mode(Mode::OUTPUT);
+    userLed3_2::mode(Mode::OUTPUT);
+    userLed4::mode(Mode::OUTPUT);
+    userSwitch::mode(Mode::INPUT);
+
+    using namespace interfaces;
+    spi1::sck::mode(Mode::ALTERNATE);
+    spi1::sck::alternateFunction(5);
+    spi1::miso::mode(Mode::ALTERNATE);
+    spi1::miso::alternateFunction(5);
+    spi1::mosi::mode(Mode::ALTERNATE);
+    spi1::mosi::alternateFunction(5);
+
+    spi3::sck::mode(Mode::ALTERNATE);
+    spi3::sck::alternateFunction(6);
+    spi3::miso::mode(Mode::ALTERNATE);
+    spi3::miso::alternateFunction(6);
+    spi3::mosi::mode(Mode::ALTERNATE);
+    spi3::mosi::alternateFunction(5);
+
+    spi4::sck::mode(Mode::ALTERNATE);
+    spi4::sck::alternateFunction(5);
+    spi4::miso::mode(Mode::ALTERNATE);
+    spi4::miso::alternateFunction(5);
+    spi4::mosi::mode(Mode::ALTERNATE);
+    spi4::mosi::alternateFunction(5);
+
+    can2::rx::mode(Mode::ALTERNATE);
+    can2::rx::alternateFunction(9);
+    can2::tx::mode(Mode::ALTERNATE);
+    can2::tx::alternateFunction(9);
+
+    usart1::tx::mode(Mode::ALTERNATE);
+    usart1::tx::alternateFunction(7);
+    usart1::rx::mode(Mode::ALTERNATE);
+    usart1::rx::alternateFunction(7);
+
+    usart2::tx::mode(Mode::ALTERNATE);
+    usart2::tx::alternateFunction(7);
+    usart2::rx::mode(Mode::ALTERNATE);
+    usart2::rx::alternateFunction(7);
+
+    using namespace radio;
+    cs::mode(Mode::OUTPUT);
+    cs::getPin().high();
+    dio0::mode(Mode::INPUT);
+    dio1::mode(Mode::INPUT);
+    dio3::mode(Mode::INPUT);
+    tx_enable::mode(Mode::OUTPUT);
+    rx_enable::mode(Mode::OUTPUT);
+
+    using namespace timers;
+    tim3ch2::mode(Mode::ALTERNATE);
+    tim3ch2::alternateFunction(2);
+    tim4ch1::mode(Mode::ALTERNATE);
+    tim4ch1::alternateFunction(2);
+
+    stepper1::enable::mode(Mode::OUTPUT);
+    stepper1::direction::mode(Mode::OUTPUT);
+
+    stepper2::enable::mode(Mode::OUTPUT);
+    stepper2::direction::mode(Mode::OUTPUT);
+
+    ethernet::cs::mode(Mode::OUTPUT);
+    ethernet::cs::high();
+    ethernet::nrst::mode(Mode::OUTPUT);
+    ethernet::nrst::high();
+    ethernet::intr::mode(Mode::INPUT);
+
+    DefaultConsole::instance().IRQset(intrusive_ref_ptr<Device>(new STM32Serial(
+        defaultSerial, defaultSerialSpeed, STM32Serial::NOFLOWCTRL)));
+}
+
+void bspInit2()
+{
+#ifdef WITH_FILESYSTEM
+    basicFilesystemSetup(SDIODriver::instance());
+#endif  // WITH_FILESYSTEM
+
+#ifdef WITH_BACKUP_SRAM
+    // Print the reset reason
+    bootlog("Reset reson: ");
+    switch (SGM::instance().lastResetReason())
+    {
+        case ResetReason::RST_LOW_PWR:
+            bootlog("low power\n");
+            break;
+        case ResetReason::RST_WINDOW_WDG:
+            bootlog("window watchdog\n");
+            break;
+        case ResetReason::RST_INDEPENDENT_WDG:
+            bootlog("indeendent watchdog\n");
+            break;
+        case ResetReason::RST_SW:
+            bootlog("software reset\n");
+            break;
+        case ResetReason::RST_POWER_ON:
+            bootlog("power on\n");
+            break;
+        case ResetReason::RST_PIN:
+            bootlog("reset pin\n");
+            break;
+        case ResetReason::RST_UNKNOWN:
+            bootlog("unknown\n");
+            break;
+    }
+#endif  // WITH_BACKUP_SRAM
+}
+
+//
+// Shutdown and reboot
+//
+
+void shutdown()
+{
+    ioctl(STDOUT_FILENO, IOCTL_SYNC, 0);
+
+#ifdef WITH_FILESYSTEM
+    FilesystemManager::instance().umountAll();
+#endif  // WITH_FILESYSTEM
+
+    disableInterrupts();
+    for (;;)
+        ;
+}
+
+void reboot()
+{
+    ioctl(STDOUT_FILENO, IOCTL_SYNC, 0);
+
+#ifdef WITH_FILESYSTEM
+    FilesystemManager::instance().umountAll();
+#endif  // WITH_FILESYSTEM
+
+    disableInterrupts();
+    miosix_private::IRQsystemReboot();
+}
+
+}  // namespace miosix
diff --git a/src/bsps/stm32f767zi_automated_antennas/interfaces-impl/bsp_impl.h b/src/bsps/stm32f767zi_automated_antennas/interfaces-impl/bsp_impl.h
new file mode 100644
index 000000000..1e0498f6b
--- /dev/null
+++ b/src/bsps/stm32f767zi_automated_antennas/interfaces-impl/bsp_impl.h
@@ -0,0 +1,116 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Emilio Corigliano
+ *
+ * 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.
+ */
+
+/***************************************************************************
+ * bsp_impl.h Part of the Miosix Embedded OS.
+ * Board support package, this file initializes hardware.
+ ***************************************************************************/
+
+#ifndef BSP_IMPL_H
+#define BSP_IMPL_H
+
+#include "config/miosix_settings.h"
+#include "interfaces/gpio.h"
+
+namespace miosix
+{
+
+/**
+\addtogroup Hardware
+\{
+*/
+
+/**
+ * \internal
+ * Called by stage_1_boot.cpp to enable the SDRAM before initializing .data/.bss
+ * Requires the CPU clock to be already configured (running from the PLL)
+ */
+void configureSdram();
+
+/**
+ * \internal
+ * Board pin definition
+ */
+typedef Gpio<GPIOB_BASE, 7> userLed1;
+typedef Gpio<GPIOE_BASE, 3> userLed2;
+typedef Gpio<GPIOC_BASE, 13> userLed3_1;
+typedef Gpio<GPIOG_BASE, 9> userLed3_2;
+typedef Gpio<GPIOC_BASE, 2> userLed4;
+typedef Gpio<GPIOB_BASE, 2> userSwitch;
+
+inline void ledOn()
+{
+    userLed1::high();
+    userLed2::high();
+    userLed3_1::high();
+    userLed3_2::high();
+    userLed4::high();
+}
+
+inline void ledOff()
+{
+    userLed1::low();
+    userLed2::low();
+    userLed3_1::low();
+    userLed3_2::low();
+    userLed4::low();
+}
+
+inline void led1On() { userLed1::high(); }
+
+inline void led1Off() { userLed1::low(); }
+
+inline void led2On() { userLed2::high(); }
+
+inline void led2Off() { userLed2::low(); }
+
+inline void led3On()
+{
+    userLed3_1::high();
+    userLed3_2::high();
+}
+
+inline void led3Off()
+{
+    userLed3_1::low();
+    userLed3_2::low();
+}
+
+/**
+ * Polls the SD card sense GPIO.
+ *
+ * This board has no SD card whatsoever, but a card can be connected to the
+ * following GPIOs:
+ * TODO: never tested
+ *
+ * \return true. As there's no SD card sense switch, let's pretend that
+ * the card is present.
+ */
+inline bool sdCardSense() { return true; }
+
+/**
+\}
+*/
+
+}  // namespace miosix
+
+#endif  // BSP_IMPL_H
diff --git a/src/bsps/stm32f767zi_automated_antennas/interfaces-impl/hwmapping.h b/src/bsps/stm32f767zi_automated_antennas/interfaces-impl/hwmapping.h
new file mode 100644
index 000000000..49e21359d
--- /dev/null
+++ b/src/bsps/stm32f767zi_automated_antennas/interfaces-impl/hwmapping.h
@@ -0,0 +1,131 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Emilio Corigliano
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include "interfaces/gpio.h"
+
+#define MIOSIX_ETHERNET_IRQ EXTI1_IRQHandlerImpl
+#define MIOSIX_ETHERNET_SPI SPI4
+
+namespace miosix
+{
+namespace interfaces
+{
+// RADIO 1
+namespace spi1
+{
+using sck  = Gpio<GPIOA_BASE, 5>;
+using miso = Gpio<GPIOA_BASE, 6>;
+using mosi = Gpio<GPIOA_BASE, 7>;
+}  // namespace spi1
+
+// FREE SPI
+namespace spi3
+{
+using sck  = Gpio<GPIOB_BASE, 3>;
+using miso = Gpio<GPIOB_BASE, 4>;
+using mosi = Gpio<GPIOD_BASE, 6>;
+}  // namespace spi3
+
+// ETHERNET
+namespace spi4
+{
+using sck  = Gpio<GPIOE_BASE, 2>;
+using miso = Gpio<GPIOE_BASE, 5>;
+using mosi = Gpio<GPIOE_BASE, 6>;
+}  // namespace spi4
+
+namespace can2
+{
+using rx = Gpio<GPIOB_BASE, 12>;
+using tx = Gpio<GPIOB_BASE, 13>;
+}  // namespace can2
+
+// DBG
+namespace usart1
+{
+using tx = Gpio<GPIOA_BASE, 9>;
+using rx = Gpio<GPIOA_BASE, 10>;
+}  // namespace usart1
+
+// FREE USART
+namespace usart2
+{
+using tx = Gpio<GPIOA_BASE, 2>;
+using rx = Gpio<GPIOA_BASE, 3>;
+}  // namespace usart2
+
+namespace timers
+{
+using tim3ch2 = Gpio<GPIOC_BASE, 7>;   // step 1
+using tim1ch4 = Gpio<GPIOA_BASE, 11>;  // count 1
+using tim4ch1 = Gpio<GPIOD_BASE, 12>;  // step 2
+using tim8ch4 = Gpio<GPIOC_BASE, 9>;   // count 2
+}  // namespace timers
+
+}  // namespace interfaces
+
+namespace radio
+{
+using sck       = interfaces::spi1::sck;
+using miso      = interfaces::spi1::miso;
+using mosi      = interfaces::spi1::mosi;
+using cs        = Gpio<GPIOA_BASE, 4>;
+using dio0      = Gpio<GPIOC_BASE, 6>;
+using dio1      = Gpio<GPIOD_BASE, 4>;
+using dio3      = Gpio<GPIOD_BASE, 5>;
+using rx_enable = Gpio<GPIOB_BASE, 9>;
+using tx_enable = Gpio<GPIOB_BASE, 8>;
+}  // namespace radio
+
+namespace stepper1
+{
+using enable     = Gpio<GPIOA_BASE, 8>;
+using direction  = Gpio<GPIOA_BASE, 12>;
+using pulseTimer = interfaces::timers::tim3ch2;
+using countTimer = interfaces::timers::tim1ch4;
+}  // namespace stepper1
+
+namespace stepper2
+{
+using enable     = Gpio<GPIOB_BASE, 14>;
+using direction  = Gpio<GPIOG_BASE, 7>;
+using pulseTimer = interfaces::timers::tim4ch1;
+using countTimer = interfaces::timers::tim8ch4;
+}  // namespace stepper2
+
+namespace ethernet
+{
+namespace spi
+{
+using sck  = miosix::interfaces::spi4::sck;
+using miso = miosix::interfaces::spi4::miso;
+using mosi = miosix::interfaces::spi4::mosi;
+}  // namespace spi
+
+using cs   = Gpio<GPIOE_BASE, 4>;
+using intr = Gpio<GPIOC_BASE, 1>;
+using nrst = Gpio<GPIOB_BASE, 1>;
+}  // namespace ethernet
+
+}  // namespace miosix
\ No newline at end of file
diff --git a/src/bsps/stm32f767zi_automated_antennas/stm32_2m+16m_xram.ld b/src/bsps/stm32f767zi_automated_antennas/stm32_2m+16m_xram.ld
new file mode 100644
index 000000000..5b3545abd
--- /dev/null
+++ b/src/bsps/stm32f767zi_automated_antennas/stm32_2m+16m_xram.ld
@@ -0,0 +1,190 @@
+/*
+ * C++ enabled linker script for stm32f769ni (2M FLASH, 512K RAM, 16MB XRAM)
+ * Developed by TFT: Terraneo Federico Technologies
+ * Optimized for use with the Miosix kernel
+ */
+
+/*
+ * This linker script puts:
+ * - read only data and code (.text, .rodata, .eh_*) in FLASH
+ * - the 512Byte main (IRQ) stack, .data and .bss in the DTCM 128KB RAM
+ * - .data, .bss, stacks and heap in the external 16MB SDRAM.
+ */
+
+/*
+ * The main stack is used for interrupt handling by the kernel.
+ *
+ * *** Readme ***
+ * This linker script places the main stack (used by the kernel for interrupts)
+ * at the bottom of the ram, instead of the top. This is done for two reasons:
+ *
+ * - as an optimization for microcontrollers with little ram memory. In fact
+ *   the implementation of malloc from newlib requests memory to the OS in 4KB
+ *   block (except the first block that can be smaller). This is probably done
+ *   for compatibility with OSes with an MMU and paged memory. To see why this
+ *   is bad, consider a microcontroller with 8KB of ram: when malloc finishes
+ *   up the first 4KB it will call _sbrk_r asking for a 4KB block, but this will
+ *   fail because the top part of the ram is used by the main stack. As a
+ *   result, the top part of the memory will not be used by malloc, even if
+ *   available (and it is nearly *half* the ram on an 8KB mcu). By placing the
+ *   main stack at the bottom of the ram, the upper 4KB block will be entirely
+ *   free and available as heap space.
+ *
+ * - In case of main stack overflow the cpu will fault because access to memory
+ *   before the beginning of the ram faults. Instead with the default stack
+ *   placement the main stack will silently collide with the heap.
+ * Note: if increasing the main stack size also increase the ORIGIN value in
+ * the MEMORY definitions below accordingly.
+ */
+
+_main_stack_size = 512;                             /* main stack = 512Bytes */
+_main_stack_top  = 0x20000000 + _main_stack_size;
+ASSERT(_main_stack_size   % 8 == 0, "MAIN stack size error");
+
+/* Mapping the heap into XRAM */
+_heap_end = 0xd0000000 + 16M;                        /* end of available ram */
+
+/* Identify the Entry Point  */
+ENTRY(_Z13Reset_Handlerv)
+
+/*
+ * Specify the memory areas
+ *
+ * NOTE: starting at 0x20000000 there's 128KB of DTCM (Data Tightly Coupled
+ * Memory). Technically, we could use this as normal RAM as there's a way for
+ * the DMA to access it, but the datasheet is unclear about performance
+ * penalties for doing so. To avoid nonuniform DMA memory access latencies,
+ * we leave this 128KB DTCM unused except for the first 512Bytes which are for
+ * the interrupt stack. This leaves us with 384KB of RAM
+ */
+MEMORY
+{
+    xram(wx)  : ORIGIN = 0xd0000000, LENGTH =  16M
+    sram(wx)  : ORIGIN = 0x20020000, LENGTH = 384K
+    dtcm(wx)  : ORIGIN = 0x20000000, LENGTH = 128K    /* Used for main stack */
+    bram(rw)  : ORIGIN = 0x40024000, LENGTH =   4K    /* Bakup SRAM */
+    flash(rx) : ORIGIN = 0x08000000, LENGTH =   2M
+}
+
+/* now define the output sections  */
+SECTIONS
+{
+    . = 0;
+    
+    /* .text section: code goes to flash */
+    .text :
+    {
+        /* Startup code must go at address 0 */
+        KEEP(*(.isr_vector))
+        
+        *(.text)
+        *(.text.*)
+        *(.gnu.linkonce.t.*)
+        /* these sections for thumb interwork? */
+        *(.glue_7)
+        *(.glue_7t)
+        /* these sections for C++? */
+        *(.gcc_except_table)
+        *(.gcc_except_table.*)
+        *(.ARM.extab*)
+        *(.gnu.linkonce.armextab.*)
+
+        . = ALIGN(4);
+        /* .rodata: constant data */
+        *(.rodata)
+        *(.rodata.*)
+        *(.gnu.linkonce.r.*)
+
+        /* C++ Static constructors/destructors (eabi) */
+        . = ALIGN(4);
+        KEEP(*(.init))
+        
+        . = ALIGN(4);
+        __miosix_init_array_start = .;
+        KEEP (*(SORT(.miosix_init_array.*)))
+        KEEP (*(.miosix_init_array))
+        __miosix_init_array_end = .;
+
+        . = ALIGN(4);
+        __preinit_array_start = .;
+        KEEP (*(.preinit_array))
+        __preinit_array_end = .;
+
+        . = ALIGN(4);
+        __init_array_start = .;
+        KEEP (*(SORT(.init_array.*)))
+        KEEP (*(.init_array))
+        __init_array_end = .;
+
+        . = ALIGN(4);
+        KEEP(*(.fini))
+
+        . = ALIGN(4);
+        __fini_array_start = .;
+        KEEP (*(.fini_array))
+        KEEP (*(SORT(.fini_array.*)))
+        __fini_array_end = .;
+
+        /* C++ Static constructors/destructors (elf)  */
+        . = ALIGN(4);
+        _ctor_start = .;
+        KEEP (*crtbegin.o(.ctors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .ctors))
+        KEEP (*(SORT(.ctors.*)))
+        KEEP (*crtend.o(.ctors))
+       _ctor_end = .;
+
+        . = ALIGN(4);
+        KEEP (*crtbegin.o(.dtors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .dtors))
+        KEEP (*(SORT(.dtors.*)))
+        KEEP (*crtend.o(.dtors))
+    } > flash
+
+    /* .ARM.exidx is sorted, so has to go in its own output section.  */
+    __exidx_start = .;
+    .ARM.exidx :
+    {
+        *(.ARM.exidx* .gnu.linkonce.armexidx.*)
+    } > flash
+    __exidx_end = .;
+
+	/*
+     * .data section: global variables go to xram, but also store a copy to
+     * flash to initialize them
+     */
+    .data : ALIGN(8)
+    {
+        _data = .;
+        *(.data)
+        *(.data.*)
+        *(.gnu.linkonce.d.*)
+        . = ALIGN(8);
+        _edata = .;
+    } > xram AT > flash
+    _etext = LOADADDR(.data);
+
+    /* .bss section: uninitialized global variables go to xram */
+    _bss_start = .;
+    .bss :
+    {
+        *(.bss)
+        *(.bss.*)
+        *(.gnu.linkonce.b.*)
+        . = ALIGN(8);
+    } > xram
+    _bss_end = .;
+
+    _end = .;
+    PROVIDE(end = .);
+    
+    .preserve(NOLOAD) : ALIGN(4)
+    {
+        _preserve_start = .;
+        . = ALIGN(4);
+        *(.preserve);
+        *(.preserve*);
+        . = ALIGN(4);
+        _preserve_end = .;
+    } > bram
+}
diff --git a/src/bsps/stm32f767zi_automated_antennas/stm32_2m+384k_ram.ld b/src/bsps/stm32f767zi_automated_antennas/stm32_2m+384k_ram.ld
new file mode 100644
index 000000000..497bd5198
--- /dev/null
+++ b/src/bsps/stm32f767zi_automated_antennas/stm32_2m+384k_ram.ld
@@ -0,0 +1,189 @@
+/*
+ * C++ enabled linker script for stm32f767zi (2M FLASH, 512K RAM)
+ * Developed by TFT: Terraneo Federico Technologies
+ * Optimized for use with the Miosix kernel
+ */
+
+/*
+ * This linker script puts:
+ * - read only data and code (.text, .rodata, .eh_*) in FLASH
+ * - the 512Byte main (IRQ) stack, .data and .bss in the DTCM 128KB RAM
+ * - .data, .bss, stacks and heap in the internal RAM.
+ */
+
+/*
+ * The main stack is used for interrupt handling by the kernel.
+ *
+ * *** Readme ***
+ * This linker script places the main stack (used by the kernel for interrupts)
+ * at the bottom of the ram, instead of the top. This is done for two reasons:
+ *
+ * - as an optimization for microcontrollers with little ram memory. In fact
+ *   the implementation of malloc from newlib requests memory to the OS in 4KB
+ *   block (except the first block that can be smaller). This is probably done
+ *   for compatibility with OSes with an MMU and paged memory. To see why this
+ *   is bad, consider a microcontroller with 8KB of ram: when malloc finishes
+ *   up the first 4KB it will call _sbrk_r asking for a 4KB block, but this will
+ *   fail because the top part of the ram is used by the main stack. As a
+ *   result, the top part of the memory will not be used by malloc, even if
+ *   available (and it is nearly *half* the ram on an 8KB mcu). By placing the
+ *   main stack at the bottom of the ram, the upper 4KB block will be entirely
+ *   free and available as heap space.
+ *
+ * - In case of main stack overflow the cpu will fault because access to memory
+ *   before the beginning of the ram faults. Instead with the default stack
+ *   placement the main stack will silently collide with the heap.
+ * Note: if increasing the main stack size also increase the ORIGIN value in
+ * the MEMORY definitions below accordingly.
+ */
+
+_main_stack_size = 512;                             /* main stack = 512Bytes */
+_main_stack_top  = 0x20000000 + _main_stack_size;
+ASSERT(_main_stack_size   % 8 == 0, "MAIN stack size error");
+
+/* Mapping the heap to the end of SRAM2 */
+_heap_end = 0x20000000 + 512K;                       /* end of available ram */
+
+/* Identify the Entry Point  */
+ENTRY(_Z13Reset_Handlerv)
+
+/*
+ * Specify the memory areas
+ *
+ * NOTE: starting at 0x20000000 there's 128KB of DTCM (Data Tightly Coupled
+ * Memory). Technically, we could use this as normal RAM as there's a way for
+ * the DMA to access it, but the datasheet is unclear about performance
+ * penalties for doing so. To avoid nonuniform DMA memory access latencies,
+ * we leave this 128KB DTCM unused except for the first 512Bytes which are for
+ * the interrupt stack. This leaves us with 384KB of RAM
+ */
+MEMORY
+{
+    sram(wx)  : ORIGIN = 0x20020000, LENGTH = 384K
+    dtcm(wx)  : ORIGIN = 0x20000000, LENGTH = 128K    /* Used for main stack */
+    bram(rw)  : ORIGIN = 0x40024000, LENGTH =   4K    /* Bakup SRAM */
+    flash(rx) : ORIGIN = 0x08000000, LENGTH =   2M
+}
+
+/* now define the output sections  */
+SECTIONS
+{
+    . = 0;
+    
+    /* .text section: code goes to flash */
+    .text :
+    {
+        /* Startup code must go at address 0 */
+        KEEP(*(.isr_vector))
+        
+        *(.text)
+        *(.text.*)
+        *(.gnu.linkonce.t.*)
+        /* these sections for thumb interwork? */
+        *(.glue_7)
+        *(.glue_7t)
+        /* these sections for C++? */
+        *(.gcc_except_table)
+        *(.gcc_except_table.*)
+        *(.ARM.extab*)
+        *(.gnu.linkonce.armextab.*)
+
+        . = ALIGN(4);
+        /* .rodata: constant data */
+        *(.rodata)
+        *(.rodata.*)
+        *(.gnu.linkonce.r.*)
+
+        /* C++ Static constructors/destructors (eabi) */
+        . = ALIGN(4);
+        KEEP(*(.init))
+        
+        . = ALIGN(4);
+        __miosix_init_array_start = .;
+        KEEP (*(SORT(.miosix_init_array.*)))
+        KEEP (*(.miosix_init_array))
+        __miosix_init_array_end = .;
+
+        . = ALIGN(4);
+        __preinit_array_start = .;
+        KEEP (*(.preinit_array))
+        __preinit_array_end = .;
+
+        . = ALIGN(4);
+        __init_array_start = .;
+        KEEP (*(SORT(.init_array.*)))
+        KEEP (*(.init_array))
+        __init_array_end = .;
+
+        . = ALIGN(4);
+        KEEP(*(.fini))
+
+        . = ALIGN(4);
+        __fini_array_start = .;
+        KEEP (*(.fini_array))
+        KEEP (*(SORT(.fini_array.*)))
+        __fini_array_end = .;
+
+        /* C++ Static constructors/destructors (elf)  */
+        . = ALIGN(4);
+        _ctor_start = .;
+        KEEP (*crtbegin.o(.ctors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .ctors))
+        KEEP (*(SORT(.ctors.*)))
+        KEEP (*crtend.o(.ctors))
+       _ctor_end = .;
+
+        . = ALIGN(4);
+        KEEP (*crtbegin.o(.dtors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .dtors))
+        KEEP (*(SORT(.dtors.*)))
+        KEEP (*crtend.o(.dtors))
+    } > flash
+
+    /* .ARM.exidx is sorted, so has to go in its own output section.  */
+    __exidx_start = .;
+    .ARM.exidx :
+    {
+        *(.ARM.exidx* .gnu.linkonce.armexidx.*)
+    } > flash
+    __exidx_end = .;
+
+	/*
+     * .data section: global variables go to sram, but also store a copy to
+     * flash to initialize them
+     */
+    .data : ALIGN(8)
+    {
+        _data = .;
+        *(.data)
+        *(.data.*)
+        *(.gnu.linkonce.d.*)
+        . = ALIGN(8);
+        _edata = .;
+    } > sram AT > flash
+    _etext = LOADADDR(.data);
+
+    /* .bss section: uninitialized global variables go to sram */
+    _bss_start = .;
+    .bss :
+    {
+        *(.bss)
+        *(.bss.*)
+        *(.gnu.linkonce.b.*)
+        . = ALIGN(8);
+    } > sram
+    _bss_end = .;
+
+    _end = .;
+    PROVIDE(end = .);
+    
+    .preserve(NOLOAD) : ALIGN(4)
+    {
+        _preserve_start = .;
+        . = ALIGN(4);
+        *(.preserve);
+        *(.preserve*);
+        . = ALIGN(4);
+        _preserve_end = .;
+    } > bram
+}
diff --git a/src/bsps/stm32f767zi_compute_unit/config/board_options.cmake b/src/bsps/stm32f767zi_compute_unit/config/board_options.cmake
new file mode 100644
index 000000000..d0bceacf5
--- /dev/null
+++ b/src/bsps/stm32f767zi_compute_unit/config/board_options.cmake
@@ -0,0 +1,106 @@
+# Copyright (C) 2023 by Skyward
+#
+# This program is free software; you can redistribute it and/or 
+# it under the terms of the GNU General Public License as published 
+# the Free Software Foundation; either version 2 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.
+#
+# As a special exception, if other files instantiate templates or use
+# macros or inline functions from this file, or you compile this file
+# and link it with other works to produce a work based on this file,
+# this file does not by itself cause the resulting work to be covered
+# by the GNU General Public License. However the source code for this
+# file must still be made available in accordance with the GNU 
+# Public License. This exception does not invalidate any other 
+# why a work based on this file might be covered by the GNU General
+# Public License.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, see <http://www.gnu.org/licenses/>
+
+set(BOARD_NAME stm32f767zi_compute_unit)
+set(ARCH_NAME cortexM7_stm32f7)
+
+# Base directories with header files for this board
+set(ARCH_PATH ${KPATH}/arch/${ARCH_NAME}/common)
+set(BOARD_PATH ${BOARDCORE_PATH}/src/bsps/${BOARD_NAME})
+set(BOARD_CONFIG_PATH ${BOARDCORE_PATH}/src/bsps/${BOARD_NAME}/config)
+
+# Specify where to find the board specific config/miosix_settings.h
+set(BOARD_MIOSIX_SETTINGS_PATH ${BOARD_PATH})
+
+# Specify where to find the board specific config/mxgui_settings.h
+set(BOARD_MXGUI_SETTINGS_PATH ${BOARD_PATH})
+
+# Optimization flags:
+# -O0 do no optimization, the default if no optimization level is specified
+# -O or -O1 optimize minimally
+# -O2 optimize more
+# -O3 optimize even more
+# -Ofast optimize very aggressively to the point of breaking the standard
+# -Og Optimize debugging experience, enables optimizations that do not
+# interfere with debugging
+# -Os Optimize for size with -O2 optimizations that do not increase code size
+set(OPT_OPTIMIZATION -O2)
+
+# Boot file and linker script
+set(BOOT_FILE ${BOARD_PATH}/core/stage_1_boot.cpp)
+# set(LINKER_SCRIPT ${BOARD_PATH}/stm32_2m+384k_ram.ld)
+set(LINKER_SCRIPT ${BOARD_PATH}/stm32_2m+16m_xram.ld)
+
+# Enables the initialization of the external 16MB SDRAM memory
+set(XRAM -D__ENABLE_XRAM)
+
+# Select clock frequency (HSE_VALUE is the xtal on board, fixed)
+set(CLOCK_FREQ -DHSE_VALUE=25000000 -DSYSCLK_FREQ_216MHz=216000000)
+
+# C++ Exception/rtti support disable flags.
+# To save code size if not using C++ exceptions (nor some STL code which
+# implicitly uses it) uncomment this option.
+# -D__NO_EXCEPTIONS is used by Miosix to know if exceptions are used.
+# set(OPT_EXCEPT -fno-exceptions -fno-rtti -D__NO_EXCEPTIONS)
+
+# Specify a custom flash command
+# This is the program that is invoked when the flash flag (-f or --flash) is
+# used with the Miosix Build System. Use $binary or $hex as placeolders, they
+# will be replaced by the build systems with the binary or hex file repectively.
+# If a command is not specified, the build system will use st-flash if found
+# set(PROGRAM_CMDLINE "here your custom flash command")
+
+# Basic flags
+set(FLAGS_BASE -mcpu=cortex-m7 -mthumb -mfloat-abi=hard -mfpu=fpv5-d16)
+
+# Flags for ASM and linker
+set(AFLAGS_BASE ${FLAGS_BASE})
+set(LFLAGS_BASE ${FLAGS_BASE} -Wl,--gc-sections,-Map,main.map -Wl,-T${LINKER_SCRIPT} ${OPT_EXCEPT} ${OPT_OPTIMIZATION} -nostdlib)
+
+# Flags for C/C++
+string(TOUPPER ${BOARD_NAME} BOARD_UPPER)
+set(CFLAGS_BASE
+    -D_BOARD_${BOARD_UPPER} -D_MIOSIX_BOARDNAME=\"${BOARD_NAME}\"
+    -D_DEFAULT_SOURCE=1 -ffunction-sections -Wall -Werror=return-type -g
+    -D_ARCH_CORTEXM7_STM32F7
+    ${CLOCK_FREQ} ${XRAM} ${SRAM_BOOT} ${FLAGS_BASE} ${OPT_OPTIMIZATION} -c
+)
+set(CXXFLAGS_BASE ${CFLAGS_BASE} ${OPT_EXCEPT})
+
+# Select architecture specific files
+set(ARCH_SRC
+    ${ARCH_PATH}/interfaces-impl/delays.cpp
+    ${ARCH_PATH}/interfaces-impl/gpio_impl.cpp
+    ${ARCH_PATH}/interfaces-impl/portability.cpp
+    ${BOARD_PATH}/interfaces-impl/bsp.cpp
+    ${KPATH}/arch/common/CMSIS/Device/ST/STM32F7xx/Source/Templates/system_stm32f7xx.c
+    ${KPATH}/arch/common/core/cache_cortexMx.cpp
+    ${KPATH}/arch/common/core/interrupts_cortexMx.cpp
+    ${KPATH}/arch/common/core/mpu_cortexMx.cpp
+    ${KPATH}/arch/common/core/stm32f2_f4_l4_f7_h7_os_timer.cpp
+    ${KPATH}/arch/common/drivers/sd_stm32f2_f4_f7.cpp
+    ${KPATH}/arch/common/drivers/serial_stm32.cpp
+    ${KPATH}/arch/common/drivers/stm32_hardware_rng.cpp
+)
diff --git a/src/bsps/stm32f767zi_compute_unit/config/board_settings.h b/src/bsps/stm32f767zi_compute_unit/config/board_settings.h
new file mode 100644
index 000000000..2332aa30e
--- /dev/null
+++ b/src/bsps/stm32f767zi_compute_unit/config/board_settings.h
@@ -0,0 +1,62 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * 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.
+ */
+
+#pragma once
+
+/**
+ * \internal
+ * Versioning for board_settings.h for out of git tree projects
+ */
+#define BOARD_SETTINGS_VERSION 300
+
+namespace miosix
+{
+
+/**
+ * \addtogroup Settings
+ * \{
+ */
+
+/// Size of stack for main().
+/// The C standard library is stack-heavy (iprintf requires 1KB)
+const unsigned int MAIN_STACK_SIZE = 16 * 1024;
+
+/// Serial port
+const unsigned int defaultSerial      = 1;
+const unsigned int defaultSerialSpeed = 115200;
+#define SERIAL_1_DMA
+// #define SERIAL_2_DMA
+// #define SERIAL_3_DMA
+
+// SD card driver
+static const unsigned char sdVoltage = 33;  // Board powered @ 3.3V
+// #define SD_ONE_BIT_DATABUS
+#define SD_SDMMC 1  // Select either SDMMC1 or SDMMC2
+
+/// Analog supply voltage for ADC, DAC, Reset blocks, RCs and PLL
+#define V_DDA_VOLTAGE 3.3f
+
+/**
+ * \}
+ */
+
+}  // namespace miosix
diff --git a/src/bsps/stm32f767zi_compute_unit/config/miosix_settings.h b/src/bsps/stm32f767zi_compute_unit/config/miosix_settings.h
new file mode 100644
index 000000000..e1ca4aac6
--- /dev/null
+++ b/src/bsps/stm32f767zi_compute_unit/config/miosix_settings.h
@@ -0,0 +1,240 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * 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.
+ */
+
+#pragma once
+
+// Before you can compile the kernel you have to configure it by editing this
+// file. After that, comment out this line to disable the reminder error.
+// The PARSING_FROM_IDE is because Netbeans gets confused by this, it is never
+// defined when compiling the code.
+#ifndef PARSING_FROM_IDE
+//#error This error is a reminder that you have not edited miosix_settings.h
+// yet.
+#endif  // PARSING_FROM_IDE
+
+/**
+ * \file miosix_settings.h
+ * NOTE: this file contains ONLY configuration options that are not dependent
+ * on architecture specific details. The other options are in the following
+ * files which are included here:
+ * miosix/arch/architecture name/common/arch_settings.h
+ * miosix/config/arch/architecture name/board name/board_settings.h
+ */
+#include "arch_settings.h"
+#include "board_settings.h"
+#include "util/version.h"
+
+/**
+ * \internal
+ * Versioning for miosix_settings.h for out of git tree projects
+ */
+#define MIOSIX_SETTINGS_VERSION 300
+
+namespace miosix
+{
+
+/**
+ * \addtogroup Settings
+ * \{
+ */
+
+//
+// Scheduler options
+//
+
+/// \def SCHED_TYPE_PRIORITY
+/// If uncommented selects the priority scheduler
+/// \def SCHED_TYPE_CONTROL_BASED
+/// If uncommented selects the control based scheduler
+/// \def SCHED_TYPE_EDF
+/// If uncommented selects the EDF scheduler
+// Uncomment only *one* of those
+
+#define SCHED_TYPE_PRIORITY
+//#define SCHED_TYPE_CONTROL_BASED
+//#define SCHED_TYPE_EDF
+
+/// \def WITH_CPU_TIME_COUNTER
+/// Allows to enable/disable CPUTimeCounter to save code size and remove its
+/// overhead from the scheduling process. By default it is not defined
+/// (CPUTimeCounter is disabled).
+//#define WITH_CPU_TIME_COUNTER
+
+//
+// Filesystem options
+//
+
+/// \def WITH_FILESYSTEM
+/// Allows to enable/disable filesystem support to save code size
+/// By default it is defined (filesystem support is enabled)
+#define WITH_FILESYSTEM
+
+/// \def WITH_DEVFS
+/// Allows to enable/disable DevFs support to save code size
+/// By default it is defined (DevFs is enabled)
+#define WITH_DEVFS
+
+/// \def SYNC_AFTER_WRITE
+/// Increases filesystem write robustness. After each write operation the
+/// filesystem is synced so that a power failure happens data is not lost
+/// (unless power failure happens exactly between the write and the sync)
+/// Unfortunately write latency and throughput becomes twice as worse
+/// By default it is defined (slow but safe)
+#define SYNC_AFTER_WRITE
+
+/// Maximum number of open files. Trying to open more will fail.
+/// Cannot be lower than 3, as the first three are stdin, stdout, stderr
+const unsigned char MAX_OPEN_FILES = 8;
+
+/// \def WITH_PROCESSES
+/// If uncommented enables support for processes as well as threads.
+/// This enables the dynamic loader to load elf programs, the extended system
+/// call service and, if the hardware supports it, the MPU to provide memory
+/// isolation of processes
+//#define WITH_PROCESSES
+
+#if defined(WITH_PROCESSES) && defined(__NO_EXCEPTIONS)
+#error Processes require C++ exception support
+#endif  // defined(WITH_PROCESSES) && defined(__NO_EXCEPTIONS)
+
+#if defined(WITH_PROCESSES) && !defined(WITH_FILESYSTEM)
+#error Processes require filesystem support
+#endif  // defined(WITH_PROCESSES) && !defined(WITH_FILESYSTEM)
+
+#if defined(WITH_PROCESSES) && !defined(WITH_DEVFS)
+#error Processes require devfs support
+#endif  // defined(WITH_PROCESSES) && !defined(WITH_DEVFS)
+
+//
+// C/C++ standard library I/O (stdin, stdout and stderr related)
+//
+
+/// \def WITH_BOOTLOG
+/// Uncomment to print bootlogs on stdout.
+/// By default it is defined (bootlogs are printed)
+#define WITH_BOOTLOG
+
+/// \def WITH_ERRLOG
+/// Uncomment for debug information on stdout.
+/// By default it is defined (error information is printed)
+#define WITH_ERRLOG
+
+//
+// Kernel related options (stack sizes, priorities)
+//
+
+/// \def WITH_DEEP_SLEEP
+/// Adds interfaces and required variables to support deep sleep state switch
+/// automatically when peripherals are not required
+//#define WITH_DEEP_SLEEP
+
+/**
+ * \def JTAG_DISABLE_SLEEP
+ * JTAG debuggers lose communication with the device if it enters sleep
+ * mode, so to use debugging it is necessary to disable sleep in the idle
+ * thread. By default it is not defined (idle thread calls sleep).
+ */
+//#define JTAG_DISABLE_SLEEP
+
+#if defined(WITH_DEEP_SLEEP) && defined(JTAG_DISABLE_SLEEP)
+#error Deep sleep cannot work together with jtag
+#endif  // defined(WITH_PROCESSES) && !defined(WITH_DEVFS)
+
+/// Minimum stack size (MUST be divisible by 4)
+const unsigned int STACK_MIN = 256;
+
+/// \internal Size of idle thread stack.
+/// Should be >=STACK_MIN (MUST be divisible by 4)
+const unsigned int STACK_IDLE = 256;
+
+/// Default stack size for pthread_create.
+/// The chosen value is enough to call C standard library functions
+/// such as printf/fopen which are stack-heavy
+const unsigned int STACK_DEFAULT_FOR_PTHREAD = 2048;
+
+/// Maximum size of the RAM image of a process. If a program requires more
+/// the kernel will not run it (MUST be divisible by 4)
+const unsigned int MAX_PROCESS_IMAGE_SIZE = 64 * 1024;
+
+/// Minimum size of the stack for a process. If a program specifies a lower
+/// size the kernel will not run it (MUST be divisible by 4)
+const unsigned int MIN_PROCESS_STACK_SIZE = STACK_MIN;
+
+/// Every userspace thread has two stacks, one for when it is running in
+/// userspace and one for when it is running in kernelspace (that is, while it
+/// is executing system calls). This is the size of the stack for when the
+/// thread is running in kernelspace (MUST be divisible by 4)
+const unsigned int SYSTEM_MODE_PROCESS_STACK_SIZE = 2 * 1024;
+
+/// Number of priorities (MUST be >1)
+/// PRIORITY_MAX-1 is the highest priority, 0 is the lowest. -1 is reserved as
+/// the priority of the idle thread.
+/// The meaning of a thread's priority depends on the chosen scheduler.
+#ifdef SCHED_TYPE_PRIORITY
+// Can be modified, but a high value makes context switches more expensive
+const short int PRIORITY_MAX = 4;
+#elif defined(SCHED_TYPE_CONTROL_BASED)
+// Don't touch, the limit is due to the fixed point implementation
+// It's not needed for if floating point is selected, but kept for consistency
+const short int PRIORITY_MAX = 64;
+#else  // SCHED_TYPE_EDF
+// Doesn't exist for this kind of scheduler
+#endif
+
+/// Priority of main()
+/// The meaning of a thread's priority depends on the chosen scheduler.
+const unsigned char MAIN_PRIORITY = 1;
+
+#ifdef SCHED_TYPE_PRIORITY
+/// Maximum thread time slice in nanoseconds, after which preemption occurs
+const unsigned int MAX_TIME_SLICE = 1000000;
+#endif  // SCHED_TYPE_PRIORITY
+
+//
+// Other low level kernel options. There is usually no need to modify these.
+//
+
+/// \internal Length of wartermark (in bytes) to check stack overflow.
+/// MUST be divisible by 4 and can also be zero.
+/// A high value increases context switch time.
+const unsigned int WATERMARK_LEN = 16;
+
+/// \internal Used to fill watermark
+const unsigned int WATERMARK_FILL = 0xaaaaaaaa;
+
+/// \internal Used to fill stack (for checking stack usage)
+const unsigned int STACK_FILL = 0xbbbbbbbb;
+
+// Compiler version checks
+#if !defined(_MIOSIX_GCC_PATCH_MAJOR) || _MIOSIX_GCC_PATCH_MAJOR < 3
+#error \
+    "You are using a too old or unsupported compiler. Get the latest one from https://miosix.org/wiki/index.php?title=Miosix_Toolchain"
+#endif
+#if _MIOSIX_GCC_PATCH_MAJOR > 3
+#warning "You are using a too new compiler, which may not be supported"
+#endif
+
+/**
+ * \}
+ */
+
+}  // namespace miosix
diff --git a/src/bsps/stm32f767zi_compute_unit/core/stage_1_boot.cpp b/src/bsps/stm32f767zi_compute_unit/core/stage_1_boot.cpp
new file mode 100644
index 000000000..83eac53b4
--- /dev/null
+++ b/src/bsps/stm32f767zi_compute_unit/core/stage_1_boot.cpp
@@ -0,0 +1,505 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * 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 <string.h>
+
+#include "core/cache_cortexMx.h"
+#include "core/interrupts.h"  //For the unexpected interrupt call
+#include "core/interrupts_cortexMx.h"
+#include "interfaces/arch_registers.h"
+#include "interfaces/bsp.h"
+#include "kernel/stage_2_boot.h"
+
+/*
+ * startup.cpp
+ * STM32 C++ startup.
+ * NOTE: for stm32f767 devices ONLY.
+ * Supports interrupt handlers in C++ without extern "C"
+ * Developed by Terraneo Federico, based on ST startup code.
+ * Additionally modified to boot Miosix.
+ */
+
+/**
+ * Called by Reset_Handler, performs initialization and calls main.
+ * Never returns.
+ */
+void program_startup() __attribute__((noreturn));
+void program_startup()
+{
+    // Cortex M7 core appears to get out of reset with interrupts already
+    // enabled
+    __disable_irq();
+
+    miosix::IRQconfigureCache((const unsigned int *)0xd0000000,
+                              8 * 1024 * 1024);
+
+    // These are defined in the linker script
+    extern unsigned char _etext asm("_etext");
+    extern unsigned char _data asm("_data");
+    extern unsigned char _edata asm("_edata");
+    extern unsigned char _bss_start asm("_bss_start");
+    extern unsigned char _bss_end asm("_bss_end");
+
+    // Initialize .data section, clear .bss section
+    unsigned char *etext     = &_etext;
+    unsigned char *data      = &_data;
+    unsigned char *edata     = &_edata;
+    unsigned char *bss_start = &_bss_start;
+    unsigned char *bss_end   = &_bss_end;
+    memcpy(data, etext, edata - data);
+    memset(bss_start, 0, bss_end - bss_start);
+
+    // Move on to stage 2
+    _init();
+
+    // If main returns, reboot
+    NVIC_SystemReset();
+    for (;;)
+        ;
+}
+
+/**
+ * Reset handler, called by hardware immediately after reset
+ */
+void Reset_Handler() __attribute__((__interrupt__, noreturn));
+void Reset_Handler()
+{
+    /**
+     * SystemInit() is called *before* initializing .data and zeroing .bss
+     * Despite all startup files provided by ST do the opposite, there are three
+     * good reasons to do so:
+     * 1. First, the CMSIS specifications say that SystemInit() must not access
+     *    global variables, so it is actually possible to call it before
+     * 2. Second, when running Miosix with the xram linker scripts .data and
+     *    .bss are placed in the external RAM, so we *must* call SystemInit(),
+     *    which enables xram, before touching .data and .bss
+     * 3. Third, this is a performance improvement since the loops that
+     *    initialize .data and zeros .bss now run with the CPU at full speed
+     *    instead of 8MHz
+     */
+    SystemInit();
+
+/**
+ * ST does not provide code to initialize the SDRAM at boot.
+ * Put after SystemInit() as SDRAM is timing-sensitive and needs the full
+ * clock speed.
+ */
+#ifdef __ENABLE_XRAM
+    miosix::configureSdram();
+#endif  //__ENABLE_XRAM
+
+    /*
+     * Load into the program stack pointer the heap end address and switch from
+     * the msp to sps.
+     * This is required for booting Miosix, a small portion of the top of the
+     * heap area will be used as stack until the first thread starts. After,
+     * this stack will be abandoned and the process stack will point to the
+     * current thread's stack.
+     */
+    asm volatile(
+        "ldr r0,  =_heap_end          \n\t"
+        "msr psp, r0                  \n\t"
+        "movw r0, #2                  \n\n"  // Set the control register to use
+        "msr control, r0              \n\t"  // the process stack
+        "isb                          \n\t" ::
+            : "r0");
+
+    program_startup();
+}
+
+/**
+ * All unused interrupts call this function.
+ */
+extern "C" void Default_Handler() { unexpectedInterrupt(); }
+
+// System handlers
+void /*__attribute__((weak))*/ Reset_Handler();      // These interrupts are not
+void /*__attribute__((weak))*/ NMI_Handler();        // weak because they are
+void /*__attribute__((weak))*/ HardFault_Handler();  // surely defined by Miosix
+void /*__attribute__((weak))*/ MemManage_Handler();
+void /*__attribute__((weak))*/ BusFault_Handler();
+void /*__attribute__((weak))*/ UsageFault_Handler();
+void /*__attribute__((weak))*/ SVC_Handler();
+void /*__attribute__((weak))*/ DebugMon_Handler();
+void /*__attribute__((weak))*/ PendSV_Handler();
+void __attribute__((weak)) SysTick_Handler();
+
+// Interrupt handlers
+void __attribute__((weak)) WWDG_IRQHandler();
+void __attribute__((weak)) PVD_IRQHandler();
+void __attribute__((weak)) TAMP_STAMP_IRQHandler();
+void __attribute__((weak)) RTC_WKUP_IRQHandler();
+void __attribute__((weak)) FLASH_IRQHandler();
+void __attribute__((weak)) RCC_IRQHandler();
+void __attribute__((weak)) EXTI0_IRQHandler();
+void __attribute__((weak)) EXTI1_IRQHandler();
+void __attribute__((weak)) EXTI2_IRQHandler();
+void __attribute__((weak)) EXTI3_IRQHandler();
+void __attribute__((weak)) EXTI4_IRQHandler();
+void __attribute__((weak)) DMA1_Stream0_IRQHandler();
+void __attribute__((weak)) DMA1_Stream1_IRQHandler();
+void __attribute__((weak)) DMA1_Stream2_IRQHandler();
+void __attribute__((weak)) DMA1_Stream3_IRQHandler();
+void __attribute__((weak)) DMA1_Stream4_IRQHandler();
+void __attribute__((weak)) DMA1_Stream5_IRQHandler();
+void __attribute__((weak)) DMA1_Stream6_IRQHandler();
+void __attribute__((weak)) ADC_IRQHandler();
+void __attribute__((weak)) CAN1_TX_IRQHandler();
+void __attribute__((weak)) CAN1_RX0_IRQHandler();
+void __attribute__((weak)) CAN1_RX1_IRQHandler();
+void __attribute__((weak)) CAN1_SCE_IRQHandler();
+void __attribute__((weak)) EXTI9_5_IRQHandler();
+void __attribute__((weak)) TIM1_BRK_TIM9_IRQHandler();
+void __attribute__((weak)) TIM1_UP_TIM10_IRQHandler();
+void __attribute__((weak)) TIM1_TRG_COM_TIM11_IRQHandler();
+void __attribute__((weak)) TIM1_CC_IRQHandler();
+void __attribute__((weak)) TIM2_IRQHandler();
+void __attribute__((weak)) TIM3_IRQHandler();
+void __attribute__((weak)) TIM4_IRQHandler();
+void __attribute__((weak)) I2C1_EV_IRQHandler();
+void __attribute__((weak)) I2C1_ER_IRQHandler();
+void __attribute__((weak)) I2C2_EV_IRQHandler();
+void __attribute__((weak)) I2C2_ER_IRQHandler();
+void __attribute__((weak)) SPI1_IRQHandler();
+void __attribute__((weak)) SPI2_IRQHandler();
+void __attribute__((weak)) USART1_IRQHandler();
+void __attribute__((weak)) USART2_IRQHandler();
+void __attribute__((weak)) USART3_IRQHandler();
+void __attribute__((weak)) EXTI15_10_IRQHandler();
+void __attribute__((weak)) RTC_Alarm_IRQHandler();
+void __attribute__((weak)) OTG_FS_WKUP_IRQHandler();
+void __attribute__((weak)) TIM8_BRK_TIM12_IRQHandler();
+void __attribute__((weak)) TIM8_UP_TIM13_IRQHandler();
+void __attribute__((weak)) TIM8_TRG_COM_TIM14_IRQHandler();
+void __attribute__((weak)) TIM8_CC_IRQHandler();
+void __attribute__((weak)) DMA1_Stream7_IRQHandler();
+void __attribute__((weak)) FMC_IRQHandler();
+void __attribute__((weak)) SDMMC1_IRQHandler();
+void __attribute__((weak)) TIM5_IRQHandler();
+void __attribute__((weak)) SPI3_IRQHandler();
+void __attribute__((weak)) UART4_IRQHandler();
+void __attribute__((weak)) UART5_IRQHandler();
+void __attribute__((weak)) TIM6_DAC_IRQHandler();
+void __attribute__((weak)) TIM7_IRQHandler();
+void __attribute__((weak)) DMA2_Stream0_IRQHandler();
+void __attribute__((weak)) DMA2_Stream1_IRQHandler();
+void __attribute__((weak)) DMA2_Stream2_IRQHandler();
+void __attribute__((weak)) DMA2_Stream3_IRQHandler();
+void __attribute__((weak)) DMA2_Stream4_IRQHandler();
+void __attribute__((weak)) ETH_IRQHandler();
+void __attribute__((weak)) ETH_WKUP_IRQHandler();
+void __attribute__((weak)) CAN2_TX_IRQHandler();
+void __attribute__((weak)) CAN2_RX0_IRQHandler();
+void __attribute__((weak)) CAN2_RX1_IRQHandler();
+void __attribute__((weak)) CAN2_SCE_IRQHandler();
+void __attribute__((weak)) OTG_FS_IRQHandler();
+void __attribute__((weak)) DMA2_Stream5_IRQHandler();
+void __attribute__((weak)) DMA2_Stream6_IRQHandler();
+void __attribute__((weak)) DMA2_Stream7_IRQHandler();
+void __attribute__((weak)) USART6_IRQHandler();
+void __attribute__((weak)) I2C3_EV_IRQHandler();
+void __attribute__((weak)) I2C3_ER_IRQHandler();
+void __attribute__((weak)) OTG_HS_EP1_OUT_IRQHandler();
+void __attribute__((weak)) OTG_HS_EP1_IN_IRQHandler();
+void __attribute__((weak)) OTG_HS_WKUP_IRQHandler();
+void __attribute__((weak)) OTG_HS_IRQHandler();
+void __attribute__((weak)) DCMI_IRQHandler();
+void __attribute__((weak)) CRYP_IRQHandler();
+void __attribute__((weak)) RNG_IRQHandler();
+void __attribute__((weak)) FPU_IRQHandler();
+void __attribute__((weak)) UART7_IRQHandler();
+void __attribute__((weak)) UART8_IRQHandler();
+void __attribute__((weak)) SPI4_IRQHandler();
+void __attribute__((weak)) SPI5_IRQHandler();
+void __attribute__((weak)) SPI6_IRQHandler();
+void __attribute__((weak)) SAI1_IRQHandler();
+void __attribute__((weak)) LTDC_IRQHandler();
+void __attribute__((weak)) LTDC_ER_IRQHandler();
+void __attribute__((weak)) DMA2D_IRQHandler();
+void __attribute__((weak)) SAI2_IRQHandler();
+void __attribute__((weak)) QUADSPI_IRQHandler();
+void __attribute__((weak)) LPTIM1_IRQHandler();
+void __attribute__((weak)) CEC_IRQHandler();
+void __attribute__((weak)) I2C4_EV_IRQHandler();
+void __attribute__((weak)) I2C4_ER_IRQHandler();
+void __attribute__((weak)) SPDIF_RX_IRQHandler();
+void __attribute__((weak)) DSIHOST_IRQHandler();
+void __attribute__((weak)) DFSDM1_FLT0_IRQHandler();
+void __attribute__((weak)) DFSDM1_FLT1_IRQHandler();
+void __attribute__((weak)) DFSDM1_FLT2_IRQHandler();
+void __attribute__((weak)) DFSDM1_FLT3_IRQHandler();
+void __attribute__((weak)) SDMMC2_IRQHandler();
+void __attribute__((weak)) CAN3_TX_IRQHandler();
+void __attribute__((weak)) CAN3_RX0_IRQHandler();
+void __attribute__((weak)) CAN3_RX1_IRQHandler();
+void __attribute__((weak)) CAN3_SCE_IRQHandler();
+void __attribute__((weak)) JPEG_IRQHandler();
+void __attribute__((weak)) MDIOS_IRQHandler();
+
+// Stack top, defined in the linker script
+extern char _main_stack_top asm("_main_stack_top");
+
+// Interrupt vectors, must be placed @ address 0x00000000
+// The extern declaration is required otherwise g++ optimizes it out
+extern void (*const __Vectors[])();
+void (*const __Vectors[])() __attribute__((section(".isr_vector"))) = {
+    reinterpret_cast<void (*)()>(&_main_stack_top), /* Stack pointer*/
+    Reset_Handler,                                  /* Reset Handler */
+    NMI_Handler,                                    /* NMI Handler */
+    HardFault_Handler,                              /* Hard Fault Handler */
+    MemManage_Handler,                              /* MPU Fault Handler */
+    BusFault_Handler,                               /* Bus Fault Handler */
+    UsageFault_Handler,                             /* Usage Fault Handler */
+    0,                                              /* Reserved */
+    0,                                              /* Reserved */
+    0,                                              /* Reserved */
+    0,                                              /* Reserved */
+    SVC_Handler,                                    /* SVCall Handler */
+    DebugMon_Handler,                               /* Debug Monitor Handler */
+    0,                                              /* Reserved */
+    PendSV_Handler,                                 /* PendSV Handler */
+    SysTick_Handler,                                /* SysTick Handler */
+
+    /* External Interrupts */
+    WWDG_IRQHandler,
+    PVD_IRQHandler,
+    TAMP_STAMP_IRQHandler,
+    RTC_WKUP_IRQHandler,
+    FLASH_IRQHandler,
+    RCC_IRQHandler,
+    EXTI0_IRQHandler,
+    EXTI1_IRQHandler,
+    EXTI2_IRQHandler,
+    EXTI3_IRQHandler,
+    EXTI4_IRQHandler,
+    DMA1_Stream0_IRQHandler,
+    DMA1_Stream1_IRQHandler,
+    DMA1_Stream2_IRQHandler,
+    DMA1_Stream3_IRQHandler,
+    DMA1_Stream4_IRQHandler,
+    DMA1_Stream5_IRQHandler,
+    DMA1_Stream6_IRQHandler,
+    ADC_IRQHandler,
+    CAN1_TX_IRQHandler,
+    CAN1_RX0_IRQHandler,
+    CAN1_RX1_IRQHandler,
+    CAN1_SCE_IRQHandler,
+    EXTI9_5_IRQHandler,
+    TIM1_BRK_TIM9_IRQHandler,
+    TIM1_UP_TIM10_IRQHandler,
+    TIM1_TRG_COM_TIM11_IRQHandler,
+    TIM1_CC_IRQHandler,
+    TIM2_IRQHandler,
+    TIM3_IRQHandler,
+    TIM4_IRQHandler,
+    I2C1_EV_IRQHandler,
+    I2C1_ER_IRQHandler,
+    I2C2_EV_IRQHandler,
+    I2C2_ER_IRQHandler,
+    SPI1_IRQHandler,
+    SPI2_IRQHandler,
+    USART1_IRQHandler,
+    USART2_IRQHandler,
+    USART3_IRQHandler,
+    EXTI15_10_IRQHandler,
+    RTC_Alarm_IRQHandler,
+    OTG_FS_WKUP_IRQHandler,
+    TIM8_BRK_TIM12_IRQHandler,
+    TIM8_UP_TIM13_IRQHandler,
+    TIM8_TRG_COM_TIM14_IRQHandler,
+    TIM8_CC_IRQHandler,
+    DMA1_Stream7_IRQHandler,
+    FMC_IRQHandler,
+    SDMMC1_IRQHandler,
+    TIM5_IRQHandler,
+    SPI3_IRQHandler,
+    UART4_IRQHandler,
+    UART5_IRQHandler,
+    TIM6_DAC_IRQHandler,
+    TIM7_IRQHandler,
+    DMA2_Stream0_IRQHandler,
+    DMA2_Stream1_IRQHandler,
+    DMA2_Stream2_IRQHandler,
+    DMA2_Stream3_IRQHandler,
+    DMA2_Stream4_IRQHandler,
+    ETH_IRQHandler,
+    ETH_WKUP_IRQHandler,
+    CAN2_TX_IRQHandler,
+    CAN2_RX0_IRQHandler,
+    CAN2_RX1_IRQHandler,
+    CAN2_SCE_IRQHandler,
+    OTG_FS_IRQHandler,
+    DMA2_Stream5_IRQHandler,
+    DMA2_Stream6_IRQHandler,
+    DMA2_Stream7_IRQHandler,
+    USART6_IRQHandler,
+    I2C3_EV_IRQHandler,
+    I2C3_ER_IRQHandler,
+    OTG_HS_EP1_OUT_IRQHandler,
+    OTG_HS_EP1_IN_IRQHandler,
+    OTG_HS_WKUP_IRQHandler,
+    OTG_HS_IRQHandler,
+    DCMI_IRQHandler,
+    CRYP_IRQHandler,
+    RNG_IRQHandler,
+    FPU_IRQHandler,
+    UART7_IRQHandler,
+    UART8_IRQHandler,
+    SPI4_IRQHandler,
+    SPI5_IRQHandler,
+    SPI6_IRQHandler,
+    SAI1_IRQHandler,
+    LTDC_IRQHandler,
+    LTDC_ER_IRQHandler,
+    DMA2D_IRQHandler,
+    SAI2_IRQHandler,
+    QUADSPI_IRQHandler,
+    LPTIM1_IRQHandler,
+    CEC_IRQHandler,
+    I2C4_EV_IRQHandler,
+    I2C4_ER_IRQHandler,
+    SPDIF_RX_IRQHandler,
+    DSIHOST_IRQHandler,
+    DFSDM1_FLT0_IRQHandler,
+    DFSDM1_FLT1_IRQHandler,
+    DFSDM1_FLT2_IRQHandler,
+    DFSDM1_FLT3_IRQHandler,
+    SDMMC2_IRQHandler,
+    CAN3_TX_IRQHandler,
+    CAN3_RX0_IRQHandler,
+    CAN3_RX1_IRQHandler,
+    CAN3_SCE_IRQHandler,
+    JPEG_IRQHandler,
+    MDIOS_IRQHandler,
+};
+
+#pragma weak SysTick_IRQHandler            = Default_Handler
+#pragma weak WWDG_IRQHandler               = Default_Handler
+#pragma weak PVD_IRQHandler                = Default_Handler
+#pragma weak TAMP_STAMP_IRQHandler         = Default_Handler
+#pragma weak RTC_WKUP_IRQHandler           = Default_Handler
+#pragma weak FLASH_IRQHandler              = Default_Handler
+#pragma weak RCC_IRQHandler                = Default_Handler
+#pragma weak EXTI0_IRQHandler              = Default_Handler
+#pragma weak EXTI1_IRQHandler              = Default_Handler
+#pragma weak EXTI2_IRQHandler              = Default_Handler
+#pragma weak EXTI3_IRQHandler              = Default_Handler
+#pragma weak EXTI4_IRQHandler              = Default_Handler
+#pragma weak DMA1_Stream0_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream1_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream2_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream3_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream4_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream5_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream6_IRQHandler       = Default_Handler
+#pragma weak ADC_IRQHandler                = Default_Handler
+#pragma weak CAN1_TX_IRQHandler            = Default_Handler
+#pragma weak CAN1_RX0_IRQHandler           = Default_Handler
+#pragma weak CAN1_RX1_IRQHandler           = Default_Handler
+#pragma weak CAN1_SCE_IRQHandler           = Default_Handler
+#pragma weak EXTI9_5_IRQHandler            = Default_Handler
+#pragma weak TIM1_BRK_TIM9_IRQHandler      = Default_Handler
+#pragma weak TIM1_UP_TIM10_IRQHandler      = Default_Handler
+#pragma weak TIM1_TRG_COM_TIM11_IRQHandler = Default_Handler
+#pragma weak TIM1_CC_IRQHandler            = Default_Handler
+#pragma weak TIM2_IRQHandler               = Default_Handler
+#pragma weak TIM3_IRQHandler               = Default_Handler
+#pragma weak TIM4_IRQHandler               = Default_Handler
+#pragma weak I2C1_EV_IRQHandler            = Default_Handler
+#pragma weak I2C1_ER_IRQHandler            = Default_Handler
+#pragma weak I2C2_EV_IRQHandler            = Default_Handler
+#pragma weak I2C2_ER_IRQHandler            = Default_Handler
+#pragma weak SPI1_IRQHandler               = Default_Handler
+#pragma weak SPI2_IRQHandler               = Default_Handler
+// #pragma weak USART1_IRQHandler             = Default_Handler
+// #pragma weak USART2_IRQHandler             = Default_Handler
+// #pragma weak USART3_IRQHandler             = Default_Handler
+#pragma weak EXTI15_10_IRQHandler          = Default_Handler
+#pragma weak RTC_Alarm_IRQHandler          = Default_Handler
+#pragma weak OTG_FS_WKUP_IRQHandler        = Default_Handler
+#pragma weak TIM8_BRK_TIM12_IRQHandler     = Default_Handler
+#pragma weak TIM8_UP_TIM13_IRQHandler      = Default_Handler
+#pragma weak TIM8_TRG_COM_TIM14_IRQHandler = Default_Handler
+#pragma weak TIM8_CC_IRQHandler            = Default_Handler
+#pragma weak DMA1_Stream7_IRQHandler       = Default_Handler
+#pragma weak FMC_IRQHandler                = Default_Handler
+#pragma weak SDMMC1_IRQHandler             = Default_Handler
+#pragma weak TIM5_IRQHandler               = Default_Handler
+#pragma weak SPI3_IRQHandler               = Default_Handler
+// #pragma weak UART4_IRQHandler              = Default_Handler
+// #pragma weak UART5_IRQHandler              = Default_Handler
+#pragma weak TIM6_DAC_IRQHandler     = Default_Handler
+#pragma weak TIM7_IRQHandler         = Default_Handler
+#pragma weak DMA2_Stream0_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream1_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream2_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream3_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream4_IRQHandler = Default_Handler
+#pragma weak ETH_IRQHandler          = Default_Handler
+#pragma weak ETH_WKUP_IRQHandler     = Default_Handler
+#pragma weak CAN2_TX_IRQHandler      = Default_Handler
+#pragma weak CAN2_RX0_IRQHandler     = Default_Handler
+#pragma weak CAN2_RX1_IRQHandler     = Default_Handler
+#pragma weak CAN2_SCE_IRQHandler     = Default_Handler
+#pragma weak OTG_FS_IRQHandler       = Default_Handler
+#pragma weak DMA2_Stream5_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream6_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream7_IRQHandler = Default_Handler
+// #pragma weak USART6_IRQHandler             = Default_Handler
+#pragma weak I2C3_EV_IRQHandler        = Default_Handler
+#pragma weak I2C3_ER_IRQHandler        = Default_Handler
+#pragma weak OTG_HS_EP1_OUT_IRQHandler = Default_Handler
+#pragma weak OTG_HS_EP1_IN_IRQHandler  = Default_Handler
+#pragma weak OTG_HS_WKUP_IRQHandler    = Default_Handler
+#pragma weak OTG_HS_IRQHandler         = Default_Handler
+#pragma weak DCMI_IRQHandler           = Default_Handler
+#pragma weak CRYP_IRQHandler           = Default_Handler
+#pragma weak RNG_IRQHandler            = Default_Handler
+#pragma weak FPU_IRQHandler            = Default_Handler
+// #pragma weak UART7_IRQHandler              = Default_Handler
+// #pragma weak UART8_IRQHandler              = Default_Handler
+#pragma weak SPI4_IRQHandler        = Default_Handler
+#pragma weak SPI5_IRQHandler        = Default_Handler
+#pragma weak SPI6_IRQHandler        = Default_Handler
+#pragma weak SAI1_IRQHandler        = Default_Handler
+#pragma weak LTDC_IRQHandler        = Default_Handler
+#pragma weak LTDC_ER_IRQHandler     = Default_Handler
+#pragma weak DMA2D_IRQHandler       = Default_Handler
+#pragma weak SAI2_IRQHandler        = Default_Handler
+#pragma weak QUADSPI_IRQHandler     = Default_Handler
+#pragma weak LPTIM1_IRQHandler      = Default_Handler
+#pragma weak CEC_IRQHandler         = Default_Handler
+#pragma weak I2C4_EV_IRQHandler     = Default_Handler
+#pragma weak I2C4_ER_IRQHandler     = Default_Handler
+#pragma weak SPDIF_RX_IRQHandler    = Default_Handler
+#pragma weak DSIHOST_IRQHandler     = Default_Handler
+#pragma weak DFSDM1_FLT0_IRQHandler = Default_Handler
+#pragma weak DFSDM1_FLT1_IRQHandler = Default_Handler
+#pragma weak DFSDM1_FLT2_IRQHandler = Default_Handler
+#pragma weak DFSDM1_FLT3_IRQHandler = Default_Handler
+#pragma weak SDMMC2_IRQHandler      = Default_Handler
+#pragma weak CAN3_TX_IRQHandler     = Default_Handler
+#pragma weak CAN3_RX0_IRQHandler    = Default_Handler
+#pragma weak CAN3_RX1_IRQHandler    = Default_Handler
+#pragma weak CAN3_SCE_IRQHandler    = Default_Handler
+#pragma weak JPEG_IRQHandler        = Default_Handler
+#pragma weak MDIOS_IRQHandler       = Default_Handler
diff --git a/src/bsps/stm32f767zi_compute_unit/interfaces-impl/arch_registers_impl.h b/src/bsps/stm32f767zi_compute_unit/interfaces-impl/arch_registers_impl.h
new file mode 100644
index 000000000..3224edc9e
--- /dev/null
+++ b/src/bsps/stm32f767zi_compute_unit/interfaces-impl/arch_registers_impl.h
@@ -0,0 +1,39 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * 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.
+ */
+
+#ifndef ARCH_REGISTERS_IMPL_H
+#define ARCH_REGISTERS_IMPL_H
+
+// stm32f7xx.h defines a few macros like __ICACHE_PRESENT, __DCACHE_PRESENT and
+// includes core_cm7.h. Do not include core_cm7.h before.
+#define STM32F767xx
+#include "CMSIS/Device/ST/STM32F7xx/Include/stm32f7xx.h"
+
+#if (__ICACHE_PRESENT != 1) || (__DCACHE_PRESENT != 1)
+#error "Wrong include order"
+#endif
+
+#include "CMSIS/Device/ST/STM32F7xx/Include/system_stm32f7xx.h"
+
+#define RCC_SYNC() __DSB()  // TODO: can this dsb be removed?
+
+#endif  // ARCH_REGISTERS_IMPL_H
diff --git a/src/bsps/stm32f767zi_compute_unit/interfaces-impl/bsp.cpp b/src/bsps/stm32f767zi_compute_unit/interfaces-impl/bsp.cpp
new file mode 100644
index 000000000..c1f5a0d80
--- /dev/null
+++ b/src/bsps/stm32f767zi_compute_unit/interfaces-impl/bsp.cpp
@@ -0,0 +1,302 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * 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.
+ */
+
+/***********************************************************************
+ * bsp.cpp Part of the Miosix Embedded OS.
+ * Board support package, this file initializes hardware.
+ ************************************************************************/
+
+#include "interfaces/bsp.h"
+
+#include <inttypes.h>
+#include <sys/ioctl.h>
+
+#include <cstdlib>
+
+#include "board_settings.h"
+#include "config/miosix_settings.h"
+#include "drivers/sd_stm32f2_f4_f7.h"
+#include "drivers/serial.h"
+#include "drivers/serial_stm32.h"
+#include "drivers/stm32_sgm.h"
+#include "filesystem/console/console_device.h"
+#include "filesystem/file_access.h"
+#include "interfaces/arch_registers.h"
+#include "interfaces/delays.h"
+#include "interfaces/portability.h"
+#include "kernel/kernel.h"
+#include "kernel/logging.h"
+#include "kernel/sync.h"
+
+namespace miosix
+{
+
+//
+// Initialization
+//
+
+static void sdramCommandWait()
+{
+    for (int i = 0; i < 0xffff; i++)
+        if ((FMC_Bank5_6->SDSR & FMC_SDSR_BUSY) == 0)
+            return;
+}
+
+void configureSdram()
+{
+    // Enable gpios used by the ram
+    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN | RCC_AHB1ENR_GPIOCEN |
+                    RCC_AHB1ENR_GPIODEN | RCC_AHB1ENR_GPIOEEN |
+                    RCC_AHB1ENR_GPIOFEN | RCC_AHB1ENR_GPIOGEN;
+    RCC_SYNC();
+
+    // On the compute unit with F767ZI, the SDRAM pins are:
+    // - PG8:  FMC_SDCLK  (sdram clock)
+    // - PB5:  FMC_SDCKE1 (sdram bank 2 clock enable)
+    // - PB6:  FMC_SDNE1  (sdram bank 2 chip enable)
+    // - PF0:  FMC_A0
+    // - PF1:  FMC_A1
+    // - PF2:  FMC_A2
+    // - PF3:  FMC_A3
+    // - PF4:  FMC_A4
+    // - PF5:  FMC_A5
+    // - PF12: FMC_A6
+    // - PF13: FMC_A7
+    // - PF14: FMC_A8
+    // - PF15: FMC_A9
+    // - PG0:  FMC_A10
+    // - PG1:  FMC_A11
+    // - PG2:  FMC_A12    (used only by the 32MB ram, not by the 8MB one)
+    // - PD14: FMC_D0
+    // - PD15: FMC_D1
+    // - PD0:  FMC_D2
+    // - PD1:  FMC_D3
+    // - PE7:  FMC_D4
+    // - PE8:  FMC_D5
+    // - PE9:  FMC_D6
+    // - PE10: FMC_D7
+    // - PE11: FMC_D8
+    // - PE12: FMC_D9
+    // - PE13: FMC_D10
+    // - PE14: FMC_D11
+    // - PE15: FMC_D12
+    // - PD8:  FMC_D13
+    // - PD9:  FMC_D14
+    // - PD10: FMC_D15
+
+    // - PG4:  FMC_BA0
+    // - PG5:  FMC_BA1
+    // - PF11: FMC_SDNRAS
+    // - PG15: FMC_SDNCAS
+    // - PC0:  FMC_SDNWE
+    // - PE0:  FMC_NBL0
+    // - PE1:  FMC_NBL1
+
+    // All SDRAM GPIOs needs to be configured with alternate function 12 and
+    // maximum speed
+
+    // WARNING: The current configuration is for the 8MB ram
+
+    // Alternate functions
+    GPIOB->AFR[0] = 0x0cc00000;
+    GPIOC->AFR[0] = 0x0000000c;
+    GPIOD->AFR[0] = 0x000000cc;
+    GPIOD->AFR[1] = 0xcc000ccc;
+    GPIOE->AFR[0] = 0xc00000cc;
+    GPIOE->AFR[1] = 0xcccccccc;
+    GPIOF->AFR[0] = 0x00cccccc;
+    GPIOF->AFR[1] = 0xccccc000;
+    GPIOG->AFR[0] = 0x00cc0ccc;
+    GPIOG->AFR[1] = 0xc000000c;
+
+    // Mode
+    GPIOB->MODER = 0x00002800;
+    GPIOC->MODER = 0x00000002;
+    GPIOD->MODER = 0xa02a000a;
+    GPIOE->MODER = 0xaaaa800a;
+    GPIOF->MODER = 0xaa800aaa;
+    GPIOG->MODER = 0x80020a2a;
+
+    // Speed (high speed for all, very high speed for SDRAM pins)
+    GPIOB->OSPEEDR = 0x00003c00;
+    GPIOC->OSPEEDR = 0x00000003;
+    GPIOD->OSPEEDR = 0xf03f000f;
+    GPIOE->OSPEEDR = 0xffffc00f;
+    GPIOF->OSPEEDR = 0xffc00fff;
+    GPIOG->OSPEEDR = 0xc0030f3f;
+
+    // Since we'we un-configured PB3 and PB4 (by default they are SWO and NJRST)
+    // finish the job and remove the default pull-up
+    GPIOB->PUPDR = 0;
+
+    // Enable the SDRAM controller clock
+    RCC->AHB3ENR |= RCC_AHB3ENR_FMCEN;
+    RCC_SYNC();
+
+    // The SDRAM is a AS4C4M16SA-6TAN
+    // HCLK = 216MHz -> SDRAM clock = HCLK/2 = 133MHz
+
+    // 1. Memory device features
+    FMC_Bank5_6->SDCR[0] = 0                     // 0 delay after CAS latency
+                           | FMC_SDCR1_RBURST    // Enable read bursts
+                           | FMC_SDCR1_SDCLK_1;  // SDCLK = HCLK / 2
+    FMC_Bank5_6->SDCR[1] = 0                     // Write accesses allowed
+                           | FMC_SDCR2_CAS_1     // 2 cycles CAS latency
+                           | FMC_SDCR2_NB        // 4 internal banks
+                           | FMC_SDCR2_MWID_0    // 16 bit data bus
+                           | FMC_SDCR2_NR_1      // 13 bit row address
+                           // cppcheck-suppress duplicateExpression
+                           | 0;  // 8 bit column address
+
+// 2. Memory device timings
+#ifdef SYSCLK_FREQ_216MHz
+    // SDRAM timings. One clock cycle is 9.26ns
+    FMC_Bank5_6->SDTR[0] =
+        (2 - 1) << FMC_SDTR1_TRP_Pos     // 2 cycles TRP  (18.52ns > 18ns)
+        | (7 - 1) << FMC_SDTR1_TRC_Pos;  // 7 cycles TRC  (64.82ns > 60ns)
+    FMC_Bank5_6->SDTR[1] =
+        (2 - 1) << FMC_SDTR1_TRCD_Pos     // 2 cycles TRCD (18.52ns > 18ns)
+        | (2 - 1) << FMC_SDTR1_TWR_Pos    // 2 cycles TWR  (min 2cc > 12ns)
+        | (5 - 1) << FMC_SDTR1_TRAS_Pos   // 5 cycles TRAS (46.3ns  > 42ns)
+        | (7 - 1) << FMC_SDTR1_TXSR_Pos   // 7 cycles TXSR (74.08ns > 61.5ns)
+        | (2 - 1) << FMC_SDTR1_TMRD_Pos;  // 2 cycles TMRD (min 2cc > 12ns)
+#else
+#error No SDRAM timings for this clock
+#endif
+
+    // 3. Enable the bank 2 clock
+    FMC_Bank5_6->SDCMR = FMC_SDCMR_MODE_0   // Clock Configuration Enable
+                         | FMC_SDCMR_CTB2;  // Bank 2
+    sdramCommandWait();
+
+    // 4. Wait during command execution
+    delayUs(100);
+
+    // 5. Issue a "Precharge All" command
+    FMC_Bank5_6->SDCMR = FMC_SDCMR_MODE_1   // Precharge all
+                         | FMC_SDCMR_CTB2;  // Bank 2
+    sdramCommandWait();
+
+    // 6. Issue Auto-Refresh commands
+    FMC_Bank5_6->SDCMR = FMC_SDCMR_MODE_1 | FMC_SDCMR_MODE_0  // Auto-Refresh
+                         | FMC_SDCMR_CTB2                     // Bank 2
+                         | (8 - 1) << FMC_SDCMR_NRFS_Pos;     // 2 Auto-Refresh
+    sdramCommandWait();
+
+    // 7. Issue a Load Mode Register command
+    FMC_Bank5_6->SDCMR = FMC_SDCMR_MODE_2               /// Load mode register
+                         | FMC_SDCMR_CTB2               // Bank 2
+                         | 0x220 << FMC_SDCMR_MRD_Pos;  // CAS = 2, burst = 1
+    sdramCommandWait();
+
+// 8. Program the refresh rate (4K / 32ms)
+// 64ms / 8192 = 7.8125us
+#ifdef SYSCLK_FREQ_216MHz
+    // 7.8125us * 133MHz = 1039 - 20 = 1019
+    FMC_Bank5_6->SDRTR = 1019 << FMC_SDRTR_COUNT_Pos;
+#else
+#error No SDRAM refresh timings for this clock
+#endif
+}
+
+void IRQbspInit()
+{
+    // Enable USART1 pins port
+    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN;
+
+    userLed1::mode(Mode::OUTPUT);
+    userLed2::mode(Mode::OUTPUT);
+    userLed3_1::mode(Mode::OUTPUT);
+    userLed3_2::mode(Mode::OUTPUT);
+    userLed4::mode(Mode::OUTPUT);
+    userSwitch::mode(Mode::INPUT);
+
+    DefaultConsole::instance().IRQset(intrusive_ref_ptr<Device>(new STM32Serial(
+        defaultSerial, defaultSerialSpeed, STM32Serial::NOFLOWCTRL)));
+}
+
+void bspInit2()
+{
+#ifdef WITH_FILESYSTEM
+    basicFilesystemSetup(SDIODriver::instance());
+#endif  // WITH_FILESYSTEM
+
+#ifdef WITH_BACKUP_SRAM
+    // Print the reset reason
+    bootlog("Reset reson: ");
+    switch (SGM::instance().lastResetReason())
+    {
+        case ResetReason::RST_LOW_PWR:
+            bootlog("low power\n");
+            break;
+        case ResetReason::RST_WINDOW_WDG:
+            bootlog("window watchdog\n");
+            break;
+        case ResetReason::RST_INDEPENDENT_WDG:
+            bootlog("indeendent watchdog\n");
+            break;
+        case ResetReason::RST_SW:
+            bootlog("software reset\n");
+            break;
+        case ResetReason::RST_POWER_ON:
+            bootlog("power on\n");
+            break;
+        case ResetReason::RST_PIN:
+            bootlog("reset pin\n");
+            break;
+        case ResetReason::RST_UNKNOWN:
+            bootlog("unknown\n");
+            break;
+    }
+#endif  // WITH_BACKUP_SRAM
+}
+
+//
+// Shutdown and reboot
+//
+
+void shutdown()
+{
+    ioctl(STDOUT_FILENO, IOCTL_SYNC, 0);
+
+#ifdef WITH_FILESYSTEM
+    FilesystemManager::instance().umountAll();
+#endif  // WITH_FILESYSTEM
+
+    disableInterrupts();
+    for (;;)
+        ;
+}
+
+void reboot()
+{
+    ioctl(STDOUT_FILENO, IOCTL_SYNC, 0);
+
+#ifdef WITH_FILESYSTEM
+    FilesystemManager::instance().umountAll();
+#endif  // WITH_FILESYSTEM
+
+    disableInterrupts();
+    miosix_private::IRQsystemReboot();
+}
+
+}  // namespace miosix
diff --git a/src/bsps/stm32f767zi_compute_unit/interfaces-impl/bsp_impl.h b/src/bsps/stm32f767zi_compute_unit/interfaces-impl/bsp_impl.h
new file mode 100644
index 000000000..70ed88d15
--- /dev/null
+++ b/src/bsps/stm32f767zi_compute_unit/interfaces-impl/bsp_impl.h
@@ -0,0 +1,116 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * 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.
+ */
+
+/***************************************************************************
+ * bsp_impl.h Part of the Miosix Embedded OS.
+ * Board support package, this file initializes hardware.
+ ***************************************************************************/
+
+#ifndef BSP_IMPL_H
+#define BSP_IMPL_H
+
+#include "config/miosix_settings.h"
+#include "interfaces/gpio.h"
+
+namespace miosix
+{
+
+/**
+\addtogroup Hardware
+\{
+*/
+
+/**
+ * \internal
+ * Called by stage_1_boot.cpp to enable the SDRAM before initializing .data/.bss
+ * Requires the CPU clock to be already configured (running from the PLL)
+ */
+void configureSdram();
+
+/**
+ * \internal
+ * Board pin definition
+ */
+typedef Gpio<GPIOB_BASE, 7> userLed1;
+typedef Gpio<GPIOE_BASE, 3> userLed2;
+typedef Gpio<GPIOC_BASE, 13> userLed3_1;
+typedef Gpio<GPIOG_BASE, 9> userLed3_2;
+typedef Gpio<GPIOC_BASE, 2> userLed4;
+typedef Gpio<GPIOB_BASE, 2> userSwitch;
+
+inline void ledOn()
+{
+    userLed1::high();
+    userLed2::high();
+    userLed3_1::high();
+    userLed3_2::high();
+    userLed4::high();
+}
+
+inline void ledOff()
+{
+    userLed1::low();
+    userLed2::low();
+    userLed3_1::low();
+    userLed3_2::low();
+    userLed4::low();
+}
+
+inline void led1On() { userLed1::high(); }
+
+inline void led1Off() { userLed1::low(); }
+
+inline void led2On() { userLed2::high(); }
+
+inline void led2Off() { userLed2::low(); }
+
+inline void led3On()
+{
+    userLed3_1::high();
+    userLed3_2::high();
+}
+
+inline void led3Off()
+{
+    userLed3_1::low();
+    userLed3_2::low();
+}
+
+/**
+ * Polls the SD card sense GPIO.
+ *
+ * This board has no SD card whatsoever, but a card can be connected to the
+ * following GPIOs:
+ * TODO: never tested
+ *
+ * \return true. As there's no SD card sense switch, let's pretend that
+ * the card is present.
+ */
+inline bool sdCardSense() { return true; }
+
+/**
+\}
+*/
+
+}  // namespace miosix
+
+#endif  // BSP_IMPL_H
diff --git a/src/bsps/stm32f767zi_compute_unit/stm32_2m+16m_xram.ld b/src/bsps/stm32f767zi_compute_unit/stm32_2m+16m_xram.ld
new file mode 100644
index 000000000..5b3545abd
--- /dev/null
+++ b/src/bsps/stm32f767zi_compute_unit/stm32_2m+16m_xram.ld
@@ -0,0 +1,190 @@
+/*
+ * C++ enabled linker script for stm32f769ni (2M FLASH, 512K RAM, 16MB XRAM)
+ * Developed by TFT: Terraneo Federico Technologies
+ * Optimized for use with the Miosix kernel
+ */
+
+/*
+ * This linker script puts:
+ * - read only data and code (.text, .rodata, .eh_*) in FLASH
+ * - the 512Byte main (IRQ) stack, .data and .bss in the DTCM 128KB RAM
+ * - .data, .bss, stacks and heap in the external 16MB SDRAM.
+ */
+
+/*
+ * The main stack is used for interrupt handling by the kernel.
+ *
+ * *** Readme ***
+ * This linker script places the main stack (used by the kernel for interrupts)
+ * at the bottom of the ram, instead of the top. This is done for two reasons:
+ *
+ * - as an optimization for microcontrollers with little ram memory. In fact
+ *   the implementation of malloc from newlib requests memory to the OS in 4KB
+ *   block (except the first block that can be smaller). This is probably done
+ *   for compatibility with OSes with an MMU and paged memory. To see why this
+ *   is bad, consider a microcontroller with 8KB of ram: when malloc finishes
+ *   up the first 4KB it will call _sbrk_r asking for a 4KB block, but this will
+ *   fail because the top part of the ram is used by the main stack. As a
+ *   result, the top part of the memory will not be used by malloc, even if
+ *   available (and it is nearly *half* the ram on an 8KB mcu). By placing the
+ *   main stack at the bottom of the ram, the upper 4KB block will be entirely
+ *   free and available as heap space.
+ *
+ * - In case of main stack overflow the cpu will fault because access to memory
+ *   before the beginning of the ram faults. Instead with the default stack
+ *   placement the main stack will silently collide with the heap.
+ * Note: if increasing the main stack size also increase the ORIGIN value in
+ * the MEMORY definitions below accordingly.
+ */
+
+_main_stack_size = 512;                             /* main stack = 512Bytes */
+_main_stack_top  = 0x20000000 + _main_stack_size;
+ASSERT(_main_stack_size   % 8 == 0, "MAIN stack size error");
+
+/* Mapping the heap into XRAM */
+_heap_end = 0xd0000000 + 16M;                        /* end of available ram */
+
+/* Identify the Entry Point  */
+ENTRY(_Z13Reset_Handlerv)
+
+/*
+ * Specify the memory areas
+ *
+ * NOTE: starting at 0x20000000 there's 128KB of DTCM (Data Tightly Coupled
+ * Memory). Technically, we could use this as normal RAM as there's a way for
+ * the DMA to access it, but the datasheet is unclear about performance
+ * penalties for doing so. To avoid nonuniform DMA memory access latencies,
+ * we leave this 128KB DTCM unused except for the first 512Bytes which are for
+ * the interrupt stack. This leaves us with 384KB of RAM
+ */
+MEMORY
+{
+    xram(wx)  : ORIGIN = 0xd0000000, LENGTH =  16M
+    sram(wx)  : ORIGIN = 0x20020000, LENGTH = 384K
+    dtcm(wx)  : ORIGIN = 0x20000000, LENGTH = 128K    /* Used for main stack */
+    bram(rw)  : ORIGIN = 0x40024000, LENGTH =   4K    /* Bakup SRAM */
+    flash(rx) : ORIGIN = 0x08000000, LENGTH =   2M
+}
+
+/* now define the output sections  */
+SECTIONS
+{
+    . = 0;
+    
+    /* .text section: code goes to flash */
+    .text :
+    {
+        /* Startup code must go at address 0 */
+        KEEP(*(.isr_vector))
+        
+        *(.text)
+        *(.text.*)
+        *(.gnu.linkonce.t.*)
+        /* these sections for thumb interwork? */
+        *(.glue_7)
+        *(.glue_7t)
+        /* these sections for C++? */
+        *(.gcc_except_table)
+        *(.gcc_except_table.*)
+        *(.ARM.extab*)
+        *(.gnu.linkonce.armextab.*)
+
+        . = ALIGN(4);
+        /* .rodata: constant data */
+        *(.rodata)
+        *(.rodata.*)
+        *(.gnu.linkonce.r.*)
+
+        /* C++ Static constructors/destructors (eabi) */
+        . = ALIGN(4);
+        KEEP(*(.init))
+        
+        . = ALIGN(4);
+        __miosix_init_array_start = .;
+        KEEP (*(SORT(.miosix_init_array.*)))
+        KEEP (*(.miosix_init_array))
+        __miosix_init_array_end = .;
+
+        . = ALIGN(4);
+        __preinit_array_start = .;
+        KEEP (*(.preinit_array))
+        __preinit_array_end = .;
+
+        . = ALIGN(4);
+        __init_array_start = .;
+        KEEP (*(SORT(.init_array.*)))
+        KEEP (*(.init_array))
+        __init_array_end = .;
+
+        . = ALIGN(4);
+        KEEP(*(.fini))
+
+        . = ALIGN(4);
+        __fini_array_start = .;
+        KEEP (*(.fini_array))
+        KEEP (*(SORT(.fini_array.*)))
+        __fini_array_end = .;
+
+        /* C++ Static constructors/destructors (elf)  */
+        . = ALIGN(4);
+        _ctor_start = .;
+        KEEP (*crtbegin.o(.ctors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .ctors))
+        KEEP (*(SORT(.ctors.*)))
+        KEEP (*crtend.o(.ctors))
+       _ctor_end = .;
+
+        . = ALIGN(4);
+        KEEP (*crtbegin.o(.dtors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .dtors))
+        KEEP (*(SORT(.dtors.*)))
+        KEEP (*crtend.o(.dtors))
+    } > flash
+
+    /* .ARM.exidx is sorted, so has to go in its own output section.  */
+    __exidx_start = .;
+    .ARM.exidx :
+    {
+        *(.ARM.exidx* .gnu.linkonce.armexidx.*)
+    } > flash
+    __exidx_end = .;
+
+	/*
+     * .data section: global variables go to xram, but also store a copy to
+     * flash to initialize them
+     */
+    .data : ALIGN(8)
+    {
+        _data = .;
+        *(.data)
+        *(.data.*)
+        *(.gnu.linkonce.d.*)
+        . = ALIGN(8);
+        _edata = .;
+    } > xram AT > flash
+    _etext = LOADADDR(.data);
+
+    /* .bss section: uninitialized global variables go to xram */
+    _bss_start = .;
+    .bss :
+    {
+        *(.bss)
+        *(.bss.*)
+        *(.gnu.linkonce.b.*)
+        . = ALIGN(8);
+    } > xram
+    _bss_end = .;
+
+    _end = .;
+    PROVIDE(end = .);
+    
+    .preserve(NOLOAD) : ALIGN(4)
+    {
+        _preserve_start = .;
+        . = ALIGN(4);
+        *(.preserve);
+        *(.preserve*);
+        . = ALIGN(4);
+        _preserve_end = .;
+    } > bram
+}
diff --git a/src/bsps/stm32f767zi_compute_unit/stm32_2m+384k_ram.ld b/src/bsps/stm32f767zi_compute_unit/stm32_2m+384k_ram.ld
new file mode 100644
index 000000000..497bd5198
--- /dev/null
+++ b/src/bsps/stm32f767zi_compute_unit/stm32_2m+384k_ram.ld
@@ -0,0 +1,189 @@
+/*
+ * C++ enabled linker script for stm32f767zi (2M FLASH, 512K RAM)
+ * Developed by TFT: Terraneo Federico Technologies
+ * Optimized for use with the Miosix kernel
+ */
+
+/*
+ * This linker script puts:
+ * - read only data and code (.text, .rodata, .eh_*) in FLASH
+ * - the 512Byte main (IRQ) stack, .data and .bss in the DTCM 128KB RAM
+ * - .data, .bss, stacks and heap in the internal RAM.
+ */
+
+/*
+ * The main stack is used for interrupt handling by the kernel.
+ *
+ * *** Readme ***
+ * This linker script places the main stack (used by the kernel for interrupts)
+ * at the bottom of the ram, instead of the top. This is done for two reasons:
+ *
+ * - as an optimization for microcontrollers with little ram memory. In fact
+ *   the implementation of malloc from newlib requests memory to the OS in 4KB
+ *   block (except the first block that can be smaller). This is probably done
+ *   for compatibility with OSes with an MMU and paged memory. To see why this
+ *   is bad, consider a microcontroller with 8KB of ram: when malloc finishes
+ *   up the first 4KB it will call _sbrk_r asking for a 4KB block, but this will
+ *   fail because the top part of the ram is used by the main stack. As a
+ *   result, the top part of the memory will not be used by malloc, even if
+ *   available (and it is nearly *half* the ram on an 8KB mcu). By placing the
+ *   main stack at the bottom of the ram, the upper 4KB block will be entirely
+ *   free and available as heap space.
+ *
+ * - In case of main stack overflow the cpu will fault because access to memory
+ *   before the beginning of the ram faults. Instead with the default stack
+ *   placement the main stack will silently collide with the heap.
+ * Note: if increasing the main stack size also increase the ORIGIN value in
+ * the MEMORY definitions below accordingly.
+ */
+
+_main_stack_size = 512;                             /* main stack = 512Bytes */
+_main_stack_top  = 0x20000000 + _main_stack_size;
+ASSERT(_main_stack_size   % 8 == 0, "MAIN stack size error");
+
+/* Mapping the heap to the end of SRAM2 */
+_heap_end = 0x20000000 + 512K;                       /* end of available ram */
+
+/* Identify the Entry Point  */
+ENTRY(_Z13Reset_Handlerv)
+
+/*
+ * Specify the memory areas
+ *
+ * NOTE: starting at 0x20000000 there's 128KB of DTCM (Data Tightly Coupled
+ * Memory). Technically, we could use this as normal RAM as there's a way for
+ * the DMA to access it, but the datasheet is unclear about performance
+ * penalties for doing so. To avoid nonuniform DMA memory access latencies,
+ * we leave this 128KB DTCM unused except for the first 512Bytes which are for
+ * the interrupt stack. This leaves us with 384KB of RAM
+ */
+MEMORY
+{
+    sram(wx)  : ORIGIN = 0x20020000, LENGTH = 384K
+    dtcm(wx)  : ORIGIN = 0x20000000, LENGTH = 128K    /* Used for main stack */
+    bram(rw)  : ORIGIN = 0x40024000, LENGTH =   4K    /* Bakup SRAM */
+    flash(rx) : ORIGIN = 0x08000000, LENGTH =   2M
+}
+
+/* now define the output sections  */
+SECTIONS
+{
+    . = 0;
+    
+    /* .text section: code goes to flash */
+    .text :
+    {
+        /* Startup code must go at address 0 */
+        KEEP(*(.isr_vector))
+        
+        *(.text)
+        *(.text.*)
+        *(.gnu.linkonce.t.*)
+        /* these sections for thumb interwork? */
+        *(.glue_7)
+        *(.glue_7t)
+        /* these sections for C++? */
+        *(.gcc_except_table)
+        *(.gcc_except_table.*)
+        *(.ARM.extab*)
+        *(.gnu.linkonce.armextab.*)
+
+        . = ALIGN(4);
+        /* .rodata: constant data */
+        *(.rodata)
+        *(.rodata.*)
+        *(.gnu.linkonce.r.*)
+
+        /* C++ Static constructors/destructors (eabi) */
+        . = ALIGN(4);
+        KEEP(*(.init))
+        
+        . = ALIGN(4);
+        __miosix_init_array_start = .;
+        KEEP (*(SORT(.miosix_init_array.*)))
+        KEEP (*(.miosix_init_array))
+        __miosix_init_array_end = .;
+
+        . = ALIGN(4);
+        __preinit_array_start = .;
+        KEEP (*(.preinit_array))
+        __preinit_array_end = .;
+
+        . = ALIGN(4);
+        __init_array_start = .;
+        KEEP (*(SORT(.init_array.*)))
+        KEEP (*(.init_array))
+        __init_array_end = .;
+
+        . = ALIGN(4);
+        KEEP(*(.fini))
+
+        . = ALIGN(4);
+        __fini_array_start = .;
+        KEEP (*(.fini_array))
+        KEEP (*(SORT(.fini_array.*)))
+        __fini_array_end = .;
+
+        /* C++ Static constructors/destructors (elf)  */
+        . = ALIGN(4);
+        _ctor_start = .;
+        KEEP (*crtbegin.o(.ctors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .ctors))
+        KEEP (*(SORT(.ctors.*)))
+        KEEP (*crtend.o(.ctors))
+       _ctor_end = .;
+
+        . = ALIGN(4);
+        KEEP (*crtbegin.o(.dtors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .dtors))
+        KEEP (*(SORT(.dtors.*)))
+        KEEP (*crtend.o(.dtors))
+    } > flash
+
+    /* .ARM.exidx is sorted, so has to go in its own output section.  */
+    __exidx_start = .;
+    .ARM.exidx :
+    {
+        *(.ARM.exidx* .gnu.linkonce.armexidx.*)
+    } > flash
+    __exidx_end = .;
+
+	/*
+     * .data section: global variables go to sram, but also store a copy to
+     * flash to initialize them
+     */
+    .data : ALIGN(8)
+    {
+        _data = .;
+        *(.data)
+        *(.data.*)
+        *(.gnu.linkonce.d.*)
+        . = ALIGN(8);
+        _edata = .;
+    } > sram AT > flash
+    _etext = LOADADDR(.data);
+
+    /* .bss section: uninitialized global variables go to sram */
+    _bss_start = .;
+    .bss :
+    {
+        *(.bss)
+        *(.bss.*)
+        *(.gnu.linkonce.b.*)
+        . = ALIGN(8);
+    } > sram
+    _bss_end = .;
+
+    _end = .;
+    PROVIDE(end = .);
+    
+    .preserve(NOLOAD) : ALIGN(4)
+    {
+        _preserve_start = .;
+        . = ALIGN(4);
+        *(.preserve);
+        *(.preserve*);
+        . = ALIGN(4);
+        _preserve_end = .;
+    } > bram
+}
diff --git a/src/bsps/stm32f767zi_death_stack_v4/config/board_options.cmake b/src/bsps/stm32f767zi_death_stack_v4/config/board_options.cmake
new file mode 100644
index 000000000..944580554
--- /dev/null
+++ b/src/bsps/stm32f767zi_death_stack_v4/config/board_options.cmake
@@ -0,0 +1,106 @@
+# Copyright (C) 2023 by Skyward
+#
+# This program is free software; you can redistribute it and/or 
+# it under the terms of the GNU General Public License as published 
+# the Free Software Foundation; either version 2 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.
+#
+# As a special exception, if other files instantiate templates or use
+# macros or inline functions from this file, or you compile this file
+# and link it with other works to produce a work based on this file,
+# this file does not by itself cause the resulting work to be covered
+# by the GNU General Public License. However the source code for this
+# file must still be made available in accordance with the GNU 
+# Public License. This exception does not invalidate any other 
+# why a work based on this file might be covered by the GNU General
+# Public License.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, see <http://www.gnu.org/licenses/>
+
+set(BOARD_NAME stm32f767zi_death_stack_v4)
+set(ARCH_NAME cortexM7_stm32f7)
+
+# Base directories with header files for this board
+set(ARCH_PATH ${KPATH}/arch/${ARCH_NAME}/common)
+set(BOARD_PATH ${BOARDCORE_PATH}/src/bsps/${BOARD_NAME})
+set(BOARD_CONFIG_PATH ${BOARDCORE_PATH}/src/bsps/${BOARD_NAME}/config)
+
+# Specify where to find the board specific config/miosix_settings.h
+set(BOARD_MIOSIX_SETTINGS_PATH ${BOARD_PATH})
+
+# Specify where to find the board specific config/mxgui_settings.h
+set(BOARD_MXGUI_SETTINGS_PATH ${BOARD_PATH})
+
+# Optimization flags:
+# -O0 do no optimization, the default if no optimization level is specified
+# -O or -O1 optimize minimally
+# -O2 optimize more
+# -O3 optimize even more
+# -Ofast optimize very aggressively to the point of breaking the standard
+# -Og Optimize debugging experience, enables optimizations that do not
+# interfere with debugging
+# -Os Optimize for size with -O2 optimizations that do not increase code size
+set(OPT_OPTIMIZATION -O2)
+
+# Boot file and linker script
+set(BOOT_FILE ${BOARD_PATH}/core/stage_1_boot.cpp)
+# set(LINKER_SCRIPT ${BOARD_PATH}/stm32_2m+384k_ram.ld)
+set(LINKER_SCRIPT ${BOARD_PATH}/stm32_2m+16m_xram.ld)
+
+# Enables the initialization of the external 16MB SDRAM memory
+set(XRAM -D__ENABLE_XRAM)
+
+# Select clock frequency (HSE_VALUE is the xtal on board, fixed)
+set(CLOCK_FREQ -DHSE_VALUE=25000000 -DSYSCLK_FREQ_216MHz=216000000)
+
+# C++ Exception/rtti support disable flags.
+# To save code size if not using C++ exceptions (nor some STL code which
+# implicitly uses it) uncomment this option.
+# -D__NO_EXCEPTIONS is used by Miosix to know if exceptions are used.
+# set(OPT_EXCEPT -fno-exceptions -fno-rtti -D__NO_EXCEPTIONS)
+
+# Specify a custom flash command
+# This is the program that is invoked when the flash flag (-f or --flash) is
+# used with the Miosix Build System. Use $binary or $hex as placeolders, they
+# will be replaced by the build systems with the binary or hex file repectively.
+# If a command is not specified, the build system will use st-flash if found
+# set(PROGRAM_CMDLINE "here your custom flash command")
+
+# Basic flags
+set(FLAGS_BASE -mcpu=cortex-m7 -mthumb -mfloat-abi=hard -mfpu=fpv5-d16)
+
+# Flags for ASM and linker
+set(AFLAGS_BASE ${FLAGS_BASE})
+set(LFLAGS_BASE ${FLAGS_BASE} -Wl,--gc-sections,-Map,main.map -Wl,-T${LINKER_SCRIPT} ${OPT_EXCEPT} ${OPT_OPTIMIZATION} -nostdlib)
+
+# Flags for C/C++
+string(TOUPPER ${BOARD_NAME} BOARD_UPPER)
+set(CFLAGS_BASE
+    -D_BOARD_${BOARD_UPPER} -D_MIOSIX_BOARDNAME=\"${BOARD_NAME}\"
+    -D_DEFAULT_SOURCE=1 -ffunction-sections -Wall -Werror=return-type -g
+    -D_ARCH_CORTEXM7_STM32F7
+    ${CLOCK_FREQ} ${XRAM} ${SRAM_BOOT} ${FLAGS_BASE} ${OPT_OPTIMIZATION} -c
+)
+set(CXXFLAGS_BASE ${CFLAGS_BASE} ${OPT_EXCEPT})
+
+# Select architecture specific files
+set(ARCH_SRC
+    ${ARCH_PATH}/interfaces-impl/delays.cpp
+    ${ARCH_PATH}/interfaces-impl/gpio_impl.cpp
+    ${ARCH_PATH}/interfaces-impl/portability.cpp
+    ${BOARD_PATH}/interfaces-impl/bsp.cpp
+    ${KPATH}/arch/common/CMSIS/Device/ST/STM32F7xx/Source/Templates/system_stm32f7xx.c
+    ${KPATH}/arch/common/core/cache_cortexMx.cpp
+    ${KPATH}/arch/common/core/interrupts_cortexMx.cpp
+    ${KPATH}/arch/common/core/mpu_cortexMx.cpp
+    ${KPATH}/arch/common/core/stm32f2_f4_l4_f7_h7_os_timer.cpp
+    ${KPATH}/arch/common/drivers/sd_stm32f2_f4_f7.cpp
+    ${KPATH}/arch/common/drivers/serial_stm32.cpp
+    ${KPATH}/arch/common/drivers/stm32_hardware_rng.cpp
+)
diff --git a/src/bsps/stm32f767zi_death_stack_v4/config/board_settings.h b/src/bsps/stm32f767zi_death_stack_v4/config/board_settings.h
new file mode 100644
index 000000000..0ea02bc3e
--- /dev/null
+++ b/src/bsps/stm32f767zi_death_stack_v4/config/board_settings.h
@@ -0,0 +1,62 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * 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.
+ */
+
+#pragma once
+
+/**
+ * \internal
+ * Versioning for board_settings.h for out of git tree projects
+ */
+#define BOARD_SETTINGS_VERSION 300
+
+namespace miosix
+{
+
+/**
+ * \addtogroup Settings
+ * \{
+ */
+
+/// Size of stack for main().
+/// The C standard library is stack-heavy (iprintf requires 1KB)
+const unsigned int MAIN_STACK_SIZE = 16 * 1024;
+
+/// Serial port
+const unsigned int defaultSerial      = 1;
+const unsigned int defaultSerialSpeed = 115200;
+#define SERIAL_1_DMA
+// #define SERIAL_2_DMA
+// #define SERIAL_3_DMA
+
+// SD card driver
+static const unsigned char sdVoltage = 33;  // Board powered @ 3.3V
+#define SD_ONE_BIT_DATABUS
+#define SD_SDMMC 1  // Select either SDMMC1 or SDMMC2
+
+/// Analog supply voltage for ADC, DAC, Reset blocks, RCs and PLL
+#define V_DDA_VOLTAGE 3.3f
+
+/**
+ * \}
+ */
+
+}  // namespace miosix
diff --git a/src/bsps/stm32f767zi_death_stack_v4/config/miosix_settings.h b/src/bsps/stm32f767zi_death_stack_v4/config/miosix_settings.h
new file mode 100644
index 000000000..5fe66bf6c
--- /dev/null
+++ b/src/bsps/stm32f767zi_death_stack_v4/config/miosix_settings.h
@@ -0,0 +1,240 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * 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.
+ */
+
+#pragma once
+
+// Before you can compile the kernel you have to configure it by editing this
+// file. After that, comment out this line to disable the reminder error.
+// The PARSING_FROM_IDE is because Netbeans gets confused by this, it is never
+// defined when compiling the code.
+#ifndef PARSING_FROM_IDE
+//#error This error is a reminder that you have not edited miosix_settings.h
+// yet.
+#endif  // PARSING_FROM_IDE
+
+/**
+ * \file miosix_settings.h
+ * NOTE: this file contains ONLY configuration options that are not dependent
+ * on architecture specific details. The other options are in the following
+ * files which are included here:
+ * miosix/arch/architecture name/common/arch_settings.h
+ * miosix/config/arch/architecture name/board name/board_settings.h
+ */
+#include "arch_settings.h"
+#include "board_settings.h"
+#include "util/version.h"
+
+/**
+ * \internal
+ * Versioning for miosix_settings.h for out of git tree projects
+ */
+#define MIOSIX_SETTINGS_VERSION 300
+
+namespace miosix
+{
+
+/**
+ * \addtogroup Settings
+ * \{
+ */
+
+//
+// Scheduler options
+//
+
+/// \def SCHED_TYPE_PRIORITY
+/// If uncommented selects the priority scheduler
+/// \def SCHED_TYPE_CONTROL_BASED
+/// If uncommented selects the control based scheduler
+/// \def SCHED_TYPE_EDF
+/// If uncommented selects the EDF scheduler
+// Uncomment only *one* of those
+
+#define SCHED_TYPE_PRIORITY
+//#define SCHED_TYPE_CONTROL_BASED
+//#define SCHED_TYPE_EDF
+
+/// \def WITH_CPU_TIME_COUNTER
+/// Allows to enable/disable CPUTimeCounter to save code size and remove its
+/// overhead from the scheduling process. By default it is not defined
+/// (CPUTimeCounter is disabled).
+//#define WITH_CPU_TIME_COUNTER
+
+//
+// Filesystem options
+//
+
+/// \def WITH_FILESYSTEM
+/// Allows to enable/disable filesystem support to save code size
+/// By default it is defined (filesystem support is enabled)
+#define WITH_FILESYSTEM
+
+/// \def WITH_DEVFS
+/// Allows to enable/disable DevFs support to save code size
+/// By default it is defined (DevFs is enabled)
+#define WITH_DEVFS
+
+/// \def SYNC_AFTER_WRITE
+/// Increases filesystem write robustness. After each write operation the
+/// filesystem is synced so that a power failure happens data is not lost
+/// (unless power failure happens exactly between the write and the sync)
+/// Unfortunately write latency and throughput becomes twice as worse
+/// By default it is defined (slow but safe)
+#define SYNC_AFTER_WRITE
+
+/// Maximum number of open files. Trying to open more will fail.
+/// Cannot be lower than 3, as the first three are stdin, stdout, stderr
+const unsigned char MAX_OPEN_FILES = 8;
+
+/// \def WITH_PROCESSES
+/// If uncommented enables support for processes as well as threads.
+/// This enables the dynamic loader to load elf programs, the extended system
+/// call service and, if the hardware supports it, the MPU to provide memory
+/// isolation of processes
+//#define WITH_PROCESSES
+
+#if defined(WITH_PROCESSES) && defined(__NO_EXCEPTIONS)
+#error Processes require C++ exception support
+#endif  // defined(WITH_PROCESSES) && defined(__NO_EXCEPTIONS)
+
+#if defined(WITH_PROCESSES) && !defined(WITH_FILESYSTEM)
+#error Processes require filesystem support
+#endif  // defined(WITH_PROCESSES) && !defined(WITH_FILESYSTEM)
+
+#if defined(WITH_PROCESSES) && !defined(WITH_DEVFS)
+#error Processes require devfs support
+#endif  // defined(WITH_PROCESSES) && !defined(WITH_DEVFS)
+
+//
+// C/C++ standard library I/O (stdin, stdout and stderr related)
+//
+
+/// \def WITH_BOOTLOG
+/// Uncomment to print bootlogs on stdout.
+/// By default it is defined (bootlogs are printed)
+#define WITH_BOOTLOG
+
+/// \def WITH_ERRLOG
+/// Uncomment for debug information on stdout.
+/// By default it is defined (error information is printed)
+#define WITH_ERRLOG
+
+//
+// Kernel related options (stack sizes, priorities)
+//
+
+/// \def WITH_DEEP_SLEEP
+/// Adds interfaces and required variables to support deep sleep state switch
+/// automatically when peripherals are not required
+//#define WITH_DEEP_SLEEP
+
+/**
+ * \def JTAG_DISABLE_SLEEP
+ * JTAG debuggers lose communication with the device if it enters sleep
+ * mode, so to use debugging it is necessary to disable sleep in the idle
+ * thread. By default it is not defined (idle thread calls sleep).
+ */
+//#define JTAG_DISABLE_SLEEP
+
+#if defined(WITH_DEEP_SLEEP) && defined(JTAG_DISABLE_SLEEP)
+#error Deep sleep cannot work together with jtag
+#endif  // defined(WITH_PROCESSES) && !defined(WITH_DEVFS)
+
+/// Minimum stack size (MUST be divisible by 4)
+const unsigned int STACK_MIN = 256;
+
+/// \internal Size of idle thread stack.
+/// Should be >=STACK_MIN (MUST be divisible by 4)
+const unsigned int STACK_IDLE = 256;
+
+/// Default stack size for pthread_create.
+/// The chosen value is enough to call C standard library functions
+/// such as printf/fopen which are stack-heavy
+const unsigned int STACK_DEFAULT_FOR_PTHREAD = 2048;
+
+/// Maximum size of the RAM image of a process. If a program requires more
+/// the kernel will not run it (MUST be divisible by 4)
+const unsigned int MAX_PROCESS_IMAGE_SIZE = 64 * 1024;
+
+/// Minimum size of the stack for a process. If a program specifies a lower
+/// size the kernel will not run it (MUST be divisible by 4)
+const unsigned int MIN_PROCESS_STACK_SIZE = STACK_MIN;
+
+/// Every userspace thread has two stacks, one for when it is running in
+/// userspace and one for when it is running in kernelspace (that is, while it
+/// is executing system calls). This is the size of the stack for when the
+/// thread is running in kernelspace (MUST be divisible by 4)
+const unsigned int SYSTEM_MODE_PROCESS_STACK_SIZE = 2 * 1024;
+
+/// Number of priorities (MUST be >1)
+/// PRIORITY_MAX-1 is the highest priority, 0 is the lowest. -1 is reserved as
+/// the priority of the idle thread.
+/// The meaning of a thread's priority depends on the chosen scheduler.
+#ifdef SCHED_TYPE_PRIORITY
+// Can be modified, but a high value makes context switches more expensive
+const short int PRIORITY_MAX = 4;
+#elif defined(SCHED_TYPE_CONTROL_BASED)
+// Don't touch, the limit is due to the fixed point implementation
+// It's not needed for if floating point is selected, but kept for consistency
+const short int PRIORITY_MAX = 64;
+#else  // SCHED_TYPE_EDF
+// Doesn't exist for this kind of scheduler
+#endif
+
+/// Priority of main()
+/// The meaning of a thread's priority depends on the chosen scheduler.
+const unsigned char MAIN_PRIORITY = 1;
+
+#ifdef SCHED_TYPE_PRIORITY
+/// Maximum thread time slice in nanoseconds, after which preemption occurs
+const unsigned int MAX_TIME_SLICE = 1000000;
+#endif  // SCHED_TYPE_PRIORITY
+
+//
+// Other low level kernel options. There is usually no need to modify these.
+//
+
+/// \internal Length of wartermark (in bytes) to check stack overflow.
+/// MUST be divisible by 4 and can also be zero.
+/// A high value increases context switch time.
+const unsigned int WATERMARK_LEN = 16;
+
+/// \internal Used to fill watermark
+const unsigned int WATERMARK_FILL = 0xaaaaaaaa;
+
+/// \internal Used to fill stack (for checking stack usage)
+const unsigned int STACK_FILL = 0xbbbbbbbb;
+
+// Compiler version checks
+#if !defined(_MIOSIX_GCC_PATCH_MAJOR) || _MIOSIX_GCC_PATCH_MAJOR < 3
+#error \
+    "You are using a too old or unsupported compiler. Get the latest one from https://miosix.org/wiki/index.php?title=Miosix_Toolchain"
+#endif
+#if _MIOSIX_GCC_PATCH_MAJOR > 3
+#warning "You are using a too new compiler, which may not be supported"
+#endif
+
+/**
+ * \}
+ */
+
+}  // namespace miosix
diff --git a/src/bsps/stm32f767zi_death_stack_v4/core/stage_1_boot.cpp b/src/bsps/stm32f767zi_death_stack_v4/core/stage_1_boot.cpp
new file mode 100644
index 000000000..8374c9de6
--- /dev/null
+++ b/src/bsps/stm32f767zi_death_stack_v4/core/stage_1_boot.cpp
@@ -0,0 +1,505 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * 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 <string.h>
+
+#include "core/cache_cortexMx.h"
+#include "core/interrupts.h"  //For the unexpected interrupt call
+#include "core/interrupts_cortexMx.h"
+#include "interfaces/arch_registers.h"
+#include "interfaces/bsp.h"
+#include "kernel/stage_2_boot.h"
+
+/*
+ * startup.cpp
+ * STM32 C++ startup.
+ * NOTE: for stm32f767 devices ONLY.
+ * Supports interrupt handlers in C++ without extern "C"
+ * Developed by Terraneo Federico, based on ST startup code.
+ * Additionally modified to boot Miosix.
+ */
+
+/**
+ * Called by Reset_Handler, performs initialization and calls main.
+ * Never returns.
+ */
+void program_startup() __attribute__((noreturn));
+void program_startup()
+{
+    // Cortex M7 core appears to get out of reset with interrupts already
+    // enabled
+    __disable_irq();
+
+    miosix::IRQconfigureCache((const unsigned int *)0xd0000000,
+                              8 * 1024 * 1024);
+
+    // These are defined in the linker script
+    extern unsigned char _etext asm("_etext");
+    extern unsigned char _data asm("_data");
+    extern unsigned char _edata asm("_edata");
+    extern unsigned char _bss_start asm("_bss_start");
+    extern unsigned char _bss_end asm("_bss_end");
+
+    // Initialize .data section, clear .bss section
+    unsigned char *etext     = &_etext;
+    unsigned char *data      = &_data;
+    unsigned char *edata     = &_edata;
+    unsigned char *bss_start = &_bss_start;
+    unsigned char *bss_end   = &_bss_end;
+    memcpy(data, etext, edata - data);
+    memset(bss_start, 0, bss_end - bss_start);
+
+    // Move on to stage 2
+    _init();
+
+    // If main returns, reboot
+    NVIC_SystemReset();
+    for (;;)
+        ;
+}
+
+/**
+ * Reset handler, called by hardware immediately after reset
+ */
+void Reset_Handler() __attribute__((__interrupt__, noreturn));
+void Reset_Handler()
+{
+    /**
+     * SystemInit() is called *before* initializing .data and zeroing .bss
+     * Despite all startup files provided by ST do the opposite, there are three
+     * good reasons to do so:
+     * 1. First, the CMSIS specifications say that SystemInit() must not access
+     *    global variables, so it is actually possible to call it before
+     * 2. Second, when running Miosix with the xram linker scripts .data and
+     *    .bss are placed in the external RAM, so we *must* call SystemInit(),
+     *    which enables xram, before touching .data and .bss
+     * 3. Third, this is a performance improvement since the loops that
+     *    initialize .data and zeros .bss now run with the CPU at full speed
+     *    instead of 8MHz
+     */
+    SystemInit();
+
+/**
+ * ST does not provide code to initialize the SDRAM at boot.
+ * Put after SystemInit() as SDRAM is timing-sensitive and needs the full
+ * clock speed.
+ */
+#ifdef __ENABLE_XRAM
+    miosix::configureSdram();
+#endif  //__ENABLE_XRAM
+
+    /*
+     * Load into the program stack pointer the heap end address and switch from
+     * the msp to sps.
+     * This is required for booting Miosix, a small portion of the top of the
+     * heap area will be used as stack until the first thread starts. After,
+     * this stack will be abandoned and the process stack will point to the
+     * current thread's stack.
+     */
+    asm volatile(
+        "ldr r0,  =_heap_end          \n\t"
+        "msr psp, r0                  \n\t"
+        "movw r0, #2                  \n\n"  // Set the control register to use
+        "msr control, r0              \n\t"  // the process stack
+        "isb                          \n\t" ::
+            : "r0");
+
+    program_startup();
+}
+
+/**
+ * All unused interrupts call this function.
+ */
+extern "C" void Default_Handler() { unexpectedInterrupt(); }
+
+// System handlers
+void /*__attribute__((weak))*/ Reset_Handler();      // These interrupts are not
+void /*__attribute__((weak))*/ NMI_Handler();        // weak because they are
+void /*__attribute__((weak))*/ HardFault_Handler();  // surely defined by Miosix
+void /*__attribute__((weak))*/ MemManage_Handler();
+void /*__attribute__((weak))*/ BusFault_Handler();
+void /*__attribute__((weak))*/ UsageFault_Handler();
+void /*__attribute__((weak))*/ SVC_Handler();
+void /*__attribute__((weak))*/ DebugMon_Handler();
+void /*__attribute__((weak))*/ PendSV_Handler();
+void __attribute__((weak)) SysTick_Handler();
+
+// Interrupt handlers
+void __attribute__((weak)) WWDG_IRQHandler();
+void __attribute__((weak)) PVD_IRQHandler();
+void __attribute__((weak)) TAMP_STAMP_IRQHandler();
+void __attribute__((weak)) RTC_WKUP_IRQHandler();
+void __attribute__((weak)) FLASH_IRQHandler();
+void __attribute__((weak)) RCC_IRQHandler();
+void __attribute__((weak)) EXTI0_IRQHandler();
+void __attribute__((weak)) EXTI1_IRQHandler();
+void __attribute__((weak)) EXTI2_IRQHandler();
+void __attribute__((weak)) EXTI3_IRQHandler();
+void __attribute__((weak)) EXTI4_IRQHandler();
+void __attribute__((weak)) DMA1_Stream0_IRQHandler();
+void __attribute__((weak)) DMA1_Stream1_IRQHandler();
+void __attribute__((weak)) DMA1_Stream2_IRQHandler();
+void __attribute__((weak)) DMA1_Stream3_IRQHandler();
+void __attribute__((weak)) DMA1_Stream4_IRQHandler();
+void __attribute__((weak)) DMA1_Stream5_IRQHandler();
+void __attribute__((weak)) DMA1_Stream6_IRQHandler();
+void __attribute__((weak)) ADC_IRQHandler();
+void __attribute__((weak)) CAN1_TX_IRQHandler();
+void __attribute__((weak)) CAN1_RX0_IRQHandler();
+void __attribute__((weak)) CAN1_RX1_IRQHandler();
+void __attribute__((weak)) CAN1_SCE_IRQHandler();
+void __attribute__((weak)) EXTI9_5_IRQHandler();
+void __attribute__((weak)) TIM1_BRK_TIM9_IRQHandler();
+void __attribute__((weak)) TIM1_UP_TIM10_IRQHandler();
+void __attribute__((weak)) TIM1_TRG_COM_TIM11_IRQHandler();
+void __attribute__((weak)) TIM1_CC_IRQHandler();
+void __attribute__((weak)) TIM2_IRQHandler();
+void __attribute__((weak)) TIM3_IRQHandler();
+void __attribute__((weak)) TIM4_IRQHandler();
+void __attribute__((weak)) I2C1_EV_IRQHandler();
+void __attribute__((weak)) I2C1_ER_IRQHandler();
+void __attribute__((weak)) I2C2_EV_IRQHandler();
+void __attribute__((weak)) I2C2_ER_IRQHandler();
+void __attribute__((weak)) SPI1_IRQHandler();
+void __attribute__((weak)) SPI2_IRQHandler();
+void __attribute__((weak)) USART1_IRQHandler();
+void __attribute__((weak)) USART2_IRQHandler();
+void __attribute__((weak)) USART3_IRQHandler();
+void __attribute__((weak)) EXTI15_10_IRQHandler();
+void __attribute__((weak)) RTC_Alarm_IRQHandler();
+void __attribute__((weak)) OTG_FS_WKUP_IRQHandler();
+void __attribute__((weak)) TIM8_BRK_TIM12_IRQHandler();
+void __attribute__((weak)) TIM8_UP_TIM13_IRQHandler();
+void __attribute__((weak)) TIM8_TRG_COM_TIM14_IRQHandler();
+void __attribute__((weak)) TIM8_CC_IRQHandler();
+void __attribute__((weak)) DMA1_Stream7_IRQHandler();
+void __attribute__((weak)) FMC_IRQHandler();
+void __attribute__((weak)) SDMMC1_IRQHandler();
+void __attribute__((weak)) TIM5_IRQHandler();
+void __attribute__((weak)) SPI3_IRQHandler();
+void __attribute__((weak)) UART4_IRQHandler();
+void __attribute__((weak)) UART5_IRQHandler();
+void __attribute__((weak)) TIM6_DAC_IRQHandler();
+void __attribute__((weak)) TIM7_IRQHandler();
+void __attribute__((weak)) DMA2_Stream0_IRQHandler();
+void __attribute__((weak)) DMA2_Stream1_IRQHandler();
+void __attribute__((weak)) DMA2_Stream2_IRQHandler();
+void __attribute__((weak)) DMA2_Stream3_IRQHandler();
+void __attribute__((weak)) DMA2_Stream4_IRQHandler();
+void __attribute__((weak)) ETH_IRQHandler();
+void __attribute__((weak)) ETH_WKUP_IRQHandler();
+void __attribute__((weak)) CAN2_TX_IRQHandler();
+void __attribute__((weak)) CAN2_RX0_IRQHandler();
+void __attribute__((weak)) CAN2_RX1_IRQHandler();
+void __attribute__((weak)) CAN2_SCE_IRQHandler();
+void __attribute__((weak)) OTG_FS_IRQHandler();
+void __attribute__((weak)) DMA2_Stream5_IRQHandler();
+void __attribute__((weak)) DMA2_Stream6_IRQHandler();
+void __attribute__((weak)) DMA2_Stream7_IRQHandler();
+void __attribute__((weak)) USART6_IRQHandler();
+void __attribute__((weak)) I2C3_EV_IRQHandler();
+void __attribute__((weak)) I2C3_ER_IRQHandler();
+void __attribute__((weak)) OTG_HS_EP1_OUT_IRQHandler();
+void __attribute__((weak)) OTG_HS_EP1_IN_IRQHandler();
+void __attribute__((weak)) OTG_HS_WKUP_IRQHandler();
+void __attribute__((weak)) OTG_HS_IRQHandler();
+void __attribute__((weak)) DCMI_IRQHandler();
+void __attribute__((weak)) CRYP_IRQHandler();
+void __attribute__((weak)) RNG_IRQHandler();
+void __attribute__((weak)) FPU_IRQHandler();
+void __attribute__((weak)) UART7_IRQHandler();
+void __attribute__((weak)) UART8_IRQHandler();
+void __attribute__((weak)) SPI4_IRQHandler();
+void __attribute__((weak)) SPI5_IRQHandler();
+void __attribute__((weak)) SPI6_IRQHandler();
+void __attribute__((weak)) SAI1_IRQHandler();
+void __attribute__((weak)) LTDC_IRQHandler();
+void __attribute__((weak)) LTDC_ER_IRQHandler();
+void __attribute__((weak)) DMA2D_IRQHandler();
+void __attribute__((weak)) SAI2_IRQHandler();
+void __attribute__((weak)) QUADSPI_IRQHandler();
+void __attribute__((weak)) LPTIM1_IRQHandler();
+void __attribute__((weak)) CEC_IRQHandler();
+void __attribute__((weak)) I2C4_EV_IRQHandler();
+void __attribute__((weak)) I2C4_ER_IRQHandler();
+void __attribute__((weak)) SPDIF_RX_IRQHandler();
+void __attribute__((weak)) DSIHOST_IRQHandler();
+void __attribute__((weak)) DFSDM1_FLT0_IRQHandler();
+void __attribute__((weak)) DFSDM1_FLT1_IRQHandler();
+void __attribute__((weak)) DFSDM1_FLT2_IRQHandler();
+void __attribute__((weak)) DFSDM1_FLT3_IRQHandler();
+void __attribute__((weak)) SDMMC2_IRQHandler();
+void __attribute__((weak)) CAN3_TX_IRQHandler();
+void __attribute__((weak)) CAN3_RX0_IRQHandler();
+void __attribute__((weak)) CAN3_RX1_IRQHandler();
+void __attribute__((weak)) CAN3_SCE_IRQHandler();
+void __attribute__((weak)) JPEG_IRQHandler();
+void __attribute__((weak)) MDIOS_IRQHandler();
+
+// Stack top, defined in the linker script
+extern char _main_stack_top asm("_main_stack_top");
+
+// Interrupt vectors, must be placed @ address 0x00000000
+// The extern declaration is required otherwise g++ optimizes it out
+extern void (*const __Vectors[])();
+void (*const __Vectors[])() __attribute__((section(".isr_vector"))) = {
+    reinterpret_cast<void (*)()>(&_main_stack_top), /* Stack pointer*/
+    Reset_Handler,                                  /* Reset Handler */
+    NMI_Handler,                                    /* NMI Handler */
+    HardFault_Handler,                              /* Hard Fault Handler */
+    MemManage_Handler,                              /* MPU Fault Handler */
+    BusFault_Handler,                               /* Bus Fault Handler */
+    UsageFault_Handler,                             /* Usage Fault Handler */
+    0,                                              /* Reserved */
+    0,                                              /* Reserved */
+    0,                                              /* Reserved */
+    0,                                              /* Reserved */
+    SVC_Handler,                                    /* SVCall Handler */
+    DebugMon_Handler,                               /* Debug Monitor Handler */
+    0,                                              /* Reserved */
+    PendSV_Handler,                                 /* PendSV Handler */
+    SysTick_Handler,                                /* SysTick Handler */
+
+    /* External Interrupts */
+    WWDG_IRQHandler,
+    PVD_IRQHandler,
+    TAMP_STAMP_IRQHandler,
+    RTC_WKUP_IRQHandler,
+    FLASH_IRQHandler,
+    RCC_IRQHandler,
+    EXTI0_IRQHandler,
+    EXTI1_IRQHandler,
+    EXTI2_IRQHandler,
+    EXTI3_IRQHandler,
+    EXTI4_IRQHandler,
+    DMA1_Stream0_IRQHandler,
+    DMA1_Stream1_IRQHandler,
+    DMA1_Stream2_IRQHandler,
+    DMA1_Stream3_IRQHandler,
+    DMA1_Stream4_IRQHandler,
+    DMA1_Stream5_IRQHandler,
+    DMA1_Stream6_IRQHandler,
+    ADC_IRQHandler,
+    CAN1_TX_IRQHandler,
+    CAN1_RX0_IRQHandler,
+    CAN1_RX1_IRQHandler,
+    CAN1_SCE_IRQHandler,
+    EXTI9_5_IRQHandler,
+    TIM1_BRK_TIM9_IRQHandler,
+    TIM1_UP_TIM10_IRQHandler,
+    TIM1_TRG_COM_TIM11_IRQHandler,
+    TIM1_CC_IRQHandler,
+    TIM2_IRQHandler,
+    TIM3_IRQHandler,
+    TIM4_IRQHandler,
+    I2C1_EV_IRQHandler,
+    I2C1_ER_IRQHandler,
+    I2C2_EV_IRQHandler,
+    I2C2_ER_IRQHandler,
+    SPI1_IRQHandler,
+    SPI2_IRQHandler,
+    USART1_IRQHandler,
+    USART2_IRQHandler,
+    USART3_IRQHandler,
+    EXTI15_10_IRQHandler,
+    RTC_Alarm_IRQHandler,
+    OTG_FS_WKUP_IRQHandler,
+    TIM8_BRK_TIM12_IRQHandler,
+    TIM8_UP_TIM13_IRQHandler,
+    TIM8_TRG_COM_TIM14_IRQHandler,
+    TIM8_CC_IRQHandler,
+    DMA1_Stream7_IRQHandler,
+    FMC_IRQHandler,
+    SDMMC1_IRQHandler,
+    TIM5_IRQHandler,
+    SPI3_IRQHandler,
+    UART4_IRQHandler,
+    UART5_IRQHandler,
+    TIM6_DAC_IRQHandler,
+    TIM7_IRQHandler,
+    DMA2_Stream0_IRQHandler,
+    DMA2_Stream1_IRQHandler,
+    DMA2_Stream2_IRQHandler,
+    DMA2_Stream3_IRQHandler,
+    DMA2_Stream4_IRQHandler,
+    ETH_IRQHandler,
+    ETH_WKUP_IRQHandler,
+    CAN2_TX_IRQHandler,
+    CAN2_RX0_IRQHandler,
+    CAN2_RX1_IRQHandler,
+    CAN2_SCE_IRQHandler,
+    OTG_FS_IRQHandler,
+    DMA2_Stream5_IRQHandler,
+    DMA2_Stream6_IRQHandler,
+    DMA2_Stream7_IRQHandler,
+    USART6_IRQHandler,
+    I2C3_EV_IRQHandler,
+    I2C3_ER_IRQHandler,
+    OTG_HS_EP1_OUT_IRQHandler,
+    OTG_HS_EP1_IN_IRQHandler,
+    OTG_HS_WKUP_IRQHandler,
+    OTG_HS_IRQHandler,
+    DCMI_IRQHandler,
+    CRYP_IRQHandler,
+    RNG_IRQHandler,
+    FPU_IRQHandler,
+    UART7_IRQHandler,
+    UART8_IRQHandler,
+    SPI4_IRQHandler,
+    SPI5_IRQHandler,
+    SPI6_IRQHandler,
+    SAI1_IRQHandler,
+    LTDC_IRQHandler,
+    LTDC_ER_IRQHandler,
+    DMA2D_IRQHandler,
+    SAI2_IRQHandler,
+    QUADSPI_IRQHandler,
+    LPTIM1_IRQHandler,
+    CEC_IRQHandler,
+    I2C4_EV_IRQHandler,
+    I2C4_ER_IRQHandler,
+    SPDIF_RX_IRQHandler,
+    DSIHOST_IRQHandler,
+    DFSDM1_FLT0_IRQHandler,
+    DFSDM1_FLT1_IRQHandler,
+    DFSDM1_FLT2_IRQHandler,
+    DFSDM1_FLT3_IRQHandler,
+    SDMMC2_IRQHandler,
+    CAN3_TX_IRQHandler,
+    CAN3_RX0_IRQHandler,
+    CAN3_RX1_IRQHandler,
+    CAN3_SCE_IRQHandler,
+    JPEG_IRQHandler,
+    MDIOS_IRQHandler,
+};
+
+#pragma weak SysTick_IRQHandler            = Default_Handler
+#pragma weak WWDG_IRQHandler               = Default_Handler
+#pragma weak PVD_IRQHandler                = Default_Handler
+#pragma weak TAMP_STAMP_IRQHandler         = Default_Handler
+#pragma weak RTC_WKUP_IRQHandler           = Default_Handler
+#pragma weak FLASH_IRQHandler              = Default_Handler
+#pragma weak RCC_IRQHandler                = Default_Handler
+#pragma weak EXTI0_IRQHandler              = Default_Handler
+#pragma weak EXTI1_IRQHandler              = Default_Handler
+#pragma weak EXTI2_IRQHandler              = Default_Handler
+#pragma weak EXTI3_IRQHandler              = Default_Handler
+#pragma weak EXTI4_IRQHandler              = Default_Handler
+#pragma weak DMA1_Stream0_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream1_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream2_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream3_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream4_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream5_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream6_IRQHandler       = Default_Handler
+#pragma weak ADC_IRQHandler                = Default_Handler
+#pragma weak CAN1_TX_IRQHandler            = Default_Handler
+#pragma weak CAN1_RX0_IRQHandler           = Default_Handler
+#pragma weak CAN1_RX1_IRQHandler           = Default_Handler
+#pragma weak CAN1_SCE_IRQHandler           = Default_Handler
+#pragma weak EXTI9_5_IRQHandler            = Default_Handler
+#pragma weak TIM1_BRK_TIM9_IRQHandler      = Default_Handler
+#pragma weak TIM1_UP_TIM10_IRQHandler      = Default_Handler
+#pragma weak TIM1_TRG_COM_TIM11_IRQHandler = Default_Handler
+#pragma weak TIM1_CC_IRQHandler            = Default_Handler
+#pragma weak TIM2_IRQHandler               = Default_Handler
+#pragma weak TIM3_IRQHandler               = Default_Handler
+#pragma weak TIM4_IRQHandler               = Default_Handler
+#pragma weak I2C1_EV_IRQHandler            = Default_Handler
+#pragma weak I2C1_ER_IRQHandler            = Default_Handler
+#pragma weak I2C2_EV_IRQHandler            = Default_Handler
+#pragma weak I2C2_ER_IRQHandler            = Default_Handler
+#pragma weak SPI1_IRQHandler               = Default_Handler
+#pragma weak SPI2_IRQHandler               = Default_Handler
+// #pragma weak USART1_IRQHandler             = Default_Handler
+// #pragma weak USART2_IRQHandler             = Default_Handler
+// #pragma weak USART3_IRQHandler             = Default_Handler
+#pragma weak EXTI15_10_IRQHandler          = Default_Handler
+#pragma weak RTC_Alarm_IRQHandler          = Default_Handler
+#pragma weak OTG_FS_WKUP_IRQHandler        = Default_Handler
+#pragma weak TIM8_BRK_TIM12_IRQHandler     = Default_Handler
+#pragma weak TIM8_UP_TIM13_IRQHandler      = Default_Handler
+#pragma weak TIM8_TRG_COM_TIM14_IRQHandler = Default_Handler
+#pragma weak TIM8_CC_IRQHandler            = Default_Handler
+#pragma weak DMA1_Stream7_IRQHandler       = Default_Handler
+#pragma weak FMC_IRQHandler                = Default_Handler
+#pragma weak SDMMC1_IRQHandler             = Default_Handler
+#pragma weak TIM5_IRQHandler               = Default_Handler
+#pragma weak SPI3_IRQHandler               = Default_Handler
+// #pragma weak UART4_IRQHandler              = Default_Handler
+// #pragma weak UART5_IRQHandler              = Default_Handler
+#pragma weak TIM6_DAC_IRQHandler     = Default_Handler
+#pragma weak TIM7_IRQHandler         = Default_Handler
+#pragma weak DMA2_Stream0_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream1_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream2_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream3_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream4_IRQHandler = Default_Handler
+#pragma weak ETH_IRQHandler          = Default_Handler
+#pragma weak ETH_WKUP_IRQHandler     = Default_Handler
+#pragma weak CAN2_TX_IRQHandler      = Default_Handler
+#pragma weak CAN2_RX0_IRQHandler     = Default_Handler
+#pragma weak CAN2_RX1_IRQHandler     = Default_Handler
+#pragma weak CAN2_SCE_IRQHandler     = Default_Handler
+#pragma weak OTG_FS_IRQHandler       = Default_Handler
+#pragma weak DMA2_Stream5_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream6_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream7_IRQHandler = Default_Handler
+// #pragma weak USART6_IRQHandler             = Default_Handler
+#pragma weak I2C3_EV_IRQHandler        = Default_Handler
+#pragma weak I2C3_ER_IRQHandler        = Default_Handler
+#pragma weak OTG_HS_EP1_OUT_IRQHandler = Default_Handler
+#pragma weak OTG_HS_EP1_IN_IRQHandler  = Default_Handler
+#pragma weak OTG_HS_WKUP_IRQHandler    = Default_Handler
+#pragma weak OTG_HS_IRQHandler         = Default_Handler
+#pragma weak DCMI_IRQHandler           = Default_Handler
+#pragma weak CRYP_IRQHandler           = Default_Handler
+#pragma weak RNG_IRQHandler            = Default_Handler
+#pragma weak FPU_IRQHandler            = Default_Handler
+// #pragma weak UART7_IRQHandler              = Default_Handler
+// #pragma weak UART8_IRQHandler              = Default_Handler
+#pragma weak SPI4_IRQHandler        = Default_Handler
+#pragma weak SPI5_IRQHandler        = Default_Handler
+#pragma weak SPI6_IRQHandler        = Default_Handler
+#pragma weak SAI1_IRQHandler        = Default_Handler
+#pragma weak LTDC_IRQHandler        = Default_Handler
+#pragma weak LTDC_ER_IRQHandler     = Default_Handler
+#pragma weak DMA2D_IRQHandler       = Default_Handler
+#pragma weak SAI2_IRQHandler        = Default_Handler
+#pragma weak QUADSPI_IRQHandler     = Default_Handler
+#pragma weak LPTIM1_IRQHandler      = Default_Handler
+#pragma weak CEC_IRQHandler         = Default_Handler
+#pragma weak I2C4_EV_IRQHandler     = Default_Handler
+#pragma weak I2C4_ER_IRQHandler     = Default_Handler
+#pragma weak SPDIF_RX_IRQHandler    = Default_Handler
+#pragma weak DSIHOST_IRQHandler     = Default_Handler
+#pragma weak DFSDM1_FLT0_IRQHandler = Default_Handler
+#pragma weak DFSDM1_FLT1_IRQHandler = Default_Handler
+#pragma weak DFSDM1_FLT2_IRQHandler = Default_Handler
+#pragma weak DFSDM1_FLT3_IRQHandler = Default_Handler
+#pragma weak SDMMC2_IRQHandler      = Default_Handler
+#pragma weak CAN3_TX_IRQHandler     = Default_Handler
+#pragma weak CAN3_RX0_IRQHandler    = Default_Handler
+#pragma weak CAN3_RX1_IRQHandler    = Default_Handler
+#pragma weak CAN3_SCE_IRQHandler    = Default_Handler
+#pragma weak JPEG_IRQHandler        = Default_Handler
+#pragma weak MDIOS_IRQHandler       = Default_Handler
diff --git a/src/bsps/stm32f767zi_death_stack_v4/interfaces-impl/arch_registers_impl.h b/src/bsps/stm32f767zi_death_stack_v4/interfaces-impl/arch_registers_impl.h
new file mode 100644
index 000000000..d2d82f50a
--- /dev/null
+++ b/src/bsps/stm32f767zi_death_stack_v4/interfaces-impl/arch_registers_impl.h
@@ -0,0 +1,39 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * 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.
+ */
+
+#ifndef ARCH_REGISTERS_IMPL_H
+#define ARCH_REGISTERS_IMPL_H
+
+// stm32f7xx.h defines a few macros like __ICACHE_PRESENT, __DCACHE_PRESENT and
+// includes core_cm7.h. Do not include core_cm7.h before.
+#define STM32F767xx
+#include "CMSIS/Device/ST/STM32F7xx/Include/stm32f7xx.h"
+
+#if (__ICACHE_PRESENT != 1) || (__DCACHE_PRESENT != 1)
+#error "Wrong include order"
+#endif
+
+#include "CMSIS/Device/ST/STM32F7xx/Include/system_stm32f7xx.h"
+
+#define RCC_SYNC() __DSB()  // TODO: can this dsb be removed?
+
+#endif  // ARCH_REGISTERS_IMPL_H
diff --git a/src/bsps/stm32f767zi_death_stack_v4/interfaces-impl/bsp.cpp b/src/bsps/stm32f767zi_death_stack_v4/interfaces-impl/bsp.cpp
new file mode 100644
index 000000000..557775f41
--- /dev/null
+++ b/src/bsps/stm32f767zi_death_stack_v4/interfaces-impl/bsp.cpp
@@ -0,0 +1,419 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * 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.
+ */
+
+/***********************************************************************
+ * bsp.cpp Part of the Miosix Embedded OS.
+ * Board support package, this file initializes hardware.
+ ************************************************************************/
+
+#include "interfaces/bsp.h"
+
+#include <inttypes.h>
+#include <sys/ioctl.h>
+
+#include <cstdlib>
+
+#include "board_settings.h"
+#include "config/miosix_settings.h"
+#include "drivers/sd_stm32f2_f4_f7.h"
+#include "drivers/serial.h"
+#include "drivers/serial_stm32.h"
+#include "drivers/stm32_sgm.h"
+#include "filesystem/console/console_device.h"
+#include "filesystem/file_access.h"
+#include "hwmapping.h"
+#include "interfaces/arch_registers.h"
+#include "interfaces/delays.h"
+#include "interfaces/portability.h"
+#include "kernel/kernel.h"
+#include "kernel/logging.h"
+#include "kernel/sync.h"
+
+namespace miosix
+{
+
+//
+// Initialization
+//
+
+static void sdramCommandWait()
+{
+    for (int i = 0; i < 0xffff; i++)
+        if ((FMC_Bank5_6->SDSR & FMC_SDSR_BUSY) == 0)
+            return;
+}
+
+void configureSdram()
+{
+    // Enable gpios used by the ram
+    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN | RCC_AHB1ENR_GPIOCEN |
+                    RCC_AHB1ENR_GPIODEN | RCC_AHB1ENR_GPIOEEN |
+                    RCC_AHB1ENR_GPIOFEN | RCC_AHB1ENR_GPIOGEN;
+    RCC_SYNC();
+
+    // On the compute unit with F767ZI, the SDRAM pins are:
+    // - PG8:  FMC_SDCLK  (sdram clock)
+    // - PB5:  FMC_SDCKE1 (sdram bank 2 clock enable)
+    // - PB6:  FMC_SDNE1  (sdram bank 2 chip enable)
+    // - PF0:  FMC_A0
+    // - PF1:  FMC_A1
+    // - PF2:  FMC_A2
+    // - PF3:  FMC_A3
+    // - PF4:  FMC_A4
+    // - PF5:  FMC_A5
+    // - PF12: FMC_A6
+    // - PF13: FMC_A7
+    // - PF14: FMC_A8
+    // - PF15: FMC_A9
+    // - PG0:  FMC_A10
+    // - PG1:  FMC_A11
+    // - PG2:  FMC_A12    (used only by the 32MB ram, not by the 8MB one)
+    // - PD14: FMC_D0
+    // - PD15: FMC_D1
+    // - PD0:  FMC_D2
+    // - PD1:  FMC_D3
+    // - PE7:  FMC_D4
+    // - PE8:  FMC_D5
+    // - PE9:  FMC_D6
+    // - PE10: FMC_D7
+    // - PE11: FMC_D8
+    // - PE12: FMC_D9
+    // - PE13: FMC_D10
+    // - PE14: FMC_D11
+    // - PE15: FMC_D12
+    // - PD8:  FMC_D13
+    // - PD9:  FMC_D14
+    // - PD10: FMC_D15
+
+    // - PG4:  FMC_BA0
+    // - PG5:  FMC_BA1
+    // - PF11: FMC_SDNRAS
+    // - PG15: FMC_SDNCAS
+    // - PC0:  FMC_SDNWE
+    // - PE0:  FMC_NBL0
+    // - PE1:  FMC_NBL1
+
+    // All SDRAM GPIOs needs to be configured with alternate function 12 and
+    // maximum speed
+
+    // WARNING: The current configuration is for the 8MB ram
+
+    // Alternate functions
+    GPIOB->AFR[0] = 0x0cc00000;
+    GPIOC->AFR[0] = 0x0000000c;
+    GPIOD->AFR[0] = 0x000000cc;
+    GPIOD->AFR[1] = 0xcc000ccc;
+    GPIOE->AFR[0] = 0xc00000cc;
+    GPIOE->AFR[1] = 0xcccccccc;
+    GPIOF->AFR[0] = 0x00cccccc;
+    GPIOF->AFR[1] = 0xccccc000;
+    GPIOG->AFR[0] = 0x00cc0ccc;
+    GPIOG->AFR[1] = 0xc000000c;
+
+    // Mode
+    GPIOB->MODER = 0x00002800;
+    GPIOC->MODER = 0x00000002;
+    GPIOD->MODER = 0xa02a000a;
+    GPIOE->MODER = 0xaaaa800a;
+    GPIOF->MODER = 0xaa800aaa;
+    GPIOG->MODER = 0x80020a2a;
+
+    // Speed (high speed for all, very high speed for SDRAM pins)
+    GPIOB->OSPEEDR = 0x00003c00;
+    GPIOC->OSPEEDR = 0x00000003;
+    GPIOD->OSPEEDR = 0xf03f000f;
+    GPIOE->OSPEEDR = 0xffffc00f;
+    GPIOF->OSPEEDR = 0xffc00fff;
+    GPIOG->OSPEEDR = 0xc0030f3f;
+
+    // Since we'we un-configured PB3 and PB4 (by default they are SWO and NJRST)
+    // finish the job and remove the default pull-up
+    GPIOB->PUPDR = 0;
+
+    // Enable the SDRAM controller clock
+    RCC->AHB3ENR |= RCC_AHB3ENR_FMCEN;
+    RCC_SYNC();
+
+    // The SDRAM is a AS4C4M16SA-6TAN
+    // HCLK = 216MHz -> SDRAM clock = HCLK/2 = 133MHz
+
+    // 1. Memory device features
+    FMC_Bank5_6->SDCR[0] = 0                     // 0 delay after CAS latency
+                           | FMC_SDCR1_RBURST    // Enable read bursts
+                           | FMC_SDCR1_SDCLK_1;  // SDCLK = HCLK / 2
+    FMC_Bank5_6->SDCR[1] = 0                     // Write accesses allowed
+                           | FMC_SDCR2_CAS_1     // 2 cycles CAS latency
+                           | FMC_SDCR2_NB        // 4 internal banks
+                           | FMC_SDCR2_MWID_0    // 16 bit data bus
+                           | FMC_SDCR2_NR_1      // 13 bit row address
+                           // cppcheck-suppress duplicateExpression
+                           | 0;  // 8 bit column address
+
+// 2. Memory device timings
+#ifdef SYSCLK_FREQ_216MHz
+    // SDRAM timings. One clock cycle is 9.26ns
+    FMC_Bank5_6->SDTR[0] =
+        (2 - 1) << FMC_SDTR1_TRP_Pos     // 2 cycles TRP  (18.52ns > 18ns)
+        | (7 - 1) << FMC_SDTR1_TRC_Pos;  // 7 cycles TRC  (64.82ns > 60ns)
+    FMC_Bank5_6->SDTR[1] =
+        (2 - 1) << FMC_SDTR1_TRCD_Pos     // 2 cycles TRCD (18.52ns > 18ns)
+        | (2 - 1) << FMC_SDTR1_TWR_Pos    // 2 cycles TWR  (min 2cc > 12ns)
+        | (5 - 1) << FMC_SDTR1_TRAS_Pos   // 5 cycles TRAS (46.3ns  > 42ns)
+        | (7 - 1) << FMC_SDTR1_TXSR_Pos   // 7 cycles TXSR (74.08ns > 61.5ns)
+        | (2 - 1) << FMC_SDTR1_TMRD_Pos;  // 2 cycles TMRD (min 2cc > 12ns)
+#else
+#error No SDRAM timings for this clock
+#endif
+
+    // 3. Enable the bank 2 clock
+    FMC_Bank5_6->SDCMR = FMC_SDCMR_MODE_0   // Clock Configuration Enable
+                         | FMC_SDCMR_CTB2;  // Bank 2
+    sdramCommandWait();
+
+    // 4. Wait during command execution
+    delayUs(100);
+
+    // 5. Issue a "Precharge All" command
+    FMC_Bank5_6->SDCMR = FMC_SDCMR_MODE_1   // Precharge all
+                         | FMC_SDCMR_CTB2;  // Bank 2
+    sdramCommandWait();
+
+    // 6. Issue Auto-Refresh commands
+    FMC_Bank5_6->SDCMR = FMC_SDCMR_MODE_1 | FMC_SDCMR_MODE_0  // Auto-Refresh
+                         | FMC_SDCMR_CTB2                     // Bank 2
+                         | (8 - 1) << FMC_SDCMR_NRFS_Pos;     // 2 Auto-Refresh
+    sdramCommandWait();
+
+    // 7. Issue a Load Mode Register command
+    FMC_Bank5_6->SDCMR = FMC_SDCMR_MODE_2               /// Load mode register
+                         | FMC_SDCMR_CTB2               // Bank 2
+                         | 0x220 << FMC_SDCMR_MRD_Pos;  // CAS = 2, burst = 1
+    sdramCommandWait();
+
+// 8. Program the refresh rate (4K / 32ms)
+// 64ms / 8192 = 7.8125us
+#ifdef SYSCLK_FREQ_216MHz
+    // 7.8125us * 133MHz = 1039 - 20 = 1019
+    FMC_Bank5_6->SDRTR = 1019 << FMC_SDRTR_COUNT_Pos;
+#else
+#error No SDRAM refresh timings for this clock
+#endif
+}
+
+void IRQbspInit()
+{
+    // Enable USART1 pins port
+    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN;
+
+    userLed1::mode(Mode::OUTPUT);
+    userLed2::mode(Mode::OUTPUT);
+    userLed3_1::mode(Mode::OUTPUT);
+    userLed3_2::mode(Mode::OUTPUT);
+    userLed4::mode(Mode::OUTPUT);
+    userSwitch::mode(Mode::INPUT);
+
+    using namespace interfaces;
+    spi1::sck::mode(Mode::ALTERNATE);
+    spi1::sck::alternateFunction(5);
+    spi1::miso::mode(Mode::ALTERNATE);
+    spi1::miso::alternateFunction(5);
+    spi1::mosi::mode(Mode::ALTERNATE);
+    spi1::mosi::alternateFunction(5);
+
+    spi3::sck::mode(Mode::ALTERNATE);
+    spi3::sck::alternateFunction(6);
+    spi3::miso::mode(Mode::ALTERNATE);
+    spi3::miso::alternateFunction(6);
+    spi3::mosi::mode(Mode::ALTERNATE);
+    spi3::mosi::alternateFunction(5);
+
+    spi4::sck::mode(Mode::ALTERNATE);
+    spi4::sck::alternateFunction(5);
+    spi4::miso::mode(Mode::ALTERNATE);
+    spi4::miso::alternateFunction(5);
+    spi4::mosi::mode(Mode::ALTERNATE);
+    spi4::mosi::alternateFunction(5);
+
+    spi6::sck::mode(Mode::ALTERNATE);
+    spi6::sck::alternateFunction(5);
+    spi6::miso::mode(Mode::ALTERNATE);
+    spi6::miso::alternateFunction(5);
+    spi6::mosi::mode(Mode::ALTERNATE);
+    spi6::mosi::alternateFunction(5);
+
+    i2c1::sda::mode(Mode::ALTERNATE);
+    i2c1::sda::alternateFunction(4);
+    i2c1::scl::mode(Mode::ALTERNATE);
+    i2c1::scl::alternateFunction(4);
+
+    can1::rx::mode(Mode::ALTERNATE);
+    can1::rx::alternateFunction(9);
+    can1::tx::mode(Mode::ALTERNATE);
+    can1::tx::alternateFunction(9);
+
+    can2::rx::mode(Mode::ALTERNATE);
+    can2::rx::alternateFunction(9);
+    can2::tx::mode(Mode::ALTERNATE);
+    can2::tx::alternateFunction(9);
+
+    usart1::tx::mode(Mode::ALTERNATE);
+    usart1::tx::alternateFunction(7);
+    usart1::rx::mode(Mode::ALTERNATE);
+    usart1::rx::alternateFunction(7);
+
+    usart2::tx::mode(Mode::ALTERNATE);
+    usart2::tx::alternateFunction(7);
+    usart2::rx::mode(Mode::ALTERNATE);
+    usart2::rx::alternateFunction(7);
+
+    uart4::tx::mode(Mode::ALTERNATE);
+    uart4::tx::alternateFunction(8);
+    uart4::rx::mode(Mode::ALTERNATE);
+    uart4::rx::alternateFunction(8);
+
+    using namespace timers;
+    tim3ch1::mode(Mode::ALTERNATE);
+    tim3ch1::alternateFunction(2);
+    tim3ch2::mode(Mode::ALTERNATE);
+    tim3ch2::alternateFunction(2);
+    tim1ch1::mode(Mode::ALTERNATE);
+    tim1ch1::alternateFunction(1);
+    tim12ch2::mode(Mode::ALTERNATE);
+    tim12ch2::alternateFunction(9);
+
+    using namespace sensors;
+    LSM6DSRX::cs::mode(Mode::OUTPUT);
+    LSM6DSRX::cs::getPin().high();
+    LSM6DSRX::interrupt1::mode(Mode::INPUT);
+    LSM6DSRX::interrupt2::mode(Mode::INPUT);
+
+    H3LIS331DL::cs::mode(Mode::OUTPUT);
+    H3LIS331DL::cs::getPin().high();
+
+    LIS2MDL::cs::mode(Mode::OUTPUT);
+    LIS2MDL::cs::getPin().high();
+
+    LPS22DF::cs::mode(Mode::OUTPUT);
+    LPS22DF::cs::getPin().high();
+    LPS22DF::interrupt::mode(Mode::INPUT);
+
+    GPS::cs::mode(Mode::OUTPUT);
+    GPS::cs::getPin().high();
+
+    VN100::cs::mode(Mode::OUTPUT);
+    VN100::cs::getPin().high();
+
+    ADS131::cs::mode(Mode::OUTPUT);
+    ADS131::cs::getPin().high();
+
+    using namespace radio;
+    cs::mode(Mode::OUTPUT);
+    cs::getPin().high();
+    dio0::mode(Mode::INPUT);
+    dio1::mode(Mode::INPUT);
+    dio3::mode(Mode::INPUT);
+    tx_enable::mode(Mode::OUTPUT);
+    rx_enable::mode(Mode::OUTPUT);
+
+    using namespace gpios;
+    cut_trigger::mode(Mode::OUTPUT);
+    cut_sense::mode(Mode::INPUT);
+    exp_enable::mode(Mode::OUTPUT);
+    // status_led::mode(Mode::OUTPUT);
+    camera_enable::mode(Mode::OUTPUT);
+    liftoff_detach::mode(Mode::INPUT);
+    nosecone_detach::mode(Mode::INPUT);
+    exp_sense::mode(Mode::INPUT);
+
+    using namespace actuators;
+    buzzer::mode(Mode::ALTERNATE_PULL_DOWN);
+
+    DefaultConsole::instance().IRQset(intrusive_ref_ptr<Device>(new STM32Serial(
+        defaultSerial, defaultSerialSpeed, STM32Serial::NOFLOWCTRL)));
+}
+
+void bspInit2()
+{
+#ifdef WITH_FILESYSTEM
+    basicFilesystemSetup(SDIODriver::instance());
+#endif  // WITH_FILESYSTEM
+
+#ifdef WITH_BACKUP_SRAM
+    // Print the reset reason
+    bootlog("Reset reson: ");
+    switch (SGM::instance().lastResetReason())
+    {
+        case ResetReason::RST_LOW_PWR:
+            bootlog("low power\n");
+            break;
+        case ResetReason::RST_WINDOW_WDG:
+            bootlog("window watchdog\n");
+            break;
+        case ResetReason::RST_INDEPENDENT_WDG:
+            bootlog("indeendent watchdog\n");
+            break;
+        case ResetReason::RST_SW:
+            bootlog("software reset\n");
+            break;
+        case ResetReason::RST_POWER_ON:
+            bootlog("power on\n");
+            break;
+        case ResetReason::RST_PIN:
+            bootlog("reset pin\n");
+            break;
+        case ResetReason::RST_UNKNOWN:
+            bootlog("unknown\n");
+            break;
+    }
+#endif  // WITH_BACKUP_SRAM
+}
+
+//
+// Shutdown and reboot
+//
+
+void shutdown()
+{
+    ioctl(STDOUT_FILENO, IOCTL_SYNC, 0);
+
+#ifdef WITH_FILESYSTEM
+    FilesystemManager::instance().umountAll();
+#endif  // WITH_FILESYSTEM
+
+    disableInterrupts();
+    for (;;)
+        ;
+}
+
+void reboot()
+{
+    ioctl(STDOUT_FILENO, IOCTL_SYNC, 0);
+
+#ifdef WITH_FILESYSTEM
+    FilesystemManager::instance().umountAll();
+#endif  // WITH_FILESYSTEM
+
+    disableInterrupts();
+    miosix_private::IRQsystemReboot();
+}
+
+}  // namespace miosix
diff --git a/src/bsps/stm32f767zi_death_stack_v4/interfaces-impl/bsp_impl.h b/src/bsps/stm32f767zi_death_stack_v4/interfaces-impl/bsp_impl.h
new file mode 100644
index 000000000..f39efd825
--- /dev/null
+++ b/src/bsps/stm32f767zi_death_stack_v4/interfaces-impl/bsp_impl.h
@@ -0,0 +1,116 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * 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.
+ */
+
+/***************************************************************************
+ * bsp_impl.h Part of the Miosix Embedded OS.
+ * Board support package, this file initializes hardware.
+ ***************************************************************************/
+
+#ifndef BSP_IMPL_H
+#define BSP_IMPL_H
+
+#include "config/miosix_settings.h"
+#include "interfaces/gpio.h"
+
+namespace miosix
+{
+
+/**
+\addtogroup Hardware
+\{
+*/
+
+/**
+ * \internal
+ * Called by stage_1_boot.cpp to enable the SDRAM before initializing .data/.bss
+ * Requires the CPU clock to be already configured (running from the PLL)
+ */
+void configureSdram();
+
+/**
+ * \internal
+ * Board pin definition
+ */
+typedef Gpio<GPIOB_BASE, 7> userLed1;
+typedef Gpio<GPIOE_BASE, 3> userLed2;
+typedef Gpio<GPIOC_BASE, 13> userLed3_1;
+typedef Gpio<GPIOG_BASE, 9> userLed3_2;
+typedef Gpio<GPIOC_BASE, 2> userLed4;
+typedef Gpio<GPIOB_BASE, 2> userSwitch;
+
+inline void ledOn()
+{
+    userLed1::high();
+    userLed2::high();
+    userLed3_1::high();
+    userLed3_2::high();
+    userLed4::high();
+}
+
+inline void ledOff()
+{
+    userLed1::low();
+    userLed2::low();
+    userLed3_1::low();
+    userLed3_2::low();
+    userLed4::low();
+}
+
+inline void led1On() { userLed1::high(); }
+
+inline void led1Off() { userLed1::low(); }
+
+inline void led2On() { userLed2::high(); }
+
+inline void led2Off() { userLed2::low(); }
+
+inline void led3On()
+{
+    userLed3_1::high();
+    userLed3_2::high();
+}
+
+inline void led3Off()
+{
+    userLed3_1::low();
+    userLed3_2::low();
+}
+
+/**
+ * Polls the SD card sense GPIO.
+ *
+ * This board has no SD card whatsoever, but a card can be connected to the
+ * following GPIOs:
+ * TODO: never tested
+ *
+ * \return true. As there's no SD card sense switch, let's pretend that
+ * the card is present.
+ */
+inline bool sdCardSense() { return true; }
+
+/**
+\}
+*/
+
+}  // namespace miosix
+
+#endif  // BSP_IMPL_H
diff --git a/src/bsps/stm32f767zi_death_stack_v4/interfaces-impl/hwmapping.h b/src/bsps/stm32f767zi_death_stack_v4/interfaces-impl/hwmapping.h
new file mode 100644
index 000000000..8b6166f3b
--- /dev/null
+++ b/src/bsps/stm32f767zi_death_stack_v4/interfaces-impl/hwmapping.h
@@ -0,0 +1,221 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Matteo Pignataro
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include "interfaces/gpio.h"
+
+namespace miosix
+{
+namespace interfaces
+{
+namespace spi1
+{
+using sck  = Gpio<GPIOA_BASE, 5>;
+using miso = Gpio<GPIOA_BASE, 6>;
+using mosi = Gpio<GPIOA_BASE, 7>;
+}  // namespace spi1
+
+namespace spi3
+{
+using sck  = Gpio<GPIOB_BASE, 3>;
+using miso = Gpio<GPIOB_BASE, 4>;
+using mosi = Gpio<GPIOD_BASE, 6>;
+}  // namespace spi3
+
+namespace spi4
+{
+using sck  = Gpio<GPIOE_BASE, 2>;
+using miso = Gpio<GPIOE_BASE, 5>;
+using mosi = Gpio<GPIOE_BASE, 6>;
+}  // namespace spi4
+
+namespace spi6
+{
+using sck  = Gpio<GPIOG_BASE, 13>;
+using miso = Gpio<GPIOG_BASE, 12>;
+using mosi = Gpio<GPIOG_BASE, 14>;
+}  // namespace spi6
+
+namespace i2c1
+{
+using sda = Gpio<GPIOB_BASE, 9>;
+using scl = Gpio<GPIOB_BASE, 8>;
+}  // namespace i2c1
+
+namespace can1
+{
+using rx = Gpio<GPIOA_BASE, 11>;
+using tx = Gpio<GPIOA_BASE, 12>;
+}  // namespace can1
+
+namespace can2
+{
+using rx = Gpio<GPIOB_BASE, 12>;
+using tx = Gpio<GPIOB_BASE, 13>;
+}  // namespace can2
+
+namespace usart1
+{
+using tx = Gpio<GPIOA_BASE, 9>;
+using rx = Gpio<GPIOA_BASE, 10>;
+}  // namespace usart1
+
+namespace usart2
+{
+using tx = Gpio<GPIOA_BASE, 2>;
+using rx = Gpio<GPIOA_BASE, 3>;
+}  // namespace usart2
+
+namespace uart4
+{
+using tx = Gpio<GPIOA_BASE, 0>;
+using rx = Gpio<GPIOA_BASE, 1>;
+}  // namespace uart4
+
+namespace timers
+{
+using tim3ch1  = Gpio<GPIOC_BASE, 6>;   // Airbrakes servo   - Servo1 Payload
+using tim3ch2  = Gpio<GPIOC_BASE, 7>;   // Auxiliary         - Servo2 Payload
+using tim1ch1  = Gpio<GPIOA_BASE, 8>;   // Buzzer
+using tim12ch2 = Gpio<GPIOB_BASE, 15>;  // Expulsion
+}  // namespace timers
+
+}  // namespace interfaces
+
+namespace sensors
+{
+namespace LSM6DSRX
+{
+using sck        = interfaces::spi1::sck;
+using miso       = interfaces::spi1::miso;
+using mosi       = interfaces::spi1::mosi;
+using cs         = Gpio<GPIOC_BASE, 4>;
+using interrupt1 = Gpio<GPIOD_BASE, 13>;
+using interrupt2 = Gpio<GPIOG_BASE, 7>;
+}  // namespace LSM6DSRX
+
+namespace H3LIS331DL
+{
+using sck  = interfaces::spi3::sck;
+using miso = interfaces::spi3::miso;
+using mosi = interfaces::spi3::mosi;
+using cs   = Gpio<GPIOD_BASE, 3>;
+}  // namespace H3LIS331DL
+
+namespace LIS2MDL
+{
+using sck  = interfaces::spi3::sck;
+using miso = interfaces::spi3::miso;
+using mosi = interfaces::spi3::mosi;
+using cs   = Gpio<GPIOD_BASE, 5>;
+}  // namespace LIS2MDL
+
+namespace LPS22DF
+{
+using sck       = interfaces::spi3::sck;
+using miso      = interfaces::spi3::miso;
+using mosi      = interfaces::spi3::mosi;
+using cs        = Gpio<GPIOD_BASE, 7>;
+using interrupt = Gpio<GPIOB_BASE, 11>;
+}  // namespace LPS22DF
+
+namespace GPS
+{
+using sck  = interfaces::spi4::sck;
+using miso = interfaces::spi4::miso;
+using mosi = interfaces::spi4::mosi;
+using cs   = Gpio<GPIOE_BASE, 4>;
+}  // namespace GPS
+
+namespace VN100
+{
+using sck  = interfaces::spi4::sck;
+using miso = interfaces::spi4::miso;
+using mosi = interfaces::spi4::mosi;
+using cs   = Gpio<GPIOB_BASE, 14>;
+}  // namespace VN100
+
+namespace VN100_SERIAL
+{
+using tx = interfaces::uart4::tx;
+using rx = interfaces::uart4::rx;
+}  // namespace VN100_SERIAL
+
+namespace ADS131
+{
+using sck  = interfaces::spi4::sck;
+using miso = interfaces::spi4::miso;
+using mosi = interfaces::spi4::mosi;
+using cs   = Gpio<GPIOG_BASE, 10>;
+}  // namespace ADS131
+
+namespace LPS28DFW_1
+{
+using sda = interfaces::i2c1::sda;
+using scl = interfaces::i2c1::scl;
+}  // namespace LPS28DFW_1
+
+namespace LPS28DFW_2
+{
+using sda = interfaces::i2c1::sda;
+using scl = interfaces::i2c1::scl;
+}  // namespace LPS28DFW_2
+
+}  // namespace sensors
+
+namespace radio
+{
+using sck       = interfaces::spi6::sck;
+using miso      = interfaces::spi6::miso;
+using mosi      = interfaces::spi6::mosi;
+using cs        = Gpio<GPIOG_BASE, 11>;
+using dio0      = Gpio<GPIOC_BASE, 3>;
+using dio1      = Gpio<GPIOD_BASE, 4>;
+using dio3      = Gpio<GPIOC_BASE, 5>;
+using rx_enable = Gpio<GPIOB_BASE, 0>;
+using tx_enable = Gpio<GPIOC_BASE, 1>;
+}  // namespace radio
+
+namespace actuators
+{
+using airbrakes       = interfaces::timers::tim3ch1;
+using expulsion       = interfaces::timers::tim12ch2;
+using buzzer          = interfaces::timers::tim1ch1;
+using parafoil_servo1 = interfaces::timers::tim3ch1;
+using parafoil_servo2 = interfaces::timers::tim3ch2;
+}  // namespace actuators
+
+namespace gpios
+{
+using cut_trigger     = Gpio<GPIOA_BASE, 15>;
+using cut_sense       = Gpio<GPIOD_BASE, 12>;
+using exp_enable      = Gpio<GPIOB_BASE, 1>;
+using status_led      = Gpio<GPIOA_BASE, 14>;
+using camera_enable   = Gpio<GPIOA_BASE, 12>;
+using liftoff_detach  = Gpio<GPIOA_BASE, 11>;
+using nosecone_detach = Gpio<GPIOA_BASE, 4>;
+using exp_sense       = Gpio<GPIOG_BASE, 6>;
+
+}  // namespace gpios
+
+}  // namespace miosix
\ No newline at end of file
diff --git a/src/bsps/stm32f767zi_death_stack_v4/stm32_2m+16m_xram.ld b/src/bsps/stm32f767zi_death_stack_v4/stm32_2m+16m_xram.ld
new file mode 100644
index 000000000..5b3545abd
--- /dev/null
+++ b/src/bsps/stm32f767zi_death_stack_v4/stm32_2m+16m_xram.ld
@@ -0,0 +1,190 @@
+/*
+ * C++ enabled linker script for stm32f769ni (2M FLASH, 512K RAM, 16MB XRAM)
+ * Developed by TFT: Terraneo Federico Technologies
+ * Optimized for use with the Miosix kernel
+ */
+
+/*
+ * This linker script puts:
+ * - read only data and code (.text, .rodata, .eh_*) in FLASH
+ * - the 512Byte main (IRQ) stack, .data and .bss in the DTCM 128KB RAM
+ * - .data, .bss, stacks and heap in the external 16MB SDRAM.
+ */
+
+/*
+ * The main stack is used for interrupt handling by the kernel.
+ *
+ * *** Readme ***
+ * This linker script places the main stack (used by the kernel for interrupts)
+ * at the bottom of the ram, instead of the top. This is done for two reasons:
+ *
+ * - as an optimization for microcontrollers with little ram memory. In fact
+ *   the implementation of malloc from newlib requests memory to the OS in 4KB
+ *   block (except the first block that can be smaller). This is probably done
+ *   for compatibility with OSes with an MMU and paged memory. To see why this
+ *   is bad, consider a microcontroller with 8KB of ram: when malloc finishes
+ *   up the first 4KB it will call _sbrk_r asking for a 4KB block, but this will
+ *   fail because the top part of the ram is used by the main stack. As a
+ *   result, the top part of the memory will not be used by malloc, even if
+ *   available (and it is nearly *half* the ram on an 8KB mcu). By placing the
+ *   main stack at the bottom of the ram, the upper 4KB block will be entirely
+ *   free and available as heap space.
+ *
+ * - In case of main stack overflow the cpu will fault because access to memory
+ *   before the beginning of the ram faults. Instead with the default stack
+ *   placement the main stack will silently collide with the heap.
+ * Note: if increasing the main stack size also increase the ORIGIN value in
+ * the MEMORY definitions below accordingly.
+ */
+
+_main_stack_size = 512;                             /* main stack = 512Bytes */
+_main_stack_top  = 0x20000000 + _main_stack_size;
+ASSERT(_main_stack_size   % 8 == 0, "MAIN stack size error");
+
+/* Mapping the heap into XRAM */
+_heap_end = 0xd0000000 + 16M;                        /* end of available ram */
+
+/* Identify the Entry Point  */
+ENTRY(_Z13Reset_Handlerv)
+
+/*
+ * Specify the memory areas
+ *
+ * NOTE: starting at 0x20000000 there's 128KB of DTCM (Data Tightly Coupled
+ * Memory). Technically, we could use this as normal RAM as there's a way for
+ * the DMA to access it, but the datasheet is unclear about performance
+ * penalties for doing so. To avoid nonuniform DMA memory access latencies,
+ * we leave this 128KB DTCM unused except for the first 512Bytes which are for
+ * the interrupt stack. This leaves us with 384KB of RAM
+ */
+MEMORY
+{
+    xram(wx)  : ORIGIN = 0xd0000000, LENGTH =  16M
+    sram(wx)  : ORIGIN = 0x20020000, LENGTH = 384K
+    dtcm(wx)  : ORIGIN = 0x20000000, LENGTH = 128K    /* Used for main stack */
+    bram(rw)  : ORIGIN = 0x40024000, LENGTH =   4K    /* Bakup SRAM */
+    flash(rx) : ORIGIN = 0x08000000, LENGTH =   2M
+}
+
+/* now define the output sections  */
+SECTIONS
+{
+    . = 0;
+    
+    /* .text section: code goes to flash */
+    .text :
+    {
+        /* Startup code must go at address 0 */
+        KEEP(*(.isr_vector))
+        
+        *(.text)
+        *(.text.*)
+        *(.gnu.linkonce.t.*)
+        /* these sections for thumb interwork? */
+        *(.glue_7)
+        *(.glue_7t)
+        /* these sections for C++? */
+        *(.gcc_except_table)
+        *(.gcc_except_table.*)
+        *(.ARM.extab*)
+        *(.gnu.linkonce.armextab.*)
+
+        . = ALIGN(4);
+        /* .rodata: constant data */
+        *(.rodata)
+        *(.rodata.*)
+        *(.gnu.linkonce.r.*)
+
+        /* C++ Static constructors/destructors (eabi) */
+        . = ALIGN(4);
+        KEEP(*(.init))
+        
+        . = ALIGN(4);
+        __miosix_init_array_start = .;
+        KEEP (*(SORT(.miosix_init_array.*)))
+        KEEP (*(.miosix_init_array))
+        __miosix_init_array_end = .;
+
+        . = ALIGN(4);
+        __preinit_array_start = .;
+        KEEP (*(.preinit_array))
+        __preinit_array_end = .;
+
+        . = ALIGN(4);
+        __init_array_start = .;
+        KEEP (*(SORT(.init_array.*)))
+        KEEP (*(.init_array))
+        __init_array_end = .;
+
+        . = ALIGN(4);
+        KEEP(*(.fini))
+
+        . = ALIGN(4);
+        __fini_array_start = .;
+        KEEP (*(.fini_array))
+        KEEP (*(SORT(.fini_array.*)))
+        __fini_array_end = .;
+
+        /* C++ Static constructors/destructors (elf)  */
+        . = ALIGN(4);
+        _ctor_start = .;
+        KEEP (*crtbegin.o(.ctors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .ctors))
+        KEEP (*(SORT(.ctors.*)))
+        KEEP (*crtend.o(.ctors))
+       _ctor_end = .;
+
+        . = ALIGN(4);
+        KEEP (*crtbegin.o(.dtors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .dtors))
+        KEEP (*(SORT(.dtors.*)))
+        KEEP (*crtend.o(.dtors))
+    } > flash
+
+    /* .ARM.exidx is sorted, so has to go in its own output section.  */
+    __exidx_start = .;
+    .ARM.exidx :
+    {
+        *(.ARM.exidx* .gnu.linkonce.armexidx.*)
+    } > flash
+    __exidx_end = .;
+
+	/*
+     * .data section: global variables go to xram, but also store a copy to
+     * flash to initialize them
+     */
+    .data : ALIGN(8)
+    {
+        _data = .;
+        *(.data)
+        *(.data.*)
+        *(.gnu.linkonce.d.*)
+        . = ALIGN(8);
+        _edata = .;
+    } > xram AT > flash
+    _etext = LOADADDR(.data);
+
+    /* .bss section: uninitialized global variables go to xram */
+    _bss_start = .;
+    .bss :
+    {
+        *(.bss)
+        *(.bss.*)
+        *(.gnu.linkonce.b.*)
+        . = ALIGN(8);
+    } > xram
+    _bss_end = .;
+
+    _end = .;
+    PROVIDE(end = .);
+    
+    .preserve(NOLOAD) : ALIGN(4)
+    {
+        _preserve_start = .;
+        . = ALIGN(4);
+        *(.preserve);
+        *(.preserve*);
+        . = ALIGN(4);
+        _preserve_end = .;
+    } > bram
+}
diff --git a/src/bsps/stm32f767zi_death_stack_v4/stm32_2m+384k_ram.ld b/src/bsps/stm32f767zi_death_stack_v4/stm32_2m+384k_ram.ld
new file mode 100644
index 000000000..497bd5198
--- /dev/null
+++ b/src/bsps/stm32f767zi_death_stack_v4/stm32_2m+384k_ram.ld
@@ -0,0 +1,189 @@
+/*
+ * C++ enabled linker script for stm32f767zi (2M FLASH, 512K RAM)
+ * Developed by TFT: Terraneo Federico Technologies
+ * Optimized for use with the Miosix kernel
+ */
+
+/*
+ * This linker script puts:
+ * - read only data and code (.text, .rodata, .eh_*) in FLASH
+ * - the 512Byte main (IRQ) stack, .data and .bss in the DTCM 128KB RAM
+ * - .data, .bss, stacks and heap in the internal RAM.
+ */
+
+/*
+ * The main stack is used for interrupt handling by the kernel.
+ *
+ * *** Readme ***
+ * This linker script places the main stack (used by the kernel for interrupts)
+ * at the bottom of the ram, instead of the top. This is done for two reasons:
+ *
+ * - as an optimization for microcontrollers with little ram memory. In fact
+ *   the implementation of malloc from newlib requests memory to the OS in 4KB
+ *   block (except the first block that can be smaller). This is probably done
+ *   for compatibility with OSes with an MMU and paged memory. To see why this
+ *   is bad, consider a microcontroller with 8KB of ram: when malloc finishes
+ *   up the first 4KB it will call _sbrk_r asking for a 4KB block, but this will
+ *   fail because the top part of the ram is used by the main stack. As a
+ *   result, the top part of the memory will not be used by malloc, even if
+ *   available (and it is nearly *half* the ram on an 8KB mcu). By placing the
+ *   main stack at the bottom of the ram, the upper 4KB block will be entirely
+ *   free and available as heap space.
+ *
+ * - In case of main stack overflow the cpu will fault because access to memory
+ *   before the beginning of the ram faults. Instead with the default stack
+ *   placement the main stack will silently collide with the heap.
+ * Note: if increasing the main stack size also increase the ORIGIN value in
+ * the MEMORY definitions below accordingly.
+ */
+
+_main_stack_size = 512;                             /* main stack = 512Bytes */
+_main_stack_top  = 0x20000000 + _main_stack_size;
+ASSERT(_main_stack_size   % 8 == 0, "MAIN stack size error");
+
+/* Mapping the heap to the end of SRAM2 */
+_heap_end = 0x20000000 + 512K;                       /* end of available ram */
+
+/* Identify the Entry Point  */
+ENTRY(_Z13Reset_Handlerv)
+
+/*
+ * Specify the memory areas
+ *
+ * NOTE: starting at 0x20000000 there's 128KB of DTCM (Data Tightly Coupled
+ * Memory). Technically, we could use this as normal RAM as there's a way for
+ * the DMA to access it, but the datasheet is unclear about performance
+ * penalties for doing so. To avoid nonuniform DMA memory access latencies,
+ * we leave this 128KB DTCM unused except for the first 512Bytes which are for
+ * the interrupt stack. This leaves us with 384KB of RAM
+ */
+MEMORY
+{
+    sram(wx)  : ORIGIN = 0x20020000, LENGTH = 384K
+    dtcm(wx)  : ORIGIN = 0x20000000, LENGTH = 128K    /* Used for main stack */
+    bram(rw)  : ORIGIN = 0x40024000, LENGTH =   4K    /* Bakup SRAM */
+    flash(rx) : ORIGIN = 0x08000000, LENGTH =   2M
+}
+
+/* now define the output sections  */
+SECTIONS
+{
+    . = 0;
+    
+    /* .text section: code goes to flash */
+    .text :
+    {
+        /* Startup code must go at address 0 */
+        KEEP(*(.isr_vector))
+        
+        *(.text)
+        *(.text.*)
+        *(.gnu.linkonce.t.*)
+        /* these sections for thumb interwork? */
+        *(.glue_7)
+        *(.glue_7t)
+        /* these sections for C++? */
+        *(.gcc_except_table)
+        *(.gcc_except_table.*)
+        *(.ARM.extab*)
+        *(.gnu.linkonce.armextab.*)
+
+        . = ALIGN(4);
+        /* .rodata: constant data */
+        *(.rodata)
+        *(.rodata.*)
+        *(.gnu.linkonce.r.*)
+
+        /* C++ Static constructors/destructors (eabi) */
+        . = ALIGN(4);
+        KEEP(*(.init))
+        
+        . = ALIGN(4);
+        __miosix_init_array_start = .;
+        KEEP (*(SORT(.miosix_init_array.*)))
+        KEEP (*(.miosix_init_array))
+        __miosix_init_array_end = .;
+
+        . = ALIGN(4);
+        __preinit_array_start = .;
+        KEEP (*(.preinit_array))
+        __preinit_array_end = .;
+
+        . = ALIGN(4);
+        __init_array_start = .;
+        KEEP (*(SORT(.init_array.*)))
+        KEEP (*(.init_array))
+        __init_array_end = .;
+
+        . = ALIGN(4);
+        KEEP(*(.fini))
+
+        . = ALIGN(4);
+        __fini_array_start = .;
+        KEEP (*(.fini_array))
+        KEEP (*(SORT(.fini_array.*)))
+        __fini_array_end = .;
+
+        /* C++ Static constructors/destructors (elf)  */
+        . = ALIGN(4);
+        _ctor_start = .;
+        KEEP (*crtbegin.o(.ctors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .ctors))
+        KEEP (*(SORT(.ctors.*)))
+        KEEP (*crtend.o(.ctors))
+       _ctor_end = .;
+
+        . = ALIGN(4);
+        KEEP (*crtbegin.o(.dtors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .dtors))
+        KEEP (*(SORT(.dtors.*)))
+        KEEP (*crtend.o(.dtors))
+    } > flash
+
+    /* .ARM.exidx is sorted, so has to go in its own output section.  */
+    __exidx_start = .;
+    .ARM.exidx :
+    {
+        *(.ARM.exidx* .gnu.linkonce.armexidx.*)
+    } > flash
+    __exidx_end = .;
+
+	/*
+     * .data section: global variables go to sram, but also store a copy to
+     * flash to initialize them
+     */
+    .data : ALIGN(8)
+    {
+        _data = .;
+        *(.data)
+        *(.data.*)
+        *(.gnu.linkonce.d.*)
+        . = ALIGN(8);
+        _edata = .;
+    } > sram AT > flash
+    _etext = LOADADDR(.data);
+
+    /* .bss section: uninitialized global variables go to sram */
+    _bss_start = .;
+    .bss :
+    {
+        *(.bss)
+        *(.bss.*)
+        *(.gnu.linkonce.b.*)
+        . = ALIGN(8);
+    } > sram
+    _bss_end = .;
+
+    _end = .;
+    PROVIDE(end = .);
+    
+    .preserve(NOLOAD) : ALIGN(4)
+    {
+        _preserve_start = .;
+        . = ALIGN(4);
+        *(.preserve);
+        *(.preserve*);
+        . = ALIGN(4);
+        _preserve_end = .;
+    } > bram
+}
diff --git a/src/bsps/stm32f767zi_gemini_gs/config/board_options.cmake b/src/bsps/stm32f767zi_gemini_gs/config/board_options.cmake
new file mode 100644
index 000000000..259876fc5
--- /dev/null
+++ b/src/bsps/stm32f767zi_gemini_gs/config/board_options.cmake
@@ -0,0 +1,106 @@
+# Copyright (C) 2023 by Skyward
+#
+# This program is free software; you can redistribute it and/or 
+# it under the terms of the GNU General Public License as published 
+# the Free Software Foundation; either version 2 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.
+#
+# As a special exception, if other files instantiate templates or use
+# macros or inline functions from this file, or you compile this file
+# and link it with other works to produce a work based on this file,
+# this file does not by itself cause the resulting work to be covered
+# by the GNU General Public License. However the source code for this
+# file must still be made available in accordance with the GNU 
+# Public License. This exception does not invalidate any other 
+# why a work based on this file might be covered by the GNU General
+# Public License.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, see <http://www.gnu.org/licenses/>
+
+set(BOARD_NAME stm32f767zi_gemini_gs)
+set(ARCH_NAME cortexM7_stm32f7)
+
+# Base directories with header files for this board
+set(ARCH_PATH ${KPATH}/arch/${ARCH_NAME}/common)
+set(BOARD_PATH ${BOARDCORE_PATH}/src/bsps/${BOARD_NAME})
+set(BOARD_CONFIG_PATH ${BOARDCORE_PATH}/src/bsps/${BOARD_NAME}/config)
+
+# Specify where to find the board specific config/miosix_settings.h
+set(BOARD_MIOSIX_SETTINGS_PATH ${BOARD_PATH})
+
+# Specify where to find the board specific config/mxgui_settings.h
+set(BOARD_MXGUI_SETTINGS_PATH ${BOARD_PATH})
+
+# Optimization flags:
+# -O0 do no optimization, the default if no optimization level is specified
+# -O or -O1 optimize minimally
+# -O2 optimize more
+# -O3 optimize even more
+# -Ofast optimize very aggressively to the point of breaking the standard
+# -Og Optimize debugging experience, enables optimizations that do not
+# interfere with debugging
+# -Os Optimize for size with -O2 optimizations that do not increase code size
+set(OPT_OPTIMIZATION -O2)
+
+# Boot file and linker script
+set(BOOT_FILE ${BOARD_PATH}/core/stage_1_boot.cpp)
+# set(LINKER_SCRIPT ${BOARD_PATH}/stm32_2m+384k_ram.ld)
+set(LINKER_SCRIPT ${BOARD_PATH}/stm32_2m+16m_xram.ld)
+
+# Enables the initialization of the external 16MB SDRAM memory
+set(XRAM -D__ENABLE_XRAM)
+
+# Select clock frequency (HSE_VALUE is the xtal on board, fixed)
+set(CLOCK_FREQ -DHSE_VALUE=25000000 -DSYSCLK_FREQ_216MHz=216000000)
+
+# C++ Exception/rtti support disable flags.
+# To save code size if not using C++ exceptions (nor some STL code which
+# implicitly uses it) uncomment this option.
+# -D__NO_EXCEPTIONS is used by Miosix to know if exceptions are used.
+# set(OPT_EXCEPT -fno-exceptions -fno-rtti -D__NO_EXCEPTIONS)
+
+# Specify a custom flash command
+# This is the program that is invoked when the flash flag (-f or --flash) is
+# used with the Miosix Build System. Use $binary or $hex as placeolders, they
+# will be replaced by the build systems with the binary or hex file repectively.
+# If a command is not specified, the build system will use st-flash if found
+# set(PROGRAM_CMDLINE "here your custom flash command")
+
+# Basic flags
+set(FLAGS_BASE -mcpu=cortex-m7 -mthumb -mfloat-abi=hard -mfpu=fpv5-d16)
+
+# Flags for ASM and linker
+set(AFLAGS_BASE ${FLAGS_BASE})
+set(LFLAGS_BASE ${FLAGS_BASE} -Wl,--gc-sections,-Map,main.map -Wl,-T${LINKER_SCRIPT} ${OPT_EXCEPT} ${OPT_OPTIMIZATION} -nostdlib)
+
+# Flags for C/C++
+string(TOUPPER ${BOARD_NAME} BOARD_UPPER)
+set(CFLAGS_BASE
+    -D_BOARD_${BOARD_UPPER} -D_MIOSIX_BOARDNAME=\"${BOARD_NAME}\"
+    -D_DEFAULT_SOURCE=1 -ffunction-sections -Wall -Werror=return-type -g
+    -D_ARCH_CORTEXM7_STM32F7
+    ${CLOCK_FREQ} ${XRAM} ${SRAM_BOOT} ${FLAGS_BASE} ${OPT_OPTIMIZATION} -c
+)
+set(CXXFLAGS_BASE ${CFLAGS_BASE} ${OPT_EXCEPT})
+
+# Select architecture specific files
+set(ARCH_SRC
+    ${ARCH_PATH}/interfaces-impl/delays.cpp
+    ${ARCH_PATH}/interfaces-impl/gpio_impl.cpp
+    ${ARCH_PATH}/interfaces-impl/portability.cpp
+    ${BOARD_PATH}/interfaces-impl/bsp.cpp
+    ${KPATH}/arch/common/CMSIS/Device/ST/STM32F7xx/Source/Templates/system_stm32f7xx.c
+    ${KPATH}/arch/common/core/cache_cortexMx.cpp
+    ${KPATH}/arch/common/core/interrupts_cortexMx.cpp
+    ${KPATH}/arch/common/core/mpu_cortexMx.cpp
+    ${KPATH}/arch/common/core/stm32f2_f4_l4_f7_h7_os_timer.cpp
+    ${KPATH}/arch/common/drivers/sd_stm32f2_f4_f7.cpp
+    ${KPATH}/arch/common/drivers/serial_stm32.cpp
+    ${KPATH}/arch/common/drivers/stm32_hardware_rng.cpp
+)
diff --git a/src/bsps/stm32f767zi_gemini_gs/config/board_settings.h b/src/bsps/stm32f767zi_gemini_gs/config/board_settings.h
new file mode 100644
index 000000000..168493854
--- /dev/null
+++ b/src/bsps/stm32f767zi_gemini_gs/config/board_settings.h
@@ -0,0 +1,62 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * 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.
+ */
+
+#pragma once
+
+/**
+ * \internal
+ * Versioning for board_settings.h for out of git tree projects
+ */
+#define BOARD_SETTINGS_VERSION 300
+
+namespace miosix
+{
+
+/**
+ * \addtogroup Settings
+ * \{
+ */
+
+/// Size of stack for main().
+/// The C standard library is stack-heavy (iprintf requires 1KB)
+const unsigned int MAIN_STACK_SIZE = 16 * 1024;
+
+/// Serial port
+const unsigned int defaultSerial      = 1;
+const unsigned int defaultSerialSpeed = 115200;
+#define SERIAL_1_DMA
+// #define SERIAL_2_DMA
+// #define SERIAL_3_DMA
+
+// SD card driver
+static const unsigned char sdVoltage = 33;  // Board powered @ 3.3V
+// #define SD_ONE_BIT_DATABUS
+#define SD_SDMMC 1  // Select either SDMMC1 or SDMMC2
+
+/// Analog supply voltage for ADC, DAC, Reset blocks, RCs and PLL
+#define V_DDA_VOLTAGE 3.3f
+
+/**
+ * \}
+ */
+
+}  // namespace miosix
diff --git a/src/bsps/stm32f767zi_gemini_gs/config/miosix_settings.h b/src/bsps/stm32f767zi_gemini_gs/config/miosix_settings.h
new file mode 100644
index 000000000..1b49a8e7c
--- /dev/null
+++ b/src/bsps/stm32f767zi_gemini_gs/config/miosix_settings.h
@@ -0,0 +1,240 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * 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.
+ */
+
+#pragma once
+
+// Before you can compile the kernel you have to configure it by editing this
+// file. After that, comment out this line to disable the reminder error.
+// The PARSING_FROM_IDE is because Netbeans gets confused by this, it is never
+// defined when compiling the code.
+#ifndef PARSING_FROM_IDE
+//#error This error is a reminder that you have not edited miosix_settings.h
+// yet.
+#endif  // PARSING_FROM_IDE
+
+/**
+ * \file miosix_settings.h
+ * NOTE: this file contains ONLY configuration options that are not dependent
+ * on architecture specific details. The other options are in the following
+ * files which are included here:
+ * miosix/arch/architecture name/common/arch_settings.h
+ * miosix/config/arch/architecture name/board name/board_settings.h
+ */
+#include "arch_settings.h"
+#include "board_settings.h"
+#include "util/version.h"
+
+/**
+ * \internal
+ * Versioning for miosix_settings.h for out of git tree projects
+ */
+#define MIOSIX_SETTINGS_VERSION 300
+
+namespace miosix
+{
+
+/**
+ * \addtogroup Settings
+ * \{
+ */
+
+//
+// Scheduler options
+//
+
+/// \def SCHED_TYPE_PRIORITY
+/// If uncommented selects the priority scheduler
+/// \def SCHED_TYPE_CONTROL_BASED
+/// If uncommented selects the control based scheduler
+/// \def SCHED_TYPE_EDF
+/// If uncommented selects the EDF scheduler
+// Uncomment only *one* of those
+
+#define SCHED_TYPE_PRIORITY
+//#define SCHED_TYPE_CONTROL_BASED
+//#define SCHED_TYPE_EDF
+
+/// \def WITH_CPU_TIME_COUNTER
+/// Allows to enable/disable CPUTimeCounter to save code size and remove its
+/// overhead from the scheduling process. By default it is not defined
+/// (CPUTimeCounter is disabled).
+//#define WITH_CPU_TIME_COUNTER
+
+//
+// Filesystem options
+//
+
+/// \def WITH_FILESYSTEM
+/// Allows to enable/disable filesystem support to save code size
+/// By default it is defined (filesystem support is enabled)
+#define WITH_FILESYSTEM
+
+/// \def WITH_DEVFS
+/// Allows to enable/disable DevFs support to save code size
+/// By default it is defined (DevFs is enabled)
+#define WITH_DEVFS
+
+/// \def SYNC_AFTER_WRITE
+/// Increases filesystem write robustness. After each write operation the
+/// filesystem is synced so that a power failure happens data is not lost
+/// (unless power failure happens exactly between the write and the sync)
+/// Unfortunately write latency and throughput becomes twice as worse
+/// By default it is defined (slow but safe)
+#define SYNC_AFTER_WRITE
+
+/// Maximum number of open files. Trying to open more will fail.
+/// Cannot be lower than 3, as the first three are stdin, stdout, stderr
+const unsigned char MAX_OPEN_FILES = 8;
+
+/// \def WITH_PROCESSES
+/// If uncommented enables support for processes as well as threads.
+/// This enables the dynamic loader to load elf programs, the extended system
+/// call service and, if the hardware supports it, the MPU to provide memory
+/// isolation of processes
+//#define WITH_PROCESSES
+
+#if defined(WITH_PROCESSES) && defined(__NO_EXCEPTIONS)
+#error Processes require C++ exception support
+#endif  // defined(WITH_PROCESSES) && defined(__NO_EXCEPTIONS)
+
+#if defined(WITH_PROCESSES) && !defined(WITH_FILESYSTEM)
+#error Processes require filesystem support
+#endif  // defined(WITH_PROCESSES) && !defined(WITH_FILESYSTEM)
+
+#if defined(WITH_PROCESSES) && !defined(WITH_DEVFS)
+#error Processes require devfs support
+#endif  // defined(WITH_PROCESSES) && !defined(WITH_DEVFS)
+
+//
+// C/C++ standard library I/O (stdin, stdout and stderr related)
+//
+
+/// \def WITH_BOOTLOG
+/// Uncomment to print bootlogs on stdout.
+/// By default it is defined (bootlogs are printed)
+#define WITH_BOOTLOG
+
+/// \def WITH_ERRLOG
+/// Uncomment for debug information on stdout.
+/// By default it is defined (error information is printed)
+#define WITH_ERRLOG
+
+//
+// Kernel related options (stack sizes, priorities)
+//
+
+/// \def WITH_DEEP_SLEEP
+/// Adds interfaces and required variables to support deep sleep state switch
+/// automatically when peripherals are not required
+//#define WITH_DEEP_SLEEP
+
+/**
+ * \def JTAG_DISABLE_SLEEP
+ * JTAG debuggers lose communication with the device if it enters sleep
+ * mode, so to use debugging it is necessary to disable sleep in the idle
+ * thread. By default it is not defined (idle thread calls sleep).
+ */
+//#define JTAG_DISABLE_SLEEP
+
+#if defined(WITH_DEEP_SLEEP) && defined(JTAG_DISABLE_SLEEP)
+#error Deep sleep cannot work together with jtag
+#endif  // defined(WITH_PROCESSES) && !defined(WITH_DEVFS)
+
+/// Minimum stack size (MUST be divisible by 4)
+const unsigned int STACK_MIN = 256;
+
+/// \internal Size of idle thread stack.
+/// Should be >=STACK_MIN (MUST be divisible by 4)
+const unsigned int STACK_IDLE = 256;
+
+/// Default stack size for pthread_create.
+/// The chosen value is enough to call C standard library functions
+/// such as printf/fopen which are stack-heavy
+const unsigned int STACK_DEFAULT_FOR_PTHREAD = 2048;
+
+/// Maximum size of the RAM image of a process. If a program requires more
+/// the kernel will not run it (MUST be divisible by 4)
+const unsigned int MAX_PROCESS_IMAGE_SIZE = 64 * 1024;
+
+/// Minimum size of the stack for a process. If a program specifies a lower
+/// size the kernel will not run it (MUST be divisible by 4)
+const unsigned int MIN_PROCESS_STACK_SIZE = STACK_MIN;
+
+/// Every userspace thread has two stacks, one for when it is running in
+/// userspace and one for when it is running in kernelspace (that is, while it
+/// is executing system calls). This is the size of the stack for when the
+/// thread is running in kernelspace (MUST be divisible by 4)
+const unsigned int SYSTEM_MODE_PROCESS_STACK_SIZE = 2 * 1024;
+
+/// Number of priorities (MUST be >1)
+/// PRIORITY_MAX-1 is the highest priority, 0 is the lowest. -1 is reserved as
+/// the priority of the idle thread.
+/// The meaning of a thread's priority depends on the chosen scheduler.
+#ifdef SCHED_TYPE_PRIORITY
+// Can be modified, but a high value makes context switches more expensive
+const short int PRIORITY_MAX = 4;
+#elif defined(SCHED_TYPE_CONTROL_BASED)
+// Don't touch, the limit is due to the fixed point implementation
+// It's not needed for if floating point is selected, but kept for consistency
+const short int PRIORITY_MAX = 64;
+#else  // SCHED_TYPE_EDF
+// Doesn't exist for this kind of scheduler
+#endif
+
+/// Priority of main()
+/// The meaning of a thread's priority depends on the chosen scheduler.
+const unsigned char MAIN_PRIORITY = 1;
+
+#ifdef SCHED_TYPE_PRIORITY
+/// Maximum thread time slice in nanoseconds, after which preemption occurs
+const unsigned int MAX_TIME_SLICE = 1000000;
+#endif  // SCHED_TYPE_PRIORITY
+
+//
+// Other low level kernel options. There is usually no need to modify these.
+//
+
+/// \internal Length of wartermark (in bytes) to check stack overflow.
+/// MUST be divisible by 4 and can also be zero.
+/// A high value increases context switch time.
+const unsigned int WATERMARK_LEN = 16;
+
+/// \internal Used to fill watermark
+const unsigned int WATERMARK_FILL = 0xaaaaaaaa;
+
+/// \internal Used to fill stack (for checking stack usage)
+const unsigned int STACK_FILL = 0xbbbbbbbb;
+
+// Compiler version checks
+#if !defined(_MIOSIX_GCC_PATCH_MAJOR) || _MIOSIX_GCC_PATCH_MAJOR < 3
+#error \
+    "You are using a too old or unsupported compiler. Get the latest one from https://miosix.org/wiki/index.php?title=Miosix_Toolchain"
+#endif
+#if _MIOSIX_GCC_PATCH_MAJOR > 3
+#warning "You are using a too new compiler, which may not be supported"
+#endif
+
+/**
+ * \}
+ */
+
+}  // namespace miosix
diff --git a/src/bsps/stm32f767zi_gemini_gs/core/stage_1_boot.cpp b/src/bsps/stm32f767zi_gemini_gs/core/stage_1_boot.cpp
new file mode 100644
index 000000000..cc96d3b51
--- /dev/null
+++ b/src/bsps/stm32f767zi_gemini_gs/core/stage_1_boot.cpp
@@ -0,0 +1,505 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * 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 <string.h>
+
+#include "core/cache_cortexMx.h"
+#include "core/interrupts.h"  //For the unexpected interrupt call
+#include "core/interrupts_cortexMx.h"
+#include "interfaces/arch_registers.h"
+#include "interfaces/bsp.h"
+#include "kernel/stage_2_boot.h"
+
+/*
+ * startup.cpp
+ * STM32 C++ startup.
+ * NOTE: for stm32f767 devices ONLY.
+ * Supports interrupt handlers in C++ without extern "C"
+ * Developed by Terraneo Federico, based on ST startup code.
+ * Additionally modified to boot Miosix.
+ */
+
+/**
+ * Called by Reset_Handler, performs initialization and calls main.
+ * Never returns.
+ */
+void program_startup() __attribute__((noreturn));
+void program_startup()
+{
+    // Cortex M7 core appears to get out of reset with interrupts already
+    // enabled
+    __disable_irq();
+
+    miosix::IRQconfigureCache((const unsigned int *)0xd0000000,
+                              8 * 1024 * 1024);
+
+    // These are defined in the linker script
+    extern unsigned char _etext asm("_etext");
+    extern unsigned char _data asm("_data");
+    extern unsigned char _edata asm("_edata");
+    extern unsigned char _bss_start asm("_bss_start");
+    extern unsigned char _bss_end asm("_bss_end");
+
+    // Initialize .data section, clear .bss section
+    unsigned char *etext     = &_etext;
+    unsigned char *data      = &_data;
+    unsigned char *edata     = &_edata;
+    unsigned char *bss_start = &_bss_start;
+    unsigned char *bss_end   = &_bss_end;
+    memcpy(data, etext, edata - data);
+    memset(bss_start, 0, bss_end - bss_start);
+
+    // Move on to stage 2
+    _init();
+
+    // If main returns, reboot
+    NVIC_SystemReset();
+    for (;;)
+        ;
+}
+
+/**
+ * Reset handler, called by hardware immediately after reset
+ */
+void Reset_Handler() __attribute__((__interrupt__, noreturn));
+void Reset_Handler()
+{
+    /**
+     * SystemInit() is called *before* initializing .data and zeroing .bss
+     * Despite all startup files provided by ST do the opposite, there are three
+     * good reasons to do so:
+     * 1. First, the CMSIS specifications say that SystemInit() must not access
+     *    global variables, so it is actually possible to call it before
+     * 2. Second, when running Miosix with the xram linker scripts .data and
+     *    .bss are placed in the external RAM, so we *must* call SystemInit(),
+     *    which enables xram, before touching .data and .bss
+     * 3. Third, this is a performance improvement since the loops that
+     *    initialize .data and zeros .bss now run with the CPU at full speed
+     *    instead of 8MHz
+     */
+    SystemInit();
+
+/**
+ * ST does not provide code to initialize the SDRAM at boot.
+ * Put after SystemInit() as SDRAM is timing-sensitive and needs the full
+ * clock speed.
+ */
+#ifdef __ENABLE_XRAM
+    miosix::configureSdram();
+#endif  //__ENABLE_XRAM
+
+    /*
+     * Load into the program stack pointer the heap end address and switch from
+     * the msp to sps.
+     * This is required for booting Miosix, a small portion of the top of the
+     * heap area will be used as stack until the first thread starts. After,
+     * this stack will be abandoned and the process stack will point to the
+     * current thread's stack.
+     */
+    asm volatile(
+        "ldr r0,  =_heap_end          \n\t"
+        "msr psp, r0                  \n\t"
+        "movw r0, #2                  \n\n"  // Set the control register to use
+        "msr control, r0              \n\t"  // the process stack
+        "isb                          \n\t" ::
+            : "r0");
+
+    program_startup();
+}
+
+/**
+ * All unused interrupts call this function.
+ */
+extern "C" void Default_Handler() { unexpectedInterrupt(); }
+
+// System handlers
+void /*__attribute__((weak))*/ Reset_Handler();      // These interrupts are not
+void /*__attribute__((weak))*/ NMI_Handler();        // weak because they are
+void /*__attribute__((weak))*/ HardFault_Handler();  // surely defined by Miosix
+void /*__attribute__((weak))*/ MemManage_Handler();
+void /*__attribute__((weak))*/ BusFault_Handler();
+void /*__attribute__((weak))*/ UsageFault_Handler();
+void /*__attribute__((weak))*/ SVC_Handler();
+void /*__attribute__((weak))*/ DebugMon_Handler();
+void /*__attribute__((weak))*/ PendSV_Handler();
+void __attribute__((weak)) SysTick_Handler();
+
+// Interrupt handlers
+void __attribute__((weak)) WWDG_IRQHandler();
+void __attribute__((weak)) PVD_IRQHandler();
+void __attribute__((weak)) TAMP_STAMP_IRQHandler();
+void __attribute__((weak)) RTC_WKUP_IRQHandler();
+void __attribute__((weak)) FLASH_IRQHandler();
+void __attribute__((weak)) RCC_IRQHandler();
+void __attribute__((weak)) EXTI0_IRQHandler();
+void __attribute__((weak)) EXTI1_IRQHandler();
+void __attribute__((weak)) EXTI2_IRQHandler();
+void __attribute__((weak)) EXTI3_IRQHandler();
+void __attribute__((weak)) EXTI4_IRQHandler();
+void __attribute__((weak)) DMA1_Stream0_IRQHandler();
+void __attribute__((weak)) DMA1_Stream1_IRQHandler();
+void __attribute__((weak)) DMA1_Stream2_IRQHandler();
+void __attribute__((weak)) DMA1_Stream3_IRQHandler();
+void __attribute__((weak)) DMA1_Stream4_IRQHandler();
+void __attribute__((weak)) DMA1_Stream5_IRQHandler();
+void __attribute__((weak)) DMA1_Stream6_IRQHandler();
+void __attribute__((weak)) ADC_IRQHandler();
+void __attribute__((weak)) CAN1_TX_IRQHandler();
+void __attribute__((weak)) CAN1_RX0_IRQHandler();
+void __attribute__((weak)) CAN1_RX1_IRQHandler();
+void __attribute__((weak)) CAN1_SCE_IRQHandler();
+void __attribute__((weak)) EXTI9_5_IRQHandler();
+void __attribute__((weak)) TIM1_BRK_TIM9_IRQHandler();
+void __attribute__((weak)) TIM1_UP_TIM10_IRQHandler();
+void __attribute__((weak)) TIM1_TRG_COM_TIM11_IRQHandler();
+void __attribute__((weak)) TIM1_CC_IRQHandler();
+void __attribute__((weak)) TIM2_IRQHandler();
+void __attribute__((weak)) TIM3_IRQHandler();
+void __attribute__((weak)) TIM4_IRQHandler();
+void __attribute__((weak)) I2C1_EV_IRQHandler();
+void __attribute__((weak)) I2C1_ER_IRQHandler();
+void __attribute__((weak)) I2C2_EV_IRQHandler();
+void __attribute__((weak)) I2C2_ER_IRQHandler();
+void __attribute__((weak)) SPI1_IRQHandler();
+void __attribute__((weak)) SPI2_IRQHandler();
+void __attribute__((weak)) USART1_IRQHandler();
+void __attribute__((weak)) USART2_IRQHandler();
+void __attribute__((weak)) USART3_IRQHandler();
+void __attribute__((weak)) EXTI15_10_IRQHandler();
+void __attribute__((weak)) RTC_Alarm_IRQHandler();
+void __attribute__((weak)) OTG_FS_WKUP_IRQHandler();
+void __attribute__((weak)) TIM8_BRK_TIM12_IRQHandler();
+void __attribute__((weak)) TIM8_UP_TIM13_IRQHandler();
+void __attribute__((weak)) TIM8_TRG_COM_TIM14_IRQHandler();
+void __attribute__((weak)) TIM8_CC_IRQHandler();
+void __attribute__((weak)) DMA1_Stream7_IRQHandler();
+void __attribute__((weak)) FMC_IRQHandler();
+void __attribute__((weak)) SDMMC1_IRQHandler();
+void __attribute__((weak)) TIM5_IRQHandler();
+void __attribute__((weak)) SPI3_IRQHandler();
+void __attribute__((weak)) UART4_IRQHandler();
+void __attribute__((weak)) UART5_IRQHandler();
+void __attribute__((weak)) TIM6_DAC_IRQHandler();
+void __attribute__((weak)) TIM7_IRQHandler();
+void __attribute__((weak)) DMA2_Stream0_IRQHandler();
+void __attribute__((weak)) DMA2_Stream1_IRQHandler();
+void __attribute__((weak)) DMA2_Stream2_IRQHandler();
+void __attribute__((weak)) DMA2_Stream3_IRQHandler();
+void __attribute__((weak)) DMA2_Stream4_IRQHandler();
+void __attribute__((weak)) ETH_IRQHandler();
+void __attribute__((weak)) ETH_WKUP_IRQHandler();
+void __attribute__((weak)) CAN2_TX_IRQHandler();
+void __attribute__((weak)) CAN2_RX0_IRQHandler();
+void __attribute__((weak)) CAN2_RX1_IRQHandler();
+void __attribute__((weak)) CAN2_SCE_IRQHandler();
+void __attribute__((weak)) OTG_FS_IRQHandler();
+void __attribute__((weak)) DMA2_Stream5_IRQHandler();
+void __attribute__((weak)) DMA2_Stream6_IRQHandler();
+void __attribute__((weak)) DMA2_Stream7_IRQHandler();
+void __attribute__((weak)) USART6_IRQHandler();
+void __attribute__((weak)) I2C3_EV_IRQHandler();
+void __attribute__((weak)) I2C3_ER_IRQHandler();
+void __attribute__((weak)) OTG_HS_EP1_OUT_IRQHandler();
+void __attribute__((weak)) OTG_HS_EP1_IN_IRQHandler();
+void __attribute__((weak)) OTG_HS_WKUP_IRQHandler();
+void __attribute__((weak)) OTG_HS_IRQHandler();
+void __attribute__((weak)) DCMI_IRQHandler();
+void __attribute__((weak)) CRYP_IRQHandler();
+void __attribute__((weak)) RNG_IRQHandler();
+void __attribute__((weak)) FPU_IRQHandler();
+void __attribute__((weak)) UART7_IRQHandler();
+void __attribute__((weak)) UART8_IRQHandler();
+void __attribute__((weak)) SPI4_IRQHandler();
+void __attribute__((weak)) SPI5_IRQHandler();
+void __attribute__((weak)) SPI6_IRQHandler();
+void __attribute__((weak)) SAI1_IRQHandler();
+void __attribute__((weak)) LTDC_IRQHandler();
+void __attribute__((weak)) LTDC_ER_IRQHandler();
+void __attribute__((weak)) DMA2D_IRQHandler();
+void __attribute__((weak)) SAI2_IRQHandler();
+void __attribute__((weak)) QUADSPI_IRQHandler();
+void __attribute__((weak)) LPTIM1_IRQHandler();
+void __attribute__((weak)) CEC_IRQHandler();
+void __attribute__((weak)) I2C4_EV_IRQHandler();
+void __attribute__((weak)) I2C4_ER_IRQHandler();
+void __attribute__((weak)) SPDIF_RX_IRQHandler();
+void __attribute__((weak)) DSIHOST_IRQHandler();
+void __attribute__((weak)) DFSDM1_FLT0_IRQHandler();
+void __attribute__((weak)) DFSDM1_FLT1_IRQHandler();
+void __attribute__((weak)) DFSDM1_FLT2_IRQHandler();
+void __attribute__((weak)) DFSDM1_FLT3_IRQHandler();
+void __attribute__((weak)) SDMMC2_IRQHandler();
+void __attribute__((weak)) CAN3_TX_IRQHandler();
+void __attribute__((weak)) CAN3_RX0_IRQHandler();
+void __attribute__((weak)) CAN3_RX1_IRQHandler();
+void __attribute__((weak)) CAN3_SCE_IRQHandler();
+void __attribute__((weak)) JPEG_IRQHandler();
+void __attribute__((weak)) MDIOS_IRQHandler();
+
+// Stack top, defined in the linker script
+extern char _main_stack_top asm("_main_stack_top");
+
+// Interrupt vectors, must be placed @ address 0x00000000
+// The extern declaration is required otherwise g++ optimizes it out
+extern void (*const __Vectors[])();
+void (*const __Vectors[])() __attribute__((section(".isr_vector"))) = {
+    reinterpret_cast<void (*)()>(&_main_stack_top), /* Stack pointer*/
+    Reset_Handler,                                  /* Reset Handler */
+    NMI_Handler,                                    /* NMI Handler */
+    HardFault_Handler,                              /* Hard Fault Handler */
+    MemManage_Handler,                              /* MPU Fault Handler */
+    BusFault_Handler,                               /* Bus Fault Handler */
+    UsageFault_Handler,                             /* Usage Fault Handler */
+    0,                                              /* Reserved */
+    0,                                              /* Reserved */
+    0,                                              /* Reserved */
+    0,                                              /* Reserved */
+    SVC_Handler,                                    /* SVCall Handler */
+    DebugMon_Handler,                               /* Debug Monitor Handler */
+    0,                                              /* Reserved */
+    PendSV_Handler,                                 /* PendSV Handler */
+    SysTick_Handler,                                /* SysTick Handler */
+
+    /* External Interrupts */
+    WWDG_IRQHandler,
+    PVD_IRQHandler,
+    TAMP_STAMP_IRQHandler,
+    RTC_WKUP_IRQHandler,
+    FLASH_IRQHandler,
+    RCC_IRQHandler,
+    EXTI0_IRQHandler,
+    EXTI1_IRQHandler,
+    EXTI2_IRQHandler,
+    EXTI3_IRQHandler,
+    EXTI4_IRQHandler,
+    DMA1_Stream0_IRQHandler,
+    DMA1_Stream1_IRQHandler,
+    DMA1_Stream2_IRQHandler,
+    DMA1_Stream3_IRQHandler,
+    DMA1_Stream4_IRQHandler,
+    DMA1_Stream5_IRQHandler,
+    DMA1_Stream6_IRQHandler,
+    ADC_IRQHandler,
+    CAN1_TX_IRQHandler,
+    CAN1_RX0_IRQHandler,
+    CAN1_RX1_IRQHandler,
+    CAN1_SCE_IRQHandler,
+    EXTI9_5_IRQHandler,
+    TIM1_BRK_TIM9_IRQHandler,
+    TIM1_UP_TIM10_IRQHandler,
+    TIM1_TRG_COM_TIM11_IRQHandler,
+    TIM1_CC_IRQHandler,
+    TIM2_IRQHandler,
+    TIM3_IRQHandler,
+    TIM4_IRQHandler,
+    I2C1_EV_IRQHandler,
+    I2C1_ER_IRQHandler,
+    I2C2_EV_IRQHandler,
+    I2C2_ER_IRQHandler,
+    SPI1_IRQHandler,
+    SPI2_IRQHandler,
+    USART1_IRQHandler,
+    USART2_IRQHandler,
+    USART3_IRQHandler,
+    EXTI15_10_IRQHandler,
+    RTC_Alarm_IRQHandler,
+    OTG_FS_WKUP_IRQHandler,
+    TIM8_BRK_TIM12_IRQHandler,
+    TIM8_UP_TIM13_IRQHandler,
+    TIM8_TRG_COM_TIM14_IRQHandler,
+    TIM8_CC_IRQHandler,
+    DMA1_Stream7_IRQHandler,
+    FMC_IRQHandler,
+    SDMMC1_IRQHandler,
+    TIM5_IRQHandler,
+    SPI3_IRQHandler,
+    UART4_IRQHandler,
+    UART5_IRQHandler,
+    TIM6_DAC_IRQHandler,
+    TIM7_IRQHandler,
+    DMA2_Stream0_IRQHandler,
+    DMA2_Stream1_IRQHandler,
+    DMA2_Stream2_IRQHandler,
+    DMA2_Stream3_IRQHandler,
+    DMA2_Stream4_IRQHandler,
+    ETH_IRQHandler,
+    ETH_WKUP_IRQHandler,
+    CAN2_TX_IRQHandler,
+    CAN2_RX0_IRQHandler,
+    CAN2_RX1_IRQHandler,
+    CAN2_SCE_IRQHandler,
+    OTG_FS_IRQHandler,
+    DMA2_Stream5_IRQHandler,
+    DMA2_Stream6_IRQHandler,
+    DMA2_Stream7_IRQHandler,
+    USART6_IRQHandler,
+    I2C3_EV_IRQHandler,
+    I2C3_ER_IRQHandler,
+    OTG_HS_EP1_OUT_IRQHandler,
+    OTG_HS_EP1_IN_IRQHandler,
+    OTG_HS_WKUP_IRQHandler,
+    OTG_HS_IRQHandler,
+    DCMI_IRQHandler,
+    CRYP_IRQHandler,
+    RNG_IRQHandler,
+    FPU_IRQHandler,
+    UART7_IRQHandler,
+    UART8_IRQHandler,
+    SPI4_IRQHandler,
+    SPI5_IRQHandler,
+    SPI6_IRQHandler,
+    SAI1_IRQHandler,
+    LTDC_IRQHandler,
+    LTDC_ER_IRQHandler,
+    DMA2D_IRQHandler,
+    SAI2_IRQHandler,
+    QUADSPI_IRQHandler,
+    LPTIM1_IRQHandler,
+    CEC_IRQHandler,
+    I2C4_EV_IRQHandler,
+    I2C4_ER_IRQHandler,
+    SPDIF_RX_IRQHandler,
+    DSIHOST_IRQHandler,
+    DFSDM1_FLT0_IRQHandler,
+    DFSDM1_FLT1_IRQHandler,
+    DFSDM1_FLT2_IRQHandler,
+    DFSDM1_FLT3_IRQHandler,
+    SDMMC2_IRQHandler,
+    CAN3_TX_IRQHandler,
+    CAN3_RX0_IRQHandler,
+    CAN3_RX1_IRQHandler,
+    CAN3_SCE_IRQHandler,
+    JPEG_IRQHandler,
+    MDIOS_IRQHandler,
+};
+
+#pragma weak SysTick_IRQHandler            = Default_Handler
+#pragma weak WWDG_IRQHandler               = Default_Handler
+#pragma weak PVD_IRQHandler                = Default_Handler
+#pragma weak TAMP_STAMP_IRQHandler         = Default_Handler
+#pragma weak RTC_WKUP_IRQHandler           = Default_Handler
+#pragma weak FLASH_IRQHandler              = Default_Handler
+#pragma weak RCC_IRQHandler                = Default_Handler
+#pragma weak EXTI0_IRQHandler              = Default_Handler
+#pragma weak EXTI1_IRQHandler              = Default_Handler
+#pragma weak EXTI2_IRQHandler              = Default_Handler
+#pragma weak EXTI3_IRQHandler              = Default_Handler
+#pragma weak EXTI4_IRQHandler              = Default_Handler
+#pragma weak DMA1_Stream0_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream1_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream2_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream3_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream4_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream5_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream6_IRQHandler       = Default_Handler
+#pragma weak ADC_IRQHandler                = Default_Handler
+#pragma weak CAN1_TX_IRQHandler            = Default_Handler
+#pragma weak CAN1_RX0_IRQHandler           = Default_Handler
+#pragma weak CAN1_RX1_IRQHandler           = Default_Handler
+#pragma weak CAN1_SCE_IRQHandler           = Default_Handler
+#pragma weak EXTI9_5_IRQHandler            = Default_Handler
+#pragma weak TIM1_BRK_TIM9_IRQHandler      = Default_Handler
+#pragma weak TIM1_UP_TIM10_IRQHandler      = Default_Handler
+#pragma weak TIM1_TRG_COM_TIM11_IRQHandler = Default_Handler
+#pragma weak TIM1_CC_IRQHandler            = Default_Handler
+#pragma weak TIM2_IRQHandler               = Default_Handler
+#pragma weak TIM3_IRQHandler               = Default_Handler
+#pragma weak TIM4_IRQHandler               = Default_Handler
+#pragma weak I2C1_EV_IRQHandler            = Default_Handler
+#pragma weak I2C1_ER_IRQHandler            = Default_Handler
+#pragma weak I2C2_EV_IRQHandler            = Default_Handler
+#pragma weak I2C2_ER_IRQHandler            = Default_Handler
+#pragma weak SPI1_IRQHandler               = Default_Handler
+#pragma weak SPI2_IRQHandler               = Default_Handler
+// #pragma weak USART1_IRQHandler             = Default_Handler
+// #pragma weak USART2_IRQHandler             = Default_Handler
+// #pragma weak USART3_IRQHandler             = Default_Handler
+#pragma weak EXTI15_10_IRQHandler          = Default_Handler
+#pragma weak RTC_Alarm_IRQHandler          = Default_Handler
+#pragma weak OTG_FS_WKUP_IRQHandler        = Default_Handler
+#pragma weak TIM8_BRK_TIM12_IRQHandler     = Default_Handler
+#pragma weak TIM8_UP_TIM13_IRQHandler      = Default_Handler
+#pragma weak TIM8_TRG_COM_TIM14_IRQHandler = Default_Handler
+#pragma weak TIM8_CC_IRQHandler            = Default_Handler
+#pragma weak DMA1_Stream7_IRQHandler       = Default_Handler
+#pragma weak FMC_IRQHandler                = Default_Handler
+#pragma weak SDMMC1_IRQHandler             = Default_Handler
+#pragma weak TIM5_IRQHandler               = Default_Handler
+#pragma weak SPI3_IRQHandler               = Default_Handler
+// #pragma weak UART4_IRQHandler              = Default_Handler
+// #pragma weak UART5_IRQHandler              = Default_Handler
+#pragma weak TIM6_DAC_IRQHandler     = Default_Handler
+#pragma weak TIM7_IRQHandler         = Default_Handler
+#pragma weak DMA2_Stream0_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream1_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream2_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream3_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream4_IRQHandler = Default_Handler
+#pragma weak ETH_IRQHandler          = Default_Handler
+#pragma weak ETH_WKUP_IRQHandler     = Default_Handler
+#pragma weak CAN2_TX_IRQHandler      = Default_Handler
+#pragma weak CAN2_RX0_IRQHandler     = Default_Handler
+#pragma weak CAN2_RX1_IRQHandler     = Default_Handler
+#pragma weak CAN2_SCE_IRQHandler     = Default_Handler
+#pragma weak OTG_FS_IRQHandler       = Default_Handler
+#pragma weak DMA2_Stream5_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream6_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream7_IRQHandler = Default_Handler
+// #pragma weak USART6_IRQHandler             = Default_Handler
+#pragma weak I2C3_EV_IRQHandler        = Default_Handler
+#pragma weak I2C3_ER_IRQHandler        = Default_Handler
+#pragma weak OTG_HS_EP1_OUT_IRQHandler = Default_Handler
+#pragma weak OTG_HS_EP1_IN_IRQHandler  = Default_Handler
+#pragma weak OTG_HS_WKUP_IRQHandler    = Default_Handler
+#pragma weak OTG_HS_IRQHandler         = Default_Handler
+#pragma weak DCMI_IRQHandler           = Default_Handler
+#pragma weak CRYP_IRQHandler           = Default_Handler
+#pragma weak RNG_IRQHandler            = Default_Handler
+#pragma weak FPU_IRQHandler            = Default_Handler
+// #pragma weak UART7_IRQHandler              = Default_Handler
+// #pragma weak UART8_IRQHandler              = Default_Handler
+#pragma weak SPI4_IRQHandler        = Default_Handler
+#pragma weak SPI5_IRQHandler        = Default_Handler
+#pragma weak SPI6_IRQHandler        = Default_Handler
+#pragma weak SAI1_IRQHandler        = Default_Handler
+#pragma weak LTDC_IRQHandler        = Default_Handler
+#pragma weak LTDC_ER_IRQHandler     = Default_Handler
+#pragma weak DMA2D_IRQHandler       = Default_Handler
+#pragma weak SAI2_IRQHandler        = Default_Handler
+#pragma weak QUADSPI_IRQHandler     = Default_Handler
+#pragma weak LPTIM1_IRQHandler      = Default_Handler
+#pragma weak CEC_IRQHandler         = Default_Handler
+#pragma weak I2C4_EV_IRQHandler     = Default_Handler
+#pragma weak I2C4_ER_IRQHandler     = Default_Handler
+#pragma weak SPDIF_RX_IRQHandler    = Default_Handler
+#pragma weak DSIHOST_IRQHandler     = Default_Handler
+#pragma weak DFSDM1_FLT0_IRQHandler = Default_Handler
+#pragma weak DFSDM1_FLT1_IRQHandler = Default_Handler
+#pragma weak DFSDM1_FLT2_IRQHandler = Default_Handler
+#pragma weak DFSDM1_FLT3_IRQHandler = Default_Handler
+#pragma weak SDMMC2_IRQHandler      = Default_Handler
+#pragma weak CAN3_TX_IRQHandler     = Default_Handler
+#pragma weak CAN3_RX0_IRQHandler    = Default_Handler
+#pragma weak CAN3_RX1_IRQHandler    = Default_Handler
+#pragma weak CAN3_SCE_IRQHandler    = Default_Handler
+#pragma weak JPEG_IRQHandler        = Default_Handler
+#pragma weak MDIOS_IRQHandler       = Default_Handler
diff --git a/src/bsps/stm32f767zi_gemini_gs/interfaces-impl/arch_registers_impl.h b/src/bsps/stm32f767zi_gemini_gs/interfaces-impl/arch_registers_impl.h
new file mode 100644
index 000000000..662ddc6ec
--- /dev/null
+++ b/src/bsps/stm32f767zi_gemini_gs/interfaces-impl/arch_registers_impl.h
@@ -0,0 +1,39 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * 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.
+ */
+
+#ifndef ARCH_REGISTERS_IMPL_H
+#define ARCH_REGISTERS_IMPL_H
+
+// stm32f7xx.h defines a few macros like __ICACHE_PRESENT, __DCACHE_PRESENT and
+// includes core_cm7.h. Do not include core_cm7.h before.
+#define STM32F767xx
+#include "CMSIS/Device/ST/STM32F7xx/Include/stm32f7xx.h"
+
+#if (__ICACHE_PRESENT != 1) || (__DCACHE_PRESENT != 1)
+#error "Wrong include order"
+#endif
+
+#include "CMSIS/Device/ST/STM32F7xx/Include/system_stm32f7xx.h"
+
+#define RCC_SYNC() __DSB()  // TODO: can this dsb be removed?
+
+#endif  // ARCH_REGISTERS_IMPL_H
diff --git a/src/bsps/stm32f767zi_gemini_gs/interfaces-impl/bsp.cpp b/src/bsps/stm32f767zi_gemini_gs/interfaces-impl/bsp.cpp
new file mode 100644
index 000000000..f1b9d7daf
--- /dev/null
+++ b/src/bsps/stm32f767zi_gemini_gs/interfaces-impl/bsp.cpp
@@ -0,0 +1,355 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * 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.
+ */
+
+/***********************************************************************
+ * bsp.cpp Part of the Miosix Embedded OS.
+ * Board support package, this file initializes hardware.
+ ************************************************************************/
+
+#include "interfaces/bsp.h"
+
+#include <inttypes.h>
+#include <sys/ioctl.h>
+
+#include <cstdlib>
+
+#include "board_settings.h"
+#include "config/miosix_settings.h"
+#include "drivers/sd_stm32f2_f4_f7.h"
+#include "drivers/serial.h"
+#include "drivers/serial_stm32.h"
+#include "drivers/stm32_sgm.h"
+#include "filesystem/console/console_device.h"
+#include "filesystem/file_access.h"
+#include "hwmapping.h"
+#include "interfaces/arch_registers.h"
+#include "interfaces/delays.h"
+#include "interfaces/portability.h"
+#include "kernel/kernel.h"
+#include "kernel/logging.h"
+#include "kernel/sync.h"
+
+namespace miosix
+{
+
+//
+// Initialization
+//
+
+static void sdramCommandWait()
+{
+    for (int i = 0; i < 0xffff; i++)
+        if ((FMC_Bank5_6->SDSR & FMC_SDSR_BUSY) == 0)
+            return;
+}
+
+void configureSdram()
+{
+    // Enable gpios used by the ram
+    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN | RCC_AHB1ENR_GPIOCEN |
+                    RCC_AHB1ENR_GPIODEN | RCC_AHB1ENR_GPIOEEN |
+                    RCC_AHB1ENR_GPIOFEN | RCC_AHB1ENR_GPIOGEN;
+    RCC_SYNC();
+
+    // On the compute unit with F767ZI, the SDRAM pins are:
+    // - PG8:  FMC_SDCLK  (sdram clock)
+    // - PB5:  FMC_SDCKE1 (sdram bank 2 clock enable)
+    // - PB6:  FMC_SDNE1  (sdram bank 2 chip enable)
+    // - PF0:  FMC_A0
+    // - PF1:  FMC_A1
+    // - PF2:  FMC_A2
+    // - PF3:  FMC_A3
+    // - PF4:  FMC_A4
+    // - PF5:  FMC_A5
+    // - PF12: FMC_A6
+    // - PF13: FMC_A7
+    // - PF14: FMC_A8
+    // - PF15: FMC_A9
+    // - PG0:  FMC_A10
+    // - PG1:  FMC_A11
+    // - PG2:  FMC_A12    (used only by the 32MB ram, not by the 8MB one)
+    // - PD14: FMC_D0
+    // - PD15: FMC_D1
+    // - PD0:  FMC_D2
+    // - PD1:  FMC_D3
+    // - PE7:  FMC_D4
+    // - PE8:  FMC_D5
+    // - PE9:  FMC_D6
+    // - PE10: FMC_D7
+    // - PE11: FMC_D8
+    // - PE12: FMC_D9
+    // - PE13: FMC_D10
+    // - PE14: FMC_D11
+    // - PE15: FMC_D12
+    // - PD8:  FMC_D13
+    // - PD9:  FMC_D14
+    // - PD10: FMC_D15
+
+    // - PG4:  FMC_BA0
+    // - PG5:  FMC_BA1
+    // - PF11: FMC_SDNRAS
+    // - PG15: FMC_SDNCAS
+    // - PC0:  FMC_SDNWE
+    // - PE0:  FMC_NBL0
+    // - PE1:  FMC_NBL1
+
+    // All SDRAM GPIOs needs to be configured with alternate function 12 and
+    // maximum speed
+
+    // WARNING: The current configuration is for the 8MB ram
+
+    // Alternate functions
+    GPIOB->AFR[0] = 0x0cc00000;
+    GPIOC->AFR[0] = 0x0000000c;
+    GPIOD->AFR[0] = 0x000000cc;
+    GPIOD->AFR[1] = 0xcc000ccc;
+    GPIOE->AFR[0] = 0xc00000cc;
+    GPIOE->AFR[1] = 0xcccccccc;
+    GPIOF->AFR[0] = 0x00cccccc;
+    GPIOF->AFR[1] = 0xccccc000;
+    GPIOG->AFR[0] = 0x00cc0ccc;
+    GPIOG->AFR[1] = 0xc000000c;
+
+    // Mode
+    GPIOB->MODER = 0x00002800;
+    GPIOC->MODER = 0x00000002;
+    GPIOD->MODER = 0xa02a000a;
+    GPIOE->MODER = 0xaaaa800a;
+    GPIOF->MODER = 0xaa800aaa;
+    GPIOG->MODER = 0x80020a2a;
+
+    // Speed (high speed for all, very high speed for SDRAM pins)
+    GPIOB->OSPEEDR = 0x00003c00;
+    GPIOC->OSPEEDR = 0x00000003;
+    GPIOD->OSPEEDR = 0xf03f000f;
+    GPIOE->OSPEEDR = 0xffffc00f;
+    GPIOF->OSPEEDR = 0xffc00fff;
+    GPIOG->OSPEEDR = 0xc0030f3f;
+
+    // Since we'we un-configured PB3 and PB4 (by default they are SWO and NJRST)
+    // finish the job and remove the default pull-up
+    GPIOB->PUPDR = 0;
+
+    // Enable the SDRAM controller clock
+    RCC->AHB3ENR |= RCC_AHB3ENR_FMCEN;
+    RCC_SYNC();
+
+    // The SDRAM is a AS4C4M16SA-6TAN
+    // HCLK = 216MHz -> SDRAM clock = HCLK/2 = 133MHz
+
+    // 1. Memory device features
+    FMC_Bank5_6->SDCR[0] = 0                     // 0 delay after CAS latency
+                           | FMC_SDCR1_RBURST    // Enable read bursts
+                           | FMC_SDCR1_SDCLK_1;  // SDCLK = HCLK / 2
+    FMC_Bank5_6->SDCR[1] = 0                     // Write accesses allowed
+                           | FMC_SDCR2_CAS_1     // 2 cycles CAS latency
+                           | FMC_SDCR2_NB        // 4 internal banks
+                           | FMC_SDCR2_MWID_0    // 16 bit data bus
+                           | FMC_SDCR2_NR_1      // 13 bit row address
+                           // cppcheck-suppress duplicateExpression
+                           | 0;  // 8 bit column address
+
+// 2. Memory device timings
+#ifdef SYSCLK_FREQ_216MHz
+    // SDRAM timings. One clock cycle is 9.26ns
+    FMC_Bank5_6->SDTR[0] =
+        (2 - 1) << FMC_SDTR1_TRP_Pos     // 2 cycles TRP  (18.52ns > 18ns)
+        | (7 - 1) << FMC_SDTR1_TRC_Pos;  // 7 cycles TRC  (64.82ns > 60ns)
+    FMC_Bank5_6->SDTR[1] =
+        (2 - 1) << FMC_SDTR1_TRCD_Pos     // 2 cycles TRCD (18.52ns > 18ns)
+        | (2 - 1) << FMC_SDTR1_TWR_Pos    // 2 cycles TWR  (min 2cc > 12ns)
+        | (5 - 1) << FMC_SDTR1_TRAS_Pos   // 5 cycles TRAS (46.3ns  > 42ns)
+        | (7 - 1) << FMC_SDTR1_TXSR_Pos   // 7 cycles TXSR (74.08ns > 61.5ns)
+        | (2 - 1) << FMC_SDTR1_TMRD_Pos;  // 2 cycles TMRD (min 2cc > 12ns)
+#else
+#error No SDRAM timings for this clock
+#endif
+
+    // 3. Enable the bank 2 clock
+    FMC_Bank5_6->SDCMR = FMC_SDCMR_MODE_0   // Clock Configuration Enable
+                         | FMC_SDCMR_CTB2;  // Bank 2
+    sdramCommandWait();
+
+    // 4. Wait during command execution
+    delayUs(100);
+
+    // 5. Issue a "Precharge All" command
+    FMC_Bank5_6->SDCMR = FMC_SDCMR_MODE_1   // Precharge all
+                         | FMC_SDCMR_CTB2;  // Bank 2
+    sdramCommandWait();
+
+    // 6. Issue Auto-Refresh commands
+    FMC_Bank5_6->SDCMR = FMC_SDCMR_MODE_1 | FMC_SDCMR_MODE_0  // Auto-Refresh
+                         | FMC_SDCMR_CTB2                     // Bank 2
+                         | (8 - 1) << FMC_SDCMR_NRFS_Pos;     // 2 Auto-Refresh
+    sdramCommandWait();
+
+    // 7. Issue a Load Mode Register command
+    FMC_Bank5_6->SDCMR = FMC_SDCMR_MODE_2               /// Load mode register
+                         | FMC_SDCMR_CTB2               // Bank 2
+                         | 0x220 << FMC_SDCMR_MRD_Pos;  // CAS = 2, burst = 1
+    sdramCommandWait();
+
+// 8. Program the refresh rate (4K / 32ms)
+// 64ms / 8192 = 7.8125us
+#ifdef SYSCLK_FREQ_216MHz
+    // 7.8125us * 133MHz = 1039 - 20 = 1019
+    FMC_Bank5_6->SDRTR = 1019 << FMC_SDRTR_COUNT_Pos;
+#else
+#error No SDRAM refresh timings for this clock
+#endif
+}
+
+void IRQbspInit()
+{
+    // Enable USART1 pins port
+    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN;
+
+    userLed1::mode(Mode::OUTPUT);
+    userLed2::mode(Mode::OUTPUT);
+    userLed3_1::mode(Mode::OUTPUT);
+    userLed3_2::mode(Mode::OUTPUT);
+    userLed4::mode(Mode::OUTPUT);
+    userSwitch::mode(Mode::INPUT);
+
+    interfaces::spi1::miso::mode(Mode::ALTERNATE);
+    interfaces::spi1::miso::alternateFunction(5);
+    interfaces::spi1::mosi::mode(Mode::ALTERNATE);
+    interfaces::spi1::mosi::alternateFunction(5);
+    interfaces::spi1::sck::mode(Mode::ALTERNATE);
+    interfaces::spi1::sck::alternateFunction(5);
+
+    interfaces::spi3::miso::mode(Mode::ALTERNATE);
+    interfaces::spi3::miso::alternateFunction(6);
+    interfaces::spi3::mosi::mode(Mode::ALTERNATE);
+    interfaces::spi3::mosi::alternateFunction(5);
+    interfaces::spi3::sck::mode(Mode::ALTERNATE);
+    interfaces::spi3::sck::alternateFunction(6);
+
+    interfaces::spi4::miso::mode(Mode::ALTERNATE);
+    interfaces::spi4::miso::alternateFunction(5);
+    interfaces::spi4::mosi::mode(Mode::ALTERNATE);
+    interfaces::spi4::mosi::alternateFunction(5);
+    interfaces::spi4::sck::mode(Mode::ALTERNATE);
+    interfaces::spi4::sck::alternateFunction(5);
+
+    radio1::cs::mode(Mode::OUTPUT);
+    radio1::cs::high();
+    radio1::nrst::mode(Mode::OUTPUT);
+    radio1::nrst::high();
+    radio1::txen::mode(Mode::OUTPUT);
+    radio1::txen::low();
+    radio1::rxen::mode(Mode::OUTPUT);
+    radio1::rxen::low();
+    radio1::dio0::mode(Mode::INPUT);
+    radio1::dio1::mode(Mode::INPUT);
+    radio1::dio3::mode(Mode::INPUT);
+
+    radio2::cs::mode(Mode::OUTPUT);
+    radio2::cs::high();
+    radio2::nrst::mode(Mode::OUTPUT);
+    radio2::nrst::high();
+    radio2::txen::mode(Mode::OUTPUT);
+    radio2::txen::low();
+    radio2::rxen::mode(Mode::OUTPUT);
+    radio2::rxen::low();
+    radio2::dio0::mode(Mode::INPUT);
+    radio2::dio1::mode(Mode::INPUT);
+    radio2::dio3::mode(Mode::INPUT);
+
+    ethernet::cs::mode(Mode::OUTPUT);
+    ethernet::cs::high();
+    ethernet::nrst::mode(Mode::OUTPUT);
+    ethernet::nrst::high();
+    ethernet::intr::mode(Mode::INPUT);
+
+    DefaultConsole::instance().IRQset(intrusive_ref_ptr<Device>(new STM32Serial(
+        defaultSerial, defaultSerialSpeed, STM32Serial::NOFLOWCTRL)));
+}
+
+void bspInit2()
+{
+#ifdef WITH_FILESYSTEM
+    // Init devfs with empty device
+    basicFilesystemSetup(intrusive_ref_ptr<Device>());
+#endif  // WITH_FILESYSTEM
+
+#ifdef WITH_BACKUP_SRAM
+    // Print the reset reason
+    bootlog("Reset reson: ");
+    switch (SGM::instance().lastResetReason())
+    {
+        case ResetReason::RST_LOW_PWR:
+            bootlog("low power\n");
+            break;
+        case ResetReason::RST_WINDOW_WDG:
+            bootlog("window watchdog\n");
+            break;
+        case ResetReason::RST_INDEPENDENT_WDG:
+            bootlog("indeendent watchdog\n");
+            break;
+        case ResetReason::RST_SW:
+            bootlog("software reset\n");
+            break;
+        case ResetReason::RST_POWER_ON:
+            bootlog("power on\n");
+            break;
+        case ResetReason::RST_PIN:
+            bootlog("reset pin\n");
+            break;
+        case ResetReason::RST_UNKNOWN:
+            bootlog("unknown\n");
+            break;
+    }
+#endif  // WITH_BACKUP_SRAM
+}
+
+//
+// Shutdown and reboot
+//
+
+void shutdown()
+{
+    ioctl(STDOUT_FILENO, IOCTL_SYNC, 0);
+
+#ifdef WITH_FILESYSTEM
+    FilesystemManager::instance().umountAll();
+#endif  // WITH_FILESYSTEM
+
+    disableInterrupts();
+    for (;;)
+        ;
+}
+
+void reboot()
+{
+    ioctl(STDOUT_FILENO, IOCTL_SYNC, 0);
+
+#ifdef WITH_FILESYSTEM
+    FilesystemManager::instance().umountAll();
+#endif  // WITH_FILESYSTEM
+
+    disableInterrupts();
+    miosix_private::IRQsystemReboot();
+}
+
+}  // namespace miosix
diff --git a/src/bsps/stm32f767zi_gemini_gs/interfaces-impl/bsp_impl.h b/src/bsps/stm32f767zi_gemini_gs/interfaces-impl/bsp_impl.h
new file mode 100644
index 000000000..aeba49af9
--- /dev/null
+++ b/src/bsps/stm32f767zi_gemini_gs/interfaces-impl/bsp_impl.h
@@ -0,0 +1,116 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * 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.
+ */
+
+/***************************************************************************
+ * bsp_impl.h Part of the Miosix Embedded OS.
+ * Board support package, this file initializes hardware.
+ ***************************************************************************/
+
+#ifndef BSP_IMPL_H
+#define BSP_IMPL_H
+
+#include "config/miosix_settings.h"
+#include "interfaces/gpio.h"
+
+namespace miosix
+{
+
+/**
+\addtogroup Hardware
+\{
+*/
+
+/**
+ * \internal
+ * Called by stage_1_boot.cpp to enable the SDRAM before initializing .data/.bss
+ * Requires the CPU clock to be already configured (running from the PLL)
+ */
+void configureSdram();
+
+/**
+ * \internal
+ * Board pin definition
+ */
+typedef Gpio<GPIOB_BASE, 7> userLed1;
+typedef Gpio<GPIOE_BASE, 3> userLed2;
+typedef Gpio<GPIOC_BASE, 13> userLed3_1;
+typedef Gpio<GPIOG_BASE, 9> userLed3_2;
+typedef Gpio<GPIOC_BASE, 2> userLed4;
+typedef Gpio<GPIOB_BASE, 2> userSwitch;
+
+inline void ledOn()
+{
+    userLed1::high();
+    userLed2::high();
+    userLed3_1::high();
+    userLed3_2::high();
+    userLed4::high();
+}
+
+inline void ledOff()
+{
+    userLed1::low();
+    userLed2::low();
+    userLed3_1::low();
+    userLed3_2::low();
+    userLed4::low();
+}
+
+inline void led1On() { userLed1::high(); }
+
+inline void led1Off() { userLed1::low(); }
+
+inline void led2On() { userLed2::high(); }
+
+inline void led2Off() { userLed2::low(); }
+
+inline void led3On()
+{
+    userLed3_1::high();
+    userLed3_2::high();
+}
+
+inline void led3Off()
+{
+    userLed3_1::low();
+    userLed3_2::low();
+}
+
+/**
+ * Polls the SD card sense GPIO.
+ *
+ * This board has no SD card whatsoever, but a card can be connected to the
+ * following GPIOs:
+ * TODO: never tested
+ *
+ * \return true. As there's no SD card sense switch, let's pretend that
+ * the card is present.
+ */
+inline bool sdCardSense() { return true; }
+
+/**
+\}
+*/
+
+}  // namespace miosix
+
+#endif  // BSP_IMPL_H
diff --git a/src/bsps/stm32f767zi_gemini_gs/interfaces-impl/hwmapping.h b/src/bsps/stm32f767zi_gemini_gs/interfaces-impl/hwmapping.h
new file mode 100644
index 000000000..b81a36d1e
--- /dev/null
+++ b/src/bsps/stm32f767zi_gemini_gs/interfaces-impl/hwmapping.h
@@ -0,0 +1,130 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Davide Mor
+ *
+ * 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.
+ */
+
+#ifndef HWMAPPING_H
+#define HWMAPPING_H
+
+#include "interfaces/gpio.h"
+
+#define MIOSIX_RADIO1_DIO0_IRQ EXTI8_IRQHandlerImpl
+#define MIOSIX_RADIO1_DIO1_IRQ EXTI10_IRQHandlerImpl
+#define MIOSIX_RADIO1_DIO2_IRQ EXTI11_IRQHandlerImpl
+#define MIOSIX_RADIO1_DIO3_IRQ EXTI12_IRQHandlerImpl
+#define MIOSIX_RADIO1_SPI SPI3
+
+#define MIOSIX_RADIO2_DIO0_IRQ EXTI6_IRQHandlerImpl
+#define MIOSIX_RADIO2_DIO1_IRQ EXTI4_IRQHandlerImpl
+#define MIOSIX_RADIO2_DIO2_IRQ EXTI7_IRQHandlerImpl
+#define MIOSIX_RADIO2_DIO3_IRQ EXTI5_IRQHandlerImpl
+#define MIOSIX_RADIO2_DIO4_IRQ EXTI2_IRQHandlerImpl
+#define MIOSIX_RADIO2_DIO5_IRQ EXTI3_IRQHandlerImpl
+#define MIOSIX_RADIO2_SPI SPI1
+
+#define MIOSIX_ETHERNET_IRQ EXTI1_IRQHandlerImpl
+#define MIOSIX_ETHERNET_SPI SPI4
+
+// Remember to modify pins of leds
+namespace miosix
+{
+
+namespace interfaces
+{
+
+// Radio 1
+namespace spi3
+{
+using sck  = Gpio<GPIOB_BASE, 3>;
+using miso = Gpio<GPIOB_BASE, 4>;
+using mosi = Gpio<GPIOD_BASE, 6>;
+}  // namespace spi3
+
+// Radio 2
+namespace spi1
+{
+using sck  = Gpio<GPIOA_BASE, 5>;
+using miso = Gpio<GPIOA_BASE, 6>;
+using mosi = Gpio<GPIOA_BASE, 7>;
+}  // namespace spi1
+
+// Ethernet module
+namespace spi4
+{
+using sck  = Gpio<GPIOE_BASE, 2>;
+using miso = Gpio<GPIOE_BASE, 5>;
+using mosi = Gpio<GPIOE_BASE, 6>;
+}  // namespace spi4
+
+}  // namespace interfaces
+
+namespace radio1
+{
+namespace spi
+{
+using sck  = miosix::interfaces::spi3::sck;
+using miso = miosix::interfaces::spi3::miso;
+using mosi = miosix::interfaces::spi3::mosi;
+}  // namespace spi
+
+using cs   = Gpio<GPIOA_BASE, 15>;
+using dio0 = Gpio<GPIOC_BASE, 8>;
+using dio1 = Gpio<GPIOC_BASE, 10>;
+using dio3 = Gpio<GPIOC_BASE, 12>;
+using txen = Gpio<GPIOG_BASE, 12>;
+using rxen = Gpio<GPIOG_BASE, 14>;
+using nrst = Gpio<GPIOA_BASE, 1>;
+}  // namespace radio1
+
+namespace radio2
+{
+namespace spi
+{
+using sck  = miosix::interfaces::spi1::sck;
+using miso = miosix::interfaces::spi1::miso;
+using mosi = miosix::interfaces::spi1::mosi;
+}  // namespace spi
+
+using cs   = Gpio<GPIOA_BASE, 4>;
+using dio0 = Gpio<GPIOC_BASE, 6>;
+using dio1 = Gpio<GPIOD_BASE, 4>;
+using dio3 = Gpio<GPIOD_BASE, 5>;
+using txen = Gpio<GPIOB_BASE, 8>;
+using rxen = Gpio<GPIOB_BASE, 9>;
+using nrst = Gpio<GPIOA_BASE, 0>;
+}  // namespace radio2
+
+namespace ethernet
+{
+namespace spi
+{
+using sck  = miosix::interfaces::spi4::sck;
+using miso = miosix::interfaces::spi4::miso;
+using mosi = miosix::interfaces::spi4::mosi;
+}  // namespace spi
+
+using cs   = Gpio<GPIOE_BASE, 4>;
+using intr = Gpio<GPIOC_BASE, 1>;
+using nrst = Gpio<GPIOB_BASE, 1>;
+}  // namespace ethernet
+
+}  // namespace miosix
+
+#endif  // HWMAPPING_H
diff --git a/src/bsps/stm32f767zi_gemini_gs/stm32_2m+16m_xram.ld b/src/bsps/stm32f767zi_gemini_gs/stm32_2m+16m_xram.ld
new file mode 100644
index 000000000..5b3545abd
--- /dev/null
+++ b/src/bsps/stm32f767zi_gemini_gs/stm32_2m+16m_xram.ld
@@ -0,0 +1,190 @@
+/*
+ * C++ enabled linker script for stm32f769ni (2M FLASH, 512K RAM, 16MB XRAM)
+ * Developed by TFT: Terraneo Federico Technologies
+ * Optimized for use with the Miosix kernel
+ */
+
+/*
+ * This linker script puts:
+ * - read only data and code (.text, .rodata, .eh_*) in FLASH
+ * - the 512Byte main (IRQ) stack, .data and .bss in the DTCM 128KB RAM
+ * - .data, .bss, stacks and heap in the external 16MB SDRAM.
+ */
+
+/*
+ * The main stack is used for interrupt handling by the kernel.
+ *
+ * *** Readme ***
+ * This linker script places the main stack (used by the kernel for interrupts)
+ * at the bottom of the ram, instead of the top. This is done for two reasons:
+ *
+ * - as an optimization for microcontrollers with little ram memory. In fact
+ *   the implementation of malloc from newlib requests memory to the OS in 4KB
+ *   block (except the first block that can be smaller). This is probably done
+ *   for compatibility with OSes with an MMU and paged memory. To see why this
+ *   is bad, consider a microcontroller with 8KB of ram: when malloc finishes
+ *   up the first 4KB it will call _sbrk_r asking for a 4KB block, but this will
+ *   fail because the top part of the ram is used by the main stack. As a
+ *   result, the top part of the memory will not be used by malloc, even if
+ *   available (and it is nearly *half* the ram on an 8KB mcu). By placing the
+ *   main stack at the bottom of the ram, the upper 4KB block will be entirely
+ *   free and available as heap space.
+ *
+ * - In case of main stack overflow the cpu will fault because access to memory
+ *   before the beginning of the ram faults. Instead with the default stack
+ *   placement the main stack will silently collide with the heap.
+ * Note: if increasing the main stack size also increase the ORIGIN value in
+ * the MEMORY definitions below accordingly.
+ */
+
+_main_stack_size = 512;                             /* main stack = 512Bytes */
+_main_stack_top  = 0x20000000 + _main_stack_size;
+ASSERT(_main_stack_size   % 8 == 0, "MAIN stack size error");
+
+/* Mapping the heap into XRAM */
+_heap_end = 0xd0000000 + 16M;                        /* end of available ram */
+
+/* Identify the Entry Point  */
+ENTRY(_Z13Reset_Handlerv)
+
+/*
+ * Specify the memory areas
+ *
+ * NOTE: starting at 0x20000000 there's 128KB of DTCM (Data Tightly Coupled
+ * Memory). Technically, we could use this as normal RAM as there's a way for
+ * the DMA to access it, but the datasheet is unclear about performance
+ * penalties for doing so. To avoid nonuniform DMA memory access latencies,
+ * we leave this 128KB DTCM unused except for the first 512Bytes which are for
+ * the interrupt stack. This leaves us with 384KB of RAM
+ */
+MEMORY
+{
+    xram(wx)  : ORIGIN = 0xd0000000, LENGTH =  16M
+    sram(wx)  : ORIGIN = 0x20020000, LENGTH = 384K
+    dtcm(wx)  : ORIGIN = 0x20000000, LENGTH = 128K    /* Used for main stack */
+    bram(rw)  : ORIGIN = 0x40024000, LENGTH =   4K    /* Bakup SRAM */
+    flash(rx) : ORIGIN = 0x08000000, LENGTH =   2M
+}
+
+/* now define the output sections  */
+SECTIONS
+{
+    . = 0;
+    
+    /* .text section: code goes to flash */
+    .text :
+    {
+        /* Startup code must go at address 0 */
+        KEEP(*(.isr_vector))
+        
+        *(.text)
+        *(.text.*)
+        *(.gnu.linkonce.t.*)
+        /* these sections for thumb interwork? */
+        *(.glue_7)
+        *(.glue_7t)
+        /* these sections for C++? */
+        *(.gcc_except_table)
+        *(.gcc_except_table.*)
+        *(.ARM.extab*)
+        *(.gnu.linkonce.armextab.*)
+
+        . = ALIGN(4);
+        /* .rodata: constant data */
+        *(.rodata)
+        *(.rodata.*)
+        *(.gnu.linkonce.r.*)
+
+        /* C++ Static constructors/destructors (eabi) */
+        . = ALIGN(4);
+        KEEP(*(.init))
+        
+        . = ALIGN(4);
+        __miosix_init_array_start = .;
+        KEEP (*(SORT(.miosix_init_array.*)))
+        KEEP (*(.miosix_init_array))
+        __miosix_init_array_end = .;
+
+        . = ALIGN(4);
+        __preinit_array_start = .;
+        KEEP (*(.preinit_array))
+        __preinit_array_end = .;
+
+        . = ALIGN(4);
+        __init_array_start = .;
+        KEEP (*(SORT(.init_array.*)))
+        KEEP (*(.init_array))
+        __init_array_end = .;
+
+        . = ALIGN(4);
+        KEEP(*(.fini))
+
+        . = ALIGN(4);
+        __fini_array_start = .;
+        KEEP (*(.fini_array))
+        KEEP (*(SORT(.fini_array.*)))
+        __fini_array_end = .;
+
+        /* C++ Static constructors/destructors (elf)  */
+        . = ALIGN(4);
+        _ctor_start = .;
+        KEEP (*crtbegin.o(.ctors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .ctors))
+        KEEP (*(SORT(.ctors.*)))
+        KEEP (*crtend.o(.ctors))
+       _ctor_end = .;
+
+        . = ALIGN(4);
+        KEEP (*crtbegin.o(.dtors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .dtors))
+        KEEP (*(SORT(.dtors.*)))
+        KEEP (*crtend.o(.dtors))
+    } > flash
+
+    /* .ARM.exidx is sorted, so has to go in its own output section.  */
+    __exidx_start = .;
+    .ARM.exidx :
+    {
+        *(.ARM.exidx* .gnu.linkonce.armexidx.*)
+    } > flash
+    __exidx_end = .;
+
+	/*
+     * .data section: global variables go to xram, but also store a copy to
+     * flash to initialize them
+     */
+    .data : ALIGN(8)
+    {
+        _data = .;
+        *(.data)
+        *(.data.*)
+        *(.gnu.linkonce.d.*)
+        . = ALIGN(8);
+        _edata = .;
+    } > xram AT > flash
+    _etext = LOADADDR(.data);
+
+    /* .bss section: uninitialized global variables go to xram */
+    _bss_start = .;
+    .bss :
+    {
+        *(.bss)
+        *(.bss.*)
+        *(.gnu.linkonce.b.*)
+        . = ALIGN(8);
+    } > xram
+    _bss_end = .;
+
+    _end = .;
+    PROVIDE(end = .);
+    
+    .preserve(NOLOAD) : ALIGN(4)
+    {
+        _preserve_start = .;
+        . = ALIGN(4);
+        *(.preserve);
+        *(.preserve*);
+        . = ALIGN(4);
+        _preserve_end = .;
+    } > bram
+}
diff --git a/src/bsps/stm32f767zi_gemini_gs/stm32_2m+384k_ram.ld b/src/bsps/stm32f767zi_gemini_gs/stm32_2m+384k_ram.ld
new file mode 100644
index 000000000..497bd5198
--- /dev/null
+++ b/src/bsps/stm32f767zi_gemini_gs/stm32_2m+384k_ram.ld
@@ -0,0 +1,189 @@
+/*
+ * C++ enabled linker script for stm32f767zi (2M FLASH, 512K RAM)
+ * Developed by TFT: Terraneo Federico Technologies
+ * Optimized for use with the Miosix kernel
+ */
+
+/*
+ * This linker script puts:
+ * - read only data and code (.text, .rodata, .eh_*) in FLASH
+ * - the 512Byte main (IRQ) stack, .data and .bss in the DTCM 128KB RAM
+ * - .data, .bss, stacks and heap in the internal RAM.
+ */
+
+/*
+ * The main stack is used for interrupt handling by the kernel.
+ *
+ * *** Readme ***
+ * This linker script places the main stack (used by the kernel for interrupts)
+ * at the bottom of the ram, instead of the top. This is done for two reasons:
+ *
+ * - as an optimization for microcontrollers with little ram memory. In fact
+ *   the implementation of malloc from newlib requests memory to the OS in 4KB
+ *   block (except the first block that can be smaller). This is probably done
+ *   for compatibility with OSes with an MMU and paged memory. To see why this
+ *   is bad, consider a microcontroller with 8KB of ram: when malloc finishes
+ *   up the first 4KB it will call _sbrk_r asking for a 4KB block, but this will
+ *   fail because the top part of the ram is used by the main stack. As a
+ *   result, the top part of the memory will not be used by malloc, even if
+ *   available (and it is nearly *half* the ram on an 8KB mcu). By placing the
+ *   main stack at the bottom of the ram, the upper 4KB block will be entirely
+ *   free and available as heap space.
+ *
+ * - In case of main stack overflow the cpu will fault because access to memory
+ *   before the beginning of the ram faults. Instead with the default stack
+ *   placement the main stack will silently collide with the heap.
+ * Note: if increasing the main stack size also increase the ORIGIN value in
+ * the MEMORY definitions below accordingly.
+ */
+
+_main_stack_size = 512;                             /* main stack = 512Bytes */
+_main_stack_top  = 0x20000000 + _main_stack_size;
+ASSERT(_main_stack_size   % 8 == 0, "MAIN stack size error");
+
+/* Mapping the heap to the end of SRAM2 */
+_heap_end = 0x20000000 + 512K;                       /* end of available ram */
+
+/* Identify the Entry Point  */
+ENTRY(_Z13Reset_Handlerv)
+
+/*
+ * Specify the memory areas
+ *
+ * NOTE: starting at 0x20000000 there's 128KB of DTCM (Data Tightly Coupled
+ * Memory). Technically, we could use this as normal RAM as there's a way for
+ * the DMA to access it, but the datasheet is unclear about performance
+ * penalties for doing so. To avoid nonuniform DMA memory access latencies,
+ * we leave this 128KB DTCM unused except for the first 512Bytes which are for
+ * the interrupt stack. This leaves us with 384KB of RAM
+ */
+MEMORY
+{
+    sram(wx)  : ORIGIN = 0x20020000, LENGTH = 384K
+    dtcm(wx)  : ORIGIN = 0x20000000, LENGTH = 128K    /* Used for main stack */
+    bram(rw)  : ORIGIN = 0x40024000, LENGTH =   4K    /* Bakup SRAM */
+    flash(rx) : ORIGIN = 0x08000000, LENGTH =   2M
+}
+
+/* now define the output sections  */
+SECTIONS
+{
+    . = 0;
+    
+    /* .text section: code goes to flash */
+    .text :
+    {
+        /* Startup code must go at address 0 */
+        KEEP(*(.isr_vector))
+        
+        *(.text)
+        *(.text.*)
+        *(.gnu.linkonce.t.*)
+        /* these sections for thumb interwork? */
+        *(.glue_7)
+        *(.glue_7t)
+        /* these sections for C++? */
+        *(.gcc_except_table)
+        *(.gcc_except_table.*)
+        *(.ARM.extab*)
+        *(.gnu.linkonce.armextab.*)
+
+        . = ALIGN(4);
+        /* .rodata: constant data */
+        *(.rodata)
+        *(.rodata.*)
+        *(.gnu.linkonce.r.*)
+
+        /* C++ Static constructors/destructors (eabi) */
+        . = ALIGN(4);
+        KEEP(*(.init))
+        
+        . = ALIGN(4);
+        __miosix_init_array_start = .;
+        KEEP (*(SORT(.miosix_init_array.*)))
+        KEEP (*(.miosix_init_array))
+        __miosix_init_array_end = .;
+
+        . = ALIGN(4);
+        __preinit_array_start = .;
+        KEEP (*(.preinit_array))
+        __preinit_array_end = .;
+
+        . = ALIGN(4);
+        __init_array_start = .;
+        KEEP (*(SORT(.init_array.*)))
+        KEEP (*(.init_array))
+        __init_array_end = .;
+
+        . = ALIGN(4);
+        KEEP(*(.fini))
+
+        . = ALIGN(4);
+        __fini_array_start = .;
+        KEEP (*(.fini_array))
+        KEEP (*(SORT(.fini_array.*)))
+        __fini_array_end = .;
+
+        /* C++ Static constructors/destructors (elf)  */
+        . = ALIGN(4);
+        _ctor_start = .;
+        KEEP (*crtbegin.o(.ctors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .ctors))
+        KEEP (*(SORT(.ctors.*)))
+        KEEP (*crtend.o(.ctors))
+       _ctor_end = .;
+
+        . = ALIGN(4);
+        KEEP (*crtbegin.o(.dtors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .dtors))
+        KEEP (*(SORT(.dtors.*)))
+        KEEP (*crtend.o(.dtors))
+    } > flash
+
+    /* .ARM.exidx is sorted, so has to go in its own output section.  */
+    __exidx_start = .;
+    .ARM.exidx :
+    {
+        *(.ARM.exidx* .gnu.linkonce.armexidx.*)
+    } > flash
+    __exidx_end = .;
+
+	/*
+     * .data section: global variables go to sram, but also store a copy to
+     * flash to initialize them
+     */
+    .data : ALIGN(8)
+    {
+        _data = .;
+        *(.data)
+        *(.data.*)
+        *(.gnu.linkonce.d.*)
+        . = ALIGN(8);
+        _edata = .;
+    } > sram AT > flash
+    _etext = LOADADDR(.data);
+
+    /* .bss section: uninitialized global variables go to sram */
+    _bss_start = .;
+    .bss :
+    {
+        *(.bss)
+        *(.bss.*)
+        *(.gnu.linkonce.b.*)
+        . = ALIGN(8);
+    } > sram
+    _bss_end = .;
+
+    _end = .;
+    PROVIDE(end = .);
+    
+    .preserve(NOLOAD) : ALIGN(4)
+    {
+        _preserve_start = .;
+        . = ALIGN(4);
+        *(.preserve);
+        *(.preserve*);
+        . = ALIGN(4);
+        _preserve_end = .;
+    } > bram
+}
diff --git a/src/bsps/stm32f767zi_gemini_motor/config/board_options.cmake b/src/bsps/stm32f767zi_gemini_motor/config/board_options.cmake
new file mode 100644
index 000000000..5608769a1
--- /dev/null
+++ b/src/bsps/stm32f767zi_gemini_motor/config/board_options.cmake
@@ -0,0 +1,106 @@
+# Copyright (C) 2023 by Skyward
+#
+# This program is free software; you can redistribute it and/or 
+# it under the terms of the GNU General Public License as published 
+# the Free Software Foundation; either version 2 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.
+#
+# As a special exception, if other files instantiate templates or use
+# macros or inline functions from this file, or you compile this file
+# and link it with other works to produce a work based on this file,
+# this file does not by itself cause the resulting work to be covered
+# by the GNU General Public License. However the source code for this
+# file must still be made available in accordance with the GNU 
+# Public License. This exception does not invalidate any other 
+# why a work based on this file might be covered by the GNU General
+# Public License.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, see <http://www.gnu.org/licenses/>
+
+set(BOARD_NAME stm32f767zi_gemini_motor)
+set(ARCH_NAME cortexM7_stm32f7)
+
+# Base directories with header files for this board
+set(ARCH_PATH ${KPATH}/arch/${ARCH_NAME}/common)
+set(BOARD_PATH ${BOARDCORE_PATH}/src/bsps/${BOARD_NAME})
+set(BOARD_CONFIG_PATH ${BOARDCORE_PATH}/src/bsps/${BOARD_NAME}/config)
+
+# Specify where to find the board specific config/miosix_settings.h
+set(BOARD_MIOSIX_SETTINGS_PATH ${BOARD_PATH})
+
+# Specify where to find the board specific config/mxgui_settings.h
+set(BOARD_MXGUI_SETTINGS_PATH ${BOARD_PATH})
+
+# Optimization flags:
+# -O0 do no optimization, the default if no optimization level is specified
+# -O or -O1 optimize minimally
+# -O2 optimize more
+# -O3 optimize even more
+# -Ofast optimize very aggressively to the point of breaking the standard
+# -Og Optimize debugging experience, enables optimizations that do not
+# interfere with debugging
+# -Os Optimize for size with -O2 optimizations that do not increase code size
+set(OPT_OPTIMIZATION -O2)
+
+# Boot file and linker script
+set(BOOT_FILE ${BOARD_PATH}/core/stage_1_boot.cpp)
+# set(LINKER_SCRIPT ${BOARD_PATH}/stm32_2m+384k_ram.ld)
+set(LINKER_SCRIPT ${BOARD_PATH}/stm32_2m+16m_xram.ld)
+
+# Enables the initialization of the external 16MB SDRAM memory
+set(XRAM -D__ENABLE_XRAM)
+
+# Select clock frequency (HSE_VALUE is the xtal on board, fixed)
+set(CLOCK_FREQ -DHSE_VALUE=25000000 -DSYSCLK_FREQ_216MHz=216000000)
+
+# C++ Exception/rtti support disable flags.
+# To save code size if not using C++ exceptions (nor some STL code which
+# implicitly uses it) uncomment this option.
+# -D__NO_EXCEPTIONS is used by Miosix to know if exceptions are used.
+# set(OPT_EXCEPT -fno-exceptions -fno-rtti -D__NO_EXCEPTIONS)
+
+# Specify a custom flash command
+# This is the program that is invoked when the flash flag (-f or --flash) is
+# used with the Miosix Build System. Use $binary or $hex as placeolders, they
+# will be replaced by the build systems with the binary or hex file repectively.
+# If a command is not specified, the build system will use st-flash if found
+# set(PROGRAM_CMDLINE "here your custom flash command")
+
+# Basic flags
+set(FLAGS_BASE -mcpu=cortex-m7 -mthumb -mfloat-abi=hard -mfpu=fpv5-d16)
+
+# Flags for ASM and linker
+set(AFLAGS_BASE ${FLAGS_BASE})
+set(LFLAGS_BASE ${FLAGS_BASE} -Wl,--gc-sections,-Map,main.map -Wl,-T${LINKER_SCRIPT} ${OPT_EXCEPT} ${OPT_OPTIMIZATION} -nostdlib)
+
+# Flags for C/C++
+string(TOUPPER ${BOARD_NAME} BOARD_UPPER)
+set(CFLAGS_BASE
+    -D_BOARD_${BOARD_UPPER} -D_MIOSIX_BOARDNAME=\"${BOARD_NAME}\"
+    -D_DEFAULT_SOURCE=1 -ffunction-sections -Wall -Werror=return-type -g
+    -D_ARCH_CORTEXM7_STM32F7
+    ${CLOCK_FREQ} ${XRAM} ${SRAM_BOOT} ${FLAGS_BASE} ${OPT_OPTIMIZATION} -c
+)
+set(CXXFLAGS_BASE ${CFLAGS_BASE} ${OPT_EXCEPT})
+
+# Select architecture specific files
+set(ARCH_SRC
+    ${ARCH_PATH}/interfaces-impl/delays.cpp
+    ${ARCH_PATH}/interfaces-impl/gpio_impl.cpp
+    ${ARCH_PATH}/interfaces-impl/portability.cpp
+    ${BOARD_PATH}/interfaces-impl/bsp.cpp
+    ${KPATH}/arch/common/CMSIS/Device/ST/STM32F7xx/Source/Templates/system_stm32f7xx.c
+    ${KPATH}/arch/common/core/cache_cortexMx.cpp
+    ${KPATH}/arch/common/core/interrupts_cortexMx.cpp
+    ${KPATH}/arch/common/core/mpu_cortexMx.cpp
+    ${KPATH}/arch/common/core/stm32f2_f4_l4_f7_h7_os_timer.cpp
+    ${KPATH}/arch/common/drivers/sd_stm32f2_f4_f7.cpp
+    ${KPATH}/arch/common/drivers/serial_stm32.cpp
+    ${KPATH}/arch/common/drivers/stm32_hardware_rng.cpp
+)
diff --git a/src/bsps/stm32f767zi_gemini_motor/config/board_settings.h b/src/bsps/stm32f767zi_gemini_motor/config/board_settings.h
new file mode 100644
index 000000000..2332aa30e
--- /dev/null
+++ b/src/bsps/stm32f767zi_gemini_motor/config/board_settings.h
@@ -0,0 +1,62 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * 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.
+ */
+
+#pragma once
+
+/**
+ * \internal
+ * Versioning for board_settings.h for out of git tree projects
+ */
+#define BOARD_SETTINGS_VERSION 300
+
+namespace miosix
+{
+
+/**
+ * \addtogroup Settings
+ * \{
+ */
+
+/// Size of stack for main().
+/// The C standard library is stack-heavy (iprintf requires 1KB)
+const unsigned int MAIN_STACK_SIZE = 16 * 1024;
+
+/// Serial port
+const unsigned int defaultSerial      = 1;
+const unsigned int defaultSerialSpeed = 115200;
+#define SERIAL_1_DMA
+// #define SERIAL_2_DMA
+// #define SERIAL_3_DMA
+
+// SD card driver
+static const unsigned char sdVoltage = 33;  // Board powered @ 3.3V
+// #define SD_ONE_BIT_DATABUS
+#define SD_SDMMC 1  // Select either SDMMC1 or SDMMC2
+
+/// Analog supply voltage for ADC, DAC, Reset blocks, RCs and PLL
+#define V_DDA_VOLTAGE 3.3f
+
+/**
+ * \}
+ */
+
+}  // namespace miosix
diff --git a/src/bsps/stm32f767zi_gemini_motor/config/miosix_settings.h b/src/bsps/stm32f767zi_gemini_motor/config/miosix_settings.h
new file mode 100644
index 000000000..e1ca4aac6
--- /dev/null
+++ b/src/bsps/stm32f767zi_gemini_motor/config/miosix_settings.h
@@ -0,0 +1,240 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * 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.
+ */
+
+#pragma once
+
+// Before you can compile the kernel you have to configure it by editing this
+// file. After that, comment out this line to disable the reminder error.
+// The PARSING_FROM_IDE is because Netbeans gets confused by this, it is never
+// defined when compiling the code.
+#ifndef PARSING_FROM_IDE
+//#error This error is a reminder that you have not edited miosix_settings.h
+// yet.
+#endif  // PARSING_FROM_IDE
+
+/**
+ * \file miosix_settings.h
+ * NOTE: this file contains ONLY configuration options that are not dependent
+ * on architecture specific details. The other options are in the following
+ * files which are included here:
+ * miosix/arch/architecture name/common/arch_settings.h
+ * miosix/config/arch/architecture name/board name/board_settings.h
+ */
+#include "arch_settings.h"
+#include "board_settings.h"
+#include "util/version.h"
+
+/**
+ * \internal
+ * Versioning for miosix_settings.h for out of git tree projects
+ */
+#define MIOSIX_SETTINGS_VERSION 300
+
+namespace miosix
+{
+
+/**
+ * \addtogroup Settings
+ * \{
+ */
+
+//
+// Scheduler options
+//
+
+/// \def SCHED_TYPE_PRIORITY
+/// If uncommented selects the priority scheduler
+/// \def SCHED_TYPE_CONTROL_BASED
+/// If uncommented selects the control based scheduler
+/// \def SCHED_TYPE_EDF
+/// If uncommented selects the EDF scheduler
+// Uncomment only *one* of those
+
+#define SCHED_TYPE_PRIORITY
+//#define SCHED_TYPE_CONTROL_BASED
+//#define SCHED_TYPE_EDF
+
+/// \def WITH_CPU_TIME_COUNTER
+/// Allows to enable/disable CPUTimeCounter to save code size and remove its
+/// overhead from the scheduling process. By default it is not defined
+/// (CPUTimeCounter is disabled).
+//#define WITH_CPU_TIME_COUNTER
+
+//
+// Filesystem options
+//
+
+/// \def WITH_FILESYSTEM
+/// Allows to enable/disable filesystem support to save code size
+/// By default it is defined (filesystem support is enabled)
+#define WITH_FILESYSTEM
+
+/// \def WITH_DEVFS
+/// Allows to enable/disable DevFs support to save code size
+/// By default it is defined (DevFs is enabled)
+#define WITH_DEVFS
+
+/// \def SYNC_AFTER_WRITE
+/// Increases filesystem write robustness. After each write operation the
+/// filesystem is synced so that a power failure happens data is not lost
+/// (unless power failure happens exactly between the write and the sync)
+/// Unfortunately write latency and throughput becomes twice as worse
+/// By default it is defined (slow but safe)
+#define SYNC_AFTER_WRITE
+
+/// Maximum number of open files. Trying to open more will fail.
+/// Cannot be lower than 3, as the first three are stdin, stdout, stderr
+const unsigned char MAX_OPEN_FILES = 8;
+
+/// \def WITH_PROCESSES
+/// If uncommented enables support for processes as well as threads.
+/// This enables the dynamic loader to load elf programs, the extended system
+/// call service and, if the hardware supports it, the MPU to provide memory
+/// isolation of processes
+//#define WITH_PROCESSES
+
+#if defined(WITH_PROCESSES) && defined(__NO_EXCEPTIONS)
+#error Processes require C++ exception support
+#endif  // defined(WITH_PROCESSES) && defined(__NO_EXCEPTIONS)
+
+#if defined(WITH_PROCESSES) && !defined(WITH_FILESYSTEM)
+#error Processes require filesystem support
+#endif  // defined(WITH_PROCESSES) && !defined(WITH_FILESYSTEM)
+
+#if defined(WITH_PROCESSES) && !defined(WITH_DEVFS)
+#error Processes require devfs support
+#endif  // defined(WITH_PROCESSES) && !defined(WITH_DEVFS)
+
+//
+// C/C++ standard library I/O (stdin, stdout and stderr related)
+//
+
+/// \def WITH_BOOTLOG
+/// Uncomment to print bootlogs on stdout.
+/// By default it is defined (bootlogs are printed)
+#define WITH_BOOTLOG
+
+/// \def WITH_ERRLOG
+/// Uncomment for debug information on stdout.
+/// By default it is defined (error information is printed)
+#define WITH_ERRLOG
+
+//
+// Kernel related options (stack sizes, priorities)
+//
+
+/// \def WITH_DEEP_SLEEP
+/// Adds interfaces and required variables to support deep sleep state switch
+/// automatically when peripherals are not required
+//#define WITH_DEEP_SLEEP
+
+/**
+ * \def JTAG_DISABLE_SLEEP
+ * JTAG debuggers lose communication with the device if it enters sleep
+ * mode, so to use debugging it is necessary to disable sleep in the idle
+ * thread. By default it is not defined (idle thread calls sleep).
+ */
+//#define JTAG_DISABLE_SLEEP
+
+#if defined(WITH_DEEP_SLEEP) && defined(JTAG_DISABLE_SLEEP)
+#error Deep sleep cannot work together with jtag
+#endif  // defined(WITH_PROCESSES) && !defined(WITH_DEVFS)
+
+/// Minimum stack size (MUST be divisible by 4)
+const unsigned int STACK_MIN = 256;
+
+/// \internal Size of idle thread stack.
+/// Should be >=STACK_MIN (MUST be divisible by 4)
+const unsigned int STACK_IDLE = 256;
+
+/// Default stack size for pthread_create.
+/// The chosen value is enough to call C standard library functions
+/// such as printf/fopen which are stack-heavy
+const unsigned int STACK_DEFAULT_FOR_PTHREAD = 2048;
+
+/// Maximum size of the RAM image of a process. If a program requires more
+/// the kernel will not run it (MUST be divisible by 4)
+const unsigned int MAX_PROCESS_IMAGE_SIZE = 64 * 1024;
+
+/// Minimum size of the stack for a process. If a program specifies a lower
+/// size the kernel will not run it (MUST be divisible by 4)
+const unsigned int MIN_PROCESS_STACK_SIZE = STACK_MIN;
+
+/// Every userspace thread has two stacks, one for when it is running in
+/// userspace and one for when it is running in kernelspace (that is, while it
+/// is executing system calls). This is the size of the stack for when the
+/// thread is running in kernelspace (MUST be divisible by 4)
+const unsigned int SYSTEM_MODE_PROCESS_STACK_SIZE = 2 * 1024;
+
+/// Number of priorities (MUST be >1)
+/// PRIORITY_MAX-1 is the highest priority, 0 is the lowest. -1 is reserved as
+/// the priority of the idle thread.
+/// The meaning of a thread's priority depends on the chosen scheduler.
+#ifdef SCHED_TYPE_PRIORITY
+// Can be modified, but a high value makes context switches more expensive
+const short int PRIORITY_MAX = 4;
+#elif defined(SCHED_TYPE_CONTROL_BASED)
+// Don't touch, the limit is due to the fixed point implementation
+// It's not needed for if floating point is selected, but kept for consistency
+const short int PRIORITY_MAX = 64;
+#else  // SCHED_TYPE_EDF
+// Doesn't exist for this kind of scheduler
+#endif
+
+/// Priority of main()
+/// The meaning of a thread's priority depends on the chosen scheduler.
+const unsigned char MAIN_PRIORITY = 1;
+
+#ifdef SCHED_TYPE_PRIORITY
+/// Maximum thread time slice in nanoseconds, after which preemption occurs
+const unsigned int MAX_TIME_SLICE = 1000000;
+#endif  // SCHED_TYPE_PRIORITY
+
+//
+// Other low level kernel options. There is usually no need to modify these.
+//
+
+/// \internal Length of wartermark (in bytes) to check stack overflow.
+/// MUST be divisible by 4 and can also be zero.
+/// A high value increases context switch time.
+const unsigned int WATERMARK_LEN = 16;
+
+/// \internal Used to fill watermark
+const unsigned int WATERMARK_FILL = 0xaaaaaaaa;
+
+/// \internal Used to fill stack (for checking stack usage)
+const unsigned int STACK_FILL = 0xbbbbbbbb;
+
+// Compiler version checks
+#if !defined(_MIOSIX_GCC_PATCH_MAJOR) || _MIOSIX_GCC_PATCH_MAJOR < 3
+#error \
+    "You are using a too old or unsupported compiler. Get the latest one from https://miosix.org/wiki/index.php?title=Miosix_Toolchain"
+#endif
+#if _MIOSIX_GCC_PATCH_MAJOR > 3
+#warning "You are using a too new compiler, which may not be supported"
+#endif
+
+/**
+ * \}
+ */
+
+}  // namespace miosix
diff --git a/src/bsps/stm32f767zi_gemini_motor/core/stage_1_boot.cpp b/src/bsps/stm32f767zi_gemini_motor/core/stage_1_boot.cpp
new file mode 100644
index 000000000..83eac53b4
--- /dev/null
+++ b/src/bsps/stm32f767zi_gemini_motor/core/stage_1_boot.cpp
@@ -0,0 +1,505 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * 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 <string.h>
+
+#include "core/cache_cortexMx.h"
+#include "core/interrupts.h"  //For the unexpected interrupt call
+#include "core/interrupts_cortexMx.h"
+#include "interfaces/arch_registers.h"
+#include "interfaces/bsp.h"
+#include "kernel/stage_2_boot.h"
+
+/*
+ * startup.cpp
+ * STM32 C++ startup.
+ * NOTE: for stm32f767 devices ONLY.
+ * Supports interrupt handlers in C++ without extern "C"
+ * Developed by Terraneo Federico, based on ST startup code.
+ * Additionally modified to boot Miosix.
+ */
+
+/**
+ * Called by Reset_Handler, performs initialization and calls main.
+ * Never returns.
+ */
+void program_startup() __attribute__((noreturn));
+void program_startup()
+{
+    // Cortex M7 core appears to get out of reset with interrupts already
+    // enabled
+    __disable_irq();
+
+    miosix::IRQconfigureCache((const unsigned int *)0xd0000000,
+                              8 * 1024 * 1024);
+
+    // These are defined in the linker script
+    extern unsigned char _etext asm("_etext");
+    extern unsigned char _data asm("_data");
+    extern unsigned char _edata asm("_edata");
+    extern unsigned char _bss_start asm("_bss_start");
+    extern unsigned char _bss_end asm("_bss_end");
+
+    // Initialize .data section, clear .bss section
+    unsigned char *etext     = &_etext;
+    unsigned char *data      = &_data;
+    unsigned char *edata     = &_edata;
+    unsigned char *bss_start = &_bss_start;
+    unsigned char *bss_end   = &_bss_end;
+    memcpy(data, etext, edata - data);
+    memset(bss_start, 0, bss_end - bss_start);
+
+    // Move on to stage 2
+    _init();
+
+    // If main returns, reboot
+    NVIC_SystemReset();
+    for (;;)
+        ;
+}
+
+/**
+ * Reset handler, called by hardware immediately after reset
+ */
+void Reset_Handler() __attribute__((__interrupt__, noreturn));
+void Reset_Handler()
+{
+    /**
+     * SystemInit() is called *before* initializing .data and zeroing .bss
+     * Despite all startup files provided by ST do the opposite, there are three
+     * good reasons to do so:
+     * 1. First, the CMSIS specifications say that SystemInit() must not access
+     *    global variables, so it is actually possible to call it before
+     * 2. Second, when running Miosix with the xram linker scripts .data and
+     *    .bss are placed in the external RAM, so we *must* call SystemInit(),
+     *    which enables xram, before touching .data and .bss
+     * 3. Third, this is a performance improvement since the loops that
+     *    initialize .data and zeros .bss now run with the CPU at full speed
+     *    instead of 8MHz
+     */
+    SystemInit();
+
+/**
+ * ST does not provide code to initialize the SDRAM at boot.
+ * Put after SystemInit() as SDRAM is timing-sensitive and needs the full
+ * clock speed.
+ */
+#ifdef __ENABLE_XRAM
+    miosix::configureSdram();
+#endif  //__ENABLE_XRAM
+
+    /*
+     * Load into the program stack pointer the heap end address and switch from
+     * the msp to sps.
+     * This is required for booting Miosix, a small portion of the top of the
+     * heap area will be used as stack until the first thread starts. After,
+     * this stack will be abandoned and the process stack will point to the
+     * current thread's stack.
+     */
+    asm volatile(
+        "ldr r0,  =_heap_end          \n\t"
+        "msr psp, r0                  \n\t"
+        "movw r0, #2                  \n\n"  // Set the control register to use
+        "msr control, r0              \n\t"  // the process stack
+        "isb                          \n\t" ::
+            : "r0");
+
+    program_startup();
+}
+
+/**
+ * All unused interrupts call this function.
+ */
+extern "C" void Default_Handler() { unexpectedInterrupt(); }
+
+// System handlers
+void /*__attribute__((weak))*/ Reset_Handler();      // These interrupts are not
+void /*__attribute__((weak))*/ NMI_Handler();        // weak because they are
+void /*__attribute__((weak))*/ HardFault_Handler();  // surely defined by Miosix
+void /*__attribute__((weak))*/ MemManage_Handler();
+void /*__attribute__((weak))*/ BusFault_Handler();
+void /*__attribute__((weak))*/ UsageFault_Handler();
+void /*__attribute__((weak))*/ SVC_Handler();
+void /*__attribute__((weak))*/ DebugMon_Handler();
+void /*__attribute__((weak))*/ PendSV_Handler();
+void __attribute__((weak)) SysTick_Handler();
+
+// Interrupt handlers
+void __attribute__((weak)) WWDG_IRQHandler();
+void __attribute__((weak)) PVD_IRQHandler();
+void __attribute__((weak)) TAMP_STAMP_IRQHandler();
+void __attribute__((weak)) RTC_WKUP_IRQHandler();
+void __attribute__((weak)) FLASH_IRQHandler();
+void __attribute__((weak)) RCC_IRQHandler();
+void __attribute__((weak)) EXTI0_IRQHandler();
+void __attribute__((weak)) EXTI1_IRQHandler();
+void __attribute__((weak)) EXTI2_IRQHandler();
+void __attribute__((weak)) EXTI3_IRQHandler();
+void __attribute__((weak)) EXTI4_IRQHandler();
+void __attribute__((weak)) DMA1_Stream0_IRQHandler();
+void __attribute__((weak)) DMA1_Stream1_IRQHandler();
+void __attribute__((weak)) DMA1_Stream2_IRQHandler();
+void __attribute__((weak)) DMA1_Stream3_IRQHandler();
+void __attribute__((weak)) DMA1_Stream4_IRQHandler();
+void __attribute__((weak)) DMA1_Stream5_IRQHandler();
+void __attribute__((weak)) DMA1_Stream6_IRQHandler();
+void __attribute__((weak)) ADC_IRQHandler();
+void __attribute__((weak)) CAN1_TX_IRQHandler();
+void __attribute__((weak)) CAN1_RX0_IRQHandler();
+void __attribute__((weak)) CAN1_RX1_IRQHandler();
+void __attribute__((weak)) CAN1_SCE_IRQHandler();
+void __attribute__((weak)) EXTI9_5_IRQHandler();
+void __attribute__((weak)) TIM1_BRK_TIM9_IRQHandler();
+void __attribute__((weak)) TIM1_UP_TIM10_IRQHandler();
+void __attribute__((weak)) TIM1_TRG_COM_TIM11_IRQHandler();
+void __attribute__((weak)) TIM1_CC_IRQHandler();
+void __attribute__((weak)) TIM2_IRQHandler();
+void __attribute__((weak)) TIM3_IRQHandler();
+void __attribute__((weak)) TIM4_IRQHandler();
+void __attribute__((weak)) I2C1_EV_IRQHandler();
+void __attribute__((weak)) I2C1_ER_IRQHandler();
+void __attribute__((weak)) I2C2_EV_IRQHandler();
+void __attribute__((weak)) I2C2_ER_IRQHandler();
+void __attribute__((weak)) SPI1_IRQHandler();
+void __attribute__((weak)) SPI2_IRQHandler();
+void __attribute__((weak)) USART1_IRQHandler();
+void __attribute__((weak)) USART2_IRQHandler();
+void __attribute__((weak)) USART3_IRQHandler();
+void __attribute__((weak)) EXTI15_10_IRQHandler();
+void __attribute__((weak)) RTC_Alarm_IRQHandler();
+void __attribute__((weak)) OTG_FS_WKUP_IRQHandler();
+void __attribute__((weak)) TIM8_BRK_TIM12_IRQHandler();
+void __attribute__((weak)) TIM8_UP_TIM13_IRQHandler();
+void __attribute__((weak)) TIM8_TRG_COM_TIM14_IRQHandler();
+void __attribute__((weak)) TIM8_CC_IRQHandler();
+void __attribute__((weak)) DMA1_Stream7_IRQHandler();
+void __attribute__((weak)) FMC_IRQHandler();
+void __attribute__((weak)) SDMMC1_IRQHandler();
+void __attribute__((weak)) TIM5_IRQHandler();
+void __attribute__((weak)) SPI3_IRQHandler();
+void __attribute__((weak)) UART4_IRQHandler();
+void __attribute__((weak)) UART5_IRQHandler();
+void __attribute__((weak)) TIM6_DAC_IRQHandler();
+void __attribute__((weak)) TIM7_IRQHandler();
+void __attribute__((weak)) DMA2_Stream0_IRQHandler();
+void __attribute__((weak)) DMA2_Stream1_IRQHandler();
+void __attribute__((weak)) DMA2_Stream2_IRQHandler();
+void __attribute__((weak)) DMA2_Stream3_IRQHandler();
+void __attribute__((weak)) DMA2_Stream4_IRQHandler();
+void __attribute__((weak)) ETH_IRQHandler();
+void __attribute__((weak)) ETH_WKUP_IRQHandler();
+void __attribute__((weak)) CAN2_TX_IRQHandler();
+void __attribute__((weak)) CAN2_RX0_IRQHandler();
+void __attribute__((weak)) CAN2_RX1_IRQHandler();
+void __attribute__((weak)) CAN2_SCE_IRQHandler();
+void __attribute__((weak)) OTG_FS_IRQHandler();
+void __attribute__((weak)) DMA2_Stream5_IRQHandler();
+void __attribute__((weak)) DMA2_Stream6_IRQHandler();
+void __attribute__((weak)) DMA2_Stream7_IRQHandler();
+void __attribute__((weak)) USART6_IRQHandler();
+void __attribute__((weak)) I2C3_EV_IRQHandler();
+void __attribute__((weak)) I2C3_ER_IRQHandler();
+void __attribute__((weak)) OTG_HS_EP1_OUT_IRQHandler();
+void __attribute__((weak)) OTG_HS_EP1_IN_IRQHandler();
+void __attribute__((weak)) OTG_HS_WKUP_IRQHandler();
+void __attribute__((weak)) OTG_HS_IRQHandler();
+void __attribute__((weak)) DCMI_IRQHandler();
+void __attribute__((weak)) CRYP_IRQHandler();
+void __attribute__((weak)) RNG_IRQHandler();
+void __attribute__((weak)) FPU_IRQHandler();
+void __attribute__((weak)) UART7_IRQHandler();
+void __attribute__((weak)) UART8_IRQHandler();
+void __attribute__((weak)) SPI4_IRQHandler();
+void __attribute__((weak)) SPI5_IRQHandler();
+void __attribute__((weak)) SPI6_IRQHandler();
+void __attribute__((weak)) SAI1_IRQHandler();
+void __attribute__((weak)) LTDC_IRQHandler();
+void __attribute__((weak)) LTDC_ER_IRQHandler();
+void __attribute__((weak)) DMA2D_IRQHandler();
+void __attribute__((weak)) SAI2_IRQHandler();
+void __attribute__((weak)) QUADSPI_IRQHandler();
+void __attribute__((weak)) LPTIM1_IRQHandler();
+void __attribute__((weak)) CEC_IRQHandler();
+void __attribute__((weak)) I2C4_EV_IRQHandler();
+void __attribute__((weak)) I2C4_ER_IRQHandler();
+void __attribute__((weak)) SPDIF_RX_IRQHandler();
+void __attribute__((weak)) DSIHOST_IRQHandler();
+void __attribute__((weak)) DFSDM1_FLT0_IRQHandler();
+void __attribute__((weak)) DFSDM1_FLT1_IRQHandler();
+void __attribute__((weak)) DFSDM1_FLT2_IRQHandler();
+void __attribute__((weak)) DFSDM1_FLT3_IRQHandler();
+void __attribute__((weak)) SDMMC2_IRQHandler();
+void __attribute__((weak)) CAN3_TX_IRQHandler();
+void __attribute__((weak)) CAN3_RX0_IRQHandler();
+void __attribute__((weak)) CAN3_RX1_IRQHandler();
+void __attribute__((weak)) CAN3_SCE_IRQHandler();
+void __attribute__((weak)) JPEG_IRQHandler();
+void __attribute__((weak)) MDIOS_IRQHandler();
+
+// Stack top, defined in the linker script
+extern char _main_stack_top asm("_main_stack_top");
+
+// Interrupt vectors, must be placed @ address 0x00000000
+// The extern declaration is required otherwise g++ optimizes it out
+extern void (*const __Vectors[])();
+void (*const __Vectors[])() __attribute__((section(".isr_vector"))) = {
+    reinterpret_cast<void (*)()>(&_main_stack_top), /* Stack pointer*/
+    Reset_Handler,                                  /* Reset Handler */
+    NMI_Handler,                                    /* NMI Handler */
+    HardFault_Handler,                              /* Hard Fault Handler */
+    MemManage_Handler,                              /* MPU Fault Handler */
+    BusFault_Handler,                               /* Bus Fault Handler */
+    UsageFault_Handler,                             /* Usage Fault Handler */
+    0,                                              /* Reserved */
+    0,                                              /* Reserved */
+    0,                                              /* Reserved */
+    0,                                              /* Reserved */
+    SVC_Handler,                                    /* SVCall Handler */
+    DebugMon_Handler,                               /* Debug Monitor Handler */
+    0,                                              /* Reserved */
+    PendSV_Handler,                                 /* PendSV Handler */
+    SysTick_Handler,                                /* SysTick Handler */
+
+    /* External Interrupts */
+    WWDG_IRQHandler,
+    PVD_IRQHandler,
+    TAMP_STAMP_IRQHandler,
+    RTC_WKUP_IRQHandler,
+    FLASH_IRQHandler,
+    RCC_IRQHandler,
+    EXTI0_IRQHandler,
+    EXTI1_IRQHandler,
+    EXTI2_IRQHandler,
+    EXTI3_IRQHandler,
+    EXTI4_IRQHandler,
+    DMA1_Stream0_IRQHandler,
+    DMA1_Stream1_IRQHandler,
+    DMA1_Stream2_IRQHandler,
+    DMA1_Stream3_IRQHandler,
+    DMA1_Stream4_IRQHandler,
+    DMA1_Stream5_IRQHandler,
+    DMA1_Stream6_IRQHandler,
+    ADC_IRQHandler,
+    CAN1_TX_IRQHandler,
+    CAN1_RX0_IRQHandler,
+    CAN1_RX1_IRQHandler,
+    CAN1_SCE_IRQHandler,
+    EXTI9_5_IRQHandler,
+    TIM1_BRK_TIM9_IRQHandler,
+    TIM1_UP_TIM10_IRQHandler,
+    TIM1_TRG_COM_TIM11_IRQHandler,
+    TIM1_CC_IRQHandler,
+    TIM2_IRQHandler,
+    TIM3_IRQHandler,
+    TIM4_IRQHandler,
+    I2C1_EV_IRQHandler,
+    I2C1_ER_IRQHandler,
+    I2C2_EV_IRQHandler,
+    I2C2_ER_IRQHandler,
+    SPI1_IRQHandler,
+    SPI2_IRQHandler,
+    USART1_IRQHandler,
+    USART2_IRQHandler,
+    USART3_IRQHandler,
+    EXTI15_10_IRQHandler,
+    RTC_Alarm_IRQHandler,
+    OTG_FS_WKUP_IRQHandler,
+    TIM8_BRK_TIM12_IRQHandler,
+    TIM8_UP_TIM13_IRQHandler,
+    TIM8_TRG_COM_TIM14_IRQHandler,
+    TIM8_CC_IRQHandler,
+    DMA1_Stream7_IRQHandler,
+    FMC_IRQHandler,
+    SDMMC1_IRQHandler,
+    TIM5_IRQHandler,
+    SPI3_IRQHandler,
+    UART4_IRQHandler,
+    UART5_IRQHandler,
+    TIM6_DAC_IRQHandler,
+    TIM7_IRQHandler,
+    DMA2_Stream0_IRQHandler,
+    DMA2_Stream1_IRQHandler,
+    DMA2_Stream2_IRQHandler,
+    DMA2_Stream3_IRQHandler,
+    DMA2_Stream4_IRQHandler,
+    ETH_IRQHandler,
+    ETH_WKUP_IRQHandler,
+    CAN2_TX_IRQHandler,
+    CAN2_RX0_IRQHandler,
+    CAN2_RX1_IRQHandler,
+    CAN2_SCE_IRQHandler,
+    OTG_FS_IRQHandler,
+    DMA2_Stream5_IRQHandler,
+    DMA2_Stream6_IRQHandler,
+    DMA2_Stream7_IRQHandler,
+    USART6_IRQHandler,
+    I2C3_EV_IRQHandler,
+    I2C3_ER_IRQHandler,
+    OTG_HS_EP1_OUT_IRQHandler,
+    OTG_HS_EP1_IN_IRQHandler,
+    OTG_HS_WKUP_IRQHandler,
+    OTG_HS_IRQHandler,
+    DCMI_IRQHandler,
+    CRYP_IRQHandler,
+    RNG_IRQHandler,
+    FPU_IRQHandler,
+    UART7_IRQHandler,
+    UART8_IRQHandler,
+    SPI4_IRQHandler,
+    SPI5_IRQHandler,
+    SPI6_IRQHandler,
+    SAI1_IRQHandler,
+    LTDC_IRQHandler,
+    LTDC_ER_IRQHandler,
+    DMA2D_IRQHandler,
+    SAI2_IRQHandler,
+    QUADSPI_IRQHandler,
+    LPTIM1_IRQHandler,
+    CEC_IRQHandler,
+    I2C4_EV_IRQHandler,
+    I2C4_ER_IRQHandler,
+    SPDIF_RX_IRQHandler,
+    DSIHOST_IRQHandler,
+    DFSDM1_FLT0_IRQHandler,
+    DFSDM1_FLT1_IRQHandler,
+    DFSDM1_FLT2_IRQHandler,
+    DFSDM1_FLT3_IRQHandler,
+    SDMMC2_IRQHandler,
+    CAN3_TX_IRQHandler,
+    CAN3_RX0_IRQHandler,
+    CAN3_RX1_IRQHandler,
+    CAN3_SCE_IRQHandler,
+    JPEG_IRQHandler,
+    MDIOS_IRQHandler,
+};
+
+#pragma weak SysTick_IRQHandler            = Default_Handler
+#pragma weak WWDG_IRQHandler               = Default_Handler
+#pragma weak PVD_IRQHandler                = Default_Handler
+#pragma weak TAMP_STAMP_IRQHandler         = Default_Handler
+#pragma weak RTC_WKUP_IRQHandler           = Default_Handler
+#pragma weak FLASH_IRQHandler              = Default_Handler
+#pragma weak RCC_IRQHandler                = Default_Handler
+#pragma weak EXTI0_IRQHandler              = Default_Handler
+#pragma weak EXTI1_IRQHandler              = Default_Handler
+#pragma weak EXTI2_IRQHandler              = Default_Handler
+#pragma weak EXTI3_IRQHandler              = Default_Handler
+#pragma weak EXTI4_IRQHandler              = Default_Handler
+#pragma weak DMA1_Stream0_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream1_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream2_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream3_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream4_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream5_IRQHandler       = Default_Handler
+#pragma weak DMA1_Stream6_IRQHandler       = Default_Handler
+#pragma weak ADC_IRQHandler                = Default_Handler
+#pragma weak CAN1_TX_IRQHandler            = Default_Handler
+#pragma weak CAN1_RX0_IRQHandler           = Default_Handler
+#pragma weak CAN1_RX1_IRQHandler           = Default_Handler
+#pragma weak CAN1_SCE_IRQHandler           = Default_Handler
+#pragma weak EXTI9_5_IRQHandler            = Default_Handler
+#pragma weak TIM1_BRK_TIM9_IRQHandler      = Default_Handler
+#pragma weak TIM1_UP_TIM10_IRQHandler      = Default_Handler
+#pragma weak TIM1_TRG_COM_TIM11_IRQHandler = Default_Handler
+#pragma weak TIM1_CC_IRQHandler            = Default_Handler
+#pragma weak TIM2_IRQHandler               = Default_Handler
+#pragma weak TIM3_IRQHandler               = Default_Handler
+#pragma weak TIM4_IRQHandler               = Default_Handler
+#pragma weak I2C1_EV_IRQHandler            = Default_Handler
+#pragma weak I2C1_ER_IRQHandler            = Default_Handler
+#pragma weak I2C2_EV_IRQHandler            = Default_Handler
+#pragma weak I2C2_ER_IRQHandler            = Default_Handler
+#pragma weak SPI1_IRQHandler               = Default_Handler
+#pragma weak SPI2_IRQHandler               = Default_Handler
+// #pragma weak USART1_IRQHandler             = Default_Handler
+// #pragma weak USART2_IRQHandler             = Default_Handler
+// #pragma weak USART3_IRQHandler             = Default_Handler
+#pragma weak EXTI15_10_IRQHandler          = Default_Handler
+#pragma weak RTC_Alarm_IRQHandler          = Default_Handler
+#pragma weak OTG_FS_WKUP_IRQHandler        = Default_Handler
+#pragma weak TIM8_BRK_TIM12_IRQHandler     = Default_Handler
+#pragma weak TIM8_UP_TIM13_IRQHandler      = Default_Handler
+#pragma weak TIM8_TRG_COM_TIM14_IRQHandler = Default_Handler
+#pragma weak TIM8_CC_IRQHandler            = Default_Handler
+#pragma weak DMA1_Stream7_IRQHandler       = Default_Handler
+#pragma weak FMC_IRQHandler                = Default_Handler
+#pragma weak SDMMC1_IRQHandler             = Default_Handler
+#pragma weak TIM5_IRQHandler               = Default_Handler
+#pragma weak SPI3_IRQHandler               = Default_Handler
+// #pragma weak UART4_IRQHandler              = Default_Handler
+// #pragma weak UART5_IRQHandler              = Default_Handler
+#pragma weak TIM6_DAC_IRQHandler     = Default_Handler
+#pragma weak TIM7_IRQHandler         = Default_Handler
+#pragma weak DMA2_Stream0_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream1_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream2_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream3_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream4_IRQHandler = Default_Handler
+#pragma weak ETH_IRQHandler          = Default_Handler
+#pragma weak ETH_WKUP_IRQHandler     = Default_Handler
+#pragma weak CAN2_TX_IRQHandler      = Default_Handler
+#pragma weak CAN2_RX0_IRQHandler     = Default_Handler
+#pragma weak CAN2_RX1_IRQHandler     = Default_Handler
+#pragma weak CAN2_SCE_IRQHandler     = Default_Handler
+#pragma weak OTG_FS_IRQHandler       = Default_Handler
+#pragma weak DMA2_Stream5_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream6_IRQHandler = Default_Handler
+#pragma weak DMA2_Stream7_IRQHandler = Default_Handler
+// #pragma weak USART6_IRQHandler             = Default_Handler
+#pragma weak I2C3_EV_IRQHandler        = Default_Handler
+#pragma weak I2C3_ER_IRQHandler        = Default_Handler
+#pragma weak OTG_HS_EP1_OUT_IRQHandler = Default_Handler
+#pragma weak OTG_HS_EP1_IN_IRQHandler  = Default_Handler
+#pragma weak OTG_HS_WKUP_IRQHandler    = Default_Handler
+#pragma weak OTG_HS_IRQHandler         = Default_Handler
+#pragma weak DCMI_IRQHandler           = Default_Handler
+#pragma weak CRYP_IRQHandler           = Default_Handler
+#pragma weak RNG_IRQHandler            = Default_Handler
+#pragma weak FPU_IRQHandler            = Default_Handler
+// #pragma weak UART7_IRQHandler              = Default_Handler
+// #pragma weak UART8_IRQHandler              = Default_Handler
+#pragma weak SPI4_IRQHandler        = Default_Handler
+#pragma weak SPI5_IRQHandler        = Default_Handler
+#pragma weak SPI6_IRQHandler        = Default_Handler
+#pragma weak SAI1_IRQHandler        = Default_Handler
+#pragma weak LTDC_IRQHandler        = Default_Handler
+#pragma weak LTDC_ER_IRQHandler     = Default_Handler
+#pragma weak DMA2D_IRQHandler       = Default_Handler
+#pragma weak SAI2_IRQHandler        = Default_Handler
+#pragma weak QUADSPI_IRQHandler     = Default_Handler
+#pragma weak LPTIM1_IRQHandler      = Default_Handler
+#pragma weak CEC_IRQHandler         = Default_Handler
+#pragma weak I2C4_EV_IRQHandler     = Default_Handler
+#pragma weak I2C4_ER_IRQHandler     = Default_Handler
+#pragma weak SPDIF_RX_IRQHandler    = Default_Handler
+#pragma weak DSIHOST_IRQHandler     = Default_Handler
+#pragma weak DFSDM1_FLT0_IRQHandler = Default_Handler
+#pragma weak DFSDM1_FLT1_IRQHandler = Default_Handler
+#pragma weak DFSDM1_FLT2_IRQHandler = Default_Handler
+#pragma weak DFSDM1_FLT3_IRQHandler = Default_Handler
+#pragma weak SDMMC2_IRQHandler      = Default_Handler
+#pragma weak CAN3_TX_IRQHandler     = Default_Handler
+#pragma weak CAN3_RX0_IRQHandler    = Default_Handler
+#pragma weak CAN3_RX1_IRQHandler    = Default_Handler
+#pragma weak CAN3_SCE_IRQHandler    = Default_Handler
+#pragma weak JPEG_IRQHandler        = Default_Handler
+#pragma weak MDIOS_IRQHandler       = Default_Handler
diff --git a/src/bsps/stm32f767zi_gemini_motor/interfaces-impl/arch_registers_impl.h b/src/bsps/stm32f767zi_gemini_motor/interfaces-impl/arch_registers_impl.h
new file mode 100644
index 000000000..3224edc9e
--- /dev/null
+++ b/src/bsps/stm32f767zi_gemini_motor/interfaces-impl/arch_registers_impl.h
@@ -0,0 +1,39 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * 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.
+ */
+
+#ifndef ARCH_REGISTERS_IMPL_H
+#define ARCH_REGISTERS_IMPL_H
+
+// stm32f7xx.h defines a few macros like __ICACHE_PRESENT, __DCACHE_PRESENT and
+// includes core_cm7.h. Do not include core_cm7.h before.
+#define STM32F767xx
+#include "CMSIS/Device/ST/STM32F7xx/Include/stm32f7xx.h"
+
+#if (__ICACHE_PRESENT != 1) || (__DCACHE_PRESENT != 1)
+#error "Wrong include order"
+#endif
+
+#include "CMSIS/Device/ST/STM32F7xx/Include/system_stm32f7xx.h"
+
+#define RCC_SYNC() __DSB()  // TODO: can this dsb be removed?
+
+#endif  // ARCH_REGISTERS_IMPL_H
diff --git a/src/bsps/stm32f767zi_gemini_motor/interfaces-impl/bsp.cpp b/src/bsps/stm32f767zi_gemini_motor/interfaces-impl/bsp.cpp
new file mode 100644
index 000000000..12303cae3
--- /dev/null
+++ b/src/bsps/stm32f767zi_gemini_motor/interfaces-impl/bsp.cpp
@@ -0,0 +1,370 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * 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.
+ */
+
+/***********************************************************************
+ * bsp.cpp Part of the Miosix Embedded OS.
+ * Board support package, this file initializes hardware.
+ ************************************************************************/
+
+#include "interfaces/bsp.h"
+
+#include <inttypes.h>
+#include <sys/ioctl.h>
+
+#include <cstdlib>
+
+#include "board_settings.h"
+#include "config/miosix_settings.h"
+#include "drivers/sd_stm32f2_f4_f7.h"
+#include "drivers/serial.h"
+#include "drivers/serial_stm32.h"
+#include "drivers/stm32_sgm.h"
+#include "filesystem/console/console_device.h"
+#include "filesystem/file_access.h"
+#include "interfaces/arch_registers.h"
+#include "interfaces/delays.h"
+#include "interfaces/portability.h"
+#include "kernel/kernel.h"
+#include "kernel/logging.h"
+#include "kernel/sync.h"
+
+namespace miosix
+{
+
+//
+// Initialization
+//
+
+static void sdramCommandWait()
+{
+    for (int i = 0; i < 0xffff; i++)
+        if ((FMC_Bank5_6->SDSR & FMC_SDSR_BUSY) == 0)
+            return;
+}
+
+void configureSdram()
+{
+    // Enable all gpios
+    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN | RCC_AHB1ENR_GPIOBEN |
+                    RCC_AHB1ENR_GPIOCEN | RCC_AHB1ENR_GPIODEN |
+                    RCC_AHB1ENR_GPIOEEN | RCC_AHB1ENR_GPIOFEN |
+                    RCC_AHB1ENR_GPIOGEN;
+    RCC_SYNC();
+
+    // On the compute unit with F767ZI, the SDRAM pins are:
+    // - PG8:  FMC_SDCLK  (sdram clock)
+    // - PB5:  FMC_SDCKE1 (sdram bank 2 clock enable)
+    // - PB6:  FMC_SDNE1  (sdram bank 2 chip enable)
+    // - PF0:  FMC_A0
+    // - PF1:  FMC_A1
+    // - PF2:  FMC_A2
+    // - PF3:  FMC_A3
+    // - PF4:  FMC_A4
+    // - PF5:  FMC_A5
+    // - PF12: FMC_A6
+    // - PF13: FMC_A7
+    // - PF14: FMC_A8
+    // - PF15: FMC_A9
+    // - PG0:  FMC_A10
+    // - PG1:  FMC_A11
+    // - PG2:  FMC_A12    (used only by the 32MB ram, not by the 8MB one)
+    // - PD14: FMC_D0
+    // - PD15: FMC_D1
+    // - PD0:  FMC_D2
+    // - PD1:  FMC_D3
+    // - PE7:  FMC_D4
+    // - PE8:  FMC_D5
+    // - PE9:  FMC_D6
+    // - PE10: FMC_D7
+    // - PE11: FMC_D8
+    // - PE12: FMC_D9
+    // - PE13: FMC_D10
+    // - PE14: FMC_D11
+    // - PE15: FMC_D12
+    // - PD8:  FMC_D13
+    // - PD9:  FMC_D14
+    // - PD10: FMC_D15
+
+    // - PG4:  FMC_BA0
+    // - PG5:  FMC_BA1
+    // - PF11: FMC_SDNRAS
+    // - PG15: FMC_SDNCAS
+    // - PC0:  FMC_SDNWE
+    // - PE0:  FMC_NBL0
+    // - PE1:  FMC_NBL1
+
+    // All SDRAM GPIOs needs to be configured with alternate function 12 and
+    // maximum speed
+
+    // WARNING: The current configuration is for the 8MB ram
+
+    // Alternate functions
+    GPIOB->AFR[0] = 0x0cc00000;
+    GPIOC->AFR[0] = 0x0000000c;
+    GPIOD->AFR[0] = 0x000000cc;
+    GPIOD->AFR[1] = 0xcc000ccc;
+    GPIOE->AFR[0] = 0xc00000cc;
+    GPIOE->AFR[1] = 0xcccccccc;
+    GPIOF->AFR[0] = 0x00cccccc;
+    GPIOF->AFR[1] = 0xccccc000;
+    GPIOG->AFR[0] = 0x00cc0ccc;
+    GPIOG->AFR[1] = 0xc000000c;
+
+    // Mode
+    GPIOB->MODER = 0x00002800;
+    GPIOC->MODER = 0x00000002;
+    GPIOD->MODER = 0xa02a000a;
+    GPIOE->MODER = 0xaaaa800a;
+    GPIOF->MODER = 0xaa800aaa;
+    GPIOG->MODER = 0x80020a2a;
+
+    // Speed (high speed for all, very high speed for SDRAM pins)
+    GPIOB->OSPEEDR = 0x00003c00;
+    GPIOC->OSPEEDR = 0x00000003;
+    GPIOD->OSPEEDR = 0xf03f000f;
+    GPIOE->OSPEEDR = 0xffffc00f;
+    GPIOF->OSPEEDR = 0xffc00fff;
+    GPIOG->OSPEEDR = 0xc0030f3f;
+
+    // Since we'we un-configured PB3 and PB4 (by default they are SWO and NJRST)
+    // finish the job and remove the default pull-up
+    GPIOB->PUPDR = 0;
+
+    // Enable the SDRAM controller clock
+    RCC->AHB3ENR |= RCC_AHB3ENR_FMCEN;
+    RCC_SYNC();
+
+    // The SDRAM is a AS4C4M16SA-6TAN
+    // HCLK = 216MHz -> SDRAM clock = HCLK/2 = 133MHz
+
+    // 1. Memory device features
+    FMC_Bank5_6->SDCR[0] = 0                     // 0 delay after CAS latency
+                           | FMC_SDCR1_RBURST    // Enable read bursts
+                           | FMC_SDCR1_SDCLK_1;  // SDCLK = HCLK / 2
+    FMC_Bank5_6->SDCR[1] = 0                     // Write accesses allowed
+                           | FMC_SDCR2_CAS_1     // 2 cycles CAS latency
+                           | FMC_SDCR2_NB        // 4 internal banks
+                           | FMC_SDCR2_MWID_0    // 16 bit data bus
+                           | FMC_SDCR2_NR_1      // 13 bit row address
+                           // cppcheck-suppress duplicateExpression
+                           | 0;  // 8 bit column address
+
+// 2. Memory device timings
+#ifdef SYSCLK_FREQ_216MHz
+    // SDRAM timings. One clock cycle is 9.26ns
+    FMC_Bank5_6->SDTR[0] =
+        (2 - 1) << FMC_SDTR1_TRP_Pos     // 2 cycles TRP  (18.52ns > 18ns)
+        | (7 - 1) << FMC_SDTR1_TRC_Pos;  // 7 cycles TRC  (64.82ns > 60ns)
+    FMC_Bank5_6->SDTR[1] =
+        (2 - 1) << FMC_SDTR1_TRCD_Pos     // 2 cycles TRCD (18.52ns > 18ns)
+        | (2 - 1) << FMC_SDTR1_TWR_Pos    // 2 cycles TWR  (min 2cc > 12ns)
+        | (5 - 1) << FMC_SDTR1_TRAS_Pos   // 5 cycles TRAS (46.3ns  > 42ns)
+        | (7 - 1) << FMC_SDTR1_TXSR_Pos   // 7 cycles TXSR (74.08ns > 61.5ns)
+        | (2 - 1) << FMC_SDTR1_TMRD_Pos;  // 2 cycles TMRD (min 2cc > 12ns)
+#else
+#error No SDRAM timings for this clock
+#endif
+
+    // 3. Enable the bank 2 clock
+    FMC_Bank5_6->SDCMR = FMC_SDCMR_MODE_0   // Clock Configuration Enable
+                         | FMC_SDCMR_CTB2;  // Bank 2
+    sdramCommandWait();
+
+    // 4. Wait during command execution
+    delayUs(100);
+
+    // 5. Issue a "Precharge All" command
+    FMC_Bank5_6->SDCMR = FMC_SDCMR_MODE_1   // Precharge all
+                         | FMC_SDCMR_CTB2;  // Bank 2
+    sdramCommandWait();
+
+    // 6. Issue Auto-Refresh commands
+    FMC_Bank5_6->SDCMR = FMC_SDCMR_MODE_1 | FMC_SDCMR_MODE_0  // Auto-Refresh
+                         | FMC_SDCMR_CTB2                     // Bank 2
+                         | (8 - 1) << FMC_SDCMR_NRFS_Pos;     // 2 Auto-Refresh
+    sdramCommandWait();
+
+    // 7. Issue a Load Mode Register command
+    FMC_Bank5_6->SDCMR = FMC_SDCMR_MODE_2               /// Load mode register
+                         | FMC_SDCMR_CTB2               // Bank 2
+                         | 0x220 << FMC_SDCMR_MRD_Pos;  // CAS = 2, burst = 1
+    sdramCommandWait();
+
+// 8. Program the refresh rate (4K / 32ms)
+// 64ms / 8192 = 7.8125us
+#ifdef SYSCLK_FREQ_216MHz
+    // 7.8125us * 133MHz = 1039 - 20 = 1019
+    FMC_Bank5_6->SDRTR = 1019 << FMC_SDRTR_COUNT_Pos;
+#else
+#error No SDRAM refresh timings for this clock
+#endif
+}
+
+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::miso::alternateFunction(5);
+    interfaces::spi1::mosi::mode(Mode::ALTERNATE);
+    interfaces::spi1::mosi::alternateFunction(5);
+
+    interfaces::spi3::sck::mode(Mode::ALTERNATE);
+    interfaces::spi3::sck::alternateFunction(6);
+    interfaces::spi3::miso::mode(Mode::ALTERNATE);
+    interfaces::spi3::miso::alternateFunction(6);
+    interfaces::spi3::mosi::mode(Mode::ALTERNATE);
+    interfaces::spi3::mosi::alternateFunction(5);
+
+    interfaces::spi4::sck::mode(Mode::ALTERNATE);
+    interfaces::spi4::sck::alternateFunction(5);
+    interfaces::spi4::miso::mode(Mode::ALTERNATE);
+    interfaces::spi4::miso::alternateFunction(5);
+    interfaces::spi4::mosi::mode(Mode::ALTERNATE);
+    interfaces::spi4::mosi::alternateFunction(5);
+
+    // 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::rx::alternateFunction(7);
+
+    interfaces::can2::tx::mode(Mode::ALTERNATE);
+    interfaces::can2::tx::alternateFunction(9);
+    interfaces::can2::rx::mode(Mode::ALTERNATE);
+    interfaces::can2::rx::alternateFunction(9);
+
+    peripherals::leds::userLed1::mode(Mode::OUTPUT);
+    peripherals::leds::userLed2::mode(Mode::OUTPUT);
+    peripherals::leds::userLed3_1::mode(Mode::OUTPUT);
+    peripherals::leds::userLed3_2::mode(Mode::OUTPUT);
+    peripherals::leds::userLed4::mode(Mode::OUTPUT);
+
+    peripherals::switches::userSwitch1::mode(Mode::INPUT);
+
+    peripherals::lsm6dsrx::cs::mode(Mode::OUTPUT);
+    peripherals::lsm6dsrx::cs::high();
+    peripherals::lsm6dsrx::int1::mode(Mode::INPUT);
+    peripherals::lsm6dsrx::int2::mode(Mode::INPUT);
+
+    peripherals::h3lis331dl::cs::mode(Mode::OUTPUT);
+    peripherals::h3lis331dl::cs::high();
+    peripherals::h3lis331dl::int1::mode(Mode::INPUT);
+
+    peripherals::lis2mdl::cs::mode(Mode::OUTPUT);
+    peripherals::lis2mdl::cs::high();
+
+    peripherals::lps22df::cs::mode(Mode::OUTPUT);
+    peripherals::lps22df::cs::high();
+    peripherals::lps22df::int1::mode(Mode::INPUT);
+
+    peripherals::ads131m08::cs::mode(Mode::OUTPUT);
+    peripherals::ads131m08::cs::high();
+
+    peripherals::max31856::cs::mode(Mode::OUTPUT);
+    peripherals::max31856::cs::high();
+
+    peripherals::servos::servo1::mode(Mode::ALTERNATE);
+    peripherals::servos::servo1::alternateFunction(3);
+    peripherals::servos::servo2::mode(Mode::ALTERNATE);
+    peripherals::servos::servo2::alternateFunction(3);
+
+    peripherals::tank_level::lvl1::mode(Mode::INPUT);
+    peripherals::tank_level::lvl2::mode(Mode::INPUT);
+    peripherals::tank_level::lvl3::mode(Mode::INPUT);
+
+    peripherals::battery_voltage::ch15::mode(Mode::INPUT_ANALOG);
+
+    DefaultConsole::instance().IRQset(intrusive_ref_ptr<Device>(new STM32Serial(
+        defaultSerial, defaultSerialSpeed, STM32Serial::NOFLOWCTRL)));
+}
+
+void bspInit2()
+{
+#ifdef WITH_FILESYSTEM
+    basicFilesystemSetup(SDIODriver::instance());
+#endif  // WITH_FILESYSTEM
+
+#ifdef WITH_BACKUP_SRAM
+    // Print the reset reason
+    bootlog("Reset reson: ");
+    switch (SGM::instance().lastResetReason())
+    {
+        case ResetReason::RST_LOW_PWR:
+            bootlog("low power\n");
+            break;
+        case ResetReason::RST_WINDOW_WDG:
+            bootlog("window watchdog\n");
+            break;
+        case ResetReason::RST_INDEPENDENT_WDG:
+            bootlog("indeendent watchdog\n");
+            break;
+        case ResetReason::RST_SW:
+            bootlog("software reset\n");
+            break;
+        case ResetReason::RST_POWER_ON:
+            bootlog("power on\n");
+            break;
+        case ResetReason::RST_PIN:
+            bootlog("reset pin\n");
+            break;
+        case ResetReason::RST_UNKNOWN:
+            bootlog("unknown\n");
+            break;
+    }
+#endif  // WITH_BACKUP_SRAM
+}
+
+//
+// Shutdown and reboot
+//
+
+void shutdown()
+{
+    ioctl(STDOUT_FILENO, IOCTL_SYNC, 0);
+
+#ifdef WITH_FILESYSTEM
+    FilesystemManager::instance().umountAll();
+#endif  // WITH_FILESYSTEM
+
+    disableInterrupts();
+    for (;;)
+        ;
+}
+
+void reboot()
+{
+    ioctl(STDOUT_FILENO, IOCTL_SYNC, 0);
+
+#ifdef WITH_FILESYSTEM
+    FilesystemManager::instance().umountAll();
+#endif  // WITH_FILESYSTEM
+
+    disableInterrupts();
+    miosix_private::IRQsystemReboot();
+}
+
+}  // namespace miosix
diff --git a/src/bsps/stm32f767zi_gemini_motor/interfaces-impl/bsp_impl.h b/src/bsps/stm32f767zi_gemini_motor/interfaces-impl/bsp_impl.h
new file mode 100644
index 000000000..5c82116ba
--- /dev/null
+++ b/src/bsps/stm32f767zi_gemini_motor/interfaces-impl/bsp_impl.h
@@ -0,0 +1,106 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * 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.
+ */
+
+/***************************************************************************
+ * bsp_impl.h Part of the Miosix Embedded OS.
+ * Board support package, this file initializes hardware.
+ ***************************************************************************/
+
+#ifndef BSP_IMPL_H
+#define BSP_IMPL_H
+
+#include "config/miosix_settings.h"
+#include "hwmapping.h"
+#include "interfaces/gpio.h"
+
+namespace miosix
+{
+
+/**
+\addtogroup Hardware
+\{
+*/
+
+/**
+ * \internal
+ * Called by stage_1_boot.cpp to enable the SDRAM before initializing .data/.bss
+ * Requires the CPU clock to be already configured (running from the PLL)
+ */
+void configureSdram();
+
+inline void ledOn()
+{
+    peripherals::leds::userLed1::high();
+    peripherals::leds::userLed2::high();
+    peripherals::leds::userLed3_1::high();
+    peripherals::leds::userLed3_2::high();
+    peripherals::leds::userLed4::high();
+}
+
+inline void ledOff()
+{
+    peripherals::leds::userLed1::low();
+    peripherals::leds::userLed2::low();
+    peripherals::leds::userLed3_1::low();
+    peripherals::leds::userLed3_2::low();
+    peripherals::leds::userLed4::low();
+}
+
+inline void led1On() { peripherals::leds::userLed1::high(); }
+
+inline void led1Off() { peripherals::leds::userLed1::low(); }
+
+inline void led2On() { peripherals::leds::userLed2::high(); }
+
+inline void led2Off() { peripherals::leds::userLed2::low(); }
+
+inline void led3On()
+{
+    peripherals::leds::userLed3_1::high();
+    peripherals::leds::userLed3_2::high();
+}
+
+inline void led3Off()
+{
+    peripherals::leds::userLed3_1::low();
+    peripherals::leds::userLed3_2::low();
+}
+
+/**
+ * Polls the SD card sense GPIO.
+ *
+ * This board has no SD card whatsoever, but a card can be connected to the
+ * following GPIOs:
+ * TODO: never tested
+ *
+ * \return true. As there's no SD card sense switch, let's pretend that
+ * the card is present.
+ */
+inline bool sdCardSense() { return true; }
+
+/**
+\}
+*/
+
+}  // namespace miosix
+
+#endif  // BSP_IMPL_H
diff --git a/src/bsps/stm32f767zi_gemini_motor/interfaces-impl/hwmapping.h b/src/bsps/stm32f767zi_gemini_motor/interfaces-impl/hwmapping.h
new file mode 100644
index 000000000..5916f30a4
--- /dev/null
+++ b/src/bsps/stm32f767zi_gemini_motor/interfaces-impl/hwmapping.h
@@ -0,0 +1,145 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Alberto Nidasio
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include "interfaces/gpio.h"
+
+namespace miosix
+{
+
+namespace interfaces
+{
+
+namespace spi1
+{
+using sck  = Gpio<GPIOA_BASE, 5>;
+using miso = Gpio<GPIOA_BASE, 6>;
+using mosi = Gpio<GPIOA_BASE, 7>;
+}  // namespace spi1
+
+namespace spi3
+{
+using sck  = Gpio<GPIOB_BASE, 3>;
+using miso = Gpio<GPIOB_BASE, 4>;
+using mosi = Gpio<GPIOD_BASE, 6>;
+}  // namespace spi3
+
+namespace spi4
+{
+using sck  = Gpio<GPIOE_BASE, 2>;
+using miso = Gpio<GPIOE_BASE, 5>;
+using mosi = Gpio<GPIOE_BASE, 6>;
+}  // namespace spi4
+
+namespace usart1
+{
+using tx = Gpio<GPIOA_BASE, 2>;
+using rx = Gpio<GPIOA_BASE, 3>;
+}  // namespace usart1
+
+namespace usart2
+{
+using tx = Gpio<GPIOA_BASE, 9>;
+using rx = Gpio<GPIOA_BASE, 10>;
+}  // namespace usart2
+
+namespace can2
+{
+using tx = Gpio<GPIOB_BASE, 12>;
+using rx = Gpio<GPIOB_BASE, 13>;
+}  // namespace can2
+
+}  // namespace interfaces
+
+namespace peripherals
+{
+
+namespace leds
+{
+using userLed1   = Gpio<GPIOB_BASE, 7>;
+using userLed2   = Gpio<GPIOE_BASE, 3>;
+using userLed3_1 = Gpio<GPIOC_BASE, 13>;  // On MCU rev 2
+using userLed3_2 = Gpio<GPIOG_BASE, 9>;   // On MCU rev 3
+using userLed4   = Gpio<GPIOC_BASE, 2>;
+}  // namespace leds
+
+namespace switches
+{
+using userSwitch1 = Gpio<GPIOB_BASE, 2>;
+}
+
+namespace lsm6dsrx
+{
+using cs   = Gpio<GPIOC_BASE, 4>;
+using int1 = Gpio<GPIOD_BASE, 13>;
+using int2 = Gpio<GPIOG_BASE, 7>;
+}  // namespace lsm6dsrx
+
+namespace h3lis331dl
+{
+using cs   = Gpio<GPIOD_BASE, 3>;
+using int1 = Gpio<GPIOC_BASE, 3>;
+}  // namespace h3lis331dl
+
+namespace lis2mdl
+{
+using cs = Gpio<GPIOD_BASE, 5>;
+}  // namespace lis2mdl
+
+namespace lps22df
+{
+using cs   = Gpio<GPIOD_BASE, 7>;
+using int1 = Gpio<GPIOB_BASE, 11>;
+}  // namespace lps22df
+
+namespace ads131m08
+{
+using cs = Gpio<GPIOG_BASE, 10>;
+}  // namespace ads131m08
+
+namespace max31856
+{
+using cs = Gpio<GPIOD_BASE, 4>;
+}  // namespace max31856
+
+namespace servos
+{
+using servo1 = Gpio<GPIOC_BASE, 6>;  // TIM8 CH1
+using servo2 = Gpio<GPIOC_BASE, 7>;  // TIM8 CH2
+}  // namespace servos
+
+namespace tank_level
+{
+using lvl1 = Gpio<GPIOB_BASE, 1>;
+using lvl2 = Gpio<GPIOG_BASE, 6>;
+using lvl3 = Gpio<GPIOB_BASE, 14>;
+}  // namespace tank_level
+
+namespace battery_voltage
+{
+using ch15 = Gpio<GPIOC_BASE, 5>;
+}
+
+}  // namespace peripherals
+
+}  // namespace miosix
diff --git a/src/bsps/stm32f767zi_gemini_motor/stm32_2m+16m_xram.ld b/src/bsps/stm32f767zi_gemini_motor/stm32_2m+16m_xram.ld
new file mode 100644
index 000000000..5b3545abd
--- /dev/null
+++ b/src/bsps/stm32f767zi_gemini_motor/stm32_2m+16m_xram.ld
@@ -0,0 +1,190 @@
+/*
+ * C++ enabled linker script for stm32f769ni (2M FLASH, 512K RAM, 16MB XRAM)
+ * Developed by TFT: Terraneo Federico Technologies
+ * Optimized for use with the Miosix kernel
+ */
+
+/*
+ * This linker script puts:
+ * - read only data and code (.text, .rodata, .eh_*) in FLASH
+ * - the 512Byte main (IRQ) stack, .data and .bss in the DTCM 128KB RAM
+ * - .data, .bss, stacks and heap in the external 16MB SDRAM.
+ */
+
+/*
+ * The main stack is used for interrupt handling by the kernel.
+ *
+ * *** Readme ***
+ * This linker script places the main stack (used by the kernel for interrupts)
+ * at the bottom of the ram, instead of the top. This is done for two reasons:
+ *
+ * - as an optimization for microcontrollers with little ram memory. In fact
+ *   the implementation of malloc from newlib requests memory to the OS in 4KB
+ *   block (except the first block that can be smaller). This is probably done
+ *   for compatibility with OSes with an MMU and paged memory. To see why this
+ *   is bad, consider a microcontroller with 8KB of ram: when malloc finishes
+ *   up the first 4KB it will call _sbrk_r asking for a 4KB block, but this will
+ *   fail because the top part of the ram is used by the main stack. As a
+ *   result, the top part of the memory will not be used by malloc, even if
+ *   available (and it is nearly *half* the ram on an 8KB mcu). By placing the
+ *   main stack at the bottom of the ram, the upper 4KB block will be entirely
+ *   free and available as heap space.
+ *
+ * - In case of main stack overflow the cpu will fault because access to memory
+ *   before the beginning of the ram faults. Instead with the default stack
+ *   placement the main stack will silently collide with the heap.
+ * Note: if increasing the main stack size also increase the ORIGIN value in
+ * the MEMORY definitions below accordingly.
+ */
+
+_main_stack_size = 512;                             /* main stack = 512Bytes */
+_main_stack_top  = 0x20000000 + _main_stack_size;
+ASSERT(_main_stack_size   % 8 == 0, "MAIN stack size error");
+
+/* Mapping the heap into XRAM */
+_heap_end = 0xd0000000 + 16M;                        /* end of available ram */
+
+/* Identify the Entry Point  */
+ENTRY(_Z13Reset_Handlerv)
+
+/*
+ * Specify the memory areas
+ *
+ * NOTE: starting at 0x20000000 there's 128KB of DTCM (Data Tightly Coupled
+ * Memory). Technically, we could use this as normal RAM as there's a way for
+ * the DMA to access it, but the datasheet is unclear about performance
+ * penalties for doing so. To avoid nonuniform DMA memory access latencies,
+ * we leave this 128KB DTCM unused except for the first 512Bytes which are for
+ * the interrupt stack. This leaves us with 384KB of RAM
+ */
+MEMORY
+{
+    xram(wx)  : ORIGIN = 0xd0000000, LENGTH =  16M
+    sram(wx)  : ORIGIN = 0x20020000, LENGTH = 384K
+    dtcm(wx)  : ORIGIN = 0x20000000, LENGTH = 128K    /* Used for main stack */
+    bram(rw)  : ORIGIN = 0x40024000, LENGTH =   4K    /* Bakup SRAM */
+    flash(rx) : ORIGIN = 0x08000000, LENGTH =   2M
+}
+
+/* now define the output sections  */
+SECTIONS
+{
+    . = 0;
+    
+    /* .text section: code goes to flash */
+    .text :
+    {
+        /* Startup code must go at address 0 */
+        KEEP(*(.isr_vector))
+        
+        *(.text)
+        *(.text.*)
+        *(.gnu.linkonce.t.*)
+        /* these sections for thumb interwork? */
+        *(.glue_7)
+        *(.glue_7t)
+        /* these sections for C++? */
+        *(.gcc_except_table)
+        *(.gcc_except_table.*)
+        *(.ARM.extab*)
+        *(.gnu.linkonce.armextab.*)
+
+        . = ALIGN(4);
+        /* .rodata: constant data */
+        *(.rodata)
+        *(.rodata.*)
+        *(.gnu.linkonce.r.*)
+
+        /* C++ Static constructors/destructors (eabi) */
+        . = ALIGN(4);
+        KEEP(*(.init))
+        
+        . = ALIGN(4);
+        __miosix_init_array_start = .;
+        KEEP (*(SORT(.miosix_init_array.*)))
+        KEEP (*(.miosix_init_array))
+        __miosix_init_array_end = .;
+
+        . = ALIGN(4);
+        __preinit_array_start = .;
+        KEEP (*(.preinit_array))
+        __preinit_array_end = .;
+
+        . = ALIGN(4);
+        __init_array_start = .;
+        KEEP (*(SORT(.init_array.*)))
+        KEEP (*(.init_array))
+        __init_array_end = .;
+
+        . = ALIGN(4);
+        KEEP(*(.fini))
+
+        . = ALIGN(4);
+        __fini_array_start = .;
+        KEEP (*(.fini_array))
+        KEEP (*(SORT(.fini_array.*)))
+        __fini_array_end = .;
+
+        /* C++ Static constructors/destructors (elf)  */
+        . = ALIGN(4);
+        _ctor_start = .;
+        KEEP (*crtbegin.o(.ctors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .ctors))
+        KEEP (*(SORT(.ctors.*)))
+        KEEP (*crtend.o(.ctors))
+       _ctor_end = .;
+
+        . = ALIGN(4);
+        KEEP (*crtbegin.o(.dtors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .dtors))
+        KEEP (*(SORT(.dtors.*)))
+        KEEP (*crtend.o(.dtors))
+    } > flash
+
+    /* .ARM.exidx is sorted, so has to go in its own output section.  */
+    __exidx_start = .;
+    .ARM.exidx :
+    {
+        *(.ARM.exidx* .gnu.linkonce.armexidx.*)
+    } > flash
+    __exidx_end = .;
+
+	/*
+     * .data section: global variables go to xram, but also store a copy to
+     * flash to initialize them
+     */
+    .data : ALIGN(8)
+    {
+        _data = .;
+        *(.data)
+        *(.data.*)
+        *(.gnu.linkonce.d.*)
+        . = ALIGN(8);
+        _edata = .;
+    } > xram AT > flash
+    _etext = LOADADDR(.data);
+
+    /* .bss section: uninitialized global variables go to xram */
+    _bss_start = .;
+    .bss :
+    {
+        *(.bss)
+        *(.bss.*)
+        *(.gnu.linkonce.b.*)
+        . = ALIGN(8);
+    } > xram
+    _bss_end = .;
+
+    _end = .;
+    PROVIDE(end = .);
+    
+    .preserve(NOLOAD) : ALIGN(4)
+    {
+        _preserve_start = .;
+        . = ALIGN(4);
+        *(.preserve);
+        *(.preserve*);
+        . = ALIGN(4);
+        _preserve_end = .;
+    } > bram
+}
diff --git a/src/bsps/stm32f767zi_gemini_motor/stm32_2m+384k_ram.ld b/src/bsps/stm32f767zi_gemini_motor/stm32_2m+384k_ram.ld
new file mode 100644
index 000000000..497bd5198
--- /dev/null
+++ b/src/bsps/stm32f767zi_gemini_motor/stm32_2m+384k_ram.ld
@@ -0,0 +1,189 @@
+/*
+ * C++ enabled linker script for stm32f767zi (2M FLASH, 512K RAM)
+ * Developed by TFT: Terraneo Federico Technologies
+ * Optimized for use with the Miosix kernel
+ */
+
+/*
+ * This linker script puts:
+ * - read only data and code (.text, .rodata, .eh_*) in FLASH
+ * - the 512Byte main (IRQ) stack, .data and .bss in the DTCM 128KB RAM
+ * - .data, .bss, stacks and heap in the internal RAM.
+ */
+
+/*
+ * The main stack is used for interrupt handling by the kernel.
+ *
+ * *** Readme ***
+ * This linker script places the main stack (used by the kernel for interrupts)
+ * at the bottom of the ram, instead of the top. This is done for two reasons:
+ *
+ * - as an optimization for microcontrollers with little ram memory. In fact
+ *   the implementation of malloc from newlib requests memory to the OS in 4KB
+ *   block (except the first block that can be smaller). This is probably done
+ *   for compatibility with OSes with an MMU and paged memory. To see why this
+ *   is bad, consider a microcontroller with 8KB of ram: when malloc finishes
+ *   up the first 4KB it will call _sbrk_r asking for a 4KB block, but this will
+ *   fail because the top part of the ram is used by the main stack. As a
+ *   result, the top part of the memory will not be used by malloc, even if
+ *   available (and it is nearly *half* the ram on an 8KB mcu). By placing the
+ *   main stack at the bottom of the ram, the upper 4KB block will be entirely
+ *   free and available as heap space.
+ *
+ * - In case of main stack overflow the cpu will fault because access to memory
+ *   before the beginning of the ram faults. Instead with the default stack
+ *   placement the main stack will silently collide with the heap.
+ * Note: if increasing the main stack size also increase the ORIGIN value in
+ * the MEMORY definitions below accordingly.
+ */
+
+_main_stack_size = 512;                             /* main stack = 512Bytes */
+_main_stack_top  = 0x20000000 + _main_stack_size;
+ASSERT(_main_stack_size   % 8 == 0, "MAIN stack size error");
+
+/* Mapping the heap to the end of SRAM2 */
+_heap_end = 0x20000000 + 512K;                       /* end of available ram */
+
+/* Identify the Entry Point  */
+ENTRY(_Z13Reset_Handlerv)
+
+/*
+ * Specify the memory areas
+ *
+ * NOTE: starting at 0x20000000 there's 128KB of DTCM (Data Tightly Coupled
+ * Memory). Technically, we could use this as normal RAM as there's a way for
+ * the DMA to access it, but the datasheet is unclear about performance
+ * penalties for doing so. To avoid nonuniform DMA memory access latencies,
+ * we leave this 128KB DTCM unused except for the first 512Bytes which are for
+ * the interrupt stack. This leaves us with 384KB of RAM
+ */
+MEMORY
+{
+    sram(wx)  : ORIGIN = 0x20020000, LENGTH = 384K
+    dtcm(wx)  : ORIGIN = 0x20000000, LENGTH = 128K    /* Used for main stack */
+    bram(rw)  : ORIGIN = 0x40024000, LENGTH =   4K    /* Bakup SRAM */
+    flash(rx) : ORIGIN = 0x08000000, LENGTH =   2M
+}
+
+/* now define the output sections  */
+SECTIONS
+{
+    . = 0;
+    
+    /* .text section: code goes to flash */
+    .text :
+    {
+        /* Startup code must go at address 0 */
+        KEEP(*(.isr_vector))
+        
+        *(.text)
+        *(.text.*)
+        *(.gnu.linkonce.t.*)
+        /* these sections for thumb interwork? */
+        *(.glue_7)
+        *(.glue_7t)
+        /* these sections for C++? */
+        *(.gcc_except_table)
+        *(.gcc_except_table.*)
+        *(.ARM.extab*)
+        *(.gnu.linkonce.armextab.*)
+
+        . = ALIGN(4);
+        /* .rodata: constant data */
+        *(.rodata)
+        *(.rodata.*)
+        *(.gnu.linkonce.r.*)
+
+        /* C++ Static constructors/destructors (eabi) */
+        . = ALIGN(4);
+        KEEP(*(.init))
+        
+        . = ALIGN(4);
+        __miosix_init_array_start = .;
+        KEEP (*(SORT(.miosix_init_array.*)))
+        KEEP (*(.miosix_init_array))
+        __miosix_init_array_end = .;
+
+        . = ALIGN(4);
+        __preinit_array_start = .;
+        KEEP (*(.preinit_array))
+        __preinit_array_end = .;
+
+        . = ALIGN(4);
+        __init_array_start = .;
+        KEEP (*(SORT(.init_array.*)))
+        KEEP (*(.init_array))
+        __init_array_end = .;
+
+        . = ALIGN(4);
+        KEEP(*(.fini))
+
+        . = ALIGN(4);
+        __fini_array_start = .;
+        KEEP (*(.fini_array))
+        KEEP (*(SORT(.fini_array.*)))
+        __fini_array_end = .;
+
+        /* C++ Static constructors/destructors (elf)  */
+        . = ALIGN(4);
+        _ctor_start = .;
+        KEEP (*crtbegin.o(.ctors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .ctors))
+        KEEP (*(SORT(.ctors.*)))
+        KEEP (*crtend.o(.ctors))
+       _ctor_end = .;
+
+        . = ALIGN(4);
+        KEEP (*crtbegin.o(.dtors))
+        KEEP (*(EXCLUDE_FILE (*crtend.o) .dtors))
+        KEEP (*(SORT(.dtors.*)))
+        KEEP (*crtend.o(.dtors))
+    } > flash
+
+    /* .ARM.exidx is sorted, so has to go in its own output section.  */
+    __exidx_start = .;
+    .ARM.exidx :
+    {
+        *(.ARM.exidx* .gnu.linkonce.armexidx.*)
+    } > flash
+    __exidx_end = .;
+
+	/*
+     * .data section: global variables go to sram, but also store a copy to
+     * flash to initialize them
+     */
+    .data : ALIGN(8)
+    {
+        _data = .;
+        *(.data)
+        *(.data.*)
+        *(.gnu.linkonce.d.*)
+        . = ALIGN(8);
+        _edata = .;
+    } > sram AT > flash
+    _etext = LOADADDR(.data);
+
+    /* .bss section: uninitialized global variables go to sram */
+    _bss_start = .;
+    .bss :
+    {
+        *(.bss)
+        *(.bss.*)
+        *(.gnu.linkonce.b.*)
+        . = ALIGN(8);
+    } > sram
+    _bss_end = .;
+
+    _end = .;
+    PROVIDE(end = .);
+    
+    .preserve(NOLOAD) : ALIGN(4)
+    {
+        _preserve_start = .;
+        . = ALIGN(4);
+        *(.preserve);
+        *(.preserve*);
+        . = ALIGN(4);
+        _preserve_end = .;
+    } > bram
+}
diff --git a/src/entrypoints/bmx160-calibration-entry.cpp b/src/entrypoints/bmx160-calibration-entry.cpp
index 40e57dc55..2d36db1e9 100644
--- a/src/entrypoints/bmx160-calibration-entry.cpp
+++ b/src/entrypoints/bmx160-calibration-entry.cpp
@@ -78,12 +78,12 @@ constexpr const char* testHumanFriendlyDirection[]{
     "Z up", "X up", "Y up", "Z down", "X down", "Y down",
 };
 
-#if defined(_BOARD_STM32F429ZI_SKYWARD_DEATHST_X)
+#if defined(_BOARD_STM32F429ZI_DEATH_STACK_V2)
 SPIBus bus(SPI1);
 
 void __attribute__((used)) EXTI5_IRQHandlerImpl()
 {
-#elif defined(_BOARD_STM32F429ZI_SKYWARD_DEATHST_V3)
+#elif defined(_BOARD_STM32F429ZI_DEATH_STACK_V3)
 SPIBus bus(SPI4);
 
 void __attribute__((used)) EXTI3_IRQHandlerImpl()
@@ -105,11 +105,11 @@ void calibrateGyroscope();
 int main()
 {
 // Enable interrupt from BMX pin
-#if defined(_BOARD_STM32F429ZI_SKYWARD_DEATHST_X)
+#if defined(_BOARD_STM32F429ZI_DEATH_STACK_V2)
     enableExternalInterrupt(miosix::sensors::bmx160::intr::getPin().getPort(),
                             miosix::sensors::bmx160::intr::getPin().getNumber(),
                             InterruptTrigger::FALLING_EDGE);
-#elif defined(_BOARD_STM32F429ZI_SKYWARD_DEATHST_V3)
+#elif defined(_BOARD_STM32F429ZI_DEATH_STACK_V3)
     enableExternalInterrupt(miosix::sensors::bmx160::intr::getPin().getPort(),
                             miosix::sensors::bmx160::intr::getPin().getNumber(),
                             InterruptTrigger::FALLING_EDGE);
diff --git a/src/entrypoints/sx1278-serial.cpp b/src/entrypoints/sx1278-serial.cpp
index ae0e6f41a..ede273cb2 100644
--- a/src/entrypoints/sx1278-serial.cpp
+++ b/src/entrypoints/sx1278-serial.cpp
@@ -36,7 +36,7 @@ using namespace miosix;
 // Or use SBS to define it for you
 // #define SX1278_IS_LORA
 
-#if defined _BOARD_STM32F429ZI_SKYWARD_GS_V2
+#if defined _BOARD_STM32F429ZI_NOKIA
 #include "interfaces-impl/hwmapping.h"
 
 // Uncomment the following line to enable Ebyte module
@@ -64,7 +64,7 @@ using rxen = Gpio<GPIOD_BASE, 4>;
 #define SX1278_IRQ_DIO1 EXTI4_IRQHandlerImpl
 #define SX1278_IRQ_DIO3 EXTI11_IRQHandlerImpl
 
-#elif defined _BOARD_STM32F429ZI_SKYWARD_RIG
+#elif defined _BOARD_STM32F429ZI_RIG
 #include "interfaces-impl/hwmapping.h"
 
 #define SX1278_IS_EBYTE
diff --git a/src/tests/drivers/xbee/test-xbee-bidir.cpp b/src/tests/drivers/xbee/test-xbee-bidir.cpp
index 27a1af9e7..def6764fd 100644
--- a/src/tests/drivers/xbee/test-xbee-bidir.cpp
+++ b/src/tests/drivers/xbee/test-xbee-bidir.cpp
@@ -43,7 +43,7 @@
 using namespace Boardcore;
 using namespace miosix;
 
-#ifdef _BOARD_STM32F429ZI_SKYWARD_DEATHST_X
+#ifdef _BOARD_STM32F429ZI_DEATH_STACK_V2
 #include "interfaces-impl/hwmapping.h"
 using GpioMiso = miosix::interfaces::spi2::miso;
 using GpioMosi = miosix::interfaces::spi2::mosi;
@@ -74,7 +74,7 @@ using GpioUserBtn = Gpio<GPIOA_BASE, 0>;
 Xbee::Xbee* xbeeDriver = nullptr;
 Logger& logger         = Logger::getInstance();
 
-#ifdef _BOARD_STM32F429ZI_SKYWARD_DEATHST_X
+#ifdef _BOARD_STM32F429ZI_DEATH_STACK_V2
 void __attribute__((used)) EXTI10_IRQHandlerImpl()
 #else
 void __attribute__((used)) EXTI5_IRQHandlerImpl()
@@ -88,7 +88,7 @@ void __attribute__((used)) EXTI5_IRQHandlerImpl()
 
 int getUserBtnValue()
 {
-#ifdef _BOARD_STM32F429ZI_SKYWARD_DEATHST_X
+#ifdef _BOARD_STM32F429ZI_DEATH_STACK_V2
     return 0;
 #else
     return GpioUserBtn::value();
@@ -97,7 +97,7 @@ int getUserBtnValue()
 
 void configure()
 {
-#ifndef _BOARD_STM32F429ZI_SKYWARD_DEATHST_X
+#ifndef _BOARD_STM32F429ZI_DEATH_STACK_V2
     {
         FastInterruptDisableLock dLock;
 
@@ -123,7 +123,7 @@ void configure()
     GpioLedLog::low();
 #endif
 
-#ifdef _BOARD_STM32F429ZI_SKYWARD_DEATHST_X
+#ifdef _BOARD_STM32F429ZI_DEATH_STACK_V2
     enableExternalInterrupt(GPIOF_BASE, 10, InterruptTrigger::FALLING_EDGE);
 #else
     enableExternalInterrupt(GPIOE_BASE, 5, InterruptTrigger::FALLING_EDGE);
diff --git a/src/tests/radio/sx1278/fsk/test-sx1278-bidir.cpp b/src/tests/radio/sx1278/fsk/test-sx1278-bidir.cpp
index 19de226e5..1d3670053 100644
--- a/src/tests/radio/sx1278/fsk/test-sx1278-bidir.cpp
+++ b/src/tests/radio/sx1278/fsk/test-sx1278-bidir.cpp
@@ -33,7 +33,7 @@
 using namespace Boardcore;
 using namespace miosix;
 
-#if defined _BOARD_STM32F429ZI_SKYWARD_GS_V2
+#if defined _BOARD_STM32F429ZI_NOKIA
 #include "interfaces-impl/hwmapping.h"
 
 using cs   = peripherals::ra01::pc13::cs;
diff --git a/src/tests/radio/sx1278/fsk/test-sx1278-mavlink.cpp b/src/tests/radio/sx1278/fsk/test-sx1278-mavlink.cpp
index 2ea433077..794a7378c 100644
--- a/src/tests/radio/sx1278/fsk/test-sx1278-mavlink.cpp
+++ b/src/tests/radio/sx1278/fsk/test-sx1278-mavlink.cpp
@@ -53,7 +53,7 @@ constexpr uint32_t FLIGHT_TM_PERIOD     = 250;
 using Mav =
     MavlinkDriver<RADIO_PKT_LENGTH, RADIO_OUT_QUEUE_SIZE, RADIO_MAV_MSG_LENGTH>;
 
-#if defined _BOARD_STM32F429ZI_SKYWARD_GS_V2
+#if defined _BOARD_STM32F429ZI_NOKIA
 #include "interfaces-impl/hwmapping.h"
 
 using cs   = peripherals::ra01::pc13::cs;
@@ -176,7 +176,7 @@ void flightTmLoop()
 
     while (1)
     {
-        long long start = miosix::getTick();
+        long long start = miosix::getTick;
 
         {
             Lock<FastMutex> l(mutex);
diff --git a/src/tests/radio/sx1278/lora/test-sx1278-bidir.cpp b/src/tests/radio/sx1278/lora/test-sx1278-bidir.cpp
index a08f009cb..673354430 100644
--- a/src/tests/radio/sx1278/lora/test-sx1278-bidir.cpp
+++ b/src/tests/radio/sx1278/lora/test-sx1278-bidir.cpp
@@ -31,7 +31,7 @@
 using namespace Boardcore;
 using namespace miosix;
 
-#if defined _BOARD_STM32F429ZI_SKYWARD_GS_V2
+#if defined _BOARD_STM32F429ZI_NOKIA
 #include "interfaces-impl/hwmapping.h"
 
 // Uncomment the following line to enable Ebyte module
diff --git a/src/tests/radio/sx1278/lora/test-sx1278-mavlink.cpp b/src/tests/radio/sx1278/lora/test-sx1278-mavlink.cpp
index 4bf32e6ba..383980384 100644
--- a/src/tests/radio/sx1278/lora/test-sx1278-mavlink.cpp
+++ b/src/tests/radio/sx1278/lora/test-sx1278-mavlink.cpp
@@ -51,7 +51,7 @@ constexpr uint32_t STATS_TM_PERIOD      = 1000;
 using Mav =
     MavlinkDriver<RADIO_PKT_LENGTH, RADIO_OUT_QUEUE_SIZE, RADIO_MAV_MSG_LENGTH>;
 
-#if defined _BOARD_STM32F429ZI_SKYWARD_GS_V2
+#if defined _BOARD_STM32F429ZI_NOKIA
 #include "interfaces-impl/hwmapping.h"
 
 // Uncomment the following line to enable Ebyte module
diff --git a/src/tests/radio/sx1278/lora/test-sx1278-simple.cpp b/src/tests/radio/sx1278/lora/test-sx1278-simple.cpp
index 2d4bdc75b..efca424cc 100644
--- a/src/tests/radio/sx1278/lora/test-sx1278-simple.cpp
+++ b/src/tests/radio/sx1278/lora/test-sx1278-simple.cpp
@@ -27,7 +27,7 @@
 using namespace Boardcore;
 using namespace miosix;
 
-#if defined _BOARD_STM32F429ZI_SKYWARD_GS_V2
+#if defined _BOARD_STM32F429ZI_NOKIA
 #include "interfaces-impl/hwmapping.h"
 
 // Uncomment the following line to enable Ebyte module
diff --git a/src/tests/radio/sx1278/sx1278-init.h b/src/tests/radio/sx1278/sx1278-init.h
index f30494872..068d7eb28 100644
--- a/src/tests/radio/sx1278/sx1278-init.h
+++ b/src/tests/radio/sx1278/sx1278-init.h
@@ -34,7 +34,7 @@
 // Or use SBS to define it for you
 // #define SX1278_IS_LORA
 
-#if defined _BOARD_STM32F429ZI_SKYWARD_GS_V2
+#if defined _BOARD_STM32F429ZI_NOKIA
 #include "interfaces-impl/hwmapping.h"
 
 // Uncomment the following line to enable Ebyte module
@@ -62,7 +62,7 @@ using rxen = miosix::Gpio<GPIOD_BASE, 4>;
 #define SX1278_IRQ_DIO1 EXTI4_IRQHandlerImpl
 #define SX1278_IRQ_DIO3 EXTI11_IRQHandlerImpl
 
-#elif defined _BOARD_STM32F429ZI_SKYWARD_RIG
+#elif defined _BOARD_STM32F429ZI_RIG
 #include "interfaces-impl/hwmapping.h"
 
 #define SX1278_IS_EBYTE
@@ -76,8 +76,8 @@ using sck  = miosix::radio::sck;
 using miso = miosix::radio::miso;
 using mosi = miosix::radio::mosi;
 
-using txen                         = miosix::radio::txEn;
-using rxen                         = miosix::radio::rxEn;
+using txen = miosix::radio::txEn;
+using rxen = miosix::radio::rxEn;
 
 #define SX1278_SPI SPI4
 
@@ -208,11 +208,11 @@ bool initRadio()
 #elif defined SX1278_IS_SKYWARD433
     printf("[sx1278] Confuring Skyward 433 frontend...\n");
     std::unique_ptr<Boardcore::SX1278::ISX1278Frontend> frontend(
-              new Boardcore::Skyward433Frontend());
+        new Boardcore::Skyward433Frontend());
 #else
     printf("[sx1278] Confuring RA01 frontend...\n");
     std::unique_ptr<Boardcore::SX1278::ISX1278Frontend> frontend(
-         new Boardcore::RA01Frontend());
+        new Boardcore::RA01Frontend());
 #endif
 
     // Initialize actual radio driver
@@ -240,14 +240,14 @@ bool initRadio()
     Boardcore::SX1278Fsk::Error err;
 
     sx1278 = new Boardcore::SX1278Fsk(sx1278_bus, cs::getPin(), dio0::getPin(),
-                                            dio1::getPin(), dio3::getPin(),
-                                            Boardcore::SPI::ClockDivider::DIV_256,
-                                            std::move(frontend));
+                                      dio1::getPin(), dio3::getPin(),
+                                      Boardcore::SPI::ClockDivider::DIV_256,
+                                      std::move(frontend));
 
     printf("\n[sx1278] Configuring sx1278 fsk...\n");
     if ((err = sx1278->init(config)) != Boardcore::SX1278Fsk::Error::NONE)
     {
-              // FIXME: Why does clang-format put this line up here?
+        // FIXME: Why does clang-format put this line up here?
         printf("[sx1278] sx1278->init error\n");
         return false;
     }
diff --git a/src/tests/sensors/test-ubxgps-spi.cpp b/src/tests/sensors/test-ubxgps-spi.cpp
index 3fcecde01..bb60e2342 100644
--- a/src/tests/sensors/test-ubxgps-spi.cpp
+++ b/src/tests/sensors/test-ubxgps-spi.cpp
@@ -28,7 +28,7 @@ using namespace Boardcore;
 
 int main()
 {
-#ifdef _BOARD_STM32F429ZI_SKYWARD_DEATHST_X
+#ifdef _BOARD_STM32F429ZI_DEATH_STACK_V2
     SPIBus spiBus(SPI2);
     GpioPin spiCs(GPIOG_BASE, 3);
     GpioPin spiSck(GPIOB_BASE, 13);
-- 
GitLab