GPS, trama NMEA y parseo

[elgarbe]

Bueno, siguiendo con los elementos que componen un sistema de control para UAV, no puede faltar el GPS para la navegación.
Es por eso que conseguí un módulo con el chip Ublox neo-6. Este módulo posee todo integrado, el ublox, la antena, la batería de respaldo para la configuracion, etc. Posee solo 4 terminales de conexion, +3.3V, GND, Tx y Rx. Posee dos LED's, uno verde que indica alimentacion y uno rojo que parpadea cuando tiene al menos 4 satélites linkeados y empieza a dar informacion útil.
Para ver la trama del GPS basta con conectar el módulo al puerto serie de la PC, teniendo presente que la parte de niveles TTL debe ser 3.3V y no 5V, configurar la velocidad e 9600 y listo!

Por ejemplo, mi GPS, de fábrica vino configurado así y al conectarlo me arroja cada 1 seg la siguiente trama:



Como pueden ver hay distintas tramas (renglones), cada una con informacion útil.
Pueden leer sobre las tramas en este enlace: http://www.gpsinformation.org/dale/nmea.htm

Yo me voy a centrar en la trama GGA, la cual posee la informacion organizada de la siguiente manera:

Código: [Seleccionar]

$GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47

Where:
     GGA          Global Positioning System Fix Data
     123519       Fix taken at 12:35:19 UTC
     4807.038,N   Latitude 48 deg 07.038' N
     01131.000,E  Longitude 11 deg 31.000' E
     1            Fix quality: 0 = invalid
                               1 = GPS fix (SPS)
                               2 = DGPS fix
                               3 = PPS fix
       4 = Real Time Kinematic
       5 = Float RTK
                               6 = estimated (dead reckoning) (2.3 feature)
       7 = Manual input mode
       8 = Simulation mode
     08           Number of satellites being tracked
     0.9          Horizontal dilution of position
     545.4,M      Altitude, Meters, above mean sea level
     46.9,M       Height of geoid (mean sea level) above WGS84
                      ellipsoid
     (empty field) time in seconds since last DGPS update
     (empty field) DGPS station ID number
     *47          the checksum data, always begins with *

De todos los campos (separados por comas) me interesan el de la hora, la latitud, la longitud, Calidad Fix, número de satélites linkeados y la altura en metros.

A la técnica utilizada para decodificar la trama en sus campos se denomina "parsear".
Si buscamos en google "nmea gps parser" van a aparecer cientos de librerías. A mi me gustó este: http://code.google.com/p/shake-drivers/source/browse/trunk/%20shake-drivers%20--username%20stephenahughes/NMEA%20GPS%20parser%20-%20isr_uart.c?r=21

no es el código más óptimo, pero me gusta el hecho de que es, para mi, intuitivo y va parseando caracter por caracter, por lo que la interrupcion de recepcion de caracter en el UART está activa muy poco tiempo.

La técnica se basa en ir detectando caracteres conocidos o esperados e ir avanzando en el estado del parseo incrementando una variable. Yo modifiqué un poco el código para mi uso y en definitiva el código de mi uart3.c quedó así:

Código: [Seleccionar]

#include "LPC17xx.h"
#include "uart3.h"

// PCUART3
#define PCUART3_POWERON (1 << 25)

//Peripheral Clock Selection. Para el uart3 usar PCLKSEL1
#define PCLK_UART3 18
#define PCLK_UART3_MASK (3 << 18) //Esta mascara se usara par aponer 0b00 -> PCLK = CCLK / 4 ->

#define IER_RBR 0x01 //
#define IER_THRE 0x02
#define IER_RLS 0x04

#define IIR_PEND 0x01
#define IIR_RLS 0x03
#define IIR_RDA 0x02
#define IIR_CTI 0x06
#define IIR_THRE 0x01

#define LSR_RDR 0x01
#define LSR_OE 0x02
#define LSR_PE 0x04
#define LSR_FE 0x08
#define LSR_BI 0x10
#define LSR_THRE 0x20
#define LSR_TEMT 0x40
#define LSR_RXFE 0x80

volatile uint32_t UART3Status;
volatile uint8_t UART3TxEmpty=1;
volatile uint8_t UART3Buffer[BUFSIZE];
volatile uint32_t UART3Count = 0;
volatile uint8_t nmea_state = 0;
volatile char char_rcv = 0;

extern char gps_height[5];
extern char gps_height_decimeters;

extern char calculateoutputgps;

extern char gps_latitud[10];
extern char gps_longitud[10];
extern char gps_UTC[9];
extern char gps_Sats[2];

void uart3_interruptsOn(void) {
    LPC_UART3->IER = 0x07; // RBR, THRE, RLS
}

