Skip to main content

C Language

The ranging results of TOFSense and TOFSense-F series modules can be represented by the following structure.

typedef struct {
  uint8_t id;               // ID
  uint32_t system_time;     // System time
  float dis;                // Distance
  uint8_t dis_status;       // Status for distance
  uint16_t signal_strength; // Signal strength
  uint8_t range_precision;  // Only tofsense-f/fP
} nts_frame0_result_t;

The ranging results of the TOFSense-M series modules can be represented by the following structure.

typedef struct {
  float dis;                // Distance
  uint8_t dis_status;       // Status for distance
  uint16_t signal_strength; // Signal strength
} ntsm_frame0_pixel_t;

typedef struct {
  uint8_t id;               // ID
  uint32_t system_time;     // System time
  uint8_t pixel_count;      // Number of pixels
  ntsm_frame0_pixel_t pixels[64];
} ntsm_frame0_result_t;

Built on NLink, pure C language version, including sample parsing code of each frame protocol. For TOFSense, you only need to add the following files to the project, and select the corresponding parsing files according to different series.

Usage Examples

TOFSense/S/F/FP/F2/F2 Mini/F2 P/F2 PH：

#include "nlink_tofsense_frame0.h"    // TOFSense/S/F/FP/F2/F2 Mini/F2 P/F2 PH

int main(void)
{
  const uint8_t rx_buf[16] = {0x57, 0x00, 0xff, 0x00, 0x9e, 0x8f, 0x00, 0x00, 0xad, 0x08, 0x00, 0x00, 0x03, 0x00, 0x06, 0x41};  // Simulate receiving data: one frame of complete data is 16 bytes

  data_length = sizeof(rx_buf)/sizeof(rx_buf[0]); // Calculate the length of received data

  if (g_nts_frame0.UnpackData(rx_buf, data_length)) // Call the parsing function and pass in data TOFSense/S/F/FP/F2/F2 Mini/F2 P/F2 PH
  {
    // Parsing success
    printf("TOFSense Frame0 data unpack successfully:\r\n");  
    printf("id:%d, distance:%f\r\n", g_nts_frame0.result.id,
             g_nts_frame0.result.dis);
  }
}

TOFSense-M/MS：

#include "nlink_tofsense_m_frame0.h"  // TOFSense-M/MS

