diff options
Diffstat (limited to 'libloragw/tst/test_loragw_gps.c')
| -rw-r--r-- | libloragw/tst/test_loragw_gps.c | 185 |
1 files changed, 153 insertions, 32 deletions
diff --git a/libloragw/tst/test_loragw_gps.c b/libloragw/tst/test_loragw_gps.c index 994c0ad..16ea6f0 100644 --- a/libloragw/tst/test_loragw_gps.c +++ b/libloragw/tst/test_loragw_gps.c @@ -51,6 +51,8 @@ struct tref ppm_ref; /* --- PRIVATE FUNCTIONS DECLARATION ---------------------------------------- */ static void sig_handler(int sigio); +static void gps_process_sync(void); +static void gps_process_coords(void); /* -------------------------------------------------------------------------- */ /* --- PRIVATE FUNCTIONS DEFINITION ----------------------------------------- */ @@ -63,6 +65,72 @@ static void sig_handler(int sigio) { } } +static void gps_process_sync(void) { + /* variables for PPM pulse GPS synchronization */ + uint32_t ppm_tstamp; + struct timespec ppm_gps; + struct timespec ppm_utc; + + /* variables for timestamp <-> GPS time conversions */ + uint32_t x, z; + struct timespec y; + + /* get GPS time for synchronization */ + int i = lgw_gps_get(&ppm_utc, &ppm_gps, NULL, NULL); + if (i != LGW_GPS_SUCCESS) { + printf(" No valid reference GPS time available, synchronization impossible.\n"); + return; + } + + /* get timestamp for synchronization */ + i = lgw_get_trigcnt(&ppm_tstamp); + if (i != LGW_HAL_SUCCESS) { + printf(" Failed to read timestamp, synchronization impossible.\n"); + return; + } + + /* try to update synchronize time reference with the new GPS & timestamp */ + i = lgw_gps_sync(&ppm_ref, ppm_tstamp, ppm_utc, ppm_gps); + if (i != LGW_GPS_SUCCESS) { + printf(" Synchronization error.\n"); + return; + } + + /* display result */ + printf(" * Synchronization successful *\n"); + printf(" UTC reference time: %lld.%09ld\n", (long long)ppm_ref.utc.tv_sec, ppm_ref.utc.tv_nsec); + printf(" GPS reference time: %lld.%09ld\n", (long long)ppm_ref.gps.tv_sec, ppm_ref.gps.tv_nsec); + printf(" Internal counter reference value: %u\n", ppm_ref.count_us); + printf(" Clock error: %.9f\n", ppm_ref.xtal_err); + + x = ppm_tstamp + 500000; + printf(" * Test of timestamp counter <-> GPS value conversion *\n"); + printf(" Test value: %u\n", x); + lgw_cnt2gps(ppm_ref, x, &y); + printf(" Conversion to GPS: %lld.%09ld\n", (long long)y.tv_sec, y.tv_nsec); + lgw_gps2cnt(ppm_ref, y, &z); + printf(" Converted back: %u ==> %dµs\n", z, (int32_t)(z-x)); + printf(" * Test of timestamp counter <-> UTC value conversion *\n"); + printf(" Test value: %u\n", x); + lgw_cnt2utc(ppm_ref, x, &y); + printf(" Conversion to UTC: %lld.%09ld\n", (long long)y.tv_sec, y.tv_nsec); + lgw_utc2cnt(ppm_ref, y, &z); + printf(" Converted back: %u ==> %dµs\n", z, (int32_t)(z-x)); +} + +static void gps_process_coords(void) { + /* position variable */ + struct coord_s coord; + struct coord_s gpserr; + int i = lgw_gps_get(NULL, NULL, &coord, &gpserr); + + /* update gateway coordinates */ + if (i == LGW_GPS_SUCCESS) { + printf("# GPS coordinates: latitude %.5f, longitude %.5f, altitude %i m\n", coord.lat, coord.lon, coord.alt); + printf("# GPS err: latitude %.5f, longitude %.5f, altitude %i m\n", gpserr.lat, gpserr.lon, gpserr.alt); + } +} + /* -------------------------------------------------------------------------- */ /* --- MAIN FUNCTION -------------------------------------------------------- */ @@ -78,9 +146,15 @@ int main() /* serial variables */ char serial_buff[128]; /* buffer to receive GPS data */ + size_t wr_idx = 0; /* pointer to end of chars in buffer */ + int gps_tty_dev; /* file descriptor to the serial port of the GNSS module */ /* NMEA/UBX variables */ - static struct tref time_reference_gps; /* time reference used for GPS <-> timestamp conversion */ + enum gps_msg latest_msg = UNKNOWN; /* keep track of latest NMEA/UBX message parsed */ + + fd_set fds; + char delim[4] = "$"; + char *token[254]; /* configure signal handling */ sigemptyset(&sigact.sa_mask); @@ -95,9 +169,9 @@ int main() printf("*** Library version information ***\n%s\n***\n", lgw_version_info()); /* Open and configure GPS */ - i = lgw_gps_enable(&gpsdata, &source); + i = lgw_gps_enable("/dev/ttyS0", "ubx7", 0, &gps_tty_dev, 1); if (i != LGW_GPS_SUCCESS) { - printf("ERROR: IMPOSSIBLE TO ENABLE GPS\n"); + printf("ERROR: Failed to enable GPS\n"); exit(EXIT_FAILURE); } @@ -124,41 +198,88 @@ int main() memset(&ppm_ref, 0, sizeof ppm_ref); /* loop until user action */ - while ((quit_sig != 1) && (exit_sig != 1)) { - wait_ms(100); - int r = gps_read(&gpsdata, 0, 0); - if (r!= -1 && (gpsdata.status != STATUS_NO_FIX) && - (gpsdata.fix.mode == MODE_2D || gpsdata.fix.mode == MODE_3D) && - !isnan(gpsdata.fix.latitude) && - !isnan(gpsdata.fix.longitude)) { - - - uint32_t trig_tstamp; /* concentrator timestamp associated with PPM pulse */ - - i = lgw_get_trigcnt(&trig_tstamp); - lgw_gps_sync(&time_reference_gps, trig_tstamp, gpsdata.fix.time); - - printf("\n--- GPS ---\n"); - printf("Set: %lld\n", gpsdata.set); - printf("Online: %10.0f\n", gpsdata.online); - printf("Status: %d\n", gpsdata.status); - printf("Satellites Used: %d\n", gpsdata.satellites_used); - printf("Mode: %d\n", gpsdata.fix.mode); - printf("UTC time: %lld.%09ld\n", (long long)time_reference_gps.utc.tv_sec, time_reference_gps.utc.tv_nsec); - printf("GPS time: %lld.%09ld\n", (long long)time_reference_gps.gps.tv_sec, time_reference_gps.gps.tv_nsec); - printf("Latitude: %f\n", gpsdata.fix.latitude); - printf("Longitude: %f\n", gpsdata.fix.longitude); - printf("Altitude: %fm\n", gpsdata.fix.altitude); - printf("Speed: %f\n", gpsdata.fix.speed); - printf("Track: %f\n", gpsdata.fix.track); - printf("Pdop: %f\n", gpsdata.dop.pdop); + 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); + } + 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(); + } + } + } 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); + } + 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 (r == -1) { + if (errno == EAGAIN) + continue; + else { + printf(stderr, "gpspipe: read error %s(%d)\n", strerror(errno), errno); + exit(EXIT_FAILURE); + } + } else { + exit(EXIT_SUCCESS); + } } } /* clean up before leaving */ if (exit_sig == 1) { - lgw_gps_disable(&gpsdata); + lgw_gps_disable(gps_tty_dev); lgw_stop(); } |