Apollo 10.0
自动驾驶开放平台
novatel_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 <iostream>
23#include <limits>
24#include <memory>
25#include <vector>
26
27#include "modules/common_msgs/sensor_msgs/gnss.pb.h"
28#include "modules/common_msgs/sensor_msgs/gnss_best_pose.pb.h"
29#include "modules/common_msgs/sensor_msgs/gnss_raw_observation.pb.h"
30#include "modules/common_msgs/sensor_msgs/heading.pb.h"
31#include "modules/common_msgs/sensor_msgs/imu.pb.h"
32#include "modules/common_msgs/sensor_msgs/ins.pb.h"
33
34#include "cyber/cyber.h"
40
41namespace apollo {
42namespace drivers {
43namespace gnss {
44
45// Anonymous namespace that contains helper constants and functions.
46namespace {
47
48constexpr size_t BUFFER_SIZE = 256;
49
50// The NovAtel's orientation covariance matrix is pitch, roll, and yaw. We use
51// the index array below
52// to convert it to the orientation covariance matrix with order roll, pitch,
53// and yaw.
54constexpr int INDEX[] = {4, 3, 5, 1, 0, 2, 7, 6, 8};
55static_assert(sizeof(INDEX) == 9 * sizeof(int), "Incorrect size of INDEX");
56
57template <typename T>
58constexpr bool is_zero(T value) {
59 return value == static_cast<T>(0);
60}
61
62// CRC algorithm from the NovAtel document.
63inline uint32_t crc32_word(uint32_t word) {
64 for (int j = 0; j < 8; ++j) {
65 if (word & 1) {
66 word = (word >> 1) ^ 0xEDB88320;
67 } else {
68 word >>= 1;
69 }
70 }
71 return word;
72}
73
74inline uint32_t crc32_block(const uint8_t* buffer, size_t length) {
75 uint32_t word = 0;
76 while (length--) {
77 uint32_t t1 = (word >> 8) & 0xFFFFFF;
78 uint32_t t2 = crc32_word((word ^ *buffer++) & 0xFF);
79 word = t1 ^ t2;
80 }
81 return word;
82}
83
84} // namespace
85
86class NovatelParser : public Parser {
87 public:
89 explicit NovatelParser(const config::Config& config);
90
91 virtual MessageType GetMessage(MessagePtr* message_ptr);
92
93 virtual void GetMessages(MessageInfoVec* messages);
94
95 virtual bool GetInsStat(MessagePtr* message_ptr);
96
97 private:
98 bool check_crc();
99
100 MessageType PrepareMessage(MessagePtr* message_ptr);
101
102 // The handle_xxx functions return whether a message is ready.
103 bool HandleBestPos(const novatel::BestPos* pos, uint16_t gps_week,
104 uint32_t gps_millisecs);
105
106 bool HandleGnssBestpos(const novatel::BestPos* pos, uint16_t gps_week,
107 uint32_t gps_millisecs);
108
109 bool HandleBestVel(const novatel::BestVel* vel, uint16_t gps_week,
110 uint32_t gps_millisecs);
111
112 bool HandleCorrImuData(const novatel::CorrImuData* imu);
113
114 bool HandleInsCov(const novatel::InsCov* cov);
115
116 bool HandleInsPva(const novatel::InsPva* pva);
117
118 bool HandleInsPvax(const novatel::InsPvaX* pvax, uint16_t gps_week,
119 uint32_t gps_millisecs);
120
121 bool HandleRawImuX(const novatel::RawImuX* imu);
122
123 bool HandleRawImu(const novatel::RawImu* imu);
124
125 bool HandleBdsEph(const novatel::BDS_Ephemeris* bds_emph);
126
127 bool HandleGpsEph(const novatel::GPS_Ephemeris* gps_emph);
128
129 bool HandleGloEph(const novatel::GLO_Ephemeris* glo_emph);
130
131 void SetObservationTime();
132
133 bool DecodeGnssObservation(const uint8_t* obs_data,
134 const uint8_t* obs_data_end);
135
136 bool HandleHeading(const novatel::Heading* heading, uint16_t gps_week,
137 uint32_t gps_millisecs);
138
139 double gyro_scale_ = 0.0;
140
141 double accel_scale_ = 0.0;
142
143 float imu_measurement_span_ = 1.0f / 200.0f;
144 float imu_measurement_hz_ = 200.0f;
145
146 int imu_frame_mapping_ = 5;
147
148 double imu_measurement_time_previous_ = -1.0;
149
150 std::vector<uint8_t> buffer_;
151
152 size_t header_length_ = 0;
153
154 size_t total_length_ = 0;
155
157
158 // -1 is an unused value.
159 novatel::SolutionStatus solution_status_ =
161 novatel::SolutionType position_type_ =
163 novatel::SolutionType velocity_type_ =
165 novatel::InsStatus ins_status_ =
167
168 raw_t raw_; // used for observation data
169 bool has_ins_stat_message_ = false;
170 bool has_corr_imu_message_ = false;
171
180};
181
183 return new NovatelParser(config);
184}
185
187 buffer_.reserve(BUFFER_SIZE);
188 ins_.mutable_position_covariance()->Resize(9, FLOAT_NAN);
189 ins_.mutable_euler_angles_covariance()->Resize(9, FLOAT_NAN);
190 ins_.mutable_linear_velocity_covariance()->Resize(9, FLOAT_NAN);
191
192 if (1 != init_raw(&raw_)) {
193 AFATAL << "memory allocation error for observation data structure.";
194 }
195}
196
198 buffer_.reserve(BUFFER_SIZE);
199 ins_.mutable_position_covariance()->Resize(9, FLOAT_NAN);
200 ins_.mutable_euler_angles_covariance()->Resize(9, FLOAT_NAN);
201 ins_.mutable_linear_velocity_covariance()->Resize(9, FLOAT_NAN);
202
203 if (config.has_imu_type()) {
204 imu_type_ = config.imu_type();
205 }
206
207 if (1 != init_raw(&raw_)) {
208 AFATAL << "memory allocation error for observation data structure.";
209 }
210}
211
213 if (data_ == nullptr) {
214 return MessageType::NONE;
215 }
216
217 while (data_ < data_end_) {
218 if (buffer_.empty()) { // Looking for SYNC0
219 if (*data_ == novatel::SYNC_0) {
220 buffer_.push_back(*data_);
221 }
222 ++data_;
223 } else if (buffer_.size() == 1) { // Looking for SYNC1
224 if (*data_ == novatel::SYNC_1) {
225 buffer_.push_back(*data_++);
226 } else {
227 buffer_.clear();
228 }
229 } else if (buffer_.size() == 2) { // Looking for SYNC2
230 switch (*data_) {
232 buffer_.push_back(*data_++);
233 header_length_ = sizeof(novatel::LongHeader);
234 break;
236 buffer_.push_back(*data_++);
237 header_length_ = sizeof(novatel::ShortHeader);
238 break;
239 default:
240 buffer_.clear();
241 }
242 } else if (header_length_ > 0) { // Working on header.
243 if (buffer_.size() < header_length_) {
244 buffer_.push_back(*data_++);
245 } else {
246 if (header_length_ == sizeof(novatel::LongHeader)) {
247 total_length_ = header_length_ + novatel::CRC_LENGTH +
248 reinterpret_cast<novatel::LongHeader*>(buffer_.data())
249 ->message_length;
250 } else if (header_length_ == sizeof(novatel::ShortHeader)) {
251 total_length_ =
252 header_length_ + novatel::CRC_LENGTH +
253 reinterpret_cast<novatel::ShortHeader*>(buffer_.data())
254 ->message_length;
255 } else {
256 AERROR << "Incorrect header_length_. Should never reach here.";
257 buffer_.clear();
258 }
259 header_length_ = 0;
260 }
261 } else if (total_length_ > 0) {
262 if (buffer_.size() < total_length_) { // Working on body.
263 buffer_.push_back(*data_++);
264 continue;
265 }
266 MessageType type = PrepareMessage(message_ptr);
267 buffer_.clear();
268 total_length_ = 0;
269 if (type != MessageType::NONE) {
270 return type;
271 }
272 }
273 }
274 return MessageType::NONE;
275}
276
278 std::set<MessageType> s;
279 while (cyber::OK()) {
280 MessageInfo message_info;
281 message_info.type = GetMessage(&message_info.message_ptr);
282 if (message_info.type == MessageType::NONE) {
283 break;
284 }
285 if (s.find(message_info.type) == s.end()) {
286 messages->push_back(std::move(message_info));
287 s.insert(message_info.type);
288 }
289 }
290}
291
292bool NovatelParser::check_crc() {
293 size_t l = buffer_.size() - novatel::CRC_LENGTH;
294 return crc32_block(buffer_.data(), l) ==
295 *reinterpret_cast<uint32_t*>(buffer_.data() + l);
296}
297
298MessageType NovatelParser::PrepareMessage(MessagePtr* message_ptr) {
299 if (!check_crc()) {
300 AERROR << "CRC check failed.";
301 return MessageType::NONE;
302 }
303
304 uint8_t* message = nullptr;
305 novatel::MessageId message_id;
306 uint16_t message_length;
307 uint16_t gps_week;
308 uint32_t gps_millisecs;
309 if (buffer_[2] == novatel::SYNC_2_LONG_HEADER) {
310 auto header = reinterpret_cast<const novatel::LongHeader*>(buffer_.data());
311 message = buffer_.data() + sizeof(novatel::LongHeader);
312 gps_week = header->gps_week;
313 gps_millisecs = header->gps_millisecs;
314 message_id = header->message_id;
315 message_length = header->message_length;
316 } else {
317 auto header = reinterpret_cast<const novatel::ShortHeader*>(buffer_.data());
318 message = buffer_.data() + sizeof(novatel::ShortHeader);
319 gps_week = header->gps_week;
320 gps_millisecs = header->gps_millisecs;
321 message_id = header->message_id;
322 message_length = header->message_length;
323 }
324 switch (message_id) {
326 if (message_length != sizeof(novatel::BestPos)) {
327 AERROR << "Incorrect message_length";
328 break;
329 }
330 if (HandleGnssBestpos(reinterpret_cast<novatel::BestPos*>(message),
331 gps_week, gps_millisecs)) {
332 *message_ptr = &bestpos_;
334 }
335 break;
336
337 case novatel::BESTPOS:
338 case novatel::PSRPOS:
339 if (message_length != sizeof(novatel::BestPos)) {
340 AERROR << "Incorrect message_length";
341 break;
342 }
343 if (HandleBestPos(reinterpret_cast<novatel::BestPos*>(message), gps_week,
344 gps_millisecs)) {
345 *message_ptr = &gnss_;
346 return MessageType::GNSS;
347 }
348 break;
349
351 case novatel::BESTVEL:
352 case novatel::PSRVEL:
353 if (message_length != sizeof(novatel::BestVel)) {
354 AERROR << "Incorrect message_length";
355 break;
356 }
357 if (HandleBestVel(reinterpret_cast<novatel::BestVel*>(message), gps_week,
358 gps_millisecs)) {
359 *message_ptr = &gnss_;
360 return MessageType::GNSS;
361 }
362 break;
363
367 if (message_length != sizeof(novatel::CorrImuData)) {
368 AERROR << "Incorrect message_length";
369 break;
370 }
371 has_corr_imu_message_ = true;
372 if (HandleCorrImuData(reinterpret_cast<novatel::CorrImuData*>(message))) {
373 *message_ptr = &ins_;
374 return MessageType::INS;
375 }
376 break;
377
378 case novatel::INSCOV:
379 case novatel::INSCOVS:
380 if (message_length != sizeof(novatel::InsCov)) {
381 AERROR << "Incorrect message_length";
382 break;
383 }
384
385 if (HandleInsCov(reinterpret_cast<novatel::InsCov*>(message))) {
386 *message_ptr = &ins_;
387 return MessageType::INS;
388 }
389 break;
390
391 case novatel::INSPVA:
392 case novatel::INSPVAS:
393 if (message_length != sizeof(novatel::InsPva)) {
394 AERROR << "Incorrect message_length";
395 break;
396 }
397
398 if (HandleInsPva(reinterpret_cast<novatel::InsPva*>(message))) {
399 *message_ptr = &ins_;
400 return MessageType::INS;
401 }
402 break;
403
404 case novatel::RAWIMUX:
406 if (message_length != sizeof(novatel::RawImuX)) {
407 AERROR << "Incorrect message_length";
408 break;
409 }
410
411 if (HandleRawImuX(reinterpret_cast<novatel::RawImuX*>(message))) {
412 *message_ptr = &imu_;
413 return MessageType::IMU;
414 }
415 break;
416
417 case novatel::RAWIMU:
418 case novatel::RAWIMUS:
419 if (message_length != sizeof(novatel::RawImu)) {
420 AERROR << "Incorrect message_length";
421 break;
422 }
423
424 if (HandleRawImu(reinterpret_cast<novatel::RawImu*>(message))) {
425 *message_ptr = &imu_;
426 return MessageType::IMU;
427 }
428 break;
429
430 case novatel::INSPVAX:
431 if (message_length != sizeof(novatel::InsPvaX)) {
432 AERROR << "Incorrect message_length";
433 break;
434 }
435 has_ins_stat_message_ = true;
436 if (HandleInsPvax(reinterpret_cast<novatel::InsPvaX*>(message), gps_week,
437 gps_millisecs)) {
438 *message_ptr = &ins_stat_;
440 }
441 break;
442
444 if (message_length != sizeof(novatel::BDS_Ephemeris)) {
445 AERROR << "Incorrect BDSEPHEMERIS message_length";
446 break;
447 }
448 if (HandleBdsEph(reinterpret_cast<novatel::BDS_Ephemeris*>(message))) {
449 *message_ptr = &gnss_ephemeris_;
451 }
452 break;
453
455 if (message_length != sizeof(novatel::GPS_Ephemeris)) {
456 AERROR << "Incorrect GPSEPHEMERIS message_length";
457 break;
458 }
459 if (HandleGpsEph(reinterpret_cast<novatel::GPS_Ephemeris*>(message))) {
460 *message_ptr = &gnss_ephemeris_;
462 }
463 break;
464
466 if (message_length != sizeof(novatel::GLO_Ephemeris)) {
467 AERROR << "Incorrect GLOEPHEMERIS message length";
468 break;
469 }
470 if (HandleGloEph(reinterpret_cast<novatel::GLO_Ephemeris*>(message))) {
471 *message_ptr = &gnss_ephemeris_;
473 }
474 break;
475
476 case novatel::RANGE:
477 if (DecodeGnssObservation(buffer_.data(),
478 buffer_.data() + buffer_.size())) {
479 *message_ptr = &gnss_observation_;
481 }
482 break;
483
484 case novatel::HEADING:
485 if (message_length != sizeof(novatel::Heading)) {
486 AERROR << "Incorrect message_length";
487 break;
488 }
489 if (HandleHeading(reinterpret_cast<novatel::Heading*>(message), gps_week,
490 gps_millisecs)) {
491 *message_ptr = &heading_;
493 }
494 break;
495
496 default:
497 break;
498 }
499 return MessageType::NONE;
500}
501
502bool NovatelParser::HandleGnssBestpos(const novatel::BestPos* pos,
503 uint16_t gps_week,
504 uint32_t gps_millisecs) {
505 bestpos_.set_sol_status(
506 static_cast<apollo::drivers::gnss::SolutionStatus>(pos->solution_status));
507 bestpos_.set_sol_type(
508 static_cast<apollo::drivers::gnss::SolutionType>(pos->position_type));
509
510 // if not has innspvax message, set ins stat from gnss best pose message
511 if (!has_ins_stat_message_) {
512 ins_stat_.set_ins_status(static_cast<uint32_t>(pos->solution_status));
513 ins_stat_.set_pos_type(static_cast<uint32_t>(pos->position_type));
514 }
515
516 bestpos_.set_latitude(pos->latitude);
517 bestpos_.set_longitude(pos->longitude);
518 bestpos_.set_height_msl(pos->height_msl);
519 bestpos_.set_undulation(pos->undulation);
520 bestpos_.set_datum_id(
521 static_cast<apollo::drivers::gnss::DatumId>(pos->datum_id));
522 bestpos_.set_latitude_std_dev(pos->latitude_std_dev);
523 bestpos_.set_longitude_std_dev(pos->longitude_std_dev);
524 bestpos_.set_height_std_dev(pos->height_std_dev);
525 bestpos_.set_base_station_id(pos->base_station_id);
526 bestpos_.set_differential_age(pos->differential_age);
527 bestpos_.set_solution_age(pos->solution_age);
528 bestpos_.set_num_sats_tracked(pos->num_sats_tracked);
529 bestpos_.set_num_sats_in_solution(pos->num_sats_in_solution);
530 bestpos_.set_num_sats_l1(pos->num_sats_l1);
531 bestpos_.set_num_sats_multi(pos->num_sats_multi);
532 bestpos_.set_extended_solution_status(pos->extended_solution_status);
533 bestpos_.set_galileo_beidou_used_mask(pos->galileo_beidou_used_mask);
534 bestpos_.set_gps_glonass_used_mask(pos->gps_glonass_used_mask);
535
536 double seconds = gps_week * SECONDS_PER_WEEK + gps_millisecs * 1e-3;
537 bestpos_.set_measurement_time(seconds);
538 // AINFO << "Best gnss pose:\r\n" << bestpos_.DebugString();
539 return true;
540}
541
542bool NovatelParser::HandleBestPos(const novatel::BestPos* pos,
543 uint16_t gps_week, uint32_t gps_millisecs) {
544 gnss_.mutable_position()->set_lon(pos->longitude);
545 gnss_.mutable_position()->set_lat(pos->latitude);
546 gnss_.mutable_position()->set_height(pos->height_msl + pos->undulation);
547 gnss_.mutable_position_std_dev()->set_x(pos->longitude_std_dev);
548 gnss_.mutable_position_std_dev()->set_y(pos->latitude_std_dev);
549 gnss_.mutable_position_std_dev()->set_z(pos->height_std_dev);
550 gnss_.set_num_sats(pos->num_sats_in_solution);
551 if (solution_status_ != pos->solution_status) {
552 solution_status_ = pos->solution_status;
553 AINFO << "Solution status: " << static_cast<int>(solution_status_);
554 }
555 if (position_type_ != pos->position_type) {
556 position_type_ = pos->position_type;
557 AINFO << "Position type: " << static_cast<int>(position_type_);
558 }
559 gnss_.set_solution_status(static_cast<uint32_t>(pos->solution_status));
560 if (pos->solution_status == novatel::SolutionStatus::SOL_COMPUTED) {
561 gnss_.set_position_type(static_cast<uint32_t>(pos->position_type));
562 switch (pos->position_type) {
566 break;
571 break;
579 break;
587 break;
598 gnss_.set_type(apollo::drivers::gnss::Gnss::PPP);
599 break;
602 break;
603 default:
605 }
606 } else {
608 gnss_.set_position_type(0);
609 }
610
611 // if not has innspvax message, set ins stat from best pose message
612 if (!has_ins_stat_message_) {
613 ins_stat_.set_ins_status(static_cast<uint32_t>(pos->solution_status));
614 ins_stat_.set_pos_type(static_cast<uint32_t>(pos->position_type));
615 }
616
617 if (pos->datum_id != novatel::DatumId::WGS84) {
618 AERROR_EVERY(5) << "Unexpected Datum Id: "
619 << static_cast<int>(pos->datum_id);
620 }
621
622 double seconds = gps_week * SECONDS_PER_WEEK + gps_millisecs * 1e-3;
623 if (gnss_.measurement_time() != seconds) {
624 gnss_.set_measurement_time(seconds);
625 return false;
626 }
627 return true;
628}
629
630bool NovatelParser::HandleBestVel(const novatel::BestVel* vel,
631 uint16_t gps_week, uint32_t gps_millisecs) {
632 if (velocity_type_ != vel->velocity_type) {
633 velocity_type_ = vel->velocity_type;
634 AINFO << "Velocity type: " << static_cast<int>(velocity_type_);
635 }
636 if (!gnss_.has_velocity_latency() ||
637 gnss_.velocity_latency() != vel->latency) {
638 AINFO << "Velocity latency: " << static_cast<int>(vel->latency);
639 gnss_.set_velocity_latency(vel->latency);
640 }
641 double yaw = azimuth_deg_to_yaw_rad(vel->track_over_ground);
642 gnss_.mutable_linear_velocity()->set_x(vel->horizontal_speed * cos(yaw));
643 gnss_.mutable_linear_velocity()->set_y(vel->horizontal_speed * sin(yaw));
644 gnss_.mutable_linear_velocity()->set_z(vel->vertical_speed);
645
646 double seconds = gps_week * SECONDS_PER_WEEK + gps_millisecs * 1e-3;
647 if (gnss_.measurement_time() != seconds) {
648 gnss_.set_measurement_time(seconds);
649 return false;
650 }
651 return true;
652}
653
654bool NovatelParser::HandleCorrImuData(const novatel::CorrImuData* imu) {
655 rfu_to_flu(imu->x_velocity_change * imu_measurement_hz_,
656 imu->y_velocity_change * imu_measurement_hz_,
657 imu->z_velocity_change * imu_measurement_hz_ + 9.8,
658 ins_.mutable_linear_acceleration());
659 rfu_to_flu(imu->x_angle_change * imu_measurement_hz_,
660 imu->y_angle_change * imu_measurement_hz_,
661 imu->z_angle_change * imu_measurement_hz_,
662 ins_.mutable_angular_velocity());
663
664 double seconds = imu->gps_week * SECONDS_PER_WEEK + imu->gps_seconds;
665 if (ins_.measurement_time() != seconds) {
666 ins_.set_measurement_time(seconds);
667 return false;
668 }
669
670 ins_.mutable_header()->set_timestamp_sec(cyber::Time::Now().ToSecond());
671 return true;
672}
673
674bool NovatelParser::HandleInsCov(const novatel::InsCov* cov) {
675 for (int i = 0; i < 9; ++i) {
676 ins_.set_position_covariance(
677 i, static_cast<float>(cov->position_covariance[i]));
678 ins_.set_euler_angles_covariance(
679 INDEX[i], static_cast<float>((DEG_TO_RAD * DEG_TO_RAD) *
680 cov->attitude_covariance[i]));
681 ins_.set_linear_velocity_covariance(
682 i, static_cast<float>(cov->velocity_covariance[i]));
683 }
684 return false;
685}
686
687bool NovatelParser::HandleInsPva(const novatel::InsPva* pva) {
688 if (ins_status_ != pva->status) {
689 ins_status_ = pva->status;
690 AINFO << "INS status: " << static_cast<int>(ins_status_);
691 }
692 ins_.mutable_position()->set_lon(pva->longitude);
693 ins_.mutable_position()->set_lat(pva->latitude);
694 ins_.mutable_position()->set_height(pva->height);
695 ins_.mutable_euler_angles()->set_x(pva->roll * DEG_TO_RAD);
696 ins_.mutable_euler_angles()->set_y(-pva->pitch * DEG_TO_RAD);
697 ins_.mutable_euler_angles()->set_z(azimuth_deg_to_yaw_rad(pva->azimuth));
698 ins_.mutable_linear_velocity()->set_x(pva->east_velocity);
699 ins_.mutable_linear_velocity()->set_y(pva->north_velocity);
700 ins_.mutable_linear_velocity()->set_z(pva->up_velocity);
701
702 switch (pva->status) {
706 break;
711 break;
712 default:
714 }
715
716 double seconds = pva->gps_week * SECONDS_PER_WEEK + pva->gps_seconds;
717 if (has_corr_imu_message_ &&
718 std::abs(ins_.measurement_time() - seconds) > EPS) {
719 ins_.set_measurement_time(seconds);
720 return false;
721 }
722 ins_.set_measurement_time(seconds);
723
724 auto now = cyber::Time::Now().ToSecond();
725 ins_.mutable_header()->set_timestamp_sec(now);
726 if (!has_ins_stat_message_) {
727 ins_stat_.mutable_header()->set_timestamp_sec(now);
728 }
729 return true;
730}
731
733 *message_ptr = &ins_stat_;
734 return true;
735}
736
737bool NovatelParser::HandleInsPvax(const novatel::InsPvaX* pvax,
738 uint16_t gps_week, uint32_t gps_millisecs) {
739 double seconds = gps_week * SECONDS_PER_WEEK + gps_millisecs * 1e-3;
740 double unix_sec = apollo::drivers::util::gps2unix(seconds);
741 ins_stat_.mutable_header()->set_timestamp_sec(unix_sec);
742 ins_stat_.set_ins_status(pvax->ins_status);
743 ins_stat_.set_pos_type(pvax->pos_type);
744 return true;
745}
746
747bool NovatelParser::HandleRawImuX(const novatel::RawImuX* imu) {
748 if (imu->imu_error != 0) {
749 AWARN << "IMU error. Status: " << std::hex << std::showbase
750 << imu->imuStatus;
751 }
752 if (is_zero(gyro_scale_)) {
753 config::ImuType imu_type = imu_type_;
754 novatel::ImuParameter param = novatel::GetImuParameter(imu_type);
755 AINFO << "IMU type: " << config::ImuType_Name(imu_type) << "; "
756 << "Gyro scale: " << param.gyro_scale << "; "
757 << "Accel scale: " << param.accel_scale << "; "
758 << "Sampling rate: " << param.sampling_rate_hz << ".";
759
760 if (is_zero(param.sampling_rate_hz)) {
761 AERROR_EVERY(5) << "Unsupported IMU type: "
762 << config::ImuType_Name(imu_type);
763 return false;
764 }
765 gyro_scale_ = param.gyro_scale * param.sampling_rate_hz;
766 accel_scale_ = param.accel_scale * param.sampling_rate_hz;
767 imu_measurement_hz_ = static_cast<float>(param.sampling_rate_hz);
768 imu_measurement_span_ = static_cast<float>(1.0 / param.sampling_rate_hz);
769 imu_.set_measurement_span(imu_measurement_span_);
770 }
771
772 double time = imu->gps_week * SECONDS_PER_WEEK + imu->gps_seconds;
773 if (imu_measurement_time_previous_ > 0.0 &&
774 fabs(time - imu_measurement_time_previous_ - imu_measurement_span_) >
775 1e-4) {
776 AWARN_EVERY(5) << "Unexpected delay between two IMU measurements at: "
777 << time - imu_measurement_time_previous_;
778 }
779 imu_.set_measurement_time(time);
780 switch (imu_frame_mapping_) {
781 case 5: // Default mapping.
782 rfu_to_flu(imu->x_velocity_change * accel_scale_,
783 -imu->y_velocity_change_neg * accel_scale_,
784 imu->z_velocity_change * accel_scale_,
785 imu_.mutable_linear_acceleration());
786 rfu_to_flu(imu->x_angle_change * gyro_scale_,
787 -imu->y_angle_change_neg * gyro_scale_,
788 imu->z_angle_change * gyro_scale_,
789 imu_.mutable_angular_velocity());
790 break;
791 case 6:
792 rfu_to_flu(-imu->y_velocity_change_neg * accel_scale_,
793 imu->x_velocity_change * accel_scale_,
794 -imu->z_velocity_change * accel_scale_,
795 imu_.mutable_linear_acceleration());
796 rfu_to_flu(-imu->y_angle_change_neg * gyro_scale_,
797 imu->x_angle_change * gyro_scale_,
798 -imu->z_angle_change * gyro_scale_,
799 imu_.mutable_angular_velocity());
800 break;
801 default:
802 AERROR_EVERY(5) << "Unsupported IMU frame mapping: "
803 << imu_frame_mapping_;
804 }
805 imu_measurement_time_previous_ = time;
806 return true;
807}
808
809bool NovatelParser::HandleRawImu(const novatel::RawImu* imu) {
810 double gyro_scale = 0.0;
811 double accel_scale = 0.0;
812 float imu_measurement_span = 1.0f / 200.0f;
813
814 if (is_zero(gyro_scale_)) {
815 novatel::ImuParameter param = novatel::GetImuParameter(imu_type_);
816
817 if (is_zero(param.sampling_rate_hz)) {
818 AERROR_EVERY(5) << "Unsupported IMU type ADUS16488.";
819 return false;
820 }
821 gyro_scale = param.gyro_scale * param.sampling_rate_hz;
822 accel_scale = param.accel_scale * param.sampling_rate_hz;
823 imu_measurement_span = static_cast<float>(1.0 / param.sampling_rate_hz);
824 imu_.set_measurement_span(imu_measurement_span);
825 } else {
826 gyro_scale = gyro_scale_;
827 accel_scale = accel_scale_;
828 imu_measurement_span = imu_measurement_span_;
829 imu_.set_measurement_span(imu_measurement_span);
830 }
831
832 double time = imu->gps_week * SECONDS_PER_WEEK + imu->gps_seconds;
833 if (imu_measurement_time_previous_ > 0.0 &&
834 fabs(time - imu_measurement_time_previous_ - imu_measurement_span) >
835 1e-4) {
836 AWARN << "Unexpected delay between two IMU measurements at: "
837 << time - imu_measurement_time_previous_;
838 }
839
840 imu_.set_measurement_time(time);
841 switch (imu_frame_mapping_) {
842 case 5: // Default mapping.
843 rfu_to_flu(imu->x_velocity_change * accel_scale,
844 -imu->y_velocity_change_neg * accel_scale,
845 imu->z_velocity_change * accel_scale,
846 imu_.mutable_linear_acceleration());
847 rfu_to_flu(imu->x_angle_change * gyro_scale,
848 -imu->y_angle_change_neg * gyro_scale,
849 imu->z_angle_change * gyro_scale,
850 imu_.mutable_angular_velocity());
851
852 // if not has corr imu message, filled it by imu.
853 if (!has_corr_imu_message_) {
854 rfu_to_flu(imu->x_velocity_change * accel_scale,
855 -imu->y_velocity_change_neg * accel_scale,
856 imu->z_velocity_change * accel_scale,
857 ins_.mutable_linear_acceleration());
858 rfu_to_flu(imu->x_angle_change * gyro_scale,
859 -imu->y_angle_change_neg * gyro_scale,
860 imu->z_angle_change * gyro_scale,
861 ins_.mutable_angular_velocity());
862 }
863 break;
864 case 6:
865 rfu_to_flu(-imu->y_velocity_change_neg * accel_scale,
866 imu->x_velocity_change * accel_scale,
867 -imu->z_velocity_change * accel_scale,
868 imu_.mutable_linear_acceleration());
869 rfu_to_flu(-imu->y_angle_change_neg * gyro_scale,
870 imu->x_angle_change * gyro_scale,
871 -imu->z_angle_change * gyro_scale,
872 imu_.mutable_angular_velocity());
873
874 // if not has corr imu message, filled by imu.
875 if (!has_corr_imu_message_) {
876 rfu_to_flu(-imu->y_velocity_change_neg * accel_scale,
877 imu->x_velocity_change * accel_scale,
878 -imu->z_velocity_change * accel_scale,
879 ins_.mutable_linear_acceleration());
880 rfu_to_flu(-imu->y_velocity_change_neg * accel_scale,
881 imu->x_velocity_change * accel_scale,
882 -imu->z_velocity_change * accel_scale,
883 ins_.mutable_angular_velocity());
884 }
885 break;
886 default:
887 AERROR_EVERY(5) << "Unsupported IMU frame mapping: "
888 << imu_frame_mapping_;
889 }
890 imu_measurement_time_previous_ = time;
891 return true;
892}
893
894bool NovatelParser::HandleGpsEph(const novatel::GPS_Ephemeris* gps_emph) {
895 gnss_ephemeris_.set_gnss_type(apollo::drivers::gnss::GnssType::GPS_SYS);
896
898 gnss_ephemeris_.mutable_keppler_orbit();
899
900 keppler_orbit->set_gnss_type(apollo::drivers::gnss::GnssType::GPS_SYS);
901 keppler_orbit->set_gnss_time_type(
903 keppler_orbit->set_sat_prn(gps_emph->prn);
904 keppler_orbit->set_week_num(gps_emph->week);
905 keppler_orbit->set_af0(gps_emph->af0);
906 keppler_orbit->set_af1(gps_emph->af1);
907 keppler_orbit->set_af2(gps_emph->af2);
908 keppler_orbit->set_iode(gps_emph->iode1);
909 keppler_orbit->set_deltan(gps_emph->delta_A);
910 keppler_orbit->set_m0(gps_emph->M_0);
911 keppler_orbit->set_e(gps_emph->ecc);
912 keppler_orbit->set_roota(sqrt(gps_emph->A));
913 keppler_orbit->set_toe(gps_emph->toe);
914 keppler_orbit->set_toc(gps_emph->toc);
915 keppler_orbit->set_cic(gps_emph->cic);
916 keppler_orbit->set_crc(gps_emph->crc);
917 keppler_orbit->set_cis(gps_emph->cis);
918 keppler_orbit->set_crs(gps_emph->crs);
919 keppler_orbit->set_cuc(gps_emph->cuc);
920 keppler_orbit->set_cus(gps_emph->cus);
921 keppler_orbit->set_omega0(gps_emph->omega_0);
922 keppler_orbit->set_omega(gps_emph->omega);
923 keppler_orbit->set_i0(gps_emph->I_0);
924 keppler_orbit->set_omegadot(gps_emph->dot_omega);
925 keppler_orbit->set_idot(gps_emph->dot_I);
926 keppler_orbit->set_accuracy(static_cast<unsigned int>(sqrt(gps_emph->ura)));
927 keppler_orbit->set_health(gps_emph->health);
928 keppler_orbit->set_tgd(gps_emph->tgd);
929 keppler_orbit->set_iodc(gps_emph->iodc);
930 return true;
931}
932
933bool NovatelParser::HandleBdsEph(const novatel::BDS_Ephemeris* bds_emph) {
934 gnss_ephemeris_.set_gnss_type(apollo::drivers::gnss::GnssType::BDS_SYS);
935
937 gnss_ephemeris_.mutable_keppler_orbit();
938
939 keppler_orbit->set_gnss_type(apollo::drivers::gnss::GnssType::BDS_SYS);
940 keppler_orbit->set_gnss_time_type(
942 keppler_orbit->set_sat_prn(bds_emph->satellite_id);
943 keppler_orbit->set_week_num(bds_emph->week);
944 keppler_orbit->set_af0(bds_emph->a0);
945 keppler_orbit->set_af1(bds_emph->a1);
946 keppler_orbit->set_af2(bds_emph->a2);
947 keppler_orbit->set_iode(bds_emph->aode);
948 keppler_orbit->set_deltan(bds_emph->delta_N);
949 keppler_orbit->set_m0(bds_emph->M0);
950 keppler_orbit->set_e(bds_emph->ecc);
951 keppler_orbit->set_roota(bds_emph->rootA);
952 keppler_orbit->set_toe(bds_emph->toe);
953 keppler_orbit->set_toc(bds_emph->toc);
954 keppler_orbit->set_cic(bds_emph->cic);
955 keppler_orbit->set_crc(bds_emph->crc);
956 keppler_orbit->set_cis(bds_emph->cis);
957 keppler_orbit->set_crs(bds_emph->crs);
958 keppler_orbit->set_cuc(bds_emph->cuc);
959 keppler_orbit->set_cus(bds_emph->cus);
960 keppler_orbit->set_omega0(bds_emph->omega0);
961 keppler_orbit->set_omega(bds_emph->omega);
962 keppler_orbit->set_i0(bds_emph->inc_angle);
963 keppler_orbit->set_omegadot(bds_emph->rra);
964 keppler_orbit->set_idot(bds_emph->idot);
965 keppler_orbit->set_accuracy(static_cast<unsigned int>(bds_emph->ura));
966 keppler_orbit->set_health(bds_emph->health1);
967 keppler_orbit->set_tgd(bds_emph->tdg1);
968 keppler_orbit->set_iodc(bds_emph->aodc);
969 return true;
970}
971
972bool NovatelParser::HandleGloEph(const novatel::GLO_Ephemeris* glo_emph) {
973 gnss_ephemeris_.set_gnss_type(apollo::drivers::gnss::GnssType::GLO_SYS);
974
976 gnss_ephemeris_.mutable_glonass_orbit();
977 glonass_orbit->set_gnss_type(apollo::drivers::gnss::GnssType::GLO_SYS);
978 glonass_orbit->set_gnss_time_type(
980 glonass_orbit->set_slot_prn(glo_emph->sloto - 37);
981 glonass_orbit->set_toe(glo_emph->e_time / 1000);
982 glonass_orbit->set_frequency_no(glo_emph->freqo - 7);
983 glonass_orbit->set_week_num(glo_emph->e_week);
984 glonass_orbit->set_week_second_s(glo_emph->e_time / 1000);
985 glonass_orbit->set_tk(glo_emph->Tk);
986 glonass_orbit->set_clock_offset(-glo_emph->tau_n);
987 glonass_orbit->set_clock_drift(glo_emph->gamma);
988
989 if (glo_emph->health <= 3) {
990 glonass_orbit->set_health(0); // 0 means good.
991 } else {
992 glonass_orbit->set_health(1); // 1 means bad.
993 }
994 glonass_orbit->set_position_x(glo_emph->pos_x);
995 glonass_orbit->set_position_y(glo_emph->pos_y);
996 glonass_orbit->set_position_z(glo_emph->pos_z);
997
998 glonass_orbit->set_velocity_x(glo_emph->vel_x);
999 glonass_orbit->set_velocity_y(glo_emph->vel_y);
1000 glonass_orbit->set_velocity_z(glo_emph->vel_z);
1001
1002 glonass_orbit->set_accelerate_x(glo_emph->acc_x);
1003 glonass_orbit->set_accelerate_y(glo_emph->acc_y);
1004 glonass_orbit->set_accelerate_z(glo_emph->acc_z);
1005
1006 glonass_orbit->set_infor_age(glo_emph->age);
1007
1008 return true;
1009}
1010
1011bool NovatelParser::HandleHeading(const novatel::Heading* heading,
1012 uint16_t gps_week, uint32_t gps_millisecs) {
1013 heading_.set_solution_status(static_cast<uint32_t>(heading->solution_status));
1014 heading_.set_position_type(static_cast<uint32_t>(heading->position_type));
1015 heading_.set_baseline_length(heading->length);
1016 heading_.set_heading(heading->heading);
1017 heading_.set_pitch(heading->pitch);
1018 heading_.set_reserved(heading->reserved);
1019 heading_.set_heading_std_dev(heading->heading_std_dev);
1020 heading_.set_pitch_std_dev(heading->pitch_std_dev);
1021 heading_.set_station_id(heading->station_id);
1022 heading_.set_satellite_tracked_number(heading->num_sats_tracked);
1023 heading_.set_satellite_soulution_number(heading->num_sats_in_solution);
1024 heading_.set_satellite_number_obs(heading->num_sats_ele);
1025 heading_.set_satellite_number_multi(heading->num_sats_l2);
1026 heading_.set_solution_source(heading->solution_source);
1027 heading_.set_extended_solution_status(heading->extended_solution_status);
1028 heading_.set_galileo_beidou_sig_mask(heading->galileo_beidou_sig_mask);
1029 heading_.set_gps_glonass_sig_mask(heading->gps_glonass_sig_mask);
1030 double seconds = gps_week * SECONDS_PER_WEEK + gps_millisecs * 1e-3;
1031 heading_.set_measurement_time(seconds);
1032 return true;
1033}
1034
1035void NovatelParser::SetObservationTime() {
1036 int week = 0;
1037 double second = time2gpst(raw_.time, &week);
1038 gnss_observation_.set_gnss_time_type(apollo::drivers::gnss::GPS_TIME);
1039 gnss_observation_.set_gnss_week(week);
1040 gnss_observation_.set_gnss_second_s(second);
1041}
1042
1043bool NovatelParser::DecodeGnssObservation(const uint8_t* obs_data,
1044 const uint8_t* obs_data_end) {
1045 while (obs_data < obs_data_end) {
1046 const int status = input_oem4(&raw_, *obs_data++);
1047 switch (status) {
1048 case 1: // observation data
1049 if (raw_.obs.n == 0) {
1050 AWARN << "Obs is zero";
1051 }
1052
1053 gnss_observation_.Clear();
1054 gnss_observation_.set_receiver_id(0);
1055 SetObservationTime();
1056 gnss_observation_.set_sat_obs_num(raw_.obs.n);
1057 for (int i = 0; i < raw_.obs.n; ++i) {
1058 int prn = 0;
1059 int sys = 0;
1060
1061 sys = satsys(raw_.obs.data[i].sat, &prn);
1062
1064 if (!gnss_sys_type(sys, &gnss_type)) {
1065 break;
1066 }
1067
1068 auto sat_obs = gnss_observation_.add_sat_obs(); // create obj
1069 sat_obs->set_sat_prn(prn);
1070 sat_obs->set_sat_sys(gnss_type);
1071
1072 int j = 0;
1073 for (j = 0; j < NFREQ + NEXOBS; ++j) {
1074 if (is_zero(raw_.obs.data[i].L[j])) {
1075 break;
1076 }
1077
1079 if (!gnss_baud_id(gnss_type, j, &baud_id)) {
1080 break;
1081 }
1082
1083 auto band_obs = sat_obs->add_band_obs();
1084 if (raw_.obs.data[i].code[i] == CODE_L1C) {
1085 band_obs->set_pseudo_type(
1087 } else if (raw_.obs.data[i].code[i] == CODE_L1P) {
1088 band_obs->set_pseudo_type(
1090 } else {
1091 AINFO << "Code " << raw_.obs.data[i].code[i] << ", in seq " << j
1092 << ", gnss type " << static_cast<int>(gnss_type);
1093 }
1094
1095 band_obs->set_band_id(baud_id);
1096 band_obs->set_pseudo_range(raw_.obs.data[i].P[j]);
1097 band_obs->set_carrier_phase(raw_.obs.data[i].L[j]);
1098 band_obs->set_loss_lock_index(raw_.obs.data[i].SNR[j]);
1099 band_obs->set_doppler(raw_.obs.data[i].D[j]);
1100 band_obs->set_snr(raw_.obs.data[i].SNR[j]);
1101 band_obs->set_snr(raw_.obs.data[i].SNR[j]);
1102 }
1103 sat_obs->set_band_obs_num(j);
1104 }
1105 return true;
1106
1107 default:
1108 break;
1109 }
1110 }
1111 return false;
1112}
1113
1114} // namespace gnss
1115} // namespace drivers
1116} // namespace apollo
static Time Now()
get the current time.
Definition time.cc:57
double ToSecond() const
convert time to second.
Definition time.cc:77
virtual MessageType GetMessage(MessagePtr *message_ptr)
virtual bool GetInsStat(MessagePtr *message_ptr)
virtual void GetMessages(MessageInfoVec *messages)
const uint8_t * data_
Definition parser.h:138
static Parser * CreateNovatel(const config::Config &config)
const uint8_t * data_end_
Definition parser.h:139
#define AERROR
Definition log.h:44
#define AERROR_EVERY(freq)
Definition log.h:86
#define AWARN_EVERY(freq)
Definition log.h:84
#define AFATAL
Definition log.h:45
#define AINFO
Definition log.h:42
#define AWARN
Definition log.h:43
float cos(Angle16 a)
Definition angle.cc:42
float sin(Angle16 a)
Definition angle.cc:25
bool OK()
Definition state.h:44
ImuParameter GetImuParameter(ImuType type)
::google::protobuf::Message * MessagePtr
Definition parser.h:34
constexpr size_t BUFFER_SIZE
constexpr double DEG_TO_RAD
constexpr double EPS
void rfu_to_flu(double r, double f, double u, ::apollo::common::Point3D *flu)
constexpr int SECONDS_PER_WEEK
constexpr double azimuth_deg_to_yaw_rad(double azimuth)
std::vector< MessageInfo > MessageInfoVec
Definition parser.h:57
constexpr float FLOAT_NAN
T gps2unix(const T gps_seconds)
class register implement
Definition arena_queue.h:37
optional float velocity_latency
Definition gnss.proto:22
optional double measurement_time
Definition gnss.proto:15
optional double measurement_time
Definition ins.proto:20