sgslib 1.5.1 → 1.7.1

data/lib/sgs/mission.rb

@@ -32,6 +32,10 @@
 # ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #
 # ABSTRACT
+# Mostly this class is for the overall daemon running on Mother. It takes
+# care of listening for GPS updates and adjusting the sailing route,
+# accordingly. It is responsible for keeping MissionStatus up to
+# date. The actual mission information is stored in a YAML file.
 #
 
 ##
@@ -45,37 +49,11 @@ module SGS
   class Mission
     attr_accessor :title, :url, :description
     attr_accessor :launch_site, :launch_location
-    attr_accessor :attractors, :repellors, :track
-    attr_accessor :where, :time, :course, :distance
-
-    #
-    # Main daemon function (called from executable)
-    def self.daemon
-      puts "Mission management system starting up..."
-      loop do
-        sleep 300
-      end
-    end
-
-    #
-    # Load a new mission from the missions directory.
-    def self.file_load(filename)
-      parse YAML.load(File.open(filename))
-    end
-
-    #
-    # Load a new mission from the missions directory.
-    def self.parse(data)
-      mission = new
-      mission.parse(data)
-      mission
-    end
+    attr_accessor :attractors, :repellors, :status
 
     #
     # Create the attractors and repellors as well as the track array
-    # and other items. @where is our current TrackPoint, @current_wpt is
-    # the waypoint we're working (-1 if none), @course is the heading/speed
-    # the boat is on.
+    # and other items.
     def initialize
       @title = nil
       @url = nil
@@ -84,166 +62,60 @@ module SGS
       @launch_location = nil
       @attractors = []
       @repellors = []
-      @track = nil
-      @start_time = @time = nil
-      @where = nil
-      @course = Course.new
-      @distance = 0
-      @swing = 60
+      @status = MissionStatus.load
+      super
     end
 
     #
-    # Compute the best heading based on our current position and the position
-    # of the current attractor. This is where the heavy-lifting happens
-    def navigate
-      return unless active?
-      puts "Attempting to navigate to #{waypoint}"
-      #
-      # First off, compute distance and bearing from our current location
-      # to every attractor and repellor.
-      @attractors[@current_wpt..-1].each do |waypt|
-        waypt.compute_bearing(@where)
-        puts "Angle: #{waypt.bearing.angle_d}, Distance: #{waypt.bearing.distance} (adj:#{waypt.distance})"
-      end
-      @repellors.each do |waypt|
-        waypt.compute_bearing(@where)
-        puts "Angle: #{waypt.bearing.angle_d}, Distance: #{waypt.bearing.distance} (adj:#{waypt.distance})"
-      end
+    # Main daemon function (called from executable)
+    def self.daemon
+      puts "Mission management system starting up..."
       #
-      # Right. Now look to see if we've achieved the current waypoint and
-      # adjust, accordingly
-      while active? and reached? do
-        next_waypoint!
-      end
-      return unless active?
-      puts "Angle to next waypoint: #{waypoint.bearing.angle_d}d"
-      puts "Adjusted distance to waypoint is #{@distance}"
+      # Load the mission data from Redis and augment it with the
+      # contents of the mission file.
+      config = Config.load
+      mission = Mission.file_load config.mission_file
+      nav = Navigate.new(mission)
+      otto = Otto.load
       #
-      # Now, start the vector field analysis by examining headings either side
-      # of the bearing to the waypoint.
-      best_course = @course
-      best_relvmg = 0.0
-      puts "Currently on a #{@course.tack_name} tack (heading is #{@course.heading_d} degrees)"
-      (-@swing..@swing).each do |alpha_d|
-        puts ">> Computing swing of #{alpha_d} degrees"
-        new_course = Course.new(@course.wind)
-        new_course.heading = waypoint.bearing.angle + Bearing.dtor(alpha_d)
-        #
-        # Ignore head-to-wind cases, as they're pointless.
-        next if new_course.speed < 0.001
-        puts "AWA:#{new_course.awa_d}, heading:#{new_course.heading_d}, speed:#{new_course.speed}"
-        relvmg = 0.0
-        relvmg = new_course.relative_vmg(@attractors[@current_wpt])
-        @attractors[@current_wpt..-1].each do |waypt|
-          relvmg += new_course.relative_vmg(waypt)
-        end
-        @repellors.each do |waypt|
-          relvmg -= new_course.relative_vmg(waypt)
-        end
-        relvmg *= 0.1 if new_course.tack != @course.tack
-        puts "Relative VMG: #{relvmg}"
-        if relvmg > best_relvmg
-          puts "Best heading (so far)"
-          best_relvmg = relvmg
-          best_course = new_course
+      # Keep running in our mission state, forever.
+      while true do
+        if mission.status.active?
+          #
+          # Listen for GPS data. When we have a new position, call the
+          # navigation code to determine a new course to sail (currently
+          # a compass course), and set the Otto register accordingly.
+          # Repeat until we run out of waypoints.
+          GPS.subscribe do |count|
+            puts "Mission received new GPS count: #{count}"
+            new_course = nav.navigate
+            if new_course.nil?
+              mission.status.completed!
+              break
+            end
+            mission.status.save
+            compass = Bearing.rtox(new_course.heading)
+            otto.set_register(Otto::COMPASS_HEADING_REGISTER, compass)
+          end
+        else
+          sleep 60
+          mission.status.load
         end
       end
