Apollo 10.0
自动驾驶开放平台
huace_text_parser.cc
浏览该文件的文档.
1/******************************************************************************
2 * Copyright 2017 The Apollo Authors. All Rights Reserved.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *****************************************************************************/
16
17// An parser for decoding binary messages from a NovAtel receiver. The following
18// messages must be
19// logged in order for this parser to work properly.
20//
21#include <cmath>
22#include <limits>
23#include <memory>
24#include <sstream>
25#include <string>
26#include <vector>
27
28#include "cyber/cyber.h"
31
32namespace apollo {
33namespace drivers {
34namespace gnss {
35
37 std::string GPCHC = "$GPCHC";
38 size_t GPCHC_SIZE = 24;
39 std::string GPCHCX = "$GPCHCX";
40 size_t GPCHCX_SIZE = 46;
41};
42
43#define ACC_UNIT 9.806
44
46 public:
47 explicit HuaCeTextParser(const config::Config &config)
48 : HuaCeBaseParser(config) {}
49 bool PrepareMessage() override;
50
51 private:
52 void PrepareMessageGPCHC(const std::vector<std::string> &fields);
53 void PrepareMessageGPCHCX(const std::vector<std::string> &fields);
54
55 HuaCeProtocol protocol_;
56 std::string input_str_;
57};
58
60 return new HuaCeTextParser(config);
61}
62
64 ADEBUG << "HUACE ASCII is: " << data_;
65
66 // message may be truncated, just save the last message
67 const uint8_t *data_start = data_end_;
68 for (; data_start != data_; --data_start) {
69 if (*data_start == '$') {
70 break;
71 }
72 }
73 if (data_start != data_) {
74 AWARN << "HUACE message has been truncated: " << data_;
75 }
76
77 if (*data_start != '$') {
78 input_str_.append(reinterpret_cast<const char *>(data_start),
79 std::distance(data_start, data_end_));
80 } else {
81 input_str_.assign(reinterpret_cast<const char *>(data_start),
82 std::distance(data_start, data_end_));
83 }
84
85 if (*(data_end_ - 1) != 0x0A) {
86 AWARN << "HUACE ASCII message is not complete: " << data_start;
87 return false;
88 }
89
90 std::vector<std::string> fields;
91 std::stringstream ss(input_str_.substr(0, input_str_.rfind('*')));
92 for (std::string field; std::getline(ss, field, ',');) {
93 fields.push_back(field);
94 }
95 if (fields.empty()) {
96 return false;
97 }
98 auto valid_fields = [&]() -> bool {
99 for (size_t i = 1; i < fields.size(); ++i) {
100 if (fields[i].empty()) {
101 fields[i] = "0";
102 } else if (i == 33 && fields[i] == "X") {
103 continue;
104 } else if (fields[i].find_first_not_of("0123456789.- ") !=
105 std::string::npos) {
106 AERROR << "HUACE ASCII message field error: " << fields[i]
107 << ", input str: " << input_str_;
108 return false;
109 }
110 }
111 return true;
112 };
113 if (fields[0] == protocol_.GPCHC) {
114 if (fields.size() < protocol_.GPCHC_SIZE) {
115 AERROR << "GPCHC message format error: " << data_start;
116 return false;
117 }
118 if (!valid_fields()) {
119 return false;
120 }
121 PrepareMessageGPCHC(fields);
122 return true;
123 } else if (fields[0] == protocol_.GPCHCX) {
124 if (fields.size() < protocol_.GPCHCX_SIZE) {
125 AERROR << "GPCHCX message format error:" << data_start;
126 return false;
127 }
128 if (!valid_fields()) {
129 return false;
130 }
131 PrepareMessageGPCHCX(fields);
132 return true;
133 }
134 return false;
135}
136
137void HuaCeTextParser::PrepareMessageGPCHC(
138 const std::vector<std::string> &fields) {
139 decode_message_.messageID = fields[0];
140 decode_message_.GPSWeek = std::stoi(fields[1]);
141 decode_message_.GPSTime = std::stod(fields[2]);
144 decode_message_.Heading = std::stod(fields[3]);
145 decode_message_.Pitch = std::stod(fields[4]);
146 decode_message_.Roll = std::stod(fields[5]);
147 decode_message_.GyroX = std::stod(fields[6]) * DEG_TO_RAD;
148 decode_message_.GyroY = std::stod(fields[7]) * DEG_TO_RAD;
149 decode_message_.GyroZ = std::stod(fields[8]) * DEG_TO_RAD;
150 decode_message_.AccX = std::stod(fields[9]) * ACC_UNIT;
151 decode_message_.AccY = std::stod(fields[10]) * ACC_UNIT;
152 decode_message_.AccZ = std::stod(fields[11]) * ACC_UNIT;
153 decode_message_.Latitude = std::stod(fields[12].empty() ? "0" : fields[12]);
154 decode_message_.Longitude = std::stod(fields[13].empty() ? "0" : fields[13]);
155 decode_message_.Altitude = std::stod(fields[14].empty() ? "0" : fields[14]);
156 decode_message_.Ve = std::stod(fields[15]);
157 decode_message_.Vn = std::stod(fields[16]);
158 decode_message_.Vu = std::stod(fields[17]);
159 decode_message_.V = std::stod(fields[18]);
160 decode_message_.NSV1 = std::stoi(fields[19]);
161 decode_message_.NSV2 = std::stoi(fields[20]);
162 int status = std::stoi(fields[21]);
163 PrepareMessageStatus(status % 10, status / 10);
164 decode_message_.Age = std::stod(fields[22]);
165 decode_message_.WarningCs = fields[23];
166}
167
168void HuaCeTextParser::PrepareMessageGPCHCX(
169 const std::vector<std::string> &fields) {
170 PrepareMessageGPCHC(fields);
171 decode_message_.lat_std = std::stod(fields[24]);
172 decode_message_.lon_std = std::stod(fields[25]);
173 decode_message_.alti_std = std::stod(fields[26]);
174 decode_message_.ve_std = std::stod(fields[27]);
175 decode_message_.vn_std = std::stod(fields[28]);
176 decode_message_.vu_std = std::stod(fields[29]);
177 decode_message_.roll_std = std::stod(fields[30]);
178 decode_message_.pitch_std = std::stod(fields[31]);
179 decode_message_.yaw_std = std::stod(fields[32]);
180}
181
182} // namespace gnss
183} // namespace drivers
184} // namespace apollo
void PrepareMessageStatus(const uint8_t &system_state, const uint8_t &satellite_status)
HuaCeTextParser(const config::Config &config)
const uint8_t * data_
Definition parser.h:138
static Parser * CreateHuaCeText(const config::Config &config)
const uint8_t * data_end_
Definition parser.h:139
#define ACC_UNIT
#define ADEBUG
Definition log.h:41
#define AERROR
Definition log.h:44
#define AWARN
Definition log.h:43
constexpr double DEG_TO_RAD
constexpr int SECONDS_PER_WEEK
class register implement
Definition arena_queue.h:37