SyncBasedFifo

有些无人机厂商的图像和飞控数据分开传输，由于两者传输速率的不同，容易造成图像帧和GPS数据的不同步，从而导致GPS融合全景图后定位不准或无法融合。通过UDP协议模拟图像端和飞控端收发数据，设置图像端传输速率为两秒一帧，飞控端传输速率为一秒一帧，以图像端接收到数据的时间戳为准，飞控端接收当前时刻的飞控数据，基于FIFO实现两者的数据同步。

关键思路

• fifo通知

  • 图像端写，飞控端读

   //图像端接收到数据后，通知飞控端
   write(fifo_fd, buf, strlen(buf));
  
   //飞控端接收通知
   ret = read(fifo_fd, buf, sizeof(buf));

• 清空飞控服务器端读缓冲区

  • fifo文件描述符设置非阻塞

   int flag = fcntl(fifo_fd, F_GETFL);
   flag |= O_NONBLOCK;
   fcntl(fifo_fd, F_SETFL, flag);

  • 清空读缓冲区

   while(1){
       bzero(buffer,BUFFER_SIZE);
       fileTrans = recvfrom(sockfd,buffer,BUFFER_SIZE,0,(struct sockaddr *)&client,&addrlen);
       char buf[2];
       int ret;
       ret = read(fifo_fd, buf, sizeof(buf));
       if(ret > 0){
           break;
           }
    }
  
   //读取当前时刻的飞控数据
   bzero(buffer,BUFFER_SIZE);
   fileTrans = recvfrom(sockfd,buffer,BUFFER_SIZE,0,(struct sockaddr *)&client,&addrlen);

• 测试

  • 创建fifo文件

   mkfifo imageGPSFifo

  • 编译生成可执行文件

   gcc imageSer -o imageser
   gcc imageCli -o imagecli
   gcc gpsSer -o gpsser
   gcc gpsCli -o gpscli

  • 启动图像端和飞控端

   //清空图像接收文件夹内的文件
   cd ./imageRecv
   rm -f *
  
   //先启动飞控端
   ./gpsser imageGPSfifo
   ./gpscli	//输入测试数据 ./gps/gps.txt
  
   //启动图像端
   ./imageser imageGPSfifo
   ./imagecli

  • 结果文件

   //飞控端接收文件为当前文件夹gps.txt
   //图像端接收的图片在imageRecv