From 5c27f341efdc1111cb476ca1a263bb4ba35b291c Mon Sep 17 00:00:00 2001 From: fenrir Date: Tue, 15 Aug 2023 16:08:11 +0900 Subject: [PATCH 01/25] Fix sdcc build error in newer GCC --- scripts/build-sdcc.sh | 31 +++++++++++++++++++++++++++++++ 1 file changed, 31 insertions(+) diff --git a/scripts/build-sdcc.sh b/scripts/build-sdcc.sh index 49c16c83b..4f7279c41 100644 --- a/scripts/build-sdcc.sh +++ b/scripts/build-sdcc.sh @@ -126,6 +126,37 @@ __PATCH_LINE__ ;; esac +case "${SDCC}" in +3.3.*) +patch -u -N -p1 <<'__PATCH_LINE__' || let "$?<=1" +diff -uprN sdcc-3.3.0.orig/support/sdbinutils/bfd/Makefile.in sdcc-3.3.0/support/sdbinutils/bfd/Makefile.in +--- sdcc-3.3.0.orig/support/sdbinutils/bfd/Makefile.in 2012-11-08 16:44:00.000000000 +0900 ++++ sdcc-3.3.0/support/sdbinutils/bfd/Makefile.in 2023-08-15 15:42:47.661249300 +0900 +@@ -239,7 +239,7 @@ STRIP = @STRIP@ + TDEFINES = @TDEFINES@ + USE_NLS = @USE_NLS@ + VERSION = @VERSION@ +-WARN_CFLAGS = @WARN_CFLAGS@ ++WARN_CFLAGS = @WARN_CFLAGS@ -Wno-format-overflow -Wno-pointer-compare -Wno-implicit-fallthrough -Wno-unused-const-variable -Wno-cast-function-type + XGETTEXT = @XGETTEXT@ + abs_builddir = @abs_builddir@ + abs_srcdir = @abs_srcdir@ +diff -uprN sdcc-3.3.0.orig/support/sdbinutils/binutils/Makefile.in sdcc-3.3.0/support/sdbinutils/binutils/Makefile.in +--- sdcc-3.3.0.orig/support/sdbinutils/binutils/Makefile.in 2012-11-08 23:22:15.000000000 +0900 ++++ sdcc-3.3.0/support/sdbinutils/binutils/Makefile.in 2023-08-15 15:44:30.335377200 +0900 +@@ -264,7 +264,7 @@ SHELL = @SHELL@ + STRIP = @STRIP@ + USE_NLS = @USE_NLS@ + VERSION = @VERSION@ +-WARN_CFLAGS = @WARN_CFLAGS@ ++WARN_CFLAGS = @WARN_CFLAGS@ -Wno-format-overflow -Wno-pointer-compare -Wno-implicit-fallthrough -Wno-unused-const-variable -Wno-cast-function-type + XGETTEXT = @XGETTEXT@ + YACC = `if [ -f ../bison/bison ]; then echo ../bison/bison -y -L$(srcdir)/../bison/; else echo @YACC@; fi` + YFLAGS = -d +__PATCH_LINE__ +;; +esac + DISABLE_DEVICES="z80 z180 r2k r3ka gbz80 ds390 ds400 pic14 pic16 hc08 s08" CONFIGURE_OPT="--prefix=${SDCC_DIR} --disable-ucsim --disable-sdcdb --disable-non-free" case "${SDCC}" in From f1205d329b171e78f1d4937f7873e3f92d552666 Mon Sep 17 00:00:00 2001 From: fenrir Date: Wed, 23 Aug 2023 23:12:25 +0900 Subject: [PATCH 02/25] Add MS5803-01/30 converter to log_CSV --- tool/log_CSV.cpp | 99 ++++++++++++++++++++++++++++++++++++++++++------ 1 file changed, 88 insertions(+), 11 deletions(-) diff --git a/tool/log_CSV.cpp b/tool/log_CSV.cpp index 242ae651e..6dd7fc2d0 100644 --- a/tool/log_CSV.cpp +++ b/tool/log_CSV.cpp @@ -466,19 +466,27 @@ class StreamProcessor : public SylphideProcessor { void (HandlerP::*formatter)( const float_sylph_t ¤t, const int &index, const Int32 &pressure, const Int32 &temperature) const; - void ms5611_convert( - const Int32 &d1, const Int32 &d2, - Int32 &pressure, Int32 &temperature, - const Uint16 (&coef)[6]) const { - Int32 dT(d2 - (coef[4] << 8)); + + static void ms5XXX_convert( + const Int32 &d1, const Int32 &d2, const Uint16 (&coef)[6], + Int32 &dT, Int32 &temperature, int64_t &off, int64_t &sens){ + dT = d2 - (coef[4] << 8); temperature = (Int32)2000 + (((int64_t)dT * coef[5]) >> 23); - int64_t off(((int64_t)coef[1] << 16) + (((int64_t)coef[3] * dT) >> 7)); - int64_t sens(((int64_t)coef[0] << 15) + (((int64_t)coef[2] * dT) >> 8)); + off = ((int64_t)coef[1] << 16) + (((int64_t)coef[3] * dT) >> 7); + sens = ((int64_t)coef[0] << 15) + (((int64_t)coef[2] * dT) >> 8); + } + static void ms5611_convert( + const Int32 &d1, const Int32 &d2, + Int32 &pressure, Int32 &temperature, + const Uint16 (&coef)[6]){ + Int32 dT; + int64_t off, sens; + ms5XXX_convert(d1, d2, coef, dT, temperature, off, sens); // Figure 3 if(temperature < 2000){ - Int32 t2(((int64_t)dT * dT) << 31); + Int32 t2(((int64_t)dT * dT) >> 31); Int32 dT2(temperature - 2000), dT2_2(dT2 * dT2); Int32 off2((dT2_2 * 5) >> 1); Int32 sens2((dT2_2 * 5) >> 2); @@ -494,6 +502,66 @@ class StreamProcessor : public SylphideProcessor { pressure = (Int32)(((((int64_t)d1 * sens) >> 21) - off) >> 15); } + static void ms5803_01_convert( + const Int32 &d1, const Int32 &d2, + Int32 &pressure, Int32 &temperature, + const Uint16 (&coef)[6]){ + Int32 dT; + int64_t off, sens; + ms5XXX_convert(d1, d2, coef, dT, temperature, off, sens); + + // Figure 16 + if(temperature < 2000){ + Int32 t2(((int64_t)dT * dT) >> 31); + Int32 dT2(temperature - 2000), dT2_2(dT2 * dT2); + Int32 off2(dT2_2 * 3); + Int32 sens2((dT2_2 * 7) >> 3); + if(temperature < -1500){ + Int32 dT3(temperature + 1500), dT3_2(dT3 * dT3); + sens2 += (dT3_2 * 2); + } + temperature -= t2; + off -= off2; + sens -= sens2; + }else if(temperature >= 4500){ + Int32 dT2(temperature - 4500), dT2_2(dT2 * dT2); + Int32 sens2(-(dT2_2 >> 3)); + sens -= sens2; + } + + pressure = (Int32)(((((int64_t)d1 * sens) >> 21) - off) >> 15); + } + static void ms5803_30_convert( + const Int32 &d1, const Int32 &d2, + Int32 &pressure, Int32 &temperature, + const Uint16 (&coef)[6]){ + Int32 dT; + int64_t off, sens; + ms5XXX_convert(d1, d2, coef, dT, temperature, off, sens); + + // Figure 16 + Int32 dT2(temperature - 2000), dT2_2(dT2 * dT2); + if(temperature < 2000){ + Int32 t2(((int64_t)dT * dT * 3) >> 33); + Int32 off2((dT2_2 * 3) >> 1); + Int32 sens2((dT2_2 * 7) >> 3); + if(temperature < -1500){ + Int32 dT3(temperature + 1500), dT3_2(dT3 * dT3); + off2 += (dT3_2 * 7); + sens2 += (dT3_2 * 4); + } + temperature -= t2; + off -= off2; + sens -= sens2; + }else{ + Int32 t2(((int64_t)dT * dT * 7) >> 37); + Int32 off2(dT2_2 >> 4); + temperature -= t2; + off -= off2; + } + + pressure = (Int32)(((((int64_t)d1 * sens) >> 21) - off) >> 13) * 10; + } /** * check P page (pressure sensor) @@ -507,7 +575,9 @@ class StreamProcessor : public SylphideProcessor { if(!options.is_time_in_range(current)){return;} switch(options.page_P_mode){ - case 5: { // MS5611 with coefficients + case 5: // MS5611 with coefficients + case 6: // MS5803-01 with coefficients + case 7: { // MS5803-30 with coefficients Uint16 coef[6]; char buf[sizeof(Uint16)]; for(int i(0); i < sizeof(coef) / sizeof(coef[0]); i++){ @@ -524,7 +594,14 @@ class StreamProcessor : public SylphideProcessor { d1(be_char4_2_num(buf[0][0])), d2(be_char4_2_num(buf[1][0])); Int32 pressure, temperature; - ms5611_convert(d1, d2, pressure, temperature, coef); + + void (*converter[])( + const Int32 &d1, const Int32 &d2, + Int32 &pressure, Int32 &temperature, + const Uint16 (&coef)[6]) = { + ms5611_convert, ms5803_01_convert, ms5803_30_convert, + }; + (*(converter[options.page_P_mode - 5]))(d1, d2, pressure, temperature, coef); (this->*formatter)(current, j, pressure, temperature); } break; @@ -757,7 +834,7 @@ class StreamProcessor : public SylphideProcessor { case mark: if(cnd){ \ super_t::process_packet( \ buf, buf_size, \ - observer_ ## type , previous_seek_next_ ## type, handler_ ## type); \ + observer_ ## type, previous_seek_next_ ## type, handler_ ## type); \ } \ break; #define assign_case(type, mark) \ From 5b3341691709cd27e0d04cd5dfb7b95455230864 Mon Sep 17 00:00:00 2001 From: fenrir Date: Tue, 29 Aug 2023 23:46:26 +0900 Subject: [PATCH 03/25] Add PoR to MS5611 initialization to fix read failure of PROM --- firmware/ms5611.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/firmware/ms5611.c b/firmware/ms5611.c index d130defd9..7bc09517e 100644 --- a/firmware/ms5611.c +++ b/firmware/ms5611.c @@ -58,6 +58,8 @@ static void send_command(u8 cmd){ } void ms5611_init(){ + send_command(0x1E); // reset + { // Read Coefficient 1-6 from PROM u8 i, cmd; __xdata u8 *buf; From d5ec3c43d4dd5794df47d6c73f78eafabafc2fba Mon Sep 17 00:00:00 2001 From: fenrir Date: Tue, 26 Sep 2023 23:13:14 +0900 Subject: [PATCH 04/25] Fix log_mixer.rb to ignore leading spaces in imu_data(CSV) --- tool/log_mixer.rb | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tool/log_mixer.rb b/tool/log_mixer.rb index ff9abdf92..75bc9b8d4 100644 --- a/tool/log_mixer.rb +++ b/tool/log_mixer.rb @@ -162,7 +162,7 @@ def initialize(io, opt = {}) end def read_chunk while !@io.eof? - items = @io.readline.split(/[,\s]+/) # space, tab or comma + items = @io.readline.split(/\b[,\s]+/) # space, tab or comma items.collect!{|v| Float(v)} rescue next t = (items[@t_index] * @t_scale) + @t_offset accel = items.values_at(*@acc_index).zip(@acc_units).collect{|v, sf| v * sf} From 673605896f832b95a754f6f3caad6b01a357b742 Mon Sep 17 00:00:00 2001 From: fenrir Date: Thu, 28 Sep 2023 17:40:02 +0900 Subject: [PATCH 05/25] Add comment for old ubx support --- tool/ubx_filter.rb | 1 + 1 file changed, 1 insertion(+) diff --git a/tool/ubx_filter.rb b/tool/ubx_filter.rb index 1be0038b2..0dc5def75 100644 --- a/tool/ubx_filter.rb +++ b/tool/ubx_filter.rb @@ -177,6 +177,7 @@ def run(&b) event,2100:2000,drop,GPS:03 # start dropping GPS 3 after GPS time 2100[week] 20000[s] + Note: For old ubx, just PRN number (without satellite system) like "1" (<-instead of "GPS:01") works well. __STRING__ options = { From ee2d58fe5d8bef716ea7bf4a2d00596dab83c0e8 Mon Sep 17 00:00:00 2001 From: fenrir Date: Tue, 3 Oct 2023 09:35:44 +0900 Subject: [PATCH 06/25] Add drop_ubx specifier to ubx_filter.rb --- tool/ubx_filter.rb | 31 +++++++++++++++++++++++++++---- 1 file changed, 27 insertions(+), 4 deletions(-) diff --git a/tool/ubx_filter.rb b/tool/ubx_filter.rb index 0dc5def75..36083c503 100644 --- a/tool/ubx_filter.rb +++ b/tool/ubx_filter.rb @@ -91,6 +91,22 @@ class UBX_Filter UBX::update(packet) } }, + :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 + } + proc{|packet, prop| + next nil if class_id_list.include?(prop[:class_id]) \ + || class_id_list.include?(prop[:class_id][0]) + packet + } + }, } def initialize(io, opt = {}) @@ -135,10 +151,10 @@ def run(&b) while true break unless (@prop[:packet] = @ubx.read_packet) @prop[:class_id] = @prop[:packet][2..3] - @prop[:itow] = (ITOW_PARSER[@prop[:packet][2..3]].call(@prop[:packet]) rescue @prop[:itow]) - @prop[:week] = (WEEK_PARSER[@prop[:packet][2..3]].call(@prop[:packet]) rescue @prop[:week]) + @prop[:itow] = (ITOW_PARSER[@prop[:class_id]].call(@prop[:packet]) rescue @prop[:itow]) + @prop[:week] = (WEEK_PARSER[@prop[:class_id]].call(@prop[:packet]) rescue @prop[:week]) b.call(@prop) if b - next unless filtered = @prop[:gates].inject(@prop[:packet]){|packet, gate| + return "" unless filtered = @prop[:gates].inject(@prop[:packet]){|packet, gate| break nil unless packet gate.call(packet, @prop) } @@ -177,6 +193,11 @@ def run(&b) event,2100:2000,drop,GPS:03 # start dropping GPS 3 after GPS time 2100[week] 20000[s] + Selection with ubx packet type is performed with drop_ubx, the below is example; + 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__ @@ -191,7 +212,7 @@ def run(&b) when :cmd act, *specs = v.split(/ *, */) options[:cmd] << [act.to_sym, specs] - when :drop, :pass, :event + when :drop, :drop_ubx, :pass, :event options[:cmd] << [k, v.split(/ *, */)] when :cmdfile open(v, 'r').each{|line| @@ -231,6 +252,8 @@ def run(&b) case act 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)] #when :pass # ignore, because default is to accept all packets else [] From 3b025c08f317f78139f4938a1e0b94218d5b523e Mon Sep 17 00:00:00 2001 From: fenrir Date: Tue, 3 Oct 2023 11:29:13 +0900 Subject: [PATCH 07/25] Support '-' argument as stdin input in log_mixer --- tool/log_mixer.rb | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/tool/log_mixer.rb b/tool/log_mixer.rb index 75bc9b8d4..5a6e097d8 100644 --- a/tool/log_mixer.rb +++ b/tool/log_mixer.rb @@ -236,8 +236,10 @@ def read_chunk :imu => ARGV.shift, } $log_mix.call({:readers => [ - IMU_CSV::new(open(src[:imu], 'r')), - GPS_UBX::new(open(src[:gps], 'rb')), + IMU_CSV::new((src[:imu] == '-') ? $stdin : open(src[:imu], 'r')), + GPS_UBX::new((src[:gps] == '-') \ + ? proc{STDIN.binmode; $stdin}.call \ + : open(src[:gps], 'rb')), ]}) end \ No newline at end of file From a4d996a023ba4346f9720f51e4871e0ecfc0688a Mon Sep 17 00:00:00 2001 From: fenrir Date: Sat, 21 Oct 2023 11:58:39 +0900 Subject: [PATCH 08/25] Improve ubx_filter to accept multiple commands for specific time --- tool/ubx_filter.rb | 26 +++++++++++++++++++++----- 1 file changed, 21 insertions(+), 5 deletions(-) diff --git a/tool/ubx_filter.rb b/tool/ubx_filter.rb index 36083c503..a5c97feb8 100644 --- a/tool/ubx_filter.rb +++ b/tool/ubx_filter.rb @@ -169,6 +169,10 @@ def run(&b) $stderr.puts <<-__STRING__ UBX filter Usage: #{__FILE__} [--option_key[=option_value]] [original.ubx, otherwise stdin] > filtered.ubx +__STRING__ + +if ARGV.any?{|arg| arg =~ /^--help/} then + $stderr.puts <<-__STRING__ --cmdfile=cmdfile.txt is the most useful option. Example cmdfile.txt content is the following. @@ -182,10 +186,12 @@ def run(&b) # Note: GPS 1,4,5, and any QZSS has still been dropped # Note2: If this filter is intended to be active no less than 1000 (i.e, >= 1000), # pleas use smaller seconds such as 999.9. + event,1000,drop,GPS:06 + # Additionally GPS 6 will be dropped from GPS time 1000[s]. event,2000,drop,GPS:03 # after GPS time 2000[s] (any week), dropping GPS:03 is activated # In addition, all previous event commands are cleared, - # which means stop dropping GPS:02. + # which means stop dropping GPS:02 and GPS:06. # Note: GPS 1,4,5, and any QZSS has still been dropped event,3000,pass,all # after GPS time 3000[s] (any week), @@ -200,13 +206,19 @@ def run(&b) Note: For old ubx, just PRN number (without satellite system) like "1" (<-instead of "GPS:01") works well. __STRING__ + exit(0) +else + $stderr.puts <<-__STRING__ + --help shows the details. +__STRING__ +end options = { :out => nil, :cmd => [], } ARGV.reject!{|arg| - if arg =~ /--([^=]+)=?/ then + if arg =~ /^--([^=]+)=?/ then k, v = [$1.to_sym, $'] case k when :cmd @@ -270,12 +282,15 @@ def run(&b) else # Other time spec GPSTime::new(specs[0]).to_a end - events << [t, *make_gates.call(specs[1].to_sym, specs[2..-1])] + if idx_last = events.find_index{|event| event[0] == t} then + events[idx_last] += make_gates.call(specs[1].to_sym, specs[2..-1]) + else + events << [t, *make_gates.call(specs[1].to_sym, specs[2..-1])] + end else gates += make_gates.call(act, specs) end } - $stderr.puts events.inspect generate_scenario = proc{|event| next nil unless event @@ -311,7 +326,8 @@ def run(&b) end }.call, {:gates => gates}) -dest = if options[:out] then +dest = case options[:out] +when String $stderr.puts "#{File::exist?(options[:out]) ? 'Appending' : 'Writing'} to #{options[:out]} ..." open(options[:out], 'wb+') else From faa867a4ef9641c0d73329bbf3623320deff2726 Mon Sep 17 00:00:00 2001 From: fenrir Date: Mon, 23 Oct 2023 16:13:18 +0900 Subject: [PATCH 09/25] Add UBX::gnss_svid in misc/ubx.rb as fenrir-naru/gps_pvt@fd1027d --- tool/misc/ubx.rb | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/tool/misc/ubx.rb b/tool/misc/ubx.rb index ea922302a..d8777d480 100644 --- a/tool/misc/ubx.rb +++ b/tool/misc/ubx.rb @@ -131,4 +131,19 @@ def UBX.svid(id, gnss = :GPS) nil end end + + def UBX.gnss_svid(legacy_svid) + case legacy_svid + when 1..32; [:GPS, legacy_svid] + when 120..158; [:SBAS, legacy_svid] + when 211..246; [:Galileo, legacy_svid - 210] + when 159..163; [:Beido, legacy_svid - 158] + when 33..64; [:Beido, legacy_svid - 27] + #when 173..182 # IMES + when 193..197; [:QZSS, legacy_svid - 192] + when 65..96; [:GLONASS, legacy_svid - 64] + when 255; [:GLONASS, nil] + else; nil + end + end end From 81ba42b2da5ae8300525fd3f0d19849792c7e058 Mon Sep 17 00:00:00 2001 From: fenrir Date: Tue, 24 Oct 2023 13:37:00 +0900 Subject: [PATCH 10/25] Improve filter specification in ubx_filter.rb --- tool/ubx_filter.rb | 95 +++++++++++++++++++++++++++------------------- 1 file changed, 56 insertions(+), 39 deletions(-) diff --git a/tool/ubx_filter.rb b/tool/ubx_filter.rb index a5c97feb8..d4e27505f 100644 --- a/tool/ubx_filter.rb +++ b/tool/ubx_filter.rb @@ -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 @@ -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 @@ -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 } }, @@ -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 @@ -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 @@ -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, @@ -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 [] From 1457f6b0c49b3ad31bdc25c63400c0818ae5a724 Mon Sep 17 00:00:00 2001 From: fenrir Date: Thu, 26 Oct 2023 14:55:36 +0900 Subject: [PATCH 11/25] Reflect fenrir-naru/gps_pvt@7154148 --- tool/misc/receiver_debug.rb | 36 +++++++++++++++++++++--------------- 1 file changed, 21 insertions(+), 15 deletions(-) diff --git a/tool/misc/receiver_debug.rb b/tool/misc/receiver_debug.rb index be47d214a..0f958f4fc 100644 --- a/tool/misc/receiver_debug.rb +++ b/tool/misc/receiver_debug.rb @@ -318,25 +318,31 @@ def make_critical(fname) end GPS::Measurement.class_eval{ - proc{ - key2sym = [] - GPS::Measurement.constants.each{|k| - i = GPS::Measurement.const_get(k) - key2sym[i] = k if i.kind_of?(Integer) - } - define_method(:to_a2){ - to_a.collect{|prn, k, v| [prn, key2sym[k] || k, v]} - } - define_method(:to_hash2){ - Hash[*(to_hash.collect{|prn, k_v| - [prn, Hash[*(k_v.collect{|k, v| [key2sym[k] || k, v]}.flatten(1))]] - }.flatten(1))] - } - }.call add_orig = instance_method(:add) define_method(:add){|prn, key, value| add_orig.bind(self).call(prn, key.kind_of?(Symbol) ? GPS::Measurement.const_get(key) : key, value) } + key2sym = GPS::Measurement.constants.inject([]){|res, k| + res[GPS::Measurement.const_get(k)] = k if /^L\d/ =~ k.to_s + res + } + define_method(:to_a2){ + collect{|prn, k, v| [prn, key2sym[k] || k, v]} + } + cl_hash2 = Class::new(Hash){ + define_method(:to_meas){ + GPS::Measurement::new.tap{|res| + each{|prn, k_v| + k_v.each{|k, v| res.add(prn, k, v)} + } + } + } + } + define_method(:to_hash2){ + cl_hash2::new.tap{|res| + each{|prn, k, v| (res[prn] ||= {})[key2sym[k] || k] = v} + } + } } def run(meas, t_meas, ref_pos = @base_station) From 280187a6065313ea077d04cde345648ee3334c74 Mon Sep 17 00:00:00 2001 From: fenrir Date: Sat, 28 Oct 2023 01:01:35 +0900 Subject: [PATCH 12/25] Reflect fenrir-naru/gps_pvt@bf1b61c --- tool/misc/receiver_debug.rb | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tool/misc/receiver_debug.rb b/tool/misc/receiver_debug.rb index 0f958f4fc..31614bb12 100644 --- a/tool/misc/receiver_debug.rb +++ b/tool/misc/receiver_debug.rb @@ -140,7 +140,7 @@ def self.meas_items(opt = {}) opt[:satellites].collect{|prn, label| pr, rate, doppler, freq = keys.collect{|k| meas_hash[prn][k] rescue nil} freq ||= GPS::SpaceNode.L1_Frequency - [pr, rate || ((doppler * GPS::SpaceNode::light_speed / freq) rescue nil)] + [pr, rate || ((-doppler * GPS::SpaceNode::light_speed / freq) rescue nil)] } } ]] From f47ce18749232d1e4b5918f2ee98cc3048a3ac73 Mon Sep 17 00:00:00 2001 From: fenrir Date: Mon, 6 Nov 2023 14:35:48 +0900 Subject: [PATCH 13/25] Add detection of wrong initialization options to INS_GPS --- tool/INS_GPS.cpp | 60 +++++++++++++++++++++++++++++------------------- 1 file changed, 36 insertions(+), 24 deletions(-) diff --git a/tool/INS_GPS.cpp b/tool/INS_GPS.cpp index 7a6cda392..f79327769 100644 --- a/tool/INS_GPS.cpp +++ b/tool/INS_GPS.cpp @@ -907,25 +907,32 @@ class INS_GPS_NAV : public NAV { } protected: - static void set_matrix_full(mat_t &mat, const char *spec){ - char *_spec(const_cast(spec)); + static bool set_matrix_full(mat_t &mat, const char *spec){ + char *_spec(const_cast(spec)), *_spec2; for(unsigned int i(0), i_end(mat.rows()); i < i_end; i++){ for(unsigned int j(0), j_end(mat.columns()); j < j_end; j++){ - mat(i, j) = std::strtod(_spec, &_spec); + mat(i, j) = std::strtod(_spec2 = _spec, &_spec); + if(_spec == _spec2){return false;} // No conversion } } + return true; } - static void set_matrix_diagonal(mat_t &mat, const char *spec){ - char *_spec(const_cast(spec)); + static bool set_matrix_diagonal(mat_t &mat, const char *spec){ + char *_spec(const_cast(spec)), *_spec2; for(unsigned int i(0), i_end(mat.rows()); i < i_end; i++){ - mat(i, i) = std::strtod(_spec, &_spec); + mat(i, i) = std::strtod(_spec2 = _spec, &_spec); + if(_spec == _spec2){return false;} // No conversion } + return true; } - static void set_matrix_1element(mat_t &mat, const char *spec){ - char *_spec(const_cast(spec)); - int i((int)std::strtol(_spec, &_spec, 10)); - int j((int)std::strtol(_spec, &_spec, 10)); - mat(i, j) = std::strtod(_spec, &_spec); + static bool set_matrix_1element(mat_t &mat, const char *spec){ + char *_spec(const_cast(spec)), *_spec2; + int i((int)std::strtol(_spec2 = _spec, &_spec, 10)); + if(_spec == _spec2){return false;} // No conversion + int j((int)std::strtol(_spec2 = _spec, &_spec, 10)); + if(_spec == _spec2){return false;} // No conversion + mat(i, j) = std::strtod(_spec2 = _spec, &_spec); + return (_spec != _spec2); } bool init_misc(const char *line, void *){ @@ -934,41 +941,43 @@ class INS_GPS_NAV : public NAV { bool init_misc(const char *line, INS *){ const char *value; if(value = Options::get_value2(line, "x")){ - char *spec(const_cast(value)); - int i((int)std::strtol(spec, &spec, 10)); - (*ins_gps)[i] = std::strtod(spec, &spec); - return true; + char *spec(const_cast(value)), *spec2; + int i((int)std::strtol(spec2 = spec, &spec, 10)); + if(spec == spec2){return false;} // No conversion + (*ins_gps)[i] = std::strtod(spec2 = spec, &spec); + return spec != spec2; } return false; } template class Filter> bool init_misc(const char *line, Filtered_INS2 *){ const char *value; + bool res; while(true){ mat_t P(ins_gps->getFilter().getP()); if(value = Options::get_value2(line, "P")){ - set_matrix_full(P, value); + res = set_matrix_full(P, value); }else if(value = Options::get_value2(line, "P_diag")){ - set_matrix_diagonal(P, value); + res = set_matrix_diagonal(P, value); }else if(value = Options::get_value2(line, "P_elm")){ - set_matrix_1element(P, value); + res = set_matrix_1element(P, value); }else{break;} ins_gps->getFilter().setP(P); - return true; + return res; } while(true){ mat_t Q(ins_gps->getFilter().getQ()); if(value = Options::get_value2(line, "Q")){ - set_matrix_full(Q, value); + res = set_matrix_full(Q, value); }else if(value = Options::get_value2(line, "Q_diag")){ - set_matrix_diagonal(Q, value); + res = set_matrix_diagonal(Q, value); }else if(value = Options::get_value2(line, "Q_elm")){ - set_matrix_1element(Q, value); + res = set_matrix_1element(Q, value); }else{break;} ins_gps->getFilter().setQ(Q); - return true; + return res; } return init_misc(line, (BaseINS *)ins_gps); @@ -2067,7 +2076,10 @@ class INS_GPS_NAV::Helper { for(char buf[0x4000]; !options.init_misc->eof(); ){ // Miscellaneous setup options.init_misc->getline(buf, sizeof(buf)); - nav.init_misc(buf); + if(!nav.init_misc(buf)){ + cerr << "(error!) Initialization failure: " << buf << endl; + exit(-1); + } } } From c9d3668c1d29439d5eae275a70e8b744a57f0ef2 Mon Sep 17 00:00:00 2001 From: fenrir Date: Thu, 16 Nov 2023 17:29:47 +0900 Subject: [PATCH 14/25] Add ubx parser in R --- tool/misc/ubx.R | 98 +++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 98 insertions(+) create mode 100644 tool/misc/ubx.R diff --git a/tool/misc/ubx.R b/tool/misc/ubx.R new file mode 100644 index 000000000..34f0de16a --- /dev/null +++ b/tool/misc/ubx.R @@ -0,0 +1,98 @@ +ubx.checksum <- function(packet, skip_head = 2, skip_tail = 2){ + ck_a <- 0 + ck_b <- 0 + if(skip_head > 0){ + packet <- tail(packet, -skip_head) + } + if(skip_tail > 0){ + packet <- head(packet, -skip_tail) + } + sapply(packet, function(b){ + ck_a <<- ck_a + b + ck_b <<- ck_b + ck_a + }) + c(ck_a %% 0x100, ck_b %% 0x100) +} +ubx.update_checksum <- function(packet){ + packet[rev(head(rev(seq(length(packet))), 2))] <- ubx.checksum(packet) + packet +} +ubx.update_size <- function(packet, size = NA){ + if(is.na(size)){ + size <- length(packet) - 8 + } + packet[c(5, 6)] <- c(size %% 0x100, size / 0x100) + packet +} +ubx.update <- function(packet){ + packet %>% update_size %>% update_checksum +} + +ubx.read_packet <- function(conn, yield.fun=NULL){ + res <- list() + if(is.null(yield.fun)){ + yield.fun <- function(packet){res <<- c(res, list(packet))} + } + buf <- c() + while(T){ + if(length(buf) < 8){ + buf <- c(buf, readBin(conn, "int", n=8 - length(buf), size=1, signed=F)) + if(length(buf) < 8){break} + } + + if(buf[[1]] != 0xB5){ + buf <- tail(buf, -1) + next + }else if(buf[[2]] != 0x62){ + buf <- tail(buf, -2) + next + } + + len <- (buf[[6]] * 0x100) + buf[[5]] + if(length(buf) < len + 8){ + buf <- c(buf, readBin(conn, "int", n=len + 8 - length(buf), size=1, signed=F)) + if(length(buf) < len + 8){break} + } + + ck <- ubx.checksum(head(buf, len + 6), skip_head = 2, skip_tail = 0) + if(any(buf[c(len + 7, len + 8)] != ck)){ + buf <- tail(buf, -2) + next + } + + if(is.null(yield.fun(head(buf, len + 8)))){break} + buf <- tail(buf, -(len + 8)) + } + return(res) +} + +ubx.read_pv <- function(fname){ + df_0102 <- data.frame() + df_0112 <- data.frame() + u4 <- function(buf){ + ary <- readBin(as.raw(buf), "int", 2, size=2, signed=F, endian = "little") + ary[[2]] * 0x10000 + ary[[1]] + } + ubx.read_packet(file(fname, "rb"), yield.fun = function(packet){ + if(all(packet[c(3, 4)] == c(0x01, 0x02))){ + df_0102 <<- rbind(df_0102, data.frame( + itow = 1E-3 * u4(tail(packet, -6)), + lng = 1E-7 * u4(tail(packet, -10)), + lat = 1E-7 * u4(tail(packet, -14)), + alt = 1E-3 * u4(tail(packet, -18)), + hacc = 1E-3 * u4(tail(packet, -26)), + vacc = 1E-3 * u4(tail(packet, -30)) )) + NULL + }else if(all(packet[c(3, 4)] == c(0x01, 0x12))){ + df_0112 <<- rbind(df_0112, data.frame( + itow = 1E-3 * u4(tail(packet, -6)), + vn = 1E-2 * u4(tail(packet, -10)), + ve = 1E-2 * u4(tail(packet, -14)), + vd = 1E-2 * u4(tail(packet, -18)), + vel_acc = 1E-2 * u4(tail(packet, -34)) )) + NULL + } + packet + }) + full_join(df_0102, df_0112, by=c("itow")) +} From 008b4943f38659d9eaa7715a9d635a5cf2f4a546 Mon Sep 17 00:00:00 2001 From: fenrir Date: Sun, 19 Nov 2023 22:45:15 +0900 Subject: [PATCH 15/25] Optimize R ubx parser --- tool/misc/ubx.R | 57 +++++++++++++++++++++++++++---------------------- 1 file changed, 32 insertions(+), 25 deletions(-) diff --git a/tool/misc/ubx.R b/tool/misc/ubx.R index 34f0de16a..4f3982afa 100644 --- a/tool/misc/ubx.R +++ b/tool/misc/ubx.R @@ -1,17 +1,15 @@ +library(dplyr) + ubx.checksum <- function(packet, skip_head = 2, skip_tail = 2){ - ck_a <- 0 - ck_b <- 0 if(skip_head > 0){ packet <- tail(packet, -skip_head) } if(skip_tail > 0){ packet <- head(packet, -skip_tail) } - sapply(packet, function(b){ - ck_a <<- ck_a + b - ck_b <<- ck_b + ck_a - }) - c(ck_a %% 0x100, ck_b %% 0x100) + Reduce( + function(ck, x){ck_a <- (ck[[1]] + x) %% 0x100; c(ck_a, (ck_a + ck[[2]]) %% 0x100)}, + packet, init=c(0, 0)) } ubx.update_checksum <- function(packet){ packet[rev(head(rev(seq(length(packet))), 2))] <- ubx.checksum(packet) @@ -67,32 +65,41 @@ ubx.read_packet <- function(conn, yield.fun=NULL){ } ubx.read_pv <- function(fname){ - df_0102 <- data.frame() - df_0112 <- data.frame() + df_0102 <- c() + df_0112 <- c() u4 <- function(buf){ ary <- readBin(as.raw(buf), "int", 2, size=2, signed=F, endian = "little") ary[[2]] * 0x10000 + ary[[1]] } ubx.read_packet(file(fname, "rb"), yield.fun = function(packet){ if(all(packet[c(3, 4)] == c(0x01, 0x02))){ - df_0102 <<- rbind(df_0102, data.frame( - itow = 1E-3 * u4(tail(packet, -6)), - lng = 1E-7 * u4(tail(packet, -10)), - lat = 1E-7 * u4(tail(packet, -14)), - alt = 1E-3 * u4(tail(packet, -18)), - hacc = 1E-3 * u4(tail(packet, -26)), - vacc = 1E-3 * u4(tail(packet, -30)) )) - NULL + df_0102 <<- c(df_0102, + u4(tail(packet, -6)), # itow + u4(tail(packet, -10)), # lng + u4(tail(packet, -14)), # lat + u4(tail(packet, -18)), # alt + u4(tail(packet, -26)), # hacc + u4(tail(packet, -30)) ) # vacc }else if(all(packet[c(3, 4)] == c(0x01, 0x12))){ - df_0112 <<- rbind(df_0112, data.frame( - itow = 1E-3 * u4(tail(packet, -6)), - vn = 1E-2 * u4(tail(packet, -10)), - ve = 1E-2 * u4(tail(packet, -14)), - vd = 1E-2 * u4(tail(packet, -18)), - vel_acc = 1E-2 * u4(tail(packet, -34)) )) - NULL + df_0112 <<- c(df_0112, + u4(tail(packet, -6)), # itow + u4(tail(packet, -10)), # vn + u4(tail(packet, -14)), # ve + u4(tail(packet, -18)), # vd + u4(tail(packet, -34)) ) # vel_acc } packet }) - full_join(df_0102, df_0112, by=c("itow")) + df_0102 <- as.data.frame(matrix(df_0102, ncol=6, byrow=T, + dimnames=list(NULL, c("itow", "lng", "lat", "alt", "hacc", "vacc")))) + df_0112 <- as.data.frame(matrix(df_0112, ncol=5, byrow=T, + dimnames=list(NULL, c("itow", "vn", "ve", "vd", "vel_acc")))) + full_join(df_0102, df_0112, by=c("itow")) %>% + mutate( + itow = 1E-3 * itow, + lng = 1E-7 * lng, lat = 1E-7 * lat, alt = 1E-3 * alt, + hacc = 1E-3 * hacc, vacc = 1E-3 * vacc, + vn = 1E-2 * vn, ve = 1E-2 * ve, vd = 1E-2 * vd, + vel_acc = 1E-2 * vel_acc) } + From 9490579aadcd3abd7d82ca3dbf268205ebbb421d Mon Sep 17 00:00:00 2001 From: fenrir Date: Tue, 21 Nov 2023 14:03:43 +0900 Subject: [PATCH 16/25] Activate dump_stddev option for pure inertial debug mode --- tool/INS_GPS.cpp | 10 +++++++--- tool/navigation/INS_GPS_Debug.h | 6 +++--- 2 files changed, 10 insertions(+), 6 deletions(-) diff --git a/tool/INS_GPS.cpp b/tool/INS_GPS.cpp index f79327769..8e4c63b59 100644 --- a/tool/INS_GPS.cpp +++ b/tool/INS_GPS.cpp @@ -1139,9 +1139,13 @@ struct INS_GPS_NAV_Factory : public NAV_Factory { struct Checker > { template static NAV *check_navdata(const Calibration &calibration){ - return INS_GPS_NAV_Factory< - INS_GPS_NAVData > - >::generate(calibration); + return options.dump_stddev ? + INS_GPS_NAV_Factory< + INS_GPS_NAVData > + >::generate(calibration) : + INS_GPS_NAV_Factory< + INS_GPS_NAVData > + >::generate(calibration); } }; diff --git a/tool/navigation/INS_GPS_Debug.h b/tool/navigation/INS_GPS_Debug.h index c609addce..5991487ec 100644 --- a/tool/navigation/INS_GPS_Debug.h +++ b/tool/navigation/INS_GPS_Debug.h @@ -207,10 +207,10 @@ class INS_GPS_Debug_Covariance : public INS_GPS_Debug { } }; -template -class INS_GPS_Debug_PureInertial : public INS_GPS_Debug { +template +class INS_GPS_Debug_PureInertial : public INS_GPS_Debug { public: - typedef INS_GPS_Debug super_t; + typedef INS_GPS_Debug super_t; #if defined(__GNUC__) && (__GNUC__ < 5) typedef typename super_t::float_t float_t; typedef typename super_t::vec3_t vec3_t; From 1bcba1ca6150839de09c8344791ee17b0ba9c1cf Mon Sep 17 00:00:00 2001 From: fenrir Date: Mon, 30 Oct 2023 10:08:46 +0900 Subject: [PATCH 17/25] Revise typo and add comments --- tool/navigation/GPS.h | 3 ++- tool/navigation/GPS_Solver.h | 15 ++++++++------- tool/navigation/GPS_Solver_Base.h | 4 ++-- 3 files changed, 12 insertions(+), 10 deletions(-) diff --git a/tool/navigation/GPS.h b/tool/navigation/GPS.h index 99dd996b9..4b1c0c0fe 100644 --- a/tool/navigation/GPS.h +++ b/tool/navigation/GPS.h @@ -1156,7 +1156,8 @@ static void name ## _set(InputT *dest, const s ## bits ## _t &src){ \ // Subframe.1 uint_t WN; ///< Week number - float_t URA; ///< User range accuracy (m) + float_t URA; ///< User range accuracy (m) including all errors + ///< for which the Space and Control Segments are responsible. (ICD 6.2.1) uint_t SV_health; ///< Health status int_t iodc; ///< Issue of clock data float_t t_GD; ///< Group delay (s) diff --git a/tool/navigation/GPS_Solver.h b/tool/navigation/GPS_Solver.h index 097485e07..390b0101d 100644 --- a/tool/navigation/GPS_Solver.h +++ b/tool/navigation/GPS_Solver.h @@ -262,9 +262,9 @@ class GPS_SinglePositioning : public SolverBaseT { * * @param sat satellite * @param range "corrected" pseudo range subtracted by (temporal solution of) receiver clock error in meter - * @param time_arrival time when signal arrive at receiver + * @param time_arrival time when signal arrives at receiver * @param usr_pos (temporal solution of) user position - * @param residual calculated residual with line of site vector, and pseudorange standard deviation (sigma); + * @param residual calculated residual with line of sight vector, and pseudorange standard deviation (sigma); * When sigma is equal to or less than zero, the calculated results should not be used. * @param error Some correction can be overwritten. If its unknown_flag is zero, * corrections will be skipped as possible. @see range_errors_t @@ -280,7 +280,7 @@ class GPS_SinglePositioning : public SolverBaseT { static const float_t &c(space_node_t::light_speed); - // Clock error correction + // Satellite clock error correction range += ((error.unknown_flag & range_error_t::SATELLITE_CLOCK) ? (sat.clock_error(time_arrival - range / c) * c) : error.value[range_error_t::SATELLITE_CLOCK]); @@ -301,13 +301,14 @@ class GPS_SinglePositioning : public SolverBaseT { enu_t relative_pos(enu_t::relative(sat_pos, usr_pos.xyz)); + // Ionospheric correction if(error.unknown_flag & range_error_t::MASK_IONOSPHERIC){ residual.residual += ionospheric_correction(time_arrival, usr_pos, relative_pos); }else{ residual.residual += error.value[range_error_t::IONOSPHERIC]; } - // Tropospheric + // Tropospheric correction residual.residual += (error.unknown_flag & range_error_t::MASK_TROPOSPHERIC) ? tropospheric_correction(time_arrival, usr_pos, relative_pos) : error.value[range_error_t::TROPOSPHERIC]; @@ -347,9 +348,9 @@ class GPS_SinglePositioning : public SolverBaseT { * @param range "corrected" pseudo range subtracted by (temporal solution of) receiver clock error in meter * @param time_arrival time when signal arrive at receiver * @param usr_vel (temporal solution of) user velocity - * @param los_neg_x line of site X - * @param los_neg_y line of site Y - * @param los_neg_z line of site Z + * @param los_neg_x line of sight X + * @param los_neg_y line of sight Y + * @param los_neg_z line of sight Z * @return (float_t) relative rate. */ float_t rate_relative_neg( diff --git a/tool/navigation/GPS_Solver_Base.h b/tool/navigation/GPS_Solver_Base.h index f2021db1a..0efb9e131 100644 --- a/tool/navigation/GPS_Solver_Base.h +++ b/tool/navigation/GPS_Solver_Base.h @@ -148,7 +148,7 @@ struct GPS_Solver_Base { * of measurement_t::mapped_type * @param values key-value map * @param key key - * @param buf buffer into which found value is stored + * @param buf buffer for storing found value, and preserving original value if value is not found * @return (float_t *) When value is found, pointer of buf will be returned. * Otherwise, NULL is returned. */ @@ -389,7 +389,7 @@ struct GPS_Solver_Base { float_t range_corrected; ///< corrected range just including delay, and excluding receiver/satellite error float_t range_residual; ///< residual range float_t rate_relative_neg; /// relative rate - float_t los_neg[3]; ///< line of site vector from satellite to user + float_t los_neg[3]; ///< line of sight vector from satellite to user in ECEF coordinate }; /** From e54b6dc01b62fd388dd334b98df2bbf668584088 Mon Sep 17 00:00:00 2001 From: fenrir Date: Mon, 30 Oct 2023 15:42:01 +0900 Subject: [PATCH 18/25] Move rate sigma calculation to relative_property() * Temporary pseudorange and Doppler sigmas provided by receiver are ignored. --- tool/navigation/GPS_Solver.h | 9 +++++++-- tool/navigation/GPS_Solver_Base.h | 25 +++++++++++++------------ tool/navigation/INS_GPS2_Tightly.h | 20 ++++++-------------- tool/swig/GPS.i | 13 ++++++++++--- tool/swig/spec/GPS_spec.rb | 14 +++++++------- 5 files changed, 43 insertions(+), 38 deletions(-) diff --git a/tool/navigation/GPS_Solver.h b/tool/navigation/GPS_Solver.h index 390b0101d..3135bb313 100644 --- a/tool/navigation/GPS_Solver.h +++ b/tool/navigation/GPS_Solver.h @@ -294,7 +294,7 @@ class GPS_SinglePositioning : public SolverBaseT { // Calculate residual residual.residual = range - geometric_range; - // Setup design matrix + // Setup design matrix (direction is negative; satellite -> user) residual.los_neg_x = -(sat_pos.x() - usr_pos.xyz.x()) / geometric_range; residual.los_neg_y = -(sat_pos.y() - usr_pos.xyz.y()) / geometric_range; residual.los_neg_z = -(sat_pos.z() - usr_pos.xyz.z()) / geometric_range; @@ -426,13 +426,18 @@ class GPS_SinglePositioning : public SolverBaseT { usr_pos, residual, range_error); res.rate_relative_neg = rate_relative_neg(sat, res.range_corrected, time_arrival, usr_vel, res.los_neg[0], res.los_neg[1], res.los_neg[2]); + res.rate_sigma = sat.rate_sigma(time_arrival); #if 0 - // TODO consider case when standard deviation of pseudorange measurement is provided by receiver + // TODO consider case when standard deviation of pseudorange and/or its rate are provided by receiver if(!this->range_sigma(measurement, res.range_sigma)){ // If receiver's range variance is not provided res.range_sigma = 1E0; // TODO range error variance [m] } + if(!this->rate_sigma(measurement, res.rate_sigma)){ + // If receiver's rate variance is not provided + res.rate_sigma = 1E-1; + } #endif return res; diff --git a/tool/navigation/GPS_Solver_Base.h b/tool/navigation/GPS_Solver_Base.h index 0efb9e131..b73f241aa 100644 --- a/tool/navigation/GPS_Solver_Base.h +++ b/tool/navigation/GPS_Solver_Base.h @@ -388,7 +388,8 @@ struct GPS_Solver_Base { float_t range_sigma; ///< Standard deviation of pseudorange; If zero or negative, other values are invalid. float_t range_corrected; ///< corrected range just including delay, and excluding receiver/satellite error float_t range_residual; ///< residual range - float_t rate_relative_neg; /// relative rate + float_t rate_sigma; ///< Standard deviation of range rate; + float_t rate_relative_neg; /// relative rate (taking negative value if range is increasing) float_t los_neg[3]; ///< line of sight vector from satellite to user in ECEF coordinate }; @@ -701,14 +702,13 @@ struct GPS_Solver_Base { delta_r.circular(offset, 0, size, 1)); } template - void copy_G_W_row(const linear_solver_t &src, + void copy_G_rowwise(const linear_solver_t &src, const unsigned int &i_src, const unsigned int &i_dst){ unsigned int c_dst(G.columns()), c_src(src.G.columns()), c(c_dst > c_src ? c_src : c_dst); // Normally c=4 for(unsigned int j(0); j < c; ++j){ G(i_dst, j) = src.G(i_src, j); } - W(i_dst, i_dst) = src.W(i_src, i_src); } }; @@ -806,15 +806,15 @@ struct GPS_Solver_Base { res.receiver_error = receiver_error_init; geometric_matrices_t geomat(measurement.size()); - typedef std::vector > index_obs_t; - index_obs_t idx_rate_rel; // [(index of measurement, relative rate), ...] - idx_rate_rel.reserve(measurement.size()); + typedef std::vector > index_prop_t; + index_prop_t idx_prop; // [(index of measurement, relative property), ...] + idx_prop.reserve(measurement.size()); // If initialization is not appropriate, more iteration will be performed. bool converged(false); for(int i_trial(-opt.warmup); i_trial < opt.max_trial; i_trial++){ - idx_rate_rel.clear(); + idx_prop.clear(); unsigned int i_row(0), i_measurement(0); res.used_satellite_mask.clear(); @@ -841,7 +841,7 @@ struct GPS_Solver_Base { if(coarse_estimation){ prop.range_sigma = 1; }else{ - idx_rate_rel.push_back(std::make_pair(i_measurement, prop.rate_relative_neg)); + idx_prop.push_back(std::make_pair(i_measurement, prop)); } geomat.delta_r(i_row, 0) = prop.range_residual; @@ -892,7 +892,7 @@ struct GPS_Solver_Base { geometric_matrices_t geomat2(res.used_satellites); int i_range(0), i_rate(0); - for(typename index_obs_t::const_iterator it(idx_rate_rel.begin()), it_end(idx_rate_rel.end()); + for(typename index_prop_t::const_iterator it(idx_prop.begin()), it_end(idx_prop.end()); it != it_end; ++it, ++i_range){ @@ -901,9 +901,10 @@ struct GPS_Solver_Base { *(measurement[it->first].k_v_map), // const version of measurement[PRN] rate))){continue;} - // Copy design matrix and set rate - geomat2.copy_G_W_row(geomat, i_range, i_rate); - geomat2.delta_r(i_rate, 0) = rate + it->second; + // Copy design matrix and set rate and weight + geomat2.copy_G_rowwise(geomat, i_range, i_rate); + geomat2.delta_r(i_rate, 0) = rate + it->second.rate_relative_neg; + geomat2.W(i_rate, i_rate) = 1. / std::pow(it->second.rate_sigma, 2); ++i_rate; } diff --git a/tool/navigation/INS_GPS2_Tightly.h b/tool/navigation/INS_GPS2_Tightly.h index c4bd03e73..4d6f7b6a7 100644 --- a/tool/navigation/INS_GPS2_Tightly.h +++ b/tool/navigation/INS_GPS2_Tightly.h @@ -466,7 +466,6 @@ class INS_GPS2_Tightly : public BaseFINS { struct relative_property_t : public solver_t::relative_property_t { float_t rate_residual; - float_t rate_sigma; }; /** @@ -485,7 +484,6 @@ class INS_GPS2_Tightly : public BaseFINS { const receiver_state_t &x) const { relative_property_t res; - res.rate_sigma = -1; // initialization with invalid value; const solver_t &solver_selected(solver.select(prn)); (typename solver_t::relative_property_t &)res = solver_selected.relative_property( @@ -493,18 +491,12 @@ class INS_GPS2_Tightly : public BaseFINS { if(res.range_sigma <= 0){return res;} - do{ - float_t rate; - if(!solver_selected.rate(measurement, rate)){break;} - res.rate_residual = rate - - super_t::m_clock_error_rate[x.clock_index] + res.rate_relative_neg; - - if(!solver_selected.rate_sigma(measurement, res.rate_sigma)){ - // If receiver's rate variance is not provided - res.rate_sigma = solver_selected.select_satellite(prn, x.t).rate_sigma(x.t); - } - }while(false); - + if(!solver_selected.rate(measurement, res.rate_residual)){ + res.rate_sigma = -1; // overwrite with invalid value + }else{ + res.rate_residual += -super_t::m_clock_error_rate[x.clock_index] + + res.rate_relative_neg; + } return res; } diff --git a/tool/swig/GPS.i b/tool/swig/GPS.i index 0120eabb4..e2a6f9b64 100644 --- a/tool/swig/GPS.i +++ b/tool/swig/GPS.i @@ -1024,7 +1024,7 @@ struct HookableSolver : public BaseT { const GPS_Solver::base_t::relative_property_t &res_orig) const { union { base_t::relative_property_t prop; - FloatT values[7]; + FloatT values[8]; } res = {res_orig}; #ifdef SWIGRUBY do{ @@ -1032,15 +1032,19 @@ struct HookableSolver : public BaseT { static const int prop_items(sizeof(res.values) / sizeof(res.values[0])); VALUE hook(rb_hash_lookup(hooks, key)); if(NIL_P(hook)){break;} - FloatT weight((res.prop.range_sigma > 0) + FloatT weight_range((res.prop.range_sigma > 0) ? (1. / std::pow(res.prop.range_sigma, 2)) // weight=1/(sigma^2) : res.prop.range_sigma); + FloatT weight_rate((res.prop.rate_sigma > 0) + ? (1. / std::pow(res.prop.rate_sigma, 2)) // weight=1/(sigma^2) + : res.prop.rate_sigma); VALUE values[] = { SWIG_From(int)(prn), // prn rb_ary_new_from_args(prop_items, // relative_property - swig::from(weight), + swig::from(weight_range), swig::from(res.prop.range_corrected), swig::from(res.prop.range_residual), + swig::from(weight_rate), swig::from(res.prop.rate_relative_neg), swig::from(res.prop.los_neg[0]), swig::from(res.prop.los_neg[1]), @@ -1073,6 +1077,9 @@ struct HookableSolver : public BaseT { if(res.values[0] > 0){ res.values[0] = std::pow(1. / res.values[0], 0.5); // sigma=(1/weight)^0.5 } + if(res.values[3] > 0){ + res.values[3] = std::pow(1. / res.values[3], 0.5); // sigma=(1/weight)^0.5 + } }while(false); #endif return res.prop; diff --git a/tool/swig/spec/GPS_spec.rb b/tool/swig/spec/GPS_spec.rb index cd877fbdf..1825f95a9 100644 --- a/tool/swig/spec/GPS_spec.rb +++ b/tool/swig/spec/GPS_spec.rb @@ -875,10 +875,10 @@ expect(pvt.vdop).to be_within(1E-2).of(1.87) expect(pvt.tdop).to be_within(1E-2).of(1.08) expect(pvt.velocity.to_a).to eq([:e, :n, :u].collect{|k| pvt.velocity.send(k)}) - expect(pvt.velocity.north).to be_within(1E-2).of(-0.86) # north - expect(pvt.velocity.east) .to be_within(1E-2).of(-1.10) # east - expect(pvt.velocity.down) .to be_within(1E-2).of(-0.22) # down - expect(pvt.receiver_error_rate).to be_within(1E-2).of(-1061.86) + expect(pvt.velocity.north).to be_within(1E-2).of(-0.68) # north + expect(pvt.velocity.east) .to be_within(1E-2).of(-0.90) # east + expect(pvt.velocity.down) .to be_within(1E-2).of(0.26) # down + expect(pvt.receiver_error_rate).to be_within(1E-2).of(-1062.14) expect(pvt.G.rows).to eq(6) expect(pvt.W.rows).to eq(6) expect(pvt.delta_r.rows).to eq(6) @@ -942,9 +942,9 @@ expect(t_arv).to be_a_kind_of(GPS::Time) expect(usr_pos).to be_a_kind_of(Coordinate::XYZ) expect(usr_vel).to be_a_kind_of(Coordinate::XYZ) - weight, range_c, range_r, rate_rel_neg, *los_neg = rel_prop - weight = 1 - [weight, range_c, range_r, rate_rel_neg] + los_neg + weight_range, range_c, range_r, weight_rate, rate_rel_neg, *los_neg = rel_prop + weight_range = 1 + [weight_range, range_c, range_r, weight_rate, rate_rel_neg] + los_neg } solver.hooks[:update_position_solution] = proc{|mat_G, mat_W, mat_delta_r, temp_pvt| expect(temp_pvt).to be_a_kind_of(GPS::PVT) From 5dbf4851cfbe91ac207b39f79ff1573b58342e4e Mon Sep 17 00:00:00 2001 From: fenrir Date: Wed, 8 Nov 2023 00:36:23 +0900 Subject: [PATCH 19/25] Fix conversion from PVT to PV solutions; set uninitialized flags --- tool/navigation/INS_GPS2_Tightly.h | 35 ++++++++++++++++++------------ 1 file changed, 21 insertions(+), 14 deletions(-) diff --git a/tool/navigation/INS_GPS2_Tightly.h b/tool/navigation/INS_GPS2_Tightly.h index 4d6f7b6a7..36cdd1217 100644 --- a/tool/navigation/INS_GPS2_Tightly.h +++ b/tool/navigation/INS_GPS2_Tightly.h @@ -317,20 +317,27 @@ struct GPS_Solution_PVT : public PVT_BaseT { */ operator GPS_Solution () const { GPS_Solution res; - res.v_n = this->user_velocity_enu.north(); - res.v_e = this->user_velocity_enu.east(); - res.v_d = -this->user_velocity_enu.up(); - res.latitude = this->user_position.llh.latitude(); - res.longitude = this->user_position.llh.longitude(); - res.height = this->user_position.llh.height(); - // Calculation of estimated accuracy - /* Position standard deviation is roughly estimated as (DOP * 2 meters) - * @see https://www.gps.gov/systems/gps/performance/2016-GPS-SPS-performance-analysis.pdf Table 3.2 - */ - res.sigma_2d = this->dop.h * 2; - res.sigma_height = this->dop.v * 2; - // Speed standard deviation is roughly estimated as (DOP * 0.1 meter / seconds) - res.sigma_vel = this->dop.p * 0.1; + res.valid_velocity = res.valid_position = false; + if(this->position_solved()){ + res.valid_position = true; + res.latitude = this->user_position.llh.latitude(); + res.longitude = this->user_position.llh.longitude(); + res.height = this->user_position.llh.height(); + // Calculation of estimated accuracy + /* Position standard deviation is roughly estimated as (DOP * 2 meters) + * @see https://www.gps.gov/systems/gps/performance/2016-GPS-SPS-performance-analysis.pdf Table 3.2 + */ + res.sigma_2d = this->dop.h * 2; + res.sigma_height = this->dop.v * 2; + } + if(this->velocity_solved()){ + res.valid_velocity = true; + res.v_n = this->user_velocity_enu.north(); + res.v_e = this->user_velocity_enu.east(); + res.v_d = -this->user_velocity_enu.up(); + // Speed standard deviation is roughly estimated as (DOP * 0.1 meter / seconds) + res.sigma_vel = this->dop.p * 0.1; + } return res; } }; From aad218313062b094776bcd675fb2c81135c6b225 Mon Sep 17 00:00:00 2001 From: fenrir Date: Sat, 11 Nov 2023 22:16:58 +0900 Subject: [PATCH 20/25] Add sigma calculation to GPS solver by using pseudorange/doppler sigma --- tool/misc/receiver_debug.rb | 5 ++- tool/navigation/GPS_Solver_Base.h | 60 ++++++++++++++++++------------ tool/navigation/GPS_Solver_RAIM.h | 3 +- tool/navigation/INS_GPS2_Tightly.h | 19 ++++------ tool/navigation/INS_GPS_Debug.h | 2 +- tool/swig/GPS.i | 5 ++- 6 files changed, 54 insertions(+), 40 deletions(-) diff --git a/tool/misc/receiver_debug.rb b/tool/misc/receiver_debug.rb index 31614bb12..54f120c33 100644 --- a/tool/misc/receiver_debug.rb +++ b/tool/misc/receiver_debug.rb @@ -43,11 +43,12 @@ def self.pvt_items(opt = {}) ] + (pvt.rel_ENU.to_a rescue [nil] * 3) } ]] + [proc{ - labels = [:g, :p, :h, :v, :t].collect{|k| "#{k}dop".to_sym} + labels = [:g, :p, :h, :v, :t].collect{|k| "#{k}dop".to_sym} \ + + [:h, :v, :t].collect{|k| "#{k}sigma".to_sym} [ labels, proc{|pvt| - next [nil] * 5 unless pvt.position_solved? + next [nil] * 8 unless pvt.position_solved? labels.collect{|k| pvt.send(k)} } ] diff --git a/tool/navigation/GPS_Solver_Base.h b/tool/navigation/GPS_Solver_Base.h index b73f241aa..d71223708 100644 --- a/tool/navigation/GPS_Solver_Base.h +++ b/tool/navigation/GPS_Solver_Base.h @@ -434,16 +434,16 @@ struct GPS_Solver_Base { float_t receiver_error; enu_t user_velocity_enu; float_t receiver_error_rate; - struct dop_t { + struct precision_t { float_t g, p, h, v, t; - dop_t() {} - dop_t(const matrix_t &C_enu) - : g(std::sqrt(C_enu.trace())), - p(std::sqrt(C_enu.partial(3, 3).trace())), - h(std::sqrt(C_enu.partial(2, 2).trace())), - v(std::sqrt(C_enu(2, 2))), - t(std::sqrt(C_enu(3, 3))) {} - } dop; + precision_t() {} + precision_t(const matrix_t &C_or_P_enu) + : g(std::sqrt(C_or_P_enu.trace())), + p(std::sqrt(C_or_P_enu.partial(3, 3).trace())), + h(std::sqrt(C_or_P_enu.partial(2, 2).trace())), + v(std::sqrt(C_or_P_enu(2, 2))), + t(std::sqrt(C_or_P_enu(3, 3))) {} + } dop, sigma_pos, sigma_vel; unsigned int used_satellites; typedef BitArray<0x400> satellite_mask_t; satellite_mask_t used_satellite_mask; ///< bit pattern(use=1, otherwise=0), PRN 1(LSB) to 32 for GPS @@ -566,25 +566,34 @@ struct GPS_Solver_Base { return (G.transpose() * G).inverse(); } /** - * Transform coordinate of matrix C - * C' = (G * R^t)^t * (G * R^t) = R * G^t * G * R^t = R * C * R^t, + * Calculate P matrix, where P = E[x^t x] = (G^t * W * G)^{-1} to be used for estimated accuracy calculation + * @return P matrix + */ + matrix_t P() const { + return (G.transpose() * W * G).inverse(); + } + /** + * Transform coordinate of matrix C/P + * C' = ((G * R^t)^t * (G * R^t))^-1 = (R * G^t * G * R^t)^-1 + * = (R^t)^-1 * (G^t * G)^-1 * R^-1 = R * C * R^t, + * P' = ((G * R^t)^t * W * (G * R^t))^-1 = R * P * R^t, * where R is a rotation matrix, for example, ECEF to ENU. * * @param rotation_matrix 3 by 3 rotation matrix - * @return transformed matrix C' + * @return transformed matrix C' or P' * @see rotate_G */ - static matrix_t rotate_C(const matrix_t &C, const matrix_t &rotation_matrix){ - unsigned int n(C.rows()); // Normally n=4 + static matrix_t rotate_CP(const matrix_t &C_or_P, const matrix_t &rotation_matrix){ + unsigned int n(C_or_P.rows()); // Normally n=4 matrix_t res(n, n); res.partial(3, 3).replace( // upper left - rotation_matrix * C.partial(3, 3) * rotation_matrix.transpose()); + rotation_matrix * C_or_P.partial(3, 3) * rotation_matrix.transpose()); res.partial(3, n - 3, 0, 3).replace( // upper right - rotation_matrix * C.partial(3, n - 3, 0, 3)); + rotation_matrix * C_or_P.partial(3, n - 3, 0, 3)); res.partial(n - 3, 3, 3, 0).replace( // lower left - C.partial(n - 3, 3, 3, 0) * rotation_matrix.transpose()); + C_or_P.partial(n - 3, 3, 3, 0) * rotation_matrix.transpose()); res.partial(n - 3, n - 3, 3, 3).replace( // lower right - C.partial(n - 3, n - 3, 3, 3)); + C_or_P.partial(n - 3, n - 3, 3, 3)); return res; } /** @@ -595,7 +604,7 @@ struct GPS_Solver_Base { * * 4 by row(y) S matrix (=(G^t * W * G)^{-1} * (G^t * W)) will be used to calculate protection level * to investigate relationship between bias on each satellite and solution. - * residual v = (I - P) = (I - G S), where P = G S, which is irrelevant to rotation, + * residual v = (I - G S) y, which is irrelevant to rotation, * because P = G R R^{t} S = G' S'. * * @param S (output) coefficient matrix to calculate solution, i.e., (G^t * W * G)^{-1} * (G^t * W) @@ -872,9 +881,12 @@ struct GPS_Solver_Base { return; } + matrix_t rot(res.user_position.ecef2enu()); try{ - res.dop = typename user_pvt_t::dop_t(geomat.rotate_C( - geomat.partial(res.used_satellites).C(), res.user_position.ecef2enu())); + res.dop = typename user_pvt_t::precision_t( + geomat.rotate_CP(geomat.partial(res.used_satellites).C(), rot)); + res.sigma_pos = typename user_pvt_t::precision_t( + geomat.rotate_CP(geomat.partial(res.used_satellites).P(), rot)); }catch(const std::runtime_error &e){ // expect to detect matrix operation error res.error_code = user_pvt_t::ERROR_DOP; return; @@ -922,6 +934,8 @@ struct GPS_Solver_Base { res.user_velocity_enu = enu_t::relative_rel( vel_xyz, res.user_position.llh); res.receiver_error_rate = sol(3, 0); + res.sigma_vel = typename user_pvt_t::precision_t( + geomat.rotate_CP(geomat2.partial(i_rate).P(), rot)); }catch(const std::runtime_error &e){ // expect to detect matrix operation error res.error_code = user_pvt_t::ERROR_VELOCITY_LS; return; @@ -1147,8 +1161,8 @@ struct GPS_Solver_Base { return solver_interface_t >(*this); } - static typename user_pvt_t::dop_t dop(const matrix_t &C, const pos_t &user_position) { - return typename user_pvt_t::dop_t(geometric_matrices_t::rotate_C(C, user_position.ecef2enu())); + static typename user_pvt_t::precision_t dop(const matrix_t &C, const pos_t &user_position) { + return typename user_pvt_t::precision_t(geometric_matrices_t::rotate_CP(C, user_position.ecef2enu())); } }; diff --git a/tool/navigation/GPS_Solver_RAIM.h b/tool/navigation/GPS_Solver_RAIM.h index f657004b1..9e5b632c5 100644 --- a/tool/navigation/GPS_Solver_RAIM.h +++ b/tool/navigation/GPS_Solver_RAIM.h @@ -56,7 +56,7 @@ struct GPS_PVT_RAIM_LSR : public PVT_BaseT { typename GPS_Solver_Base::prn_t excluded; typename GPS_Solver_Base::pos_t user_position; typename GPS_Solver_Base::float_t receiver_error; - typename GPS_Solver_Base::user_pvt_t::dop_t dop; + typename GPS_Solver_Base::user_pvt_t::precision_t dop, sigma_pos; } FDE_min, FDE_2nd; ///< Fault exclusion }; @@ -231,6 +231,7 @@ struct GPS_Solver_RAIM_LSR : public SolverBaseT { target->user_position = pvt_FDE.user_position; target->receiver_error = pvt_FDE.receiver_error; target->dop = pvt_FDE.dop; + target->sigma_pos = pvt_FDE.sigma_pos; } } }; diff --git a/tool/navigation/INS_GPS2_Tightly.h b/tool/navigation/INS_GPS2_Tightly.h index 36cdd1217..b3fd0bf27 100644 --- a/tool/navigation/INS_GPS2_Tightly.h +++ b/tool/navigation/INS_GPS2_Tightly.h @@ -323,20 +323,15 @@ struct GPS_Solution_PVT : public PVT_BaseT { res.latitude = this->user_position.llh.latitude(); res.longitude = this->user_position.llh.longitude(); res.height = this->user_position.llh.height(); - // Calculation of estimated accuracy - /* Position standard deviation is roughly estimated as (DOP * 2 meters) - * @see https://www.gps.gov/systems/gps/performance/2016-GPS-SPS-performance-analysis.pdf Table 3.2 - */ - res.sigma_2d = this->dop.h * 2; - res.sigma_height = this->dop.v * 2; + res.sigma_2d = this->sigma_pos.h; + res.sigma_height = this->sigma_pos.v; } if(this->velocity_solved()){ res.valid_velocity = true; res.v_n = this->user_velocity_enu.north(); res.v_e = this->user_velocity_enu.east(); res.v_d = -this->user_velocity_enu.up(); - // Speed standard deviation is roughly estimated as (DOP * 0.1 meter / seconds) - res.sigma_vel = this->dop.p * 0.1; + res.sigma_vel = this->sigma_vel.p; } return res; } @@ -636,12 +631,12 @@ class INS_GPS2_Tightly : public BaseFINS { * @param x receiver state represented by current position and clock properties * @param props relative properties * @param res buffer of calculation result - * @return (dop_t *) unavailable when null; otherwise, DOP + * @return (precision_t *) unavailable when null; otherwise, DOP */ - static typename solver_t::user_pvt_t::dop_t *get_DOP( + static typename solver_t::user_pvt_t::precision_t *get_DOP( const receiver_state_t &x, const relative_property_list_t &props, - typename solver_t::user_pvt_t::dop_t &res) { + typename solver_t::user_pvt_t::precision_t &res) { mat_t G_full(props.size(), 4); // design matrix //mat_t R_full(props.size(), props.size()); @@ -687,7 +682,7 @@ class INS_GPS2_Tightly : public BaseFINS { * intentional exclusion. */ /*{ // check DOP - typename solver_t::user_pvt_t::dop_t dop; + typename solver_t::user_pvt_t::precision_t dop; if(get_DOP(x, props, dop)){ // do something std::cerr << dop.t << std::endl; diff --git a/tool/navigation/INS_GPS_Debug.h b/tool/navigation/INS_GPS_Debug.h index 0833ba9f1..f7d294077 100644 --- a/tool/navigation/INS_GPS_Debug.h +++ b/tool/navigation/INS_GPS_Debug.h @@ -261,7 +261,7 @@ class INS_GPS_Debug_Tightly : public INS_GPS_Debug { out << snapshot.props.size() << ','; // number of satellites do{ // DOP - typename super_t::solver_t::user_pvt_t::dop_t dop; + typename super_t::solver_t::user_pvt_t::precision_t dop; if(!super_t::get_DOP(snapshot.state, snapshot.props, dop)){ out << ",,,,,"; break; diff --git a/tool/swig/GPS.i b/tool/swig/GPS.i index e2a6f9b64..c1b0efb38 100644 --- a/tool/swig/GPS.i +++ b/tool/swig/GPS.i @@ -638,6 +638,9 @@ struct GPS_User_PVT const FloatT &hdop() const {return base_t::dop.h;} const FloatT &vdop() const {return base_t::dop.v;} const FloatT &tdop() const {return base_t::dop.t;} + const FloatT &hsigma() const {return base_t::sigma_pos.h;} + const FloatT &vsigma() const {return base_t::sigma_pos.v;} + const FloatT &tsigma() const {return base_t::sigma_pos.t;} const unsigned int &used_satellites() const {return base_t::used_satellites;} std::vector used_satellite_list() const {return base_t::used_satellite_mask.indices_one();} bool position_solved() const {return base_t::position_solved();} @@ -662,7 +665,7 @@ struct GPS_User_PVT return linear_solver().C(); } Matrix > C_enu() const { - return proxy_t::linear_solver_t::rotate_C(C(), base_t::user_position.ecef2enu()); + return proxy_t::linear_solver_t::rotate_CP(C(), base_t::user_position.ecef2enu()); } Matrix > S() const { Matrix > res; From 6d49dd0d7b3edf396c2164a87c9e460b0ec1bcbb Mon Sep 17 00:00:00 2001 From: fenrir Date: Sat, 11 Nov 2023 23:50:56 +0900 Subject: [PATCH 21/25] Refactor DOP and sigma calculation in GPS solver --- tool/INS_GPS/GNSS_Receiver.h | 10 +++++++-- tool/navigation/GPS_Solver_Base.h | 29 +++++++++++++++--------- tool/navigation/INS_GPS2_Tightly.h | 36 +++++++++++++++++------------- tool/navigation/INS_GPS_Debug.h | 11 +++++---- 4 files changed, 53 insertions(+), 33 deletions(-) diff --git a/tool/INS_GPS/GNSS_Receiver.h b/tool/INS_GPS/GNSS_Receiver.h index 76b80de25..37943fc72 100644 --- a/tool/INS_GPS/GNSS_Receiver.h +++ b/tool/INS_GPS/GNSS_Receiver.h @@ -538,6 +538,9 @@ data.gps.solver_options. expr << ',' << "hdop" << ',' << "vdop" << ',' << "tdop" + << ',' << "hsigma" + << ',' << "vsigma" + << ',' << "tsigma" << ',' << "v_north" << ',' << "v_east" << ',' << "v_down" @@ -608,9 +611,12 @@ data.gps.solver_options. expr << ',' << src.dop.p << ',' << src.dop.h << ',' << src.dop.v - << ',' << src.dop.t; + << ',' << src.dop.t + << ',' << src.sigma_pos.h + << ',' << src.sigma_pos.v + << ',' << src.sigma_pos.t; }else{ - out << ",,,,,,,,,"; + out << ",,,,,,,,,,,,"; } if(src.velocity_solved()){ out << ',' << src.user_velocity_enu.north() diff --git a/tool/navigation/GPS_Solver_Base.h b/tool/navigation/GPS_Solver_Base.h index d71223708..272462d28 100644 --- a/tool/navigation/GPS_Solver_Base.h +++ b/tool/navigation/GPS_Solver_Base.h @@ -596,6 +596,18 @@ struct GPS_Solver_Base { C_or_P.partial(n - 3, n - 3, 3, 3)); return res; } + typename user_pvt_t::precision_t dop(){ + return typename user_pvt_t::precision_t(C()); + } + typename user_pvt_t::precision_t dop(const matrix_t &rotation_matrix){ + return typename user_pvt_t::precision_t(rotate_CP(C(), rotation_matrix)); + } + typename user_pvt_t::precision_t sigma(){ + return typename user_pvt_t::precision_t(P()); + } + typename user_pvt_t::precision_t sigma(const matrix_t &rotation_matrix){ + return typename user_pvt_t::precision_t(rotate_CP(P(), rotation_matrix)); + } /** * Solve x of linear equation (y = G x + v) to minimize sigma{v^t * v} * where v =~ N(0, sigma), and y and G are observation delta (=delta_r variable) @@ -883,10 +895,9 @@ struct GPS_Solver_Base { matrix_t rot(res.user_position.ecef2enu()); try{ - res.dop = typename user_pvt_t::precision_t( - geomat.rotate_CP(geomat.partial(res.used_satellites).C(), rot)); - res.sigma_pos = typename user_pvt_t::precision_t( - geomat.rotate_CP(geomat.partial(res.used_satellites).P(), rot)); + typename geometric_matrices_t::partial_t geomat_used(geomat.partial(res.used_satellites)); + res.dop = geomat_used.dop(rot); + res.sigma_pos = geomat_used.sigma(rot); }catch(const std::runtime_error &e){ // expect to detect matrix operation error res.error_code = user_pvt_t::ERROR_DOP; return; @@ -928,14 +939,14 @@ struct GPS_Solver_Base { try{ // Least square - matrix_t sol(geomat2.partial(i_rate).least_square()); + typename geometric_matrices_t::partial_t geomat2_used(geomat2.partial(i_rate)); + matrix_t sol(geomat2_used.least_square()); xyz_t vel_xyz(sol.partial(3, 1, 0, 0)); res.user_velocity_enu = enu_t::relative_rel( vel_xyz, res.user_position.llh); res.receiver_error_rate = sol(3, 0); - res.sigma_vel = typename user_pvt_t::precision_t( - geomat.rotate_CP(geomat2.partial(i_rate).P(), rot)); + res.sigma_vel = geomat2_used.sigma(rot); }catch(const std::runtime_error &e){ // expect to detect matrix operation error res.error_code = user_pvt_t::ERROR_VELOCITY_LS; return; @@ -1160,10 +1171,6 @@ struct GPS_Solver_Base { solver_interface_t > solve() const { return solver_interface_t >(*this); } - - static typename user_pvt_t::precision_t dop(const matrix_t &C, const pos_t &user_position) { - return typename user_pvt_t::precision_t(geometric_matrices_t::rotate_CP(C, user_position.ecef2enu())); - } }; template diff --git a/tool/navigation/INS_GPS2_Tightly.h b/tool/navigation/INS_GPS2_Tightly.h index b3fd0bf27..7622d58fb 100644 --- a/tool/navigation/INS_GPS2_Tightly.h +++ b/tool/navigation/INS_GPS2_Tightly.h @@ -626,20 +626,24 @@ class INS_GPS2_Tightly : public BaseFINS { } /** - * Calculate DOP + * Calculate DOP and sigma * * @param x receiver state represented by current position and clock properties * @param props relative properties - * @param res buffer of calculation result - * @return (precision_t *) unavailable when null; otherwise, DOP + * @param dop buffer of DOP calculation result + * @param sigma_pos buffer of position sigma calculation result + * @return (precision_t *) unavailable when null; otherwise, pointer to DOP */ - static typename solver_t::user_pvt_t::precision_t *get_DOP( + static typename solver_t::user_pvt_t::precision_t *get_DOP_sigma( const receiver_state_t &x, const relative_property_list_t &props, - typename solver_t::user_pvt_t::precision_t &res) { + typename solver_t::user_pvt_t::precision_t &dop, + typename solver_t::user_pvt_t::precision_t &sigma_pos) { - mat_t G_full(props.size(), 4); // design matrix - //mat_t R_full(props.size(), props.size()); + struct proxy_t : public solver_t { + typedef typename solver_t::geometric_matrices_t geomat_t; + }; + typename proxy_t::geomat_t geomat(props.size()); // count up valid measurement int i_row(0); @@ -650,17 +654,17 @@ class INS_GPS2_Tightly : public BaseFINS { if(prop.range_sigma <= 0){continue;} // No measurement for(int i(0); i < sizeof(prop.los_neg) / sizeof(prop.los_neg[0]); ++i){ - G_full(i_row, i) = prop.los_neg[i]; + geomat.G(i_row, i) = prop.los_neg[i]; } - G_full(i_row, 3) = 1; - //R_full(i_row, i_row) = 1. / std::pow(prop.range_sigma, 2); + geomat.W(i_row, i_row) = 1. / std::pow(prop.range_sigma, 2); ++i_row; } if(i_row < 4){return NULL;} - typename mat_t::partial_offsetless_t G(G_full.partial(i_row, 4)); - return &(res = solver_t::dop( - (G.transpose() /* * R_full.partial(i_row, i_row)*/ * G).inverse(), x.pos)); + typename proxy_t::geomat_t::partial_t geomat_used(geomat.partial(i_row)); + typename solver_t::matrix_t rot(x.pos.ecef2enu()); + sigma_pos = geomat_used.sigma(rot); + return &(dop = geomat_used.dop(rot)); } /** @@ -681,9 +685,9 @@ class INS_GPS2_Tightly : public BaseFINS { * by regulating props[n].sigma_(range|rate), whose negative or zero value yields * intentional exclusion. */ - /*{ // check DOP - typename solver_t::user_pvt_t::precision_t dop; - if(get_DOP(x, props, dop)){ + /*{ // check DOP/sigma + typename solver_t::user_pvt_t::precision_t dop, sigma_pos; + if(get_DOP_sigma(x, props, dop, sigma_pos)){ // do something std::cerr << dop.t << std::endl; } diff --git a/tool/navigation/INS_GPS_Debug.h b/tool/navigation/INS_GPS_Debug.h index f7d294077..8d190092a 100644 --- a/tool/navigation/INS_GPS_Debug.h +++ b/tool/navigation/INS_GPS_Debug.h @@ -261,16 +261,19 @@ class INS_GPS_Debug_Tightly : public INS_GPS_Debug { out << snapshot.props.size() << ','; // number of satellites do{ // DOP - typename super_t::solver_t::user_pvt_t::precision_t dop; - if(!super_t::get_DOP(snapshot.state, snapshot.props, dop)){ - out << ",,,,,"; + typename super_t::solver_t::user_pvt_t::precision_t dop, sigma_pos; + if(!super_t::get_DOP_sigma(snapshot.state, snapshot.props, dop, sigma_pos)){ + out << ",,,,,,,,"; break; } out << dop.g << ',' << dop.p << ',' << dop.h << ',' << dop.v << ',' - << dop.t << ','; + << dop.t << ',' + << sigma_pos.h << ',' + << sigma_pos.v << ',' + << sigma_pos.t << ','; }while(false); typedef std::vector order_t; From 595a3d211312b47cd59720347739a700e90a4d3f Mon Sep 17 00:00:00 2001 From: fenrir Date: Tue, 14 Nov 2023 14:08:18 +0900 Subject: [PATCH 22/25] Add velocity sigma to GPS solution outputs --- tool/INS_GPS/GNSS_Receiver.h | 6 ++++-- tool/misc/receiver_debug.rb | 4 ++-- tool/swig/GPS.i | 1 + 3 files changed, 7 insertions(+), 4 deletions(-) diff --git a/tool/INS_GPS/GNSS_Receiver.h b/tool/INS_GPS/GNSS_Receiver.h index 37943fc72..d8542c74c 100644 --- a/tool/INS_GPS/GNSS_Receiver.h +++ b/tool/INS_GPS/GNSS_Receiver.h @@ -545,6 +545,7 @@ data.gps.solver_options. expr << ',' << "v_east" << ',' << "v_down" << ',' << "receiver_clock_error_dot_ms" + << ',' << "vel_sigma" << ',' << "used_satellites" << ',' << "PRN"; } @@ -622,9 +623,10 @@ data.gps.solver_options. expr out << ',' << src.user_velocity_enu.north() << ',' << src.user_velocity_enu.east() << ',' << -src.user_velocity_enu.up() - << ',' << src.receiver_error_rate; + << ',' << src.receiver_error_rate + << ',' << src.sigma_vel.p; }else{ - out << ",,,,"; + out << ",,,,,"; } if(src.position_solved()){ out << ',' << src.used_satellites diff --git a/tool/misc/receiver_debug.rb b/tool/misc/receiver_debug.rb index 54f120c33..29b564e8a 100644 --- a/tool/misc/receiver_debug.rb +++ b/tool/misc/receiver_debug.rb @@ -53,11 +53,11 @@ def self.pvt_items(opt = {}) } ] }.call] + [[ - [:v_north, :v_east, :v_down, :receiver_clock_error_dot_ms], + [:v_north, :v_east, :v_down, :receiver_clock_error_dot_ms, :vel_sigma], proc{|pvt| next [nil] * 4 unless pvt.velocity_solved? [:north, :east, :down].collect{|k| pvt.velocity.send(k)} \ - + [pvt.receiver_error_rate] + + [pvt.receiver_error_rate, pvt.vel_sigma] } ]] + [ [:used_satellites, proc{|pvt| pvt.used_satellites}], diff --git a/tool/swig/GPS.i b/tool/swig/GPS.i index c1b0efb38..d5fa4d328 100644 --- a/tool/swig/GPS.i +++ b/tool/swig/GPS.i @@ -641,6 +641,7 @@ struct GPS_User_PVT const FloatT &hsigma() const {return base_t::sigma_pos.h;} const FloatT &vsigma() const {return base_t::sigma_pos.v;} const FloatT &tsigma() const {return base_t::sigma_pos.t;} + const FloatT &vel_sigma() const {return base_t::sigma_vel.p;} const unsigned int &used_satellites() const {return base_t::used_satellites;} std::vector used_satellite_list() const {return base_t::used_satellite_mask.indices_one();} bool position_solved() const {return base_t::position_solved();} From b0b4e303648e85baa23210bcafcd14c619eceb30 Mon Sep 17 00:00:00 2001 From: fenrir Date: Thu, 16 Nov 2023 22:54:45 +0900 Subject: [PATCH 23/25] Use freq. input to calculate rate with Doppler if freq. is available --- tool/navigation/GPS_Solver_Base.h | 18 ++++++++++++++---- 1 file changed, 14 insertions(+), 4 deletions(-) diff --git a/tool/navigation/GPS_Solver_Base.h b/tool/navigation/GPS_Solver_Base.h index 272462d28..9b6aa51bc 100644 --- a/tool/navigation/GPS_Solver_Base.h +++ b/tool/navigation/GPS_Solver_Base.h @@ -209,8 +209,13 @@ struct GPS_Solver_Base { if((res = find_value(values, measurement_items_t::L1_RANGE_RATE, buf))){ }else if((res = find_value(values, measurement_items_t::L1_DOPPLER, buf))){ - // Fall back to doppler - buf *= -space_node_t::L1_WaveLength(); + // Fall back to Doppler + float_t freq; + if(find_value(values, measurement_items_t::L1_FREQUENCY, freq)){ + buf *= -(space_node_t::light_speed / freq); + }else{ + buf *= -space_node_t::L1_WaveLength(); + } } return res; } @@ -221,8 +226,13 @@ struct GPS_Solver_Base { if((res = find_value(values, measurement_items_t::L1_RANGE_RATE_SIGMA, buf))){ }else if((res = find_value(values, measurement_items_t::L1_DOPPLER_SIGMA, buf))){ - // Fall back to doppler - buf *= space_node_t::L1_WaveLength(); + // Fall back to Doppler + float_t freq; + if(find_value(values, measurement_items_t::L1_FREQUENCY, freq)){ + buf *= (space_node_t::light_speed / freq); + }else{ + buf *= space_node_t::L1_WaveLength(); + } } return res; } From 5ebcbdd5bc998ac695d05ec70e3c93cf9980a9ff Mon Sep 17 00:00:00 2001 From: fenrir Date: Thu, 28 Mar 2024 16:22:07 +0900 Subject: [PATCH 24/25] Adjust psuedorange/rate sigma and add external sigma switch (def. off) --- tool/navigation/GPS_Solver.h | 22 ++++++++++------------ tool/navigation/GPS_Solver_Base.h | 10 ++++++++-- tool/swig/GPS.i | 1 + 3 files changed, 19 insertions(+), 14 deletions(-) diff --git a/tool/navigation/GPS_Solver.h b/tool/navigation/GPS_Solver.h index 3135bb313..1eca94727 100644 --- a/tool/navigation/GPS_Solver.h +++ b/tool/navigation/GPS_Solver.h @@ -55,10 +55,12 @@ template struct GPS_Solver_GeneralOptions { FloatT elevation_mask; FloatT residual_mask; + bool use_external_sigma; GPS_Solver_GeneralOptions() : elevation_mask(0), // elevation mask default is 0 [deg] - residual_mask(30) { // range residual mask is 30 [m] + residual_mask(1E3), // range residual mask is 1000 [m] + use_external_sigma(false) { // default is to ignore external sigma inputs } }; @@ -158,7 +160,9 @@ class GPS_SinglePositioning : public SolverBaseT { return sat(ptr).clock_error_dot(t_tx); } static float_t range_sigma(const void *ptr, const gps_time_t &t_tx) { - return sat(ptr).ephemeris().URA; + return std::sqrt( + std::pow(sat(ptr).ephemeris().URA, 2) + + std::pow(4.5 / 1.96, 2)); // UEE of modern receiver Table.B2-1 of April 2020 GPS SPS PS; } }; satellite_t res = { @@ -428,17 +432,11 @@ class GPS_SinglePositioning : public SolverBaseT { res.los_neg[0], res.los_neg[1], res.los_neg[2]); res.rate_sigma = sat.rate_sigma(time_arrival); -#if 0 - // TODO consider case when standard deviation of pseudorange and/or its rate are provided by receiver - if(!this->range_sigma(measurement, res.range_sigma)){ - // If receiver's range variance is not provided - res.range_sigma = 1E0; // TODO range error variance [m] + if(_options.use_external_sigma){ + // Use standard deviation of pseudorange and/or its rate if they are provided by receiver + this->range_sigma(measurement, res.range_sigma); + this->rate_sigma(measurement, res.rate_sigma); } - if(!this->rate_sigma(measurement, res.rate_sigma)){ - // If receiver's rate variance is not provided - res.rate_sigma = 1E-1; - } -#endif return res; } diff --git a/tool/navigation/GPS_Solver_Base.h b/tool/navigation/GPS_Solver_Base.h index 9b6aa51bc..340ecba1a 100644 --- a/tool/navigation/GPS_Solver_Base.h +++ b/tool/navigation/GPS_Solver_Base.h @@ -289,7 +289,8 @@ struct GPS_Solver_Base { inline float_t range_sigma(const gps_time_t &t_tx) const { return impl_range_sigma ? impl_range_sigma(impl_error, t_tx) - : 3.5; // 7.0 [m] of 95% (2-sigma) URE in Sec. 3.4.1 of April 2020 GPS SPS PS; + : (std::sqrt((9.7 * 9.7) + (4.5 * 4.5)) / 1.96); // 5.45 [m] + // UERE (User Equivalent Range Error) of modern receiver in Eq.(B-8) of April 2020 GPS SPS PS; } /** * Return expected user range rate accuracy (URRA) in standard deviation (1-sigma) @@ -299,7 +300,12 @@ struct GPS_Solver_Base { inline float_t rate_sigma(const gps_time_t &t_tx) const { return impl_rate_sigma ? impl_rate_sigma(impl_error, t_tx) - : 0.003; // 0.006 [m/s] of 95% (2-sigma) URRE in Sec. 3.4.2 of April 2020 GPS SPS PS + : 5E-1; // (Empirical value) + // Alternatives (may occur failure); + // 1) (std::sqrt(((2.0 * 2.0) + (1.0 * 1.0)) * 2) / 1.96); // 1.61 [m/s] + // assume differential range with error consisting of "Receiver noise and resolution" + // and "Other user segment errors" in UEE Table B.2-1 of April 2020 GPS SPS PS + // 2) 0.006 m/s(95%) SPS SIS URRE in Table 3.4-2 is not considered. } static const satellite_t &unavailable() { struct impl_t { diff --git a/tool/swig/GPS.i b/tool/swig/GPS.i index d5fa4d328..9e620301f 100644 --- a/tool/swig/GPS.i +++ b/tool/swig/GPS.i @@ -896,6 +896,7 @@ struct GPS_Measurement { %extend GPS_SolverOptions_Common { MAKE_ACCESSOR2(elevation_mask, cast_general()->elevation_mask, FloatT); MAKE_ACCESSOR2(residual_mask, cast_general()->residual_mask, FloatT); + MAKE_ACCESSOR2(use_external_sigma, cast_general()->use_external_sigma, bool); MAKE_VECTOR2ARRAY(int); %ignore cast_general; } From d2dd789137db19895ca072e166ccafc4b1b3c861 Mon Sep 17 00:00:00 2001 From: fenrir Date: Thu, 28 Mar 2024 17:07:32 +0900 Subject: [PATCH 25/25] Add GPS URA_index and fit_interval(flag) interface to SWIG wrapper --- tool/swig/GPS.i | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/tool/swig/GPS.i b/tool/swig/GPS.i index 9e620301f..37cb752bd 100644 --- a/tool/swig/GPS.i +++ b/tool/swig/GPS.i @@ -363,6 +363,18 @@ struct GPS_Ephemeris : public GPS_SpaceNode::SatelliteProperties::Epheme System_XYZ position, velocity; FloatT clock_error, clock_error_dot; }; + + int get_URA_index() const { + return GPS_Ephemeris::URA_index(this->URA); + } + int set_URA_index(const int &idx) { + this->URA = GPS_Ephemeris::URA_meter(idx); + return get_URA_index(); + } + FloatT set_fit_interval(const bool &flag) { + // set iodc before invocation of this function + return (this->fit_interval = GPS_Ephemeris::raw_t::fit_interval(flag, this->iodc)); + } }; %} %extend GPS_Ephemeris { @@ -398,6 +410,9 @@ struct GPS_Ephemeris : public GPS_SpaceNode::SatelliteProperties::Epheme MAKE_ACCESSOR(dot_Omega0, FloatT); MAKE_ACCESSOR(dot_i0, FloatT); + %rename(%str(URA_index=)) set_URA_index; + %rename(%str(URA_index)) get_URA_index; + MAKE_ARRAY_INPUT(const unsigned int, buf, SWIG_AsVal(unsigned int)); %apply int *OUTPUT { int *subframe_no, int *iodc_or_iode }; void parse(const unsigned int buf[10], int *subframe_no, int *iodc_or_iode){