def line_sequence(velocity):
global pos_fixed
#先记录固定点的坐标用于计算距离
with SyncCrazyflie(uri_fixed, cf=Crazyflie(rw_cache='./cache')) as scf:
lpos = LogConfig(name='position', period_in_ms=100)
lpos.add_variable('stateEstimate.x')
lpos.add_variable('stateEstimate.y')
lpos.add_variable('stateEstimate.z')
scf.cf.log.add_config(lpos)
lpos.data_received_cb.add_callback(pos_data_fixed)
lpos.start()
time.sleep(2)
lpos.stop()
pos_fixed = np.mean(fixed_dt, axis=0).tolist()
print(pos_fixed)
with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf:
lpos = LogConfig(name='position', period_in_ms=100)
lpos.add_variable('stateEstimate.x')
lpos.add_variable('stateEstimate.y')
lpos.add_variable('stateEstimate.z')
lpos.add_variable(distance_mapper[uri_fixed])
scf.cf.log.add_config(lpos)
lpos.data_received_cb.add_callback(pos_data)
with PositionHlCommander(scf, default_velocity=velocity) as pc:
time.sleep(2)
pc.go_to(2.0, 0.0, 0.5)
lpos.start()
pc.go_to(-0.5, 0, 0.5)
lpos.stop()
df = pd.DataFrame(
moving_dt,
columns=['x', 'y', 'z', 'distance_UWB', 'distance_lighthouse'])
df.to_csv('./antenna_' + str(velocity) + '_154.65_8' + '.csv', index=False)
def test_different_underlying_cf_instances(self):
# Fixture
# Test
scf1 = SyncCrazyflie('uri 1')
scf2 = SyncCrazyflie('uri 2')
# Assert
actual1 = scf1.cf
actual2 = scf2.cf
self.assertNotEqual(actual1, actual2)
def log_ranging(link_uri, period_in_ms=100, keep_time_in_s=5):
cflib.crtp.init_drivers()
log_data = pd.DataFrame(columns=[
'timestamp', 'receive_from_1', 'receive_from_2', 'receive_from_3',
'receive_from_4', 'compute_from_1', 'compute_from_2', 'compute_from_3',
'compute_from_4', 'total_receive', 'total_send', 'total_compute',
'receive_error', 'compute_error'
])
with SyncCrazyflie(link_uri=link_uri,
cf=Crazyflie(rw_cache="./cache")) as scf:
log_ranging = LogConfig(name='TSranging', period_in_ms=period_in_ms)
log_ranging.add_variable('TSranging.receive_from_1', 'uint16_t')
log_ranging.add_variable('TSranging.receive_from_2', 'uint16_t')
log_ranging.add_variable('TSranging.receive_from_3', 'uint16_t')
log_ranging.add_variable('TSranging.receive_from_4', 'uint16_t')
log_ranging.add_variable('TSranging.compute_from_1', 'uint16_t')
log_ranging.add_variable('TSranging.compute_from_2', 'uint16_t')
log_ranging.add_variable('TSranging.compute_from_3', 'uint16_t')
log_ranging.add_variable('TSranging.compute_from_4', 'uint16_t')
log_ranging.add_variable('TSranging.total_receive', 'uint16_t')
log_ranging.add_variable('TSranging.total_send', 'uint16_t')
log_ranging.add_variable('TSranging.total_compute', 'uint16_t')
log_ranging.add_variable('TSranging.receive_error', 'uint16_t')
log_ranging.add_variable('TSranging.compute_error', 'uint16_t')
with SyncLogger(scf, log_ranging) as logger:
end_time = time.time() + keep_time_in_s
for log_entry in logger:
timestamp = log_entry[0]
data = log_entry[1]
logconf_name = log_entry[2]
temp = {
'timestamp': timestamp,
'receive_from_1': data['TSranging.receive_from_1'],
'receive_from_2': data['TSranging.receive_from_2'],
'receive_from_3': data['TSranging.receive_from_3'],
'receive_from_4': data['TSranging.receive_from_4'],
'compute_from_1': data['TSranging.compute_from_1'],
'compute_from_2': data['TSranging.compute_from_2'],
'compute_from_3': data['TSranging.compute_from_3'],
'compute_from_4': data['TSranging.compute_from_4'],
'total_receive': data['TSranging.total_receive'],
'total_send': data['TSranging.total_send'],
'total_compute': data['TSranging.total_compute'],
'receive_error': data['TSranging.receive_error'],
'compute_error': data['TSranging.compute_error']
}
log_data = log_data.append(temp, ignore_index=True)
# print(log_data.iloc[log_data.shape[0] - 1, :])
print(temp)
if time.time() > end_time:
break
log_data.to_csv('../data/LAB2.csv', index=False)
def main():
cflib.crtp.init_drivers(enable_debug_driver=False)
numRepeticions = int(input("Quants cops vols repetir la ruta?"))
ruta = input("Quina ruta vols realitzar?")
with SyncCrazyflie(URI) as scf:
with MotionCommander(scf) as mq90:
if ruta == rutaABC:
mq90.up(1)
mq90.forward(1)
...
...
elif ruta == rutaBC:
mq90.up(1)
mq90.forward(1)
...
...
elif ruta == rutaCB:
mq90.up(1)
mq90.forward(1)
...
...
elif ruta == rutaCA:
mq90.up(1)
mq90.forward(1)
...
...
elif ruta == rutaAC:
mq90.up(1)
mq90.forward(1)
...
...
mq90.land()
def main():
cflib.crtp.init_drivers(enable_debug_driver=False)
with SyncCrazyflie(URI) as scf:
with MotionCommander(scf) as mq90:
print("Take off done!")
mq90.up(1)
def __init__(self, uri, geos, calibs):
self.data_written = False
self.result_received = False
with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf:
mems = scf.cf.mem.get_mems(MemoryElement.TYPE_LH)
count = len(mems)
if count != 1:
raise Exception('Unexpected nr of memories found:', count)
lh_mem = mems[0]
for bs, geo in geos.items():
self.data_written = False
print('Write geoetry', bs, 'to RAM')
lh_mem.write_geo_data(bs, geo, self._data_written, write_failed_cb=self._data_failed)
while not self.data_written:
time.sleep(0.1)
for bs, calib in calibs.items():
self.data_written = False
print('Write calibration', bs, 'to RAM')
lh_mem.write_calib_data(bs, calib, self._data_written, write_failed_cb=self._data_failed)
while not self.data_written:
time.sleep(0.1)
print('Persist data')
scf.cf.loc.receivedLocationPacket.add_callback(self._data_persisted)
scf.cf.loc.send_lh_persist_data_packet(list(range(16)), list(range(16)))
while not self.result_received:
time.sleep(0.1)
def run_flight():
cflib.crtp.init_drivers(enable_debug_driver=False)
available = cflib.crtp.scan_interfaces()
print('Crazyflies found:')
for i in available:
print(i[0])
if len(available) == 0:
print('No Crazyflies found, cannot run test')
return
threads = []
for uri in available:
scf = SyncCrazyflie(uri[0], cf=Crazyflie(rw_cache='./cache'))
scfs.append(scf)
t = Thread(target=run_drone, args=(scf, uri[0]), daemon=True)
threads.append(t)
t.start()
for t in threads:
t.join()
def processNatNetPacket:
def init_optitrack(iface):
self.optitrack = rx.mkcmdsock(ip_address=iface)
msg = struct.pack("I", rx.NAT_PING)
server_address = '192.168.1.205'
result = optitrack.sendto(msg, (server_address, rx.PORT_COMMAND))
if __name__ == '__main__':
cflib.crtp.init_drivers(enable_debug_driver=False)
iface = "192.168.1.7"
""" Initialize Otitrack """
init_optitrack(iface)
with SyncCrazyflie(uri) as scf:
reset_estimator(scf)
# start_position_printing(scf)
run_sequence(scf, sequence)
while(1)
self.processNatNetPacket()
def square():
with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf:
with PositionHlCommander(
scf,
x=0, y=0, z=0,
default_velocity=0.3,
default_height=0.2,
controller=PositionHlCommander.CONTROLLER_MELLINGER) as pc:
print(pc.get_position())
pc.up(.5)
print(pc.get_position())
pc.right(.5)
print(pc.get_position())
pc.forward(.5)
print(pc.get_position())
pc.left(1)
print(pc.get_position())
pc.back(1)
print(pc.get_position())
pc.right(1)
print(pc.get_position())
pc.forward(.5)
print(pc.get_position())
pc.left(.5)
print(pc.get_position())
pc.down(.5)
print(pc.get_position())
def selectDrone(self):
self.cfs = []
self.scfs = []
self.mcs = []
self.selected = []
message = 'Quais drones deseja controlar? (1, 2, 3, 4 ou combinacao delas):'
while (len(self.selected) == 0):
user_input_numbers = input(message)
if (user_input_numbers.find('1') != -1):
self.selected.append(1)
if (user_input_numbers.find('2') != -1):
self.selected.append(2)
if (user_input_numbers.find('3') != -1):
self.selected.append(3)
if (user_input_numbers.find('4') != -1):
self.selected.append(4)
if (len(self.selected) == 0):
print("comando invalido")
try:
for i in self.selected:
cf = Crazyflie(rw_cache='./cache')
self.cfs.append(cf)
sync = SyncCrazyflie(self.uris[i - 1], cf=cf)
sync.open_link()
self.mcs.append(MotionCommander(sync))
self.scfs.append(sync)
except:
print("Exception: Too many packets lost\n")
self.selected = []
def simple_sequence():
with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf:
scf.cf.param.add_update_callback(group="deck",
name="bcLighthouse4",
cb=param_deck_flow)
time.sleep(1)
logconf = LogConfig(name='Parameters', period_in_ms=SAMPLE_PERIOD_MS)
for param in log_parameters:
logconf.add_variable(param[0], param[1])
scf.cf.log.add_config(logconf)
logconf.data_received_cb.add_callback(logconf_callback)
if not is_deck_attached:
return
logconf.start()
with PositionHlCommander(
scf,
x=0.0,
y=0.0,
z=0.0,
default_velocity=VELOCITY,
default_height=DEFAULT_HEIGHT,
controller=PositionHlCommander.CONTROLLER_MELLINGER) as pc:
for position in SEQUENCE:
pc.go_to(position[0], position[1])
time.sleep(5)
logconf.stop()
write_log_history()
def setUp(self):
self.uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7')
self.cf_mock = MagicMock(spec=Crazyflie)
self.cf_mock.connected = Caller()
self.cf_mock.connection_failed = Caller()
self.cf_mock.disconnected = Caller()
self.cf_mock.open_link = AsyncCallbackCaller(
cb=self.cf_mock.connected,
args=[self.uri],
delay=0.2
).trigger
self.close_link_mock = AsyncCallbackCaller(
cb=self.cf_mock.disconnected,
args=[self.uri],
delay=0.2
)
self.cf_mock.close_link = self.close_link_mock.trigger
# Mock the behaviour that param values are updated(downloaded) after connection
self.param_mock = MagicMock(spec=Param)
self.param_mock.all_updated = Caller()
self.cf_mock.param = self.param_mock
# Register a callback to be called when connected. Use it to trigger a callback
# to trigger the call to the param.all_updated() callback
self.cf_mock.connected.add_callback(self._connected_callback)
self.sut = SyncCrazyflie(self.uri, cf=self.cf_mock)
def __init__(self, uri, threaded=False):
super().__init__(threaded)
# TODO: maybe add a parameter so people can change the default velocity
# Initialize the low-level drivers (don't list the debug drivers)
cflib.crtp.init_drivers(enable_debug_driver=False)
# the connection to the crazyflie
self._scf = SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache'))
# temp (or maybe will be permanent) state variable
self._is_open = False
self._running = False
self._send_rate = 5 # want to send messages at 5Hz NOTE: the minimum is 2 Hz
self._out_msg_queue = queue.Queue(
) # a queue for sending data between threads
self._write_handle = threading.Thread(target=self.command_loop)
self._write_handle.daemon = True
# since can only command velocities and not positions, the connection
# needs some awareness of the current position to be able to do
# the math necessary
self._current_position_xyz = [0.0, 0.0, 0.0] # [x, y, z]
def stateInit(self):
"""Initialize CF (scan, open link) and logging framework"""
#--- Scan for cf
cflib.crtp.init_drivers(enable_debug_driver=False)
print('Scanning interfaces for Crazyflies...')
available = cflib.crtp.scan_interfaces()
if len(available) == 0:
print("No cf found, aborting cf code.")
self.cf = None
else:
print('Crazyflies found:')
for i in available:
print(str(i[0]))
self.URI = 'radio://0/80/2M' #available[0][0]
self.cf = Crazyflie(rw_cache='./cache')
if self.cf is None:
self.b.wait()
print('Not running cf code.')
return None
self.scf = SyncCrazyflie(self.URI,cf=Crazyflie(rw_cache='./cache'))
self.scf.open_link()
self.mc = MotionCommander(self.scf)
self.file = open("ugly_log.txt","a")
#-- Barrier to wait for CV thread
self.b.wait()
self.lgr = UglyLogger(self.URI, self.scf, self.file)
self.enterTakingOff()
return self.stateTakingOff
def cf_handler(self):
uri = "radio://0/30"
lg_block = LogConfig(name='LH', period_in_ms=50)
lg_block.add_variable('lighthouse.angle0x', 'float')
lg_block.add_variable('lighthouse.angle0y', 'float')
lg_block.add_variable('lighthouse.angle1x', 'float')
lg_block.add_variable('lighthouse.angle1y', 'float')
cf = Crazyflie(rw_cache='./cache')
with SyncCrazyflie(uri, cf=cf) as scf:
print("Getting LH geometry")
mems = scf.cf.mem.get_mems(MemoryElement.TYPE_LH)
mems[0].update(self._update_geometry)
with SyncLogger(scf, lg_block) as logger:
for log_entry in logger:
data = log_entry[1]
self.angles[0][0] = data['lighthouse.angle0x']
self.angles[0][1] = data['lighthouse.angle0y']
self.angles[1][0] = data['lighthouse.angle1x']
self.angles[1][1] = data['lighthouse.angle1y']
if self.terminate:
break
self.update_scene()
def __init__(self, cf_id, radio_uri):
self._id = cf_id
self._uri = radio_uri
cflib.crtp.init_drivers(enable_debug_driver=False)
try:
self.scf = SyncCrazyflie(URI)
self.cf = self.scf.cf
except e:
print("Unable to connect to CF %d at URI %s" %
(self._id, self._uri))
self.scf = None
self.cf = None
self.data_pub = rospy.Publisher('cf/%d/data' % self._id,
CFData,
queue_size=10)
self.image_pub = rospy.Publisher('cf/%d/image' % self._id,
CFImage,
queue_size=10)
self.cmd_sub = rospy.Subscriber('cf/%d/command' % self._id, CFCommand,
command_cb)
self.motion_sub = rospy.Subscriber('cf/%d/motion' % self._id, CFMotion,
motion_cb)
threading.Thread(target=self.image_thread).start()
def simple_sequence():
with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf:
with PositionHlCommander(scf) as pc:
pc.forward(1.0)
pc.left(1.0)
pc.back(1.0)
pc.go_to(0.0, 0.0, 1.0)
def slightly_more_complex_usage():
with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf:
with PositionHlCommander(
scf,
x=0.0,
y=0.0,
z=0.0,
default_velocity=0.3,
default_height=0.5,
controller=PositionHlCommander.CONTROLLER_PID) as pc:
# Go to a coordinate
pc.go_to(1.0, 1.0, 1.0)
# Move relative to the current position
pc.right(1.0)
# Go to a coordinate and use default height
pc.go_to(0.0, 0.0)
# Go slowly to a coordinate
pc.go_to(1.0, 1.0, velocity=0.2)
# Set new default velocity and height
pc.set_default_velocity(0.3)
pc.set_default_height(1.0)
pc.go_to(0.0, 0.0)
def run_sequence(drone, waypoints):
with SyncCrazyflie(drone.uri, cf=Crazyflie(rw_cache='./cache')) as scf:
drone.scf = scf
reset_estimator(drone)
drone.start_position_reading() # 20 Hz
drone.start_battery_status_reading() # 2 Hz
time.sleep(1)
drone.pose_home = drone.pose
print('Home position:', drone.pose_home)
print('Battery status: %.2f' % drone.V_bat)
# takeoff to z=0.3 m:
drone.takeoff(toFly=toFly)
drone.hover(1, toFly=toFly)
# """ Flight mission """
for goal in waypoints:
drone.goTo(goal, toFly=toFly)
drone.hover(1, toFly=toFly)
print('Go home before landing...')
drone.goTo([drone.pose_home[0], drone.pose_home[1], 0.3, 0],
toFly=toFly)
drone.hover(2, toFly=toFly)
drone.land(toFly=toFly)
print('Battery status %.2f:' % drone.V_bat)
def main():
try:
#logfile reset
myfile = open('SensorMaster.txt', 'w')
myfile.write
myfile.close()
cflib.crtp.init_drivers(enable_debug_driver=False)
scf = SyncCrazyflie(URI)
fc = FlightCtrl(scf)
m = Myo()
Thread(target=fc.gesture_ctrl, args=(scf, fc, gesture)).start()
Thread(target=m.gesture_detection, args=(gesture, )).start()
while True:
time.sleep(0.5)
except KeyboardInterrupt:
print('\nClosing link...')
#perhaps land gracefully here
scf.close_link()
print('Shutting down...')
os.kill(os.getpid(), signal.SIGINT)
except Exception, e:
print(str(e))
print('\nShutting down...')
scf.close_link()
sys.exit(0)
def producer(queue):
fileTag = open(filenameTag, 'w')
lg_stab = LogConfig(name='Stabilizer', period_in_ms=100)
lg_stab.add_variable('stateEstimate.x', 'float')
lg_stab.add_variable('stateEstimate.y', 'float')
lg_stab.add_variable('stateEstimate.z', 'float')
cnt = 0
with SyncCrazyflie(uriTag, cf=Crazyflie(rw_cache='./cache')) as scf:
with SyncLogger(scf, lg_stab) as logger:
for log_entry in logger:
timestamp = log_entry[0]
data = log_entry[1]
logconf_name = log_entry[2]
# print('[%d][%s]: %s' % (timestamp, logconf_name, data))
a = '%s, %s, %s\n' % (data["stateEstimate.x"], data["stateEstimate.y"], data["stateEstimate.z"])
x = float(data["stateEstimate.x"])
y = float(data["stateEstimate.y"])
fileTag.write(a)
if cnt == 15:
consumerPos = (x, y)
queue.put(consumerPos)
cnt = 0
else:
cnt += 1
if is_finished == True:
endPos = (x, y)
queue.put(endPos)
break
fileTag.close()
print('Productor ended')
def main():
cflib.crtp.init_drivers(enable_debug_driver=False)
numRepeticiones = int(input("¿Cuántas veces quieres repetir la ruta?"))
ruta = input("¿Qué ruta quieres realizar?")
with SyncCrazyflie(URI) as scf:
with MotionCommander(scf) as mq90:
if ruta == rutaABC:
mq90.up(1)
mq90.forward(1)
...
...
elif ruta == rutaBC:
mq90.up(1)
mq90.forward(1)
...
...
elif ruta == rutaCB:
mq90.up(1)
mq90.forward(1)
...
...
elif ruta == rutaCA:
mq90.up(1)
mq90.forward(1)
...
...
elif ruta == rutaAC:
mq90.up(1)
mq90.forward(1)
...
...
mq90.land()
def main():
cflib.crtp.init_drivers(enable_debug_driver=False)
with SyncCrazyflie(URI) as scf:
with MotionCommander(scf) as mq90:
mq90.up(valueToMove)
mq90.forward(valueGoForward)
mq90.land()
def __init__(self, uri='radio://0/80/2M'):
self.uri = uri # URI to the Crazyflie to connect to
self.cf = SyncCrazyflie(self.uri, cf=Crazyflie(rw_cache='./cache')).cf
self.pose = None
self.pose_home = np.array([0, 0, 0])
self.orient = None
self.sp = None
self.battery_state = ''
def __init__(self, uri, file_name):
self._event = Event()
with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf:
writer = LighthouseConfigWriter(scf.cf)
writer.write_and_store_config_from_file(self._data_written,
file_name)
self._event.wait()
def test_that_the_same_cf_object_can_be_connected_multiple_times(self):
# Fixture
self.test_rig_support.restart_devices(self.test_rig_support.all_uris)
cf = Crazyflie(rw_cache='./cache')
# Test
for uri in self.test_rig_support.all_uris:
with SyncCrazyflie(uri, cf=cf):
pass
def test_that_requested_logging_is_received_properly_from_one_cf(self):
# Fixture
uri = self.test_rig_support.all_uris[0]
self.test_rig_support.restart_devices([uri])
cf = Crazyflie(rw_cache='./cache')
# Test and Assert
with SyncCrazyflie(uri, cf=cf) as scf:
self.assert_add_logging_and_get_non_zero_value(scf)
def land_on_elevated_surface():
with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf:
with PositionHlCommander(scf,
default_height=0.5,
default_velocity=0.2,
default_landing_height=0.35) as pc:
# fly onto a landing platform at non-zero height (ex: from floor to desk, etc)
pc.forward(1.0)
pc.left(1.0)
def test_memory_mapping_with_one_cf(self):
# Fixture
uri = self.test_rig_support.all_uris[0]
self.test_rig_support.restart_devices([uri])
cf = Crazyflie(rw_cache='./cache')
# Test and Assert
with SyncCrazyflie(uri, cf=cf) as scf:
self.assert_memory_mapping(scf)
def __init__(self, uri, geos, calibs):
self._event = Event()
with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf:
writer = LighthouseConfigWriter(scf.cf, nr_of_base_stations=2)
writer.write_and_store_config(self._data_written,
geos=geos,
calibs=calibs)
self._event.wait()
