Ejemplo n.º 1
0
class SerialMessagesInterface(threading.Thread):
    def __init__(self, callback, verbose=False, device='/dev/ttyUSB0', baudrate=115200,
                 msg_class='telemetry', interface_id=0):
        threading.Thread.__init__(self)
        self.callback = callback
        self.verbose = verbose
        self.msg_class = msg_class
        self.id = interface_id
        self.running = True
        try:
            self.ser = serial.Serial(device, baudrate, timeout=1.0)
        except serial.SerialException:
            print("Error: unable to open serial port '%s'" % device)
            exit(0)
        self.trans = PprzTransport(msg_class)

    def stop(self):
        print("End thread and close serial link")
        self.running = False
        self.ser.close()

    def shutdown(self):
        self.stop()

    def __del__(self):
        try:
            self.ser.close()
        except:
            pass

    def send(self, msg, sender_id,receiver_id = 0, component_id = 0):
        """ Send a message over a serial link"""
        if isinstance(msg, PprzMessage):
            data = self.trans.pack_pprz_msg(sender_id, msg, receiver_id, component_id)
            self.ser.write(data)
            self.ser.flush()

    def run(self):
        """Thread running function"""
        try:
            while self.running:
                # Parse incoming data
                c = self.ser.read(1)
                if len(c) == 1:
                    if self.trans.parse_byte(c):
                        (sender_id, receiver_id, component_id, msg) = self.trans.unpack()
                        if self.verbose:
                            print("New incoming message '%s' from %i (%i) to %i" % (msg.name, sender_id, component_id, receiver_id))
                        # Callback function on new message
                        if self.id == receiver_id:
                            self.callback(sender_id, msg)

        except StopIteration:
            pass
Ejemplo n.º 2
0
class SerialMessagesInterface(threading.Thread):
    def __init__(self, callback, verbose=False, device='/dev/ttyUSB0', baudrate=115200,
                 msg_class='telemetry'):
        threading.Thread.__init__(self)
        self.callback = callback
        self.verbose = verbose
        self.msg_class = msg_class
        self.running = True
        try:
            self.ser = serial.Serial(device, baudrate, timeout=1.0)
        except serial.SerialException:
            print("Error: unable to open serial port '%s'" % device)
            exit(0)
        self.trans = PprzTransport(msg_class)

    def stop(self):
        print("End thread and close serial link")
        self.running = False
        self.ser.close()

    def shutdown(self):
        self.stop()

    def __del__(self):
        try:
            self.ser.close()
        except:
            pass

    def send(self, msg, sender_id):
        """ Send a message over a serial link"""
        if isinstance(msg, PprzMessage):
            data = self.trans.pack_pprz_msg(sender_id, msg)
            self.ser.write(data)
            self.ser.flush()

    def run(self):
        """Thread running function"""
        try:
            while self.running:
                # Parse incoming data
                c = self.ser.read(1)
                if len(c) == 1:
                    if self.trans.parse_byte(c):
                        (sender_id, msg) = self.trans.unpack()
                        if self.verbose:
                            print("New incoming message '%s' from %i" % (msg.name, sender_id))
                        # Callback function on new message 
                        self.callback(sender_id, msg)

        except StopIteration:
            pass
