cricketm.nc

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

/* 
 * CricketM.nc
 * David Moore <[e-mail]>
 *
 * Top-level code for cricket application for robust distributed
 * localization.
 *
 * 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 CricketM {
  provides {
    interface StdControl;
  }
  uses {
    interface Timer as BeaconTimer;
    interface Timer as LocTimer;
    interface Leds;
    interface Random;
    interface StdControl as RadioControl;
    interface BareSendMsg as RadioSend;
    interface ReceiveMsg as RadioReceive;
    interface HardwareId;
    async command uint8_t GetRxBitOffset();
    async command result_t CancelQueuedPacket();
    interface UltrasoundControl;
    interface RadioCoordinator as RadioReceiveCoord;
    interface RadioCoordinator as RadioSendCoord;
    interface BeacManage as BeaconsManage;
    interface StdControl as BeaconsControl;
    interface Serial;
    interface LocalizeStatic as Localize;
    async command uint16_t GetClockLow();
    async command uint32_t GetClockSec();
    interface Storage;
  }
}

implementation {

    /* Format of the Cricket beacon RF packet */
#define RF_PROTOCOL_VER   4
    struct CricketBeacon {
        uint8_t     ver;        // the packet layout version
        uint8_t     id[4];      // the Cricket's unique ID
        uint16_t    sendtime;   // the packet timestamp of the sender
        uint8_t     flags;      // any useful bits such as motion
        uint8_t     relay[0];   // any relay data
    };

    /* Types of beacon messages */
#define SYN1 0x0f
#define SYN2 0x0e

    /* message structure used for sending beacons */
    TOS_Msg   beaconmsg;
    struct CricketBeacon * beacondata = (struct CricketBeacon *)beaconmsg.data;

    /* clear_to_send set to one when ultrasound channel is clear for
     * sending a pulse. */
    uint8_t   clear_to_send = 1;

    /* temporary buffer for holding our cricket ID */
    uint8_t   id_buf[8];

    /* structure for holding saved constants */
