2.5/bertos/net/nmeap/tst/test3.c

   1 /*
   2 Copyright (c) 2005, David M Howard (daveh at dmh2000.com)
   3 All rights reserved.
   4
   5 This product is licensed for use and distribution under the BSD Open Source License.
   6 see the file COPYING for more details.
   7
   8 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
   9 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  10 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  11 ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
  12 LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
  13 OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
  14 OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
  15 OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
  16 WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
  17 OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
  18 EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  19
  20 */
  21
  22 /*
  23 ========================================================================================================
  24 EXAMPLE : SETUP FOR GGA AND RMC SENTENCES + A CUSTOM SENTENCE PARSER WITH CHARACTER BY CHARACTER IO
  25 =======================================================================================================
  26 */
  27
  28
  29 /*
  30 $PGRMF
  31
  32 GARMIN PROPRIETARY GPS Position Fix Data
  33
  34 $PGRMF,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15*HH
  35 1 = GPS week number
  36 2 = GPS seconds in current week
  37 3 = UTC date, ddmmyy format
  38 4 = UTC time, hhmmss format
  39 5 = GPS leap second count
  40 6 = Latitude, dddmm.mmmm format
  41 7 = Latitude hemisphere, N or S
  42 8 = Longitude, dddmm.mmmm format
  43 9 = Longitude hemisphere, E or W
  44 10 = Mode (M=Manual, A=Automatic)
  45 11 = Fix type (0=No fix, 1=2D fix, 2=3D fix)
  46 12 = Speed over ground, kilometres / hour
  47 13 = Course over ground, degrees true
  48 14 = PDOP (Position dilution of precision), rounded to nearest integer
  49 15 = TDOP (Time dilution of precision), rounded to nearest integer
  50 HH = Checksum
  51 */
  52
  53
  54
  55 #include <stdio.h>
  56 #include <stdlib.h>
  57 #include <string.h>
  58 #include <assert.h>
  59 #include "nmeap.h"
  60
  61 nmeap_gga_t g_gga;
  62
  63 char test_vector[] = {
  64 "$GPGGA,123519,3929.946667,N,11946.086667,E,1,08,0.9,545.4,M,46.9,M,,*4A\r\n"
  65 "$GPRMC,225446,A,4916.45,N,12311.12,W,000.5,054.7,191194,020.3,E*68\r\n"
  66 "$PGRMF,1,100,191105,123519,13,3929.946667,N,12311.12,W,A,2,100.1,181.2,3,8*35\r\n"
  67 };
  68
  69 char *pvec = test_vector;
  70
  71 /** simulate character by character IO */
  72 int readchar()
  73 {
  74     int ch;
  75     if (*pvec == 0) {
  76         ch = -1;
  77     }
  78     else {
  79         ch = *pvec++;
  80     }
  81     return ch;
  82 }
  83 /* --------------------------------------------------------------*/
  84 /*STEP 1a : define a data structure to contain the sentence data */
  85 /* ------------------------------------------------------------- */
  86 struct garmin_rmf {
  87     /* field                            position in sentence */
  88     int            week;             /*    1 = GPS week number                                                          */
  89     int            seconds_of_week;  /*    2 = GPS seconds in current week                                              */
  90     unsigned long  date;             /*    3 = UTC date, ddmmyy format                                                  */
  91     unsigned long  time;             /*    4 = UTC time, hhmmss format                                                  */
  92     int            leap;             /*    5 = GPS leap second count                                                    */
  93     double         lat;              /*  6,7 = Latitude, dddmm.mmmm format (north positive)                             */
  94     double         lon;              /*  8,9 = Longitude, dddmm.mmmm format (east positive)                             */
  95     int            mode;             /*   10 = Mode (M=Manual, A=Automatic)                                             */
  96     int            fix;              /*   11 = Fix type (0=No fix, 1=2D fix, 2=3D fix)                                  */
  97     double         speed;            /*   12 = Speed over ground, kilometres / hour                                     */
  98     double         course;           /*   13 = Course over ground, degrees true                                         */
  99     int            pdop;             /*   14 = PDOP (Position dilution of precision), rounded to nearest integer        */
 100     int            tdop;             /*   15 = TDOP (Time dilution of precision), rounded to nearest integer            */
 101 };
 102 typedef struct garmin_rmf garmin_rmf_t;
 103
 104 /* --------------------------------------------------------------*/
 105 /*STEP 1b : define an id value for the message                   */
 106 /* ------------------------------------------------------------- */
 107 #define GARMIN_PGRMF (NMEAP_USER + 0)
 108
 109
 110 /* --------------------------------------------------------------*/
 111 /* STEP 1c : write the sentence parser                            */
 112 /* ------------------------------------------------------------- */
 113 int custom_pgrmf(nmeap_context_t *context,nmeap_sentence_t *sentence)
 114 {
 115 #ifndef NDEBUG
 116     int i;
 117 #endif
 118
 119         /* get pointer to sentence data */
 120         garmin_rmf_t *rmf = (garmin_rmf_t *)sentence->data;
 121
 122         if (rmf != 0) {
 123                 /* if the sentence has a data storage element, use it */
 124
 125
 126                 /* extract data from the tokens */
 127                 rmf->week            = atoi(context->token[1]);
 128                 rmf->seconds_of_week = atoi(context->token[2]);
 129                 rmf->date            = (unsigned long)atol(context->token[3]);
 130                 rmf->time            = (unsigned long)atol(context->token[4]);
 131                 rmf->leap            = atoi(context->token[5]);
 132                 rmf->lat             = nmeap_latitude(context->token[6],context->token[7]);
 133                 rmf->lon             = nmeap_longitude(context->token[8],context->token[9]);
 134                 rmf->mode            = atoi(context->token[10]);
 135                 rmf->fix             = atoi(context->token[11]);
 136                 rmf->speed           = atof(context->token[12]);
 137                 rmf->course          = atof(context->token[13]);
 138                 rmf->pdop            = atoi(context->token[14]);
 139                 rmf->tdop            = atoi(context->token[15]);
 140         }
 141         /* else there was no data element to store into */
 142
 143 #ifndef NDEBUG
 144     /* print raw input string */
 145     printf("%s",context->debug_input);
 146
 147     /* print some validation data */
 148     printf("%s==%s %02x==%02x\n",context->input_name,sentence->name,context->icks,context->ccks);
 149
 150     /* print the tokens */
 151     for(i=0;i<context->tokens;i++) {
 152         printf("%d:%s\n",i,context->token[i]);
 153     }
 154 #endif
 155
 156     /* if the sentence has a callout, call it */
 157     if (sentence->callout != 0) {
 158         (*sentence->callout)(context,rmf,context->user_data);
 159     }
 160
 161     return GARMIN_PGRMF;
 162 }
 163
 164
 165 /* -------------------------------------------------------------*/
 166 /*STEP 2 : write a function to do something with the data       */
 167 /* ------------------------------------------------------------ */
 168 static void print_pgrmf(garmin_rmf_t *rmf)
 169 {
 170         assert(rmf != 0);
 171
 172         printf("                    w sec date   time   lp lat       lon         m f spd    crs    p t\n");
 173     printf("found PGRMF message %d %d %lu %lu %d %.6f %.6f %d %d %.2f %.2f %d %d\n",
 174             rmf->week,
 175             rmf->seconds_of_week,
 176             rmf->date,
 177             rmf->time,
 178             rmf->leap,
 179             rmf->lat,
 180             rmf->lon,
 181             rmf->mode,
 182             rmf->fix,
 183             rmf->speed,
 184             rmf->course,
 185             rmf->pdop,
 186             rmf->tdop
 187             );
 188 }
 189
 190 /* -------------------------------------------------------------*/
 191 /*STEP 3 : if using the callout method, write the callout       */
 192 /* ------------------------------------------------------------ */
 193 static void pgrmf_callout(nmeap_context_t *context,void *data,void *user_data)
 194 {
 195     garmin_rmf_t *rmf = (garmin_rmf_t *)data;
 196
 197     printf("-------------callout\n");
 198     print_pgrmf(rmf);
 199 }
 200
 201
 202 /* ---------------------------------------------------------------------------------------*/
 203 /* STEP 4 : allocate the data structures. be careful if you put them on the stack because */
 204 /*          they need to be live for the duration of the parser                           */
 205 /* ---------------------------------------------------------------------------------------*/
 206 static nmeap_context_t nmea;       /* parser context */
 207 static nmeap_gga_t     gga;        /* this is where the data from GGA messages will show up */
 208 static nmeap_rmc_t     rmc;        /* this is where the data from RMC messages will show up */
 209 static garmin_rmf_t    rmf;        /* this is where the data from RMF messages will show up */
 210 static int             user_data; /* user can pass in anything. typically it will be a pointer to some user data */
 211
 212 int main(int argc,char *argv[])
 213 {
 214     int             status;
 215     char            ch;
 216
 217     /* ---------------------------------------*/
 218     /*STEP 5 : initialize the nmea context    */
 219     /* ---------------------------------------*/
 220     status = nmeap_init(&nmea,(void *)&user_data);
 221     if (status != 0) {
 222         printf("nmeap_init %d\n",status);
 223         exit(1);
 224     }
 225
 226     /* ---------------------------------------*/
 227     /*STEP 6 : add standard GPGGA parser      */
 228         /*         (no callout this time)         */
 229     /* -------------------------------------- */
 230     status = nmeap_addParser(&nmea,"GPGGA",nmeap_gpgga,0,&gga);
 231     if (status != 0) {
 232         printf("nmeap_add %d\n",status);
 233         exit(1);
 234     }
 235
 236     /* ---------------------------------------*/
 237     /*STEP 7 : add standard GPRMC parser      */
 238         /*         (no callout this time)         */
 239     /* -------------------------------------- */
 240     status = nmeap_addParser(&nmea,"GPRMC",nmeap_gprmc,0,&rmc);
 241     if (status != 0) {
 242         printf("nmeap_add %d\n",status);
 243         exit(1);
 244     }
 245
 246     /* ---------------------------------------*/
 247     /*STEP 8 : ADD THE CUSTOM PARSER          */
 248         /*         with callout         )         */
 249     /* -------------------------------------- */
 250     status = nmeap_addParser(&nmea,"PGRMF",custom_pgrmf,pgrmf_callout,&rmf);
 251     if (status != 0) {
 252         printf("nmeap_add %d\n",status);
 253         exit(1);
 254     }
 255     /* ---------------------------------------*/
 256     /*STEP 9 : process input until done       */
 257     /* -------------------------------------- */
 258     for(;;) {
 259         /* ---------------------------------------*/
 260         /*STEP 10: get a byte at a time           */
 261         /* -------------------------------------- */
 262         ch = readchar();
 263         if (ch <= 0) {
 264             break;
 265         }
 266
 267         /* --------------------------------------- */
 268         /*STEP 11: pass it to the parser          */
 269         /* status indicates whether a complete msg */
 270         /* arrived for this byte                   */
 271         /* NOTE : in addition to the return status */
 272         /* the message callout will be fired when  */
 273         /* a complete message is processed         */
 274         /* --------------------------------------- */
 275         status = nmeap_parse(&nmea,ch);
 276
 277         /* ---------------------------------------*/
 278         /*STEP 12 : process the return code       */
 279         /* -------------------------------------- */
 280         switch(status) {
 281         case NMEAP_GPGGA:
 282             /* GOT A GPGGA MESSAGE */
 283             printf("-------------switch\n");
 284             printf("GPGGA\n");
 285             printf("-------------\n");
 286             break;
 287         case NMEAP_GPRMC:
 288             /* GOT A GPRMC MESSAGE */
 289             printf("-------------switch\n");
 290             printf("GPRMC\n");
 291             printf("-------------\n");
 292             break;
 293         case GARMIN_PGRMF:
 294             /* GOT A PGRMF MESSAGE */
 295             printf("-------------switch\n");
 296             print_pgrmf(&rmf);
 297             printf("-------------\n");
 298             break;
 299         default:
 300             break;
 301         }
 302     }
 303
 304     return 0;
 305 }
 306