pyRBLidar/LakiBeamUDP.cpp

#include "LakiBeamUDP.h"

//初始化了一些UDP网络通信相关的资源
//异步接收数据，并使用多线程处理接收到的数据
void LakiBeamUDP::LakiBeamUDP(string local_ip, string local_port, string laser_ip, string laser_port)
{
    //将本地设备的IP地址和端口号保存到对象的成员变量
    this->local_ip = local_ip;
    this->local_port = local_port;
    //远程（激光设备）
    this->laser_ip = laser_ip;
    this->laser_port = laser_port;

    //用于统计和控制某个资源
    dbmain = 0;
    dbmain_used = 0;
    urpmain = 0;
    urpmain_used = 0;

    char *temp = reinterpret_cast<char *>(udprepack);
    memset(temp, 0, CONFIG_MAX_REPACK * sizeof(repark_t));

    local_ep = new udp_endpoint_t(address_t::from_string("0.0.0.0"), atoi(local_port.c_str()));
    laser_ep = new udp_endpoint_t(address_t::from_string(laser_ip), atoi(laser_port.c_str()));
    socket = new udp_socket_t(io_servicess);
    socket->open(local_ep->protocol());
    socket->set_option(boost::asio::ip::udp::socket::reuse_address(true));
    socket->bind(*local_ep);
    socket->async_receive_from(boost::asio::buffer(buffff, CONFIG_FRAME), *laser_ep, boost::bind(&LakiBeamUDP::on_read, this, boost::asio::placeholders::error, boost::asio::placeholders::bytes_transferred));

    //创建并启动一个线程udprepack_thd，执行udprepack_thread函数
    //数据包处理
    udprepack_thd.reset(new boost::thread(boost::bind(&LakiBeamUDP::udprepack_thread, this)));

    //创建并启动一个线程receive_thd，执行receive_thread函数
    //接收UDP数据
    receive_thd.reset(new boost::thread(boost::bind(&LakiBeamUDP::receive_thread, this)));
}

//运行在独立线程中的方法，用于持续处理网络事件并接收UDP数据
void LakiBeamUDP::receive_thread()
{
    //这个线程持续监听和处理网络事件
    while (1)
    {
        try {
            io_servicess.run();
            boost::this_thread::interruption_point();
        }
        //检查当前线程是否被请求中断
        //如果当前线程被请求中断就会抛出一个异常，并使得线程退出
        catch (boost::system::error_code& err)
        {
            break;
        }
    }
}

//析构函数，实现了资源的清理和线程的安全退出
LakiBeamUDP::~LakiBeamUDP()
{
    io_servicess.stop();

    receive_thd->interrupt();
    receive_thd->join();

    delete socket;
    delete local_ep;
    delete laser_ep;

    udprepack_thd->interrupt();
    udprepack_thd->join();
}

//从缓冲区中同步地获取重新打包的UDP数据包，并将其传递给pack变量（数据同步一）
void LakiBeamUDP::sync_get_repackedpack(repark_t &pack)
{
    while (urpmain < 2)
    {
		
    }
    while (urpmain <= (urpmain_used + 1))
    {

    }
    urpmain_used = urpmain - 1;

    pack = udprepack[(urpmain_used) % CONFIG_MAX_REPACK];
}

//该方法通过非阻塞的方式从缓冲区中获取UDP数据包（数据同步二）
bool LakiBeamUDP::get_repackedpack(repark_t &pack)
{
	bool result = true;

    //如果数据包太少，只有一个或没有，则无法进行获取。
	if ((urpmain < 2)||(urpmain <= (urpmain_used + 1)))
	{
		result = false;
	}
	else
	{
        //获取了最新的数据包
		urpmain_used = urpmain - 1;
	}

	if (result == true)
	{
        pack = udprepack[(urpmain_used) % CONFIG_MAX_REPACK];
	}

	return result;
}

//用于处理异步接收的UDP数据，将接收到的数据存入缓冲区
void LakiBeamUDP::on_read(const boost::system::error_code& err, std::size_t read_bytes)
{
    if (!err)
    {
        char* temp;
        if (read_bytes == CONFIG_FRAME)
        {
            temp = reinterpret_cast<char*>(&(doublebuffer[dbmain % CONFIG_FRAME_MAX]));
            memcpy(temp, buffff, CONFIG_FRAME);
            //dbmain:用于指示当前主缓冲区的位置或索引
            //dbmain_used:用于指示当前在双缓冲区的主索引或位置，可能用于跟踪已处理的数据数量或状态
            dbmain++;
        }
    }
    //重新启动异步接收。即使在本次接收完成后，系统会立即等待下一个数据包的到来。
    socket->async_receive_from(boost::asio::buffer(buffff, CONFIG_FRAME), *laser_ep, boost::bind(&LakiBeamUDP::on_read, this, boost::asio::placeholders::error, boost::asio::placeholders::bytes_transferred));
}

//udprepack_thread线程定期检查缓冲区，获取数据包（根据数据包的特性去调用函数处理数据）
//用于处理从UDP数据包双缓冲区中提取数据并进行封装的逻辑
void LakiBeamUDP::udprepack_thread()
{
    while (1)
    {
        try
        {
            //该标志位用于控制后续的数据处理是否继续
            bool goon = true;
            udp_pack_t doublebuffertemp[2]; //存储两个UDP数据包的数据
            {
                //检查当前的双缓冲区是否有足够的数据可以被封装处理。至少有两个数据帧待处理
                if ((dbmain - dbmain_used) >= 2)
                {
                    char *source1 = reinterpret_cast<char *>(&doublebuffertemp[0]);
                    char *source2 = reinterpret_cast<char *>(&doublebuffertemp[1]);
                    char *from1 = reinterpret_cast<char *>(&(doublebuffer[dbmain_used % CONFIG_FRAME_MAX]));
                    char *from2 = reinterpret_cast<char *>(&(doublebuffer[(dbmain_used + 1) % CONFIG_FRAME_MAX]));
                    u32_t realindex = dbmain_used % CONFIG_FRAME_MAX;
                    if (realindex == CONFIG_FRAME_INDEX_MAX)
                    {
                        memcpy(source1, from1, CONFIG_FRAME);
                        memcpy(source2, from2, CONFIG_FRAME);
                    }
                    else
                    {
                        memcpy(source1, from1, CONFIG_FRAME * 2);
                    }
                    dbmain_used++;
                }
                //如果数据不足，
                else
                {
                    goon = false;
                }
            }

            if (goon == true)
            {
                if ((doublebuffertemp[0].factory == 0x4037) && (doublebuffertemp[1].factory == 0x4037))
                {
                    // 寻找0位包裹
                    u32_t start_index = 0xFFFFFFFF;
                    for (int i = 0; i < CONFIG_UDP_BLOCKS; i++)
                    {
                        //寻找起始数据包
                        if (doublebuffertemp[0].depths[i].azimuth == 0)
                        {
                            start_index = i;
                        }
                    }
                    // 0位数据包
                    if (start_index != 0xFFFFFFFF)
                    {
                        fill_havestart(doublebuffertemp, start_index);
                    }
                    // 非0位数据包
                    else
                    {
                        fill_nostart(doublebuffertemp);
                    }
                }
                else
                {
                    //                    DEBUG(doublebuffertemp[1].factory);
                    //                    DEBUG(doublebuffertemp[0].factory);
                }
            }

            //检查当前线程是否被请求中断
            boost::this_thread::interruption_point();
            //如果剩余的数据包不足两个，则线程会休眠2毫秒
			if ((dbmain - dbmain_used) < 2)
			{
				boost::this_thread::sleep(boost::posix_time::milliseconds(2));
			}
        }
        catch (...)
        {
            break;
        }
    }
}

