Skip to content

Commit

Permalink
Improve filter specification in ubx_filter.rb
Browse files Browse the repository at this point in the history
  • Loading branch information
@fenrir-naru
fenrir-naru committed Oct 24, 2023
1 parent faa867a commit 81ba42b
Showing 1 changed file with 56 additions and 39 deletions.
95 changes: 56 additions & 39 deletions tool/ubx_filter.rb
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,30 +35,39 @@
require 'ubx'

class UBX_Filter
ANY = Object::new.tap{|x| x.define_singleton_method(:==){|another| true}}
FILTERS = {
:pass_all => proc{|packet, prop| packet},
:drop_all => proc{|packet, prop| nil},
:drop_measurement => proc{|drop_satellites|
drop_satellites = drop_satellites.collect{|item|
next item unless item.kind_of?(Array) # PRN only
gnss = item[0]
if gnss.kind_of?(Integer)
gnss = UBX::GNSS_ID.invert[gnss]
else
item[0] = UBX::GNSS_ID[gnss] # Symbol => int
#list_legacy, list_current
drop_list, drop_list_legacy = drop_satellites.collect{|item|
gnss_svid, svid_legacy = case item
when :all
[[ANY] * 3, ANY]
when Integer # PRN only
gnss_sym, svid = UBX::gnss_svid(item)
[[UBX::GNSS_ID.invert[gnss_sym], svid, ANY], item]
when Array
gnss_sym, gnss_int = if item[0].kind_of?(Symbol) then
[item[0], UBX::GNSS_ID[item[0]]]
else
[UBX::GNSS_ID.invert[item[0]], item[0]]
end
svid, svid_legacy = if item[1] == :all then
[ANY, Object::new.tap{|x| x.define_singleton_method(:==){|svid|
gnss_sym == UBX::gnss_svid(svid)[0]
}}]
else
[item[1], UBX::svid(item[1], gnss_sym)]
end
sigid = case item[2]
when Symbol; UBX::SIGNAL_ID[gnss_sym][item[2]]
else; item[2] || ANY
end
[[gnss_int, svid, sigid], svid_legacy]
end
item[2] = UBX::SIGNAL_ID[gnss][item[2]] if item[2] && item[2].kind_of?(Symbol) # Symbol => int
item
}.collect{|item|
# Handle special keyword :all
if item == :all then # global :all
Class::new{def ==(another); true; end}::new
elsif item.kind_of?(Array) && item[1] == :all # :all for each GNSS
Class::new{GNSS = item[0]; def ==(another); another[0] == GNSS; end}::new
else
item
end
}
}.transpose
proc{|packet, prop|
idx_measurements = nil
len_per_meas = nil
Expand All @@ -67,17 +76,21 @@ class UBX_Filter
idx_measurements = 6 + 6
len_per_meas = 24
packet[idx_measurements].times.collect{|i|
next nil unless drop_satellites.include?(packet[6 + 28 + (i * len_per_meas)]) # check PRN
next nil unless drop_list_legacy.any?{|cmp|
cmp === packet[6 + 28 + (i * len_per_meas)] # check PRN
}
(6 + 8 + (i * len_per_meas))
}.compact
when [0x02, 0x15] # RXM-RAWX
idx_measurements = 6 + 11
len_per_meas = 32
packet[idx_measurements].times.collect{|i|
gnss_sv_sig_freq = packet[6 + 36 + (i * len_per_meas), 4] # check gnssID, svID, signalID, freqID
next nil unless (drop_satellites.include?(gnss_sv_sig_freq[0..1]) ||
drop_satellites.include?(gnss_sv_sig_freq[0..2]) ||
drop_satellites.include?(gnss_sv_sig_freq))
gnss_sv_sig_freq = packet[6 + 36 + (i * len_per_meas), 3] # check gnssID, svID, signalID
next nil unless drop_list.any?{|cmp|
(cmp[0] === gnss_sv_sig_freq[0]) &&
(cmp[1] === gnss_sv_sig_freq[1]) &&
(cmp[2] === gnss_sv_sig_freq[2])
}
(6 + 16 + (i * len_per_meas))
}.compact
else
Expand All @@ -92,18 +105,13 @@ class UBX_Filter
}
},
:drop_ubx => proc{|class_id_list|
any = Class::new{def ==(another); true; end}::new
class_id_list.collect!{|item|
case item
when "all"; any
else
res = Integer(item)
(res >= 0x100) ? res.divmod(0x100) : res
end
([(:all == item) ? ANY : item].flatten(1) + [ANY])[0..1]
}
proc{|packet, prop|
next nil if class_id_list.include?(prop[:class_id]) \
|| class_id_list.include?(prop[:class_id][0])
next nil if class_id_list.any?{|class_id|
(class_id[0] === prop[:class_id][0]) && (class_id[1] === prop[:class_id][1])
}
packet
}
},
Expand Down Expand Up @@ -179,6 +187,7 @@ def run(&b)
# The default filter is to pass all packets
drop,GPS:01 # (text after # is comment) always drop GPS 1
drop,GPS:04,GPS:05 # always drop GPS 4, 5 (multiple satellites in a line)
# same as drop,GPS:4..5 (range specification is supported)
drop,QZSS:all # always drop any QZSS
event,1000,drop,GPS:02
# after GPS time 1000[s] (any week) (i.e., GPS_sec > 1000), dropping GPS:02 is activated
Expand All @@ -203,8 +212,6 @@ def run(&b)
drop_ubx,all # drop All packets
drop_ubx,0x01 # drop All NAV packets
drop_ubx,0x0102,0x0112 # drop NAV-POSLLH,VELNED packets
Note: For old ubx, just PRN number (without satellite system) like "1" (<-instead of "GPS:01") works well.
__STRING__
exit(0)
else
Expand Down Expand Up @@ -249,10 +256,11 @@ def run(&b)

proc{ # Build filter elements
parse_sat = proc{|spec|
spec =~ /^ *(?:([A-Za-z]+):?)?(\d+|all)/ # each satellite, (GNSS:)sat and special keyword, "all"
gnss, sat = [$1, ($2 == 'all') ? :all : $2.to_i]
raise "Invalid spec: #{spec}" unless spec =~ /^ *(?:([A-Za-z]+):?)?(?:(\d+)(?:\.\.(\d+))?|(all))/
# each satellite, (GNSS:)sat and special keyword, "all"
gnss, sat = [$1, $4 ? $4.to_sym : ($3 ? ($2.to_i)..($3.to_i) : ($2.to_i))]
next sat unless gnss
raise "Unknown satellites: #{spec}" unless gnss = {
raise "Unknown system: #{spec}" unless gnss = {
'G' => :GPS, 'S' => :SBAS, 'E' => :Galileo, 'B' => :Beisou, 'Q' => :QZSS, 'R' => :GLONASS,
'GPS' => :GPS, 'SBAS' => :SBAS, 'GALILEO' => :Galileo,
'BEIDOU' => :Beisou, 'QZSS' => :QZSS, 'GLONASS' => :GLONASS,
Expand All @@ -265,7 +273,16 @@ def run(&b)
when :drop
[UBX_Filter::FILTERS[:drop_measurement].call(specs.collect{|spec| parse_sat.call(spec)})]
when :drop_ubx
[UBX_Filter::FILTERS[:drop_ubx].call(specs)]
[UBX_Filter::FILTERS[:drop_ubx].call(specs.collect{|spec|
raise "Invalid spec: #{spec}" unless \
spec =~ /^ *(?:(?:(0x[\dA-Fa-f]+)|(\d+))(?:\.\.(?:(0x[\dA-Fa-f]+)|(\d+)))?|(all))/
next $5.to_sym if $5
u1, l1 = ($1 ? $1.to_i(16) : $2.to_i).divmod(0x100)
next (u1 == 0 ? l1 : [u1, l1]) unless ($3 || $4)
u2, l2 = ($3 ? $3.to_i(16) : $4.to_i).divmod(0x100)
raise if u1 != u2
u1 == 0 ? l1..l2 : [u1, l1..l2]
})]
#when :pass # ignore, because default is to accept all packets
else
[]
Expand Down

0 comments on commit 81ba42b

Please sign in to comment.