-      puts "Best RELVMG: #{best_relvmg}"
-      puts "TACKING!" if best_course.tack != @course.tack
-      puts "New HDG: #{best_course.heading_d} (AWA:#{best_course.awa_d}), WPT:#{waypoint.name}"
-      @course = best_course
     end
 
     #
-    # Set new position
-    def set_position(time, loc)
-      @where = loc
-      @time = time
-      @track << TrackPoint.new(@time, @where)
-    end
-
-    #
-    # Advance the mission by a number of seconds (computing the new location
-    # in the process). Fake out the speed and thus the location.
-    def simulated_movement(how_long = 60)
-      puts "Advancing mission by #{how_long}s"
-      distance = @course.speed * how_long.to_f / 3600.0
-      puts "Travelled #{distance * 1852.0} metres in that time."
-      set_position(@time + how_long, @where + Bearing.new(@course.heading, distance))
-    end
-
-    #
-    # How long has the mission been active?
-    def elapsed
-      @time - @start_time
-    end
-
-    #
-    # Return the current waypoint.
-    def waypoint
-     #@attractors[@current_wpt] : nil
-    end
-
-    #
-    # Have we reached the waypoint? Note that even though the waypoints have
-    # a "reached" circle, we discard the last 10m on the basis that it is
-    # within the GPS error.
-    def reached?
-      @distance = @attractors[@current_wpt].distance
-      puts "ARE WE THERE YET? (dist=#{@distance})"
-      return true if @distance <= 0.0027
-      #
-      # Check to see if the next WPT is nearer than the current one
-      #if @current_wpt < (@attractors.count - 1)
-      #  next_wpt = @attractors[@current_wpt + 1]
-      #  brng = @attractors[@current_wpt].location - next_wpt.location
-      #  angle = Bearing.absolute(waypoint.bearing.angle - next_wpt.bearing.angle)
-      #  return true if brng.distance > next_wpt.distance and
-      #                 angle > (0.25 * Math::PI) and
-      #                 angle < (0.75 * Math::PI)
-      #end
-      puts "... Sadly, no."
-      return false
-    end
-
-    #
-    # Advance to the next waypoint. Return TRUE if
-    # there actually is one...
-    def next_waypoint!
-      raise "No mission currently active" unless active?
-      @current_wpt += 1
-      puts "Attempting to navigate to #{waypoint.name}" if active?
-    end
-
-    #
-    # Return the mission status as a string
-    def status_str
-      mins = elapsed / 60
-      hours = mins / 60
-      mins %= 60
-      days = hours / 24
-      hours %= 24
-      str = ">>> #{@time}, "
-      if days < 1
-        str += "%dh%02dm" % [hours, mins]
-      else
-        str += "+%dd%%02dh%02dm" % [days, hours, mins]
-      end
-      str + ": My position is #{@where}"
+    # Load a new mission from the missions directory.
+    def self.file_load(filename)
+      parse YAML.load(File.open(filename))
     end
 
     #
-    # Compute the remaining distance from the current location
-    def overall_distance
-      start_wpt = active? ? @current_wpt : 0
-      dist = 0.0
-      loc = @where
-      @attractors[start_wpt..-1].each do |wpt|
-        wpt.compute_bearing(loc)
-        dist += wpt.bearing.distance
-        loc = wpt.location
-      end
-      dist
+    # Load a new mission from the missions directory.
+    def self.parse(data)
+      mission = new
+      mission.parse(data)
+      mission
     end
 
     #
@@ -253,18 +125,23 @@ module SGS
       @url = data["url"]
       @description = data["description"]
       if data["launch"]
-        @launch_site = data["launch"]["name"] || "Launch Site"
-        @launch_location = SGS::Location.parse data["launch"]
+        @launch_site = data["launch"]["site"] || "Launch Site"
+        @launch_location = Location.new
+        @launch_location.parse_hash data["launch"]
       end
