cricketm.nc

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

                && msg->type == SYN1) {
            call UltrasoundControl.SendPulse();
            atomic beacon_pending = 0;
            call Leds.yellowOff();
        }
        /* Put a timestamp in outgoing packets, used for clock
         * synchronization. */
        else if (cnt == (offsetof(struct TOS_Msg,data))
                && msg->type == SYN1) {
            struct CricketBeacon * beac = (struct CricketBeacon *) msg->data;
            beac->sendtime = call GetClockLow();
        }
    }
        

    /* Callback for when a radio packet has been sent. */
    event result_t RadioSend.sendDone(TOS_MsgPtr rmsg, result_t success) {
        return SUCCESS;
    }

    uint8_t compensation = 0;
    uint16_t us_time = 0;
    uint16_t pulse_timestamp = 0;
    uint16_t last_time = 0;
    uint16_t last_timestamp = 0;
    uint8_t got_id = 0;
    uint8_t reported = 1;
    uint8_t detector_is_on = 0;
    norace uint8_t   beac_id[4];
    norace uint8_t   last_id[4];
    uint8_t last_flags;
    uint8_t pulse_flags;

    /* If a beacon sending is interrupted by an incoming packet, we
     * reschedule it a short time later using this task. */
    task void reschedule_beacon()
    {
        call BeaconTimer.start(TIMER_ONE_SHOT, MAX_US_TRAVEL_TIME);
    }

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

    /* Callback for each byte of an incoming radio packet as it is
     * still being received off the air. */
    async event void RadioReceiveCoord.byte(TOS_MsgPtr msg, uint8_t cnt)
    {
        /* If the packet is a beacon, start processing it once we
         * get to a certain offset. */
        if (cnt == (offsetof(struct TOS_Msg,type) +
                sizeof(((struct TOS_Msg*)0)->type) - 1)
                && msg->type == SYN1) {
            /* TODO: if a beacon is received immediately after our own or
               after receiving another, all distance measurements should
               be discarded for the next 45ms */

            /* Begin the ultrasound timer */
            call UltrasoundControl.StartDetector(MAX_ALLOWED_TIMER_VAL);
            atomic {
                /* If an outgoing beacon is already queued up, cancel it
                 * and postpone it until later. */
                if (beacon_pending) {
                    call CancelQueuedPacket();
                    call Leds.yellowOff();
                    beacon_pending = 0;
                    aborted_beacons++;
                    post reschedule_beacon();
                }

                /* update statistics */
                beacon_recv_count++;
                if (!clear_to_send)
                    out_of_order_beacons++;

                /* The channel is now not clear until the ultrasound
                 * pulse is dissipated. */
                clear_to_send = 0;

                /* update state */
                us_time = 0;
                got_id = 0;
                detector_is_on = 1;
                /* Since incoming radio bits are buffered in batches
                 * of 8, we need to know the offset of the first bit
                 * of the packet relative to buffer in order to compute
                 * the exact bit-level timing of the packet. */
                compensation = call GetRxBitOffset();
                /* Get the millisecond timestamp of the packet -- this
                 * is not very accurate, but useful finding corresponding
                 * beacons on other devices. */
                pulse_timestamp = call GetClockLow();
            }
            call Leds.greenOn();
        }
    }

    /* Task used to perform higher-level processing on an incoming beacon
     * after it has been received.  This task is dispatched by
     * try_post_report() once both the RF and Ultrasound portions of the
     * beacon have been received and timed. */
    task void ReportPulse()
    {
        uint32_t distance = 0;
        uint16_t timer, ts;
        uint8_t comp, flags;

        atomic {
            /* The ultrasound timer value (approx. microseconds) of the
             * beacon */
            timer = last_time;
            /* The millisecond timestamp of the beacon */
            ts = last_timestamp;
            /* The bit offset of the radio packet start relative to
             * the buffering performed by the CPU. */
            comp = compensation;
            /* Any relevant flags, such as if the node is moving */
            flags = last_flags;
        }

        if (timer == 0) {
            /* In this case, the Ultrasound pulse was not received,
             * so we discard the beacon. */
            UARTOutput(OUT_DEBUG, "beacon:%02d timer=0\n",
                    call BeaconsManage.FindId(last_id));
        }
        else if ((timer + COMPENSATION_VAL * comp) > TIMER_OFFSET) {
            /* The Ultrasound pulse was received in a time that
             * is physically possible. */

            /* Compute the distance in centimeters based on the timer,
             * the known hardware offset, and the variable radio bit
             * offset. */
            distance = (uint32_t) (timer + (COMPENSATION_VAL * 
                        comp) - TIMER_OFFSET) / DISTANCE_MULT;
            /* Record the measurement for later use in higher layers. */
            call BeaconsManage.NewMeasurement(last_id,
                   timer + COMPENSATION_VAL * comp - TIMER_OFFSET, ts, flags);
            UARTOutput(OUT_DEBUG,
                    "beacon:%02d,dist=%d,time=%d,clock=%u\n",
                    call BeaconsManage.FindId(last_id),
                    (uint16_t) distance,
                    (uint16_t) (timer + COMPENSATION_VAL * comp - TIMER_OFFSET),
                    ts);
        }
        else {
            /* The Ultrasound pulse was received in a time physically
             * impossible with the hardware. */
            UARTOutput(OUT_DEBUG, "beacon:%02d timer=%d\n",
                    call BeaconsManage.FindId(last_id), timer + COMPENSATION_VAL * comp);
        }

        atomic {
            reported = 1;
        }
    }

    /* Callback when the Ultrasound timer receives an input capture
     * event, indicating that an ultrasound pulse has been picked up
     * on the receiver. */
    async event result_t UltrasoundControl.PulseDetected(uint16_t timer)
    {
        atomic {
            /* If this is the first capture event for this pulse,
             * record the time. */
            if (us_time == 0) {
                call Leds.greenOff();
                us_time = timer;
            }
        }
        return SUCCESS;
    }

    /* This function is called to possibly report an incoming beacon to
     * the higher level code.  This function checks to make sure both
     * the Ultrasound timer is expired AND the full radio message has
     * arrived before dispatching the higher level. */
    void try_post_report() {
        uint8_t i;
        /* Check to make sure the full radio packet has been received
         * (when got_id=1), that the ultrasound timer has expired (when
         * detector_is_on=0), and that previous beacon has already been
         * handled (when reported=1). */
        if (got_id && !detector_is_on && reported) {
            /* Copy the values of the ultrasound timer, the timestamp,
             * the flags, and the ID of the node.  This allows new
             * incoming beacons to be handled even if the previous
             * beacon is not yet reported. */
            last_time = us_time;
            last_timestamp = pulse_timestamp;
            last_flags = pulse_flags;
            for (i=0; i<4; i++)
                last_id[i] = beac_id[i];
            /* post the handler task if possible */
            if (post ReportPulse()) {
                reported = 0;
            }
        }
        /* If the beacon could not be reported due to a previous
         * beacon still not being handled, the beacon is effectively
         * dropped.  We increment a statistic so we know about it. */
        else if (got_id && !detector_is_on) {
            missed_pings++;
        }
    }

    /* Callback for when the Ultrasound timer has expired.  This
     * always happens once for each time it is activated. */
    async event result_t UltrasoundControl.DetectorTimeout()
    {
        atomic {
            /* Indicate that the detector is no longer in use */
            detector_is_on = 0;
            if (us_time == 0) {
                call Leds.greenOff();
            }
            /* If possible, dispatch to higher level code. */
            try_post_report();
            /* The channel is now clear */
            clear_to_send = 1;
        }
        return SUCCESS;
    }
    
    /* Callback for a new packet received on the radio.  This is called
     * within a task once the entire packet is received. */
    event TOS_MsgPtr RadioReceive.receive(TOS_MsgPtr data) {
        uint8_t i;
        /* We discard the packet if its CRC check failed. */
        if (!data->crc) {
            bad_crcs++;
            return data;
        }
        /* TODO: If the CRC check fails, no beacons will be trusted
         * for a while and we will not be clear to send our own */

        /* Check if the packet is a beacon */
        if (data->type == SYN1) {
            struct CricketBeacon * beac =
                (struct CricketBeacon *)data->data;
            /* Test if there's a version mismatch with the other Cricket */
            if (beac->ver != RF_PROTOCOL_VER) {
                UARTOutput(OUT_WARNING, "Ignored packet, ver=%d (expect %d)\n",
                        beac->ver, RF_PROTOCOL_VER);
                return data;
            }
            /* Copy the ID of the other Cricket */
            for (i=0; i<4; i++)
                beac_id[i] = beac->id[i];
            /* If possible, report the packet to higher layers. */
            atomic {
                pulse_flags = beac->flags;
                got_id = 1;
                try_post_report();
            }
            /* Record any relayed information included in the packet */
            call BeaconsManage.NewRelay(beac->id, beac->relay,
                    data->length - sizeof(struct CricketBeacon),
                    data->time - beac->sendtime - 1);
            /* Print the received signal strength for debugging. */
            UARTOutput(OUT_DEBUG, "RSSI %d: 0x%x\n",
                    call BeaconsManage.FindId(beac_id),
                    data->strength);
        }
        return data;
    }

    /* Called once the ID of our Cricket has been read at startup time */
    event result_t HardwareId.readDone(uint8_t *id, result_t success) {
        if (success == SUCCESS) {
            int i;
            for (i=0; i < 4; i++)
                beacondata->id[i] = id[4-i];
        }
        call BeaconsManage.SetId(beacondata->id);
        call BeaconsManage.SetFlags(params.flags);
        return SUCCESS;
    }

    void update_params()
    {
        call BeaconsManage.SetFlags(params.flags);
        call Storage.WriteDataHeader(CR_PARAM_VER, (uint8_t *) &params,
                sizeof(params));
    }
    
    /* Callback for when a new string arrives on the Serial port.
     * This is called when the user sends ENTER. */
    event result_t Serial.Receive(char * buf, uint8_t len)
    {
        /* The possible command strings */
        static char __attribute__((progmem)) dbg_on[] = "DEBUG ON";
        static char __attribute__((progmem)) dbg_off[] = "DEBUG OFF";
        static char __attribute__((progmem)) cn_on[] = "CONES ON";
        static char __attribute__((progmem)) cn_off[] = "CONES OFF";
        static char __attribute__((progmem)) move_on[] = "MOVING ON";
        static char __attribute__((progmem)) move_off[] = "MOVING OFF";

        UARTOutput(OUT_INFO, "Received command: %s\n", buf);

#ifndef PLATFORM_PC
        /* Handle each type of command string */
        if (!strcmp_P(buf, dbg_on)) {
            debug_out = OUT_DEBUG;
            call BeaconsManage.ListNodes();
        }
        else if (!strcmp_P(buf, dbg_off)) {
            debug_out = OUT_INFO;
        }
        else if (!strcmp_P(buf, cn_on)) {
            has_cones = 1;
            params.flags |= CR_HAS_CONE;
            UARTOutput(OUT_INFO, "Using cones\n");
            update_params();
        }
        else if (!strcmp_P(buf, cn_off)) {
            has_cones = 0;
            params.flags &= ~CR_HAS_CONE;
            UARTOutput(OUT_INFO, "Not using cones\n");
            update_params();
        }
        else if (!strcmp_P(buf, move_on)) {
            params.flags |= CR_MOVING;
            UARTOutput(OUT_INFO, "Cricket is moving\n");
            update_params();
        }
        else if (!strcmp_P(buf, move_off)) {
            params.flags &= ~CR_MOVING;
            UARTOutput(OUT_INFO, "Cricket is not moving\n");
            update_params();
        }
#endif
        return SUCCESS;
    }
}