MCU code bugfixes