ULoPoS: angle added to watchtower

diff --git a/src/uzv/watchtower/wt.c b/src/uzv/watchtower/wt.c
index 69e8aead45bfde037ed7db4a7eafb63eab52efc6..d01ebca1f8004c04c142c18cf06ca7f3231a5277 100644 (file)
--- a/src/uzv/watchtower/wt.c
+++ b/src/uzv/watchtower/wt.c
@@ -77,6 +77,7 @@ int main(int argc, char *argv[]) {
     data.x1[0] = x[0];  data.x2[0] = x[1];
     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 */
 

diff --git a/src/uzv/watchtower/wt.h b/src/uzv/watchtower/wt.h
index 983905e9703a2d58d7648e87b1704bbc7d20f0f0..a8d041b7911ed7a3ede1590e93d36f9a4b32ff05 100644 (file)
--- a/src/uzv/watchtower/wt.h
+++ b/src/uzv/watchtower/wt.h
@@ -20,7 +20,8 @@ typedef struct plot_data_t {
   int n;
   real_t x1[256], x2[256];
   real_t v1, v2, s11, s12, s22;
-  int err[3];
+  real_t phi;
+  int err[5];
 } plot_data_t;
 
 extern real_t beacon_xy[3][2];

diff --git a/src/uzv/watchtower/xw.c b/src/uzv/watchtower/xw.c
index fc6798e13a11b8797fcd2e077f28699ae90d6a9a..3a177a268e785967fcf3c61f2dc9a8d1ecfa7c1b 100644 (file)
--- a/src/uzv/watchtower/xw.c
+++ b/src/uzv/watchtower/xw.c
@@ -25,7 +25,7 @@ Window w, dw;
 int dpy_dbe;
 XdbeBackBuffer w_bb;
 XdbeSwapInfo bb_swap;
-GC gc_path, gc_velo, gc_cov, gc_fld1, gc_fld2;
+GC gc_path, gc_velo, gc_cov, gc_fld1, gc_fld2, gc_bot;
 
 real_t xy_scale, cov_c[COV_N], cov_s[COV_N];
 int x0x, y0y;
@@ -55,15 +55,16 @@ void set_coord() {
 }
 
 void init_gfx() {
-  GC *p, *gc[] = {&gc_path, &gc_velo, &gc_cov, &gc_fld1, &gc_fld2};
-  char *color[] = {"#FFFFFF", "#FF0000", "#FF00FF", "#00FFFF", "#0000FF"};
+  GC *p, *gc[] = {&gc_path, &gc_velo, &gc_cov, &gc_fld1, &gc_fld2, &gc_bot};
+  char *color[] = {"#FFFFFF", "#FF0000", "#FF00FF", "#00FFFF", "#0000FF",
+                  "#777777"};
   Colormap cmap;
   XColor col;
   int i;
 
   cmap = DefaultColormap(dpy, 0);
 
-  for (i = 0; i < 5; i++) {
+  for (i = 0; i < 6; i++) {
     p = gc[i];
     *p = XCreateGC(dpy, w, 0, NULL);
     XParseColor(dpy, cmap, color[i], &col);
@@ -191,6 +192,28 @@ void plot_velo(plot_data_t *data) {
   XDrawLine(dpy, dw, gc_velo, x2, y2, x2 + dx, y2 - dy);
 }
 
+void plot_bot(plot_data_t *data) {
+  static real_t
+    b1[] = {0, 0,  258, 0,  258, 290,  0, 290,  0, 0},
+    b2[] = {35, 0,  35, 40,  115, 40,  115, 0,  35, 0},
+    b3[] = {35, 290,  35, 250,  115, 250,  115, 290,  35, 290};
+  static real_t *b[] = {b1, b2, b3};
+  //static int len[] = {length(b1), length(b2), length(b3)};
+  real_t cp, sp, x, y;
+  XPoint pt[5];
+  int i, j, k;
+
+  cp = cos(data->phi);  sp = sin(data->phi);
+  for (i = 0; i < 3 /*length(len)*/; i++) {
+    for (j = 0, k = 0; j < 5/*len[i]/2*/; j++, k += 2) {
+      x = b[i][k] - 129.0;  y = b[i][k+1] - 145.0;
+      pt[j].x = x2x(data->x1[0] + 0.001*(cp*x - sp*y));
+      pt[j].y = y2y(data->x2[0] + 0.001*(sp*x + cp*y));
+    }
+    XDrawLines(dpy, dw, gc_bot, pt, 5 /*len[i]/2*/, CoordModeOrigin);
+  }
+}
+
 void xw_draw(plot_data_t *data) {
   xw_flush_events();
 
@@ -199,6 +222,7 @@ void xw_draw(plot_data_t *data) {
 
   draw_field(data);
   plot_path(data);
+  plot_bot(data);
   plot_cov(data);
   plot_velo(data);
 

diff --git a/src/uzv/watchtower/xw6.c b/src/uzv/watchtower/xw6.c
new file mode 100644 (file)
index 0000000..fc6798e
--- /dev/null
+++ b/src/uzv/watchtower/xw6.c
@@ -0,0 +1,209 @@
+#include <X11/Xlib.h>
+#include <X11/extensions/Xdbe.h>
+#include <inttypes.h>
+#include <limits.h>
+#include <stdlib.h>
+#include <math.h>
+
+#include <stdio.h>
+
+#include "wt.h"
+
+#define WIDTH   700
+#define HEIGHT  500
+#define BORDER   20
+
+#define PT_SIZE   1
+#define VELO_SCALE 10.0
+#define VELO_DX  -5
+#define VELO_DY  -3
+#define COV_SCALE 1000.0
+#define COV_N    12
+
+Display *dpy;
+Window w, dw;
+int dpy_dbe;
+XdbeBackBuffer w_bb;
+XdbeSwapInfo bb_swap;
+GC gc_path, gc_velo, gc_cov, gc_fld1, gc_fld2;
+
+real_t xy_scale, cov_c[COV_N], cov_s[COV_N];
+int x0x, y0y;
+
+void set_coord() {
+  real_t tmp, min_x, max_x, min_y, max_y;
+  int i, m;
+
+  min_x = max_x = beacon_xy[0][0];
+  min_y = max_y = beacon_xy[0][1];
+  for (m = 0; m < 3; m++) {
+    if ((tmp = beacon_xy[m][0] - 0.04) < min_x) min_x = tmp;
+    if ((tmp = beacon_xy[m][0] + 0.04) > max_x) max_x = tmp;
+    if ((tmp = beacon_xy[m][1] - 0.04) < min_y) min_y = tmp;
+    if ((tmp = beacon_xy[m][1] + 0.04) > max_y) max_y = tmp;
+  }
+  xy_scale = (WIDTH - 2*BORDER)/(max_x - min_x);
+  if ((tmp = (HEIGHT - 2*BORDER)/(max_y - min_y)) < xy_scale)
+    xy_scale = tmp;
+  x0x = 0.5*(WIDTH - xy_scale*(min_x + max_x) + 1.0);
+  y0y = 0.5*(HEIGHT + xy_scale*(min_y + max_y) + 1.0);
+
+  for (i = 0; i < COV_N; i++) {
+    tmp = (2*M_PI*i)/(real_t)COV_N;
+    cov_c[i] = cos(tmp);  cov_s[i] = sin(tmp);
+  }
+}
+
+void init_gfx() {
+  GC *p, *gc[] = {&gc_path, &gc_velo, &gc_cov, &gc_fld1, &gc_fld2};
+  char *color[] = {"#FFFFFF", "#FF0000", "#FF00FF", "#00FFFF", "#0000FF"};
+  Colormap cmap;
+  XColor col;
+  int i;
+
+  cmap = DefaultColormap(dpy, 0);
+
+  for (i = 0; i < 5; i++) {
+    p = gc[i];
+    *p = XCreateGC(dpy, w, 0, NULL);
+    XParseColor(dpy, cmap, color[i], &col);
+    XAllocColor(dpy, cmap, &col);
+    XSetForeground(dpy, *p, col.pixel);
+    XSetLineAttributes(dpy, *p, 0, LineSolid, CapButt, JoinBevel);
+  }
+}
+
+void xw_init() {
+  XEvent e;
+  int v_maj, v_min;
+
+  dpy = XOpenDisplay(NULL);
+  w = XCreateSimpleWindow(dpy, DefaultRootWindow(dpy), 0, 0, WIDTH, HEIGHT, 0,
+                          WhitePixel(dpy, 0), BlackPixel(dpy, 0));
+
+  if ((dpy_dbe = XdbeQueryExtension(dpy, &v_maj, &v_min))) {
+    w_bb = XdbeAllocateBackBufferName(dpy, w, XdbeBackground);
+    bb_swap.swap_window = w;
+    bb_swap.swap_action = XdbeBackground;
+    dw = w_bb;
+  }
+  else
+    dw = w;
+
+  init_gfx();
+  set_coord();
+
+  XSelectInput(dpy, w, ExposureMask);
+  XMapWindow(dpy, w);
+
+  for (;;) {
+    XNextEvent(dpy, &e);
+    if (e.type == Expose)
+      break;
+  }
+}
+
+void xw_flush_events() {
+  XEvent e;
+  while (XCheckWindowEvent(dpy, w, -1, &e));
+}
+
+inline int x2x(real_t x) {
+  real_t tmp = xy_scale*x + 0.5;
+  if (tmp < INT_MIN)
+    return INT_MIN;
+  if (tmp > INT_MAX)
+    return INT_MAX;
+  return x0x + (int)tmp;
+}
+
+inline int y2y(real_t y) {
+  real_t tmp = - xy_scale*y + 0.5;
+  if (tmp < INT_MIN)
+    return INT_MIN;
+  if (tmp > INT_MAX)
+    return INT_MAX;
+  return y0y + (int)tmp;
+}
+
+void draw_field(plot_data_t *data) {
+  XPoint pt[5];
+  int m, x, y, a = (int)(xy_scale*0.04 + 0.5), b = 1+a/2;
+
+  for (m = 0; m < 3; m++) {
+    x = x2x(beacon_xy[m][0]);  y = y2y(beacon_xy[m][1]);
+    pt[0].x = x-a;  pt[0].y = y-a;
+    pt[1].x = x-a;  pt[1].y = y+a;
+    pt[2].x = x+a;  pt[2].y = y+a;
+    pt[3].x = x+a;  pt[3].y = y-a;
+    pt[4] = pt[0];
+    XDrawLines(dpy, dw, gc_fld1, pt, 5, CoordModeOrigin);
+
+    if (data->err[m])
+      XFillArc(dpy, dw, gc_velo, x-b, y-b, b, b, 0, 360*64);
+  }
+
+  /***/
+}
+
+void plot_path(plot_data_t *data) {
+  XPoint pt[MAX_POINTS];
+  int i;
+
+  for (i = 0; i < data->n; i++) {
+    pt[i].x = x2x(data->x1[i]);
+    pt[i].y = y2y(data->x2[i]);
+  }
+  XDrawLines(dpy, dw, gc_path, pt, data->n, CoordModeOrigin);
+  for (i = 0; i < data->n; i++)
+    XFillArc(dpy, dw, gc_path, pt[i].x - PT_SIZE, pt[i].y - PT_SIZE,
+            2*PT_SIZE, 2*PT_SIZE, 0, 360*64);
+}
+
+void plot_cov(plot_data_t *data) {
+  XPoint pt[COV_N+1];
+  int i;
+
+  for (i = 0; i < COV_N; i++) {
+    pt[i].x = x2x(data->x1[0] +
+                 COV_SCALE*(data->s11*cov_c[i] + data->s12*cov_s[i]));
+    pt[i].y = y2y(data->x2[0] +
+                 COV_SCALE*(data->s12*cov_c[i] + data->s22*cov_s[i]));
+  }
+  pt[COV_N] = pt[0];
+  XDrawLines(dpy, dw, gc_cov, pt, COV_N+1, CoordModeOrigin);
+}
+
+void plot_velo(plot_data_t *data) {
+  int dx, dy,
+    x1 = x2x(data->x1[0]), y1 = y2y(data->x2[0]),
+    x2 = x1 + (int)(VELO_SCALE*xy_scale*data->v1 + 0.5),
+    y2 = y1 - (int)(VELO_SCALE*xy_scale*data->v2 + 0.5);
+  real_t c;
+
+  XDrawLine(dpy, dw, gc_velo, x1, y1, x2, y2);
+  c = 1.0/sqrt(sqr(data->v1) + sqr(data->v2) + 1e-12);
+  dx = (int)(c*(data->v1*VELO_DX - data->v2*VELO_DY) + 0.5);
+  dy = (int)(c*(data->v2*VELO_DX + data->v1*VELO_DY) + 0.5);
+  XDrawLine(dpy, dw, gc_velo, x2, y2, x2 + dx, y2 - dy);
+  dx = (int)(c*(data->v1*VELO_DX + data->v2*VELO_DY) + 0.5);
+  dy = (int)(c*(data->v2*VELO_DX - data->v1*VELO_DY) + 0.5);
+  XDrawLine(dpy, dw, gc_velo, x2, y2, x2 + dx, y2 - dy);
+}
+
+void xw_draw(plot_data_t *data) {
+  xw_flush_events();
+
+  if (!dpy_dbe)
+    XClearWindow(dpy, dw);
+
+  draw_field(data);
+  plot_path(data);
+  plot_cov(data);
+  plot_velo(data);
+
+  if (dpy_dbe)
+    XdbeSwapBuffers(dpy, &bb_swap, 1);
+
+  XFlush(dpy);
+}