29constexpr bool is_zero(T value) {
30 return value ==
static_cast<T
>(0);
40 if (1 != init_rtcm(&rtcm_)) {
48 is_base_station_ = is_base_station;
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.";
58 observation_.set_position_x(iter->second.x);
59 observation_.set_position_y(iter->second.y);
60 observation_.set_position_z(iter->second.z);
64void Rtcm3Parser::FillKepplerOrbit(
66 keppler_orbit->set_week_num(eph.week);
68 keppler_orbit->set_af0(eph.f0);
69 keppler_orbit->set_af1(eph.f1);
70 keppler_orbit->set_af2(eph.f2);
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);
77 keppler_orbit->set_roota(std::sqrt(eph.A));
79 keppler_orbit->set_toe(eph.toes);
80 keppler_orbit->set_toc(eph.tocs);
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);
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);
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);
103 satsys(eph.sat, &prn);
104 keppler_orbit->set_sat_prn(prn);
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]);
113 orbit->set_velocity_x(eph.vel[0]);
114 orbit->set_velocity_y(eph.vel[1]);
115 orbit->set_velocity_z(eph.vel[2]);
117 orbit->set_accelerate_x(eph.acc[0]);
118 orbit->set_accelerate_y(eph.acc[1]);
119 orbit->set_accelerate_z(eph.acc[2]);
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);
126 orbit->set_frequency_no(eph.frq);
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);
137 second = time2gpst(eph.tof, &week);
138 orbit->set_tk(second);
143 satsys(eph.sat, &prn);
144 orbit->set_slot_prn(prn);
147void Rtcm3Parser::SetObservationTime() {
149 double second = time2gpst(rtcm_.time, &week);
151 observation_.set_gnss_week(week);
152 observation_.set_gnss_second_s(second);
156 if (
data_ ==
nullptr) {
161 const int status = input_rtcm3(&rtcm_, *
data_++);
165 if (ProcessObservation()) {
166 *message_ptr = &observation_;
172 if (ProcessEphemerides()) {
173 *message_ptr = &ephemeris_;
179 ProcessStationParameters();
191bool Rtcm3Parser::ProcessObservation() {
192 if (rtcm_.obs.n == 0) {
193 AWARN <<
"Obs is zero.";
196 observation_.Clear();
197 SetStationPosition();
198 if (!is_base_station_) {
199 observation_.set_receiver_id(0);
201 observation_.set_receiver_id(rtcm_.staid + 0x80000000);
205 SetObservationTime();
208 observation_.set_sat_obs_num(rtcm_.obs.n);
209 observation_.set_health_flag(rtcm_.stah);
211 for (
int i = 0; i < rtcm_.obs.n; ++i) {
215 sys = satsys(rtcm_.obs.data[i].sat, &prn);
220 if (!gnss_sys_type(sys, &gnss_type)) {
224 auto sat_obs = observation_.add_sat_obs();
225 sat_obs->set_sat_prn(prn);
226 sat_obs->set_sat_sys(gnss_type);
230 for (j = 0; j < NFREQ + NEXOBS; ++j) {
231 if (is_zero(rtcm_.obs.data[i].L[j])) {
236 if (!gnss_baud_id(gnss_type, j, &baud_id)) {
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(
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]);
258 sat_obs->set_band_obs_num(j);
264bool Rtcm3Parser::ProcessEphemerides() {
267 if (!gnss_sys(rtcm_.message_type, &gnss_type)) {
268 AINFO <<
"Failed get gnss type from message type " << rtcm_.message_type;
273 gnss_time_type(gnss_type, &time_type);
275 AINFO <<
"Gnss sys " <<
static_cast<int>(gnss_type) <<
"ephemeris info.";
278 ephemeris_.set_gnss_type(gnss_type);
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);
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);
295bool Rtcm3Parser::ProcessStationParameters() {
299 auto iter = station_location_.find(rtcm_.staid);
300 if (iter == station_location_.end()) {
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));
308 iter->second.x = rtcm_.sta.pos[0];
309 iter->second.y = rtcm_.sta.pos[1];
310 iter->second.z = rtcm_.sta.pos[2];
static Parser * CreateRtcmV3(bool is_base_station=false)
const uint8_t * data_end_
Rtcm3Parser(bool is_base_satation)
virtual MessageType GetMessage(MessagePtr *message_ptr)
::google::protobuf::Message * MessagePtr