Apollo 11.0
自动驾驶开放平台
asensing_can_parser.cc
浏览该文件的文档.
1/******************************************************************************
2 * Copyright 2024 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#include <time.h>
17
20
21namespace apollo {
22namespace drivers {
23namespace gnss {
24
25#define ACC_UNIT 0.0001220703 - 4
26#define G_UNIT 9.7883105
27#define GYRO_UNIT 0.0076293 - 250
28#define ANGLE_UNIT 0.010986 - 360
29#define ALT_UINT 0.001 - 10000
30#define LOC_UNIT 1E-07 - 180
31#define SPEED_UNIT 0.0030517 - 100
32#define LOC_STD_UNIT 0.001
33#define HEADING_STD_UNIT 0.01
34
35constexpr uint32_t FRAME_ins_acc = 0x500;
36constexpr uint32_t FRAME_ins_gyro = 0x501;
37constexpr uint32_t FRAME_ins_angle = 0x502;
38constexpr uint32_t FRAME_ins_height_and_time = 0x503;
39constexpr uint32_t FRAME_ins_latitude_longitude = 0x504;
40constexpr uint32_t FRAME_ins_speed = 0x505;
41constexpr uint32_t FRAME_ins_datainfo = 0x506;
42constexpr uint32_t FRAME_ins_std = 0x507;
43constexpr uint32_t FRAME_gnss_utc = 0x508;
44constexpr uint32_t FRAME_gps_latitude_longitude = 0x509;
45constexpr uint32_t FRAME_gps_height_and_week = 0x50A;
46constexpr uint32_t FRAME_gps_heading_and_pitch = 0x50C;
47constexpr uint32_t FRAME_gps_std = 0x512;
48
50 uint32_t gps_week = 0;
51 uint32_t week_ms = 0;
52 double ins_gps_sec = 0;
53 double gps_sec = 0;
54
55 // gps
56 double gps_latitude = -1.0;
59 float gps_heading = -1.0;
60 float gps_pitch;
61 float gps_lat_std = 0.0;
62 float gps_lon_std = 0.0;
63 float gps_alti_std = 0.0;
64 float gps_yaw_std = 0.0;
65
66 // INS
67 float heading;
68 float pitch;
69 float roll;
70 float gyro_x;
71 float gyro_y;
72 float gyro_z;
73 float acc_x;
74 float acc_y;
75 float acc_z;
76 double latitude;
77 double longitude;
78 double altitude;
79 float ve;
80 float vn;
81 float vd;
82 float lat_std = 0.0;
83 float lon_std = 0.0;
84 float alti_std = 0.0;
85 float vn_std = 0.0;
86 float ve_std = 0.0;
87 float vd_std = 0.0;
88 float roll_std = 0.0;
89 float pitch_std = 0.0;
90 float yaw_std = 0.0;
91 uint8_t satellites_num = 0;
92 double age;
93 float differential_age = 0.0;
96};
97
98template <typename T>
99T ToValue(const uint8_t *data, const int &index) {
100 T t = 0;
101 for (uint8_t i = 0; i < sizeof(T); ++i) {
102 t |= static_cast<T>(data[index + i]) << ((sizeof(T) - i - 1) * 8);
103 }
104 return t;
105}
106
107template <typename T0, typename T1>
108T1 ToValue(const uint8_t *data, const int &index) {
109 return static_cast<T1>(ToValue<T0>(data, index));
110}
111
113 public:
114 explicit AsensingCanParser(const config::Config &config)
115 : AsensingBaseParser(config) {}
116 void GetMessages(MessageInfoVec *messages) override;
117 bool PrepareMessage() override;
118 void FillGnssBestpos() override;
119 void FillIns() override;
120 void FillInsStat() override;
121 void FillImu() override;
122 void FillHeading() override;
123
124 private:
125 void Decode(const apollo::drivers::canbus::CanFrame &frame);
126 void DecodeInsAcc(const apollo::drivers::canbus::CanFrame &frame);
127 void DecodeInsGyro(const apollo::drivers::canbus::CanFrame &frame);
128 void DecodeInsAngle(const apollo::drivers::canbus::CanFrame &frame);
129 void DecodeInsHeightAndTime(const apollo::drivers::canbus::CanFrame &frame);
130 void DecodeInsLatitudeLongitude(
132 void DecodeInsSpeed(const apollo::drivers::canbus::CanFrame &frame);
133 void DecodeInsDataInfo(const apollo::drivers::canbus::CanFrame &frame);
134 void DecodeInsStd(const apollo::drivers::canbus::CanFrame &frame);
135 void DecodeGnssUtc(const apollo::drivers::canbus::CanFrame &frame);
136 void DecodeGpsLatitudeLongitude(
138 void DecodeGpsHeightAndWeek(const apollo::drivers::canbus::CanFrame &frame);
139 void DecodeGpsHeadingAndPitch(const apollo::drivers::canbus::CanFrame &frame);
140 void DecodeGpsStd(const apollo::drivers::canbus::CanFrame &frame);
141
142 AsensingData adata_;
143 uint32_t cur_can_id_;
144};
145
147 return new AsensingCanParser(config);
148}
149
151 if (data_ == nullptr) {
152 return;
153 }
155
156 if (cur_can_id_ == FRAME_gps_latitude_longitude) {
158 messages->push_back(MessageInfo{MessageType::BEST_GNSS_POS,
159 reinterpret_cast<MessagePtr>(&bestpos_)});
160 return;
161 } else if (cur_can_id_ == FRAME_gps_heading_and_pitch) {
162 FillHeading();
163 messages->push_back(MessageInfo{MessageType::HEADING,
164 reinterpret_cast<MessagePtr>(&heading_)});
165 return;
166 } else if (cur_can_id_ != FRAME_ins_height_and_time) {
167 return;
168 }
169
170 FillImu();
171 FillIns();
172 FillInsStat();
173 if (auto_fill_gps_msg_ && adata_.gps_latitude < 0 &&
176 messages->push_back(MessageInfo{MessageType::BEST_GNSS_POS,
177 reinterpret_cast<MessagePtr>(&bestpos_)});
178 }
179 if (auto_fill_gps_msg_ && adata_.gps_heading < -0.1) {
180 FillHeading();
181 messages->push_back(MessageInfo{MessageType::HEADING,
182 reinterpret_cast<MessagePtr>(&heading_)});
183 }
184 messages->push_back(
185 MessageInfo{MessageType::IMU, reinterpret_cast<MessagePtr>(&imu_)});
186
187 messages->push_back(
188 MessageInfo{MessageType::INS, reinterpret_cast<MessagePtr>(&ins_)});
189 messages->push_back(MessageInfo{MessageType::INS_STAT,
190 reinterpret_cast<MessagePtr>(&ins_stat_)});
191}
192
194 auto can_frame =
195 reinterpret_cast<const apollo::drivers::canbus::CanFrame *>(data_);
196 ADEBUG << "Received can frame: " << can_frame->CanFrameString();
197 Decode(*can_frame);
198 cur_can_id_ = can_frame->id;
199 return true;
200}
201
202void AsensingCanParser::Decode(const apollo::drivers::canbus::CanFrame &frame) {
203 switch (frame.id) {
204 case FRAME_ins_acc:
205 DecodeInsAcc(frame);
206 break;
207 case FRAME_ins_gyro:
208 DecodeInsGyro(frame);
209 break;
210 case FRAME_ins_angle:
211 DecodeInsAngle(frame);
212 break;
214 DecodeInsHeightAndTime(frame);
215 break;
217 DecodeInsLatitudeLongitude(frame);
218 break;
219 case FRAME_ins_speed:
220 DecodeInsSpeed(frame);
221 break;
223 DecodeInsDataInfo(frame);
224 break;
225 case FRAME_ins_std:
226 DecodeInsStd(frame);
227 break;
228 case FRAME_gnss_utc:
229 DecodeGnssUtc(frame);
230 break;
232 DecodeGpsLatitudeLongitude(frame);
233 break;
235 DecodeGpsHeightAndWeek(frame);
236 break;
238 DecodeGpsHeadingAndPitch(frame);
239 break;
240 case FRAME_gps_std:
241 DecodeGpsStd(frame);
242 break;
243 default:
244 AWARN << "Unknown CAN frame id: " << std::hex << frame.id;
245 break;
246 }
247}
248
249void AsensingCanParser::DecodeInsAcc(
251 adata_.acc_x = (ToValue<uint16_t, float>(frame.data, 0) * ACC_UNIT) * G_UNIT;
252 adata_.acc_y = (ToValue<uint16_t, float>(frame.data, 2) * ACC_UNIT) * G_UNIT;
253 adata_.acc_z = -(ToValue<uint16_t, float>(frame.data, 4) * ACC_UNIT) * G_UNIT;
254}
255
256void AsensingCanParser::DecodeInsGyro(
258 adata_.gyro_x =
259 (ToValue<uint16_t, float>(frame.data, 0) * GYRO_UNIT) * DEG_TO_RAD;
260 adata_.gyro_y =
261 (ToValue<uint16_t, float>(frame.data, 2) * GYRO_UNIT) * DEG_TO_RAD;
262 adata_.gyro_z =
263 (ToValue<uint16_t, float>(frame.data, 4) * GYRO_UNIT) * DEG_TO_RAD;
264}
265void AsensingCanParser::DecodeInsAngle(
267 adata_.pitch = ToValue<uint16_t, float>(frame.data, 0) * ANGLE_UNIT;
268 adata_.roll = ToValue<uint16_t, float>(frame.data, 2) * ANGLE_UNIT;
269 adata_.heading = ToValue<uint16_t, float>(frame.data, 4) * ANGLE_UNIT;
270 if (adata_.heading < 0) {
271 adata_.heading += 360;
272 }
273}
274void AsensingCanParser::DecodeInsHeightAndTime(
276 adata_.altitude = ToValue<uint32_t, double>(frame.data, 0) * ALT_UINT;
277 adata_.week_ms = ToValue<uint32_t>(frame.data, 4);
278 adata_.ins_gps_sec = adata_.gps_week * SECONDS_PER_WEEK +
279 static_cast<double>(adata_.week_ms) / 1000;
280}
281void AsensingCanParser::DecodeInsLatitudeLongitude(
283 adata_.latitude = ToValue<uint32_t, double>(frame.data, 0) * LOC_UNIT;
284 adata_.longitude = ToValue<uint32_t, double>(frame.data, 4) * LOC_UNIT;
285}
286void AsensingCanParser::DecodeInsSpeed(
288 adata_.vn = ToValue<uint16_t, float>(frame.data, 0) * SPEED_UNIT;
289 adata_.ve = ToValue<uint16_t, float>(frame.data, 2) * SPEED_UNIT;
290 adata_.vd = ToValue<uint16_t, float>(frame.data, 4) * SPEED_UNIT;
291}
292void AsensingCanParser::DecodeInsDataInfo(
294 uint8_t flag_pos = frame.data[0];
295 if (SolutionType_IsValid(flag_pos)) {
296 solution_type_ = SolutionType(flag_pos);
297 } else {
299 }
300 adata_.differential_age = frame.data[3];
301 switch (frame.data[7]) {
302 case 2:
304 break;
305 default:
307 }
308}
309void AsensingCanParser::DecodeInsStd(
311 adata_.lat_std = ToValue<uint16_t, float>(frame.data, 0) * LOC_STD_UNIT;
312 adata_.lon_std = ToValue<uint16_t, float>(frame.data, 2) * LOC_STD_UNIT;
313 adata_.alti_std = ToValue<uint16_t, float>(frame.data, 4) * LOC_STD_UNIT;
314 adata_.yaw_std = ToValue<uint16_t, float>(frame.data, 6) * HEADING_STD_UNIT;
315}
316void AsensingCanParser::DecodeGnssUtc(
318 struct tm utc_tm = {.tm_sec = frame.data[5],
319 .tm_min = frame.data[4],
320 .tm_hour = frame.data[3],
321 .tm_mday = frame.data[2],
322 .tm_mon = frame.data[1] - 1,
323 .tm_year = frame.data[0] + 2000 - 1900};
324 time_t timestamp = mktime(&utc_tm);
325 double ms = ToValue<uint16_t, double>(frame.data, 6) * 0.001;
326 if (timestamp != -1) {
327 adata_.gps_sec =
328 apollo::drivers::util::unix2gps(static_cast<double>(timestamp) + ms);
329 } else {
330 AERROR << "Invalid utc time: " << utc_tm.tm_year << "-" << utc_tm.tm_mon
331 << "-" << utc_tm.tm_mday << " " << utc_tm.tm_hour << ":"
332 << utc_tm.tm_min << ":" << utc_tm.tm_sec << "." << ms;
333 }
334}
335
336void AsensingCanParser::DecodeGpsLatitudeLongitude(
338 adata_.gps_latitude = ToValue<uint32_t, double>(frame.data, 0) * LOC_UNIT;
339 adata_.gps_longitude = ToValue<uint32_t, double>(frame.data, 4) * LOC_UNIT;
340}
341void AsensingCanParser::DecodeGpsHeightAndWeek(
343 adata_.gps_altitude = ToValue<uint32_t, double>(frame.data, 0) * ALT_UINT;
344 adata_.gps_week = ToValue<uint32_t>(frame.data, 4);
345}
346void AsensingCanParser::DecodeGpsHeadingAndPitch(
348 adata_.gps_heading = ToValue<uint16_t, float>(frame.data, 0) * ANGLE_UNIT;
349 if (adata_.gps_heading < 0) {
350 adata_.gps_heading += 360;
351 }
352 adata_.gps_pitch = ToValue<uint16_t, double>(frame.data, 2) * ANGLE_UNIT;
353}
354void AsensingCanParser::DecodeGpsStd(
356 adata_.gps_lat_std = ToValue<uint16_t, float>(frame.data, 0) * LOC_STD_UNIT;
357 adata_.gps_lon_std = ToValue<uint16_t, float>(frame.data, 2) * LOC_STD_UNIT;
358 adata_.gps_alti_std = ToValue<uint16_t, float>(frame.data, 4) * LOC_STD_UNIT;
359 adata_.gps_yaw_std =
360 ToValue<uint16_t, float>(frame.data, 6) * HEADING_STD_UNIT;
361}
362
364 bestpos_.set_measurement_time(adata_.gps_sec);
365 bestpos_.set_sol_status(solution_status_);
366 bestpos_.set_sol_type(solution_type_);
367 if (adata_.gps_latitude > 0) {
368 bestpos_.set_latitude(adata_.gps_latitude);
369 bestpos_.set_longitude(adata_.gps_longitude);
370 bestpos_.set_height_msl(adata_.gps_altitude);
371 bestpos_.set_latitude_std_dev(adata_.gps_lat_std);
372 bestpos_.set_longitude_std_dev(adata_.gps_lon_std);
373 bestpos_.set_height_std_dev(adata_.gps_alti_std);
374 } else {
375 bestpos_.set_latitude(adata_.latitude);
376 bestpos_.set_longitude(adata_.longitude);
377 bestpos_.set_height_msl(adata_.altitude);
378 bestpos_.set_latitude_std_dev(adata_.lat_std);
379 bestpos_.set_longitude_std_dev(adata_.lon_std);
380 bestpos_.set_height_std_dev(adata_.alti_std);
381 }
382 bestpos_.set_differential_age(adata_.differential_age);
383 // bestpos_.set_num_sats_tracked(0);
384 // bestpos_.set_num_sats_in_solution(0);
385 // bestpos_.set_num_sats_l1(0);
386 // bestpos_.set_num_sats_multi(0);
387}
388
390 ins_.mutable_euler_angles()->set_x(adata_.roll * DEG_TO_RAD);
391 ins_.mutable_euler_angles()->set_y(-adata_.pitch * DEG_TO_RAD);
392 ins_.mutable_euler_angles()->set_z(azimuth_deg_to_yaw_rad(adata_.heading));
393
394 ins_.mutable_position()->set_lon(adata_.longitude);
395 ins_.mutable_position()->set_lat(adata_.latitude);
396 ins_.mutable_position()->set_height(adata_.altitude);
397 ins_.mutable_linear_velocity()->set_x(adata_.ve);
398 ins_.mutable_linear_velocity()->set_y(adata_.vn);
399 ins_.mutable_linear_velocity()->set_z(-adata_.vd);
400 rfu_to_flu(adata_.acc_x, adata_.acc_y, adata_.acc_z,
401 ins_.mutable_linear_acceleration());
402 rfu_to_flu(adata_.gyro_x, adata_.gyro_y, adata_.gyro_z,
403 ins_.mutable_angular_velocity());
404 ins_.set_measurement_time(adata_.ins_gps_sec);
405 ins_.mutable_header()->set_timestamp_sec(cyber::Time::Now().ToSecond());
406
407 switch (solution_type_) {
412 ins_.set_type(Ins::GOOD);
413 break;
415 ins_.set_type(Ins::CONVERGING);
416 break;
417 default:
418 ins_.set_type(Ins::INVALID);
419 break;
420 }
421}
422
424 ins_stat_.set_ins_status(solution_status_);
425 ins_stat_.set_pos_type(solution_type_);
426}
427
429 rfu_to_flu(adata_.acc_x, adata_.acc_y, adata_.acc_z,
430 imu_.mutable_linear_acceleration());
431 rfu_to_flu(adata_.gyro_x, adata_.gyro_x, adata_.gyro_z,
432 imu_.mutable_angular_velocity());
433 imu_.set_measurement_time(adata_.ins_gps_sec);
434}
435
437 heading_.set_solution_status(solution_status_);
438 heading_.set_position_type(solution_type_);
439 heading_.set_measurement_time(adata_.gps_sec);
440 if (adata_.gps_heading > -0.1) {
441 heading_.set_heading(adata_.gps_heading);
442 heading_.set_pitch(adata_.gps_pitch);
443 heading_.set_heading_std_dev(adata_.gps_yaw_std);
444 } else {
445 heading_.set_heading(adata_.heading);
446 heading_.set_pitch(adata_.pitch);
447 heading_.set_heading_std_dev(adata_.yaw_std);
448 }
449 heading_.set_pitch_std_dev(adata_.pitch_std);
450
451 // heading_.set_station_id("0");
452 // heading_.set_satellite_number_multi(0);
453 // heading_.set_satellite_soulution_number(0);
454}
455
456} // namespace gnss
457} // namespace drivers
458} // namespace apollo
#define LOC_UNIT
#define SPEED_UNIT
#define ACC_UNIT
#define ALT_UINT
#define GYRO_UNIT
#define ANGLE_UNIT
#define HEADING_STD_UNIT
#define G_UNIT
#define LOC_STD_UNIT
Defines the CanFrame struct and CanClient interface.
static Time Now()
get the current time.
Definition time.cc:57
AsensingCanParser(const config::Config &config)
void GetMessages(MessageInfoVec *messages) override
static Parser * CreateAsensingCan(const config::Config &config)
const uint8_t * data_
Definition parser.h:145
#define ADEBUG
Definition log.h:41
#define AERROR
Definition log.h:44
#define AWARN
Definition log.h:43
constexpr uint32_t FRAME_ins_gyro
::google::protobuf::Message * MessagePtr
Definition parser.h:34
constexpr uint32_t FRAME_ins_datainfo
constexpr uint32_t FRAME_gps_std
constexpr uint32_t FRAME_ins_latitude_longitude
constexpr double DEG_TO_RAD
void rfu_to_flu(double r, double f, double u, ::apollo::common::Point3D *flu)
constexpr uint32_t FRAME_ins_height_and_time
T ToValue(const uint8_t *data, const int &index)
constexpr int SECONDS_PER_WEEK
constexpr uint32_t FRAME_ins_std
constexpr uint32_t FRAME_gps_heading_and_pitch
constexpr uint32_t FRAME_ins_speed
constexpr uint32_t FRAME_gps_height_and_week
constexpr double azimuth_deg_to_yaw_rad(double azimuth)
constexpr uint32_t FRAME_gps_latitude_longitude
std::vector< MessageInfo > MessageInfoVec
Definition parser.h:57
constexpr uint32_t FRAME_ins_acc
constexpr uint32_t FRAME_ins_angle
constexpr uint32_t FRAME_gnss_utc
T unix2gps(const T unix_seconds)
class register implement
Definition arena_queue.h:37
The class which defines the information to send and receive.
Definition can_client.h:48
std::string CanFrameString() const
CanFrame string including essential information about the message.
Definition can_client.h:69
uint8_t data[8]
Message content
Definition can_client.h:54