Fixed MCP23016 register write function

[kaolpr]

mcp23016_write_reg was first reading 2 registers and than writing them back with one changed. Problem is that writing operation is done instantly after reading and MCP23016 seems to need some time to process stuff:

Removing extra read operations seems to fix #111 (guess when MCP is hot it gets sluggish). However, this should be explicitly taken into account when multiple operations are executed on the MCP chip. I will make a separate issue to address that.

[augustofg]

Use sizeof(cmd_data) instead of '2' (cmd_data already declared with the correct size)

[augustofg]

I'm trying to understand why it was doing a read-modify-write operation in the first place as it seems completely unnecessary.

[kaolpr]

I'm trying to understand why it was doing a read-modify-write operation in the first place as it seems completely unnecessary.

I have no idea. I can't see any reason for it.

[augustofg]

I'm merging this now as it seems obviously correct, but from my understanding of the mcp23016 datasheet it is supposed to held the SCL line low until it finishes processing the previous byte, making the microcontroller wait for the I2C bus to be free before doing the next transmission.