#define CR_PARAM_VER  1
    struct CricketParameters {
        uint8_t     flags;
    };
    struct CricketParameters params;

    task void ReadParams();

   /**
    * Initialize the hardware.  Called at startup.
    * 
    * @return Always returns SUCCESS
    **/
    command result_t StdControl.init() {
#ifndef PLATFORM_PC
        /* Enable RS-232 Chip */
#ifdef PLATFORM_CRICKET_MIT
        TOSH_SET_RS_232_SHDN_PIN();
#else
        TOSH_CLR_US_IN_EN_PIN();
        TOSH_SET_BAT_MON_PIN();
#endif
        /* Wait until the RS-232 start */
        TOSH_uwait(2000);
#endif

        /* First task will read saved parameters from flash */
        post ReadParams();
        /* Read the ID of the cricket */
        call HardwareId.read(id_buf);
        /* Init radio */
        call RadioControl.init();
        /* Init Leds */
        call Leds.init();
        /* Init beacon database */
        call BeaconsControl.init();
        /* Set stdout to the UART. */
        call Serial.SetStdoutSerial();

#ifndef PLATFORM_PC
        /* Print the reset status */
        UARTOutput(OUT_INFO, "\nReset: 0x%02x\n", inb(MCUCSR));
        /* Very important: setting the high bit of MCUCSR twice in
         * a row disables the JTAG interface.  This is necessary since
         * RF RSSI is hooked up to the ADC7 pin.  This value will be
         * corrupted if the JTAG interface is enabled. */
        outb(MCUCSR, 0x80);
        outb(MCUCSR, 0x80);
#endif

        /* Initialize constant flags of the beacon message */
        beaconmsg.length = 4;
        beaconmsg.type = SYN1;
        beaconmsg.addr = TOS_BCAST_ADDR;
        beacondata->ver = RF_PROTOCOL_VER;

        return SUCCESS;
    }


   /**
    * Start things up.  This happens at startup after init.
    * 
    * @return Always returns SUCCESS
    **/
    command result_t StdControl.start() {
        /* Initialize random number generator */
        call Random.init();
        /* Send a beacon shortly; after the timer expires */
        call BeaconTimer.start(TIMER_ONE_SHOT, MAX_US_TRAVEL_TIME);
        /* Set a recurring timer to perform localization once per second */
        call LocTimer.start(TIMER_REPEAT, 1024);
        /* Start the radio */
        call RadioControl.start();
        /* Start the beacon database */
        call BeaconsControl.start();
        return SUCCESS;
    }

    /* Reads saved cricket parameters from the flash memory */
    task void ReadParams() {
        uint8_t version;

        params.flags = 0;
        if (call Storage.ReadDataHeader(&version, (uint8_t *) &params,
                    sizeof(params))) {
            if (version != CR_PARAM_VER) {
                UARTOutput(OUT_ERROR, "Error: version mismatch in stored parameters, ignoring them\n");
                params.flags = 0;
            }
            else {
                UARTOutput(OUT_INFO, "Read stored parameters\n");
            }
        }
        if (params.flags & CR_MOVING)
            UARTOutput(OUT_INFO, "Cricket is moving\n");
        if (params.flags & CR_HAS_CONE)
            UARTOutput(OUT_INFO, "Cricket has cone\n");
    }

   /**
    * Halt execution of the application.
    * 
    * @return Always returns SUCCESS
    **/
    command result_t StdControl.stop() {
        call BeaconsControl.stop();
        call BeaconTimer.stop();
        call RadioControl.stop();
        return SUCCESS;
    }

    /* Set to one when a beacon has been pushed to the MAC layer, but
     * not yet sent. */
    uint8_t beacon_pending = 0;

    /* Transmit statistics */
    uint32_t beacon_send_count = 0;
    uint32_t aborted_beacons = 0;
    uint32_t beacon_1_retry = 0;
    uint32_t beacon_2_retry = 0;
    uint32_t beacon_3p_retry = 0;
    uint8_t num_retries = 0;

    /* Receive statistics */
    uint32_t beacon_recv_count = 0;
    uint32_t out_of_order_beacons = 0;
    uint32_t bad_crcs = 0;
    uint32_t missed_pings = 0;

    /* Prints network statistics */
    void print_network_stats()
    {
        uint32_t ab, recvs, OoO, mp, uptime;
        atomic {
            ab = aborted_beacons;
            recvs = beacon_recv_count;
            OoO = out_of_order_beacons;
            mp = missed_pings;
        }

        /* Read the clock and print the uptime */
        uptime = call GetClockSec();
        UARTOutput(OUT_DEBUG, "uptime %u:%02u:%02u\n",
                (uint16_t)(uptime / 3600),
                (uint16_t)((uptime / 60) % 60),
                (uint16_t)(uptime % 60));

        /* Network statistics */
        UARTOutput(OUT_DEBUG, "TX:%lu aborted:%lu 1retry:%lu 2retry:%lu 3+retry:%lu\n", beacon_send_count, ab, beacon_1_retry, beacon_2_retry, beacon_3p_retry);
        UARTOutput(OUT_DEBUG, "RX:%lu out_of_order:%lu bad_CRC:%lu missed:%lu\n",
                recvs, OoO, bad_crcs, mp);
    }

    uint8_t loc_in_progress = 0;
    
    /* Task for pending localization operations. */
    task void DoLocalization()
    {
        loc_in_progress = 1;
        /* First, localize any moving nodes that are due. */
        call BeaconsManage.LocalizeMovingNodes();
        /* Set the time of the next localization operation */
        call BeaconsManage.SetLocTime(call GetClockLow());
        /* Do static localization */
        call Localize.LocalizeStaticNodes();
        /* Do any mobile localizations that are due */
        call BeaconsManage.LocalizeMovingNodes();
        loc_in_progress = 0;
    }
    
    event int Localize.GetDistances(uint8_t o[MAX_NEIGHBORS+1],
            struct NodeInfo ** nodeinfo,
            uint16_t dist[MAX_NEIGHBORS+1][MAX_NEIGHBORS+1])
    {
        return call BeaconsManage.RefineDistances(o, nodeinfo, dist);
    }

    /* Timer event fired once a second for localization */
    event result_t LocTimer.fired()
    {
        if (debug_out >= OUT_DEBUG) {
            print_network_stats();

            /* Print the contents of our distance cache. */
            call BeaconsManage.DebugPrint(NULL);
        }

        /* Do the localization */
        if (!loc_in_progress)
            post DoLocalization();
        else
            UARTOutput(OUT_ERROR, "Error: skipped localization\n");

        return SUCCESS;
    }

    /* Timer event fired whenever a beacon is due to be sent */
    event result_t BeaconTimer.fired()
    {
        uint8_t cts;
        uint16_t desync_delay;

        atomic cts = clear_to_send;

        /* If the Ultrasound channel is not clear, perform a random
         * backoff. */
        if (!cts) {
            desync_delay = call Random.rand();
            desync_delay = desync_delay % 3;
            call BeaconTimer.start(TIMER_ONE_SHOT,
                    MAX_US_TRAVEL_TIME + desync_delay);
            num_retries++;
            return SUCCESS;
        }

        /* We're good to go, proceed with the beacon. */

        call Leds.yellowOn();
        atomic beacon_pending = 1;
        
        /* Fill the beacon packet with any distances that we've measured
         * recently and send the packet. */
        beaconmsg.length = sizeof(struct CricketBeacon) +
            call BeaconsManage.GetMeasurements(beacondata->relay,
                    TOSH_DATA_LENGTH - sizeof(struct CricketBeacon));
        beacondata->flags = params.flags;
        call RadioSend.send(&beaconmsg);

        /* Statistics maintenence */
        if (num_retries == 1)
            beacon_1_retry++;
        else if (num_retries == 2)
            beacon_2_retry++;
        else if (num_retries >= 3)
            beacon_3p_retry++;

        /* Set the timer for our next beacon */
        desync_delay = call Random.rand();
        desync_delay = desync_delay % (MAX_BEAC_IVAL - MIN_BEAC_IVAL);
        call BeaconTimer.start(TIMER_ONE_SHOT, MIN_BEAC_IVAL + desync_delay);
        num_retries = 0;
        beacon_send_count++;

        return SUCCESS;
    }

    async event void RadioSendCoord.startSymbol(uint8_t bitsPerBlock, uint8_t offset, TOS_MsgPtr msgBuff) {}
    async event void RadioSendCoord.blockTimer() {}

    /* Callback for each byte of an outgoing radio packet as it is put
     * on the air. */
    async event void RadioSendCoord.byte(TOS_MsgPtr msg, uint8_t cnt)
    {
        /* Once we get to the right offset in the packet, send the
         * Ultrasound pulse that goes along with the beacon. */
        if (cnt == (offsetof(struct TOS_Msg,data) + 1)