Fixed a payload isse.

The header offset wasn't being considered; and the message_id was being
read for all Quality of Service levels (which is wrong).