SNOCC / GPS

Forked para SNOCC

Fork of GPS by Simon Ford

GPS.cpp@1:1de2fc75bf38, 2016-12-04 (annotated)

Committer:
gstedile
Date:
Sun Dec 04 20:14:00 2016 +0000
Revision:
1:1de2fc75bf38
Parent:
0:15611c7938a3 
Prueba

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
simon 0:15611c7938a3 25 GPS::GPS(PinName tx, PinName rx) : _gps(tx, rx) {
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
simon 0:15611c7938a3 32 int GPS::sample() {
simon 0:15611c7938a3 33 float time;
gstedile 1:1de2fc75bf38 34 char ns, ew, signal; // Agrego signal para parsear el campo NMEA que indica es estado de señal.
simon 0:15611c7938a3 35 int lock;
gstedile 1:1de2fc75bf38 36
simon 0:15611c7938a3 37
simon 0:15611c7938a3 38 while(1) {
simon 0:15611c7938a3 39 getline();
gstedile 1:1de2fc75bf38 40
gstedile 1:1de2fc75bf38 41
simon 0:15611c7938a3 42
gstedile 1:1de2fc75bf38 43 /*
gstedile 1:1de2fc75bf38 44 GGA - essential fix data which provide 3D location and accuracy data.
gstedile 1:1de2fc75bf38 45
gstedile 1:1de2fc75bf38 46 $GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47
gstedile 1:1de2fc75bf38 47
gstedile 1:1de2fc75bf38 48 Where:
gstedile 1:1de2fc75bf38 49 GGA Global Positioning System Fix Data
gstedile 1:1de2fc75bf38 50 123519 Fix taken at 12:35:19 UTC
gstedile 1:1de2fc75bf38 51 4807.038,N Latitude 48 deg 07.038' N
gstedile 1:1de2fc75bf38 52 01131.000,E Longitude 11 deg 31.000' E
gstedile 1:1de2fc75bf38 53 1 Fix quality: 0 = invalid
gstedile 1:1de2fc75bf38 54 1 = GPS fix (SPS)
gstedile 1:1de2fc75bf38 55 2 = DGPS fix
gstedile 1:1de2fc75bf38 56 3 = PPS fix
gstedile 1:1de2fc75bf38 57 4 = Real Time Kinematic
gstedile 1:1de2fc75bf38 58 5 = Float RTK
gstedile 1:1de2fc75bf38 59 6 = estimated (dead reckoning) (2.3 feature)
gstedile 1:1de2fc75bf38 60 7 = Manual input mode
gstedile 1:1de2fc75bf38 61 8 = Simulation mode
gstedile 1:1de2fc75bf38 62 08 Number of satellites being tracked
gstedile 1:1de2fc75bf38 63 0.9 Horizontal dilution of position
gstedile 1:1de2fc75bf38 64 545.4,M Altitude, Meters, above mean sea level
gstedile 1:1de2fc75bf38 65 46.9,M Height of geoid (mean sea level) above WGS84
gstedile 1:1de2fc75bf38 66 ellipsoid
gstedile 1:1de2fc75bf38 67 (empty field) time in seconds since last DGPS update
gstedile 1:1de2fc75bf38 68 (empty field) DGPS station ID number
gstedile 1:1de2fc75bf38 69 *47 the checksum data, always begins with *
gstedile 1:1de2fc75bf38 70 */
gstedile 1:1de2fc75bf38 71
gstedile 1:1de2fc75bf38 72
gstedile 1:1de2fc75bf38 73 /*
gstedile 1:1de2fc75bf38 74 sscanf(msg, "GN %s",mensaje1);
gstedile 1:1de2fc75bf38 75 sscanf(msg, "GP,%s", mensaje2);
gstedile 1:1de2fc75bf38 76 sscanf(msg, "GSV,%s", mensaje3);
gstedile 1:1de2fc75bf38 77 sscanf(msg, "G,%s", mensaje4);
gstedile 1:1de2fc75bf38 78 */
gstedile 1:1de2fc75bf38 79 // $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 80
gstedile 1:1de2fc75bf38 81 if(sscanf(msg, "GNRMC, %f,%c,%f,%c,%f,%c,%f", &time, &signal, &latitude, &ns, &longitude, &ew, &speed) >=1){
gstedile 1:1de2fc75bf38 82 // Agrego las siguientes lineas porque originalmente se analizaba el mensaje GGA y se evaluaba "lock" para saber si habia datos validos
gstedile 1:1de2fc75bf38 83 // 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 84
gstedile 1:1de2fc75bf38 85 if(signal == 'A'){
gstedile 1:1de2fc75bf38 86 lock = 1;
gstedile 1:1de2fc75bf38 87 }
gstedile 1:1de2fc75bf38 88 else {
gstedile 1:1de2fc75bf38 89 lock = 0;
gstedile 1:1de2fc75bf38 90 }
gstedile 1:1de2fc75bf38 91
gstedile 1:1de2fc75bf38 92 // if(sscanf(msg, "GNGGA,%f,%f,%c,%f,%c,%d", &time, &latitude, &ns, &longitude, &ew, &lock) >= 1) {
gstedile 1:1de2fc75bf38 93
gstedile 1:1de2fc75bf38 94
gstedile 1:1de2fc75bf38 95
gstedile 1:1de2fc75bf38 96 if(!lock) {
gstedile 1:1de2fc75bf38 97
simon 0:15611c7938a3 98 longitude = 0.0;
gstedile 1:1de2fc75bf38 99 latitude = 0.0;
gstedile 1:1de2fc75bf38 100 speed = 0.0; //
simon 0:15611c7938a3 101 return 0;
gstedile 1:1de2fc75bf38 102 }
gstedile 1:1de2fc75bf38 103
gstedile 1:1de2fc75bf38 104 else {
simon 0:15611c7938a3 105 if(ns == 'S') { latitude *= -1.0; }
simon 0:15611c7938a3 106 if(ew == 'W') { longitude *= -1.0; }
gstedile 1:1de2fc75bf38 107 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 108 float minutes = latitude - (degrees * 100.0f); // Resto los grados y me quedo con los minutos
gstedile 1:1de2fc75bf38 109 latitude = degrees + minutes / 60.0f; // Convierto a decimal sumando grados y dividiendo entre 60 los minutos
gstedile 1:1de2fc75bf38 110 //degrees = trunc(longitude / 100.0f * 0.01f); // Corrijo esta linea porque es como dividir por 10000.
gstedile 1:1de2fc75bf38 111 degrees = trunc(longitude / 100.0f); // Repito para la latitud.
simon 0:15611c7938a3 112 minutes = longitude - (degrees * 100.0f);
simon 0:15611c7938a3 113 longitude = degrees + minutes / 60.0f;
gstedile 1:1de2fc75bf38 114 speed *= 1.852000f; // Convierto nudos a km/h
gstedile 1:1de2fc75bf38 115
simon 0:15611c7938a3 116 return 1;
gstedile 1:1de2fc75bf38 117
simon 0:15611c7938a3 118 }
simon 0:15611c7938a3 119 }
gstedile 1:1de2fc75bf38 120
simon 0:15611c7938a3 121 }
simon 0:15611c7938a3 122 }
simon 0:15611c7938a3 123
simon 0:15611c7938a3 124 float GPS::trunc(float v) {
simon 0:15611c7938a3 125 if(v < 0.0) {
simon 0:15611c7938a3 126 v*= -1.0;
simon 0:15611c7938a3 127 v = floor(v);
simon 0:15611c7938a3 128 v*=-1.0;
simon 0:15611c7938a3 129 } else {
simon 0:15611c7938a3 130 v = floor(v);
simon 0:15611c7938a3 131 }
simon 0:15611c7938a3 132 return v;
simon 0:15611c7938a3 133 }
simon 0:15611c7938a3 134
simon 0:15611c7938a3 135 void GPS::getline() {
simon 0:15611c7938a3 136 while(_gps.getc() != '$'); // wait for the start of a line
simon 0:15611c7938a3 137 for(int i=0; i<256; i++) {
simon 0:15611c7938a3 138 msg[i] = _gps.getc();
simon 0:15611c7938a3 139 if(msg[i] == '\r') {
simon 0:15611c7938a3 140 msg[i] = 0;
simon 0:15611c7938a3 141 return;
simon 0:15611c7938a3 142 }
simon 0:15611c7938a3 143 }
simon 0:15611c7938a3 144 error("Overflowed message limit");
simon 0:15611c7938a3 145 }