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 CHANGED
@@ -1,7 +1,7 @@
1
1
  ---
2
2
  SHA256:
3
- metadata.gz: a7ecb05d8a8096186dd2d79b8126cc50e501e71d5b7c80a58f7b8bcc9a0f7ec0
4
- data.tar.gz: c4d5073036b612bc4333ad2e176e58c6dae5c262d748a981eb55992526c0a29e
3
+ metadata.gz: 87786bab467aa776dcf6c87f887469ed917b43019d4863dd3be4249ff6fe299d
4
+ data.tar.gz: 3142d29756860081e97292373f37647d83f1a409f78ec6aad702dcf6d6484dd4
5
5
  SHA512:
6
- metadata.gz: 891aa77f6714876a1c2ec5432b760aeab2a7a9421c2e97713dac0ad618f068961142d06020a24eec586222f3ddc4762e1e549461152fa3ab32d18b93b1f046b9
7
- data.tar.gz: 03d3070b475d819e0f1f855ba6a83fb83596b4c2900a4bb201348570ca9b2e7e5769ab5c9a7f00a3d71c1b31cac12023d362224dd190af01a46695b20df43f7d
6
+ metadata.gz: 120c638f13a6cb70844ec022fef4366c23b19d51643179bd5f1805fe75fbff11167db8338703c0f0a48a75ab77f6d76589b5ddd9d6f26a28fc03fca45e9beacf
7
+ data.tar.gz: 67c4abc829bf6c3a0bbdc75de66b12bf797de3b348103db654300edd08eb7fb098b3b66fe3e571fe250da1546d04b4012b5b8e7199780988024c39b1a6eb0a7a
data/exe/sgs_otto CHANGED
File without changes
data/lib/sgs/alarm.rb CHANGED
@@ -44,7 +44,9 @@ module SGS
44
44
  class Alarm < RedisBase
45
45
  attr_accessor :last_report, :time
46
46
 
47
- OTTO_RESTART = 0
47
+ #
48
+ # Alarms generated by Otto.
49
+ MISSION_SWITCH = 0
48
50
  RUDDSRV_FAULT = 1
49
51
  SAILSRV_FAULT = 2
50
52
  VBATT_CRITICAL = 3
@@ -60,16 +62,18 @@ module SGS
60
62
  RUDDER_NOZERO = 13
61
63
  SAIL_NOZERO = 14
62
64
  MOTHER_UNRESP = 15
63
-
64
- MISSION_COMMENCE = 16
65
- MISSION_COMPLETE = 17
66
- MISSION_ABORT = 18
67
- WAYPOINT_REACHED = 19
68
- CROSS_TRACK_ERROR = 20
69
- INSIDE_FENCE = 21
65
+ #
66
+ # Alarms generated by Mother.
67
+ OTTO_RESTART = 16
68
+ MISSION_COMMENCE = 17
69
+ MISSION_COMPLETE = 18
70
+ MISSION_ABORT = 19
71
+ WAYPOINT_REACHED = 20
72
+ CROSS_TRACK_ERROR = 21
73
+ INSIDE_FENCE = 22
70
74
 
