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@3:de86e5eb7a8a, 2017-04-02 (annotated)

Committer:
gstedile
Date:
Sun Apr 02 19:26:20 2017 +0000
Revision:
3:de86e5eb7a8a
Parent:
2:86e9ac5dcacc
Child:
4:400b92fb528c
VCODATA.h sin errrores

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 2:86e9ac5dcacc 24 #include <math.h>
gstedile 0:9370c06bf7f3 25
gstedile 0:9370c06bf7f3 26 /*############################################################################
gstedile 1:48e2f1194609 27 ##############################################################################*/
gstedile 0:9370c06bf7f3 28
gstedile 2:86e9ac5dcacc 29 /* CONSTRUCTOR: Si no se pasan argumentos se toman los seteados en la declaracion; Latitud/Longitud expresadas en Millas Náuticas y Cte. del sensor en pulsos por Litro*/
gstedile 0:9370c06bf7f3 30
gstedile 3:de86e5eb7a8a 31 NAVDATA::NAVDATA(float Klongitud=49.38797982997941, float Klatitud=59.99999981628485, float Ksensor=2500, int ZoneHour=-3) : Klong(Klongitud), Klat(Klatitud), Ksen(Ksensor), ZH(ZoneHour) {
gstedile 3:de86e5eb7a8a 32 this->LAST_NAV_DATA[longitude_f]=-58,5797; // Longitud y Latitud iniciales y finales correspondiente a Tigre.
gstedile 3:de86e5eb7a8a 33 this->LAST_NAV_DATA[longitude_i]=-58,5797;
gstedile 3:de86e5eb7a8a 34 this->LAST_NAV_DATA[latitude_f]=-34,4251;
gstedile 3:de86e5eb7a8a 35 this->LAST_NAV_DATA[latitude_i]=-34,4251;
gstedile 3:de86e5eb7a8a 36 this->LAST_NAV_DATA[time_f]=0; // El dato (timestamp al final del período en cuestion, en horas) se obtendrá luego proveniente del GPS.
gstedile 3:de86e5eb7a8a 37 this->LAST_NAV_DATA[time_i]=0; // Timestamp al inicio del período en horas.
gstedile 3:de86e5eb7a8a 38 this->LAST_NAV_DATA[distance_p]=0; // Distancia recorrida durante este período en millas nauticas.
gstedile 3:de86e5eb7a8a 39 this->LAST_NAV_DATA[speed_p]=0; // Velocidad media del período en nudos.
gstedile 3:de86e5eb7a8a 40 this->LAST_NAV_DATA[consumption_p]=0; // Consumo en litros del período.
gstedile 3:de86e5eb7a8a 41 this->LAST_NAV_DATA[cons_mile_p]=0; // Consumo en litros por milla nautica.
gstedile 3:de86e5eb7a8a 42 this->LAST_NAV_DATA[cons_hour_p]=0; // Consumo en litros por hora.
gstedile 0:9370c06bf7f3 43 }
gstedile 0:9370c06bf7f3 44
gstedile 2:86e9ac5dcacc 45 int NAVDATA::rotate_data(float x, float y, float t, float c){ // Longitud, Latitud, Timestamp, Consumption;
gstedile 3:de86e5eb7a8a 46 if ( t > LAST_NAV_DATA[time_f] ) { // Tiempo válido?
gstedile 3:de86e5eb7a8a 47 this->LAST_NAV_DATA[longitude_i]=this->LAST_NAV_DATA[longitude_f];
gstedile 3:de86e5eb7a8a 48 this->LAST_NAV_DATA[longitude_f]=x;
gstedile 3:de86e5eb7a8a 49 this->LAST_NAV_DATA[latitude_i]=this->LAST_NAV_DATA[latitude_f];
gstedile 3:de86e5eb7a8a 50 this->LAST_NAV_DATA[latitude_f]=y;
gstedile 3:de86e5eb7a8a 51 this->LAST_NAV_DATA[time_i]=this->LAST_NAV_DATA[time_f] + ZH; // EN HORAS
gstedile 3:de86e5eb7a8a 52 this->LAST_NAV_DATA[time_f]=time_NMEA2hours(t); // EN HORAS
gstedile 3:de86e5eb7a8a 53 this->LAST_NAV_DATA[distance_p]= sqrt(pow(Klong*(this->LAST_NAV_DATA[longitude_f]-this->LAST_NAV_DATA[longitude_i]),2) + pow(Klat*(this->LAST_NAV_DATA[latitude_f]-this->LAST_NAV_DATA[latitude_i]),2)) ; // Distancia recorrida durante este período.
gstedile 3:de86e5eb7a8a 54 this->LAST_NAV_DATA[speed_p]=this->LAST_NAV_DATA[distance_p]/(this->LAST_NAV_DATA[time_f]-this->LAST_NAV_DATA[time_i]);
gstedile 3:de86e5eb7a8a 55 this->LAST_NAV_DATA[consumption_p]=c; // Consumo en litros del período.
gstedile 3:de86e5eb7a8a 56
gstedile 3:de86e5eb7a8a 57 if (this->LAST_NAV_DATA[distance_p] != 0) this->LAST_NAV_DATA[cons_mile_p]=c/this->LAST_NAV_DATA[distance_p]; // Consumo en litros por milla nautica.
gstedile 3:de86e5eb7a8a 58 else this->LAST_NAV_DATA[cons_mile_p]= -1; // Variable no mensurable (-1)
gstedile 3:de86e5eb7a8a 59
gstedile 3:de86e5eb7a8a 60 this->LAST_NAV_DATA[cons_hour_p]=c/(this->LAST_NAV_DATA[time_f]-this->LAST_NAV_DATA[time_i]); // Consumo en litros por hora.
gstedile 2:86e9ac5dcacc 61 return 1;
gstedile 2:86e9ac5dcacc 62 }
gstedile 2:86e9ac5dcacc 63 else
gstedile 3:de86e5eb7a8a 64 return 0; // Período no válido. Tiempo <=0!
gstedile 2:86e9ac5dcacc 65 }
gstedile 3:de86e5eb7a8a 66
gstedile 3:de86e5eb7a8a 67 float NAVDATA::time_NMEA2hours(float timestickNMEA){
gstedile 3:de86e5eb7a8a 68 float seconds, minutes, hours;
gstedile 3:de86e5eb7a8a 69 hours= int(timestickNMEA/10000); //Formato NMEA: hhmmss,sss; => int(hh,mmssss)=hh
gstedile 3:de86e5eb7a8a 70 seconds= (timestickNMEA/100 - int(timestickNMEA/100))*100; // => (hhmmss,sss/100 - int(hhmmss,sss/100))*100 = ss,sss
gstedile 3:de86e5eb7a8a 71 minutes= (int(timestickNMEA/10000 - hours))*100; // => int((hh,mmsssss - hh)*100)= mm
gstedile 3:de86e5eb7a8a 72 return (hours + minutes/60 + seconds/3600); // => expresado en horas, formato decimal
gstedile 3:de86e5eb7a8a 73 }
gstedile 3:de86e5eb7a8a 74
gstedile 3:de86e5eb7a8a 75 /// AGREGAR ACA int NAVDATA::rotate_data(float x, float y, float t, float c) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%<<<<
gstedile 3:de86e5eb7a8a 76
gstedile 3:de86e5eb7a8a 77
gstedile 0:9370c06bf7f3 78 /*
gstedile 0:9370c06bf7f3 79 GGA - essential fix data which provide 3D location and accuracy data.
gstedile 0:9370c06bf7f3 80
gstedile 0:9370c06bf7f3 81 $GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47
gstedile 0:9370c06bf7f3 82
gstedile 0:9370c06bf7f3 83 Where:
gstedile 0:9370c06bf7f3 84 GGA Global Positioning System Fix Data
gstedile 0:9370c06bf7f3 85 123519 Fix taken at 12:35:19 UTC
gstedile 0:9370c06bf7f3 86 4807.038,N Latitude 48 deg 07.038' N
gstedile 0:9370c06bf7f3 87 01131.000,E Longitude 11 deg 31.000' E
gstedile 0:9370c06bf7f3 88 1 Fix quality: 0 = invalid
gstedile 0:9370c06bf7f3 89 1 = GPS fix (SPS)
gstedile 0:9370c06bf7f3 90 2 = DGPS fix
gstedile 0:9370c06bf7f3 91 3 = PPS fix
gstedile 0:9370c06bf7f3 92 4 = Real Time Kinematic
gstedile 0:9370c06bf7f3 93 5 = Float RTK
gstedile 0:9370c06bf7f3 94 6 = estimated (dead reckoning) (2.3 feature)
gstedile 0:9370c06bf7f3 95 7 = Manual input mode
gstedile 0:9370c06bf7f3 96 8 = Simulation mode
gstedile 0:9370c06bf7f3 97 08 Number of satellites being tracked
gstedile 0:9370c06bf7f3 98 0.9 Horizontal dilution of position
gstedile 0:9370c06bf7f3 99 545.4,M Altitude, Meters, above mean sea level
gstedile 0:9370c06bf7f3 100 46.9,M Height of geoid (mean sea level) above WGS84
gstedile 0:9370c06bf7f3 101 ellipsoid
gstedile 0:9370c06bf7f3 102 (empty field) time in seconds since last DGPS update
gstedile 0:9370c06bf7f3 103 (empty field) DGPS station ID number
gstedile 0:9370c06bf7f3 104 *47 the checksum data, always begins with *
gstedile 0:9370c06bf7f3 105 */
gstedile 0:9370c06bf7f3 106
gstedile 0:9370c06bf7f3 107
gstedile 0:9370c06bf7f3 108 /*
gstedile 0:9370c06bf7f3 109 sscanf(msg, "GN %s",mensaje1);
gstedile 0:9370c06bf7f3 110 sscanf(msg, "GP,%s", mensaje2);
gstedile 0:9370c06bf7f3 111 sscanf(msg, "GSV,%s", mensaje3);
gstedile 0:9370c06bf7f3 112 sscanf(msg, "G,%s", mensaje4);
gstedile 0:9370c06bf7f3 113 */
gstedile 0:9370c06bf7f3 114 // $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 115
gstedile 0:9370c06bf7f3 116
gstedile 0:9370c06bf7f3 117
gstedile 0:9370c06bf7f3 118 /* BORRAR ESTO
gstedile 0:9370c06bf7f3 119
gstedile 0:9370c06bf7f3 120
gstedile 0:9370c06bf7f3 121
gstedile 0:9370c06bf7f3 122 if(sscanf(msg, "GNRMC, %f,%c,%f,%c,%f,%c,%f", &time, &signal, &latitude, &ns, &longitude, &ew, &speed) >=1){
gstedile 0:9370c06bf7f3 123 // Agrego las siguientes lineas porque originalmente se analizaba el mensaje GGA y se evaluaba "lock" para saber si habia datos validos
gstedile 0:9370c06bf7f3 124 // 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 125
gstedile 0:9370c06bf7f3 126 if(signal == 'A'){
gstedile 0:9370c06bf7f3 127 lock = 1;
gstedile 0:9370c06bf7f3 128 }
gstedile 0:9370c06bf7f3 129 else {
gstedile 0:9370c06bf7f3 130 lock = 0;
gstedile 0:9370c06bf7f3 131 }
gstedile 0:9370c06bf7f3 132
gstedile 0:9370c06bf7f3 133 // if(sscanf(msg, "GNGGA,%f,%f,%c,%f,%c,%d", &time, &latitude, &ns, &longitude, &ew, &lock) >= 1) {
gstedile 0:9370c06bf7f3 134
gstedile 0:9370c06bf7f3 135
gstedile 0:9370c06bf7f3 136
gstedile 0:9370c06bf7f3 137 if(!lock) {
gstedile 0:9370c06bf7f3 138
gstedile 0:9370c06bf7f3 139 longitude = 0.0;
gstedile 0:9370c06bf7f3 140 latitude = 0.0;
gstedile 0:9370c06bf7f3 141 speed = 0.0; //
gstedile 0:9370c06bf7f3 142 return 0;
gstedile 0:9370c06bf7f3 143 }
gstedile 0:9370c06bf7f3 144
gstedile 0:9370c06bf7f3 145 else {
gstedile 0:9370c06bf7f3 146 if(ns == 'S') { latitude *= -1.0; }
gstedile 0:9370c06bf7f3 147 if(ew == 'W') { longitude *= -1.0; }
gstedile 0:9370c06bf7f3 148 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 149 float minutes = latitude - (degrees * 100.0f); // Resto los grados y me quedo con los minutos
gstedile 0:9370c06bf7f3 150 latitude = degrees + minutes / 60.0f; // Convierto a decimal sumando grados y dividiendo entre 60 los minutos
gstedile 0:9370c06bf7f3 151 //degrees = trunc(longitude / 100.0f * 0.01f); // Corrijo esta linea porque es como dividir por 10000.
gstedile 0:9370c06bf7f3 152 degrees = trunc(longitude / 100.0f); // Repito para la latitud.
gstedile 0:9370c06bf7f3 153 minutes = longitude - (degrees * 100.0f);
gstedile 0:9370c06bf7f3 154 longitude = degrees + minutes / 60.0f;
gstedile 0:9370c06bf7f3 155 speed *= 1.852000f; // Convierto nudos a km/h
gstedile 0:9370c06bf7f3 156
gstedile 0:9370c06bf7f3 157 return 1;
gstedile 0:9370c06bf7f3 158
gstedile 0:9370c06bf7f3 159 }
gstedile 0:9370c06bf7f3 160 }
gstedile 0:9370c06bf7f3 161
gstedile 0:9370c06bf7f3 162 }
gstedile 0:9370c06bf7f3 163 }
gstedile 0:9370c06bf7f3 164
gstedile 0:9370c06bf7f3 165 float GPS::trunc(float v) {
gstedile 0:9370c06bf7f3 166 if(v < 0.0) {
gstedile 0:9370c06bf7f3 167 v*= -1.0;
gstedile 0:9370c06bf7f3 168 v = floor(v);
gstedile 0:9370c06bf7f3 169 v*=-1.0;
gstedile 0:9370c06bf7f3 170 } else {
gstedile 0:9370c06bf7f3 171 v = floor(v);
gstedile 0:9370c06bf7f3 172 }
gstedile 0:9370c06bf7f3 173 return v;
gstedile 0:9370c06bf7f3 174 }
gstedile 0:9370c06bf7f3 175
gstedile 0:9370c06bf7f3 176 void GPS::getline() {
gstedile 0:9370c06bf7f3 177 while(_gps.getc() != '$'); // wait for the start of a line
gstedile 0:9370c06bf7f3 178 for(int i=0; i<256; i++) {
gstedile 0:9370c06bf7f3 179 msg[i] = _gps.getc();
gstedile 0:9370c06bf7f3 180 if(msg[i] == '\r') {
gstedile 0:9370c06bf7f3 181 msg[i] = 0;
gstedile 0:9370c06bf7f3 182 return;
gstedile 0:9370c06bf7f3 183 }
gstedile 0:9370c06bf7f3 184 }
gstedile 0:9370c06bf7f3 185 error("Overflowed message limit");
gstedile 0:9370c06bf7f3 186 }
gstedile 0:9370c06bf7f3 187
gstedile 0:9370c06bf7f3 188 BORRAR ESTO */
gstedile 0:9370c06bf7f3 189
gstedile 0:9370c06bf7f3 190
gstedile 0:9370c06bf7f3 191 /*void GPS::getline() {
gstedile 0:9370c06bf7f3 192 if(_gps.getc() != '$') return; // exit if there isn't start of a line
gstedile 0:9370c06bf7f3 193 for(int i=0; i<256; i++) {
gstedile 0:9370c06bf7f3 194 msg[i] = _gps.getc();
gstedile 0:9370c06bf7f3 195 if(msg[i] == '\r') {
gstedile 0:9370c06bf7f3 196 msg[i] = 0;
gstedile 0:9370c06bf7f3 197 return;
gstedile 0:9370c06bf7f3 198 }
gstedile 0:9370c06bf7f3 199 }
gstedile 0:9370c06bf7f3 200 error("Overflowed message limit");
gstedile 2:86e9ac5dcacc 201 */