gps.cpp

00001 /*
00002  * Author: Pedro M. Campos
00003  * Date: 21-05-2020
00004  * Notes: Rutinas para recepcion y proceso de tramas de GPS
00005 */
00006 
00007 #include "mbed.h"
00008 #include "declaraciones.h"
00009 #include "gps.h"
00010 
00011 
00012 
00013 
00014 // CANALES DE COMUNICACION
00015 extern Serial pc;
00016 extern Serial serGPS;
00017 extern st_datos_GPS datos_GPS;           // ESTRUCTURA DE DATOS DE ESTADO DEL GPS
00018 extern st_datos_servo datos_servo;       // ESTRUCTURA DE VARIABLES DE ESTADO DEL SERVO DE DIRECCION
00019 
00020 
00021 char buf[500];
00022 char bufGPS[500];
00023 int nc = 0;
00024 bool capturaGPS = false;    // Si true, trama capturada
00025 
00026 // SALIDAS PARA LEDS
00027 extern DigitalOut led1;               //LED1
00028 extern DigitalOut led2;               //LED2
00029 extern DigitalOut led3;               //LED3
00030 
00031 
00032 
00033 //******************************************************************************
00034 // Rutina de atencion de interrupciones de los datos recibidos por el canal serie del GPS
00035 void onSerialGPS(){
00036 char c;
00037 
00038     while(serGPS.readable()){
00039         c = serGPS.getc();
00040         if(c=='$' || nc>=sizeof(bufGPS)){
00041             nc=0;
00042             }
00043         bufGPS[nc] = c;                 // guarda en buffer
00044         nc++;
00045         if(c=='\n'){   // final de trama CR
00046             if(capturaGPS==false){
00047                 memcpy(buf, bufGPS, nc);        // copia trama recibida
00048                 buf[nc] = '\0';                 // marca final de string
00049                 capturaGPS = true;              // trama capturada
00050                 led1=1;
00051             }
00052             nc=0;
00053         }
00054     }
00055 }
00056 
00057 
00058 
00059 //*****************************************************************************
00060 // Combrueba si hay trama capturada y la procesa
00061 // En programa BASE
00062 //*****************************************************************************
00063 void procesaGPS() 
00064 {   
00065     if(capturaGPS == true){                 // trama capturada
00066         parserGPS(buf);                     // procesa la trama recibida
00067         capturaGPS=false;
00068         led1=0;                             // Una vez procesada apaga led1
00069     }    
00070 }
00071 
00072 
00073 
00074 float trunc(float v) {
00075     if(v < 0.0) {
00076         v*= -1.0;
00077         v = floor(v);
00078         v*=-1.0;
00079     } else {
00080         v = floor(v);
00081     }
00082     return v;
00083 }
00084 
00085 
00086 //*****************************************************************************
00087 // Decodifica trama capturada y actualiza valores en los datos del GPS
00088 // En programa BASE
00089 //*****************************************************************************
00090 void parserGPS(char *cmd)
00091 {    
00092     char *pt = cmd;
00093    
00094     if(tramaGPSOK( cmd )==false)
00095         return;     // error en inicio de trama
00096     pt+=3;
00097     
00098     
00099     // Global Positioning System Fix Data
00100     if(strncmp(pt,"GGA", 3) == 0) 
00101     {
00102         sscanf(pt, "GGA,%f,%f,%c,%f,%c,%d,%d,%*f,%f", &datos_GPS.timefix, &datos_GPS.latitude, &datos_GPS.ns, &datos_GPS.longitude,
00103             &datos_GPS.ew, &datos_GPS.fq, &datos_GPS.nst, &datos_GPS.altitude);
00104 //        pc.printf("GGA Fix taken at: %f, Latitude: %f %c, Longitude: %f %c, Fix quality: %d, Number of sat: %d, Altitude: %f M\r\n", timefix, latitude, ns, longitude, ew, fq, nst, altitude);
00105     }
00106     
00107     // Satellite status
00108     if(strncmp(pt,"GSA", 3) == 0) 
00109     {
00110         sscanf(pt, "GSA,%c,%d,%d", &datos_GPS.tf, &datos_GPS.fix, &datos_GPS.nst);
00111 //        pc.printf("GSA Type fix: %c, 3D fix: %d, number of sat: %d\r\n", tf, fix, nst);
00112     }
00113     
00114     // Geographic position, Latitude and Longitude
00115     if(strncmp(pt,"GLL", 3) == 0) 
00116     {
00117         sscanf(pt, "GLL,%f,%c,%f,%c,%f", &datos_GPS.latitude, &datos_GPS.ns, &datos_GPS.longitude, &datos_GPS.ew, &datos_GPS.timefix);
00118 //        pc.printf("GLL Latitude: %f %c, Longitude: %f %c, Fix taken at: %f\r\n", latitude, ns, longitude, ew, timefix);
00119     }
00120     
00121     // Geographic position, Latitude and Longitude
00122     if(strncmp(pt,"RMC", 3) == 0) 
00123     {
00124         sscanf(pt, "RMC,%f,%c,%f,%c,%f,%c,%f,%f,%d", &datos_GPS.timefix, &datos_GPS.status, &datos_GPS.latitude, &datos_GPS.ns, &datos_GPS.longitude, &datos_GPS.ew, &datos_GPS.speed, &datos_GPS.track, &datos_GPS.date);
00125 //        pc.printf("RMC Fix taken at: %f, Status: %c, Latitude: %f %c, Longitude: %f %c, Speed: %f, Track: %f, Date: %d\r\n", timefix, status, latitude, ns, longitude, ew, speed, track, date);
00126     }
00127 
00128     if(datos_GPS.fq >= 2){
00129         datos_GPS.latitud = datos_GPS.latitude;
00130         datos_GPS.longitud = datos_GPS.longitude;
00131         
00132         if(datos_GPS.ns == 'S') { datos_GPS.latitud  *= -1.0; }
00133         if(datos_GPS.ew == 'W') { datos_GPS.longitud *= -1.0; }
00134         float degrees = trunc(datos_GPS.latitud / 100.0f);
00135         float minutes = datos_GPS.latitud - (degrees * 100.0f);
00136         datos_GPS.latitud = degrees + minutes / 60.0f;    
00137     
00138         degrees = trunc(datos_GPS.longitud / 100.0f);
00139         minutes = datos_GPS.longitud - (degrees * 100.0f);
00140         datos_GPS.longitud = degrees + minutes / 60.0f;
00141     
00142         datos_servo.latitud = datos_GPS.latitud;
00143         datos_servo.longitud = datos_GPS.longitud;
00144 //        datos_servo.velocidad = datos_GPS.speed;  // Velocidad en nudos
00145         datos_servo.velocidad = 30;  // DEBUG *** 30 nudos
00146         
00147         actualizaFechaHoraGPS(datos_GPS.date, datos_GPS.timefix);
00148     
00149     //    pc.printf("Latitud: %f, Longitud: %f\r\n", datos_GPS.latitud, datos_GPS.longitud);
00150 //        pc.printf("fq: %d, nst: %d, fix: %d\r\n", datos_GPS.fq, datos_GPS.nst, datos_GPS.fix);
00151     }
00152 }
00153 
00154 
00155 
00156 //******************************************************************************
00157 // COMPRUEBA la trama NMEA y devuelve true si es correcta
00158 bool tramaGPSOK( char *nmeaStr ){
00159 
00160     char *p = nmeaStr;
00161     uint8_t checkNum = 0;
00162     char chkstr[4];
00163 
00164     // Check input parameters
00165     if( ( nmeaStr == NULL || nmeaStr[0] != '$' ) )
00166     {
00167         return false;
00168     }
00169 
00170     p++;
00171     
00172     // XOR hasta que encuentre '*' o final de la cadena
00173     while( *p != '*' && *p != '\r' && *p != 0)
00174     {
00175         checkNum ^= *p++;
00176     }
00177     if(*p != '*')
00178         return false;   // No ha encontrado '*'
00179 
00180     // Convert checksum value to 2 hexadecimal characters
00181     /*checksum[0] = upper nibble, checksum[1] = lower nibble
00182     */
00183     sprintf(chkstr, "%02X", checkNum);
00184 
00185 #ifdef GPS_OUT
00186     printf("%s", nmeaStr);
00187 #endif
00188 
00189     if(chkstr[0]==*(p+1) && chkstr[1]==*(p+2)){
00190 //        printf("\r\nOK checksum: %c%c == %c%c\r\n", chkstr[0], chkstr[1], *(p+1), *(p+2));
00191         return true;
00192         }
00193     else{
00194 #ifdef GPS_OUT
00195         printf("\r\nERROR checksum: %c%c != %c%c\r\n", chkstr[0], chkstr[1], *(p+1), *(p+2));
00196 #endif
00197         return false;
00198     }
00199 }
00200 
00201