71
75
  ALARM_NAMES = [
72
- "OTTO Restarted",
76
+ "Mission Activation Switch",
73
77
  "Rudder Servo Fault",
74
78
  "Sail Servo Fault",
75
79
  "Battery voltage is critically low",
@@ -85,6 +89,7 @@ module SGS
85
89
  "Cannot zero the rudder position",
86
90
  "Cannot zero the sail position",
87
91
  "Mother is unresponsive",
92
+ "OTTO Restarted",
88
93
  "Mission has commenced",
89
94
  "Mission is completed",
90
95
  "*** MISSION ABORT ***",
@@ -103,8 +108,13 @@ module SGS
103
108
  #
104
109
  # Main daemon function (called from executable)
105
110
  def self.daemon
111
+ puts "Alarm daemon starting up..."
112
+ otto = RPCClient.new(:otto)
106
113
  loop do
107
- sleep 300
114
+ #puts "Check for any alarms..."
115
+ #resp = otto.command "A?"
116
+ #puts "Response: #{resp}"
117
+ sleep 30
108
118
  end
109
119
  end
110
120
 
@@ -116,7 +126,7 @@ module SGS
116
126
  f.puts "/*\n * Autogenerated by #{__FILE__}.\n * DO NOT HAND-EDIT!\n */"
117
127
  constants.sort.each do |c|
118
128
  unless c == :ALARM_NAMES
119
- cval = SGS::Alarm.const_get(c)
129
+ cval = Alarm.const_get(c)
120
130
  str = "#define SGS_ALARM_#{c.to_s}"
121
131
  str += "\t" if str.length < 32
122
132
  str += "\t#{cval}\t/* #{alarm.name(cval)} */"
data/lib/sgs/bearing.rb CHANGED
@@ -1,4 +1,5 @@
1
1
  #
2
+ #
2
3
  # Copyright (c) 2013-2023, Kalopa Robotics Limited. All rights
3
4
  # reserved.
4
5
  #
@@ -48,7 +49,8 @@ module SGS
48
49
  attr_accessor :distance
49
50
 
50
51
  #
51
- # Create the Bearing instance.
52
+ # Create the Bearing instance. Angle is in radians, distance in nautical
53
+ # miles.
52
54
  def initialize(angle = 0.0, distance = 0.0)
53
55
  self.angle = angle.to_f
54
56
  self.distance = distance.to_f
@@ -72,6 +74,23 @@ module SGS
72
74
  rad.to_f * 180.0 / Math::PI
73
75
  end
74
76
 
77
+ #
78
+ # Convert boat angle (0->255) to radians. The boat uses an 8bit quantity
79
+ # to represent an angle, where 256 maps to 360 degrees. This makes angle
80
+ # arithmetic quite simple on an 8 bit processor, but useless for
81
+ # something which has a proper FPU. These two helper functions convert
82
+ # to and from radians.
83
+ def self.xtor(val)
84
+ val &= 0xff
85
+ val.to_f * Math::PI / 128.0
86
+ end
87
+
88
+ #
89
+ # Convert radians to hex-degrees.
90
+ def self.rtox(rad)
91
+ (rad.to_f * 128.0 / Math::PI).round.to_i & 0xff
92
+ end
93
+
75
94
  #
76
95
  # Handy function to re-adjust an angle away from negative
77
96
  def self.absolute(angle)
@@ -110,7 +129,7 @@ module SGS
110
129
  sin_dlon = Math.sin(loc2.longitude - loc1.longitude)
111
130
  cos_dlon = Math.cos(loc2.longitude - loc1.longitude)
112
131
  bearing.distance = Math.acos(sin_lat1*sin_lat2 + cos_lat1*cos_lat2*cos_dlon) *
113
- SGS::EARTH_RADIUS
132
+ EARTH_RADIUS
114
133
  y = sin_dlon * cos_lat2
115
134
  x = cos_lat1 * sin_lat2 - sin_lat1 * cos_lat2 * cos_dlon
116
135
  bearing.angle = Math.atan2(y, x)
@@ -141,10 +160,20 @@ module SGS
141
160
  Bearing.absolute(@angle - Math::PI)
142
161
  end
143
162
 
163
+ #
164
+ # Return the distance in metres
165
+ def distance_m
166
+ @distance * 1852.0
167
+ end
168
+
144
169
  #
145
170
  # Convert to a string
146
171
  def to_s
147
- "BRNG %03dd,%.3fnm" % [angle_d, @distance]
172
+ if @distance > 0.9
173
+ "BRNG %03dd,%.3fNM" % [angle_d, @distance]
174
+ else
175
+ "BRNG %03dd,%.1fm" % [angle_d, distance_m]
176
+ end
148
177
  end
149
178
  end
150
179
  end
data/lib/sgs/config.rb CHANGED
@@ -56,7 +56,6 @@ module SGS
56
56
  GPS.setup
57
57
  Otto.setup
58
58
  Timing.setup
59
- Waypoint.setup
60
59
  end
61
60
  end
62
61
  end
data/lib/sgs/course.rb CHANGED
@@ -149,11 +149,21 @@ module SGS
149
149
  end
150
150
 
151
151
  #
152
- # Compute a relative VMG based on the waypoint
152
+ # Compute a relative VMG based on the waypoint. If you're computing
153
+ # angles to one waypoint, the relative VMG will allow you to compare
154
+ # against each possible angle. If you're computing to different
155
+ # waypoints, then the distance from your position to that waypoint is
156
+ # taken into consideration so that waypoints further away have less
157
+ # impact on your VMG choice.
153
158
  def relative_vmg(waypt)
154
- relvmg = @speed * Math::cos(waypt.bearing.angle - @heading) / waypt.distance
155
- puts "Relative VMG to WPT: #{waypt.name} is #{relvmg}"
156
- relvmg
159
+ @speed * Math::cos(waypt.bearing.angle - @heading) / waypt.distance
160
+ end
161
+
162
+ #
163
+ # Compute the wind (as a Bearing) based on the compass heading and the
164
+ # apparent wind as reported.
165
+ def compute_wind
166
+ @wind.angle = @heading + @awa
157
167
  end
158
168
 
159
169
  #
@@ -161,7 +171,10 @@ module SGS
161
171
  # fast the boat will travel at the particular apparent wind angle.
162
172
  def compute_speed
163
173
  awa = @awa.abs
164
- return 0.0 if awa < 0.75
174
+ if awa < 0.75
175
+ @speed = 0.0
176
+ return
177
+ end
165
178
  ap = 1.0
166
179
  @speed = 0.0
167
180
  @polar_curve.each do |poly_val|
@@ -37,10 +37,11 @@
37
37
  # Routines for handling sailboat navigation and route planning.
38
38
  #
39
39
  module SGS
40
- class Diagnostics < RedisBase
40
+ class Diagnostics
41
41
  #
42
42
  # Main daemon function (called from executable)
43
43
  def self.daemon
44
+ puts "Diagnostics subsystem starting up..."
44
45
  loop do
45
46
  sleep 300
46
47
  end
data/lib/sgs/gps.rb CHANGED
@@ -32,10 +32,14 @@
32
32
  #
33
33
  # ABSTRACT
34
34
  #
35
+ require 'serialport'
36
+
35
37
  module SGS
36
38
  class GPS < RedisBase
37
- attr_accessor :time, :location, :sog, :cmg, :magvar
39
+ attr_accessor :time, :location, :sog, :cmg, :magvar, :val
38
40
 
41
+ #
42
+ # Create a new GPS record. Lat/Long are in radians.
39
43
  def initialize(lat = nil, long = nil)
40
44
  @time = Time.new(2000, 1, 1)
41
45
  @location = Location.new(lat, long)
@@ -49,11 +53,30 @@ module SGS
49
53
  #
50
54
  # Main daemon function (called from executable)
51
55
  def self.daemon
56
+ puts "GPS reader starting up..."
57
+ config = Config.load
58
+
59
+ sp = SerialPort.new config.gps_device, config.gps_speed
60
+ sp.read_timeout = 10000
61
+
52
62
  loop do
53
- sleep 300
63
+ nmea = NMEA.parse sp.readline
64
+ if nmea.is_gprmc?
65
+ gps = nmea.parse_gprmc
66
+ p gps
67
+ gps.save_and_publish if gps and gps.valid?
68
+ end
54
69
  end
55
70
  end
56
71
 
72
+ #
73
+ # Hard-code a GPS value (usually for debugging purposes)
74
+ def force(lat, long, time = nil)
75
+ @time = time || Time.now
76
+ @location = Location.new(lat, long)
77
+ @valid = true
78
+ end
79
+
57
80
  #
58
81
  # Set the validity
59
82
  def is_valid
@@ -65,5 +88,15 @@ module SGS
65
88
  def valid?
66
89
  @valid == true
67
90
  end
91
+
92
+ #
93
+ # Display the GPS data as a useful string (in degrees)
94
+ def to_s
95
+ if valid?
96
+ "@#{@time.strftime('%Y%m%d-%T')}, #{@location}, SOG:#{@sog}, CMG:#{@cmg}"
97
+ else
98
+ "GPS error"
99
+ end
100
+ end
68
101
  end
69
102
  end
data/lib/sgs/location.rb CHANGED
@@ -56,7 +56,7 @@ module SGS
56
56
  attr_accessor :latitude, :longitude
57
57
 
58
58
  #
59
- # Create the Location instance.
59
+ # Create the Location instance. Latitude and longitude passed in radians.
60
60
  def initialize(lat = nil, long = nil)
61
61
  @latitude = lat.to_f if lat
62
62
  @longitude = long.to_f if long
@@ -65,7 +65,6 @@ module SGS
65
65
  #
66
66
  # The difference between two locations is a Bearing
67
67
  def -(loc)
68
- puts "Distance from #{self} to #{loc}"
69
68
  Bearing.compute(self, loc)
70
69
  end
71
70
 
@@ -84,8 +83,8 @@ module SGS
84
83
  loc = Location.new
85
84
  sin_angle = Math.sin(bearing.angle)
86
85
  cos_angle = Math.cos(bearing.angle)
87
- sin_dstr = Math.sin(bearing.distance / SGS::EARTH_RADIUS)
88
- cos_dstr = Math.cos(bearing.distance / SGS::EARTH_RADIUS)
86
+ sin_dstr = Math.sin(bearing.distance / EARTH_RADIUS)
87
+ cos_dstr = Math.cos(bearing.distance / EARTH_RADIUS)
89
88
  sin_lat1 = Math.sin(@latitude)
90
89
  cos_lat1 = Math.cos(@latitude)
91
90
  loc.latitude = Math.asin(sin_lat1*cos_dstr + cos_lat1*sin_dstr*cos_angle)
@@ -113,10 +112,33 @@ module SGS
113
112
  end
114
113
 
115
114
  #
116
- # Set the lat/long from a hash.
115
+ # Parse the lat/long values passed as a string. This function
116
+ # should be able to handle any type of lat/long string, in most
117
+ # general formats. See :to_s for examples.
117
118
  def parse(data)
118
- @latitude = ll_parse(data["latitude"], "NS")
119
- @longitude = ll_parse(data["longitude"], "EW")
119
+ llvals = data.split /,/
120
+ if llvals.count == 1
121
+ #
122
+ # Must be space-separated. Try that...
123
+ llvals = data.split(/ /)
124
+ if llvals.count != 2
125
+ raise ArgumentError.new "Cannot split lat/long values"
126
+ end
127
+ elsif llvals.count != 2
128
+ #
129
+ # Too many comma separators.
130
+ raise ArgumentError.new "Invalid lat/long values"
131
+ end
132
+ self.latitude_d = _ll_parse(llvals[0], "NS")
133
+ self.longitude_d = _ll_parse(llvals[1], "EW")
134
+ true
135
+ end
136
+
137
+ #
138
+ # Parse the lat/long from a hash object.
139
+ def parse_hash(data = {})
140
+ self.latitude_d = _ll_parse(data["latitude"], "NS")
141
+ self.longitude_d = _ll_parse(data["longitude"], "EW")
120
142
  end
121
143
 
122
144
  #
@@ -128,15 +150,21 @@ module SGS
128
150
  #
129
151
  # Convert the lat/long to a hash.
130
152
  def to_hash
131
- {"latitude" => ll_to_s(latitude, "NS"),
132
- "longitude" => ll_to_s(longitude, "EW")}
153
+ {"latitude" => latitude_d.round(6), "longitude" => longitude_d.round(6)}
133
154
  end
134
155
 
135
156
  #
136
- # Display the lat/long as a useful string (in degrees).
137
- def to_s
157
+ # Display the lat/long as a useful string (in degrees). Output
158
+ # formats are as follows (default is :d):
159
+ # :d "48.104051, -7.282614"
160
+ # :dd "48.104051N, 7.282614W"
161
+ # :dmm "48 6.243060N, 7 16.956840W"
162
+ # :dms "48 6 14.583600N, 7 16 57.410400W"
163
+ def to_s(opts = {})
138
164
  if valid?
139
- "%s, %s" % [ll_to_s(@latitude, "NS"), ll_to_s(@longitude, "EW")]
165
+ lat_str = _ll_conv(latitude_d.round(6), "NS", opts)
166
+ lon_str = _ll_conv(longitude_d.round(6), "EW", opts)
167
+ "#{lat_str}, #{lon_str}"
140
168
  else
141
169
  "unknown"
142
170
  end
@@ -151,29 +179,39 @@ module SGS
151
179
  end
152
180
 
153
181
  #
154
- # Helper functions for working in degrees.
182
+ # Return the latitude in degrees.
155
183
  def latitude_d
156
184
  Bearing.rtod @latitude
157
185
  end
158
186
 
187
+ #
188
+ # Set the latitude using a value in degrees.
159
189
  def latitude_d=(val)
160
190
  @latitude = Bearing.dtor val
161
191
  end
162
192
 
193
+ #
194
+ # Produce a latitude array for NMEA output.
163
195
  def latitude_array(fmt = nil)
164
- make_ll_array latitude_d, "NS", fmt
196
+ _make_ll_array latitude_d, "NS", fmt
165
197
  end
166
198
 
199
+ #
200
+ # Return the longitude in degrees.
167
201
  def longitude_d
168
202
  Bearing.rtod @longitude
169
203
  end
170
204
 
205
+ #
206
+ # Set the longitude using a value in degrees.
171
207
  def longitude_d=(val)
172
208
  @longitude = Bearing.dtor val
173
209
  end
174
210
 
211
+ #
212
+ # Produce a longitude array for NMEA output.
175
213
  def longitude_array(fmt = nil)
176
- make_ll_array longitude_d, "EW", fmt
214
+ _make_ll_array longitude_d, "EW", fmt
177
215
  end
178
216
 
179
217
  #
@@ -182,53 +220,71 @@ module SGS
182
220
  Bearing.compute(self, loc)
183
221
  end
184
222
 
185
- private
186
- #
187
- # Parse a string into a lat or long. The latitude or longitude string
188
- # should be of the format dd mm ss.sssssC (where C is NS or EW). Note
189
- # that latitude and longitude are saved internally in radians.
190
- def ll_parse(value, nsew)
191
- args = value.split
192
- dir = args[-1].gsub(/[\d\. ]+/, '').upcase
193
- args.map! {|val| val.to_f}
194
- val = args.shift
195
- val = val + args.shift / 60.0 if args.length > 0
196
- val = val + args.shift / 3600.0 if args.length > 0
197
- Bearing.dtor val * ((nsew.index(dir) == 1) ? -1 : 1)
198
- end
223
+ private
224
+ #
225
+ # Parse a latitude or longitude value, from a wide range of
226
+ # formats. Should handle D.ddd, D M.mmm and D M S.sss values.
227
+ # Can also strip out the special degrees unicode, as well as
228
+ # single and double quotes.
229
+ def _ll_parse(arg, nsew)
230
+ str = arg.chomp.gsub /[\u00B0'"]/, ' '
231
+ if str[-1].upcase =~ /[#{nsew}]/
232
+ sign = (str[-1].upcase == nsew[1]) ? -1 : 1
233
+ str[-1] = ' '
234
+ else
235
+ sign = 1
236
+ end
237
+ args = str.split
238
+ raise ArgumentError.new "Cannot parse lat/long value" if args.count > 3
239
+ value = 0.0
240
+ (args.count - 1).downto(0).each do |idx|
241
+ value = args[idx].to_f + value / 60.0
242
+ end
243
+ sign * value
244
+ end
199
245
 
200
- #
201
- # Convert a latitude or longitude into an ASCII string of the form:
202
- # dd mm ss.ssssssC (where C is NS or EW). The value should be in
203
- # radians.
204
- def ll_to_s(val, str)
205
- val = Bearing.rtod val
206
- if val < 0.0
207
- chr = str[1]
208
- val = -val
209
- else
210
- chr = str[0]
246
+ #
247
+ # Convert a latitude/longitude to a string. Can specify
248
+ # multiple output formats such as :d, :dd, :dmm, :dms to
249
+ # format according to various different styles.
250
+ def _ll_conv(value, nsew, opts)
251
+ format = opts[:format] || :d
252
+ return "%.6f" % value if format == :d
253
+ if value < 0.0
254
+ suffix = nsew[1]
255
+ value = -value
256
+ else
257
+ suffix = nsew[0]
258
+ end
259
+ case opts[:format] || :d
260
+ when :dd
261
+ "%.6f%s" % [value, suffix]
262
+ when :dmm
263
+ dd = value.to_i
264
+ mm = (value - dd.to_f) * 60.0
265
+ "%d %.6f%s" % [dd, mm, suffix]
266
+ when :dms
267
+ dd = value.to_i
268
+ value = (value - dd.to_f) * 60.0
269
+ mm = value.to_i
270
+ ss = (value - mm.to_f) * 60.0
271
+ "%d %d %.6f%s" % [dd, mm, ss, suffix]
272
+ end
211
273
  end
212
- deg = val.to_i
213
- val = (val - deg.to_f) * 60.0
214
- min = val.to_i
215
- sec = (val - min.to_f) * 60.0
216
- "%d %d %8.6f%c" % [deg, min, sec, chr]
217
- end
218
274
 
219
- #
220
- # Create a Lat/Long array suitable for an NMEA output
221
- def make_ll_array(val, nsew, fmt = nil)
222
- fmt ||= "%02d%07.4f"
223
- if (val < 0)
224
- val = -val
225
- ne = nsew[1]
226
- else
227
- ne = nsew[0]
275
+ #
276
+ # Create a Lat/Long array suitable for an NMEA output.
277
+ def _make_ll_array(val, nsew, fmt = nil)
278
+ fmt ||= "%02d%07.4f"
279
+ if (val < 0)
280
+ val = -val
281
+ suffix = nsew[1]
282
+ else
283
+ suffix = nsew[0]
284
+ end
285
+ deg = val.to_i
286
+ val = (val - deg) * 60
287
+ [fmt % [deg, val], suffix.chr]
228
288
  end
229
- deg = val.to_i
230
- val = (val - deg) * 60
231
- [fmt % [deg, val], ne.chr]
232
- end
233
289
  end
234
290
  end
data/lib/sgs/logger.rb CHANGED
@@ -37,13 +37,6 @@
37
37
  # Routines for handling sailboat navigation and route planning.
38
38
  #
39
39
  module SGS
40
- class Logger < RedisBase
41
- #
42
- # Main daemon function (called from executable)
43
- def self.daemon
44
- loop do
45
- sleep 300
46
- end
47
- end
40
+ class Logger
48
41
  end
49
42
  end