diff --git a/examples/aideck/fpv.py b/examples/aideck/fpv.py index 8a00f52a0..bea7924de 100644 --- a/examples/aideck/fpv.py +++ b/examples/aideck/fpv.py @@ -48,6 +48,7 @@ import struct import sys import threading +from time import time import numpy as np @@ -185,6 +186,10 @@ def __init__(self, URI): self.hover = {'x': 0.0, 'y': 0.0, 'z': 0.0, 'yaw': 0.0, 'height': 0.3} + # Arm the Crazyflie + self.cf.platform.send_arming_request(True) + time.sleep(1.0) + self.hoverTimer = QtCore.QTimer() self.hoverTimer.timeout.connect(self.sendHoverCommand) self.hoverTimer.setInterval(100) diff --git a/examples/autonomy/autonomousSequence.py b/examples/autonomy/autonomousSequence.py index 48f3047c6..da6cb9a7c 100644 --- a/examples/autonomy/autonomousSequence.py +++ b/examples/autonomy/autonomousSequence.py @@ -127,6 +127,10 @@ def start_position_printing(scf): def run_sequence(scf, sequence): cf = scf.cf + # Arm the Crazyflie + cf.platform.send_arming_request(True) + time.sleep(1.0) + for position in sequence: print('Setting position {}'.format(position)) for i in range(50): diff --git a/examples/autonomy/autonomous_sequence_high_level.py b/examples/autonomy/autonomous_sequence_high_level.py index 1880bb767..6bf737e4f 100644 --- a/examples/autonomy/autonomous_sequence_high_level.py +++ b/examples/autonomy/autonomous_sequence_high_level.py @@ -140,6 +140,10 @@ def upload_trajectory(cf, trajectory_id, trajectory): def run_sequence(cf, trajectory_id, duration): commander = cf.high_level_commander + # Arm the Crazyflie + cf.platform.send_arming_request(True) + time.sleep(1.0) + commander.takeoff(1.0, 2.0) time.sleep(3.0) relative = True diff --git a/examples/autonomy/autonomous_sequence_high_level_compressed.py b/examples/autonomy/autonomous_sequence_high_level_compressed.py index 7aea72cba..b28059a5b 100644 --- a/examples/autonomy/autonomous_sequence_high_level_compressed.py +++ b/examples/autonomy/autonomous_sequence_high_level_compressed.py @@ -137,6 +137,10 @@ def upload_trajectory(cf, trajectory_id, trajectory): def run_sequence(cf, trajectory_id, duration): commander = cf.high_level_commander + # Arm the Crazyflie + cf.platform.send_arming_request(True) + time.sleep(1.0) + commander.takeoff(1.0, 2.0) time.sleep(3.0) relative = True diff --git a/examples/autonomy/circling_square_demo.py b/examples/autonomy/circling_square_demo.py index 88f2ed495..6cc58ff45 100644 --- a/examples/autonomy/circling_square_demo.py +++ b/examples/autonomy/circling_square_demo.py @@ -162,6 +162,10 @@ def turn_off_leds(scf): def run_sequence(scf, alpha, r): + # Arm the Crazyflie + scf.cf.platform.send_arming_request(True) + time.sleep(1.0) + commander = scf.cf.high_level_commander trajectory_id = 1 duration = 4*t diff --git a/examples/autonomy/full_state_setpoint_demo.py b/examples/autonomy/full_state_setpoint_demo.py index 9b6ea00c6..0eaac6a32 100644 --- a/examples/autonomy/full_state_setpoint_demo.py +++ b/examples/autonomy/full_state_setpoint_demo.py @@ -134,6 +134,10 @@ def run_sequence(scf): # Set to mellinger controller # cf.param.set_value('stabilizer.controller', '2') + # Arm the Crazyflie + cf.platform.send_arming_request(True) + time.sleep(1.0) + print('takeoff') send_setpoint(cf, 4.0, [0.0, 0.0, 1.0], diff --git a/examples/autonomy/motion_commander_demo.py b/examples/autonomy/motion_commander_demo.py index 1f34e178a..da86b55b9 100644 --- a/examples/autonomy/motion_commander_demo.py +++ b/examples/autonomy/motion_commander_demo.py @@ -52,6 +52,10 @@ cflib.crtp.init_drivers() with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: + # Arm the Crazyflie + scf.cf.platform.send_arming_request(True) + time.sleep(1.0) + # We take off when the commander is created with MotionCommander(scf) as mc: time.sleep(1) diff --git a/examples/autonomy/position_commander_demo.py b/examples/autonomy/position_commander_demo.py index 6908138fc..b8597e9d8 100644 --- a/examples/autonomy/position_commander_demo.py +++ b/examples/autonomy/position_commander_demo.py @@ -29,6 +29,8 @@ Change the URI variable to your Crazyflie configuration. """ +from time import time + import cflib.crtp from cflib.crazyflie import Crazyflie from cflib.crazyflie.syncCrazyflie import SyncCrazyflie @@ -41,6 +43,10 @@ def slightly_more_complex_usage(): with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: + # Arm the Crazyflie + scf.cf.platform.send_arming_request(True) + time.sleep(1.0) + with PositionHlCommander( scf, x=0.0, y=0.0, z=0.0, @@ -67,6 +73,10 @@ def slightly_more_complex_usage(): def land_on_elevated_surface(): with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: + # Arm the Crazyflie + scf.cf.platform.send_arming_request(True) + time.sleep(1.0) + with PositionHlCommander(scf, default_height=0.5, default_velocity=0.2, @@ -80,6 +90,10 @@ def land_on_elevated_surface(): def simple_sequence(): with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: + # Arm the Crazyflie + scf.cf.platform.send_arming_request(True) + time.sleep(1.0) + with PositionHlCommander(scf, controller=PositionHlCommander.CONTROLLER_PID) as pc: pc.forward(1.0) pc.left(1.0) diff --git a/examples/lighthouse/lighthouse_openvr_grab.py b/examples/lighthouse/lighthouse_openvr_grab.py index 63ee8ec7e..8aff29538 100644 --- a/examples/lighthouse/lighthouse_openvr_grab.py +++ b/examples/lighthouse/lighthouse_openvr_grab.py @@ -171,6 +171,11 @@ def run_sequence(scf): with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: reset_estimator(scf) + + # Arm the Crazyflie + scf.cf.platform.send_arming_request(True) + time.sleep(1.0) + run_sequence(scf) openvr.shutdown() diff --git a/examples/lighthouse/lighthouse_openvr_grab_color.py b/examples/lighthouse/lighthouse_openvr_grab_color.py index 765b06be0..6e8abef55 100644 --- a/examples/lighthouse/lighthouse_openvr_grab_color.py +++ b/examples/lighthouse/lighthouse_openvr_grab_color.py @@ -203,6 +203,11 @@ def run_sequence(scf): with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: reset_estimator(scf) # start_position_printing(scf) + + # Arm the Crazyflie + scf.cf.platform.send_arming_request(True) + time.sleep(1.0) + run_sequence(scf) openvr.shutdown() diff --git a/examples/lighthouse/lighthouse_openvr_multigrab.py b/examples/lighthouse/lighthouse_openvr_multigrab.py index d4d2377f2..9948e359c 100644 --- a/examples/lighthouse/lighthouse_openvr_multigrab.py +++ b/examples/lighthouse/lighthouse_openvr_multigrab.py @@ -199,6 +199,12 @@ def run_sequence(scf0, scf1): reset_estimator(scf0) with SyncCrazyflie(uri1, cf=Crazyflie(rw_cache='./cache')) as scf1: reset_estimator(scf1) + + # Arm the Crazyflies + scf0.cf.platform.send_arming_request(True) + scf1.cf.platform.send_arming_request(True) + time.sleep(1.0) + run_sequence(scf0, scf1) openvr.shutdown() diff --git a/examples/mocap/mocap_hl_commander.py b/examples/mocap/mocap_hl_commander.py index 2a73fb455..3d9f0ea13 100644 --- a/examples/mocap/mocap_hl_commander.py +++ b/examples/mocap/mocap_hl_commander.py @@ -233,6 +233,11 @@ def run_sequence(cf, trajectory_id, duration): duration = upload_trajectory(cf, trajectory_id, figure8) print('The sequence is {:.1f} seconds long'.format(duration)) reset_estimator(cf) + + # Arm the Crazyflie + cf.platform.send_arming_request(True) + time.sleep(1.0) + run_sequence(cf, trajectory_id, duration) mocap_wrapper.close() diff --git a/examples/mocap/qualisys_hl_commander.py b/examples/mocap/qualisys_hl_commander.py index b46d369c1..d2dcb301f 100644 --- a/examples/mocap/qualisys_hl_commander.py +++ b/examples/mocap/qualisys_hl_commander.py @@ -296,6 +296,11 @@ def run_sequence(cf, trajectory_id, duration): duration = upload_trajectory(cf, trajectory_id, figure8) print('The sequence is {:.1f} seconds long'.format(duration)) reset_estimator(cf) + + # Arm the Crazyflie + cf.platform.send_arming_request(True) + time.sleep(1.0) + run_sequence(cf, trajectory_id, duration) qtm_wrapper.close() diff --git a/examples/motors/multiramp.py b/examples/motors/multiramp.py index 5e89bb1e8..df77fec51 100644 --- a/examples/motors/multiramp.py +++ b/examples/motors/multiramp.py @@ -59,6 +59,10 @@ def _connected(self, link_uri): """ This callback is called form the Crazyflie API when a Crazyflie has been connected and the TOCs have been downloaded.""" + # Arm the Crazyflie + self._cf.platform.send_arming_request(True) + time.sleep(1.0) + # Start a separate thread to do the motor test. # Do not hijack the calling thread! Thread(target=self._ramp_motors).start() diff --git a/examples/motors/ramp.py b/examples/motors/ramp.py index d17261d4a..a91d87bac 100644 --- a/examples/motors/ramp.py +++ b/examples/motors/ramp.py @@ -60,6 +60,10 @@ def _connected(self, link_uri): """ This callback is called form the Crazyflie API when a Crazyflie has been connected and the TOCs have been downloaded.""" + # Arm the Crazyflie + self._cf.platform.send_arming_request(True) + time.sleep(1.0) + # Start a separate thread to do the motor test. # Do not hijack the calling thread! Thread(target=self._ramp_motors).start() @@ -98,7 +102,7 @@ def _ramp_motors(self): self._cf.commander.send_setpoint(0, 0, 0, 0) # Make sure that the last packet leaves before the link is closed # since the message queue is not flushed before closing - time.sleep(0.1) + time.sleep(1) self._cf.close_link() diff --git a/examples/multiranger/multiranger_pointcloud.py b/examples/multiranger/multiranger_pointcloud.py index 3c42a41f9..837c7f570 100644 --- a/examples/multiranger/multiranger_pointcloud.py +++ b/examples/multiranger/multiranger_pointcloud.py @@ -50,6 +50,7 @@ import logging import math import sys +from time import time import numpy as np from vispy import scene @@ -111,6 +112,10 @@ def __init__(self, URI): # Connect to the Crazyflie self.cf.open_link(URI) + # Arm the Crazyflie + self.cf.platform.send_arming_request(True) + time.sleep(1.0) + self.hover = {'x': 0.0, 'y': 0.0, 'z': 0.0, 'yaw': 0.0, 'height': 0.3} self.hoverTimer = QtCore.QTimer() diff --git a/examples/multiranger/multiranger_push.py b/examples/multiranger/multiranger_push.py index 9905d6472..491f30038 100755 --- a/examples/multiranger/multiranger_push.py +++ b/examples/multiranger/multiranger_push.py @@ -74,6 +74,10 @@ def is_close(range): cf = Crazyflie(rw_cache='./cache') with SyncCrazyflie(URI, cf=cf) as scf: + # Arm the Crazyflie + scf.cf.platform.send_arming_request(True) + time.sleep(1.0) + with MotionCommander(scf) as motion_commander: with Multiranger(scf) as multiranger: keep_flying = True diff --git a/examples/multiranger/multiranger_wall_following.py b/examples/multiranger/multiranger_wall_following.py index 710f9b6c1..d54d4a871 100644 --- a/examples/multiranger/multiranger_wall_following.py +++ b/examples/multiranger/multiranger_wall_following.py @@ -82,6 +82,10 @@ def handle_range_measurement(range): cf = Crazyflie(rw_cache='./cache') with SyncCrazyflie(URI, cf=cf) as scf: + # Arm the Crazyflie + scf.cf.platform.send_arming_request(True) + time.sleep(1.0) + with MotionCommander(scf) as motion_commander: with Multiranger(scf) as multiranger: with SyncLogger(scf, lg_stab) as logger: diff --git a/examples/positioning/flowsequenceSync.py b/examples/positioning/flowsequenceSync.py index 42d7a691f..24c0bd939 100644 --- a/examples/positioning/flowsequenceSync.py +++ b/examples/positioning/flowsequenceSync.py @@ -54,6 +54,10 @@ cf.param.set_value('kalman.resetEstimation', '0') time.sleep(2) + # Arm the Crazyflie + cf.platform.send_arming_request(True) + time.sleep(1.0) + for y in range(10): cf.commander.send_hover_setpoint(0, 0, 0, y / 25) time.sleep(0.1) diff --git a/examples/positioning/initial_position.py b/examples/positioning/initial_position.py index d2119c635..b0c0274ce 100644 --- a/examples/positioning/initial_position.py +++ b/examples/positioning/initial_position.py @@ -118,6 +118,10 @@ def reset_estimator(scf): def run_sequence(scf, sequence, base_x, base_y, base_z, yaw): cf = scf.cf + # Arm the Crazyflie + cf.platform.send_arming_request(True) + time.sleep(1.0) + for position in sequence: print('Setting position {}'.format(position)) diff --git a/examples/positioning/matrix_light_printer.py b/examples/positioning/matrix_light_printer.py index 8aa37b447..4de1d7f02 100644 --- a/examples/positioning/matrix_light_printer.py +++ b/examples/positioning/matrix_light_printer.py @@ -110,5 +110,9 @@ def matrix_print(cf, pc, image_def): image_def = ImageDef('monalisa.png') with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: + # Arm the Crazyflie + scf.cf.platform.send_arming_request(True) + time.sleep(1.0) + with PositionHlCommander(scf, default_height=0.5, controller=PositionHlCommander.CONTROLLER_PID) as pc: matrix_print(scf.cf, pc, image_def) diff --git a/examples/step-by-step/sbs_swarm.py b/examples/step-by-step/sbs_swarm.py index 7332950e9..3cdfa2d9f 100644 --- a/examples/step-by-step/sbs_swarm.py +++ b/examples/step-by-step/sbs_swarm.py @@ -44,6 +44,11 @@ def light_check(scf: SyncCrazyflie): time.sleep(2) +def arm(scf: SyncCrazyflie): + scf.cf.platform.send_arming_request(True) + time.sleep(1.0) + + def take_off(scf: SyncCrazyflie): commander = scf.cf.high_level_commander @@ -161,6 +166,7 @@ def run_sequence(scf: SyncCrazyflie, sequence): swarm.reset_estimators() print('Estimators have been reset') + swarm.parallel_safe(arm) swarm.parallel_safe(take_off) # swarm.parallel_safe(run_square_sequence) swarm.parallel_safe(run_sequence, args_dict=seq_args) diff --git a/examples/swarm/hl-commander-swarm.py b/examples/swarm/hl-commander-swarm.py index 346d21aeb..662d19317 100644 --- a/examples/swarm/hl-commander-swarm.py +++ b/examples/swarm/hl-commander-swarm.py @@ -44,6 +44,11 @@ def activate_mellinger_controller(scf, use_mellinger): scf.cf.param.set_value('stabilizer.controller', controller) +def arm(scf): + scf.cf.platform.send_arming_request(True) + time.sleep(1.0) + + def run_shared_sequence(scf): activate_mellinger_controller(scf, False) @@ -84,4 +89,5 @@ def run_shared_sequence(scf): factory = CachedCfFactory(rw_cache='./cache') with Swarm(uris, factory=factory) as swarm: swarm.reset_estimators() + swarm.parallel_safe(arm) swarm.parallel_safe(run_shared_sequence) diff --git a/examples/swarm/swarmSequence.py b/examples/swarm/swarmSequence.py index dea78169c..a3d5e9e2d 100644 --- a/examples/swarm/swarmSequence.py +++ b/examples/swarm/swarmSequence.py @@ -171,6 +171,11 @@ def wait_for_param_download(scf): print('Parameters downloaded for', scf.cf.link_uri) +def arm(scf): + scf.cf.platform.send_arming_request(True) + time.sleep(1.0) + + def take_off(cf, position): take_off_time = 1.0 sleep_time = 0.1 @@ -209,6 +214,7 @@ def run_sequence(scf, sequence): try: cf = scf.cf + arm(cf) take_off(cf, sequence[0]) for position in sequence: print('Setting position {}'.format(position)) diff --git a/examples/swarm/swarmSequenceCircle.py b/examples/swarm/swarmSequenceCircle.py index b80e176ad..90835e3ff 100644 --- a/examples/swarm/swarmSequenceCircle.py +++ b/examples/swarm/swarmSequenceCircle.py @@ -87,6 +87,10 @@ def poshold(cf, t, z): def run_sequence(scf, params): cf = scf.cf + # Arm the Crazyflie + cf.platform.send_arming_request(True) + time.sleep(1.0) + # Number of setpoints sent per second fs = 4 fsi = 1.0 / fs diff --git a/examples/swarm/synchronizedSequence.py b/examples/swarm/synchronizedSequence.py index 17661a0e0..7d839df33 100755 --- a/examples/swarm/synchronizedSequence.py +++ b/examples/swarm/synchronizedSequence.py @@ -45,6 +45,7 @@ STEP_TIME = 1 # Possible commands, all times are in seconds +Arm = namedtuple('Arm', []) Takeoff = namedtuple('Takeoff', ['height', 'time']) Land = namedtuple('Land', ['time']) Goto = namedtuple('Goto', ['x', 'y', 'z', 'time']) @@ -62,6 +63,10 @@ sequence = [ # Step, CF_id, action + (0, 0, Arm()), + (0, 1, Arm()), + (0, 2, Arm()), + (0, 0, Takeoff(0.5, 2)), (0, 2, Takeoff(0.5, 2)), @@ -105,6 +110,11 @@ def activate_mellinger_controller(scf, use_mellinger): scf.cf.param.set_value('stabilizer.controller', str(controller)) +def arm(scf): + scf.cf.platform.send_arming_request(True) + time.sleep(1.0) + + def set_ring_color(cf, r, g, b, intensity, time): cf.param.set_value('ring.fadeTime', str(time)) @@ -133,6 +143,8 @@ def crazyflie_control(scf): command = control.get() if type(command) is Quit: return + elif type(command) is Arm: + arm(scf) elif type(command) is Takeoff: commander.takeoff(command.height, command.time) elif type(command) is Land: