gps – udp

Programming

gps – udp

Our c program can read serial data from a gps device. In this post we’ll look at how to set up a UDP server that other clients can query for up to date gps data.

...
#include <string.h>
...
//for threading
#include <pthread.h>

//for udp server
#include <sys/types.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <netinet/in.h>
...
#define PORT 9998
#define UDP_MAXLINE 1024 //udp packet size
...
#define GGA_STR_BUFLEN 100
char gga_str[GGA_STR_BUFLEN]; //to store GPGGA nmea sentence

We include <pthread.h> for threading, and a number of libraries to implement the UDP server. We define UDP server PORT as 9998, this can be changed if it clashes. Finally we define a buffer length for UDP packets and a buffer length and string to store GPGGA nmea sentences.

void *udp_serv(void *arg)
{
	printf("Started UDP thread\n");

	int sockfd; //socket file descriptor
	char buffer[UDP_MAXLINE];
	struct sockaddr_in servaddr, cliaddr;
	socklen_t cliaddr_len = sizeof(cliaddr);

	//create a socket, domain=AF_INET, type=SOCK_DGRAM, protocol=unspecified
	if((sockfd = socket(AF_INET, SOCK_DGRAM, 0)) < 0){
		perror("Failed to create socket.\n");
		exit(EXIT_FAILURE);
	}

	memset(&servaddr, 0, sizeof(servaddr));
	memset(&cliaddr, 0, sizeof(cliaddr));

	servaddr.sin_family = AF_INET;
	servaddr.sin_addr.s_addr = INADDR_ANY;
	servaddr.sin_port = htons(PORT);

	//bind the socket to network address
	if(bind(sockfd, (const struct sockaddr *)&servaddr, sizeof(servaddr)) < 0){
		perror("Failed to bind socket to address.\n");
		exit(EXIT_FAILURE);
	}
	printf("Started UDP server\n");

	while(1){
		int n;
		n = recvfrom(sockfd, (char *)buffer, UDP_MAXLINE,
			MSG_WAITALL, (struct sockaddr *)&cliaddr,
			&cliaddr_len);
		buffer[n] = '\0'; //make it a cstring
		printf("Message from client: %s\n", buffer);

		if(!strcmp(buffer, "GPGGA")){
			//overwrite the buffer with msg to send
			snprintf(buffer, UDP_MAXLINE, "%s\n", gga_str);
			sendto(sockfd, (const char *)buffer, strlen(buffer),
				MSG_CONFIRM, (const struct sockaddr *)&cliaddr,
				cliaddr_len);
			printf("Sent GGA to client: %s", buffer);
		} else {
			sprintf(buffer, "NACK\n"); //Generic response No-Acknowledge
			sendto(sockfd, (const char *)buffer, strlen(buffer),
				MSG_CONFIRM, (const struct sockaddr *)&cliaddr,
				cliaddr_len);
			printf("sent NACK to client\n");
		}
	}
	close(sockfd);
	return NULL; //we need a return value
}

The udp_serv handles the UDP server. Because this function is spawned with pthread, it must return a void * and receive void * arguments.  See man socket and man bind for more information on these functions.

The while(1) loops forever, waiting for clients to send a message to the server using the recvfrom function. Once a message is received, we check if the client has said “GPGGA”, this is a request for the data kept in our gga_str. We send the data to the client using the sendto function. See man recvfrom and man sendto for more information on these functions. One gotcha, as we’ve done in this code, ensure that the cliaddr_len variable is initialized prior to sending the client a message. I encountered a bug where the first message sent from server to client was always dropped, it turns out that setting socklen_t cliaddr_len = sizeof(cliaddr) prior to passing it to the sendto function fixed this problem.

int main(void)
{
	fd = open(DEVICE_NAME, O_RDONLY | O_NOCTTY); //open the gps device
	if(fd < 0){
		perror("open device error\n");
		exit(EXIT_FAILURE);
	}
	
	set_input_mode(); //set the terminal input mode
	
	pthread_t udp_thread;
	if(pthread_create(&udp_thread, NULL, udp_serv, NULL)){
		perror("Failed to spawn udp thread.\n");
		exit(EXIT_FAILURE);
	}

	char buf[DEVICE_LINE_BUFLEN];
	while(1){
		serial_getline(buf);
		strncpy(gga_str, buf, GGA_STR_BUFLEN);
		//printf("%s\n", buf);
	}
	return EXIT_SUCCESS; //never reached
}

Main is mostly unchanged, except for the addition of pthreads and the copying of buf into our gga_str variable using strncpy. I’ve commented out the printf statement to hush the server a bit as well.

Our program has two threads, the parent thread (in main) is responsible for polling data from the gps device, while the child thread (called by pthread_create) acts as a UDP server. Check out man pthread_create.