int main(void)
{
  const uint8_t rx_buf[400] = {0x57,0x01,0xff,0x00,0x17,0x56,0x00,0x00,0x40,0x1d,0x00,0x00,0x05,0xa1,0x84,0x20,0x00,0x00,0x05,0xcf,0x69,0x20,0x00,
0x00,0x05,0x9e,0x75,0x21,0x00,0x00,0x05,0x99,0x73,0x20,0x00,0x00,0x05,0xb0,0x72,0x1f,0x00,0x00,0x05,0x73,0x51,0x1f,
0x00,0x00,0x05,0x7c,0xc3,0x1e,0x00,0x00,0xff,0x57,0x40,0x21,0x00,0x00,0x05,0x9a,0xd5,0x22,0x00,0x00,0x05,0x71,0xdc,
0x26,0x00,0x00,0x05,0xee,0xe7,0x22,0x00,0x00,0x05,0xd6,0xde,0x23,0x00,0x00,0x05,0x6a,0xef,0x22,0x00,0x00,0x05,0xf8,
0xe5,0x22,0x00,0x00,0x05,0x84,0x9b,0x20,0x00,0x00,0xff,0x23,0x30,0x27,0x00,0x00,0x05,0x74,0x5b,0x29,0x00,0x00,0x05,
0xb6,0x70,0x27,0x00,0x00,0x05,0xe2,0x5c,0x27,0x00,0x00,0x05,0x04,0x4e,0x27,0x00,0x00,0x05,0xec,0x4c,0x24,0x00,0x00,
0x05,0x57,0x4d,0x23,0x00,0x00,0xff,0xf0,0x3a,0x21,0x00,0x00,0xff,0xff,0x20,0x2b,0x00,0x00,0xff,0x3c,0x25,0x29,0x00,
0x00,0xff,0x6f,0x2e,0x29,0x00,0x00,0xff,0xde,0x26,0x28,0x00,0x00,0xff,0xa7,0x22,0x26,0x00,0x00,0xff,0x57,0x23,0x23,
0x00,0x00,0xff,0x91,0x27,0x23,0x00,0x00,0xff,0x05,0x23,0x21,0x00,0x00,0xff,0x34,0x15,0x27,0x00,0x00,0xff,0x61,0x13,
0x27,0x00,0x00,0xff,0xeb,0x18,0x24,0x00,0x00,0xff,0x8a,0x18,0x24,0x00,0x00,0xff,0x86,0x19,0x22,0x00,0x00,0xff,0x13,
0x1c,0x22,0x00,0x00,0xff,0xa1,0x1e,0x21,0x00,0x00,0xff,0x3b,0x18,0x1f,0x00,0x00,0xff,0x47,0x0c,0x23,0x00,0x00,0xff,
0xff,0x0b,0x22,0x00,0x00,0xff,0x35,0x0e,0x20,0x00,0x00,0xff,0x8b,0x10,0x20,0x00,0x00,0xff,0x80,0x11,0x20,0x00,0x00,
0xff,0xe8,0x11,0x20,0x00,0x00,0xff,0x22,0x10,0x1e,0x00,0x00,0xff,0x7d,0x0b,0x1e,0x00,0x00,0xff,0x0b,0x0b,0x1e,0x00,
0x00,0xff,0x4a,0x09,0x1e,0x00,0x00,0xff,0xb7,0x0a,0x1f,0x00,0x00,0xff,0x2d,0x0d,0x1e,0x00,0x00,0xff,0x1c,0x0f,0x1d,
0x00,0x00,0xff,0x06,0x10,0x1e,0x00,0x00,0xff,0x21,0x0d,0x1e,0x00,0x00,0xff,0x89,0x0b,0x17,0x00,0x00,0xff,0x4b,0x17,
0x1c,0x00,0x00,0xff,0x9d,0x09,0x1c,0x00,0x00,0xff,0x43,0x0d,0x1d,0x00,0x00,0xff,0xd5,0x0c,0x1b,0x00,0x00,0xff,0xaa,
0x0e,0x1c,0x00,0x00,0xff,0x29,0x0f,0x1a,0x00,0x00,0xff,0x57,0x0d,0x18,0x00,0x00,0xff,0xd1,0x0f,0x13,0x00,0x00,0x05,
0xa4,0x5c,0xff,0xff,0xff,0xff,0xff,0xff,0x05};  // Simulate receiving data: 400 bytes of complete data in one frame in 8*8 mode

  data_length = sizeof(rx_buf)/sizeof(rx_buf[0]); // Calculate the length of received data

  if (g_ntsm_frame0.UnpackData(rx_buf, data_length)) // Call the analysis function and pass in data TOFSense-M/MS
  {
    // Parsing success
    for (int i = 0; i < g_ntsm_frame0.pixel_count; ++i) {
      ntsm_frame0_pixel_t *pixel = &g_ntsm_frame0.pixels[i];
      printf("pixel %d: dis:%f, dis_status:%d, signal_strength:%d\r\n", i,
             pixel->dis, pixel->dis_status, pixel->signal_strength);
    }
  }
}

UART Mode Query Command Example​

uint8_t readframe0[8];
void get_tof_readFrame0(uint8_t id, uint8_t *data)
{
    if (!data) return;
    data[0] = 0x57;
    data[1] = 0x10;
    data[2] = 0xff;
    data[3] = 0xff;
    data[4] = id;
    data[5] = 0xff;
    data[6] = 0xff;
    data[7] = 0x00;
    for (int i = 0; i < 7; i++)
    {
        data[7] += data[i];
    }
}

