class Espdrone():
"""The Espdrone class"""
def __init__(self, name=None, link=None, ro_cache=None, rw_cache=None):
"""
Create the objects from this module and register callbacks.
ro_cache -- Path to read-only cache (string)
rw_cache -- Path to read-write cache (string)
"""
# Called on disconnect, no matter the reason
self.disconnected = Caller()
# Called on unintentional disconnect only
self.connection_lost = Caller()
# Called when the first packet in a new link is received
self.link_established = Caller()
# Called when the user requests a connection
self.connection_requested = Caller()
# Called when the link is established and the TOCs (that are not
# cached) have been downloaded
self.connected = Caller()
# Called if establishing of the link fails (i.e times out)
self.connection_failed = Caller()
# Called for every packet received
self.packet_received = Caller()
# Called for every packet sent
self.packet_sent = Caller()
# Called when the link driver updates the link quality measurement
self.link_quality_updated = Caller()
self.state = State.DISCONNECTED
self.link = link
self.name = name
self._toc_cache = TocCache(ro_cache=ro_cache,
rw_cache=rw_cache)
self.incoming = _IncomingPacketHandler(self)
self.incoming.setDaemon(True)
self.incoming.start()
self.camera = Camera(self)
self.commander = Commander(self)
self.high_level_commander = HighLevelCommander(self)
self.loc = Localization(self)
self.extpos = Extpos(self)
self.log = Log(self)
self.console = Console(self)
self.param = Param(self)
self.mem = Memory(self)
self.platform = PlatformService(self)
self.link_uri = ''
# Used for retry when no reply was sent back
self.packet_received.add_callback(self._check_for_initial_packet_cb)
self.packet_received.add_callback(self._check_for_answers)
self._answer_patterns = {}
self._send_lock = Lock()
self.connected_ts = None
# Connect callbacks to logger
self.disconnected.add_callback(
lambda uri: logger.info('Callback->Disconnected from [%s]', uri))
self.disconnected.add_callback(self._disconnected)
self.link_established.add_callback(
lambda uri: logger.info('Callback->Connected to [%s]', uri))
self.connection_lost.add_callback(
lambda uri, errmsg: logger.info(
'Callback->Connection lost to [%s]: %s', uri, errmsg))
self.connection_failed.add_callback(
lambda uri, errmsg: logger.info(
'Callback->Connected failed to [%s]: %s', uri, errmsg))
self.connection_requested.add_callback(
lambda uri: logger.info(
'Callback->Connection initialized[%s]', uri))
self.connected.add_callback(
lambda uri: logger.info(
'Callback->Connection setup finished [%s]', uri))
def _disconnected(self, link_uri):
""" Callback when disconnected."""
self.connected_ts = None
def _start_connection_setup(self):
"""Start the connection setup by refreshing the TOCs"""
logger.info('We are connected[%s], request connection setup',
self.link_uri)
self.platform.fetch_platform_informations(self._platform_info_fetched)
def _platform_info_fetched(self):
self.log.refresh_toc(self._log_toc_updated_cb, self._toc_cache)
def _param_toc_updated_cb(self):
"""Called when the param TOC has been fully updated"""
logger.info('Param TOC finished updating')
self.connected_ts = datetime.datetime.now()
self.connected.call(self.link_uri)
# Trigger the update for all the parameters
self.param.request_update_of_all_params()
def _mems_updated_cb(self):
"""Called when the memories have been identified"""
logger.info('Memories finished updating')
self.param.refresh_toc(self._param_toc_updated_cb, self._toc_cache)
def _log_toc_updated_cb(self):
"""Called when the log TOC has been fully updated"""
logger.info('Log TOC finished updating')
self.mem.refresh(self._mems_updated_cb)
def _link_error_cb(self, errmsg):
"""Called from the link driver when there's an error"""
logger.warning('Got link error callback [%s] in state [%s]',
errmsg, self.state)
if (self.link is not None):
self.link.close()
self.link = None
if (self.state == State.INITIALIZED):
self.connection_failed.call(self.link_uri, errmsg)
if (self.state == State.CONNECTED or
self.state == State.SETUP_FINISHED):
self.disconnected.call(self.link_uri)
self.connection_lost.call(self.link_uri, errmsg)
self.state = State.DISCONNECTED
def _link_quality_cb(self, percentage):
"""Called from link driver to report link quality"""
self.link_quality_updated.call(percentage)
def _check_for_initial_packet_cb(self, data):
"""
Called when first packet arrives from Espdrone.
This is used to determine if we are connected to something that is
answering.
"""
self.state = State.CONNECTED
self.link_established.call(self.link_uri)
self.packet_received.remove_callback(self._check_for_initial_packet_cb)
def open_link(self, link_uri):
"""
Open the communication link to a copter at the given URI and setup the
connection (download log/parameter TOC).
"""
self.connection_requested.call(link_uri)
self.state = State.INITIALIZED
self.link_uri = link_uri
try:
self.link = edlib.crtp.get_link_driver(
link_uri, self._link_quality_cb, self._link_error_cb)
if not self.link:
message = 'No driver found or malformed URI: {}' \
.format(link_uri)
logger.warning(message)
self.connection_failed.call(link_uri, message)
else:
# Add a callback so we can check that any data is coming
# back from the copter
self.packet_received.add_callback(
self._check_for_initial_packet_cb)
self._start_connection_setup()
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.link:
self.link.close()
self.link = None
self.connection_failed.call(link_uri, exception_text)
raise ConnectionError()
def close_link(self):
"""Close the communication link."""
logger.info('Closing link')
if (self.link is not None):
self.commander.send_setpoint(0, 0, 0, 0)
if (self.link is not None):
self.link.close()
self.link = None
self._answer_patterns = {}
self.disconnected.call(self.link_uri)
"""Check if the communication link is open or not."""
def is_connected(self):
return self.connected_ts is not None
def add_port_callback(self, port, cb):
"""Add a callback to cb on port"""
self.incoming.add_port_callback(port, cb)
def remove_port_callback(self, port, cb):
"""Remove the callback cb on port"""
self.incoming.remove_port_callback(port, cb)
def _no_answer_do_retry(self, pk, pattern):
"""Resend packets that we have not gotten answers to"""
logger.info('Resending for pattern %s', pattern)
# Set the timer to None before trying to send again
self.send_packet(pk, expected_reply=pattern, resend=True)
def _check_for_answers(self, pk):
"""
Callback called for every packet received to check if we are
waiting for an answer on this port. If so, then cancel the retry
timer.
"""
longest_match = ()
if len(self._answer_patterns) > 0:
data = (pk.header,) + tuple(pk.data)
for p in list(self._answer_patterns.keys()):
logger.debug('Looking for pattern match on %s vs %s', p, data)
if len(p) <= len(data):
if p == data[0:len(p)]:
match = data[0:len(p)]
if len(match) >= len(longest_match):
logger.debug('Found new longest match %s', match)
longest_match = match
if len(longest_match) > 0:
self._answer_patterns[longest_match].cancel()
del self._answer_patterns[longest_match]
def send_packet(self, pk, expected_reply=(), resend=False, timeout=0.1):
"""
Send a packet through the link interface.
pk -- Packet to send
expect_answer -- True if a packet from the Espdrone is expected to
be sent back, otherwise false
"""
self._send_lock.acquire()
if self.link is not None:
if len(expected_reply) > 0 and not resend and \
self.link.needs_resending:
pattern = (pk.header,) + expected_reply
logger.debug(
'Sending packet and expecting the %s pattern back',
pattern)
new_timer = Timer(timeout,
lambda: self._no_answer_do_retry(pk,
pattern))
self._answer_patterns[pattern] = new_timer
new_timer.start()
elif resend:
# Check if we have gotten an answer, if not try again
pattern = expected_reply
if pattern in self._answer_patterns:
logger.debug('We want to resend and the pattern is there')
if self._answer_patterns[pattern]:
new_timer = Timer(timeout,
lambda:
self._no_answer_do_retry(
pk, pattern))
self._answer_patterns[pattern] = new_timer
new_timer.start()
else:
logger.debug('Resend requested, but no pattern found: %s',
self._answer_patterns)
self.link.send_packet(pk)
self.packet_sent.call(pk)
self._send_lock.release()
class Memory():
"""Access memories on the Espdrone"""
# These codes can be decoded using os.stderror, but
# some of the text messages will look very strange
# in the UI, so they are redefined here
_err_codes = {
errno.ENOMEM: 'No more memory available',
errno.ENOEXEC: 'Command not found',
errno.ENOENT: 'No such block id',
errno.E2BIG: 'Block too large',
errno.EEXIST: 'Block already exists'
}
def __init__(self, espdrone=None):
"""Instantiate class and connect callbacks"""
# Called when new memories have been added
self.mem_added_cb = Caller()
# Called when new data has been read
self.mem_read_cb = Caller()
self.mem_write_cb = Caller()
self.ed = espdrone
self.ed.add_port_callback(CRTPPort.MEM, self._new_packet_cb)
self.ed.disconnected.add_callback(self._disconnected)
self._write_requests_lock = Lock()
self._clear_state()
def _clear_state(self):
self.mems = []
self._refresh_callback = None
self._fetch_id = 0
self.nbr_of_mems = 0
self._ow_mem_fetch_index = 0
self._elem_data = ()
self._read_requests = {}
self._write_requests = {}
self._ow_mems_left_to_update = []
self._getting_count = False
def _mem_update_done(self, mem):
"""
Callback from each individual memory (only 1-wire) when reading of
header/elements are done
"""
if mem.id in self._ow_mems_left_to_update:
self._ow_mems_left_to_update.remove(mem.id)
logger.debug(mem)
if len(self._ow_mems_left_to_update) == 0:
if self._refresh_callback:
self._refresh_callback()
self._refresh_callback = None
def get_mem(self, id):
"""Fetch the memory with the supplied id"""
for m in self.mems:
if m.id == id:
return m
return None
def get_mems(self, type):
"""Fetch all the memories of the supplied type"""
ret = ()
for m in self.mems:
if m.type == type:
ret += (m,)
return ret
def ow_search(self, vid=0xBC, pid=None, name=None):
"""Search for specific memory id/name and return it"""
for m in self.get_mems(MemoryElement.TYPE_1W):
if pid and m.pid == pid or name and m.name == name:
return m
return None
def write(self, memory, addr, data, flush_queue=False):
"""Write the specified data to the given memory at the given address"""
wreq = _WriteRequest(memory, addr, data, self.ed)
if memory.id not in self._write_requests:
self._write_requests[memory.id] = []
# Workaround until we secure the uplink and change messages for
# mems to non-blocking
self._write_requests_lock.acquire()
if flush_queue:
self._write_requests[memory.id] = self._write_requests[
memory.id][:1]
self._write_requests[memory.id].insert(len(self._write_requests), wreq)
if len(self._write_requests[memory.id]) == 1:
wreq.start()
self._write_requests_lock.release()
return True
def read(self, memory, addr, length):
"""
Read the specified amount of bytes from the given memory at the given
address
"""
if memory.id in self._read_requests:
logger.warning('There is already a read operation ongoing for '
'memory id {}'.format(memory.id))
return False
rreq = _ReadRequest(memory, addr, length, self.ed)
self._read_requests[memory.id] = rreq
rreq.start()
return True
def refresh(self, refresh_done_callback):
"""Start fetching all the detected memories"""
self._refresh_callback = refresh_done_callback
self._fetch_id = 0
for m in self.mems:
try:
self.mem_read_cb.remove_callback(m.new_data)
m.disconnect()
except Exception as e:
logger.info(
'Error when removing memory after update: {}'.format(e))
self.mems = []
self.nbr_of_mems = 0
self._getting_count = False
logger.debug('Requesting number of memories')
pk = CRTPPacket()
pk.set_header(CRTPPort.MEM, CHAN_INFO)
pk.data = (CMD_INFO_NBR,)
self.ed.send_packet(pk, expected_reply=(CMD_INFO_NBR,))
def _disconnected(self, uri):
"""The link to the Espdrone has been broken. Reset state"""
self._clear_state()
def _new_packet_cb(self, packet):
"""Callback for newly arrived packets for the memory port"""
chan = packet.channel
cmd = packet.data[0]
payload = packet.data[1:]
if chan == CHAN_INFO:
if cmd == CMD_INFO_NBR:
self.nbr_of_mems = payload[0]
logger.info('{} memories found'.format(self.nbr_of_mems))
# Start requesting information about the memories,
# if there are any...
if self.nbr_of_mems > 0:
if not self._getting_count:
self._getting_count = True
logger.debug('Requesting first id')
pk = CRTPPacket()
pk.set_header(CRTPPort.MEM, CHAN_INFO)
pk.data = (CMD_INFO_DETAILS, 0)
self.ed.send_packet(pk, expected_reply=(
CMD_INFO_DETAILS, 0))
else:
self._refresh_callback()
if cmd == CMD_INFO_DETAILS:
# Did we get a good reply, otherwise try again:
if len(payload) < 5:
# Workaround for 1-wire bug when memory is detected
# but updating the info crashes the communication with
# the 1-wire. Fail by saying we only found 1 memory
# (the I2C).
logger.error(
'-------->Got good count, but no info on mem!')
self.nbr_of_mems = 1
if self._refresh_callback:
self._refresh_callback()
self._refresh_callback = None
return
# Create information about a new memory
# Id - 1 byte
mem_id = payload[0]
# Type - 1 byte
mem_type = payload[1]
# Size 4 bytes (as addr)
mem_size = struct.unpack('I', payload[2:6])[0]
# Addr (only valid for 1-wire?)
mem_addr_raw = struct.unpack('B' * 8, payload[6:14])
mem_addr = ''
for m in mem_addr_raw:
mem_addr += '{:02X}'.format(m)
if (not self.get_mem(mem_id)):
if mem_type == MemoryElement.TYPE_1W:
mem = OWElement(id=mem_id, type=mem_type,
size=mem_size,
addr=mem_addr, mem_handler=self)
self.mem_read_cb.add_callback(mem.new_data)
self.mem_write_cb.add_callback(mem.write_done)
self._ow_mems_left_to_update.append(mem.id)
elif mem_type == MemoryElement.TYPE_I2C:
mem = I2CElement(id=mem_id, type=mem_type,
size=mem_size,
mem_handler=self)
self.mem_read_cb.add_callback(mem.new_data)
self.mem_write_cb.add_callback(mem.write_done)
elif mem_type == MemoryElement.TYPE_DRIVER_LED:
mem = LEDDriverMemory(id=mem_id, type=mem_type,
size=mem_size, mem_handler=self)
logger.debug(mem)
self.mem_read_cb.add_callback(mem.new_data)
self.mem_write_cb.add_callback(mem.write_done)
elif mem_type == MemoryElement.TYPE_LOCO:
mem = LocoMemory(id=mem_id, type=mem_type,
size=mem_size, mem_handler=self)
logger.debug(mem)
self.mem_read_cb.add_callback(mem.new_data)
elif mem_type == MemoryElement.TYPE_TRAJ:
mem = TrajectoryMemory(id=mem_id, type=mem_type,
size=mem_size, mem_handler=self)
logger.debug(mem)
self.mem_write_cb.add_callback(mem.write_done)
elif mem_type == MemoryElement.TYPE_LOCO2:
mem = LocoMemory2(id=mem_id, type=mem_type,
size=mem_size, mem_handler=self)
logger.debug(mem)
self.mem_read_cb.add_callback(mem.new_data)
elif mem_type == MemoryElement.TYPE_LH:
mem = LighthouseMemory(id=mem_id, type=mem_type,
size=mem_size, mem_handler=self)
logger.debug(mem)
self.mem_read_cb.add_callback(mem.new_data)
self.mem_write_cb.add_callback(mem.write_done)
elif mem_type == MemoryElement.TYPE_MEMORY_TESTER:
mem = MemoryTester(id=mem_id, type=mem_type,
size=mem_size, mem_handler=self)
logger.debug(mem)
self.mem_read_cb.add_callback(mem.new_data)
self.mem_write_cb.add_callback(mem.write_done)
else:
mem = MemoryElement(id=mem_id, type=mem_type,
size=mem_size, mem_handler=self)
logger.debug(mem)
self.mems.append(mem)
self.mem_added_cb.call(mem)
self._fetch_id = mem_id + 1
if self.nbr_of_mems - 1 >= self._fetch_id:
logger.debug(
'Requesting information about memory {}'.format(
self._fetch_id))
pk = CRTPPacket()
pk.set_header(CRTPPort.MEM, CHAN_INFO)
pk.data = (CMD_INFO_DETAILS, self._fetch_id)
self.ed.send_packet(pk, expected_reply=(
CMD_INFO_DETAILS, self._fetch_id))
else:
logger.debug(
'Done getting all the memories, start reading the OWs')
ows = self.get_mems(MemoryElement.TYPE_1W)
# If there are any OW mems start reading them, otherwise
# we are done
for ow_mem in ows:
ow_mem.update(self._mem_update_done)
if len(ows) == 0:
if self._refresh_callback:
self._refresh_callback()
self._refresh_callback = None
if chan == CHAN_WRITE:
id = cmd
(addr, status) = struct.unpack('<IB', payload[0:5])
logger.debug(
'WRITE: Mem={}, addr=0x{:X}, status=0x{}'.format(
id, addr, status))
# Find the read request
if id in self._write_requests:
self._write_requests_lock.acquire()
wreq = self._write_requests[id][0]
if status == 0:
if wreq.write_done(addr):
# self._write_requests.pop(id, None)
# Remove the first item
self._write_requests[id].pop(0)
self.mem_write_cb.call(wreq.mem, wreq.addr)
# Get a new one to start (if there are any)
if len(self._write_requests[id]) > 0:
self._write_requests[id][0].start()
else:
logger.debug(
'Status {}: write resending...'.format(status))
wreq.resend()
self._write_requests_lock.release()
if chan == CHAN_READ:
id = cmd
(addr, status) = struct.unpack('<IB', payload[0:5])
data = struct.unpack('B' * len(payload[5:]), payload[5:])
logger.debug('READ: Mem={}, addr=0x{:X}, status=0x{}, '
'data={}'.format(id, addr, status, data))
# Find the read request
if id in self._read_requests:
logger.debug(
'READING: We are still interested in request for '
'mem {}'.format(id))
rreq = self._read_requests[id]
if status == 0:
if rreq.add_data(addr, payload[5:]):
self._read_requests.pop(id, None)
self.mem_read_cb.call(rreq.mem, rreq.addr, rreq.data)
else:
logger.debug('Status {}: resending...'.format(status))
rreq.resend()
class CallerTest(unittest.TestCase):
def setUp(self):
self.callback_count = 0
self.sut = Caller()
def test_that_callback_is_added(self):
# Fixture
# Test
self.sut.add_callback(self._callback)
# Assert
self.sut.call()
self.assertEqual(1, self.callback_count)
def test_that_callback_is_added_only_one_time(self):
# Fixture
# Test
self.sut.add_callback(self._callback)
self.sut.add_callback(self._callback)
# Assert
self.sut.call()
self.assertEqual(1, self.callback_count)
def test_that_multiple_callbacks_are_added(self):
# Fixture
# Test
self.sut.add_callback(self._callback)
self.sut.add_callback(self._callback2)
# Assert
self.sut.call()
self.assertEqual(2, self.callback_count)
def test_that_callback_is_removed(self):
# Fixture
self.sut.add_callback(self._callback)
# Test
self.sut.remove_callback(self._callback)
# Assert
self.sut.call()
self.assertEqual(0, self.callback_count)
def test_that_callback_is_called_with_arguments(self):
# Fixture
self.sut.add_callback(self._callback_with_args)
# Test
self.sut.call('The token')
# Assert
self.assertEqual('The token', self.callback_token)
def _callback(self):
self.callback_count += 1
def _callback2(self):
self.callback_count += 1
def _callback_with_args(self, token):
self.callback_token = token