The final code is at the end of this post.

$ make
cc -Iinclude -Wall -c src/whereami.c -o obj/whereami.o
cc -Llib obj/whereami.o -lm -lpthread -o whereami
$ ./whereami
Started UDP thread
Started UDP server

The thread has started successfully and the server is waiting for messages from a client. Now we just need a client implementation!

$ vim client.sh
#!/bin/bash

if [[ $# -eq 0 ]] ; then
	echo 'require 1 argument'
	exit 1
fi

exec 3<>/dev/udp/127.0.0.1/9998 #open file descriptor to UDP server
echo -ne $1 >&3                 #echo first argument to the server
head -n1 <&3                    #print the server response
exec 3>&-                       #close the file descriptor
$ chmod +x client.sh
$ ./client.sh GPGGA
$GPGGA,073206.00,,,,,0,00,99.99,,,,,,*66

Our bash client is simple. It does little error checking, other than to validate that a message to pass to the server is in fact supplied.

Speaking of shortcomings, the server program itself lacks some error checking.

  • Check that gga_str has been initialized prior to sending data to clients
  • Only store sentences in gga_str if they have been validated non-zero and checksum matches
  • Handle gps hot-plugging
  • Among other things

Oh. A happy side-effect of setting up the UDP server the way we did is that it can be accessed from anywhere on the local network. We can extend the bash client to accept some commandline arguments for address and port, I’m using an example found here:

$ vim client.sh
#!/bin/bash

#defaults
params=""
address="127.0.0.1"
port="9998"

#process arguments
while (( "$#" )); do
	case "$1" in
		-a|--address)
			address=$2
			shift 2
			;;
		-p|--port)
			port=$2
			shift 2
			;;
		--)
			shift
			break
			;;
		-*|--*=)
			echo "Error: Unsupported flag $1" >&2
			exit 1
			;;
		*)
			params="$params $1"
			shift
			;;
	esac
done

eval set -- "$params"

