![]() |
ATC
Advanced Trip Computer
|
00001 00007 #include <cstring> 00008 #include <cmath> 00009 #include <cstdlib> 00010 #include <cstdio> 00011 00012 #include "Position.h" 00013 00014 extern "C" 00015 { 00016 #include "com.h" 00017 } 00018 00019 #define TIME_POS 7 00020 #define VALIDITY_POS 18 00021 #define LAT_POS 20 00022 #define LAT_SIGN_POS 30 00023 #define LONG_POS 32 00024 #define LONG_SIGN_POS 43 00025 #define SPEED_POS 45 00026 //#define DIR_POS 50 00027 //#define DATE_POS 57 00028 00029 #define KNOT_TO_KMH 1.852 00030 00031 00035 class GPSobserver 00036 { 00037 public: 00038 00039 virtual void GPSnotify(Position position, double direction, double speed) = 0; 00040 }; 00041 00042 00047 class GPS 00048 { 00049 private: 00050 00053 static GPS* gps; 00054 00057 char linea[128]; 00060 unsigned int indice; 00063 GPSobserver* osservatore; 00066 int com_id; 00067 00073 GPS(unsigned char serial_port) 00074 { 00075 com_id = COM_open(serial_port, READ, 4800, 'n', 8, 1, NO_HANDSHAKE); 00076 indice = 0; 00077 osservatore = NULL; 00078 } 00079 GPS(void) 00080 { 00081 } 00091 Position extractData(char nmea_string[], unsigned int* speed, unsigned int* dir, bool* valid) 00092 { 00093 char substring[16]; 00094 unsigned int degree, hour, minute, second, day, month, year; 00095 int orientation; 00096 double tmp; 00097 int pos, date_pos, dir_pos; 00098 00099 if (nmea_string[VALIDITY_POS] != 'A') // dati invalidi 00100 { 00101 Position pos; 00102 *valid = false; 00103 return pos; 00104 } 00105 00106 pos = SPEED_POS; 00107 while (nmea_string[pos] != ',') 00108 pos++; 00109 pos++; 00110 dir_pos = pos; 00111 while (nmea_string[pos] != ',') 00112 pos++; 00113 pos++; 00114 date_pos = pos; 00115 00116 // estrazione orario 00117 strncpy(substring,&nmea_string[TIME_POS],2); 00118 substring[2] = '\0'; 00119 hour = (unsigned int)atoi(substring); 00120 strncpy(substring,&nmea_string[TIME_POS+2],2); 00121 substring[2] = '\0'; 00122 minute = (unsigned int)atoi(substring); 00123 strncpy(substring,&nmea_string[TIME_POS+4],2); 00124 substring[2] = '\0'; 00125 second = (unsigned int)atoi(substring); 00126 Time time(hour, minute, second); 00127 00128 // estrazione data 00129 strncpy(substring,&nmea_string[date_pos],2); 00130 substring[2] = '\0'; 00131 day = (unsigned int)atoi(substring); 00132 strncpy(substring,&nmea_string[date_pos+2],2); 00133 substring[2] = '\0'; 00134 month = (unsigned int)atoi(substring); 00135 strncpy(substring,&nmea_string[date_pos+4],2); 00136 substring[2] = '\0'; 00137 year = 2000+(unsigned int)atoi(substring); 00138 Date date(day, month, year); 00139 00140 // estrazione latitudine 00141 strncpy(substring,&nmea_string[LAT_POS],2); 00142 substring[2] = '\0'; 00143 degree = (unsigned int)atoi(substring); 00144 strncpy(substring,&nmea_string[LAT_POS+2],7); 00145 substring[7] = '\0'; 00146 tmp = atof(substring); 00147 minute = (unsigned int)floor(tmp); 00148 tmp = (tmp - floor(tmp))*60; 00149 second = (unsigned int)floor(tmp+0.5); 00150 if (nmea_string[LAT_SIGN_POS] == 'S') 00151 orientation = -1; 00152 else 00153 orientation = +1; 00154 Latitude lat(degree, minute, second, orientation); 00155 00156 // estrazione longitudine 00157 strncpy(substring,&nmea_string[LONG_POS],3); 00158 substring[3] = '\0'; 00159 degree = (unsigned int)atoi(substring); 00160 strncpy(substring,&nmea_string[LONG_POS+3],7); 00161 substring[7] = '\0'; 00162 tmp = atof(substring); 00163 minute = (unsigned int)floor(tmp); 00164 tmp = (tmp - floor(tmp))*60; 00165 second = (unsigned int)floor(tmp+0.5); 00166 if (nmea_string[LONG_SIGN_POS] == 'W') 00167 orientation = -1; 00168 else 00169 orientation = +1; 00170 Longitude lon(degree, minute, second, orientation); 00171 00172 Position position(date, time, lat, lon); // creazione oggetto di tipo Position 00173 00174 // estrazione velocita' 00175 strncpy(substring,&nmea_string[SPEED_POS],dir_pos-SPEED_POS-1); 00176 substring[dir_pos-SPEED_POS-1] = '\0'; 00177 tmp = atof(substring); 00178 *speed = (unsigned int)floor(tmp*KNOT_TO_KMH+.5); 00179 00180 // estrazione direzione 00181 strncpy(substring,&nmea_string[dir_pos],date_pos-dir_pos-1); 00182 substring[date_pos-dir_pos-1] = '\0'; 00183 tmp = atof(substring); 00184 *dir = (unsigned int)floor(tmp+.5); 00185 00186 *valid = true; 00187 return position; 00188 } 00189 00190 public: 00191 00198 static GPS* getGPS(unsigned char serial_port) 00199 { 00200 if (gps == NULL) // prima invocazione: creazione dell'unica istanza della classe 00201 gps = new GPS(serial_port); 00202 return gps; 00203 } 00208 void subscribe(GPSobserver* oss) 00209 { 00210 osservatore = oss; 00211 return; 00212 } 00217 void unsubscribe(void) 00218 { 00219 osservatore = NULL; 00220 return; 00221 } 00226 void update(void) 00227 { 00228 int n,i; 00229 char buffer[1024]; 00230 unsigned int speed, dir; 00231 bool valid; 00232 00233 if (com_id >= 0) 00234 { 00235 n = COM_read(com_id, buffer, sizeof(buffer)); // ricezione 00236 if (n > 0) 00237 { 00238 //printf("%s\r\n",buffer); 00239 for (i=0; i<n; i++) // ricerca carattere EOT 00240 { 00241 if (buffer[i] == '$') 00242 { 00243 linea[0] = buffer[i]; 00244 indice = 1; 00245 } 00246 else 00247 { 00248 if (buffer[i] == '\n') 00249 { 00250 linea[indice] = '\0'; 00251 if (strncmp(linea,"$GPRMC",6)==0) 00252 { 00253 Position pos = extractData(linea, &speed, &dir, &valid); // estrazione/formattazione dati da stringa GPS 00254 if (valid && osservatore != NULL) 00255 { 00256 osservatore->GPSnotify(pos, (double)dir, (double)speed); // inoltro informazioni a osservatore 00257 } 00258 } 00259 indice = 0; 00260 } 00261 else 00262 { 00263 linea[indice] = buffer[i]; 00264 indice++; 00265 } 00266 } // else 00267 } // for 00268 } // if 00269 } // if 00270 } 00271 }; 00272 00273 GPS* GPS::gps = NULL; 00274