-      data["attractors"].each do |waypt_data|
-        waypt = Waypoint.parse(waypt_data)
-        waypt.attractor = true
-        @attractors << waypt
+      if data["attractors"]
+        data["attractors"].each do |waypt_data|
+          waypt = Waypoint.parse(waypt_data)
+          waypt.attractor = true
+          @attractors << waypt
+        end
       end
-      data["repellors"].each do |waypt_data|
-        waypt = Waypoint.parse(waypt_data)
-        waypt.attractor = false
-        @repellors << waypt
+      if data["repellors"]
+        data["repellors"].each do |waypt_data|
+          waypt = Waypoint.parse(waypt_data)
+          waypt.attractor = false
+          @repellors << waypt
+        end
       end
     end
 

data/lib/sgs/mission_status.rb

@@ -31,16 +31,26 @@
 # ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #
 # ABSTRACT
+# This class is used to store the actual mission status. As the SGS::Mission
+# class doesn't actually store anything in Redis. Nor does the SGS::Navigate
+# class. In order to save the mission and navigation details, this class
+# manages that information. Note that only the SGS::Mission daemon should
+# save this object, to avoid race conditions.
+#
+# The state here refers to the mission states, as opposed to Otto modes.
+# Initially the boat will be in AWAITING mode until something wakes it up. At
+# that point it may go to READY_TO_START or START_TEST followed by something
+# like pre-mission trying to sail to a start line or awaiting mission control.
 #
 
 ##
-# Mission state
+# Mission status
 #
 module SGS
   #
   # Handle a specific mission.
   class MissionStatus < RedisBase
-    attr_accessor :state, :current_waypoint, :start_time, :end_time
+    attr_accessor :state, :current_waypoint, :course, :distance, :start_time, :end_time
 
     STATE_AWAITING = 0
     STATE_READY_TO_START = 1
@@ -71,8 +81,10 @@ module SGS
     # the boat is on.
     def initialize
       @state = STATE_AWAITING
-      @current_waypoint = 0
-      @start_time = @end_time = nil
+      @current_waypoint = -1
+      @where = nil
+      @distance = 0
+      @track = nil
     end
 
     #

data/lib/sgs/navigate.rb

@@ -31,102 +31,226 @@
 # ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #
 # ABSTRACT
+# All of the code to navigate a sailboat to a series of waypoints is defined
+# herein. The main Navigate class does not save anything to Redis, it
+# is purely a utility class for navigation. The navigation is based on my
+# paper "An Attractor/Repellor Approach to Autonomous Sailboat Navigation".
+# https://link.springer.com/chapter/10.1007/978-3-319-72739-4_6
+#
+# We save a copy of the actual mission so we can find the attractors and
+# repellors. We also assume that doing a GPS.load will pull the latest
+# GPS co-ordinates and an Otto.load will pull the latest telemetry from
+# the boat. Specifically, the GPS will give us our lat/long and the Otto
+# data will allow us to compute the actual wind direction (as well as the
+# boat heading and apparent wind angle).
 #
 
 ##
 #
 module SGS
   class Navigate
-    attr_reader :mode
-
-    MODE_SLEEP = 0
-    MODE_TEST = 1
-    MODE_MANUAL = 2
-    MODE_UPDOWN = 3
-    MODE_OLYMPIC = 4
-    MODE_PRE_MISSION = 5
-    MODE_MISSION = 6
-    MODE_MISSION_END = 7
-    MODE_MISSION_ABORT = 8
-
-    MODENAMES = [
-      "Sleeping...",
-      "Test Mode",
-      "Manual Steering",
-      "Sail Up and Down",
-      "Sail a Triangle",
-      "Pre-Mission Wait",
-      "On Mission",
-      "Mission Ended",
-      "** Mission Abort **"
-    ].freeze
-
-    def initialize
-      @mode = MODE_SLEEP
-      @waypoint = nil
-      @curpos = nil
-      super
-    end
-
-    #
-    # Main daemon function (called from executable)
-    def self.daemon
-      puts "Navigation system starting up..."
+    #
+    # Initialize the navigational parameters
+    def initialize(mission)
+      @mission = mission
+      @swing = 45
+    end
+
+    #
+    # Compute the best heading based on our current position and the position
+    # of the current attractor. This is where the heavy-lifting happens
+    def navigate
+      if @mission.status.current_waypoint == -1
+        @mission.status.current_waypoint = 0
+        @mission.status.distance = 0
+      end
+      set_waypoint
+      puts "Attempting to navigate to #{@waypoint}..."
+      #
+      # Pull the latest GPS data...
+      @gps = GPS.load
+      puts "GPS: #{@gps}"
+      return unless @gps.valid?
+      #
+      # Pull the latest Otto data...
+      @otto = Otto.load
+      puts "OTTO:"
+      p @otto
+      puts "Compass: #{@otto.compass}"
+      puts "AWA: #{@otto.awa}"
+      puts "Wind: #{@otto.wind}"
+      #
+      # Update our local copy of the course based on what Otto says.
+      puts "Course:"
+      @course = Course.new
+      @course.heading = @otto.compass
+      @course.awa = @otto.awa
+      @course.compute_wind
+      #
+      # Compute a new course from the parameter set
+      compute_new_course
+    end
+
+    #
+    # Compute a new course based on our position and other information.
+    def compute_new_course
+      puts "Compute new course..."
+      #
+      # First off, compute distance and bearing from our current location
+      # to every attractor and repellor. We only look at forward attractors,
+      # not ones behind us.
+      compute_bearings(@mission.attractors[@mission.status.current_waypoint..-1])
+      compute_bearings(@mission.repellors)
       #