if [[ $# -eq 0 ]] ; then
	echo 'require message to send'
	exit 1
fi

exec 3<>/dev/udp/$address/$port #open file descriptor to UDP server
echo -ne $1 >&3                 #echo first argument to the server
head -n1 <&3                    #print the server response
exec 3>&-   

We can use this from any computer on the local network by specifying the local ip and port that the server is using. Below I use hostname -i to find my servers local ip (note; this is not very portable. You may need another way of finding it), then start the server. On another computer, I run the client.sh script and point it to UDP server address and port.

[serverpc]$ hostname -i
192.168.1.111
[serverpc]$ ./whereami
Started UDP thread
Started UDP server

[someotherpc]$ ./client.sh --address 192.168.1.111 --port 9998 GPGGA
$GPGGA,085750.00,,,,,0,00,99.99,,,,,,*69

We get a response! Nice one
Next post we’ll look at daemonizing the server process.

Complete whereami.c:

#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

//for serial connection
#include <termios.h>
#include <fcntl.h>

#define BAUDRATE B9600
#define DEVICE_NAME "/dev/ttyUSB0"
#define DEVICE_LINE_BUFLEN 100	//size of buffer when reading lines from device

//for udp server
#include <pthread.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <netinet/in.h>

#define PORT 9998
#define UDP_MAXLINE 1024 //udp packet size

static int fd; //gps device file descriptor
static struct termios old_config; //old terminal config, to reset later

#define GGA_STR_BUFLEN 100
char gga_str[GGA_STR_BUFLEN]; //to store GPGGA nmea sentence

/*
 * Called at process exit to reset the terminal state to how we found it.
 */
void reset_input_mode()
{
	tcsetattr(fd, TCSANOW, &old_config);
	close(fd);
}

/*
 * Set the terminal mode so that we can read the gps device.
 */
int set_input_mode()
{
	struct termios new_config;
	memset(&new_config, 0, sizeof(new_config));

	//get the current terminal settings
	if(tcgetattr(fd, &old_config) == -1){
		perror("tcgetattr termios function error\n");
		exit(EXIT_FAILURE);
	}
	
	atexit(reset_input_mode); //reset terminal settings once we're done
	
	new_config.c_cflag |= CLOCAL | CREAD;
	new_config.c_cflag &= ~CSIZE;
	new_config.c_cflag |= CS8; //set 8-bit char, stop bit
	new_config.c_cflag &= ~PARENB; //unset parity bit
	new_config.c_cflag &= ~CSTOPB; //unset double stop bit
	new_config.c_cflag &= ~CRTSCTS; //unset hardware control

	new_config.c_lflag |= ICANON | ISIG; //set canonical input
	new_config.c_lflag &= ~(ECHO | ECHOE | ECHONL | IEXTEN); //unset echo

	new_config.c_iflag &= ~INPCK;
	new_config.c_iflag |= IGNCR; //set ignore carriage return
	new_config.c_iflag &= ~(INLCR | ICRNL | IUCLC | IMAXBEL);
	new_config.c_iflag &= ~(IXON | IXOFF | IXANY); //unset control char's

	new_config.c_oflag &= ~OPOST; //unset post processing

	cfsetospeed(&new_config, BAUDRATE); //set outgoing baudrate
	cfsetispeed(&new_config, BAUDRATE); //set incoming baudrate

	tcflush(fd, TCIOFLUSH); //flush

	//set terminal settings with above.
	if(tcsetattr(fd, TCSAFLUSH, &new_config) < 0){	//TCSAFLUSH wait for incoming data to
		perror("tcsetattr function error.\n");	//finish before writing changes
		exit(EXIT_FAILURE);			//TCSANOW sometimes causes incomplete
	}						//lines to be written, avoid here.
	
	return 0;
}

/*
 * Fill line[] buffer with incoming line from device
 * return number of characters filled into buffer.
 */
int serial_getline(char line[]){
	if(fd < 0){
		printf("device not opened correctly.\n");
		exit(EXIT_FAILURE);
	}
	//read incoming data from device into line buffer.
	int rrv = read(fd, line, DEVICE_LINE_BUFLEN - 1);
	if(rrv < 0){
		perror("read error\n");
		exit(EXIT_FAILURE);
	}
	line[rrv - 1] = '\0'; //intentionally overwrite the newline with null byte
	return(rrv - 2);
}

void *udp_serv(void *arg)
{
	printf("Started UDP thread\n");

	int sockfd; //socket file descriptor
	char buffer[UDP_MAXLINE];
	struct sockaddr_in servaddr, cliaddr;
	socklen_t cliaddr_len = sizeof(cliaddr);

	//create a socket, domain=AF_INET, type=SOCK_DGRAM, protocol=unspecified
	if((sockfd = socket(AF_INET, SOCK_DGRAM, 0)) < 0){
		perror("Failed to create socket.\n");
		exit(EXIT_FAILURE);
	}

	memset(&servaddr, 0, sizeof(servaddr));
	memset(&cliaddr, 0, sizeof(cliaddr));

	servaddr.sin_family = AF_INET;
	servaddr.sin_addr.s_addr = INADDR_ANY;
	servaddr.sin_port = htons(PORT);

	//bind the socket to network address
	if(bind(sockfd, (const struct sockaddr *)&servaddr, sizeof(servaddr)) < 0){
		perror("Failed to bind socket to address.\n");
		exit(EXIT_FAILURE);
	}
	printf("Started UDP server\n");

	while(1){
		int n;
		n = recvfrom(sockfd, (char *)buffer, UDP_MAXLINE,
			MSG_WAITALL, (struct sockaddr *)&cliaddr,
			&cliaddr_len);
		buffer[n] = '\0'; //make it a cstring
		printf("Message from client: %s\n", buffer);

		if(!strcmp(buffer, "GPGGA")){
			//overwrite the buffer with msg to send
			snprintf(buffer, UDP_MAXLINE, "%s\n", gga_str);
			sendto(sockfd, (const char *)buffer, strlen(buffer),
				MSG_CONFIRM, (const struct sockaddr *)&cliaddr,
				cliaddr_len);
			printf("Sent GGA to client: %s", buffer);
		} else {
			sprintf(buffer, "NACK\n"); //Generic response No-Acknowledge
			sendto(sockfd, (const char *)buffer, strlen(buffer),
				MSG_CONFIRM, (const struct sockaddr *)&cliaddr,
				cliaddr_len);
			printf("sent NACK to client\n");
		}
	}
	close(sockfd);
	return NULL; //we need a return value
}

int main(void)
{
	fd = open(DEVICE_NAME, O_RDONLY | O_NOCTTY); //open the gps device
	if(fd < 0){
		perror("open device error\n");
		exit(EXIT_FAILURE);
	}
	
	set_input_mode(); //set the terminal input mode
	
	pthread_t udp_thread;
	if(pthread_create(&udp_thread, NULL, udp_serv, NULL)){
		perror("Failed to spawn udp thread.\n");
		exit(EXIT_FAILURE);
	}

	char buf[DEVICE_LINE_BUFLEN];
	while(1){
		serial_getline(buf);
		if(strstr(buf, "GPGGA") != NULL){
			strncpy(gga_str, buf, GGA_STR_BUFLEN);
		}
		//printf("%s\n", buf);
	}
	return EXIT_SUCCESS; //never reached
}

Leave a Reply

Your email address will not be published. Required fields are marked *

This site uses Akismet to reduce spam. Learn how your comment data is processed.