beacmanagem.nc

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

/*
 * BeacManageM.nc
 * David Moore <[e-mail]>
 *
 * Module for handling distance measurements to neighboring beacons.  Keeps
 * track of recent measurements, and purges outdated ones.  Also allows
 * measurements to be relayed to other nodes and handles received relays.
 * Kalman filtering is performed on new measurements to reduce noise
 * and outliers.
 *
 * Provides the ability to dump these measurements to the serial port.
 * Although no specific query functions are implemented, one can write them
 * as necessary for the specific application.
 *
 * TODO: support more than one mobile node at a time.
 * 
 *
 * Copyright (C) 2004  Massachusetts Institute of Technology
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * as published by the Free Software Foundation; either version 2
 * of the License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
 */

module BeacManageM {
    provides {
        interface StdControl;
        interface BeacManage;
    }
    uses {
        interface Timer as AgeTimer;
        interface LocalizeMoving as Localize;
        async command uint16_t GetClockLow();
    }
}

/* The time in seconds between each purging of outdated measurements. */
#define MEAS_EXPIRE_SECONDS 10

/* Number of milliseconds between a beacon from a mobile node and the
 * time its localization is computed. */
#define MOBILE_LOCALIZATION_DELAY   1500

/* Number of measurements recorded before the Kalman filter is started. */
#define NUM_INIT_MEAS   5

/* Special value of distance we use to keep track of outliers so we
 * can debug their effect on the system. */
#define OUTLIER_DIST    1

/* Macros for getting and setting the recorded distance. */
#define SET_DIST(a,b,x)     dists[a][b] = (x)
#define GET_DIST(a,b)       dists[a][b]

/* Clamps a value between two extremum */
#define CLAMP(x,a,b)        (((x)<(a))?(a):(((x)>(b))?(b):(x)))

/* The maximum number of previous beacons from mobile nodes that can
 * be kept in memory at once.  This should provide enough time for
 * a beacon to be remembered as it is recorded and then relayed for
 * processing at a later time. */
#define MAX_TRACK       4

