Issue #660 - Firmware part done. I need to check the ASM because when

Ready for merge, my comment about flash usage is wrong (bad compiler version)