//用于处理非起始的UDP数据包，并将其转换为内部udprepack结构中的点云数据。
//接收两个数据包，根据时间戳、角度和深度信息计算每个的点云的相关数据
void LakiBeamUDP::fill_nostart(udp_pack_t pack[2])
{
    (void)pack;
    u32_t start_time = pack[0].timestamp;
    u32_t end_time = pack[1].timestamp;

    u16_t tempdots = udprepack[urpmain % CONFIG_MAX_REPACK].maxdots;
    u32_t div_time = (end_time - start_time) / CONFIG_BLOCK_DEPTHS / CONFIG_UDP_BLOCKS;

    for (int i = 0; i < CONFIG_UDP_BLOCKS; i++)
    {
        if (pack[0].depths[i].head == 0xEEFF)
        {
            if (i < (CONFIG_UDP_BLOCKS - 1))
            {
                u16_t start_angle = pack[0].depths[i].azimuth;//当前block的起始角度
                u16_t end_angle = pack[0].depths[i + 1].azimuth;//下一个block的起始角度
                u16_t div_angle;        //每个数据块中相邻两个测距数据的角度递增值
                if (end_angle >= start_angle)
                {
                    div_angle = (end_angle - start_angle) / CONFIG_BLOCK_DEPTHS;
                }
                else
                {
                    div_angle = (CONFIG_DEGREE_MAX - start_angle + end_angle) / CONFIG_BLOCK_DEPTHS;
                }

                for (int j = 0; j < CONFIG_BLOCK_DEPTHS; j++)
                {
                    udprepack[urpmain % CONFIG_MAX_REPACK].dotcloud[tempdots % CONFIG_CIRCLE_DOTS].timestamp = start_time;
                    udprepack[urpmain % CONFIG_MAX_REPACK].dotcloud[tempdots % CONFIG_CIRCLE_DOTS].angle = start_angle;
                    udprepack[urpmain % CONFIG_MAX_REPACK].dotcloud[tempdots % CONFIG_CIRCLE_DOTS].distance = pack->depths[i].depth[j].distance0;
                    udprepack[urpmain % CONFIG_MAX_REPACK].dotcloud[tempdots % CONFIG_CIRCLE_DOTS].rssi = pack->depths[i].depth[j].rssi0;
                    udprepack[urpmain % CONFIG_MAX_REPACK].interval += div_time;
                    tempdots++; //在每次插入后递增，用于跟踪当前已插入的点运输量
                    if ((start_angle + div_angle) >= CONFIG_DEGREE_MAX)
                    {
                        start_angle = start_angle + div_angle - CONFIG_DEGREE_MAX;
                    }
                    else
                    {
                        start_angle += div_angle;
                    }
                    start_time += div_time;
                }
            }
            else
            {
                u16_t start_angle = pack[0].depths[i].azimuth;
                u16_t end_angle = pack[1].depths[0].azimuth;
                u16_t div_angle;
                if (end_angle >= start_angle)
                {
                    div_angle = (end_angle - start_angle) / CONFIG_BLOCK_DEPTHS;
                }
                else
                {
                    div_angle = (CONFIG_DEGREE_MAX - start_angle + end_angle) / CONFIG_BLOCK_DEPTHS;
                }
                for (int j = 0; j < CONFIG_BLOCK_DEPTHS; j++)
                {
                    udprepack[urpmain % CONFIG_MAX_REPACK].dotcloud[tempdots % CONFIG_CIRCLE_DOTS].timestamp = start_time;
                    udprepack[urpmain % CONFIG_MAX_REPACK].dotcloud[tempdots % CONFIG_CIRCLE_DOTS].angle = start_angle;
                    udprepack[urpmain % CONFIG_MAX_REPACK].dotcloud[tempdots % CONFIG_CIRCLE_DOTS].distance = pack->depths[i].depth[j].distance0;
                    udprepack[urpmain % CONFIG_MAX_REPACK].dotcloud[tempdots % CONFIG_CIRCLE_DOTS].rssi = pack->depths[i].depth[j].rssi0;
                    udprepack[urpmain % CONFIG_MAX_REPACK].interval += div_time;
                    tempdots++;
                    if ((start_angle + div_angle) >= CONFIG_DEGREE_MAX)
                    {
                        start_angle = start_angle + div_angle - CONFIG_DEGREE_MAX;
                    }
                    else
                    {
                        start_angle += div_angle;
                    }
                    start_time += div_time;
                }
            }
        }
    }
    udprepack[urpmain % CONFIG_MAX_REPACK].maxdots = tempdots;
}