-      # Load the mission data from Redis and augment it with the
-      # contents of the mission file.
-      config = SGS::Config.load
-      mission = SGS::Mission.file_load config.mission_file
+      # Right. Now look to see if we've achieved the current waypoint and
+      # adjust, accordingly
+      while active? and reached?
+        next_waypoint!
+      end
+      return nil unless active?
+      puts "Angle to next waypoint: #{@waypoint.bearing.angle_d}d"
+      puts "Adjusted distance to waypoint is #{@waypoint.distance}"
       #
-      # Now listen for GPS data...
-      SGS::GPS.subscribe do |count|
-        puts "Received new GPS count: #{count}"
-        case SGS::MissionStatus.state
-        when STATE_COMPASS_FOLLOW
-        when STATE_WIND_FOLLOW
-          mission.navigate
-        when STATE_COMPLETE
-        when STATE_TERMINATED
-        when STATE_FAILURE
-          mission.hold_station
+      # Now, start the vector field analysis by examining headings either side
+      # of the bearing to the waypoint.
+      best_course = @course
+      best_relvmg = 0.0
+      puts "Currently on a #{@course.tack_name} tack (heading is #{@course.heading_d} degrees)"
+      (-@swing..@swing).each do |alpha_d|
+        new_course = Course.new(@course.wind)
+        new_course.heading = waypoint.bearing.angle + Bearing.dtor(alpha_d)
+        #
+        # Ignore head-to-wind cases, as they're pointless. When looking at
+        # the list of waypoints to compute relative VMG, only look to the next
+        # three or so waypoints.
+        next if new_course.speed < 0.001
+        relvmg = 0.0
+        relvmg = new_course.relative_vmg(@mission.attractors[@mission.status.current_waypoint])
+        end_wpt = @mission.status.current_waypoint + 3
+        if end_wpt >= @mission.attractors.count
+          end_wpt = @mission.attractors.count - 1
+        end
+        @mission.attractors[@mission.status.current_waypoint..end_wpt].each do |waypt|
+          relvmg += new_course.relative_vmg(waypt)
+        end
+        @mission.repellors.each do |waypt|
+          relvmg -= new_course.relative_vmg(waypt)
         end
-        gps = SGS::GPS.load
-        p gps
+        relvmg *= 0.1 if new_course.tack != @course.tack
+        if relvmg > best_relvmg
+          best_relvmg = relvmg
+          best_course = new_course
+        end
+      end
+      if best_course.tack != @course.tack
+        puts "TACKING!!!!"
+      end
+      best_course
+    end
+
+    #
+    # Compute the bearing for every attractor or repellor
+    def compute_bearings(waypoints)
+      waypoints.each do |waypt|
+        waypt.compute_bearing(@gps.location)
       end
     end
 
     #
