sgslib 1.5.1 → 1.6.0
Sign up to get free protection for your applications and to get access to all the features.
- checksums.yaml +4 -4
- data/lib/sgs/alarm.rb +2 -2
- data/lib/sgs/bearing.rb +31 -3
- data/lib/sgs/config.rb +0 -1
- data/lib/sgs/course.rb +18 -5
- data/lib/sgs/diagnostics.rb +1 -1
- data/lib/sgs/gps.rb +23 -3
- data/lib/sgs/location.rb +115 -59
- data/lib/sgs/logger.rb +1 -1
- data/lib/sgs/mission.rb +61 -185
- data/lib/sgs/mission_status.rb +16 -4
- data/lib/sgs/navigate.rb +203 -79
- data/lib/sgs/nmea.rb +3 -2
- data/lib/sgs/otto.rb +181 -78
- data/lib/sgs/redis_base.rb +9 -9
- data/lib/sgs/report.rb +2 -2
- data/lib/sgs/rpc.rb +5 -6
- data/lib/sgs/version.rb +1 -1
- data/lib/sgs/waypoint.rb +2 -2
- data/sgslib.gemspec +3 -3
- metadata +8 -10
- data/exe/sgs_nav +0 -43
data/lib/sgs/mission.rb
CHANGED
@@ -32,6 +32,10 @@
|
|
32
32
|
# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
33
33
|
#
|
34
34
|
# ABSTRACT
|
35
|
+
# Mostly this class is for the overall daemon running on Mother. It takes
|
36
|
+
# care of listening for GPS updates and adjusting the sailing route,
|
37
|
+
# accordingly. It is responsible for keeping MissionStatus up to
|
38
|
+
# date. The actual mission information is stored in a YAML file.
|
35
39
|
#
|
36
40
|
|
37
41
|
##
|
@@ -45,37 +49,11 @@ module SGS
|
|
45
49
|
class Mission
|
46
50
|
attr_accessor :title, :url, :description
|
47
51
|
attr_accessor :launch_site, :launch_location
|
48
|
-
attr_accessor :attractors, :repellors, :
|
49
|
-
attr_accessor :where, :time, :course, :distance
|
50
|
-
|
51
|
-
#
|
52
|
-
# Main daemon function (called from executable)
|
53
|
-
def self.daemon
|
54
|
-
puts "Mission management system starting up..."
|
55
|
-
loop do
|
56
|
-
sleep 300
|
57
|
-
end
|
58
|
-
end
|
59
|
-
|
60
|
-
#
|
61
|
-
# Load a new mission from the missions directory.
|
62
|
-
def self.file_load(filename)
|
63
|
-
parse YAML.load(File.open(filename))
|
64
|
-
end
|
65
|
-
|
66
|
-
#
|
67
|
-
# Load a new mission from the missions directory.
|
68
|
-
def self.parse(data)
|
69
|
-
mission = new
|
70
|
-
mission.parse(data)
|
71
|
-
mission
|
72
|
-
end
|
52
|
+
attr_accessor :attractors, :repellors, :status
|
73
53
|
|
74
54
|
#
|
75
55
|
# Create the attractors and repellors as well as the track array
|
76
|
-
# and other items.
|
77
|
-
# the waypoint we're working (-1 if none), @course is the heading/speed
|
78
|
-
# the boat is on.
|
56
|
+
# and other items.
|
79
57
|
def initialize
|
80
58
|
@title = nil
|
81
59
|
@url = nil
|
@@ -84,166 +62,60 @@ module SGS
|
|
84
62
|
@launch_location = nil
|
85
63
|
@attractors = []
|
86
64
|
@repellors = []
|
87
|
-
@
|
88
|
-
|
89
|
-
@where = nil
|
90
|
-
@course = Course.new
|
91
|
-
@distance = 0
|
92
|
-
@swing = 60
|
65
|
+
@status = MissionStatus.load
|
66
|
+
super
|
93
67
|
end
|
94
68
|
|
95
69
|
#
|
96
|
-
#
|
97
|
-
|
98
|
-
|
99
|
-
return unless active?
|
100
|
-
puts "Attempting to navigate to #{waypoint}"
|
101
|
-
#
|
102
|
-
# First off, compute distance and bearing from our current location
|
103
|
-
# to every attractor and repellor.
|
104
|
-
@attractors[@current_wpt..-1].each do |waypt|
|
105
|
-
waypt.compute_bearing(@where)
|
106
|
-
puts "Angle: #{waypt.bearing.angle_d}, Distance: #{waypt.bearing.distance} (adj:#{waypt.distance})"
|
107
|
-
end
|
108
|
-
@repellors.each do |waypt|
|
109
|
-
waypt.compute_bearing(@where)
|
110
|
-
puts "Angle: #{waypt.bearing.angle_d}, Distance: #{waypt.bearing.distance} (adj:#{waypt.distance})"
|
111
|
-
end
|
70
|
+
# Main daemon function (called from executable)
|
71
|
+
def self.daemon
|
72
|
+
puts "Mission management system starting up..."
|
112
73
|
#
|
113
|
-
#
|
114
|
-
#
|
115
|
-
|
116
|
-
|
117
|
-
|
118
|
-
|
119
|
-
puts "Angle to next waypoint: #{waypoint.bearing.angle_d}d"
|
120
|
-
puts "Adjusted distance to waypoint is #{@distance}"
|
74
|
+
# Load the mission data from Redis and augment it with the
|
75
|
+
# contents of the mission file.
|
76
|
+
config = Config.load
|
77
|
+
mission = Mission.file_load config.mission_file
|
78
|
+
nav = Navigate.new(mission)
|
79
|
+
otto = Otto.load
|
121
80
|
#
|
122
|
-
#
|
123
|
-
|
124
|
-
|
125
|
-
|
126
|
-
|
127
|
-
|
128
|
-
|
129
|
-
|
130
|
-
|
131
|
-
|
132
|
-
|
133
|
-
|
134
|
-
|
135
|
-
|
136
|
-
|
137
|
-
|
138
|
-
|
139
|
-
|
140
|
-
|
141
|
-
|
142
|
-
|
143
|
-
|
144
|
-
puts "Relative VMG: #{relvmg}"
|
145
|
-
if relvmg > best_relvmg
|
146
|
-
puts "Best heading (so far)"
|
147
|
-
best_relvmg = relvmg
|
148
|
-
best_course = new_course
|
81
|
+
# Keep running in our mission state, forever.
|
82
|
+
while true do
|
83
|
+
if mission.status.active?
|
84
|
+
#
|
85
|
+
# Listen for GPS data. When we have a new position, call the
|
86
|
+
# navigation code to determine a new course to sail (currently
|
87
|
+
# a compass course), and set the Otto register accordingly.
|
88
|
+
# Repeat until we run out of waypoints.
|
89
|
+
GPS.subscribe do |count|
|
90
|
+
puts "Mission received new GPS count: #{count}"
|
91
|
+
new_course = nav.navigate
|
92
|
+
if new_course.nil?
|
93
|
+
mission.status.completed!
|
94
|
+
break
|
95
|
+
end
|
96
|
+
mission.status.save
|
97
|
+
compass = Bearing.rtox(new_course.heading)
|
98
|
+
otto.set_register(Otto::COMPASS_HEADING_REGISTER, compass)
|
99
|
+
end
|
100
|
+
else
|
101
|
+
sleep 60
|
102
|
+
mission.status.load
|
149
103
|
end
|
150
104
|
end
|
151
|
-
puts "Best RELVMG: #{best_relvmg}"
|
152
|
-
puts "TACKING!" if best_course.tack != @course.tack
|
153
|
-
puts "New HDG: #{best_course.heading_d} (AWA:#{best_course.awa_d}), WPT:#{waypoint.name}"
|
154
|
-
@course = best_course
|
155
105
|
end
|
156
106
|
|
157
107
|
#
|
158
|
-
#
|
159
|
-
def
|
160
|
-
|
161
|
-
@time = time
|
162
|
-
@track << TrackPoint.new(@time, @where)
|
163
|
-
end
|
164
|
-
|
165
|
-
#
|
166
|
-
# Advance the mission by a number of seconds (computing the new location
|
167
|
-
# in the process). Fake out the speed and thus the location.
|
168
|
-
def simulated_movement(how_long = 60)
|
169
|
-
puts "Advancing mission by #{how_long}s"
|
170
|
-
distance = @course.speed * how_long.to_f / 3600.0
|
171
|
-
puts "Travelled #{distance * 1852.0} metres in that time."
|
172
|
-
set_position(@time + how_long, @where + Bearing.new(@course.heading, distance))
|
173
|
-
end
|
174
|
-
|
175
|
-
#
|
176
|
-
# How long has the mission been active?
|
177
|
-
def elapsed
|
178
|
-
@time - @start_time
|
179
|
-
end
|
180
|
-
|
181
|
-
#
|
182
|
-
# Return the current waypoint.
|
183
|
-
def waypoint
|
184
|
-
#@attractors[@current_wpt] : nil
|
185
|
-
end
|
186
|
-
|
187
|
-
#
|
188
|
-
# Have we reached the waypoint? Note that even though the waypoints have
|
189
|
-
# a "reached" circle, we discard the last 10m on the basis that it is
|
190
|
-
# within the GPS error.
|
191
|
-
def reached?
|
192
|
-
@distance = @attractors[@current_wpt].distance
|
193
|
-
puts "ARE WE THERE YET? (dist=#{@distance})"
|
194
|
-
return true if @distance <= 0.0027
|
195
|
-
#
|
196
|
-
# Check to see if the next WPT is nearer than the current one
|
197
|
-
#if @current_wpt < (@attractors.count - 1)
|
198
|
-
# next_wpt = @attractors[@current_wpt + 1]
|
199
|
-
# brng = @attractors[@current_wpt].location - next_wpt.location
|
200
|
-
# angle = Bearing.absolute(waypoint.bearing.angle - next_wpt.bearing.angle)
|
201
|
-
# return true if brng.distance > next_wpt.distance and
|
202
|
-
# angle > (0.25 * Math::PI) and
|
203
|
-
# angle < (0.75 * Math::PI)
|
204
|
-
#end
|
205
|
-
puts "... Sadly, no."
|
206
|
-
return false
|
207
|
-
end
|
208
|
-
|
209
|
-
#
|
210
|
-
# Advance to the next waypoint. Return TRUE if
|
211
|
-
# there actually is one...
|
212
|
-
def next_waypoint!
|
213
|
-
raise "No mission currently active" unless active?
|
214
|
-
@current_wpt += 1
|
215
|
-
puts "Attempting to navigate to #{waypoint.name}" if active?
|
216
|
-
end
|
217
|
-
|
218
|
-
#
|
219
|
-
# Return the mission status as a string
|
220
|
-
def status_str
|
221
|
-
mins = elapsed / 60
|
222
|
-
hours = mins / 60
|
223
|
-
mins %= 60
|
224
|
-
days = hours / 24
|
225
|
-
hours %= 24
|
226
|
-
str = ">>> #{@time}, "
|
227
|
-
if days < 1
|
228
|
-
str += "%dh%02dm" % [hours, mins]
|
229
|
-
else
|
230
|
-
str += "+%dd%%02dh%02dm" % [days, hours, mins]
|
231
|
-
end
|
232
|
-
str + ": My position is #{@where}"
|
108
|
+
# Load a new mission from the missions directory.
|
109
|
+
def self.file_load(filename)
|
110
|
+
parse YAML.load(File.open(filename))
|
233
111
|
end
|
234
112
|
|
235
113
|
#
|
236
|
-
#
|
237
|
-
def
|
238
|
-
|
239
|
-
|
240
|
-
|
241
|
-
@attractors[start_wpt..-1].each do |wpt|
|
242
|
-
wpt.compute_bearing(loc)
|
243
|
-
dist += wpt.bearing.distance
|
244
|
-
loc = wpt.location
|
245
|
-
end
|
246
|
-
dist
|
114
|
+
# Load a new mission from the missions directory.
|
115
|
+
def self.parse(data)
|
116
|
+
mission = new
|
117
|
+
mission.parse(data)
|
118
|
+
mission
|
247
119
|
end
|
248
120
|
|
249
121
|
#
|
@@ -253,18 +125,22 @@ module SGS
|
|
253
125
|
@url = data["url"]
|
254
126
|
@description = data["description"]
|
255
127
|
if data["launch"]
|
256
|
-
@launch_site = data["launch"]["
|
257
|
-
@launch_location =
|
128
|
+
@launch_site = data["launch"]["site"] || "Launch Site"
|
129
|
+
@launch_location = Location.parse data["launch"]
|
258
130
|
end
|
259
|
-
data["attractors"]
|
260
|
-
|
261
|
-
|
262
|
-
|
131
|
+
if data["attractors"]
|
132
|
+
data["attractors"].each do |waypt_data|
|
133
|
+
waypt = Waypoint.parse(waypt_data)
|
134
|
+
waypt.attractor = true
|
135
|
+
@attractors << waypt
|
136
|
+
end
|
263
137
|
end
|
264
|
-
data["repellors"]
|
265
|
-
|
266
|
-
|
267
|
-
|
138
|
+
if data["repellors"]
|
139
|
+
data["repellors"].each do |waypt_data|
|
140
|
+
waypt = Waypoint.parse(waypt_data)
|
141
|
+
waypt.attractor = false
|
142
|
+
@repellors << waypt
|
143
|
+
end
|
268
144
|
end
|
269
145
|
end
|
270
146
|
|
data/lib/sgs/mission_status.rb
CHANGED
@@ -31,16 +31,26 @@
|
|
31
31
|
# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
32
32
|
#
|
33
33
|
# ABSTRACT
|
34
|
+
# This class is used to store the actual mission status. As the SGS::Mission
|
35
|
+
# class doesn't actually store anything in Redis. Nor does the SGS::Navigate
|
36
|
+
# class. In order to save the mission and navigation details, this class
|
37
|
+
# manages that information. Note that only the SGS::Mission daemon should
|
38
|
+
# save this object, to avoid race conditions.
|
39
|
+
#
|
40
|
+
# The state here refers to the mission states, as opposed to Otto modes.
|
41
|
+
# Initially the boat will be in AWAITING mode until something wakes it up. At
|
42
|
+
# that point it may go to READY_TO_START or START_TEST followed by something
|
43
|
+
# like pre-mission trying to sail to a start line or awaiting mission control.
|
34
44
|
#
|
35
45
|
|
36
46
|
##
|
37
|
-
# Mission
|
47
|
+
# Mission status
|
38
48
|
#
|
39
49
|
module SGS
|
40
50
|
#
|
41
51
|
# Handle a specific mission.
|
42
52
|
class MissionStatus < RedisBase
|
43
|
-
attr_accessor :state, :current_waypoint, :start_time, :end_time
|
53
|
+
attr_accessor :state, :current_waypoint, :course, :distance, :start_time, :end_time
|
44
54
|
|
45
55
|
STATE_AWAITING = 0
|
46
56
|
STATE_READY_TO_START = 1
|
@@ -71,8 +81,10 @@ module SGS
|
|
71
81
|
# the boat is on.
|
72
82
|
def initialize
|
73
83
|
@state = STATE_AWAITING
|
74
|
-
@current_waypoint =
|
75
|
-
@
|
84
|
+
@current_waypoint = -1
|
85
|
+
@where = nil
|
86
|
+
@distance = 0
|
87
|
+
@track = nil
|
76
88
|
end
|
77
89
|
|
78
90
|
#
|
data/lib/sgs/navigate.rb
CHANGED
@@ -31,102 +31,226 @@
|
|
31
31
|
# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
32
32
|
#
|
33
33
|
# ABSTRACT
|
34
|
+
# All of the code to navigate a sailboat to a series of waypoints is defined
|
35
|
+
# herein. The main Navigate class does not save anything to Redis, it
|
36
|
+
# is purely a utility class for navigation. The navigation is based on my
|
37
|
+
# paper "An Attractor/Repellor Approach to Autonomous Sailboat Navigation".
|
38
|
+
# https://link.springer.com/chapter/10.1007/978-3-319-72739-4_6
|
39
|
+
#
|
40
|
+
# We save a copy of the actual mission so we can find the attractors and
|
41
|
+
# repellors. We also assume that doing a GPS.load will pull the latest
|
42
|
+
# GPS co-ordinates and an Otto.load will pull the latest telemetry from
|
43
|
+
# the boat. Specifically, the GPS will give us our lat/long and the Otto
|
44
|
+
# data will allow us to compute the actual wind direction (as well as the
|
45
|
+
# boat heading and apparent wind angle).
|
34
46
|
#
|
35
47
|
|
36
48
|
##
|
37
49
|
#
|
38
50
|
module SGS
|
39
51
|
class Navigate
|
40
|
-
|
41
|
-
|
42
|
-
|
43
|
-
|
44
|
-
|
45
|
-
|
46
|
-
|
47
|
-
|
48
|
-
|
49
|
-
|
50
|
-
|
51
|
-
|
52
|
-
|
53
|
-
|
54
|
-
|
55
|
-
|
56
|
-
"
|
57
|
-
|
58
|
-
|
59
|
-
|
60
|
-
"
|
61
|
-
|
62
|
-
|
63
|
-
|
64
|
-
|
65
|
-
|
66
|
-
@
|
67
|
-
|
68
|
-
|
69
|
-
|
70
|
-
|
71
|
-
|
72
|
-
|
73
|
-
|
74
|
-
|
52
|
+
#
|
53
|
+
# Initialize the navigational parameters
|
54
|
+
def initialize(mission)
|
55
|
+
@mission = mission
|
56
|
+
@swing = 45
|
57
|
+
end
|
58
|
+
|
59
|
+
#
|
60
|
+
# Compute the best heading based on our current position and the position
|
61
|
+
# of the current attractor. This is where the heavy-lifting happens
|
62
|
+
def navigate
|
63
|
+
if @mission.status.current_waypoint == -1
|
64
|
+
@mission.status.current_waypoint = 0
|
65
|
+
@mission.status.distance = 0
|
66
|
+
end
|
67
|
+
set_waypoint
|
68
|
+
puts "Attempting to navigate to #{@waypoint}..."
|
69
|
+
#
|
70
|
+
# Pull the latest GPS data...
|
71
|
+
@gps = GPS.load
|
72
|
+
puts "GPS: #{@gps}"
|
73
|
+
return unless @gps.valid?
|
74
|
+
#
|
75
|
+
# Pull the latest Otto data...
|
76
|
+
@otto = Otto.load
|
77
|
+
puts "OTTO:"
|
78
|
+
p @otto
|
79
|
+
puts "Compass: #{@otto.compass}"
|
80
|
+
puts "AWA: #{@otto.awa}"
|
81
|
+
puts "Wind: #{@otto.wind}"
|
82
|
+
#
|
83
|
+
# Update our local copy of the course based on what Otto says.
|
84
|
+
puts "Course:"
|
85
|
+
@course = Course.new
|
86
|
+
@course.heading = @otto.compass
|
87
|
+
@course.awa = @otto.awa
|
88
|
+
@course.compute_wind
|
89
|
+
#
|
90
|
+
# Compute a new course from the parameter set
|
91
|
+
compute_new_course
|
92
|
+
end
|
93
|
+
|
94
|
+
#
|
95
|
+
# Compute a new course based on our position and other information.
|
96
|
+
def compute_new_course
|
97
|
+
puts "Compute new course..."
|
98
|
+
#
|
99
|
+
# First off, compute distance and bearing from our current location
|
100
|
+
# to every attractor and repellor. We only look at forward attractors,
|
101
|
+
# not ones behind us.
|
102
|
+
compute_bearings(@mission.attractors[@mission.status.current_waypoint..-1])
|
103
|
+
compute_bearings(@mission.repellors)
|
75
104
|
#
|
76
|
-
#
|
77
|
-
#
|
78
|
-
|
79
|
-
|
105
|
+
# Right. Now look to see if we've achieved the current waypoint and
|
106
|
+
# adjust, accordingly
|
107
|
+
while active? and reached?
|
108
|
+
next_waypoint!
|
109
|
+
end
|
110
|
+
return nil unless active?
|
111
|
+
puts "Angle to next waypoint: #{@waypoint.bearing.angle_d}d"
|
112
|
+
puts "Adjusted distance to waypoint is #{@waypoint.distance}"
|
80
113
|
#
|
81
|
-
# Now
|
82
|
-
|
83
|
-
|
84
|
-
|
85
|
-
|
86
|
-
|
87
|
-
|
88
|
-
|
89
|
-
|
90
|
-
|
91
|
-
|
114
|
+
# Now, start the vector field analysis by examining headings either side
|
115
|
+
# of the bearing to the waypoint.
|
116
|
+
best_course = @course
|
117
|
+
best_relvmg = 0.0
|
118
|
+
puts "Currently on a #{@course.tack_name} tack (heading is #{@course.heading_d} degrees)"
|
119
|
+
(-@swing..@swing).each do |alpha_d|
|
120
|
+
new_course = Course.new(@course.wind)
|
121
|
+
new_course.heading = waypoint.bearing.angle + Bearing.dtor(alpha_d)
|
122
|
+
#
|
123
|
+
# Ignore head-to-wind cases, as they're pointless. When looking at
|
124
|
+
# the list of waypoints to compute relative VMG, only look to the next
|
125
|
+
# three or so waypoints.
|
126
|
+
next if new_course.speed < 0.001
|
127
|
+
relvmg = 0.0
|
128
|
+
relvmg = new_course.relative_vmg(@mission.attractors[@mission.status.current_waypoint])
|
129
|
+
end_wpt = @mission.status.current_waypoint + 3
|
130
|
+
if end_wpt >= @mission.attractors.count
|
131
|
+
end_wpt = @mission.attractors.count - 1
|
132
|
+
end
|
133
|
+
@mission.attractors[@mission.status.current_waypoint..end_wpt].each do |waypt|
|
134
|
+
relvmg += new_course.relative_vmg(waypt)
|
135
|
+
end
|
136
|
+
@mission.repellors.each do |waypt|
|
137
|
+
relvmg -= new_course.relative_vmg(waypt)
|
92
138
|
end
|
93
|
-
|
94
|
-
|
139
|
+
relvmg *= 0.1 if new_course.tack != @course.tack
|
140
|
+
if relvmg > best_relvmg
|
141
|
+
best_relvmg = relvmg
|
142
|
+
best_course = new_course
|
143
|
+
end
|
144
|
+
end
|
145
|
+
if best_course.tack != @course.tack
|
146
|
+
puts "TACKING!!!!"
|
147
|
+
end
|
148
|
+
best_course
|
149
|
+
end
|
150
|
+
|
151
|
+
#
|
152
|
+
# Compute the bearing for every attractor or repellor
|
153
|
+
def compute_bearings(waypoints)
|
154
|
+
waypoints.each do |waypt|
|
155
|
+
waypt.compute_bearing(@gps.location)
|
95
156
|
end
|
96
157
|
end
|
97
158
|
|
98
159
|
#
|
99
|
-
#
|
100
|
-
def
|
101
|
-
|
160
|
+
# Set new position
|
161
|
+
def set_position(time, loc)
|
162
|
+
@where = loc
|
163
|
+
@time = time
|
164
|
+
@track << TrackPoint.new(@time, @where)
|
165
|
+
end
|
166
|
+
|
167
|
+
#
|
168
|
+
# Advance the mission by a number of seconds (computing the new location
|
169
|
+
# in the process). Fake out the speed and thus the location.
|
170
|
+
def simulated_movement(how_long = 60)
|
171
|
+
puts "Advancing mission by #{how_long}s"
|
172
|
+
distance = @course.speed * how_long.to_f / 3600.0
|
173
|
+
puts "Travelled #{distance * 1852.0} metres in that time."
|
174
|
+
set_position(@time + how_long, @where + Bearing.new(@course.heading, distance))
|
175
|
+
end
|
176
|
+
|
177
|
+
#
|
178
|
+
# How long has the mission been active?
|
179
|
+
def elapsed
|
180
|
+
@time - @start_time
|
181
|
+
end
|
182
|
+
|
183
|
+
#
|
184
|
+
# Check we're active - basically, are there any more waypoints left?
|
185
|
+
def active?
|
186
|
+
@mission.status.current_waypoint < @mission.attractors.count
|
187
|
+
end
|
188
|
+
|
189
|
+
#
|
190
|
+
# Have we reached the waypoint? Note that even though the waypoints have
|
191
|
+
# a "reached" circle, we discard the last 10m on the basis that it is
|
192
|
+
# within the GPS error.
|
193
|
+
def reached?
|
194
|
+
puts "ARE WE THERE YET? (dist=#{@waypoint.distance})"
|
195
|
+
p @waypoint
|
196
|
+
return true if @waypoint.distance <= 0.0054
|
197
|
+
#
|
198
|
+
# Check to see if the next WPT is nearer than the current one
|
199
|
+
#if current_wpt < (@mission.attractors.count - 1)
|
200
|
+
# next_wpt = @mission.attractors[@current_wpt + 1]
|
201
|
+
# brng = @mission.attractors[@current_wpt].location - next_wpt.location
|
202
|
+
# angle = Bearing.absolute(waypoint.bearing.angle - next_wpt.bearing.angle)
|
203
|
+
# return true if brng.distance > next_wpt.distance and
|
204
|
+
# angle > (0.25 * Math::PI) and
|
205
|
+
# angle < (0.75 * Math::PI)
|
206
|
+
#end
|
207
|
+
puts "... Sadly, no."
|
208
|
+
return false
|
209
|
+
end
|
210
|
+
|
211
|
+
#
|
212
|
+
# Advance to the next waypoint. Return TRUE if
|
213
|
+
# there actually is one...
|
214
|
+
def next_waypoint!
|
215
|
+
@mission.status.current_waypoint += 1
|
216
|
+
puts "Attempting to navigate to new waypoint: #{waypoint}"
|
217
|
+
set_waypoint
|
102
218
|
end
|
103
219
|
|
104
|
-
|
105
|
-
|
106
|
-
|
220
|
+
#
|
221
|
+
# Set the waypoint instance variable based on where we are
|
222
|
+
def set_waypoint
|
223
|
+
@waypoint = @mission.attractors[@mission.status.current_waypoint]
|
224
|
+
end
|
225
|
+
|
226
|
+
#
|
227
|
+
# Return the mission status as a string
|
228
|
+
def status_str
|
229
|
+
mins = elapsed / 60
|
230
|
+
hours = mins / 60
|
231
|
+
mins %= 60
|
232
|
+
days = hours / 24
|
233
|
+
hours %= 24
|
234
|
+
str = ">>> #{@time}, "
|
235
|
+
if days < 1
|
236
|
+
str += "%dh%02dm" % [hours, mins]
|
237
|
+
else
|
238
|
+
str += "+%dd%%02dh%02dm" % [days, hours, mins]
|
239
|
+
end
|
240
|
+
str + ": My position is #{@where}"
|
107
241
|
end
|
108
242
|
|
109
243
|
#
|
110
|
-
#
|
111
|
-
|
112
|
-
|
113
|
-
|
114
|
-
|
115
|
-
|
116
|
-
|
117
|
-
|
118
|
-
case @mode
|
119
|
-
when MODE_UPDOWN
|
120
|
-
upwind_downwind_course
|
121
|
-
when MODE_OLYMPIC
|
122
|
-
olympic_course
|
123
|
-
when MODE_MISSION
|
124
|
-
mission
|
125
|
-
when MODE_MISSION_END
|
126
|
-
mission_end
|
127
|
-
when MODE_MISSION_ABORT
|
128
|
-
mission_abort
|
244
|
+
# Compute the remaining distance from the current location
|
245
|
+
def overall_distance
|
246
|
+
dist = 0.0
|
247
|
+
loc = @where
|
248
|
+
@mission.attractors[@mission.status.current_waypoint..-1].each do |wpt|
|
249
|
+
wpt.compute_bearing(loc)
|
250
|
+
dist += wpt.bearing.distance
|
251
|
+
loc = wpt.location
|
129
252
|
end
|
253
|
+
dist
|
130
254
|
end
|
131
255
|
|
132
256
|
#
|
@@ -164,13 +288,13 @@ module SGS
|
|
164
288
|
#
|
165
289
|
# What is our current position?
|
166
290
|
def curpos
|
167
|
-
@curpos ||=
|
291
|
+
@curpos ||= GPS.load
|
168
292
|
end
|
169
293
|
|
170
294
|
#
|
171
295
|
# What is the next waypoint?
|
172
296
|
def waypoint
|
173
|
-
@waypoint ||=
|
297
|
+
@waypoint ||= Waypoint.load
|
174
298
|
end
|
175
299
|
end
|
176
300
|
end
|
data/lib/sgs/nmea.rb
CHANGED
@@ -87,7 +87,7 @@ module SGS
|
|
87
87
|
if @args.count < 12 or @args.count > 13
|
88
88
|
return nil
|
89
89
|
end
|
90
|
-
gps =
|
90
|
+
gps = GPS.new
|
91
91
|
gps.is_valid if @args[2] == "A"
|
92
92
|
hh = @args[1][0..1].to_i
|
93
93
|
mm = @args[1][2..3].to_i
|
@@ -100,7 +100,8 @@ module SGS
|
|
100
100
|
gps.time = Time.gm(yy, mn, dd, hh, mm, ss, us)
|
101
101
|
pos = {"latitude" => ll_nmea(@args[3,4]),
|
102
102
|
"longitude" => ll_nmea(@args[5,6])}
|
103
|
-
gps.location = Location.
|
103
|
+
gps.location = Location.new
|
104
|
+
gps.location.parse_hash(pos)
|
104
105
|
gps.sog = @args[7].to_f
|
105
106
|
gps.cmg = Bearing.dtor @args[8].to_f
|
106
107
|
gps
|