void uart3_interruptsOff(void) {
    LPC_UART3->IER = 0x00; // !RBR, !THRE, !RLS
}

// ***********************
// Funcion para Inicializar el UART 2
void UART3_Init(int baudrate)
{
uint32_t fdiv, pclk;

// Encendemos el UART3
LPC_SC->PCONP |=  PCUART3_POWERON;

// Encendemos el reloj del UART3
LPC_SC->PCLKSEL1 &= ~(PCLK_UART3_MASK); // Borro los bits corrspondientes al PCLK
LPC_SC->PCLKSEL1 |=  (0 << PCLK_UART3); // Aplico el PCLK_periph = CCLK/4

// Configuro el IO P4.28 = RX, P4.29 = TX
// LPC_PINCON->PINSEL9 &= ~0xF000000; //Pongo en 0 los bits 24..27
LPC_PINCON->PINSEL9 |= (0xF << 24); //Pongo en 1 los bits 24..27

LPC_UART3->LCR = 0x83; // 0b10000011 -> 8 bits, no Parity, 1 Stop bit, Divisor Latch Access Bit=1

// PCLK_UART3 se va a setear a SystemCoreClock dividido 4 -> 25MHz
// pclk = SystemCoreClock / 4;
pclk = SystemCoreClock/4;
// do baudrate calculation
    fdiv = (pclk / (16 * baudrate));
    LPC_UART3->DLM = (fdiv >> 8) & 0xFF;
    LPC_UART3->DLL = (fdiv) & 0xFF;
    if(baudrate==115200){
        LPC_UART3->DLM = 0;
        LPC_UART3->DLL = 9;
        LPC_UART3->FDR = 1 | 2<<4;
    }

    LPC_UART3->LCR = 0x03; // 8 bits, no Parity, 1 Stop bit DLAB = 0
LPC_UART3->FCR = 0x07; // Enable and reset TX and RX FIFO. 1 carcter en FIFO

NVIC_EnableIRQ(UART3_IRQn);

LPC_UART3->IER = IER_RBR | IER_THRE | IER_RLS; /* Enable UART3 interrupt */
}

// ***********************
// Funcion para enviar un caracter por el UART 2
void UART3_Sendchar(char c)
{
while( (LPC_UART3->LSR & LSR_THRE) == 0 ); // Esperamos hasta que Transmit Holding Register Empty esté en 1

LPC_UART3->THR = c; //Escrivo en el Transmit Holding Register para que se envíe el caracter
}

// ***********************
// Funcion para leer un caracter desde el UART 2
char UART3_Getchar()
{
char c;
while( (LPC_UART3->LSR & LSR_RDR) == 0 );  // Espero hasta que haya datos mirando el Receive Data Register
c = LPC_UART3->RBR; // Leo el Receive Buffer Register
return c;
}

// ***********************
// Funcion para enviar una cadena de caracteres por el UART 2
void UART3_PrintString(char *pcString)
{
int i = 0;
// loop through until reach string's zero terminator
while (pcString[i] != 0) {
UART3_Sendchar(pcString[i]); // print each character
i++;
}
}


