tos.py

tinyos2.0版本驱动

# Copyright (c) 2008 Johns Hopkins University.
# All rights reserved.
#
# Permission to use, copy, modify, and distribute this software and its
# documentation for any purpose, without fee, and without written
# agreement is hereby granted, provided that the above copyright
# notice, the (updated) modification history and the author appear in
# all copies of this source code.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS `AS IS'
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS
# BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, LOSS OF USE, DATA,
# OR PROFITS) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
# THE POSSIBILITY OF SUCH DAMAGE.

# @author Razvan Musaloiu-E. <[e-mail]>

"""A library that implements the T2 serial communication.

This library has two parts: one that deals with sending and receiving
packets using the serial format from T2 (TEP113) and a second one that
tries to simplifies the work with arbitrary packets.

"""

import sys, struct, time, serial, socket, operator, thread
from Queue import Queue
from threading import Lock, Condition

__version__ = "$Id: tos.py,v 1.1 2008/05/19 21:25:08 razvanm Exp $"

__all__ = ['Serial', 'AM',
           'Packet', 'RawPacket',
           'AckFrame', 'DataFrame', 'NoAckDataFrame',
           'ActiveMessage']

def list2hex(v):
    return " ".join(["%02x" % p for p in v])

class Serial:
    """
    A Serial object offers a way to send and data using a HDLC-like
    formating.
    """
    
    HDLC_FLAG_BYTE = 0x7e
    HDLC_CTLESC_BYTE = 0x7d
    
    TOS_SERIAL_ACTIVE_MESSAGE_ID = 0
    TOS_SERIAL_CC1000_ID = 1
    TOS_SERIAL_802_15_4_ID = 2
    TOS_SERIAL_UNKNOWN_ID = 255
    
    SERIAL_PROTO_ACK = 67
    SERIAL_PROTO_PACKET_ACK = 68
    SERIAL_PROTO_PACKET_NOACK = 69
    SERIAL_PROTO_PACKET_UNKNOWN = 255
  
    def __init__(self, port, baudrate, flush=False, debug=False, qsize=10):
        self._debug = debug
        self._in_queue = Queue(qsize)
        self._out_lock = Lock()
        self._out_ack = Condition()
        self._seqno = 0
        self._ack = None
        self._write_counter = 0
        self._write_counter_failures = 0
        self._read_counter = 0
        self._ts = None

        self._s = serial.Serial(port, baudrate, rtscts=0, timeout=0.5)
        self._s.flushInput()
        start = time.time();
        if flush:
            print >>sys.stdout, "Flushing the serial port",
            while time.time() - start < 1:
                p = self._read()
                sys.stdout.write(".")
            if not self._debug:
                sys.stdout.write("\n")
        self._s.close()
        self._s = serial.Serial(port, baudrate, rtscts=0, timeout=None)

        thread.start_new_thread(self.run, ())

    def run(self):
        
        while True:
            p = self._read()
            self._read_counter += 1
            if self._debug:
                print "Serial:run: got a packet(%d): %s" % (self._read_counter, p)
            ack = AckFrame(p.data)
            if ack.protocol == self.SERIAL_PROTO_ACK:
                if not self._ack:
                    self._ack = ack
                if self._debug:
                    print "Serial:run: got an ack:", ack
                self._ack = ack
                # Wake up the writer
                self._out_ack.acquire()
                self._out_ack.notify()
                self._out_ack.release()
            else:
                ampkt = ActiveMessage(NoAckDataFrame(p.data).data)
                if ampkt.type == 100:
                    for t in "".join([chr(i) for i in ampkt.data]).strip('\n\0').split('\n'):
                        print "PRINTF:", t.strip('\n')
                else:
                    if self._in_queue.full():
                        print "Warning: Buffer overflow"
                        self._in_queue.get()
                    self._in_queue.put(p, block=False)


    # Returns the next incoming serial packet
    def _read(self):
        """Wait for a packet and return it as a RawPacket."""
        
        try:
            d = self._get_byte()
            ts = time.time()
            while d != self.HDLC_FLAG_BYTE:
                d = self._get_byte()
                ts = time.time()
            packet = [d]
            d = self._get_byte()
            if d == self.HDLC_FLAG_BYTE:
                d = self._get_byte()
                ts = time.time()
            else:
                packet.append(d)
            while d != self.HDLC_FLAG_BYTE:
                d = self._get_byte()
                packet.append(d)
            if self._debug == True:
                print "Serial:_read: unescaped", packet
            packet = self._unescape(packet)
            
            crc = self._crc16(0, packet[1:-3])
            packet_crc = self._decode(packet[-3:-1])
            
            if crc != packet_crc:
                print "Warning: wrong CRC! %x != %x %s" % (crc, packet_crc, ["%2x" % i for i in packet])
            if self._debug:
                if self._ts == None:
                    self._ts = ts
                else:
                    print "Serial:_read: %.4f (%.4f) Recv:" % (ts, ts - self._ts), self._format_packet(packet[1:-3])
                self._ts = ts
            return RawPacket(ts, packet[1:-3], crc == packet_crc)
        except socket.timeout:
            return None


    def read(self, timeout=0):
        start = time.time();
        done = False
        while not done:
            p = None
            while p == None:
                if timeout == 0 or time.time() - start < timeout:
                    p = self._in_queue.get()
                else:
                    return None
            if p.crc:
                done = True
            else:
                p = None
        # In the current TinyOS the packets from the mote are always NoAckDataFrame
        return NoAckDataFrame(p.data)

    def write(self, payload):
        """
        Write a packet. If the payload argument is a list, it is
        assumed to be exactly the payload. Otherwise the payload is
        assume to be a Packet and the real payload is obtain by
        calling the .payload().
        """
        
        if type(payload) != type([]):
            # Assume this will be derived from Packet
            payload = payload.payload()
        self._out_lock.acquire()
        self._seqno = (self._seqno + 1) % 100
        packet = DataFrame();
        packet.protocol = self.SERIAL_PROTO_PACKET_ACK
        packet.seqno = self._seqno
        packet.dispatch = 0
        packet.data = payload
        packet = packet.payload()
        crc = self._crc16(0, packet)
        packet.append(crc & 0xff)
        packet.append((crc >> 8) & 0xff)
        packet = [self.HDLC_FLAG_BYTE] + self._escape(packet) + [self.HDLC_FLAG_BYTE]

        while True:
            self._put_bytes(packet)
            self._write_counter += 1
            if self._debug == True:
                print "Send(%d/%d): %s" % (self._write_counter, self._write_counter_failures, packet)
                print "Wait for ack %d ..." % (self._seqno)
            self._out_ack.acquire()
            self._out_ack.wait(0.2)
            if self._debug:
                print "Wait for ack %d done. Latest ack:" % (self._seqno), self._ack
            self._out_ack.release()
            if self._ack and self._ack.seqno == self._seqno:
                if self._debug:
                    print "The packet was acked."
                self._out_lock.release()
                if self._debug:
                    print "Returning from Serial.write..."
                return True
            else:
                self._write_counter_failures += 1
                if self._debug:
                    print "The packet was not acked. Try again."
            # break # make only one sending attempt
        self._out_lock.release()
        return False


    def _format_packet(self, payload):
        f = NoAckDataFrame(payload)
        if f.protocol == self.SERIAL_PROTO_ACK:
            rpacket = AckFrame(payload)
            return "Ack seqno: %d" % (rpacket.seqno)
        else:
            rpacket = ActiveMessage(f.data)
            return "D: %04x S: %04x L: %02x G: %02x T: %02x | %s" % \
                   (rpacket.destination, rpacket.source,
                    rpacket.length, rpacket.group, rpacket.type,
                    list2hex(rpacket.data))

    def _crc16(self, base_crc, frame_data):
        crc = base_crc
        for b in frame_data:
            crc = crc ^ (b << 8)
            for i in range(0, 8):
                if crc & 0x8000 == 0x8000:
                    crc = (crc << 1) ^ 0x1021
                else:
                    crc = crc << 1
                crc = crc & 0xffff
        return crc
    
    def _encode(self, val, dim):
        output = []
        for i in range(dim):
            output.append(val & 0xFF)
            val = val >> 8
        return output
    
    def _decode(self, v):
        r = long(0)
        for i in v[::-1]:
            r = (r << 8) + i
        return r
    
    def _get_byte(self):
        try:
            r = struct.unpack("B", self._s.read())[0]
            return r
        except struct.error:
            # Serial port read timeout
            raise socket.timeout
    
    def _put_bytes(self, data):
        #print "DEBUG: _put_bytes:", data
        for b in data:
            self._s.write(struct.pack('B', b))
    
    def _unescape(self, packet):
        r = []
        esc = False
        for b in packet:
            if esc:
                r.append(b ^ 0x20)
                esc = False
            elif b == self.HDLC_CTLESC_BYTE:
                esc = True
            else: