2 * This file is part of INAV
4 * Cleanflight is free software: you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation, either version 3 of the License, or
7 * (at your option) any later version.
9 * Cleanflight is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
14 * You should have received a copy of the GNU General Public License
15 * along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
23 #include "common/time.h"
24 #include "common/maths.h"
25 #include "common/utils.h"
26 #include "build/debug.h"
32 #define GPS_CFG_CMD_TIMEOUT_MS 500
33 #define GPS_VERSION_RETRY_TIMES 3
34 #ifndef UBLOX_MAX_SIGNALS
35 #define UBLOX_MAX_SIGNALS 64
37 #define MAX_UBLOX_PAYLOAD_SIZE ((UBLOX_MAX_SIGNALS * 16) + 8) // UBX-NAV-SIG info would be UBLOX_MAX_SIGNALS * 16 + 8
38 #define UBLOX_BUFFER_SIZE MAX_UBLOX_PAYLOAD_SIZE
39 #define UBLOX_SBAS_MESSAGE_LENGTH 16
40 #define GPS_CAPA_INTERVAL 5000
42 STATIC_ASSERT(MAX_UBLOX_PAYLOAD_SIZE
>= 256, ubx_size_too_small
);
44 #define UBX_DYNMODEL_PORTABLE 0
45 #define UBX_DYNMODEL_STATIONARY 2
46 #define UBX_DYNMODEL_PEDESTRIAN 3
47 #define UBX_DYNMODEL_AUTOMOVITE 4
48 #define UBX_DYNMODEL_SEA 5
49 #define UBX_DYNMODEL_AIR_1G 6
50 #define UBX_DYNMODEL_AIR_2G 7
51 #define UBX_DYNMODEL_AIR_4G 8
52 #define UBX_DYNMODEL_WRIST 9
53 #define UBX_DYNMODEL_BIKE 10
54 #define UBX_DYNMODEL_MOWER 11
55 #define UBX_DYNMODEL_ESCOOTER 12
57 #define UBX_FIXMODE_2D_ONLY 1
58 #define UBX_FIXMODE_3D_ONLY 2
59 #define UBX_FIXMODE_AUTO 3
61 #define UBX_VALID_GPS_DATE(valid) (valid & 1 << 0)
62 #define UBX_VALID_GPS_TIME(valid) (valid & 1 << 1)
63 #define UBX_VALID_GPS_DATE_TIME(valid) (UBX_VALID_GPS_DATE(valid) && UBX_VALID_GPS_TIME(valid))
65 #define UBX_HW_VERSION_UNKNOWN 0
66 #define UBX_HW_VERSION_UBLOX5 500
67 #define UBX_HW_VERSION_UBLOX6 600
68 #define UBX_HW_VERSION_UBLOX7 700
69 #define UBX_HW_VERSION_UBLOX8 800
70 #define UBX_HW_VERSION_UBLOX9 900
71 #define UBX_HW_VERSION_UBLOX10 1000
73 #define UBLOX_CFG_MSGOUT_NAV_POSLLH_UART1 0x2091002a // U1
74 #define UBLOX_CFG_MSGOUT_NAV_SAT_UART1 0x20910016 // U1
75 #define UBLOX_CFG_MSGOUT_NAV_SIG_UART1 0x20910346 // U1
76 #define UBLOX_CFG_MSGOUT_NAV_STATUS_UART1 0x2091001b // U1
77 #define UBLOX_CFG_MSGOUT_NAV_VELNED_UART1 0x20910043 // U1
78 #define UBLOX_CFG_MSGOUT_NAV_TIMEUTC_UART1 0x2091005c // U1
79 #define UBLOX_CFG_MSGOUT_NAV_PVT_UART1 0x20910007 // U1
80 #define UBLOX_CFG_MSGOUT_NMEA_ID_GGA_UART1 0x209100bb // U1
81 #define UBLOX_CFG_MSGOUT_NMEA_ID_GLL_UART1 0x209100ca // U1
82 #define UBLOX_CFG_MSGOUT_NMEA_ID_GSA_UART1 0x209100c0 // U1
83 #define UBLOX_CFG_MSGOUT_NMEA_ID_RMC_UART1 0x209100ac // U1
84 #define UBLOX_CFG_MSGOUT_NMEA_ID_VTG_UART1 0x209100b1 // U1
85 #define UBLOX_CFG_NAVSPG_FIXMODE 0x20110011 // E1
86 #define UBLOX_CFG_NAVSPG_DYNMODEL 0x20110021 // E1
87 #define UBLOX_CFG_RATE_MEAS 0x30210001 // U2
88 #define UBLOX_CFG_RATE_NAV 0x30210002 // U2
89 #define UBLOX_CFG_RATE_TIMEREF 0x30210002 // E1
93 #define UBLOX_CFG_SIGNAL_SBAS_ENA 0x10310020 // U1
94 #define UBLOX_CFG_SIGNAL_SBAS_L1CA_ENA 0x10310005 // U1
96 #define UBLOX_CFG_SIGNAL_GAL_ENA 0x10310021 // U1
97 #define UBLOX_CFG_SIGNAL_GAL_E1_ENA 0x10310007 // U1
99 #define UBLOX_CFG_SIGNAL_BDS_ENA 0x10310022 // U1
100 #define UBLOX_CFG_SIGNAL_BDS_B1_ENA 0x1031000d // U1
101 #define UBLOX_CFG_SIGNAL_BDS_B1C_ENA 0x1031000f // U1 default off
103 #define UBLOX_CFG_QZSS_ENA 0x10310024 // U1
104 #define UBLOX_CFG_QZSS_L1CA_ENA 0x10310012 // U1
105 #define UBLOX_CFG_QZSS_L1S_ENA 0x10310014 // U1
107 #define UBLOX_CFG_GLO_ENA 0x10310025 // U1 default off - may conflict with other constelations
108 #define UBLOX_CFG_GLO_L1_ENA 0x10310018 // U1 default off
110 #define UBLOX_CFG_SBAS_PRNSCANMASK 0x50360006 // 0 = auto // X8
111 #define UBLOX_SBAS_ALL 0x0000000000000000 //Enable search for all SBAS PRNs
112 #define UBLOX_SBAS_PRN120 0x0000000000000001 //Enable search for SBAS PRN120
113 #define UBLOX_SBAS_PRN121 0x0000000000000002 //Enable search for SBAS PRN121
114 #define UBLOX_SBAS_PRN122 0x0000000000000004 //Enable search for SBAS PRN122
115 #define UBLOX_SBAS_PRN123 0x0000000000000008 //Enable search for SBAS PRN123
116 #define UBLOX_SBAS_PRN124 0x0000000000000010 //Enable search for SBAS PRN124
117 #define UBLOX_SBAS_PRN125 0x0000000000000020 //Enable search for SBAS PRN125
118 #define UBLOX_SBAS_PRN126 0x0000000000000040 //Enable search for SBAS PRN126
119 #define UBLOX_SBAS_PRN127 0x0000000000000080 //Enable search for SBAS PRN127
120 #define UBLOX_SBAS_PRN128 0x0000000000000100 //Enable search for SBAS PRN128
121 #define UBLOX_SBAS_PRN129 0x0000000000000200 //Enable search for SBAS PRN129
122 #define UBLOX_SBAS_PRN130 0x0000000000000400 //Enable search for SBAS PRN130
123 #define UBLOX_SBAS_PRN131 0x0000000000000800 //Enable search for SBAS PRN131
124 #define UBLOX_SBAS_PRN132 0x0000000000001000 //Enable search for SBAS PRN132
125 #define UBLOX_SBAS_PRN133 0x0000000000002000 //Enable search for SBAS PRN133
126 #define UBLOX_SBAS_PRN134 0x0000000000004000 //Enable search for SBAS PRN134
127 #define UBLOX_SBAS_PRN135 0x0000000000008000 //Enable search for SBAS PRN135
128 #define UBLOX_SBAS_PRN136 0x0000000000010000 //Enable search for SBAS PRN136
129 #define UBLOX_SBAS_PRN137 0x0000000000020000 //Enable search for SBAS PRN137
130 #define UBLOX_SBAS_PRN138 0x0000000000040000 //Enable search for SBAS PRN138
131 #define UBLOX_SBAS_PRN139 0x0000000000080000 //Enable search for SBAS PRN139
132 #define UBLOX_SBAS_PRN140 0x0000000000100000 //Enable search for SBAS PRN140
133 #define UBLOX_SBAS_PRN141 0x0000000000200000 //Enable search for SBAS PRN141
134 #define UBLOX_SBAS_PRN142 0x0000000000400000 //Enable search for SBAS PRN142
135 #define UBLOX_SBAS_PRN143 0x0000000000800000 //Enable search for SBAS PRN143
136 #define UBLOX_SBAS_PRN144 0x0000000001000000 //Enable search for SBAS PRN144
137 #define UBLOX_SBAS_PRN145 0x0000000002000000 //Enable search for SBAS PRN145
138 #define UBLOX_SBAS_PRN146 0x0000000004000000 //Enable search for SBAS PRN146
139 #define UBLOX_SBAS_PRN147 0x0000000008000000 //Enable search for SBAS PRN147
140 #define UBLOX_SBAS_PRN148 0x0000000010000000 //Enable search for SBAS PRN148
141 #define UBLOX_SBAS_PRN149 0x0000000020000000 //Enable search for SBAS PRN149
142 #define UBLOX_SBAS_PRN150 0x0000000040000000 //Enable search for SBAS PRN150
143 #define UBLOX_SBAS_PRN151 0x0000000080000000 //Enable search for SBAS PRN151
144 #define UBLOX_SBAS_PRN152 0x0000000100000000 //Enable search for SBAS PRN152
145 #define UBLOX_SBAS_PRN153 0x0000000200000000 //Enable search for SBAS PRN153
146 #define UBLOX_SBAS_PRN154 0x0000000400000000 //Enable search for SBAS PRN154
147 #define UBLOX_SBAS_PRN155 0x0000000800000000 //Enable search for SBAS PRN155
148 #define UBLOX_SBAS_PRN156 0x0000001000000000 //Enable search for SBAS PRN156
149 #define UBLOX_SBAS_PRN157 0x0000002000000000 //Enable search for SBAS PRN157
150 #define UBLOX_SBAS_PRN158 0x0000004000000000 //Enable search for SBAS PRN158
183 } ubx_gnss_element_t
;
189 uint8_t numConfigBlocks
;
190 ubx_gnss_element_t config
[0];
197 } __attribute__((packed
)) ubx_config_data_header_v0_t
;
204 } __attribute__((packed
)) ubx_config_data_header_v1_t
;
206 #define UBLOX_SIG_HEALTH_MASK (BIT(0) | BIT(1))
207 #define UBLOX_SIG_PRSMOOTHED (BIT(2))
208 #define UBLOX_SIG_PRUSED (BIT(3))
209 #define UBLOX_SIG_CRUSED (BIT(4))
210 #define UBLOX_SIG_DOUSED (BIT(5))
211 #define UBLOX_SIG_PRCORRUSED (BIT(6))
212 #define UBLOX_SIG_CRCORRUSED (BIT(7))
213 #define UBLOX_SIG_DOCORRUSED (BIT(8))
214 #define UBLOX_SIG_AUTHSTATUS (BIT(9))
217 UBLOX_SIG_HEALTH_UNKNOWN
= 0,
218 UBLOX_SIG_HEALTH_HEALTHY
= 1,
219 UBLOX_SIG_HEALTH_UNHEALTHY
= 2
220 } ublox_nav_sig_health_e
;
223 UBLOX_SIG_QUALITY_NOSIGNAL
= 0,
224 UBLOX_SIG_QUALITY_SEARCHING
= 1,
225 UBLOX_SIG_QUALITY_ACQUIRED
= 2,
226 UBLOX_SIG_QUALITY_UNUSABLE
= 3,
227 UBLOX_SIG_QUALITY_CODE_LOCK_TIME_SYNC
= 4,
228 UBLOX_SIG_QUALITY_CODE_CARRIER_LOCK_TIME_SYNC
= 5,
229 UBLOX_SIG_QUALITY_CODE_CARRIER_LOCK_TIME_SYNC2
= 6,
230 UBLOX_SIG_QUALITY_CODE_CARRIER_LOCK_TIME_SYNC3
= 7,
231 } ublox_nav_sig_quality
;
234 uint8_t gnssId
; // gnssid 0 = GPS, 1 = SBAS, 2 = GALILEO, 3 = BEIDOU, 4 = IMES, 5 = QZSS, 6 = GLONASS
235 uint8_t svId
; // space vehicle ID
236 uint8_t sigId
; // signal ID
237 uint8_t freqId
; // 0-13 slot +, 0-13, glonass only
238 int16_t prRes
; // pseudo range residual (0.1m)
239 uint8_t cno
; // carrier to noise density ratio (dbHz)
240 uint8_t quality
; // 0 = no signal, 1 = search, 2 = acq, 3 = detected, 4 = code lock + time, 5,6,7 = code/carrier lock + time
241 uint8_t corrSource
; // Correction source: 0 = no correction, 1 = SBAS, 2 = BeiDou, 3 = RTCM2, 4 = RTCM3 OSR, 5 = RTCM3 SSR, 6 = QZSS SLAS, 7 = SPARTN
242 uint8_t ionoModel
; // 0 = no mode, 1 = Klobuchar GPS, 2 = SBAS, 3 = Klobuchar BeiDou, 8 = Iono derived from dual frequency observations
243 uint16_t sigFlags
; // bit:0-1 UBLOX_SIG_HEALTH_MASK
244 // bit2: pseudorange smoothed,
245 // bit3: pseudorange used,
246 // bit4: carrioer range used;
247 // bit5: doppler used
248 // bit6: pseudorange corrections used
249 // bit7: carrier correction used
250 // bit8: doper corrections used
252 } __attribute__((packed
)) ubx_nav_sig_info
;
254 STATIC_ASSERT(sizeof(ubx_nav_sig_info
) == 16, wrong_ubx_nav_sig_info_size
);
257 uint32_t time
; // GPS iToW
258 uint8_t version
; // We support version 0
259 uint8_t numSigs
; // number of signals
261 ubx_nav_sig_info sig
[UBLOX_MAX_SIGNALS
]; // 32 signals
262 } __attribute__((packed
)) ubx_nav_sig
;
265 #define MAX_GNSS_SIZE_BYTES (sizeof(ubx_gnss_msg_t) + sizeof(ubx_gnss_element_t)*MAX_GNSS)
268 uint8_t bytes
[MAX_GNSS_SIZE_BYTES
]; // placeholder
287 } __attribute__((packed
)) ubx_config_data8_payload_t
;
292 } __attribute__((packed
)) ubx_config_data16_payload_t
;
297 #define MAX_CONFIG_SET_VAL_VALUES 32
301 ubx_config_data_header_v1_t configHeader
;
303 ubx_config_data8_payload_t payload
[0];
304 uint8_t buffer
[(MAX_CONFIG_SET_VAL_VALUES
* sizeof(ubx_config_data8_payload_t
)) + 2]; // 12 key/value pairs + 2 checksum bytes
306 } __attribute__((packed
)) ubx_config_data8_t
;
310 ubx_config_data_header_v1_t configHeader
;
312 ubx_config_data16_payload_t payload
[0];
313 uint8_t buffer
[(MAX_CONFIG_SET_VAL_VALUES
* sizeof(ubx_config_data16_payload_t
)) + 2]; // 12 key/value pairs + 2 checksum bytes
315 } __attribute__((packed
)) ubx_config_data16_t
;
322 } __attribute__((packed
)) ubx_message
;
325 char swVersion
[30]; // Zero-terminated Software Version String
326 char hwVersion
[10]; // Zero-terminated Hardware Version String
330 uint32_t time
; // GPS msToW
333 int32_t altitude_ellipsoid
;
334 int32_t altitude_msl
;
335 uint32_t horizontal_accuracy
;
336 uint32_t vertical_accuracy
;
340 uint32_t time
; // GPS msToW
343 uint8_t differential_status
;
345 uint32_t time_to_first_fix
;
346 uint32_t uptime
; // milliseconds
358 uint32_t position_accuracy_3d
;
359 int32_t ecef_x_velocity
;
360 int32_t ecef_y_velocity
;
361 int32_t ecef_z_velocity
;
362 uint32_t speed_accuracy
;
363 uint16_t position_DOP
;
370 uint32_t time
; // GPS msToW
377 uint32_t speed_accuracy
;
378 uint32_t heading_accuracy
;
382 uint8_t gnssId
; // Channel number, 255 for SVx not assigned to channel
383 uint8_t svId
; // Satellite ID
384 uint8_t cno
; // Carrier to Noise Ratio (Signal Strength) // dbHz, 0-55.
385 int8_t elev
; // Elevation in integer degrees +/-90
386 int16_t azim
; // Azimuth in integer degrees 0-360
387 int16_t prRes
; // Pseudo range residual in .1m
388 uint32_t flags
; // Bitmask
389 } ubx_nav_svinfo_channel
;
391 STATIC_ASSERT(sizeof(ubx_nav_svinfo_channel
) == 12, wrong_ubx_nav_svinfo_channel_size
);
394 uint32_t itow
; // GPS Millisecond time of week
395 uint8_t version
; // Version = 0-1
396 uint8_t numSvs
; // (Space vehicle) Satelite count
397 uint16_t reserved2
; // Reserved
398 ubx_nav_svinfo_channel channel
[UBLOX_MAX_SIGNALS
]; // UBLOX_MAX_SIGNALS satellites * 12 byte
402 uint32_t time
; // GPS msToW
415 uint32_t time
; // GPS msToW
431 int32_t altitude_ellipsoid
;
432 int32_t altitude_msl
;
433 uint32_t horizontal_accuracy
;
434 uint32_t vertical_accuracy
;
440 uint32_t speed_accuracy
;
441 uint32_t heading_accuracy
;
442 uint16_t position_DOP
;
447 #define UBX_MON_GNSS_GPS_MASK (1 << 0)
448 #define UBX_MON_GNSS_GLONASS_MASK (1 << 1)
449 #define UBX_MON_GNSS_BEIDOU_MASK (1 << 2)
450 #define UBX_MON_GNSS_GALILEO_MASK (1 << 3)
454 uint8_t supported
; // bitfield for GNSS types: 0:GPS, 1:Glonass, 2:Beidou, 3:Galileo
455 uint8_t defaultGnss
; // bitfield for GNSS types: 0:GPS, 1:Glonass, 2:Beidou, 3:Galileo
456 uint8_t enabled
; // bitfield for GNSS types: 0:GPS, 1:Glonass, 2:Beidou, 3:Galileo
457 uint8_t maxConcurrent
;
483 MSG_CLASS_UBX
= 0x01,
484 MSG_CLASS_NMEA
= 0xF0,
504 MSG_CFG_SET_RATE
= 0x01,
505 MSG_CFG_NAV_SETTINGS
= 0x24,
510 } ubx_protocol_bytes_t
;
514 FIX_DEAD_RECKONING
= 1,
517 FIX_GPS_DEAD_RECKONING
= 4,
519 } ubs_nav_fix_type_t
;
522 NAV_STATUS_FIX_VALID
= 1
523 } ubx_nav_status_bits_t
;
525 uint8_t gpsUbloxMaxGnss(void);
526 timeMs_t
gpsUbloxCapLastUpdate(void);
528 bool gpsUbloxHasGalileo(void);
529 bool gpsUbloxHasBeidou(void);
530 bool gpsUbloxHasGlonass(void);
532 bool gpsUbloxGalileoDefault(void);
533 bool gpsUbloxBeidouDefault(void);
534 bool gpsUbloxGlonassDefault(void);
536 bool gpsUbloxGalileoEnabled(void);
537 bool gpsUbloxBeidouEnabled(void);
538 bool gpsUbloxGlonassEnabled(void);
539 bool gpsUbloxSendCommand(uint8_t *rawCommand
, uint16_t commandLen
, uint16_t timeout
);
541 bool isGpsUblox(void);
543 const ubx_nav_sig_info
*gpsGetUbloxSatelite(uint8_t index
);
545 bool ubloxVersionLTE(uint8_t mj
, uint8_t mn
);
546 bool ubloxVersionLT(uint8_t mj
, uint8_t mn
);
547 bool ubloxVersionGT(uint8_t mj
, uint8_t mn
);
548 bool ubloxVersionGTE(uint8_t mj
, uint8_t mn
);
549 bool ubloxVersionE(uint8_t mj
, uint8_t mn
);