Changed firmware to send OK just when command received