-    # What is the mode name?
-    def mode_name
-      MODENAMES[@mode]
+    # Set new position
+    def set_position(time, loc)
+      @where = loc
+      @time = time
+      @track << TrackPoint.new(@time, @where)
+    end
+
+    #
+    # Advance the mission by a number of seconds (computing the new location
+    # in the process). Fake out the speed and thus the location.
+    def simulated_movement(how_long = 60)
+      puts "Advancing mission by #{how_long}s"
+      distance = @course.speed * how_long.to_f / 3600.0
+      puts "Travelled #{distance * 1852.0} metres in that time."
+      set_position(@time + how_long, @where + Bearing.new(@course.heading, distance))
+    end
+
+    #
+    # How long has the mission been active?
+    def elapsed
+      @time - @start_time
+    end
+
+    #
+    # Check we're active - basically, are there any more waypoints left?
+    def active?
+      @mission.status.current_waypoint < @mission.attractors.count
+    end
+
+    #
+    # Have we reached the waypoint? Note that even though the waypoints have
+    # a "reached" circle, we discard the last 10m on the basis that it is
+    # within the GPS error.
+    def reached?
+      puts "ARE WE THERE YET? (dist=#{@waypoint.distance})"
+      p @waypoint
+      return true if @waypoint.distance <= 0.0054
+      #
+      # Check to see if the next WPT is nearer than the current one
+      #if current_wpt < (@mission.attractors.count - 1)
+      #  next_wpt = @mission.attractors[@current_wpt + 1]
+      #  brng = @mission.attractors[@current_wpt].location - next_wpt.location
+      #  angle = Bearing.absolute(waypoint.bearing.angle - next_wpt.bearing.angle)
+      #  return true if brng.distance > next_wpt.distance and
+      #                 angle > (0.25 * Math::PI) and
+      #                 angle < (0.75 * Math::PI)
+      #end
+      puts "... Sadly, no."
+      return false
+    end
+
+    #
+    # Advance to the next waypoint. Return TRUE if
+    # there actually is one...
+    def next_waypoint!
+      @mission.status.current_waypoint += 1
+      puts "Attempting to navigate to new waypoint: #{waypoint}"
+      set_waypoint
     end
 
-    def mode=(val)
-      puts "SETTING NEW MODE TO #{MODENAMES[val]}"
-      @mode = val
+    #
+    # Set the waypoint instance variable based on where we are
+    def set_waypoint
+      @waypoint = @mission.attractors[@mission.status.current_waypoint]
+    end
+
+    #
+    # Return the mission status as a string
+    def status_str
+      mins = elapsed / 60
+      hours = mins / 60
+      mins %= 60
+      days = hours / 24
+      hours %= 24
+      str = ">>> #{@time}, "
+      if days < 1
+        str += "%dh%02dm" % [hours, mins]
+      else
+        str += "+%dd%%02dh%02dm" % [days, hours, mins]
+      end
+      str + ": My position is #{@where}"
     end
 
     #
-    # This is the main navigator function. It does several things;
-    # 1. Look for the next waypoint and compute bearing and distance to it
-    # 2. Decide if we have reached the waypoint (and adjust accordingly)
-    # 3. Compute the boat heading (and adjust accordingly)
-    def run
-      puts "Navigator mode is #{mode_name}: Current Position:"
-      p curpos
-      p waypoint
-      case @mode
-      when MODE_UPDOWN
-        upwind_downwind_course
-      when MODE_OLYMPIC
-        olympic_course
-      when MODE_MISSION
-        mission
-      when MODE_MISSION_END
-        mission_end
-      when MODE_MISSION_ABORT
-        mission_abort
+    # Compute the remaining distance from the current location
+    def overall_distance
+      dist = 0.0
+      loc = @where
+      @mission.attractors[@mission.status.current_waypoint..-1].each do |wpt|
+        wpt.compute_bearing(loc)
+        dist += wpt.bearing.distance
+        loc = wpt.location
       end
+      dist
     end
 
     #
@@ -164,13 +288,13 @@ module SGS
     #
     # What is our current position?
     def curpos
-      @curpos ||= SGS::GPS.load
+      @curpos ||= GPS.load
     end
 
     #
     # What is the next waypoint?
     def waypoint
-      @waypoint ||= SGS::Waypoint.load
+      @waypoint ||= Waypoint.load
     end
   end
 end

data/lib/sgs/nmea.rb

@@ -87,7 +87,7 @@ module SGS
       if @args.count < 12 or @args.count > 13
         return nil
       end
-      gps = SGS::GPS.new
+      gps = GPS.new
       gps.is_valid if @args[2] == "A"
       hh = @args[1][0..1].to_i
       mm = @args[1][2..3].to_i
@@ -100,7 +100,8 @@ module SGS
       gps.time = Time.gm(yy, mn, dd, hh, mm, ss, us)
       pos = {"latitude" => ll_nmea(@args[3,4]),
              "longitude" => ll_nmea(@args[5,6])}
-      gps.location = Location.parse pos
+      gps.location = Location.new
+      gps.location.parse_hash(pos)
       gps.sog = @args[7].to_f
       gps.cmg = Bearing.dtor @args[8].to_f
       gps