百度apollo novatel_parser.cc部分解算代码

 

Parser::MessageType NovatelParser::GetMessage(MessagePtr* message_ptr) {//获取消息 放入栈中 调用PrepareMessage解析数据 yuphe2019040211
  if (data_ == nullptr) {
    return MessageType::NONE;
  }

  while (data_ < data_end_) {
    if (buffer_.empty()) {  // Looking for SYNC0
      if (*data_ == novatel::SYNC_0) {
        buffer_.push_back(*data_);
      }
      ++data_;
    } else if (buffer_.size() == 1) {  // Looking for SYNC1
      if (*data_ == novatel::SYNC_1) {
        buffer_.push_back(*data_++);
      } else {
        buffer_.clear();
      }
    } else if (buffer_.size() == 2) {  // Looking for SYNC2
      switch (*data_) {
        case novatel::SYNC_2_LONG_HEADER:
          buffer_.push_back(*data_++);
          header_length_ = sizeof(novatel::LongHeader);
          break;
        case novatel::SYNC_2_SHORT_HEADER:
          buffer_.push_back(*data_++);
          header_length_ = sizeof(novatel::ShortHeader);
          break;
        default:
          buffer_.clear();
      }
    } else if (header_length_ > 0) {  // Working on header.
      if (buffer_.size() < header_length_) {
        buffer_.push_back(*data_++);
      } else {
        if (header_length_ == sizeof(novatel::LongHeader)) {
          total_length_ = header_length_ + novatel::CRC_LENGTH +
                          reinterpret_cast<novatel::LongHeader*>(buffer_.data())
                              ->message_length;
        } else if (header_length_ == sizeof(novatel::ShortHeader)) {
          total_length_ =
              header_length_ + novatel::CRC_LENGTH +
              reinterpret_cast<novatel::ShortHeader*>(buffer_.data())
                  ->message_length;
        } else {
          AERROR << "Incorrect header_length_. Should never reach here.";
          buffer_.clear();
        }
        header_length_ = 0;
      }
    } else if (total_length_ > 0) {
      if (buffer_.size() < total_length_) {  // Working on body.
        buffer_.push_back(*data_++);
        continue;
      }
      MessageType type = PrepareMessage(message_ptr);//调用 处理
      buffer_.clear();
      total_length_ = 0;
      if (type != MessageType::NONE) {
        return type;
      }
    }
  }
  return MessageType::NONE;
}

下面是解析函数,case 语句中：

