Skip to main content
Peter111
Associate
August 11, 2021
Question

VL53L5CX driver vl53l5cx_init() fails

  • August 11, 2021
  • 19 replies
  • 18433 views

The Ultra Light Driver (STSW-IMG023) init function vl53l5cx_init() fails in this section :

/* Get offset NVM data and store them into the offset buffer */
 
	status |= WrMulti(&(p_dev->platform), 0x2fd8,
 
		(uint8_t*)VL53L5CX_GET_NVM_CMD, sizeof(VL53L5CX_GET_NVM_CMD));
 
	status |= _vl53l5cx_poll_for_answer(p_dev, 4, 0,
 
		VL53L5CX_UI_CMD_STATUS, 0xff, 2);

The sensor does not answer with '2' as expected by _vl53l5cx_poll_for_answer. (The senosr answers '0')

This results in a failed initialization.

How can this be fixed?

19 replies

jspiller
Associate II
June 7, 2022

Did you end up solving this issue?

tekochip
Associate
July 15, 2022

Like so many of you, I have visited this site trying to get an answer on making this sensor work. I didn’t get any usable guidance, but I figured quite a few things out, so here’s a few answers.

The reason why initialization fails at /* Get offset NVM data and store them into the offset buffer */ Is because this is where the ToF sensor is asked to start responding to commands from the firmware that was downloaded in the previous steps. I don’t know what is checked during the comment /* Check if FW correctly downloaded */, but it’s not a CRC or anything that verifies the firmware transfer. This exposes the first issue with the sensor, and that’s that it requires the firmware to be transferred in rather large, 32KB chunks. Whatever I2C driver you are using needs to handle transfers of that size. You cannot break the transfer up into smaller blocks, it must be sent as called in the API. Make sure your driver can handle that size, and in the case of Nordic, you may have to copy the FLASH transfer over to RAM, because the DMA cannot handle transfers originating in FLASH.

The second issue is that the API is written in such a way that it requires an enormous amount of stack space. Many compilers optimize their code so that the stack space inside a function is not reserved until an instruction inside the function addresses the local variable. I am using Zephyr with Nordic’s nRF52, and the code would always fail during a memcpy(), rather than at the start of the function that blew the stack. The cure here is to declare variables static so that they are not being created on the stack. In example1() this would be:

               static VL53L5CX_Configuration  Dev ;                                     /* Sensor configuration */

               static VL53L5CX_ResultsData     Results;               /* Results data from VL53L5CX */

The other examples have additional variables that you will want to make static or global, just to keep them off the stack. I presently have everything running in nRF52’s Zephyr, and also in Nordic’s SDK.

I hope this helps, and if anyone needs some details, let me know.

jspiller
Associate II
July 19, 2022

I was able to get it working by breaking up the transfer. Things to keep in mind in doing so is incrementing the register address accordingly when breaking it up, and comply with the standard I2C spec as there is an error in one of their diagrams in the datasheet.

tekochip
Associate
July 19, 2022

I stand corrected, I have also able to break the transfer into pieces.

Peter111
Peter111Author
Associate
July 19, 2022
mute Am 19.07.2022 23:45 schrieb ST Community : I stand corrected, I have also able to break the transfer into pieces. [https://community.st.com/img/userprofile/default_profile_45_v2.png?fromEmail=1]<> tekochip<> (Community Member) I stand corrected, I have also able to break the transfer into pieces. View/Answer<> or reply to this email Replying to [https://community.st.com/img/userprofile/default_profile_45_v2.png?fromEmail=1]<> Peter111<> (Community Member) asked a question. Wednesday, August 11, 2021 2:47 AM<> VL53L5CX driver vl53l5cx_init() fails The Ultra Light Driver (STSW-IMG023) init function vl53l5cx_init() fails in this section : /* Get offset NVM data and store them into the offset buffer */ status |= WrMulti(&(p_dev->platform), 0x2fd8, (uint8_t*)VL53L5CX_GET_NVM_CMD, sizeof(VL53L5CX_GET_NVM_CMD)); status |= _vl53l5cx_poll_for_answer(p_dev, 4, 0, VL53L5CX_UI_CMD_STATUS, 0xff, 2); The sensor does not answer with '2' as expected by _vl53l5cx_poll_for_answer. (The senosr answers '0') This results in a failed initialization. How can this be fixed? [https://community.st.com/img/userprofile/default_profile_45_v2.png?fromEmail=1]<> tekochip<> (Community Member) I stand corrected, I have also able to break the transfer into pieces. Tuesday, July 19, 2022 2:44 PM<> You're receiving emails when someone "Comments on my posts." To change or turn off ST Community email, log in<> as felix.hundhausen@kit.edu.st. Are notifications about this post getting annoying? Reply to this email with the word " mute ". STMicroelectronics N.V. [ST Community]
WParr.1
Visitor II
January 25, 2024

I am also getting an error here:

	/* Get offset NVM data and store them into the offset buffer */
	status |= WrMulti(&(p_dev->platform), 0x2fd8,(uint8_t*)VL53L5CX_GET_NVM_CMD, sizeof(VL53L5CX_GET_NVM_CMD));

	status |= _vl53l5cx_poll_for_answer(p_dev, 4, 0, VL53L5CX_UI_CMD_STATUS, 0xff, 2);

	status |= RdMulti(&(p_dev->platform), VL53L5CX_UI_CMD_START, p_dev->temp_buffer, VL53L5CX_NVM_DATA_SIZE)); //ErrorHere

