Browse Source

Expand nmea parser.

Add gps input to freedv_eth.
master
Jeroen Vreeken 4 years ago
parent
commit
1bf9337c60
4 changed files with 236 additions and 36 deletions
  1. +134
    -14
      fprs_test.c
  2. +48
    -20
      freedv_eth.c
  3. +39
    -1
      nmea.c
  4. +15
    -1
      nmea.h

+ 134
- 14
fprs_test.c View File

@ -458,37 +458,156 @@ static int test_fprs2aprs(void)
return 0;
}
static bool fp_check(double val, double checkval, double prec)
{
double diff = fabs(val - checkval);
return diff <= prec;
}
static int test_nmea_line(void)
{
struct nmea_state *state;
state = nmea_state_create();
if (!state) {
printf("NMEA state struct create failed\n");
return -1;
}
if (nmea_parse_line(state, "$GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47")) {
printf("Parsing NMEA GGA line failed\n");
return -1;
}
if (!state->position_valid || !state->altitude_valid)
return -1;
if (!fp_check(state->latitude, 48.117300, 0.000001)) {
return -1;
}
if (!fp_check(state->longitude, 11.516667, 0.000001)) {
return -1;
}
if (!fp_check(state->altitude, 545.4, 0.01)) {
return -1;
}
if (nmea_parse_line(state, "$GPVTG,054.7,T,034.4,M,005.5,N,010.2,K*48")) {
printf("Parsing NMEA VTG line failed\n");
return -1;
}
if (!state->course_valid || !state->speed_valid)
return -1;
if (!fp_check(state->course, 54.7, 0.01)) {
return -1;
}
if (!fp_check(state->speed, 2.82, 0.01)) {
return -1;
}
if (nmea_parse_line(state, "$GPRMC,123519,A,4807.038,N,01131.000,E,022.4,084.4,230394,003.1,W*6A")) {
printf("Parsing NMEA RMC line failed\n");
return -1;
}
if (!state->position_valid)
return -1;
if (!fp_check(state->latitude, 48.117300, 0.000001)) {
return -1;
}
if (!fp_check(state->longitude, 11.516667, 0.000001)) {
return -1;
}
if (!state->course_valid || !state->speed_valid)
return -1;
if (!fp_check(state->course, 84.4, 0.01)) {
return -1;
}
if (!fp_check(state->speed, 11.52, 0.01)) {
return -1;
}
nmea_state_destroy(state);
return 0;
}
static int test_nmea(void)
{
struct nmea_state state;
struct nmea_state *state;
if (nmea_parse(&state, "$GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47")) {
state = nmea_state_create();
if (!state) {
printf("NMEA state struct create failed\n");
return -1;
}
char *data1 = "blabla*22$GPGSHIT*33$GPGGA,123519,0407.838,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47";
char *data2 = "#$@#*33$GPVTG,054.7,T,034.4,M,005.5,N,010.2,K*48";
char *data3 = "$GPRMC,123519,A,4807.038,N,01131.000,E,022.4,084.4,230394,003.1,W*6A";
if (nmea_parse(state, data1, strlen(data1))) {
printf("Parsing NMEA GGA line failed\n");
return -1;
}
printf("%d %f %f %d %f\n",
state.position_valid, state.latitude, state.longitude,
state.altitude_valid, state.altitude);
if (!state->position_valid || !state->altitude_valid)
return -1;
if (!fp_check(state->latitude, 4.130633, 0.000001)) {
return -1;
}
if (!fp_check(state->longitude, 11.516667, 0.000001)) {
return -1;
}
if (!fp_check(state->altitude, 545.4, 0.01)) {
return -1;
}
if (nmea_parse(&state, "$GPVTG,054.7,T,034.4,M,005.5,N,010.2,K*48")) {
if (nmea_parse(state, data2, strlen(data2))) {
printf("Parsing NMEA VTG line failed\n");
return -1;
}
printf("%d %f %d %f\n",
state.course_valid, state.course,
state.speed_valid, state.speed);
if (nmea_parse(&state, "$GPRMC,123519,A,4807.038,N,01131.000,E,022.4,084.4,230394,003.1,W*6A")) {
if (!state->course_valid || !state->speed_valid)
return -1;
if (!fp_check(state->course, 54.7, 0.01)) {
return -1;
}
if (!fp_check(state->speed, 2.82, 0.01)) {
return -1;
}
if (nmea_parse(state, data3, strlen(data3))) {
printf("Parsing NMEA RMC line failed\n");
return -1;
}
printf("%d %f %f %d %f %d %f\n",
state.position_valid, state.latitude, state.longitude,
state.course_valid, state.course,
state.speed_valid, state.speed);
if (!state->position_valid)
return -1;
if (!fp_check(state->latitude, 48.117300, 0.000001)) {
return -1;
}
if (!fp_check(state->longitude, 11.516667, 0.000001)) {
return -1;
}
if (!state->course_valid || !state->speed_valid)
return -1;
if (!fp_check(state->course, 84.4, 0.01)) {
return -1;
}
if (!fp_check(state->speed, 11.52, 0.01)) {
return -1;
}
nmea_state_destroy(state);
return 0;
}
@ -506,6 +625,7 @@ struct fprs_test {
{ "Objectname", test_objectname },
{ "vector", test_vector },
{ "fprs2aprs", test_fprs2aprs },
{ "nmea (line)", test_nmea_line },
{ "nmea", test_nmea },
};


+ 48
- 20
freedv_eth.c View File

@ -22,9 +22,12 @@
#include <poll.h>
#include <sched.h>
#include <errno.h>
#include <fcntl.h>
#include <string.h>
#include <time.h>
#include <sys/mman.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <hamlib/rig.h>
#include <codec2/freedv_api.h>
@ -76,7 +79,7 @@ static uint8_t rx_add[6];
static uint8_t tx_add[6];
static int rx_sync = 0;
struct nmea_state nmea;
struct nmea_state *nmea;
#define RX_SYNC_THRESHOLD 20
@ -223,7 +226,7 @@ static void dequeue_voice(void)
if (tx_data_len == bytes_per_codec_frame) {
double energy = codec2_get_energy(codec2, tx_data);
bool fprs_late = tx_state_fprs_cnt >= tx_fprs && nmea.position_valid;
bool fprs_late = nmea && tx_state_fprs_cnt >= tx_fprs && nmea->position_valid;
bool header_late = tx_state_data_header_cnt >= tx_header;
bool have_data = fprs_late || queue_data || header_late;
// printf("e: %f\n", energy);
@ -333,7 +336,7 @@ static void freedv_cb_datarx(void *arg, unsigned char *packet, size_t size)
static void freedv_cb_datatx(void *arg, unsigned char *packet, size_t *size)
{
if (tx_state == TX_STATE_ON) {
bool fprs_late = tx_state_fprs_cnt >= tx_fprs && nmea.position_valid;
bool fprs_late = nmea && tx_state_fprs_cnt >= tx_fprs && nmea->position_valid;
printf("data %d %d %d\n", tx_state_fprs_cnt, fprs_late, tx_state_data_header_cnt);
if ((!queue_data && !fprs_late) ||
@ -345,12 +348,12 @@ static void freedv_cb_datatx(void *arg, unsigned char *packet, size_t *size)
/* Send fprs frame */
struct fprs_frame *frame = fprs_frame_create();
fprs_frame_add_position(frame,
nmea.longitude, nmea.latitude, false);
if (nmea.altitude_valid)
fprs_frame_add_altitude(frame, nmea.altitude);
if (nmea.speed_valid && nmea.course_valid &&
nmea.speed >= FPRS_VECTOR_SPEED_EPSILON)
fprs_frame_add_vector(frame, nmea.course, 0.0, nmea.speed);
nmea->longitude, nmea->latitude, false);
if (nmea->altitude_valid)
fprs_frame_add_altitude(frame, nmea->altitude);
if (nmea->speed_valid && nmea->course_valid &&
nmea->speed >= FPRS_VECTOR_SPEED_EPSILON)
fprs_frame_add_vector(frame, nmea->course, 0.0, nmea->speed);
fprs_frame_add_symbol(frame, (uint8_t[2]){'F','#'});
@ -540,6 +543,17 @@ static char vc_callback_tx(void *arg)
return c;
}
void read_nmea(int fd_nmea)
{
char buffer[256];
ssize_t r;
r = read(fd_nmea, buffer, 256);
if (r > 0) {
nmea_parse(nmea, buffer, r);
}
}
static void usage(void)
{
printf("Options:\n");
@ -564,11 +578,13 @@ int main(int argc, char **argv)
char *netname = "freedv";
char *nmeadev = NULL;
int fd_int;
int fd_nmea = -1;
struct pollfd *fds;
int sound_fdc_tx;
int sound_fdc_rx;
int nfds;
int poll_int;
int poll_nmea = 0;
int opt;
uint16_t type = ETH_P_CODEC2_700;
int nr_samples;
@ -699,17 +715,21 @@ int main(int argc, char **argv)
printf("TX header max: %d periods\n", tx_header_max);
if (nmeadev) {
nmea.position_valid = true;
nmea.longitude = 5.44397;
nmea.latitude = 51.3528;
nmea = nmea_state_create();
fd_nmea = open(nmeadev, O_RDONLY);
nmea.altitude_valid = true;
nmea.altitude = 25.0;
nmea->position_valid = true;
nmea->longitude = 5.44397;
nmea->latitude = 51.3528;
nmea.speed_valid = true;
nmea.speed = 0;
nmea.course_valid = true;
nmea.course = 0;
nmea->altitude_valid = true;
nmea->altitude = 25.0;
nmea->speed_valid = true;
nmea->speed = 0;
nmea->course_valid = true;
nmea->course = 0;
}
hl_init();
@ -723,7 +743,7 @@ int main(int argc, char **argv)
sound_fdc_tx = sound_poll_count_tx();
sound_fdc_rx = sound_poll_count_rx();
nfds = sound_fdc_tx + sound_fdc_rx + 1;
nfds = sound_fdc_tx + sound_fdc_rx + 1 + (nmea ? 1 : 0);
fds = calloc(sizeof(struct pollfd), nfds);
sound_poll_fill_tx(fds, sound_fdc_tx);
@ -731,7 +751,12 @@ int main(int argc, char **argv)
poll_int = sound_fdc_tx + sound_fdc_rx;
fds[poll_int].fd = fd_int;
fds[poll_int].events = POLLIN;
if (nmea) {
poll_nmea = poll_int + 1;
fds[poll_nmea].fd = fd_nmea;
fds[poll_nmea].events = POLLIN;
}
do {
poll(fds, nfds, -1);
if (fds[poll_int].revents & POLLIN) {
@ -743,6 +768,9 @@ int main(int argc, char **argv)
if (sound_poll_in_rx(fds + sound_fdc_tx, sound_fdc_rx)) {
sound_rx();
}
if (nmea && fds[poll_nmea].revents & POLLIN) {
read_nmea(fd_nmea);
}
} while (1);


+ 39
- 1
nmea.c View File

@ -21,6 +21,21 @@
#include <string.h>
#include <stdio.h>
struct nmea_state *nmea_state_create(void)
{
struct nmea_state *nmea;
nmea = calloc(1, sizeof(struct nmea_state));
return nmea;
}
void nmea_state_destroy(struct nmea_state *nmea)
{
free(nmea);
}
static bool nmea_check(char *line)
{
int i;
@ -50,7 +65,7 @@ static bool nmea_check(char *line)
return false;
}
int nmea_parse(struct nmea_state *state, char *line)
int nmea_parse_line(struct nmea_state *state, char *line)
{
if (!nmea_check(line))
return -1;
@ -133,3 +148,26 @@ int nmea_parse(struct nmea_state *state, char *line)
}
int nmea_parse(struct nmea_state *nmea, char *data, size_t size)
{
int i;
for (i = 0; i < size; i++) {
if (data[i] == '$') {
nmea->pos = 0;
}
nmea->line[nmea->pos] = data[i];
nmea->pos++;
if (nmea->pos >= 3 && nmea->line[nmea->pos-3] == '*') {
nmea->line[nmea->pos] = 0;
nmea_parse_line(nmea, nmea->line);
nmea->pos = 0;
}
if (nmea->pos >= NMEA_LEN)
nmea->pos = 0;
}
return 0;
}

+ 15
- 1
nmea.h View File

@ -18,9 +18,14 @@
#ifndef _INCLUDE_NMEA_H_
#define _INCLUDE_NMEA_H_
#include <stdlib.h>
#include <stdbool.h>
#define NMEA_LEN 83
#define NMEA_SIZE 84
struct nmea_state {
/* Parsed state */
bool position_valid;
double latitude;
double longitude;
@ -33,8 +38,17 @@ struct nmea_state {
bool course_valid;
double course;
/* Internal state */
char line[NMEA_SIZE];
int pos;
};
int nmea_parse(struct nmea_state *state, char *line);
struct nmea_state *nmea_state_create(void);
void nmea_state_destroy(struct nmea_state *nmea);
int nmea_parse_line(struct nmea_state *state, char *line);
int nmea_parse(struct nmea_state *nmea, char *data, size_t size);
#endif /* _INCLUDE_NMEA_H_ */

Loading…
Cancel
Save