Added routine required to fix issue #5
Fixed issue #5 and some marshalling issues
New portion of unit tests. PUBLISH QoS 2 validated