glcricket.c

无线传感器网络中的节点定位算法。详见ReadMe文件。在TinyOS上实现的节点定位算法。

            i = node_new_v1(id);

        if (i != -1) {
            if (solve_read) {
                pos[i].x = x;
                pos[i].y = y;
                pos[i].last_x = x;
                pos[i].last_y = y;
                pos[i].stat |= STAT_VISIBLE;
                pos[i].stat &= ~STAT_MARKED;
            }
            else if (opt_read) {
                pos[i].ox = x;
                pos[i].oy = y;
                pos[i].stat |= STAT_OPTIMIZED;
            }
        }
    }
    else if (!strcmp(str, "done")) {
        solve_read = 0;
        memcpy(quads, quads_new, sizeof(quads));
        num_quads = num_quads_new;
        memcpy(pings_summary, pings_summary_new, sizeof(pings_summary));
        //drawGLScene();
    }
    else if (!strcmp(str, "opt done")) {
        opt_read = 0;
    }
    else if (!strncmp(str, "id=", 3)) {
        int dist, time;
        int myid, j;
        i = sscanf(str, "id=%x,beacon=%x,dist=%d,time=%d", &myid, &id, &dist, &time);
        if (i != 4)
            return;
        for (j=0; j<NUM_NAMES; j++) {
            if (names[j].crc == myid) {
                pos[max_nodes].name = names[j].name;
                pos[max_nodes].id[3] = myid;
            }
        }
        i = node_find_v1(id);
        if (i == -1)
            return;
        getcrickettime(&pings[max_nodes][i]);
        pings_d[max_nodes][i] = time;
        if (edge_picked && edge_v[0] == max_nodes && edge_v[1] == i) {
            new_edge = 1;
            new_edge_len = time/(float)30;
        }
        //drawGLScene();
    }
    else if (!strncmp(str, "  from 0x", 9)) {
        char buf[16];
        i = sscanf(str, " from 0x%x%7c:", &id, buf);
        buf[7] = '\0';
        if (i == 2) {
            if (summary_complete) {
                bzero(pings_summary_new, sizeof(pings_summary_new));
                summary_complete = 0;
            }
            if (!strcmp(buf, " (self)")) {
                summary_complete = 1;
            }
            i = node_find_v1(id);
            if (i == -1)
                return;
            last_node = i;
            process_dists_v1(i, str+19);
        }
    }
    else if (!strncmp(str, "          0x", 12)) {
        process_dists_v1(last_node, str);
    }
}

char linebuf[256];

void get_serial_input(int serfd)
{
    struct timeval tv;
    fd_set rfds;
    char buf[256];
    int len;
    char * a, * s;

    tv.tv_sec = 0;
    tv.tv_usec = 50000;
    FD_ZERO(&rfds);
    FD_SET(serfd, &rfds);

    if (select(serfd+1, &rfds, NULL, NULL, &tv) == 1) {
        len = read(serfd, buf, sizeof(buf)-1);
        buf[len] = '\0';
        s = buf;
        while ((a = strchr(s, '\n'))) {
            *a = '\0';
            strcat(linebuf, s);
            process_line(linebuf);
            strcpy(linebuf, "");
            s = a+1;
        }
        strcat(linebuf, s);
    }
}

struct timeval tdiff = {0, 0};

void settimediff(int skipusec)
{
    struct timeval tv;
    int lower;
    gettimeofday(&tv, NULL);
    /*
    if (tv.tv_usec < crickettime.tv_usec) {
        tdiff.tv_sec = tv.tv_sec - crickettime.tv_sec - 1;
        tdiff.tv_usec = 1000000 + tv.tv_usec - crickettime.tv_usec;
    }
    else {
        tdiff.tv_sec = tv.tv_sec - crickettime.tv_sec;
        tdiff.tv_usec = tv.tv_usec - crickettime.tv_usec;
    }
    */

    lower = tv.tv_usec-crickettime.tv_usec-skipusec;
    tdiff.tv_sec = tv.tv_sec - crickettime.tv_sec + (lower-1000000)/1000000;
    lower %= 1000000;
    tdiff.tv_usec = (lower < 0)?(1000000+lower):lower;
}


