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 _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 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 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_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 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._cf.send_packet(pk)
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. """crazyflie 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 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 send_extpose(self, pos, quat): """ Send the current Crazyflie pose (position [x, y, z] and attitude quaternion [qx, qy, qz, qw]). This is going to be forwarded to the Crazyflie'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._cf.send_packet(pk)
def test_that_lighthouse_persist_data_is_correctly_encoded(self): # fixture geo_bs_list = [0, 2, 4, 6, 8, 10, 12, 14] calib_bs_list = [1, 3, 5, 7, 9, 11, 13, 15] # test actual = self.sut.send_lh_persist_data_packet( geo_bs_list, calib_bs_list) # assert data_check = 2863289685 expected = CRTPPacket() expected.port = CRTPPort.LOCALIZATION expected.channel = self.sut.GENERIC_CH expected.data = struct.pack( '<BI', Localization.LH_PERSIST_DATA, data_check) actual_object = self.cf_mock.send_packet.call_args actual = actual_object[0][0] self.assertEqual(expected.port, actual.port) self.assertEqual(expected.channel, actual.channel) self.assertEqual(expected.data, actual.data)
def send_packet(self, data): packet = CRTPPacket() packet.port = CRTPPort.PLATFORM packet.channel = cflib.crazyflie.platformservice.APP_CHANNEL packet.data = data self._cf.send_packet(packet)
def send_usb_disable(self): pk = CRTPPacket() pk.port = CRTPPort.COMMANDER_GENERIC pk.channel = self.CONFIG_USB_CH pk.data = struct.pack('<B', 0) self._cf.send_packet(pk)