diff --git a/src/drivers/mlx90640/src/MLX90640_I2C_Driver.c b/src/drivers/mlx90640/src/MLX90640_I2C_Driver.c index 5e972c836..c3df82dde 100644 --- a/src/drivers/mlx90640/src/MLX90640_I2C_Driver.c +++ b/src/drivers/mlx90640/src/MLX90640_I2C_Driver.c @@ -35,16 +35,23 @@ int MLX90640_I2CGeneralReset() int MLX90640_I2CRead(uint8_t slaveAddr, uint16_t startAddress, uint16_t nMemAddressRead, uint16_t *data) { - startAddress = __REVSH(startAddress); + for (uint16_t n = nMemAddressRead, *d = data; n; ) { + uint16_t write_address = __REVSH(startAddress); + uint16_t read_size = n > OMV_I2C_MAX_16BIT_XFER ? OMV_I2C_MAX_16BIT_XFER : n; - if (omv_i2c_write_bytes(bus, (slaveAddr<<1), (uint8_t *) &startAddress, 2, OMV_I2C_XFER_NO_STOP) != 0) { - return -1; - } + if (omv_i2c_write_bytes(bus, (slaveAddr<<1), (uint8_t *) &write_address, 2, OMV_I2C_XFER_NO_STOP) != 0) { + return -1; + } - if (omv_i2c_read_bytes(bus, (slaveAddr<<1), (uint8_t *) data, nMemAddressRead*2, OMV_I2C_XFER_NO_FLAGS) != 0) { - return -1; - } + if (omv_i2c_read_bytes(bus, (slaveAddr<<1), (uint8_t *) d, read_size*2, OMV_I2C_XFER_NO_FLAGS) != 0) { + return -1; + } + + startAddress += read_size; + n -= read_size; + d += read_size; + } for(int i=0; i OMV_I2C_MAX_16BIT_XFER ? OMV_I2C_MAX_16BIT_XFER : n; - if (omv_i2c_write_bytes(bus, (slaveAddr<<1), (uint8_t *) &startAddress, 2, OMV_I2C_XFER_NO_STOP) != 0) { - return -1; - } + if (omv_i2c_write_bytes(bus, (slaveAddr<<1), (uint8_t *) &write_address, 2, OMV_I2C_XFER_NO_STOP) != 0) { + return -1; + } - if (omv_i2c_read_bytes(bus, (slaveAddr<<1), (uint8_t *) data, nMemAddressRead*2, OMV_I2C_XFER_NO_FLAGS) != 0) { - return -1; - } + if (omv_i2c_read_bytes(bus, (slaveAddr<<1), (uint8_t *) d, read_size*2, OMV_I2C_XFER_NO_FLAGS) != 0) { + return -1; + } + + startAddress += read_size; + n -= read_size; + d += read_size; + } for(int i=0; i