Ejemplo n.º 3
0
class UdpMessagesInterface(threading.Thread):
    def __init__(self,
                 callback,
                 verbose=False,
                 uplink_port=UPLINK_PORT,
                 downlink_port=DOWNLINK_PORT,
                 msg_class='telemetry',
                 interface_id=0):
        threading.Thread.__init__(self)
        self.callback = callback
        self.verbose = verbose
        self.msg_class = msg_class
        self.uplink_port = uplink_port
        self.downlink_port = downlink_port
        self.ac_downlink_status = {}
        self.id = interface_id  # set to None to disable id filtering
        self.running = True
        try:
            self.server = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
            self.server.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
            self.server.settimeout(2.0)
            self.server.bind(('0.0.0.0', self.downlink_port))
        except OSError:
            logger.error(
                "Error: unable to open socket on ports '%d' (up) and '%d' (down)"
                % (self.uplink_port, self.downlink_port))
            exit(0)
        self.trans = PprzTransport(msg_class)

    def stop(self):
        logger.info("End thread and close UDP link")
        self.running = False
        self.server.close()

    def shutdown(self):
        self.stop()

    def __del__(self):
        try:
            self.server.close()
        except:
            pass

    def send(self, msg, sender_id, address, receiver=0, component=0):
        """ Send a message over a UDP link"""
        #TODO use sender_id from constructor
        if isinstance(msg, PprzMessage):
            data = self.trans.pack_pprz_msg(sender_id, msg, receiver,
                                            component)
            try:
                self.server.sendto(data, (address, self.uplink_port))
            except:
                pass  # TODO better error handling

    def run(self):
        """Thread running function"""
        try:
            while self.running:
                # Parse incoming data
                try:
                    (msg, address) = self.server.recvfrom(2048)
                    length = len(msg)
                    for c in msg:
                        if self.trans.parse_byte(c):
                            try:
                                (sender_id, receiver_id, component_id,
                                 msg) = self.trans.unpack()
                            except ValueError as e:
                                logger.warning("Ignoring unknown message, %s" %
                                               e)
                            else:
                                if self.verbose:
                                    logger.info(
                                        "New incoming message '%s' from %i (%i, %s) to %i"
                                        % (msg.name, sender_id, component_id,
                                           address, receiver_id))
                                # Callback function on new message
                                if self.id is None or self.id == receiver_id:
                                    self.callback(sender_id, address, msg,
                                                  length, receiver_id,
                                                  component_id)
                except socket.timeout:
                    pass

        except StopIteration:
            pass
Ejemplo n.º 4
0
class RadioBridge:
    def __init__(self, link_uri, msg_class='telemetry', verbose=False):
        """ Initialize and run with the specified link_uri """
        self.verbose = verbose

        # Ivy interface and stream parser
        self._ivy = IvyMessagesInterface("cf2ivy")
        self._transport = PprzTransport(msg_class)

        # Create a Crazyradio
        self._driver = RadioDriver()
        self._driver.connect(link_uri, self._link_quality_cb,
                             self._link_error_cb)

        if self.verbose:
            print('Connecting to %s' % link_uri)

        # Variable used to keep main loop occupied until disconnect
        self.is_connected = True

        # Bind to all messages from ac_id
        def _forward_to_cf(ac_id, msg):
            try:
                data = self._transport.pack_pprz_msg(0,
                                                     msg)  # sender_id 0 = GCS
                for i in range(0, len(data), 30):
                    pk = CRTPPacket()
                    pk.port = CRTP_PORT_PPRZLINK
                    pk.data = data[i:(i + 30)]
                    self._driver.send_packet(pk)
                if self.verbose:
                    print('Forward message', msg.name)
            except:
                if self.verbose:
                    print('Forward error for', ac_id)

        messages_datalink = messages_xml_map.get_msgs("datalink")
        for msg in messages_datalink:
            self._ivy.subscribe(_forward_to_cf, PprzMessage("datalink", msg))

    def shutdown(self):
        if self.verbose:
            print('closing cf2ivy interfaces')
        self._ivy.shutdown()
        self._driver.close()

    def run(self):
        pk = self._driver.receive_packet(
            0.1)  # wait for next message with timeout
        if pk is not None:
            self._got_packet(pk)

    def _got_packet(self, pk):
        if pk.port == CRTP_PORT_PPRZLINK:
            for c in pk.data:
                if self._transport.parse_byte(bytes([c])):
                    (sender_id, _, _, msg) = self._transport.unpack()
                    if self.verbose:
                        print("Got message {} from {}".format(
                            msg.name, sender_id))
                    # Forward message to Ivy bus
                    if self.is_connected:
                        self._ivy.send(msg, sender_id=sender_id)

    def _link_quality_cb(self, quality):
        pass

    def _link_error_cb(self, msg):
        if self.verbose:
            print("Link error: {}".format(msg))