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