mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-25 23:46:04 +08:00
[gps] Update ublox RTK support and ground tool (#2831)
This commit is contained in:
Freek van Tienen
GitHub
@@ -168,6 +168,7 @@ Cargo.lock
|
||||
/sw/ground_segment/misc/video_synchronizer
|
||||
/sw/ground_segment/misc/ivy2serial
|
||||
/sw/ground_segment/misc/sbs2ivy
|
||||
/sw/ground_segment/misc/ublox2ivy
|
||||
|
||||
# /sw/airborne/arch/lpc21/test/bootloader
|
||||
/sw/airborne/arch/lpc21/test/bootloader/bl.dmp
|
||||
|
||||
@@ -60,7 +60,7 @@
|
||||
#define GOT_CHECKSUM1 8
|
||||
|
||||
#define RXM_RTCM_VERSION 0x02
|
||||
#define NAV_RELPOSNED_VERSION 0x00
|
||||
#define NAV_RELPOSNED_VERSION 0x01
|
||||
/* last error type */
|
||||
#define GPS_UBX_ERR_NONE 0
|
||||
#define GPS_UBX_ERR_OVERRUN 1
|
||||
@@ -83,13 +83,13 @@ extern struct GpsRelposNED gps_relposned;
|
||||
extern struct RtcmMan rtcm_man;
|
||||
|
||||
#ifndef INJECT_BUFF_SIZE
|
||||
#define INJECT_BUFF_SIZE 512
|
||||
#define INJECT_BUFF_SIZE 1024 + 6
|
||||
#endif
|
||||
/* RTCM control struct type */
|
||||
struct rtcm_t {
|
||||
uint32_t nbyte; ///< number of bytes in message buffer
|
||||
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 };
|
||||
|
||||
@@ -125,17 +125,13 @@ static void gps_ubx_parse_nav_pvt(void)
|
||||
uint8_t gnssFixOK = bit_is_set(flags, 0);
|
||||
uint8_t diffSoln = bit_is_set(flags, 1);
|
||||
uint8_t carrSoln = (flags & 0xC0) >> 6;
|
||||
if (gnssFixOK > 0) {
|
||||
if (diffSoln > 0) {
|
||||
if (carrSoln == 2) {
|
||||
gps_ubx.state.fix = 5; // rtk
|
||||
} else {
|
||||
gps_ubx.state.fix = 4; // dgnss
|
||||
}
|
||||
} else {
|
||||
gps_ubx.state.fix = 3; // 3D
|
||||
}
|
||||
} else {
|
||||
if (diffSoln && carrSoln == 2) {
|
||||
gps_ubx.state.fix = 5; // rtk
|
||||
} else if(diffSoln && carrSoln == 1) {
|
||||
gps_ubx.state.fix = 4; // dgnss
|
||||
} else if(gnssFixOK) {
|
||||
gps_ubx.state.fix = 3; // 3D
|
||||
} else{
|
||||
gps_ubx.state.fix = 0;
|
||||
}
|
||||
|
||||
@@ -349,35 +345,40 @@ static void gps_ubx_parse_nav_relposned(void)
|
||||
{
|
||||
#if USE_GPS_UBX_RTCM
|
||||
uint8_t version = UBX_NAV_RELPOSNED_version(gps_ubx.msg_buf);
|
||||
if (version == NAV_RELPOSNED_VERSION) {
|
||||
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);
|
||||
uint8_t flags = UBX_NAV_RELPOSNED_flags(gps_ubx.msg_buf);
|
||||
gps_relposned.carrSoln = RTCMgetbitu(&flags, 3, 2);
|
||||
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 {
|
||||
if (version <= NAV_RELPOSNED_VERSION) {
|
||||
uint8_t flags = UBX_NAV_RELPOSNED_flags(gps_ubx.msg_buf);
|
||||
uint8_t carrSoln = RTCMgetbitu(&flags, 3, 2);
|
||||
uint8_t relPosValid = RTCMgetbitu(&flags, 5, 1);
|
||||
uint8_t diffSoln = RTCMgetbitu(&flags, 6, 1);
|
||||
uint8_t gnssFixOK = RTCMgetbitu(&flags, 7, 1);
|
||||
|
||||
/* Only save the latest valid relative position */
|
||||
if(relPosValid) {
|
||||
if (diffSoln && carrSoln == 2) {
|
||||
gps_ubx.state.fix = 5; // rtk
|
||||
} else if(diffSoln && carrSoln == 1) {
|
||||
gps_ubx.state.fix = 4; // dgnss
|
||||
} else if(gnssFixOK) {
|
||||
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
|
||||
|
||||
@@ -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 += -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:
|
||||
$(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
|
||||
@echo CC $@
|
||||
@@ -82,6 +82,10 @@ rtcm2ivy: rtcm2ivy.o serial_port.o pprz_geodetic_float.o
|
||||
@echo CC $@
|
||||
$(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
|
||||
@echo CC $@
|
||||
$(Q)$(CC) $(CFLAGS) $(GTK_CFLAGS) $(LIBRARYS) $(INCLUDES) -o $@ $< $(GTK_LDFLAGS)
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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 */
|
||||
Reference in New Issue
Block a user