/*****************************************************************************
** Function name: UART3_IRQHandler
**
** Descriptions: UART3 interrupt handler
**
Aquí intentamos parsear la trama $GPGGA del GPS para leer la informacion que provee.
La trama es así:
* $GPGGA
* $GPGGA,hhmmss.ss,llll.ll,a,yyyyy.yy,a,x,xx,x.x,x.x,M,x.x,M,x.x,xxxx*hh
* ex: $GPGGA,230600.501,4543.8895,N,02112.7238,E,1,03,3.3,96.7,M,39.0,M,,0000*6A,
*
* WORDS:
*  1    = UTC of Position
*  2    = Latitude
*  3    = N or S
*  4    = Longitude
*  5    = E or W
*  6    = GPS quality indicator (0=invalid; 1=GPS fix; 2=Diff. GPS fix)
*  7    = Number of satellites in use [not those in view]
*  8    = Horizontal dilution of position
*  9    = Antenna altitude above/below mean sea level (geoid)
*  10   = Meters  (Antenna height unit)
*  11   = Geoidal separation (Diff. between WGS-84 earth ellipsoid and mean sea level.
*      -geoid is below WGS-84 ellipsoid)
*  12   = Meters  (Units of geoidal separation)
*  13   = Age in seconds since last update from diff. reference station
*  14   = Diff. reference station ID#
*  15   = Checksum

El tema con las tramas es que no los campos no tienen longitud fija. Por ejemplo el dato de la
Latitud puede variar de la forma xxx.y a xxx.yyyy ó xxxxx.yyyy dependiendo de la lectura, entonces
debemos ir tomando caracter por caracter y ver que formato tiene

La salida de datos esta de la siguiente forma:
gps_latitude_degrees[3],gps_longitude_degrees[3], where [0]*100 + [1]*10 + [2] = decimal degrees
gps_latitude_minutes[2],gps_longitude_minutes[2], where [0]*10 + [1] = decimal minutes
gps_latitude_decimal_seconds[4],gps_longitude_decimal_seconds[4], where ([0]*1000 + [1]*100 + [2]*10 + [3]) / 10000 * 60 = decimal seconds
gps_height[5], gps_height_decimeters, where ([0]*10000 + [1]*1000 + [2]*100 + [3]*10 + [4]) + gps_height_decimeters/1° = decimal height

tengo que agregar la lectura de los satélites linkeados...
**
*****************************************************************************/
void UART3_IRQHandler (void){
uint8_t IIRValue, LSRValue;
uint8_t Dummy = Dummy;
static uint8_t idx=0;

IIRValue = LPC_UART3->IIR;

IIRValue >>= 1; /* skip pending bit in IIR */
IIRValue &= 0x07; /* check bit 1~3, interrupt identification */
if ( IIRValue == IIR_RLS ){ /* Receive Line Status */
LSRValue = LPC_UART3->LSR;
/* Receive Line Status */
if ( LSRValue & (LSR_OE|LSR_PE|LSR_FE|LSR_RXFE|LSR_BI) ){
/* There are errors or break interrupt */
/* Read LSR will clear the interrupt */
UART3Status = LSRValue;
Dummy = LPC_UART3->RBR; /* Dummy read on RX to clear interrupt, then bail out */
return;
}
if ( LSRValue & LSR_RDR ){ /* Receive Data Ready */
/* If no error on RLS, normal ready, save into the data buffer. */
/* Note: read RBR will clear the interrupt */
UART3Buffer[UART3Count] = LPC_UART3->RBR;
UART3Count++;
if ( UART3Count == BUFSIZE ){
UART3Count = 0; /* buffer overflow */
}
}
}
else if ( IIRValue == IIR_RDA ){ /* Receive Data Available */
char_rcv = LPC_UART3->RBR;
UART3Buffer[UART3Count] = char_rcv;
UART3Count++;
if ( UART3Count == BUFSIZE ){
UART3Count = 0; /* buffer overflow */
}
// now try to see where this character fits in the NMEA GGA message we're trying to decipher
switch ( nmea_state )               // evaluate expression
{
case ( 1 ):                    // wait for 'G'
if ( char_rcv == 'G')
nmea_state++;
break;
case ( 2 ):         // wait for 'G'
if ( char_rcv == 'G')
nmea_state++;
else
nmea_state=1; //Si no es una G volvemos al principio
break;
case ( 3 ):          // wait for 'A'
if ( char_rcv == 'A')
nmea_state++;
else
nmea_state=1; //Si no es una G volvemos al principio
break;
case ( 4 ):          // wait for ','
if ( char_rcv == ',')
nmea_state++;
break;
case ( 5 ):          // Estoy en el Campo UTS
if ( char_rcv == ','){ //Si ya finalizó el campo
nmea_state++; //Paso al siguiente estado
idx=0;
}else{
gps_UTC[idx] = char_rcv; // Voy almacenando los caracteres recividos
idx++; // Incremento el índice dentro del campo actual
}
calculateoutputgps=0;
break;
case ( 6 ): // Estamos en el Campo LATITUD
if ( char_rcv == ','){ //Si ya finalizó el campo
nmea_state++; //Paso al siguiente estado
idx=0;
}else{
idx++; // Incremento el índice dentro del campo actual
gps_latitud[idx] = char_rcv; // Voy almacenando los caracteres recividos
}
break;
case ( 7 ):
if ( char_rcv == 'N') // Si es Norte
gps_latitud[0] = '+'; // Latitud positiva
else // Si es Sur
gps_latitud[0] = '-'; // Latitud negativa
nmea_state++;
break;
case ( 8 ): // this is a comma
nmea_state++;
break;
case ( 9 ): // Estamos en el Campo LONGITUD
if ( char_rcv == ','){ //Si ya finalizó el campo
nmea_state++; //Paso al siguiente estado
idx=0;
}else{
idx++; // Incremento el índice dentro del campo actual
gps_longitud[idx] = char_rcv; // Voy almacenando los caracteres recividos
}
break;
case ( 10 ):
if ( char_rcv == 'E') // Si es Este
gps_longitud[0] = '+'; // Latitud positiva
else // Si es Sur
gps_longitud[0] = '-'; // Latitud negativa
nmea_state++;
break;
case ( 11 ): // this is a comma
nmea_state++;
break;
case ( 12 ): // Este campo es Fix quality
nmea_state++;
break;
case (13): // Este es una ','
nmea_state++;
break;
case (14): // Este es el campo de número de satelites linkeados.
if ( char_rcv == ','){ //Si ya finalizó el campo
nmea_state++; //Paso al siguiente estado
idx=0;
}else{
gps_Sats[idx] = char_rcv; // Voy almacenando los caracteres recividos
idx++; // Incremento el índice dentro del campo actual
}
break;
case (15): // this is the 4th comma
if ( char_rcv == 0x2C) {
// move to the next state
nmea_state++;
// but first prepare the meters input variable
gps_height[0]= 0x30;
gps_height[1]= 0x30;
gps_height[2]= 0x30;
gps_height[3]= 0x30;
gps_height[4]= 0x30;
}
break;
case (16): // this is the heigth in meters
if ( char_rcv == 0x2e) {          // waiting for a decimal point
// no more numbers? OK, done with the shifting, move to the next step
nmea_state++;
}
else {
// oh no, 1 more number! Just shift the digits around and we'll be fine
gps_height[0]=gps_height[1];
gps_height[1]=gps_height[2];
gps_height[2]=gps_height[3];
gps_height[3]=gps_height[4];
gps_height[4]=char_rcv;
}
break;
case (17): // this is the decimal part of the heigth
gps_height_decimeters = char_rcv;
// notify the main loop we have gps data available
calculateoutputgps = 1;
// and wait for the next NMEA message
nmea_state = 1;
break;
default:
nmea_state = 1;     // we'll never and up here, but in case of a brownout make sure we start at the beginning
break;
}
}
else if ( IIRValue == IIR_CTI ){ /* Character timeout indicator */
/* Character Time-out indicator */
UART3Status |= 0x100; /* Bit 9 as the CTI error */
}
else if ( IIRValue == IIR_THRE ){ /* THRE, transmit holding register empty */
/* THRE interrupt */
LSRValue = LPC_UART3->LSR; /* Check status in the LSR to see if valid data in U0THR or not */
if ( LSRValue & LSR_THRE ){
UART3TxEmpty = 1;
}
else{
UART3TxEmpty = 0;
}
}
}

