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 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 _send_to_commander_angle(self, cmd1, cmd2, cmd3, cmd4, roll, pitch, yaw): pk = CRTPPacket() pk.port = CRTPPort.COMMANDER_GENERIC pk.data = struct.pack('<BHHHHfff', CF_COMMAND_TYPE_ANGLE, cmd1, cmd2, cmd3, cmd4, roll, pitch, yaw) self._cf.send_packet(pk)
def _send_to_commander_rate(self, cmd1, cmd2, cmd3, cmd4, roll_rate, pitch_rate, yaw_rate): pk = CRTPPacket() pk.port = CRTPPort.COMMANDER_GENERIC pk.data = struct.pack('<BHHHHfff', CF_COMMAND_TYPE_RATE, cmd1, cmd2, cmd3, cmd4, roll_rate, pitch_rate, yaw_rate) self._cf.send_packet(pk)
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 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 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 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 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 _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 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 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_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 _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 _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 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 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 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 upload_buffer(self, 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", 0xFF, 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", 0xFF, 0x14, page, i + address + 1) #sys.stdout.write("+") #sys.stdout.flush() self.link.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, 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 _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 _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): """ 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 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 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 _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 send_bypass_setpoint(self, m1, m2, m3, m4): """ Control mode where the motor setpoints are sent directly (in form uint16) """ pk = CRTPPacket() pk.port = CRTPPort.COMMANDER_GENERIC pk.data = struct.pack('<Bffff', TYPE_BYPASS, m1, m2, m3, m4) self._cf.send_packet(pk)
def _connected(self, link_uri): print('Connected to %s' % link_uri) pk = CRTPPacket() pk.port = CRTPPort.LOCALIZATION pk.channel = self.POSITION_CH pk.data = struct.pack('<fff', 3.1, 3.2, 3.3) self._cf.send_packet(pk)
def request_extended_types(self, elements): self._count = len(elements) for element in elements: pk = CRTPPacket() pk.set_header(CRTPPort.PARAM, MISC_CHANNEL) pk.data = struct.pack('<BH', MISC_GET_EXTENDED_TYPE, element.ident) self.request_queue.put(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 _serial_listener(self): while self.is_connected: data = self._serial.read(size=29) if (data): pk = CRTPPacket() pk.port = CRTP_PORT_MICROROS pk.data = data self.link.send_packet(pk)
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_keep_alive(self): """ Keep the drone with previous control command which is saved in the microcontroller """ pk = CRTPPacket() pk.port = CRTPPort.COMMANDER_GENERIC pk.channel = self.KEEP_ALIVE_CH self._cf.send_packet(pk)
def send_height(self, vx, vy, yaw, height): if height > 2.0 or height < 0.0: raise ValueError('Height must be between 0.0 and 2.0m') pk = CRTPPacket() pk.port = CRTPPort.COMMANDER_POSHOLD pk.channel = 1 pk.data = struct.pack('<Bffff', TYPE_POSHOLD, vx, vy, yaw, height) 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, PLATFORM_COMMAND) pk.data = (0, enabled) 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 send_shoot(self, notused): if notused == 0: return sock.sendto(struct.pack("<LBB", 1, 2, 1), (SOUND_IP, SOUND_PORT)) print("Sending Shoot!") pk = CRTPPacket() pk.port = CRTPPort.SHOOT pk.data = struct.pack('<fffH', 1, 2, 3, 4) #"test"#struct.pack() 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 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 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)
def refresh_toc(self, refresh_done_callback, toc_cache): """Start refreshing the table of loggale variables""" 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.cf.send_packet(pk, expected_reply=(CMD_RESET_LOGGING,))
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 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 close(self): if (self.cf.link is not None): if (self.blockId is None): logger.warning("Delete block, but no block registered") else: logger.debug("LogEntry: Sending delete logging for block %d" % self.blockId) pk = CRTPPacket() pk.set_header(5, CHAN_SETTINGS) pk.data = (CMD_DELETE_BLOCK, self.blockId) self.cf.send_packet(pk) self.blockId = None # Wait until we get confirmation of delete
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 Crazflie). Roll, pitch, yawrate are defined as rad, rad, rad/s """ pk = CRTPPacket() pk.port = CRTPPort.COMMANDER_GENERIC pk.data = struct.pack('<Bffff', TYPE_ZDISTANCE, roll, pitch, yawrate, zdistance) self._cf.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 rad/s """ pk = CRTPPacket() pk.port = CRTPPort.COMMANDER_GENERIC pk.data = struct.pack('<Bffff', TYPE_VELOCITY_WORLD, vx, vy, vz, yawrate) self._cf.send_packet(pk)
def start(self): """Initiate fetching of the TOC.""" logger.debug("[%d]: Start fetching...", self.port) # Register callback in this class for the port self.cf.add_port_callback(self.port, self._new_packet_cb) # Request the TOC CRC self.state = GET_TOC_INFO pk = CRTPPacket() pk.set_header(self.port, TOC_CHANNEL) pk.data = (CMD_TOC_INFO, ) self.cf.send_packet(pk, expect_answer=True)
def delete(self): """Delete this entry in the Crazyflie""" if (self.cf.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 %d" % self.id) pk = CRTPPacket() pk.set_header(5, CHAN_SETTINGS) pk.data = (CMD_DELETE_BLOCK, self.id) self.cf.send_packet(pk, expect_answer=True)