Apollo 10.0
自动驾驶开放平台
rtcm3_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
18
19#include <utility>
20
21namespace apollo {
22namespace drivers {
23namespace gnss {
24
25// Anonymous namespace that contains helper constants and functions.
26namespace {
27
28template <typename T>
29constexpr bool is_zero(T value) {
30 return value == static_cast<T>(0);
31}
32
33} // namespace
34
35Parser *Parser::CreateRtcmV3(bool is_base_station) {
36 return new Rtcm3Parser(is_base_station);
37}
38
39Rtcm3Parser::Rtcm3Parser(bool is_base_station) {
40 if (1 != init_rtcm(&rtcm_)) {
41 init_flag_ = true;
42 } else {
43 init_flag_ = false;
44 }
45
46 ephemeris_.Clear();
47 observation_.Clear();
48 is_base_station_ = is_base_station;
49}
50
51bool Rtcm3Parser::SetStationPosition() {
52 auto iter = station_location_.find(rtcm_.staid);
53 if (iter == station_location_.end()) {
54 AWARN << "Station " << rtcm_.staid << " has no location info.";
55 return false;
56 }
57
58 observation_.set_position_x(iter->second.x);
59 observation_.set_position_y(iter->second.y);
60 observation_.set_position_z(iter->second.z);
61 return true;
62}
63
64void Rtcm3Parser::FillKepplerOrbit(
65 const eph_t &eph, apollo::drivers::gnss::KepplerOrbit *keppler_orbit) {
66 keppler_orbit->set_week_num(eph.week);
67
68 keppler_orbit->set_af0(eph.f0);
69 keppler_orbit->set_af1(eph.f1);
70 keppler_orbit->set_af2(eph.f2);
71
72 keppler_orbit->set_iode(eph.iode);
73 keppler_orbit->set_deltan(eph.deln);
74 keppler_orbit->set_m0(eph.M0);
75 keppler_orbit->set_e(eph.e);
76
77 keppler_orbit->set_roota(std::sqrt(eph.A));
78
79 keppler_orbit->set_toe(eph.toes);
80 keppler_orbit->set_toc(eph.tocs);
81
82 keppler_orbit->set_cic(eph.cic);
83 keppler_orbit->set_crc(eph.crc);
84 keppler_orbit->set_cis(eph.cis);
85 keppler_orbit->set_crs(eph.crs);
86 keppler_orbit->set_cuc(eph.cuc);
87 keppler_orbit->set_cus(eph.cus);
88
89 keppler_orbit->set_omega0(eph.OMG0);
90 keppler_orbit->set_omega(eph.omg);
91 keppler_orbit->set_i0(eph.i0);
92 keppler_orbit->set_omegadot(eph.OMGd);
93 keppler_orbit->set_idot(eph.idot);
94
95 // keppler_orbit->set_codesonL2channel(eph.);
96 keppler_orbit->set_l2pdataflag(eph.flag);
97 keppler_orbit->set_accuracy(eph.sva);
98 keppler_orbit->set_health(eph.svh);
99 keppler_orbit->set_tgd(eph.tgd[0]);
100 keppler_orbit->set_iodc(eph.iodc);
101
102 int prn = 0;
103 satsys(eph.sat, &prn);
104 keppler_orbit->set_sat_prn(prn);
105}
106
107void Rtcm3Parser::FillGlonassOrbit(const geph_t &eph,
109 orbit->set_position_x(eph.pos[0]);
110 orbit->set_position_y(eph.pos[1]);
111 orbit->set_position_z(eph.pos[2]);
112
113 orbit->set_velocity_x(eph.vel[0]);
114 orbit->set_velocity_y(eph.vel[1]);
115 orbit->set_velocity_z(eph.vel[2]);
116
117 orbit->set_accelerate_x(eph.acc[0]);
118 orbit->set_accelerate_y(eph.acc[1]);
119 orbit->set_accelerate_z(eph.acc[2]);
120
121 orbit->set_health(eph.svh);
122 orbit->set_clock_offset(-eph.taun);
123 orbit->set_clock_drift(eph.gamn);
124 orbit->set_infor_age(eph.age);
125
126 orbit->set_frequency_no(eph.frq);
127 // orbit->set_toe(eph.toe.time + eph.toe.sec);
128 // orbit->set_tk(eph.tof.time + eph.tof.sec);
129
130 int week = 0;
131
132 double second = time2gpst(eph.toe, &week);
133 orbit->set_week_num(week);
134 orbit->set_week_second_s(second);
135 orbit->set_toe(second);
136
137 second = time2gpst(eph.tof, &week);
138 orbit->set_tk(second);
139
140 orbit->set_gnss_time_type(apollo::drivers::gnss::GnssTimeType::GPS_TIME);
141
142 int prn = 0;
143 satsys(eph.sat, &prn);
144 orbit->set_slot_prn(prn);
145}
146
147void Rtcm3Parser::SetObservationTime() {
148 int week = 0;
149 double second = time2gpst(rtcm_.time, &week);
150 observation_.set_gnss_time_type(apollo::drivers::gnss::GPS_TIME);
151 observation_.set_gnss_week(week);
152 observation_.set_gnss_second_s(second);
153}
154
156 if (data_ == nullptr) {
157 return MessageType::NONE;
158 }
159
160 while (data_ < data_end_) {
161 const int status = input_rtcm3(&rtcm_, *data_++); // parse data use rtklib
162
163 switch (status) {
164 case 1: // observation data
165 if (ProcessObservation()) {
166 *message_ptr = &observation_;
168 }
169 break;
170
171 case 2: // ephemeris
172 if (ProcessEphemerides()) {
173 *message_ptr = &ephemeris_;
175 }
176 break;
177
178 case 5:
179 ProcessStationParameters();
180 break;
181
182 case 10: // ssr messages
183 default:
184 break;
185 }
186 }
187
188 return MessageType::NONE;
189}
190
191bool Rtcm3Parser::ProcessObservation() {
192 if (rtcm_.obs.n == 0) {
193 AWARN << "Obs is zero.";
194 }
195
196 observation_.Clear();
197 SetStationPosition();
198 if (!is_base_station_) {
199 observation_.set_receiver_id(0);
200 } else {
201 observation_.set_receiver_id(rtcm_.staid + 0x80000000);
202 }
203
204 // set time
205 SetObservationTime();
206
207 // set satellite obs
208 observation_.set_sat_obs_num(rtcm_.obs.n);
209 observation_.set_health_flag(rtcm_.stah);
210
211 for (int i = 0; i < rtcm_.obs.n; ++i) {
212 int prn = 0;
213 int sys = 0;
214
215 sys = satsys(rtcm_.obs.data[i].sat, &prn);
216
218
219 // transform sys to local sys type
220 if (!gnss_sys_type(sys, &gnss_type)) {
221 return false;
222 }
223
224 auto sat_obs = observation_.add_sat_obs(); // create obj
225 sat_obs->set_sat_prn(prn);
226 sat_obs->set_sat_sys(gnss_type);
227
228 int j = 0;
229
230 for (j = 0; j < NFREQ + NEXOBS; ++j) {
231 if (is_zero(rtcm_.obs.data[i].L[j])) {
232 break;
233 }
234
236 if (!gnss_baud_id(gnss_type, j, &baud_id)) {
237 break;
238 }
239
240 auto band_obs = sat_obs->add_band_obs();
241 if (rtcm_.obs.data[i].code[i] == CODE_L1C) {
242 band_obs->set_pseudo_type(
244 } else if (rtcm_.obs.data[i].code[i] == CODE_L1P) {
245 band_obs->set_pseudo_type(
247 } else {
248 // AINFO << "Message type " << rtcm_.message_type;
249 }
250
251 band_obs->set_band_id(baud_id);
252 band_obs->set_pseudo_range(rtcm_.obs.data[i].P[j]);
253 band_obs->set_carrier_phase(rtcm_.obs.data[i].L[j]);
254 band_obs->set_loss_lock_index(rtcm_.obs.data[i].SNR[j]);
255 band_obs->set_doppler(rtcm_.obs.data[i].D[j]);
256 band_obs->set_snr(rtcm_.obs.data[i].SNR[j]);
257 }
258 sat_obs->set_band_obs_num(j);
259 }
260
261 return true;
262}
263
264bool Rtcm3Parser::ProcessEphemerides() {
266
267 if (!gnss_sys(rtcm_.message_type, &gnss_type)) {
268 AINFO << "Failed get gnss type from message type " << rtcm_.message_type;
269 return false;
270 }
271
273 gnss_time_type(gnss_type, &time_type);
274
275 AINFO << "Gnss sys " << static_cast<int>(gnss_type) << "ephemeris info.";
276
277 ephemeris_.Clear();
278 ephemeris_.set_gnss_type(gnss_type);
279
281 auto obit = ephemeris_.mutable_glonass_orbit();
282 obit->set_gnss_type(gnss_type);
283 obit->set_gnss_time_type(time_type);
284 FillGlonassOrbit(rtcm_.nav.geph[rtcm_.ephsat - 1], obit);
285 } else {
286 auto obit = ephemeris_.mutable_keppler_orbit();
287 obit->set_gnss_type(gnss_type);
288 obit->set_gnss_time_type(time_type);
289 FillKepplerOrbit(rtcm_.nav.eph[rtcm_.ephsat - 1], obit);
290 }
291
292 return true;
293}
294
295bool Rtcm3Parser::ProcessStationParameters() {
296 // station pose/ant parameters, set pose.
297
298 // update station location
299 auto iter = station_location_.find(rtcm_.staid);
300 if (iter == station_location_.end()) {
301 Point3D point;
302 AINFO << "Add pose for station id: " << rtcm_.staid;
303 point.x = rtcm_.sta.pos[0];
304 point.y = rtcm_.sta.pos[1];
305 point.z = rtcm_.sta.pos[2];
306 station_location_.insert(std::make_pair(rtcm_.staid, point));
307 } else {
308 iter->second.x = rtcm_.sta.pos[0];
309 iter->second.y = rtcm_.sta.pos[1];
310 iter->second.z = rtcm_.sta.pos[2];
311 }
312 return true;
313}
314
315} // namespace gnss
316} // namespace drivers
317} // namespace apollo
static Parser * CreateRtcmV3(bool is_base_station=false)
const uint8_t * data_
Definition parser.h:138
const uint8_t * data_end_
Definition parser.h:139
Rtcm3Parser(bool is_base_satation)
virtual MessageType GetMessage(MessagePtr *message_ptr)
#define AINFO
Definition log.h:42
#define AWARN
Definition log.h:43
::google::protobuf::Message * MessagePtr
Definition parser.h:34
class register implement
Definition arena_queue.h:37