int get_file_input(FILE * in, int wait)
{
    struct timeval tv;
    int sleeptime = 50000;
    int c, sec, usec;

    gettimeofday(&tv, NULL);
    do {
        c = sscanf(linebuf, "%d.%d:", &sec, &usec);
        if (c != 2)
            continue;
        usec *= 1000;
        if (tdiff.tv_sec == 0 && tdiff.tv_usec == 0) {
            crickettime.tv_sec = sec;
            crickettime.tv_usec = usec;
            settimediff(0);
        }
        sleeptime = (sec + tdiff.tv_sec - tv.tv_sec) * 1000000 +
            (usec + tdiff.tv_usec - tv.tv_usec);
        if (sleeptime < 0)
            sleeptime = 0;

        crickettime.tv_sec = sec + (usec-sleeptime-1000000)/1000000;
        crickettime.tv_usec = (usec+1000000-sleeptime)%1000000;
        
        if (sleeptime > 50000)
            sleeptime = 50000;

        if (sleeptime > 0) {
            if (wait)
                usleep(sleeptime);
     //                   getcrickettime(&tv);
     //                   printf("s = %d, us = %d\n", tv.tv_sec, tv.tv_usec);
            return 0;
        }
        linebuf[strlen(linebuf)-1] = '\0';
        process_line(strchr(linebuf,' ')+1);
    } while (fgets(linebuf, sizeof(linebuf), in));

    strcpy(linebuf, "");
    if (wait)
        usleep(50000);
    return -1;
}

void read_truth(char * filename)
{
    FILE * in;
    char buf[256];

    in = fopen(filename, "r");
    if (!in)
        return;

    free(truth);
    num_truth = 0;

    while (fgets(buf, sizeof(buf), in)) {
        char name[8];
        float x,y;
        if (sscanf(buf, "%8s %f %f\n", name, &x, &y) != 3)
            continue;

        truth = realloc(truth, (num_truth+1)*sizeof(struct truth_pos));
        truth[num_truth].x = x;
        truth[num_truth].y = y;
        truth[num_truth].name = strdup(name);
        num_truth++;
    }

    fclose(in);
}