Bueno, el parseo sucede en la interrupcion del UART3. Despues de verificar ausencia de errores y ver que la interrupcion es por Receiv Data Available empiezo con el parseo.
Primero me aseguro de guardar la trama, caracter por caracter en un baffer (UART3Buffer) para hacer debug. Ese buffer lo voy a eliminar cuando tenga todo funcionando bien.
Luego comienza un SWITCH contra una variable, nmea_state, la cual iremos incrementando a medida que vallamos procesando los caracteres.
Las primeras opciones son para asegurarce de recivir la secuencia GGA. Luego biene el reconocimiento de la ',' y llegamos a la opcion 5. Aquí empiezan mis modificaciones. Lo que yo hago es mantenerme en el estado 5 y almacenando caracter por caracter en un solo array de char's, hasta que llegue la ','. Ese array de chars más tarde lo pasaré por una funcion que me la transformará en un flotante. Con ese método voy verificando caracteres, almacenando campos y avanzando en el estado, hasta llegar al final de la trama. Este código no esta 100% completo, pero es funcional y con el siguiente main:

Código: [Seleccionar]

/*
===============================================================================
 Author      : $(author)
 Version     :
 Copyright   : $(copyright)
 Description : main definition
===============================================================================
*/

char gps_longitud[10];
char gps_latitud[10];
char gps_UTC[9];
char gps_Sats[2];

char gps_height[5];
char gps_height_decimeters;

char calculateoutputgps=0;

#ifdef __USE_CMSIS
#include "LPC17xx.h"
#endif

#include <cr_section_macros.h>
#include <NXP/crp.h>
#include "uart3.h"
#include "uart2.h"

volatile uint32_t msTicks;

__INLINE static void delay_ms (uint32_t delayTicks) {
  uint32_t currentTicks;
  currentTicks = msTicks; // read current tick counter
  while ((msTicks - currentTicks) < delayTicks);
}

float string2float(char* s);

void SysTick_Handler(void) {
msTicks++; /* increment counter necessary in Delay() */
}

