def reset_to_bootloader(self, target_id: int) -> bool: pk = CRTPPacket(0xFF, [target_id, 0xFF]) self.link.send_packet(pk) address = None timeout = 5 # seconds ts = time.time() while time.time() - ts < timeout: pk = self.link.receive_packet(2) if pk is None: continue if pk.port == 15 and pk.channel == 3 and len(pk.data) > 3: if struct.unpack('<BB', pk.data[0:2]) != (target_id, 0xFF): continue address = 'B1' + binascii.hexlify( pk.data[2:6][::-1]).upper().decode('utf8') pk = CRTPPacket(0xFF, [target_id, 0xF0, 0x00]) self.link.send_packet(pk) time.sleep(0.5) self.link.close() self.link = cflib.crtp.get_link_driver( f'radio://0/0/2M/{address}?safelink=0') time.sleep(0.5) return True return False
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 reset_to_firmware(self, target_id: int) -> bool: """ Reset to firmware The parameter target_id corresponds to the device to reset. Return True if the reset has been done, False on timeout """ pk = CRTPPacket(0xFF, [target_id, 0xFF]) self.link.send_packet(pk) timeout = 5 # seconds ts = time.time() while time.time() - ts < timeout: answer = self.link.receive_packet(2) if answer is None: self.link.send_packet(pk) continue if answer.port == 15 and answer.channel == 3 and len( answer.data) > 2: if struct.unpack('<BB', pk.data[0:2]) != (target_id, 0xFF): continue pk = CRTPPacket(0xff, [target_id, 0xf0, 0x01]) self.link.send_packet(pk) time.sleep(1) return True time.sleep(0.1) return False
def handleNotification(self, cHandle, data): raw_packet = bytearray(data) header = Control_Byte(raw_packet=raw_packet) return_packet = None if (header.start): if header.length < 20: self.tempByteArray.clear() self.tempByteArray[0:header.length - 1] = raw_packet[1:] return_packet = CRTPPacket(header=self.tempByteArray[0], data=self.tempByteArray[1:]) self.in_queue.put(return_packet) else: self.tempByteArray[0:header.length - 1] = raw_packet[1:] self.temp_pid = header.pid self.temp_length = header.length else: if header.pid == self.temp_pid: self.tempByteArray[19:header.length - 1] = raw_packet[1:] return_packet = CRTPPacket(header=self.tempByteArray[0], data=self.tempByteArray[1:]) self.in_queue.put(return_packet) else: self.temp_pid = -1 self.temp_length = 0
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 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 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): """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 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 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 _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 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 _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 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 _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 _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 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 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 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 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 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 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 _send(self, cmd): if not self.link: packet = [0xf3, 0xfe, cmd] cr = RadioManager.open(devid=self.devid) cr.set_channel(self.channel) cr.set_data_rate(self.datarate) cr.set_address(self.address) cr.set_arc(3) success = False for i in range(50): res = cr.send_packet(packet) if res and res.ack: success = True break time.sleep(0.01) cr.close() if not success: raise Exception( 'Failed to connect to Crazyflie at {}'.format(self.uri)) else: # send command (will be repeated until acked) pk = CRTPPacket(0xFF, [0xfe, cmd]) self.link.send_packet(pk) # wait up to 1s pk = self.link.receive_packet(0.1) if pk is None: raise Exception( 'Failed to connect to Crazyflie at {}'.format(self.uri))
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 _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 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 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)