BeRTOS
|
00001 /* 00002 Copyright (c) 2005, David M Howard (daveh at dmh2000.com) 00003 All rights reserved. 00004 00005 This product is licensed for use and distribution under the BSD Open Source License. 00006 see the file COPYING for more details. 00007 00008 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00009 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00010 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00011 ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00012 LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 00013 OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 00014 OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 00015 OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 00016 WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 00017 OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 00018 EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 00019 00020 */ 00021 00022 /* 00023 ======================================================================================================== 00024 EXAMPLE : SETUP FOR GGA AND RMC SENTENCES + A CUSTOM SENTENCE PARSER WITH CHARACTER BY CHARACTER IO 00025 ======================================================================================================= 00026 */ 00027 00028 00029 /* 00030 $PGRMF 00031 00032 GARMIN PROPRIETARY GPS Position Fix Data 00033 00034 $PGRMF,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15*HH 00035 1 = GPS week number 00036 2 = GPS seconds in current week 00037 3 = UTC date, ddmmyy format 00038 4 = UTC time, hhmmss format 00039 5 = GPS leap second count 00040 6 = Latitude, dddmm.mmmm format 00041 7 = Latitude hemisphere, N or S 00042 8 = Longitude, dddmm.mmmm format 00043 9 = Longitude hemisphere, E or W 00044 10 = Mode (M=Manual, A=Automatic) 00045 11 = Fix type (0=No fix, 1=2D fix, 2=3D fix) 00046 12 = Speed over ground, kilometres / hour 00047 13 = Course over ground, degrees true 00048 14 = PDOP (Position dilution of precision), rounded to nearest integer 00049 15 = TDOP (Time dilution of precision), rounded to nearest integer 00050 HH = Checksum 00051 */ 00052 00053 00054 00055 #include <stdio.h> 00056 #include <stdlib.h> 00057 #include <string.h> 00058 #include <assert.h> 00059 #include "nmeap.h" 00060 00061 nmeap_gga_t g_gga; 00062 00063 char test_vector[] = { 00064 "$GPGGA,123519,3929.946667,N,11946.086667,E,1,08,0.9,545.4,M,46.9,M,,*4A\r\n" 00065 "$GPRMC,225446,A,4916.45,N,12311.12,W,000.5,054.7,191194,020.3,E*68\r\n" 00066 "$PGRMF,1,100,191105,123519,13,3929.946667,N,12311.12,W,A,2,100.1,181.2,3,8*35\r\n" 00067 }; 00068 00069 char *pvec = test_vector; 00070 00072 int readchar() 00073 { 00074 int ch; 00075 if (*pvec == 0) { 00076 ch = -1; 00077 } 00078 else { 00079 ch = *pvec++; 00080 } 00081 return ch; 00082 } 00083 /* --------------------------------------------------------------*/ 00084 /*STEP 1a : define a data structure to contain the sentence data */ 00085 /* ------------------------------------------------------------- */ 00086 struct garmin_rmf { 00087 /* field position in sentence */ 00088 int week; /* 1 = GPS week number */ 00089 int seconds_of_week; /* 2 = GPS seconds in current week */ 00090 unsigned long date; /* 3 = UTC date, ddmmyy format */ 00091 unsigned long time; /* 4 = UTC time, hhmmss format */ 00092 int leap; /* 5 = GPS leap second count */ 00093 double lat; /* 6,7 = Latitude, dddmm.mmmm format (north positive) */ 00094 double lon; /* 8,9 = Longitude, dddmm.mmmm format (east positive) */ 00095 int mode; /* 10 = Mode (M=Manual, A=Automatic) */ 00096 int fix; /* 11 = Fix type (0=No fix, 1=2D fix, 2=3D fix) */ 00097 double speed; /* 12 = Speed over ground, kilometres / hour */ 00098 double course; /* 13 = Course over ground, degrees true */ 00099 int pdop; /* 14 = PDOP (Position dilution of precision), rounded to nearest integer */ 00100 int tdop; /* 15 = TDOP (Time dilution of precision), rounded to nearest integer */ 00101 }; 00102 typedef struct garmin_rmf garmin_rmf_t; 00103 00104 /* --------------------------------------------------------------*/ 00105 /*STEP 1b : define an id value for the message */ 00106 /* ------------------------------------------------------------- */ 00107 #define GARMIN_PGRMF (NMEAP_USER + 0) 00108 00109 00110 /* --------------------------------------------------------------*/ 00111 /* STEP 1c : write the sentence parser */ 00112 /* ------------------------------------------------------------- */ 00113 int custom_pgrmf(nmeap_context_t *context,nmeap_sentence_t *sentence) 00114 { 00115 #ifndef NDEBUG 00116 int i; 00117 #endif 00118 00119 /* get pointer to sentence data */ 00120 garmin_rmf_t *rmf = (garmin_rmf_t *)sentence->data; 00121 00122 if (rmf != 0) { 00123 /* if the sentence has a data storage element, use it */ 00124 00125 00126 /* extract data from the tokens */ 00127 rmf->week = atoi(context->token[1]); 00128 rmf->seconds_of_week = atoi(context->token[2]); 00129 rmf->date = (unsigned long)atol(context->token[3]); 00130 rmf->time = (unsigned long)atol(context->token[4]); 00131 rmf->leap = atoi(context->token[5]); 00132 rmf->lat = nmeap_latitude(context->token[6],context->token[7]); 00133 rmf->lon = nmeap_longitude(context->token[8],context->token[9]); 00134 rmf->mode = atoi(context->token[10]); 00135 rmf->fix = atoi(context->token[11]); 00136 rmf->speed = atof(context->token[12]); 00137 rmf->course = atof(context->token[13]); 00138 rmf->pdop = atoi(context->token[14]); 00139 rmf->tdop = atoi(context->token[15]); 00140 } 00141 /* else there was no data element to store into */ 00142 00143 #ifndef NDEBUG 00144 /* print raw input string */ 00145 printf("%s",context->debug_input); 00146 00147 /* print some validation data */ 00148 printf("%s==%s %02x==%02x\n",context->input_name,sentence->name,context->icks,context->ccks); 00149 00150 /* print the tokens */ 00151 for(i=0;i<context->tokens;i++) { 00152 printf("%d:%s\n",i,context->token[i]); 00153 } 00154 #endif 00155 00156 /* if the sentence has a callout, call it */ 00157 if (sentence->callout != 0) { 00158 (*sentence->callout)(context,rmf,context->user_data); 00159 } 00160 00161 return GARMIN_PGRMF; 00162 } 00163 00164 00165 /* -------------------------------------------------------------*/ 00166 /*STEP 2 : write a function to do something with the data */ 00167 /* ------------------------------------------------------------ */ 00168 static void print_pgrmf(garmin_rmf_t *rmf) 00169 { 00170 assert(rmf != 0); 00171 00172 printf(" w sec date time lp lat lon m f spd crs p t\n"); 00173 printf("found PGRMF message %d %d %lu %lu %d %.6f %.6f %d %d %.2f %.2f %d %d\n", 00174 rmf->week, 00175 rmf->seconds_of_week, 00176 rmf->date, 00177 rmf->time, 00178 rmf->leap, 00179 rmf->lat, 00180 rmf->lon, 00181 rmf->mode, 00182 rmf->fix, 00183 rmf->speed, 00184 rmf->course, 00185 rmf->pdop, 00186 rmf->tdop 00187 ); 00188 } 00189 00190 /* -------------------------------------------------------------*/ 00191 /*STEP 3 : if using the callout method, write the callout */ 00192 /* ------------------------------------------------------------ */ 00193 static void pgrmf_callout(nmeap_context_t *context,void *data,void *user_data) 00194 { 00195 garmin_rmf_t *rmf = (garmin_rmf_t *)data; 00196 00197 printf("-------------callout\n"); 00198 print_pgrmf(rmf); 00199 } 00200 00201 00202 /* ---------------------------------------------------------------------------------------*/ 00203 /* STEP 4 : allocate the data structures. be careful if you put them on the stack because */ 00204 /* they need to be live for the duration of the parser */ 00205 /* ---------------------------------------------------------------------------------------*/ 00206 static nmeap_context_t nmea; /* parser context */ 00207 static nmeap_gga_t gga; /* this is where the data from GGA messages will show up */ 00208 static nmeap_rmc_t rmc; /* this is where the data from RMC messages will show up */ 00209 static garmin_rmf_t rmf; /* this is where the data from RMF messages will show up */ 00210 static int user_data; /* user can pass in anything. typically it will be a pointer to some user data */ 00211 00212 int main(int argc,char *argv[]) 00213 { 00214 int status; 00215 char ch; 00216 00217 /* ---------------------------------------*/ 00218 /*STEP 5 : initialize the nmea context */ 00219 /* ---------------------------------------*/ 00220 status = nmeap_init(&nmea,(void *)&user_data); 00221 if (status != 0) { 00222 printf("nmeap_init %d\n",status); 00223 exit(1); 00224 } 00225 00226 /* ---------------------------------------*/ 00227 /*STEP 6 : add standard GPGGA parser */ 00228 /* (no callout this time) */ 00229 /* -------------------------------------- */ 00230 status = nmeap_addParser(&nmea,"GPGGA",nmeap_gpgga,0,&gga); 00231 if (status != 0) { 00232 printf("nmeap_add %d\n",status); 00233 exit(1); 00234 } 00235 00236 /* ---------------------------------------*/ 00237 /*STEP 7 : add standard GPRMC parser */ 00238 /* (no callout this time) */ 00239 /* -------------------------------------- */ 00240 status = nmeap_addParser(&nmea,"GPRMC",nmeap_gprmc,0,&rmc); 00241 if (status != 0) { 00242 printf("nmeap_add %d\n",status); 00243 exit(1); 00244 } 00245 00246 /* ---------------------------------------*/ 00247 /*STEP 8 : ADD THE CUSTOM PARSER */ 00248 /* with callout ) */ 00249 /* -------------------------------------- */ 00250 status = nmeap_addParser(&nmea,"PGRMF",custom_pgrmf,pgrmf_callout,&rmf); 00251 if (status != 0) { 00252 printf("nmeap_add %d\n",status); 00253 exit(1); 00254 } 00255 /* ---------------------------------------*/ 00256 /*STEP 9 : process input until done */ 00257 /* -------------------------------------- */ 00258 for(;;) { 00259 /* ---------------------------------------*/ 00260 /*STEP 10: get a byte at a time */ 00261 /* -------------------------------------- */ 00262 ch = readchar(); 00263 if (ch <= 0) { 00264 break; 00265 } 00266 00267 /* --------------------------------------- */ 00268 /*STEP 11: pass it to the parser */ 00269 /* status indicates whether a complete msg */ 00270 /* arrived for this byte */ 00271 /* NOTE : in addition to the return status */ 00272 /* the message callout will be fired when */ 00273 /* a complete message is processed */ 00274 /* --------------------------------------- */ 00275 status = nmeap_parse(&nmea,ch); 00276 00277 /* ---------------------------------------*/ 00278 /*STEP 12 : process the return code */ 00279 /* -------------------------------------- */ 00280 switch(status) { 00281 case NMEAP_GPGGA: 00282 /* GOT A GPGGA MESSAGE */ 00283 printf("-------------switch\n"); 00284 printf("GPGGA\n"); 00285 printf("-------------\n"); 00286 break; 00287 case NMEAP_GPRMC: 00288 /* GOT A GPRMC MESSAGE */ 00289 printf("-------------switch\n"); 00290 printf("GPRMC\n"); 00291 printf("-------------\n"); 00292 break; 00293 case GARMIN_PGRMF: 00294 /* GOT A PGRMF MESSAGE */ 00295 printf("-------------switch\n"); 00296 print_pgrmf(&rmf); 00297 printf("-------------\n"); 00298 break; 00299 default: 00300 break; 00301 } 00302 } 00303 00304 return 0; 00305 } 00306