int main(int argc, char **argv)
{
    int fd;
    XEvent event;
    Bool done;
    char c;
    FILE * input = NULL;
    char outstr[256];
    int do_output = 0;
    int frame = 0;

    while ((c = getopt(argc, argv, "i:v:t:o:cr:f:s")) >= 0) {
        switch (c) {
            case 'i':
                input = fopen(optarg, "r");
                isrecorded = 1;
                if (!input) {
                    perror("open");
                    exit(1);
                }
                break;
            case 'v':
                input_version = atoi(optarg);
                if (input_version == 1) {
                    process_line = process_line_v1;
                    max_nodes = 20;
                }
                break;
            case 't':
                read_truth(optarg);
                break;
            case 'o':
                do_output = 1;
                paused = 1;
                strcpy(outstr, optarg);
                break;
            case 'c':
                do_cones = 1;
                break;
            case 'r':
                do_rot = 1;
                strcpy(rotname, optarg);
                break;
            case 'f':
                do_flip = 1;
                strcpy(flipname, optarg);
                break;
            case 's':
                do_smoothing = 1;
                break;
            default:
                exit(1);
        }
    }

    if (do_rot != do_flip) {
        fprintf(stderr, "Must specify -r and -f together\n");
        exit(1);
    }
    
    strcpy(linebuf, "");
    init_pos();

    if (!input) {
        fd = serial_open("/dev/ttyS0");
        if (fd < 0) {
            return 1;
        }
    }

    done = False;
    /* default to fullscreen */
    GLWin.fs = False;
    createGLWindow("Cricket Viewer", WIDTH, HEIGHT, 24, GLWin.fs);

    if (input)
        get_file_input(input, 0);

    getcrickettime(&starttime);

    /* wait for events*/ 
    while (!done)
    {
        if (input) {
            if (!paused)
                get_file_input(input, 1);
            else if (do_output) {
                if (get_file_input(input, 0) == -1)
                    break;
            }
            else
                usleep(50000);
        }
        else
            get_serial_input(fd);

        if (auto_zoom) {
            recompute_bounding(&target_ox, &target_oy, &target_fov, origin_x,
                    origin_y, fov);

            if (do_rot == 2 && (pos[rot_node].stat & STAT_VISIBLE)) {
                float newrot;
                if (do_flip == 2 && pos[flip_node].stat & STAT_VISIBLE) {
                    float cross = pos[flip_node].x * pos[rot_node].y - pos[flip_node].y * pos[rot_node].x;
                    /* last_cross will be negative if cross and last_cross
                     * differ in sign */
                    last_cross *= cross;
                    if (last_cross < 0) {
                        comp_flip = -comp_flip;
                        printf("adjust: FLIPPED\n");
                    }
                    last_cross = cross;
                }
                newrot = -atan2(pos[rot_node].y, comp_flip*pos[rot_node].x);
                newrot = 180 * newrot / M_PI;
                comp_rot += (newrot - last_rot);
                printf("adjust: %f\n", comp_rot);
                last_rot = newrot;
            }

            origin_x += (target_ox - origin_x) * 0.1;
            origin_y += (target_oy - origin_y) * 0.1;
            fov += (target_fov - fov) * 0.1;
        }
        /* handle the events in the queue */
        while (XPending(GLWin.dpy) > 0)
        {
            KeySym k;
            XNextEvent(GLWin.dpy, &event);
            switch (event.type)
            {
                case Expose:
	                if (event.xexpose.count != 0)
	                    break;
                    drawGLScene();
         	        break;
            case ConfigureNotify:
            /* call resizeGLScene only if our window-size changed */
                if ((event.xconfigure.width != GLWin.width) || 
                    (event.xconfigure.height != GLWin.height))
                {
                    GLWin.width = event.xconfigure.width;
                    GLWin.height = event.xconfigure.height;
                    //    printf("Resize event\n");
                    resizeGLScene(event.xconfigure.width,
                        event.xconfigure.height);
                }
                break;
            case ButtonPress:
                if (picking_edge == 1) {
                    edge_v[0] = find_node_by_xy(event.xbutton.x, event.xbutton.y);
                    picking_edge = 2;
                }
                else if (picking_edge == 2) {
                    struct timeval tv;
                    edge_v[1] = find_node_by_xy(event.xbutton.x, event.xbutton.y);
                    picking_edge = 0;
                    edge_picked = 1;
                    edge_head = 0;
                    edge_tail = 0;
                    getcrickettime(&tv);
                    edgeupdate.tv_sec = tv.tv_sec;
                    edgeupdate.tv_usec = tv.tv_usec;
                }
                else {
                    show_vertex = find_node_by_xy(event.xbutton.x, event.xbutton.y);
                    if (show_vertex != -1)
                        print_rigidity(show_vertex);
                }
                break;
            case KeyPress:
                k = XLookupKeysym(&event.xkey, 0);
                if (k == XK_Escape)
                {
                    printf("quitting...\n");
                    done = True;
                }
                if (k == XK_F1)
                {
                    killGLWindow();
                    GLWin.fs = !GLWin.fs;
                    createGLWindow("Cricket Viewer", WIDTH, HEIGHT, 24, GLWin.fs);
                }
                if (k == XK_Left) {
                    origin_x -= 2;
                    auto_zoom = 0;
                    drawGLScene();
                }
                if (k == XK_Right) {
                    origin_x += 2;
                    auto_zoom = 0;
                    drawGLScene();
                }
                if (k == XK_Down) {
                    origin_y -= 2;
                    auto_zoom = 0;
                    drawGLScene();
                }
                if (k == XK_Up) {
                    origin_y += 2;
                    auto_zoom = 0;
                    drawGLScene();
                }
                if (k == XK_Page_Up) {
                    fov += 5;
                    auto_zoom = 0;
                    drawGLScene();
                }
                if (k == XK_Page_Down) {
                    fov -= 5;
                    auto_zoom = 0;
                    drawGLScene();
                }
                if (k == XK_i) {
                    trot += 0.5;
                    drawGLScene();
                }
                if (k == XK_o) {
                    trot -= 0.5;
                    drawGLScene();
                }
                if (k == XK_k) {
                    trot += 5;
                    drawGLScene();
                }
                if (k == XK_l) {
                    trot -= 5;
                    drawGLScene();
                }
                if (k == XK_comma) {
                    ui_rot += 5;
                    drawGLScene();
                }
                if (k == XK_period) {
                    ui_rot -= 5;
                    drawGLScene();
                }
                if (k == XK_f) {
                    ui_flip = -ui_flip;
                    drawGLScene();
                }
                if (k == XK_b) {
                //    target_fov = 1;
                    auto_zoom = 1;
                //    recompute_bounding(&target_ox, &target_oy, &target_fov);
                }
                if (k == XK_p) {
                    show_pings = (show_pings+1)%3;
                }
                if (k == XK_q) {
                    show_quads = !show_quads;
                }
                if (k == XK_h) {
                    show_all_history = !show_all_history;
                }
                if (k == XK_w) {
                    if (picking_edge)
                        pickin