/*******************************************************************************
**   Main Function  main()
*******************************************************************************/
int main (void){
//Configuro el SysTick para que interrumpa cada 1 mseg
if (SysTick_Config(SystemCoreClock / 1000)) {
while (1);
}

UART2_Init(115200); // Inicializo el UART a 115200
UART3_Init(9600); // Inicializo el UART a 9600

UART2_PrintString ("\rGPS\r\n");
UART2_PrintString ("======== elgarbe ==========\r\n");
float flongitud=0.0;
float flatitud=0.0;
float fUTC=0.0;
float fSats=0.0;

while ( 1 ){
if(calculateoutputgps){
fUTC=string2float(gps_UTC);
flatitud=string2float(gps_latitud);
flongitud=string2float(gps_longitud);
fSats=string2float(gps_Sats);

UART2_PrintString("\n\rHora actual: ");
uart2_printUint32(((short)(fUTC/10000)+24-3), 10);
UART2_PrintString(":");
uart2_printUint32(((short)(fUTC/100) - (short)(fUTC/10000)*100), 10);
UART2_PrintString(":");
uart2_printUint32(( (short)fUTC - (short)(fUTC/100)*100 ), 10);

UART2_PrintString("\n\rLatitud: ");
uart2_printInt32((short)flatitud/100, 10);
UART2_PrintString("°");
uart2_printInt32( ((short)flatitud - ((short)flatitud/100)*100), 10);
UART2_PrintString("'");

UART2_PrintString("\n\rLongitud : ");
uart2_printInt32((short)flongitud/100, 10);
UART2_PrintString("°");
uart2_printInt32( ((short)flongitud - ((short)flongitud/100)*100), 10);
UART2_PrintString("'");

UART2_PrintString("\n\rSatélites linkeados: ");
uart2_printDouble(fSats, 0);
}
delay_ms(1000);
}
}

/* returns base-10 value of zero-terminated string
 * that contains only chars '+','-','0'-'9','.';
 * does not trap invalid strings!
 */
float string2float(char* s) {
long  integer_part = 0;
float decimal_part = 0.0;
float decimal_pivot = 0.1;
char isdecimal = 0, isnegative = 0;

char c;
while ( ( c = *s++) )  {
// skip special/sign chars
if (c == '-') { isnegative = 1; continue; }
if (c == '+') continue;
if (c == '.') { isdecimal = 1; continue; }

if (!isdecimal) {
integer_part = (10 * integer_part) + (c - 48);
}
else {
decimal_part += decimal_pivot * (float)(c - 48);
decimal_pivot /= 10.0;
}
}
// add integer part
decimal_part += (float)integer_part;

// check negative
if (isnegative)  decimal_part = - decimal_part;

return decimal_part;
}

Consigo la impresion de los datos reconocidos en el hyperterminal:



Lamentablemente no se como configurar el hyperterminal para que muestre acentos y simbolos especiales...

Aparte hay algunos errores en la conversion ya que tengo minutos de grados negativos....

Cuando corrija todo eso pondré el código definitivo.

Saludos!

[PCCM]

No sabia de el termino "parsear", nuevo vocabulario   

Para el GPS hice mi propio algoritmo: primero lleno los datos de GGA y después en el tiempo muerto lo decodifico toda esa trama y lo convierto a grados sexagesimales:

Para la interrupción serial  uso el sgte código:

Código: [Seleccionar]

if(comenzar_llenar<=2){
dato_gps[0] = U1RXREG;
if(dato_gps[0]=='A' && comenzar_llenar==2)
comenzar_llenar=3;
else if(dato_gps[0]=='G' && comenzar_llenar==1)
comenzar_llenar=2;
else if(dato_gps[0]=='G' && comenzar_llenar==0 )
comenzar_llenar=1;
  else
  comenzar_llenar=0;
}
//Llenando los datos de GGA hasta que llegue el OD OA
else{
dato_gps[up] = U1RXREG;
up++;
if(up>=2)
if(dato_gps[up-1]==0x0A && dato_gps[up-2]==0x0D){
up=0;
strncpy(string,dato_gps,sizeof(string));
comenzar_llenar=0;
trama_recibida_gps=1;

}
}


Para decodificar toda la trama GGA recibida por serial uso mi funcion "obtener_datos_gps":

Código: [Seleccionar]

