sgslib 1.5.0 → 1.6.0

checksums.yaml

@@ -1,7 +1,7 @@
 ---
 SHA256:
-  metadata.gz: a7ecb05d8a8096186dd2d79b8126cc50e501e71d5b7c80a58f7b8bcc9a0f7ec0
-  data.tar.gz: c4d5073036b612bc4333ad2e176e58c6dae5c262d748a981eb55992526c0a29e
+  metadata.gz: 87786bab467aa776dcf6c87f887469ed917b43019d4863dd3be4249ff6fe299d
+  data.tar.gz: 3142d29756860081e97292373f37647d83f1a409f78ec6aad702dcf6d6484dd4
 SHA512:
-  metadata.gz: 891aa77f6714876a1c2ec5432b760aeab2a7a9421c2e97713dac0ad618f068961142d06020a24eec586222f3ddc4762e1e549461152fa3ab32d18b93b1f046b9
-  data.tar.gz: 03d3070b475d819e0f1f855ba6a83fb83596b4c2900a4bb201348570ca9b2e7e5769ab5c9a7f00a3d71c1b31cac12023d362224dd190af01a46695b20df43f7d
+  metadata.gz: 120c638f13a6cb70844ec022fef4366c23b19d51643179bd5f1805fe75fbff11167db8338703c0f0a48a75ab77f6d76589b5ddd9d6f26a28fc03fca45e9beacf
+  data.tar.gz: 67c4abc829bf6c3a0bbdc75de66b12bf797de3b348103db654300edd08eb7fb098b3b66fe3e571fe250da1546d04b4012b5b8e7199780988024c39b1a6eb0a7a

data/lib/sgs/alarm.rb

@@ -44,7 +44,9 @@ module SGS
   class Alarm < RedisBase
     attr_accessor :last_report, :time
 
-    OTTO_RESTART = 0
+    #
+    # Alarms generated by Otto.
+    MISSION_SWITCH = 0
     RUDDSRV_FAULT = 1
     SAILSRV_FAULT = 2
     VBATT_CRITICAL = 3
@@ -60,16 +62,18 @@ module SGS
     RUDDER_NOZERO = 13
     SAIL_NOZERO = 14
     MOTHER_UNRESP = 15
