Fixed several bugs in modbus code