File content as of revision 4:7564d0ffd595:

/*
 * Author: Pedro M. Campos
 * Date: 21-05-2020
 * Notes: Rutinas para recepcion y proceso de tramas de GPS
*/

#include "mbed.h"
#include "declaraciones.h"
#include "gps.h"




// CANALES DE COMUNICACION
extern Serial pc;
extern Serial serGPS;
extern st_datos_GPS datos_GPS;           // ESTRUCTURA DE DATOS DE ESTADO DEL GPS
extern st_datos_servo datos_servo;       // ESTRUCTURA DE VARIABLES DE ESTADO DEL SERVO DE DIRECCION


char buf[500];
char bufGPS[500];
int nc = 0;
bool capturaGPS = false;    // Si true, trama capturada

// SALIDAS PARA LEDS
extern DigitalOut led1;               //LED1
extern DigitalOut led2;               //LED2
extern DigitalOut led3;               //LED3



//******************************************************************************
// Rutina de atencion de interrupciones de los datos recibidos por el canal serie del GPS
void onSerialGPS(){
char c;

    while(serGPS.readable()){
        c = serGPS.getc();
        if(c=='$' || nc>=sizeof(bufGPS)){
            nc=0;
            }
        bufGPS[nc] = c;                 // guarda en buffer
        nc++;
        if(c=='\n'){   // final de trama CR
            if(capturaGPS==false){
                memcpy(buf, bufGPS, nc);        // copia trama recibida
                buf[nc] = '\0';                 // marca final de string
                capturaGPS = true;              // trama capturada
                led1=1;
            }
            nc=0;
        }
    }
}



//*****************************************************************************
// Combrueba si hay trama capturada y la procesa
// En programa BASE
//*****************************************************************************
void procesaGPS() 
{   
    if(capturaGPS == true){                 // trama capturada
        parserGPS(buf);                     // procesa la trama recibida
        capturaGPS=false;
        led1=0;                             // Una vez procesada apaga led1
    }    
}



float trunc(float v) {
    if(v < 0.0) {
        v*= -1.0;
        v = floor(v);
        v*=-1.0;
    } else {
        v = floor(v);
    }
    return v;
}


//*****************************************************************************
// Decodifica trama capturada y actualiza valores en los datos del GPS
// En programa BASE
//*****************************************************************************
void parserGPS(char *cmd)
{    
    char *pt = cmd;
   
    if(tramaGPSOK( cmd )==false)
        return;     // error en inicio de trama
    pt+=3;
    
    
    // Global Positioning System Fix Data
    if(strncmp(pt,"GGA", 3) == 0) 
    {
        sscanf(pt, "GGA,%f,%f,%c,%f,%c,%d,%d,%*f,%f", &datos_GPS.timefix, &datos_GPS.latitude, &datos_GPS.ns, &datos_GPS.longitude,
            &datos_GPS.ew, &datos_GPS.fq, &datos_GPS.nst, &datos_GPS.altitude);
//        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);
    }
    
    // Satellite status
    if(strncmp(pt,"GSA", 3) == 0) 
    {
        sscanf(pt, "GSA,%c,%d,%d", &datos_GPS.tf, &datos_GPS.fix, &datos_GPS.nst);
//        pc.printf("GSA Type fix: %c, 3D fix: %d, number of sat: %d\r\n", tf, fix, nst);
    }
    
    // Geographic position, Latitude and Longitude
    if(strncmp(pt,"GLL", 3) == 0) 
    {
        sscanf(pt, "GLL,%f,%c,%f,%c,%f", &datos_GPS.latitude, &datos_GPS.ns, &datos_GPS.longitude, &datos_GPS.ew, &datos_GPS.timefix);
//        pc.printf("GLL Latitude: %f %c, Longitude: %f %c, Fix taken at: %f\r\n", latitude, ns, longitude, ew, timefix);
    }
    
    // Geographic position, Latitude and Longitude
    if(strncmp(pt,"RMC", 3) == 0) 
    {
        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);
//        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);
    }

    if(datos_GPS.fq >= 2){
        datos_GPS.latitud = datos_GPS.latitude;
        datos_GPS.longitud = datos_GPS.longitude;
        
        if(datos_GPS.ns == 'S') { datos_GPS.latitud  *= -1.0; }
        if(datos_GPS.ew == 'W') { datos_GPS.longitud *= -1.0; }
        float degrees = trunc(datos_GPS.latitud / 100.0f);
        float minutes = datos_GPS.latitud - (degrees * 100.0f);
        datos_GPS.latitud = degrees + minutes / 60.0f;    
    
        degrees = trunc(datos_GPS.longitud / 100.0f);
        minutes = datos_GPS.longitud - (degrees * 100.0f);
        datos_GPS.longitud = degrees + minutes / 60.0f;
    
        datos_servo.latitud = datos_GPS.latitud;
        datos_servo.longitud = datos_GPS.longitud;
//        datos_servo.velocidad = datos_GPS.speed;  // Velocidad en nudos
        datos_servo.velocidad = 30;  // DEBUG *** 30 nudos
        
        actualizaFechaHoraGPS(datos_GPS.date, datos_GPS.timefix);
    
    //    pc.printf("Latitud: %f, Longitud: %f\r\n", datos_GPS.latitud, datos_GPS.longitud);
//        pc.printf("fq: %d, nst: %d, fix: %d\r\n", datos_GPS.fq, datos_GPS.nst, datos_GPS.fix);
    }
}



//******************************************************************************
// COMPRUEBA la trama NMEA y devuelve true si es correcta
bool tramaGPSOK( char *nmeaStr ){

    char *p = nmeaStr;
    uint8_t checkNum = 0;
    char chkstr[4];

    // Check input parameters
    if( ( nmeaStr == NULL || nmeaStr[0] != '$' ) )
    {
        return false;
    }

    p++;
    
    // XOR hasta que encuentre '*' o final de la cadena
    while( *p != '*' && *p != '\r' && *p != 0)
    {
        checkNum ^= *p++;
    }
    if(*p != '*')
        return false;   // No ha encontrado '*'

    // Convert checksum value to 2 hexadecimal characters
    /*checksum[0] = upper nibble, checksum[1] = lower nibble
    */
    sprintf(chkstr, "%02X", checkNum);

#ifdef GPS_OUT
    printf("%s", nmeaStr);
#endif

    if(chkstr[0]==*(p+1) && chkstr[1]==*(p+2)){
//        printf("\r\nOK checksum: %c%c == %c%c\r\n", chkstr[0], chkstr[1], *(p+1), *(p+2));
        return true;
        }
    else{
#ifdef GPS_OUT
        printf("\r\nERROR checksum: %c%c != %c%c\r\n", chkstr[0], chkstr[1], *(p+1), *(p+2));
#endif
        return false;
    }
}