UKHAS Wiki

/* gpsparser.c 
 * James Coxon - jacoxon@googlemail.com
 * Serial code adapted from:
 * Copyleft (c) 2006, Tod E. Kurt, tod@todbot.com
 * http://todbot.com/blog/
 * http://todbot.com/arduino/host/arduino-serial/arduino-serial.c
 * 
 * Compile on the gumstix:
 * gcc -o gpsparser gpsparser.c -static
 *
 * Probably doesn' work :-D */
 
 
#include <stdio.h>   /* Standard input/output definitions */
#include <string.h>  /* String function definitions */
#include <unistd.h>  /* UNIX standard function definitions */
#include <fcntl.h>   /* File control definitions */
#include <errno.h>   /* Error number definitions */
#include <termios.h> /* POSIX terminal control definitions */
#include <stdlib.h>
 
 
int x; /* int to allow read_serial_port to loop 10 times*/
int fd, res; /* File descriptor for the port */
char buf[255];
int gpsTime;
float gpsLong, gpsLat, RMCgroundSpeed, gpsTangle, magVariation;
float trueTrack, magTrack, VTGgroundSpeedKn, VTGgroundSpeedKm;
int GGAsats, GGAtime, GGAfix;
float GGAlat, GGAlong, GGAaltM, GGAhori, GGAgeoid;
 
FILE * logFile;
 
int open_serial_port(void)
{
 
        fd = open("/dev/ttyS2", O_RDWR | O_NOCTTY | O_NDELAY);
        if (fd == -1)
        {
            /*
            * Could not open the port.
            */
 
            perror("open_port: Unable to open /dev/ttyS2 - ");
        }
        else
            fcntl(fd, F_SETFL, 0);
 
        return (fd);
}
 
int close_serial_port(void)
{
    close (fd);
}
 
int read_serial_port(void)
{
	char *token, *str1;
//	while (1) {
    	res = read(fd,buf,255); 
    buf[res]=0;             /* set end of string, so we can printf */
   // printf(":%s:%d\n", buf, res);
   /* if (strncmp(buf, "$GPRMC", 6) == 0) {
//	    printf("GPRMC found\n");
	    str1 = strdup(buf);
	    token = strtok(str1, ",");
	    token = strtok(NULL, ",");
	    gpsTime = atoi(token);
	  //  printf("gpsTime = %d\n", gpsTime);
	    token = strtok(NULL, ",");
	    if (token == "A") {
	 //   printf("%s\n", token);
	    token = strtok(NULL, ",");
	    gpsLat = atof(token);
	    token = strtok(NULL, ",");
	    token = strtok(NULL, ",");
	    gpsLong = atof(token);
	    token = strtok(NULL, ",");
	    token = strtok(NULL, ",");
	    RMCgroundSpeed = atof(token);
	    token = strtok(NULL, ",");
	    gpsTangle = atof(token);
	    token = strtok(NULL, ",");
	    token = strtok(NULL, ",");
	    magVariation = atof(token);
	    printf("%d,%f,%f,%f,%f,%f\n", gpsTime, gpsLat, gpsLong, RMCgroundSpeed, gpsTangle, magVariation);
		}
	    else {
		   // printf("No GPS Lock\n");
	    }
    } */
    if (strncmp(buf, "$GPVTG", 6) ==0) {
	    printf("GPVTG found\n");
	    str1 = strdup(buf);
	    token = strtok(str1, ",");
	    token = strtok(NULL, ",");
	    trueTrack = atof(token);
	    token = strtok(NULL, ",");
	    token = strtok(NULL, ",");
	    magTrack = atof(token);
	    token = strtok(NULL, ",");
	    token = strtok(NULL, ",");
	    VTGgroundSpeedKn = atof(token);
	    token = strtok(NULL, ",");
	    token = strtok(NULL, ",");
	    VTGgroundSpeedKm = atof(token);
	    printf("%f,%f,%f,%f\n", trueTrack, magTrack, VTGgroundSpeedKn, VTGgroundSpeedKm);
    }
 
    else if (strncmp(buf, "$GPGGA", 6) ==0) {
//    	printf ("GPGGA found\n");
/* Splits the GPGGA sequence into tokens and then converts them into respective ints or floats */
  	str1 = strdup(buf);
	token = strtok(str1, ",");
	token = strtok(NULL, ",");
	GGAtime = atoi(token);
	token = strtok(NULL, ",");
	GGAlat = atof(token);
	/* Convert into decimal */
	GGAlat = GGAlat / 100.0;
	int deglat = (int)GGAlat;
	float minlat = GGAlat - (float)deglat;
	minlat = minlat / 0.6;
	GGAlat = (float)deglat + minlat;
	token = strtok(NULL, ",");
	token = strtok(NULL, ",");
	GGAlong = atof(token);
	/* Convert into decimal */
	GGAlong = GGAlong / 100.0;
	int deglong = (int)GGAlong;
	float minlong = GGAlong - (float)deglong;
	minlong = minlong / 0.6;
	GGAlong = (float)deglong + minlong;
	token = strtok(NULL, ",");
	token = strtok(NULL, ",");
	GGAfix = atoi(token);
	token = strtok(NULL, ",");
	GGAsats = atoi(token);
	token = strtok(NULL, ",");
	GGAhori = atof(token);
	token = strtok(NULL, ",");
	GGAaltM = atof(token);
	token = strtok(NULL, ",");
	GGAgeoid = atof(token);
	printf("%d,lat:%f,long:%f,fix:%d,sats:%d,hori:%f,alt:%f,geo:%f\n", GGAtime, GGAlat, GGAlong, GGAfix, GGAsats, GGAhori, GGAaltM, GGAgeoid);	
    }
    else {
//	    printf("No Lock\n");
    }
//    write_log();
/* code below breaks loop after every line read*/    
//    if (buf[-1] == '\n' || buf[-1] == '\r')
//	    printf ("break\n");
//	    break;
 
//}
}
 
int open_log_file()
{
	logFile = fopen("gps.log", "a+");
}
int write_log()
{
//	FILE *logFile;
//	logFile = fopen("gps.log", "a+");
	fprintf( logFile, "%d,%f,%f,%d,%d,%f,%f,%f\n", GGAtime, GGAlat, GGAlong, GGAfix, GGAsats, GGAhori, GGAaltM, GGAgeoid);
//	fclose(logFile);
 
}
 
int close_log_file()
{
	fclose(logFile);
}
int calculate_distance()
{
	/*Calculate the distance between present position and landing site (on the ground) using high altitude slug */
	/* code (on wiki) */
}
 
int calculate_bearing()
{
	/* Calculate bearing between the two distances that needs to be travellied again from slug code */
}
 
int calculate_differences()
{
	/*Compare required bearing to present bearing and calculate difference, also trigger alarms if getting near */
	/* to the landing site and finally look at altitude and how much height we have left before landing */
}
 
int adjust_servo_position()
{
	/* Take data from calculate_differences and adjust servo position accordingly */
}
 
int main () 
{
	printf("HARVe Lifting Body Navigation Software\n");
    open_serial_port();
    open_log_file();
    while (1) 
    {
    read_serial_port();
    write_log();
    calculate_distance();
    calculate_bearing();
    calculate_differences();
    adjust_servo_position();
    }
    close_log_file();
    close_serial_port();
}