def create(self):
"""Save the log configuration in the Crazyflie"""
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.cf.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.cf.log.toc.get_element_id(var.name)
pk.data.append(ident & 0x0ff)
pk.data.append((ident >> 8) & 0x0ff)
else:
pk.data.append(self.cf.log.toc.get_element_id(var.name))
logger.debug('Adding log block id {}'.format(self.id))
if self.useV2:
self.cf.send_packet(pk,
expected_reply=(CMD_CREATE_BLOCK_V2, self.id))
else:
self.cf.send_packet(pk, expected_reply=(CMD_CREATE_BLOCK, self.id))
def _transmitter_thread(self):
sensor_request_pk = CRTPPacket()
sensor_request_pk.port = CRTPPort.SENSORS
control_input_pk = CRTPPacket()
control_input_pk.port = CRTPPort.OFFBOARDCTRL
vicon_yaw = 0.0
if SE_LISTEN_TO_VICON:
use_vicon_yaw = 1
else:
use_vicon_yaw = 0
imu_lc = lcm.LCM()
while True:
#t0 = time.time()
sensor_request_pk.data = struct.pack('<fi', vicon_yaw,
use_vicon_yaw)
sensor_request_dataout = self._pk_to_dataout(sensor_request_pk)
datain = self._write_read_usb(sensor_request_dataout)
sensor_packet = self._datain_to_pk(datain)
if not sensor_packet:
continue
try:
imu_reading = struct.unpack('<7f', sensor_packet.data)
except:
continue
self._state_estimator.add_imu_reading(imu_reading)
# msg = crazyflie_imu_t()
# msg.omegax = imu_reading[0]
# msg.omegay = imu_reading[1]
# msg.omegaz = imu_reading[2]
# msg.alphax = imu_reading[3]
# msg.alphay = imu_reading[4]
# msg.alphaz = imu_reading[5]
# imu_lc.publish('crazyflie_imu', msg.encode())
self._control_input_updated_flag.clear()
xhat = self._state_estimator.get_xhat()
vicon_yaw = xhat[5]
if self._use_drake_controller:
# wait for Drake to give us the control input
self._control_input_updated_flag.wait(0.005)
control_input = self._controller.get_control_input(xhat=xhat)
if self._use_pos_control:
control_input_pk.data = struct.pack('<7f', *control_input)
else:
control_input_pk.data = struct.pack('<5fi', *control_input)
control_input_dataout = self._pk_to_dataout(control_input_pk)
self._write_usb(control_input_dataout)
if not (self._use_pos_control):
# TODO: position control could still update the state
# estimator about the last input sent
self._state_estimator.add_input(control_input[0:4])
def set_value(self, complete_name, value):
"""
Set the value for the supplied parameter.
"""
if not self._initialized.wait(timeout=60):
raise Exception('Connection timed out')
element = self.toc.get_element_by_complete_name(complete_name)
if not element:
logger.warning("Cannot set value for [%s], it's not in the TOC!",
complete_name)
raise KeyError('{} not in param TOC'.format(complete_name))
elif element.access == ParamTocElement.RO_ACCESS:
logger.debug('[%s] is read only, no trying to set value',
complete_name)
raise AttributeError('{} is read-only!'.format(complete_name))
else:
varid = element.ident
pk = CRTPPacket()
pk.set_header(CRTPPort.PARAM, WRITE_CHANNEL)
if self._useV2:
pk.data = struct.pack('<H', varid)
else:
pk.data = struct.pack('<B', varid)
try:
value_nr = eval(value)
except TypeError:
value_nr = value
pk.data += struct.pack(element.pytype, value_nr)
self.param_updater.request_param_setvalue(pk)
def create(self):
"""Save the log configuration in the Crazyflie"""
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.cf.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.cf.log.toc.get_element_id(var.name)
pk.data.append(ident & 0x0ff)
pk.data.append((ident >> 8) & 0x0ff)
else:
pk.data.append(self.cf.log.toc.get_element_id(var.name))
logger.debug('Adding log block id {}'.format(self.id))
if self.useV2:
self.cf.send_packet(pk, expected_reply=(
CMD_CREATE_BLOCK_V2, self.id))
else:
self.cf.send_packet(pk, expected_reply=(CMD_CREATE_BLOCK, self.id))
def start(self):
"""Start the logging for this entry"""
if (self.cf.link is not None):
if (self._added is False):
logger.debug("First time block is started, add block")
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
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 += struct.pack('<B', var.get_storage_and_fetch_byte())
pk.data += struct.pack('<I', var.address)
else: # Item in TOC
logger.debug("Adding %s with id=%d and type=0x%02X",
var.name,
self.cf.log._toc.get_element_id(
var.name), var.get_storage_and_fetch_byte())
pk.data += struct.pack('<B', var.get_storage_and_fetch_byte())
pk.data += struct.pack('<B', self.cf.log._toc.
get_element_id(var.name))
logger.debug("Adding log block id {}".format(self.id))
self.cf.send_packet(pk, expected_reply=(CMD_CREATE_BLOCK, self.id))
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.cf.send_packet(pk, expected_reply=(CMD_START_LOGGING, self.id))
def set_value(self, complete_name, value):
"""
Set the value for the supplied parameter.
"""
element = self.toc.get_element_by_complete_name(complete_name)
if not element:
logger.warning("Cannot set value for [%s], it's not in the TOC!",
complete_name)
raise KeyError('{} not in param TOC'.format(complete_name))
elif element.access == ParamTocElement.RO_ACCESS:
logger.debug('[%s] is read only, no trying to set value',
complete_name)
raise AttributeError('{} is read-only!'.format(complete_name))
else:
varid = element.ident
pk = CRTPPacket()
pk.set_header(CRTPPort.PARAM, WRITE_CHANNEL)
if self._useV2:
pk.data = struct.pack('<H', varid)
else:
pk.data = struct.pack('<B', varid)
try:
value_nr = eval(value)
except TypeError:
value_nr = value
pk.data += struct.pack(element.pytype, value_nr)
self.param_updater.request_param_setvalue(pk)
def reset_to_firmware(self, target_id):
""" Reset to firmware
The parameter cpuid shall correspond to the device to reset.
Return true if the reset has been done
"""
fake_cpu_id = (1, 2, 4, 5, 6, 7, 8, 9, 10, 11, 12)
#Send the reset to bootloader request
pk = CRTPPacket()
pk.set_header(0xFF, 0xFF)
pk.data = (target_id, 0xFF) + fake_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.header == 0xFF and
struct.unpack("B" * len(pk.data),
pk.data)[:2] == (target_id, 0xFF)):
pk.data = (target_id, 0xF0, 0x01)
self.link.send_packet(pk)
break
time.sleep(0.1)
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)
pk = self.link.receive_packet(1)
while ((not pk or pk.header != 0xFF or
struct.unpack("<BB", pk.data[0:2]) != (target_id, 0xFF)) and
retry_counter >= 0 ):
pk = self.link.receive_packet(1)
retry_counter -= 1
if pk:
new_address = (0xb1, ) + struct.unpack("<BBBB", pk.data[2:6][::-1])
pk = CRTPPacket()
pk.set_header(0xFF, 0xFF)
pk.data = (target_id, 0xF0, 0x00)
self.link.send_packet(pk)
addr = int(struct.pack("B"*5, *new_address).encode('hex'), 16)
time.sleep(0.2)
self.link.close()
time.sleep(0.2)
self.link = cflib.crtp.get_link_driver("radio://0/0/2M/{}".format(addr))
return True
else:
return False
def start(self):
if (self.cf.link is not None):
if (self.blockCreated is False):
logger.debug("First time block is started, add block")
self.blockCreated = True
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
pk.data = (CMD_CREATE_BLOCK, self.blockId)
for v in self.logconf.getVariables():
if (v.isTocVariable() is False): # Memory location
logger.debug("Logging to raw memory %d, 0x%04X",
v.getStoredFetchAs(), v.getAddress())
pk.data += struct.pack('<B', v.getStoredFetchAs())
pk.data += struct.pack('<I', v.getAddress())
else: # Item in TOC
logger.debug("Adding %s with id=%d and type=0x%02X",
v.getName(),
self.cf.log.toc.get_element_id(
v.getName()), v.getStoredFetchAs())
pk.data += struct.pack('<B', v.getStoredFetchAs())
pk.data += struct.pack('<B', self.cf.log.toc.
get_element_id(v.getName()))
logger.debug("Adding log block id {}".format(self.blockId))
self.cf.send_packet(pk)
else:
logger.debug("Block already registered, starting logging"
" for %d", self.blockId)
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
pk.data = (CMD_START_LOGGING, self.blockId, self.period)
self.cf.send_packet(pk)
def start(self):
"""Start the logging for this entry"""
if (self.cf.link is not None):
if (self._added is False):
logger.debug("First time block is started, add block")
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
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 += struct.pack('<B', var.get_storage_and_fetch_byte())
pk.data += struct.pack('<I', var.address)
else: # Item in TOC
logger.debug("Adding %s with id=%d and type=0x%02X",
var.name,
self.cf.log.toc.get_element_id(
var.name), var.get_storage_and_fetch_byte())
pk.data += struct.pack('<B', var.get_storage_and_fetch_byte())
pk.data += struct.pack('<B', self.cf.log.toc.
get_element_id(var.name))
logger.debug("Adding log block id {}".format(self.id))
self.cf.send_packet(pk, expect_answer=True)
else:
logger.debug("Block already registered, starting logging"
" for %d", self.id)
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
pk.data = (CMD_START_LOGGING, self.id, self.period)
self.cf.send_packet(pk, expect_answer=True)
def start(self):
"""Start the logging for this entry"""
if (self.cf.link is not None):
if (self.block_created is False):
logger.debug("First time block is started, add block")
self.block_created = True
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
pk.data = (CMD_CREATE_BLOCK, self.block_id)
for var in self.logconf.getVariables():
if (var.isTocVariable() is False): # Memory location
logger.debug("Logging to raw memory %d, 0x%04X",
var.getStoredFetchAs(), var.getAddress())
pk.data += struct.pack('<B', var.getStoredFetchAs())
pk.data += struct.pack('<I', var.getAddress())
else: # Item in TOC
logger.debug("Adding %s with id=%d and type=0x%02X",
var.getName(),
self.cf.log.toc.get_element_id(
var.getName()), var.getStoredFetchAs())
pk.data += struct.pack('<B', var.getStoredFetchAs())
pk.data += struct.pack('<B', self.cf.log.toc.
get_element_id(var.getName()))
logger.debug("Adding log block id {}".format(self.block_id))
self.cf.send_packet(pk)
else:
logger.debug("Block already registered, starting logging"
" for %d", self.block_id)
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
pk.data = (CMD_START_LOGGING, self.block_id, self.period)
self.cf.send_packet(pk)
def upload_buffer(self, target_id, page, address, buff):
"""Upload data into a buffer on the Crazyflie"""
#print len(buff)
count = 0
pk = CRTPPacket()
pk.set_header(0xFF, 0xFF)
pk.data = struct.pack("=BBHH", target_id, 0x14, page, address)
for i in range(0, len(buff)):
#print "[0x%02X]" % ord(buff[i]),
pk.data += buff[i]
count += 1
if count > 24:
self.link.send_packet(pk)
count = 0
pk = CRTPPacket()
pk.set_header(0xFF, 0xFF)
pk.data = struct.pack("=BBHH", target_id, 0x14, page,
i + address + 1)
#sys.stdout.write("+")
#sys.stdout.flush()
self.link.send_packet(pk)
def _transmitter_thread(self):
sensor_request_pk = CRTPPacket()
sensor_request_pk.port = CRTPPort.SENSORS
control_input_pk = CRTPPacket()
control_input_pk.port = CRTPPort.OFFBOARDCTRL
vicon_yaw = 0.0
if SE_LISTEN_TO_VICON:
use_vicon_yaw = 1
else:
use_vicon_yaw = 0
imu_lc = lcm.LCM()
while True:
#t0 = time.time()
sensor_request_pk.data = struct.pack('<fi',vicon_yaw,use_vicon_yaw)
sensor_request_dataout = self._pk_to_dataout(sensor_request_pk)
datain = self._write_read_usb(sensor_request_dataout)
sensor_packet = self._datain_to_pk(datain)
if not sensor_packet:
continue
try:
imu_reading = struct.unpack('<7f',sensor_packet.data)
except:
continue
self._state_estimator.add_imu_reading(imu_reading)
# msg = crazyflie_imu_t()
# msg.omegax = imu_reading[0]
# msg.omegay = imu_reading[1]
# msg.omegaz = imu_reading[2]
# msg.alphax = imu_reading[3]
# msg.alphay = imu_reading[4]
# msg.alphaz = imu_reading[5]
# imu_lc.publish('crazyflie_imu', msg.encode())
self._control_input_updated_flag.clear()
xhat = self._state_estimator.get_xhat()
vicon_yaw = xhat[5]
if self._use_drake_controller:
# wait for Drake to give us the control input
self._control_input_updated_flag.wait(0.005)
control_input = self._controller.get_control_input(xhat=xhat)
if self._use_pos_control:
control_input_pk.data = struct.pack('<7f',*control_input)
else:
control_input_pk.data = struct.pack('<5fi',*control_input)
control_input_dataout = self._pk_to_dataout(control_input_pk)
self._write_usb(control_input_dataout)
if not(self._use_pos_control):
# TODO: position control could still update the state
# estimator about the last input sent
self._state_estimator.add_input(control_input[0:4])
def GetVBat():
cr = crazyradio.Crazyradio()
cr.set_channel(0)
cr.set_data_rate(cr.DR_2MPS)
pk = CRTPPacket()
# Create the log block
pk.set_header(CRTPPort.LOGGING, log.CHAN_SETTINGS)
pk.data = ([log.CMD_CREATE_BLOCK, 0x01, 0x07, 42])
packet = bytearray([pk.header]) + pk.data
retry = True
while retry:
res = cr.send_packet(packet)
if res.ack:
if len(res.data) >= 2:
if res.data[0] | 0x3 << 2 == pk.header:
if res.data[1] == log.CMD_CREATE_BLOCK:
retry = False
print("Log created with return value 0x{:02x}".format(res.data[2]))
# Start the log block
pk.set_header(CRTPPort.LOGGING, log.CHAN_SETTINGS)
pk.data = ([log.CMD_START_LOGGING, 0x01, 50])
packet = bytearray([pk.header]) + pk.data
retry = True
while retry:
res = cr.send_packet(packet)
if res.ack:
if len(res.data) >= 2:
if res.data[0] | 0x3 << 2 == pk.header:
if res.data[1] == log.CMD_START_LOGGING:
retry = False
print("Log started with return value 0x{:02x}".format(res.data[2]))
# Now loop forever sending empty CRTP packets, waiting for log data in the ACK.
pk.set_header(CRTPPort.COMMANDER, 0)
roll = 0.0
pitch = 0.0
yaw = 0.0
thrust = 0
pk.data = bytearray(struct.pack("f", roll))
pk.data += bytearray(struct.pack("f", pitch))
pk.data += bytearray(struct.pack("f", yaw))
pk.data += bytearray(struct.pack("H", thrust))
packet = bytearray([pk.header]) + pk.data
logPacket = CRTPPacket()
logPacket.set_header(CRTPPort.LOGGING, log.CHAN_LOGDATA)
while True:
res = cr.send_packet(packet)
if res.ack:
if len(res.data) >= 2:
if res.data[0] | 0x3 << 2 == logPacket.header:
if res.data[1] == 0x01:
print(struct.unpack("<f", res.data[5:])[0])
def request_param_update(self, var_id):
"""Place a param update request on the queue"""
self._useV2 = self.cf.platform.get_protocol_version() >= 4
pk = CRTPPacket()
pk.set_header(CRTPPort.PARAM, READ_CHANNEL)
if self._useV2:
pk.data = struct.pack('<H', var_id)
else:
pk.data = struct.pack('<B', var_id)
logger.debug('Requesting request to update param [%d]', var_id)
self.request_queue.put(pk)
def request_param_update(self, var_id):
"""Place a param update request on the queue"""
self._useV2 = self.cf.platform.get_protocol_version() >= 4
pk = CRTPPacket()
pk.set_header(CRTPPort.PARAM, READ_CHANNEL)
if self._useV2:
pk.data = struct.pack('<H', var_id)
else:
pk.data = struct.pack('<B', var_id)
logger.debug('Requesting request to update param [%d]', var_id)
self.request_queue.put(pk)
def DownloadTOC():
cr = crazyradio.Crazyradio()
cr.set_channel(0)
cr.set_data_rate(cr.DR_2MPS)
pk = CRTPPacket()
pk.set_header(CRTPPort.LOGGING, log.CHAN_TOC)
pk.data = ([log.CMD_TOC_INFO])
packet = bytearray([pk.header]) + pk.data
retry = True
while retry:
print("Sending TOC info request...")
res = cr.send_packet(packet)
if res.ack:
print("ACK received")
if len(res.data) >= 2:
if res.data[0] | 0x3 << 2 == pk.header:
if res.data[1] == log.CMD_TOC_INFO:
retry = False
print("Received TOC info!")
print("Entire ACK: " + "".join("0x%02x " % b for b in res.data))
tocSize = res.data[2]
#tocChkSum = res.data[3:6]
tocMaxPacket = res.data[7]
tocMaxOps = res.data[8]
print("TOC size={:d}".format(tocSize))
#print("TOC chksum={:d}".format(tocChkSum))
print("TOC MaxPacket={:d}".format(tocMaxPacket))
print("TOC MaxOps={:d}".format(tocMaxOps))
for i in range(0, tocSize):
pk.data = ([log.CMD_TOC_ELEMENT, i])
packet = bytearray([pk.header]) + pk.data
retry = True
while retry:
#print("Sending TOC Element request for {:d}".format(i))
res = cr.send_packet(packet)
if res.ack:
if len(res.data) >= 2:
if res.data[0] | 0x3 << 2 == pk.header:
if res.data[1] == log.CMD_TOC_ELEMENT:
if res.data[2] == i:
retry = False
strs = bytes.decode(bytes(res.data[4:])).split("\0")
print("Element {:d} of type {:d} is called \"{:s}\" of group \"{:s}\"".
format(res.data[2], res.data[3], strs[1], strs[0]))
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.cf.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.cf.send_packet(pk, expected_reply=(CMD_TOC_ELEMENT, index))
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.cf.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.cf.send_packet(pk, expected_reply=(CMD_TOC_ELEMENT, index))
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 crazyflie firmware that prevent
#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 = cflib.crtp.get_link_driver(self.clink_address)
#time.sleep(0.1)
return self._update_info()
def request_param_update(self, var_id):
"""Place a param update request on the queue"""
pk = CRTPPacket()
pk.set_header(CRTPPort.PARAM, READ_CHANNEL)
pk.data = struct.pack('<B', var_id)
logger.debug('Requesting request to update param [%d]', var_id)
self.request_queue.put(pk)
def _update_info(self):
""" Call the command getInfo and fill up the information received in
the fields of the object
"""
#Call getInfo ...
pk = CRTPPacket()
pk.set_header(0xFF, 0xFF)
pk.data = (0xFF, 0x10)
self.link.send_packet(pk)
#Wait for the answer
pk = self.link.receive_packet(2)
if (pk and pk.header == 0xFF and
struct.unpack("<BB", pk.data[0:2]) == (0xFF, 0x10)):
tab = struct.unpack("BBHHHH", pk.data[0:10])
cpuid = struct.unpack("B" * 12, pk.data[10:22])
if len(pk.data) > 22:
self.protocol_version = pk.data[22]
self.page_size = tab[2]
self.buffer_pages = tab[3]
self.flash_pages = tab[4]
self.start_page = tab[5]
self.cpuid = "%02X" % cpuid[0]
for i in cpuid[1:]:
self.cpuid += ":%02X" % i
return True
return False
def send_setpoint(self, roll, pitch, yaw, thrust, hover=False, set_hover=False, hover_change = 0.0):
#print "roll: ", roll, "; pitch: ", roll, "; yaw: ", yaw, "; thrust: ", thrust
"""
Send a new control setpoint for roll/pitch/yaw/thust to the copter
The arguments roll/pitch/yaw/thrust is the new setpoints that should
be sent to the copter. Hover = True enabled hover mode for as long as it is true
"""
if self._x_mode:
roll = 0.707*(roll-pitch)
pitch = 0.707*(roll+pitch)
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER
if set_hover or not hover:
hover_change = 0.0
pk.data = struct.pack('<fffH??f', roll, -pitch, yaw, thrust, hover, set_hover, hover_change)
self._cf.send_packet(pk)
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()
pk.set_header(self.port, TOC_CHANNEL)
pk.data = (CMD_TOC_ELEMENT, index)
self.cf.send_packet(pk, expect_answer=True)
def send_setpoint(self, roll, pitch, yaw, thrust):
"""
Send a new control setpoint for roll/pitch/yaw/thust to the copter
The arguments roll/pitch/yaw/trust is the new setpoints that should
be sent to the copter
"""
if self.pointerYaw is not None: #elif = else if
# roll = x, pitch = y, yaw = A, A >= 0
# x' = x*cos(A) - y*sin(A)
# y' = x*sin(A) + y*cos(A)
# A < 0
# x' = x*cos(A) + y*sin(A)
# y' = -x*sin(A) + y*cos(A)
currentYaw = self.pointerYaw
self._yaw = math.radians(currentYaw)
cosy = math.cos(self._yaw)
siny = math.sin(self._yaw)
#print "Roll: %3.3f -- Pitch: %3.3f -- Yaw: %3.3f" % (self._actualPoint["stabilizer.roll"], self._actualPoint["stabilizer.pitch"], currentYaw)
#print "Degree Yaw: %3.3f -- Radians Yaw: %3.3f" % (currentYaw, self._yaw)
roll1 = roll
#if self._yaw >= 0:
# roll = roll*cosy - pitch*siny
# pitch = roll1*siny + pitch*cosy
#else:
roll = roll * cosy + pitch * siny
pitch = pitch * cosy - roll1 * siny
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER
pk.data = struct.pack('<fffH', roll, -pitch, yaw, thrust)
self._cf.send_packet(pk)
def _request_param_update(self, varid):
"""Send a request to the Crazyflie for an update of a param value"""
logger.debug("Requesting update for varid %d", varid)
pk = CRTPPacket()
pk.set_header(CRTPPort.PARAM, READ_CHANNEL)
pk.data = struct.pack('<B', varid)
self.cf.send_packet(pk, expect_answer=True)
def _update_info(self):
""" Call the command getInfo and fill up the information received in
the fields of the object
"""
#Call getInfo ...
pk = CRTPPacket()
pk.set_header(0xFF, 0xFF)
pk.data = (0xFF, 0x10)
self.link.send_packet(pk)
#Wait for the answer
pk = self.link.receive_packet(2)
if (pk and pk.header == 0xFF and
struct.unpack("<BB", pk.data[0:2]) == (0xFF, 0x10)):
tab = struct.unpack("BBHHHH", pk.data[0:10])
cpuid = struct.unpack("B"*12, pk.data[10:22])
if len(pk.data)>22:
self.protocol_version = pk.data[22]
self.page_size = tab[2]
self.buffer_pages = tab[3]
self.flash_pages = tab[4]
self.start_page = tab[5]
self.cpuid = "%02X" % cpuid[0]
for i in cpuid[1:]:
self.cpuid += ":%02X" % i
return True
return False
def batch_read_flash(self, addr=0xFF, page=0x00):
"""Read back a flash page from the Crazyflie and return it"""
page_size = self.targets[addr].page_size
batch_index = []
for i in range(0, int(math.ceil(page_size / 25.0))):
batch_index.append(1)
buff = bytearray(len(batch_index) * 25)
while not sum(batch_index) == 0:
for index in range(len(batch_index)):
if batch_index[index] == 1:
pk = CRTPPacket()
pk.set_header(0xFF, 0xFF)
pk.data = struct.pack('<BBHH', addr, 0x1C, page,
(index * 25))
self.link.send_packet(pk)
pk = self.link.receive_packet(0)
if pk and len(pk.data) >= 6:
temp_pk = struct.unpack('<BBHH', pk.data[0:6])
back_element = int(temp_pk[3])
if list(temp_pk[:2]) == [addr, 0x1C]:
buff[back_element:back_element + 25] = pk.data[6:]
batch_index[int(back_element / 25)] = 0
return buff[0:page_size]
def _request_param_update(self, varid):
"""Send a request to the Crazyflie for an update of a param value"""
logger.debug("Requesting update for varid %d", varid)
pk = CRTPPacket()
pk.set_header(CRTPPort.PARAM, READ_CHANNEL)
pk.data = struct.pack('<B', varid)
self.cf.send_packet(pk, expect_answer=True)
def read_flash(self, addr=0xFF, page=0x00):
"""Read back a flash page from the Crazyflie 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 _write_new_chunk(self):
"""
Called to request a new chunk of data to be read from the Crazyflie
"""
# 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.info('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.cf.send_packet(pk, expected_reply=reply, timeout=1)
self._addr_add = len(data)
def send_synchronisation(self):
if self.cf.state == State.CONNECTED:
pk = CRTPPacket()
pk.port = CRTPPort.SYNC
t = rospy.Time.now().to_nsec()
pk.data = struct.pack('<Q',t) # timestamp in m
self.cf.send_packet(pk)
def _request_protocol_version(self):
# Sending a sink request to detect if the connected Crazyflie
# supports protocol versioning
pk = CRTPPacket()
pk.set_header(CRTPPort.LINKCTRL, LINKSERVICE_SOURCE)
pk.data = (0,)
self._cf.send_packet(pk)
def send_synchronisation(self):
if self.cf.state == State.CONNECTED:
pk = CRTPPacket()
pk.port = CRTPPort.SYNC
t = rospy.Time.now().to_nsec()
pk.data = struct.pack('<Q', t) # timestamp in m
self.cf.send_packet(pk)
def _write_new_chunk(self):
"""
Called to request a new chunk of data to be read from the Crazyflie
"""
# 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.info('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.cf.send_packet(pk, expected_reply=reply, timeout=1)
self._addr_add = len(data)
def persistent_store(self, complete_name, callback=None):
"""
Store the current value of the specified persistent parameter to
eeprom. The supplied callback will be called with `True` as an
argument on success, and with `False` as an argument on failure.
@param complete_name The 'group.name' name of the parameter to store
@param callback Optional callback should take `complete_name` and boolean status as arguments
"""
element = self.toc.get_element_by_complete_name(complete_name)
if not element.is_persistent():
raise AttributeError(f"Param '{complete_name}' is not persistent")
def new_packet_cb(pk):
if pk.channel == MISC_CHANNEL and pk.data[0] == MISC_PERSISTENT_STORE:
callback(complete_name, pk.data[3] == 0)
self.cf.remove_port_callback(CRTPPort.PARAM, new_packet_cb)
if callback is not None:
self.cf.add_port_callback(CRTPPort.PARAM, new_packet_cb)
pk = CRTPPacket()
pk.set_header(CRTPPort.PARAM, MISC_CHANNEL)
pk.data = struct.pack('<BH', MISC_PERSISTENT_STORE, element.ident)
self.param_updater.send_param_misc(pk)
def get_default_value(self, complete_name, callback):
"""
Get the default value of the specified parameter.
The supplied callback will be called with the name of the parameter
as well as the default value. None if there is an error.
@param complete_name The 'group.name' name of the parameter to store
@param callback The callback should take `complete_name` and default value as argument
"""
element = self.toc.get_element_by_complete_name(complete_name)
def new_packet_cb(pk):
if pk.channel == MISC_CHANNEL and pk.data[0] == MISC_GET_DEFAULT_VALUE:
if pk.data[3] == errno.ENOENT:
callback(complete_name, None)
self.cf.remove_port_callback(CRTPPort.PARAM, new_packet_cb)
return
default_value, = struct.unpack(element.pytype, pk.data[3:])
callback(complete_name, default_value)
self.cf.remove_port_callback(CRTPPort.PARAM, new_packet_cb)
self.cf.add_port_callback(CRTPPort.PARAM, new_packet_cb)
pk = CRTPPacket()
pk.set_header(CRTPPort.PARAM, MISC_CHANNEL)
pk.data = struct.pack('<BH', MISC_GET_DEFAULT_VALUE, element.ident)
self.param_updater.send_param_misc(pk)
def _ramp_motors(self):
thrust_mult = 1
thrust_step = 100
thrust_base = 3000
thrust = thrust_base
pitch = 0
roll = 0
yawrate = 0
# Unlock startup thrust protection
pk = CRTPPacket()
pk.set_header(0xF, 2) # unlock drone
pk.data = '0'
self._cf.send_packet(pk)
while thrust >= thrust_base:
self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
time.sleep(0.1)
if thrust >= thrust_base + 1000:
thrust_mult = -1
thrust += thrust_step * thrust_mult
# Make sure that the last packet leaves before the link is closed
# since the message queue is not flushed before closing
self._cf.commander.send_setpoint(0, 0, 0, 0)
time.sleep(0.2)
self._cf.close_link()
def latency(self, uri, packet_size=4, count=500):
link = cflib.crtp.get_link_driver(uri)
# # wait until no more packets in queue
# while True:
# pk = link.receive_packet(0.5)
# print(pk)
# if not pk or pk.header == 0xFF:
# break
pk = CRTPPacket()
pk.set_header(CRTPPort.LINKCTRL, 0) # Echo channel
latencies = []
for i in range(count):
pk.data = self.build_data(i, packet_size)
start_time = time.time()
link.send_packet(pk)
while True:
pk_ack = link.receive_packet(-1)
if pk_ack.port == CRTPPort.LINKCTRL and pk_ack.channel == 0:
break
end_time = time.time()
# make sure we actually received the expected value
i_recv, = struct.unpack('<I', pk_ack.data[0:4])
self.assertEqual(i, i_recv)
latencies.append((end_time - start_time) * 1000)
link.close()
result = np.min(latencies)
print('Latency for {} (packet size {} B): {:.2f} ms'.format(uri, packet_size, result))
return result
def bandwidth(self, uri, packet_size=4, count=500):
link = cflib.crtp.get_link_driver(uri, link_error_callback=self.error_cb)
# # wait until no more packets in queue
# while True:
# pk = link.receive_packet(0.5)
# if not pk:
# break
# enqueue packets
start_time = time.time()
for i in range(count):
pk = CRTPPacket()
pk.set_header(CRTPPort.LINKCTRL, 0) # Echo channel
pk.data = self.build_data(i, packet_size)
link.send_packet(pk)
# get the result
for i in range(count):
while True:
pk_ack = link.receive_packet(-1)
if pk_ack.port == CRTPPort.LINKCTRL and pk_ack.channel == 0:
break
# make sure we actually received the expected value
i_recv, = struct.unpack('<I', pk_ack.data[0:4])
self.assertEqual(i, i_recv)
end_time = time.time()
link.close()
result = count / (end_time - start_time)
kbps = (count * packet_size) / 1024 / (end_time - start_time)
print('Bandwith for {} (packet size {} B): {:.2f} packets/s ({:.2f} kB/s)'.format(
uri, packet_size, result, kbps))
return result
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()
pk.set_header(self.port, TOC_CHANNEL)
pk.data = (CMD_TOC_ELEMENT, index)
self.cf.send_packet(pk, expected_reply=(CMD_TOC_ELEMENT, index))
def read_flash(self, addr=0xFF, page=0x00):
"""Read back a flash page from the Crazyflie 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 write_flash(self, addr, page_buffer, target_page, page_count):
"""Initate flashing of data in the buffer to flash."""
#print "Write page", flashPage
#print "Writing page [%d] and [%d] forward" % (flashPage, nPage)
pk = None
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 = ord(pk.data[3])
return ord(pk.data[2]) == 1
def request_param_update(self, var_id):
"""Place a param update request on the queue"""
pk = CRTPPacket()
pk.set_header(CRTPPort.PARAM, READ_CHANNEL)
pk.data = struct.pack('<B', var_id)
logger.debug("Requesting request to update param [%d]", var_id)
self.request_queue.put(pk)
def send_lh_persist_data_packet(self, geo_list, calib_list):
"""
Send geometry and calibration data to persistent memory subsystem
"""
geo_list.sort()
calib_list.sort()
max_bs_nr = 15
if len(geo_list) > 0:
if geo_list[0] < 0 or geo_list[-1] > max_bs_nr:
raise Exception('Geometry BS list is not valid')
if len(calib_list) > 0:
if calib_list[0] < 0 or calib_list[-1] > max_bs_nr:
raise Exception('Calibration BS list is not valid')
mask_geo = 0
mask_calib = 0
for bs in geo_list:
mask_geo += 1 << bs
for bs in calib_list:
mask_calib += 1 << bs
pk = CRTPPacket()
pk.port = CRTPPort.LOCALIZATION
pk.channel = self.GENERIC_CH
pk.data = struct.pack('<BHH', self.LH_PERSIST_DATA, mask_geo,
mask_calib)
self._cf.send_packet(pk)
return pk.data
def _new_packet_cb(self, packet):
"""Callback for newly arrived packets with TOC information"""
chan = packet.channel
cmd = packet.datal[0]
payload = struct.pack("B" * (len(packet.datal) - 1), *packet.datal[1:])
if (chan == CHAN_SETTINGS):
block_id = ord(payload[0])
error_status = ord(payload[1])
block = self._find_block(block_id)
if (cmd == CMD_CREATE_BLOCK):
if (block is not None):
if (error_status == 0): # No error
logger.debug("Have successfully added block_id=%d",
block_id)
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
pk.data = (CMD_START_LOGGING, block_id,
block.period)
self.cf.send_packet(pk)
block.added = True
else:
msg = self._err_codes[error_status]
logger.warning("Error %d when adding block_id=%d (%s)"
, error_status, block_id, msg)
block.err_no = error_status
block.added_cb.call(False)
block.error.call(block.logconf, msg)
else:
logger.warning("No LogEntry to assign block to !!!")
if (cmd == CMD_START_LOGGING):
if (error_status == 0x00):
logger.info("Have successfully logging for block=%d",
block_id)
if block:
block.started = True
else:
msg = self._err_codes[error_status]
logger.warning("Error %d when starting block_id=%d (%s)"
, error_status, new_block_id, msg)
if block:
block.err_no = error_status
block.started_cb.call(False)
block.error.call(block.logconf, msg)
if (chan == CHAN_LOGDATA):
chan = packet.channel
block_id = packet.datal[0]
block = self._find_block(block_id)
timestamps = struct.unpack("<BBB", packet.data[1:4])
timestamp = (timestamps[0] | timestamps[1] << 8 | timestamps[2] << 16)
logdata = packet.data[4:]
if (block is not None):
block.unpack_log_data(logdata, timestamp)
else:
logger.warning("Error no LogEntry to handle block=%d", block_id)
def assist_now(self):
"""Callback from assistnow button"""
# print "Hello World"
if (self.helper.cf.link != None):
pk = CRTPPacket()
pk.port = CRTPPort.ABLOCK
pk.data = "<S>\n"
self.helper.cf.send_packet(pk)
def send_stop_setpoint(self):
"""
Send STOP setpoing, stopping the motors and (potentially) falling.
"""
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER_GENERIC
pk.data = struct.pack('<B', TYPE_STOP)
self._cf.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 Crazyflie.
"""
pk = CRTPPacket()
pk.set_header(CRTPPort.PLATFORM, 0)
pk.data = (0, enabled)
self._cf.send_packet(pk)
def stop(self):
if (self.cf.link is not None):
if (self.blockId is None):
logger.warning("Stopping block, but no block registered")
else:
logger.debug("Sending stop logging for block %d", self.blockId)
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
pk.data = (CMD_STOP_LOGGING, self.blockId)
self.cf.send_packet(pk)
def refresh_toc(self, refreshDoneCallback, toc_cache):
pk = CRTPPacket()
pk.set_header(CRTPPort.LOGGING, CHAN_SETTINGS)
pk.data = (CMD_RESET_LOGGING, )
self.cf.send_packet(pk)
self.toc = Toc()
tocFetcher = TocFetcher(self.cf, LogTocElement, CRTPPort.LOGGING,
self.toc, refreshDoneCallback, toc_cache)
tocFetcher.start()
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._cf.send_packet(pk)
def stop(self):
"""Stop the logging for this entry"""
if (self.cf.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 %d", self.id)
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
pk.data = (CMD_STOP_LOGGING, self.id)
self.cf.send_packet(pk, expect_answer=True)
def send_extpos(self, pos):
"""
Send the current Crazyflie X, Y, Z position. This is going to be
forwarded to the Crazyflie'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._cf.send_packet(pk)