status errors with  VL53L5CX_COMMS_ERROR thrown by the following use in "platforms.c": int32_t write_read_multi function:

			if (ioctl(fd, I2C_RDWR, &packets) < 0)
				return VL53L5CX_COMMS_ERROR;
			position += data_size;

It appears that ioctl throws an error:

 

		do {
			data_size = (count - position) > VL53L5CX_COMMS_CHUNK_SIZE ? VL53L5CX_COMMS_CHUNK_SIZE : (count - position);

			i2c_buffer[0] = (reg_address + position) >> 8;
			i2c_buffer[1] = (reg_address + position) & 0xFF;

			messages[0].addr = i2c_address >> 1;
			messages[0].flags = 0; //I2C_M_WR;
			messages[0].len = 2;
			messages[0].buf = i2c_buffer;

			messages[1].addr = i2c_address >> 1;
			messages[1].flags = I2C_M_RD;
			messages[1].len = data_size;
			messages[1].buf = pdata + position;

			packets.msgs = messages;
			packets.nmsgs = 2;

//			int ret = ioctl(fd, I2C_RDWR, &packets);
//			if (ret < 0){		//throws error here
//				return VL53L5CX_COMMS_ERROR;
//			}
			if (ioctl(fd, I2C_RDWR, &packets) < 0)
				return VL53L5CX_COMMS_ERROR;

			position += data_size;

		} while (position < count);

 

I'm struggling to find the issue, as this read function is used regularly throughout the initialisation before the error occurs.

 

How can this be fixed?

AA.16
Associate III
February 14, 2024

I have the same exact issue. It stops at the VL53L8CX_UI_CMD_STATUS with a timeout after not receiving the number 2 as answer.

 

 

I rewrote my WrMulti to deal with bigger data and break it down into chunks:

 

uint8_t WrMulti(VL53L8CX_Platform *p_platform, uint16_t RegisterAddress, uint8_t *p_values, uint32_t size) {
 // Initialize buffer for register address
 uint8_t regBuff[2] = { RegisterAddress >> 8, RegisterAddress & 0xFF };

 // Loop through the data in chunks of 255 bytes
 for (uint32_t offset = 0; offset < size; offset += 255) {
 uint8_t chunkSize = (size - offset > 255) ? 255 : (size - offset);

 // Send register address
 if (HAL_I2C_Master_Transmit(&hi2c2, addr, regBuff, 2, 5000) != HAL_OK) {
 	APP_LOG(TS_ON, 3, "[PLAT] WrMulti Send register address failed\n");
 return -1;
 }

 // Write data chunk
 if (HAL_I2C_Master_Transmit(&hi2c2, addr, p_values + offset, chunkSize, 5000) != HAL_OK) {
 	APP_LOG(TS_ON, 3, "[PLAT] WrMulti Write data chunk failed\n");
 return -1;
 }

 // Delay between chunks
 HAL_Delay(2);
 }

 return 0;
}

 This code should  write only chunks of 255 bytes at once and adjust the register perfectly. This seems to work until we reach the dreaded lines:

status |= _vl53l8cx_poll_for_answer(p_dev, 4, 0,VL53L8CX_UI_CMD_STATUS, 0xff, 2); // ### ERROR HERE!!! TIMEOUT !!!

 

The status is always 0 up until this line where it changes into a 1. looking into it its a timeout issue because we never receive the 2 as answer.

 

Please advise on how to get the 8x8 ToF to work properly.

