diff --git a/Firmware/CMakeLists.txt b/Firmware/CMakeLists.txt
new file mode 100644
index 0000000..af8a35a
--- /dev/null
+++ b/Firmware/CMakeLists.txt
@@ -0,0 +1,151 @@
+# STM32 Minimal CMake project for C/C++ projects
+cmake_minimum_required(VERSION 3.5)
+###############################################################################
+
+include(./gcc-arm-none-eabi.cmake)
+
+# Set project name and some defaults
+if ("${PROJECT_NAME}" STREQUAL "")
+    set(PROJECT_NAME Probe)
+endif()
+project(${PROJECT_NAME})
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+
+# Set debug mode as default
+if ("${CMAKE_BUILD_TYPE}" STREQUAL "")
+    set(CMAKE_BUILD_TYPE Debug)
+endif()
+message("Build type: ${CMAKE_BUILD_TYPE}")
+
+# Set microcontroller information
+set(MCU_FAMILY STM32L4xx)
+set(MCU_MODEL STM32L432xx)
+set(CPU_PARAMETERS
+    -mcpu=cortex-m4
+    -mthumb
+    -mfpu=fpv4-sp-d16
+    -mfloat-abi=hard)
+
+set(STARTUP_SCRIPT ${CMAKE_CURRENT_SOURCE_DIR}/startup_stm32l432xx.s)
+set(MCU_LINKER_SCRIPT ${CMAKE_CURRENT_SOURCE_DIR}/STM32L432KCUx_FLASH.ld)
+
+###############################################################################
+set(EXECUTABLE ${CMAKE_PROJECT_NAME})
+
+enable_language(C CXX ASM)
+set(CMAKE_C_STANDARD 11)
+set(CMAKE_C_STANDARD_REQUIRED ON)
+set(CMAKE_C_EXTENSIONS ON)
+set(CMAKE_CXX_STANDARD 20)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_STANDARD_INCLUDE_DIRECTORIES ${CMAKE_CXX_IMPLICIT_INCLUDE_DIRECTORIES})
+set(CMAKE_CXX_EXTENSIONS ON)
+###############################################################################
+
+set(STM32CUBEMX_INCLUDE_DIRECTORIES
+    ${CMAKE_CURRENT_SOURCE_DIR}/Core/Inc
+    ${CMAKE_CURRENT_SOURCE_DIR}/Drivers/${MCU_FAMILY}_HAL_Driver/Inc
+    ${CMAKE_CURRENT_SOURCE_DIR}/Drivers/${MCU_FAMILY}_HAL_Driver/Inc/Legacy
+    ${CMAKE_CURRENT_SOURCE_DIR}/Drivers/CMSIS/Device/ST/${MCU_FAMILY}/Include
+    ${CMAKE_CURRENT_SOURCE_DIR}/Drivers/CMSIS/Include)
+
+set(PROJECT_INCLUDE_DIRECTORIES ${CMAKE_CURRENT_SOURCE_DIR})
+
+set (TINY_USB_INCLUDE_DIRECTORIES
+    ${CMAKE_CURRENT_SOURCE_DIR}/Core/tinyusb/src
+    ${CMAKE_CURRENT_SOURCE_DIR}/Core/tinyusb/src/class/cdc
+    ${CMAKE_CURRENT_SOURCE_DIR}/Core/tinyusb/src/class/msc
+    ${CMAKE_CURRENT_SOURCE_DIR}/Core/tinyusb/src/device
+    ${CMAKE_CURRENT_SOURCE_DIR}/Core/tinyusb/hw
+    )
+
+file(GLOB_RECURSE STM32CUBEMX_SOURCES CONFIGURE_DEPENDS
+${CMAKE_CURRENT_SOURCE_DIR}/Core/Cam/*.c
+${CMAKE_CURRENT_SOURCE_DIR}/Core/Inc/*.c
+${CMAKE_CURRENT_SOURCE_DIR}/Core/Src/*.c
+${CMAKE_CURRENT_SOURCE_DIR}/Drivers/*.c)
+
+
+#set(PROJECT_DIR ${CMAKE_CURRENT_SOURCE_DIR}/Project)
+#file(GLOB_RECURSE PROJECT_SOURCES CONFIGURE_DEPENDS
+#    ${PROJECT_DIR}/*.cpp
+#    ${PROJECT_DIR}/*.c)
+
+set(PROJECT_DIR ${CMAKE_CURRENT_SOURCE_DIR}/Core/Src)
+file(GLOB_RECURSE PROJECT_SOURCES CONFIGURE_DEPENDS
+    ${PROJECT_DIR}/*.cpp
+    ${PROJECT_DIR}/*.c)
+
+set(TINY_USB_DIR ${CMAKE_CURRENT_SOURCE_DIR}/Core/tinyusb/src)
+file(GLOB_RECURSE TINY_USB_SOURCES CONFIGURE_DEPENDS
+    ${TINY_USB_DIR}/*.cpp
+    ${TINY_USB_DIR}/*.c
+    ${TINY_USB_DIR}/class/cdc/*.c
+    ${TINY_USB_DIR}/class/msc/*.c
+    ${TINY_USB_DIR}/device/*.c)
+
+add_executable(${EXECUTABLE}
+    ${STM32CUBEMX_SOURCES} 
+    ${PROJECT_SOURCES}
+    ${TINY_USB_SOURCES}
+    ${STARTUP_SCRIPT})
+
+target_compile_definitions(${EXECUTABLE} PRIVATE
+    #$<$<CONFIG:Debug>:DEBUG>
+    ${MCU_MODEL}
+    USE_HAL_DRIVER)
+
+target_include_directories(${EXECUTABLE} SYSTEM PRIVATE
+    ${STM32CUBEMX_INCLUDE_DIRECTORIES})
+
+target_include_directories(${EXECUTABLE} PRIVATE
+    ${PROJECT_INCLUDE_DIRECTORIES})
+
+target_include_directories(${EXECUTABLE} PRIVATE
+    ${TINY_USB_INCLUDE_DIRECTORIES})
+###############################################################################
+target_compile_options(${EXECUTABLE} PRIVATE
+    ${CPU_PARAMETERS}
+    -Wall
+    -Wextra
+    -Wpedantic
+    -Wshadow
+    -Wdouble-promotion
+    -Wformat=2 -Wformat-truncation
+    -Wundef
+    -fno-common
+    -Wno-unused-parameter
+    $<$<COMPILE_LANGUAGE:CXX>:
+        -Wconversion
+        -Wno-volatile
+        -Wold-style-cast
+        -Wuseless-cast
+        -Wsuggest-override>
+    $<$<CONFIG:Debug>:-Og -g3 -ggdb>
+    $<$<CONFIG:Release>:-Og -g0>)
+
+target_link_options(${EXECUTABLE} PRIVATE
+    -T${MCU_LINKER_SCRIPT}
+    ${CPU_PARAMETERS}
+    -Wl,-Map=${CMAKE_PROJECT_NAME}.map
+    $<$<VERSION_GREATER:$<C_COMPILER_VERSION>,10.3.1>:-Wl,--no-warn-rwx-segments>
+    -Wl,--start-group
+    -lc
+    -lm
+    -lstdc++
+    -Wl,--end-group
+    -Wl,--print-memory-usage)
+###############################################################################
+
+# The last command can take a couple of seconds on larger project, usefull for debugging
+add_custom_command(TARGET ${EXECUTABLE} POST_BUILD
+    COMMAND ${CMAKE_SIZE} $<TARGET_FILE:${EXECUTABLE}>
+    COMMAND ${CMAKE_OBJCOPY} -O ihex $<TARGET_FILE:${EXECUTABLE}> ${EXECUTABLE}.hex
+    COMMAND ${CMAKE_OBJCOPY} -O binary $<TARGET_FILE:${EXECUTABLE}> ${EXECUTABLE}.bin)
+
+option(DUMP_ASM "Create full assembly of final executable" OFF)
+if (${DUMP_ASM})
+    add_custom_command(TARGET ${EXECUTABLE} POST_BUILD
+        COMMAND ${CMAKE_OBJDUMP} -D -C $<TARGET_FILE:${EXECUTABLE}> > ${EXECUTABLE}.s)
+endif()
+
diff --git a/Firmware/Dockerfile b/Firmware/Dockerfile
new file mode 100644
index 0000000..0cafe73
--- /dev/null
+++ b/Firmware/Dockerfile
@@ -0,0 +1,28 @@
+FROM fedora:34
+
+RUN dnf update -y && dnf install -y \
+    git \
+    wget \
+    lbzip2 \
+    make \
+    cmake \
+    glibc-locale-source \
+    findutils \
+    clang-tools-extra
+
+RUN wget -q https://developer.arm.com/-/media/Files/downloads/gnu-rm/10.3-2021.10/gcc-arm-none-eabi-10.3-2021.10-x86_64-linux.tar.bz2 -O /tmp/gcc-arm-none-eabi-10.3.tar.bz2
+RUN mkdir -p /opt/gcc-arm-none-eabi-10.3
+RUN tar -xf /tmp/gcc-arm-none-eabi-10.3.tar.bz2 -C /opt/gcc-arm-none-eabi-10.3 --strip-components=1
+RUN ln -s /opt/gcc-arm-none-eabi-10.3/bin/* /usr/local/bin
+RUN rm -rf /tmp/*
+
+ARG UID
+ARG GID
+ARG USERNAME
+ARG GROUPNAME
+RUN groupadd --gid $GID $GROUPNAME
+RUN useradd --uid $UID --gid $GID $USERNAME
+RUN usermod --append --groups $GROUPNAME $USERNAME
+RUN usermod --shell /bin/bash $USERNAME
+
+USER $USERNAME
diff --git a/Firmware/Legacy/Makefile b/Firmware/Legacy/Makefile
new file mode 100644
index 0000000..f1ab9c3
--- /dev/null
+++ b/Firmware/Legacy/Makefile
@@ -0,0 +1,220 @@
+##########################################################################################################################
+# File automatically-generated by tool: [projectgenerator] version: [3.18.0-B7] date: [Thu Feb 23 10:07:32 CET 2023] 
+##########################################################################################################################
+
+# ------------------------------------------------
+# Generic Makefile (based on gcc)
+#
+# ChangeLog :
+#	2017-02-10 - Several enhancements + project update mode
+#   2015-07-22 - first version
+# ------------------------------------------------
+
+######################################
+# target
+######################################
+TARGET = Probe
+
+
+######################################
+# building variables
+######################################
+# debug build?
+DEBUG = 1
+# optimization
+OPT = -Og
+
+
+#######################################
+# paths
+#######################################
+# Build path
+BUILD_DIR = build
+
+######################################
+# source
+######################################
+# C sources
+C_SOURCES =  \
+Core/Src/main.c \
+Core/Src/stm32l4xx_it.c \
+Core/Src/stm32l4xx_hal_msp.c \
+Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_i2c.c \
+Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_i2c_ex.c \
+Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal.c \
+Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_rcc.c \
+Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_rcc_ex.c \
+Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_flash.c \
+Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_flash_ex.c \
+Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_flash_ramfunc.c \
+Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_gpio.c \
+Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_dma.c \
+Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_dma_ex.c \
+Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_pwr.c \
+Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_pwr_ex.c \
+Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_cortex.c \
+Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_exti.c \
+Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_spi.c \
+Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_spi_ex.c \
+Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_tim.c \
+Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_tim_ex.c \
+Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_uart.c \
+Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_uart_ex.c \
+Core/Src/system_stm32l4xx.c \
+Core/Cam/Cam.c \
+Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_pcd.c \
+Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_hal_pcd_ex.c \
+Drivers/STM32L4xx_HAL_Driver/Src/stm32l4xx_ll_usb.c \
+
+
+# ASM sources
+ASM_SOURCES =  \
+startup_stm32l432xx.s
+
+
+#######################################
+# binaries
+#######################################
+PREFIX = arm-none-eabi-
+# The gcc compiler bin path can be either defined in make command via GCC_PATH variable (> make GCC_PATH=xxx)
+# either it can be added to the PATH environment variable.
+ifdef GCC_PATH
+CC = $(GCC_PATH)/$(PREFIX)gcc
+AS = $(GCC_PATH)/$(PREFIX)gcc -x assembler-with-cpp
+CP = $(GCC_PATH)/$(PREFIX)objcopy
+SZ = $(GCC_PATH)/$(PREFIX)size
+else
+CC = $(PREFIX)gcc
+AS = $(PREFIX)gcc -x assembler-with-cpp
+CP = $(PREFIX)objcopy
+SZ = $(PREFIX)size
+endif
+HEX = $(CP) -O ihex
+BIN = $(CP) -O binary -S
+ 
+#######################################
+# CFLAGS
+#######################################
+# cpu
+CPU = -mcpu=cortex-m4
+
+# fpu
+FPU = -mfpu=fpv4-sp-d16
+
+# float-abi
+FLOAT-ABI = -mfloat-abi=hard
+
+# mcu
+MCU = $(CPU) -mthumb $(FPU) $(FLOAT-ABI)
+
+# macros for gcc
+# AS defines
+AS_DEFS = 
+
+# C defines
+C_DEFS =  \
+-DUSE_HAL_DRIVER \
+-DSTM32L432xx
+
+
+# AS includes
+AS_INCLUDES = 
+
+# C includes
+C_INCLUDES =  \
+-ICore/Inc \
+-IDrivers/STM32L4xx_HAL_Driver/Inc \
+-IDrivers/STM32L4xx_HAL_Driver/Inc/Legacy \
+-IDrivers/CMSIS/Device/ST/STM32L4xx/Include \
+-IDrivers/CMSIS/Include \
+-ICore/Cam \
+-ICore/tinyusb \
+-ICore/tinyusb/src \
+-ICore/tinyusb/src/common \
+-ICore/tinyusb/src/class \
+-ICore/tinyusb/src/device
+
+# compile gcc flags
+ASFLAGS = $(MCU) $(AS_DEFS) $(AS_INCLUDES) $(OPT) -Wall -fdata-sections -ffunction-sections -ggdb -fsingle-precision-constant -fno-strict-aliasing
+
+CFLAGS += $(MCU) $(C_DEFS) $(C_INCLUDES) $(OPT) -Wall -fdata-sections -ffunction-sections -ggdb -fsingle-precision-constant -fno-strict-aliasing
+
+ifeq ($(DEBUG), 1)
+CFLAGS += -g -gdwarf-2
+endif
+
+
+# Generate dependency information
+CFLAGS += -MMD -MP -MF"$(@:%.o=%.d)"
+
+
+#######################################
+# LDFLAGS
+#######################################
+# link script
+LDSCRIPT = STM32L432KCUx_FLASH.ld
+
+# libraries
+LIBS = -lc -lm -lnosys 
+LIBDIR = 
+LDFLAGS = $(MCU) -specs=nano.specs -T$(LDSCRIPT) $(LIBDIR) $(LIBS) -Wl,-Map=$(BUILD_DIR)/$(TARGET).map,--cref -Wl,--gc-sections
+
+# default action: build all
+all: $(BUILD_DIR)/$(TARGET).elf $(BUILD_DIR)/$(TARGET).hex $(BUILD_DIR)/$(TARGET).bin
+
+flash_build: $(BUILD_DIR)/$(TARGET).elf $(BUILD_DIR)/$(TARGET).hex $(BUILD_DIR)/$(TARGET).bin
+
+#######################################
+# build the application
+#######################################
+# list of objects
+OBJECTS = $(addprefix $(BUILD_DIR)/,$(notdir $(C_SOURCES:.c=.o)))
+vpath %.c $(sort $(dir $(C_SOURCES)))
+# list of ASM program objects
+OBJECTS += $(addprefix $(BUILD_DIR)/,$(notdir $(ASM_SOURCES:.s=.o)))
+vpath %.s $(sort $(dir $(ASM_SOURCES)))
+
+$(BUILD_DIR)/%.o: %.c Makefile | $(BUILD_DIR) 
+	$(CC) -c $(CFLAGS) -Wa,-a,-ad,-alms=$(BUILD_DIR)/$(notdir $(<:.c=.lst)) $< -o $@
+
+$(BUILD_DIR)/%.o: %.s Makefile | $(BUILD_DIR)
+	$(AS) -c $(CFLAGS) $< -o $@
+
+$(BUILD_DIR)/$(TARGET).elf: $(OBJECTS) Makefile
+	$(CC) $(OBJECTS) $(LDFLAGS) -o $@
+	$(SZ) $@
+
+$(BUILD_DIR)/%.hex: $(BUILD_DIR)/%.elf | $(BUILD_DIR)
+	$(HEX) $< $@
+	
+$(BUILD_DIR)/%.bin: $(BUILD_DIR)/%.elf | $(BUILD_DIR)
+	$(BIN) $< $@	
+	
+$(BUILD_DIR):
+	mkdir $@		
+
+#######################################
+# clean up
+#######################################
+clean:
+	-rm -fR $(BUILD_DIR)
+  
+#######################################
+# dependencies
+#######################################
+-include $(wildcard $(BUILD_DIR)/*.d)
+DEPENDENCIES := $(OBJECTS:.o=.d)
+
+-include $(shell mkdir .dep 2>/dev/null) $(wildcard .dep/*)
+-include $(DEPENDENCIES)
+
+# Flash
+flash: flash_build
+	openocd -f interface/stlink.cfg -f target/stm32l4x.cfg -c "program build/$(TARGET).bin verify reset exit 0x08000000"
+
+reset:
+	openocd -f interface/stlink.cfg -f target/stm32l4x.cfg -c init -c "reset halt"
+
+
+
+# *** EOF ***
diff --git a/Firmware/docker-compose.yml b/Firmware/docker-compose.yml
new file mode 100644
index 0000000..95925e5
--- /dev/null
+++ b/Firmware/docker-compose.yml
@@ -0,0 +1,33 @@
+version: '3'
+services:
+
+    container:
+        build:
+            context: .
+            dockerfile: ./Dockerfile
+        image: fedora-arm-embedded-dev
+        container_name: fedora-arm-embedded-dev
+        volumes:
+            - ${PWD}/:/workdir
+        working_dir: /workdir
+        hostname: fedora-arm-embedded-dev
+
+    stmbuild:
+        extends:
+            service: container
+        command: bash -lc 'make -j8'
+
+    stmrebuild:
+        extends:
+            service: container
+        command: bash -lc 'make clean && make -j8'
+
+    stmclean:
+        extends:
+            service: container
+        command: bash -lc 'make clean'
+
+    shell:
+        extends:
+            service: container
+        command: bash -l
diff --git a/Firmware/gcc-arm-none-eabi.cmake b/Firmware/gcc-arm-none-eabi.cmake
new file mode 100644
index 0000000..dd2b7ce
--- /dev/null
+++ b/Firmware/gcc-arm-none-eabi.cmake
@@ -0,0 +1,21 @@
+set(CMAKE_SYSTEM_NAME Generic)
+set(CMAKE_SYSTEM_PROCESSOR arm)
+
+set(TOOLCHAIN_PREFIX arm-none-eabi-)
+set(CMAKE_C_FLAGS_INIT
+    "-fdata-sections -ffunction-sections --specs=nano.specs -Wl,--gc-sections")
+set(CMAKE_CXX_FLAGS_INIT
+    "${CMAKE_C_FLAGS_INIT} -fno-rtti -fno-exceptions -fno-threadsafe-statics")
+
+set(CMAKE_C_COMPILER ${TOOLCHAIN_PREFIX}gcc)
+set(CMAKE_ASM_COMPILER ${CMAKE_C_COMPILER})
+set(CMAKE_CXX_COMPILER ${TOOLCHAIN_PREFIX}g++)
+set(CMAKE_OBJCOPY ${TOOLCHAIN_PREFIX}objcopy)
+set(CMAKE_SIZE ${TOOLCHAIN_PREFIX}size)
+set(CMAKE_OBJDUMP ${TOOLCHAIN_PREFIX}objdump)
+
+set(CMAKE_EXECUTABLE_SUFFIX_ASM ".elf")
+set(CMAKE_EXECUTABLE_SUFFIX_C ".elf")
+set(CMAKE_EXECUTABLE_SUFFIX_CXX ".elf")
+
+set(CMAKE_TRY_COMPILE_TARGET_TYPE STATIC_LIBRARY)