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 = []
class Node: def __init__(self): self.vx = 0 self.vy = 0 self.vz = 0 self.va = 0 self.vel = Twist() self.vel.linear.x = self.vx self.vel.linear.y = self.vy self.vel.linear.z = self.vz self.vel.angular.z = self.va self.vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=10) cflib.crtp.init_drivers(enable_debug_driver=False) self.crazyflie = SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) self.commander = MotionCommander(self.crazyflie) self.cf = Crazyflie() self.crazyflie.open_link() self.commander.take_off() def shut_down(self): try: self.pitch_log.stop() finally: self.crazyflie.close_link() def run_node(self): self.vel.linear.x = 0 self.vel.linear.y = 0 self.vel.linear.z = 0.2 self.vel.angular.z = 0 self.vel_pub.publish(self.vel) self.commander._set_vel_setpoint(0, 0, 0.2, 0) time.sleep(3) self.vel.linear.x = 0 self.vel.linear.y = 0 self.vel.linear.z = 0 self.vel.angular.z = 72 self.vel_pub.publish(self.vel) self.commander._set_vel_setpoint(0, 0, 0, 72) time.sleep(3) self.vel.linear.x = 0 self.vel.linear.y = 0 self.vel.linear.z = -0.2 self.vel.angular.z = 0 self.vel_pub.publish(self.vel) self.commander._set_vel_setpoint(0, 0, -0.2, 0) time.sleep(3)
def initDrone(self, posAddressList): print("Resetting and locating ", self.address) scf = SyncCrazyflie(self.address, cf=Crazyflie(rw_cache='./cache')) scf.open_link() self.reset_estimator(scf) self.start_position_printing(scf) time.sleep(0.2) self.pos posAddressList.append([self.pos, self.address]) print("added", [self.pos, self.address]) scf.close_link()
def initialize(self, address, disconnectCallback): Logger.log("Connecting to address " + address, self.swarmIndex) crazyflie = Crazyflie(ro_cache='./cache', rw_cache='./cache') syncCrazyflie = SyncCrazyflie(address, cf=crazyflie) crazyflie.connection_lost.add_callback(disconnectCallback) syncCrazyflie.open_link() self.crazyflie = syncCrazyflie self.commander = syncCrazyflie.cf.high_level_commander self.light_controller = syncCrazyflie.cf.light_controller self.light_controller.set_color(0, 0, 0, 0.0, True) threadUtil.interruptibleSleep(0.5) self.state = DroneState.INITIALIZING self.light_controller.set_color(255, 200, 0, 0.0, True)
def change_fly(event): """ What happens when you press the fly button on the matplotlib interface. This button toggles flying. If pressed while still flying it turns lands the drone and turns off """ global flying, scf scf = SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) scf.open_link() reset_estimator(scf) check_battery(scf, "Drone") start_console(scf, "Drone") flying = not flying if not flying and scf: scf.cf.commander.send_position_setpoint(0, 0, 0, 0)
class CfCom: def __init__(self, URI): cflib.crtp.init_drivers(enable_debug_driver=False) self.URI = URI self.scf = SyncCrazyflie(self.URI, cf=Crazyflie(rw_cache='./cache')) def __del__(self): self.scf.close_link() def open_link(self): self.scf.open_link() def close_link(self): self.scf.close_link() def __enter__(self): self.scf.open_link() return self def __exit__(self, exc_type, exc_val, exc_tb): self.scf.close_link()
class Drone(BaseDrone): """Wrapper class for the crazyflie python lib. This class handles the init and cleanup of the connection to the drone. It also has some methods for some functionality like flying sequences. """ ################################################################################ # Initialization and cleanup code ################################################################################ def __init__(self, uri, initial_x = 0, initial_y = 0, initial_z = 0, initial_yaw = 0): """Establishes the connection to the drone. Uses the uri parameter to connect to the corresponding drone. the initial_ values are used to save the starting position of the drone. """ self.uri = uri self.initial_x = initial_x self.initial_y = initial_y self.initial_z = initial_z self.initial_yaw = initial_z # Only output errors from the logging framework logging.basicConfig(level=logging.ERROR) cflib.crtp.init_drivers(enable_debug_driver=False) self.cf = Crazyflie(rw_cache='./cache') self.scf = SyncCrazyflie (self.uri, cf=self.cf) def __enter__(self): """opens the link to the drone and sets the initial position and resets the estimator. Returns the object of the Drone class """ self.scf.open_link() self.set_initial_position() self.reset_estimator() return self ################################################################################ # Check for "safe" drone position ################################################################################ def wait_for_position_estimator(self): """waits until the estimator has determined the position of the drones """ print('Waiting for estimator to find position...') log_config = LogConfig(name='Kalman Variance', period_in_ms=500) log_config.add_variable('kalman.varPX', 'float') log_config.add_variable('kalman.varPY', 'float') log_config.add_variable('kalman.varPZ', 'float') var_y_history = [1000] * 10 var_x_history = [1000] * 10 var_z_history = [1000] * 10 threshold = 0.001 with SyncLogger(self.scf, log_config) as logger: for log_entry in logger: data = log_entry[1] var_x_history.append(data['kalman.varPX']) var_x_history.pop(0) var_y_history.append(data['kalman.varPY']) var_y_history.pop(0) var_z_history.append(data['kalman.varPZ']) var_z_history.pop(0) min_x = min(var_x_history) max_x = max(var_x_history) min_y = min(var_y_history) max_y = max(var_y_history) min_z = min(var_z_history) max_z = max(var_z_history) # print("{} {} {}". # format(max_x - min_x, max_y - min_y, max_z - min_z)) if (max_x - min_x) < threshold and ( max_y - min_y) < threshold and ( max_z - min_z) < threshold: break def set_initial_position(self): """Sets the given intial variables to the drone params kalman.initial... """ self.scf.cf.param.set_value('kalman.initialX', self.initial_x) self.scf.cf.param.set_value('kalman.initialY', self.initial_y) self.scf.cf.param.set_value('kalman.initialZ', self.initial_z) yaw_radians = math.radians(self.initial_yaw) self.scf.cf.param.set_value('kalman.initialYaw', yaw_radians) def reset_estimator(self): """Resets the estimator of the drone. Calls wait_for_position_estimator afterwards""" cf = self.scf.cf cf.param.set_value('kalman.resetEstimation', '1') time.sleep(0.1) cf.param.set_value('kalman.resetEstimation', '0') self.wait_for_position_estimator() ################################################################################ # Higher Functionality like flying a Sequence # (get creative here) ################################################################################ #implement flyingSequence here def flySequence(self, sequence,time_between_commandos = 2, checkZSafety=True): """Lets the drone fly to the points specified in the sequence array. The sequence array should be like this: [(x1,y1,z1),(x2,y2,z2),...] The time_between_commandos parameter specifies the time the drone has to execute the fly command. (TODO: maybe adapt it to the distance between the points? Or change the speed?) If checkZSafety is True, after the last sequence element the program checks if the drone is more than 0.2m above the initial_z value (see initialization of the object). If thats true it sends an extra command (lastXValue,lastYValue, 0.1+initial_z) to avoid shutting the engines off while the drone is flying high. """ cf = self.scf.cf x = 0 y = 0 z = 0 for position in sequence: print('Setting position {}'.format(position)) x = position[0] + self.initial_x y = position[1] + self.initial_y z = position[2] + self.initial_z for i in range(10): cf.commander.send_position_setpoint(x, y, z, self.initial_yaw) time.sleep(time_between_commandos) if z - self.initial_z > 0.2: print("Sending additional position_setpoint to drone because z coordinate is {}m higher than the starting position. If you don't want that behaviour, set checkZSafety parameter to False".format(z-self.initial_z) ) cf.commander.send_position_setpoint(x, y, self.initial_z + 0.1, self.initial_yaw) time.sleep(time_between_commandos) cf.commander.send_stop_setpoint() # 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) def flyToPoint(self, x, y, z, yaw=-1000,position ="relative"): """Sends a command to the drone to fly to a specified point. X,Y,Z is the coordinate the drone should fly to. yaw is the direction the drone should "look" position = relative: the x,y,z values will be added to the initial_x/y/z values. position = absolute: the x,y,z values will be send to the drone directly. WARNING: only use when you know what you are doing """ if yaw == -1000: yaw = self.initial_yaw if position == "relative" or position == "absolute": if position == "relative": x = x + self.initial_x y = y + self.initial_y z = z + self.initial_z for i in range(10): self.scf.cf.commander.send_position_setpoint(x,y,z,yaw) time.sleep(0.1) if position == "absolute": self.scf.cf.commander.send_position_setpoint(x,y,z,yaw) def land(self): for i in range(20): self.scf.cf.commander.send_position_setpoint(self.initial_x,self.initial_y,self.initial_z+0.1,self.initial_yaw) time.sleep(0.1) self.scf.cf.commander.send_stop_setpoint()
class Node: def __init__(self): self.vx = 0 self.vy = 0 self.vz = 0 self.va = 0 self.vel = Twist() self.pose = Pose() self.vel.linear.x = self.vx self.vel.linear.y = self.vy self.vel.linear.z = self.vz self.vel.angular.z = self.va cflib.crtp.init_drivers(enable_debug_driver=False) self.crazyflie = SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) self.commander = MotionCommander(self.crazyflie) self.cf = Crazyflie() self.crazyflie.open_link() self.commander.take_off() def write_to_file(self, data): # Two loggers should yield an even number of rows of data # being collected in the end. # There is one packet missing if the array only contains # an even number of rows of data. if len(data) % 2 is not 0: data = data[:len(data) - 1] temp_df = pd.DataFrame(data) m, _ = temp_df.shape even_rows = temp_df.iloc[np.arange(0, m, 2), :] odd_rows = temp_df.iloc[np.arange(1, m, 2), :] columns = [ 'timestamp_start', 'timestamp_end', 'acc.x', 'acc.y', 'acc.z', 'gyro.x', 'gyro.y', 'gyro.z', 'stateEstimate.x', 'stateEstimate.y', 'stateEstimate.z', 'stabilizer.pitch', 'stabilizer.roll', 'stabilizer.yaw' ] df = pd.DataFrame(data=np.zeros((int(m / 2), 14)), columns=columns) df[['gyro.x', 'gyro.y', 'gyro.z']] = np.array(even_rows[['gyro.x', 'gyro.y', 'gyro.z']]) df[['stabilizer.pitch', 'stabilizer.roll', 'stabilizer.yaw']] = np.array(even_rows[[ 'stabilizer.pitch', 'stabilizer.roll', 'stabilizer.yaw' ]]) df[["acc.x", "acc.y", "acc.z"]] = np.array(odd_rows[["acc.x", "acc.y", "acc.z"]]) df[["stateEstimate.x", "stateEstimate.y", "stateEstimate.z"]] = \ np.array(odd_rows[["stateEstimate.x", "stateEstimate.y", "stateEstimate.z"]]) df['timestamp_start'] = np.array(even_rows.timestamp) df['timestamp_end'] = np.array(odd_rows.timestamp) # df.to_csv("data/project2/drone2/data_set_label_" # +class_label+"_packet_"+packet_num+".csv") df.to_csv("test.csv") def write(self, data): print(data) def log1(self): lg1 = LogConfig(name='pose_acc', period_in_ms=10) lg1.add_variable('stateEstimate.x', 'float') lg1.add_variable('stateEstimate.y', 'float') lg1.add_variable('stateEstimate.z', 'float') lg1.add_variable('acc.x', 'float') lg1.add_variable('acc.y', 'float') lg1.add_variable('acc.z', 'float') return lg1 def log2(self): lg2 = LogConfig(name='stabilizer_gyro', period_in_ms=10) lg2.add_variable('stabilizer.roll', 'float') lg2.add_variable('stabilizer.pitch', 'float') lg2.add_variable('stabilizer.yaw', 'float') lg2.add_variable('gyro.x', 'float') lg2.add_variable('gyro.y', 'float') lg2.add_variable('gyro.z', 'float') return lg2 def sync(self, position_pub, data): switch = 0 with SyncLogger(self.crazyflie, self.log1()) as logger1, \ SyncLogger(self.crazyflie, self.log2()) as logger2: end_time = time.time() + 24 while time.time() < end_time: if switch == 0: logger = logger2 elif switch == 1: logger = logger1 for log_entry in logger: row = log_entry[1] row["timestamp"] = log_entry[0] if switch == 1: x = row['stateEstimate.x'] y = row['stateEstimate.y'] z = row['stateEstimate.z'] self.pose.position.x = x self.pose.position.y = y self.pose.position.z = z position_pub.publish(self.pose) print('x:', x, ' y:', y, ' z:', z) print() data.append(row) switch += 1 break if switch == 2: switch = 0 return None def shut_down(self): self.crazyflie.close_link() def cmd_vel(self, msg): self.vx = msg.linear.x self.vy = msg.linear.y self.vz = msg.linear.z self.va = msg.angular.z self.commander._set_vel_setpoint(self.vx, self.vy, self.vz, self.va) print(self.vx, self.vy, self.vz, self.va)
class CrazyflieConnection(connection.Connection): DEFAULT_VELOCITY = 0.2 # [m/s] the default velocity to use for position commands 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] # state information is to be updated and managed by this connection class # for the crazyflie, since the crazyflie doesn't exactly pass down the # state information # # defining the states to be: # armed -> should roughly mimic connection state (though this does have a problem at the end...) # guided -> this seems to only be used at the end condition..... @property def open(self): """ Returns: Boolean. True if connection is able to send and/or receive messages, False otherwise. """ # TODO: figure out the open condition for crazyflie if self._is_open == -1: return False return True def start(self): """Command to start a connection with a drone""" self._scf.open_link( ) # this is a blocking function that will not return until the link is opened # TODO: need a better version of this self._is_open = True # need to now register for callbacks on the data of interest from the crazyflie # TODO: decide on the appropriate rates log_pos = LogConfig(name='LocalPosition', period_in_ms=100) log_pos.add_variable('kalman.stateX', 'float') log_pos.add_variable('kalman.stateY', 'float') log_pos.add_variable('kalman.stateZ', 'float') try: self._scf.cf.log.add_config(log_pos) # This callback will receive the data log_pos.data_received_cb.add_callback(self._cf_callback_pos) # This callback will be called on errors log_pos.error_cb.add_callback(self._cf_callback_error) # Start the logging log_pos.start() except KeyError as e: print('Could not start position log configuration,' '{} not found in TOC'.format(str(e))) except AttributeError: print('Could not add Position log config, bad configuration.') log_vel = LogConfig(name='LocalVelocity', period_in_ms=500) log_vel.add_variable('kalman.statePX', 'float') log_vel.add_variable('kalman.statePY', 'float') log_vel.add_variable('kalman.statePZ', 'float') try: self._scf.cf.log.add_config(log_vel) # This callback will receive the data log_vel.data_received_cb.add_callback(self._cf_callback_vel) # This callback will be called on errors log_vel.error_cb.add_callback(self._cf_callback_error) # Start the logging log_vel.start() except KeyError as e: print('Could not start velocity log configuration,' '{} not found in TOC'.format(str(e))) except AttributeError: print('Could not add velocity log config, bad configuration.') log_att = LogConfig(name='Attitude', period_in_ms=100) log_att.add_variable('stabilizer.roll', 'float') log_att.add_variable('stabilizer.pitch', 'float') log_att.add_variable('stabilizer.yaw', 'float') try: self._scf.cf.log.add_config(log_att) # This callback will receive the data log_att.data_received_cb.add_callback(self._cf_callback_att) # This callback will be called on errors log_att.error_cb.add_callback(self._cf_callback_error) # Start the logging log_att.start() except KeyError as e: print('Could not start attitude log configuration,' '{} not found in TOC'.format(str(e))) except AttributeError: print('Could not add attitude log config, bad configuration.') log_state = LogConfig(name='State', period_in_ms=1000) log_state.add_variable('kalman.inFlight', 'uint8_t') # TODO: check the data type try: self._scf.cf.log.add_config(log_state) log_state.data_received_cb.add_callback(self._cf_callback_state) log_state.error_cb.add_callback(self._cf_callback_error) # Start the logging log_state.start() except KeyError as e: print('Could not start position log configuration,' '{} not found in TOC'.format(str(e))) except AttributeError: print('Could not add state log config, bad configuration.') # start the write thread now that the connection is open self._running = True self._write_handle.start() def stop(self): """Command to stop a connection with a drone""" # need to send a stop command cmd = CrazyflieCommand(CrazyflieCommand.CMD_TYPE_STOP, None) self._out_msg_queue.put(cmd) time.sleep( 0.5 ) # add a sleep to make sure this command is sent and executed properly self._running = False # this is to stop the command thread time.sleep(1) # make sure the command thread has stopped self._scf.close_link() # close the link # TODO: find a better way to handle this... self._is_open = False def dispatch_loop(self): """Main loop that triggers callbacks as messages are recevied""" # NOTE: nothing needed here for the crazyflie # NOTE: the crazyflie API sends data down as callbacks already, so this # NOTE: connection class is nothing but a passthrough for those pass def command_loop(self): """loop to send commands at a specified rate""" # the last time a command was sent # to be used to ensure commands are at the desired rate last_write_time = time.time() cmd_start_time = 0 # the time [s] that the command started -> needed for distance commands # the current command that should be being sent, default to 0 everything current_cmd = CrazyflieCommand(CrazyflieCommand.CMD_TYPE_STOP, None) # the last commanded height # if this is not 0, want the hold commands to be hover to hold the specific height current_height = 0 while self._running: # empty out the queue of pending messages -> want to always send the messages asap # NOTE: this effectively only sends the last command in the queue.... # TODO: see if need to handle the fact that maybe want to send all the commands in the queue... cmd = None new_cmd = False while not self._out_msg_queue.empty(): try: cmd = self._out_msg_queue.get_nowait() except queue.Empty: # if there is no msgs in the queue, will just continue pass else: if cmd is not None: new_cmd = True current_cmd = cmd cmd_start_time = time.time() # TODO: maybe want to handle the command here... self._out_msg_queue.task_done() # DEBUG # print("recevied command, type {}, cmd {}, delay {}".format( # current_cmd.type, current_cmd.cmd, current_cmd.delay)) # first thing to check is the timer, if applicable if current_cmd.delay is not None: if time.time() - cmd_start_time >= current_cmd.delay: # DEBUG # print("command timer completed, completed command: {}, {}".format( # current_cmd.type, current_cmd.cmd)) # time to stop new_cmd = True if current_height > 0: current_cmd = CrazyflieCommand( CrazyflieCommand.CMD_TYPE_HOVER, (0.0, 0.0, 0.0, current_height)) else: current_cmd = CrazyflieCommand( CrazyflieCommand.CMD_TYPE_VELOCITY, (0.0, 0.0, 0.0, 0.0)) # if this isn't a new command, want to rate limit accordingly # rate limit the loop if not new_cmd: current_time = time.time() if (current_time - last_write_time) < (1.0 / self._send_rate): continue last_write_time = time.time() # DEBUG # print("sending command, type {}, cmd {}, delay {}".format( # current_cmd.type, current_cmd.cmd, current_cmd.delay)) # TODO: probably need to constantly update the velocity commands here.... # TODO: effectively need to wrap a high level control loop, at the very least on height # now do the actual sending of the command if current_cmd.type == CrazyflieCommand.CMD_TYPE_VELOCITY: self._scf.cf.commander.send_velocity_world_setpoint( *current_cmd.cmd) elif current_cmd.type == CrazyflieCommand.CMD_TYPE_HOVER: current_height = current_cmd.cmd[3] self._scf.cf.commander.send_hover_setpoint(*current_cmd.cmd) elif current_cmd.type == CrazyflieCommand.CMD_TYPE_ATTITUDE_THRUST: self._scf.cf.commander.send_setpoint(*current_cmd.cmd) elif current_cmd.type == CrazyflieCommand.CMD_TYPE_ATTITUDE_DIST: current_height = current_cmd.cmd[3] self._scf.cf.commander.send_zdistance_setpoint( *current_cmd.cmd) elif current_cmd.type == CrazyflieCommand.CMD_TYPE_STOP: # TODO: probably want to send appropriate flags that state disarmed, etc # TODO: basically need to update the drone state here self._scf.cf.commander.send_stop_setpoint() else: print("invalid command type!") def _cf_callback_pos(self, timestamp, data, logconf): x = data['kalman.stateX'] y = data['kalman.stateY'] z = data['kalman.stateZ'] # print("current height: {}".format(z)) self._current_position_xyz = [x, y, z] # save for our internal use pos = mt.LocalFrameMessage(timestamp, x, -y, -z) self.notify_message_listeners(MsgID.LOCAL_POSITION, pos) def _cf_callback_vel(self, timestamp, data, logconf): x = data['kalman.statePX'] y = data['kalman.statePY'] z = data['kalman.statePZ'] vel = mt.LocalFrameMessage(timestamp, x, y, z) self.notify_message_listeners(MsgID.LOCAL_VELOCITY, vel) def _cf_callback_att(self, timestamp, data, logconf): roll = data['stabilizer.roll'] pitch = data['stabilizer.pitch'] yaw = data['stabilizer.yaw'] fm = mt.FrameMessage(timestamp, roll, pitch, yaw) self.notify_message_listeners(MsgID.ATTITUDE, fm) def _cf_callback_state(self, timestamp, data, logconf): # in_flight = data['kalman.inFlight'] # armed = False # guided = False # if in_flight: # armed = True # guided = True # TODO: probably need a better metric for armed / guided # since the quad is basically always armed and guided comes into play # once the connection is made, so basically the second the script starts... # state = mt.StateMessage(timestamp, armed, guided) # self.notify_message_listeners(MsgID.STATE, state) pass def _cf_callback_error(self, logconf, msg): print('Error when logging %s: %s' % (logconf.name, msg)) def _reset_position_estimator(self): """reset the estimator to give the best performance possible""" self._scf.cf.param.set_value('kalman.resetEstimation', '1') time.sleep(0.1) self._scf.cf.param.set_value('kalman.resetEstimation', '0') # TODO: instead of a sleep, probably want a condition on the variance time.sleep(2) def arm(self): """Command to arm the drone""" # NOTE: this doesn't exist for the crazyflie # TODO: could have arm or take control be where we reset the estimator... pass def disarm(self): """Command to disarm the drone""" # NOTE: this doesn't exist for the crazyflie pass def take_control(self): """ Command the drone to switch into a mode that allows external control. e.g. for PX4 this commands 'offboard' mode, while for APM this commands 'guided' mode """ # NOTE: this doesn't exist for the crazyflie pass def release_control(self): """Command to return the drone to a manual mode""" # NOTE: this doesn't exist for the crazyflie pass def cmd_attitude(self, roll, pitch, yawrate, thrust): """Command to set the desired attitude and thrust Args: yaw: the desired yaw in radians pitch: the desired pitch in radians roll: the deisred roll in radians thrust: the normalized desired thrust level on [0, 1] """ pass def cmd_attitude_rate(self, roll_rate, pitch_rate, yaw_rate, thrust): """Command to set the desired attitude rates and thrust Args: yaw_rate: the desired yaw rate in radians/second pitch_rate: the desired pitch rate in radians/second roll_rate: the desired roll rate in radians/second thrust: the normalized desired thrust level on [0, 1] """ pass def cmd_moment(self, roll_moment, pitch_moment, yaw_moment, thrust): """Command to set the desired moments and thrust Args: roll_moment: the desired roll moment in Newton*meter yaw_moment: the desired yaw moment in Newton*meter pitch_moment: the desired pitch moment in Newton*meter thrust: the normalized desired thrust level in Newton """ pass def cmd_velocity(self, vn, ve, vd, heading): """Command to set the desired velocity (NED frame) and heading Args: vn: desired north velocity component in meters/second ve: desired east velocity component in meters/second vd: desired down velocity component in meters/second (note: positive down!) heading: desired drone heading in radians """ pass def cmd_motors(self, motor1, motor2, motor3, motor4): """Command the thrust levels for each motor on a quadcopter Args: motor1: normalized thrust level for motor 1 on [0, 1] motor2: normalized thrust level for motor 2 on [0, 1] motor3: normalized thrust level for motor 3 on [0, 1] motor4: normalized thrust level for motor 4 on [0, 1] """ pass def cmd_position(self, n, e, d, heading): """ NOTE: THIS CURRENTLY COMMANDS RELATIVE POSITION!!!!! BODY ALIGNED!!!! """ """Command to set the desired position (NED frame) and heading Args: n: desired north position in meters e: desired east position in meters d: desired down position in meters (note: positive down!) heading: desired drone heading in radians """ # need to know the current position: for now going to simply map NED to XYZ!!! # x is forward # y is left # z is up # also completely ignoring heading for now # DEBUG # print("current position (x,y,z): ({}, {}, {})".format( # self._current_position_xyz[0], self._current_position_xyz[1], self._current_position_xyz[2])) # print("commanded position (x,y,z): ({}, {}, {})".format(n, -e, -d)) # calculate the change vector needed # note the slight oddity that happens in converting NED to XYZ # as things are used as XYZ internally for the crazyflie dx = n - self._current_position_xyz[0] dy = -e - self._current_position_xyz[1] z = -1 * d # holding a specific altitude, so just pass altitude through directly # DEBUG # print("move vector: ({}, {}) at height {}".format(dx, dy, z)) distance = math.sqrt(dx * dx + dy * dy) delay_time = distance / self.DEFAULT_VELOCITY # DEBUG # print("the delay time for the move command: {}".format(delay_time)) # need to now calculate the velocity vector -> need to have a magnitude of default velocity vx = self.DEFAULT_VELOCITY * dx / distance vy = self.DEFAULT_VELOCITY * dy / distance # DEBUG # print("vel vector: ({}, {})".format(vx, vy)) # create and send the command # TODO: determine if would want to use the hover command instead of the velocity command.... # TODO: problem with the hover command is have no feedback on the current altitude!! cmd = CrazyflieCommand(CrazyflieCommand.CMD_TYPE_HOVER, (vx, vy, 0.0, z), delay_time) self._out_msg_queue.put(cmd) def cmd_relative_position(self, dx, dy, z, heading): print("move vector: ({}, {}) at height {}".format(dx, dy, z)) distance = math.sqrt(dx * dx + dy * dy) delay_time = distance / self.DEFAULT_VELOCITY print("the delay time for the move command: {}".format(delay_time)) # need to now calculate the velocity vector -> need to have a magnitude of default velocity vx = self.DEFAULT_VELOCITY * dx / distance vy = self.DEFAULT_VELOCITY * dy / distance print("vel vector: ({}, {})".format(vx, vy)) # create and send the command # TODO: determine if would want to use the hover command instead of the velocity command.... # TODO: problem with the hover command is have no feedback on the current altitude!! cmd = CrazyflieCommand(CrazyflieCommand.CMD_TYPE_HOVER, (vx, vy, 0.0, z), delay_time) self._out_msg_queue.put(cmd) def takeoff(self, n, e, d): """Command the drone to takeoff. Note some autopilots need a full position for takeoff and since this class is not aware of current position.`n` and `e` must be passed along with `d` for this command. Args: n: current north position in meters e: current east position in meters altitde: desired altitude """ # first step: reset the estimator to make sure all is good self._reset_position_estimator() # TODO: add to queue a command with 0 x,y vel, 0 yawrate, and the desired height off the ground cmd = CrazyflieCommand(CrazyflieCommand.CMD_TYPE_HOVER, (0.0, 0.0, 0.0, d)) self._out_msg_queue.put(cmd) def land(self, n, e): """Command the drone to land. Note some autopilots need a full position for landing and since this class is not aware of current position.`n` and `e` must be passed along with `d` for this command. Args: n: current north position in meters e: current east position in meters """ # need to know the current height here... current_height = self._current_position_xyz[2] decent_velocity = -self.DEFAULT_VELOCITY # [m/s] # calculate how long that command should be executed for # we aren't going to go all the way down before then sending a stop command # TODO: figure out a way to do this without sleeping!! delay_time = (current_height - 0.02) / (-1 * decent_velocity ) # the wait time in seconds # DEBUG # print("current height: {}, delay time: {}".format(current_height, delay_time)); cmd = CrazyflieCommand(CrazyflieCommand.CMD_TYPE_VELOCITY, (0.0, 0.0, decent_velocity, 0.0), delay_time) self._out_msg_queue.put(cmd) # wait the desired amount of time and then send a stop command to kill the motors time.sleep(delay_time) self._out_msg_queue.put( CrazyflieCommand(CrazyflieCommand.CMD_TYPE_STOP, None)) def set_home_position(self, lat, lon, alt): """Command to change the home position of the drone. Args: lat: desired home latitude in decimal degrees lon: desired home longitude in decimal degrees alt: desired home altitude in meters (AMSL) """ # NOTE: this concept doesn't exist for the crazyflie pass
class SyncCrazyflieTest(unittest.TestCase): def setUp(self): self.uri = 'radio://0/60/2M' 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]).trigger self.sut = SyncCrazyflie(self.uri, self.cf_mock) 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 test_open_link(self): # Fixture # Test self.sut.open_link() # Assert self.assertTrue(self.sut.is_link_open()) def test_failed_open_link_raises_exception(self): # Fixture expected = 'Some error message' self.cf_mock.open_link = AsyncCallbackCaller( cb=self.cf_mock.connection_failed, args=[self.uri, expected]).trigger # Test try: self.sut.open_link() except Exception as e: actual = e.args[0] else: self.fail('Expect exception') # Assert self.assertEqual(expected, actual) self._assertAllCallbacksAreRemoved() def test_open_link_of_already_open_link_raises_exception(self): # Fixture self.sut.open_link() # Test # Assert with self.assertRaises(Exception): self.sut.open_link() def test_close_link(self): # Fixture self.sut.open_link() # Test self.sut.close_link() # Assert self.cf_mock.close_link.assert_called_once_with() self.assertFalse(self.sut.is_link_open()) self._assertAllCallbacksAreRemoved() def test_close_link_that_is_not_open(self): # Fixture # Test self.sut.close_link() # Assert self.assertFalse(self.sut.is_link_open()) def test_closed_if_connection_is_lost(self): # Fixture self.sut.open_link() # Test AsyncCallbackCaller( cb=self.cf_mock.disconnected, args=[self.uri]).call_and_wait() # Assert self.assertFalse(self.sut.is_link_open()) self._assertAllCallbacksAreRemoved() def test_open_close_with_context_mangement(self): # Fixture # Test with SyncCrazyflie(self.uri, self.cf_mock) as sut: self.assertTrue(sut.is_link_open()) # Assert self.cf_mock.close_link.assert_called_once_with() self._assertAllCallbacksAreRemoved() def _assertAllCallbacksAreRemoved(self): self.assertEqual(0, len(self.cf_mock.connected.callbacks)) self.assertEqual(0, len(self.cf_mock.connection_failed.callbacks)) self.assertEqual(0, len(self.cf_mock.disconnected.callbacks))
class CrazyflieConnection(connection.Connection): def __init__(self, uri, velocity=0.2, threaded=False): super().__init__(threaded) # set the default velocity self._velocity = 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 = 50 # 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 = np.array([0.0, 0.0, 0.0]) # [x, y, z] # due to a "bug" in the crazyflie's position estimator with the flow # deck that results in the estimator to reset the position to 0,0,0 mid # flight if there are changes in lighting or terrain (note: may also # be under other conditions, but so far only seen in those conditions) self._dynamic_home_xyz = np.array([0.0, 0.0, 0.0]) # [x, y, z] self._home_position_xyz = np.array([0.0, 0.0, 0.0]) # [x, y, z] self._cmd_position_xyz = np.array([0.0, 0.0, 0.0]) # the commanded position # state information is to be updated and managed by this connection class # for the crazyflie, since the crazyflie doesn't exactly pass down the # state information # # defining the states to be: # armed -> should roughly mimic connection state (though this does have a problem at the end...) # guided -> this seems to only be used at the end condition..... self._armed = True self._guided = True # kalman filter state self._converged = False self._var_y_history = [1000] * 10 self._var_x_history = [1000] * 10 self._var_z_history = [1000] * 10 self._filter_threshold = 0.001 @property def open(self): """ Returns: Boolean. True if connection is able to send and/or receive messages, False otherwise. """ # TODO: figure out the open condition for crazyflie if self._is_open == -1: return False return True def start(self): """Command to start a connection with a drone""" self._scf.open_link( ) # this is a blocking function that will not return until the link is opened # TODO: need a better version of this self._is_open = True # need to now register for callbacks on the data of interest from the crazyflie # TODO: decide on the appropriate rates log_pos = LogConfig(name='LocalPosition', period_in_ms=150) log_pos.add_variable('kalman.stateX', 'float') log_pos.add_variable('kalman.stateY', 'float') log_pos.add_variable('kalman.stateZ', 'float') try: self._scf.cf.log.add_config(log_pos) # This callback will receive the data log_pos.data_received_cb.add_callback(self._cf_callback_pos) # This callback will be called on errors log_pos.error_cb.add_callback(self._cf_callback_error) # Start the logging log_pos.start() except KeyError as e: print('Could not start position log configuration,' '{} not found in TOC'.format(str(e))) except AttributeError: print('Could not add Position log config, bad configuration.') log_vel = LogConfig(name='LocalVelocity', period_in_ms=100) log_vel.add_variable('kalman.statePX', 'float') log_vel.add_variable('kalman.statePY', 'float') log_vel.add_variable('kalman.statePZ', 'float') try: self._scf.cf.log.add_config(log_vel) # This callback will receive the data log_vel.data_received_cb.add_callback(self._cf_callback_vel) # This callback will be called on errors log_vel.error_cb.add_callback(self._cf_callback_error) # Start the logging log_vel.start() except KeyError as e: print('Could not start velocity log configuration,' '{} not found in TOC'.format(str(e))) except AttributeError: print('Could not add velocity log config, bad configuration.') log_att = LogConfig(name='Attitude', period_in_ms=500) log_att.add_variable('stabilizer.roll', 'float') log_att.add_variable('stabilizer.pitch', 'float') log_att.add_variable('stabilizer.yaw', 'float') try: self._scf.cf.log.add_config(log_att) # This callback will receive the data log_att.data_received_cb.add_callback(self._cf_callback_att) # This callback will be called on errors log_att.error_cb.add_callback(self._cf_callback_error) # Start the logging log_att.start() except KeyError as e: print('Could not start attitude log configuration,' '{} not found in TOC'.format(str(e))) except AttributeError: print('Could not add attitude log config, bad configuration.') log_state = LogConfig(name='State', period_in_ms=1000) log_state.add_variable('kalman.inFlight', 'uint8_t') # TODO: check the data type try: self._scf.cf.log.add_config(log_state) log_state.data_received_cb.add_callback(self._cf_callback_state) log_state.error_cb.add_callback(self._cf_callback_error) # Start the logging log_state.start() except KeyError as e: print('Could not start position log configuration,' '{} not found in TOC'.format(str(e))) except AttributeError: print('Could not add state log config, bad configuration.') # start the write thread now that the connection is open self._running = True self._write_handle.start() # reset the estimator self._reset_position_estimator() def stop(self): """Command to stop a connection with a drone""" # need to send a stop command cmd = CrazyflieCommand(CrazyflieCommand.CMD_TYPE_STOP, None) self._out_msg_queue.put(cmd) time.sleep( 0.5 ) # add a sleep to make sure this command is sent and executed properly self._running = False # this is to stop the command thread time.sleep(1) # make sure the command thread has stopped self._scf.close_link() # close the link # TODO: find a better way to handle this... self._is_open = False def dispatch_loop(self): """Main loop that triggers callbacks as messages are recevied""" # NOTE: nothing needed here for the crazyflie # NOTE: the crazyflie API sends data down as callbacks already, so this # NOTE: connection class is nothing but a passthrough for those pass def _send_command(self, cmd): """helper function to send the appropriate CF command based on the desired command type, send the corresponding setpoint command to the crazyflie. Args: cmd: the CrazyflieCommand to send """ # based on the command type, send the appropriate crazyflie command if cmd.type == CrazyflieCommand.CMD_TYPE_VELOCITY: self._scf.cf.commander.send_velocity_world_setpoint(*cmd.cmd) elif cmd.type == CrazyflieCommand.CMD_TYPE_HOVER: # TODO: see if maybe want to get this current height information # back to the write loop... # current_height = cmd.cmd[3] self._scf.cf.commander.send_hover_setpoint(*cmd.cmd) elif cmd.type == CrazyflieCommand.CMD_TYPE_ATTITUDE_THRUST: self._scf.cf.commander.send_setpoint(*cmd.cmd) elif cmd.type == CrazyflieCommand.CMD_TYPE_ATTITUDE_DIST: # current_height = cmd.cmd[3] self._scf.cf.commander.send_zdistance_setpoint(*cmd.cmd) elif cmd.type == CrazyflieCommand.CMD_TYPE_STOP: # TODO: probably want to send appropriate flags that state disarmed, etc # TODO: basically need to update the drone state here self._scf.cf.commander.send_stop_setpoint() else: print("invalid command type!") def command_loop(self): """loop to send commands at a specified rate""" # the last time a command was sent # to be used to ensure commands are at the desired rate last_write_time = time.time() cmd_start_time = 0 # the time [s] that the command started -> needed for distance commands # the current command that should be being sent, default to 0 everything # current_cmd = CrazyflieCommand(CrazyflieCommand.CMD_TYPE_STOP, None) current_cmd = CrazyflieCommand( CrazyflieCommand.CMD_TYPE_ATTITUDE_THRUST, (0, 0, 0, 0), None) # the last commanded height # if this is not 0, want the hold commands to be hover to hold the specific height current_height = 0 while self._running: # want to make sure the kalman filter has converged before sending any command while not self._converged: self._send_command( CrazyflieCommand(CrazyflieCommand.CMD_TYPE_ATTITUDE_THRUST, (0, 0, 0, 0), None)) continue # empty out the queue of pending messages -> want to always send the messages asap # NOTE: the commands are immediately sent, which can result in fast back to back command sets, but # ensures all the commands are sent cmd = None while not self._out_msg_queue.empty(): try: cmd = self._out_msg_queue.get_nowait() except queue.Empty: # if there is no msgs in the queue, will just continue pass else: if cmd is not None: current_cmd = cmd cmd_start_time = time.time() # mark this entity as being parsed self._out_msg_queue.task_done() # convert the position command to a velocity (hover) command if needed if cmd.type == CrazyflieCommand.CMD_TYPE_POSITION: current_cmd = self._pos_cmd_to_cf_vel_cmd( np.array(cmd.cmd[0:3]), cmd.cmd[3]) # immediately handle the new command self._send_command(current_cmd) # DEBUG # print("recevied command, type {}, cmd {}, delay {}".format( # current_cmd.type, current_cmd.cmd, current_cmd.delay)) # now that have handled any potentially new commands, let's handle the timer if current_cmd.delay is not None: if time.time() - cmd_start_time >= current_cmd.delay: # DEBUG # print("command timer completed, completed command: {}, {}".format( # current_cmd.type, current_cmd.cmd)) # time to stop -> want to hold the commanded height (instead of the current height as current # height may drift and that will cause the crazyflie to bob a lot) current_height = self._cmd_position_xyz[2] # print("stopping and holding a cmd height of {}".format(current_height)) if current_height > 0.05: current_cmd = CrazyflieCommand( CrazyflieCommand.CMD_TYPE_HOVER, (0.0, 0.0, 0.0, current_height)) else: current_cmd = CrazyflieCommand( CrazyflieCommand.CMD_TYPE_VELOCITY, (0.0, 0.0, 0.0, 0.0)) # now immediately handle the new command self._send_command(current_cmd) # want to make sure that the commands are set at minimum specified rate # so send the command again if that rate timer requires it current_time = time.time() if (current_time - last_write_time) < (1.0 / self._send_rate): continue # resend the command and update the timestamp for when the last command was sent last_write_time = current_time self._send_command(current_cmd) def _cf_callback_pos(self, timestamp, data, logconf): """callback on the crazyflie's position update""" x = data['kalman.stateX'] y = data['kalman.stateY'] z = data['kalman.stateZ'] # print("current height: {}".format(z)) # compute a difference between the previou current position and this one # if there is a large jump, that means there is a chance the estimator has reset! new_position = np.array([x, y, z]) dpos = new_position - self._current_position_xyz dx = dpos[0] dy = dpos[1] pos_change = math.sqrt(dx * dx + dy * dy) # DEBUG # print("position change: ({}, {})".format(dx, dy)) # TODO: find the correct limit here for defining a jump if pos_change >= 1: # print("esitmator has reset, adjusting home position") self._dynamic_home_xyz += np.array([dx, dy, 0]) # print("\tdynamic adjustment = ({}, {})".format(self._dynamic_home_xyz[0], self._dynamic_home_xyz[1])) self._current_position_xyz = np.array([x, y, z]) # save for our internal use # the position that should be published is an NED position, adjusted for the set home position adjusted_pos = self._current_position_xyz - self._home_position_xyz - self._dynamic_home_xyz pos = mt.LocalFrameMessage(timestamp, adjusted_pos[0], -adjusted_pos[1], -adjusted_pos[2]) self.notify_message_listeners(MsgID.LOCAL_POSITION, pos) def _cf_callback_vel(self, timestamp, data, logconf): """callback on the crazyflie's velocity update""" if not self._converged: return x = data['kalman.statePX'] y = data['kalman.statePY'] z = data['kalman.statePZ'] vel = mt.LocalFrameMessage(timestamp, x, -y, -z) self.notify_message_listeners(MsgID.LOCAL_VELOCITY, vel) def _cf_callback_att(self, timestamp, data, logconf): """callback on the crazyflie's attitude update""" if not self._converged: return roll = data['stabilizer.roll'] pitch = data['stabilizer.pitch'] yaw = data['stabilizer.yaw'] fm = mt.FrameMessage(timestamp, roll, pitch, yaw) self.notify_message_listeners(MsgID.ATTITUDE, fm) def _cf_callback_state(self, timestamp, data, logconf): """callback on the crazyflie's state information""" # in_flight = data['kalman.inFlight'] # armed = False # guided = False # if in_flight: # armed = True # guided = True # send a state message to the drone to set armed and guided to be True # since these constructs don't exist for the crazyflie, but the armed -> guided transition needs to be # robust to work from the sim to the crazyflie with minimal changes state = mt.StateMessage(timestamp, self._armed, self._guided) self.notify_message_listeners(MsgID.STATE, state) # TODO: probably need a better metric for armed / guided # since the quad is basically always armed and guided comes into play # once the connection is made, so basically the second the script starts... # state = mt.StateMessage(timestamp, armed, guided) # self.notify_message_listeners(MsgID.STATE, state) pass def _cf_callback_kf_variance(self, timestamp, data, logconf): """callback on the crazyflie's KF varaince information""" self._var_x_history.append(data['kalman.varPX']) self._var_x_history.pop(0) self._var_y_history.append(data['kalman.varPY']) self._var_y_history.pop(0) self._var_z_history.append(data['kalman.varPZ']) self._var_z_history.pop(0) min_x = min(self._var_x_history) max_x = max(self._var_x_history) min_y = min(self._var_y_history) max_y = max(self._var_y_history) min_z = min(self._var_z_history) max_z = max(self._var_z_history) # print("filter variances: {} {} {}".format(max_x - min_x, max_y - min_y, max_z - min_z)) dx = max_x - min_x dy = max_y - min_y dz = max_z - min_z if dx < self._filter_threshold and dy < self._filter_threshold and dz < self._filter_threshold: print("filter has converge, position is good!") self._converged = True self._kf_log_config.stop( ) # no longer care to keep getting the kalman filter variance def _cf_callback_error(self, logconf, msg): """callback for an error from one of the loggers""" print('Error when logging %s: %s' % (logconf.name, msg)) def _wait_for_position_estimator(self): """Start listening for the kalman filter variance to determine when it converges""" print('Waiting for estimator to find position...') # configure the log for the variance self._kf_log_config = LogConfig(name='Kalman Variance', period_in_ms=100) self._kf_log_config.add_variable('kalman.varPX', 'float') self._kf_log_config.add_variable('kalman.varPY', 'float') self._kf_log_config.add_variable('kalman.varPZ', 'float') try: self._scf.cf.log.add_config(self._kf_log_config) # This callback will receive the data self._kf_log_config.data_received_cb.add_callback( self._cf_callback_kf_variance) # This callback will be called on errors self._kf_log_config.error_cb.add_callback(self._cf_callback_error) # Start the logging self._kf_log_config.start() except KeyError as e: print('Could not start kalman log configuration,' '{} not found in TOC'.format(str(e))) except AttributeError: print('Could not add kalman log config, bad configuration.') def _reset_position_estimator(self): """reset the estimator to give the best performance possible""" self._scf.cf.param.set_value('kalman.resetEstimation', '1') time.sleep(0.1) self._scf.cf.param.set_value('kalman.resetEstimation', '0') # wait for the variance of the estimator to become small enough self._wait_for_position_estimator() def _convert_to_cf_xyz(self, pos): """convert the position to a position in the crazyflie's current frame handle the conversion from the user's drone frame to the frame being used in the crazyflie. Args: pos: numpy array of the desired position in the XYZ frame Returns: the XYZ position vector in the crazyflie's coordinate frame numpy array """ return pos + self._dynamic_home_xyz + self._home_position_xyz def _create_velocity_cmd(self, dx, dy, z, heading): """helper function to create a velocity command given a desired change in position. computes the velocity command given the velocity setting and then passed in position change. also computes the necessary delay time to wait before finishing the command. Args: dx: distance to travel in the crazyflie's X direction dy: distance to travel in the crazyflie's Y direction dz: distance to travel in the crazyflie's Z direction z: the height above ground to hold heading: desired heading Returns: a velocity command (CF type HOVER) to execute the desired motion. CrazyflieCommand """ # calculate the distance needed to travel and the delay time for the command distance = math.sqrt(dx * dx + dy * dy) delay_time = distance / self._velocity print("the delay time for the move command: {}".format(delay_time)) # need to now calculate the velocity vector -> need to have a magnitude of default velocity vx = self._velocity * dx / distance vy = self._velocity * dy / distance # vz = self._velocity * dz / distance vz = 0 print("vel vector: ({}, {}, {})".format(vx, vy, vz)) # create and send the command # TODO: determine if would want to use the hover command instead of the velocity command.... # TODO: problem with the hover command is have no feedback on the current altitude!! # return CrazyflieCommand(CrazyflieCommand.CMD_TYPE_VELOCITY, (vx, vy, vz, 0.0), delay_time) return CrazyflieCommand(CrazyflieCommand.CMD_TYPE_HOVER, (vx, vy, 0.0, z), delay_time) def _pos_cmd_to_cf_vel_cmd(self, cmd_pos_xyz, heading): """convert an XYZ command from the user to a velocity command in the crazyflie's frame handles the translation that needs to occur to take into account the set home position and any potential dynamic home adjustments needed due to the estimator onboard the crazyflie resetting. then computes the velocity command and delay time needed to reach the desired point, creating the necessary crazyflie command to successfully fly to the commanded position. Note: the user's XYZ frame is a frame with (0,0) at the location at which `set_home_position()` was called. Args: cmd_pos_xyz: the desired position in the user's XYZ frame heading: the desired vehicle heading Returns: a velocity move command (CF type hover) required to reach the desired position. CrazyflieCommand """ # convert from the user's frame to the cf's internal frame cmd_pos_cf_xyz = self._convert_to_cf_xyz(cmd_pos_xyz) self._cmd_position_xyz = np.copy(cmd_pos_cf_xyz) # DEBUG - position info # print("current positions:") # print("\tvehicle: ({}, {}, {})".format(self._current_position_xyz[0], self._current_position_xyz[1], # self._current_position_xyz[2])) # print("\thome: ({}, {}, {})".format(self._home_position_xyz[0], self._home_position_xyz[1], # self._home_position_xyz[2])) # print("\tdynamic: ({}, {}, {})".format(self._dynamic_home_xyz[0], self._dynamic_home_xyz[1], # self._dynamic_home_xyz[2])) # DEBUG - command info # print("command detailed:") # print("\tuser xyz frame: ({}, {}, {})".format(cmd_pos_xyz[0], cmd_pos_xyz[1], cmd_pos_xyz[2])) # print("\tcf frame: ({}, {}, {})".format(cmd_pos_cf_xyz[0], cmd_pos_cf_xyz[1], cmd_pos_cf_xyz[2])) # calculate the change vector needed # note the slight oddity that happens in converting NED to XYZ # as things are used as XYZ internally for the crazyflie dx = cmd_pos_cf_xyz[0] - self._current_position_xyz[0] dy = cmd_pos_cf_xyz[1] - self._current_position_xyz[1] # dz = cmd_pos_cf_xyz[2] - self._current_position_xyz[2] z = cmd_pos_cf_xyz[2] return self._create_velocity_cmd(dx, dy, z, heading) def set_velocity(self, velocity): """set the velocity the drone should use in flight""" self._velocity = velocity def arm(self): """Command to arm the drone""" # NOTE: this doesn't exist for the crazyflie pass def disarm(self): """Command to disarm the drone""" # NOTE: this doesn't exist for the crazyflie pass def take_control(self): """ Command the drone to switch into a mode that allows external control. e.g. for PX4 this commands 'offboard' mode, while for APM this commands 'guided' mode """ # NOTE: this doesn't exist for the crazyflie # however, if this command is being used, want to make sure # the state output conforms to the expected changes self._armed = True self._guided = True def release_control(self): """Command to return the drone to a manual mode""" # NOTE: this doesn't exist for the crazyflie # however, if this command is being used, want to make sure # the state output conforms to the expected changes self._armed = False self._guided = False def cmd_attitude(self, roll, pitch, yaw, thrust): """Command to set the desired attitude and thrust Args: yaw: the desired yaw in radians pitch: the desired pitch in radians roll: the deisred roll in radians thrust: the normalized desired thrust level on [0, 1] """ # NOTE: for the crazyflie, the attitude commands are in degrees, so will need to adjust accordingly # TODO: crazyflie takes in roll/pitch/yaw, should figure out the impact of making this function correct # to the definition of cmd_attitude... not sure why commanding yaw rate here # NOTE: thrust is also a bit weird for the crazyflie, it's a value between 10001 and 60000 # with hover thrust being around 36850.0 roll_deg = np.degrees(roll) pitch_deg = -np.degrees( pitch ) # crazyflie is in an XYZ frame, so pitch direction is reversed yaw_deg = np.degrees(yaw) # map the thrust from [0, 1] to the crazyflie accepted [10000, 65000] thrust = thrust * 55000 + 10000 # thrust needs to be an int thrust = int(thrust) # NOTE: again no delay time as that is not used when sending commands at this level self._out_msg_queue.put( CrazyflieCommand(CrazyflieCommand.CMD_TYPE_ATTITUDE_THRUST, (roll_deg, pitch_deg, yaw_deg, thrust), None)) # self._out_msg_queue.put(CrazyflieCommand(CrazyflieCommand.CMD_TYPE_ATTITUDE_DIST, # (roll_deg, pitch_deg, yaw_deg, 0.5), # None)) def cmd_attitude_zdist(self, roll, pitch, yaw, altitude): """Command to set the desired attitude and altitude. This is a custom crazyflie command that has the crazyflie worry about holding altitude and attitude is controlled by the user Args: roll: the desired roll in [radians] pitch: the desired pitch in [radians] yaw: the desired yaw in [radians] altitude: the desired altitude in [m] """ roll_deg = np.degrees(roll) pitch_deg = np.degrees(pitch) yaw_deg = np.degrees(yaw) # no time delay here # create the attitude zdistance command self._out_msg_queue.put( CrazyflieCommand(CrazyflieCommand.CMD_TYPE_ATTITUDE_DIST, (roll_deg, pitch_deg, yaw_deg, altitude), None)) def cmd_attitude_rate(self, roll_rate, pitch_rate, yaw_rate, thrust): """Command to set the desired attitude rates and thrust Args: yaw_rate: the desired yaw rate in radians/second pitch_rate: the desired pitch rate in radians/second roll_rate: the desired roll rate in radians/second thrust: the normalized desired thrust level on [0, 1] """ pass def cmd_moment(self, roll_moment, pitch_moment, yaw_moment, thrust): """Command to set the desired moments and thrust Args: roll_moment: the desired roll moment in Newton*meter yaw_moment: the desired yaw moment in Newton*meter pitch_moment: the desired pitch moment in Newton*meter thrust: the normalized desired thrust level in Newton """ pass def cmd_velocity(self, vn, ve, vd, heading): """Command to set the desired velocity (NED frame) and heading Note: For the crazyflie, NED is defined when the crazyflie starts, not aligned with world NED Args: vn: desired north velocity component in meters/second ve: desired east velocity component in meters/second vd: desired down velocity component in meters/second (note: positive down!) heading: desired drone heading in radians """ # crazyflie works in an XYZ "world" frame, so need to convert from NED to XYZ vx = vn vy = -ve vz = -vd # TODO: crazyflie takes yaw_rate here, need to handle this correctly # for now, ignore all heading commands # for a velocity command the idea of a delay time doesn't exist, it's up to the user to make sure # that velocity commands keep getting sent delay_time = None self._out_msg_queue.put( CrazyflieCommand(CrazyflieCommand.CMD_TYPE_VELOCITY, (vx, vy, vz, 0.0), delay_time)) def cmd_motors(self, motor1, motor2, motor3, motor4): """Command the thrust levels for each motor on a quadcopter Args: motor1: normalized thrust level for motor 1 on [0, 1] motor2: normalized thrust level for motor 2 on [0, 1] motor3: normalized thrust level for motor 3 on [0, 1] motor4: normalized thrust level for motor 4 on [0, 1] """ pass def cmd_position(self, n, e, d, heading): """Command to set the desired position ("NED" frame) and heading Note: For the crazyflie, NED is really the body XYZ frame fixed unpon startup of the crazyflie to be a world frame Args: n: desired north position in meters e: desired east position in meters d: desired down position in meters (note: positive down!) heading: desired drone heading in radians """ # consider the waypoint as reached, so command the cf to stop current_height = self._cmd_position_xyz[2] stop_moving_cmd = CrazyflieCommand(CrazyflieCommand.CMD_TYPE_HOVER, (0.0, 0.0, 0.0, current_height)) self._out_msg_queue.put(stop_moving_cmd) # need to know the current position: for now going to simply map NED to XYZ!!! # x is forward # y is left # z is up # also completely ignoring heading for now # send a position command - this will allow the write loop # to use the most up to date information for generating the # corresponding velocity command cmd = CrazyflieCommand(CrazyflieCommand.CMD_TYPE_POSITION, (n, -e, -d, heading)) self._out_msg_queue.put(cmd) # # need to covert the commanded position to the crazyflie's # # "world" frame # cmd_pos_cf_xyz = self._convert_to_cf_xyz(cmd_pos_xyz) # # DEBUG - position info # print("current positions:") # print("\tvehicle: ({}, {}, {})".format( # self._current_position_xyz[0], # self._current_position_xyz[1], # self._current_position_xyz[2])) # print("\thome: ({}, {}, {})".format( # self._home_position_xyz[0], # self._home_position_xyz[1], # self._home_position_xyz[2])) # print("\tdynamic: ({}, {}, {})".format( # self._dynamic_home_xyz[0], # self._dynamic_home_xyz[1], # self._dynamic_home_xyz[2])) # # DEBUG - command info # print("command detailed:") # print("\tuser xyz frame: ({}, {}, {})".format(n, -e, -d)) # print("\tcf frame: ({}, {}, {})".format( # cmd_pos_cf_xyz[0], cmd_pos_cf_xyz[1], cmd_pos_cf_xyz[2])) # # calculate the change vector needed # # note the slight oddity that happens in converting NED to XYZ # # as things are used as XYZ internally for the crazyflie # dx = cmd_pos_cf_xyz[0] - self._current_position_xyz[0] # dy = cmd_pos_cf_xyz[1] - self._current_position_xyz[1] # z = cmd_pos_cf_xyz[2] # holding a specific altitude, so just pass altitude through directly # # DEBUG # # print("move vector: ({}, {}) at height {}".format(dx, dy, z)) # # command the relative position # self.cmd_relative_position(dx, dy, z, heading) def cmd_relative_position(self, dx, dy, dz, heading): print("move vector: ({}, {}, {})".format(dx, dy, dz)) # update the commanded position information # want to be able to keep track of the desired "world frame" # coordinates to be able to catch estimator errors. self._cmd_position_xyz = self._current_position_xyz + np.array( [dx, dy, dz]) # use the helper function for this cmd = self._create_velocity_cmd(dx, dy, self._cmd_position_xyz[2], heading) self._out_msg_queue.put(cmd) # distance = math.sqrt(dx * dx + dy * dy) # delay_time = distance / self._velocity # print("the delay time for the move command: {}".format(delay_time)) # # need to now calculate the velocity vector -> need to have a magnitude of default velocity # vx = self._velocity * dx / distance # vy = self._velocity * dy / distance # print("vel vector: ({}, {})".format(vx, vy)) # # create and send the command # # TODO: determine if would want to use the hover command instead of the velocity command.... # # TODO: problem with the hover command is have no feedback on the current altitude!! # cmd = CrazyflieCommand(CrazyflieCommand.CMD_TYPE_HOVER, (vx, vy, 0.0, z), delay_time) # self._out_msg_queue.put(cmd) def takeoff(self, n, e, d): """Command the drone to takeoff. Note some autopilots need a full position for takeoff and since this class is not aware of current position.`n` and `e` must be passed along with `d` for this command. Args: n: current north position in meters e: current east position in meters altitde: desired altitude """ # first step: reset the estimator to make sure all is good, this will take a variable amount of time # as it waits for the filter to converge before returning # self._reset_position_estimator() # set the command position self._cmd_position_xyz = np.copy(self._current_position_xyz) self._cmd_position_xyz[2] = -d # add to queue a command with 0 x,y vel, 0 yawrate, and the desired height off the ground cmd = CrazyflieCommand(CrazyflieCommand.CMD_TYPE_HOVER, (0.0, 0.0, 0.0, d)) self._out_msg_queue.put(cmd) def land(self, n, e): """Command the drone to land. Note some autopilots need a full position for landing and since this class is not aware of current position.`n` and `e` must be passed along with `d` for this command. Args: n: current north position in meters e: current east position in meters """ # set the command position self._cmd_position_xyz = np.copy(self._current_position_xyz) self._cmd_position_xyz[2] = 0 # need to know the current height here... current_height = self._current_position_xyz[2] decent_velocity = -self._velocity / 2 # [m/s] # calculate how long that command should be executed for # we aren't going to go all the way down before then sending a stop command # TODO: figure out a way to do this without sleeping!! delay_time = (current_height - 0.02) / (-1 * decent_velocity ) # the wait time in seconds # DEBUG # print("current height: {}, delay time: {}".format(current_height, delay_time)) # make sure delay time is always positive and non-zero if delay_time < 0: delay_time = 0.1 cmd = CrazyflieCommand(CrazyflieCommand.CMD_TYPE_VELOCITY, (0.0, 0.0, decent_velocity, 0.0), delay_time) self._out_msg_queue.put(cmd) # wait the desired amount of time and then send a stop command to kill the motors time.sleep(delay_time) self._out_msg_queue.put( CrazyflieCommand(CrazyflieCommand.CMD_TYPE_STOP, None)) def set_home_position(self, lat, lon, alt): """Command to change the home position of the drone. Note: for the crazyflie, there is no global position coordinates. Therefore when this command is called, the current local position of the crazyflie will be used as the home position. **This will therefore ignore all input arguments!** Args: lat: desired home latitude in decimal degrees lon: desired home longitude in decimal degrees alt: desired home altitude in meters (AMSL) """ # NOTE: for the crazyflie, this takes the current local position # and sets that value to home. # Therefore all of the inputs are ignored! # update the home position to be the current position # this will be added to all the waypoint commands to get the # proper coordinate to command self._home_position_xyz = self._current_position_xyz self._home_position_xyz[2] = 0.0 # for now keep this at 0 # want to reset the dynamic home adjustment at this point, since resetting the home position self._dynamic_home_xyz = np.array([0.0, 0.0, 0.0])
class SyncCrazyflieTest(unittest.TestCase): 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 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 test_open_link(self): # Fixture # Test self.sut.open_link() # Assert self.assertTrue(self.sut.is_link_open()) def test_failed_open_link_raises_exception(self): # Fixture expected = 'Some error message' self.cf_mock.open_link = AsyncCallbackCaller( cb=self.cf_mock.connection_failed, args=[self.uri, expected]).trigger # Test try: self.sut.open_link() except Exception as e: actual = e.args[0] else: self.fail('Expect exception') # Assert self.assertEqual(expected, actual) self._assertAllCallbacksAreRemoved() def test_open_link_of_already_open_link_raises_exception(self): # Fixture self.sut.open_link() # Test # Assert with self.assertRaises(Exception): self.sut.open_link() def test_wait_for_params(self): # Fixture self.sut.open_link() # Test self.sut.wait_for_params() # Assert self.assertTrue(self.sut.is_params_updated()) def test_do_not_wait_for_params(self): # Fixture # Test self.sut.open_link() # Assert self.assertFalse(self.sut.is_params_updated()) def test_close_link(self): # Fixture self.sut.open_link() # Test self.sut.close_link() # Assert self.assertEqual(1, self.close_link_mock.call_count) self.assertFalse(self.sut.is_link_open()) self._assertAllCallbacksAreRemoved() def test_close_link_that_is_not_open(self): # Fixture # Test self.sut.close_link() # Assert self.assertFalse(self.sut.is_link_open()) def test_closed_if_connection_is_lost(self): # Fixture self.sut.open_link() # Test AsyncCallbackCaller( cb=self.cf_mock.disconnected, args=[self.uri]).call_and_wait() # Assert self.assertFalse(self.sut.is_link_open()) self._assertAllCallbacksAreRemoved() def test_open_close_with_context_management(self): # Fixture # Test with SyncCrazyflie(self.uri, self.cf_mock) as sut: self.assertTrue(sut.is_link_open()) # Assert self.assertEqual(1, self.close_link_mock.call_count) self._assertAllCallbacksAreRemoved() def test_wait_for_params_with_context_management(self): # Fixture # Test with SyncCrazyflie(self.uri, cf=self.cf_mock) as sut: sut.wait_for_params() self.assertTrue(sut.is_params_updated()) # Assert self._assertAllCallbacksAreRemoved() def test_multiple_open_close_of_link(self): # Fixture # Test self.sut.open_link() self.sut.close_link() self.sut.open_link() self.sut.close_link() # Assert self.assertEqual(2, self.close_link_mock.call_count) self.assertFalse(self.sut.is_link_open()) self._assertAllCallbacksAreRemoved() def test_wait_for_params_with_multiple_open_close_of_link(self): # Fixture # Test self.sut.open_link() self.assertFalse(self.sut.is_params_updated()) self.sut.wait_for_params() self.assertTrue(self.sut.is_params_updated()) self.sut.close_link() self.assertFalse(self.sut.is_params_updated()) self.sut.open_link() self.assertFalse(self.sut.is_params_updated()) self.sut.wait_for_params() self.assertTrue(self.sut.is_params_updated()) self.sut.close_link() # Assert self.assertFalse(self.sut.is_params_updated()) self._assertAllCallbacksAreRemoved() def _assertAllCallbacksAreRemoved(self): # Remove our probe callback self.cf_mock.connected.remove_callback(self._connected_callback) self.assertEqual(0, len(self.cf_mock.connected.callbacks)) self.assertEqual(0, len(self.cf_mock.connection_failed.callbacks)) self.assertEqual(0, len(self.cf_mock.disconnected.callbacks)) self.assertEqual(0, len(self.param_mock.all_updated.callbacks)) def _connected_callback(self, uri): AsyncCallbackCaller( cb=self.param_mock.all_updated, delay=0.2 ).trigger()
class Drone: # RIGIDBODYMASS = 1 # RIGIDBODYRADIUS = 0.1 # LINEARDAMPING = 0.97 # SENSORRANGE = 0.6 # TARGETFORCE = 3 # AVOIDANCEFORCE = 25 # FORCEFALLOFFDISTANCE = .5 RIGIDBODYMASS = 0.5 RIGIDBODYRADIUS = 0.1 LINEARDAMPING = 0.95 SENSORRANGE = 0.6 TARGETFORCE = 1 AVOIDANCEFORCE = 10 FORCEFALLOFFDISTANCE = .5 def __init__(self, manager, position: Vec3, uri="-1", printDebugInfo=False): self.base = manager.base self.manager = manager # The position of the real drone this virtual drone is connected to. # If a connection is active, this value is updated each frame. self.realDronePosition = Vec3(0, 0, 0) self.canConnect = False # true if the virtual drone has a uri to connect to a real drone self.isConnected = False # true if the connection to a real drone is currently active self.uri = uri if self.uri != "-1": self.canConnect = True self.id = int(uri[-1]) self.randVec = Vec3(random.uniform(-1, 1), random.uniform(-1, 1), random.uniform(-1, 1)) # add the rigidbody to the drone, which has a mass and linear damping self.rigidBody = BulletRigidBodyNode( "RigidSphere") # derived from PandaNode self.rigidBody.setMass(self.RIGIDBODYMASS) # body is now dynamic self.rigidBody.addShape(BulletSphereShape(self.RIGIDBODYRADIUS)) self.rigidBody.setLinearSleepThreshold(0) self.rigidBody.setFriction(0) self.rigidBody.setLinearDamping(self.LINEARDAMPING) self.rigidBodyNP = self.base.render.attachNewNode(self.rigidBody) self.rigidBodyNP.setPos(position) # self.rigidBodyNP.setCollideMask(BitMask32.bit(1)) self.base.world.attach(self.rigidBody) # add a 3d model to the drone to be able to see it in the 3d scene model = self.base.loader.loadModel(self.base.modelDir + "/drones/drone1.egg") model.setScale(0.2) model.reparentTo(self.rigidBodyNP) self.target = position # the long term target that the virtual drones tries to reach self.setpoint = position # the immediate target (setpoint) that the real drone tries to reach, usually updated each frame self.waitingPosition = Vec3(position[0], position[1], 0.7) self.lastSentPosition = self.waitingPosition # the position that this drone last sent around self.printDebugInfo = printDebugInfo if self.printDebugInfo: # put a second drone model on top of drone that outputs debug stuff model = self.base.loader.loadModel(self.base.modelDir + "/drones/drone1.egg") model.setScale(0.4) model.setPos(0, 0, .2) model.reparentTo(self.rigidBodyNP) # initialize line renderers self.targetLineNP = self.base.render.attachNewNode(LineSegs().create()) self.velocityLineNP = self.base.render.attachNewNode( LineSegs().create()) self.forceLineNP = self.base.render.attachNewNode(LineSegs().create()) self.actualDroneLineNP = self.base.render.attachNewNode( LineSegs().create()) self.setpointNP = self.base.render.attachNewNode(LineSegs().create()) def connect(self): """Connects the virtual drone to a real one with the uri supplied at initialization.""" if not self.canConnect: return print(self.uri, "connecting") self.isConnected = True self.scf = SyncCrazyflie(self.uri, cf=Crazyflie(rw_cache='./cache')) self.scf.open_link() self._reset_estimator() self.start_position_printing() # MOVE THIS BACK TO SENDPOSITIONS() IF STUFF BREAKS self.scf.cf.param.set_value('flightmode.posSet', '1') def sendPosition(self): """Sends the position of the virtual drone to the real one.""" cf = self.scf.cf self.setpoint = self.getPos() # send the setpoint cf.commander.send_position_setpoint(self.setpoint[0], self.setpoint[1], self.setpoint[2], 0) def disconnect(self): """Disconnects the real drone.""" print(self.uri, "disconnecting") self.isConnected = False cf = self.scf.cf cf.commander.send_stop_setpoint() time.sleep(0.1) self.scf.close_link() def update(self): """Update the virtual drone.""" # self.updateSentPositionBypass(0) self._updateTargetForce() self._updateAvoidanceForce() self._clampForce() if self.isConnected: self.sendPosition() # draw various lines to get a better idea of whats happening self._drawTargetLine() # self._drawVelocityLine() self._drawForceLine() # self._drawSetpointLine() self._printDebugInfo() def updateSentPosition(self, timeslot): transmissionProbability = 1 if self.id == timeslot: if random.uniform(0, 1) < transmissionProbability: # print(f"drone {self.id} updated position") self.lastSentPosition = self.getPos() else: pass # print(f"drone {self.id} failed!") def updateSentPositionBypass(self, timeslot): self.lastSentPosition = self.getPos() def getPos(self) -> Vec3: return self.rigidBodyNP.getPos() def getLastSentPos(self) -> Vec3: return self.lastSentPosition def _updateTargetForce(self): """Applies a force to the virtual drone which moves it closer to its target.""" dist = (self.target - self.getPos()) if (dist.length() > self.FORCEFALLOFFDISTANCE): force = dist.normalized() else: force = (dist / self.FORCEFALLOFFDISTANCE) self.addForce(force * self.TARGETFORCE) def _updateAvoidanceForce(self): """Applies a force the the virtual drone which makes it avoid other drones.""" # get all drones within the sensors reach and put them in a list nearbyDrones = [] for drone in self.manager.drones: dist = (drone.getLastSentPos() - self.getPos()).length() if drone.id == self.id: # prevent drone from detecting itself continue if dist < self.SENSORRANGE: nearbyDrones.append(drone) # calculate and apply forces for nearbyDrone in nearbyDrones: distVec = nearbyDrone.getLastSentPos() - self.getPos() if distVec.length() < 0.2: print("BONK") distMult = self.SENSORRANGE - distVec.length() avoidanceDirection = self.randVec.normalized( ) * 2 - distVec.normalized() * 10 avoidanceDirection.normalize() self.addForce(avoidanceDirection * distMult * self.AVOIDANCEFORCE) def _clampForce(self): """Clamps the total force acting in the drone.""" totalForce = self.rigidBody.getTotalForce() if totalForce.length() > 2: self.rigidBody.clearForces() self.rigidBody.applyCentralForce(totalForce.normalized()) def targetVector(self) -> Vec3: """Returns the vector from the drone to its target.""" return self.target - self.getPos() def _printDebugInfo(self): if self.printDebugInfo: pass def setTarget(self, target: Vec3): """Sets a new target for the drone.""" self.target = target def setRandomTarget(self): """Sets a new random target for the drone.""" self.target = self.manager.getRandomRoomCoordinate() def setNewRandVec(self): self.randVec = Vec3(random.uniform(-1, 1), random.uniform(-1, 1), random.uniform(-1, 1)) def addForce(self, force: Vec3): self.rigidBody.applyCentralForce(force) def setPos(self, position: Vec3): self.rigidBodyNP.setPos(position) def getVel(self) -> Vec3: return self.rigidBody.getLinearVelocity() def setVel(self, velocity: Vec3): return self.rigidBody.setLinearVelocity(velocity) def _drawTargetLine(self): self.targetLineNP.removeNode() ls = LineSegs() # ls.setThickness(1) ls.setColor(1.0, 0.0, 0.0, 1.0) ls.moveTo(self.getPos()) ls.drawTo(self.target) node = ls.create() self.targetLineNP = self.base.render.attachNewNode(node) def _drawVelocityLine(self): self.velocityLineNP.removeNode() ls = LineSegs() # ls.setThickness(1) ls.setColor(0.0, 0.0, 1.0, 1.0) ls.moveTo(self.getPos()) ls.drawTo(self.getPos() + self.getVel()) node = ls.create() self.velocityLineNP = self.base.render.attachNewNode(node) def _drawForceLine(self): self.forceLineNP.removeNode() ls = LineSegs() # ls.setThickness(1) ls.setColor(0.0, 1.0, 0.0, 1.0) ls.moveTo(self.getPos()) ls.drawTo(self.getPos() + self.rigidBody.getTotalForce() * 0.2) node = ls.create() self.forceLineNP = self.base.render.attachNewNode(node) def _drawSetpointLine(self): self.setpointNP.removeNode() ls = LineSegs() # ls.setThickness(1) ls.setColor(1.0, 1.0, 1.0, 1.0) ls.moveTo(self.getPos()) ls.drawTo(self.setpoint) node = ls.create() self.setpointNP = self.base.render.attachNewNode(node) def _wait_for_position_estimator(self): """Waits until the position estimator reports a consistent location after resetting.""" print('Waiting for estimator to find position...') log_config = LogConfig(name='Kalman Variance', period_in_ms=500) log_config.add_variable('kalman.varPX', 'float') log_config.add_variable('kalman.varPY', 'float') log_config.add_variable('kalman.varPZ', 'float') var_y_history = [1000] * 10 var_x_history = [1000] * 10 var_z_history = [1000] * 10 threshold = 0.001 with SyncLogger(self.scf, log_config) as logger: for log_entry in logger: data = log_entry[1] var_x_history.append(data['kalman.varPX']) var_x_history.pop(0) var_y_history.append(data['kalman.varPY']) var_y_history.pop(0) var_z_history.append(data['kalman.varPZ']) var_z_history.pop(0) min_x = min(var_x_history) max_x = max(var_x_history) min_y = min(var_y_history) max_y = max(var_y_history) min_z = min(var_z_history) max_z = max(var_z_history) if (max_x - min_x) < threshold and ( max_y - min_y) < threshold and (max_z - min_z) < threshold: break def _reset_estimator(self): """Resets the position estimator, this should be run before flying the drones or they might report a wrong position.""" cf = self.scf.cf cf.param.set_value('kalman.resetEstimation', '1') time.sleep(0.1) cf.param.set_value('kalman.resetEstimation', '0') self._wait_for_position_estimator() def position_callback(self, timestamp, data, logconf): """Updates the variable holding the position of the actual drone. It is not called in the update method, but by the drone itself (I think).""" x = data['kalman.stateX'] y = data['kalman.stateY'] z = data['kalman.stateZ'] self.realDronePosition = Vec3(x, y, z) # print('pos: ({}, {}, {})'.format(x, y, z)) def start_position_printing(self): """Activate logging of the position of the real drone.""" log_conf = LogConfig(name='Position', period_in_ms=50) log_conf.add_variable('kalman.stateX', 'float') log_conf.add_variable('kalman.stateY', 'float') log_conf.add_variable('kalman.stateZ', 'float') self.scf.cf.log.add_config(log_conf) log_conf.data_received_cb.add_callback(self.position_callback) log_conf.start()
import paralelo from cflib.crazyflie import Crazyflie from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.positioning.motion_commander import MotionCommander from cflib.crazyflie.swarm import CachedCfFactory from cflib.crazyflie.swarm import Swarm URI1 = 'radio://0/80/250K/E7E7E7E7E9' URI2 = 'radio://0/80/250K/E7E7E7E7EA' cflib.crtp.init_drivers(enable_debug_driver=False) factory = CachedCfFactory(rw_cache='./cache') scf1 = SyncCrazyflie(URI1, cf=Crazyflie(rw_cache='./cache')) scf1.open_link() mc1 = MotionCommander(scf1) # scf2 = SyncCrazyflie(URI2, cf=Crazyflie(rw_cache='./cache')) # scf2.open_link() # mc2 = MotionCommander(scf2) # mcs = {mc1, mc2} pr = paralelo.Paralelo(mc1) pr.putCommand(paralelo.TAKEOFF, mc1) # pr.putCommand(paralelo.TAKEOFF, mc2) pr.execute() #pr.putCommand(paralelo.UP, mc1) # pr.putCommand(paralelo.TAKEOFF, mc2) pr.execute()
class MyCrazyFlieClient: def __init__(self): cflib.crtp.init_drivers(enable_debug_driver=False) cf = Crazyflie(rw_cache='./cache') self.scf = SyncCrazyflie(URI, cf=cf) self.scf.open_link() self.client = None log_config = LogConfig(name='kalman', period_in_ms=100) log_config.add_variable('kalman.stateX', 'float') log_config.add_variable('kalman.stateY', 'float') self.logger_pos = SyncLogger(self.scf, log_config) log_config_2 = LogConfig(name='stabilizer', period_in_ms=100) log_config_2.add_variable('stabilizer.yaw', 'float') self.logger_orientation = SyncLogger(self.scf, log_config_2) self.home_pos = self.get_position() print('Home = %s ' % self.home_pos) self.orientation = self.get_orientation() self.client = MotionCommander(self.scf) self.client.take_off(height=0.6) def land(self): self.client.land() self.scf.close_link() def check_if_in_target(self, goal): pos = self.get_position() distance = np.sqrt( np.power((goal[0] - pos[0]), 2) + np.power((goal[1] - pos[1]), 2)) if distance < 1: self.land() return True return False def get_position(self): self.logger_pos.connect() data = self.logger_pos.next()[1] self.logger_pos.disconnect() self.logger_pos._queue.empty() return [data['kalman.stateX'], -data['kalman.stateY']] def get_orientation(self): self.logger_orientation.connect() data = self.logger_orientation.next()[1] self.logger_orientation.disconnect() self.logger_orientation._queue.empty() return -data['stabilizer.yaw'] def straight(self, speed): self.client.forward(0.25, speed) start = time.time() return start def yaw_right(self): self.client.turn_right(30, 72) start = time.time() return start def yaw_left(self): self.client.turn_left(30, 72) start = time.time() return start def take_action(self, action): start = time.time() collided = False if action == 0: start = self.straight(0.25) if action == 1: start = self.yaw_right() if action == 2: start = self.yaw_left() # print(start) return collided def goal_direction(self, goal, pos): yaw = self.get_orientation() print('yaw now %s' % yaw) pos_angle = math.atan2(goal[1] - pos[1], goal[0] - pos[0]) pos_angle = math.degrees(pos_angle) % 360 print('pos angle %s' % pos_angle) track = pos_angle - yaw track = ((track - 180) % 360) - 180 print('Track = %s ' % track) return track def observe(self, goal): position_now = self.get_position() track = self.goal_direction(goal, position_now) distance = np.sqrt( np.power((goal[0] - position_now[0]), 2) + np.power((goal[1] - position_now[1]), 2)) distance_sensor = Multiranger(self.scf) distance_sensor.start() front_distance_sensor = distance_sensor.front while front_distance_sensor is None: time.sleep(0.01) front_distance_sensor = distance_sensor.front distance_sensor.stop() print('Position now = %s ' % position_now) # print('Track = %s ' % track) print('Distance to target: %s' % distance) print('Front distance: %s' % front_distance_sensor) return position_now[0], position_now[ 1], track, distance, front_distance_sensor
class SyncCrazyflieTest(unittest.TestCase): def setUp(self): self.uri = 'radio://0/60/2M' 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]).trigger self.sut = SyncCrazyflie(self.uri, self.cf_mock) 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.assertNotEquals(actual1, actual2) def test_open_link(self): # Fixture # Test self.sut.open_link() # Assert self.assertTrue(self.sut.is_link_open()) def test_failed_open_link_raises_exception(self): # Fixture expected = 'Some error message' self.cf_mock.open_link = AsyncCallbackCaller( cb=self.cf_mock.connection_failed, args=[self.uri, expected]).trigger # Test try: self.sut.open_link() except Exception as e: actual = e.args[0] else: self.fail('Expect exception') # Assert self.assertEqual(expected, actual) def test_open_link_of_already_open_link_raises_exception(self): # Fixture self.sut.open_link() # Test # Assert with self.assertRaises(Exception): self.sut.open_link() def test_close_link(self): # Fixture self.sut.open_link() # Test self.sut.close_link() # Assert self.cf_mock.close_link.assert_called_once_with() self.assertFalse(self.sut.is_link_open()) def test_close_link_that_is_not_open(self): # Fixture # Test self.sut.close_link() # Assert self.assertFalse(self.sut.is_link_open()) def test_closed_if_connection_is_lost(self): # Fixture self.sut.open_link() # Test AsyncCallbackCaller( cb=self.cf_mock.disconnected, args=[self.uri]).call_and_wait() # Assert self.assertFalse(self.sut.is_link_open()) def test_open_close_with_context_mangement(self): # Fixture # Test with SyncCrazyflie(self.uri, self.cf_mock) as sut: self.assertTrue(sut.is_link_open()) # Assert self.cf_mock.close_link.assert_called_once_with()
class Aruco_tracker(): def __init__(self, distance): """ Inicialização dos drivers, parâmetros do controle PID e decolagem do drone. """ self.distance = distance self.pid_foward = PID(distance, 0.01, 0.0001, 0.01, 500, -500, 0.7, -0.7) self.pid_yaw = PID(0, 0.33, 0.0, 0.33, 500, -500, 100, -100) self.pid_angle = PID(0.0, 0.01, 0.0, 0.01, 500, -500, 0.3, -0.3) self.pid_height = PID(0.0, 0.002, 0.0002, 0.002, 500, -500, 0.3, -0.2) cflib.crtp.init_drivers(enable_debug_driver=False) self.factory = CachedCfFactory(rw_cache='./cache') self.URI4 = 'radio://0/40/2M/E7E7E7E7E4' self.cf = Crazyflie(rw_cache='./cache') self.sync = SyncCrazyflie(self.URI4, cf=self.cf) self.sync.open_link() self.mc = MotionCommander(self.sync) self.cont = 0 self.notFoundCount = 0 self.calculator = DistanceCalculator() self.cap = cv2.VideoCapture(1) self.mc.take_off() time.sleep(1) def search_marker(self): """ Interrompe o movimento se nao encontrar o marcador por tres frames consecutivos. Após 4 segundos, inicia movimento de rotação para procurar pelo marcador. """ if ((3 <= self.notFoundCount <= 20) and self.mc.isRunning): self.mc.stop() self.mc.setIsRunning(False) elif (self.notFoundCount > 20): self.mc._set_vel_setpoint(0.0, 0.0, 0.0, 80) self.mc.setIsRunning(True) return def update_motion(self): """ Atualiza as velocidades utilizando controlador PID. """ horizontal_distance = self.calculator.horizontal_distance vertical_distance = self.calculator.vertical_distance x_distance = self.calculator.distance_x alpha = self.calculator.alpha velocity_x = self.pid_foward.update(x_distance) Velocity_z = self.pid_height.update(vertical_distance) if (x_distance < (self.distance + 10)): velocity_y = self.pid_angle.update(alpha) else: velocity_y = 0 velocity_z = 0 yaw = self.pid_yaw.update(horizontal_distance) self.mc._set_vel_setpoint(velocity_x, velocity_y, 0, yaw) self.mc.setIsRunning(True) return def track_marker(self): """ Programa principal, drone segue um marcador aruco. """ while (self.cap.isOpened()): ret, frame = self.cap.read() if (frame is None): break if (self.cont % 6 == 0): thread = threading.Thread( target=self.calculator.calculateDistance, args=(frame, )) thread.setDaemon(True) thread.start() if (self.calculator.markerFound): self.notFoundCount = 0 self.update_motion() else: self.notFoundCount += 1 self.search_marker() self.calculator.writeDistance(frame) cv2.imshow('frame', frame) self.cont += 1 if cv2.waitKey(10) & 0xFF == ord('q'): self.mc.land() cv2.destroyAllWindows() break cv2.waitKey() self.sync.close_link()
cv2.imwrite(s, frame) self.cont += 1 def readImage(self): s = str(self.cont) + ".jpg" while len(s) < 9: s = "0" + s s = "frames/" + s frame = cv2.imread(s) self.cont += 1 return frame if __name__ == '__main__': #conexção do cf cflib.crtp.init_drivers(enable_debug_driver=False) factory = CachedCfFactory(rw_cache='./cache') URI3 = 'radio://0/30/2M/E7E7E7E7E3' cf = Crazyflie(rw_cache='./cache') sync = SyncCrazyflie(URI3, cf=cf) sync.open_link() mc = MotionCommander(sync) scan = Scan(mc) scan.explore() input("tecle enter") cv2.destroyAllWindows()
class BaseDrone (): """ BaseDrone class that is used to communicate to a crazyflie2.1 drone. The BaseDrone class establishes a connection and gives a small set of functions to verify that the connection is set e.g. communicating with the led ring. """ ################################################################################ # Initialization and cleanup code ################################################################################ def __init__(self, uri): """Initialize the connection to the Drone. The uri is depending on the drone and has to be given at initialization. """ self.uri = uri cflib.crtp.init_drivers(enable_debug_driver=False) self.cf = Crazyflie(rw_cache='./cache') self.scf = SyncCrazyflie (self.uri, cf=self.cf) def __enter__(self): """Establishes the link to the drone and returns the BaseDroneObject""" self.scf.open_link() return self def __exit__(self, exc_type, exc_val, exc_tb): """Closes the connection to the drone to secure a clean shutdown even in case of an exeption""" self.close_link() def close_link(self): """Closes the Link to the drone and removes possible callbacks""" self.scf.cf.close_link() self.scf._remove_callbacks() self.scf._is_link_open = False ################################################################################ # Higher Functionality like setting the color of the LED's # (get creative here) ################################################################################ def red_green_color(self, sequence): """Gets an array of values and displays a green or a red light on the led ring if sequence[n] - startXPositionOfDrone < 0: red light else: green light """ cf = self.scf.cf self.setRingEffect(7) for position in sequence: x = position[0] if x<0: self.setRingColor(red=100) else: self.setRingColor(green=100) time.sleep(1) # time for led to be shown def setRingEffect(self, value): """Sets the ring.effect param of the cf to the specified value If the value is not in range of the specified effects (see https://wiki.bitcraze.io/projects:crazyflie2:expansionboards:ledring), it will print an warning """ MAX_NUM_EFFECTS = 13 if value >= 0 and value < MAX_NUM_EFFECTS: self.scf.cf.param.set_value('ring.effect',str(value)) else: print("WARNING: Could not set ring.effect to value: {}. Use a value between 0 and {}. For further Information see https://wiki.bitcraze.io/projects:crazyflie2:expansionboards:ledring".format(value,MAX_NUM_EFFECTS)) def setRingColor(self, red = 0, green = 0, blue = 0): """Sets the color of the ring to a specific value. Red, Green and Blue can be numbers between 0 and 100. TODO: test if ring effect has to be set to 7 all the time. """ if red < 0 or red > 100 or green < 0 or green > 100 or blue < 0 or blue > 100: return False else: self.scf.cf.param.set_value('ring.solidRed', str(red)) self.scf.cf.param.set_value('ring.solidGreen', str(green)) self.scf.cf.param.set_value('ring.solidBlue', str(blue)) return True
class CrazyflieThread(threading.Thread): def __init__(self, ctrl_message, state_message, message_lock, barrier): threading.Thread.__init__(self) self.ctrl = ctrl_message self.state = state_message self.lock = message_lock self.cf = None self.mc = None self.scf = None self.runSM = True self.cmd_height_old = uglyConst.TAKEOFF_HEIGHT self.b = barrier self.descCounter = 0 def raise_exception(self): self.runSM = False #-- FSM condition funcitons def isCloseXYP(self, r): """Determines if drone is within radius r and aligned.""" x = self.ctrl.errorx y = self.ctrl.errory if (np.sqrt(x*x+y*y) > r) or (np.abs(self.ctrl.erroryaw) > uglyConst.FAR_ANGL): return False else: return True def isCloseCone(self): """Determines if drone within an inverted cone (defined in constants).""" x = self.ctrl.errorx y = self.ctrl.errory r = (self.mc._thread.get_height()-uglyConst.DIST_IGE)*uglyConst.FAR_CONE if (np.sqrt(x*x+y*y) > r): # or (np.abs(self.ctrl.erroryaw) > uglyConst.FAR_ANGL): return False else: return True def isClosePix(self): """Determines if drone is within radius (in pixels, defined in constants).""" x = self.ctrl.errorpixx y = self.ctrl.errorpixy if (np.sqrt(x*x+y*y) > uglyConst.CLOSE_PIX): return False else: return True def limOutputVel(self, vx, vy, vz): """Limits output of velocity controller.""" return min(vx, uglyConst.MAX_XVEL), min(vy, uglyConst.MAX_YVEL), min(vz, uglyConst.MAX_ZVEL) #-- FSM state functions 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 enterTakingOff(self): """Entry to state: Take off""" print("enterTakingOff") self.mc.take_off(uglyConst.TAKEOFF_HEIGHT,uglyConst.TAKEOFF_ZVEL) def stateTakingOff(self): """State: Taking off""" print("stateTakingOff") if self.mc._thread.get_height() >= uglyConst.TAKEOFF_HEIGHT: return self.stateSeeking else: time.sleep(0.05) return self.stateTakingOff def stateSeeking(self): """State: Seeking. Slowly ascend until marker is detected. TODO: Implement some kind of search algorithm (circle outward?)""" self.mc._set_vel_setpoint(0.0, 0.0, 0.01, 0.0) print("stateSeeking") if self.state.isMarkerDetected: return self.stateNearing else: time.sleep(0.05) return self.stateSeeking def stateNearing(self): """ State: Nearing Control in pixel frame. Takes in error in pixels (note: camera coordinates), outputs velocity command in x,y,z. Z vel inversely proportional to radius. """ x = self.ctrl.errorpixx y = self.ctrl.errorpixy cmdx = y*uglyConst.PIX_Kx cmdy = x*uglyConst.PIX_Ky r = np.sqrt(x*x+y*y) if r > 0.0: cmdz = (1/r)*uglyConst.PIX_Kz else: cmdz = 1 cmdx, cmdy, cmdz = self.limOutputVel(cmdx, cmdy, cmdz) self.mc._set_vel_setpoint(cmdx, cmdy, -cmdz, 0.0) print("stateNearing") if self.isClosePix() and self.mc._thread.get_height() < uglyConst.NEARING2APPROACH_HEIGHT: self.state.cv_mode = uglyConst.CVMODE_POSE return self.stateApproachin else: time.sleep(0.05) return self.stateNearing def stateApproachingXY(self): """ State: Approaching (XY plane) Control in the horizontal XY plane and yaw angle. """ #self.mc._set_vel_setpoint(self.ctrl.errorx*(uglyConst.Kx+self.ctrl.K), self.ctrl.errory*(uglyConst.Ky+self.ctrl.K), 0.0, 30.0) self.mc._set_vel_setpoint(self.ctrl.errorx*uglyConst.Kx, self.ctrl.errory*uglyConst.Ky, 0.0, -self.ctrl.erroryaw*uglyConst.Kyaw) print("stateApproachingXY") if not self.isClosePix: return self.stateNearing if self.isCloseCone() and np.abs(self.ctrl.erroryaw) < uglyConst.FAR_ANGL: return self.stateApproachingXYZ else: time.sleep(0.05) return self.stateApproachingXY def stateApproachingXYZ(self): """ State: Approaching Control in world frame. Takes in error in meters, outputs velocity command in x,y. Constant vel cmd in z. """ self.mc._set_vel_setpoint(self.ctrl.errorx*uglyConst.Kx, self.ctrl.errory*uglyConst.Ky, -uglyConst.APPROACH_ZVEL, -self.ctrl.erroryaw*uglyConst.Kyaw) print("stateApproachingXYZ") if not self.isCloseCone: return self.stateApproachingXY elif self.mc._thread.get_height() < uglyConst.APPROACH2LANDING_HEIGHT: return self.stateDescending else: time.sleep(0.05) return self.stateApproachingXYZ def stateDescending(self): """ State: Landing Procedure: Descend to a set height, then stop and land. """ self.mc._set_vel_setpoint(self.ctrl.errorx*uglyConst.Kx*2.0, self.ctrl.errory*uglyConst.Ky*2.0, -uglyConst.LANDING_ZVEL, -self.ctrl.erroryaw*uglyConst.Kyaw) print("stateDescending") if self.mc._thread.get_height() > uglyConst.APPROACH2LANDING_HEIGHT: return self.stateApproaching elif self.mc._thread.get_height() < uglyConst.LANDING2LANDED_HEIGHT: #self.exitLanding() #return self.stateTerminating return self.stateLanding else: time.sleep(0.01) return self.stateDescending def stateLanding(self): """""" self.mc._set_vel_setpoint(self.ctrl.errorx*uglyConst.Kx*4.0, self.ctrl.errory*uglyConst.Ky*4.0, -uglyConst.MAX_ZVEL, -self.ctrl.erroryaw*uglyConst.Kyaw*2.0) self.descCounter += 1 print("stateLanding") if self.descCounter > 10: self.mc.land() return self.stateTerminating else: time.sleep(0.01) return self.stateLanding def exitLanding(self): """ Exit from state: Landing Stop movement (vel cmds=0), then descend. """ self.mc.land() print("exitLandning") def stateTerminating(self): """ State: Landed Dummy state. """ print("stateTerminating") return None def run(self): """Main loop, state machine""" try: state = self.stateInit # initial state self.stateNum = uglyConst.S0_INIT while state and self.runSM: state = state() finally: #-- Clean up print('Stopping cf_thread') if self.cf is not None: if self.mc._is_flying: # if cf has not landed, do so self.mc.stop() print('Finally landing') self.mc.land() #-- Close link and logging self.scf.close_link() self.file.close()
myfile = open('StabilizerData.txt', 'w') myfile.write('') myfile.close() myfile = open('AccelerometerData.txt', 'w') myfile.write('') myfile.close() myfile = open('GyroscopeData.txt', 'w') myfile.write('') myfile.close() myfile = open('SensorMaster.txt', 'w') myfile.write cflib.crtp.init_drivers(enable_debug_driver=False) scf = SyncCrazyflie(URI) scf.open_link() mc = MotionCommander(scf) #mc._reset_position_estimator() #mc.take_off() #key = Thread(target = key_ctrl, args = (mc,scf)) #key.start() letstest = yaw_test(scf) """fig = plt.figure() axRoll = fig.add_subplot(3,1,1) axYaw = fig.add_subplot(3,1,2) axPitch = fig.add_subplot(3,1,3) animate = displayStb(axRoll,axYaw,axPitch) ani = animation.FuncAnimation(fig, animate.update, interval=20)