Apollo 10.0
自动驾驶开放平台
novatel_messages.h
浏览该文件的文档.
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// This defines enums and structures for parsing NovAtel binary messages. Please
18// refer to NovAtel's
19// documents for details about these messages.
20// http://www.novatel.com/assets/Documents/Manuals/om-20000129.pdf
21// http://www.novatel.com/assets/Documents/Manuals/OM-20000144UM.pdf
22
23#pragma once
24
25#include <cstdint>
26#include <limits>
27
28#include "modules/drivers/gnss/proto/config.pb.h"
29
30namespace apollo {
31namespace drivers {
32namespace gnss {
33namespace novatel {
34
35enum MessageId : uint16_t {
38 BESTPOS = 42,
39 BESTVEL = 99,
42 INSCOV = 264,
43 INSCOVS = 320,
44 INSPVA = 507,
45 INSPVAS = 508,
46 INSPVAX = 1465,
47 PSRPOS = 47,
48 PSRVEL = 100,
49 RAWIMU = 268,
50 RAWIMUS = 325,
51 RAWIMUX = 1461,
52 RAWIMUSX = 1462,
53 MARK1PVA = 1067,
54 GPGGA = 218,
58 RANGE = 43,
59 HEADING = 971,
61};
62
63// Every binary message has 32-bit CRC performed on all data including the
64// header.
65constexpr uint16_t CRC_LENGTH = 4;
66
67#pragma pack(push, 1) // Turn off struct padding.
68
69enum SyncByte : uint8_t {
70 SYNC_0 = 0xAA,
71 SYNC_1 = 0x44,
74};
75
78 BINARY = 0b00,
79 ASCII = 0b01,
81 NMEA = 0b11,
82 };
83
88
89 uint8_t reserved : 5;
92};
93
94struct LongHeader {
99 uint8_t port_address; // Address of the data port the log was received on.
100 uint16_t
101 message_length; // Message length (not including the header nor the CRC).
102 uint16_t sequence; // Counts down from N-1 to 0 for multiple related logs.
103 uint8_t idle; // Time the processor was idle in last second between logs with
104 // same ID.
105 uint8_t time_status; // Indicates the quality of the GPS time.
106 uint16_t gps_week; // GPS Week number.
107 uint32_t gps_millisecs; // Milliseconds of week.
108 uint32_t status; // Receiver status.
109 uint16_t reserved;
110 uint16_t version; // Receiver software build number.
111};
112static_assert(sizeof(LongHeader) == 28, "Incorrect size of LongHeader");
113
116 uint8_t
117 message_length; // Message length (not including the header nor the CRC).
119 uint16_t gps_week; // GPS Week number.
120 uint32_t gps_millisecs; // Milliseconds of week.
121};
122static_assert(sizeof(ShortHeader) == 12, "Incorrect size of ShortHeader");
123
124enum class SolutionStatus : uint32_t {
125 SOL_COMPUTED = 0, // solution computed
126 INSUFFICIENT_OBS, // insufficient observations
127 NO_CONVERGENCE, // no convergence
128 SINGULARITY, // singularity at parameters matrix
129 COV_TRACE, // covariance trace exceeds maximum (trace > 1000 m)
130 TEST_DIST, // test distance exceeded (max of 3 rejections if distance > 10
131 // km)
132 COLD_START, // not yet converged from cold start
133 V_H_LIMIT, // height or velocity limits exceeded
134 VARIANCE, // variance exceeds limits
135 RESIDUALS, // residuals are too large
136 INTEGRITY_WARNING = 13, // large residuals make position questionable
137 PENDING = 18, // receiver computes its position and determines if the fixed
138 // position is valid
139 INVALID_FIX = 19, // the fixed position entered using the fix position
140 // command is invalid
141 UNAUTHORIZED = 20, // position type is unauthorized
143 22, // selected logging rate is not supported for this solution type
144 NONE = std::numeric_limits<uint32_t>::max(),
145};
146
147enum class SolutionType : uint32_t {
148 NONE = 0,
149 FIXEDPOS = 1,
150 FIXEDHEIGHT = 2,
151 FLOATCONV = 4,
152 WIDELANE = 5,
153 NARROWLANE = 6,
155 SINGLE = 16,
156 PSRDIFF = 17,
157 WAAS = 18,
158 PROPOGATED = 19,
159 OMNISTAR = 20,
160 L1_FLOAT = 32,
161 IONOFREE_FLOAT = 33,
162 NARROW_FLOAT = 34,
163 L1_INT = 48,
164 WIDE_INT = 49,
165 NARROW_INT = 50,
166 RTK_DIRECT_INS = 51, // RTK filter is directly initialized
167 // from the INS filter.
168 INS_SBAS = 52,
169 INS_PSRSP = 53,
170 INS_PSRDIFF = 54,
171 INS_RTKFLOAT = 55,
172 INS_RTKFIXED = 56,
173 INS_OMNISTAR = 57,
174 INS_OMNISTAR_HP = 58,
175 INS_OMNISTAR_XP = 59,
176 OMNISTAR_HP = 64,
177 OMNISTAR_XP = 65,
178 PPP_CONVERGING = 68,
179 PPP = 69,
181 INS_PPP = 74,
182};
183
184enum class DatumId : uint32_t {
185 // We only use WGS-84.
186 WGS84 = 61,
187};
188
190 uint32_t satellite_id; // ID/ranging code
191 uint32_t week; // week number
192 double ura; // user range accuracy(meters).
193 // This is the evaluated URAI/URA lookup-table value
194 uint32_t health1; // Autonomous satellite health flag.
195 // 0 means broadcasting satellite is good, 1 means not
196 double tdg1; // Equipment group delay differential for the B1 signal(seconds)
197 double tdg2; // Equipment group delay differential for the B2 signal(seconds)
198 uint32_t aodc; // Age of data, clock
199 uint32_t toc; // Reference time of clock parameters
200 double a0; // Constant term of clock correction polynomial(seconds)
201 double a1; // Linear term of clock correction polynomial (seconds/seconds)
202 double a2; // Quadratic term of clock correction polynomial
203 // (second/seconds^2)
204 uint32_t aode; // Age of data, ephemeris
205 uint32_t toe; // Reference time of ephemeris parameters
206 double rootA; // Square root of semi-major axis (sqrt(meters))
207 double ecc; // Eccentricity (sqrt(meters))
208 double omega; // Argument of perigee
209 double delta_N; // Mean motion difference from computed value
210 // (radians/second)
211 double M0; // Mean anomaly at reference time (radians)
212 double omega0; // Longitude of ascending node of orbital of plane computed
213 // according to reference time(radians)
214 double rra; // Rate of right ascension(radians/second)
215 double inc_angle; // inclination angle at reference time(radians)
216 double idot; // Rate of inclination angle(radians/second)
217 double cuc; // Amplitude of cosine harmonic correction term to the argument
218 // of latitude(radians)
219 double cus; // Amplitude of sine harmonic correction term to the argument
220 // of latitude(radians)
221 double crc; // Amplitude of cosine harmonic correction term to
222 // the orbit radius(meters)
223 double crs; // Amplitude of sine harmonic correction term to
224 // the orbit radius(meters)
225 double cic; // Amplitude of cosine harmonic correction term to the angle of
226 // inclination(radians)
227 double cis; // Amplitude of sine harmonic correction term to the angle of
228 // inclination(radians)
229};
230static_assert(sizeof(BDS_Ephemeris) == 196, "Incorrect size of BDS_Ephemeris.");
231
233 uint16_t sloto; // Slot information offset-PRNidentification(Slot+37).
234 uint16_t freqo; // frequency channel offset for satellite
235 // in the range 0 to 20
236 uint8_t sat_type; // Satellite type where(0=GLO_SAT, 1=GLO_SAT_M,
237 // 2=GLO_SAT_K)
238 uint8_t reserved_1; // Reserved
239 uint16_t e_week; // reference week of ephemeris(GPS reference time)
240 uint32_t e_time; // reference time of ephemeris(GPS reference time) in ms
241 uint32_t t_offset; // integer seconds between GPS and GLONASS time.
242 // A positive value implies GLONASS is ahead
243 // of GPS reference time.
244 uint16_t Nt; // Calendar number of day within 4 year interval starting at
245 // Jan 1 of leap year
246 uint8_t reserved_2; // Reserved
247 uint8_t reserved_3; // Reserved
248 uint32_t issue; // 15 minute interval number corresponding to ephemeris
249 // reference time
250 uint32_t health; // Ephemeris health where 0-3=GOOD, 4-15=BAD
251 double pos_x; // X coordinate for satellite at reference time (PZ-90.02),
252 // in meters
253 double pos_y; // Y coordinate for satellite at reference time (PZ-90.02),
254 // in meters
255 double pos_z; // Z coordinate for satellite at reference time (PZ-90.02),
256 // in meters
257 double vel_x; // X coordinate for satellite velocity at reference
258 // time(PZ-90.02), in meters/s
259 double vel_y; // Y coordinate for satellite velocity at reference
260 // time(PZ-90.02), in meters/s
261 double vel_z; // Z coordinate for satellite velocity at reference
262 // time(PZ-90.02), in meters/s
263 double acc_x; // X coordinate for lunisolar acceleration at reference
264 // time(PZ-90.02), in meters/s/s
265 double acc_y; // Y coordinate for lunisolar acceleration at reference
266 // time(PZ-90.02), in meters/s/s
267 double acc_z; // Z coordinate for lunisolar acceleration at reference
268 // time(PZ-90.02), in meters/s/s
269 double tau_n; // Correction to the nth satellite time t_n relative to
270 // GLONASS time_t, in seconds
271 double delta_tau_n; // Time difference between navigation RF signal
272 // transmitted in L2 sub-band and
273 // navigation RF signal transmitted in
274 // L1 sub-band by nth satellite , in seconds
275 double gamma; // frequency correction , in seconds/second
276 uint32_t Tk; // Time of frame start(since start of GLONASS day), in seconds
277 uint32_t P; // technological parameter
278 uint32_t Ft; // User range
279 uint32_t age; // age of data, in days
280 uint32_t Flags; // information flags
281};
282static_assert(sizeof(GLO_Ephemeris) == 144, "Incorrect size of GLO_Ephemeris.");
283
285 uint32_t prn; // Satellite prn number
286 double tow; // Time stamp of subframe 0
287 uint32_t health; // Health status -a 6-bit health code as defined in
288 // ICD-GPS-200
289 uint32_t iode1; // issue of ephemeris data 1
290 uint32_t iode2; // issue of ephemeris data 2
291 uint32_t week; // GPS reference week number
292 uint32_t z_week; // Z count week number
293 double toe; // reference time for ephemeris, seconds
294 double A; // semi-major axis, metres
295 double delta_A; // Mean motion difference, radians/second
296 double M_0; // Mean anomaly of reference time, radians
297 double ecc; // Eccentricity. dimensionless-quantity defined for
298 // a conic section where e=0 is a circle, e=1 is a parabola
299 // 0<e<1 os an ellipse and e>1 is a hyperbola
300 double omega; // Argument of perigee, radians -measurement along the
301 // orbital path from the ascending node to the point where
302 // the SV os closest to the earth, in the direction of
303 // the SV's motion
304 double cuc; // Argument of latitude
305 double cus; // Argument of latitude
306 double crc; // Orbit radius
307 double crs; // Orbit radius
308 double cic; // Inclination
309 double cis; // Inclination
310 double I_0; // Inclination angle at reference time, radians
311 double dot_I; // Rate of inclination angle, radians/second
312 double omega_0; // right ascension, radians
313 double dot_omega; // rate of right ascension, radians/second
314 uint32_t iodc; // issue of data clock
315 double toc; // SV clock correction term, seconds
316 double tgd; // Estimated group delay difference seconds
317 double af0; // Clock aging parameter. seconds
318 double af1; // Clock aging parameter
319 double af2; // Clock aging parameter
320 uint32_t AS; // Anti-spoofing on : 0=false, 1=true
321 double N; // Corrected mean motion, radians/second
322 double ura; // User Range Acceracy variance.
323};
324static_assert(sizeof(GPS_Ephemeris) == 224, "Incorrect size of GPS_Ephemeris.");
325
326struct BestPos {
329 double latitude; // in degrees
330 double longitude; // in degrees
331 double height_msl; // height above mean sea level in meters
332 float undulation; // undulation = height_wgs84 - height_msl
333 DatumId datum_id; // datum id number
334 float latitude_std_dev; // latitude standard deviation (m)
335 float longitude_std_dev; // longitude standard deviation (m)
336 float height_std_dev; // height standard deviation (m)
337 char base_station_id[4]; // base station id
338 float differential_age; // differential position age (sec)
339 float solution_age; // solution age (sec)
340 uint8_t num_sats_tracked; // number of satellites tracked
341 uint8_t num_sats_in_solution; // number of satellites used in solution
342 uint8_t num_sats_l1; // number of L1/E1/B1 satellites used in solution
343 uint8_t
344 num_sats_multi; // number of multi-frequency satellites used in solution
345 uint8_t reserved; // reserved
346 uint8_t extended_solution_status; // extended solution status - OEMV and
347 // greater only
350};
351static_assert(sizeof(BestPos) == 72, "Incorrect size of BestPos");
352
353struct BestVel {
354 SolutionStatus solution_status; // Solution status
356 float latency; // measure of the latency of the velocity time tag in seconds
357 float age; // differential age in seconds
358 double horizontal_speed; // horizontal speed in m/s
359 double track_over_ground; // direction of travel in degrees
360 double vertical_speed; // vertical speed in m/s
361 float reserved;
362};
363static_assert(sizeof(BestVel) == 44, "Incorrect size of BestVel");
364
365// IMU data corrected for gravity, the earth's rotation and estimated sensor
366// errors.
368 uint32_t gps_week;
369 double gps_seconds; // seconds of week
370 // All the measurements are in the SPAN computational frame: right, forward,
371 // up.
372 double x_angle_change; // change in angle around x axis in radians
373 double y_angle_change; // change in angle around y axis in radians
374 double z_angle_change; // change in angle around z axis in radians
375 double x_velocity_change; // change in velocity along x axis in m/s
376 double y_velocity_change; // change in velocity along y axis in m/s
377 double z_velocity_change; // change in velocity along z axis in m/s
378};
379static_assert(sizeof(CorrImuData) == 60, "Incorrect size of CorrImuData");
380
381struct InsCov {
382 uint32_t gps_week;
383 double gps_seconds; // seconds of week
384 double position_covariance[9]; // Position covariance matrix [m^2]
385 // (xx,xy,xz,yz,yy,...)
386 double attitude_covariance[9]; // Attitude covariance matrix [deg^2]
387 // (xx,xy,xz,yz,yy,...)
388 double velocity_covariance[9]; // Velocity covariance matrix [(m/s)^2]
389 // (xx,xy,xz,yz,yy,...)
390};
391static_assert(sizeof(InsCov) == 228, "Incorrect size of InsCov");
392
393enum class InsStatus : uint32_t {
394 INACTIVE = 0,
395 ALIGNING,
398 SOLUTION_FREE = 6,
402 NONE = std::numeric_limits<uint32_t>::max(),
403};
404
405struct InsPva {
406 uint32_t gps_week;
407 double gps_seconds; // seconds of week
408 double latitude; // in degrees
409 double longitude; // in degrees
410 double height; // Ellipsoidal height - WGS84 (m)
411 double north_velocity; // velocity in a northerly direction (m/s)
412 double east_velocity; // velocity in an easterly direction (m/s)
413 double up_velocity; // velocity in an up direction
414 double roll; // right handed rotation around y-axis (degrees)
415 double pitch; // right handed rotation around x-axis (degrees)
416 double azimuth; // left handed rotation around z-axis (degrees)
417 InsStatus status; // status of the INS system
418};
419static_assert(sizeof(InsPva) == 88, "Incorrect size of InsPva");
420
421struct InsPvaX {
422 uint32_t ins_status;
423 uint32_t pos_type;
424 double latitude; // in degrees
425 double longitude; // in degrees
426 double height; // Ellipsoidal height - WGS84 (m)
428
429 double north_velocity; // velocity in a northerly direction (m/s)
430 double east_velocity; // velocity in an easterly direction (m/s)
431 double up_velocity; // velocity in an up direction
432 double roll; // right handed rotation around y-axis (degrees)
433 double pitch; // right handed rotation around x-axis (degrees)
434 double azimuth; // left handed rotation around z-axis (degrees)
435
442
443 float roll_std;
446 uint32_t ext_slo_stat;
447 uint16_t elapsed_time;
448};
449static_assert(sizeof(InsPvaX) == 126, "Incorrect size of InsPvaX");
450
451// enum class ImuType : uint8_t {
452// // We currently use the following IMUs. We'll extend this list when a new
453// IMU
454// // is introduced.
455// IMAR_FSAS = 13, // iMAR iIMU-FSAS
456// ISA100C = 26, // Northrop Grumman Litef ISA-100C
457// ADIS16488 = 31, // Analog Devices ADIS16488
458// STIM300 = 32, // Sensonor STIM300
459// ISA100 = 34, // Northrop Grumman Litef ISA-100
460// ISA100_400HZ = 38, // Northrop Grumman Litef ISA-100
461// ISA100C_400HZ = 39, // Northrop Grumman Litef ISA-100
462// G320N = 40, // EPSON G320N
463// CPT_X25651 = 41, // IMU@SPAN-CPT, and XingWangYuda 5651
464// BMI055 = 42 // BOSCH BMI055 IMU
465// };
466
467struct RawImuX {
468 uint8_t imu_error; // Simple IMU error flag. 0 means IMU okay.
469 uint8_t imu_type;
470 uint16_t gps_week;
471 double gps_seconds; // Seconds of week.
472 uint32_t imuStatus; // Status of the IMU. The content varies with IMU type.
473 // All the measurements are in the IMU reference frame. Scale factors varies
474 // with IMU type.
475 int32_t z_velocity_change; // change in velocity along z axis.
476 int32_t y_velocity_change_neg; // -change in velocity along y axis.
477 int32_t x_velocity_change; // change in velocity along x axis.
478 int32_t z_angle_change; // change in angle around z axis.
479 int32_t y_angle_change_neg; // -change in angle around y axis.
480 int32_t x_angle_change; // change in angle around x axis.
481};
482static_assert(sizeof(RawImuX) == 40, "Incorrect size of RawImuX");
483
484struct RawImu {
485 uint32_t gps_week;
486 double gps_seconds; // Seconds of week.
487 uint32_t imuStatus; // Status of the IMU. The content varies with IMU type.
488 int32_t z_velocity_change; // change in velocity along z axis.
489 int32_t y_velocity_change_neg; // -change in velocity along y axis.
490 int32_t x_velocity_change; // change in velocity along x axis.
491 int32_t z_angle_change; // change in angle around z axis.
492 int32_t y_angle_change_neg; // -change in angle around y axis.
493 int32_t x_angle_change; // change in angle around x axis.
494};
495static_assert(sizeof(RawImu) == 40, "Incorrect size of RawImu");
496
497struct Heading {
500 float length;
501 float heading;
502 float pitch;
503 float reserved;
506 char station_id[4]; // station id
507 uint8_t num_sats_tracked; // number of satellites tracked
508 uint8_t num_sats_in_solution; // number of satellites used in solution
510 uint8_t num_sats_l2;
515};
516static_assert(sizeof(Heading) == 44, "Incorrect size of Heading");
517
518#pragma pack(pop) // Back to whatever the previous packing mode was.
519
525
526using ::apollo::drivers::gnss::config::ImuType;
527inline ImuParameter GetImuParameter(ImuType type) {
528 switch (type) {
529 case ImuType::IMAR_FSAS:
530 // 0.1 * (2 ** -8) * (math.pi / 180 / 3600), (0.05 * (2 ** -15)
531 return {1.893803441835e-9, 1.52587890625e-6, 200.0};
532
533 case ImuType::ADIS16488:
534 // 720/2**31 deg/LSB, 200/2**31 m/s/LSB
535 return {5.8516723170686385e-09, 9.31322574615478515625e-8, 200.0};
536
537 case ImuType::STIM300:
538 // 2**-21 deg/LSB, 2**-22 m/s/LSB
539 return {8.32237840649762e-09, 2.384185791015625e-07, 125.0};
540
541 case ImuType::ISA100:
542 case ImuType::ISA100C:
543 // 1.0e-9 rad/LSB, 2.0e-8 m/s/LSB
544 return {1.0e-9, 2.0e-8, 200.0};
545
546 case ImuType::ISA100_400HZ:
547 case ImuType::ISA100C_400HZ:
548 return {1.0e-9, 2.0e-8, 400.0};
549
550 case ImuType::G320N:
551 return {1.7044230976507124e-11, 2.3929443359375006e-10, 125.0};
552
553 case ImuType::CPT_XW5651:
554 return {1.0850694444444445e-07, 1.52587890625e-06, 100.0};
555
556 case ImuType::UM442:
557 return {6.6581059144655048e-6, 2.99127170628e-5, 20.0};
558
559 case ImuType::IAM20680:
560 // (1.0/65.5)/125.0 deg/LSB (1.0/8192.0)*9.80665/125.0 m/s/LSB
561 return {0.0001221374045, 9.57680664e-06, 125};
562
563 default:
564 return {0.0, 0.0, 0.0};
565 }
566}
567
568} // namespace novatel
569} // namespace gnss
570} // namespace drivers
571} // namespace apollo
ImuParameter GetImuParameter(ImuType type)
class register implement
Definition arena_queue.h:37