147 lines
3.7 KiB
C
147 lines
3.7 KiB
C
#include "gps.h"
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
#include <fcntl.h>
|
|
#include <termios.h>
|
|
#include <unistd.h>
|
|
|
|
#define GPS_DEV_FILE "/dev/ttyS0" // Define GPS serial port file
|
|
#define BUFFER_SIZE 100 // Define receive buffer size
|
|
|
|
/**
|
|
* Configure UART channel
|
|
* TODO : Set baud rate to ???? see NMEA datasheet
|
|
*/
|
|
void configSerialPort(int fd){
|
|
struct termios Termios;
|
|
tcgetattr(fd,&Termios);
|
|
Termios.c_lflag |= ICANON;
|
|
tcsetattr(fd,TCSANOW,&Termios);
|
|
tcflush(fd, TCIFLUSH); // Clear already received frames
|
|
}
|
|
|
|
/**
|
|
* Fetch NMEA Frame type
|
|
* TODO : Other frame types
|
|
*/
|
|
NMEA_TYPE getNMEA_TYPE(unsigned char *data){
|
|
unsigned char* typeChar=malloc(sizeof(char)*4);
|
|
typeChar[3]='\0';
|
|
memcpy(typeChar, data+3,3);
|
|
if(strcmp(typeChar, "GGA")==0){
|
|
return(GGA);
|
|
}
|
|
free(typeChar);
|
|
return(NONE);
|
|
}
|
|
|
|
/**
|
|
* Build NMEA frame from array of char
|
|
* TODO : Other GGA parameters
|
|
*/
|
|
struct NMEA_GGA buildNMEA_GGA(unsigned char *data){
|
|
// data="$GPGGA,095003.000,2055.9571,S,05517.4159,E,1,6,1.59,138.8,M,-9.9,M,,*5D"; // To test !!!
|
|
struct NMEA_GGA frame;
|
|
unsigned char *buffer=malloc(sizeof(char)*strlen(data));
|
|
memcpy(buffer,data,strlen(data));
|
|
|
|
short i=1;
|
|
char *saveP;
|
|
char *token=strtok_r(buffer,",",&saveP);
|
|
while(token!=NULL){
|
|
if(strlen(token)>0){
|
|
char tmp[6];
|
|
switch(i){
|
|
case 2: // Fetch hour, min, sec and ms
|
|
memcpy(tmp,token,2);tmp[2]='\0';
|
|
frame.hour=atoi(tmp);
|
|
memcpy(tmp,token+2,2);
|
|
frame.min=atoi(tmp);
|
|
memcpy(tmp,token+4,2);
|
|
frame.sec=atoi(tmp);
|
|
memcpy(tmp,token+7,3);tmp[3]='\0';
|
|
frame.ms=atoi(tmp);
|
|
break;
|
|
case 3: // Fetch Latitude
|
|
memcpy(tmp,token,2);tmp[2]='\0';
|
|
frame.latDeg=atoi(tmp);
|
|
memcpy(tmp,token+2,2);
|
|
frame.latMin=atoi(tmp);
|
|
memcpy(tmp,token+5,4);tmp[4]='\0';
|
|
frame.latSec=atoi(tmp);
|
|
frame.latSec=frame.latSec/10000;
|
|
frame.latSec=(3600*frame.latSec)/60; // Convertion degrès min en degrès sec
|
|
break;
|
|
case 4: // Fetch latitude direction
|
|
frame.latDir=token[0];
|
|
break;
|
|
case 5: // Fetch longitude
|
|
memcpy(tmp,token,3);tmp[3]='\0';
|
|
frame.lonDeg=atoi(tmp);
|
|
tmp[2]='\0';
|
|
memcpy(tmp,token+3,2);
|
|
frame.lonMin=atoi(tmp);
|
|
memcpy(tmp,token+6,4);tmp[4]='\0';
|
|
frame.lonSec=atoi(tmp);
|
|
frame.lonSec=frame.lonSec/10000;
|
|
frame.lonSec=(3600*frame.lonSec)/60; // Convertion degrès min en degrès sec
|
|
break;
|
|
case 6: // Fetch longitude direction
|
|
frame.lonDir=token[0];
|
|
break;
|
|
case 7: // Fetch GPS state
|
|
frame.state=atoi(token);
|
|
break;
|
|
case 8: // Fetch number of statellites
|
|
frame.sats=atoi(token);
|
|
break;
|
|
}
|
|
}
|
|
token=strtok_r(NULL,",",&saveP);
|
|
i++;
|
|
}
|
|
|
|
free(buffer);
|
|
return(frame);
|
|
|
|
}
|
|
|
|
/**
|
|
* Fetch NMEA FRAME
|
|
*/
|
|
struct NMEA_GGA getNMEA_GGAFrame(){
|
|
|
|
int fd; // File descriptor
|
|
const char *device = "/dev/ttyS0";
|
|
fd = open(device, O_RDWR);
|
|
if(fd == -1) {
|
|
printf( "Failed to open gps device.\n" );
|
|
}
|
|
|
|
// Configure the serial port
|
|
configSerialPort(fd);
|
|
|
|
struct NMEA_GGA frame; // Def empty frame
|
|
short i;
|
|
// Try to feth the GGA frame (100 times max)
|
|
for(i=0;i<100;i++){
|
|
char out[BUFFER_SIZE];
|
|
memset(out,0,BUFFER_SIZE);
|
|
read(fd,out,BUFFER_SIZE);
|
|
if(strlen(out)>1){
|
|
if(getNMEA_TYPE(out)==GGA){
|
|
frame=buildNMEA_GGA(out);
|
|
//if(frame.state!=UNFIXED){
|
|
// printf("%d:%d:%d:%d\nLat : %d deg %d min %f sec %d Dir\n",frame.hour,frame.min,frame.sec,frame.ms,frame.latDeg,frame.latMin,frame.latSec, frame.latDir);
|
|
// printf("Lon : %d deg %d min %f sec %d Dir\n",frame.lonDeg,frame.lonMin,frame.lonSec, frame.lonDir);
|
|
// printf("Sats : %d",frame.sats);
|
|
// printf("%s",out);
|
|
//}
|
|
break; // If the frame is found, exit
|
|
}
|
|
}
|
|
}
|
|
close(fd);
|
|
return(frame);
|
|
}
|