void tof_uart_send_read_frame0(uint8_t id)
{
    get_tof_readFrame0(id, readframe0);
    // Send function. Here we take the HAL library as an example.
    HAL_UART_Transmit_DMA(&TOF_UART_HANDLE, readframe0, 8);
}

CAN Mode Query Command Example​

CAN_TxHeaderTypeDef TxHeader;
uint8_t can_tx_buf[8];

void tof_can_send_read_frame0(uint8_t id)
{
    TxHeader.StdId=0x402;	//Query the standard frame ID
    TxHeader.ExtId=0;
    TxHeader.IDE=CAN_ID_STD;
    TxHeader.RTR=CAN_RTR_DATA;
    TxHeader.DLC=8;
    
    can_tx_buf[0] = 0xFF;
    can_tx_buf[1] = 0xFF;
    can_tx_buf[2] = 0xFF;
    can_tx_buf[3] = id;
    can_tx_buf[4] = 0xFF;
    can_tx_buf[5] = 0xFF;
    can_tx_buf[6] = 0xFF;
    can_tx_buf[7] = 0xFF;
    // Send function. Here we take the HAL library as an example.
    HAL_CAN_AddTxMessage(&TOF_CANHANDLE, &TxHeader, can_tx_buf, (uint32_t*)CAN_TX_MAILBOX0);
}

IIC​

//To be updated

Raw data analysis​

Users can also parse data by directly shifting fixed bytes.

#define TOFSENSE_DATA_LENGTH 16

nts_frame0_result_t my_tof;

int main(void)
{
    const uint8_t u_rx_buf[16] = {0x57, 0x00, 0xff, 0x00, 0x9e, 0x8f, 0x00, 0x00, 0xad, 0x08, 0x00, 0x00, 0x03, 0x00, 0x06, 0x41};  // Simulate receiving data: one frame of complete data is 16 bytes
    uint8_t checksum = 0;
    if (u_rx_buf[0] == 0x57 && u_rx_buf[1] == 0x00) // Verification frame header
    {
        for (int i = 0; i < TOFSENSE_M_DATA_LENGTH - 1; i++)    //Checksum
        {
            checksum += u_rx_buf[i];
        }
        if (checksum == u_rx_buf[TOFSENSE_M_DATA_LENGTH - 1])
        {
            // ID
            my_tof.id = u_rx_buf[3];
            // System_time
            my_tof.system_time = (uint32_t)(u_rx_buf[4] | (u_rx_buf[5] << 8) | (u_rx_buf[6] << 16) | (u_rx_buf[7] << 24));
            // dis
            my_tof.dis = ((int32_t)(u_rx_buf[8] << 8 | u_rx_buf[9] << 16 | u_rx_buf[10] << 24) / 256) / 1000.0f;
            // Status
            my_tof.dis_status = u_rx_buf[11];
            // Signal Strength
            my_tof.signal_Strength = u_rx_buf[12] | (u_rx_buf[13] << 8);
            
            printf("dis : %0.2f dis_status : %d signal_Strength : %d \r\n", my_tof.dis, my_tof.dis_status, my_tof.signal_Strength);
        }
    }
}

TOFSense-M/MS：

#define TOFSENSE_M_DATA_LENGTH 400

ntsm_frame0_result_t my_tof;

