Fixed bugs with control_xx messages

Tested on bench top only