Add OK/FAIL response, unit ID/code. More error checking and bit of a tidy up.