Expand nmea parser.

Add gps input to freedv_eth.

fprs_test.c

@@ -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 },
 };
 

freedv_eth.c

@@ -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);
 	
 	

nmea.c

@@ -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;
+}

nmea.h

@@ -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_ */