char obtener_datos_gps(void){

///############################################   GPS
//En el caso que termine de recibir la trama GPGGS, lo decodifico para adquirir la latitud, longitud, altura.
if(trama_recibida_gps && strlen(string)>50){
trama_recibida_gps=0;
strncpy(string_decodificar,string,sizeof(string_decodificar));
//En el caso que el dato recibido sea valido
contador_gps=1;
llenador_gps=0;
//Almaceno la hora recibida del GPS
while(string_decodificar[contador_gps]!=','){
almacen_hora[llenador_gps]=string_decodificar[contador_gps];
llenador_gps++;
contador_gps++;
}
llenador_gps=0;
contador_gps++;
//Almaceno la latitud recibida por el gps
while(string_decodificar[contador_gps]!=','){
almacen_latitud[llenador_gps]=string_decodificar[contador_gps];
llenador_gps++;
contador_gps++;
}
//Almaceno si la latitud esta en el norte o sur
llenador_gps=0;
contador_gps++;
almacen_latitud_direccion=string_decodificar[contador_gps];
contador_gps++;
contador_gps++;
//Almaceno la longitud recibida por el gps
while(string_decodificar[contador_gps]!=','){
almacen_longitud[llenador_gps]=string_decodificar[contador_gps];
llenador_gps++;
contador_gps++;
}
//Almaceno si la longitud esta en el este u oeste
llenador_gps=0;
contador_gps++;
almacen_longitud_direccion=string_decodificar[contador_gps];
contador_gps++;
contador_gps++;
//Almaceno si la valides del dato
almacen_valides_mensaje=string_decodificar[contador_gps];
contador_gps++;
contador_gps++;
//Almaceno la cantidad de satelites recibidos
while(string_decodificar[contador_gps]!=','){
almacen_satelites[llenador_gps]=string_decodificar[contador_gps];
llenador_gps++;
contador_gps++;
}
llenador_gps=0;
contador_gps++;
//Almaceno HDOP,
while(string_decodificar[contador_gps]!=','){
almacen_HDOP[llenador_gps]=string_decodificar[contador_gps];
llenador_gps++;
contador_gps++;
}
llenador_gps=0;
contador_gps++;
//Almaceno altura sobre el nivel del mar(ojo segun la ecuacion elipsoidal)
while(string_decodificar[contador_gps]!=','){
almacen_altura[llenador_gps]=string_decodificar[contador_gps];
llenador_gps++;
contador_gps++;
}
//-------------------------------------------------
//Hay mas datos para decodificar en esta trama pero ya no es necesario
//-------------------------------------------
//sprintf(data_enviar,"I%s,%c,%s,%c,%s,F\n",almacen_latitud,almacen_latitud_direccion,almacen_longitud,almacen_longitud_direccion,almacen_altura);

return 0x01;
}

return 0x00;
}


Y para convertirlo a grados sexagesimales y poder manejar con ellos uso la siguiente funcion:

Código: [Seleccionar]


if(flag_gps==0x01){

almacen_latitud_numero=atof(almacen_latitud);
almacen_longitud_numero=atof(almacen_longitud);

almacen_latitud_numero_int=(int)(almacen_latitud_numero/100)/1;
almacen_longitud_numero_int=(int)(almacen_longitud_numero/100)/1;

minutos_latitud=almacen_latitud_numero-(double)(almacen_latitud_numero_int*100);
minutos_longitud=almacen_longitud_numero-(double)(almacen_longitud_numero_int*100);

minutos_latitud=minutos_latitud/60;
minutos_longitud=minutos_longitud/60;

almacen_latitud_numero=(double)almacen_latitud_numero_int+minutos_latitud;
almacen_longitud_numero=(double)almacen_longitud_numero_int+minutos_longitud;

sprintf(data_enviar,"I-%.5f,-%.5fF\n",almacen_latitud_numero,almacen_longitud_numero);
}



Espero que te pueda ayudar en algo mi código.

Además el GPS que uso es el D2523T, que utiliza el protocolo u-blox 6, nose muy bien pero creo que el tuyo también es parecido.
Lo que hice también es configurarlo para que el GPS me enviara a una frecuencia de 5Hz y unicamente la trama GGA.

Pero para que la configuración se quede guardada bien, puedes usar una bateria para no se borre la información o cargarla cada vez que enciendes el sistema.
El software que use para configurarlo fue el siguiente(linkeo otro software porque revisando el link de sparkfun donde lo descargue aparece que ya no existe):

https://www.u-blox.com/en/evaluation-tools-a-software/u-center/u-center.html
(Lo acabo de instalar y es igualito al que usaba)

Saludos.

[elgarbe]

me gusta la conversion a sexagesimal... pero la decodificacion caracter x caracter que tengo yo me gusta más.
la funcion atof en que librería se encuentra? en stdlib?

el tema de los grados y minutos negativos ya lo solucione, los grados esta bien que sea negativo por que mi latitud y longitud son sur y oeste, pero los minutos deben ser positivos.

tenes algo para calcular la distancia entre 2 puntos con las coordenadas del gps, teniendo en cuenta la diferencia de altura de los 2 puntos también?

Saludos y gracias!

[PCCM]

Claro esta en stdlib.

La distancia entre 2 puntos con latitud y longitud lo puedes hacer directamente con esos datos o sino transformándolos a coordenadas UTM, de las dos formas tienen una buena precisión pero no exacta, por diversos factores, precisión del gps, aproximaciones de los datos, etc(lo probé con google maps y en un simulador llamado flightgear).

Considerando la altura puedes obtener la distancia entre los dos puntos(sin considerar la altura y luego por pitagoras con la diferencia de altura hayas la hipotenusa, que seria la distancia entre esos dos puntos.)

