Apollo 10.0
自动驾驶开放平台
enbroad_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// An parser for decoding binary messages from a ENS receiver. The following
17// messages must be
18// logged in order for this parser to work properly.
19
20#include <cmath>
21#include <iostream>
22#include <limits>
23#include <memory>
24#include <vector>
25
26#include "modules/common_msgs/sensor_msgs/gnss.pb.h"
27#include "modules/common_msgs/sensor_msgs/gnss_best_pose.pb.h"
28#include "modules/common_msgs/sensor_msgs/gnss_raw_observation.pb.h"
29#include "modules/common_msgs/sensor_msgs/heading.pb.h"
30#include "modules/common_msgs/sensor_msgs/imu.pb.h"
31#include "modules/common_msgs/sensor_msgs/ins.pb.h"
32
33#include "cyber/cyber.h"
34#include "modules/drivers/gnss/parser/enbroad_parser/enbroad_messages.h"
35#include "modules/drivers/gnss/parser/parser.h"
36#include "modules/drivers/gnss/parser/parser_common.h"
37
38namespace apollo {
39namespace drivers {
40namespace gnss {
41
42// Encoding and decoding protocol scaling factor setting
43#define Coder_Accel_Scale 12.0
44#define Coder_Rate_Scale 300.0
45#define Coder_MAG_Scale 1.0
46#define Coder_IFof_Rate_Scale 600.0
47#define Coder_Angle_Scale 360.0
48#define Coder_Temp_Scale 200.0
49#define Coder_Sensor_Scale 32768.0
50#define Coder_IFof_Sensor_Scale 2147483648.0
51#define Coder_EXP_E 2.718282.0
52#define Coder_Vel_Scale 100.0
53#define Coder_Pos_Scale 10000000000.0
54
55constexpr size_t BUFFER_SIZE = 256;
56
57class EnbroadParse : public Parser {
58 public:
59 EnbroadParse();
60 explicit EnbroadParse(const config::Config& config);
61
62 virtual void GetMessages(MessageInfoVec* messages);
63
64 private:
65 bool PrepareMessage();
66 bool check_sum();
67 bool HandleNavData(const enbroad::NAV_DATA_TypeDef* pNavData);
68 bool HandleSINSData(const enbroad::NAV_SINS_TypeDef* pSinsData);
69 bool HandleIMUData(const enbroad::NAV_IMU_TypeDef* pImuData);
70 bool HandleGNSSData(const enbroad::NAV_GNSS_TypeDef* pGnssData);
71
72 size_t header_length_ = 0;
73 size_t total_length_ = 0;
74 std::vector<uint8_t> buffer_;
75
76 GnssBestPose bestpos_;
77 Imu imu_;
78 Heading heading_;
79 Ins ins_;
80 InsStat ins_stat_;
81};
82
83Parser* Parser::CreateEnbroad(const config::Config& config) {
84 return new EnbroadParse(config);
85}
86
87EnbroadParse::EnbroadParse() { buffer_.reserve(BUFFER_SIZE); }
88
89EnbroadParse::EnbroadParse(const config::Config& config) {
90 buffer_.reserve(BUFFER_SIZE);
91}
92
93void EnbroadParse::GetMessages(MessageInfoVec* messages) {
94 if (data_ == nullptr) {
95 return;
96 }
97 while (data_ < data_end_) {
98 if (buffer_.empty()) { // Looking for SYNC_HEAD_0
99 if (*data_ == enbroad::SYNC_HEAD_0) {
100 buffer_.push_back(*data_);
101 }
102 ++data_;
103 } else if (buffer_.size() == 1) { // Looking for SYNC_HEAD_1
104 if (*data_ == enbroad::SYNC_HEAD_1) {
105 buffer_.push_back(*data_++);
106 } else {
107 buffer_.clear();
108 }
109 } else if (buffer_.size() == 2) { // Looking for SYNC_HEAD_2
110 if (*data_ == enbroad::SYNC_HEAD_2) {
111 buffer_.push_back(*data_++);
112 header_length_ = sizeof(enbroad::FrameHeader);
113 } else {
114 buffer_.clear();
115 }
116 } else if (header_length_ > 0) { // Working on header.
117 if (buffer_.size() < header_length_) {
118 buffer_.push_back(*data_++);
119 } else {
120 total_length_ = header_length_ +
121 reinterpret_cast<enbroad::FrameHeader*>(buffer_.data())
122 ->message_length +
123 1 + 2;
124 header_length_ = 0;
125 }
126 } else if (total_length_ > 0) {
127 if (buffer_.size() < total_length_) { // Working on body.
128 buffer_.push_back(*data_++);
129 continue;
130 }
131 if (!PrepareMessage()) {
132 buffer_.clear();
133 total_length_ = 0;
134 } else {
135 buffer_.clear();
136 total_length_ = 0;
137 messages->push_back(
138 MessageInfo{MessageType::BEST_GNSS_POS,
139 reinterpret_cast<MessagePtr>(&bestpos_)});
140 messages->push_back(
141 MessageInfo{MessageType::IMU, reinterpret_cast<MessagePtr>(&imu_)});
142 messages->push_back(MessageInfo{
143 MessageType::HEADING, reinterpret_cast<MessagePtr>(&heading_)});
144 messages->push_back(
145 MessageInfo{MessageType::INS, reinterpret_cast<MessagePtr>(&ins_)});
146 messages->push_back(MessageInfo{
147 MessageType::INS_STAT, reinterpret_cast<MessagePtr>(&ins_stat_)});
148 }
149 }
150 }
151}
152double normalizeAngleTo180(double angle) {
153 while (angle > 180.0) {
154 angle -= 360.0;
155 }
156 while (angle <= -180.0) {
157 angle += 360.0;
158 }
159 return angle;
160}
161bool EnbroadParse::check_sum() {
162 char checksum = 0;
163 char compare = 0;
164 size_t len = buffer_.size() - 3;
165 size_t i = 0;
166 while (len--) {
167 checksum += *reinterpret_cast<int8_t*>(buffer_.data() + i);
168 i++;
169 }
170 checksum = ~checksum;
171 checksum = checksum | 0x30;
172 compare = *reinterpret_cast<char*>(buffer_.data() + buffer_.size() - 3);
173 if (checksum == compare) {
174 return true;
175 } else {
176 return false;
177 }
178}
179
180bool EnbroadParse::PrepareMessage() {
181 if (!check_sum()) {
182 AWARN << "check sum failed. bad frame ratio";
183 return false;
184 }
185 uint8_t* message = nullptr;
186 enbroad::MessageId message_id;
187 uint16_t message_length;
188 auto header = reinterpret_cast<const enbroad::FrameHeader*>(buffer_.data());
189 message = buffer_.data() + sizeof(enbroad::FrameHeader);
190 message_id = header->message_id;
191 message_length = header->message_length;
192 switch (message_id) {
193 case enbroad::BIN_NAV_DATA:
194 if (message_length != sizeof(enbroad::NAV_DATA_TypeDef)) {
195 AWARN << "Incorrect message_length";
196 break;
197 }
198 if (!HandleNavData(
199 reinterpret_cast<enbroad::NAV_DATA_TypeDef*>(message))) {
200 AWARN << "HandleNavData fail";
201 return false;
202 }
203 break;
204 case enbroad::BIN_SINS_DATA:
205 if (message_length != sizeof(enbroad::NAV_SINS_TypeDef)) {
206 AWARN << "Incorrect message_length";
207 break;
208 }
209 if (!HandleSINSData(
210 reinterpret_cast<enbroad::NAV_SINS_TypeDef*>(message))) {
211 AWARN << "HandleSINSData fail";
212 return false;
213 }
214 break;
215 case enbroad::BIN_IMU_DATA:
216 if (message_length != sizeof(enbroad::NAV_IMU_TypeDef)) {
217 AWARN << "Incorrect message_length";
218 break;
219 }
220 if (!HandleIMUData(
221 reinterpret_cast<enbroad::NAV_IMU_TypeDef*>(message))) {
222 AWARN << "HandleIMUData fail";
223 return false;
224 }
225 break;
226 case enbroad::BIN_GNSS_DATA:
227 if (message_length != sizeof(enbroad::NAV_GNSS_TypeDef)) {
228 AWARN << "Incorrect message_length";
229 break;
230 }
231 if (!HandleGNSSData(
232 reinterpret_cast<enbroad::NAV_GNSS_TypeDef*>(message))) {
233 return false;
234 }
235 break;
236 default:
237 return false;
238 }
239 return true;
240}
241
242bool EnbroadParse::HandleSINSData(const enbroad::NAV_SINS_TypeDef* pSinsData) {
243 double seconds =
244 pSinsData->gps_week * SECONDS_PER_WEEK + pSinsData->gpssecond * 1e-3;
245 ins_.set_measurement_time(seconds);
246 ins_.mutable_header()->set_timestamp_sec(cyber::Time::Now().ToSecond());
247 ins_.mutable_euler_angles()->set_x(pSinsData->roll * DEG_TO_RAD);
248 ins_.mutable_euler_angles()->set_y(pSinsData->pitch * DEG_TO_RAD);
249 // enbroad set northwest as right direction, Here northeast as right direction
250 ins_.mutable_euler_angles()->set_z(
251 azimuth_deg_to_yaw_rad(normalizeAngleTo180(-pSinsData->heading)));
252 ins_.mutable_position()->set_lon(pSinsData->longitude);
253 ins_.mutable_position()->set_lat(pSinsData->latitude);
254 ins_.mutable_position()->set_height(pSinsData->altitude);
255 ins_.mutable_linear_velocity()->set_x(pSinsData->ve);
256 ins_.mutable_linear_velocity()->set_y(pSinsData->vn);
257 ins_.mutable_linear_velocity()->set_z(pSinsData->vu);
258 if (enbroad::E_NAV_STATUS_IN_NAV == pSinsData->navStatus) {
259 ins_.set_type(Ins::GOOD);
260 } else if (enbroad::E_NAV_STATUS_SYSTEM_STANDARD == pSinsData->navStatus) {
261 ins_.set_type(Ins::CONVERGING);
262 } else {
263 ins_.set_type(Ins::INVALID);
264 }
265
266 bestpos_.set_measurement_time(seconds);
267 bestpos_.set_longitude(pSinsData->longitude);
268 bestpos_.set_latitude(pSinsData->latitude);
269 bestpos_.set_height_msl(pSinsData->altitude);
270 // bestpos_.set_undulation(0.0);//undulation = height_wgs84 - height_msl
271 // datum id number.WGS84
272 bestpos_.set_datum_id(
273 static_cast<apollo::drivers::gnss::DatumId>(enbroad::DatumId::WGS84));
274 // sins standard deviation
275 bestpos_.set_latitude_std_dev(pSinsData->xigema_lat);
276 bestpos_.set_longitude_std_dev(pSinsData->xigema_lon);
277 bestpos_.set_height_std_dev(pSinsData->xigema_alt);
278
279 ins_stat_.mutable_header()->set_timestamp_sec(cyber::Time::Now().ToSecond());
280 // ins pos type define as follows
281 // fusion GPS as INS_RTKFIXED,
282 // fusion wheel as INS_RTKFLOAT,fusion motion as SINGLE,others as NONE
283 if (enbroad::E_FUNSION_GPS == pSinsData->fusion) {
284 ins_stat_.set_pos_type(SolutionType::INS_RTKFIXED);
285 bestpos_.set_sol_type(SolutionType::INS_RTKFIXED);
286 } else {
287 ins_stat_.set_pos_type(SolutionType::NONE);
288 bestpos_.set_sol_type(SolutionType::NONE);
289 }
290
291 if (enbroad::E_NAV_STATUS_IN_NAV == pSinsData->navStatus) {
292 ins_stat_.set_ins_status(SolutionStatus::SOL_COMPUTED);
293 bestpos_.set_sol_status(SolutionStatus::SOL_COMPUTED);
294 } else if (enbroad::E_NAV_STATUS_SYSTEM_STANDARD == pSinsData->navStatus) {
295 ins_stat_.set_ins_status(SolutionStatus::COLD_START);
296 bestpos_.set_sol_status(SolutionStatus::COLD_START);
297 } else {
298 ins_stat_.set_ins_status(SolutionStatus::INSUFFICIENT_OBS);
299 bestpos_.set_sol_status(SolutionStatus::INSUFFICIENT_OBS);
300 }
301 return true;
302}
303bool EnbroadParse::HandleIMUData(const enbroad::NAV_IMU_TypeDef* pImuData) {
304 float imu_measurement_span = 1.0f / 100.0f;
305 double seconds =
306 pImuData->gps_week * SECONDS_PER_WEEK + pImuData->gpssecond * 1e-3;
307
308 imu_.set_measurement_time(seconds);
309 imu_.set_measurement_span(imu_measurement_span);
310 imu_.mutable_linear_acceleration()->set_x(pImuData->accX);
311 imu_.mutable_linear_acceleration()->set_y(pImuData->accY);
312 imu_.mutable_linear_acceleration()->set_z(pImuData->accZ);
313 imu_.mutable_angular_velocity()->set_x(pImuData->gyroX * DEG_TO_RAD);
314 imu_.mutable_angular_velocity()->set_y(pImuData->gyroY * DEG_TO_RAD);
315 imu_.mutable_angular_velocity()->set_z(pImuData->gyroZ * DEG_TO_RAD);
316 return true;
317}
318bool EnbroadParse::HandleGNSSData(const enbroad::NAV_GNSS_TypeDef* pGnssData) {
319 double seconds =
320 pGnssData->gps_week * SECONDS_PER_WEEK + pGnssData->gpssecond * 1e-3;
321 // bestpos_.set_base_station_id("0"); // base station id
322 bestpos_.set_solution_age(pGnssData->age); // solution age (sec)
323 bestpos_.set_num_sats_tracked(pGnssData->satsNum);
324 bestpos_.set_num_sats_in_solution(pGnssData->satsNum);
325 bestpos_.set_num_sats_in_solution(pGnssData->satsNum);
326 bestpos_.set_num_sats_multi(pGnssData->satsNum);
327 // bestpos_.set_galileo_beidou_used_mask(0);
328 // bestpos_.set_gps_glonass_used_mask(0);
329
330 heading_.set_measurement_time(seconds);
331 heading_.set_heading(pGnssData->heading);
332 heading_.set_baseline_length(pGnssData->baseline);
333 heading_.set_reserved(0);
334 // heading_.set_heading_std_dev(0.0);
335 // heading_.set_pitch_std_dev(0.0);
336 // heading_.set_station_id("0");
337 heading_.set_satellite_tracked_number(pGnssData->satsNum);
338 heading_.set_satellite_soulution_number(pGnssData->satsNum);
339 heading_.set_satellite_number_obs(pGnssData->satsNum);
340 heading_.set_satellite_number_multi(pGnssData->satsNum);
341 // heading_.set_solution_source(0);
342 // heading_.set_extended_solution_status(0);
343 // heading_.set_galileo_beidou_sig_mask(0);
344 // heading_.set_gps_glonass_sig_mask(0);
345 if (enbroad::E_GPS_RTK_FIXED == pGnssData->headingStatus) {
346 heading_.set_position_type(SolutionType::INS_RTKFIXED);
347 } else if (enbroad::E_GPS_RTK_FLOAT == pGnssData->headingStatus) {
348 heading_.set_position_type(SolutionType::INS_RTKFLOAT);
349 } else if (enbroad::E_GPS_RTK_SPP == pGnssData->headingStatus ||
350 enbroad::E_GPS_RTK_DGPS == pGnssData->headingStatus) {
351 heading_.set_position_type(SolutionType::SINGLE);
352 } else {
353 heading_.set_position_type(SolutionType::NONE);
354 }
355 return true;
356}
357bool EnbroadParse::HandleNavData(const enbroad::NAV_DATA_TypeDef* pNavData) {
358 static uint16_t rtkStatus;
359 static uint16_t Nav_Standard_flag;
360 static uint16_t Sate_Num;
361 static float baseline;
362 float imu_measurement_span = 1.0f / 100.0f;
363 double seconds =
364 pNavData->gps_week * SECONDS_PER_WEEK + pNavData->gps_millisecs * 1e-3;
365 ins_.set_measurement_time(seconds);
366 ins_.mutable_header()->set_timestamp_sec(cyber::Time::Now().ToSecond());
367 ins_.mutable_euler_angles()->set_x(static_cast<double>(
368 pNavData->roll * Coder_Angle_Scale / Coder_Sensor_Scale * DEG_TO_RAD));
369 ins_.mutable_euler_angles()->set_y(static_cast<double>(
370 pNavData->pitch * Coder_Angle_Scale / Coder_Sensor_Scale * DEG_TO_RAD));
371 // enbroad set northwest as right direction, Here northeast as right direction
372 ins_.mutable_euler_angles()->set_z(azimuth_deg_to_yaw_rad(normalizeAngleTo180(
373 -pNavData->head * Coder_Angle_Scale / Coder_Sensor_Scale)));
374 ins_.mutable_position()->set_lon(
375 static_cast<double>(pNavData->lon / Coder_Pos_Scale));
376 ins_.mutable_position()->set_lat(
377 static_cast<double>(pNavData->lat / Coder_Pos_Scale));
378 ins_.mutable_position()->set_height(
379 static_cast<double>(pNavData->alt / 1000.0));
380 ins_.mutable_linear_acceleration()->set_x(static_cast<double>(
381 pNavData->accX * Coder_Accel_Scale / Coder_Sensor_Scale));
382 ins_.mutable_linear_acceleration()->set_y(static_cast<double>(
383 pNavData->accY * Coder_Accel_Scale / Coder_Sensor_Scale));
384 ins_.mutable_linear_acceleration()->set_z(static_cast<double>(
385 pNavData->accZ * Coder_Accel_Scale / Coder_Sensor_Scale));
386 ins_.mutable_angular_velocity()->set_x(
387 static_cast<double>(pNavData->gyroX * Coder_Rate_Scale /
388 Coder_Sensor_Scale) *
389 DEG_TO_RAD);
390 ins_.mutable_angular_velocity()->set_y(
391 static_cast<double>(pNavData->gyroY * Coder_Rate_Scale /
392 Coder_Sensor_Scale) *
393 DEG_TO_RAD);
394 ins_.mutable_angular_velocity()->set_z(
395 static_cast<double>(pNavData->gyroZ * Coder_Rate_Scale /
396 Coder_Sensor_Scale) *
397 DEG_TO_RAD);
398 ins_.mutable_linear_velocity()->set_x(
399 static_cast<double>(pNavData->ve * Coder_Vel_Scale / Coder_Sensor_Scale));
400 ins_.mutable_linear_velocity()->set_y(
401 static_cast<double>(pNavData->vn * Coder_Vel_Scale / Coder_Sensor_Scale));
402 ins_.mutable_linear_velocity()->set_z(
403 static_cast<double>(pNavData->vu * Coder_Vel_Scale / Coder_Sensor_Scale));
404 ins_.set_type(Ins::GOOD);
405 switch (pNavData->poll_type) {
406 case enbroad::E_POLL_DEV_TEMP:
407 break;
408 case enbroad::E_POLL_GNSS_STATE:
409 rtkStatus = pNavData->poll_frame1;
410 break;
411 case enbroad::E_POLL_CAN_STATE:
412 break;
413 case enbroad::E_POLL_INS_STATE:
414 Nav_Standard_flag = pNavData->poll_frame1;
415 break;
416 case enbroad::E_POLL_GNSS2_STATE:
417 Sate_Num = pNavData->poll_frame1;
418 baseline = pNavData->poll_frame2 / 1000.0;
419 break;
420 default:
421 break;
422 }
423 imu_.set_measurement_time(seconds);
424 imu_.set_measurement_span(imu_measurement_span);
425 imu_.mutable_linear_acceleration()->set_x(static_cast<double>(
426 pNavData->accX * Coder_Accel_Scale / Coder_Sensor_Scale));
427 imu_.mutable_linear_acceleration()->set_y(static_cast<double>(
428 pNavData->accY * Coder_Accel_Scale / Coder_Sensor_Scale));
429 imu_.mutable_linear_acceleration()->set_z(static_cast<double>(
430 pNavData->accZ * Coder_Accel_Scale / Coder_Sensor_Scale));
431 imu_.mutable_angular_velocity()->set_x(
432 static_cast<double>(pNavData->gyroX * Coder_Rate_Scale /
433 Coder_Sensor_Scale) *
434 DEG_TO_RAD);
435 imu_.mutable_angular_velocity()->set_y(
436 static_cast<double>(pNavData->gyroY * Coder_Rate_Scale /
437 Coder_Sensor_Scale) *
438 DEG_TO_RAD);
439 imu_.mutable_angular_velocity()->set_z(
440 static_cast<double>(pNavData->gyroZ * Coder_Rate_Scale /
441 Coder_Sensor_Scale) *
442 DEG_TO_RAD);
443 bestpos_.set_measurement_time(seconds);
444 bestpos_.set_longitude(static_cast<double>(pNavData->lon / Coder_Pos_Scale));
445 bestpos_.set_latitude(static_cast<double>(pNavData->lat / Coder_Pos_Scale));
446 bestpos_.set_height_msl(static_cast<double>(pNavData->alt / 1000.0));
447 bestpos_.set_undulation(0.0); // undulation = height_wgs84 - height_msl
448 bestpos_.set_datum_id(
449 static_cast<apollo::drivers::gnss::DatumId>(enbroad::DatumId::WGS84));
450 bestpos_.set_latitude_std_dev(0.0);
451 bestpos_.set_longitude_std_dev(0.0);
452 bestpos_.set_height_std_dev(0.0);
453 bestpos_.set_base_station_id("0");
454 bestpos_.set_solution_age(0.0); // solution age (sec)
455 bestpos_.set_num_sats_tracked(Sate_Num);
456 bestpos_.set_num_sats_in_solution(Sate_Num);
457 bestpos_.set_num_sats_in_solution(Sate_Num);
458 bestpos_.set_num_sats_multi(Sate_Num);
459 bestpos_.set_extended_solution_status(SolutionType::INS_RTKFIXED);
460 bestpos_.set_galileo_beidou_used_mask(0);
461 bestpos_.set_gps_glonass_used_mask(0);
462 heading_.set_measurement_time(seconds);
463 heading_.set_pitch(static_cast<double>(pNavData->pitch * Coder_Angle_Scale /
464 Coder_Sensor_Scale));
465 // enbroad set northwest as right direction,
466 // Here northeast as right direction
467 heading_.set_heading(normalizeAngleTo180(-pNavData->head * Coder_Angle_Scale /
468 Coder_Sensor_Scale));
469 heading_.set_baseline_length(baseline);
470 heading_.set_reserved(0);
471 heading_.set_heading_std_dev(0.0);
472 heading_.set_pitch_std_dev(0.0);
473 heading_.set_station_id("0");
474 heading_.set_satellite_tracked_number(Sate_Num);
475 heading_.set_satellite_soulution_number(Sate_Num);
476 heading_.set_satellite_number_obs(Sate_Num);
477 heading_.set_satellite_number_multi(Sate_Num);
478 heading_.set_solution_source(0);
479 heading_.set_extended_solution_status(0);
480 heading_.set_galileo_beidou_sig_mask(0);
481 heading_.set_gps_glonass_sig_mask(0);
482 ins_stat_.mutable_header()->set_timestamp_sec(cyber::Time::Now().ToSecond());
483 // According to the RTK state:
484 // fixed as "INS-RTKFIXED",float as "INS-RTKFLOAT",
485 // single or RTD as "SINGLE",no solution as "NONE"
486 if (enbroad::E_GPS_RTK_FIXED == rtkStatus) {
487 ins_stat_.set_pos_type(SolutionType::INS_RTKFIXED);
488 bestpos_.set_sol_type(SolutionType::INS_RTKFIXED);
489 heading_.set_position_type(SolutionType::INS_RTKFIXED);
490 } else if (enbroad::E_GPS_RTK_FLOAT == rtkStatus) {
491 ins_stat_.set_pos_type(SolutionType::INS_RTKFLOAT);
492 bestpos_.set_sol_type(SolutionType::INS_RTKFLOAT);
493 heading_.set_position_type(SolutionType::INS_RTKFLOAT);
494 } else if (enbroad::E_GPS_RTK_SPP == rtkStatus ||
495 enbroad::E_GPS_RTK_DGPS == rtkStatus) {
496 ins_stat_.set_pos_type(SolutionType::SINGLE);
497 bestpos_.set_sol_type(SolutionType::SINGLE);
498 heading_.set_position_type(SolutionType::SINGLE);
499 } else {
500 ins_stat_.set_pos_type(SolutionType::NONE);
501 bestpos_.set_sol_type(SolutionType::NONE);
502 heading_.set_position_type(SolutionType::NONE);
503 }
504 // According to sins calibration status:
505 // no calibration as "SOL_COMPUTED",
506 // during calibration as "SOL_COMPUTED",
507 // completing calibration as "SOL_COMPUTED"
508 if (enbroad::E_NAV_STANDARD_PROCCSSED == Nav_Standard_flag) {
509 ins_stat_.set_ins_status(SolutionStatus::SOL_COMPUTED);
510 bestpos_.set_sol_status(SolutionStatus::SOL_COMPUTED);
511 heading_.set_solution_status(SolutionStatus::SOL_COMPUTED);
512 } else if (enbroad::E_NAV_STANDARD_PROCCSSING == Nav_Standard_flag) {
513 ins_stat_.set_ins_status(SolutionStatus::COLD_START);
514 bestpos_.set_sol_status(SolutionStatus::COLD_START);
515 heading_.set_solution_status(SolutionStatus::COLD_START);
516 } else {
517 ins_stat_.set_ins_status(SolutionStatus::INSUFFICIENT_OBS);
518 bestpos_.set_sol_status(SolutionStatus::INSUFFICIENT_OBS);
519 heading_.set_solution_status(SolutionStatus::INSUFFICIENT_OBS);
520 }
521 return true;
522}
523
524} // namespace gnss
525} // namespace drivers
526} // namespace apollo
apollo::cyber::Time::Now
static Time Now()
get the current time.
Definition time.cc:57
apollo::drivers::gnss::EnbroadParse::GetMessages
virtual void GetMessages(MessageInfoVec *messages)
Definition enbroad_parser.cc:93
apollo::drivers::gnss::Parser::CreateEnbroad
static Parser * CreateEnbroad(const config::Config &config)
Definition enbroad_parser.cc:83
apollo::drivers::gnss::Parser::data_
const uint8_t * data_
Definition parser.h:138
apollo::drivers::gnss::Parser::data_end_
const uint8_t * data_end_
Definition parser.h:139
enbroad_messages.h
Coder_Vel_Scale
#define Coder_Vel_Scale
Definition enbroad_parser.cc:52
Coder_Sensor_Scale
#define Coder_Sensor_Scale
Definition enbroad_parser.cc:49
Coder_Pos_Scale
#define Coder_Pos_Scale
Definition enbroad_parser.cc:53
Coder_Accel_Scale
#define Coder_Accel_Scale
Definition enbroad_parser.cc:43
Coder_Rate_Scale
#define Coder_Rate_Scale
Definition enbroad_parser.cc:44
Coder_Angle_Scale
#define Coder_Angle_Scale
Definition enbroad_parser.cc:47
AWARN
#define AWARN
Definition log.h:43
apollo::drivers::gnss::MessagePtr
::google::protobuf::Message * MessagePtr
Definition parser.h:34
apollo::drivers::gnss::BUFFER_SIZE
constexpr size_t BUFFER_SIZE
Definition enbroad_parser.cc:55
apollo::drivers::gnss::normalizeAngleTo180
double normalizeAngleTo180(double angle)
Definition enbroad_parser.cc:152
apollo::drivers::gnss::DEG_TO_RAD
constexpr double DEG_TO_RAD
Definition parser_common.h:32
apollo::drivers::gnss::SECONDS_PER_WEEK
constexpr int SECONDS_PER_WEEK
Definition parser_common.h:30
apollo::drivers::gnss::azimuth_deg_to_yaw_rad
constexpr double azimuth_deg_to_yaw_rad(double azimuth)
Definition parser_common.h:38
apollo::drivers::gnss::MessageInfoVec
std::vector< MessageInfo > MessageInfoVec
Definition parser.h:57
apollo
class register implement
Definition arena_queue.h:37
parser_common.h
apollo::drivers::gnss::enbroad::NAV_DATA_TypeDef
Definition enbroad_messages.h:117
apollo::drivers::gnss::enbroad::NAV_GNSS_TypeDef
Definition enbroad_messages.h:190
apollo::drivers::gnss::enbroad::NAV_IMU_TypeDef
Definition enbroad_messages.h:171
apollo::drivers::gnss::enbroad::NAV_SINS_TypeDef
Definition enbroad_messages.h:145