Skip to content

Commit c4b4eca

Browse files
wrsolichinjulianoesdakejahl
authored andcommitted
Expose u-blox min elevation, min SNR and DGNSS timeout parameters RTK Fix Improvements (PX4#25720)
* Exposing u-Blox min CNO, min elevation, and DGNSS timeout for RTK Fix Improvement * update gps submodule --------- Co-authored-by: Julian Oes <julian@oes.ch> Co-authored-by: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> Co-authored-by: Jacob Dahl <dahl.jakejacob@gmail.com>
1 parent 0926604 commit c4b4eca

3 files changed

Lines changed: 366 additions & 0 deletions

File tree

src/drivers/gps/gps.cpp

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -798,13 +798,16 @@ GPS::run()
798798
param_get(handle, &gps_ubx_min_elev);
799799
}
800800

801+
<<<<<<< HEAD
801802
int32_t gps_ubx_rate = 0;
802803
handle = param_find("GPS_UBX_RATE");
803804

804805
if (handle != PARAM_INVALID) {
805806
param_get(handle, &gps_ubx_rate);
806807
}
807808

809+
=======
810+
>>>>>>> 58fbdcdff4 (Expose u-blox min elevation, min SNR and DGNSS timeout parameters RTK Fix Improvements (#25720))
808811
handle = param_find("GPS_UBX_MODE");
809812

810813
GPSDriverUBX::UBXMode ubx_mode{GPSDriverUBX::UBXMode::Normal};
@@ -964,13 +967,22 @@ GPS::run()
964967
.dgnss_timeout = (uint8_t)gps_ubx_dgnss_to,
965968
.min_cno = (uint8_t)gps_ubx_min_cno,
966969
.min_elev = (int8_t)gps_ubx_min_elev,
970+
<<<<<<< HEAD
967971
.output_rate = (uint8_t)gps_ubx_rate,
968972
.heading_offset = heading_offset,
969973
.uart2_baudrate = f9p_uart2_baudrate,
970974
.ppk_output = ppk_output > 0,
971975
.jam_det_sensitivity_hi = jam_det_sensitivity_hi > 0,
972976
.mode = ubx_mode,
973977
};
978+
=======
979+
.heading_offset = heading_offset,
980+
.uart2_baudrate = f9p_uart2_baudrate,
981+
.ppk_output = ppk_output > 0,
982+
.mode = ubx_mode,
983+
};
984+
985+
>>>>>>> 58fbdcdff4 (Expose u-blox min elevation, min SNR and DGNSS timeout parameters RTK Fix Improvements (#25720))
974986
_helper = new GPSDriverUBX(_interface, &GPS::callback, this, &_sensor_gps, _p_report_sat_info, settings);
975987

976988
set_device_type(DRV_GPS_DEVTYPE_UBX);

src/drivers/gps/params.c

Lines changed: 350 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,350 @@
1+
/****************************************************************************
2+
*
3+
* Copyright (c) 2016-2024 PX4 Development Team. All rights reserved.
4+
*
5+
* Redistribution and use in source and binary forms, with or without
6+
* modification, are permitted provided that the following conditions
7+
* are met:
8+
*
9+
* 1. Redistributions of source code must retain the above copyright
10+
* notice, this list of conditions and the following disclaimer.
11+
* 2. Redistributions in binary form must reproduce the above copyright
12+
* notice, this list of conditions and the following disclaimer in
13+
* the documentation and/or other materials provided with the
14+
* distribution.
15+
* 3. Neither the name PX4 nor the names of its contributors may be
16+
* used to endorse or promote products derived from this software
17+
* without specific prior written permission.
18+
*
19+
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20+
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21+
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22+
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23+
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24+
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25+
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26+
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27+
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28+
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29+
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30+
* POSSIBILITY OF SUCH DAMAGE.
31+
*
32+
****************************************************************************/
33+
34+
/**
35+
* Log GPS communication data
36+
*
37+
* If this is set to 1, all GPS communication data will be published via uORB,
38+
* and written to the log file as gps_dump message.
39+
*
40+
* If this is set to 2, the main GPS is configured to output RTCM data,
41+
* which is then logged as gps_dump and can be used for PPK.
42+
*
43+
* @min 0
44+
* @max 2
45+
* @value 0 Disable
46+
* @value 1 Full communication
47+
* @value 2 RTCM output (PPK)
48+
* @group GPS
49+
*/
50+
PARAM_DEFINE_INT32(GPS_DUMP_COMM, 0);
51+
52+
/**
53+
* u-blox GPS dynamic platform model
54+
*
55+
* u-blox receivers support different dynamic platform models to adjust the navigation engine to
56+
* the expected application environment.
57+
*
58+
* @min 0
59+
* @max 9
60+
* @value 2 stationary
61+
* @value 4 automotive
62+
* @value 6 airborne with <1g acceleration
63+
* @value 7 airborne with <2g acceleration
64+
* @value 8 airborne with <4g acceleration
65+
*
66+
* @reboot_required true
67+
*
68+
* @group GPS
69+
*/
70+
PARAM_DEFINE_INT32(GPS_UBX_DYNMODEL, 7);
71+
72+
/**
73+
* u-blox GPS DGNSS timeout
74+
*
75+
* When set to 0 (default), default DGNSS timeout set by u-blox will be used.
76+
*
77+
* @min 0
78+
* @max 255
79+
* @unit s
80+
*
81+
* @reboot_required true
82+
*
83+
* @group GPS
84+
*/
85+
PARAM_DEFINE_INT32(GPS_UBX_DGNSS_TO, 0);
86+
87+
/**
88+
* u-blox GPS minimum satellite signal level for navigation
89+
*
90+
* When set to 0 (default), default minimum satellite signal level set by u-blox wll be used.
91+
*
92+
* @min 0
93+
* @max 255
94+
* @unit dBHz
95+
*
96+
* @reboot_required true
97+
*
98+
* @group GPS
99+
*/
100+
PARAM_DEFINE_INT32(GPS_UBX_MIN_CNO, 0);
101+
102+
/**
103+
* u-blox GPS minimum elevation for a GNSS satellite to be used in navigation
104+
*
105+
* When set to 0 (default), default minimum elevation set by u-blox will be used.
106+
*
107+
* @min 0
108+
* @max 127
109+
* @unit deg
110+
*
111+
* @reboot_required true
112+
*
113+
* @group GPS
114+
*/
115+
PARAM_DEFINE_INT32(GPS_UBX_MIN_ELEV, 0);
116+
117+
/**
118+
* Enable sat info (if available)
119+
*
120+
* Enable publication of satellite info (ORB_ID(satellite_info)) if possible.
121+
* Not available on MTK.
122+
*
123+
* @boolean
124+
* @reboot_required true
125+
* @group GPS
126+
*/
127+
PARAM_DEFINE_INT32(GPS_SAT_INFO, 0);
128+
129+
/**
130+
* u-blox GPS Mode
131+
*
132+
* Select the u-blox configuration setup. Most setups will use the default, including RTK and
133+
* dual GPS without heading.
134+
*
135+
* If rover has RTCM corrections from a static base (or other static correction source) coming in on UART2, then select Mode 5.
136+
* The Heading mode requires 2 F9P devices to be attached. The main GPS will act as rover and output
137+
* heading information, whereas the secondary will act as moving base.
138+
* Modes 1 and 2 require each F9P UART1 to be connected to the Autopilot. In addition, UART2 on the
139+
* F9P units are connected to each other.
140+
* Modes 3 and 4 only require UART1 on each F9P connected to the Autopilot or Can Node. UART RX DMA is required.
141+
* RTK is still possible with this setup.
142+
* Mode 6 is intended for use with a ground control station (not necessarily an RTK correction base).
143+
*
144+
* @min 0
145+
* @max 1
146+
* @value 0 Default
147+
* @value 1 Heading (Rover With Moving Base UART1 Connected To Autopilot, UART2 Connected To Moving Base)
148+
* @value 2 Moving Base (UART1 Connected To Autopilot, UART2 Connected To Rover)
149+
* @value 3 Heading (Rover With Moving Base UART1 Connected to Autopilot Or Can Node At 921600)
150+
* @value 4 Moving Base (Moving Base UART1 Connected to Autopilot Or Can Node At 921600)
151+
* @value 5 Rover with Static Base on UART2 (similar to Default, except coming in on UART2)
152+
* @value 6 Ground Control Station (UART2 outputs NMEA)
153+
*
154+
* @reboot_required true
155+
* @group GPS
156+
*/
157+
PARAM_DEFINE_INT32(GPS_UBX_MODE, 0);
158+
159+
160+
/**
161+
* u-blox F9P UART2 Baudrate
162+
*
163+
* Select a baudrate for the F9P's UART2 port.
164+
* In GPS_UBX_MODE 1, 2, and 3, the F9P's UART2 port is configured to send/receive RTCM corrections.
165+
* Set this to 57600 if you want to attach a telemetry radio on UART2.
166+
*
167+
* @min 0
168+
* @unit B/s
169+
*
170+
* @reboot_required true
171+
* @group GPS
172+
*/
173+
PARAM_DEFINE_INT32(GPS_UBX_BAUD2, 230400);
174+
175+
/**
176+
* u-blox protocol configuration for interfaces
177+
*
178+
* @min 0
179+
* @max 32
180+
* @bit 0 Enable I2C input protocol UBX
181+
* @bit 1 Enable I2C input protocol NMEA
182+
* @bit 2 Enable I2C input protocol RTCM3X
183+
* @bit 3 Enable I2C output protocol UBX
184+
* @bit 4 Enable I2C output protocol NMEA
185+
* @bit 5 Enable I2C output protocol RTCM3X
186+
*
187+
* @reboot_required true
188+
* @group GPS
189+
*/
190+
PARAM_DEFINE_INT32(GPS_UBX_CFG_INTF, 0);
191+
192+
/**
193+
* Enable MSM7 message output for PPK workflow.
194+
*
195+
* @boolean
196+
* @reboot_required true
197+
* @group GPS
198+
*/
199+
PARAM_DEFINE_INT32(GPS_UBX_PPK, 0);
200+
201+
/**
202+
* Wipes the flash config of UBX modules.
203+
*
204+
* Some UBX modules have a FLASH that allows to store persistent configuration that will be loaded on start.
205+
* PX4 does override all configuration parameters it needs in RAM, which takes precedence over the values in FLASH.
206+
* However, configuration parameters that are not overriden by PX4 can still cause unexpected problems during flight.
207+
* To avoid these kind of problems a clean config can be reached by wiping the FLASH on boot.
208+
*
209+
* Note: Currently only supported on UBX.
210+
*
211+
* @reboot_required true
212+
* @group GPS
213+
* @boolean
214+
*/
215+
PARAM_DEFINE_INT32(GPS_CFG_WIPE, 0);
216+
217+
/**
218+
* Heading/Yaw offset for dual antenna GPS
219+
*
220+
* Heading offset angle for dual antenna GPS setups that support heading estimation.
221+
*
222+
* Set this to 0 if the antennas are parallel to the forward-facing direction
223+
* of the vehicle and the rover (or Unicore primary) antenna is in front.
224+
*
225+
* The offset angle increases clockwise.
226+
*
227+
* Set this to 90 if the rover (or Unicore primary, or Septentrio Mosaic Aux)
228+
* antenna is placed on the right side of the vehicle and the moving base
229+
* antenna is on the left side.
230+
*
231+
* (Note: the Unicore primary antenna is the one connected on the right as seen
232+
* from the top).
233+
*
234+
* @min 0
235+
* @max 360
236+
* @unit deg
237+
* @reboot_required true
238+
* @decimal 3
239+
*
240+
* @group GPS
241+
*/
242+
PARAM_DEFINE_FLOAT(GPS_YAW_OFFSET, 0.f);
243+
244+
/**
245+
* Protocol for Main GPS
246+
*
247+
* Select the GPS protocol over serial.
248+
*
249+
* Auto-detection will probe all protocols, and thus is a bit slower.
250+
*
251+
* @min 0
252+
* @max 7
253+
* @value 0 Auto detect
254+
* @value 1 u-blox
255+
* @value 2 MTK
256+
* @value 3 Ashtech / Trimble
257+
* @value 4 Emlid Reach
258+
* @value 5 Femtomes
259+
* @value 6 NMEA (generic)
260+
*
261+
* @reboot_required true
262+
* @group GPS
263+
*/
264+
PARAM_DEFINE_INT32(GPS_1_PROTOCOL, 1);
265+
266+
/**
267+
* Protocol for Secondary GPS
268+
*
269+
* Select the GPS protocol over serial.
270+
*
271+
* Auto-detection will probe all protocols, and thus is a bit slower.
272+
*
273+
* @min 0
274+
* @max 6
275+
* @value 0 Auto detect
276+
* @value 1 u-blox
277+
* @value 2 MTK
278+
* @value 3 Ashtech / Trimble
279+
* @value 4 Emlid Reach
280+
* @value 5 Femtomes
281+
* @value 6 NMEA (generic)
282+
*
283+
* @reboot_required true
284+
* @group GPS
285+
*/
286+
PARAM_DEFINE_INT32(GPS_2_PROTOCOL, 1);
287+
288+
/**
289+
* GNSS Systems for Primary GPS (integer bitmask)
290+
*
291+
* This integer bitmask controls the set of GNSS systems used by the receiver. Check your
292+
* receiver's documentation on how many systems are supported to be used in parallel.
293+
*
294+
* Currently this functionality is just implemented for u-blox receivers.
295+
*
296+
* When no bits are set, the receiver's default configuration should be used.
297+
*
298+
* Set bits true to enable:
299+
* 0 : Use GPS (with QZSS)
300+
* 1 : Use SBAS (multiple GPS augmentation systems)
301+
* 2 : Use Galileo
302+
* 3 : Use BeiDou
303+
* 4 : Use GLONASS
304+
* 5 : Use NAVIC
305+
*
306+
* @min 0
307+
* @max 63
308+
* @bit 0 GPS (with QZSS)
309+
* @bit 1 SBAS
310+
* @bit 2 Galileo
311+
* @bit 3 BeiDou
312+
* @bit 4 GLONASS
313+
* @bit 5 NAVIC
314+
*
315+
* @reboot_required true
316+
* @group GPS
317+
*/
318+
PARAM_DEFINE_INT32(GPS_1_GNSS, 0);
319+
320+
/**
321+
* GNSS Systems for Secondary GPS (integer bitmask)
322+
*
323+
* This integer bitmask controls the set of GNSS systems used by the receiver. Check your
324+
* receiver's documentation on how many systems are supported to be used in parallel.
325+
*
326+
* Currently this functionality is just implemented for u-blox receivers.
327+
*
328+
* When no bits are set, the receiver's default configuration should be used.
329+
*
330+
* Set bits true to enable:
331+
* 0 : Use GPS (with QZSS)
332+
* 1 : Use SBAS (multiple GPS augmentation systems)
333+
* 2 : Use Galileo
334+
* 3 : Use BeiDou
335+
* 4 : Use GLONASS
336+
* 5 : Use NAVIC
337+
*
338+
* @min 0
339+
* @max 63
340+
* @bit 0 GPS (with QZSS)
341+
* @bit 1 SBAS
342+
* @bit 2 Galileo
343+
* @bit 3 BeiDou
344+
* @bit 4 GLONASS
345+
* @bit 5 NAVIC
346+
*
347+
* @reboot_required true
348+
* @group GPS
349+
*/
350+
PARAM_DEFINE_INT32(GPS_2_GNSS, 0);

0 commit comments

Comments
 (0)