int main(void)
{
      const uint8_t rx_buf[400] = {0x57,0x01,0xff,0x00,0x17,0x56,0x00,0x00,0x40,0x1d,0x00,0x00,0x05,0xa1,0x84,0x20,0x00,0x00,0x05,0xcf,0x69,0x20,0x00,
0x00,0x05,0x9e,0x75,0x21,0x00,0x00,0x05,0x99,0x73,0x20,0x00,0x00,0x05,0xb0,0x72,0x1f,0x00,0x00,0x05,0x73,0x51,0x1f,
0x00,0x00,0x05,0x7c,0xc3,0x1e,0x00,0x00,0xff,0x57,0x40,0x21,0x00,0x00,0x05,0x9a,0xd5,0x22,0x00,0x00,0x05,0x71,0xdc,
0x26,0x00,0x00,0x05,0xee,0xe7,0x22,0x00,0x00,0x05,0xd6,0xde,0x23,0x00,0x00,0x05,0x6a,0xef,0x22,0x00,0x00,0x05,0xf8,
0xe5,0x22,0x00,0x00,0x05,0x84,0x9b,0x20,0x00,0x00,0xff,0x23,0x30,0x27,0x00,0x00,0x05,0x74,0x5b,0x29,0x00,0x00,0x05,
0xb6,0x70,0x27,0x00,0x00,0x05,0xe2,0x5c,0x27,0x00,0x00,0x05,0x04,0x4e,0x27,0x00,0x00,0x05,0xec,0x4c,0x24,0x00,0x00,
0x05,0x57,0x4d,0x23,0x00,0x00,0xff,0xf0,0x3a,0x21,0x00,0x00,0xff,0xff,0x20,0x2b,0x00,0x00,0xff,0x3c,0x25,0x29,0x00,
0x00,0xff,0x6f,0x2e,0x29,0x00,0x00,0xff,0xde,0x26,0x28,0x00,0x00,0xff,0xa7,0x22,0x26,0x00,0x00,0xff,0x57,0x23,0x23,
0x00,0x00,0xff,0x91,0x27,0x23,0x00,0x00,0xff,0x05,0x23,0x21,0x00,0x00,0xff,0x34,0x15,0x27,0x00,0x00,0xff,0x61,0x13,
0x27,0x00,0x00,0xff,0xeb,0x18,0x24,0x00,0x00,0xff,0x8a,0x18,0x24,0x00,0x00,0xff,0x86,0x19,0x22,0x00,0x00,0xff,0x13,
0x1c,0x22,0x00,0x00,0xff,0xa1,0x1e,0x21,0x00,0x00,0xff,0x3b,0x18,0x1f,0x00,0x00,0xff,0x47,0x0c,0x23,0x00,0x00,0xff,
0xff,0x0b,0x22,0x00,0x00,0xff,0x35,0x0e,0x20,0x00,0x00,0xff,0x8b,0x10,0x20,0x00,0x00,0xff,0x80,0x11,0x20,0x00,0x00,
0xff,0xe8,0x11,0x20,0x00,0x00,0xff,0x22,0x10,0x1e,0x00,0x00,0xff,0x7d,0x0b,0x1e,0x00,0x00,0xff,0x0b,0x0b,0x1e,0x00,
0x00,0xff,0x4a,0x09,0x1e,0x00,0x00,0xff,0xb7,0x0a,0x1f,0x00,0x00,0xff,0x2d,0x0d,0x1e,0x00,0x00,0xff,0x1c,0x0f,0x1d,
0x00,0x00,0xff,0x06,0x10,0x1e,0x00,0x00,0xff,0x21,0x0d,0x1e,0x00,0x00,0xff,0x89,0x0b,0x17,0x00,0x00,0xff,0x4b,0x17,
0x1c,0x00,0x00,0xff,0x9d,0x09,0x1c,0x00,0x00,0xff,0x43,0x0d,0x1d,0x00,0x00,0xff,0xd5,0x0c,0x1b,0x00,0x00,0xff,0xaa,
0x0e,0x1c,0x00,0x00,0xff,0x29,0x0f,0x1a,0x00,0x00,0xff,0x57,0x0d,0x18,0x00,0x00,0xff,0xd1,0x0f,0x13,0x00,0x00,0x05,
0xa4,0x5c,0xff,0xff,0xff,0xff,0xff,0xff,0x05};  // Simulate receiving data: 400 bytes of complete data in one frame in 8*8 mode
    uint8_t checksum = 0;
    if (u_rx_buf[0] == 0x57 && u_rx_buf[1] == 0x01) // Verification frame header
    {
        for (int i = 0; i < TOFSENSE_M_DATA_LENGTH - 1; i++)    // Checksum
        {
            checksum += u_rx_buf[i];
        }
        if (checksum == u_rx_buf[TOFSENSE_M_DATA_LENGTH - 1])
        {
            // ID
            my_tof.id = u_rx_buf[3];

            // System_time
            my_tof.system_time = (uint32_t)(u_rx_buf[4] | (u_rx_buf[5] << 8) | (u_rx_buf[6] << 16) | (u_rx_buf[7] << 24));

            // Zone map
            my_tof.zone_map = u_rx_buf[8];

            for (int i = 0; i < my_tof.zone_map; i++)
            {
                my_tof.pixel[i].dis = ((int32_t)(u_rx_buf[9 + 6 * i] << 8 | u_rx_buf[10 + 6 * i] << 16 | u_rx_buf[11 + 6 * i] << 24) / 256) / 1000.0f / 1000.0f;
                my_tof.pixel[i].dis_status = u_rx_buf[12 + 6 * i];
                my_tof.pixel[i].signal_Strength = u_rx_buf[13 + 6 * i] | (u_rx_buf[14 + 6 * i] << 8);
            }
            
            // Print data in 8*8 format
            printf("dis ： \r\n");
            for (int j = 0; j < 8; j++)
            {
                for (int h = 0; h < 8; h++)
                {
                    printf("%0.2f  ", my_tof.pixel[j * 8 + h].dis);
                }
                printf("\r\n");
            }
        }
    }
}

