ULoPoS: dump2dat adapted to new odo format & watchtower cleaned up

diff --git a/src/uzv/watchtower/dump2dat.c b/src/uzv/watchtower/dump2dat.c
index a09325311c3194af67a9072a16165b6f4dfee363..5e7cd2021854c32c08e83148cd1d6fc65c4ee65c 100644 (file)
--- a/src/uzv/watchtower/dump2dat.c
+++ b/src/uzv/watchtower/dump2dat.c
@@ -6,7 +6,7 @@
 
 int main(int argc, char *argv[]) {
   long long line = 0;
-  int i, virgo = 1;
+  int i, odo_n = 0, virgo = 1;
   int msg_id, msg_length;
   uint64_t msg_data;
   uint8_t b, seq, seq0 = 0;
@@ -37,8 +37,9 @@ int main(int argc, char *argv[]) {
       d3 = (msg_data>>32) & 0xffff;
       break;
     case 0x022:
-      odo_l = ntohl(msg_data & 0xffffffff);
-      odo_r = ntohl((msg_data>>32) & 0xffffffff);
+      odo_l = ntohl(msg_data & 0xffffff);
+      odo_r = ntohl((msg_data>>24) & 0xffffff);
+      ++odo_n;
       break;
     case 0x008:
       seq = msg_data & 0xff;
@@ -49,10 +50,11 @@ int main(int argc, char *argv[]) {
          fprintf(stderr, "L %llu: Ztrata vzorku (0x%02x misto 0x%02x)\n",
                  line, seq, seq0);
        printf("%u\t%u\t%u\t%u\t%u\n", d1, d2, d3, odo_l, odo_r);
-       //printf("%u\t%u\t%u\n", d1, d2, d3);
        fflush(stdout);
+       //fprintf(stderr, "%d\n", odo_n);
       }
       seq0 = seq;
+      odo_n = 0;
       break;
     }
   }

diff --git a/src/uzv/watchtower/wt.c b/src/uzv/watchtower/wt.c
index d01ebca1f8004c04c142c18cf06ca7f3231a5277..d98191bce16893746e8a699486f0d7046b8d5039 100644 (file)
--- a/src/uzv/watchtower/wt.c
+++ b/src/uzv/watchtower/wt.c
@@ -22,25 +22,6 @@ real_t beacon_xy[3][2] = {
 plot_data_t data = { .n = MAX_POINTS, };
 ekf8_t ekf8;
 
-int hyperb_xy(real_t *hx, real_t *hy, real_t *t) {
-  real_t x[2], y[2];
-  int i1, i2, i3, ok;
-
-  /* unwrap */
-  i1 = (t[0]<t[1]) ? ((t[0]<t[2])?0:2) : ((t[1]<t[2])?1:2);
-  i3 = (t[0]>t[1]) ? ((t[0]>t[2])?0:2) : ((t[1]>t[2])?1:2);
-  i2 = 6 - i1 - i3;
-  if (t[i3] - t[i1] > 0.5*D_MAX)
-    t[i1] += D_MAX;
-  if (t[i3] - t[i2] > 0.5*D_MAX)
-    t[i2] += D_MAX;
-  
-  ok = hypintsec((real_t*)beacon_xy, x, y, t[0]-t[1], t[0]-t[2]);
-
-  *hx = x[0];  *hy = y[0];
-  return ok;
-}
-
 int main(int argc, char *argv[]) {
   real_t y[5], t[3], x[8], P[8*8];
   uint32_t u[3], odo0[2], odo[2];
@@ -49,22 +30,27 @@ int main(int argc, char *argv[]) {
   xw_init();
   
   while (!feof(stdin)) {
+#if 1
     scanf("%u%u%u%u%u", u, u+1, u+2, odo, odo+1);
 
     for (i = 0; i < 3; i++)
       t[i] = (XCORR2METER/32.0)*u[i];
 
-#if 1
     y[0] = t[0];  y[1] = t[1];  y[2] = t[2];
     y[3] = ODO_C*(real_t)((int32_t)(odo[0] - odo0[0]));
     y[4] = ODO_C*(real_t)((int32_t)(odo0[1] - odo[1]));
     odo0[0] = odo[0];  odo0[1] = odo[1];
-    printf("%g\t%g\n", y[3], y[4]);
+    y[3] = -y[3];  y[4] = -y[4];
+    printf("ODO:  %f  %f\n", y[3], y[4]);
+#else
+    scanf("%lf%lf%lf%lf%lf", &y[0], &y[1], &y[2], &y[3], &y[4]);
+#endif
 
     if (virgo) {
       real_t xy0[] = {2.63, 0.35};
       y[3] = y[4] = 0.0;
       ekf8_init(&ekf8, (real_t*)beacon_xy, D_MAX, 30.0, xy0, y);
+      ekf8.ekf.x[7] = -45.0*M_PI/180.0;
       virgo = 0;
     }
 
@@ -78,20 +64,16 @@ int main(int argc, char *argv[]) {
     data.v1 = x[4];  data.v2 = x[5];
     data.s11 = P[0];  data.s12 = P[1];  data.s22 = P[9];
     data.phi = x[6];
-#else
-    /* alternative: no EKF estimation, use analytic calculation */
 
-    real_t xx[2], yy[2];
-    hypintsec(xx, yy, t[0]-t[1], t[0]-t[2]);
-    data.x1[0] = xx[0];
-    data.x2[0] = yy[0];
-#endif
+    //printf("%f   %f   %f\t%8.4f\n", x[0], x[1], x[2], x[6]*(180.0/M_PI));
+    printf("exc=");
+    for (i = 0; i < 5; i++)
+      printf("%g%c", ekf8.ekf.exc[i], (i==4)?'\n':'\t');
     /*
-    for (i = 0; i < 8; i++)
-      printf("%f\t", x[i]);
-    printf("%d %d %d\n", data.err[0], data.err[1], data.err[2]);
+    printf("P=");
+    for (i = 0; i < 8*8; i++)
+      printf("%g%c", P[i], (i==8*8-1)?'\n':'\t');
     */
-    printf("%f   %f   %f\t%8.4f\n", x[0], x[1], x[2], x[6]*(180.0/M_PI));
 
     xw_draw(&data);
   }