sgslib 1.5.0 → 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/exe/sgs_otto +0 -0
- data/lib/sgs/alarm.rb +21 -11
- data/lib/sgs/bearing.rb +32 -3
- data/lib/sgs/config.rb +0 -1
- data/lib/sgs/course.rb +18 -5
- data/lib/sgs/diagnostics.rb +2 -1
- data/lib/sgs/gps.rb +35 -2
- data/lib/sgs/location.rb +115 -59
- data/lib/sgs/logger.rb +1 -8
- data/lib/sgs/mission.rb +61 -184
- data/lib/sgs/mission_status.rb +20 -9
- data/lib/sgs/navigate.rb +206 -63
- data/lib/sgs/nmea.rb +3 -2
- data/lib/sgs/otto.rb +317 -45
- data/lib/sgs/redis_base.rb +9 -9
- data/lib/sgs/report.rb +6 -1
- data/lib/sgs/rpc.rb +5 -6
- data/lib/sgs/version.rb +1 -1
- data/lib/sgs/waypoint.rb +2 -2
- data/lib/sgslib.rb +1 -4
- data/sgslib.gemspec +7 -5
- metadata +52 -12
- 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,36 +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
|
-
loop do
|
55
|
-
sleep 300
|
56
|
-
end
|
57
|
-
end
|
58
|
-
|
59
|
-
#
|
60
|
-
# Load a new mission from the missions directory.
|
61
|
-
def self.file_load(filename)
|
62
|
-
parse YAML.load(File.open(filename))
|
63
|
-
end
|
64
|
-
|
65
|
-
#
|
66
|
-
# Load a new mission from the missions directory.
|
67
|
-
def self.parse(data)
|
68
|
-
mission = new
|
69
|
-
mission.parse(data)
|
70
|
-
mission
|
71
|
-
end
|
52
|
+
attr_accessor :attractors, :repellors, :status
|
72
53
|
|
73
54
|
#
|
74
55
|
# Create the attractors and repellors as well as the track array
|
75
|
-
# and other items.
|
76
|
-
# the waypoint we're working (-1 if none), @course is the heading/speed
|
77
|
-
# the boat is on.
|
56
|
+
# and other items.
|
78
57
|
def initialize
|
79
58
|
@title = nil
|
80
59
|
@url = nil
|
@@ -83,166 +62,60 @@ module SGS
|
|
83
62
|
@launch_location = nil
|
84
63
|
@attractors = []
|
85
64
|
@repellors = []
|
86
|
-
@
|
87
|
-
|
88
|
-
@where = nil
|
89
|
-
@course = Course.new
|
90
|
-
@distance = 0
|
91
|
-
@swing = 60
|
65
|
+
@status = MissionStatus.load
|
66
|
+
super
|
92
67
|
end
|
93
68
|
|
94
69
|
#
|
95
|
-
#
|
96
|
-
|
97
|
-
|
98
|
-
return unless active?
|
99
|
-
puts "Attempting to navigate to #{waypoint}"
|
100
|
-
#
|
101
|
-
# First off, compute distance and bearing from our current location
|
102
|
-
# to every attractor and repellor.
|
103
|
-
@attractors[@current_wpt..-1].each do |waypt|
|
104
|
-
waypt.compute_bearing(@where)
|
105
|
-
puts "Angle: #{waypt.bearing.angle_d}, Distance: #{waypt.bearing.distance} (adj:#{waypt.distance})"
|
106
|
-
end
|
107
|
-
@repellors.each do |waypt|
|
108
|
-
waypt.compute_bearing(@where)
|
109
|
-
puts "Angle: #{waypt.bearing.angle_d}, Distance: #{waypt.bearing.distance} (adj:#{waypt.distance})"
|
110
|
-
end
|
70
|
+
# Main daemon function (called from executable)
|
71
|
+
def self.daemon
|
72
|
+
puts "Mission management system starting up..."
|
111
73
|
#
|
112
|
-
#
|
113
|
-
#
|
114
|
-
|
115
|
-
|
116
|
-
|
117
|
-
|
118
|
-
puts "Angle to next waypoint: #{waypoint.bearing.angle_d}d"
|
119
|
-
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
|
120
80
|
#
|
121
|
-
#
|
122
|
-
|
123
|
-
|
124
|
-
|
125
|
-
|
126
|
-
|
127
|
-
|
128
|
-
|
129
|
-
|
130
|
-
|
131
|
-
|
132
|
-
|
133
|
-
|
134
|
-
|
135
|
-
|
136
|
-
|
137
|
-
|
138
|
-
|
139
|
-
|
140
|
-
|
141
|
-
|
142
|
-
|
143
|
-
puts "Relative VMG: #{relvmg}"
|
144
|
-
if relvmg > best_relvmg
|
145
|
-
puts "Best heading (so far)"
|
146
|
-
best_relvmg = relvmg
|
147
|
-
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
|
148
103
|
end
|
149
104
|
end
|
150
|
-
puts "Best RELVMG: #{best_relvmg}"
|
151
|
-
puts "TACKING!" if best_course.tack != @course.tack
|
152
|
-
puts "New HDG: #{best_course.heading_d} (AWA:#{best_course.awa_d}), WPT:#{waypoint.name}"
|
153
|
-
@course = best_course
|
154
105
|
end
|
155
106
|
|
156
107
|
#
|
157
|
-
#
|
158
|
-
def
|
159
|
-
|
160
|
-
@time = time
|
161
|
-
@track << TrackPoint.new(@time, @where)
|
162
|
-
end
|
163
|
-
|
164
|
-
#
|
165
|
-
# Advance the mission by a number of seconds (computing the new location
|
166
|
-
# in the process). Fake out the speed and thus the location.
|
167
|
-
def simulated_movement(how_long = 60)
|
168
|
-
puts "Advancing mission by #{how_long}s"
|
169
|
-
distance = @course.speed * how_long.to_f / 3600.0
|
170
|
-
puts "Travelled #{distance * 1852.0} metres in that time."
|
171
|
-
set_position(@time + how_long, @where + Bearing.new(@course.heading, distance))
|
172
|
-
end
|
173
|
-
|
174
|
-
#
|
175
|
-
# How long has the mission been active?
|
176
|
-
def elapsed
|
177
|
-
@time - @start_time
|
178
|
-
end
|
179
|
-
|
180
|
-
#
|
181
|
-
# Return the current waypoint.
|
182
|
-
def waypoint
|
183
|
-
#@attractors[@current_wpt] : nil
|
184
|
-
end
|
185
|
-
|
186
|
-
#
|
187
|
-
# Have we reached the waypoint? Note that even though the waypoints have
|
188
|
-
# a "reached" circle, we discard the last 10m on the basis that it is
|
189
|
-
# within the GPS error.
|
190
|
-
def reached?
|
191
|
-
@distance = @attractors[@current_wpt].distance
|
192
|
-
puts "ARE WE THERE YET? (dist=#{@distance})"
|
193
|
-
return true if @distance <= 0.0027
|
194
|
-
#
|
195
|
-
# Check to see if the next WPT is nearer than the current one
|
196
|
-
#if @current_wpt < (@attractors.count - 1)
|
197
|
-
# next_wpt = @attractors[@current_wpt + 1]
|
198
|
-
# brng = @attractors[@current_wpt].location - next_wpt.location
|
199
|
-
# angle = Bearing.absolute(waypoint.bearing.angle - next_wpt.bearing.angle)
|
200
|
-
# return true if brng.distance > next_wpt.distance and
|
201
|
-
# angle > (0.25 * Math::PI) and
|
202
|
-
# angle < (0.75 * Math::PI)
|
203
|
-
#end
|
204
|
-
puts "... Sadly, no."
|
205
|
-
return false
|
206
|
-
end
|
207
|
-
|
208
|
-
#
|
209
|
-
# Advance to the next waypoint. Return TRUE if
|
210
|
-
# there actually is one...
|
211
|
-
def next_waypoint!
|
212
|
-
raise "No mission currently active" unless active?
|
213
|
-
@current_wpt += 1
|
214
|
-
puts "Attempting to navigate to #{waypoint.name}" if active?
|
215
|
-
end
|
216
|
-
|
217
|
-
#
|
218
|
-
# Return the mission status as a string
|
219
|
-
def status_str
|
220
|
-
mins = elapsed / 60
|
221
|
-
hours = mins / 60
|
222
|
-
mins %= 60
|
223
|
-
days = hours / 24
|
224
|
-
hours %= 24
|
225
|
-
str = ">>> #{@time}, "
|
226
|
-
if days < 1
|
227
|
-
str += "%dh%02dm" % [hours, mins]
|
228
|
-
else
|
229
|
-
str += "+%dd%%02dh%02dm" % [days, hours, mins]
|
230
|
-
end
|
231
|
-
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))
|
232
111
|
end
|
233
112
|
|
234
113
|
#
|
235
|
-
#
|
236
|
-
def
|
237
|
-
|
238
|
-
|
239
|
-
|
240
|
-
@attractors[start_wpt..-1].each do |wpt|
|
241
|
-
wpt.compute_bearing(loc)
|
242
|
-
dist += wpt.bearing.distance
|
243
|
-
loc = wpt.location
|
244
|
-
end
|
245
|
-
dist
|
114
|
+
# Load a new mission from the missions directory.
|
115
|
+
def self.parse(data)
|
116
|
+
mission = new
|
117
|
+
mission.parse(data)
|
118
|
+
mission
|
246
119
|
end
|
247
120
|
|
248
121
|
#
|
@@ -252,18 +125,22 @@ module SGS
|
|
252
125
|
@url = data["url"]
|
253
126
|
@description = data["description"]
|
254
127
|
if data["launch"]
|
255
|
-
@launch_site = data["launch"]["
|
256
|
-
@launch_location =
|
128
|
+
@launch_site = data["launch"]["site"] || "Launch Site"
|
129
|
+
@launch_location = Location.parse data["launch"]
|
257
130
|
end
|
258
|
-
data["attractors"]
|
259
|
-
|
260
|
-
|
261
|
-
|
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
|
262
137
|
end
|
263
|
-
data["repellors"]
|
264
|
-
|
265
|
-
|
266
|
-
|
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
|
267
144
|
end
|
268
145
|
end
|
269
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,9 +81,10 @@ module SGS
|
|
71
81
|
# the boat is on.
|
72
82
|
def initialize
|
73
83
|
@state = STATE_AWAITING
|
74
|
-
@current_waypoint =
|
75
|
-
@
|
76
|
-
@
|
84
|
+
@current_waypoint = -1
|
85
|
+
@where = nil
|
86
|
+
@distance = 0
|
87
|
+
@track = nil
|
77
88
|
end
|
78
89
|
|
79
90
|
#
|
@@ -91,7 +102,7 @@ module SGS
|
|
91
102
|
#
|
92
103
|
# Commence a mission...
|
93
104
|
def start_test!(time = nil)
|
94
|
-
|
105
|
+
puts "***** Starting test phase *****"
|
95
106
|
@start_time = time || Time.now
|
96
107
|
@state = STATE_START_TEST
|
97
108
|
@current_waypoint = 0
|
@@ -104,7 +115,7 @@ module SGS
|
|
104
115
|
@end_time = time || Time.now
|
105
116
|
@state = STATE_COMPLETE
|
106
117
|
save_and_publish
|
107
|
-
|
118
|
+
puts "***** Mission completed! *****"
|
108
119
|
end
|
109
120
|
|
110
121
|
#
|
@@ -113,7 +124,7 @@ module SGS
|
|
113
124
|
@end_time = time || Time.now
|
114
125
|
@state = STATE_TERMINATED
|
115
126
|
save_and_publish
|
116
|
-
|
127
|
+
puts "***** Mission terminated! *****"
|
117
128
|
end
|
118
129
|
|
119
130
|
#
|
@@ -122,7 +133,7 @@ module SGS
|
|
122
133
|
@end_time = time || Time.now
|
123
134
|
@state = STATE_FAILURE
|
124
135
|
save_and_publish
|
125
|
-
|
136
|
+
puts "***** Mission failure! *****"
|
126
137
|
end
|
127
138
|
end
|
128
139
|
end
|
data/lib/sgs/navigate.rb
CHANGED
@@ -31,83 +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
|
-
|
75
|
-
|
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)
|
104
|
+
#
|
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!
|
76
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}"
|
113
|
+
#
|
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)
|
138
|
+
end
|
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)
|
156
|
+
end
|
157
|
+
end
|
158
|
+
|
159
|
+
#
|
160
|
+
# Set new position
|
161
|
+
def set_position(time, loc)
|
162
|
+
@where = loc
|
163
|
+
@time = time
|
164
|
+
@track << TrackPoint.new(@time, @where)
|
77
165
|
end
|
78
166
|
|
79
167
|
#
|
80
|
-
#
|
81
|
-
|
82
|
-
|
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))
|
83
175
|
end
|
84
176
|
|
85
|
-
|
86
|
-
|
87
|
-
|
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
|
218
|
+
end
|
219
|
+
|
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}"
|
88
241
|
end
|
89
242
|
|
90
243
|
#
|
91
|
-
#
|
92
|
-
|
93
|
-
|
94
|
-
|
95
|
-
|
96
|
-
|
97
|
-
|
98
|
-
|
99
|
-
case @mode
|
100
|
-
when MODE_UPDOWN
|
101
|
-
upwind_downwind_course
|
102
|
-
when MODE_OLYMPIC
|
103
|
-
olympic_course
|
104
|
-
when MODE_MISSION
|
105
|
-
mission
|
106
|
-
when MODE_MISSION_END
|
107
|
-
mission_end
|
108
|
-
when MODE_MISSION_ABORT
|
109
|
-
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
|
110
252
|
end
|
253
|
+
dist
|
111
254
|
end
|
112
255
|
|
113
256
|
#
|
@@ -145,13 +288,13 @@ module SGS
|
|
145
288
|
#
|
146
289
|
# What is our current position?
|
147
290
|
def curpos
|
148
|
-
@curpos ||=
|
291
|
+
@curpos ||= GPS.load
|
149
292
|
end
|
150
293
|
|
151
294
|
#
|
152
295
|
# What is the next waypoint?
|
153
296
|
def waypoint
|
154
|
-
@waypoint ||=
|
297
|
+
@waypoint ||= Waypoint.load
|
155
298
|
end
|
156
299
|
end
|
157
300
|
end
|