Pedro Campos / Mbed OS SERVOS_V0_3

test

gps.cpp@4:7564d0ffd595, 2020-06-18 (annotated)

Committer:
dapi08
Date:
Thu Jun 18 07:31:09 2020 +0000
Revision:
4:7564d0ffd595
Parent:
2:82fdfeec5799 
Comienzo pruebas con los dos mandos

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dapi08 2:82fdfeec5799 1 /*
dapi08 2:82fdfeec5799 2 * Author: Pedro M. Campos
dapi08 2:82fdfeec5799 3 * Date: 21-05-2020
dapi08 2:82fdfeec5799 4 * Notes: Rutinas para recepcion y proceso de tramas de GPS
dapi08 2:82fdfeec5799 5 */
dapi08 2:82fdfeec5799 6
dapi08 2:82fdfeec5799 7 #include "mbed.h"
dapi08 2:82fdfeec5799 8 #include "declaraciones.h"
dapi08 2:82fdfeec5799 9 #include "gps.h"
dapi08 2:82fdfeec5799 10
dapi08 2:82fdfeec5799 11
dapi08 2:82fdfeec5799 12
dapi08 2:82fdfeec5799 13
dapi08 2:82fdfeec5799 14 // CANALES DE COMUNICACION
dapi08 2:82fdfeec5799 15 extern Serial pc;
dapi08 2:82fdfeec5799 16 extern Serial serGPS;
dapi08 2:82fdfeec5799 17 extern st_datos_GPS datos_GPS; // ESTRUCTURA DE DATOS DE ESTADO DEL GPS
dapi08 2:82fdfeec5799 18 extern st_datos_servo datos_servo; // ESTRUCTURA DE VARIABLES DE ESTADO DEL SERVO DE DIRECCION
dapi08 2:82fdfeec5799 19
dapi08 2:82fdfeec5799 20
dapi08 2:82fdfeec5799 21 char buf[500];
dapi08 2:82fdfeec5799 22 char bufGPS[500];
dapi08 2:82fdfeec5799 23 int nc = 0;
dapi08 2:82fdfeec5799 24 bool capturaGPS = false; // Si true, trama capturada
dapi08 2:82fdfeec5799 25
dapi08 2:82fdfeec5799 26 // SALIDAS PARA LEDS
dapi08 2:82fdfeec5799 27 extern DigitalOut led1; //LED1
dapi08 2:82fdfeec5799 28 extern DigitalOut led2; //LED2
dapi08 2:82fdfeec5799 29 extern DigitalOut led3; //LED3
dapi08 2:82fdfeec5799 30
dapi08 2:82fdfeec5799 31
dapi08 2:82fdfeec5799 32
dapi08 2:82fdfeec5799 33 //******************************************************************************
dapi08 2:82fdfeec5799 34 // Rutina de atencion de interrupciones de los datos recibidos por el canal serie del GPS
dapi08 2:82fdfeec5799 35 void onSerialGPS(){
dapi08 2:82fdfeec5799 36 char c;
dapi08 2:82fdfeec5799 37
dapi08 2:82fdfeec5799 38 while(serGPS.readable()){
dapi08 2:82fdfeec5799 39 c = serGPS.getc();
dapi08 2:82fdfeec5799 40 if(c=='$' || nc>=sizeof(bufGPS)){
dapi08 2:82fdfeec5799 41 nc=0;
dapi08 2:82fdfeec5799 42 }
dapi08 2:82fdfeec5799 43 bufGPS[nc] = c; // guarda en buffer
dapi08 2:82fdfeec5799 44 nc++;
dapi08 2:82fdfeec5799 45 if(c=='\n'){ // final de trama CR
dapi08 2:82fdfeec5799 46 if(capturaGPS==false){
dapi08 2:82fdfeec5799 47 memcpy(buf, bufGPS, nc); // copia trama recibida
dapi08 2:82fdfeec5799 48 buf[nc] = '\0'; // marca final de string
dapi08 2:82fdfeec5799 49 capturaGPS = true; // trama capturada
dapi08 2:82fdfeec5799 50 led1=1;
dapi08 2:82fdfeec5799 51 }
dapi08 2:82fdfeec5799 52 nc=0;
dapi08 2:82fdfeec5799 53 }
dapi08 2:82fdfeec5799 54 }
dapi08 2:82fdfeec5799 55 }
dapi08 2:82fdfeec5799 56
dapi08 2:82fdfeec5799 57
dapi08 2:82fdfeec5799 58
dapi08 2:82fdfeec5799 59 //*****************************************************************************
dapi08 2:82fdfeec5799 60 // Combrueba si hay trama capturada y la procesa
dapi08 2:82fdfeec5799 61 // En programa BASE
dapi08 2:82fdfeec5799 62 //*****************************************************************************
dapi08 2:82fdfeec5799 63 void procesaGPS()
dapi08 2:82fdfeec5799 64 {
dapi08 2:82fdfeec5799 65 if(capturaGPS == true){ // trama capturada
dapi08 2:82fdfeec5799 66 parserGPS(buf); // procesa la trama recibida
dapi08 2:82fdfeec5799 67 capturaGPS=false;
dapi08 2:82fdfeec5799 68 led1=0; // Una vez procesada apaga led1
dapi08 2:82fdfeec5799 69 }
dapi08 2:82fdfeec5799 70 }
dapi08 2:82fdfeec5799 71
dapi08 2:82fdfeec5799 72
dapi08 2:82fdfeec5799 73
dapi08 2:82fdfeec5799 74 float trunc(float v) {
dapi08 2:82fdfeec5799 75 if(v < 0.0) {
dapi08 2:82fdfeec5799 76 v*= -1.0;
dapi08 2:82fdfeec5799 77 v = floor(v);
dapi08 2:82fdfeec5799 78 v*=-1.0;
dapi08 2:82fdfeec5799 79 } else {
dapi08 2:82fdfeec5799 80 v = floor(v);
dapi08 2:82fdfeec5799 81 }
dapi08 2:82fdfeec5799 82 return v;
dapi08 2:82fdfeec5799 83 }
dapi08 2:82fdfeec5799 84
dapi08 2:82fdfeec5799 85
dapi08 2:82fdfeec5799 86 //*****************************************************************************
dapi08 2:82fdfeec5799 87 // Decodifica trama capturada y actualiza valores en los datos del GPS
dapi08 2:82fdfeec5799 88 // En programa BASE
dapi08 2:82fdfeec5799 89 //*****************************************************************************
dapi08 2:82fdfeec5799 90 void parserGPS(char *cmd)
dapi08 2:82fdfeec5799 91 {
dapi08 2:82fdfeec5799 92 char *pt = cmd;
dapi08 2:82fdfeec5799 93
dapi08 2:82fdfeec5799 94 if(tramaGPSOK( cmd )==false)
dapi08 2:82fdfeec5799 95 return; // error en inicio de trama
dapi08 2:82fdfeec5799 96 pt+=3;
dapi08 2:82fdfeec5799 97
dapi08 2:82fdfeec5799 98
dapi08 2:82fdfeec5799 99 // Global Positioning System Fix Data
dapi08 2:82fdfeec5799 100 if(strncmp(pt,"GGA", 3) == 0)
dapi08 2:82fdfeec5799 101 {
dapi08 2:82fdfeec5799 102 sscanf(pt, "GGA,%f,%f,%c,%f,%c,%d,%d,%*f,%f", &datos_GPS.timefix, &datos_GPS.latitude, &datos_GPS.ns, &datos_GPS.longitude,
dapi08 2:82fdfeec5799 103 &datos_GPS.ew, &datos_GPS.fq, &datos_GPS.nst, &datos_GPS.altitude);
dapi08 2:82fdfeec5799 104 // 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);
dapi08 2:82fdfeec5799 105 }
dapi08 2:82fdfeec5799 106
dapi08 2:82fdfeec5799 107 // Satellite status
dapi08 2:82fdfeec5799 108 if(strncmp(pt,"GSA", 3) == 0)
dapi08 2:82fdfeec5799 109 {
dapi08 2:82fdfeec5799 110 sscanf(pt, "GSA,%c,%d,%d", &datos_GPS.tf, &datos_GPS.fix, &datos_GPS.nst);
dapi08 2:82fdfeec5799 111 // pc.printf("GSA Type fix: %c, 3D fix: %d, number of sat: %d\r\n", tf, fix, nst);
dapi08 2:82fdfeec5799 112 }
dapi08 2:82fdfeec5799 113
dapi08 2:82fdfeec5799 114 // Geographic position, Latitude and Longitude
dapi08 2:82fdfeec5799 115 if(strncmp(pt,"GLL", 3) == 0)
dapi08 2:82fdfeec5799 116 {
dapi08 2:82fdfeec5799 117 sscanf(pt, "GLL,%f,%c,%f,%c,%f", &datos_GPS.latitude, &datos_GPS.ns, &datos_GPS.longitude, &datos_GPS.ew, &datos_GPS.timefix);
dapi08 2:82fdfeec5799 118 // pc.printf("GLL Latitude: %f %c, Longitude: %f %c, Fix taken at: %f\r\n", latitude, ns, longitude, ew, timefix);
dapi08 2:82fdfeec5799 119 }
dapi08 2:82fdfeec5799 120
dapi08 2:82fdfeec5799 121 // Geographic position, Latitude and Longitude
dapi08 2:82fdfeec5799 122 if(strncmp(pt,"RMC", 3) == 0)
dapi08 2:82fdfeec5799 123 {
dapi08 2:82fdfeec5799 124 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);
dapi08 2:82fdfeec5799 125 // 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);
dapi08 2:82fdfeec5799 126 }
dapi08 2:82fdfeec5799 127
dapi08 2:82fdfeec5799 128 if(datos_GPS.fq >= 2){
dapi08 2:82fdfeec5799 129 datos_GPS.latitud = datos_GPS.latitude;
dapi08 2:82fdfeec5799 130 datos_GPS.longitud = datos_GPS.longitude;
dapi08 2:82fdfeec5799 131
dapi08 2:82fdfeec5799 132 if(datos_GPS.ns == 'S') { datos_GPS.latitud *= -1.0; }
dapi08 2:82fdfeec5799 133 if(datos_GPS.ew == 'W') { datos_GPS.longitud *= -1.0; }
dapi08 2:82fdfeec5799 134 float degrees = trunc(datos_GPS.latitud / 100.0f);
dapi08 2:82fdfeec5799 135 float minutes = datos_GPS.latitud - (degrees * 100.0f);
dapi08 2:82fdfeec5799 136 datos_GPS.latitud = degrees + minutes / 60.0f;
dapi08 2:82fdfeec5799 137
dapi08 2:82fdfeec5799 138 degrees = trunc(datos_GPS.longitud / 100.0f);
dapi08 2:82fdfeec5799 139 minutes = datos_GPS.longitud - (degrees * 100.0f);
dapi08 2:82fdfeec5799 140 datos_GPS.longitud = degrees + minutes / 60.0f;
dapi08 2:82fdfeec5799 141
dapi08 2:82fdfeec5799 142 datos_servo.latitud = datos_GPS.latitud;
dapi08 2:82fdfeec5799 143 datos_servo.longitud = datos_GPS.longitud;
dapi08 4:7564d0ffd595 144 // datos_servo.velocidad = datos_GPS.speed; // Velocidad en nudos
dapi08 4:7564d0ffd595 145 datos_servo.velocidad = 30; // DEBUG *** 30 nudos
dapi08 2:82fdfeec5799 146
dapi08 2:82fdfeec5799 147 actualizaFechaHoraGPS(datos_GPS.date, datos_GPS.timefix);
dapi08 2:82fdfeec5799 148
dapi08 2:82fdfeec5799 149 // pc.printf("Latitud: %f, Longitud: %f\r\n", datos_GPS.latitud, datos_GPS.longitud);
dapi08 2:82fdfeec5799 150 // pc.printf("fq: %d, nst: %d, fix: %d\r\n", datos_GPS.fq, datos_GPS.nst, datos_GPS.fix);
dapi08 2:82fdfeec5799 151 }
dapi08 2:82fdfeec5799 152 }
dapi08 2:82fdfeec5799 153
dapi08 2:82fdfeec5799 154
dapi08 2:82fdfeec5799 155
dapi08 2:82fdfeec5799 156 //******************************************************************************
dapi08 2:82fdfeec5799 157 // COMPRUEBA la trama NMEA y devuelve true si es correcta
dapi08 2:82fdfeec5799 158 bool tramaGPSOK( char *nmeaStr ){
dapi08 2:82fdfeec5799 159
dapi08 2:82fdfeec5799 160 char *p = nmeaStr;
dapi08 2:82fdfeec5799 161 uint8_t checkNum = 0;
dapi08 2:82fdfeec5799 162 char chkstr[4];
dapi08 2:82fdfeec5799 163
dapi08 2:82fdfeec5799 164 // Check input parameters
dapi08 2:82fdfeec5799 165 if( ( nmeaStr == NULL || nmeaStr[0] != '$' ) )
dapi08 2:82fdfeec5799 166 {
dapi08 2:82fdfeec5799 167 return false;
dapi08 2:82fdfeec5799 168 }
dapi08 2:82fdfeec5799 169
dapi08 2:82fdfeec5799 170 p++;
dapi08 2:82fdfeec5799 171
dapi08 2:82fdfeec5799 172 // XOR hasta que encuentre '*' o final de la cadena
dapi08 2:82fdfeec5799 173 while( *p != '*' && *p != '\r' && *p != 0)
dapi08 2:82fdfeec5799 174 {
dapi08 2:82fdfeec5799 175 checkNum ^= *p++;
dapi08 2:82fdfeec5799 176 }
dapi08 2:82fdfeec5799 177 if(*p != '*')
dapi08 2:82fdfeec5799 178 return false; // No ha encontrado '*'
dapi08 2:82fdfeec5799 179
dapi08 2:82fdfeec5799 180 // Convert checksum value to 2 hexadecimal characters
dapi08 2:82fdfeec5799 181 /*checksum[0] = upper nibble, checksum[1] = lower nibble
dapi08 2:82fdfeec5799 182 */
dapi08 2:82fdfeec5799 183 sprintf(chkstr, "%02X", checkNum);
dapi08 2:82fdfeec5799 184
dapi08 2:82fdfeec5799 185 #ifdef GPS_OUT
dapi08 2:82fdfeec5799 186 printf("%s", nmeaStr);
dapi08 2:82fdfeec5799 187 #endif
dapi08 2:82fdfeec5799 188
dapi08 2:82fdfeec5799 189 if(chkstr[0]==*(p+1) && chkstr[1]==*(p+2)){
dapi08 2:82fdfeec5799 190 // printf("\r\nOK checksum: %c%c == %c%c\r\n", chkstr[0], chkstr[1], *(p+1), *(p+2));
dapi08 2:82fdfeec5799 191 return true;
dapi08 2:82fdfeec5799 192 }
dapi08 2:82fdfeec5799 193 else{
dapi08 2:82fdfeec5799 194 #ifdef GPS_OUT
dapi08 2:82fdfeec5799 195 printf("\r\nERROR checksum: %c%c != %c%c\r\n", chkstr[0], chkstr[1], *(p+1), *(p+2));
dapi08 2:82fdfeec5799 196 #endif
dapi08 2:82fdfeec5799 197 return false;
dapi08 2:82fdfeec5799 198 }
dapi08 2:82fdfeec5799 199 }
dapi08 2:82fdfeec5799 200
dapi08 2:82fdfeec5799 201