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