Example code

C Language

对于TOFSense以及TOFSense-F系列的模块测距结果都可以通过用下面的结构体进行表示。

typedef struct {
  uint8_t id;               // ID
  uint32_t system_time;     // 系统上电至此帧的时间
  float dis;                // 测距值
  uint8_t dis_status;       // 距离状态指示
  uint16_t signal_strength; // 信号强度
  uint8_t range_precision;  // 仅tofsense-f/fP型号有效
} nts_frame0_result_t;

对于TOFSense-M系列的模块测距结果可以通过用下面的结构体进行表示。

typedef struct {
  float dis;                // 测距值
  uint8_t dis_status;       // 距离状态指示
  uint16_t signal_strength; // 信号强度
} ntsm_frame0_pixel_t;

typedef struct {
  uint8_t id;               // ID
  uint32_t system_time;     // 系统上电至此帧的时间
  uint8_t pixel_count;      // 像素点数量
  ntsm_frame0_pixel_t pixels[64];
} ntsm_frame0_result_t;

Nlink解析包

官网提供有纯c语言编写的解析包。 对于TOFSense在工程中只需要添加如下文件即可，必要前置+根据不同系列选择对应的解析文件。

使用示例

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};  // 模拟接收数据 一帧完整数据16个字节

  data_length = sizeof(rx_buf)/sizeof(rx_buf[0]); // 计算接收数据长度

  if (g_nts_frame0.UnpackData(rx_buf, data_length)) // 调用解析函数，传入数据 TOFSense/S/F/FP/F2/F2 Mini/F2 P/F2 PH
  {
    // 解析成功
    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};  // 模拟接收数据 8*8模式下一帧完整数据400个字节

  data_length = sizeof(rx_buf)/sizeof(rx_buf[0]); // 计算接收数据长度

  if (g_ntsm_frame0.UnpackData(rx_buf, data_length)) // 调用解析函数，传入数据 TOFSense-M/MS
  {
    // 解析成功
    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模式查询指令示例

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);
    // 发送函数  这里以HAL库为例
    HAL_UART_Transmit_DMA(&TOF_UART_HANDLE, readframe0, 8);
}

CAN模式查询指令示例

CAN_TxHeaderTypeDef TxHeader;
uint8_t can_tx_buf[8];

void tof_can_send_read_frame0(uint8_t id)
{
    TxHeader.StdId=0x402;	//查询标准帧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;
    // 发送函数  这里以HAL库为例
    HAL_CAN_AddTxMessage(&TOF_CANHANDLE, &TxHeader, can_tx_buf, (uint32_t*)CAN_TX_MAILBOX0);
}

IIC

//待更新

原始数据解析

用户也可以按照固定字节直接移位的方式解析数据。

#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};  // 模拟接收数据 一帧完整数据16个字节
    uint8_t checksum = 0;
    if (u_rx_buf[0] == 0x57 && u_rx_buf[1] == 0x00) // 校验帧头
    {
        for (int i = 0; i < TOFSENSE_M_DATA_LENGTH - 1; i++)    //校验和
        {
            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};  // 模拟接收数据 8*8模式下一帧完整数据400个字节
    uint8_t checksum = 0;
    if (u_rx_buf[0] == 0x57 && u_rx_buf[1] == 0x01) // 校验帧头
    {
        for (int i = 0; i < TOFSENSE_M_DATA_LENGTH - 1; i++)    // 校验和
        {
            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);
            }
            
            // 以8*8的格式打印数据
            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 = sum(data[:-1]) & 0xFF
    return checksum == data[-1]

def check_header(data, header):
    """校验帧头"""
    return data[0] == header[0] and data[1] == header[1]

def nByteUnpack(data, index, byte_num, signed=False):
    """
    返回单个解析数据
    Args:
        data:原数据 bytes
        index:起始位置 int
        byte_num:单个字节数 int
        signed:是否为有符号数 bool
    """
    return (
        int.from_bytes(data[index : index + byte_num], byteorder="little", signed=signed),
        index + byte_num,
    )

def TOFSenseFrame0Unpack(data):
    """TOFSense/TOFSense-F系列帧解析"""
    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):
    """TOFSense-M系列帧解析"""
    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使用示例

import serial
_Frame_Header = b"\x57"  # 帧头0x57
_Frame_Mark = b"\x00"  # 帧关键字0x00
_One_Frmae_length = 16  # 一帧字节数

PORT = "COM26"  # 用户TOF所连接的串口
BAUD = 921600  # 波特率

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:  # 确认帧头
                buffer.extend(data)
                data = ser.read(1)
                if data == _Frame_Mark:  # 确认关键字
                    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使用示例

import serial
_Frame_Header = b"\x57"  # 帧头0x57
_Frame_Mark = b"\x01"  # 帧关键字0x00
_One_Frmae_length = 400  # 一帧字节数

PORT = "COM26"  # 用户TOF所连接的串口
BAUD = 921600  # 波特率

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:  # 确认帧头
                buffer.extend(data)
                data = ser.read(1)
                if data == _Frame_Mark:  # 确认关键字
                    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]}