-
-    MISSION_COMMENCE = 16
-    MISSION_COMPLETE = 17
-    MISSION_ABORT = 18
-    WAYPOINT_REACHED = 19
-    CROSS_TRACK_ERROR = 20
-    INSIDE_FENCE = 21
+    #
+    # Alarms generated by Mother.
+    OTTO_RESTART = 16
+    MISSION_COMMENCE = 17
+    MISSION_COMPLETE = 18
+    MISSION_ABORT = 19
+    WAYPOINT_REACHED = 20
+    CROSS_TRACK_ERROR = 21
+    INSIDE_FENCE = 22
 
     ALARM_NAMES = [
-      "OTTO Restarted",
+      "Mission Activation Switch",
       "Rudder Servo Fault",
       "Sail Servo Fault",
       "Battery voltage is critically low",
@@ -85,6 +89,7 @@ module SGS
       "Cannot zero the rudder position",
       "Cannot zero the sail position",
       "Mother is unresponsive",
+      "OTTO Restarted",
       "Mission has commenced",
       "Mission is completed",
       "*** MISSION ABORT ***",
@@ -103,8 +108,13 @@ module SGS
     #
     # Main daemon function (called from executable)
     def self.daemon
+      puts  "Alarm daemon starting up..."
+      otto = RPCClient.new(:otto)
       loop do
-        sleep 300
+        #puts "Check for any alarms..."
+        #resp = otto.command "A?"
+        #puts "Response: #{resp}"
+        sleep 30
       end
     end
 
@@ -116,7 +126,7 @@ module SGS
         f.puts "/*\n * Autogenerated by #{__FILE__}.\n * DO NOT HAND-EDIT!\n */"
         constants.sort.each do |c|
           unless c == :ALARM_NAMES
-            cval = SGS::Alarm.const_get(c)
+            cval = Alarm.const_get(c)
             str = "#define SGS_ALARM_#{c.to_s}"
             str += "\t" if str.length < 32
             str += "\t#{cval}\t/* #{alarm.name(cval)} */"

data/lib/sgs/bearing.rb

@@ -1,4 +1,5 @@
 #
+#
 # Copyright (c) 2013-2023, Kalopa Robotics Limited.  All rights
 # reserved.
 #
@@ -48,7 +49,8 @@ module SGS
     attr_accessor :distance
 
     #
-    # Create the Bearing instance.
+    # Create the Bearing instance. Angle is in radians, distance in nautical
+    # miles.
     def initialize(angle = 0.0, distance = 0.0)
       self.angle = angle.to_f
       self.distance = distance.to_f
@@ -72,6 +74,23 @@ module SGS
       rad.to_f * 180.0 / Math::PI
     end
 
+    #
+    # Convert boat angle (0->255) to radians. The boat uses an 8bit quantity
+    # to represent an angle, where 256 maps to 360 degrees. This makes angle
+    # arithmetic quite simple on an 8 bit processor, but useless for
+    # something which has a proper FPU. These two helper functions convert
+    # to and from radians.
+    def self.xtor(val)
+      val &= 0xff
+      val.to_f * Math::PI / 128.0
+    end
+
+    #
+    # Convert radians to hex-degrees.
+    def self.rtox(rad)
+      (rad.to_f * 128.0 / Math::PI).round.to_i & 0xff
+    end
+
     #
     # Handy function to re-adjust an angle away from negative
     def self.absolute(angle)
@@ -110,7 +129,7 @@ module SGS
       sin_dlon = Math.sin(loc2.longitude - loc1.longitude)
       cos_dlon = Math.cos(loc2.longitude - loc1.longitude)
       bearing.distance = Math.acos(sin_lat1*sin_lat2 + cos_lat1*cos_lat2*cos_dlon) *
-                                SGS::EARTH_RADIUS
+                                EARTH_RADIUS
       y = sin_dlon * cos_lat2
       x = cos_lat1 * sin_lat2 - sin_lat1 * cos_lat2 * cos_dlon
       bearing.angle = Math.atan2(y, x)
@@ -141,10 +160,20 @@ module SGS
       Bearing.absolute(@angle - Math::PI)
     end
 
+    #
+    # Return the distance in metres
+    def distance_m
+      @distance * 1852.0
+    end
+
     #
     # Convert to a string
     def to_s
-      "BRNG %03dd,%.3fnm" % [angle_d, @distance]
+      if @distance > 0.9
+        "BRNG %03dd,%.3fNM" % [angle_d, @distance]
+      else
+        "BRNG %03dd,%.1fm" % [angle_d, distance_m]
+      end
     end
   end
 end

data/lib/sgs/config.rb

@@ -56,7 +56,6 @@ module SGS
       GPS.setup
       Otto.setup
       Timing.setup
-      Waypoint.setup
     end
   end
 end

data/lib/sgs/course.rb

@@ -149,11 +149,21 @@ module SGS
     end
 
     #
-    # Compute a relative VMG based on the waypoint
+    # Compute a relative VMG based on the waypoint. If you're computing
+    # angles to one waypoint, the relative VMG will allow you to compare
+    # against each possible angle. If you're computing to different
+    # waypoints, then the distance from your position to that waypoint is
+    # taken into consideration so that waypoints further away have less
+    # impact on your VMG choice.
     def relative_vmg(waypt)
-      relvmg = @speed * Math::cos(waypt.bearing.angle - @heading) / waypt.distance
-      puts "Relative VMG to WPT: #{waypt.name} is #{relvmg}"
-      relvmg
+      @speed * Math::cos(waypt.bearing.angle - @heading) / waypt.distance
+    end
+
+    #
+    # Compute the wind (as a Bearing) based on the compass heading and the
+    # apparent wind as reported.
+    def compute_wind
+      @wind.angle = @heading + @awa
     end
 
     #
@@ -161,7 +171,10 @@ module SGS
     # fast the boat will travel at the particular apparent wind angle.
     def compute_speed
       awa = @awa.abs
-      return 0.0 if awa < 0.75
+      if awa < 0.75
+        @speed = 0.0
+        return
+      end
       ap = 1.0
       @speed = 0.0
       @polar_curve.each do |poly_val|

data/lib/sgs/diagnostics.rb

@@ -37,10 +37,11 @@
 # Routines for handling sailboat navigation and route planning.
 #
 module SGS
-  class Diagnostics < RedisBase
+  class Diagnostics
     #
     # Main daemon function (called from executable)
     def self.daemon
+      puts "Diagnostics subsystem starting up..."
       loop do
         sleep 300
       end

data/lib/sgs/gps.rb

@@ -32,10 +32,14 @@
 #
 # ABSTRACT
 #
+require 'serialport'
+
 module SGS
   class GPS < RedisBase
-    attr_accessor :time, :location, :sog, :cmg, :magvar
+    attr_accessor :time, :location, :sog, :cmg, :magvar, :val
 
+    #
+    # Create a new GPS record. Lat/Long are in radians.
     def initialize(lat = nil, long = nil)
       @time = Time.new(2000, 1, 1)
       @location = Location.new(lat, long)
@@ -49,11 +53,30 @@ module SGS
     #
     # Main daemon function (called from executable)
     def self.daemon
+      puts "GPS reader starting up..."
+      config = Config.load
+
+      sp = SerialPort.new config.gps_device, config.gps_speed
+      sp.read_timeout = 10000
+
       loop do
-        sleep 300
+        nmea = NMEA.parse sp.readline
+        if nmea.is_gprmc?
+          gps = nmea.parse_gprmc
+          p gps
+          gps.save_and_publish if gps and gps.valid?
+        end
       end
     end
 
+    #
+    # Hard-code a GPS value (usually for debugging purposes)
+    def force(lat, long, time = nil)
+      @time = time || Time.now
+      @location = Location.new(lat, long)
+      @valid = true
+    end
+
     #
     # Set the validity
     def is_valid
@@ -65,5 +88,15 @@ module SGS
     def valid?
       @valid == true
     end
+
+    #
+    # Display the GPS data as a useful string (in degrees)
+    def to_s
+      if valid?
+        "@#{@time.strftime('%Y%m%d-%T')}, #{@location}, SOG:#{@sog}, CMG:#{@cmg}"
+      else
+        "GPS error"
+      end
+    end
   end
 end

data/lib/sgs/location.rb

@@ -56,7 +56,7 @@ module SGS
     attr_accessor :latitude, :longitude
 
     #
-    # Create the Location instance.
+    # Create the Location instance. Latitude and longitude passed in radians.
     def initialize(lat = nil, long = nil)
       @latitude = lat.to_f if lat
       @longitude = long.to_f if long
@@ -65,7 +65,6 @@ module SGS
     #
     # The difference between two locations is a Bearing
     def -(loc)
-      puts "Distance from #{self} to #{loc}"
       Bearing.compute(self, loc)
     end
 
@@ -84,8 +83,8 @@ module SGS
       loc = Location.new
       sin_angle = Math.sin(bearing.angle)
       cos_angle = Math.cos(bearing.angle)
-      sin_dstr = Math.sin(bearing.distance / SGS::EARTH_RADIUS)
-      cos_dstr = Math.cos(bearing.distance / SGS::EARTH_RADIUS)
+      sin_dstr = Math.sin(bearing.distance / EARTH_RADIUS)
+      cos_dstr = Math.cos(bearing.distance / EARTH_RADIUS)
       sin_lat1 = Math.sin(@latitude)
       cos_lat1 = Math.cos(@latitude)
       loc.latitude = Math.asin(sin_lat1*cos_dstr + cos_lat1*sin_dstr*cos_angle)
@@ -113,10 +112,33 @@ module SGS
     end
 
     #
-    # Set the lat/long from a hash.
+    # Parse the lat/long values passed as a string. This function
+    # should be able to handle any type of lat/long string, in most
+    # general formats. See :to_s for examples.
     def parse(data)
-      @latitude = ll_parse(data["latitude"], "NS")
-      @longitude = ll_parse(data["longitude"], "EW")
+      llvals = data.split /,/
+      if llvals.count == 1
+        #
+        # Must be space-separated. Try that...
+        llvals = data.split(/ /)
+        if llvals.count != 2
+          raise ArgumentError.new "Cannot split lat/long values"
+        end
+      elsif llvals.count != 2
+        #
+        # Too many comma separators.
+        raise ArgumentError.new "Invalid lat/long values"
+      end
+      self.latitude_d = _ll_parse(llvals[0], "NS")
+      self.longitude_d = _ll_parse(llvals[1], "EW")
+      true
+    end
+
+    #
+    # Parse the lat/long from a hash object.
+    def parse_hash(data = {})
+      self.latitude_d = _ll_parse(data["latitude"], "NS")
+      self.longitude_d = _ll_parse(data["longitude"], "EW")
     end
 
     #
@@ -128,15 +150,21 @@ module SGS
     #
     # Convert the lat/long to a hash.
     def to_hash
-      {"latitude" => ll_to_s(latitude, "NS"),
-        "longitude" => ll_to_s(longitude, "EW")}
+      {"latitude" => latitude_d.round(6), "longitude" => longitude_d.round(6)}
     end
 
     #
-    # Display the lat/long as a useful string (in degrees).
-    def to_s
+    # Display the lat/long as a useful string (in degrees). Output
+    # formats are as follows (default is :d):
+    # :d    "48.104051, -7.282614"
+    # :dd   "48.104051N, 7.282614W"
+    # :dmm  "48 6.243060N, 7 16.956840W"
+    # :dms  "48 6 14.583600N, 7 16 57.410400W"
+    def to_s(opts = {})
       if valid?
-        "%s, %s" % [ll_to_s(@latitude, "NS"), ll_to_s(@longitude, "EW")]
+        lat_str = _ll_conv(latitude_d.round(6), "NS", opts)
+        lon_str = _ll_conv(longitude_d.round(6), "EW", opts)
+        "#{lat_str}, #{lon_str}"
       else
         "unknown"
       end
@@ -151,29 +179,39 @@ module SGS
     end
 
     #
-    # Helper functions for working in degrees.
+    # Return the latitude in degrees.
     def latitude_d
       Bearing.rtod @latitude
     end
 
+    #
+    # Set the latitude using a value in degrees.
     def latitude_d=(val)
       @latitude = Bearing.dtor val
     end
 
+    #
+    # Produce a latitude array for NMEA output.
     def latitude_array(fmt = nil)
-      make_ll_array latitude_d, "NS", fmt
+      _make_ll_array latitude_d, "NS", fmt
     end
 
+    #
+    # Return the longitude in degrees.
     def longitude_d
       Bearing.rtod @longitude
     end
 
+    #
+    # Set the longitude using a value in degrees.
     def longitude_d=(val)
       @longitude = Bearing.dtor val
     end
 
+    #
+    # Produce a longitude array for NMEA output.
     def longitude_array(fmt = nil)
-      make_ll_array longitude_d, "EW", fmt
+      _make_ll_array longitude_d, "EW", fmt
     end
 
     #
@@ -182,53 +220,71 @@ module SGS
       Bearing.compute(self, loc)
     end
 
-  private
-    #
-    # Parse a string into a lat or long. The latitude or longitude string
-    # should be of the format dd mm ss.sssssC (where C is NS or EW). Note
-    # that latitude and longitude are saved internally in radians.
-    def ll_parse(value, nsew)
-      args = value.split
-      dir = args[-1].gsub(/[\d\. ]+/, '').upcase
-      args.map! {|val| val.to_f}
-      val = args.shift
-      val = val + args.shift / 60.0 if args.length > 0
-      val = val + args.shift / 3600.0 if args.length > 0
-      Bearing.dtor val * ((nsew.index(dir) == 1) ? -1 : 1)
-    end
+    private
+      #
+      # Parse a latitude or longitude value, from a wide range of
+      # formats. Should handle D.ddd, D M.mmm and D M S.sss values.
+      # Can also strip out the special degrees unicode, as well as
+      # single and double quotes.
+      def _ll_parse(arg, nsew)
+        str = arg.chomp.gsub /[\u00B0'"]/, ' '
+        if str[-1].upcase =~ /[#{nsew}]/
+          sign = (str[-1].upcase == nsew[1]) ? -1 : 1
+          str[-1] = ' '
+        else
+          sign = 1
+        end
+        args = str.split
+        raise ArgumentError.new "Cannot parse lat/long value" if args.count > 3
+        value = 0.0
+        (args.count - 1).downto(0).each do |idx|
+          value = args[idx].to_f + value / 60.0
+        end
+        sign * value
+      end
 
-    #
-    # Convert a latitude or longitude into an ASCII string of the form:
-    # dd mm ss.ssssssC (where C is NS or EW). The value should be in
-    # radians.
-    def ll_to_s(val, str)
-      val = Bearing.rtod val
-      if val < 0.0
-        chr = str[1]
-        val = -val
-      else
-        chr = str[0]
+      #
+      # Convert a latitude/longitude to a string. Can specify
+      # multiple output formats such as :d, :dd, :dmm, :dms to
+      # format according to various different styles.
+      def _ll_conv(value, nsew, opts)
+        format = opts[:format] || :d
+        return "%.6f" % value if format == :d
+        if value < 0.0
+          suffix = nsew[1]
+          value = -value
+        else
+          suffix = nsew[0]
+        end
+        case opts[:format] || :d
+        when :dd
+          "%.6f%s" % [value, suffix]
+        when :dmm
+          dd = value.to_i
+          mm = (value - dd.to_f) * 60.0
+          "%d %.6f%s" % [dd, mm, suffix]
+        when :dms
+          dd = value.to_i
+          value = (value - dd.to_f) * 60.0
+          mm = value.to_i
+          ss = (value - mm.to_f) * 60.0
+          "%d %d %.6f%s" % [dd, mm, ss, suffix]
+        end
       end
-      deg = val.to_i
-      val = (val - deg.to_f) * 60.0
-      min = val.to_i
-      sec = (val - min.to_f) * 60.0
-      "%d %d %8.6f%c" % [deg, min, sec, chr]
-    end
 
-    #
-    # Create a Lat/Long array suitable for an NMEA output
-    def make_ll_array(val, nsew, fmt = nil)
-      fmt ||= "%02d%07.4f"
-      if (val < 0)
-        val = -val
-        ne = nsew[1]
-      else
-        ne = nsew[0]
+      #
+      # Create a Lat/Long array suitable for an NMEA output.
+      def _make_ll_array(val, nsew, fmt = nil)
+        fmt ||= "%02d%07.4f"
+        if (val < 0)
+          val = -val
+          suffix = nsew[1]
+        else
+          suffix = nsew[0]
+        end
+        deg = val.to_i
+        val = (val - deg) * 60
+        [fmt % [deg, val], suffix.chr]
       end
-      deg = val.to_i
-      val = (val - deg) * 60
-      [fmt % [deg, val], ne.chr]
-    end
   end
 end

data/lib/sgs/logger.rb

@@ -37,13 +37,6 @@
 # Routines for handling sailboat navigation and route planning.
 #
 module SGS
-  class Logger < RedisBase
-    #
-    # Main daemon function (called from executable)
-    def self.daemon
-      loop do
-        sleep 300
-      end
-    end
+  class Logger
   end
 end