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"
54constexpr int INDEX[] = {4, 3, 5, 1, 0, 2, 7, 6, 8};
55static_assert(
sizeof(INDEX) == 9 *
sizeof(
int),
"Incorrect size of INDEX");
58constexpr bool is_zero(T value) {
59 return value ==
static_cast<T
>(0);
63inline uint32_t crc32_word(uint32_t word) {
64 for (
int j = 0; j < 8; ++j) {
66 word = (word >> 1) ^ 0xEDB88320;
74inline uint32_t crc32_block(
const uint8_t* buffer,
size_t length) {
77 uint32_t t1 = (word >> 8) & 0xFFFFFF;
78 uint32_t t2 = crc32_word((word ^ *buffer++) & 0xFF);
104 uint32_t gps_millisecs);
107 uint32_t gps_millisecs);
110 uint32_t gps_millisecs);
119 uint32_t gps_millisecs);
131 void SetObservationTime();
133 bool DecodeGnssObservation(
const uint8_t* obs_data,
134 const uint8_t* obs_data_end);
137 uint32_t gps_millisecs);
139 double gyro_scale_ = 0.0;
141 double accel_scale_ = 0.0;
143 float imu_measurement_span_ = 1.0f / 200.0f;
144 float imu_measurement_hz_ = 200.0f;
146 int imu_frame_mapping_ = 5;
148 double imu_measurement_time_previous_ = -1.0;
150 std::vector<uint8_t> buffer_;
152 size_t header_length_ = 0;
154 size_t total_length_ = 0;
169 bool has_ins_stat_message_ =
false;
170 bool has_corr_imu_message_ =
false;
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);
192 if (1 != init_raw(&raw_)) {
193 AFATAL <<
"memory allocation error for observation data structure.";
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);
203 if (config.has_imu_type()) {
207 if (1 != init_raw(&raw_)) {
208 AFATAL <<
"memory allocation error for observation data structure.";
213 if (
data_ ==
nullptr) {
218 if (buffer_.empty()) {
220 buffer_.push_back(*
data_);
223 }
else if (buffer_.size() == 1) {
225 buffer_.push_back(*
data_++);
229 }
else if (buffer_.size() == 2) {
232 buffer_.push_back(*
data_++);
236 buffer_.push_back(*
data_++);
242 }
else if (header_length_ > 0) {
243 if (buffer_.size() < header_length_) {
244 buffer_.push_back(*
data_++);
256 AERROR <<
"Incorrect header_length_. Should never reach here.";
261 }
else if (total_length_ > 0) {
262 if (buffer_.size() < total_length_) {
263 buffer_.push_back(*
data_++);
278 std::set<MessageType> s;
285 if (s.find(message_info.
type) == s.end()) {
286 messages->push_back(std::move(message_info));
287 s.insert(message_info.
type);
292bool NovatelParser::check_crc() {
294 return crc32_block(buffer_.data(), l) ==
295 *
reinterpret_cast<uint32_t*
>(buffer_.data() + l);
300 AERROR <<
"CRC check failed.";
304 uint8_t* message =
nullptr;
306 uint16_t message_length;
308 uint32_t gps_millisecs;
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;
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;
324 switch (message_id) {
326 if (message_length !=
sizeof(novatel::BestPos)) {
327 AERROR <<
"Incorrect message_length";
330 if (HandleGnssBestpos(
reinterpret_cast<novatel::BestPos*
>(message),
331 gps_week, gps_millisecs)) {
332 *message_ptr = &bestpos_;
339 if (message_length !=
sizeof(novatel::BestPos)) {
340 AERROR <<
"Incorrect message_length";
343 if (HandleBestPos(
reinterpret_cast<novatel::BestPos*
>(message), gps_week,
345 *message_ptr = &gnss_;
353 if (message_length !=
sizeof(novatel::BestVel)) {
354 AERROR <<
"Incorrect message_length";
357 if (HandleBestVel(
reinterpret_cast<novatel::BestVel*
>(message), gps_week,
359 *message_ptr = &gnss_;
367 if (message_length !=
sizeof(novatel::CorrImuData)) {
368 AERROR <<
"Incorrect message_length";
371 has_corr_imu_message_ =
true;
372 if (HandleCorrImuData(
reinterpret_cast<novatel::CorrImuData*
>(message))) {
373 *message_ptr = &ins_;
380 if (message_length !=
sizeof(novatel::InsCov)) {
381 AERROR <<
"Incorrect message_length";
385 if (HandleInsCov(
reinterpret_cast<novatel::InsCov*
>(message))) {
386 *message_ptr = &ins_;
393 if (message_length !=
sizeof(novatel::InsPva)) {
394 AERROR <<
"Incorrect message_length";
398 if (HandleInsPva(
reinterpret_cast<novatel::InsPva*
>(message))) {
399 *message_ptr = &ins_;
406 if (message_length !=
sizeof(novatel::RawImuX)) {
407 AERROR <<
"Incorrect message_length";
411 if (HandleRawImuX(
reinterpret_cast<novatel::RawImuX*
>(message))) {
412 *message_ptr = &imu_;
419 if (message_length !=
sizeof(novatel::RawImu)) {
420 AERROR <<
"Incorrect message_length";
424 if (HandleRawImu(
reinterpret_cast<novatel::RawImu*
>(message))) {
425 *message_ptr = &imu_;
431 if (message_length !=
sizeof(novatel::InsPvaX)) {
432 AERROR <<
"Incorrect message_length";
435 has_ins_stat_message_ =
true;
436 if (HandleInsPvax(
reinterpret_cast<novatel::InsPvaX*
>(message), gps_week,
438 *message_ptr = &ins_stat_;
444 if (message_length !=
sizeof(novatel::BDS_Ephemeris)) {
445 AERROR <<
"Incorrect BDSEPHEMERIS message_length";
448 if (HandleBdsEph(
reinterpret_cast<novatel::BDS_Ephemeris*
>(message))) {
449 *message_ptr = &gnss_ephemeris_;
455 if (message_length !=
sizeof(novatel::GPS_Ephemeris)) {
456 AERROR <<
"Incorrect GPSEPHEMERIS message_length";
459 if (HandleGpsEph(
reinterpret_cast<novatel::GPS_Ephemeris*
>(message))) {
460 *message_ptr = &gnss_ephemeris_;
466 if (message_length !=
sizeof(novatel::GLO_Ephemeris)) {
467 AERROR <<
"Incorrect GLOEPHEMERIS message length";
470 if (HandleGloEph(
reinterpret_cast<novatel::GLO_Ephemeris*
>(message))) {
471 *message_ptr = &gnss_ephemeris_;
477 if (DecodeGnssObservation(buffer_.data(),
478 buffer_.data() + buffer_.size())) {
479 *message_ptr = &gnss_observation_;
485 if (message_length !=
sizeof(novatel::Heading)) {
486 AERROR <<
"Incorrect message_length";
489 if (HandleHeading(
reinterpret_cast<novatel::Heading*
>(message), gps_week,
491 *message_ptr = &heading_;
502bool NovatelParser::HandleGnssBestpos(
const novatel::BestPos* pos,
504 uint32_t gps_millisecs) {
505 bestpos_.set_sol_status(
507 bestpos_.set_sol_type(
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));
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(
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);
537 bestpos_.set_measurement_time(seconds);
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_);
555 if (position_type_ != pos->position_type) {
556 position_type_ = pos->position_type;
557 AINFO <<
"Position type: " <<
static_cast<int>(position_type_);
559 gnss_.set_solution_status(
static_cast<uint32_t
>(pos->solution_status));
561 gnss_.set_position_type(
static_cast<uint32_t
>(pos->position_type));
562 switch (pos->position_type) {
608 gnss_.set_position_type(0);
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));
619 <<
static_cast<int>(pos->datum_id);
624 gnss_.set_measurement_time(seconds);
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_);
636 if (!gnss_.has_velocity_latency() ||
638 AINFO <<
"Velocity latency: " <<
static_cast<int>(vel->latency);
639 gnss_.set_velocity_latency(vel->latency);
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);
648 gnss_.set_measurement_time(seconds);
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());
666 ins_.set_measurement_time(seconds);
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(
680 cov->attitude_covariance[i]));
681 ins_.set_linear_velocity_covariance(
682 i,
static_cast<float>(cov->velocity_covariance[i]));
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_);
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);
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);
702 switch (pva->status) {
717 if (has_corr_imu_message_ &&
719 ins_.set_measurement_time(seconds);
722 ins_.set_measurement_time(seconds);
725 ins_.mutable_header()->set_timestamp_sec(now);
726 if (!has_ins_stat_message_) {
727 ins_stat_.mutable_header()->set_timestamp_sec(now);
733 *message_ptr = &ins_stat_;
738 uint16_t gps_week, uint32_t gps_millisecs) {
741 ins_stat_.mutable_header()->set_timestamp_sec(unix_sec);
743 ins_stat_.set_pos_type(pvax->
pos_type);
747bool NovatelParser::HandleRawImuX(
const novatel::RawImuX* imu) {
748 if (imu->imu_error != 0) {
749 AWARN <<
"IMU error. Status: " << std::hex << std::showbase
752 if (is_zero(gyro_scale_)) {
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 <<
".";
760 if (is_zero(param.sampling_rate_hz)) {
762 << config::ImuType_Name(imu_type);
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_);
773 if (imu_measurement_time_previous_ > 0.0 &&
774 fabs(time - imu_measurement_time_previous_ - imu_measurement_span_) >
776 AWARN_EVERY(5) <<
"Unexpected delay between two IMU measurements at: "
777 << time - imu_measurement_time_previous_;
779 imu_.set_measurement_time(time);
780 switch (imu_frame_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());
787 -imu->y_angle_change_neg * gyro_scale_,
788 imu->z_angle_change * gyro_scale_,
789 imu_.mutable_angular_velocity());
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());
803 << imu_frame_mapping_;
805 imu_measurement_time_previous_ = time;
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;
814 if (is_zero(gyro_scale_)) {
817 if (is_zero(param.sampling_rate_hz)) {
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);
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);
833 if (imu_measurement_time_previous_ > 0.0 &&
834 fabs(time - imu_measurement_time_previous_ - imu_measurement_span) >
836 AWARN <<
"Unexpected delay between two IMU measurements at: "
837 << time - imu_measurement_time_previous_;
840 imu_.set_measurement_time(time);
841 switch (imu_frame_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());
848 -imu->y_angle_change_neg * gyro_scale,
849 imu->z_angle_change * gyro_scale,
850 imu_.mutable_angular_velocity());
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());
859 -imu->y_angle_change_neg * gyro_scale,
860 imu->z_angle_change * gyro_scale,
861 ins_.mutable_angular_velocity());
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());
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());
888 << imu_frame_mapping_;
890 imu_measurement_time_previous_ = time;
894bool NovatelParser::HandleGpsEph(
const novatel::GPS_Ephemeris* gps_emph) {
898 gnss_ephemeris_.mutable_keppler_orbit();
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);
933bool NovatelParser::HandleBdsEph(
const novatel::BDS_Ephemeris* bds_emph) {
937 gnss_ephemeris_.mutable_keppler_orbit();
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);
972bool NovatelParser::HandleGloEph(
const novatel::GLO_Ephemeris* glo_emph) {
976 gnss_ephemeris_.mutable_glonass_orbit();
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);
989 if (glo_emph->health <= 3) {
990 glonass_orbit->set_health(0);
992 glonass_orbit->set_health(1);
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);
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);
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);
1006 glonass_orbit->set_infor_age(glo_emph->age);
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);
1031 heading_.set_measurement_time(seconds);
1035void NovatelParser::SetObservationTime() {
1037 double second = time2gpst(raw_.time, &week);
1039 gnss_observation_.set_gnss_week(week);
1040 gnss_observation_.set_gnss_second_s(second);
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++);
1049 if (raw_.obs.n == 0) {
1050 AWARN <<
"Obs is zero";
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) {
1061 sys = satsys(raw_.obs.data[i].sat, &prn);
1064 if (!gnss_sys_type(sys, &gnss_type)) {
1068 auto sat_obs = gnss_observation_.add_sat_obs();
1069 sat_obs->set_sat_prn(prn);
1070 sat_obs->set_sat_sys(gnss_type);
1073 for (j = 0; j < NFREQ + NEXOBS; ++j) {
1074 if (is_zero(raw_.obs.data[i].L[j])) {
1079 if (!gnss_baud_id(gnss_type, j, &baud_id)) {
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(
1091 AINFO <<
"Code " << raw_.obs.data[i].code[i] <<
", in seq " << j
1092 <<
", gnss type " <<
static_cast<int>(gnss_type);
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]);
1103 sat_obs->set_band_obs_num(j);
static Time Now()
get the current time.
double ToSecond() const
convert time to second.
virtual MessageType GetMessage(MessagePtr *message_ptr)
virtual bool GetInsStat(MessagePtr *message_ptr)
virtual void GetMessages(MessageInfoVec *messages)
static Parser * CreateNovatel(const config::Config &config)
const uint8_t * data_end_
#define AERROR_EVERY(freq)
#define AWARN_EVERY(freq)
constexpr uint16_t CRC_LENGTH
ImuParameter GetImuParameter(ImuType type)
::google::protobuf::Message * MessagePtr
constexpr size_t BUFFER_SIZE
constexpr double DEG_TO_RAD
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
constexpr float FLOAT_NAN
T gps2unix(const T gps_seconds)
optional float velocity_latency
optional double measurement_time
optional double measurement_time
optional ImuType imu_type