Python

_TOFSense_Frame_Header = b"\x57\x00"
_TOFSense_M_Frame_Header = b"\x57\x01"

def verify_checksum(data):
    """Checksum"""
    checksum = sum(data[:-1]) & 0xFF
    return checksum == data[-1]

def check_header(data, header):
    """Verification frame header"""
    return data[0] == header[0] and data[1] == header[1]

def nByteUnpack(data, index, byte_num, signed=False):
    return (
        int.from_bytes(data[index : index + byte_num], byteorder="little", signed=signed),
        index + byte_num,
    )

def TOFSenseFrame0Unpack(data):
    if not check_header(data, _TOFSense_Frame_Header) or not verify_checksum(data):
        return None
    index = 3
    parsed_data = {}
    parsed_data["id"], index = nByteUnpack(data, index, 1)
    parsed_data["system_time"], index = nByteUnpack(data, index, 4)
    dis, index = nByteUnpack(data, index, 3)
    parsed_data["dis"] = dis / 1000.0
    parsed_data["dis_status"], index = nByteUnpack(data, index, 1)
    parsed_data["signal_strength"], index = nByteUnpack(data, index, 2)
    return parsed_data

def TOFSenseMFrame0Unpack(data):
    if not check_header(data, _TOFSense_M_Frame_Header) or not verify_checksum(data):
        return None
    index = 3
    parsed_data = {}
    parsed_data["id"], index = nByteUnpack(data, index, 1)
    parsed_data["system_time"], index = nByteUnpack(data, index, 4)
    parsed_data["zone_map"], index = nByteUnpack(data, index, 1)
    parsed_data["dis"] = []
    parsed_data["dis_status"] = []
    parsed_data["signal_strength"] = []
    for i in range(parsed_data["zone_map"]):
        dis, index = nByteUnpack(data, index, 3)
        status, index = nByteUnpack(data, index, 1)
        signal, index = nByteUnpack(data, index, 2)
        parsed_data["dis"].append(dis / 1000.0 / 1000.0)
        parsed_data["dis_status"].append(status)
        parsed_data["signal_strength"].append(signal)
    return parsed_data

