Version 2

rewritten + some modifications
tested on my MSP430FR5728 board