This functionality is tested, so removed debug. Saved 74 SRAM, 148 flash