Add command handler and example (Tested with STM32)