Para la distancia entre dos puntos con latitud y longitud(lo tengo como codificación en lenguaje nasal, pero como se ve es fácilmente convertido a C):

Código: [Seleccionar]

dlon = (NumlonFinal-Longitud_Inicial);
dvalor = (math.sin(NumlatFinal*GradToRad)*math.sin(Latitud_Inicial*GradToRad))+(math.cos(NumlatFinal*GradToRad)*math.cos(Latitud_Inicial*GradToRad)*math.cos(dlon*GradToRad));
dd = math.acos(dvalor)*RadToGrad;
distanciaKM = (dd*111.302);
Dist_To_Target=distanciaKM*1000;


La distancia te dará en metros, donde los datos de gps de la posición uno sera: (Longitud_Inicial, Latitud_Inicial) y de la posición dos sera: (NumlonFinal, NumlatFinal).

Pero como para aplicar pitagoras con esa distancia lo cual será curva es menos imprecisa que si fuera recta.
Por ello es mas recomendable primero convertirlas a coordenadas UTM.

La conversion de coordenadas geograficas(latitud, longitud a coordenadas UTM lo tengo en matlab), ya que la  conversión a lenguaje nasal que es parecido en C, lo tengo en la PC de mi trabajo.

Conversion Geografica a UTM

Código: [Seleccionar]

clear
clc

ddLat=29.52483223;
ddLon=7.31407171;
toRAD = pi/180;

%set ellipsoid parameters to GRS80
   
elpsd_a = 6378137;
elpsd_e2 = 0.0066943800229;
   
%set parameters to UTM zone 2 north
   
phi = 0  ;       %origin latitude: equator
lambda = 9;   %origin longitude: 111º W (zone 12)
kappa = 0.9996 ; %scale factor of UTM system
   
%set remaining parameters


ePrime2 = elpsd_e2 / (1 - elpsd_e2);
N = elpsd_a / sqrt((1 - elpsd_e2*(sin(toRAD*ddLat))^2));
T = tan(toRAD*ddLat)^2;
C = ePrime2*((cos(toRAD*ddLat))^2);
A = (toRAD*ddLon - toRAD*lambda) * cos(toRAD*ddLat);
M = 6367449.1459*toRAD*ddLat - 16038.508*sin(2*toRAD*ddLat) + 16.83*sin(4*toRAD*ddLat) - 0.02*sin(6*toRAD*ddLat);
Mo = 0;       
%calculate easting xxxxxxx
 
easting = kappa*N*(A + (1 - T + C)*(A^3) / 6 + (5 - 18*T + (T^2) + 72*C - 58*ePrime2)*(A^5) / 120) + 500000
 
%calculate northing   yyyyyyy
 
nA = A^2/2;
nB = A^4/24*(5 - T + 9*C + 4*C^2);
nC = A^6/720*(61 - 58*T + T^2 + 600 * C - 330*ePrime2);
   
northing = kappa * (M - Mo + N*tan(toRAD*ddLat)*(nA + nB + nC))
 

Saludos.

[elgarbe]

Fantástico!!!!

En mi caso, por ahora la distancia sería para tener una idea de cuán lejos estoy con el avión. En principio no puedo superar los 1000 - 2000mts por el sistema de RC que estoy usando. En esas distancias la altura creo que tiene importancia, ya que puedo estar a 200mts de altura y 1000mts de distancia en el plano de la tierra. Pero creo que la curvatura de la tierra no sería importante, verdad?

o sea, puedo usar tu fórma de calcular la distancia (la primera, con latitud y longitud directo) y luego como me dijiste usar pitágoras... bueno, lo veo simple entonces

La idea es cuando prendo el equipo que el uC toma la coordenada actual como punto de partida y luego me calcule a que distancia estoy cuando me aleje del punto de origen. Para medir la altura será mejor usar un altimetro barometrico como el bmp0805 o el dato del gps?

Otra más, vieron que en los sistemas OSD cuando el avion esta en vuelo, hay una flecha que marca hacia donde está el punto de partida: http://aeroquad.com/showwiki.php?title=OSD+On-Screen-Display

como podría calcular el ángulo de la flecha partiendo de la latitud y la longitud?

Saludos y gracias!

Por cierto, ya acomodé la conversion de lat long en grados sexagesimales...

[elgarbe]

..., donde los datos de gps de la posición uno sera: (Longitud_Inicial, Latitud_Inicial) y de la posición dos sera: (NumlonFinal, NumlatFinal)....

Los datos del gps en que formato son necesarios? GGMM.fraccionDeMinuto como lo da el GPS? o sea, en mi caso me da -3329.74686 que son 33 grados 29 minutos y 0.74686 minutos... hay que expresar así el dato?

