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))
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 request_param_update(self, var_id): """Place a param update request on the queue""" self._useV2 = self.ed.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_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_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 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 _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 _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 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_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 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_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 start(self): """Initiate fetching of the TOC.""" self._useV2 = self.ed.platform.get_protocol_version() >= 4 logger.debug('[%d]: Using V2 protocol: %d', self.port, self._useV2) logger.debug('[%d]: Start fetching...', self.port) # Register callback in this class for the port self.ed.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) if self._useV2: pk.data = (CMD_TOC_INFO_V2,) self.ed.send_packet(pk, expected_reply=(CMD_TOC_INFO_V2,)) else: pk.data = (CMD_TOC_INFO,) self.ed.send_packet(pk, expected_reply=(CMD_TOC_INFO,))
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 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_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 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 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_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 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 upload_buffer(self, target_id, page, address, buff): """Upload data into a buffer on the Espdrone""" # 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)): pk.data.append(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) self.link.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 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 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 _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_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 """ # The fake CPU ID is legacy from the Espdrone 1.0 # In order to reset the CPU id had to be sent, but this # was removed before launching it. But the length check is # still in the bootloader. So to work around this bug so # some extra data needs to be sent. 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)): # Difference in ed1 and ed2 (CPU ID) if target_id == 0xFE: pk.data = (target_id, 0xF0, 0x01) else: pk.data = (target_id, 0xF0) + fake_cpu_id self.link.send_packet(pk) break time.sleep(0.1)
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))