Parser::MessageType NovatelParser::PrepareMessage(MessagePtr* message_ptr) {//这一段是处理的函数
  if (!check_crc()) {
    AERROR << "CRC check failed.";
    return MessageType::NONE;
  }

  uint8_t* message = nullptr;
  novatel::MessageId message_id;
  uint16_t message_length;
  uint16_t gps_week;
  uint32_t gps_millisecs;
  if (buffer_[2] == novatel::SYNC_2_LONG_HEADER) {
    auto header = reinterpret_cast<const novatel::LongHeader*>(buffer_.data());
    message = buffer_.data() + sizeof(novatel::LongHeader);
    gps_week = header->gps_week;
    gps_millisecs = header->gps_millisecs;
    message_id = header->message_id;
    message_length = header->message_length;
  } else {
    auto header = reinterpret_cast<const novatel::ShortHeader*>(buffer_.data());
    message = buffer_.data() + sizeof(novatel::ShortHeader);
    gps_week = header->gps_week;
    gps_millisecs = header->gps_millisecs;
    message_id = header->message_id;
    message_length = header->message_length;
  }
  switch (message_id) {//消息类型
    case novatel::BESTGNSSPOS:
      if (message_length != sizeof(novatel::BestPos)) {
        AERROR << "Incorrect message_length";
        break;
      }
      if (HandleGnssBestpos(reinterpret_cast<novatel::BestPos*>(message),
                            gps_week, gps_millisecs)) {
        *message_ptr = &bestpos_;
        return MessageType::BEST_GNSS_POS;
      }
      break;

    case novatel::BESTPOS:
    case novatel::PSRPOS:
      if (message_length != sizeof(novatel::BestPos)) {
        AERROR << "Incorrect message_length";
        break;
      }
      if (HandleBestPos(reinterpret_cast<novatel::BestPos*>(message), gps_week,
                        gps_millisecs)) {
        *message_ptr = &gnss_;
        return MessageType::GNSS;
      }
      break;

    case novatel::BESTGNSSVEL:
    case novatel::BESTVEL:
    case novatel::PSRVEL:
      if (message_length != sizeof(novatel::BestVel)) {
        AERROR << "Incorrect message_length";
        break;
      }
      if (HandleBestVel(reinterpret_cast<novatel::BestVel*>(message), gps_week,
                        gps_millisecs)) {
        *message_ptr = &gnss_;
        return MessageType::GNSS;
      }
      break;

    case novatel::CORRIMUDATA:
    case novatel::CORRIMUDATAS:
      if (message_length != sizeof(novatel::CorrImuData)) {
        AERROR << "Incorrect message_length";
        break;
      }

      if (HandleCorrImuData(reinterpret_cast<novatel::CorrImuData*>(message))) {
        *message_ptr = &ins_;
        return MessageType::INS;
      }
      break;

    case novatel::INSCOV:
    case novatel::INSCOVS:
      if (message_length != sizeof(novatel::InsCov)) {
        AERROR << "Incorrect message_length";
        break;
      }

      if (HandleInsCov(reinterpret_cast<novatel::InsCov*>(message))) {
        *message_ptr = &ins_;
        return MessageType::INS;
      }
      break;

    case novatel::INSPVA:
    case novatel::INSPVAS:
      if (message_length != sizeof(novatel::InsPva)) {
        AERROR << "Incorrect message_length";
        break;
      }

      if (HandleInsPva(reinterpret_cast<novatel::InsPva*>(message))) {
        *message_ptr = &ins_;
        return MessageType::INS;
      }
      break;

    case novatel::RAWIMUX:
    case novatel::RAWIMUSX:
      if (message_length != sizeof(novatel::RawImuX)) {
        AERROR << "Incorrect message_length";
        break;
      }

      if (HandleRawImuX(reinterpret_cast<novatel::RawImuX*>(message))) {
        *message_ptr = &imu_;
        return MessageType::IMU;
      }
      break;

    case novatel::RAWIMU:
    case novatel::RAWIMUS:
      if (message_length != sizeof(novatel::RawImu)) {
        AERROR << "Incorrect message_length";
        break;
      }

      if (HandleRawImu(reinterpret_cast<novatel::RawImu*>(message))) {
        *message_ptr = &imu_;
        return MessageType::IMU;
      }
      break;

    case novatel::INSPVAX:
      if (message_length != sizeof(novatel::InsPvaX)) {
        AERROR << "Incorrect message_length";
        break;
      }

      if (HandleInsPvax(reinterpret_cast<novatel::InsPvaX*>(message), gps_week,
                        gps_millisecs)) {
        *message_ptr = &ins_stat_;
        return MessageType::INS_STAT;
      }
      break;

    case novatel::BDSEPHEMERIS:
      if (message_length != sizeof(novatel::BDS_Ephemeris)) {
        AERROR << "Incorrect BDSEPHEMERIS message_length";
        break;
      }
      if (HandleBdsEph(reinterpret_cast<novatel::BDS_Ephemeris*>(message))) {
        *message_ptr = &gnss_ephemeris_;
        return MessageType::BDSEPHEMERIDES;
      }
      break;

    case novatel::GPSEPHEMERIS:
      if (message_length != sizeof(novatel::GPS_Ephemeris)) {
        AERROR << "Incorrect GPSEPHEMERIS message_length";
        break;
      }
      if (HandleGpsEph(reinterpret_cast<novatel::GPS_Ephemeris*>(message))) {
        *message_ptr = &gnss_ephemeris_;
        return MessageType::GPSEPHEMERIDES;
      }
      break;

    case novatel::GLOEPHEMERIS:
      if (message_length != sizeof(novatel::GLO_Ephemeris)) {
        AERROR << "Incorrect GLOEPHEMERIS message length";
        break;
      }
      if (HandleGloEph(reinterpret_cast<novatel::GLO_Ephemeris*>(message))) {
        *message_ptr = &gnss_ephemeris_;
        return MessageType::GLOEPHEMERIDES;
      }
      break;

    case novatel::RANGE:
      if (DecodeGnssObservation(buffer_.data(),
                                buffer_.data() + buffer_.size())) {
        *message_ptr = &gnss_observation_;
        return MessageType::OBSERVATION;
      }
      break;

    case novatel::HEADING:
      if (message_length != sizeof(novatel::Heading)) {
        AERROR << "Incorrect message_length";
        break;
      }
      if (HandleHeading(reinterpret_cast<novatel::Heading*>(message), gps_week,
                        gps_millisecs)) {
        *message_ptr = &heading_;
        return MessageType::HEADING;
      }
      break;

    default:
      break;
  }
  return MessageType::NONE;
}