[tool] crazyradio to Ivy bridge

Used for datalink with crazyflie drones.
Update pprzlink for compatibility between Python 2 and 3.
Requires to install crazflie-lib-python from Bitcraze.

conf/tools/crazyradio2ivy.xml

@@ -0,0 +1,2 @@
+<program command="sw/ground_segment/python/bitcraze/crazyradio2ivy.py" name="Crazyradio/IVY bridge" />
+

sw/ground_segment/python/bitcraze/crazyradio2ivy.py

@@ -0,0 +1,172 @@
+#!/usr/bin/env python3
+"""
+Bridge a Crazyflie connected to a Crazyradio to the Ivy software bus
+with support of PPRZLINK messages
+
+Requires 'pip3 install cflib'
+
+As the ESB protocol works using PTX and PRX (Primary Transmitter/Reciever)
+modes. Thus, data is only recieved as a response to a sent packet.
+So, we need to constantly poll the receivers for bidirectional communication.
+
+@author: Dennis Shtatnov (densht@gmail.com)
+         Gautier Hattenberger for Paparazzi UAV support
+
+"""
+# import struct
+from os import path, getenv
+import logging
+import sys
+import time
+
+from cflib.crtp.crtpstack import CRTPPacket
+from cflib.crtp import RadioDriver
+
+# if PAPARAZZI_HOME not set, then assume the tree containing this
+# file is a reasonable substitute
+PPRZ_HOME = getenv("PAPARAZZI_HOME", path.normpath(path.join(path.dirname(path.abspath(__file__)), '../../../../')))
+sys.path.append(PPRZ_HOME + "/var/lib/python")
+
+from pprzlink.ivy import IvyMessagesInterface
+from pprzlink.pprz_transport import PprzTransport
+from pprzlink.message import PprzMessage
+import pprzlink.messages_xml_map as messages_xml_map
+
+CRTP_PORT_PPRZLINK = 9
+
+
+# Only output errors from the logging framework
+#logging.basicConfig(level=logging.DEBUG)
+logging.basicConfig(level=logging.ERROR)
+
+
+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))
+
+
+if __name__ == '__main__':
+    from argparse import ArgumentParser
+
+    parser = ArgumentParser(description="Crazyradio link for paparazzi")
+    parser.add_argument('-a','--address', default=None, help="URI address of Crazyflie")
+    parser.add_argument('-c','--chanel', default='80', help="URI chanel of Crazyflie (full URI will be 'radio://0/%(default)/2M'")
+    parser.add_argument('-u','--uri', default=None, help="URI of Crazyflie (chanel option will not be effective)")
+    parser.add_argument('-b','--bus', default=None, help="Ivy bus [default to system IVY bus]")
+    parser.add_argument('-s','--scan', action='store_true', help="Scan available Crazyflie at startup")
+    parser.add_argument('-v', '--verbose', dest='verbose', action='store_true', help="display debug messages")
+    args = parser.parse_args()
+
+    if args.scan:
+        import cflib.crtp
+        # Initialize the low-level drivers (don't list the debug drivers)
+        cflib.crtp.radiodriver.set_retries_before_disconnect(1500)
+        cflib.crtp.radiodriver.set_retries(3)
+        cflib.crtp.init_drivers(enable_debug_driver=False)
+
+        # Scan for Crazyflies and use the first one found
+        print('Scanning interfaces for Crazyflies...')
+        if args.address is not None:
+            address = int(args.address, 16)
+        else:
+            address = None # equivalent to default 0xE7E7E7E7E7
+        available = cflib.crtp.scan_interfaces(address)
+        if len(available) > 0:
+            print('Crazyflies found:')
+            for i in available:
+                print(' ',i[0])
+        else:
+            print('No radio. Leaving')
+            sys.exit(1)
+
+    bridge = None
+    link_uri = None
+    if args.uri is not None:
+        link_uri = args.uri
+    else:
+        link_uri = 'radio://0/' + args.chanel + '/2M'
+
+    try:
+        # Start radio to ivy bridge
+        bridge = RadioBridge(link_uri, verbose=args.verbose)
+
+        # The Crazyflie lib doesn't contain anything to keep the application alive,
+        # so this is where your application should do something. In our case we
+        # are just waiting until we are disconnected.
+        while bridge.is_connected:
+            bridge.run()
+    except KeyboardInterrupt:
+        pass
+    except Exception as e:
+        print("Failing with error:", e)
+
+    if bridge is not None:
+        bridge.is_connected = False
+        bridge.shutdown()
+        time.sleep(1)
+
+    sys.exit()
+