John E KVAM
ST Employee
February 14, 2024

I think I see your issue.

The chip starts when the last byte is written. 

But your chunk size of 255 is not a sub-multiple of the larger write.

So, you end up needing one last write to complete the download.

 

AA.16
Associate III
February 15, 2024

i rewrote it to this:

 

uint8_t WrMulti(VL53L8CX_Platform *p_platform, uint16_t RegisterAddress, uint8_t *p_values, uint32_t size) {
	// Send register address
	uint8_t regBuff[2] = { RegisterAddress >> 8, RegisterAddress & 0xFF };
	if (HAL_I2C_Master_Transmit(&hi2c2, addr, regBuff, 2, 5000) != HAL_OK) {
		APP_LOG(TS_ON, 3, "[PLAT] WrMulti Send register address failed\n");
		return -1;
	}

	// Loop through the data in chunks of 255 bytes // #define chunk 1<<8
	for (uint32_t offset = 0; offset < size; offset += chunk) {
 uint8_t chunkSize = (size - offset > chunk) ? chunk : (size - offset);
 // Write data chunk
 if (HAL_I2C_Master_Transmit(&hi2c2, addr, p_values + offset, chunkSize, 5000) != HAL_OK) {
 APP_LOG(TS_ON, 3, "[PLAT] WrMulti Write data chunk failed\n");
 return -1;
 }
 }

 // If the size is not a multiple of 255, we need to write the remaining bytes
 if (size % chunk != 0) {
 // Write remaining data
 if (HAL_I2C_Master_Transmit(&hi2c2, addr, p_values + size - (size % chunk), size % chunk, 5000) != HAL_OK) {
 APP_LOG(TS_ON, 3, "[PLAT] WrMulti Write remaining data failed\n");
 return -1;
 }
 }
 return 0;
}

However the same problem occurs.

 

this passes:

VL53L8CX_GET_NVM_CMD returns status 0 -> success

 

however this does not

VL53L8CX_UI_CMD_STATUS returns status 1 -> fail

AA.16
Associate III
February 15, 2024

i wanted to be sure so i started to read the registers where the firmware should be written to.

here is what i get as output:

 

 

/* Check if firmware correctly downloaded and compare with firmware */
	/* Read the first chunk of chip data */
	uint8_t chipFirstChunk[16];
	status |= RdMulti(&(p_dev->platform), 0, chipFirstChunk, 16);
	APP_LOG(TS_ON, 3, "[PLAT] Read the first chunk of chip data. Status: %d\n", status);

	/* Compare the first chunk of firmware with the first chunk of chip data */
	bool firstChunkMatch = true;
	for (int i = 0; i < 16; i++) {
		if (VL53L8CX_FIRMWARE[i] != chipFirstChunk[i]) {
			firstChunkMatch = false;
			break;
		}
	}

	/* Print chipFirstChunk */
	APP_LOG(TS_ON, 3, "[PLAT] Chip First Chunk:\n");
	for (int i = 0; i < 16; i++) {
		APP_LOG(TS_OFF, 3, "%02X, ", chipFirstChunk[i]);
	}
	APP_LOG(TS_OFF, 3, "\n");
	/* Print VL53L8CX_FIRMWARE */
	APP_LOG(TS_ON, 3, "[PLAT] Firmware:\n");
	for (int i = 0; i < 16; i++) {
		APP_LOG(TS_OFF, 3, "%02X, ", VL53L8CX_FIRMWARE[i]);
	}
	APP_LOG(TS_OFF, 3, "\n");
	if (firstChunkMatch) {
		APP_LOG(TS_ON, 3, "[PLAT] First chunk of firmware matches with chip data.\n");
	} else {
		APP_LOG(TS_ON, 3, "[PLAT] First chunk of firmware does not match with chip data.\n");
	}

 

 

here is the output:

 

 

17:37:44.211 -> 26s958:[PLAT] Check if FW correctly downloaded status 0; temp 0
17:37:44.211 -> 26s960:[PLAT] Read the first chunk of chip data. Status: 0
17:37:44.211 -> 26s960:[PLAT] Chip First Chunk:
17:37:44.211 -> F0, 0C, 62, 01, 29, 00, 00, 00, 01, 04, 01, 01, 01, 00, 01, 40, 
17:37:44.211 -> 26s961:[PLAT] Firmware:
17:37:44.211 -> E0, 00, 03, 08, E0, 00, 0A, C8, E0, 00, 05, 08, E0, 60, 37, C8, 
17:37:44.211 -> 26s962:[PLAT] First chunk of firmware does not match with chip data.

 

 

 

is this correct?

 

 

here is my two functions that read write multiple bytes:

 

 

uint8_t WrMulti(VL53L8CX_Platform *p_platform, uint16_t RegisterAddress, uint8_t *p_values, uint32_t size) {
	// Send register address
	uint8_t regBuff[2] = { RegisterAddress >> 8, RegisterAddress & 0xFF };
	if (HAL_I2C_Master_Transmit(&hi2c2, addr, regBuff, 2, 5000) != HAL_OK) {
		APP_LOG(TS_ON, 3, "[PLAT] WrMulti Send register address failed\n");
		return -1;
	}

	// Loop through the data in chunks of 255 bytes // #define chunk 1<<8
	for (uint32_t offset = 0; offset < size; offset += chunk) {
 uint8_t chunkSize = (size - offset > chunk) ? chunk : (size - offset);
 // Write data chunk
 if (HAL_I2C_Master_Transmit(&hi2c2, addr, p_values + offset, chunkSize, 5000) != HAL_OK) {
 APP_LOG(TS_ON, 3, "[PLAT] WrMulti Write data chunk failed\n");
 return -1;
 }
 }

 // If the size is not a multiple of 255, we need to write the remaining bytes
 if (size % chunk != 0) {
 // Write remaining data
 if (HAL_I2C_Master_Transmit(&hi2c2, addr, p_values + size - (size % chunk), size % chunk, 5000) != HAL_OK) {
 APP_LOG(TS_ON, 3, "[PLAT] WrMulti Write remaining data failed\n");
 return -1;
 }
 }
 return 0;
}

 

 

uint8_t RdMulti(VL53L8CX_Platform *p_platform, uint16_t RegisterAdress, uint8_t *p_values, uint32_t size) {
	uint8_t buff[2] = { RegisterAdress >> 8, RegisterAdress & 0xFF };

 if (size>chunk) {
 	APP_LOG(TS_ON, 3, "[PLAT]RdMulti size>%d, %d \n",chunk, size);
	}

	//send which register we want to write to
	if (HAL_I2C_Master_Transmit(&hi2c2, addr, buff, 2, 5000) != HAL_OK) {
		APP_LOG(TS_ON, 3, "[PLAT] RdMulti send which register we want to write to failed \n");
		return -1;
	}

	//now read it
	if (HAL_I2C_Master_Receive(&hi2c2, addr, p_values, size, 5000) != HAL_OK) {
		APP_LOG(TS_ON, 3, "[PLAT] RdMulti now read it failed \n");
		return -2;
	}
	return 0;
}

 

i am at a loss what is causing this mismatch. can someone point it out to me?

AA.16
Associate III
February 16, 2024

ok i kept trying and now i get random new bytes when reading the first few registers after FW download. At least they are different now ...

 

i changed my write functions as follows:

 

uint8_t WrMulti(VL53L8CX_Platform *p_platform, uint16_t RegisterAddress, uint8_t *p_values, uint32_t size) {
	 return HAL_I2C_Mem_Write(&hi2c2, addr, RegisterAddress, I2C_MEMADD_SIZE_16BIT, p_values, size, 100);
}

uint8_t WrByte(VL53L8CX_Platform *p_platform, uint16_t RegisterAdress, uint8_t value) {
	uint8_t data_write[3];
	uint8_t status = 0;

	data_write[0] = (RegisterAdress >> 8) & 0xFF;
	data_write[1] = RegisterAdress & 0xFF;
	data_write[2] = value & 0xFF;
	status = HAL_I2C_Master_Transmit(&hi2c2, addr, data_write, 3, 100);

	return status;
}

 

i am still trying to check if things got written correctly by reading the first few bytes after writing. is this even a correct approach? i was thinking about making a checksum but its obviously different from the original firmware and i wanted to see if its maybe shifted by a few bytes. but no its different each time i run the init code.

 

