Updated code - to be used with latest bootloader