TOF400F sensor help

Greetings,

I purchased a TOF400F distance sensor from Ali express. I had to speak with them to get documentation and example code which I have now managed to get running but it is not very accurate and give constant errors. I have emailed the supplier and await their response but I thought someone here may have had some experience and could give me some advice.

The serial output shows the distance +/- 5mm when fixed a constant distance from a solid surface this is not accurate but even worse randomly every 10-20 readings the sensor reports a distance 10 digits long and a distance in the millions of meters is clearly wrong.. I will add a serial monitor output shortly....

Here is the code 100% as is from the supplier:

/************************************************
Connection:    UNO         TOFxxxH
             5V/3.3V   ---  VIN  
               GND     ---  GND 
                2      ---  RXD 
                3      ---  TXD  
************************************************/
#include <MsTimer2.h>
#include <SoftwareSerial.h>
#include <Wire.h>

SoftwareSerial DT(3, 2); //TX--3  RX--2
/**********************************type类型定义**************************/
typedef signed char     int8;
typedef unsigned char   uint8;
typedef signed short    int16;
typedef unsigned short    uint16;
typedef signed long     int32;
typedef unsigned long   uint32;
typedef unsigned long long  uint64;
typedef signed long long  int64;
/**********************************time定时配置**************************/
uint32 timCount = 0;
void tim_isr(void)
{
  timCount++;
  timCount++;
}
uint32 tim_get_count(void)
{
  uint32 count;;
  do{
    count = timCount;
  }while(count != timCount);
  return count;
}
uint8 tim_check_timeout(uint32 start, uint32 now, uint32 invt)
{
  if((uint32)(start + invt) >= start)//未溢出
  {
    if((now >= (uint32)(start + invt)) || (now < start))
    {
      return 1;
    }
  }
  else//溢出
  {
    if((now < start) && (now >= (uint32)(start + invt)))
    {
      return 1;
    }
  }
  return 0;
}
/**********************************modbus set 配置**************************/
//modbus接收到正确数据帧回调函数
void modbus_recv_data(uint8 *p_data, uint16 len)
{
/*读命令响应  
  deviceAddr = p_data[0];//响应的设备地址
  cmd = p_data[1];      //响应的命令,读03
  byteNum = p_data[2];  //返回读数据长
  readData = p_data[3]; //读回数据H
  readData <<= 8;
  readData |= p_data[4];//读回数据H
*/
/*写命令响应  
  deviceAddr = p_data[0];//响应的设备地址
  cmd = p_data[1];      //响应的命令,写06
  regAddr = p_data[2];  //寄存器地址H
  regAddr <<= 8;
  regAddr |= p_data[3]; //寄存器地址L
  writeData = p_data[4];//写数据H
  writeData <<= 8;
  writeData |= p_data[5];//写数据L
*/
  //Serial.write(p_data, len);
}
uint8 rxRdIndex = 0;
uint8 rxWrIndex = 0;
uint32 lastRxTimestamp = 0;
uint8 framTimeInvt = 4;
#define RX_BUF_LEN  80
#define TX_BUF_LEN  80
uint8 a_rxBuff[RX_BUF_LEN];
uint8 a_tempBuff[RX_BUF_LEN];
uint8 a_txBuff[TX_BUF_LEN];
static const uint8 a_crc16mTableHi[] = {
  0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81,
  0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
  0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01,
  0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
  0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81,
  0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
  0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01,
  0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
  0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81,
  0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
  0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01,
  0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
  0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81,
  0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
  0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01,
  0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
  0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81,
  0x40
};
static const uint8 a_crc16mTableLo[] = {
  0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7, 0x05, 0xC5, 0xC4,
  0x04, 0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09,
  0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A, 0x1E, 0xDE, 0xDF, 0x1F, 0xDD,
  0x1D, 0x1C, 0xDC, 0x14, 0xD4, 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3,
  0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 0xF2, 0x32, 0x36, 0xF6, 0xF7,
  0x37, 0xF5, 0x35, 0x34, 0xF4, 0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A,
  0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, 0x28, 0xE8, 0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA, 0xEE,
  0x2E, 0x2F, 0xEF, 0x2D, 0xED, 0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26,
  0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60, 0x61, 0xA1, 0x63, 0xA3, 0xA2,
  0x62, 0x66, 0xA6, 0xA7, 0x67, 0xA5, 0x65, 0x64, 0xA4, 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F,
  0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68, 0x78, 0xB8, 0xB9, 0x79, 0xBB,
  0x7B, 0x7A, 0xBA, 0xBE, 0x7E, 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5,
  0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 0x70, 0xB0, 0x50, 0x90, 0x91,
  0x51, 0x93, 0x53, 0x52, 0x92, 0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C,
  0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98, 0x88,
  0x48, 0x49, 0x89, 0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C,
  0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, 0x43, 0x83, 0x41, 0x81, 0x80,
  0x40
};
uint16 crc16_calculate_modbus(uint8 *p_data, uint16 len)
{
  uint8 crchi = 0xff;
  uint8 crclo = 0xff;
  uint16 index;
  while (len--)
  {
    index = crclo ^ *p_data++;
    crclo = crchi ^ a_crc16mTableHi[index];
    crchi = a_crc16mTableLo[index];
  }
  return (crchi << 8 | crclo);
}
uint16 uart_read_isr(uint8 channel, uint8 *p_buff)
{
  uint16 i = 0, wrIndex = 0;
  for(wrIndex=rxWrIndex; wrIndex!=rxRdIndex; rxRdIndex=(rxRdIndex+1)%RX_BUF_LEN,i++)
  {
    p_buff[i] = a_rxBuff[rxRdIndex];
  }
  return i;
}
uint8 uart_tpm_rx_isr(uint8 channel, uint8 data)
{
  if(rxRdIndex == ((rxWrIndex + 1) % RX_BUF_LEN))
  {
    return 1;
  }
  else
  {
    a_rxBuff[rxWrIndex] = data;
    rxWrIndex = (rxWrIndex+1)%RX_BUF_LEN;
  }
  lastRxTimestamp = tim_get_count();
  return 0;
}
void uart_tpm_main(void)
{
  uint16 rxLen = 0;
  uint16 crc16 = 0;
  if(tim_check_timeout(lastRxTimestamp, tim_get_count(), framTimeInvt))
  {
    rxLen = uart_read_isr(0, a_tempBuff);
    if(rxLen > 2)
    {
      crc16 = a_tempBuff[rxLen-1];
      crc16 <<= 8;
      crc16 |= a_tempBuff[rxLen-2];
      if(crc16 == crc16_calculate_modbus(a_tempBuff, rxLen-2))
      {
          modbus_recv_data(a_tempBuff, rxLen-2);
      }
    }
    lastRxTimestamp = tim_get_count();
  }
}
uint8 uart_tpm_tx_data(uint8 channel, uint8 *p_data, uint16 len)
{
  uint16 crc16 = 0;
  if(NULL == p_data || 0 == len ||
    len + 2 > TX_BUF_LEN)
  {
    return 1;
  }
  memcpy(a_txBuff, p_data, len);
  crc16 = crc16_calculate_modbus(a_txBuff, len);
  a_txBuff[len] = crc16;    //crc16l
  a_txBuff[len+1] = crc16>>8;   //crc16h
  //Serial.write(a_txBuff, len+2);
  DT.write(a_txBuff, len+2);
  return 1;
}