implementation {
    /* neighbors[] is a record of each node that has distance information. */
    struct NodeInfo neighbors[MAX_NEIGHBORS+1];
    /* dists[][] records pairwise distances between every pair of
     * neighbors. */
    int16_t dists[MAX_NEIGHBORS+1][MAX_NEIGHBORS+1];

    /* Kalman filter state for each distance measured to a neighbor. */
    typedef struct NodeFiltInfo {
        union {
            uint16_t meas_time; // timestamp of last measurement
            uint16_t n;         // number of initial measurements
        };
        uint8_t bad_count;      // number of consecutive outliers
        int16_t vel;            // rate of change of distance
        union { // used either for covariance or filter initialization
            float P[4];         // current covariance matrix
            uint16_t d[8];      // initial measurements
        };
    } node_filt_info;
    node_filt_info filt[MAX_NEIGHBORS];

    /* State for each beacon from a mobile node.  This array is
     * used as a ring-buffer (or queue) */
    typedef struct MovingTracker {
        uint16_t    time;       // timestamp of the beacon
        uint8_t     node;       // node which sent beacon
        uint16_t    dist[MAX_NEIGHBORS+1];  // distance recorded by other
                                            // neighbors
    } moving_tracker;
    moving_tracker track[MAX_TRACK];
    uint8_t track_start = 0;    // head of the queue
    uint8_t track_num = 0;      // length of the queue
    uint16_t tracked_latest;    // timestamp of last tracked beacon
    
    /* a simple hash table so we don't have to loop through a long list
     * comparing IDs */
    uint8_t id_hash[127];

    /* time at which we will perform next localization */
    uint16_t next_loc_time;

    command result_t StdControl.init() {
        uint8_t i;

        /* Initialize the list of nodes and the hash table */
        for (i=0; i<=MAX_NEIGHBORS; i++)
            neighbors[i].status = EMPTY;
        for (i=0; i<0x7f; i++)
            id_hash[i] = EMPTY;

        return SUCCESS;
    }

    command result_t StdControl.start() {
        /* Start a timer for purging stale distances. */
        call AgeTimer.start(TIMER_REPEAT, 1024*MEAS_EXPIRE_SECONDS);
        tracked_latest = call GetClockLow();
        return SUCCESS;
    }

    command result_t StdControl.stop() {
        call AgeTimer.stop();
        return SUCCESS;
    }

    /* Copies an ID */
    void copy_id(uint8_t * dst, uint8_t * src)
    {
        uint8_t i;

        for (i=0; i<4; i++)
            dst[i] = src[i];
    }

    /* Returns 1 if two IDs match */
    uint8_t match_id(uint8_t * id1, uint8_t * id2)
    {
        uint8_t i;

        for (i=0; i<4; i++)
            if (id1[i] != id2[i])
                return 0;

        return 1;
    }

    /* Deletes a node from our cache.  Each node has a 4-byte ID (from
     * hardware) and an 8-bit number, assigned at runtime, which is its
     * index into neighbors[].  We use the 8-bit number for most
     * operations since it's short and easy to deal with.  The node
     * argument to node_delete() is this 8-bit index. */
    void node_delete(uint8_t node)
    {
        uint8_t nextn = neighbors[node].next;
        uint8_t id = neighbors[node].id[3];

        /* Empty the entry in neighbors[] */
        neighbors[node].status = EMPTY;

        /* Remove the node from the hash table */
        if (id_hash[id&0x7f] == node) {
            id_hash[id&0x7f] = nextn;
            return;
        }

        id = id_hash[id&0x7f];
        while (neighbors[id].next != node)
            id = neighbors[id].next;

        neighbors[id].next = nextn;
    }
    
    /* Finds a node in the list of known nodes given its 4-byte ID. */
    uint8_t node_find(uint8_t * id) {
        uint8_t node = id_hash[id[3]&0x7f];

        while (node != EMPTY) {
            if (match_id(id, neighbors[node].id))
                return node;
            node = neighbors[node].next;
        }
        return EMPTY;
    }

    /* External command for converting ID to index. */
    command uint8_t BeacManage.FindId(uint8_t * id)
    {
        return node_find(id);
    }

    /* Print useful information about a node. */
    void print_node_details(uint8_t slot)
    {
        if (slot == SELF)
            UARTOutput(OUT_INFO, "node %2d (self) ID %02x:%02x:%02x:%02x",
                    slot, neighbors[slot].id[0], neighbors[slot].id[1],
                    neighbors[slot].id[2], neighbors[slot].id[3]);
        else
            UARTOutput(OUT_INFO, "node %2d ID %02x:%02x:%02x:%02x",
                    slot, neighbors[slot].id[0], neighbors[slot].id[1],
                    neighbors[slot].id[2], neighbors[slot].id[3]);

        if (neighbors[slot].status & IS_MOVING) {
            UARTOutput(OUT_INFO, " moving");
        }
        UARTOutput(OUT_INFO, "\n");
    }

    /* Print useful information about all nodes we are aware of. */
    command result_t BeacManage.ListNodes()
    {
        uint8_t i;

        for (i=0; i<=MAX_NEIGHBORS; i++) {
            if (neighbors[i].status == EMPTY)
                continue;

            print_node_details(i);
        }
        return SUCCESS;
    }

    /* Helper function to node_add().  Stores a new node in a specified
     * slot of the array. */
    void node_add_slot(uint8_t * id, uint8_t slot)
    {
        uint8_t * ptr;

        neighbors[slot].status = 0;
        neighbors[slot].next = EMPTY;
        SET_DIST(SELF,slot,0);
        copy_id(neighbors[slot].id, id);

        ptr = &(id_hash[id[3]&0x7f]);
        while (*ptr != EMPTY) {
            ptr = &(neighbors[*ptr].next);
        }

        *ptr = slot;

        print_node_details(slot);
    }

    /* Adds a new node to the array of known neighbors.  Returns its index
     * in the array. */
    uint8_t node_add(uint8_t * id) {
        uint8_t i, node=0;

        for (i=0; i<MAX_NEIGHBORS; i++) {
            if (neighbors[i].status == EMPTY) {
                node = i;
                break;
            }
        }
        if (i == MAX_NEIGHBORS)
            return EMPTY;

        node_add_slot(id, node);
        return node;
    }

    /* Purges old entries from the cache */
    event result_t AgeTimer.fired() {
        uint8_t i;

        UARTOutput(OUT_DEBUG, "Cleaning beacon cache\n");
        for (i=0; i<MAX_NEIGHBORS; i++) {
            if (neighbors[i].status == EMPTY)
                continue;

            /* If the measurement hasn't been updated since our last timer
             * firing, delete the node. */
            if (neighbors[i].status & OLD_MEAS) {
                node_delete(i);
                continue;
            }

            /* If the relay data hasn't been updated since our last timer
             * firing, mark the data as not present. */
            if (neighbors[i].status & OLD_RELAY)
                neighbors[i].status &= ~RELAY_DATA;

            /* Set the aging flags. */
            neighbors[i].status |= OLD_MEAS;
            neighbors[i].status |= OLD_RELAY;
        }
        return SUCCESS;
    }

    /* Specifies our local ID.  This is necessary so we recognize distances to
     * ourselves measured by other nodes. */
    command result_t BeacManage.SetId(uint8_t * id)
    {
        node_add_slot(id, SELF);
        neighbors[SELF].status |= RELAY_DATA;
        return SUCCESS;
    }

    /* Stores flags about ourselves, such as if we are moving or not. */
    command result_t BeacManage.SetFlags(uint8_t flags)
    {
        if (flags & CR_MOVING)
            neighbors[SELF].status |= IS_MOVING;
        else
            neighbors[SELF].status &= ~IS_MOVING;
        return SUCCESS;
    }