HardwareSerial mySerial(1);

typedef struct {
unsigned char id; //ID of TOF module
unsigned long system_time; //The time elapsed after the TOF module is powered on, unit: ms
float dis; //The distance output by the TOF module, unit: m
unsigned char dis_status; //Distance status indicator output by the TOF module
unsigned int signal_strength; //Signal strength output by the TOF module
unsigned char range_precision; //Reference value of repeated ranging accuracy output by the TOF module, valid for TOFSense-F series, unit: cm
} tof_parameter; //Data structure after decoding TOF data

/**
@brief Read data from the serial port
@param buf Stores the read data
@param len The number of bytes to read
@return The return value is the actual number of bytes read
*/
size_t readN(uint8_t *buf, size_t len);

/**
@brief Read a complete data packet
@param buf Stores the read data
@return True means success, false means failure
*/
bool recdData(tof_parameter *buf);

void setup() {
Serial.begin(115200);
mySerial.begin(921600, SERIAL_8N1, 4, 5); //RX,TX
}

void loop() {
tof_parameter tof0; //Define a structure to store the decoded data
recdData(&tof0);
// Print data through serial port
Serial.print("id:");
Serial.println(tof0.id);
Serial.print("system_time:");
Serial.println(tof0.system_time);
Serial.print("dis:");
Serial.println(tof0.dis);
Serial.print("dis_status:");
Serial.println(tof0.dis_status);
Serial.print("signal_strength:");
Serial.println(tof0.signal_strength);
Serial.print("range_precision:");
Serial.println(tof0.range_precision);
Serial.println("");

delay(1000);
}

size_t readN(uint8_t *buf, size_t len) {
size_t offset = 0, left = len;
int16_t Tineout = 1500;
uint8_t *buffer = buf;
long curr = millis();
while (left) {
if (mySerial.available()) {
buffer[offset] = mySerial.read();
offset++;
left--;
}
if (millis() - curr > Tineout) {
break;
}
}
return offset;
}

bool recdData(tof_parameter *buf) {
uint8_t rx_buf[16]; //Serial receive array
int16_t Tineout = 5000;
uint8_t ch;
bool ret = false;
uint8_t Sum;
while (mySerial.available() > 0) {
mySerial.read();
}
long timeStart = millis();

while (!ret) {
if (millis() - timeStart > Tineout) {
break;
}
if (readN(&ch, 1) == 1) {
if (ch == 0x57) {
rx_buf[0] = ch;
if (readN(&ch, 1) == 1) {
if (ch == 0x00) {
rx_buf[1] = ch;
if (readN(&rx_buf[2], 14) == 14) {
Sum = 0;
for (int i = 0; i < 15; i++)
Sum += rx_buf[i];
if (Sum == rx_buf[15]) {
buf->id = rx_buf[3]; //Get the id of the TOF module
buf->system_time = (unsigned long)(((unsigned long)rx_buf[7]) << 24 | ((unsigned long)rx_buf[6]) << 16 | ((unsigned long)rx_buf[5]) << 8 | (unsigned long)rx_buf[4]); //Get the time elapsed after the TOF module is powered on
buf->dis = ((float)(((long)(((unsigned long)rx_buf[10] << 24) | ((unsigned long)rx_buf[9] << 16) | ((unsigned long)rx_buf[8] << 8))) / 256)) / 1000.0; //Get the distance output by the TOF module
buf->dis_status = rx_buf[11]; //Get the distance status indicator output by the TOF module
buf->signal_strength = (unsigned int)(((unsigned int)rx_buf[13] << 8) | (unsigned int)rx_buf[12]); //Get the signal strength output by the TOF module
buf->range_precision = rx_buf[14]; //Get the reference value of the repeated ranging accuracy output by the TOF module
ret = true;
}
}
}
}
}
}
}
return ret;
}

HardwareSerial mySerial(1);

