Merge pure GPS into SBAS; Make rate sigma treatment rigorous

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;

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

tool/INS_GPS.cpp

@@ -1120,25 +1120,32 @@ class INS_GPS_NAV : public NAV {
     }
 
   protected:
-    static void set_matrix_full(mat_t &mat, const char *spec){
-      char *_spec(const_cast<char *>(spec));
+    static bool set_matrix_full(mat_t &mat, const char *spec){
+      char *_spec(const_cast<char *>(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<char *>(spec));
+    static bool set_matrix_diagonal(mat_t &mat, const char *spec){
+      char *_spec(const_cast<char *>(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<char *>(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<char *>(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 *){
@@ -1147,41 +1154,43 @@ class INS_GPS_NAV : public NAV {
     bool init_misc(const char *line, INS<float_t> *){
       const char *value;
       if(value = Options::get_value2(line, "x")){
-        char *spec(const_cast<char *>(value));
-        int i((int)std::strtol(spec, &spec, 10));
-        (*ins_gps)[i] = std::strtod(spec, &spec);
-        return true;
+        char *spec(const_cast<char *>(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 BaseINS, template <class> class Filter>
     bool init_misc(const char *line, Filtered_INS2<BaseINS, Filter> *){
       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);
@@ -1367,9 +1376,13 @@ struct INS_GPS_NAV_Factory : public NAV_Factory<INS_GPS> {
   struct Checker<INS_GPS_Debug_PureInertial<T> > {
     template <class Calibration>
     static NAV *check_navdata(const Calibration &calibration){
-      return INS_GPS_NAV_Factory<
-            INS_GPS_NAVData<INS_GPS_Debug_PureInertial<T> >
-          >::generate(calibration);
+      return options.dump_stddev ?
+          INS_GPS_NAV_Factory<
+              INS_GPS_NAVData<INS_GPS_Debug_PureInertial<T, typename T::filtered_ins_t> >
+            >::generate(calibration) :
+          INS_GPS_NAV_Factory<
+              INS_GPS_NAVData<INS_GPS_Debug_PureInertial<T> >
+            >::generate(calibration);
     }
   };
 
@@ -2671,7 +2684,10 @@ class INS_GPS_NAV<INS_GPS>::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);
+        }
       }
     }
 

tool/INS_GPS/GNSS_Receiver.h

@@ -599,10 +599,14 @@ data.sbas.solver_options. expr
             << ',' << "hdop"
             << ',' << "vdop"
             << ',' << "tdop"
+            << ',' << "hsigma"
+            << ',' << "vsigma"
+            << ',' << "tsigma"
             << ',' << "v_north"
             << ',' << "v_east"
             << ',' << "v_down"
             << ',' << "receiver_clock_error_dot_ms"
+            << ',' << "vel_sigma"
             << ',' << "used_satellites"
             << ',' << "GPS_PRN(1-32)"
 #if !defined(BUILD_WITHOUT_GNSS_MULTI_CONSTELLATION)
@@ -679,17 +683,21 @@ data.sbas.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()
             << ',' << 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

tool/log_CSV.cpp

@@ -466,19 +466,27 @@ class StreamProcessor : public SylphideProcessor<float_sylph_t> {
       void (HandlerP::*formatter)(
           const float_sylph_t &current, 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<float_sylph_t> {
 
         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<float_sylph_t> {
         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<float_sylph_t> {
                   d1(be_char4_2_num<Uint32>(buf[0][0])),
                   d2(be_char4_2_num<Uint32>(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<float_sylph_t> {
 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) \

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}
@@ -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