/**********************************test set 测试配置**************************/
//led测试
void led_flash_main(void)
{
  static uint32 ledCount = 0;
  if(tim_check_timeout(ledCount, tim_get_count(), 50))
  {
    digitalWrite(13, !digitalRead(13));
    ledCount = tim_get_count();
  }  
}
uint8 a_testBuff[20];

//读测量距离命令***********************read distance reg****************************************************************
#define REG_RD_ADDR  0x0010//测量结果寄存器
void read_cmd_test_main(void)
{
  static uint32 timStamp = 0;
  if(tim_check_timeout(timStamp, tim_get_count(), 500))//500ms发送一次读命令,改变尺寸可以定义多久读一次从机的测距值
  {
    a_testBuff[0] = 0;//广播地址
    a_testBuff[1] = 3;//读命令码
    a_testBuff[2] = REG_RD_ADDR>>8;//regH
    a_testBuff[3] = REG_RD_ADDR;//regL
    a_testBuff[4] = 0;//numH 仅支持读单个寄存器
    a_testBuff[5] = 1;//numL
    uart_tpm_tx_data(0, a_testBuff, 6);
    timStamp = tim_get_count();
  }  
}

//写设置模块的寄存器********************************write reg*********************************************************************
#define REG_WR_ADDR  0x0005//连续输出控制寄存器
#define REG_WR_DATA  500//写入数据,0表示不连续输出,xx表示间隔xxmS从机自动输出测距值
void write_cmd_test_main(void)
{
  static uint32 timStamp = 0;
  static uint8 flag = 1;
  if(tim_check_timeout(timStamp, tim_get_count(), 500))//500ms后只发送一次写命令
  {
    if(flag)//只发送一次命令
    {
      flag = 0;
      a_testBuff[0] = 0;//广播地址
      a_testBuff[1] = 6;//写命令码
      a_testBuff[2] = REG_WR_ADDR>>8;//regH
      a_testBuff[3] = REG_WR_ADDR;//regL
      a_testBuff[4] = REG_WR_DATA>>8;//dataH
      a_testBuff[5] = REG_WR_DATA;//dataL
      uart_tpm_tx_data(0, a_testBuff, 6);
    }
    timStamp = tim_get_count();
  }  
}