typedef struct {
unsigned char id; //ID of TOF module
unsigned long system_time; //The time elapsed after the TOF module is powered on, unit: ms
float dis; //The distance output by the TOF module, unit: m
unsigned char dis_status; //Distance status indicator output by the TOF module
unsigned int signal_strength; //Signal strength output by the TOF module
unsigned char range_precision; //Reference value of repeated ranging accuracy output by the TOF module, valid for TOFSense-F series, unit: cm
} tof_parameter; //Data structure after decoding TOF data

/**
@brief Read data from the serial port
@param buf Stores the read data
@param len The number of bytes to read
@return The return value is the actual number of bytes read
*/
size_t readN(uint8_t *buf, size_t len);

/**
@brief Read a complete data packet
@param buf Stores the read data
@param id Module ID
@return True means success, false means failure
*/
bool recdData(tof_parameter *buf, uint8_t id = 0);

void setup() {
Serial.begin(115200);
mySerial.begin(921600, SERIAL_8N1, 4, 5); //RX,TX
}

void loop() {
tof_parameter tof0; //Define a structure to store the decoded data
recdData(&tof0, 0);
// Print data through serial port
Serial.print("id:");
Serial.println(tof0.id);
Serial.print("system_time:");
Serial.println(tof0.system_time);
Serial.print("dis:");
Serial.println(tof0.dis);
Serial.print("dis_status:");
Serial.println(tof0.dis_status);
Serial.print("signal_strength:");
Serial.println(tof0.signal_strength);
Serial.print("range_precision:");
Serial.println(tof0.range_precision);
Serial.println("");

delay(1000);
}

size_t readN(uint8_t *buf, size_t len) {
size_t offset = 0, left = len;
int16_t Tineout = 1500;
uint8_t *buffer = buf;
long curr = millis();
while (left) {
if (mySerial.available()) {
buffer[offset] = mySerial.read();
offset++;
left--;
}
if (millis() - curr > Tineout) {
break;
}
}
return offset;
}

bool recdData(tof_parameter *buf, uint8_t id) {
uint8_t rx_buf[16]; //Serial receive array
int16_t Tineout = 5000;
uint8_t ch;
bool ret = false;
uint8_t Sum;
uint8_t cmdBuf[8] = { 0x57, 0x10, 0xFF, 0xFF, 0x00, 0xFF, 0xFF, 0x00 };
cmdBuf[4] = id;
cmdBuf[7] = 0;
for (int i = 0; i < 7; i++)
cmdBuf[7] += cmdBuf[i];
long timeStart = millis();
long timeStart1 = 0;

while (!ret) {
if (millis() - timeStart > Tineout) {
break;
}

if ((millis() - timeStart1) > 1000) {
while (mySerial.available() > 0) {
mySerial.read();
}
mySerial.write(cmdBuf, 8);
timeStart1 = millis();
}

if (readN(&ch, 1) == 1) {
if (ch == 0x57) {
rx_buf[0] = ch;
if (readN(&ch, 1) == 1) {
if (ch == 0x00) {
rx_buf[1] = ch;
if (readN(&rx_buf[2], 14) == 14) {
Sum = 0;
for (int i = 0; i < 15; i++)
Sum += rx_buf[i];
if (Sum == rx_buf[15]) {
buf->id = rx_buf[3]; //Get the id of the TOF module
buf->system_time = (unsigned long)(((unsigned long)rx_buf[7]) << 24 | ((unsigned long)rx_buf[6]) << 16 | ((unsigned long)rx_buf[5]) << 8 | (unsigned long)rx_buf[4]); //Get the time elapsed after the TOF module is powered on
buf->dis = ((float)(((long)(((unsigned long)rx_buf[10] << 24) | ((unsigned long)rx_buf[9] << 16) | ((unsigned long)rx_buf[8] << 8))) / 256)) / 1000.0; //Get the distance output by the TOF module
buf->dis_status = rx_buf[11]; //Get the distance status indicator output by the TOF module
buf->signal_strength = (unsigned int)(((unsigned int)rx_buf[13] << 8) | (unsigned int)rx_buf[12]); //Get the signal strength output by the TOF module
buf->range_precision = rx_buf[14]; //Get the reference value of the repeated ranging accuracy output by the TOF module
ret = true;
}
}
}
}
}
}
}
return ret;
}