here is how i read the bytes:

	/* Check if firmware correctly downloaded and compare with firmware */
	/* Read the first chunk of chip data */
	uint8_t chipFirstChunk[32];
	status |= RdMulti(&(p_dev->platform), 0, chipFirstChunk, 32);
	APP_LOG(TS_ON, 3, "[PLAT] Read the first chunk of chip data. Status: %d\n", status);

	/* Compare the first chunk of firmware with the first chunk of chip data */
	bool firstChunkMatch = true;
	for (int i = 0; i < 32; i++) {
		if (VL53L8CX_FIRMWARE[i] != chipFirstChunk[i]) {
			firstChunkMatch = false;
			break;
		}
	}

	/* Print chipFirstChunk */
	APP_LOG(TS_ON, 3, "[PLAT] Chip First Chunk:\n");
	for (int i = 0; i < 32; i++) {
		APP_LOG(TS_OFF, 3, "%02X, ", chipFirstChunk[i]);
	}
	APP_LOG(TS_OFF, 3, "\n");
	/* Print VL53L8CX_FIRMWARE */
	APP_LOG(TS_ON, 3, "[PLAT] Firmware:\n");
	for (int i = 0; i < 32; i++) {
		APP_LOG(TS_OFF, 3, "%02X, ", VL53L8CX_FIRMWARE[i]);
	}
	APP_LOG(TS_OFF, 3, "\n");
	if (firstChunkMatch) {
		APP_LOG(TS_ON, 3, "[PLAT] First chunk of firmware matches with chip data.\n");
	} else {
		APP_LOG(TS_ON, 3, "[PLAT] First chunk of firmware does not match with chip data.\n");
	}

 

an here are two possible outputs. i show you two because its different each time.

 

16:25:06.758 -> 27s304:[PLAT] Chip First Chunk:
16:25:06.758 -> E5, 0D, 00, 08, 20, FE, 00, 20, 13, 27, 00, 08, 00, 00, 00, 00, 00, 00, 00, 00, 30, FE, 00, 20, 00, 28, 00, 40, 20, 00, 00, 00, 
16:25:06.758 -> 27s305:[PLAT] Firmware:
16:25:06.758 -> E0, 00, 03, 08, E0, 00, 0A, C8, E0, 00, 05, 08, E0, 60, 37, C8, E0, 00, 0A, 88, E0, 00, 0A, 88, E0, 00, 0A, 88, E0, 00, 0A, 88, 
16:25:06.758 -> 27s306:[PLAT] First chunk of firmware does not match with chip data.
16:25:06.758 -> 40s718:[ToF] init return value -1




16:25:29.902 -> 63s973:[PLAT] Chip First Chunk:
16:25:29.902 -> 54, 02, FF, FF, 00, 00, 00, FA, 93, 29, 00, 08, 36, 25, 00, 08, 00, 00, 00, 01, 00, 00, 00, FA, 00, 28, 00, 40, 8E, 29, 00, 08, 
16:25:29.902 -> 63s974:[PLAT] Firmware:
16:25:29.902 -> E0, 00, 03, 08, E0, 00, 0A, C8, E0, 00, 05, 08, E0, 60, 37, C8, E0, 00, 0A, 88, E0, 00, 0A, 88, E0, 00, 0A, 88, E0, 00, 0A, 88, 
16:25:29.902 -> 63s975:[PLAT] First chunk of firmware does not match with chip data.
16:25:43.335 -> 77s388:[ToF] init return value -1

 

 

John E KVAM
ST Employee
February 19, 2024

I'm really sorry you are having such issues.

Here is what I would do...

Start with the EVAL kit and its software. get that running. 

Then place a print statement inside the I2C write function. 

Print out the I2C address, register address, length, first few bytes and the last few bytes.

then put that same print into your code. 

the output hast to be identical.

