Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-27 08:55:51 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

[gps] Update ublox RTK support and ground tool (#2831)

Browse Source
This commit is contained in:
Freek van Tienen
2022-03-08 12:53:27 +01:00
committed by GitHub
parent 5758bb1058
commit e588310caa
5 changed files with 881 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitignore
+1
View File
@@ -168,6 +168,7 @@ Cargo.lock
/sw/ground_segment/misc/video_synchronizer /sw/ground_segment/misc/video_synchronizer
/sw/ground_segment/misc/ivy2serial /sw/ground_segment/misc/ivy2serial
/sw/ground_segment/misc/sbs2ivy /sw/ground_segment/misc/sbs2ivy
/sw/ground_segment/misc/ublox2ivy
# /sw/airborne/arch/lpc21/test/bootloader # /sw/airborne/arch/lpc21/test/bootloader
/sw/airborne/arch/lpc21/test/bootloader/bl.dmp /sw/airborne/arch/lpc21/test/bootloader/bl.dmp
sw/airborne/modules/gps/gps_ubx.c
+42 -41
View File
@@ -60,7 +60,7 @@
#define GOT_CHECKSUM1 8 #define GOT_CHECKSUM1 8
#define RXM_RTCM_VERSION 0x02 #define RXM_RTCM_VERSION 0x02
#define NAV_RELPOSNED_VERSION 0x00 #define NAV_RELPOSNED_VERSION 0x01
/* last error type */ /* last error type */
#define GPS_UBX_ERR_NONE 0 #define GPS_UBX_ERR_NONE 0
#define GPS_UBX_ERR_OVERRUN 1 #define GPS_UBX_ERR_OVERRUN 1
@@ -83,13 +83,13 @@ extern struct GpsRelposNED gps_relposned;
extern struct RtcmMan rtcm_man; extern struct RtcmMan rtcm_man;
#ifndef INJECT_BUFF_SIZE #ifndef INJECT_BUFF_SIZE
#define INJECT_BUFF_SIZE 512 #define INJECT_BUFF_SIZE 1024 + 6
#endif #endif
/* RTCM control struct type */ /* RTCM control struct type */
struct rtcm_t { struct rtcm_t {
uint32_t nbyte; ///< number of bytes in message buffer uint32_t nbyte; ///< number of bytes in message buffer
uint32_t len; ///< message length (bytes) uint32_t len; ///< message length (bytes)
uint8_t buff[INJECT_BUFF_SIZE + 1]; ///< message buffer uint8_t buff[INJECT_BUFF_SIZE]; ///< message buffer
}; };
struct rtcm_t rtcm = { 0 }; struct rtcm_t rtcm = { 0 };
@@ -125,17 +125,13 @@ static void gps_ubx_parse_nav_pvt(void)
uint8_t gnssFixOK = bit_is_set(flags, 0); uint8_t gnssFixOK = bit_is_set(flags, 0);
uint8_t diffSoln = bit_is_set(flags, 1); uint8_t diffSoln = bit_is_set(flags, 1);
uint8_t carrSoln = (flags & 0xC0) >> 6; uint8_t carrSoln = (flags & 0xC0) >> 6;
if (gnssFixOK > 0) { if (diffSoln && carrSoln == 2) {
if (diffSoln > 0) { gps_ubx.state.fix = 5; // rtk
if (carrSoln == 2) { } else if(diffSoln && carrSoln == 1) {
gps_ubx.state.fix = 5; // rtk gps_ubx.state.fix = 4; // dgnss
} else { } else if(gnssFixOK) {
gps_ubx.state.fix = 4; // dgnss gps_ubx.state.fix = 3; // 3D
} } else{
} else {
gps_ubx.state.fix = 3; // 3D
}
} else {
gps_ubx.state.fix = 0; gps_ubx.state.fix = 0;
} }
@@ -349,35 +345,40 @@ static void gps_ubx_parse_nav_relposned(void)
{ {
#if USE_GPS_UBX_RTCM #if USE_GPS_UBX_RTCM
uint8_t version = UBX_NAV_RELPOSNED_version(gps_ubx.msg_buf); uint8_t version = UBX_NAV_RELPOSNED_version(gps_ubx.msg_buf);
if (version == NAV_RELPOSNED_VERSION) { if (version <= NAV_RELPOSNED_VERSION) {
gps_relposned.iTOW = UBX_NAV_RELPOSNED_iTOW(gps_ubx.msg_buf); uint8_t flags = UBX_NAV_RELPOSNED_flags(gps_ubx.msg_buf);
gps_relposned.refStationId = UBX_NAV_RELPOSNED_refStationId(gps_ubx.msg_buf); uint8_t carrSoln = RTCMgetbitu(&flags, 3, 2);
gps_relposned.relPosN = UBX_NAV_RELPOSNED_relPosN(gps_ubx.msg_buf); uint8_t relPosValid = RTCMgetbitu(&flags, 5, 1);
gps_relposned.relPosE = UBX_NAV_RELPOSNED_relPosE(gps_ubx.msg_buf); uint8_t diffSoln = RTCMgetbitu(&flags, 6, 1);
gps_relposned.relPosD = UBX_NAV_RELPOSNED_relPosD(gps_ubx.msg_buf) ; uint8_t gnssFixOK = RTCMgetbitu(&flags, 7, 1);
gps_relposned.relPosHPN = UBX_NAV_RELPOSNED_relPosHPN(gps_ubx.msg_buf);
gps_relposned.relPosHPE = UBX_NAV_RELPOSNED_relPosHPE(gps_ubx.msg_buf); /* Only save the latest valid relative position */
gps_relposned.relPosHPD = UBX_NAV_RELPOSNED_relPosHPD(gps_ubx.msg_buf); if(relPosValid) {
gps_relposned.accN = UBX_NAV_RELPOSNED_accN(gps_ubx.msg_buf); if (diffSoln && carrSoln == 2) {
gps_relposned.accE = UBX_NAV_RELPOSNED_accE(gps_ubx.msg_buf); gps_ubx.state.fix = 5; // rtk
gps_relposned.accD = UBX_NAV_RELPOSNED_accD(gps_ubx.msg_buf); } else if(diffSoln && carrSoln == 1) {
uint8_t flags = UBX_NAV_RELPOSNED_flags(gps_ubx.msg_buf); gps_ubx.state.fix = 4; // dgnss
gps_relposned.carrSoln = RTCMgetbitu(&flags, 3, 2); } else if(gnssFixOK) {
gps_relposned.relPosValid = RTCMgetbitu(&flags, 5, 1);
gps_relposned.diffSoln = RTCMgetbitu(&flags, 6, 1);
gps_relposned.gnssFixOK = RTCMgetbitu(&flags, 7, 1);
if (gps_relposned.gnssFixOK > 0) {
if (gps_relposned.diffSoln > 0) {
if (gps_relposned.carrSoln == 2) {
gps_ubx.state.fix = 5; // rtk
} else {
gps_ubx.state.fix = 4; // dgnss
}
} else {
gps_ubx.state.fix = 3; // 3D gps_ubx.state.fix = 3; // 3D
} else{
gps_ubx.state.fix = 0;
} }
} else {
gps_ubx.state.fix = 0; gps_relposned.iTOW = UBX_NAV_RELPOSNED_iTOW(gps_ubx.msg_buf);
gps_relposned.refStationId = UBX_NAV_RELPOSNED_refStationId(gps_ubx.msg_buf);
gps_relposned.relPosN = UBX_NAV_RELPOSNED_relPosN(gps_ubx.msg_buf);
gps_relposned.relPosE = UBX_NAV_RELPOSNED_relPosE(gps_ubx.msg_buf);
gps_relposned.relPosD = UBX_NAV_RELPOSNED_relPosD(gps_ubx.msg_buf) ;
gps_relposned.relPosHPN = UBX_NAV_RELPOSNED_relPosHPN(gps_ubx.msg_buf);
gps_relposned.relPosHPE = UBX_NAV_RELPOSNED_relPosHPE(gps_ubx.msg_buf);
gps_relposned.relPosHPD = UBX_NAV_RELPOSNED_relPosHPD(gps_ubx.msg_buf);
gps_relposned.accN = UBX_NAV_RELPOSNED_accN(gps_ubx.msg_buf);
gps_relposned.accE = UBX_NAV_RELPOSNED_accE(gps_ubx.msg_buf);
gps_relposned.accD = UBX_NAV_RELPOSNED_accD(gps_ubx.msg_buf);
gps_relposned.carrSoln = carrSoln;
gps_relposned.relPosValid = relPosValid;
gps_relposned.diffSoln = diffSoln;
gps_relposned.gnssFixOK = gnssFixOK;
} }
} }
#endif #endif
sw/ground_segment/misc/Makefile
+6 -2
View File
@@ -57,10 +57,10 @@ GLIB_LDFLAGS = $(shell pkg-config glib-2.0 --libs) -lglibivy -lm $(shell pcre-co
INCLUDES += $(shell pkg-config glib-2.0 --cflags) -I$(PAPARAZZI_SRC)/sw/airborne/ -I$(PAPARAZZI_SRC)/sw/include/ $(IVY_INC) INCLUDES += $(shell pkg-config glib-2.0 --cflags) -I$(PAPARAZZI_SRC)/sw/airborne/ -I$(PAPARAZZI_SRC)/sw/include/ $(IVY_INC)
INCLUDES += -I$(PAPARAZZI_SRC)/sw/ext/libsbp/c/include/ -I$(PAPARAZZI_SRC)/sw/airborne/modules/gps/librtcm3/ -I$(PAPARAZZI_SRC)/sw/airborne/arch/linux/ INCLUDES += -I$(PAPARAZZI_SRC)/sw/ext/libsbp/c/include/ -I$(PAPARAZZI_SRC)/sw/airborne/modules/gps/librtcm3/ -I$(PAPARAZZI_SRC)/sw/airborne/arch/linux/
all: davis2ivy kestrel2ivy natnet2ivy sbp2ivy video_synchronizer sbs2ivy rtcm2ivy all: davis2ivy kestrel2ivy natnet2ivy sbp2ivy video_synchronizer sbs2ivy rtcm2ivy ublox2ivy
clean: clean:
$(Q)rm -f *.o davis2ivy kestrel2ivy natnet2ivy sbp2ivy video_synchronizer sbs2ivy $(Q)rm -f *.o davis2ivy kestrel2ivy natnet2ivy sbp2ivy video_synchronizer sbs2ivy rtcm2ivy ublox2ivy
davis2ivy: davis2ivy.o davis2ivy: davis2ivy.o
@echo CC $@ @echo CC $@
@@ -82,6 +82,10 @@ rtcm2ivy: rtcm2ivy.o serial_port.o pprz_geodetic_float.o
@echo CC $@ @echo CC $@
$(Q)$(CC) $(CFLAGS) -o $@ $^ $(INCLUDES) $(LIBRARYS) $(GLIB_LDFLAGS) $(IVY_LDFLAGS) $(Q)$(CC) $(CFLAGS) -o $@ $^ $(INCLUDES) $(LIBRARYS) $(GLIB_LDFLAGS) $(IVY_LDFLAGS)
ublox2ivy: ublox2ivy.o
@echo CC $@
$(Q)$(CC) $(CFLAGS) -o $@ $^ $(INCLUDES) $(LIBRARYS) -lpthread $(GLIB_LDFLAGS) $(IVY_LDFLAGS)
video_synchronizer: video_synchronizer.c video_synchronizer: video_synchronizer.c
@echo CC $@ @echo CC $@
$(Q)$(CC) $(CFLAGS) $(GTK_CFLAGS) $(LIBRARYS) $(INCLUDES) -o $@ $< $(GTK_LDFLAGS) $(Q)$(CC) $(CFLAGS) $(GTK_CFLAGS) $(LIBRARYS) $(INCLUDES) -o $@ $< $(GTK_LDFLAGS)
sw/ground_segment/misc/ublox2ivy.c
+727
View File
File diff suppressed because it is too large Load Diff
sw/ground_segment/misc/ublox2ivy.h
+105
View File
@@ -0,0 +1,105 @@
/*
* Paparazzi UBLox to Ivy
*
* Copyright (C) 2021 Freek van Tienen <freek.v.tienen@gmail.com>
*
* This file is part of paparazzi.
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*/
#ifndef UBLOX2IVY_H
#define UBLOX2IVY_H
/* Parser status */
#define UNINIT 0
#define GOT_SYNC1 1
#define GOT_SYNC2 2
#define GOT_CLASS 3
#define GOT_ID 4
#define GOT_LEN1 5
#define GOT_LEN2 6
#define GOT_PAYLOAD 7
#define GOT_CHECKSUM1 8
/* UBX / RTCM parsing */
#define GPS_RTCM_SYNC1 0xD3
#define GPS_RTCM_MAX_PAYLOAD 1024+6
#define GPS_UBX_SYNC1 0xB5
#define GPS_UBX_SYNC2 0x62
#define GPS_UBX_MAX_PAYLOAD 1024
/* UBX PVT message */
#define UBX_NAV_PVT_ID 0x07
#define UBX_NAV_PVT_iTOW(_ubx_payload) (uint32_t)(*((uint8_t*)_ubx_payload+0)|(uint32_t)(*((uint8_t*)_ubx_payload+1+0))<<8|((uint32_t)*((uint8_t*)_ubx_payload+2+0))<<16|((uint32_t)*((uint8_t*)_ubx_payload+3+0))<<24)
#define UBX_NAV_PVT_year(_ubx_payload) (uint16_t)(*((uint8_t*)_ubx_payload+4)|(uint16_t)(*((uint8_t*)_ubx_payload+1+4))<<8)
#define UBX_NAV_PVT_month(_ubx_payload) (uint8_t)(*((uint8_t*)_ubx_payload+6))
#define UBX_NAV_PVT_day(_ubx_payload) (uint8_t)(*((uint8_t*)_ubx_payload+7))
#define UBX_NAV_PVT_hour(_ubx_payload) (uint8_t)(*((uint8_t*)_ubx_payload+8))
#define UBX_NAV_PVT_min(_ubx_payload) (uint8_t)(*((uint8_t*)_ubx_payload+9))
#define UBX_NAV_PVT_sec(_ubx_payload) (uint8_t)(*((uint8_t*)_ubx_payload+10))
#define UBX_NAV_PVT_valid(_ubx_payload) (uint8_t)(*((uint8_t*)_ubx_payload+11))
#define UBX_NAV_PVT_tAcc(_ubx_payload) (uint32_t)(*((uint8_t*)_ubx_payload+12)|(uint32_t)(*((uint8_t*)_ubx_payload+1+12))<<8|((uint32_t)*((uint8_t*)_ubx_payload+2+12))<<16|((uint32_t)*((uint8_t*)_ubx_payload+3+12))<<24)
#define UBX_NAV_PVT_nano(_ubx_payload) (int32_t)(*((uint8_t*)_ubx_payload+16)|(int32_t)(*((uint8_t*)_ubx_payload+1+16))<<8|((int32_t)*((uint8_t*)_ubx_payload+2+16))<<16|((int32_t)*((uint8_t*)_ubx_payload+3+16))<<24)
#define UBX_NAV_PVT_fixType(_ubx_payload) (uint8_t)(*((uint8_t*)_ubx_payload+20))
#define UBX_NAV_PVT_flags(_ubx_payload) (uint8_t)(*((uint8_t*)_ubx_payload+21))
#define UBX_NAV_PVT_flags2(_ubx_payload) (uint8_t)(*((uint8_t*)_ubx_payload+22))
#define UBX_NAV_PVT_numSV(_ubx_payload) (uint8_t)(*((uint8_t*)_ubx_payload+23))
#define UBX_NAV_PVT_lon(_ubx_payload) (int32_t)(*((uint8_t*)_ubx_payload+24)|(int32_t)(*((uint8_t*)_ubx_payload+1+24))<<8|((int32_t)*((uint8_t*)_ubx_payload+2+24))<<16|((int32_t)*((uint8_t*)_ubx_payload+3+24))<<24)
#define UBX_NAV_PVT_lat(_ubx_payload) (int32_t)(*((uint8_t*)_ubx_payload+28)|(int32_t)(*((uint8_t*)_ubx_payload+1+28))<<8|((int32_t)*((uint8_t*)_ubx_payload+2+28))<<16|((int32_t)*((uint8_t*)_ubx_payload+3+28))<<24)
#define UBX_NAV_PVT_height(_ubx_payload) (int32_t)(*((uint8_t*)_ubx_payload+32)|(int32_t)(*((uint8_t*)_ubx_payload+1+32))<<8|((int32_t)*((uint8_t*)_ubx_payload+2+32))<<16|((int32_t)*((uint8_t*)_ubx_payload+3+32))<<24)
#define UBX_NAV_PVT_hMSL(_ubx_payload) (int32_t)(*((uint8_t*)_ubx_payload+36)|(int32_t)(*((uint8_t*)_ubx_payload+1+36))<<8|((int32_t)*((uint8_t*)_ubx_payload+2+36))<<16|((int32_t)*((uint8_t*)_ubx_payload+3+36))<<24)
#define UBX_NAV_PVT_hAcc(_ubx_payload) (uint32_t)(*((uint8_t*)_ubx_payload+40)|(uint32_t)(*((uint8_t*)_ubx_payload+1+40))<<8|((uint32_t)*((uint8_t*)_ubx_payload+2+40))<<16|((uint32_t)*((uint8_t*)_ubx_payload+3+40))<<24)
#define UBX_NAV_PVT_vAcc(_ubx_payload) (uint32_t)(*((uint8_t*)_ubx_payload+44)|(uint32_t)(*((uint8_t*)_ubx_payload+1+44))<<8|((uint32_t)*((uint8_t*)_ubx_payload+2+44))<<16|((uint32_t)*((uint8_t*)_ubx_payload+3+44))<<24)
#define UBX_NAV_PVT_velN(_ubx_payload) (int32_t)(*((uint8_t*)_ubx_payload+48)|(int32_t)(*((uint8_t*)_ubx_payload+1+48))<<8|((int32_t)*((uint8_t*)_ubx_payload+2+48))<<16|((int32_t)*((uint8_t*)_ubx_payload+3+48))<<24)
#define UBX_NAV_PVT_velE(_ubx_payload) (int32_t)(*((uint8_t*)_ubx_payload+52)|(int32_t)(*((uint8_t*)_ubx_payload+1+52))<<8|((int32_t)*((uint8_t*)_ubx_payload+2+52))<<16|((int32_t)*((uint8_t*)_ubx_payload+3+52))<<24)
#define UBX_NAV_PVT_velD(_ubx_payload) (int32_t)(*((uint8_t*)_ubx_payload+56)|(int32_t)(*((uint8_t*)_ubx_payload+1+56))<<8|((int32_t)*((uint8_t*)_ubx_payload+2+56))<<16|((int32_t)*((uint8_t*)_ubx_payload+3+56))<<24)
#define UBX_NAV_PVT_gSpeed(_ubx_payload) (int32_t)(*((uint8_t*)_ubx_payload+60)|(int32_t)(*((uint8_t*)_ubx_payload+1+60))<<8|((int32_t)*((uint8_t*)_ubx_payload+2+60))<<16|((int32_t)*((uint8_t*)_ubx_payload+3+60))<<24)
#define UBX_NAV_PVT_headMot(_ubx_payload) (int32_t)(*((uint8_t*)_ubx_payload+64)|(int32_t)(*((uint8_t*)_ubx_payload+1+64))<<8|((int32_t)*((uint8_t*)_ubx_payload+2+64))<<16|((int32_t)*((uint8_t*)_ubx_payload+3+64))<<24)
#define UBX_NAV_PVT_sAcc(_ubx_payload) (uint32_t)(*((uint8_t*)_ubx_payload+68)|(uint32_t)(*((uint8_t*)_ubx_payload+1+68))<<8|((uint32_t)*((uint8_t*)_ubx_payload+2+68))<<16|((uint32_t)*((uint8_t*)_ubx_payload+3+68))<<24)
#define UBX_NAV_PVT_headAcc(_ubx_payload) (uint32_t)(*((uint8_t*)_ubx_payload+72)|(uint32_t)(*((uint8_t*)_ubx_payload+1+72))<<8|((uint32_t)*((uint8_t*)_ubx_payload+2+72))<<16|((uint32_t)*((uint8_t*)_ubx_payload+3+72))<<24)
#define UBX_NAV_PVT_pDOP(_ubx_payload) (uint16_t)(*((uint8_t*)_ubx_payload+76)|(uint16_t)(*((uint8_t*)_ubx_payload+1+76))<<8)
#define UBX_NAV_PVT_reserved1a(_ubx_payload) (uint32_t)(*((uint8_t*)_ubx_payload+78)|(uint32_t)(*((uint8_t*)_ubx_payload+1+78))<<8|((uint32_t)*((uint8_t*)_ubx_payload+2+78))<<16|((uint32_t)*((uint8_t*)_ubx_payload+3+78))<<24)
#define UBX_NAV_PVT_reserved1b(_ubx_payload) (uint16_t)(*((uint8_t*)_ubx_payload+82)|(uint16_t)(*((uint8_t*)_ubx_payload+1+82))<<8)
#define UBX_NAV_PVT_headVeh(_ubx_payload) (int32_t)(*((uint8_t*)_ubx_payload+84)|(int32_t)(*((uint8_t*)_ubx_payload+1+84))<<8|((int32_t)*((uint8_t*)_ubx_payload+2+84))<<16|((int32_t)*((uint8_t*)_ubx_payload+3+84))<<24)
#define UBX_NAV_PVT_magDec(_ubx_payload) (int16_t)(*((uint8_t*)_ubx_payload+88)|(int16_t)(*((uint8_t*)_ubx_payload+1+88))<<8)
#define UBX_NAV_PVT_magAcc(_ubx_payload) (uint16_t)(*((uint8_t*)_ubx_payload+90)|(uint16_t)(*((uint8_t*)_ubx_payload+1+90))<<8)
struct gps_ubx_t {
bool msg_available;
uint8_t msg_buf[GPS_UBX_MAX_PAYLOAD] __attribute__((aligned));
uint8_t msg_id;
uint8_t msg_class;
uint8_t status;
uint16_t len;
uint16_t msg_idx;
uint8_t ck_a, ck_b;
uint8_t error_cnt;
};
struct gps_rtcm_t {
bool msg_available;
uint8_t msg_buf[GPS_RTCM_MAX_PAYLOAD] __attribute__((aligned));
uint8_t status;
uint16_t len;
uint16_t msg_idx;
uint8_t error_cnt;
};
#endif /* UBLOX2IVY_H */