//处理一系列UDP数据包，将数据填充道dotcloud结构中
//udp_pack_t pack[]  传入的UDP数据包数组 （包含来自传感器的数据）
//u32_t start 表示当前数据包的索引，通常是数据流中的起始点
void LakiBeamUDP::fill_havestart(udp_pack_t pack[], u32_t start)
{
    // urpmain表示当前处理的urpmain数组索引
    //表示正在处理第一包数据
    if (urpmain == 0)
    {
        //指向当前正在处理的Udprepack数组中的位置，它可能会用于确定当前的处理状态或处理的帧
        urpmain++;
        //用于记录已使用的udprepack数组的数量（已处理的帧的数量）
        urpmain_used++;

    //udprepack存储结构体的数组，用于存放解析后的数据
    //urpmain % CONFIG_MAX_REPACK  会将Urpmain的值限制在0到CONFIG_MAX_REPACK-1范围内，这样可以保证访问udprepack数组时不会越界
        udprepack[urpmain % CONFIG_MAX_REPACK].maxdots = 0;
        udprepack[urpmain % CONFIG_MAX_REPACK].interval = 0;

        (void)pack;
    //计算时间间隔，从UDP数据包中提取开始和结束时间戳
    //第一个数据包的时间戳：表示数据采集开始的时间
        u32_t start_time = pack[0].timestamp;
    //第二个数据包的时间戳：表示数据采集结束的时间
        u32_t end_time = pack[1].timestamp;

    //获取当前udprepack数组中指定索引位置的maxdots值
        u16_t tempdots = udprepack[urpmain % CONFIG_MAX_REPACK].maxdots;
    //计算每个数据块之间的时间间隔
        u32_t div_time = (end_time - start_time) / CONFIG_BLOCK_DEPTHS / CONFIG_UDP_BLOCKS;

        //根据start的值遍历数据块，每个数据块有多少个深度（遍历数据包中多个“块”）
        for (int i = start; i < CONFIG_UDP_BLOCKS; i++)
        {
            //判断是否是最后一个块
            if (i < (CONFIG_UDP_BLOCKS - 1))
            {
                u16_t start_angle = pack[0].depths[i].azimuth;
                u16_t end_angle = pack[0].depths[i + 1].azimuth;
                u16_t div_angle;
                //通过计算起始和结束角度之间的差值，得出UDP包中每个小块所需的角度增量。
                if (end_angle >= start_angle)
                {
                    div_angle = (end_angle - start_angle) / CONFIG_BLOCK_DEPTHS;
                }
                else
                {
                    div_angle = (CONFIG_DEGREE_MAX - start_angle + end_angle) / CONFIG_BLOCK_DEPTHS;
                }
                    
                //处理每个深度块中的深度数据，并将计算后的时间戳、角度、距离和信号强度存储到udprepack中
                //从输入的数据包中提取激光雷达的每个测量点，并将其数据存储到点云数据结构中
                //CONFIG_BLOCK_DEPTHS (16)
                for (int j = 0; j < CONFIG_BLOCK_DEPTHS; j++)
                {

                    //时间戳 
                    //dotcloud是udprapack数组中每个元素的一个成员，，用于存储点云数据
                    //start_time表示当前处理的数据点的时间戳，，将start_time存储到dotcloud的当前位置的timestamp字段中
                    udprepack[urpmain % CONFIG_MAX_REPACK].dotcloud[tempdots % CONFIG_CIRCLE_DOTS].timestamp = start_time;
                    //
                    udprepack[urpmain % CONFIG_MAX_REPACK].dotcloud[tempdots % CONFIG_CIRCLE_DOTS].angle = start_angle;
                    //pack是由udp_pack_t定义的结构体
                    //depths[i].depth[j].distance0  表示从第i个数据块中的第j个深度数据点获取距离信息
                    udprepack[urpmain % CONFIG_MAX_REPACK].dotcloud[tempdots % CONFIG_CIRCLE_DOTS].distance = pack->depths[i].depth[j].distance0;
                    //将当前数据点的RSSI信息pack->depths[i].depth[j].rssi0存储到udprepack数组中的dotcloud数组的对应位置
                    udprepack[urpmain % CONFIG_MAX_REPACK].dotcloud[tempdots % CONFIG_CIRCLE_DOTS].rssi = pack->depths[i].depth[j].rssi0;
                    //将当前数据点的时间间隔div_time累加到udprepack数组中当前元素的interval字段
                    udprepack[urpmain % CONFIG_MAX_REPACK].interval += div_time;
                    //用于跟踪当前正在处理的dotcloud索引
                    tempdots++; //增加点云数据的索引

                    //可能会大360
                    if ((start_angle + div_angle) >= CONFIG_DEGREE_MAX)
                    {
                        start_angle = start_angle + div_angle - CONFIG_DEGREE_MAX;
                    }
                    else
                    {
                        start_angle += div_angle;
                    }
                    //时间更新
                    //start_time：当前数据点的时间戳
                    //将start_time增加div_time的值，以反映下一个数据点的时间戳
                    //div_time可能是每个数据点之前的时间间隔
                    start_time += div_time;
                }
            }
           
            //最后一个块的处理逻辑
            //提取数据，并将其存储到udprepack结构中
            else
            {
                //假设pack是一个包含多个数据块的数组
                //从当前数据块的第i项中提取起始角度
                u16_t start_angle = pack[0].depths[i].azimuth;
                //从下一个数据块pack[1]的第一个深度点提取结束角度
                u16_t end_angle = pack[1].depths[0].azimuth;
                //用于存储每个深度点之间的角度增量
                u16_t div_angle;
                //如果结束角度大于起始角度，则可以直接计算差值
                if (end_angle >= start_angle)
                {
                    div_angle = (end_angle - start_angle) / CONFIG_BLOCK_DEPTHS;
                }
                //如果结束角度小于起始角度，则意味着角度跨越了0度
                else
                {
                    div_angle = (CONFIG_DEGREE_MAX - start_angle + end_angle) / CONFIG_BLOCK_DEPTHS;
                }

                //将start_angle更新为当前块中实际其实角度
                //(start + 1) * CONFIG_BLOCK_DEPTHS  表示当前数据块相对于第一块的偏移量，乘以div_angle可以得到当前数据块的起始角度
                //为了确保在处理多个数据块时，每个数据块的起始角度都是正确的
                start_angle = start_angle + div_angle * (start + 1) * CONFIG_BLOCK_DEPTHS;

                //处理当前数据块中的每个深度数据点
                for (int j = 0; j < CONFIG_BLOCK_DEPTHS; j++)
                {
                    udprepack[urpmain % CONFIG_MAX_REPACK].dotcloud[tempdots % CONFIG_CIRCLE_DOTS].timestamp = start_time;
                    udprepack[urpmain % CONFIG_MAX_REPACK].dotcloud[tempdots % CONFIG_CIRCLE_DOTS].angle = start_angle;
                    udprepack[urpmain % CONFIG_MAX_REPACK].dotcloud[tempdots % CONFIG_CIRCLE_DOTS].distance = pack->depths[i].depth[j].distance0;
                    udprepack[urpmain % CONFIG_MAX_REPACK].dotcloud[tempdots % CONFIG_CIRCLE_DOTS].rssi = pack->depths[i].depth[j].rssi0;
                    udprepack[urpmain % CONFIG_MAX_REPACK].interval += div_time;
                    tempdots++;
                    if ((start_angle + div_angle) >= CONFIG_DEGREE_MAX)
                    {
                        start_angle = start_angle + div_angle - CONFIG_DEGREE_MAX;
                    }
                    else
                    {
                        start_angle += div_angle;
                    }
                    start_time += div_time;
                }
            }
        }
        //将当前数据包中有效存储的数据点数量temdots更新到maxdots字段中
        //这个信息可以在后续处理中使用，以确定当前数据包中的点云数据的数量
        udprepack[urpmain % CONFIG_MAX_REPACK].maxdots = tempdots;
    }
    // 其它0包
    else
    {
        {
            (void)pack;
            u32_t start_time = pack[0].timestamp;
            u32_t end_time = pack[1].timestamp;
            //tempdots用于跟踪当前处理的数据点数量
            u16_t tempdots = udprepack[urpmain % CONFIG_MAX_REPACK].maxdots;
            
            u32_t div_time = (end_time - start_time) / CONFIG_BLOCK_DEPTHS / CONFIG_UDP_BLOCKS;
            //累加时间间隔
            //将当前数据包中所有数据点的时间间隔累加到udprepack[urpmain % CONFIG_MAX_REPACK].interval
            udprepack[urpmain % CONFIG_MAX_REPACK].interval += div_time;
            //处理当前数据包中的深度数据                 
            for (u32_t i = 0; i < start; i++)
            {
                u16_t start_angle = pack[0].depths[i].azimuth;
                u16_t end_angle = pack[0].depths[i + 1].azimuth;
                u16_t div_angle;
                if (end_angle >= start_angle)
                {
                    div_angle = (end_angle - start_angle) / CONFIG_BLOCK_DEPTHS;
                }
                else
                {
                    div_angle = (CONFIG_DEGREE_MAX - start_angle + end_angle) / CONFIG_BLOCK_DEPTHS;
                }
                for (int j = 0; j < CONFIG_BLOCK_DEPTHS; j++)
                {
                    udprepack[urpmain % CONFIG_MAX_REPACK].dotcloud[tempdots % CONFIG_CIRCLE_DOTS].timestamp = start_time;
                    udprepack[urpmain % CONFIG_MAX_REPACK].dotcloud[tempdots % CONFIG_CIRCLE_DOTS].angle = start_angle;
                    udprepack[urpmain % CONFIG_MAX_REPACK].dotcloud[tempdots % CONFIG_CIRCLE_DOTS].distance = pack->depths[i].depth[j].distance0;
                    udprepack[urpmain % CONFIG_MAX_REPACK].dotcloud[tempdots % CONFIG_CIRCLE_DOTS].rssi = pack->depths[i].depth[j].rssi0;
                    udprepack[urpmain % CONFIG_MAX_REPACK].interval += div_time;
                    tempdots++;
                    if ((start_angle + div_angle) >= CONFIG_DEGREE_MAX)
                    {
                        start_angle = start_angle + div_angle - CONFIG_DEGREE_MAX;
                    }
                    else
                    {
                        start_angle += div_angle;
                    }
                    start_time += div_time;
                }
            }
            //检查是否存在数据丢失
            //在点云数据数量为1440或3600的情况下
			if((tempdots==1440)||(tempdots==3600))
			{
                bool lost = false;  //用于标识是否存在数据丢失
                if (tempdots == 1440)
                {
                    for (int i = 180; i < (1440 - 17); i++)
                    {
                        if (udprepack[urpmain % CONFIG_MAX_REPACK].dotcloud[i+1].angle < udprepack[urpmain % CONFIG_MAX_REPACK].dotcloud[i].angle)
                        {
                            lost = true;
                        }
                    }
                }
                else
                {
                    for (int i = 450; i < (3600 - 17); i++)
                    {
                        if (udprepack[urpmain % CONFIG_MAX_REPACK].dotcloud[i + 1].angle < udprepack[urpmain % CONFIG_MAX_REPACK].dotcloud[i].angle)
                        {
                            lost = true;
                        }
                    }
                }

                if (lost == false)
                {
                    udprepack[urpmain % CONFIG_MAX_REPACK].interval /= tempdots;
                    udprepack[urpmain % CONFIG_MAX_REPACK].maxdots = tempdots;
                    urpmain++;
                }
			}
        }
        {
            //如果当前处理的不是最后一个数据块，则进入当前的数据块
            //将当前处理的点云数据包的最大点数maxdots初始化为0
            udprepack[urpmain % CONFIG_MAX_REPACK].maxdots = 0;

            (void)pack;
            u32_t start_time = pack[0].timestamp;
            u32_t end_time = pack[1].timestamp;

            u16_t tempdots = udprepack[urpmain % CONFIG_MAX_REPACK].maxdots;
            u32_t div_time = (end_time - start_time) / CONFIG_BLOCK_DEPTHS / CONFIG_UDP_BLOCKS;

            //循环处理数据包中的深度数据
            for (u32_t i = start; i < CONFIG_UDP_BLOCKS; i++)
            {
                //如果当前处理块不是最后一个块
                if (i < (CONFIG_UDP_BLOCKS - 1))
                {
                    u16_t start_angle = pack[0].depths[i].azimuth;
                    u16_t end_angle = pack[0].depths[i + 1].azimuth;
                    u16_t div_angle;
                    if (end_angle >= start_angle)
                    {
                        div_angle = (end_angle - start_angle) / CONFIG_BLOCK_DEPTHS;
                    }
                    else
                    {
                        div_angle = (CONFIG_DEGREE_MAX - start_angle + end_angle) / CONFIG_BLOCK_DEPTHS;
                    }
                    for (int j = 0; j < CONFIG_BLOCK_DEPTHS; j++)
                    {
                        udprepack[urpmain % CONFIG_MAX_REPACK].dotcloud[tempdots % CONFIG_CIRCLE_DOTS].timestamp = start_time;
                        udprepack[urpmain % CONFIG_MAX_REPACK].dotcloud[tempdots % CONFIG_CIRCLE_DOTS].angle = start_angle;
                        udprepack[urpmain % CONFIG_MAX_REPACK].dotcloud[tempdots % CONFIG_CIRCLE_DOTS].distance = pack->depths[i].depth[j].distance0;
                        udprepack[urpmain % CONFIG_MAX_REPACK].dotcloud[tempdots % CONFIG_CIRCLE_DOTS].rssi = pack->depths[i].depth[j].rssi0;
                        udprepack[urpmain % CONFIG_MAX_REPACK].interval += div_time;
                        tempdots++;
                        if ((start_angle + div_angle) >= CONFIG_DEGREE_MAX)
                        {
                            start_angle = start_angle + div_angle - CONFIG_DEGREE_MAX;
                        }
                        else
                        {
                            start_angle += div_angle;
                        }
                        start_time += div_time;
                    }
                }
                else
                {
                    u16_t start_angle = pack[0].depths[i].azimuth;
                    u16_t end_angle = pack[1].depths[0].azimuth;
                    u16_t div_angle;
                    if (end_angle >= start_angle)
                    {
                        div_angle = (end_angle - start_angle) / CONFIG_BLOCK_DEPTHS;
                    }
                    else
                    {
                        div_angle = (CONFIG_DEGREE_MAX - start_angle + end_angle) / CONFIG_BLOCK_DEPTHS;
                    }
                    for (u32_t j = 0; j < CONFIG_BLOCK_DEPTHS; j++)
                    {
                        udprepack[urpmain % CONFIG_MAX_REPACK].dotcloud[tempdots % CONFIG_CIRCLE_DOTS].timestamp = start_time;
                        udprepack[urpmain % CONFIG_MAX_REPACK].dotcloud[tempdots % CONFIG_CIRCLE_DOTS].angle = start_angle;
                        udprepack[urpmain % CONFIG_MAX_REPACK].dotcloud[tempdots % CONFIG_CIRCLE_DOTS].distance = pack->depths[i].depth[j].distance0;
                        udprepack[urpmain % CONFIG_MAX_REPACK].dotcloud[tempdots % CONFIG_CIRCLE_DOTS].rssi = pack->depths[i].depth[j].rssi0;
                        udprepack[urpmain % CONFIG_MAX_REPACK].interval += div_time;
                        tempdots++;
                        if ((start_angle + div_angle) >= CONFIG_DEGREE_MAX)
                        {
                            start_angle = start_angle + div_angle - CONFIG_DEGREE_MAX;
                        }
                        else
                        {
                            start_angle += div_angle;
                        }
                        start_time += div_time;
                    }
                }
            }
            //处理结束，将当前的点数tempdots更新为udprepack结构中的最大点数
            udprepack[urpmain % CONFIG_MAX_REPACK].maxdots = tempdots;
        }
    }
}