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_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)
