Add firmware test

M998 start firmware test. Use baudrate 115200 and arduino serial monitor