First fix implementation - not tested

Cannot test it, yet.
Only have one serial port and no serial cable....
Merge branch 'master' into dev