Temporary commit of firmware
Started port of firmware to STM32