If you have shorter chunks, we still have to verify you have the exact same bytes. There is even an include in the code that tells you what the code should be. (But keep in mind the bytes might be reversed in your processor.

there is something going on here.  And almost every time, it's byte-swap, word-swap or i2C max length.

- john

AA.16
Associate III
February 21, 2024

i got it to work. for future generations here is how i could solve it.

my implementation in wrMulti looks like this and does work. meaning init returns a 0 now:

 

uint8_t WrMulti(VL53L8CX_Platform *p_platform, uint16_t RegisterAddress, uint8_t *p_values, uint32_t size) {
 uint8_t status = HAL_OK; // Initialize status as HAL_OK
 uint32_t remaining_size = size; // Calculate remaining size to write
 uint16_t current_address = RegisterAddress; // Initialize current address
 // Loop until all data is written
 while (remaining_size > 0) {
 // Calculate the current chunk size to write
 uint32_t current_chunk_size = (remaining_size > chunk_size) ? chunk_size : remaining_size;

 // Perform write operation for current chunk
 status = HAL_I2C_Mem_Write(&hi2c2, addr, current_address, I2C_MEMADD_SIZE_16BIT, p_values, current_chunk_size, 100);

 // Check for error
 if (status != HAL_OK) {
 APP_LOG(TS_ON, 3, "[WrMulti] error. status %d, regADDR %d, size %d, remaining_size %d\n", status, current_address, size, remaining_size);
 return status; // Return error status
 }

 // Update remaining size and pointer to move to the next chunk
 remaining_size -= current_chunk_size;
 current_address += current_chunk_size; // Increment register address for next chunk
 p_values += current_chunk_size;
 HAL_Delay(10);
 }

 return status; // Return status (HAL_OK if all writes were successful)
}

notice how i used tha HAL command to write 16 bit addr and had to manually recalculate the current address before each loop. 

i have chuunk size defined as:

#define chunk_size (1<<8)

the same has to be done for RdMulti. as it tries to read 420 bytes at once at some point.

 

do not try to read the registers after downloading firmware, to check if they match with the firmware, you just downlaoded like i tried. it will make init fail. i used the default firmware download code.

John E KVAM
ST Employee
February 21, 2024

I'm really sorry that took so long. I think you should be able to mark your solution as 'solution'. That way people can skip the zillion intermediate steps. Thanks for sticking with it.

Visitor II
October 6, 2025

somebody help the below i have write the rdmulti and

def wr_multi(self, addr: int, buffer: List[int], size: int) -> None:
        try:
            chunk_size = 32  # Set this to your hardware's max message size
            remaining_size = size
            current_address = addr
            position = 0
            while remaining_size > 0:
                current_chunk_size = chunk_size if remaining_size > chunk_size else remaining_size
                reg_bytes = [(current_address >> 8) & 0xFF, current_address & 0xFF]
                chunk = buffer[position:position + current_chunk_size]
                data = bytearray(reg_bytes + chunk)
                DSYS_Write(data, slave=self.i2c_address)
                if DEBUG_IO:
                    print(f"wr_multi addr=0x{current_address:04X}, chunk_size={current_chunk_size}, data={list(data)}")
                remaining_size -= current_chunk_size
                current_address += current_chunk_size
                position += current_chunk_size
                time.sleep(0.01)  # HAL_Delay(10) in ms
        except Exception as e:
            print(f"Error writing register 0x{addr:04X}: {e}")

    def rd_byte(self, addr: int) -> int:
            try:
                reg_bytes = [(addr >> 8) & 0xFF, addr & 0xFF]
                data = DSYS_Read(1, bytearray(reg_bytes), slave=self.i2c_address, delay_before_read=1)
                if data is not None and len(data) > 0:
                    b = data[0]
                    if DEBUG_IO:
                        print(f"rd_byte addr=0x{addr:04X}, byte=0x{b:02X}")
                    return b
                else:
                    raise Exception("Couldn't read any bytes")
            except Exception as e:
                print(f"Error reading register 0x{addr:04X}: {e}")
                return None

    def wr_byte(self, addr: int, value: int) -> None:
            try:
                reg_bytes = [(addr >> 8) & 0xFF, addr & 0xFF]
                data = bytearray(reg_bytes + [value])
                DSYS_Write(data, slave=self.i2c_address)
                if DEBUG_IO:
                    print(f"wr_byte addr=0x{addr:04X}, byte=0x{value:02X}")
            except Exception as e:
                print(f"Error writing register 0x{addr:04X}: {e}")
def rd_multi(self, addr: int, buffer: List[int], size: int) -> None:
            try:
                # Split 16-bit register into two bytes (big-endian: high, low)
                reg_bytes = [(addr >> 8) & 0xFF, addr & 0xFF]
                data = DSYS_Read(size, bytearray(reg_bytes), slave=self.i2c_address, delay_before_read=1)
                if data is not None:
                    buffer[:len(data)] = list(data)
                    if DEBUG_IO:
                        print(f"rd_multi addr=0x{addr:04X}, size={size}, read_size={len(data)}, buf_len= read=[", end="")
                        print_size = len(data)
                        if print_size > PRINT_SIZE_MAX:
                            print_size = PRINT_SIZE_MAX
                        for i in range(print_size):
                            if i > 0:
                                print(", ", end="")
                            print(f"{data[i]:#02x}", end="")
                        if print_size != len(data):
                            print(", ...", end="")
                        print("]")
                else:
                    raise Exception("Couldn't read any bytes")
            except Exception as e:
                print(f"Error reading register 0x{addr:04X}: {e}")
                raise