diff --git a/.gitignore b/.gitignore index 3ec1f73d6b..07853422ad 100644 --- a/.gitignore +++ b/.gitignore @@ -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 diff --git a/sw/airborne/modules/gps/gps_ubx.c b/sw/airborne/modules/gps/gps_ubx.c index 27fea324f0..bbd7c13691 100644 --- a/sw/airborne/modules/gps/gps_ubx.c +++ b/sw/airborne/modules/gps/gps_ubx.c @@ -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 diff --git a/sw/ground_segment/misc/Makefile b/sw/ground_segment/misc/Makefile index e9a8a8d741..1686f7c706 100644 --- a/sw/ground_segment/misc/Makefile +++ b/sw/ground_segment/misc/Makefile @@ -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) diff --git a/sw/ground_segment/misc/ublox2ivy.c b/sw/ground_segment/misc/ublox2ivy.c new file mode 100644 index 0000000000..508c3bc3aa --- /dev/null +++ b/sw/ground_segment/misc/ublox2ivy.c @@ -0,0 +1,727 @@ +/* + * Paparazzi UBLox to Ivy + * + * Copyright (C) 2021 Freek van Tienen + * + * 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. + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "ublox2ivy.h" + +#define UDP_BUFFER_SIZE 1024 + +/* Endpoints */ +struct endpoint_udp_t { + char server_addr[128]; + uint16_t server_port; + char client_addr[128]; + uint16_t client_port; + + int fd; + pthread_t thread; +}; + +struct endpoint_uart_t { + char devname[512]; + int baudrate; + + int fd; + pthread_t thread; +}; + +enum endpoint_type_t { + ENDPOINT_TYPE_NONE, + ENDPOINT_TYPE_UDP, + ENDPOINT_TYPE_UART +}; + +struct endpoint_t { + enum endpoint_type_t type; + union { + struct endpoint_udp_t udp; + struct endpoint_uart_t uart; + } ep; +}; + +static bool verbose = false; +static struct endpoint_t gps_ep; +static struct gps_ubx_t gps_ubx; +static struct gps_rtcm_t gps_rtcm; +static uint8_t ac_id = 1; + +void packet_handler(void *ep, uint8_t *data, uint16_t len); + +static const unsigned int tbl_CRC24Q[] = { + 0x000000, 0x864CFB, 0x8AD50D, 0x0C99F6, 0x93E6E1, 0x15AA1A, 0x1933EC, 0x9F7F17, + 0xA18139, 0x27CDC2, 0x2B5434, 0xAD18CF, 0x3267D8, 0xB42B23, 0xB8B2D5, 0x3EFE2E, + 0xC54E89, 0x430272, 0x4F9B84, 0xC9D77F, 0x56A868, 0xD0E493, 0xDC7D65, 0x5A319E, + 0x64CFB0, 0xE2834B, 0xEE1ABD, 0x685646, 0xF72951, 0x7165AA, 0x7DFC5C, 0xFBB0A7, + 0x0CD1E9, 0x8A9D12, 0x8604E4, 0x00481F, 0x9F3708, 0x197BF3, 0x15E205, 0x93AEFE, + 0xAD50D0, 0x2B1C2B, 0x2785DD, 0xA1C926, 0x3EB631, 0xB8FACA, 0xB4633C, 0x322FC7, + 0xC99F60, 0x4FD39B, 0x434A6D, 0xC50696, 0x5A7981, 0xDC357A, 0xD0AC8C, 0x56E077, + 0x681E59, 0xEE52A2, 0xE2CB54, 0x6487AF, 0xFBF8B8, 0x7DB443, 0x712DB5, 0xF7614E, + 0x19A3D2, 0x9FEF29, 0x9376DF, 0x153A24, 0x8A4533, 0x0C09C8, 0x00903E, 0x86DCC5, + 0xB822EB, 0x3E6E10, 0x32F7E6, 0xB4BB1D, 0x2BC40A, 0xAD88F1, 0xA11107, 0x275DFC, + 0xDCED5B, 0x5AA1A0, 0x563856, 0xD074AD, 0x4F0BBA, 0xC94741, 0xC5DEB7, 0x43924C, + 0x7D6C62, 0xFB2099, 0xF7B96F, 0x71F594, 0xEE8A83, 0x68C678, 0x645F8E, 0xE21375, + 0x15723B, 0x933EC0, 0x9FA736, 0x19EBCD, 0x8694DA, 0x00D821, 0x0C41D7, 0x8A0D2C, + 0xB4F302, 0x32BFF9, 0x3E260F, 0xB86AF4, 0x2715E3, 0xA15918, 0xADC0EE, 0x2B8C15, + 0xD03CB2, 0x567049, 0x5AE9BF, 0xDCA544, 0x43DA53, 0xC596A8, 0xC90F5E, 0x4F43A5, + 0x71BD8B, 0xF7F170, 0xFB6886, 0x7D247D, 0xE25B6A, 0x641791, 0x688E67, 0xEEC29C, + 0x3347A4, 0xB50B5F, 0xB992A9, 0x3FDE52, 0xA0A145, 0x26EDBE, 0x2A7448, 0xAC38B3, + 0x92C69D, 0x148A66, 0x181390, 0x9E5F6B, 0x01207C, 0x876C87, 0x8BF571, 0x0DB98A, + 0xF6092D, 0x7045D6, 0x7CDC20, 0xFA90DB, 0x65EFCC, 0xE3A337, 0xEF3AC1, 0x69763A, + 0x578814, 0xD1C4EF, 0xDD5D19, 0x5B11E2, 0xC46EF5, 0x42220E, 0x4EBBF8, 0xC8F703, + 0x3F964D, 0xB9DAB6, 0xB54340, 0x330FBB, 0xAC70AC, 0x2A3C57, 0x26A5A1, 0xA0E95A, + 0x9E1774, 0x185B8F, 0x14C279, 0x928E82, 0x0DF195, 0x8BBD6E, 0x872498, 0x016863, + 0xFAD8C4, 0x7C943F, 0x700DC9, 0xF64132, 0x693E25, 0xEF72DE, 0xE3EB28, 0x65A7D3, + 0x5B59FD, 0xDD1506, 0xD18CF0, 0x57C00B, 0xC8BF1C, 0x4EF3E7, 0x426A11, 0xC426EA, + 0x2AE476, 0xACA88D, 0xA0317B, 0x267D80, 0xB90297, 0x3F4E6C, 0x33D79A, 0xB59B61, + 0x8B654F, 0x0D29B4, 0x01B042, 0x87FCB9, 0x1883AE, 0x9ECF55, 0x9256A3, 0x141A58, + 0xEFAAFF, 0x69E604, 0x657FF2, 0xE33309, 0x7C4C1E, 0xFA00E5, 0xF69913, 0x70D5E8, + 0x4E2BC6, 0xC8673D, 0xC4FECB, 0x42B230, 0xDDCD27, 0x5B81DC, 0x57182A, 0xD154D1, + 0x26359F, 0xA07964, 0xACE092, 0x2AAC69, 0xB5D37E, 0x339F85, 0x3F0673, 0xB94A88, + 0x87B4A6, 0x01F85D, 0x0D61AB, 0x8B2D50, 0x145247, 0x921EBC, 0x9E874A, 0x18CBB1, + 0xE37B16, 0x6537ED, 0x69AE1B, 0xEFE2E0, 0x709DF7, 0xF6D10C, 0xFA48FA, 0x7C0401, + 0x42FA2F, 0xC4B6D4, 0xC82F22, 0x4E63D9, 0xD11CCE, 0x575035, 0x5BC9C3, 0xDD8538 +}; + +/* crc-24q parity -------------------------------------------------------------- +* compute crc-24q parity for sbas, rtcm3 +* args : unsigned char *buff I data +* int len I data length (bytes) +* return : crc-24Q parity +* notes : see reference [2] A.4.3.3 Parity +*-----------------------------------------------------------------------------*/ +unsigned int crc24q(const unsigned char *buff, int len) +{ + unsigned int crc = 0; + int i; + for (i = 0; i < len; i++) { crc = ((crc << 8) & 0xFFFFFF)^tbl_CRC24Q[(crc >> 16)^buff[i]]; } + return crc; +} + +unsigned int RTCMgetbitu(unsigned char *buff, int pos, int lenb) +{ + unsigned int bits = 0; + int i; + for (i = pos; i < pos + lenb; i++) { bits = (bits << 1) + ((buff[i / 8] >> (7 - i % 8)) & 1u); } + return bits; +} + +/** + * Create an UDP endpoint thread + */ +void *udp_endpoint(void *arg) { + struct endpoint_udp_t *ep = (struct endpoint_udp_t *)arg; + + /* Create the socket */ + ep->fd = socket(AF_INET, SOCK_DGRAM, 0); + if(ep->fd < 0) { + fprintf(stderr, "Could not create socket for %s:%d\r\n", ep->server_addr, ep->server_port); + return NULL; + } + + /* Enable broadcasting */ + int one = 1; + setsockopt(ep->fd, SOL_SOCKET, SO_BROADCAST, &one, sizeof(one)); + + /* Create the input address */ + struct sockaddr_in server; + server.sin_family = AF_INET; + server.sin_addr.s_addr = inet_addr(ep->server_addr); + server.sin_port = htons(ep->server_port); + + /* Bind the socket with the server address */ + if(bind(ep->fd, (struct sockaddr *)&server, sizeof(server)) < 0) { + fprintf(stderr, "Could not bind to address %s:%d\r\n", ep->server_addr, ep->server_port); + return NULL; + } + + /* Wait for messages */ + while(true) { + struct sockaddr_in client; + uint8_t buffer[UDP_BUFFER_SIZE]; + socklen_t len = sizeof(client); + int n = recvfrom(ep->fd, buffer, UDP_BUFFER_SIZE, MSG_WAITALL, (struct sockaddr *)&client, &len); + + // Ignore errors + if(n < 0) + continue; + + if(verbose) printf("Got packet at endpoint [%s:%d] with length %d\r\n", ep->server_addr, ep->server_port, n); + + // Send the message to the handler + packet_handler(ep, buffer, n); + } +} + +/** + * Send a message out of an UDP endpoint + */ +void udp_endpoint_send(struct endpoint_udp_t *ep, uint8_t *buffer, uint16_t len) { + // Check if file descriptor is valid + if(ep->fd < 0) + return; + + struct sockaddr_in client; + client.sin_family = AF_INET; + client.sin_addr.s_addr = inet_addr(ep->client_addr); + client.sin_port = htons(ep->client_port); + + if(verbose) printf("Send packet at endpoint [%s:%d] with length %d\r\n", ep->client_addr, ep->client_port, len); + sendto(ep->fd, buffer, len, MSG_DONTWAIT, (struct sockaddr *)&client, sizeof(client)); +} + +/** + * Create an UDP endpoint + */ +void udp_create(char *server_addr, uint16_t server_port, char *client_addr, uint16_t client_port) { + /* Create the endpoint */ + gps_ep.type = ENDPOINT_TYPE_UDP; + struct endpoint_udp_t *ep = &gps_ep.ep.udp; + strncpy(ep->server_addr, server_addr, 127); + ep->server_port = server_port; + strncpy(ep->client_addr, client_addr, 127); + ep->client_port = client_port; + + /* Start the endpoint thread */ + if(verbose) printf("Created UDP endpoint with server [%s:%d] and client [%s:%d]\r\n", ep->server_addr, ep->server_port, ep->client_addr, ep->client_port); + pthread_create(&ep->thread, NULL, udp_endpoint, ep); +} + +/** + * Get the baudrate based ont he argument + */ +int get_baud(unsigned int baud_rate) +{ + int baud = 0; + switch (baud_rate) + { +#ifdef B921600 + case 921600: + baud = B921600; + break; +#endif +#ifdef B460800 + case 460800: + baud = B460800; + break; +#endif + case 230400: + baud = B230400; + break; + case 115200: + baud = B115200; + break; + case 57600: + baud = B57600; + break; + case 38400: + baud = B38400; + break; + case 19200: + baud = B19200; + break; + case 9600: + baud = B9600; + break; + case 4800: + baud = B4800; + break; + case 2400: + baud = B2400; + break; + case 1800: + baud = B1800; + break; + case 1200: + baud = B1200; + break; + case 600: + baud = B600; + break; + case 300: + baud = B300; + break; + case 200: + baud = B200; + break; + case 150: + baud = B150; + break; + case 134: + baud = B134; + break; + case 110: + baud = B110; + break; + case 75: + baud = B75; + break; + case 50: + baud = B50; + break; + default: + baud = -1; + } //end of switch baud_rate + return baud; +} + +/** + * Create an UART endpoint thread + */ +void *uart_endpoint(void *arg) { + struct endpoint_uart_t *ep = (struct endpoint_uart_t *)arg; + + /* Open de uart device in blocking Read/Write mode */ + ep->fd = open(ep->devname, O_RDWR | O_NOCTTY); + if(ep->fd < 0) { + fprintf(stderr, "Could not open uart for %s:%d\r\n", ep->devname, ep->baudrate); + return NULL; + } + + /* Conver the baudrate */ + int baud = get_baud(ep->baudrate); + if(baud < 0) { + fprintf(stderr, "Could set baudrate for %s:%d\r\n", ep->devname, ep->baudrate); + return NULL; + } + + /* Configure the UART */ + struct termios options; + tcgetattr(ep->fd, &options); + options.c_cflag = baud | CS8 | CLOCAL | CREAD; + options.c_iflag = IGNPAR; + options.c_oflag = 0; + options.c_lflag = 0; + tcflush(ep->fd, TCIFLUSH); + tcsetattr(ep->fd, TCSANOW, &options); + + /* Try to read bytes */ + while(true) { + uint8_t buffer[2048]; + int len = read(ep->fd, buffer, 2048); + + // Read got an error + if(len < 0) { + if(verbose) printf("Got read error (%d) at endpoint [%s:%d]\r\n", len, ep->devname, ep->baudrate); + usleep(20); + } + // No bytes to read + else if(len == 0) { + usleep(20); + } + // We succsfully received some bytes + else if(len > 0) { + if(verbose) printf("Got packet at endpoint [%s:%d] with length %d\r\n", ep->devname, ep->baudrate, len); + + // Send the message to the handler + packet_handler(ep, buffer, len); + } + } +} + +/** + * Send a message out of an UART endpoint + */ +void uart_endpoint_send(struct endpoint_uart_t *ep, uint8_t *buffer, uint16_t len) { + // Check if file descriptor is valid + if(ep->fd < 0) + return; + + if(verbose) printf("Send packet at endpoint [%s:%d] with length %d\r\n", ep->devname, ep->baudrate, len); + if(write(ep->fd, buffer, len) < 0) + fprintf(stderr, "Send packet at endpoint [%s:%d] with length %d errored\r\n", ep->devname, ep->baudrate, len); +} + +/** + * Create an UART endpoint + */ +void uart_create(char *devname, uint32_t baudrate) { + /* Create the endpoint */ + gps_ep.type = ENDPOINT_TYPE_UART; + struct endpoint_uart_t *ep = &gps_ep.ep.uart; + strncpy(ep->devname, devname, 511); + ep->baudrate = baudrate; + + /* Start the endpoint thread */ + if(verbose) printf("Created UART endpoint [%s:%d]\r\n", ep->devname, ep->baudrate); + pthread_create(&ep->thread, NULL, uart_endpoint, ep); +} + +/* RTCM parsing */ +void gps_rtcm_parse(uint8_t c) { + switch(gps_rtcm.status) { + case UNINIT: + if (c == GPS_RTCM_SYNC1) { + gps_rtcm.msg_idx = 0; + gps_rtcm.msg_buf[gps_rtcm.msg_idx++] = c; + gps_rtcm.status++; + } + break; + case GOT_SYNC1: + gps_rtcm.len = (c << 8) & 0x3FF; + gps_rtcm.msg_buf[gps_rtcm.msg_idx++] = c; + gps_rtcm.status = GOT_LEN1; + break; + case GOT_LEN1: + gps_rtcm.len = (gps_rtcm.len | c) & 0x3FF; + gps_rtcm.msg_buf[gps_rtcm.msg_idx++] = c; + gps_rtcm.status = GOT_LEN2; + break; + case GOT_LEN2: + gps_rtcm.msg_buf[gps_rtcm.msg_idx++] = c; + + /* Received a full message */ + if (gps_rtcm.msg_idx >= (gps_rtcm.len + 6)) { + gps_rtcm.status = UNINIT; + + /* Perform checksum check */ + if(crc24q(gps_rtcm.msg_buf, gps_rtcm.len + 3) == RTCMgetbitu(gps_rtcm.msg_buf, (gps_rtcm.len + 3) * 8, 24)) { + gps_rtcm.msg_available = true; + } else { + if(verbose) printf("GPS RTCM checksum error\r\n"); + + /* Try to resync */ + if(gps_ubx.len > 0) { + const uint8_t *p = (const uint8_t *)memchr(&gps_rtcm.msg_buf[1], GPS_RTCM_SYNC1, (gps_rtcm.msg_idx-1)); + if(p != NULL) { + if(verbose) printf("GPS RTCM parser resynced\r\n"); + uint16_t len_diff = p - gps_rtcm.msg_buf; + uint16_t len = gps_rtcm.msg_idx - len_diff; + uint8_t msg_buf[len]; + memcpy(msg_buf, p, len); + + /* Recursion (chances of extremely deep recursion are really low) */ + for(uint16_t i = 0; i < len; i++) { + gps_rtcm_parse(msg_buf[i]); + + /* If two messages are embedded, the second one is discared */ + if(gps_rtcm.msg_available) + return; + } + } + } + } + return; + } + break; + default: + /* Should not come here */ + break; + } +} + +/* UBX parsing */ +void gps_ubx_parse(uint8_t c) +{ + if (gps_ubx.status < GOT_PAYLOAD) { + gps_ubx.ck_a += c; + gps_ubx.ck_b += gps_ubx.ck_a; + } + switch (gps_ubx.status) { + case UNINIT: + if (c == GPS_UBX_SYNC1) { + gps_ubx.status++; + gps_ubx.len = 0; + } + break; + case GOT_SYNC1: + if (c != GPS_UBX_SYNC2) { + if(verbose) printf("GPS Ublox parser out of sync\r\n"); + goto error; + } + gps_ubx.ck_a = 0; + gps_ubx.ck_b = 0; + gps_ubx.status++; + break; + case GOT_SYNC2: + if (gps_ubx.msg_available) { + /* Previous message has not yet been parsed: discard this one */ + if(verbose) printf("GPS Ublox parser overrun\r\n"); + goto error; + } + gps_ubx.msg_class = c; + gps_ubx.status++; + break; + case GOT_CLASS: + gps_ubx.msg_id = c; + gps_ubx.status++; + break; + case GOT_ID: + gps_ubx.len = c; + gps_ubx.status++; + break; + case GOT_LEN1: + gps_ubx.len |= (c << 8); + if (gps_ubx.len > GPS_UBX_MAX_PAYLOAD) { + gps_ubx.len = 0; + if(verbose) printf("GPS Ublox message to long\r\n"); + goto error; + } + gps_ubx.msg_idx = 0; + gps_ubx.status++; + break; + case GOT_LEN2: + gps_ubx.msg_buf[gps_ubx.msg_idx] = c; + gps_ubx.msg_idx++; + if (gps_ubx.msg_idx >= gps_ubx.len) { + gps_ubx.status++; + } + break; + case GOT_PAYLOAD: + if (c != gps_ubx.ck_a) { + if(verbose) printf("GPS Ublox parser checksum error\r\n"); + goto error; + } + gps_ubx.status++; + break; + case GOT_CHECKSUM1: + if (c != gps_ubx.ck_b) { + if(verbose) printf("GPS Ublox parser checksum error\r\n"); + goto error; + } + gps_ubx.msg_available = true; + goto restart; + break; + default: + if(verbose) printf("GPS Ublox parser unexpected error\r\n"); + goto error; + } + return; +error: + gps_ubx.error_cnt++; + + /* Try to resync */ + if(gps_ubx.len > 0) { + const uint8_t *p = (const uint8_t *)memchr(gps_ubx.msg_buf, GPS_UBX_SYNC1, gps_ubx.len); + if(p != NULL) { + if(verbose) printf("GPS Ublox parser resynced\r\n"); + uint16_t len_diff = p - gps_ubx.msg_buf; + uint16_t len = gps_ubx.len - len_diff; + uint8_t msg_buf[len]; + memcpy(msg_buf, p, len); + + /* Recursion (chances of extremely deep recursion are really low) */ + gps_ubx.status = UNINIT; + for(uint16_t i = 0; i < len; i++) { + gps_ubx_parse(msg_buf[i]); + + /* If two messages are embedded, the second one is discared */ + if(gps_ubx.msg_available) + return; + } + return; + } + } +restart: + gps_ubx.status = UNINIT; + return; +} + +static void send_gps_inject(uint8_t *buf, int32_t len) { + const uint8_t max_len = 250; /* Remove PPRZLink overhead for maximum packet size */ + uint16_t send_len = len % max_len; + uint16_t idx = 0; + + while(len > 0) { + if(verbose) printf("Sending RTCM over ivy [%d, %d | %d, %d]\r\n", RTCMgetbitu(gps_rtcm.msg_buf, 24 + 0, 12), gps_rtcm.len+6, idx, send_len); + char gps_packet[1024], number[5]; + snprintf(gps_packet, 1024, "ground GPS_INJECT %d %d %d", ac_id, 0, gps_rtcm.msg_buf[idx]); + for(uint16_t i = idx+1; i < idx+send_len; i++) { + snprintf(number, 5, ",%d", gps_rtcm.msg_buf[i]); + strcat(gps_packet, number); + } + IvySendMsg("%s", gps_packet); + + len -= send_len; + send_len = len % max_len; + } +} + +/** + * Handle incoming packets from the GPS + */ +void packet_handler(void *ep, uint8_t *data, uint16_t len) { + /* Try to parse it as UBX message */ + for(uint16_t i = 0; i < len; i++) { + gps_ubx_parse(data[i]); + /* Whenever we have a succesful message */ + if(gps_ubx.msg_available) { + if(verbose) printf("Got a succesfull UBX message [%d]\r\n", gps_ubx.msg_id); + + /* Try to parse messages */ + switch(gps_ubx.msg_id) { + case UBX_NAV_PVT_ID: { + double lat = UBX_NAV_PVT_lat(gps_ubx.msg_buf) / 1e7; + double lon = UBX_NAV_PVT_lon(gps_ubx.msg_buf) / 1e7; + float gSpeed = UBX_NAV_PVT_gSpeed(gps_ubx.msg_buf) / 1000.; + float headMot = UBX_NAV_PVT_headMot(gps_ubx.msg_buf) / 1e5; + float alt = UBX_NAV_PVT_height(gps_ubx.msg_buf) / 1000.; + float velD = UBX_NAV_PVT_velD(gps_ubx.msg_buf) / 1000.; + uint32_t iTOW = UBX_NAV_PVT_iTOW(gps_ubx.msg_buf); + + if(verbose) printf("Got position %f %f (%f, %f, %f, %f, %d)\r\n", lat, lon, gSpeed, headMot, alt, velD, iTOW); + IvySendMsg("ground FLIGHT_PARAM GCS %f %f %f %f %f %f %f %f %f %f %f %d %f", + 0.0, // roll, + 0.0, // pitch, + 0.0, // heading + lat, + lon, + gSpeed, + headMot, // course + alt, + -velD, + 0.0, // agl + (float)time(NULL), + iTOW, // itow + 0.0); // airspeed + + IvySendMsg("ground TARGET_POS %d %d %d %d %d %f %f %f", + ac_id, + ac_id, + (int)(lat * 1e7), + (int)(lon * 1e7), + (int)(alt * 1000), + gSpeed, + -velD, + headMot); + break; + } + default: + break; + } + gps_ubx.msg_available = false; + } + } + + /* Try to parse it as RTCM message */ + for(uint16_t i = 0; i < len; i++) { + gps_rtcm_parse(data[i]); + /* Whenever we have a succesful message */ + if(gps_rtcm.msg_available) { + if(verbose) printf("Got a succesfull RTCM message [%d]\r\n", RTCMgetbitu(gps_rtcm.msg_buf, 24 + 0, 12)); + + /* Forward the message to inject into the drone */ + send_gps_inject(gps_rtcm.msg_buf, gps_rtcm.len + 6); + + gps_rtcm.msg_available = false; + } + } +} + +int main(int argc, char** argv) { + /* Defaults */ + gps_ep.type = ENDPOINT_TYPE_NONE; +#ifdef __APPLE__ + char* ivy_bus = "224.255.255.255"; +#else + char *ivy_bus = "127.255.255.255"; +#endif + + GMainLoop *ml = g_main_loop_new(NULL, FALSE); + + IvyInit ("UBLOX2Ivy", "UBLOX2Ivy READY", NULL, NULL, NULL, NULL); + IvyStart(ivy_bus); + + /* Arguments options and usage information */ + static struct option long_options[] = { + {"endpoint", required_argument, NULL, 'e'}, + {"help", no_argument, NULL, 'h'}, + {"verbose", no_argument, NULL, 'v'}, + {0, 0, 0, 0} + }; + static const char* usage = + "Usage: %s [options]\n" + " Options :\n" + " -e --endpoint [endpoint_str] Endpoint address of the GPS\n" + " -h --help Display this help\n" + " -v --verbose Print verbose information\n"; + + int c; + int option_index = 0; + while((c = getopt_long(argc, argv, "e:hv", long_options, &option_index)) != -1) { + switch (c) { + case 'e': + // Parse the endpoint argument UDP + if(!strncmp(optarg, "udp", 3)) { + char serv_addr[125], cli_addr[125]; + uint16_t serv_port, cli_port; + if(sscanf(optarg, "udp://%[^:]:%hu:%[^:]:%hu", serv_addr, &serv_port, cli_addr, &cli_port) != 4) { + fprintf(stderr, "UDP endpoint %s has incorrect arguments\r\n", optarg); + return 2; + } + udp_create(serv_addr, serv_port, cli_addr, cli_port); + } + // Parse the endpoint argument UART + else if(!strncmp(optarg, "uart", 4)) { + char devname[256]; + uint32_t baudrate; + if(sscanf(optarg, "uart://%[^:]:%u", devname, &baudrate) != 2) { + fprintf(stderr, "UART endpoint %s has incorrect arguments\r\n", optarg); + return 2; + } + uart_create(devname, baudrate); + } + else { + fprintf(stderr, "Endpoint %s has incorrect type only uart and udp are supported\r\n", optarg); + return 2; + } + break; + + case 'v': + verbose = true; + break; + + case 'h': + fprintf(stderr, usage, argv[0]); + return 0; + + default: /* ’?’ */ + printf("?? getopt returned character code %c ??\r\n", c); + fprintf(stderr, usage, argv[0]); + return 1; + } + } + + g_main_loop_run(ml); + + /*while(true) { + usleep(50000); + }*/ + + return 0; +} \ No newline at end of file diff --git a/sw/ground_segment/misc/ublox2ivy.h b/sw/ground_segment/misc/ublox2ivy.h new file mode 100644 index 0000000000..d264cede2d --- /dev/null +++ b/sw/ground_segment/misc/ublox2ivy.h @@ -0,0 +1,105 @@ +/* + * Paparazzi UBLox to Ivy + * + * Copyright (C) 2021 Freek van Tienen + * + * 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 */ \ No newline at end of file