Fixes the registers writing

- Do a XOR at idx var from writing loop: it needs to inverted because
the DSP stores variables in the order LOW|HIGH instead of HIGH|LOW
format used by MODBUS
Check if registers requested addresses exists