Home | Ruby | GPS Activities |  

#!/usr/bin/ruby -w

=begin
/***************************************************************************
 *   Copyright (C) 2008, Paul Lutus                                        *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/
=end

DEBUG = false

PVERSION = 1.2

=begin

Input/output file format for GarminReadWrite is a
plain-text comma-separated-value database:

latitude,longitude,altitude,name,description

1. No header in the data file.
2. Longitude and latitude are expressed in decimal degrees.
3. Altitude is expressed in meters.

Be sure to set GPS unit to garmin/garmin mode

=end

class GarminReadWrite

   def initialize()
      @garmin_unit_type = -1
      @garmin_serial = nil
      @read_mode = true
      @file_mode = false
      @windows = (PLATFORM =~ /mswin/)
      @baud_rate = 9600 # standard garmin mode baud rate
      # default serial port, can be changed on command line
      @ser_port = (@windows)?"COM1:":"/dev/ttyUSB0"
      at_exit { do_at_exit() }
   end # initialize

   def do_at_exit()
      @garmin_serial.close if @garmin_serial
   end # do_at_exit

   def setup()
      unless @garmin_serial # unless already set up
         unless FileTest.exist? @ser_port
            puts "Error: input port #{@ser_port} doesn't exist, quitting."
            exit 0
         end

         @ser_config = (@windows)? \
         "MODE #{@ser_port} baud=#{@baud_rate} parity=n data=8 stop=1" \
         :"stty -F #{@ser_port} #{@baud_rate} raw -echo"

         # configure the serial port
         `#{@ser_config}`

         # open it for reading and writing
         @garmin_serial = File.open(@ser_port,"rb+")

         # find out which Garmin unit
         str,id = get_garmin_id()
         case str.downcase
         when /gps iii/
            @garmin_unit_type = 1
         when /gps 5/
            @garmin_unit_type = 2
         when /gpsmap76/
            @garmin_unit_type = 2
         else # let's assume unknown units are new
            @garmin_unit_type = 2
         end
      end # unless port already opened
   end # setup

   def get_garmin_id()
      setup()
      # alert garmin: send ID
      send_garmin(0xfe,[0x00,0x02])
      data,type = read_garmin()
      id = data[2,2].pack('C*').unpack('s')[0]
      puts "ID number: #{id}" if DEBUG
      idstr = extract_strings(data,4)[0]
      puts "ID String: #{idstr}" if DEBUG
      return idstr,id
   end # get garmin id

   def debug_print(s)
      print s if DEBUG
   end

   def send_garmin_char(b,ck = 0)
      b &= 0xff;
      ck += b
      debug_print sprintf("%02x ",b)
      @garmin_serial.syswrite(b.chr)
      return ck
   end # send garmin char

   # garmin requires a double send of 0x10
   # for all data except the initial 0x10
   # and the end mark 0x1003

   def send_garmin_char_escaped(b,ck = 0)
      ck = send_garmin_char(b,ck)
      send_garmin_char(b) if(b == 0x10) # send 0x10 twice
      return ck
   end # send garmin char escaped

   def recv_garmin_char()
      b = @garmin_serial.sysread(1)[0]
      debug_print sprintf("%02x ",b)
      return b
   end # recv garmin char

=begin

read_garmin state machine:

state  explanation
---------------------------
0      start of message
1      type of message
2      data length (always 1 byte)
3      data
4      checksum
5      end mark

=end

   def read_garmin()
      debug_print "read_garmin: "
      state = 0
      type = -1
      length = -1
      data = []
      datacount = -1
      prior = -1
      cksum = -1
      cksum_valid = false
      begin
         c = recv_garmin_char()
         case state
         when 0 # start
            # skip bytes until 0x10 seen
            if(c == 0x10)
               state += 1
            end
         when 1 # type byte
            type = c
            state += 1
            prior = 0
            cksum = c # start checksum here
         when 2 # length byte
            if((c != 0x10) || (prior == 0x10))
               length = c
               datacount = 0
               cksum += c
               state += 1
            end
         when 3 # data block
            if((c != 0x10) || (prior == 0x10))
               data << c
               cksum += c
               datacount += 1
               state += 1 if(datacount == length)
            end
         when 4 # checksum byte
            if((c != 0x10) || (prior == 0x10))
               cksum = 0x100 - (cksum & 0xff)
               cksum_valid = (cksum == c)
               state += 1
            end
         when 5 # first byte of end mark
            if(c == 0x10)
               state += 1
            end
         when 6 # second byte of end mark
            if(c == 0x03)
               state += 1
            end
         end
         prior = c
      end while state < 7
      debug_print "checksum: #{cksum_valid}\n"
      if(cksum_valid)
         # don't ACK the other side's ACK
         send_garmin_ack(type) unless(type == 0x06)
         return data,type
      else
         # error condition: checksums don't match
         puts "checksum error."
         return nil
      end
   end # read garmin

   def read_garmin_ack()
      data,type = read_garmin()
      return type == 0x06
   end # read garmin ack

   def send_garmin(type,data)
      debug_print "send_garmin: "
      send_garmin_char(0x10)
      cksum = send_garmin_char(type)
      cksum = send_garmin_char_escaped(data.size,cksum)
      data.each do |b|
         cksum = send_garmin_char_escaped(b,cksum)
      end
      send_garmin_char_escaped(0x100 - (cksum & 0xff))
      send_garmin_char(0x10)
      send_garmin_char(0x03)
      debug_print "\n"
      # don't expect ACK for ACK
      read_garmin_ack() unless type == 0x06
   end # send garmin

   def send_garmin_ack(type)
      send_garmin(0x06,[type,0])
   end # send garmin ack

   def convert_int_bytes_to_float(data,n,res=5)
      # create int, convert
      v = data[n,4].pack('C*').unpack('i')[0]
      # from internal representation to radians
      v /= 683565275.576431632
      # radians to degrees
      v *= 180.0/Math::PI
      # specify decimal places
      s = sprintf("%.#{res}f",v)
      return s
   end # convert int bytes to float

   def convert_float_to_int_bytes(v)
      # to radians
      v *= Math::PI/180.0
      # to internal representation
      v *= 683565275.576431632
      # to byte array
      return [v.to_i].pack('i').unpack('C*')
   end # convert float to int bytes

   def convert_float_bytes_to_float(data,n,res=5)
      v = data[n,4].pack('C*').unpack('e')[0]
      # specify decimal places
      s = sprintf("%.#{res}f",v)
      return s
   end # convert float bytes to float

   def convert_float_to_float_bytes(v)
      return [v].pack('e').unpack('C*')
   end # convert float to float bytes

   # split string data on zero terminations

   def extract_strings(data,n)
      return data[n..-1].pack('C*').split(/\0+/)
   end # extract strings

   # remove leading and trailing whitesoace

   def trim(s)
      return s.sub(%r{^\s*(.*?)\s*$},"\\1")
   end

   # for Garmin units before "GPS V"

   def decode_old_waypoint(data)
      # waypoint ID string starts at block position 0
      name = trim(data[0...6].pack('C*'))
      # lat (int) begins at block position 6
      lat = convert_int_bytes_to_float(data,6)
      # lng (int) begins at block position 10
      lng = convert_int_bytes_to_float(data,10)
      # alt (short float) begins at block position 28
      alt = 0 # no altitude from the older units
      # name and desc, zero-terminated strings,
      # start at block position 18
      desc = extract_strings(data,18)[0]
      return "#{lat},#{lng},#{alt},#{name},#{desc}"
   end # decode old waypoint data

   # for Garmin units "GPS V" and newer

   def decode_new_waypoint(data)
      # lat (int) begins at block position 24
      lat = convert_int_bytes_to_float(data,24)
      # lng (int) begins at block position 28
      lng = convert_int_bytes_to_float(data,28)
      # alt (short float) begins at block position 28
      alt = convert_float_bytes_to_float(data,32)
      # name and desc, zero-terminated strings,
      # start at block position 52
      name,desc = extract_strings(data,52)
      return "#{lat},#{lng},#{alt},#{name},#{desc}"
   end # decode new waypoint data

   def read_garmin_waypoint_set()
      setup()
      output = []
      send_garmin(0x0a,[0x07,0])
      begin
         data,type = read_garmin()
         if(type == 0x23)
            if(@garmin_unit_type == 1)
               output << decode_old_waypoint(data)
            else
               output << decode_new_waypoint(data)
            end
         end
      end while type != 0x0c
      return output.join("\n") + "\n"
   end # read garmin waypoint set

   def create_old_garmin_block(s_lat,s_lng,s_alt,name,desc)
      block = []
      ns = name[0,6]
      while (ns.length < 6)
         ns += ' '
      end
      block << ns.unpack('C*')
      block << convert_float_to_int_bytes(s_lat.to_f)
      block << convert_float_to_int_bytes(s_lng.to_f)
      block << [0,0,0,0] # bogus date/time
      block << desc.unpack('C*') << 0 # string with terminating zero
      block.flatten!
      return block
   end

   def create_new_garmin_block(s_lat,s_lng,s_alt,name,desc)
      block = []
      # unknown block
      block << [0x01,0x00,0x1f,0x70,0x12,0x00,0x00,0x00,0x00,0x00,0x00,0x00]
      # unknown block of 12 0xff
      1.upto(12) do
         block << 0xff
      end
      block << convert_float_to_int_bytes(s_lat.to_f)
      block << convert_float_to_int_bytes(s_lng.to_f)
      block << convert_float_to_float_bytes(s_alt.to_f)
      # the value "0x51,0x59,0x04,0x69" is the default
      # and creates a data-unavailable display
      block << [ 0x51,0x59,0x04,0x69 ] # depth, unused, float
      block << [ 0x51,0x59,0x04,0x69 ] # proximity distance, unused, float
      block << [ 0x20,0x20,0x20,0x20 ] # unknown
      block << [ 0xff,0xff,0xff,0xff ] # unknown
      block << name.unpack('C*') << 0 # string with terminating zero
      block << desc.unpack('C*') << 0 # string with terminating zero
      block.flatten!
      return block
   end

   def write_garmin_waypoint_set(stream)
      setup()
      array = stream.readlines
      # must know count in advance
      count = array.size
      n = 0
      msb = count/256
      lsb = count % 256
      # alert garmin: sending (count) records
      send_garmin(0x1b,[lsb,msb])
      array.each do |record|
         n += 1
         # clean up record leading and trailing whitespace
         record = record.sub(%r{\s*(.*?)\s*},"\\1");
         if(record.length > 0)
            fields = record.split(",")
            if(fields.size != 5)
               puts "Error: record #{n} doesn't have 5 fields: \"#{record}\", quitting."
               return;
            end
            s_lat,s_lng,s_alt,name,desc = fields
            debug_print "#{s_lat}|#{s_lng}|#{s_alt}|#{name}|#{desc}\n"
            # build a waypoint data block
            if(@garmin_unit_type == 1)
               block = create_old_garmin_block(s_lat,s_lng,s_alt,name,desc)
            else
               block = create_new_garmin_block(s_lat,s_lng,s_alt,name,desc)
            end
            # send garmin waypoint block
            send_garmin(0x23,block)
         end # if record.length > 0
      end # each record
      # send garmin end-of-records frame
      send_garmin(0x0c,[0x23,0])
   end # write garmin waypoint

   def read_write_file(fn)
      if(@read_mode)
         data = read_garmin_waypoint_set()
         File.open(fn,"w") { |f| f.write data }
      else
         fh = File.open(fn,"r")
         write_garmin_waypoint_set(fh)
         fh.close
      end
   end

   def process(args)
      if(!args[0])
         puts "usage: -p(ort), default #{@ser_port}"
         puts "       -r(read from Garmin) [filename]"
         puts "       -w(write to Garmin) [filename(s)]"
         puts "       if no filename given,"
         puts "       data will be read/written from stdin/stdout"
      else
         while(arg = args.shift)
            case arg.downcase
            when /^-p/
               @ser_port = args.shift
            when /^-r/
               @read_mode = true
            when /^-w/
               @read_mode = false
            else
               @file_mode = true
               read_write_file(arg)
            end
         end
         unless(@file_mode)
            if(@read_mode)
               print read_garmin_waypoint_set()
            else
               write_garmin_waypoint_set($stdin)
            end
         end # default stdin/stdout mode
      end
   end # process

end # class GarminReadWrite

grw = GarminReadWrite.new
grw.process(ARGV)

Home | Ruby | GPS Activities |