BeRTOS
test3.c
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