forked from paparazzi/paparazzi
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Used for datalink with crazyflie drones. Update pprzlink for compatibility between Python 2 and 3. Requires to install crazflie-lib-python from Bitcraze.
- Loading branch information
1 parent
380143f
commit 2287376
Showing
3 changed files
with
175 additions
and
1 deletion.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,2 @@ | ||
<program command="sw/ground_segment/python/bitcraze/crazyradio2ivy.py" name="Crazyradio/IVY bridge" /> | ||
|
Submodule pprzlink
updated
3 files
+1 −1 | lib/v2.0/ocaml/pprzLink.ml | |
+4 −1 | lib/v2.0/python/pprzlink/message.py | |
+3 −0 | lib/v2.0/python/pprzlink/pprz_transport.py |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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() | ||
|