Guillermo Stedile / NAVDATA

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependents:   SNOCC_V1 SNOCC_V2

NAVDATA.cpp@0:9370c06bf7f3, 2017-03-19 (annotated)

Committer:
gstedile
Date:
Sun Mar 19 20:59:13 2017 +0000
Revision:
0:9370c06bf7f3
Child:
1:48e2f1194609
Para Juanma

Who changed what in which revision?

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