Fixing bug in int8_t I2Cdev::readWords()

 Issue #718

Author: alphatech56

RP2040/I2Cdev/I2Cdev.cpp

@@ -166,14 +166,13 @@ int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8
  * @return Number of words read (-1 indicates failure)
  */
 int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint32_t timeout) {
-    int8_t count = 0, j = 0;
+    int8_t count = 0;
     uint8_t data_buf[length*2];
 
     i2c_write_blocking(i2c_default, devAddr, &regAddr, 1, true);
     count = i2c_read_timeout_us(i2c_default, devAddr, data_buf, length*2, false, timeout * 1000);
     for(int i=0; i<length; i++){
-        data[i] = (data_buf[j] << 8) | data_buf[j+1];
-        j+2;
+        data[i] = (data_buf[i*2] << 8) | data_buf[(i*2)+1];
     }
 
     return count;