//测试loop,测试写功能或者读功能**************************test loop******************************************************************
void cmd_test_main(void)
{
  //write_cmd_test_main();//发送写命令,设置测距模块是否连续输出以及连续输出的时间间隔

  read_cmd_test_main();//发送读命令,读模块的测距值
}



void setup() {
  // put your setup code here, to run once:
  pinMode(13, OUTPUT);
  Serial.begin(115200);
  DT.begin(115200);
  MsTimer2::set(2 ,tim_isr);
  MsTimer2::start();
}

void loop() {
  // put your main code here, to run repeatedly:
  uart_tpm_main();
  led_flash_main();
  cmd_test_main();

  if (DT.available() > 6)
  {
    char a = DT.read();
    if(a != 0x01)          //测距模块的首地址,若改变首地址这里要改对.默认01
      return;
    byte Buf[6];
    DT.readBytes(Buf, 6);
 /*   for (int i = 0; i < 6; i++)
    {
      if (Buf[i] < 0x10)
        Serial.print("0x0");
       else
        Serial.print("0x");
      Serial.print(Buf[i], HEX);
      Serial.print(" ");
    }*/
    Serial.println();

    if (Buf[2] == 0xFF)
    {
      Serial.print("Distance:");
      Serial.print("invalid");
    }

    else
    {
    long distance = Buf[2] * 256 + Buf[3];
    Serial.print("Distance:");
    Serial.print(distance);
    Serial.print("mm");
    }
  }

  
}

void serialEvent()
{
  //uart_tpm_rx_isr(0, (uint8)Serial.read());
  uart_tpm_rx_isr(0, (uint8)DT.read());
}

attach the datasheet to a posting.
I recommend buying these sensors

precision has its price
best regards Stefan

Have you tried changing the code to Ignore results out of device spec and those that makes no sense, and, taking several measurements and averaging the results?

I am not familiar with this specific device but I do tend to do such steps on devices I use where the library is not doing it or where the quality is poor.

T
TOF400H DATASHEET_20201027.pdf (2.3 MB)
hanks but I I prefer to try and use the sensor I have first as it is environmentally sealed. of course if it is useless then the environment doesn't matter and I will need to change the sensor for a more common one like you suggest.

https://www.aliexpress.com/item/1005002427412522.html?spm=a2g0s.9042311.0.0.5bdd4c4dJds8l7

thanks, I have not implemented this yet but it has crossed my mind to do it. I didn't want to hack the code before asking some questions because it seems to be the inefficient way to solve the problem.

Here is an example of the serial output when i have an object fixed at about 80mm:

