详解 Apollo 惯导驱动Step By Step

本文

主要解读下apollo的惯导驱动

驱动框架

如下图所示，从左往右看

• 硬件设备GNSS/RTK是通过串口接到测试电脑上的;
• RawStream是一个C++对象，他会读取串口中的原始数据并通过DataSpin()与RtkSpin()方法分别调用两个对象来分别解析GNSS和RTK数据;
• 同样DataParser与RtcmParser也是一个C++对象，主要调用各自的解析方法将原始数据解析成proto文件里定义的消息类型。

程序入口

look into gnss_component.cc

1. 通过GetProtoFromFile()方法读取config文件参数
2. 通过std::unique_ptr的reset方法构造raw_stream_对象
3. raw_stream_调用Init()方法初始化
4. raw_stream_调用Start()方法启动gnss与rtk解析线程

stream

Main job: read gnss and rtk raw data from hardware interface defined in gnss_conf.pb.txt file.

Writer	Channel	Description
stream_writer_	/apollo/sensor/gnss/stream_status	gnss and rtk status
raw_writer_	/apollo/sensor/gnss/raw_data	gnss raw data
rtcm_writer_	/apollo/sensor/gnss/rtcm_data	rtk raw data

Reader	Channel	Description
gpsbin_reader_	/apollo/sensor/gnss/raw_data	record gnss raw data into binfile

构造函数

通过std::unique_ptr的reset方法构造DataParser + RtcmParser两个对象。

Init() Method

1. 对data_parser_ptr_和rtcm_parser_ptr_指针调用Init()方法初始化解析线程;
2. 通过create_stream()函数创建三个数据流data_stream_、in_rtk_stream_、out_rtk_stream_分别处理gnss数据的读取，rtk数据的读取，向rtk发生指令;
3. 创建Write和Reader，stream_writer_、raw_writer_、rtcm_writer_、gpsbin_reader_。

Start() Method

启动DataSpin()和RtkSpin()两个线程

1. DataSpin() Method

1. 通过data_stream_对象调用read()方法从数据流中读取BUFFER_SIZE个数据到buffer_中;
2. 如果有数据读入，就通过raw_writer_调用write()方法向/apollo/sensor/gnss/raw_data通道发布数据;
3. 通过data_parser_ptr_指针调用ParseRawData()方法解析gnss原始数据;
4. 最后调用StreamStatusCheck()方法更新数据流状态。

2. RtkSpin() Method

1. 通过in_rtk_stream_对象调用read()方法从数据流中读取BUFFER_SIZE个数据到buffer_rtk_中;
2. 如果有数据读入，就通过PublishRtkData()方法向/apollo/sensor/gnss/rtcm_data通道发布数据;
3. 通过rtcm_parser_ptr_指针调用ParseRtcmData()方法解析rtk原始数据。

data_parser

Main job：解析gnss原始数据并发布到apollo对应channel上

Writer	Channel	Description
gnssstatus_writer_	/apollo/sensor/gnss/gnss_status	gnss status
insstatus_writer_	/apollo/sensor/gnss/ins_status	ins status
gnssbestpose_writer_	/apollo/sensor/gnss/best_pose	best gnss pose(lla)
corrimu_writer_	/apollo/sensor/gnss/corrected_imu	corrected imu
rawimu_writer_	/apollo/sensor/gnss/imu	raw imu
gps_writer_	/apollo/sensor/gnss/odometry	pose under utm
insstat_writer_	/apollo/sensor/gnss/ins_stat	ins stat
gnssephemeris_writer_	/apollo/sensor/gnss/rtk_eph	gnss rtk ephemeris
epochobservation_writer_	/apollo/sensor/gnss/rtk_obs	gnss rtk observation
heading_writer_	/apollo/sensor/gnss/heading	heading

构造函数

主要读取utm投影配置，为ins、gnss状态赋初值。

Init() Method

1. 创建writer对象;
2. 通过CreateParser()方法创建关键的NovatelParser对象

   // Defined in novatel_parser.cc
   Parser *CreateParser(config::Config config, bool is_base_station = false) {
     switch (config.data().format()) {
       case config::Stream::NOVATEL_BINARY:
         return Parser::CreateNovatel(config);
   
       default:
         return nullptr;
     }
   }

关键函数-ParseRawData()

1. 首先通过data_parser_指针调用update()方法获取原始消息的头跟尾;

   // defined in parser.h
   void Update(const uint8_t *data, size_t length) {
       data_ = data;
       data_end_ = data + length;
     }

2. 通过data_parser_指针调用GetMessage()方法读取消息至成员变量buffer_中;

   std::vector<uint8_t> buffer_;   //in novatel_parser.cc

3. 读取完接着就会调用PrepareMessage()方法把buffer_中的数据接到novatel_messages.h头文件里定义的消息结构体中，借助一个个HandleXXX()方法，内容有点杂就不展开说了;
4. 最后通过DispatchMessage()方法借助一个个PublishXXX()函数发布消息至对应的通道。

这里补充一点，就是每次HandleXXX()处理完就会return出来，紧接着就会调用PublishXXX()方法发布消息。

rtcm_parser

Main job：解析rtk原始数据并发布到apollo对应channel上

Writer	Channel	Description
gnssephemeris_writer_	/apollo/sensor/gnss/rtk_eph	gnss rtk ephemeris
epochobservation_writer_	/apollo/sensor/gnss/rtk_obs	gnss rtk observation

关键函数-ParseRtcmData()

1. 也是通过rtcm_parser_指针调用Update()方法获取消息;
2. 然后调用GetMessage()方法解析成对应消息;
3. 最后也是借助DispatchMessage()方法将消息发到对应通道。

由于解析流程跟data_parser很像，这边就不再赘述了。