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
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
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
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))