18:40:49.994 -> Distance:81mm
18:40:50.028 -> Distance:82mm
18:40:50.132 -> Distance:82mm
18:40:50.235 -> Distance:81mm
18:40:50.337 -> Distance:82mm
18:40:50.438 -> Distance:83mm
18:40:50.541 -> Distance:21496mm
18:40:50.644 -> Distance:81mm
18:40:50.747 -> Distance:82mm
18:40:50.850 -> Distance:82mm
18:40:50.988 -> Distance:42mm
18:40:51.092 -> Distance:4294934611mm
18:40:51.161 -> Distance:82mm
18:40:51.371 -> Distance:83mm
18:40:51.510 -> Distance:85mm
18:40:51.580 -> Distance:21880mm
18:40:51.685 -> Distance:85mm
18:40:51.926 -> Distance:21496mm
18:40:51.996 -> Distance:81mm
18:40:52.136 -> Distance:21880mm
18:40:52.206 -> Distance:83mm
18:40:52.310 -> Distance:85mm
18:40:52.449 -> Distance:83mm
18:40:52.519 -> Distance:83mm
18:40:52.519 -> Distance:85mm
18:40:52.624 -> Distance:83mm
18:40:52.763 -> Distance:84mm
18:40:52.972 -> Distance:42mm
18:40:53.006 -> Distance:87mm
18:40:53.146 -> Distance:22521mm
18:40:53.285 -> Distance:87mm
18:40:53.388 -> Distance:87mm
18:40:53.458 -> Distance:172mm
18:40:53.528 -> Distance:88mm
18:40:53.598 -> Distance:89mm
18:40:53.771 -> Distance:22072mm

it seems that software-serial does not work reliable on 115200 baud.

The demo-code has CRC-checksum does the code really use the CRC-checksum?
if the received data is not valid the CRC-Sum-check should show this.

The datasheet says that there is a command to change the baudrate.

or you switch over to I2C-communication
there is a I2C-library for the VL53L1-Chip (which is the ToF-Chip of the TOF400F-module
GitHub - pololu/vl53l1x-arduino: Pololu Arduino library for VL53L1X time-of-flight distance sensor

best regards Stefan

I tried to connect TOF400F via I2C but had no success. Even the I2C scanner can't find it.
I have checked the documentation of VL53L1X about I2C. But that was pretty sobering.
Do I really have to use Modbus running on 115200Baud to switch over to I2C mode?

My idea was to use an UNO as interface between a TOF400F and a modbus master system using RS485. The distance between the TOF400F and the master is about 30m. The UNO runs the serial comm via softwareserial with 9600Baud and that works perfect. But i don't know, if a second serial connection with 115200Baud using softwareserial would work. I didn't wanted to use the default serial connection as I'm using it for debug purposes.

Is there anyone who has experience with I2C with TOF400F?

Regards Dieter

How about spending 7€ and using a seeeduino XIAO which has a second hardware serial interface and with 48 MHz CPU-frequency is surely able to even use another software-serial at 115200 baud.

The seeeduino XIAO is programmable with the Arduino-IDE after installating the XIAO-board

If you want to keep the Arduino another way could be to the XIAO as a baud--reducing adapter from 1152000 baud to 9600 baud

best regards Stefan

Thanks, just ordered 2 of them for a test. Never heard of that board before, but seems to be pretty interesting.
Just for clarification, during startup I have to send a modbus command (write single register as documented in the specs) to switch TOF400F to I2C!?
Regards Dieter

modbus is a communication-protocol independent from the used hardware-interface to transmit the data the hardware-interfaces could be RS232, RS422, or in most cases RS485.
best regards Stefan

Hi,
the compiler tells me, that SoftwareSerial only supports 20, 16 and 8Mhz CPUs. Any idea how i could change that for seeeduino xiao? The plan was to realize your proposal to use the xiao as baudrate reducer.
Regards Dieter

The Seeeduino-XIOA requires to install an additional board-defintion to the Arduino-IDE which is described here
https://wiki.seeedstudio.com/Seeed_Arduino_Boards/
After doing these steps

the first thing you have to do is adjust the board in the Arduino-IDE to Seeeduino XIAO.
For each microcontroller-type a different "core" is adjusted. As the user you choose the board and the IDE is doing all these detail adjustments.

The additional board-definition provides a modified version of SoftwareSerial that works with the Seeeduino-XIAO

This changes even which examples will appear.

Then go to file - examples - softwareSerial

There is an example two port receiving

best regards Stefan