#include <Wire.h> //I2C library

#define deviceaddress 0x08

typedef struct {
uint8_t id; //ID
uint8_t interface_mode; //Communication interface mode, 0-UART, 1-CAN, 2-I/O, 3-IIC
uint32_t uart_baudrate; //UART baud rate
uint32_t system_time; //System time
float dis; //Distance
uint16_t dis_status; //Distance status indicator
uint16_t signal_strength; //Signal strength
uint8_t range_precision; //Ranging accuracy
} tof_parameter; //Parameters output by TOFSense-F

tof_parameter tof0; //Define a structure to store the decoded data

uint8_t DATA[4] = { 0 };

uint8_t bbt[4] = { 0x00, 0xc2, 0x01, 0x00 };

void i2c_writeN(uint8_t registerAddress, uint8_t *buf, size_t len) {

Wire.beginTransmission(deviceaddress);
Wire.write(registerAddress); // MSB
Wire.write(buf, len);
Wire.endTransmission();
// delay(4);
}

int16_t i2c_readN(uint8_t registerAddress, uint8_t *buf, size_t len) {
uint8_t i = 0;
Wire.beginTransmission(deviceaddress);
Wire.write(registerAddress);
if (Wire.endTransmission(false) != 0) {
return -1;
}

Wire.requestFrom(deviceaddress, len);
delay(100);
while (Wire.available()) {
buf[i++] = Wire.read();
}
return i;
}

bool recdData() {
uint8_t pdata[48]; //Read cache array
// Serial.println(i2c_readN(0x00, iic_read_buff, 48));
if (i2c_readN(0x00, pdata, 32) == 32) {
if (i2c_readN(0x20, &pdata[32], 16) == 16) {
tof0.interface_mode = pdata[0x0c] & 0x07;
tof0.id = pdata[0x0d];
tof0.uart_baudrate = (uint32_t)(((uint32_t)pdata[0x10]) | ((uint32_t)pdata[0x11] << 8) | ((uint32_t)pdata[0x12] << 16) | ((uint32_t)pdata[0x13] << 24));
tof0.system_time = (uint32_t)(((uint32_t)pdata[0x20]) | ((uint32_t)pdata[0x21] << 8) | ((uint32_t)pdata[0x22] << 16) | ((uint32_t)pdata[0x23] << 24));
tof0.dis = (float)(((uint32_t)pdata[0x24]) | ((uint32_t)pdata[0x25] << 8) | ((uint32_t)pdata[0x26] << 16) | ((uint32_t)pdata[0x27] << 24)) / 1000;
tof0.dis_status = (uint16_t)(((uint16_t)pdata[0x28]) | ((uint16_t)pdata[0x29] << 8));
tof0.signal_strength = (uint16_t)(((uint16_t)pdata[0x2a]) | ((uint16_t)pdata[0x2ab] << 8));
tof0.range_precision = pdata[0x2c];
} else
return false;
} else
return false;
return true;
}

void setup() {
Wire.begin(5, 4); // initialise the connection
Serial.begin(115200);
// i2c_writeN(0x10, bbt, 4);
}

void loop() {
recdData();

//Print data through the serial port
Serial.print("id:");
Serial.println(tof0.id);
Serial.print("interface_mode:");
Serial.println(tof0.interface_mode);
Serial.print("uart_baudrate:");
Serial.println(tof0.uart_baudrate);
Serial.print("system_time:");
Serial.println(tof0.system_time);
Serial.print("dis:");
Serial.println(tof0.dis);
Serial.print("dis_status:");
Serial.println(tof0.dis_status);
Serial.print("signal_strength:");
Serial.println(tof0.signal_strength);
Serial.print("range_precision:");
Serial.println(tof0.range_precision);
Serial.println("");

delay(1000); //Query every 1000ms
}