def _request_protocol_version(self):
# Sending a sink request to detect if the connected Espdrone
# supports protocol versioning
pk = CRTPPacket()
pk.set_header(CRTPPort.LINKCTRL, LINKSERVICE_SOURCE)
pk.data = (0,)
self._ed.send_packet(pk)
def _write_new_chunk(self):
"""
Called to request a new chunk of data to be read from the Espdrone
"""
# Figure out the length of the next request
new_len = len(self._data)
if new_len > _WriteRequest.MAX_DATA_LENGTH:
new_len = _WriteRequest.MAX_DATA_LENGTH
logger.debug('Writing new chunk of {}bytes at 0x{:X}'.format(
new_len, self._current_addr))
data = self._data[:new_len]
self._data = self._data[new_len:]
pk = CRTPPacket()
pk.set_header(CRTPPort.MEM, CHAN_WRITE)
pk.data = struct.pack('<BI', self.mem.id, self._current_addr)
# Create a tuple used for matching the reply using id and address
reply = struct.unpack('<BBBBB', pk.data)
self._sent_reply = reply
# Add the data
pk.data += struct.pack('B' * len(data), *data)
self._sent_packet = pk
self.ed.send_packet(pk, expected_reply=reply, timeout=1)
self._addr_add = len(data)
def write_flash(self, addr, page_buffer, target_page, page_count):
"""Initiate flashing of data in the buffer to flash."""
# print "Write page", flashPage
# print "Writing page [%d] and [%d] forward" % (flashPage, nPage)
pk = None
# Flushing downlink ...
pk = self.link.receive_packet(0)
while pk is not None:
pk = self.link.receive_packet(0)
retry_counter = 5
# print "Flasing to 0x{:X}".format(addr)
while ((not pk or pk.header != 0xFF
or struct.unpack('<BB', pk.data[0:2]) != (addr, 0x18))
and retry_counter >= 0):
pk = CRTPPacket()
pk.set_header(0xFF, 0xFF)
pk.data = struct.pack('<BBHHH', addr, 0x18, page_buffer,
target_page, page_count)
self.link.send_packet(pk)
pk = self.link.receive_packet(1)
retry_counter -= 1
if retry_counter < 0:
self.error_code = -1
return False
self.error_code = pk.data[3]
return pk.data[2] == 1
def read_flash(self, addr=0xFF, page=0x00):
"""Read back a flash page from the Espdrone and return it"""
buff = bytearray()
page_size = self.targets[addr].page_size
for i in range(0, int(math.ceil(page_size / 25.0))):
pk = None
retry_counter = 5
while ((not pk or pk.header != 0xFF
or struct.unpack('<BB', pk.data[0:2]) != (addr, 0x1C))
and retry_counter >= 0):
pk = CRTPPacket()
pk.set_header(0xFF, 0xFF)
pk.data = struct.pack('<BBHH', addr, 0x1C, page, (i * 25))
self.link.send_packet(pk)
pk = self.link.receive_packet(1)
retry_counter -= 1
if (retry_counter < 0):
return None
else:
buff += pk.data[6:]
# For some reason we get one byte extra here...
return buff[0:page_size]
def send_stop_setpoint(self):
"""
Send STOP setpoint, stopping the motors and (potentially) falling.
"""
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER_GENERIC
pk.data = struct.pack('<B', TYPE_STOP)
self._ed.send_packet(pk)
def set_continous_wave(self, enabled):
"""
Enable/disable the client side X-mode. When enabled this recalculates
the setpoints before sending them to the Espdrone.
"""
pk = CRTPPacket()
pk.set_header(CRTPPort.PLATFORM, PLATFORM_COMMAND)
pk.data = (0, enabled)
self._ed.send_packet(pk)
def send_short_lpp_packet(self, dest_id, data):
"""
Send ultra-wide-band LPP packet to dest_id
"""
pk = CRTPPacket()
pk.port = CRTPPort.LOCALIZATION
pk.channel = self.GENERIC_CH
pk.data = struct.pack('<BB', self.LPS_SHORT_LPP_PACKET, dest_id) + data
self._ed.send_packet(pk)
def send_emergency_stop_watchdog(self):
"""
Send emergency stop watchdog
"""
pk = CRTPPacket()
pk.port = CRTPPort.LOCALIZATION
pk.channel = self.GENERIC_CH
pk.data = struct.pack('<B', self.EMERGENCY_STOP_WATCHDOG)
self._ed.send_packet(pk)
def send_emergency_reset(self):
"""
Send emergency reset
"""
pk = CRTPPacket()
pk.port = CRTPPort.LOCALIZATION
pk.channel = self.GENERIC_CH
pk.data = struct.pack('<B', self.EMERGENCY_RESET)
self._ed.send_packet(pk)
def test_that_header_is_set_when_constructed(self):
# Fixture
sut = CRTPPacket(header=0x21)
# Test
actual = sut.get_header()
# Assert
self.assertEqual(0x2d, actual)
self.assertEqual(2, sut.port)
self.assertEqual(1, sut.channel)
def send_extpos(self, pos):
"""
Send the current Espdrone X, Y, Z position. This is going to be
forwarded to the Espdrone's position estimator.
"""
pk = CRTPPacket()
pk.port = CRTPPort.LOCALIZATION
pk.channel = self.POSITION_CH
pk.data = struct.pack('<fff', pos[0], pos[1], pos[2])
self._ed.send_packet(pk)
def send_hover_setpoint(self, vx, vy, yawrate, zdistance):
"""
Control mode where the height is send as an absolute setpoint (intended
to be the distance to the surface under the Espdrone).
vx and vy are in m/s
yawrate is in degrees/s
"""
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER_GENERIC
pk.data = struct.pack('<Bffff', TYPE_HOVER, vx, vy, yawrate, zdistance)
self._ed.send_packet(pk)
def send_velocity_world_setpoint(self, vx, vy, vz, yawrate):
"""
Send Velocity in the world frame of reference setpoint.
vx, vy, vz are in m/s
yawrate is in degrees/s
"""
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER_GENERIC
pk.data = struct.pack('<Bffff', TYPE_VELOCITY_WORLD, vx, vy, vz,
yawrate)
self._ed.send_packet(pk)
def send_zdistance_setpoint(self, roll, pitch, yawrate, zdistance):
"""
Control mode where the height is send as an absolute setpoint (intended
to be the distance to the surface under the Espdrone).
Roll, pitch, yawrate are defined as degrees, degrees, degrees/s
"""
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER_GENERIC
pk.data = struct.pack('<Bffff', TYPE_ZDISTANCE, roll, pitch, yawrate,
zdistance)
self._ed.send_packet(pk)
def stop(self):
"""Stop the logging for this entry"""
if (self.ed.link is not None):
if (self.id is None):
logger.warning('Stopping block, but no block registered')
else:
logger.debug('Sending stop logging for block id=%d', self.id)
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
pk.data = (CMD_STOP_LOGGING, self.id)
self.ed.send_packet(pk,
expected_reply=(CMD_STOP_LOGGING, self.id))
def send_position_setpoint(self, x, y, z, yaw):
"""
Control mode where the position is sent as absolute x,y,z coordinate in
meter and the yaw is the absolute orientation.
x and y are in m
yaw is in degrees
"""
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER_GENERIC
pk.data = struct.pack('<Bffff', TYPE_POSITION, x, y, z, yaw)
self._ed.send_packet(pk)
def send_extpose(self, pos, quat):
"""
Send the current Espdrone pose (position [x, y, z] and
attitude quaternion [qx, qy, qz, qw]). This is going to be forwarded
to the Espdrone's position estimator.
"""
pk = CRTPPacket()
pk.port = CRTPPort.LOCALIZATION
pk.channel = self.GENERIC_CH
pk.data = struct.pack('<Bfffffff', self.EXT_POSE, pos[0], pos[1],
pos[2], quat[0], quat[1], quat[2], quat[3])
self._ed.send_packet(pk)
def refresh_toc(self, refresh_done_callback, toc_cache):
"""Start refreshing the table of loggale variables"""
self._useV2 = self.ed.platform.get_protocol_version() >= 4
self._toc_cache = toc_cache
self._refresh_callback = refresh_done_callback
self.toc = None
pk = CRTPPacket()
pk.set_header(CRTPPort.LOGGING, CHAN_SETTINGS)
pk.data = (CMD_RESET_LOGGING, )
self.ed.send_packet(pk, expected_reply=(CMD_RESET_LOGGING, ))
def send_full_state_setpoint(self, x, y, z, vx, vy, vz, ax, ay, az, quat_x,
quat_y, quat_z, quat_w, rate_roll, rate_pitch,
rate_yaw):
"""
Full state control!
"""
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER_GENERIC
pk.data = struct.pack(
'<Bhhhhhhhhhihhh', TYPE_POSITION, x * 1000, y * 1000, z * 1000,
vx * 1000, vy * 1000, vz * 1000, ax * 1000, ay * 1000, az * 1000,
Localization.quatcompress([quat_x, quat_y, quat_z, quat_w]),
rate_roll * 1000, rate_pitch * 1000, rate_yaw * 1000)
self._ed.send_packet(pk)
def delete(self):
"""Delete this entry in the Espdrone"""
if (self.ed.link is not None):
if (self.id is None):
logger.warning('Delete block, but no block registered')
else:
logger.debug(
'LogEntry: Sending delete logging for block id=%d' %
self.id)
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
pk.data = (CMD_DELETE_BLOCK, self.id)
self.ed.send_packet(pk,
expected_reply=(CMD_DELETE_BLOCK, self.id))
def _crt_service_callback(self, pk):
if pk.channel == LINKSERVICE_SOURCE:
# If the sink contains a magic string, get the protocol version,
# otherwise -1
if pk.data[:18].decode('utf8').rstrip('\0') == 'Edlab Espdrone':
pk = CRTPPacket()
pk.set_header(CRTPPort.PLATFORM, VERSION_COMMAND)
pk.data = (VERSION_GET_PROTOCOL, )
self._ed.send_packet(pk)
else:
print(pk.data[:18].decode('utf8'))
self._protocolVersion = -1
logger.info('Procotol version: {}'.format(
self.get_protocol_version()))
self._callback()
def reset_to_bootloader1(self, cpu_id):
""" Reset to the bootloader
The parameter cpuid shall correspond to the device to reset.
Return true if the reset has been done and the contact with the
bootloader is established.
"""
# Send an echo request and wait for the answer
# Mainly aim to bypass a bug of the espdrone firmware that prevents
# reset before normal CRTP communication
pk = CRTPPacket()
pk.port = CRTPPort.LINKCTRL
pk.data = (1, 2, 3) + cpu_id
self.link.send_packet(pk)
pk = None
while True:
pk = self.link.receive_packet(2)
if not pk:
return False
if pk.port == CRTPPort.LINKCTRL:
break
# Send the reset to bootloader request
pk = CRTPPacket()
pk.set_header(0xFF, 0xFF)
pk.data = (0xFF, 0xFE) + cpu_id
self.link.send_packet(pk)
# Wait to ack the reset ...
pk = None
while True:
pk = self.link.receive_packet(2)
if not pk:
return False
if pk.port == 0xFF and tuple(pk.data) == (0xFF, 0xFE) + cpu_id:
pk.data = (0xFF, 0xF0) + cpu_id
self.link.send_packet(pk)
break
time.sleep(0.1)
self.link.close()
self.link = edlib.crtp.get_link_driver(self.clink_address)
# time.sleep(0.1)
return self._update_info()
def start(self):
"""Start the logging for this entry"""
if (self.ed.link is not None):
if (self._added is False):
self.create()
logger.debug('First time block is started, add block')
else:
logger.debug(
'Block already registered, starting logging'
' for id=%d', self.id)
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
pk.data = (CMD_START_LOGGING, self.id, self.period)
self.ed.send_packet(pk,
expected_reply=(CMD_START_LOGGING,
self.id))
def send_setpoint(self, roll, pitch, yaw, thrust):
"""
Send a new control setpoint for roll/pitch/yaw/thrust to the copter
The arguments roll/pitch/yaw/trust is the new setpoints that should
be sent to the copter
"""
if thrust > 0xFFFF or thrust < 0:
raise ValueError('Thrust must be between 0 and 0xFFFF')
if self._x_mode:
roll, pitch = 0.707 * (roll - pitch), 0.707 * (roll + pitch)
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER
pk.data = struct.pack('<fffH', roll, -pitch, yaw, thrust)
self._ed.send_packet(pk)
def _request_new_chunk(self):
"""
Called to request a new chunk of data to be read from the Espdrone
"""
# Figure out the length of the next request
new_len = self._bytes_left
if new_len > _ReadRequest.MAX_DATA_LENGTH:
new_len = _ReadRequest.MAX_DATA_LENGTH
logger.debug('Requesting new chunk of {}bytes at 0x{:X}'.format(
new_len, self._current_addr))
# Request the data for the next address
pk = CRTPPacket()
pk.set_header(CRTPPort.MEM, CHAN_READ)
pk.data = struct.pack('<BIB', self.mem.id, self._current_addr, new_len)
reply = struct.unpack('<BBBBB', pk.data[:-1])
self.ed.send_packet(pk, expected_reply=reply, timeout=1)
def _request_toc_element(self, index):
"""Request information about a specific item in the TOC"""
logger.debug('Requesting index %d on port %d', index, self.port)
pk = CRTPPacket()
if self._useV2:
pk.set_header(self.port, TOC_CHANNEL)
pk.data = (CMD_TOC_ITEM_V2, index & 0x0ff, (index >> 8) & 0x0ff)
self.ed.send_packet(pk, expected_reply=(
CMD_TOC_ITEM_V2, index & 0x0ff, (index >> 8) & 0x0ff))
else:
pk.set_header(self.port, TOC_CHANNEL)
pk.data = (CMD_TOC_ELEMENT, index)
self.ed.send_packet(pk, expected_reply=(CMD_TOC_ELEMENT, index))
def reset_to_bootloader(self, target_id):
retry_counter = 5
pk = CRTPPacket()
pk.set_header(0xFF, 0xFF)
pk.data = (target_id, 0xFF)
self.link.send_packet(pk)
got_answer = False
while (not got_answer and retry_counter >= 0):
pk = self.link.receive_packet(1)
if pk and pk.header == 0xFF:
try:
data = struct.unpack('<BB', pk.data[0:2])
got_answer = data == (target_id, 0xFF)
except struct.error:
# Failed unpacking, retry
pass
if got_answer:
new_address = (0xb1, ) + struct.unpack('<BBBB', pk.data[2:6][::-1])
# The reset packet arrival cannot be checked.
# Send it more than one time to increase the chances it makes it.
for _ in range(10):
pk = CRTPPacket()
pk.set_header(0xFF, 0xFF)
pk.data = (target_id, 0xF0, 0x00)
self.link.send_packet(pk)
addr = int(binascii.hexlify(struct.pack('B' * 5, *new_address)),
16)
time.sleep(1)
self.link.close()
time.sleep(0.2)
self.link = edlib.crtp.get_link_driver(
'radio://0/0/2M/{:X}'.format(addr))
return True
else:
return False
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 _update_mapping(self, target_id):
pk = CRTPPacket()
pk.set_header(0xff, 0xff)
pk.data = (target_id, 0x12)
self.link.send_packet(pk)
pk = self.link.receive_packet(2)
if (pk and pk.header == 0xFF
and struct.unpack('<BB', pk.data[0:2]) == (target_id, 0x12)):
m = pk.datat[2:]
if (len(m) % 2) != 0:
raise Exception('Malformed flash mapping packet')
self.mapping = []
page = 0
for i in range(int(len(m) / 2)):
for j in range(m[2 * i]):
self.mapping.append(page)
page += m[(2 * i) + 1]
def create(self):
"""Save the log configuration in the Espdrone"""
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
if self.useV2:
pk.data = (CMD_CREATE_BLOCK_V2, self.id)
else:
pk.data = (CMD_CREATE_BLOCK, self.id)
for var in self.variables:
if (var.is_toc_variable() is False): # Memory location
logger.debug('Logging to raw memory %d, 0x%04X',
var.get_storage_and_fetch_byte(), var.address)
pk.data.append(
struct.pack('<B', var.get_storage_and_fetch_byte()))
pk.data.append(struct.pack('<I', var.address))
else: # Item in TOC
logger.debug('Adding %s with id=%d and type=0x%02X', var.name,
self.ed.log.toc.get_element_id(var.name),
var.get_storage_and_fetch_byte())
pk.data.append(var.get_storage_and_fetch_byte())
if self.useV2:
ident = self.ed.log.toc.get_element_id(var.name)
pk.data.append(ident & 0x0ff)
pk.data.append((ident >> 8) & 0x0ff)
else:
pk.data.append(self.ed.log.toc.get_element_id(var.name))
logger.debug('Adding log block id {}'.format(self.id))
if self.useV2:
self.ed.send_packet(pk,
expected_reply=(CMD_CREATE_BLOCK_V2, self.id))
else:
self.ed.send_packet(pk, expected_reply=(CMD_CREATE_BLOCK, self.id))
