Skip to content
GitLab
Projects
Groups
Snippets
Help
Loading...
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Software
Project overview
Project overview
Details
Activity
Releases
Repository
Repository
Files
Commits
Branches
Tags
Contributors
Graph
Compare
Issues
1
Issues
1
List
Boards
Labels
Service Desk
Milestones
Operations
Operations
Incidents
Environments
Analytics
Analytics
Repository
Value Stream
Wiki
Wiki
Snippets
Snippets
Members
Members
Collapse sidebar
Close sidebar
Activity
Graph
Create a new issue
Commits
Issue Boards
Open sidebar
LARN
RNA
Software
Commits
43be2f87
Commit
43be2f87
authored
Sep 13, 2019
by
Marlene Böhmer
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
Bridge connection to Crazyflie
parent
c120bbed
Changes
1
Pipelines
1
Hide whitespace changes
Inline
Side-by-side
Showing
1 changed file
with
58 additions
and
31 deletions
+58
-31
salt/drone/scripts/bridge.py
salt/drone/scripts/bridge.py
+58
-31
No files found.
salt/drone/scripts/bridge.py
View file @
43be2f87
...
...
@@ -3,15 +3,16 @@
import
logging
import
sys
import
re
import
datetime
import
time
import
prrt
import
cflib.crtp
from
cflib.crazyflie
import
Crazyflie
from
cflib.crazyflie
import
Crazyflie
,
State
from
cflib.crtp.crtpstack
import
CRTPPacket
logg
ing
.
basicConfig
(
level
=
logging
.
DEBUG
)
logg
er
=
logging
.
getLogger
(
__name__
)
MTU
=
32
DEFAULT_TARGET_DELAY
=
200
...
...
@@ -20,7 +21,9 @@ PRRT_LOCAL_PORT = 5000
class
CrazyflieConnection
:
def
__init__
(
self
,
uri
,
receive_callback
):
self
.
uri
=
uri
self
.
receive_callback
=
receive_callback
self
.
_platform_information_packet
=
None
self
.
_cf
=
Crazyflie
()
...
...
@@ -29,66 +32,88 @@ class CrazyflieConnection:
self
.
_cf
.
connection_failed
.
add_callback
(
self
.
_connection_failed
)
self
.
_cf
.
connection_lost
.
add_callback
(
self
.
_connection_lost
)
print
(
'Connecting to %s'
%
uri
)
self
.
_cf
.
open_link
(
uri
)
logger
.
info
(
'Connecting to {}'
.
format
(
uri
))
# Manually open link of _cf without starting connection setup.
# This prevents sending messages from the library at this bridging point.
self
.
_cf
.
connection_requested
.
call
(
uri
)
self
.
_cf
.
state
=
State
.
INITIALIZED
self
.
_cf
.
link_uri
=
uri
try
:
self
.
_cf
.
link
=
cflib
.
crtp
.
get_link_driver
(
uri
,
self
.
_cf
.
_link_quality_cb
,
self
.
_cf
.
_link_error_cb
)
if
not
self
.
_cf
.
link
:
message
=
'No driver found or malformed URI: {}'
.
format
(
uri
)
logger
.
warning
(
message
)
self
.
_cf
.
connection_failed
.
call
(
uri
,
message
)
else
:
# Add a callback so we can check that any data is coming back from the copter
self
.
_cf
.
packet_received
.
add_callback
(
self
.
_cf
.
_check_for_initial_packet_cb
)
self
.
_cf
.
packet_received
.
add_callback
(
self
.
_get_initial_packet
)
self
.
_cf
.
platform
.
fetch_platform_informations
(
self
.
_fetched_platform_information
)
except
Exception
as
ex
:
# pylint: disable=W0703
# We want to catch every possible exception here and show it in the user interface
import
traceback
logger
.
error
(
"Couldn't load link driver: %s
\n\n
%s"
,
ex
,
traceback
.
format_exc
())
exception_text
=
"Couldn't load link driver: %s
\n\n
%s"
%
(
ex
,
traceback
.
format_exc
())
if
self
.
_cf
.
link
:
self
.
_cf
.
link
.
close
()
self
.
_cf
.
link
=
None
self
.
_cf
.
connection_failed
.
call
(
uri
,
exception_text
)
# Variable used to keep main loop occupied until disconnect
self
.
is_connected
=
True
def
_connected
(
self
,
link_uri
):
""" This callback is called form the Crazyflie API when a Crazyflie
has been connected and the TOCs have been downloaded."""
print
(
'Connected to %s'
%
link_uri
)
def
_get_initial_packet
(
self
,
pk
):
self
.
_platform_information_packet
=
pk
self
.
_cf
.
packet_received
.
remove_callback
(
self
.
_get_initial_packet
)
def
_fetched_platform_information
(
self
):
self
.
_cf
.
connected_ts
=
datetime
.
datetime
.
now
()
self
.
_cf
.
connected
.
call
(
self
.
uri
)
def
_connected
(
self
,
link_uri
):
logger
.
info
(
'Connected to {}'
.
format
(
link_uri
))
self
.
_cf
.
packet_received
.
add_callback
(
self
.
receive_callback
)
def
send
(
self
,
pk
):
self
.
_cf
.
send_packet
(
pk
)
def
_stab_log_error
(
self
,
logconf
,
msg
):
"""Callback from the log API when an error occurs"""
print
(
'Error when logging %s: %s'
%
(
logconf
.
name
,
msg
))
def
_stab_log_data
(
self
,
timestamp
,
data
,
logconf
):
"""Callback froma the log API when data arrives"""
print
(
'[%d][%s]: %s'
%
(
timestamp
,
logconf
.
name
,
data
))
def
_connection_failed
(
self
,
link_uri
,
msg
):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the speficied address)"""
print
(
'Connection to %s failed: %s'
%
(
link_uri
,
msg
))
"""Callback when connection initial connection fails (i.e no Crazyflie at the speficied address)"""
logger
.
info
(
'Connection to {} failed: {}'
.
format
(
link_uri
,
msg
))
self
.
is_connected
=
False
def
_connection_lost
(
self
,
link_uri
,
msg
):
"""Callback when disconnected after a connection has been made (i.e
Crazyflie moves out of range)"""
print
(
'Connection to %s lost: %s'
%
(
link_uri
,
msg
))
logger
.
info
(
'Connection to {} lost: {}'
.
format
(
link_uri
,
msg
))
def
_disconnected
(
self
,
link_uri
):
"""Callback when the Crazyflie is disconnected (called in all cases)"""
print
(
'Disconnected from %s'
%
link_uri
)
logger
.
info
(
'Disconnected from {}'
.
format
(
link_uri
))
self
.
is_connected
=
False
def
close
(
self
):
print
(
'Closing Crazyflie Connection'
)
logger
.
info
(
'Closing Crazyflie Connection'
)
self
.
_cf
.
close_link
()
class
ClientConnection
:
def
__init__
(
self
,
uri
):
self
.
uri
=
uri
uri_match
=
re
.
search
(
r
'^prrt://((?:[\d]{1,3})\.(?:[\d]{1,3})\.(?:[\d]{1,3})\.(?:[\d]{1,3})):([\d]{1,5})'
r
'(?:/([\d]{1,9}))?$'
,
uri
)
if
not
uri_match
:
raise
Exception
(
'Invalid PRRT URI'
)
address
=
uri_match
.
group
(
1
)
port
=
int
(
uri_match
.
group
(
2
))
target_delay_us
=
DEFAULT_TARGET_DELAY
if
uri_match
.
group
(
3
):
target_delay_us
=
int
(
uri_match
.
group
(
3
))
print
(
'Open PRRT Link to {}:{} with target delay {}'
.
format
(
address
,
port
,
target_delay_us
))
logger
.
info
(
'Open PRRT Link to {}:{} with target delay {}'
.
format
(
address
,
port
,
target_delay_us
))
self
.
_prrt_socket
=
prrt
.
PrrtSocket
((
"0.0.0.0"
,
PRRT_LOCAL_PORT
),
maximum_payload_size
=
MTU
,
target_delay
=
target_delay_us
)
...
...
@@ -96,7 +121,7 @@ class ClientConnection:
self
.
_prrt_socket
.
connect
((
address
,
port
))
def
send
(
self
,
pk
):
print
(
'send to client'
)
print
(
'send to client'
+
pk
)
def
receive
(
self
):
print
(
'receive from client'
)
...
...
@@ -104,8 +129,9 @@ class ClientConnection:
return
pk
def
close
(
self
):
print
(
'Closing Client Connection'
)
# self._prrt_socket.close()
logger
.
info
(
'Closing Client Connection'
)
self
.
_prrt_socket
=
None
logger
.
info
(
'Disconnected from {}'
.
format
(
self
.
uri
))
class
Bridge
:
...
...
@@ -137,5 +163,6 @@ if __name__ == '__main__':
try
:
bridge
.
run
()
except
KeyboardInterrupt
:
print
(
'
\n
'
)
bridge
.
stop
()
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
.
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment