us.c

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

/*
 * us.c
 * David Moore <[e-mail]>
 *
 * Backend code for Ultrasound simulation with TOSSIM
 */

FILE * distfile = NULL;
norace int ** dist_mat = NULL;
long long nexttime = 0;
struct _cricket_id {
    uint8_t     id[4];
};
struct _cricket_id * cricket_id = NULL;

void us_use_file(char * name, int num_devices)
{
    int i;

    distfile = fopen(name, "r");
    if (!distfile)
        return;

    printf("%d devices\n", num_devices);
    dist_mat = (int **) malloc(num_devices * sizeof(int *));
    dist_mat[0] = (int *) malloc(num_devices * num_devices * sizeof(int));
    bzero(dist_mat[0], num_devices*num_devices*sizeof(int));
    for (i=1; i<num_devices; i++) {
        dist_mat[i] = dist_mat[0] + i*num_devices;
    }

    cricket_id = (struct _cricket_id *)
        malloc(num_devices * sizeof(struct _cricket_id));
    for (i=0; i<num_devices; i++) {
        int j;
        for (j=0; j<4; j++)
            cricket_id[i].id[j] = i;
    }
}

int cricket_get_id(uint8_t * dst, int node)
{
    int i;

    if (!cricket_id)
        return -1;

    for (i=0; i<4; i++) {
        dst[4-i] = cricket_id[node].id[i];
    }
    dst[0] = dst[5] = dst[6] = dst[7] = 0;
    return 0;
}

int us_parse_until(long long currtime)
{
    int sec, msec, i, j, d;
    char buf[256];

    if (!distfile || currtime < nexttime)
        return 0;

//    printf("systime is %lld\n", currtime);

    while (fgets(buf, sizeof(buf), distfile)) {
        int id[4];
        if (sscanf(buf, "time %d.%d", &sec, &msec) == 2) {
            nexttime = sec * 4000000 + msec * 4000;
            if (currtime < nexttime)
                return 0;
        }
        else if (sscanf(buf, "%d %d %d", &i, &j, &d) == 3) {
            dist_mat[i][j] = d;
//            fprintf(stderr, "dist[%d][%d] = %d\n", i, j, d);
        }
        else if (sscanf(buf, "node %d ID %x:%x:%x:%x", &i,
                    id, id+1, id+2, id+3) == 5) {
            cricket_id[i].id[0] = id[0];
            cricket_id[i].id[1] = id[1];
            cricket_id[i].id[2] = id[2];
            cricket_id[i].id[3] = id[3];
        }
    }
    fclose(distfile);
    distfile = NULL;
    return 0;
}

void us_close_file()
{
    if (distfile)
        fclose(distfile);

    if (dist_mat) {
        free(dist_mat[0]);
        free(dist_mat);
    }
    if (cricket_id) {
        free(cricket_id);
    }
}

/*******************************************************************************
 ******************* The Ultrasound implementation for Nido ********************
 *******************************************************************************/

#define US_TIMEOUT  0
#define US_PULSE    1

#define DEFAULT_PULSE   5000

norace static event_t* usTimeoutEvents[TOSNODES];

void TOSH_us_timeout();
void TOSH_us_pulse_detected(uint16_t);

void event_us_handle(event_t * event, struct TOS_state * state)
{
    us_data_t * us = (us_data_t *)event->data;
    switch (us->type) {
        case US_TIMEOUT:
            if (us->valid)
                TOSH_us_timeout();
            event->cleanup(event);
            usTimeoutEvents[NODE_NUM] = NULL;
            break;
        case US_PULSE:
            if (usTimeoutEvents[NODE_NUM] &&
                    ((us_data_t *)usTimeoutEvents[NODE_NUM]->data)->valid) {
                TOSH_us_pulse_detected(us->timer_val);
            }
            event->cleanup(event);
            break;
    }
}

void event_us_create(event_t * event, int node, int type, int interval)
{
    us_data_t * us = (us_data_t*) malloc(sizeof(us_data_t));
    us->valid = 1;
    us->type = type;
    us->timer_val = interval;

    event->data = us;
    event->mote = node;
    event->force = 0;
    event->pause = 0;
    event->time = tos_state.tos_time + interval;
    event->handle = event_us_handle;
    event->cleanup = event_total_cleanup;
}


void TOSH_us_start_detector(uint16_t timeout)
{
    event_t * event = NULL;
    if (usTimeoutEvents[NODE_NUM] == NULL) {
        event = (event_t *) malloc(sizeof(event_t));
        event_us_create(event, NODE_NUM, US_TIMEOUT, timeout);
        usTimeoutEvents[NODE_NUM] = event;
        TOS_queue_insert_event(event);
    }
    else {
        dbg(DBG_ERROR, "US detector started twice at node %d\n", NODE_NUM);
    }
}

void TOSH_us_stop_detector()
{
    if (usTimeoutEvents[NODE_NUM]) {
        ((us_data_t*)usTimeoutEvents[NODE_NUM]->data)->valid = 0;
    }
    else {
        dbg(DBG_ERROR, "US detector stopped without being started at node %d\n",
                NODE_NUM);
    }
}

void TOSH_us_send_pulse()
{
    int i;
    for (i=0; i < tos_state.num_nodes; i++) {
        event_t * event;
        if (i == NODE_NUM)
            continue;

        if (dist_mat && dist_mat[i][NODE_NUM]) {
            event = (event_t *) malloc(sizeof(event_t));
            event_us_create(event, i, US_PULSE, dist_mat[i][NODE_NUM]);
            TOS_queue_insert_event(event);
        }
        else if (!dist_mat) {
            event = (event_t *) malloc(sizeof(event_t));
            event_us_create(event, i, US_PULSE, DEFAULT_PULSE);
            TOS_queue_insert_event(event);
        }
    }
}