[uavcan] correctly check if header and message were put in FIFO buffer (#3536)