Reworked MSP firmware (hopefully)