Guillermo Stedile / GPS

Forked para SNOCC

Dependents:   RA8875

Fork of GPS by SNOCC

GPS.cpp@4:a0760d64f0f8, 2017-04-04 (annotated)

Committer:
gstedile
Date:
Tue Apr 04 06:16:56 2017 +0000
Revision:
4:a0760d64f0f8
Parent:
3:f9327dc1cc11 
Compilado y corrido sin errores. Hasta aqui carga los datos en un objeto NAVDATA y los rota. Falta cargar un objeto VCODATA con la matriz y calcular la VCO.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
simon 0:15611c7938a3 1 /* mbed EM-406 GPS Module Library
simon 0:15611c7938a3 2 * Copyright (c) 2008-2010, sford
simon 0:15611c7938a3 3 *
simon 0:15611c7938a3 4 * Permission is hereby granted, free of charge, to any person obtaining a copy
simon 0:15611c7938a3 5 * of this software and associated documentation files (the "Software"), to deal
simon 0:15611c7938a3 6 * in the Software without restriction, including without limitation the rights
simon 0:15611c7938a3 7 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
simon 0:15611c7938a3 8 * copies of the Software, and to permit persons to whom the Software is
simon 0:15611c7938a3 9 * furnished to do so, subject to the following conditions:
simon 0:15611c7938a3 10 *
simon 0:15611c7938a3 11 * The above copyright notice and this permission notice shall be included in
simon 0:15611c7938a3 12 * all copies or substantial portions of the Software.
simon 0:15611c7938a3 13 *
simon 0:15611c7938a3 14 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
simon 0:15611c7938a3 15 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
simon 0:15611c7938a3 16 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
simon 0:15611c7938a3 17 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
simon 0:15611c7938a3 18 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
simon 0:15611c7938a3 19 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
simon 0:15611c7938a3 20 * THE SOFTWARE.
simon 0:15611c7938a3 21 */
simon 0:15611c7938a3 22
simon 0:15611c7938a3 23 #include "GPS.h"
simon 0:15611c7938a3 24
gstedile 2:5b9cb7910a4b 25 GPS::GPS(PinName tx, PinName rx) : _gps(tx, rx) { // En PinNames.h (https://developer.mbed.org/users/screamer/code/mbed/file/667d61c9177b/PinNames.h) se define: typedef enum PinName PinName;
gstedile 1:1de2fc75bf38 26 _gps.baud(9600); // Cambio baudrate a 9600
simon 0:15611c7938a3 27 longitude = 0.0;
gstedile 1:1de2fc75bf38 28 latitude = 0.0;
gstedile 1:1de2fc75bf38 29
simon 0:15611c7938a3 30 }
simon 0:15611c7938a3 31
gstedile 2:5b9cb7910a4b 32
simon 0:15611c7938a3 33 int GPS::sample() {
gstedile 4:a0760d64f0f8 34 //float time;
gstedile 1:1de2fc75bf38 35 char ns, ew, signal; // Agrego signal para parsear el campo NMEA que indica es estado de señal.
simon 0:15611c7938a3 36 int lock;
gstedile 1:1de2fc75bf38 37
simon 0:15611c7938a3 38
simon 0:15611c7938a3 39 while(1) {
simon 0:15611c7938a3 40 getline();
gstedile 1:1de2fc75bf38 41
gstedile 1:1de2fc75bf38 42
simon 0:15611c7938a3 43
gstedile 1:1de2fc75bf38 44 /*
gstedile 1:1de2fc75bf38 45 GGA - essential fix data which provide 3D location and accuracy data.
gstedile 1:1de2fc75bf38 46
gstedile 1:1de2fc75bf38 47 $GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47
gstedile 1:1de2fc75bf38 48
gstedile 1:1de2fc75bf38 49 Where:
gstedile 1:1de2fc75bf38 50 GGA Global Positioning System Fix Data
gstedile 1:1de2fc75bf38 51 123519 Fix taken at 12:35:19 UTC
gstedile 1:1de2fc75bf38 52 4807.038,N Latitude 48 deg 07.038' N
gstedile 1:1de2fc75bf38 53 01131.000,E Longitude 11 deg 31.000' E
gstedile 1:1de2fc75bf38 54 1 Fix quality: 0 = invalid
gstedile 1:1de2fc75bf38 55 1 = GPS fix (SPS)
gstedile 1:1de2fc75bf38 56 2 = DGPS fix
gstedile 1:1de2fc75bf38 57 3 = PPS fix
gstedile 1:1de2fc75bf38 58 4 = Real Time Kinematic
gstedile 1:1de2fc75bf38 59 5 = Float RTK
gstedile 1:1de2fc75bf38 60 6 = estimated (dead reckoning) (2.3 feature)
gstedile 1:1de2fc75bf38 61 7 = Manual input mode
gstedile 1:1de2fc75bf38 62 8 = Simulation mode
gstedile 1:1de2fc75bf38 63 08 Number of satellites being tracked
gstedile 1:1de2fc75bf38 64 0.9 Horizontal dilution of position
gstedile 1:1de2fc75bf38 65 545.4,M Altitude, Meters, above mean sea level
gstedile 1:1de2fc75bf38 66 46.9,M Height of geoid (mean sea level) above WGS84
gstedile 1:1de2fc75bf38 67 ellipsoid
gstedile 1:1de2fc75bf38 68 (empty field) time in seconds since last DGPS update
gstedile 1:1de2fc75bf38 69 (empty field) DGPS station ID number
gstedile 1:1de2fc75bf38 70 *47 the checksum data, always begins with *
gstedile 1:1de2fc75bf38 71 */
gstedile 1:1de2fc75bf38 72
gstedile 1:1de2fc75bf38 73
gstedile 1:1de2fc75bf38 74 /*
gstedile 1:1de2fc75bf38 75 sscanf(msg, "GN %s",mensaje1);
gstedile 1:1de2fc75bf38 76 sscanf(msg, "GP,%s", mensaje2);
gstedile 1:1de2fc75bf38 77 sscanf(msg, "GSV,%s", mensaje3);
gstedile 1:1de2fc75bf38 78 sscanf(msg, "G,%s", mensaje4);
gstedile 1:1de2fc75bf38 79 */
gstedile 1:1de2fc75bf38 80 // $GPRMC,194530.000,A,3051.8007,N,10035.9989,W,1.49,111.67,310714,,,A*74 Esta es la trama GP. En nuestro caso es GN:
gstedile 1:1de2fc75bf38 81
gstedile 1:1de2fc75bf38 82 if(sscanf(msg, "GNRMC, %f,%c,%f,%c,%f,%c,%f", &time, &signal, &latitude, &ns, &longitude, &ew, &speed) >=1){
gstedile 3:f9327dc1cc11 83 // if(sscanf(msg, "GNRMC, %f,%c,%f,%c,%f,%c,%f,%f,%f", &time, &signal, &latitude, &ns, &longitude, &ew, &speed, &nosequees,&fecha) >=1){ // SE AGREGA CAMPO NOSEQUEES y FECHA; Declarar variable!!!
gstedile 1:1de2fc75bf38 84 // Agrego las siguientes lineas porque originalmente se analizaba el mensaje GGA y se evaluaba "lock" para saber si habia datos validos
gstedile 1:1de2fc75bf38 85 // y ahora evaluo si la señal (signal) está Available (A) o Void (V). Lo dejo asi por si se retorna a GGA.
gstedile 1:1de2fc75bf38 86
gstedile 1:1de2fc75bf38 87 if(signal == 'A'){
gstedile 1:1de2fc75bf38 88 lock = 1;
gstedile 1:1de2fc75bf38 89 }
gstedile 1:1de2fc75bf38 90 else {
gstedile 1:1de2fc75bf38 91 lock = 0;
gstedile 1:1de2fc75bf38 92 }
gstedile 1:1de2fc75bf38 93
gstedile 1:1de2fc75bf38 94 // if(sscanf(msg, "GNGGA,%f,%f,%c,%f,%c,%d", &time, &latitude, &ns, &longitude, &ew, &lock) >= 1) {
gstedile 1:1de2fc75bf38 95
gstedile 1:1de2fc75bf38 96
gstedile 1:1de2fc75bf38 97
gstedile 1:1de2fc75bf38 98 if(!lock) {
gstedile 1:1de2fc75bf38 99
simon 0:15611c7938a3 100 longitude = 0.0;
gstedile 1:1de2fc75bf38 101 latitude = 0.0;
gstedile 1:1de2fc75bf38 102 speed = 0.0; //
gstedile 4:a0760d64f0f8 103 time=0.0;
simon 0:15611c7938a3 104 return 0;
gstedile 1:1de2fc75bf38 105 }
gstedile 1:1de2fc75bf38 106
gstedile 1:1de2fc75bf38 107 else {
simon 0:15611c7938a3 108 if(ns == 'S') { latitude *= -1.0; }
simon 0:15611c7938a3 109 if(ew == 'W') { longitude *= -1.0; }
gstedile 1:1de2fc75bf38 110 float degrees = trunc(latitude / 100.0f); // El formato del campo es GGMM.MMMM: Divido por 100 y tomo parte entera para los grados.
gstedile 1:1de2fc75bf38 111 float minutes = latitude - (degrees * 100.0f); // Resto los grados y me quedo con los minutos
gstedile 1:1de2fc75bf38 112 latitude = degrees + minutes / 60.0f; // Convierto a decimal sumando grados y dividiendo entre 60 los minutos
gstedile 1:1de2fc75bf38 113 //degrees = trunc(longitude / 100.0f * 0.01f); // Corrijo esta linea porque es como dividir por 10000.
gstedile 1:1de2fc75bf38 114 degrees = trunc(longitude / 100.0f); // Repito para la latitud.
simon 0:15611c7938a3 115 minutes = longitude - (degrees * 100.0f);
simon 0:15611c7938a3 116 longitude = degrees + minutes / 60.0f;
gstedile 1:1de2fc75bf38 117 speed *= 1.852000f; // Convierto nudos a km/h
gstedile 1:1de2fc75bf38 118
simon 0:15611c7938a3 119 return 1;
gstedile 1:1de2fc75bf38 120
simon 0:15611c7938a3 121 }
simon 0:15611c7938a3 122 }
gstedile 1:1de2fc75bf38 123
simon 0:15611c7938a3 124 }
simon 0:15611c7938a3 125 }
simon 0:15611c7938a3 126
simon 0:15611c7938a3 127 float GPS::trunc(float v) {
simon 0:15611c7938a3 128 if(v < 0.0) {
simon 0:15611c7938a3 129 v*= -1.0;
simon 0:15611c7938a3 130 v = floor(v);
simon 0:15611c7938a3 131 v*=-1.0;
simon 0:15611c7938a3 132 } else {
simon 0:15611c7938a3 133 v = floor(v);
simon 0:15611c7938a3 134 }
simon 0:15611c7938a3 135 return v;
simon 0:15611c7938a3 136 }
simon 0:15611c7938a3 137
simon 0:15611c7938a3 138 void GPS::getline() {
simon 0:15611c7938a3 139 while(_gps.getc() != '$'); // wait for the start of a line
simon 0:15611c7938a3 140 for(int i=0; i<256; i++) {
simon 0:15611c7938a3 141 msg[i] = _gps.getc();
simon 0:15611c7938a3 142 if(msg[i] == '\r') {
simon 0:15611c7938a3 143 msg[i] = 0;
simon 0:15611c7938a3 144 return;
simon 0:15611c7938a3 145 }
simon 0:15611c7938a3 146 }
simon 0:15611c7938a3 147 error("Overflowed message limit");
gstedile 2:5b9cb7910a4b 148 }
gstedile 2:5b9cb7910a4b 149
gstedile 2:5b9cb7910a4b 150 /*void GPS::getline() {
gstedile 2:5b9cb7910a4b 151 if(_gps.getc() != '$') return; // exit if there isn't start of a line
gstedile 2:5b9cb7910a4b 152 for(int i=0; i<256; i++) {
gstedile 2:5b9cb7910a4b 153 msg[i] = _gps.getc();
gstedile 2:5b9cb7910a4b 154 if(msg[i] == '\r') {
gstedile 2:5b9cb7910a4b 155 msg[i] = 0;
gstedile 2:5b9cb7910a4b 156 return;
gstedile 2:5b9cb7910a4b 157 }
gstedile 2:5b9cb7910a4b 158 }
gstedile 2:5b9cb7910a4b 159 error("Overflowed message limit");
gstedile 2:5b9cb7910a4b 160 }*/