; gpsdemo.vnm
;
; demonstrating GPS NMEA data parsing using strings
; and protocol analyser
;
; 2207 07 24 for 2nd revision of Prot Analyser (Venom 2007 09 10 and later)
;
; In particular this makes extensive use of the protocol analyser's CSV parser,
; as NMEA sentences are an example of CSV, even though they don't use quotes.
; Altogether three protocol analysers are used, to subdivide the input into
; lower levels of input tokens:
; p1 reads the input source and extracts lines separated by CRLF
; p2 reads each line and extracts comma-separated tokens
; pa (local to nmeaproc) extracts numerical values from tokens that have concatenated
; numbers in them, such as 270607 for date DDMMYY
; or angles in format ddmm.mmm (dd = degrees, mm.mmm = minutes)

; this program will work in simulation mode using a buffer
; pre-loaded with test data as a data source, or with real live data
; coming in on serial port 2. Change the following definition to FALSE
; to use the serial port
# define simulation TRUE   ; use test buffer instead of serial 2

TO init
  fix_hr := 0     ; initialise all GPS variables to sensible defaults
  fix_min := 0
   fix_sec := 0
   fix_ok := 0
   latitude_deg := 0
   latitude_min := 0
   latitude_dir := 'N'
  longitude_deg := 0
   longitude_min := 0
   longitude_dir := 'E'
  latitude_ok := FALSE
   longitude_ok := FALSE
  speed := 0.0
   cmg := 0.0
   date_year := 0
   date_month := 0
   date_day := 0
   magvar_deg := 0.0
   magvar_dir := 'E'

   make serial2 asynchronousserial(4800, 2, 0 )
  make source_text buffer("")
   if simulation
  [
    source := source_text   ; serial2 or source_text
    ; as we are using a buffer for source data, we want to use the queue
    ; message to signify when there is no more data, so 2nd parameter = 1
    make p1 protanalyser(source, 1)
  ]
  else
   [
    source := serial2
    ; with a real serial port, in the absence of input data we should just wait.
    ; A Get message to the serial port will wait for data,
    ; so 2nd parameter = 0 (don't use queue message)
    make p1 protanalyser(source, 0)
  ]
  make nmealine string(82 )
  make p2 protanalyser(nmealine, 1)
END


TO main
;    serial2.get(nmealine, 1)
    PRINT to source_text,
      " $GPRMC,123519,A,4807.038,N,01131.000,E,022.4,084.4,230394,003.1,W*6A", CR,
      "$GPRMC,161229.487,A,3723.2475,N,12158.3416,W,0.13,309.62,120598,,*10 ", CR,
      "$GPRMC,235947.000,V,0000.0000,N,00000.0000,E,,,041299,,*1D", CR,
      "$GPXYZ,123,456,789,12,23,34,4,5,*ff",CR,
      "$GPRMC,092204.999,A,4250.5589,S,14718.5084,E,0.00,89.68,211200,,*25" ,CR,
      "$GPRMC,092204.999,A,,,,,,,,,*3C",CR
    ; We use protanalyser p1 purely to isolate lines of text from the source
    ; Actually if we were only using a serial port we could bypass this and say
    ; serial2.Get(nmealine, 1)
    ;
    ; p1.get(nmealine) uses the default delimiter set, which is all control characters
    ; As the only control characters we are expecting are the CR LF at the end of the line,
    ; this will read one line at a time into nmealine
    while p1.get(nmealine)  ; get line to CRLF
    [
      PRINT CR, "input line: ", nmealine,CR
      nmeaproc
      p1.get('x', "") ; skip LF
    ]
END


; process 1 line of input
;
; In this example we only look at $GPRMC sentences and ignore others
;
; Typical format:
; $GPRMC,123519,A,4807.038,N,01131.000,E,022.4,084.4,230394,003.1,W*6A
;
; Where:
; $GP GPS data
; RMC Recommended Minimum sentence C
; 123519 Fix taken at 12:35:19 UTC
; A Status A=active or V=Void.
; 4807.038,N Latitude 48 deg 07.038' N
; 01131.000,E Longitude 11 deg 31.000' E
; 022.4 Speed over the ground in knots
; 084.4 Track angle in degrees True
; 230394 Date - 23rd of March 1994
; 003.1,W Magnetic Variation
; *6A The checksum data, always begins with *

TO nmeaproc
   AUTODESTRUCT
  local tok := new string (50)
  local pa := new protanalyser(tok, 1)
  if nmealine.find("$GPRMC") = 0
   ; process RMC sentence, ignore others
  [
    if nmeacheck(nmealine)  ; only process if checksum was correct
    [
      ; each comma separated token is read into the local variable tok
      ; in some cases protocol analyser pa is used to split this into
      ; numerical sub-parts
      p2.reset
      nmealine.reset
      p2.get(tok, ",,")  ; get and ignore the "$GPRMC" field
      p2.get(tok, ",,")
      pa.reset           ; we're going to split this into three x 2-digit numbers
      fix_hr := pa.get(10 , 2)
      fix_min := pa.get(10 , 2)
      fix_sec := pa.get(10 , 2)
      fix_ok := p2.get('s', ",,"). compare("A") = 0
      p2.get(tok, ",,")  ; latitude in format ddmm.mmm (dd = degrees, mm.mmm = minutes)
      pa.reset           ; use pa to split this into a 2 digit integer and
                          ; a float of 6 chars including d.p.
      latitude_ok := TRUE   ; assume OK until set false
      latitude_deg := pa.get(10 , 2)
      if NOT pa.valid latitude_ok := FALSE
      latitude_min := pa.get(1.0 , 6)
      if NOT pa.valid latitude_ok := FALSE
      ; return direction as a CSV string (always 1 char long),
       ; then take a copy of 1st char
      latitude_dir := p2.get('s', ",,").(0 )
      if NOT pa.valid latitude_ok := FALSE
      p2.get(tok, ",,")  ; longitude
      pa. reset           ; similar to latitude, but degrees are 3 digits
      longitude_ok := TRUE
      longitude_deg := pa.get(10 , 3)
      if NOT pa.valid [ PRINT "(1)" longitude_ok := FALSE ]
       longitude_min := pa.get(1.0 , 6)
      if NOT pa.valid [ PRINT "(2)" longitude_ok := FALSE ]
      ; get direction as a CSV string (always 1 char long),
       ; then take a copy of 1st char
      longitude_dir := p2.get('s', ",,").(0 )
      if NOT pa.valid [ PRINT "(3)" longitude_ok := FALSE ]
      ; don't bother with CSV: get next two float values direct
      ; we trust they won't be empty. If they might be, we should instead
      ; read a CSV string and take default action if it's empty, or take its
      ; value if not (see magnetic variation code below for example)
      groundspeed := p2.get(1.0 )
       trackangle := p2.get(1.0 )
       ; now we want to use CSV again, so skip over the last comma terminator
      ; otherwise CSV will see it as an empty field
;      p2.get             ; read and discard one char
      p2.get(tok, ",,")  ; date
      pa.reset           ; another split token DDMMYY
      date_day := pa.get(10 , 2)
      date_month := pa.get(10 , 2)
      date_year := pa.get(10 , 2)
      ; test next token for non-zero length because sometimes this value is omitted
      if p2.get(tok, ",,")  
      [
        magvar := tok.value(1.0)
        ; as with latitude direction, copy element from 1-char string
         magvar_dir := p2.get('s', ",,").(0 )
      ]
       ELSE  ; assume defaults
      [
        magvar := 0.0
         magvar_dir := 'x' ; neither E nor W: we could use this to indicate no data
      ]
      ; print the results
      PRINT "RMC fix on ", date_day:-2, "/", date_month:-2, "/", date_year:-2,
        " at ", fix_hr:2, ":", fix_min:2, ":", fix_sec:2, CR
      IF fix_ok PRINT " Valid", CR ELSE PRINT "*** Invalid Data ***",CR
      PRINT "latitude ", latitude_deg:1, " deg ", latitude_min:1: 3, " min ",
             CHR latitude_dir
       IF NOT latitude_ok PRINT " *** latitude data had empty values"
      PRINT CR
      PRINT "longitude ", longitude_deg:1, " deg ", longitude_min:1: 3, " min ",
             CHR longitude_dir
       IF NOT longitude_ok PRINT " *** longitude data had empty values"
      PRINT CR
      PRINT "Velocity ", groundspeed:5, " at ", trackangle:1:3, "deg", CR
       PRINT "magnetic variation ", magvar, " ", chr magvar_dir,CR
    ]
    else
       PRINT "CHECKSUM FAILED", CR
   ]
END


; verify checksum
; for testing this shows the values if the checksums do not agree
; NMEA sentence is $*
; where is the characters to be checksummed
; is 2 hex digits, = XOR of data characters
; return TRUE if given and computed checksums are the same
TO nmeacheck(nmealine)
  AUTODESTRUCT
   local c := 0
   local x := 0  ; checksum
  local xx := 0

  nmealine.reset
  nmealine.get  ; skip 1st "$"
  while c <> '*'        ; xor all the data characters
  [
    c := nmealine.get
    if c <> '*'         ; don't include the '*' in the chacksum
      x := x EOR c
  ]
  ; use the protanalyser to read the checksum in hex
  p2.reset
  xx := p2.get('h', 2)
  if x <> xx
    PRINT "checksum computed = ", ~x, "given = ", ~xx, CR
  return x = xx   ; TRUE if they match
END