summaryrefslogtreecommitdiff
path: root/libloragw/tst/test_loragw_gps.c
diff options
context:
space:
mode:
Diffstat (limited to 'libloragw/tst/test_loragw_gps.c')
-rw-r--r--libloragw/tst/test_loragw_gps.c209
1 files changed, 131 insertions, 78 deletions
diff --git a/libloragw/tst/test_loragw_gps.c b/libloragw/tst/test_loragw_gps.c
index 16ea6f0..05ec53c 100644
--- a/libloragw/tst/test_loragw_gps.c
+++ b/libloragw/tst/test_loragw_gps.c
@@ -31,8 +31,6 @@ Maintainer: Michael Coracin
#include <signal.h> /* sigaction */
#include <stdlib.h> /* exit */
#include <unistd.h> /* read */
-#include <gps.h>
-#include <gpsd.h>
#include "loragw_hal.h"
#include "loragw_gps.h"
@@ -43,8 +41,7 @@ Maintainer: Michael Coracin
static int exit_sig = 0; /* 1 -> application terminates cleanly (shut down hardware, close open files, etc) */
static int quit_sig = 0; /* 1 -> application terminates without shutting down the hardware */
-static struct gps_data_t gpsdata;
-static struct fixsource_t source;
+
struct tref ppm_ref;
/* -------------------------------------------------------------------------- */
@@ -150,11 +147,7 @@ int main()
int gps_tty_dev; /* file descriptor to the serial port of the GNSS module */
/* NMEA/UBX variables */
- enum gps_msg latest_msg = UNKNOWN; /* keep track of latest NMEA/UBX message parsed */
-
- fd_set fds;
- char delim[4] = "$";
- char *token[254];
+ enum gps_msg latest_msg; /* keep track of latest NMEA/UBX message parsed */
/* configure signal handling */
sigemptyset(&sigact.sa_mask);
@@ -169,7 +162,7 @@ int main()
printf("*** Library version information ***\n%s\n***\n", lgw_version_info());
/* Open and configure GPS */
- i = lgw_gps_enable("/dev/ttyS0", "ubx7", 0, &gps_tty_dev, 1);
+ i = lgw_gps_enable();
if (i != LGW_GPS_SUCCESS) {
printf("ERROR: Failed to enable GPS\n");
exit(EXIT_FAILURE);
@@ -197,89 +190,149 @@ int main()
memset(serial_buff, 0, sizeof serial_buff);
memset(&ppm_ref, 0, sizeof ppm_ref);
+ uint8_t read_fail_count = 0;
+ uint8_t empty_packet_count = 0;
+ enum gps_state state = GPS_RUNNING;
+
/* loop until user action */
while ((quit_sig != 1) && (exit_sig != 1)) {
- int r = 0;
- struct timeval tv;
- tv.tv_sec = 0;
- tv.tv_usec = 100000;
- FD_ZERO(&fds);
- FD_SET(gpsdata.gps_fd, &fds);
- errno = 0;
- r = select(gpsdata.gps_fd+1, &fds, NULL, NULL, &tv);
- if (r == -1 && errno != EINTR) {
- printf("gpspipe: select error %s(%d)\n", strerror(errno), errno);
- exit(EXIT_FAILURE);
- } else if (r == 0)
- continue;
-
- /* reading directly from the socket avoids decode overhead */
- errno = 0;
- r = (int)read(gpsdata.gps_fd, serial_buff, sizeof(serial_buff));
- if (r > 0) {
- int i = 0;
- size_t frame_size = 0;
- for (i = 0; i < r; i++) {
- if (serial_buff[i] == (char)LGW_GPS_UBX_SYNC_CHAR) {
- /***********************
- * Found UBX sync char *
- ***********************/
- size_t ubx_size = (uint8_t)serial_buff[i+4];
- ubx_size |= (uint8_t)serial_buff[i+5] << 8;
- ubx_size += 8;
- if (ubx_size < 27){
- latest_msg = lgw_parse_ubx(&serial_buff[i], ubx_size , &frame_size);
+ size_t rd_idx = 0;
+ size_t frame_end_idx = 0;
+ ssize_t nb_char = 0;
+
+ switch (state) {
+ case GPS_LOST: {
+ i = lgw_gps_disable();
+ if (i == LGW_HAL_SUCCESS) {
+ printf("INFO: GPS closed successfully\n");
+ } else {
+ printf("WARNING: failed to close GPS successfully\n");
+ }
+ empty_packet_count = 0;
+ read_fail_count = 0;
+ state = GPS_RECONNECTING;
+ wait_ms(5000);
+ continue;
+ }
+ case GPS_RECONNECTING: {
+ /* try and reestablish connection */
+ i = lgw_gps_enable();
+ if (i != LGW_GPS_SUCCESS) {
+ printf("WARNING: [main] impossible to open for GPS sync (Check GPSD)\n");
+ wait_ms(2000);
+ continue;
+ } else {
+ printf("INFO: [main] GPSD polling open for GPS synchronization\n");
+ state = GPS_RUNNING;
+ wait_ms(1000);
+ continue;
+ }
+ }
+ case GPS_RUNNING: {
+ int result = lgw_gps_data_ready();
+
+ if (result != 1) {
+ empty_packet_count++;
+ if (empty_packet_count > 9) {
+ state = GPS_LOST;
}
- if (frame_size > 0) {
- if(latest_msg == INVALID || latest_msg == UNKNOWN) {
- /* checksum failed */
- frame_size = 0;
- } else if (latest_msg == UBX_NAV_TIMEGPS) {
- gps_process_sync();
- }
+ continue;
+ }
+ /* reading directly from the socket avoids decode overhead */
+ nb_char = lgw_gps_stream(serial_buff + wr_idx, sizeof(serial_buff) - wr_idx);
+ if (nb_char <= 0) {
+ printf("WARNING: [gps] read() returned value %zd\n", nb_char);
+ read_fail_count++;
+ if (read_fail_count > 9) {
+ state = GPS_LOST;
+ continue;
}
- } else if((serial_buff[i] == LGW_GPS_NMEA_SYNC_CHAR) && (serial_buff[i+1] == 0x47) && (serial_buff[i+2] == 0x50)){
- /************************
- * Found NMEA sync char *
- ************************/
- int k, l= 0;
- token[0] = strtok(serial_buff, delim);
-
- while (token[l] != NULL) {
- l++;
- token[l] = strtok(NULL, delim);
+ }
+ break;
+ }
+ default:
+ printf("ERROR: Unknown GPS state, resetting\n");
+ state = GPS_LOST;
+ continue;
+ }
+
+ wr_idx += (size_t)nb_char;
+
+ /*******************************************
+ * Scan buffer for UBX/NMEA sync chars and *
+ * attempt to decode frame if one is found *
+ *******************************************/
+ while (rd_idx < wr_idx) {
+ size_t frame_size = 0;
+
+ /* Scan buffer for UBX sync char */
+ if (serial_buff[rd_idx] == (char)LGW_GPS_UBX_SYNC_CHAR) {
+
+ /***********************
+ * Found UBX sync char *
+ ***********************/
+ latest_msg = lgw_parse_ubx(&serial_buff[rd_idx], (wr_idx - rd_idx), &frame_size);
+
+ if (frame_size > 0) {
+ if (latest_msg == INCOMPLETE) {
+ /* UBX header found but frame appears to be missing bytes */
+ frame_size = 0;
+ } else if (latest_msg == INVALID) {
+ /* message header received but message appears to be corrupted */
+ printf("WARNING: [gps] could not get a valid message from GPS (no time)\n");
+ frame_size = 0;
+ } else if (latest_msg == UBX_NAV_TIMEGPS) {
+ printf("\n~~ UBX NAV-TIMEGPS sentence, triggering synchronization attempt ~~\n");
+ gps_process_sync();
}
- for (k=0; k<=l-1; k++) {
- if ((strlen(token[k]) > 66) && (strlen(token[k]) < 74)){
- lgw_parse_nmea(token[k], strlen(token[k]));
- if(latest_msg == INVALID || latest_msg == UNKNOWN) {
- /* checksum failed */
- frame_size = 0;
- } else if (latest_msg == NMEA_RMC) {
- /* Get location from RMC frames */
- gps_process_coords();
- }
- }
+ }
+ } else if(serial_buff[rd_idx] == (char)LGW_GPS_NMEA_SYNC_CHAR) {
+ /************************
+ * Found NMEA sync char *
+ ************************/
+ /* scan for NMEA end marker (LF = 0x0a) */
+ char* nmea_end_ptr = memchr(&serial_buff[rd_idx],(int)0x0a, (wr_idx - rd_idx));
+
+ if (nmea_end_ptr) {
+ /* found end marker */
+ frame_size = nmea_end_ptr - &serial_buff[rd_idx] + 1;
+ latest_msg = lgw_parse_nmea(&serial_buff[rd_idx], frame_size);
+
+ if(latest_msg == INVALID || latest_msg == UNKNOWN) {
+ /* checksum failed */
+ frame_size = 0;
+ } else if (latest_msg == NMEA_RMC) { /* Get location from RMC frames */
+ gps_process_coords();
}
}
}
- } else {
- if (r == -1) {
- if (errno == EAGAIN)
- continue;
- else {
- printf(stderr, "gpspipe: read error %s(%d)\n", strerror(errno), errno);
- exit(EXIT_FAILURE);
- }
+
+ if (frame_size > 0) {
+ /* At this point message is a checksum verified frame
+ we're processed or ignored. Remove frame from buffer */
+ rd_idx += frame_size;
+ frame_end_idx = rd_idx;
} else {
- exit(EXIT_SUCCESS);
+ rd_idx++;
}
+ } /* ...for(rd_idx = 0... */
+
+ if (frame_end_idx) {
+ /* Frames have been processed. Remove bytes to end of last processed frame */
+ memcpy(serial_buff,&serial_buff[frame_end_idx],wr_idx - frame_end_idx);
+ wr_idx -= frame_end_idx;
+ } /* ...for(rd_idx = 0... */
+
+ /* Prevent buffer overflow */
+ if ((sizeof(serial_buff) - wr_idx) < LGW_GPS_MIN_MSG_SIZE) {
+ memcpy(serial_buff,&serial_buff[LGW_GPS_MIN_MSG_SIZE],wr_idx - LGW_GPS_MIN_MSG_SIZE);
+ wr_idx -= LGW_GPS_MIN_MSG_SIZE;
}
}
/* clean up before leaving */
if (exit_sig == 1) {
- lgw_gps_disable(gps_tty_dev);
+ lgw_gps_disable();
lgw_stop();
}
generated by cgit v1.2.3 (git 2.25.1) at 2024-05-10 01:37:09 +0000