TOFSense/S/F/FP/F2/F2 Mini/F2 P/F2 PH Usage Examples

import serial
_Frame_Header = b"\x57"  # Frame header 0x57
_Frame_Mark = b"\x00"  # Frame keyword 0x00
_One_Frmae_length = 16  # Number of bytes in one frame

PORT = "COM26"  # The serial port that the user TOF is connected to
BAUD = 921600  # Baud rate

if __name__ == "__main__":
    try:
        ser = serial.Serial(PORT, BAUD)
    except Exception as e:
        ser = None
        print(e)
    if ser:
        while True:
            buffer = bytearray()
            data = ser.read(1)
            if data == _Frame_Header:  # Confirm frame header
                buffer.extend(data)
                data = ser.read(1)
                if data == _Frame_Mark:  # Confirm keywords
                    buffer.extend(data)
                    buffer.extend(ser.read(_One_Frmae_length - 2))
                    tof_data = TOFSenseFrame0Unpack(buffer)
                    if tof_data:
                        print(tof_data)
# {'id': 0, 'system_time': 32500, 'dis': 369, 'dis_status': 1, 'signal_strength': 100}

TOFSense-M/MS Usage Examples

import serial
_Frame_Header = b"\x57"  # Frame header 0x57
_Frame_Mark = b"\x01"  # Frame keyword 0x00
_One_Frmae_length = 400  # Number of bytes in one frame

PORT = "COM26"  # The serial port that the user TOF is connected to
BAUD = 921600  # Baud rate

if __name__ == "__main__":
    try:
        ser = serial.Serial(PORT, BAUD)
    except Exception as e:
        ser = None
        print(e)
    if ser:
        while True:
            buffer = bytearray()
            data = ser.read(1)
            if data == _Frame_Header:  # Confirm frame header
                buffer.extend(data)
                data = ser.read(1)
                if data == _Frame_Mark:  # Confirm keywords
                    buffer.extend(data)
                    buffer.extend(ser.read(_One_Frmae_length - 2))
                    tof_data = TOFSenseMFrame0Unpack(buffer)
                    if tof_data:
                        print(tof_data)
# {'id': 0, 'system_time': 11366, 'zone_map': 64,
#  'dis': [2.154, 2.13243, 2.11173, 2.127596, 2.1398960000000002, 2.1082959999999997, 2.1094299999999997, 2.1198629999999996, 2.172, 2.116596, 2.124196, 2.143163, 2.1267300000000002, 2.107463, 2.123, 2.11543, 2.138163, 2.1318629999999996, 2.141896, 2.139596, 2.11843, 2.13943, 2.124863, 2.11843, 2.129596, 2.149863, 2.16833, 2.1600300000000003, 2.160063, 2.137896, 2.14, 2.11373, 2.15773, 2.126596, 2.1598629999999996, 2.1494630000000003, 2.16493, 2.1447629999999998, 2.13773, 2.10873, 2.1372959999999996, 2.15973, 2.160596, 2.16473, 2.1584630000000002, 2.1400300000000003, 2.11803, 2.11573, 2.1734299999999998, 2.139463, 2.158196, 2.166163, 2.11973, 2.1347300000000002, 2.128163, 2.1134630000000003, 2.167, 2.1527629999999998, 2.133163, 2.1372959999999996, 2.141896, 2.1318960000000002, 2.103863, 2.1174630000000003],
#  'dis_status': [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
#  'signal_strength': [20, 21, 24, 26, 29, 23, 21, 22, 20, 26, 32, 25, 24, 28, 19, 21, 25, 22, 29, 26, 21, 21, 22, 21, 26, 22, 30, 27, 34, 29, 16, 24, 24, 26, 22, 28, 36, 31, 24, 24, 23, 24, 26, 24, 28, 27, 27, 24, 21, 28, 32, 25, 24, 24, 25, 28, 20, 31, 25, 23, 29, 29, 22, 28]}