无人驾驶——1、GNSS+IMU定位（基于三阶互补算法）

       简单的搭建工作空间就不说了，从节点的编写开始，详细讲一下怎么进行多线程节点的读取和GNSS及IMU数据的处理以及飞控中三阶互补算法的实现过程。

1.编写我们自己的msg文件，在src目录下构建msg文件，里面创建一个data.msg用于自定义我们要传输的消息类型。这里我定义了2个double型的用于传输GNSS坐标点，3个float型用于传输IMU的数据。

       在编写完成之后不要忘记在Cmakelist里面加入声明message_generation

## is used, also find other catkin packages

find_package(catkin REQUIRED COMPONENTS

  roscpp

  std_msgs

  serial

  message_generation

)

       Serial是用于串口读取的，这一部分可以百度找到对应的配置说明。

## Generate messages in the 'msg' folder

 add_message_files(

   FILES

   data.msg

 )



 generate_messages(

   DEPENDENCIES

   std_msgs

 )

与此同时在package.xml中找到下面两句取消注释。

<build_depend>message_generation</build_depend>

<exec_depend>message_runtime</exec_depend>

       至此，消息配置完成。

       2.编写串口读取GNSS与IMU的节点cpp文件

       代码如下：（1）GNSS

#include <ros/ros.h>

#include <serial/serial.h>  //ROS已经内置了的串口包

#include <std_msgs/String.h>

#include <iostream>

#include <string.h>

#include <data_fusion/data.h>

using namespace serial;

serial::Serial ser; //声明串口对象



float Naxis_buff[5]={0},Eaxis_buff[5]={0},Naxis_sum=0,Eaxis_sum=0;

unsigned char axis_num=0;



//滑动平均法

void slid_aver(float Naxis,float Eaxis)

{

       unsigned char i=0;

       if(axis_num<5)

       {

              Naxis_buff[axis_num]=Naxis;Naxis_sum+=Naxis;

              Eaxis_buff[axis_num]=Eaxis;Eaxis_sum+=Eaxis;

              axis_num++;

       }

       else

       {

              Naxis_sum-=Naxis_buff[0];Eaxis_sum-=Eaxis_buff[0];

              for(i=0;i<axis_num-1;i++){Naxis_buff[i]=Naxis_buff[i+1];Eaxis_buff[i]=Eaxis_buff[i+1];}

              Naxis_buff[i]=Naxis;Naxis_sum+=Naxis;

              Eaxis_buff[i]=Eaxis;Eaxis_sum+=Eaxis;

       }

}



int main(int argc, char** argv)

{

       unsigned char i=0,j=0,k=0;

       char buff[35]={0};

       double N_axis=0,E_axis=0;

       //初始化节