saludos!

[PCCM]

Los datos de latitud o longitud lo ingresas así como lo recibes del GPS, osea para tu ejemplo si te da -3329.74686 la conversión separará los grados que son "-33" y los minutos que son "29.74686", los minutos lo convertirá a grados --> 29.74686 minutos = 0.495781 grados

Y sumará esos datos obteniendo el datos solo en grados = -33.495781.

En cuanto a precisión de si el BMP085 o el GPS es mejor sería bueno que hicieras pruebas, la ves que hice una pequeña prueba comparando con referencia de Internet sobre el punto geográfico donde estaba. vi que el GPS tenía un error de +-2.5m y el BMP085 de +-1m o creo que hasta menos, no recuerdo bien(Hice esa en un lugar fijo)

Pero si quieres aún más precisión he visto que algunos lo combinan con el acelerómetro en el ejeZ, y el BMP085 tiene varios puntos a favor, ya que es de rápido muestreo y no requiere de satélites externos para su funcionamiento.

Nunca he usado el OSD, pero si quieres calcular el ángulo actual respecto al inicio, lo puedes hacer con la siguiente fórmula:

Código: [Seleccionar]


var DiferenciaLongitudes=-122.45888550 - NumlonObjetivo;
var DiferenciaLatitudes=37.67592587 - NumlatObjetivo;

var b = math.acos(math.cos((90-NumlatObjetivo)*GradToRad)*math.cos((90-37.67592587)*GradToRad) + math.sin((90-NumlatObjetivo)*GradToRad)*math.sin((90-37.67592587)*GradToRad)*math.cos((DiferenciaLongitudes)*GradToRad));

var azimut = math.asin(math.sin((90-37.67592587)*GradToRad)*math.sin((DiferenciaLongitudes)*GradToRad)/math.sin(b));
azimut=azimut*RadToGrad;

if(DiferenciaLatitudes>=0)
azimut=azimut+180;
else if(DiferenciaLongitudes>=0)
azimut=360-azimut;
else
azimut=-1*azimut;



Donde -122.45888550 es mi longitud inicial
37.67592587 es mi latitud inicial

NumlonObjetivo seria en tu caso tu longitud actual del UAV y
NumlatObjetivo tu latitud actual del uav.

El resultado estará en "azimut".

Saludos.

[elgarbe]

Muy bueno!

voy a ir probando y publicando resultados.

Saludos y gracias!

[elgarbe]

Para la distancia entre dos puntos con latitud y longitud(lo tengo como codificación en lenguaje nasal, pero como se ve es fácilmente convertido a C):

Código: [Seleccionar]

dlon = (NumlonFinal-Longitud_Inicial);
dvalor = (math.sin(NumlatFinal*GradToRad)*math.sin(Latitud_Inicial*GradToRad))+(math.cos(NumlatFinal*GradToRad)*math.cos(Latitud_Inicial*GradToRad)*math.cos(dlon*GradToRad));
dd = math.acos(dvalor)*RadToGrad;
distanciaKM = (dd*111.302);
Dist_To_Target=distanciaKM*1000;


La distancia te dará en metros, donde los datos de gps de la posición uno sera: (Longitud_Inicial, Latitud_Inicial) y de la posición dos sera: (NumlonFinal, NumlatFinal).

Una consulta, de donde salen esos 111.302? he visto formulas iguales, pero en vez de ese valor ponen 6378.137 que es el díametro de la tierra...

Saludos!

[elgarbe]

Como es el tema de la posicion que da el GPS? en cuanto a la presicion... sin mover el GPS de lugar y al calcular la distancia recorrida con dos informes consecutivos del GPS tengo distancias de 100mts!!!! lo verifique con un calculador online, ingreso las dos coordenadas consecutivas que me da el gps y el calculador me da la misma distancia que yo calculo, tengo 10, 30 y hasta 100mts de distancia calculada, siempre sin mover el GPS...

Saludos!

[PCCM]

El 111.302 es el tamaño que tiene 1 grado, de los 360 grados de la tierra.

Derepente la cantidad de satélites que estas captando no es suficiente, cuanto más satélites será mas preciso.
Podrías hacer una comparación de precisión de los datos obtenidos  vs satélites captados.

Con los datos obtenidos ves en google maps(o google earth, no recuerdo donde esta esta opción de ver con latitud y longitud) cuanto coincide con tu posición.
Por acá capto entre 12 y 16 satélites.

[elgarbe]

uuu, yo con suerte a campo descubierto agarro 9... en mi casa, sacando el gps por la ventana solo 4 o 5 con suerte...
Bueno, entonces no es por problemas de configuracion, sino por cantidad de satélites...

voy a seguir probando!

Saludos y gracias!